float LinearInterpolation (VectorXf X , VectorXf Y, float X_PointOfInterest){
//Produce Y_point_of_interest given X,Y and target X_point_of_interest
//X : vector containing the X variables of the interpolant 
//Y : vector containing the Y variables of the interpolant 
//PointOfInterest : Point of X to estimate the new point of Y
    
  float   xk, xkp1, yk, ykp1;  //Points adjecent to the point of interpolation
  if ( X.size() != Y.size() ){cout << "Problem with vector sizes" << endl; return(-1);}
//cout <<  " X(0): " <<  X(0) <<" X(Y.size()-1): " <<X(Y.size()-1)   <<   " Point of interest: " << X_PointOfInterest<< endl;
  if ( X_PointOfInterest < X(0) || X_PointOfInterest > X(Y.size()-1) ){cout << "You interpolate out of the curve boundaries" << endl; return(-1);}
//Find the points right before and right after the point of interest
  for (int i=1; i<X.size() ; i++){
    if (X(i)>= X_PointOfInterest){
      xkp1 = X(i);
      xk = X(i-1);
      ykp1 = Y(i);
      yk = Y(i-1);
      break;}
  }
//point-slope form for a line formula
  float t = (X_PointOfInterest -xk)/(xkp1 -xk);
  float yPOI = (1-t) * yk + t * ykp1;  // estimate point of interest
// cout << "(" << xk << ",  " << X_PointOfInterest << " , " << xkp1 << ") & (" << yk << ", " << yPOI  << ", " << ykp1 << ")"<< endl;
  return (yPOI);   
} 

void Layouter::laplacianEigMap( const MatrixXd& laplacian, const VectorXf& radiusVec, const VectorXi& hashID, MatrixXf& finalPos2D, float& finalRadius, float sparseFactor /*= 1.f*/ )
{
	if (laplacian.rows() <= 0 || laplacian.cols() <= 0 || radiusVec.size() <= 0)
	{
		finalPos2D = MatrixXf::Zero(1,2);
		finalRadius = 0;
		return;
	}
	if (laplacian.rows() == 1 && laplacian.cols() == 1 && radiusVec.size() == 1)
	{
		finalPos2D = MatrixXf::Zero(1,2);
		finalRadius = radiusVec[0];
		return;
	}
	MatrixXd pos2D, finalPosd;
	LaplacianSolver::compute(laplacian, pos2D); 

	VectorXd minPos, maxPos;
	MDSPostProcesser m_postProcessor(500, 1.0f, 1.0, 1.02, radiusVec.minCoeff());
	m_postProcessor.setSparseFactor(sparseFactor);
	m_postProcessor.set2DPos(pos2D, radiusVec.cast<double>(), &hashID);
	m_postProcessor.compute();
	m_postProcessor.getFinalPos(finalPosd);
	m_postProcessor.getFinalBoundingRect(minPos, maxPos);
	finalPos2D = finalPosd.cast<float>();
	// 	finalPos2D = pos2D.cast<float>();
	// 	minPos = pos2D.colwise().minCoeff();
	// 	maxPos = pos2D.colwise().maxCoeff();
	//  VectorXd size = maxPos - minPos;
	finalRadius = m_postProcessor.getFinalRadius();// size.norm() * 0.5;//(size[0] > size[1] ? size[0] : size[1]) * 0.5f;
}

