ublas::vector<int> decodeBPSK(const ublas::vector<double> &rx) {
  ublas::vector<int> vHat(rx.size());
  for (unsigned int i = 0; i < rx.size(); i++) {
    vHat(i) = 0.5 * (sign(rx(i)) + 1);
  }

  return vHat;
}

const float NaoPose::getHomLength(const ublas::vector<float> &vec) {
    float sum = 0.0f;
    for (ublas::vector<float>::const_iterator i = vec.begin(); i != vec.end()
            - 1; ++i) {
        sum += *i * *i;
    }
    return sqrt(sum);
}

    /**
     * \f$U+=V \f$ where U and V are type
     * uvlas::vector<T> and you
     * want to specify WHERE to add
     * the DenseVector<T> V
     */
    void addVector ( const ublas::vector<value_type>& V,
                     const std::vector<size_type>& dof_indices )
    {
        FEELPP_ASSERT ( V.size() == dof_indices.size() ).error( "invalid dof indices" );

        for ( size_type i=0; i<V.size(); i++ )
            this->add ( dof_indices[i], V( i ) );
    }

double biterr(const ublas::vector<int> &u, const ublas::vector<int> &v) {
  int numErr = 0;
  for (unsigned int i = 0; i < u.size(); i++) {
    if (u(i) != v(i)) {
      numErr++;
    }
  }

  return static_cast<double>(numErr) / u.size();
}

ublas::vector<T>    cross( const ublas::vector<T> &a, const ublas::vector<T> &b )
{
	BOOST_ASSERT(a.size() == 3);
	BOOST_ASSERT(b.size() == 3);

	ublas::vector<T>	result(3);
	result(0) = a(1) * b(2) - a(2) * b(1);
	result(1) = a(2) * b(0) - a(0) * b(2);
	result(2) = a(0) * b(1) - a(1) * b(0);
	return result;
}

void printVector(const ublas::vector<T> &u) {
  for (unsigned int i = 0; i < u.size(); i++) {
    std::cout << u(i);

    if (i + 1 < u.size()) {
      std::cout << ", ";
    }
  }
  
  std::cout << std::endl;
}

ublas::vector<int> mod2(const ublas::vector<int> &u) {
  ublas::vector<int> v(u.size());

  for (unsigned int i = 0; i < u.size(); i++) {
    v(i) = u(i) % 2;

    // modulus result can be negative if u(i) negative, depending on implementation
    if (v(i) < 0) {
      v(i) += 2;
    }
  }

  return v;
}

ScalarType diff(ublas::vector<ScalarType> & v1, viennacl::vector<ScalarType> & v2)
{
   ublas::vector<ScalarType> v2_cpu(v2.size());
   viennacl::backend::finish();
   viennacl::copy(v2.begin(), v2.end(), v2_cpu.begin());

   for (unsigned int i=0;i<v1.size(); ++i)
   {
      if ( std::max( std::fabs(v2_cpu[i]), std::fabs(v1[i]) ) > 0 )
      {
        //if (std::max( std::fabs(v2_cpu[i]), std::fabs(v1[i]) ) < 1e-10 )  //absolute tolerance (avoid round-off issues)
        //  v2_cpu[i] = 0;
        //else
          v2_cpu[i] = std::fabs(v2_cpu[i] - v1[i]) / std::max( std::fabs(v2_cpu[i]), std::fabs(v1[i]) );
      }
      else
         v2_cpu[i] = 0.0;

      if (v2_cpu[i] > 0.0001)
      {
        //std::cout << "Neighbor: "      << i-1 << ": " << v1[i-1] << " vs. " << v2_cpu[i-1] << std::endl;
        std::cout << "Error at entry " << i   << ": " << v1[i]   << " vs. " << v2_cpu[i]   << std::endl;
        //std::cout << "Neighbor: "      << i+1 << ": " << v1[i+1] << " vs. " << v2_cpu[i+1] << std::endl;
        exit(EXIT_FAILURE);
      }
   }

   return norm_inf(v2_cpu);
}

ublas::vector<double> dirichlet_rnd(const ublas::vector<int>& nz) {
  //! Returns sample from a Dirichlet distribution with dimension k = len(nz).
  int k = nz.size();
  ublas::vector<double> A(k);
  for (int i=0; i<k; ++i) {
    A(i) = Rmath::rgamma(1+nz(i), 1);
  }

  return A/sum(A);
}

