#include "ProbMotionEst.h"

using namespace std;
using namespace rtm;


ProbMotionEst::ProbMotionEst(){
  srand((unsigned) time(NULL));
}


ProbMotionEst::~ProbMotionEst(){
}


void ProbMotionEst::estMotion(const vector<ColumnVector> &pres, const vector<ColumnVector> &curs,
			      const quaternion &dquat, int nrnsc,
			      coords &ans_co, Matrix &ans_Cov){
  int fn = static_cast<int>(curs.size());
  vector<Matrix> SCovs, iSCovs, iSQCovs;
  SCovs.assign(fn, Matrix(3,3,0));
  iSCovs.assign(fn, Matrix(3,3,0));
  iSQCovs.assign(fn, Matrix(3,3,0));


  { /* 誤差の共分散行列 */

    /* 回転誤差 */
    Matrix qcov(4,4,0);
    for(int i=0; i<3; i++){
      qcov(i+1, i+1) = pow(deg2rad(1.0), 2);
    }

    /* ステレオ誤差と回転誤差 */
    Matrix SCov, iSCov,SQCov, iSQCov, J;
    for(int i=0; i<fn; i++){
      SCov = makeStereoErrorCov(pres[i], 90.0, 485.0, 1.0, 1.0, 1.0);
      iSCov = inverseMatrix(SCov);
      J = dpWorldPointbyQuat(curs[i], dquat.to_vector()); /* ここではIMUの値を信じる */
      SQCov = SCov + J * qcov * J.transpose();
      iSQCov = inverseMatrix(SQCov);

      SCovs[i] = SCov;
      iSCovs[i] = iSCov;
      iSQCovs[i] = iSQCov;
    }
  }

  this->estMotion(pres, curs, SCovs, iSCovs, iSQCovs, nrnsc, ans_co, ans_Cov);
}

/*
 * estMotionの入力
 * ３次元フロー（前特徴点，現特徴点）
 * 現特徴点の観測誤差行列，観測誤差逆行列
 * 姿勢角変化
 * LMedsの繰り返し回数．
 */
void ProbMotionEst::estMotion(const vector<ColumnVector> &pres, const vector<ColumnVector> &curs,
			      const vector<Matrix> &SCovs, const vector<Matrix> &iSCovs,
			      const vector<Matrix> &iSQCovs, const int nrnsc,
			      coords &ans_co, Matrix &ans_Cov){
  int fn = static_cast<int>(pres.size());
  if( fn < 4 ) {
    cerr << "features dim is too small : " << fn << endl;
  }

  coords co, best_co;
  double med, best_med;
  Matrix tcov, best_tcov; /* 並進の共分散行列 */
  med = best_med = 1000000.0;


  for(int i=0; i<nrnsc; i++){
    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;
    vector<Matrix> tmp_icwsqs;
    selectContents(pres, slist, tmp_pres);
    selectContents(curs, slist, tmp_curs);
    selectContents(iSQCovs, slist, tmp_icwsqs);

    estMotionOne(tmp_pres, tmp_curs, tmp_icwsqs, co, tcov);
    med = evalEstimation(pres, curs, iSCovs, 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;
      best_tcov = tcov;
    }
  }

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

  ans_co = best_co;
  ans_Cov = best_tcov;
}


/*
 *カメラ座標の回転・並進を返す．
 */
void ProbMotionEst::estMotionOne(const vector<ColumnVector> &pres,
				 const vector<ColumnVector> &curs,
				 const vector<Matrix> &iSQCovs,
				 coords &co, Matrix &Cov){
  Matrix C1(4,4,0), C3(4,4,0);
  double C2 = 0;
  Matrix Qmat, Wmat;
  double vsize; /* メートルで表す */
  int fn = static_cast<int>(pres.size());
  for(int i=0; i<fn; i++){
    Qmat = makeQmat(curs[i]);
    Wmat = makeWmat(pres[i]);
    vsize = 0;
    for(int j=0; j<3; j++) vsize += pow(pres[i](j)/1000.0, 2);

    C1 += (Qmat.transpose() * Wmat) / vsize;
    C2 += 1.0 / vsize;
    C3 += (Wmat - Qmat) / vsize;
  }

  Matrix Amat = (((C3.transpose() * C3) / C2) + C1 + C1.transpose()) / 4.0;
  /* 回転 */
  quaternion rot(getMaxValEigVector(Amat));
  rot.t = -rot.t; /* 観測回転=>自己回転 */
  normalize_quat_sgn(rot);

  /* 並進共分散行列 */
  Matrix iCwSQ(3,3,0);
  for(int i=0; i<fn; i++){
    iCwSQ = iCwSQ + iSQCovs[i];
  }
  Cov = iCwSQ.inverse();

  /* 並進移動量 */
  ColumnVector trans(3,0);
  for(int i=0; i<fn; i++){
    //trans = trans + (Cov * iSQCovs[i] * (rot.inverse_rotate_vector(pres[i]) - curs[i])) ;
    trans = trans + (Cov * iSQCovs[i] * (pres[i] - rot.rotate_vector(curs[i]))); /* debug */
  }
  co = coords(trans, rot);
}


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


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


/******************************
 * Q => qmatp : true
 * W => qmatp : false
 ******************************/
Matrix ProbMotionEst::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 ProbMotionEst::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 ProbMotionEst::evalEstimation(const vector<ColumnVector> pres,
				     const vector<ColumnVector> curs,
				     const vector<Matrix> iCovs, const coords &co){
  int size = static_cast<int>(pres.size());
  list<double> vals;
  vals.clear();

  /* マハラノビス距離のリストの作成 */
  for(int i=0; i<size; i++){
    //vals.push_back(getMahalaDist(curs[i], co.transform_vector(pres[i]), iCovs[i]));
    vals.push_back(getMahalaDist(pres[i], co.transform_vector(curs[i]), iCovs[i]));
  }

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

  return (*it);
}


double ProbMotionEst::getMahalaDist(const ColumnVector &vec1, const ColumnVector &vec2,
				    const Matrix &iCov){
  return ((vec1 - vec2).transpose() * iCov * (vec1 - vec2));
}
