/*************************************************
 *                   RotEstimatorCore.cpp
 *
 *
 *************************************************/
#include "RotEstimatorCore.h"

using namespace std;
using namespace rtm;

RECore::RECore(){
}


RECore::~RECore(){
}


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


void RECore::init(double dt){
  // 周期
  dt_ = dt;

  /******** 状態変数 ********/
  // X
  ColumnVector X(4);
  X(0) = 1;
  setX(X);
  setXh(X);

  //G 時不変線形
  Matrix G = make_unit_matrix(4);
  setG(G);

  // S
  Matrix S = make_unit_matrix(4) * 0.0001;
  setS(S);

  waccs_ = lp_accs_ = ColumnVector(3,0);
  // 世界座標系での加速度
  waccs_(2) = 1.0;

  // バターワースフィルタ
  for(int i=0; i<3; i++) lp_bwf_[i].create(dt_, 2, 2, LP);
}


void RECore::convert(const ColumnVector &accs, const ColumnVector &avel,
		     ColumnVector &quat, Matrix &Cov){
  accs_ = accs;
  avel_ = avel;

  lp_accs_ = this->lowpass(accs_);
  ColumnVector inZ = accs_;
  normalize_vector(inZ);

  updateKalmanFilter(avel_, inZ);

  quat = getX();
  Cov = getS();
}



bool RECore::initFirstPose(const ColumnVector &accs){
  ColumnVector avel (3,0);
  ColumnVector quat(4,0);
  Matrix Cov(3,3,0);

  ColumnVector waccs(3,0);
  waccs(2) = 1;

  // ColumnVector raccs(3,0);
  // raccs(1) = accs(1);
  // raccs(2) = accs(2);
  // double roll = vector_angle(raccs, waccs);
  // if(raccs(1) < 0) roll = - roll;

  // ColumnVector paccs(3,0);
  // paccs(0) = accs(0);
  // paccs(2) = accs(2);
  // double pitch = vector_angle(paccs, waccs);
  // if(paccs(0) > 0) pitch = - pitch;

  ColumnVector taccs = accs;
  normalize_vector(taccs);

  double pitch = asin(taccs(1));
  //double roll  = asin(taccs(0) / cos(pitch));
  double roll  = acos(taccs(2) / cos(pitch));
  printf("@debug pitch : %f, roll %f\n", rad2deg(pitch), rad2deg(roll));

  ColumnVector rpy(3,0);
  rpy(0) = -roll;
  rpy(1) = -pitch;

  quaternion rot = rpy2quat(rpy);

  init(dt_);
  setX(rot.to_vector());

  /* lowpassのために初期値で繰り返す */
  for(int i=0; i<1000; i++) this->lowpass(accs);

  { /* debug */
    printf("@debug raw     = %10f %10f %10f\n", accs(0), accs(1), accs(2));

    ColumnVector hoge = this->lowpass(accs);
    printf("@debug lowpass = %10f %10f %10f\n", hoge(0), hoge(1), hoge(2));
  }


  for(int i=0; i<100; i++) this->convert(accs, avel, quat, Cov);
  resetYawAngle(2);
  for(int i=0; i<100; i++) this->convert(accs, avel, quat, Cov);
  resetYawAngle(2);

  if(isnan(getX()(0))){
    cerr << "@initfirstpos X is nan" << endl;
    return false;
  }
  else{
    //cerr << "@initFirstPose rpy: " << quat2rpy(quaternion(getX())).transpose() << endl;
    ColumnVector tmp = quat2rpy(quaternion(getX()));
    printf("@initFirstPose rpy : %6.3f %6.3f %6.3f", tmp(0)*180/PI, tmp(1)*180/PI, tmp(2)*180/PI);
  }
  return true;;
}


ColumnVector RECore::lowpass(const ColumnVector &raw_accs){
  ColumnVector lp_accs(3,0);
  for(int i=0; i<3; i++) lp_accs(i) = lp_bwf_[i].pass(raw_accs(i));
  return lp_accs;
}


ColumnVector RECore::g_func(const ColumnVector& X, const ColumnVector& U){
  /********** 回転項 **********/
  quaternion quat1,quat2,d_quat;

  //クォータニオン Xt[3~6]
  quat1.t = X(0);
  quat1.V = X.extract(1,3);

  //無限小回転項 Xt[10~12]
  quat2.t = 0;
  quat2.V = (dt_ / 2) * U;

  //クォータニオン足し合わせ
  d_quat = quat_cross(quat1,quat2);
  quat1.t = quat1.t + d_quat.t;
  quat1.V = quat1.V + d_quat.V;
  normalize_quat(quat1);

  ColumnVector Xh(4,0);
  Xh(0) = quat1.t;
  Xh.insert(quat1.V, 1);
  return Xh;
}


