/******************************
 *           FastSLAM.cpp
 * FastSLAMの実装(EKFCoordsからコピー)
 * 'FastSLAM 2.0
 * An Improved Particle Filtering Algorithm for Simultaneous Localization and Mapping that...'
 * を元に実装，
 * 変数名は論文に準拠(EKFCoords)と全然違うので注意
 * todo
 * パーティクルは未実装(尤度=1)
 * 対数クォータニオンも未実装
 *
 * 特徴点に関する情報 Gt, Gs, Qt, Pt(0なので無視), Sigma,mew, は外部でもつ.
 ******************************/


#include "FastSLAM.h"

using namespace std;
using namespace rtm;

FastSLAM::FastSLAM(){
}


FastSLAM::FastSLAM(double dt){
  this->init(dt);
}


FastSLAM::~FastSLAM(){
}


void FastSLAM::init(double dt){
  dt_ = dt;

  /* 座標系 */
  X_ = ColumnVector(13,0);
  X_(3) = 1.0;
  estco_.init(X_.extract_n(0,7));

  /* 共分散行列 */
  S_ = Matrix(13,13,0);

  fn_ = 0;
  sdim_ = 13;

  utimer.start();
}


/*
 * 新規特徴点の世界座標系の期待値と共分散行列を返す．
 */
void FastSLAM::makeNewWorldPoint(const ColumnVector &lpos,
				 ColumnVector &wpos, Matrix &Sigma){
  wpos = estco_.transform_vector(lpos);
  Sigma = S_.extract_n(0,0,3,3) + makeWorldPointCovfromStereo(lpos) + makePointCovfromQuat(lpos);
}


/*
 * 特徴点を世界座標系にバインドする時の共分散行列
 * local座標系での観測値をrotateしてからStereo誤差の共分散行列を算出する．
 * なので入力はローカル座標系での3次元位置
 */
Matrix FastSLAM::makeWorldPointCovfromStereo(const ColumnVector &lpos){
  return (makeStereoErrorCov(estco_.quat.rotate_vector(lpos)));
}


/*
 * ステレオ誤差の共分散行列の算出(注：ロボット座標系)
 */
Matrix FastSLAM::makeStereoErrorCov(const ColumnVector &lpos){
  const static double bb = 90.0 * 90.0;
  const static double ff = 485 * 485;
  const static double sgm_uu = 0.5 * 0.5;
  const static double sgm_vv = 0.5 * 0.5;
  const static double sgm_dd = 0.5 * 0.5;

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

  Matrix S(3,3,0);
  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) = sgm_dd * x * y / bb;
  S(0,2) = sgm_dd * x * z / bb;
  S(1,2) = sgm_dd * y * z / bb;

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

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

  return S;
}


/*
 * 姿勢の誤差が点を世界座標系にバインドする時に与える誤差を算出
 * yawを絶対値的に得る方法がない限り無限大になってしまうので，適当な値でごまかす．
 * 入力はローカル座標系での位置
 * todo 角速度に応じて変更したい 姿勢角を角速度で偏微分したものでできるはず．
 */
Matrix FastSLAM::makePointCovfromQuat(const ColumnVector &lpos){
  /* 姿勢角の共分散が与える影響 */
  const double sigma_qq = 0.1 * 0.1;
  Matrix J = dpWorldPointbyQuat(lpos, X_.extract_n(3,4));
  Matrix S = J * Matrix(DiagMatrix(4,4,sigma_qq)) * J.transpose();
  return S;
}


/********************
 *
 * 観測に基づく更新
 * 必要なもの
 * Zh : 値の予測値(h関数)
 * H  : h関数のヤコビアン
 * Q  : 観測誤差の共分散行列
 *      (誤差モデルをFastSLAM側で保持するのは不可能なので，外からセットする)
 *
 ********************/

/* 外部からQをセットする */
void FastSLAM::setQ(const Matrix &Q){
  Q_ = Q;
}


