/*************************************************
 *                   EKFCoords.cpp
 * 確率ロボティクス + Andrewの論文を元に実装
 * p284--
 * 変数名は本に準拠
 * 更新を反復でやるので切るタイミングを自由にできる．
 * ４元数を一回一回正規化できるので、発散しない．
 *
 **************************************************/

#include "EKFCoords.h"

using namespace std;
using namespace rtm;

EKFCoords::EKFCoords(){
}


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


EKFCoords::~EKFCoords(){
}


void EKFCoords::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();
}

/********************
 * 特徴点の削除と追加
 * 削除したいもののリスト(i th feature)
 * 新しい特徴点のlpos
 * X_とS_を変更する．
 * rmlistは上り順で正しくないといけない
 ********************/
void EKFCoords::updateFeatures(const vector<int> &rmlist,
			       const vector<ColumnVector> &new_lposs){
  int rmn = static_cast<int>(rmlist.size());
  int addn = static_cast<int>(new_lposs.size());
  fn_ = fn_ + addn - rmn; /* 特徴点の数 */
  sdim_ = 13 + 3*fn_;     /* 状態変数の数 */

  ColumnVector X(sdim_, 0);
  Matrix S(sdim_, sdim_, 0);

  /* 削除 */
  int fulln = 1;
  if(!rmlist.empty()){
    if ( rmlist.back() + 1 == ((S_.rows() - 13)/3) ) fulln = rmn;
    else fulln = rmn + 1;
  }

  int area[fulln][2]; /* S_からexractする領域 startとnの2次元配列 */

  if(rmlist.empty()){
    area[0][0] = 0;
    area[0][1] = S_.rows();
  }
  else {
    for(int i=0; i<fulln; i++){
      if(i==0){
	area[0][0] = 0;
	area[0][1] = 13 + 3 * rmlist[0];
      }
      else if(i == rmn){
	area[i][0] = (13 + 3 * (rmlist[i-1] + 1));
	area[i][1] = S_.rows() - (13 + 3*(rmlist[i-1] + 1));
      }
      else {
	area[i][0] = (13 + 3 * (rmlist[i-1] + 1));
	area[i][1] = 3 * (rmlist[i]-rmlist[i-1]-1);
      }}}

  int dst_row = 0;
  int dst_col = 0;
  for(int i=0; i<fulln; i++){
    X.insert(X_.extract_n(area[i][0], area[i][1]), dst_row);
    for(int j=0; j<fulln; j++){
      S.insert(S_.extract_n(area[i][0],area[j][0],area[i][1],area[j][1]), dst_row, dst_col);
      dst_col += area[j][1];
    }
    dst_col = 0;
    dst_row += area[i][1];
  }

  /* 追加 */
  int dst_idx = dst_row;
  for(int i=0; i<addn; i++){
    X.insert(estco_.transform_vector(new_lposs[i]), dst_idx);
    //S.insert(makeNewPointCov(new_lposs[i]), dst_idx, dst_idx);
    S.insert(make_unit_matrix(3), dst_idx, dst_idx);
    dst_idx += 3;
  }

  X_ = X;
  S_ = S;
}


Matrix EKFCoords::makeNewPointCov(const ColumnVector &lpos){
  Matrix S = S_.extract_n(0,0,3,3);
  S = S + makePointWorldCovfromStereo(lpos) + makePointCovfromQuat(lpos);
  return S;
}


/*
 * 特徴点を世界座標系にバインドする時の共分散行列
 * local座標系での観測値をrotateしてからStereo誤差の共分散行列を算出する．
 * estco_がpredictで更新される前に呼ばれる
 */
Matrix EKFCoords::makePointWorldCovfromStereo(const ColumnVector &lpos){
  return (makeStereoErrorCov(estco_.quat.rotate_vector(lpos)));
}


/*
 * ステレオ誤差の共分散行列の算出
 */
Matrix EKFCoords::makeStereoErrorCov(const ColumnVector &pos){
  const static double bb = 90.0 * 90.0;
  const static double ff = 485 * 485;
  const static double sgm_uu = 1.0 * 1.0;
  const static double sgm_vv = 1.0 * 1.0;
  const static double sgm_dd = 1.0 * 1.0;

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

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

  return S;
}


/*
 * 姿勢の誤差が点を世界座標系にバインドする時に与える誤差を算出
 */
Matrix EKFCoords::makePointCovfromQuat(const ColumnVector &lpos){
  /* 姿勢角の共分散が与える影響 */
  const double sigma_qq = 0.1 * 0.1;
  double x = lpos(0);
  double y = lpos(1);
  double z = lpos(2);
  double q0 = X_(3);
  double q1 = X_(4);
  double q2 = X_(5);
  double q3 = X_(6);

  Matrix J(3,4);
  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 = 2 * J;

  Matrix S = J * Matrix(DiagMatrix(4,4,sigma_qq)) * J.transpose();

  return S;
}


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

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


