/*!
  \brief Initialisation of a sub matrix
  \param m : parent matrix
  \param row : row offset 
  \param col : col offset 
  \param nrows : number of rows of the sub matrix
  \param ncols : number of columns of the sub matrix
*/
void vpSubMatrix::init(vpMatrix &m, const unsigned int & row, const unsigned int &col , const unsigned int & nrows ,  const unsigned int & ncols){
  
  if(! m.data){
    vpERROR_TRACE("\n\t\t SubMatrix parent matrix is not allocated") ;
    throw(vpMatrixException(vpMatrixException::subMatrixError,
			    "\n\t\t SubMatrix parent matrix is not allocated")) ;
  } 
  
  if(row+nrows <= m.getRows() && col+ncols <= m.getCols()){	
    data=m.data;
    parent =&m; 
    rowNum = nrows;
    colNum = ncols;
    pRowNum=m.getRows(); 
    pColNum=m.getCols(); 
    
    if(rowPtrs)
      free(rowPtrs);
    
    rowPtrs=(double**) malloc(nrows * sizeof(double*));
    for(unsigned int r=0;r<nrows;r++)
      rowPtrs[r]= m.data+col+(r+row)*pColNum;
    
    dsize = pRowNum*pColNum ;
    trsize =0 ;
  }else{
    vpERROR_TRACE("Submatrix cannot be contain in parent matrix") ;
    throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,"Submatrix cannot be contain in parent matrix")) ;
  }
}

/*!

  Compute and return the interaction matrix \f$ L_I \f$. The computation is made
  thanks to the values of the luminance features \f$ I \f$
*/
void
vpFeatureLuminance::interaction(vpMatrix &L)
{
  double x,y,Ix,Iy,Zinv;

  L.resize(dim_s,6) ;

  for(int m = 0; m< L.getRows(); m++)
    {
      Ix = pixInfo[m].Ix;
      Iy = pixInfo[m].Iy;

      x = pixInfo[m].x ;
      y = pixInfo[m].y ;
      Zinv =  1 / pixInfo[m].Z;

      {
	L[m][0] = Ix * Zinv;
	L[m][1] = Iy * Zinv;
	L[m][2] = -(x*Ix+y*Iy)*Zinv;
	L[m][3] = -Ix*x*y-(1+y*y)*Iy;
	L[m][4] = (1+x*x)*Ix + Iy*x*y;
	L[m][5]  = Iy*x-Ix*y;
      }
    }
}

/*!
  initialise the camera from a calibration matrix. 
  Using a calibration matrix leads to a camera without distorsion
  
  The K matrix in parameters must be like:
  
  \f$ K = \left(\begin{array}{ccc}
  p_x & 0 & u_0 \\
  0 & p_y & v_0  \\
  0 & 0 & 1
  \end{array} \right) \f$
  
  \param _K : the 3by3 calibration matrix
*/
void
vpCameraParameters::initFromCalibrationMatrix(const vpMatrix& _K)
{
  if(_K.getRows() != 3 || _K.getCols() != 3 ){
    throw vpException(vpException::dimensionError, "bad size for calibration matrix");
  }
  if( std::fabs(_K[2][2] - 1.0) > std::numeric_limits<double>::epsilon()){
    throw vpException(vpException::badValue, "bad value: K[2][2] must be equal to 1");
  }
  initPersProjWithoutDistortion (_K[0][0], _K[1][1], _K[0][2], _K[1][2]);
}

/*!
  \brief set the value of the interaction matrix.

  \param L_ : The matrix corresponding to the interaction matrix you computed.

  \exception an exception is thrown if the number of row of the interaction
  matrix is different from the dimension of the visual feature as specified
  in the constructor
*/
void
vpGenericFeature::setInteractionMatrix(const vpMatrix &L_)
{
  if (L_.getRows() != dim_s)
  {
    std::cout << L_.getRows() <<"  " << dim_s << std::endl ;;
    vpERROR_TRACE("size mismatch between interaction matrix size "
		"and feature dimension");
    throw(vpFeatureException(vpFeatureException::sizeMismatchError,
			     "size mismatch between interaction matrix size "
			     "and feature dimension"));
  }

  this->L = L_ ;
}

