int softmax<T>::predict(cv::Mat const &input)
{
    Eigen::Map<EigenMat> const Map(reinterpret_cast<*>(input.data),
                                   input.rows,
                                   input.step / sizeof(T));
    return predict(Map.block(0, 0, input.rows, input.cols));
}

//==============================================================================
void NullFunction::evalHessian(
    const Eigen::VectorXd& _x,
    Eigen::Map<Eigen::VectorXd, Eigen::RowMajor> _Hess)
{
  _Hess.resize(pow(_x.size(),2));
  _Hess.setZero();
}

std::vector<int> const& softmax<T>::
batch_predicts(cv::Mat const &input)
{
    Eigen::Map<EigenMat> const Map(reinterpret_cast<*>(input.data),
                                   input.rows,
                                   input.step / sizeof(T));
    return batch_predicts(Map.block(0, 0, input.rows, input.cols));
}

 void ExpMapQuaternion::applyDiffPseudoLog0_(
     RefMat out, const ConstRefMat& in, const ConstRefVec& x, ReusableTemporaryMap& m)
 {
   mnf_assert(in.cols() == InputDim_ && "Dimensions mismatch" );
   Eigen::Map<Eigen::MatrixXd, Eigen::Aligned> a = m.getMap(in.rows(),OutputDim_);
   a.noalias() = in*diffPseudoLog0_(x);
   out = a;
 }

  void evalGradient(const Eigen::VectorXd& _x,
                    Eigen::Map<Eigen::VectorXd> _grad) override
  {
    computeResultVector(_x);

    _grad.setZero();
    int smaller = std::min(mResultVector.size(), _grad.size());
    for(int i=0; i < smaller; ++i)
      _grad[i] = mResultVector[i];
  }

void RigidBody3DState::updateMandMinv()
{
  assert( unsigned( m_M0.nonZeros() ) == 6 * nbodies() );
  assert( m_M0.nonZeros() == m_Minv0.nonZeros() );
  assert( unsigned( m_M.nonZeros() ) == 12 * nbodies() );
  assert( m_M.nonZeros() == m_Minv.nonZeros() );

  for( unsigned bdy_idx = 0; bdy_idx < m_nbodies; ++bdy_idx )
  {
    // Orientation of the ith body
    const Eigen::Map<const Matrix33sr> R{ m_q.segment<9>( 3 * m_nbodies + 9 * bdy_idx ).data() };
    assert( fabs( ( R * R.transpose() - Matrix33sr::Identity() ).lpNorm<Eigen::Infinity>() ) <= 1.0e-9 );
    assert( fabs( R.determinant() - 1.0 ) <= 1.0e-9 );

    // Inertia tensor of the ith body
    {
      Eigen::Map<Matrix33sc> I{ &m_M.data().value( 3 * m_nbodies + 9 * bdy_idx ) };
      const Eigen::Map<const Vector3s> I0{ &m_M0.data().value( 3 * m_nbodies + 3 * bdy_idx ) };
      I = R * I0.asDiagonal() * R.transpose();
      assert( ( I - I.transpose() ).lpNorm<Eigen::Infinity>() <= 1.0e-12 );
      assert( I.determinant() > 0.0 );
    }

    // Inverse of the inertia tensor of the ith body
    {
      Eigen::Map<Matrix33sc> Iinv{ &m_Minv.data().value( 3 * m_nbodies + 9 * bdy_idx ) };
      const Eigen::Map<const Vector3s> Iinv0{ &m_Minv0.data().value( 3 * m_nbodies + 3 * bdy_idx ) };
      Iinv = R * Iinv0.asDiagonal() * R.transpose();
      assert( ( Iinv - Iinv.transpose() ).lpNorm<Eigen::Infinity>() <= 1.0e-12 );
      assert( Iinv.determinant() > 0.0 );
    }
  }

  assert( MathUtilities::isIdentity( m_M * m_Minv, 1.0e-9 ) );
}