void NonRigid::computeInflateDir(VectorXf &p_vec, VectorXf &g_vec)
{
    VectorXf arap_g = g_vec;
    VectorXf inflate_g = g_vec;
    VectorXf userCrsp_g = g_vec;
    double f_e = 0;
    computeArap(p_vec, arap_g);
    computeUserCrsp(p_vec, userCrsp_g);

    Vector3f cur_v(0,0,0);
    Vector3f cur_g(0,0,0);
    size_t P_Num = p_vec.size()/3;
    for (size_t i = 0; i < P_Num; ++i)
    {
        cur_v << p_vec(i + 0*P_Num), p_vec(i + 1*P_Num), p_vec(i + 2*P_Num);
        if (computeVertexGrad(cur_v, cur_g) > 0.0)
        {
            inflate_g(i + 0*P_Num) = cur_g(0);
            inflate_g(i + 1*P_Num) = cur_g(1);
            inflate_g(i + 2*P_Num) = cur_g(2);
        }
        else
        {
            inflate_g(i + 0*P_Num) = -P_Prime_N(0, i);
            inflate_g(i + 1*P_Num) = -P_Prime_N(1, i);
            inflate_g(i + 2*P_Num) = -P_Prime_N(2, i);
        }

    }

    g_vec = lamd_inflate*inflate_g + lamd_arap*arap_g + lamd_userCrsp*userCrsp_g;
    g_vec.normalize();
}

VectorXf param_sensitivity_widget::computeSensitivity(
    MatrixXf &parameterMatrix, VectorXf &responseVector)
{
    MatrixXf Ctemp = parameterMatrix.transpose()*parameterMatrix;
    MatrixXf C;
    C = Ctemp.inverse();

    VectorXf b = C*parameterMatrix.transpose()*responseVector;

    VectorXf Y_hat = parameterMatrix*b;

    int p = b.rows();

    VectorXf sigma2Vec = responseVector-Y_hat;

    float sigma2 = sigma2Vec.squaredNorm();
    sigma2= sigma2/(parameterMatrix.rows() - p);

    Ctemp = C*sigma2;

    MatrixXf denominator = Ctemp.diagonal();

    // Do element-wise division
    VectorXf t = b;
    for (int i = 0; i < b.rows(); i++)
    {
        t(i) = abs(b(i)/sqrt(denominator(i)));
    }

    return t;
}

VectorXf AutoEncoder::Calculate(int index)
{
    /*
     *  Description:
     *  Calculate the i-th samples by feedforward and store hiddenvector
     *  in the row i of hidden matrix, output in row i of output matrix
     *
     *  @return outputVector: The output of FF
     */
    
    VectorXf HiddenVector = Weight_encode * OriginalData->row(index).transpose() + Bias_encode;
    for (int i = 0; i < HiddenVector.size(); i++)
    {
        HiddenVector(i) = sigmoid(HiddenVector(i));
    }
    HiddenMatrix.row(index) = HiddenVector.transpose();

    VectorXf output_vector = VectorXf(IO_dim);
    output_vector = Weight_decode * HiddenVector + Bias_decode;
    for (int i = 0; i < output_vector.size(); i++)
    {
        output_vector(i) = sigmoid(output_vector(i));
    }
    OutputMatrix.row(index) = output_vector.transpose();
    
    return output_vector;
}

void PlaneFittingCloudOrienter::_compute() {
    assert(input_cloud_);
    assert(input_intensity_);
    assert(!output_cloud_);

    // -- Fit a plane.
    VectorXf a = fitPlane(*input_cloud_);

    // -- Rotate the points so that the direction of the best plane is the x axis.
    assert(fabs(a.norm() - 1) < 1e-4);
    double theta = M_PI/2. - atan2(a(1), a(0));
    MatrixXf rotation = MatrixXf::Identity(3, 3);
    rotation(0,0) = cos(theta);
    rotation(1,1) = cos(theta);
    rotation(0,1) = sin(theta);
    rotation(1,0) = -sin(theta);

    output_cloud_ = shared_ptr<MatrixXf>(new MatrixXf());
    *output_cloud_ = *input_cloud_ * rotation;

    VectorXf foo = fitPlane(*output_cloud_);
    //cout << "New plane: " << foo.transpose() << endl;

    // -- Subtract off the mean of the points.
    MatrixXf& points = *output_cloud_;
    VectorXf pt_mean = points.colwise().sum() / (float)points.rows();
    for(int i=0; i<points.rows(); ++i)
        points.row(i) -= pt_mean.transpose();

}