/*!
  Apply a filter to an image.

  \param I : Image to filter
  \param If : Filtered image.
  \param M : Filter coefficients.

*/
void
vpImageFilter::filter(const vpImage<unsigned char> &I,
		      vpImage<double>& If,
		      const vpMatrix& M)
{

  unsigned int size = M.getRows() ;
  unsigned int half_size = size/2 ;

  If.resize(I.getHeight(),I.getWidth()) ;

  If = 0 ;

  for (unsigned int i=half_size ; i < I.getHeight()-half_size ; i++)
  {
    for (unsigned int j=half_size ; j < I.getWidth()-half_size ; j++)
    {
      double   conv_x = 0 ;

      for(unsigned int a = 0 ; a < size ; a++ )
        for(unsigned int b = 0 ; b < size ; b++ )
	{
	  double val =  I[i-half_size+a][j-half_size+b] ;
	  conv_x += M[a][b] * val ;
	}
      If[i][j] = conv_x ;
    }
  }

}

/*!
  Get the robot jacobian expressed in the end-effector frame.

  \warning Re is not the embedded camera frame. It corresponds to the frame
  associated to the tilt axis (see also get_cMe).

  \param q : Articular position for pan and tilt axis.

  \param eJe : Jacobian between end effector frame and end effector frame (on
  tilt axis).

*/
void
vpBiclops::get_eJe(const vpColVector &q, vpMatrix &eJe)
{


  eJe.resize(6,2) ;

  if (q.getRows() != 2) {
    vpERROR_TRACE("Bad dimension for biclops articular vector");
    throw(vpException(vpException::dimensionError, "Bad dimension for biclops articular vector"));
  }

  double s2 = sin(q[1]) ;
  double c2 = cos(q[1]) ;

  eJe = 0;

  if (dh_model_ == DH1)
  {
    eJe[3][0] = -c2;
    eJe[4][1] = 1;
    eJe[5][0] = -s2;
  }
  else
  {
    eJe[3][0] = -c2;
    eJe[4][1] = -1;
    eJe[5][0] = s2;

  }

}

void
vpBiclops::get_fJe(const vpColVector &q, vpMatrix &fJe)
{

  if (q.getRows() != 2) {
    vpERROR_TRACE("Bad dimension for biclops articular vector");
    throw(vpException(vpException::dimensionError, "Bad dimension for biclops articular vector"));
  }

  fJe.resize(6,2) ;

  double s1 = sin(q[0]) ;
  double c1 = cos(q[0]) ;

  fJe = 0;

  if (dh_model_ == DH1)
  {
    fJe[3][1] = -s1;
    fJe[4][1] = c1;
    fJe[5][0] = 1;
  }
  else
  {
    fJe[3][1] = s1;
    fJe[4][1] = -c1;
    fJe[5][0] = 1;
  }
}

/*!
  Apply a filter to an image.

  \param I : Image to filter
  \param Iu : Filtered image along the horizontal axis (u = columns).
  \param Iv : Filtered image along the vertical axis (v = rows).
  \param M : Separate filter coefficients

*/
void
vpImageFilter::filter(const vpImage<double> &I,
		      vpImage<double>& Iu,
		      vpImage<double>& Iv,
		      const vpMatrix& M)
{

  unsigned int size = M.getRows() ;
  unsigned int half_size = size/2 ;

  Iu.resize(I.getHeight(),I.getWidth()) ;
  Iv.resize(I.getHeight(),I.getWidth()) ;

  Iu = 0 ;
  Iv = 0 ;
  for (unsigned int v=half_size ; v < I.getHeight()-half_size ; v++)
  {
    for (unsigned int u=half_size ; u < I.getWidth()-half_size ; u++)
    {
      double   conv_u = 0 ;
      double   conv_v = 0 ;

      for(unsigned int a = 0 ; a < size ; a++ )
        for(unsigned int b = 0 ; b < size ; b++ )
	{
	  double val =  I[v-half_size+a][u-half_size+b] ;
	  conv_u += M[a][b] * val ;
	  conv_v += M[b][a] * val  ;
	}
      Iu[v][u] = conv_u ;
      Iv[v][u] = conv_v ;
    }
  }

}

