#include "VOUtil.h"

using namespace std;
using namespace rtm;


D3::D3(double init){
  for(int i=0; i<3; i++) vec[i] = init;
}


D3::D3(double x, double y, double z){
  vec[0] = x;
  vec[1] = y;
  vec[2] = z;
}


D3::D3(double v[]){
  for(int i=0; i<3; i++) vec[i] = v[i];
}


D3::D3(const ColumnVector &a){
  for(int i=0; i<3; i++) vec[i] = a(i);
}


D3::~D3(){}


D3 &D3::operator=(const D3 &a){
  for(int i=0; i<3; i++){
    vec[i] = a.vec[i];
  }
  return *this;
}



D3 D3::operator+(const D3 &a) const{
  return D3(vec[0]+a.vec[0],vec[1]+a.vec[1],vec[2]+a.vec[2]);
}


D3 D3::operator-(const D3 &a) const{
  return D3(vec[0]-a.vec[0],vec[1]-a.vec[1],vec[2]-a.vec[2]);
}



namespace rtm{
  D3 operator*(const D3 &a, double b){
    D3 r;
    for(int i=0; i<3; i++) r(i) = b * a(i);
    return r;
  }


  D3 operator*(double b, const D3 &a){
    D3 r;
    for(int i=0; i<3; i++) r(i) = b * a(i);
    return r;
  }

  D3 operator/(const D3 &a, double b){
    D3 r;
    for(int i=0; i<3; i++) r(i) = a(i) / b;
    return r;
  }
}


double D3::norm(){
  return (sqrt (pow(vec[0],2)+pow(vec[1],2)+pow(vec[2],2)));
}


D33::D33(double init){
  for(int i=0; i<3; i++){
    for(int j=0; j<3; j++){
      mat[i][j] = init;
    }}
}


D33::D33(const Matrix &a){
  for(int i=0; i<3; i++){
    for(int j=0; j<3; j++){
      mat[i][j] = a(i,j);
    }}}


D33 &D33::operator=(const D33 &a){
  for(int i=0; i<3; i++){
    for(int j=0; j<3; j++){
      mat[i][j] = a.mat[i][j];
    }}
  return *this;
}


D33 D33::operator+(const D33 &a) const{
  D33 r;
  for(int i=0; i<3; i++){
    for(int j=0; j<3; j++){
      r.mat[i][j] = mat[i][j] + a.mat[i][j];
    }}
  return r;
}


D33 D33::operator-(const D33 &a) const{
  D33 r;
  for(int i=0; i<3; i++){
    for(int j=0; j<3; j++){
      r.mat[i][j] = mat[i][j] - a.mat[i][j];
    }}
  return r;
}

D3 D33::operator*(const D3 &a) const{
  D3 r;
  for(int i=0; i<3; i++){
    r.vec[i] = 0;
    for(int j=0; j<3; j++){
      r.vec[i] += mat[i][j] * a.vec[j];
    }}
  return r;
}


D33 D33::operator*(const D33 &a) const{
  D33 r;
  for(int i=0; i<3; i++){
    for(int j=0; j<3; j++){
      r.mat[i][j] = 0.0;
      for(int k=0; k<3; k++){
	r.mat[i][j] += mat[i][k] * a.mat[j][k];
      }}}
  return r;
}

namespace rtm{
  D33 operator*(const D33 &a, double b){
    D33 r;
    for(int i=0; i<3; i++){
      for(int j=0; j<3; j++){
	r.mat[i][j] = b * a.mat[i][j];
      }}
    return r;
  }


  D33 operator*(double b, const D33 &a){
    D33 r;
    for(int i=0; i<3; i++){
      for(int j=0; j<3; j++){
	r.mat[i][j] = b * a.mat[i][j];
      }}
    return r;
  }
}


D33 D33::transpose() const{
  D33 r;
  for(int i=0; i<3; i++){
    for(int j=0; j<3; j++){
      r.mat[i][j] = mat[j][i];
    }}
  return r;
}