ScalarType diff ( ublas::vector<ScalarType> & v1, viennacl::vector<ScalarType,Alignment> & v2 ) {
    ublas::vector<ScalarType> v2_cpu ( v2.size() );
    viennacl::copy( v2.begin(), v2.end(), v2_cpu.begin() );
    for ( unsigned int i=0; i<v1.size(); ++i ) {
        if ( std::max ( fabs ( v2_cpu[i] ), fabs ( v1[i] ) ) > 0 )
            v2_cpu[i] = fabs ( v2_cpu[i] - v1[i] ) / std::max ( fabs ( v2_cpu[i] ), fabs ( v1[i] ) );
        else
            v2_cpu[i] = 0.0;
    }
    return norm_inf ( v2_cpu );
}

    /** returns the first n elites
     * @param p_start start value of the elite values
     * @param p_end end value of the elite values ([start, end) elite elements must be created)
     * @param p_population const reference to the population
     * @param p_rankIndex rank index (first index has the position of the population element, that has the smalles fitness value)
     * @param p_elite vector with elite individual
     **/
    template<typename T, typename L> inline void bestof<T,L>::getElite( const std::size_t& p_start, const std::size_t& p_end, const std::vector< boost::shared_ptr< individual::individual<L> > >& p_population, const ublas::vector<T>&, const ublas::vector<std::size_t>& p_rankIndex, const ublas::vector<std::size_t>&, std::vector< boost::shared_ptr< individual::individual<L> > >& p_elite )
    {
        const std::size_t l_end = std::min(p_end, m_number);

        std::size_t n = p_start;
        for(std::size_t i=p_start; i < p_end; ++i) {
            p_elite.push_back( p_population[p_rankIndex[p_rankIndex.size()-1-n]] );
            n++;
            if (n >= l_end)
                n = p_start;
        }
    }

ScalarType diff(ublas::vector<ScalarType> const & v1, VCLVectorType const & v2)
{
   ublas::vector<ScalarType> v2_cpu(v2.size());
   viennacl::backend::finish();  //workaround for a bug in APP SDK 2.7 on Trinity APUs (with Catalyst 12.8)
   viennacl::copy(v2.begin(), v2.end(), v2_cpu.begin());

   for (unsigned int i=0;i<v1.size(); ++i)
   {
      if (v2_cpu[i] != v1[i])
        return 1;
   }

   return 0;
}

Serialization SDPSeriationGen::Impl::readout_plain(ublas::vector<double>& x,const AdjMat::AdjMatT& adj)
{
	unsigned int n = x.size();

	// tricky: make sure x > 0 at all times.
	x += ublas::scalar_vector<double>(n, 1 - (*min_element(x.begin(),x.end())));

	Serialization::RankT ranks(n);
	std::vector<bool> done(n,false);

	// find highest component of x
	ublas::vector<double>::iterator it = BEST_ELEM(x);
	int idx = std::distance(x.begin(),it);

	L("Determine Actual Path through Graph.\n");
	for(unsigned int i=0;i<n;i++){
		ranks[i] = idx;
		done[idx] = true;
		*it = 0.0; 
		it = BEST_ELEM(x);
		idx = std::distance(x.begin(),it);
	}
	return Serialization(ranks);
}