//Version 4
double basicBayes::evaluteMVG(std::vector<double>& sampleVecVect, std::vector<double>& meanVecVect, std::vector<double>& covMatVect){
	//this funciton is problem specific
	//Convert
	Eigen::Map<Eigen::MatrixXd> sampleVec = convertVect(sampleVecVect);
	Eigen::Map<Eigen::MatrixXd> meanVec = convertVect(meanVecVect);
	Eigen::Map<Eigen::MatrixXd> covMat = convertCovMat(covMatVect);
	
	Eigen::MatrixXd error = (sampleVec - meanVec);
	Eigen::Matrix<double,1,1> secondHalf = (error.transpose()*covMat.inverse()*error);
	return (1.0/(pow(2.0*M_PI,meanVec.rows()/2.0)*pow(covMat.determinant(),0.5)))*exp(-0.5*secondHalf(0,0));
}

void ComputePcBoundaries(const pcl::PointCloud<pcl::PointXYZ>&
    pc, Eigen::Vector3f& min, Eigen::Vector3f& max) {
  const Eigen::Map<const Eigen::MatrixXf,1,Eigen::OuterStride<> > x = pc.getMatrixXfMap(1, 4, 0); // this works for PointXYZRGBNormal
  const Eigen::Map<const Eigen::MatrixXf,1,Eigen::OuterStride<> > y = pc.getMatrixXfMap(1, 4, 1); // this works for PointXYZRGBNormal
  const Eigen::Map<const Eigen::MatrixXf,1,Eigen::OuterStride<> > z = pc.getMatrixXfMap(1, 4, 2); // this works for PointXYZRGBNormal
  min << x.minCoeff(), y.minCoeff(), z.minCoeff();
  max << x.maxCoeff(), y.maxCoeff(), z.maxCoeff();
}

    std::vector<VectorEntry<Kernel>> mapParameter(Eigen::Map<Derived>& map) {
        new(&map) Eigen::Map<Derived>(&m_parameters(++m_parameterOffset));
        
        std::vector<VectorEntry<Kernel>> result(map.rows());
        for (int i = 0; i < map.rows(); ++i)
            result[i] = {m_parameterOffset + i, &m_parameters(m_parameterOffset + i)};

        //minus one as we increase the parameter offset already in this function
        m_parameterOffset+= (map.rows()-1);
                
        return result;
    };

static SparseMatrixsc formWorldSpaceInverseMassMatrix( const std::vector<scalar>& M, const std::vector<Vector3s>& I0, const std::vector<VectorXs>& R )
{
  assert( M.size() == I0.size() );
  assert( M.size() == I0.size() );
  const unsigned nbodies{ static_cast<unsigned>( I0.size() ) };
  const unsigned nvdofs{ 6 * nbodies };

  SparseMatrixsc Mbody{ static_cast<SparseMatrixsc::Index>( nvdofs ), static_cast<SparseMatrixsc::Index>( nvdofs ) };
  {
    VectorXi column_nonzeros{ nvdofs };
    column_nonzeros.segment( 0, 3 * nbodies ).setOnes();
    column_nonzeros.segment( 3 * nbodies, 3 * nbodies ).setConstant( 3 );
    Mbody.reserve( column_nonzeros );
  }
  // Load the total masses
  for( unsigned bdy_idx = 0; bdy_idx < nbodies; ++bdy_idx )
  {
    for( unsigned dof_idx = 0; dof_idx < 3; ++dof_idx )
    {
      const unsigned col{ 3 * bdy_idx + dof_idx };
      const unsigned row{ col };
      assert( M[ bdy_idx ] > 0.0 );
      Mbody.insert( row, col ) = 1.0 / M[ bdy_idx ];
    }
  }

  // Load the inertia tensors
  for( unsigned bdy_idx = 0; bdy_idx < nbodies; ++bdy_idx )
  {
    // Transform from principal axes rep
    const Eigen::Map<const Matrix33sr> Rmat{ R[bdy_idx].data() };
    assert( ( Rmat * Rmat.transpose() - Matrix33sr::Identity() ).lpNorm<Eigen::Infinity>() <= 1.0e-9 );
    assert( fabs( Rmat.determinant() - 1.0 ) <= 1.0e-9 );
    const Matrix33sr Iinv = Rmat * I0[bdy_idx].array().inverse().matrix().asDiagonal() * Rmat.transpose();
    assert( ( Iinv - Iinv.transpose() ).lpNorm<Eigen::Infinity>() <= 1.0e-12 );
    assert( Iinv.determinant() > 0.0 );
    for( unsigned row_idx = 0; row_idx < 3; ++row_idx )
    {
      const unsigned col{ 3 * nbodies + 3 * bdy_idx + row_idx };
      for( unsigned col_idx = 0; col_idx < 3; ++col_idx )
      {
        const unsigned row{ 3 * nbodies + 3 * bdy_idx + col_idx };
        Mbody.insert( row, col ) = Iinv( row_idx, col_idx );
      }
    }
  }
  assert( 12 * nbodies == unsigned( Mbody.nonZeros() ) );
  Mbody.makeCompressed();
  return Mbody;
}

        double BSplineMotionError<SPLINE_T>::evaluateErrorImplementation()
        {       
            // the error is a scalar: c' Q c, with c the vector valued spline coefficients stacked

            const double* cMat = &((_splineDV->spline()).coefficients()(0,0));
            Eigen::Map<const Eigen::VectorXd> c = Eigen::VectorXd::Map(cMat, _coefficientVectorLength);

            // Q*c :
            // create result container:
            Eigen::VectorXd Qc(_Q.rows());  // number of rows of Q:
            Qc.setZero();
           //  std::cout << Qc->rows() << ":" << Qc->cols() << std::endl;
            _Q.multiply(&Qc, c);
            
            return c.transpose() * (Qc);
        }

 void integrate_inplace(Eigen::Map<MatrixType> integral, Callable&& f) const noexcept
 {
     for (std::size_t index{0}; index < points(); ++index)
     {
         integral.noalias() += f(femvals[index], index) * m_weights[index];
     }
 }

