#include "DualQuaternionEst.h"

using namespace std;
using namespace rtm;


DualQuaternionEst::DualQuaternionEst(){
  srand((unsigned) time(NULL));
  cffc_mode_ = 2;
}


DualQuaternionEst::~DualQuaternionEst(){
}


coords
DualQuaternionEst::estMotion(vector<ColumnVector> &pres, vector<ColumnVector> &curs, int nrnsc){
  utimer_.start();
  int fn = static_cast<int>(pres.size());
  if( fn < 4 ) {
    cerr << "features dim is too small : " << fn << endl;
    coords tmp;
    return tmp;
  }
  vector<Matrix> covs;
  makeStereoCovsfromPos(pres, covs);

  coords co, best_co;
  double med, best_med;
  med = best_med = 1000000.0;


  for(int i=0; i<nrnsc; i++){
    //cout << "@debug tm1 : " << utimer_.tick() *1000.0 << endl;

    int selectn = 4 + rand()%( (fn - 2) / 2 );
    //int selectn = (int) pres.size();/* debug */

    list<int> slist;
    getRandamList(fn, selectn, slist);

    vector<ColumnVector> tmp_pres, tmp_curs;
    selectFeatures(pres, slist, tmp_pres);
    selectFeatures(curs, slist, tmp_curs);
    co = estMotionOne(tmp_pres, tmp_curs);
    med = evalEstimation(pres, curs, covs, co);

    // { /* debug */
    //   cerr << "@debug " << i << " th estimation" << endl;
    //   cerr << "co  = " << co << endl;
    //   cerr << "med = " << med << endl;
    // }

    if(med < best_med){
      best_co = co;
      best_med = med;
    }
  }

  // { /* debug */
  //   cerr << "@debug best estimation" << endl;
  //   cerr << "co  = " << best_co << endl;
  //   cerr << "med = " << best_med << endl;
  // }


  /* 特徴点群の運動を自己運動に逆変換 */
  coords ans;
  ans.reset();
  ans.invrotate(best_co.quat, false);
  ans.translate(-best_co.worldpos, false);
  return ans;
}


/*
 * slistに入っている順番の値を持ってくる．
 */
void DualQuaternionEst::selectFeatures(const vector<ColumnVector> &src,
				       list<int> slist, vector<ColumnVector> &dst){
  /* 目標の初期化 */
  dst.clear();
  dst.assign(slist.size(), ColumnVector(3,0));

  list<int>::iterator it = slist.begin();
  for(int idx =0; it!=slist.end(); it++, idx++){
    dst[idx] = src[*it];
  }
}


coords DualQuaternionEst::estMotionOne(vector<ColumnVector> &pres, vector<ColumnVector> &curs){
  Matrix C1(4,4,0), C3(4,4,0);
  double C2 = 0;
  double vsize; /* メートルで表す */
  int fn = static_cast<int>(pres.size());
  // cout << "@debug fn = " << fn << endl; /* deubg */
  // utimer_.tick(); /* debug */
  for(int i=0; i<fn; i++){
    Matrix Qmat = makeQmat(curs[i]);
    Matrix Wmat = makeWmat(pres[i]);
    vsize = 0;
    switch( cffc_mode_ ){
    case 1:
      vsize = pres[i].norm(2.0)/1000.0;
      break;
    case 2:
      for(int j=0; j<3; j++) vsize += pow(pres[i](j)/1000.0, 2);
      break;
    default:
      vsize = 1.0;
      break;
    }

    C1 += (Qmat.transpose() * Wmat) / vsize;
    C2 += 1.0 / vsize;
    C3 += (Wmat - Qmat) / vsize;
  }
  // cout << "@debug tm11 = " << utimer_.tick() * 1000.0 << endl; /* debug */

  Matrix Amat = (((C3.transpose() * C3) / C2) + C1 + C1.transpose()) / 4.0;
  ColumnVector rot = getMaxValEigVector(Amat);
  ColumnVector trans = - ( makeWmat(rot).transpose() * C3 * rot / C2).extract(1,3);
  // cout << "@debug tm12 = " << utimer_.tick() * 1000.0<< endl; /* debug */
  return coords(trans, rot);
}


Matrix DualQuaternionEst::makeQmat(const ColumnVector& vec){
  return (this->makeWQmat(vec, true));
}


Matrix DualQuaternionEst::makeWmat(const ColumnVector& vec){
  return (this->makeWQmat(vec, false));
}