/* octaveだと遅いので3x3だけに関してはベタで記述 */
D33 D33::inverse() const{
  double detA = 0;
  detA += mat[0][0]*mat[1][1]*mat[2][2];
  detA += mat[1][0]*mat[2][1]*mat[0][2];
  detA += mat[2][0]*mat[0][1]*mat[1][2];
  detA -= mat[0][0]*mat[2][1]*mat[1][2];
  detA -= mat[2][0]*mat[1][1]*mat[0][2];
  detA -= mat[1][0]*mat[0][1]*mat[2][2];
  D33 ans;
  ans.mat[0][0] = mat[1][1]*mat[2][2] - mat[1][2]*mat[2][1];
  ans.mat[0][1] = mat[0][2]*mat[2][1] - mat[0][1]*mat[2][2];
  ans.mat[0][2] = mat[0][1]*mat[1][2] - mat[0][2]*mat[1][1];
  ans.mat[1][0] = mat[1][2]*mat[2][0] - mat[1][0]*mat[2][2];
  ans.mat[1][1] = mat[0][0]*mat[2][2] - mat[0][2]*mat[2][0];
  ans.mat[1][2] = mat[0][2]*mat[1][0] - mat[0][0]*mat[1][2];
  ans.mat[2][0] = mat[1][0]*mat[2][1] - mat[1][1]*mat[2][0];
  ans.mat[2][1] = mat[0][1]*mat[2][0] - mat[0][0]*mat[2][1];
  ans.mat[2][2] = mat[0][0]*mat[1][1] - mat[0][1]*mat[1][0];

  for(int i=0; i<3; i++){
    for(int j=0; j<3; j++){
      ans.mat[i][j] /= detA;
    }}

  return ans;
}


std::ostream &operator<<(std::ostream &s, const rtm::D3 &v){
  for(int i=0; i<3; i++){
    s.width(11);
    s << v(i);
  }
  return s;
}


std::ostream &operator<<(std::ostream &s, const rtm::D33 &m){
  for(int i=0; i<3; i++){
    s << "\n";
    for(int j=0; j<3; j++){
      s.width(15);
      s << m(i,j);
    }}

  return s;
}


ColumnVector rtm::D32ColumnVector(const D3 &a){
  ColumnVector r(3,0);
  for(int i=0; i<3; i++) r(i) = a(i);
  return r;
}


Matrix rtm::D332Matrix(const D33 &a){
  Matrix r(3,3,0);
  for(int i=0; i<3; i++){
    for(int j=0; j<3; j++){
      r(i,j) = a(i,j);
    }}
  return r;
}


ColumnVector rtm::makeColumnVectorfromD3(const D3 &a){
  ColumnVector r(3,0);
  for(int i=0; i<3; i++) r(i) = a(i);
  return r;
}


Matrix rtm::makeMatrixfromD33(const D33 &a){
  Matrix r(3,3,0);
  for(int i=0; i<3; i++){
    for(int j=0; j<3; j++){
      r(i,j) = a(i,j);
    }}
  return r;
}


double rtm::getMhlnbsDistance(const D33 &mat, const D3 &vec){
  double tmp[3] = {0.0, 0.0, 0.0};

  for(int i=0; i<3; i++){
    for(int j=0; j<3; j++){
      tmp[i] += vec(j) * mat(j, i);
    }}

  double r = 0.0;
  for(int i=0; i<3; i++){
    r += tmp[i] * vec(i);
  }
  return r;
}


D33 rtm::makeStereoErrorD33(const D3 &lpos,
			    double b, double f, double sgm_u, double sgm_v, double sgm_d){
  double bb = pow(b, 2);
  double ff = pow(f, 2);
  double sgm_uu = pow(sgm_u, 2);
  double sgm_vv = pow(sgm_v, 2);
  double sgm_dd = pow(sgm_d, 2);

  double x = lpos(0);
  double y = lpos(1);
  double z = lpos(2);
  D33 S;

  S(0,0) = sgm_dd * x * x / bb;
  S(1,1) = sgm_uu + sgm_dd * y * y / bb;
  S(2,2) = sgm_vv + sgm_dd * z * z / bb;

  S(0,1) = S(1,0) = sgm_dd * x * y / bb;
  S(0,2) = S(2,0) = sgm_dd * x * z / bb;
  S(1,2) = S(2,1) = sgm_dd * y * z / bb;

  S = (x*x/ff) * S;
  return S;
}