ScalarType diff(ublas::vector<ScalarType> & v1, viennacl::vector<ScalarType> & v2)
{
   ublas::vector<ScalarType> v2_cpu(v2.size());
   viennacl::backend::finish();  //workaround for a bug in APP SDK 2.7 on Trinity APUs (with Catalyst 12.8)
   viennacl::copy(v2.begin(), v2.end(), v2_cpu.begin());

   for (std::size_t i=0;i<v1.size(); ++i)
   {
      if ( std::max( std::fabs(v2_cpu[i]), std::fabs(v1[i]) ) > 0 )
         v2_cpu[i] = std::fabs(v2_cpu[i] - v1[i]) / std::max( std::fabs(v2_cpu[i]), std::fabs(v1[i]) );
      else
         v2_cpu[i] = 0.0;
   }

   return norm_inf(v2_cpu);
}

    void image2vector_2d(const ublas::fixed_vector<size_t, 2> size, const float_accessor_t & image, ublas::vector<float_t> & vector)
    {
      if(image.size() != size)
        throw Exception("Exception: Dimensions of image and grid do not agree in Image2Grid::image2vector().");

      size_t n_x_vertices = size(0);
      size_t n_y_vertices = size(1);
  
      vector.resize(n_x_vertices * n_y_vertices, false);
  
      for(size_t i = 0; i < n_x_vertices; i++)
        for(size_t j = 0; j < n_y_vertices; ++j)
        {
          size_t vertex_index = i + j * n_x_vertices;
          vector(vertex_index) = image[ublas::fixed_vector<size_t, 2>(i, j)];
        }
    }

   void vector2image_2d(const ublas::fixed_vector<size_t, 2> size, const ublas::vector< float_t > &vector, float_accessor_t & image)
   {
     size_t n_x_vertices = size(0);
     size_t n_y_vertices = size(1);
 
     if(vector.size() != n_x_vertices * n_y_vertices)
       throw Exception("Exception: vector of wrong length in vector2image().");
     
     if(image.size() != size)
       throw Exception("Exception: image of wrong size in vector2image().");
 
     for(size_t i = 0; i < n_x_vertices; i++)
       for(size_t j = 0; j < n_y_vertices; ++j)
       {
         size_t vertex_index = i + j * n_x_vertices;
         image[ublas::fixed_vector<size_t, 2>(i, j)] = vector(vertex_index);
       }
   }

   void vector2vector_image_2d(const ublas::fixed_vector<size_t, 2> size, const ublas::vector< float_t > &vector, vector_image_accessor_t & vector_image)
   {
     const size_t n_x_vertices = size(0);
     const size_t n_y_vertices = size(1);
     const size_t dimension = vector_image_accessor_t::data_t::dimension;
 
     if(vector.size() != n_x_vertices * n_y_vertices * dimension)
       throw Exception("Exception: tla::vector of wrong length in vector2image().");
     
     if(vector_image.size() != size)
       throw Exception("Exception: vector_image of wrong size in vector2vector_image().");
 
     for(size_t i = 0; i < n_x_vertices; i++)
       for(size_t j = 0; j < n_y_vertices; ++j)
         for(size_t k = 0; k < dimension; ++k)
         {
           size_t vertex_index = dimension * ( i + j * n_x_vertices ) + k;
           vector_image[ublas::fixed_vector<size_t, 2>(i, j)](k) = vector(vertex_index);
         }
   }

		void learn(const database<T, bool> &data,
			ublas::vector<double> vec_sample_weights)
		{
			ASSERT(data.patterns.size2() == data.targets.size(), "");

			_weak_classifiers.clear();
			_¦Á.clear();
			for (size_t k = 0; k < classifier_size(); ++k){
				auto classifier = _master_classifier.clone();
				auto sum = ublas::sum(vec_sample_weights);
				for (auto &e:vec_sample_weights){
					e /= sum;
				}

				classifier.learn(data, vec_sample_weights);

				double eps = 0.0;
				ublas::vector<bool> re(data.targets.size());
				for (size_t i = 0; i < data.patterns.size2(); ++i){
					re[i] = classifier(ublas::column(data.patterns, i));
					if (re[i] != (data.targets)[i]){
						eps += vec_sample_weights[i];
					}
				}
#ifdef _CONSOLE
				std::cout << k << "Adaboost Classifier error : " << eps << std::endl;
#endif
				if (eps >= 0.5)							continue;

				auto _¦Ák = 0.5*ln((1.0-eps) / (eps+1e-4));
				for (size_t i = 0; i < vec_sample_weights.size(); ++i){
					if ((data.targets)[i] != re[i])
						vec_sample_weights[i] *= exp(_¦Ák);
					else
						vec_sample_weights[i] *= exp(-_¦Ák);
				}
				_weak_classifiers.push_back(classifier);
				_¦Á.push_back(_¦Ák);
			}
		}