void D3DRandomProjector::doComputation(const sensor_msgs::PointCloud& data,
				    cloud_kdtree::KdTree& data_kdtree,
				    const vector<const std::vector<int>*>& interest_region_indices,
				    vector<vector<float> >& results)
{
  assert(results.size() == interest_region_indices.size());
  vvf orig_results;
  descriptor_->compute(data, data_kdtree, interest_region_indices, orig_results); //TODO: Make descriptors cache their results.
  results = vvf(orig_results.size());
  for(size_t i=0; i<orig_results.size(); ++i) {
    if(orig_results[i].empty()) {
      results[i] = vector<float>(0);
      continue;
    }
      
    // -- Project the feature.
    VectorXf vec;
    floatToEigen(orig_results[i], &vec);
    VectorXf projected = projector_ * vec;

    // -- Put into results.
    results[i] = vector<float>(projected.rows());
    for(size_t j=0; j<results[i].size(); ++j) {
      results[i][j] = projected(j);
    }
  }
}

void KF_joseph_update(VectorXf &x, MatrixXf &P,float v,float R, MatrixXf H)
{
    VectorXf PHt = P*H.transpose();
    MatrixXf S = H*PHt;
    S(0,0) += R;
    MatrixXf Si = S.inverse();
    Si = make_symmetric(Si);
    MatrixXf PSD_check = Si.llt().matrixL(); //chol of scalar is sqrt
    PSD_check.transpose();
    PSD_check.conjugate();

    VectorXf W = PHt*Si;
    x = x+W*v;
    
    //Joseph-form covariance update
    MatrixXf eye(P.rows(), P.cols());
    eye.setIdentity();
    MatrixXf C = eye - W*H;
    P = C*P*C.transpose() + W*R*W.transpose();  

    float eps = 2.2204*pow(10.0,-16); //numerical safety 
    P = P+eye*eps;

    PSD_check = P.llt().matrixL();
    PSD_check.transpose();
    PSD_check.conjugate(); //for upper tri
}

void sumAndNormalize( MatrixXf & out, const MatrixXf & in, const MatrixXf & Q ) {
	out.resize( in.rows(), in.cols() );
	for( int i=0; i<in.cols(); i++ ){
		VectorXf b = in.col(i);
		VectorXf q = Q.col(i);
		out.col(i) = b.array().sum()*q - b;
	}
}

SeedFeature::SeedFeature( const ImageOverSegmentation & ios, const VectorXf & obj_param ) {
	Image rgb_im = ios.image();
	const RMatrixXs & s = ios.s();
	const int Ns = ios.Ns(), W = rgb_im.W(), H = rgb_im.H();
	
	// Initialize various values
	VectorXf area  = bin( s, 1, [&](int x, int y){ return 1.f; } );
	VectorXf norm = (area.array()+1e-10).cwiseInverse();
	pos_ = norm.asDiagonal() * bin( s, 6, [&](int i, int j){ float x=1.0*i/(W-1)-0.5,y=1.0*j/(H-1)-0.5; return makeArray<6>( x, y, x*x, y*y, fabs(x), fabs(y) ); } );
	if (N_DYNAMIC_COL) {
		Image lab_im;
		rgb2lab( lab_im, rgb_im );
		col_ = norm.asDiagonal() * bin( s, 6, [&](int x, int y){ return makeArray<6>( rgb_im(y,x, 0), rgb_im(y,x,1), rgb_im(y,x,2), lab_im(y,x,0), lab_im(y,x,1), lab_im(y,x,2) ); } );
	}
	
	const int N_GEO = sizeof(EDGE_P)/sizeof(EDGE_P[0]);
	for( int i=0; i<N_GEO; i++ )
		gdist_.push_back( GeodesicDistance(ios.edges(),ios.edgeWeights().array().pow(EDGE_P[i])+1e-3) );
	
	// Compute the static features
	static_f_ = RMatrixXf::Zero( Ns, N_STATIC_F );
	int o=0;
	// Add the position features
	static_f_.block( 0, o, Ns, N_STATIC_POS ) = pos_.leftCols( N_STATIC_POS );
	o += N_STATIC_POS;
	// Add the geodesic features
	if( N_STATIC_GEO >= N_GEO ) {
		RMatrixXu8 bnd = findBoundary( s );
		RMatrixXf mask = (bnd.array() == 0).cast<float>()*1e10;
		for( int i=0; i<N_GEO; i++ )
			static_f_.col( o++ ) = gdist_[i].compute( mask );
		for( int j=1; (j+1)*N_GEO<=N_STATIC_GEO; j++ ) {
			mask = (bnd.array() != j).cast<float>()*1e10;
			for( int i=0; i<N_GEO; i++ )
				static_f_.col( o++ ) = gdist_[i].compute( mask );
		}
	}
	if( N_STATIC_EDGE ) {
		RMatrixXf edge_map = DirectedSobel().detect( ios.image() );
		for( int j=0; j<s.rows(); j++ )
			for( int i=0; i<s.cols(); i++ ) {
				const int id = s(j,i);
				int bin = edge_map(j,i)*N_STATIC_EDGE;
				if ( bin < 0 ) bin = 0;
				if ( bin >= N_STATIC_EDGE ) bin = N_STATIC_EDGE-1;
				static_f_(id,o+bin) += norm[id];
			}
		o += N_STATIC_EDGE;
	}
	if( N_OBJ_F>1 )
		static_f_.col(o++) = (computeObjFeatures(ios)*obj_param).transpose();
	
	// Initialize the dynamic features
	dynamic_f_ = RMatrixXf::Zero( Ns, N_DYNAMIC_F );
	n_ = 0;
	min_dist_ = RMatrixXf::Ones(Ns,5)*10;
	var_      = RMatrixXf::Zero(Ns,6);
}