// [[Rcpp::export]]
Eigen::MatrixXd cholupdateL_rcpp(const Eigen::Map<Eigen::MatrixXd>& L, 
                                 const Eigen::Map<Eigen::MatrixXd>& V12, 
                                 const Eigen::Map<Eigen::MatrixXd>& V22) { 
      
  int k = L.rows();
  int k2 = V22.rows();
  Eigen::MatrixXd S(k, k2);
  Eigen::MatrixXd U(k2, k2);
  Eigen::MatrixXd M(k2, k2);
  Eigen::MatrixXd Lup(k+k2, k+k2);
  
  Lup.setZero();
  S = L.triangularView<Lower>().solve(V12);
  M = V22 -  S.adjoint() * S ;
  U =  M.adjoint().llt().matrixL();
  Lup.topLeftCorner(k,k) = L;
  Lup.bottomLeftCorner(k2,k) = S.adjoint();
  Lup.bottomRightCorner(k2,k2) = U;
  return Lup;
}

double BodyNode::VelocityObjFunc::eval(Eigen::Map<const Eigen::VectorXd>& _x)
{
  assert(mBodyNode->getParentJoint()->getNumGenCoords() == _x.size());

  // Update forward kinematics information with _x
  // We are just insterested in spacial velocity of mBodyNode
  mBodyNode->getParentJoint()->setGenVels(_x, true, false);

  // Compute and return the geometric distance between body node transformation
  // and target transformation
  Eigen::Vector6d diff = mBodyNode->getWorldVelocity() - mVelocity;
  return diff.dot(diff);
}

Rcpp::List GetIndCEScoresCPP( const Eigen::Map<Eigen::VectorXd> & yVec, const Eigen::Map<Eigen::VectorXd> & muVec, const Eigen::Map<Eigen::VectorXd> & lamVec, const Eigen::Map<Eigen::MatrixXd> & phiMat, const Eigen::Map<Eigen::MatrixXd> & SigmaYi){ 

  // Setting up initial values

  const unsigned int lenlamVec = lamVec.size(); 
 
  Eigen::MatrixXd xiVar = Eigen::MatrixXd::Constant(lenlamVec,lenlamVec,std::numeric_limits<double>::quiet_NaN());
  Eigen::MatrixXd xiEst = Eigen::MatrixXd::Constant(lenlamVec,1,std::numeric_limits<double>::quiet_NaN());
  Eigen::MatrixXd fittedY = Eigen::MatrixXd::Constant(lenlamVec,1,std::numeric_limits<double>::quiet_NaN());
   
  Eigen::MatrixXd LamPhi = lamVec.asDiagonal() * phiMat.transpose();  
  Eigen::LDLT<Eigen::MatrixXd> ldlt_SigmaYi(SigmaYi);
  xiEst = LamPhi * ldlt_SigmaYi.solve(yVec - muVec) ;       // LamPhiSig * (yVec - muVec); 
  xiVar = -LamPhi * ldlt_SigmaYi.solve(LamPhi.transpose()); // LamPhiSig.transpose(); 
  
  xiVar.diagonal() += lamVec;
  fittedY = muVec + phiMat * xiEst;

  return Rcpp::List::create(Rcpp::Named("xiEst") = xiEst,
                            Rcpp::Named("xiVar") = xiVar,
                            Rcpp::Named("fittedY") = fittedY);
}

	void DiagonalGMM::sampleWithLimits( Eigen::Map<Eigen::VectorXf> &dst, const Eigen::VectorXf &minValues, const Eigen::VectorXf &maxValues )
	{
		//printf("sample with limits \n");
		int idx=sampleComponent();
		//printf("sampleWithLimits selected component %d\n",idx);
		if (dst.rows()!=nDimensions)
			Debug::throwError("Invalid dst dimensions for sampleWithLimits()!");
		for (int d=0; d<nDimensions; d++)
		{
			
			//dst[d]=randGaussianClipped(mean[idx][d],std[idx][d],minValues[d],maxValues[d]);
			//printf(" Sample with limit for %d-- mean %f, stdv %f, min %f, max %f \n",d,mean[idx][d],std[idx][d],minValues[d],maxValues[d] );
		
			dst[d]=randGaussianClipped(mean[idx][d],std[idx][d],minValues[d],maxValues[d]);
		}
	}