Matrix rtm::inverseMatrix(const Matrix &a){
  if(!(a.rows()==3 && a.columns()==3)){
    cerr << "inverse Matrix Error. illegal size" << endl;
    return Matrix(0,0);
  }

  double detA = 0;
  detA += a(0,0)*a(1,1)*a(2,2);
  detA += a(1,0)*a(2,1)*a(0,2);
  detA += a(2,0)*a(0,1)*a(1,2);
  detA -= a(0,0)*a(2,1)*a(1,2);
  detA -= a(2,0)*a(1,1)*a(0,2);
  detA -= a(1,0)*a(0,1)*a(2,2);

  Matrix r(3,3);
  r(0,0) = a(1,1)*a(2,2) - a(1,2)*a(2,1);
  r(0,1) = a(0,2)*a(2,1) - a(0,1)*a(2,2);
  r(0,2) = a(0,1)*a(1,2) - a(0,2)*a(1,1);
  r(1,0) = a(1,2)*a(2,0) - a(1,0)*a(2,2);
  r(1,1) = a(0,0)*a(2,2) - a(0,2)*a(2,0);
  r(1,2) = a(0,2)*a(1,0) - a(0,0)*a(1,2);
  r(2,0) = a(1,0)*a(2,1) - a(1,1)*a(2,0);
  r(2,1) = a(0,1)*a(2,0) - a(0,0)*a(2,1);
  r(2,2) = a(0,0)*a(1,1) - a(0,1)*a(1,0);

  for(int i=0; i<3; i++){
    for(int j=0; j<3; j++){
      r(i,j) /= detA;
    }}

  return r;
}


/*
 * ランダムのリストを生成する(多分遅い)
 */
void rtm::getRandamList(int srcn, int dstn, list<int> &result){
  if(srcn < dstn){
    cerr << "@getRandamList Error. srcn is smaller than dstn" << endl;
  }
  else {
    result.clear();
    for(int i=0; i<srcn; i++) result.push_back(i);
    int resultn = srcn;
    for( ; resultn!=dstn; resultn--){
      int tmp = rand()%resultn;
      list<int>::iterator it = result.begin();
      for(int i=0; i<tmp; i++) it++;
      result.erase(it);
    }}}


/* ３次方程式（必ず解がある） */
double rtm::calc3DEquation(double a, double b, double c, double d, int N, double x0){
  double F,Df;
  double eps = 1.e-10;
  double x = x0;
  double xold = x0;
  for (int k=0;k<=N;k++) {
    F=a*pow(xold,3)+b*pow(xold,2)+c*xold+d;  /* ３次方程式 */
    Df=3*a*pow(xold,2)+2*b*xold+c;  /* Df : F を微分したもの */
    if (fabs(Df)<eps) {  /* <math.h>の中にあるdouble型絶対値を返す関数 */
      x=xold;      /* ３次式の微分値が極微になったら x とする */
      break;
    }
    else {
      x=xold-F/Df;  /* これはよく分からん */
      if (fabs(x-xold)<eps) break;
      else xold=x;
    }
  }
  F = a*pow(x,3)+b*pow(x,2)+c*x+d;  /* ax^3+bx^2+cx+d=0 の確認 */
  return x;
}

ColumnVector rtm::getMaxValEigVector(Matrix& mat){
  EIG eig(mat);
  double max_value=0;
  int max_num = 0;

  ColumnVector evals = real(eig.eigenvalues());
  int vdim = evals.rows();

  for(int i=0; i<vdim; i++){
    if( max_value < evals(i) ) {
      max_value = evals(i);
      max_num = i;
    }
  }

  Matrix evecs = real(eig.eigenvectors());
  ColumnVector vec = evecs.column(max_num);
  return vec;
}


// template<class T> T rtm::getAverage(vector<T> srcs){
//   int n = static_cast<int>(srcs.size());
//   T ans = srcs[0];
//   for(int i=1; i<n; i++){
//     ans = ans + srcs[i];
//   }
//   ans = ans / n;
//   return ans;
// }


/* 3x3専用 */
bool calcInverseMatrix(double *src, double *dst){
  double detA = 0;
  detA += (src[0]*src[4]*src[8] + src[3]*src[7]*src[2] + src[6]*src[1]*src[5]);
  detA -= (src[0]*src[7]*src[5] + src[6]*src[4]*src[2] + src[3]*src[1]*src[8]);
  //printf("%f\n", detA);
  if(abs(detA) < 0.0001){
    return false;
  }

  dst[0] = src[4]*src[8] - src[5]*src[7];
  dst[1] = src[2]*src[7] - src[1]*src[8];
  dst[2] = src[1]*src[5] - src[2]*src[4];
  dst[3] = src[5]*src[6] - src[3]*src[8];
  dst[4] = src[0]*src[8] - src[2]*src[6];
  dst[5] = src[2]*src[3] - src[0]*src[5];
  dst[6] = src[3]*src[7] - src[4]*src[6];
  dst[7] = src[1]*src[6] - src[0]*src[7];
  dst[8] = src[0]*src[4] - src[1]*src[3];

  for(int i=0; i<9; i++) dst[i] /= detA;

  return true;
}