void KalmanFilter<DataTypes, DepthTypes>::predict(vpMatrix& stiffnessMatrix)
{
	
	predictedForces = estimatedForces;
	predictedPositions = estimatedPositions + stiffnessMatrix.pseudoInverse()*estimatedForces;
	
	
	for (int k = 0; k < 2*predictedForces.getCols(); k++)
	for (int l = 0; l < 2*predictedForces.getCols(); l++)
	std::cout <<k<<" "<<l<<"   "<< stiffnessMatrix[k][l] << std::endl;
	
	estimatedState = estimatedPositions;
	estimatedState.stack(estimatedForces);
	
	predictedState = predictedPositions;
	predictedState.stack(predictedForces);
	
	for (int k = predictedForces.getCols(); k < 2*predictedForces.getCols(); k++)
	for (int l = predictedForces.getCols(); l < 2*predictedForces.getCols(); l++)
		J[k][l] = stiffnessMatrix[k-predictedForces.getCols()][l-predictedForces.getCols()];
		
	PPred = J*PEst*J.transpose() + Q;
    //PfPred = PsEst + Qf;

}

/*!

  Get the inverse jacobian.

  \f[
  {^f}J_e^+ = \left[\begin{array}{cccccc}
  -(a_1s_1+d_3c_1)/(a_1^2+d_3^2) & (a_1c_1-d_3s_1)/(a_1^2+d_3^2) & 0&0&0&0 \\
  0 & 0 & 1 & 0 & 0 & 0  \\
  (a_1s_1+d_3c_1)/(a_1^2+d_3^2) & -(a_1c_1-d_3s_1)/(a_1^2+d_3^2) & 0&0&0&1 \\
  0 & 0 & 0 & c_{14} & s_{14} & 0  \\
  \end{array}
  \right]
  \f]

  \param q : Articular position of the four joints: q[0] corresponds to
  the first rotation (joint 1 with value \f$q_1\f$) of the turret
  around the vertical axis, while q[1] corresponds to the vertical
  translation (joint 2 with value \f$q_2\f$), while q[2] and q[3]
  correspond to the pan and tilt of the camera (respectively joint 4
  and 5 with values \f$q_4\f$ and \f$q_5\f$). Rotations q[0], q[2] and
  q[3] are expressed in radians. The translation q[1] is expressed in
  meters.

  \param fJe_inverse : Inverse robot jacobian expressed in the robot
  reference frame.

  \sa get_eJe() and get_fJe()

*/
void vpAfma4::get_fJe_inverse(const vpColVector &q, vpMatrix &fJe_inverse) const
{
    fJe_inverse.resize(4, 6) ;
    fJe_inverse = 0;

    double q1 = q[0]; // rot touret
    double q4 = q[2]; // pan

    double c1 = cos(q1);
    double s1 = sin(q1);
    double c14 = cos(q1 + q4);
    double s14 = sin(q1 + q4);

    double det = this->_a1 * this->_a1 + this->_d3 * this->_d3;

    fJe_inverse[0][0] = (-s1*this->_a1 - c1*this->_d3)/det;
    fJe_inverse[0][1] = (c1*this->_a1 - s1*this->_d3)/det;

    fJe_inverse[1][2] = fJe_inverse[2][5] = 1.;

    fJe_inverse[2][0] = - fJe_inverse[0][0];
    fJe_inverse[2][1] = - fJe_inverse[0][1];

    fJe_inverse[3][3] = c14;
    fJe_inverse[3][4] = s14;
}

void
vpAfma4::get_fJe(const vpColVector &q, vpMatrix &fJe) const
{
    fJe.resize(6,4) ;

    double q1 = q[0]; // rot touret
    double q4 = q[2]; // pan

    double c1 = cos(q1);
    double s1 = sin(q1);
    double c14 = cos(q1 + q4);
    double s14 = sin(q1 + q4);

    fJe = 0;

    fJe[0][0] = -s1*this->_a1 - c1*this->_d3;

    fJe[1][0] = c1*this->_a1 - s1*this->_d3;

    fJe[2][1] = 1.0;

    fJe[3][3] = c14;

    fJe[4][3] = s14;

    fJe[5][0] = fJe[5][2] = 1.0;
}

/*!
  Reshape the column vector in a matrix.
  \param M : the reshaped matrix.
  \param nrows : number of rows of the matrix.
  \param ncols : number of columns of the matrix.
*/
void vpColVector::reshape(vpMatrix & M,const unsigned int &nrows,const unsigned int &ncols){
  if(dsize!=nrows*ncols) {
    throw(vpException(vpException::dimensionError,
                      "Cannot reshape (%dx1) column vector in (%dx%d) matrix",
                      rowNum, M.getRows(), M.getCols())) ;
  }
  try {
    if ((M.getRows() != nrows) || (M.getCols() != ncols)) M.resize(nrows,ncols);
  }
  catch(...) {
    throw ;
  }

  for(unsigned int j =0; j< ncols; j++)
    for(unsigned int i =0; i< nrows; i++)
      M[i][j]=data[j*nrows+i];
}