/*
 * 位置の観測
 * type = 0 : x,  type = 1 : y,  type = 2 : z,  type = 3 : xyz
 */
ColumnVector FastSLAM::estZofCoords(){
  Zh_ = X_.extract_n(0,7);
  return Zh_;
}


void FastSLAM::setHofCoords(){
  H_ = Matrix(7,13,0);
  for(int i=0; i<7; i++){
    H_(i,i) = 1.0;
  }}



/******************************
 * 世界座標系での位置 type = x:0 y:1 z:2 xyz:3
 ******************************/
ColumnVector FastSLAM::estZofPos(int type){
  ColumnVector Zh;
  if(type == 3) Zh = X_.extract_n(0,3);
  else Zh = X_.extract_n(type, 1);
  Zh_ = Zh;
  return Zh_;
}


void FastSLAM::setHofPos(int type){
  int dim = 1;
  if(type == 3) dim = 3;

  Matrix H(dim, sdim_, 0);
  if(dim==3){
    H.insert(make_unit_matrix(3),0,0);
  }
  else{
    H(0, type) = 1;
  }
  H_ = H;
}


/******************************
 * 姿勢の観測
 * todo roll pitch yawなどをとれるようにする
 ******************************/
ColumnVector FastSLAM::estZofQuat(){
  Zh_ = X_.extract_n(3, 4);
  return Zh_;
}

void FastSLAM::setHofQuat(){
  Matrix H(4, sdim_, 0);
  for(int i=0; i<4; i++){
    H(i, i+3) = 1.0;
  }
  H_ = H;
}


/******************************
 * 鉛直方向の観測(roll pitch)
 ******************************/
ColumnVector FastSLAM::estZofVertical(){
  ColumnVector vec(3,0);
  vec(2) = 1;
  Zh_ = estco_.quat.inverse_rotate_vector(vec);
  return Zh_;
}

void FastSLAM::setHofVertical(){
  double x = 0;
  double y = 0;
  double z = 1;
  double q0 = X_(3);
  double q1 = X_(4);
  double q2 = X_(5);
  double q3 = X_(6);

  Matrix H(3, sdim_, 0);
  H(0,3) =  2*( q0*x + q3*y - q2*z);
  H(0,4) =  2*( q1*x + q2*y + q3*z);
  H(0,5) =  2*(-q2*x + q1*y - q0*z);
  H(0,6) =  2*(-q3*x + q0*y + q1*z);

  H(1,3) =  2*(-q3*x + q0*y + q1*z);
  H(1,4) =  2*( q2*x - q1*y + q0*z);
  H(1,5) =  2*( q1*x + q2*y + q3*z);
  H(1,6) =  2*(-q0*x - q3*y + q2*z);

  H(2,3) =  2*( q2*x - q1*y + q0*z);
  H(2,4) =  2*( q3*x - q0*y - q1*z);
  H(2,5) =  2*( q0*x + q3*y - q2*z);
  H(2,6) =  2*( q1*x + q2*y + q3*z);
  H_ = H;
}



/******************************
 * 速度の観測 (ワールド座標系)
 ******************************/
ColumnVector FastSLAM::estZofVel(){
  Zh_ = X_.extract_n(7, 3);
  return Zh_;
}


void FastSLAM::setHofVel(){
  Matrix H(3, sdim_, 0);
  for(int i=0; i<3; i++) H(i,i+7) = 1.0;
  H_ = H;
}


/******************************
 * 速度の観測(ローカル座標系)
 ******************************/
ColumnVector FastSLAM::estZofLVel(){
  quaternion quat(X_.extract_n(3,4));
  Zh_ = quat.inverse_rotate_vector(X_.extract_n(7,3));
  return Zh_;
}


void FastSLAM::setHofLVel(){
  Matrix H(3, sdim_, 0);
  quaternion quat(X_.extract_n(3,4));
  H.insert(dpLocalPointbyQuat(X_.extract_n(7,3), quat), 0, 3);
  quat.V = -quat.V;
  H.insert(quat2matrix(quat),0,7);
  H_ = H;
}