int main(int, char**)
{
  cout.precision(3);
  VectorXf v;
v.setConstant(3, 5);
cout << v << endl;

  return 0;
}

int main(int, char**)
{
  cout.precision(3);
  VectorXf v;
v.setOnes(3);
cout << v << endl;

  return 0;
}

///////////////////////
/////  Inference  /////
///////////////////////
void expAndNormalize ( MatrixXf & out, const MatrixXf & in ) {
	out.resize( in.rows(), in.cols() );
	for( int i=0; i<out.cols(); i++ ){
		VectorXf b = in.col(i);
		b.array() -= b.maxCoeff();
		b = b.array().exp();
		out.col(i) = b / b.array().sum();
	}
}

Hamming::Hamming( const VectorXs & gt, float class_weight_pow ):gt_( gt ) {
    int M=0,N=gt.rows();;
    for( int i=0; i<N; i++ )
        if( gt[i] >= M )
            M = gt[i]+1;
    VectorXf cnt = VectorXf::Zero( M );
    for( int i=0; i<N; i++ )
        if( gt[i] >= 0 )
            cnt[gt[i]] += 1;
    class_weight_ = cnt.array() / cnt.array().sum();
    class_weight_ = class_weight_.array().pow( -class_weight_pow );
    class_weight_ = class_weight_.array() / (cnt.array()*class_weight_.array()).sum();
}

VectorXf PBSeqWeightEstimator::calc_residue_weight(const vector<string>& msa, const int& idx) const {
    VectorXf s = VectorXf::Zero(dim);  // number of times a particular residue appears
    char c;
    for (vector<string>::const_iterator pos = msa.begin(); pos != msa.end(); ++pos) {
        c = (*pos)[idx];
        if (is_allowed(c)) s(abc_idx(c)) += 1;
    }
    double r = (double) (s.array() > 0).count();    // number of different residues
    VectorXf wt(dim);
    for (int k = 0; k < dim; ++k) {
        if (s(k) > 0) wt(k) = 1. / (r * s(k));
        else wt(k) = 0;
    }
    return wt;
}

size_t random_index (const VectorXf & likes)
{
  float total = likes.sum();
  ASSERT_LT(0, total);

  while (true) {
    float t = random_unif(0, total);

    for (int i = 0, I = likes.size(); i < I; ++i) {

      t -= likes(i);
      if (t < 0) return i;
    }
  }
}