/*
 * 位置の観測
 * type = 0 : x,  type = 1 : y,  type = 2 : z,  type = 3 : xyz
 */
ColumnVector EKFCoords::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 EKFCoords::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;
}


/*
 * 姿勢の観測
 */
ColumnVector EKFCoords::estZofQuat(){
  Zh_ = X_.extract_n(3, 4);
  return Zh_;
}

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


/*
 * 速度の観測
 * 今は世界座標系で与えられる場合だけを考える．
 */
ColumnVector EKFCoords::estZofVel(){
  Zh_ = X_.extract_n(7, 3);
  return Zh_;
}


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


/*
 * 角速度の観測
 */
ColumnVector EKFCoords::estZofAVel(){
  Zh_ = X_.extract_n(10, 3);
  return Zh_;
}

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


/*
 * 特徴点の観測
 */
ColumnVector EKFCoords::estZofPoint(int fidx){
  int idx = 13 + fidx * 3;
  Zh_ = estco_.inverse_transform_vector(X_.extract_n(idx, 3));
  return Zh_;
}


void EKFCoords::setHofPoint(int fidx){
  int idx = 13 + fidx * 3;
  Matrix H(3, sdim_, 0);

  H.insert(dpPointbyCoords(X_.extract_n(idx, 3), X_.extract_n(0,7)), 0, 0);
  H.insert(dpPointbyPoint(X_.extract_n(idx, 3), X_.extract_n(0,7)), 0, idx);

  H_ = H;
}



/********************
 *
 * 偏微分
 *
 ********************/

/*
 * 状態方程式
 * 姿勢角を角速度で偏微分[4,3]
 */
Matrix EKFCoords::dpQuatbyAVel(const ColumnVector &quat){
  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;
}


/*
 * 観測方程式
 * 特徴点の観測値(local)の特徴点(world)での偏微分[3,3]
 */
Matrix EKFCoords::dpPointbyPoint(const ColumnVector &wpos, const ColumnVector &co){
  double q0, q1, q2, q3;
  q0 = co(0); q1 = co(1); q2 = co(2); q3 = co(3);

  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;
}


/*
 * 特徴点の観測値のカメラ座標系での偏微分[3,3]
 */
Matrix EKFCoords::dpPointbyCoords(const ColumnVector &wpos, const ColumnVector &co){
  // double x = co(0);
  // double y = co(1);
  // double z = co(2);
  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);

  /* quaternionで偏微分 */
  double dx = wpos(0) - co(0);
  double dy = wpos(1) - co(1);
  double dz = wpos(2) - co(2);

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

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

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

  return H;
}



/*
 * EKFの予測(１周期に１回しか呼ばれない)
 * X_, S_自体を書き換えてしまう．
 */
void EKFCoords::predict(){
  setG(); /* Gの更新は前回の値でやるのでこちらが先 */
  setR();
  predictX();
  //S_ = G_ * S_ * G_.transpose();
  S_.insert(G_*S_.extract_n(0,0,13,13)*G_.transpose(), 0, 0);
  S_.insert(G_* S_.extract_n(0,13,13,sdim_-13), 0, 13);
  S_.insert(S_.extract_n(13,0,sdim_-13, 13)*G_.transpose(), 13, 0);

  S_.insert(S_.extract_n(0,0,13,13) + R_, 0, 0);
}


/*
 * EKFの更新(1周期に複数回呼ばれる)
 * == 前にしなければいけないこと ==
 * this->estZof..
 * this->setH or this->setHof..
 * this->setQ or this->setQof..
 */
void EKFCoords::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);
}


/*
 * EKFのX(期待値)の予測
 */
void EKFCoords::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 EKFCoords::setG(){
  G_ = Matrix(DiagMatrix(13, 13, 1));
  //G_ = Matrix(DiagMatrix(sdim_, sdim_, 1));

  /* 位置の速度での偏微分 */
  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));

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


/**********
 * ノイズベクトルは速度・角速度レベルで持つ
 * Andrew方式
 * "Real-time 3D SLAM for Humanoid Robot considering Pattern Generator Information"
 **********/
void EKFCoords::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)), 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();
}


/******************************
 *
 * FastSLAMの実装
 * 'FastSLAM 2.0
 * An Improved Particle Filtering Algorithm for Simultaneous Localization and Mapping that...'
 * を元に実装，
 * 変数名は確率ロボティクスを元に実装しているので，論文と違う
 *
 ******************************/



/**********
 * デバッグ用
 **********/
void EKFCoords::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;
  // }
}

/**************************************************
 *
 * 使い方
 * EKFCoords 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()
 *
 **************************************************/