/******************************
 * 角速度の観測
 * todo wz wy wzを別々にもとれるようにしたい．
 ******************************/
ColumnVector FastSLAM::estZofAVel(){
  Zh_ = X_.extract_n(10, 3);
  return Zh_;
}


void FastSLAM::setHofAVel(){
  Matrix H(3, sdim_, 0);
  for(int i=0; i<3; i++) H(i,i+10) = 1.0;
  H_ = H;
}


/******************************
 * EKFの更新(1周期に複数回呼ばれる)
 * == 前にしなければいけないこと ==
 * this->estZof.. ot this->setZh
 * this->setH or this->setHof..
 * this->setQ or this->setQof..
 ******************************/
void FastSLAM::update(const ColumnVector &Z){
  K_ = S_ * H_.transpose() * ( H_ * S_ * H_.transpose() + Q_ ).inverse();
  X_ = X_ + K_ * ( Z - Zh_);
  S_ = (make_unit_matrix(sdim_) - K_*H_)*S_;

  /* coordsへの登録と４元数の正規化 */
  estco_.init(X_.extract_n(0,7));
  estco_.quat.normalize();
  X_.insert(estco_.quat.to_vector(), 3);
}



void FastSLAM::updatebyPoints(const vector<ColumnVector> &lpos,
			      vector<ColumnVector> &wpos, vector<Matrix> &WPCov){
  int fn = static_cast<int>(lpos.size());
  vector<Matrix> H_ps;
  vector<Matrix> Ss;
  vector<Matrix> invN;


  Matrix H_c;
  ColumnVector Z;
  Matrix N;
  Matrix Imat = Matrix(DiagMatrix(13,13,1));

  H_ps.assign(fn ,Matrix(3,3,0));
  Ss.assign(fn, Matrix(3,3,0));
  invN.assign(fn, Matrix(3,3,0));

  utimer.tick();
  /* カメラ座標系の更新 */
  for(int i=0; i<fn; i++){
    H_c = dpLocalPointbyCoords(wpos[i], X_.extract_n(0,7));
    H_ps[i] = dpLocalPointbyWorldPoint(wpos[i], X_.extract_n(0,7));
    N = makeStereoErrorCov(lpos[i])
      + H_ps[i] * WPCov[i] * H_ps[i].transpose()
      + H_c * S_ * H_c.transpose();
    invN[i] = N.inverse();
    K_ = S_ * H_c.transpose() * invN[i];

    X_ = X_ + K_*(lpos[i] - estco_.inverse_transform_vector(wpos[i]));
    estco_.init(X_.extract_n(0,7));
    S_ = (Imat - K_ * H_c) * S_;
  }

  /* 特徴点の更新 */
  Matrix K; /* カルマンゲイン */
  Imat = Matrix(DiagMatrix(3,3,1));
  for(int i=0; i<fn; i++){
    // H_c = dpLocalPointbyCoords(wpos[i], X_.extract_n(0,7));
    // N = makeStereoErrorCov(lpos[i])
    //   + H_ps[i] * WPCov[i] * H_ps[i].transpose()
    //   + H_c * S_ * H_c.transpose();
    K = WPCov[i] * H_ps[i].transpose() * invN[i];
    //K = WPCov[i] * H_ps[i].transpose() * N.inverse();
    wpos[i] = wpos[i] + K * (lpos[i]-estco_.inverse_transform_vector(wpos[i]));
    WPCov[i] = (Imat - K*H_ps[i]) * WPCov[i];
  }

  H_ps.clear();
  Ss.clear();
}


// void FastSLAM::updatebyPoints(const vector<ColumnVector> &lpos,
// 			      vector<ColumnVector> &wpos, vector<Matrix> &WPCov){
//   int fn = static_cast<int>(lpos.size());
//   vector<Matrix> invNs;
//   vector<Matrix> H_ps;
//   vector<Matrix> Ss;