/*!
  \relates vpMatrix

  Compute the skew symmetric matrix \f$M\f$ of translation vector \f$t\f$
  (matrice de pre-produit vectoriel).

  \f[ \mbox{if} \quad  {\bf t} =  \left( \begin{array}{c} t_x \\ t_y \\ t_z
  \end{array}\right), \quad \mbox{then} \qquad
  M = \left( \begin{array}{ccc}
  0 & -t_z & t_y \\
  t_z & 0 & -t_x \\
  -t_y & t_x & 0
  \end{array}\right)
  \f]

  \param t : Translation vector in input used to compute the skew symmetric
  matrix M.

  \param M : Skew symmetric matrix of translation vector \f$t\f$.
*/
void
vpTranslationVector::skew(const vpTranslationVector &t,vpMatrix &M)
{
    M.resize(3,3) ;
    M[0][0] = 0 ;     M[0][1] = -t[2] ;  M[0][2] = t[1] ;
    M[1][0] = t[2] ;  M[1][1] = 0 ;      M[1][2] = -t[0] ;
    M[2][0] = -t[1] ; M[2][1] = t[0] ;   M[2][2] = 0 ;
}

vpColVector computeSecondaryTaskManipulability(const vpMatrix &P, vpMatrix &J, std::vector <vpMatrix> &dJ,double & cond)
{
    const unsigned int n = dJ.size();

    vpColVector z(n);

    double alpha = 10;

//    vpMatrix v;
//    vpColVector w;
//    J.svd(w, v);
//    //std::cout << "singular values:/n" << w << std::endl;
//    cond = w[0]/w[5];

//    double sqtr_detJJt = 1.0;

//    for (unsigned int i = 0; i < n-1; i++)
//        sqtr_detJJt *= w[i];
//    std::cout << "sqtr_detJJt:/n" << sqtr_detJJt << std::endl;


//    std::cout << "detJJt" << detJJt << std::endl;
    double detJJt = (J * J.transpose()).det();
     std::cout << "sqtr_detJJMulti" << sqrt(detJJt) << std::endl;

    for (unsigned int i = 0; i < n; i++)
    {
        vpMatrix dJJinv = dJ[i] * J.pseudoInverse();

        std::cout << dJ[i]  << std::endl << dJ[i] << std::endl;

        double trace = 0.0;
        for (unsigned int k = 0; k < dJJinv.getCols(); k++)
            trace += dJJinv[k][k];

        std::cout << "trace" << i << " " << trace << std::endl;

        z[i] = alpha * sqrt(detJJt) * trace;


    }

    return z;

}

// writes a matrix into a single line YAML format
string okExperiment::singleLineYAML(const vpMatrix &M)
{
    stringstream ss;
    unsigned int i,j;

    ss << "[";
    for(i=0;i<M.getRows();++i)
    {
        ss << "[";
        for(j=0;j<M.getCols()-1;++j)
            ss << M[i][j] << ", ";
        ss << M[i][j] << "]";
        if(i != M.getRows()-1)
            ss << ",";
    }
    ss << "]";
    return ss.str();
}

bool test(const std::string &s, const vpMatrix &M, const std::vector<double> &bench)
{
  static unsigned int cpt = 0;
  std::cout << "** Test " << ++cpt << std::endl;
  std::cout << s << "(" << M.getRows() << "," << M.getCols() << ") = \n" << M << std::endl;
  if(bench.size() != M.size()) {
    std::cout << "Test fails: bad size wrt bench" << std::endl;
    return false;
  }
  for (unsigned int i=0; i<M.size(); i++) {
    if (std::fabs(M.data[i]-bench[i]) > std::fabs(M.data[i])*std::numeric_limits<double>::epsilon()) {
      std::cout << "Test fails: bad content" << std::endl;
      return false;
    }
  }

  return true;
}