ColumnVector
RECore::h_func(const ColumnVector& Xh){
  //姿勢角
  quaternion rot(Xh);
  ColumnVector Zh = rot.inverse_rotate_vector(waccs_);
  return Zh;
}


Matrix
RECore::calcM(){
  //init
  ColumnVector U = getU();
  Matrix M(3,3,0);

  for(int i=0; i<3; i++)
    M(i,i) = 0.01 * U(i) * U(i) + 0.05;
  return M;
}


Matrix
RECore::calcV(){
  ColumnVector X = getX();
  Matrix V(4,3,0); //g_funcをノイズベクトルで偏微分[4,3] 
  double q0,q1,q2,q3;
  double coef = dt_ / 2;

  q0 = X(0); q1 = X(1); q2 = X(2); q3 = X(3);

  //q0
  V(0,0) = - coef * q1;
  V(0,1) = - coef * q2;
  V(0,2) = - coef * q3;

  //q1
  V(1,0) =   coef * q0;
  V(1,1) =   coef * q3;
  V(1,2) = - coef * q2;

  //q2
  V(2,0) = - coef * q3;
  V(2,1) =   coef * q0;
  V(2,2) =   coef * q1;

  //q3
  V(3,0) =   coef * q2;
  V(3,1) = - coef * q1;
  V(3,2) =   coef * q0;

  return V;
}


Matrix
RECore::calcR(){
  Matrix M = calcM();
  Matrix V = calcV();
  Matrix R = V * M * V.transpose();
  return R;
}


Matrix
RECore::calcG(){
  return (make_unit_matrix(4));
}


//NxRotEstimatorからget and setする
Matrix
RECore::calcQ(){
  // //init
  // Matrix Q(3,3,0);
  // ColumnVector Z = getZ();

  // static float i_ang = 0;
  // static float i_mag = 0.9;
  // static ColumnVector pre_Z = Z;
  // static double acc_variance;

  // float p_ang = abs((float) vector_angle(Z,pre_Z)) * (180 / PI);
  // i_ang = i_mag * i_ang + p_ang;

  // acc_variance = (double) 0.01*i_ang + 0.01*p_ang;
  // if(acc_variance < 0.01)
  //   acc_variance = 0.01;

  // for(int i=0; i<3; i++)
  //   Q(i,i) = acc_variance;

  double avel_nrm = avel_.norm(2.0);
  double daccs_nrm = (accs_ - lp_accs_).norm(2.0);
  double accs_nrm = abs(1.0 - accs_.norm(2.0));
  // printf("@debug %10.3f, %10.3f, %10.3f\n", avel_nrm, daccs_nrm, accs_nrm); /* debug */
  double var = avel_nrm * 0.1 + daccs_nrm + 3 * accs_nrm + 0.1;
  //printf("@debug %.3f\n", var);
  Matrix Q = var * make_unit_matrix(3);
  return Q;
}


Matrix
RECore::calcH(){
  ColumnVector Xh = getXh();
  double q0,q1,q2,q3;
  static double x = waccs_(0);
  static double y = waccs_(1);
  static double z = waccs_(2);
  q0 = Xh(0);
  q1 = Xh(1);
  q2 = Xh(2);
  q3 = Xh(3);

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

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

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

  return H;
}


void
RECore::resetYawAngle(int axis){
  ColumnVector X = getX();

  quaternion quat(X);
  Matrix rot = quat2matrix(quat);
  ColumnVector rpy = matrix2rpy(rot);
  rpy(axis) = 0.0;
  rot = rpy2matrix(rpy);
  quat = matrix2quat(rot);

  X = quat.to_vector();
  setX(X);
}


void
RECore::updateKalmanFilter(const ColumnVector &U, const ColumnVector &Z){
  U_=U;
  Z_=Z;

  /* 予測 */
  G_ = calcG();
  R_ = calcR();
  Xh_ = g_func(X_, U_);
  Sh_ = G_ * S_ * G_.transpose() + R_;

  /* カルマンゲイン */
  Q_ = calcQ();
  H_ = calcH();
  Matrix tmp1 = H_ * Sh_ * H_.transpose() + Q_;
  Matrix tmp2 = tmp1.pseudo_inverse();
  K_ = Sh_ * H_.transpose() * tmp2;

  /* 状態変数更新 */
  Zh_ = h_func(Xh_);
  d_Z_ = Z_ - Zh_;
  X_ = Xh_ + K_ * d_Z_; // 状態変数

  /* 共分散行列の更新 */
  Matrix E = make_unit_matrix(X_.rows());
  S_ = (E- K_ * H_) * Sh_; // 共分散行列
}