//   Matrix H_c;
//   ColumnVector Z;
//   Matrix N;


//   invNs.assign(fn, Matrix(3,3,0));
//   H_ps.assign(fn ,Matrix(3,3,0));
//   Ss.assign(fn, Matrix(3,3,0));


//   /* カメラ座標系の更新 */
//   for(int i=0; i<fn; i++){
//     utimer.tick();
//     H_c = dpLocalPointbyCoords(wpos[i], X_.extract_n(0,7));
//     cout << "@debug tm00 : " << utimer.tick() * 1000 << endl;
//     H_ps[i] = dpLocalPointbyWorldPoint(wpos[i], X_.extract_n(0,7));
//     cout << "@debug tm01 : " << utimer.tick() * 1000 << endl;
//     N = makeStereoErrorCov(lpos[i]) + H_ps[i] * WPCov[i] * H_ps[i].transpose();
//     cout << "@debug tm02 : " << utimer.tick() * 1000 << endl;
//     invNs[i]= N.inverse();
//     cout << "@debug tm03 : " << utimer.tick() * 1000 << endl;

//     /* 観測誤差 */
//     Matrix Q = (H_c.transpose() * invNs[i] * H_c + invR_).inverse();
//     setQ(Q);
//     cout << "@debug tm04 : " << utimer.tick() * 1000 << endl;
//     /* 観測値 */
//     Z = X_.extract_n(0,7) + (Q * H_c.transpose() * invNs[i])
//       *(lpos[i] - estco_.inverse_transform_vector(wpos[i]));

//     cout << "@debug tm05 : " << utimer.tick() * 1000 << endl;
//     estZofCoords();
//     cout << "@debug tm06 : " << utimer.tick() * 1000 << endl;
//     setHofCoords();
//     cout << "@debug tm07 : " << utimer.tick() * 1000 << endl;

//     update(Z);
//     cout << "@debug tm08 : " << utimer.tick() * 1000 << endl;
//   }

//   /* 特徴点の更新 */
//   Matrix K; /* カルマンゲイン */
//   Matrix Imat = Matrix(DiagMatrix(3,3,1));
//   for(int i=0; i<fn; i++){
//     K = WPCov[i] * H_ps[i].transpose() * invNs[i];
//     wpos[i] = wpos[i] + K * (lpos[i]-estco_.inverse_transform_vector(wpos[i]));
//     WPCov[i] = (Imat - K*H_ps[i]) * WPCov[i];
//   }

//   invNs.clear();
//   H_ps.clear();
//   Ss.clear();
// }



/*
 * EKFの予測(１周期に１回しか呼ばれない)
 * X_, S_自体を書き換えてしまう．
 */


void FastSLAM::predict(){
  setG(); /* 状態方程式のヤコビアン */
  setR();
  predictX();
  S_ = G_ * S_ * G_.transpose() + R_;
}


/*
 * EKFのX(期待値)の予測
 */
void FastSLAM::predictX(){
  /* 位置の更新 */
  X_.insert(X_.extract_n(0, 3) + X_.extract_n(7, 3) * dt_, 0);

  /* 姿勢の更新 */
  quaternion quat1(X_.extract_n(3, 4));
  quaternion quat2;
  quat2.t = 0;
  quat2.V = (dt_ / 2) * X_.extract_n(10, 3);
  quaternion d_quat = quat1 * quat2;
  quat1 = quat1 + d_quat;
  normalize_quat(quat1);
  X_.insert(quat1.to_vector(), 3);

  estco_.init(X_.extract_n(0,7));
}


/*
 * 状態方程式のヤコビアン
 */