int main() {
  boost::random::mt19937 gen;
  gen.seed(static_cast<unsigned int>(std::time(0)));

  const unsigned int M = M3;
  const unsigned int N = N3;

  const unsigned int iterations = 5;
  const unsigned int frames = 30;

  const double EbN0[EBN0_SIZE] = { -7.0, -6.5, -6.0, -5.5, -5.0, -4.5, -4.0, -3.5, -3.0, -2.5, -2.0, -1.5, -1.0, -0.5, 0, 0.5, 1.0, 1.5, 2.0, 2.5, 3.0, 3.5, 4.0, 4.5, 5.0, 5.5, 6.0, 6.5, 7.0  };
  double ber0[EBN0_SIZE];
  double ber1[EBN0_SIZE];
  double ber2[EBN0_SIZE];

  ublas::vector<int> fer0(EBN0_SIZE);
  ublas::vector<int> fer1(EBN0_SIZE);
  ublas::vector<int> fer2(EBN0_SIZE);

  ublas::matrix<int> H(M, N);

  initMatrix<int>(H, hData3);

  // LU matrices
  ublas::matrix<int> L(ublas::zero_matrix<int>(M, N - M));
  ublas::matrix<int> U(ublas::zero_matrix<int>(M, N - M));

  reorderHMatrix(H, L, U);

  /*
  writeMatrix<int>(H, "H.csv");
  writeMatrix<int>(L, "Lpre.csv");
  writeMatrix<int>(U, "Upre.csv");
  */

  boost::random::uniform_int_distribution<> uniformDistribution(0, 1);
  boost::random::normal_distribution<> normalDistribution;

  //ublas::matrix<int> C(EBN0_SIZE * frames, M);

  /*
  ublas::matrix<int> dSource(M, frames);
  initMatrix<int>(dSource, dSourceData3);
  writeMatrix<int>(dSource, "dSource.csv");
  */

  for (unsigned int i = 0; i < EBN0_SIZE; i++) {
    //std::cout << "EbN0 : " << i << std::endl;

    ber0[i] = 0.0;
    ber1[i] = 0.0;
    ber2[i] = 0.0;

    fer0(i) = 0;
    fer1(i) = 0;
    fer2(i) = 0;

    // Make random data (0/1)
    ublas::matrix<int> dSource(M, frames);
    for (unsigned int j = 0; j < frames; j++) {
      for (unsigned int k = 0; k < M; k++) {
        dSource(k, j) = uniformDistribution(gen);
      }
    }

    //std::cout << "dSource=" << std::endl;
    //printMatrix<int>(dSource);

    for (unsigned int j = 0; j < frames; j++) {
      //std::cout << "Frame : " << j << std::endl;

      // Encoding message
      const ublas::vector<int> c = makeParityCheck(ublas::column(dSource, j), H, L, U);
      //ublas::row(C, i * (EBN0_SIZE + 1) + j) = c;

      //std::cout << "newH=" << std::endl;
      //printMatrix<int>(newH);

      //std::cout << "c=" << std::endl;
      //printVector<int>(c);

      ublas::vector<int> u(c.size() + M);
      ublas::subrange(u, 0, c.size()) = c;
      ublas::subrange(u, c.size(), c.size() + M) = ublas::column(dSource, j);

      //std::cout << "u=" << std::endl;
      //printVector<int>(u);

      // BPSK modulation
      ublas::vector<int> bpskMod(u.size());
      for (unsigned int k = 0; k < u.size(); k++) {
        bpskMod(k) = 2 * u(k) - 1;
      }

      //std::cout << "bpskMod=" << std::endl;
      //printVector<int>(bpskMod);

      // Additional white gaussian noise
      double N0 = 1 / (std::exp(EbN0[i] * std::log(10) / 10));

//.........这里部分代码省略.........

ublas::vector<int> decodeBitFlipping(const ublas::vector<double> &rx, const ublas::matrix<int> &H, const unsigned int iterations) {
  // Get matrix dimensions
  const unsigned int M = H.size1();
  const unsigned int N = H.size2();

  // Prior hard-decision
  ublas::vector<int> ci(rx.size());
  for (unsigned int i = 0; i < rx.size(); i++) {
    ci(i) = 0.5 * (sign(rx(i)) + 1);
  }

  //std::cout << "ci=" << std::endl;
  //printVector<int>(ci);

  // Initialization
  ublas::matrix<int> rji(ublas::zero_matrix<int>(M, N));

  // Associate the ci matrix with non-zero elements of H
  ublas::matrix<int> qij(M, N);
  for (unsigned int i = 0; i < M; i++) {
    ublas::row(qij, i) = ublas::element_prod(ublas::row(H, i), ci);
  }

  //std::cout << "qij=" << std::endl;
  //printMatrix<int>(qij);

  ublas::vector<int> vHat(N);

  // Iteration
  for (unsigned int n = 0; n < iterations; n++) {
    //std::cout << "Iteration : " << n << std::endl;

    // Horizontal step
    for (unsigned int i = 0; i < M; i++) {
      int qijSum = 0;
      for (unsigned int j = 0; j < N; j++) {
        if (H(i, j) != 0) {
          qijSum += qij(i, j);
        }
      }

      for (unsigned int j = 0; j < N; j++) {
        if (H(i, j) != 0) {
          rji(i, j) = (qijSum + qij(i, j)) % 2;
        }
      }
    }

    // Vertical step
    for (unsigned int j = 0; j < N; j++) {
      int rjiNumberOfOnes = 0;
      for (unsigned int i = 0; i < M; i++) {
        if (rji(i, j) != 0) {
          rjiNumberOfOnes++;
        }
      }

      int hNumberOfOnes = 0;
      for (unsigned int i = 0; i < M; i++) {
        if (H(i, j) != 0) {
          hNumberOfOnes++;
        }
      }
      
      for (unsigned int i = 0; i < M; i++) {
        if (H(i, j) != 0) {
          // Update qij, set '1' for majority of 1s else '0', excluding i
          if (rjiNumberOfOnes + ci(j) >= hNumberOfOnes - rjiNumberOfOnes + rji(i, j)) {
            qij(i, j) = 1;
          } else {
            qij(i, j) = 0;
          }
        }
      }

      // Bit decoding
      if (rjiNumberOfOnes + ci(j) >= hNumberOfOnes - rjiNumberOfOnes) {
        vHat(j) = 1;
      } else {
        vHat(j) = 0;
      }
    }
  }

  return vHat;
}