/********************
 *
 * 偏微分（ヤコビアン）
 *
 ********************/
Matrix rtm::dpWorldPointbyQuat(const ColumnVector &lpos, const ColumnVector &quat){
  double x = lpos(0);
  double y = lpos(1);
  double z = lpos(2);
  double q0 = quat(0);
  double q1 = quat(1);
  double q2 = quat(2);
  double q3 = quat(3);

  Matrix J(3,4,0);
  // J(0,0) =  q0 * x - q3 * y + q2 * z;
  // J(0,1) =  q1 * x + q2 * y + q3 * z;
  // J(0,2) = -q2 * x + q1 * y + q0 * z;
  // J(0,3) = -q3 * x - q0 * y + q1 * z;

  // J(1,0) =  q3 * x + q0 * y - q1 * z;
  // J(1,1) =  q2 * x - q1 * y - q0 * z;
  // J(1,2) =  q1 * x + q2 * y + q3 * z;
  // J(1,3) =  q0 * x - q3 * y + q2 * z;

  // J(2,0) = -q2 * x + q1 * y + q0 * z;
  // J(2,1) =  q3 * x + q0 * y - q1 * z;
  // J(2,2) = -q0 * x + q3 * y - q2 * z;
  // J(2,3) =  q1 * x + q2 * y + q3 * z;
  J(0,0) = -q3*y + q2*z;
  J(0,1) =  q2*y + q3*z;
  J(0,2) = -q2*x + q1*y + q0*z;
  J(0,3) = -q3*x - q0*y + q1*z;
  J(1,0) =  q3*x - q1*z;
  J(1,1) =  q2*x - q1*y - q0*z;
  J(1,2) =  q1*x + q3*z;
  J(1,3) =  q0*x - q3*y + q2*z;
  J(2,0) = -q2*x + q1*y;
  J(2,1) =  q3*x + q0*y - q1*z;
  J(2,2) = -q0*x + q3*y - q2*z;
  J(2,3) =  q1*x + q2*y;
  J = 2 * J;

  return J;
}


Matrix rtm::dpLocalPointbyCoords(const ColumnVector &wpos, const ColumnVector &co){
  double q0 = co(3);
  double q1 = co(4);
  double q2 = co(5);
  double q3 = co(6);

  Matrix H(3,13,0);

  /* xyzで偏微分 */
  H(0, 0) = -(q0*q0 + q1*q1 - q2*q2 - q3*q3);
  H(0, 1) = -2*(q1*q2 + q0*q3);
  H(0, 2) = -2*(q1*q3 - q0*q2);

  H(1, 0) = -2*(q1*q2 - q0*q3);
  H(1, 1) = -(q0*q0 - q1*q1 + q2*q2 - q3*q3);
  H(1, 2) = -2*(q2*q3 + q0*q1);

  H(2, 0) = -2*(q1*q3 + q0*q2);
  H(2, 1) = -2*(q2*q3 - q0*q1);
  H(2, 2) = -(q0*q0 - q1*q1 - q2*q2 + q3*q3);

  H.insert(dpLocalPointbyQuat(wpos, co), 0, 3);
  return H;
}



Matrix rtm::dpLocalPointbyQuat(const ColumnVector &wpos, const quaternion &quat){
  coords tmpco;
  tmpco.quat = quat;
  return (dpLocalPointbyQuat(wpos, tmpco.to_vector()));
}