void FastSLAM::setG(){
  //G_ = Matrix(DiagMatrix(13, 13, 1));
  G_ = make_unit_matrix(13);

  /* 位置の速度での偏微分 */
  Matrix PosbyVel = dt_ * make_unit_matrix(3);
  G_.insert(PosbyVel, 0, 7);

  /* 姿勢から姿勢へのヤコビ (角速度の影響を受ける) */
  Matrix AVelbyAVel(4,4,0);
  double wx = X_(10);
  double wy = X_(11);
  double wz = X_(12);

  AVelbyAVel(0,1) = -wx;
  AVelbyAVel(0,2) = -wy;
  AVelbyAVel(0,3) = -wz;

  AVelbyAVel(1,0) =  wx;
  AVelbyAVel(1,2) =  wz;
  AVelbyAVel(1,3) = -wy;

  AVelbyAVel(2,0) =  wy;
  AVelbyAVel(2,1) = -wz;
  AVelbyAVel(2,3) =  wx;

  AVelbyAVel(3,0) =  wz;
  AVelbyAVel(3,1) =  wy;
  AVelbyAVel(3,2) = -wx;

  AVelbyAVel = (dt_/2) * AVelbyAVel;

  G_.insert(G_.extract_n(3,3,4,4) + AVelbyAVel, 3, 3);

  /* 姿勢角を角速度で偏微分 */
  Matrix AVel2Quat = dpQuatbyAVel(X_.extract_n(3,4), dt_);

  G_.insert(AVel2Quat, 3, 10);
}


/**********
 * ノイズベクトルは速度・角速度レベルで持つ
 * Andrew方式
 * "Real-time 3D SLAM for Humanoid Robot considering Pattern Generator Information"
 **********/
void FastSLAM::setR(){
  /**********
   * ノイズ
   **********/
  Matrix M(6,6,0);

  /* 速度のノイズ */
  ColumnVector vel = X_.extract_n(7, 3);
  for(int i=0; i<3; i++){
    M(i,i) = 0.1 * pow(vel(i), 2) + 100.0;
  }

  /* 角速度のノイズ */
  ColumnVector avel = X_.extract_n(10, 3);
  for(int i=0; i<3; i++){
    M(i+3, i+3) = 0.1 * pow(avel(i), 2) + 0.01;
  }

  /**********
   * 状態方程式をノイズベクトルで偏微分
   **********/
  Matrix V(13,6,0);

  /* 位置 */
  for(int i=0; i<3; i++) V(i,i) = dt_;

  /* 姿勢 */
  V.insert(dpQuatbyAVel(X_.extract_n(3,4), dt_), 3, 3);

  /* 速度 */
  for(int i=0; i<3; i++) V(i+7, i) = 1.0;

  /* 角速度 */
  for(int i=0; i<3; i++) V(10+i, 3+i) = 1.0;


  R_ = V * M * V.transpose();
  invR_ = (R_.extract_n(0,0,7,7)).inverse();
}




/**********
 * デバッグ用
 **********/
void FastSLAM::evalEKF(){
  // ColumnVector debug_dZ = Z_ - this->h_func(X_);//debug
  // cerr << "@EKF dquat " << d_Z_.extract(0,3).norm(2.0) 
  //      << " => " << debug_dZ.extract(0,3).norm(2.0) << endl;
  // if(d_Z_.extract(0,3).norm(2.0) > 1.0){
  //   cerr << "@EKF Z = " << Z_.transpose() << endl;
  //   cerr << "@EKF Zh = " << Zh_.transpose() << endl;
  // }


  // for(int i=0; i<fn_; i++){
  //   int idx = i*3 + 4;
  //   cerr << "@EKF distance " << i  << "th pos: "<< d_Z_.extract_n(idx,3).norm(2.0)
  // 	 << " => " << debug_dZ.extract_n(idx,3).norm(2.0) << endl;
  // }
}

/**************************************************
 *
 * 使い方
 * FastSLAM ekf;
 * 1: ekf.updateFeatures(rmlist, newfeautres);
 * 2: ekf.predict();
 * for() センサ情報や特徴点
 * 3: ekf.estZof..;
 * 4: ekf.setHof..;
 * 5: ekf.setQ(inQ);
 * 6: ekf.update(Z);
 * end for()
 *
 **************************************************/