/*
 * Q => qmatp : true
 * W => qmatp : false
 */
Matrix DualQuaternionEst::makeWQmat(const ColumnVector& vec, bool qmatp){
  Matrix ans(4,4,0);
  double pom = -1.0;
  if(qmatp) pom = 1.0;

  /*
   * Dual Quternion 形式から作る
   */
  if(vec.rows()==4){
    ans = Matrix(vec);
    Matrix tmp1(-(vec.extract(1,3)).transpose());
    Matrix tmp2 = pom * makeSkewMatrix(vec.extract(1,3));
    tmp2 += Matrix(DiagMatrix(3,3,vec(0)));
    tmp1 = tmp1.stack(tmp2);
    ans = ans.append(tmp1);
  }
  /*
   * ３次元点から直接作る（論文の２倍の値）
   */
  else if(vec.rows()==3){
    // r0 = 0
    for(int i=0; i<3; i++) ans(0,i+1) = -vec(i);
    ans(1,2) = -pom*vec(2);
    ans(1,3) =  pom*vec(1);
    ans(2,3) = -pom*vec(0);

    //+-逆の対称
    for(int i=0; i<3; i++) ans(i+1,0) = vec(i);
    ans(2,1) =  pom*vec(2);
    ans(3,1) = -pom*vec(1);
    ans(3,2) =  pom*vec(0);
  }

  else {
    printf("can't make WQmat. vector size is not good\n");
  }

  return ans;
}


Matrix DualQuaternionEst::makeSkewMatrix(const ColumnVector& vec){
  if(vec.rows()!=3){
    cout << "Error vector is not 3 dim. can't make skew mat" << endl;
    return Matrix(3,3,0);
  }
  Matrix skew(3,3,0);
  skew(0,1) = -vec(2);
  skew(0,2) =  vec(1);
  skew(1,2) = -vec(0);

  skew(1,0) = -skew(0,1);
  skew(2,0) = -skew(0,2);
  skew(2,1) = -skew(1,2);
  return skew;
}


/******************************
 * マハラノビス距離のメディアン値を取得
 ******************************/
double DualQuaternionEst::evalEstimation(const vector<ColumnVector> pres,
					 const vector<ColumnVector> curs,
					 const vector<Matrix> covs, const coords &co){
  int size = static_cast<int>(pres.size());
  list<double> vals;
  vals.clear();

  utimer_.tick();
  for(int i=0; i<size; i++){
    /* 推定運動は自己ではなく特徴点群なことに注意 */
    ColumnVector tmp = co.transform_vector(pres[i]) - curs[i];
    //ColumnVector tmp = co.inverse_transform_vector(pres[i]) - curs[i];
    double tmp2 = tmp.norm(2.0);
    vals.push_back(tmp2);
  }

  /* ソートして真ん中の値を取得 */
  vals.sort(greater<double>());
  list<double>::iterator it = vals.begin();
  for(int i=0; i<size/2; i++) it++;

  return (*it);
}


void DualQuaternionEst::makeStereoCovsfromPos(const vector<ColumnVector> &points,
					      vector<Matrix> &covs){
  int fn = static_cast<int>(points.size());
  covs.clear();
  covs.assign(fn, Matrix(3,3,0));

  for(int i=0; i<fn; i++){
    covs[i] = makeStereoCovfromPos(points[i]);
  }
}


/*
 * ステレオで得られた３次元点から共分散行列を取得する
 */
Matrix DualQuaternionEst::makeStereoCovfromPos(const ColumnVector &point){
  const static double uu = pow( 1.0, 2 );
  const static double vv = pow( 1.0, 2 );
  const static double dd = pow( 1.0, 2 );
  const static double bb = pow( 90.0, 2 ) ;  /* stereoのbase line */
  const static double f = 485.0; /* カメラの焦点距離 (だいたいで良い) */

  double x = point(0);
  double y = point(1);
  double z = point(2);

  Matrix cov(3,3,0);
  cov(0,0) = uu + dd * x * x / bb;
  cov(1,1) = vv + dd * y * y / bb;
  cov(2,2) = dd * z * z / bb;

  cov(0,1) = dd * x * y / bb;
  cov(0,2) = dd * x * z / bb;
  cov(1,2) = dd * y * z / bb;

  cov(1,0) = cov(0,1);
  cov(2,0) = cov(0,2);
  cov(2,1) = cov(1,2);

  cov = (z*z / f*f) * cov;

  return cov;
}