Matrix rtm::dpLocalPointbyQuat(const ColumnVector &wpos, const ColumnVector &co){
  double dx = wpos(0) - co(0);
  double dy = wpos(1) - co(1);
  double dz = wpos(2) - co(2);
  double q0 = co(3);
  double q1 = co(4);
  double q2 = co(5);
  double q3 = co(6);


  Matrix H(3,4,0);

  /* px成分 */
  H(0, 0) =  q0*dx + q3*dy - q2*dz;
  H(0, 1) =  q1*dx + q2*dy + q3*dz;
  H(0, 2) = -q2*dx + q1*dy - q0*dz;
  H(0, 3) = -q3*dx + q0*dy + q1*dz;

  /* py成分 */
  H(1, 0) = -q3*dx + q0*dy + q1*dz;
  H(1, 1) =  q2*dx - q1*dy + q0*dz;
  H(1, 2) =  q1*dx + q2*dy + q3*dz;
  H(1, 3) = -q0*dx - q3*dy + q2*dz;

  /* pz成分 */
  H(2, 0) =  q2*dx - q1*dy + q0*dz;
  H(2, 1) =  q3*dx - q0*dy - q1*dz;
  H(2, 2) =  q0*dx + q3*dy - q2*dz;
  H(2, 3) =  q1*dx + q2*dy + q3*dz;

  H = 2 * H;

  return H;
}


Matrix rtm::dpLocalPointbyWorldPoint(const ColumnVector &wpos, const ColumnVector &co){
  double q0, q1, q2, q3;
  q0 = co(3); q1 = co(4); q2 = co(5); q3 = co(6);

  Matrix H(3, 3);
  /*
   * px py pzで偏微分
   * xyzでの偏微分の+-逆になる．
   */
  H(0, 0) = (q0*q0 + q1*q1 - q2*q2 - q3*q3);
  H(0, 1) = 2*(q1*q2 + q0*q3);
  H(0, 2) = 2*(q1*q3 - q0*q2);

  H(1, 0) = 2*(q1*q2 - q0*q3);
  H(1, 1) = (q0*q0 - q1*q1 + q2*q2 - q3*q3);
  H(1, 2) = 2*(q2*q3 + q0*q1);

  H(2, 0) = 2*(q1*q3 + q0*q2);
  H(2, 1) = 2*(q2*q3 - q0*q1);
  H(2, 2) = (q0*q0 - q1*q1 - q2*q2 + q3*q3);

  return H;
}


Matrix rtm::dpQuatbyAVel(const ColumnVector &quat, const double &dt){
  Matrix V(4,3,0);
  double q0, q1, q2, q3;
  q0 = quat(0); q1 = quat(1); q2 = quat(2); q3 = quat(3);

  V(0,0) = -q1;
  V(0,1) = -q2;
  V(0,2) = -q3;

  V(1,0) =  q0;
  V(1,1) = -q3;
  V(1,2) =  q2;

  V(2,0) =  q3;
  V(2,1) =  q0;
  V(2,2) = -q1;

  V(3,0) = -q2;
  V(3,1) =  q1;
  V(3,2) =  q0;

  V = (dt/2) * V;

  return V;
}


Matrix rtm::makeDQuatCov(const quaternion &dquat){
  const double kM = 0.1;
  Matrix cov(4,4,0);
  cov(0,0) = 0;
  cov(1,1) = kM * pow(dquat.V(0),2) + pow(0.005, 2);
  cov(2,2) = kM * pow(dquat.V(1),2) + pow(0.005, 2);
  cov(3,3) = kM * pow(dquat.V(2),2) + pow(0.005, 2);
  return cov;
}


Matrix rtm::makeAVelCov(const ColumnVector &avel){
  const double kM = 0.1;
  Matrix Cov(3,3,0);

  for(int i=0; i<3; i++)
    Cov(i,i) = kM * pow(avel(i),2) + pow(deg2rad(1.0),2);
  return Cov;
}

ColumnVector rtm::dquat2avel(const quaternion &dquat, const double &dt){
  return (2 * dquat.V / dt);
}


ColumnVector rtm::dquat2avel(const ColumnVector &dquat, const double &dt){
  return (2 * dquat.extract(1,3) / dt);
}


quaternion rtm::avel2dquat(const ColumnVector &avel, const double &dt){
  quaternion tmp;
  tmp.t = 1;
  tmp.V = (dt / 2) * avel;
  tmp.normalize();
  return tmp;
}


quaternion rtm::getDQuat(const quaternion &pre_quat, const quaternion &cur_quat){
  quaternion r;
  r = pre_quat;
  r.t = -r.t;
  r = r * cur_quat;
  normalize_quat(r);
  normalize_quat_sgn(r);
  return r;
}