/*!
  Operator that allows to multiply a velocity twist transformation matrix by a matrix.

  As shown in the example below, this operator can be used to compute the corresponding
  camera velocity skew from the joint velocities knowing the robot jacobian.

  \code
#include <visp3/core/vpConfig.h>
#include <visp3/robot/vpSimulatorCamera.h>
#include <visp3/core/vpColVector.h>
#include <visp3/core/vpVelocityTwistMatrix.h>

int main()
{
  vpSimulatorCamera robot;

  vpColVector q_vel(6); // Joint velocity on the 6 joints
  // ... q_vel need here to be initialized
  
  vpColVector c_v(6); // Velocity in the camera frame: vx,vy,vz,wx,wy,wz 

  vpVelocityTwistMatrix cVe;  // Velocity skew transformation from camera frame to end-effector
  robot.get_cVe(cVe);

  vpMatrix eJe;       // Robot jacobian
  robot.get_eJe(eJe);

  // Compute the velocity in the camera frame
  c_v = cVe * eJe * q_vel;

  return 0;
}
  \endcode

  \exception vpException::dimensionError If M is not a 6 rows dimension matrix.
*/
vpMatrix
vpVelocityTwistMatrix::operator*(const vpMatrix &M) const
{
  if (6 != M.getRows()) {
    throw(vpException(vpException::dimensionError,
                      "Cannot multiply a (6x6) velocity twist matrix by a (%dx%d) matrix",
                      M.getRows(), M.getCols()));
  }

  vpMatrix p(6, M.getCols()) ;
  for (unsigned int i=0;i<6;i++)
    for (unsigned int j=0;j<M.getCols();j++)
      {
	double s =0 ;
	for (unsigned int k=0;k<6;k++)
	  s += rowPtrs[i][k] * M[k][j];
	p[i][j] = s ;
      }
  return p;
}

/*!
  Operator that allows to multiply a skew transformation matrix by a matrix.

  \exception vpMatrixException::incorrectMatrixSizeError If M is not a 6 rows
  dimension matrix.
*/
vpMatrix
vpForceTwistMatrix::operator*(const vpMatrix &M) const
{

  if (6 != M.getRows())
  {
    vpERROR_TRACE("vpForceTwistMatrix mismatch in vpForceTwistMatrix/vpMatrix multiply") ;
    throw(vpMatrixException::incorrectMatrixSizeError) ;
  }

  vpMatrix p(6, M.getCols()) ;
  for (unsigned int i=0;i<6;i++) {
    for (unsigned int j=0;j<M.getCols();j++) {
      double s =0 ;
      for (unsigned int k=0;k<6;k++)
	s += rowPtrs[i][k] * M[k][j];
      p[i][j] = s ;
    }
  }
  return p;
}

/*!

  Multiply a row vector by a matrix.

  \param M : Matrix.

  \warning The number of elements of the row vector must be equal to the number
  of rows of the matrix.

  \exception vpException::dimensionError If the number of elements of the
  row vector is not equal to the number of rows of the matrix.

  \return The resulting row vector.

*/
vpRowVector vpRowVector::operator*(const vpMatrix &M) const
{
  vpRowVector c(M.getCols());

  if (colNum != M.getRows()) {
    throw(vpException(vpException::dimensionError,
                      "Cannot multiply (1x%d) row vector by (%dx%d) matrix",
                      colNum, M.getRows(), M.getCols())) ;
  }

  c = 0.0;

  for (unsigned int i=0;i<colNum;i++) {
    double bi = data[i] ; // optimization em 5/12/2006
    for (unsigned int j=0;j<M.getCols();j++) {
      c[j]+=bi*M[i][j];
    }
  }

  return c ;
}

/*!
  \brief reshape the colvector in a matrix
  \param m : the reshaped Matrix
  \param nrows : number of rows of the matrix
  \param ncols : number of columns of the matrix
*/
void vpColVector::reshape(vpMatrix & m,const unsigned int &nrows,const unsigned int &ncols){
  if(dsize!=nrows*ncols)
  {
    vpERROR_TRACE("\n\t\t vpColVector mismatch size for reshape vpSubColVector in a vpMatrix") ;
    throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
			    "\n\t\t \n\t\t vpColVector mismatch size for reshape vpSubColVector in a vpMatrix")) ;
  }
  try 
  {
    if ((m.getRows() != nrows) || (m.getCols() != ncols)) m.resize(nrows,ncols);
  }
  catch(vpException me)
  {
    vpERROR_TRACE("Error caught") ;
    std::cout << me << std::endl ;
    throw ;
  }

  for(unsigned int j =0; j< ncols; j++)
     for(unsigned int i =0; i< nrows; i++)
	  m[i][j]=data[j*ncols+i];
}