/* =================================================================
   GIBBS SAMPLER ALGORITHM TO DRAW A NEW VECTOR "y" ~ TruncNormal of dimension D
       GIVEN PREVIOUS VECTOR "y" AS INPUT
   Visits each entry in y in random order, 
     and resamples that entry using efficient 1D truncated normal sampling routines.
   When we visit the *target* dimension,
     we enforce that it must be larger than the maximum of all other entries
   Otherwise, we visit other dimensions and enforce that they must be smaller than y[target]
 */
void gibbs_sample_mv_randn_trunc( VectorType& y, const Eigen::Map<VectorType>& mu, int targetDim) {
  int D = (int) mu.size();
  double maxOthers;
  int *perm = new int[D];
  randperm( D, perm );

  for (int dd=0; dd < D; dd++) {
    int d = perm[dd];
    if (d == targetDim) {
      maxOthers = get_max_value_ignore_dim( y, targetDim );
      y(d) = randn_trunc_below( mu(d), 1.0, maxOthers );
    } else {
      y(d) = randn_trunc_above( mu(d), 1.0, y(targetDim) );
    }
  }
  delete [] perm;
  perm = NULL;
}

// Correlation implementation in Eigen
//' @rdname corFamily
//' @export
// [[Rcpp::export]]
Eigen::MatrixXd corEigen(Eigen::Map<Eigen::MatrixXd> & X) {
  
  // Handle degenerate cases
  if (X.rows() == 0 && X.cols() > 0) {
    return Eigen::MatrixXd::Constant(X.cols(), X.cols(), 
                                     Rcpp::NumericVector::get_na());
  }
  
  // Computing degrees of freedom
  // n - 1 is the unbiased estimate whereas n is the MLE
  const int df = X.rows() - 1; // Subtract 1 by default
  
  X.rowwise() -= X.colwise().mean();  // Centering
  
  Eigen::MatrixXd cor = X.transpose() * X / df;   // The covariance matrix
  
  // Get 1 over the standard deviations
  Eigen::VectorXd inv_sds = cor.diagonal().array().sqrt().inverse();
  
  // Scale the covariance matrix
  cor = cor.cwiseProduct(inv_sds * inv_sds.transpose());
  
  return cor;
}

//==============================================================================
void NullFunction::evalGradient(const Eigen::VectorXd& _x,
                                Eigen::Map<Eigen::VectorXd> _grad)
{
  _grad.resize(_x.size());
  _grad.setZero();
}

//' Fast Matrix Inverse
//' 
//' Computes using RcppEigen the inverse of A
//' 
//' @param A is the matrix being inverted
//' 
//' @return inverse of A
//' 
//' @examples
//' \dontrun{
//' rcppeigen_fsolve(A)
//' }
//' 
//[[Rcpp::export]]
Eigen::MatrixXd rcppeigen_fsolve(const Eigen::Map<Eigen::MatrixXd> & A){
  Eigen::MatrixXd Ainv = A.inverse();
  return Ainv;
}