MatrixX3f Surface::compute_normals(const MatrixX3f& rr, const MatrixX3i& tris)
{
    printf("\tcomputing normals\n");
    // first, compute triangle normals
    MatrixX3f r1(tris.rows(),3); MatrixX3f r2(tris.rows(),3); MatrixX3f r3(tris.rows(),3);

    for(qint32 i = 0; i < tris.rows(); ++i)
    {
        r1.row(i) = rr.row(tris(i, 0));
        r2.row(i) = rr.row(tris(i, 1));
        r3.row(i) = rr.row(tris(i, 2));
    }

    MatrixX3f x = r2 - r1;
    MatrixX3f y = r3 - r1;
    MatrixX3f tri_nn(x.rows(),y.cols());
    tri_nn.col(0) = x.col(1).cwiseProduct(y.col(2)) - x.col(2).cwiseProduct(y.col(1));
    tri_nn.col(1) = x.col(2).cwiseProduct(y.col(0)) - x.col(0).cwiseProduct(y.col(2));
    tri_nn.col(2) = x.col(0).cwiseProduct(y.col(1)) - x.col(1).cwiseProduct(y.col(0));

    //   Triangle normals and areas
    MatrixX3f tmp = tri_nn.cwiseProduct(tri_nn);
    VectorXf normSize = tmp.rowwise().sum();
    normSize = normSize.cwiseSqrt();

    for(qint32 i = 0; i < normSize.size(); ++i)
        if(normSize(i) != 0)
            tri_nn.row(i) /= normSize(i);

    MatrixX3f nn = MatrixX3f::Zero(rr.rows(), 3);

    for(qint32 p = 0; p < tris.rows(); ++p)
    {
        Vector3i verts = tris.row(p);
        for(qint32 j = 0; j < verts.size(); ++j)
            nn.row(verts(j)) = tri_nn.row(p);
    }

    tmp = nn.cwiseProduct(nn);
    normSize = tmp.rowwise().sum();
    normSize = normSize.cwiseSqrt();

    for(qint32 i = 0; i < normSize.size(); ++i)
        if(normSize(i) != 0)
            nn.row(i) /= normSize(i);

    return nn;
}

QStringList param_sensitivity_widget::sortSensitivity(VectorXf &senVal,
        QStringList &paramNames)
{
    // We need to sort senVal and use the corresponding indices to sort
    // paramNames

    // Step 1: Convert senVal to vector of pairs
    std::vector<std::pair<float,int> > sortedSenVals;
    std::pair<float,int> senValuePair;
    for (unsigned int i = 1; i < senVal.rows(); ++i)
    {
        senValuePair.first = senVal(i);
        senValuePair.second = i-1; // We are ignoring first element
        sortedSenVals.push_back(senValuePair);
    }

    // Step 2: Sort this vector
    std::sort(sortedSenVals.begin(), sortedSenVals.end());

    QStringList sortedParamNames;

    // Step 3: Rewrite the senVal and paramNames
    for (unsigned int i = 0; i < sortedSenVals.size(); ++i)
    {
        sortedParamNames.push_back(paramNames.at(sortedSenVals.at(i).second));
        senVal[i] = sortedSenVals.at(i).first;
    }

    return sortedParamNames;

}

int MonotonicityCheck(VectorXf X){
   // evaluate vector monotonicity of vector X
   int N =  X.size(); 
   for (int i=0; i<(N-1) ; i++){  
     if ( X(i) >X (1+i) ) return (-1);   }
   return (1);
}

void test_return_by_value(int len)
{
    VectorXf in;
    VectorXf in1;
    in.setRandom( len );
    VectorXcf out1,out2;
    FFT<float> fft;

    fft.SetFlag(fft.HalfSpectrum );

    fft.fwd(out1,in);
    out2 = fft.fwd(in);
    VERIFY( (out1-out2).norm() < test_precision<float>() );
    in1 = fft.inv(out1);
    VERIFY( (in1-in).norm() < test_precision<float>() );
}