#include "TransEstimatorCore.h"

TECore::TECore(){
}


TECore::~TECore(){
}


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


void TECore::init(double dt){
  dt_ = dt;
  tvel_ = initKalman();
  ref_vel_ = ColumnVector(3,0);
}


void TECore::convert(const ColumnVector &accs, const ColumnVector &quat,
		     const ColumnVector &ref, const ColumnVector &ref_var,
		     const bool &isStatic, ColumnVector &tvel){
  accs_ = accs;
  quat_ = quaternion(quat);
  isStatic_ = isStatic;
  ref_vel_ = ref;
  ref_var_ = ref_var;

  Q_ = Matrix (3,3,0);
  for(int i=0; i<3; i++){
    Q_(i,i) = ref_var_(i);
  }

  waccs_ = quat_.rotate_vector(accs_);
  waccs_(2) = (waccs_(2) - 1.0);
  waccs_ = waccs_ * 9.80665 * 1000;

  //if(waccs_.norm(2.0) < 0.1) waccs_ = ColumnVector(3,0);
  // for(int i=0; i<3; i++){
  //   if(abs(waccs_(i)) < 100.0) waccs_(i) = 0;
  // }

  if(isStatic) tvel_ = initKalman();
  else tvel_ = updateKalmanFilter(waccs_, quat_.rotate_vector(ref_vel_)).extract(0,2);

  //tvel = tvel_; /* 世界座標系での値を返す */
  tvel = quat_.inverse_rotate_vector(tvel_); /* ローカルでの速度を返す */
}


void TECore::convert(const ColumnVector &accs, const ColumnVector &quat, const bool &isStatic,
		     ColumnVector &tvel){
  ColumnVector ref_vel = ColumnVector(3,0);
  ColumnVector ref_var = ColumnVector(3, 1000 * 1000);
  convert(accs, quat, ref_vel, ref_var, isStatic, tvel);
}


void TECore::resetTransVelociy(){
  tvel_ = ColumnVector(3,0);
  initKalman();
}


ColumnVector TECore::initKalman(){
  X_ = ColumnVector(3,0);
  S_ = 100 * make_unit_matrix(3);
  G_ = make_unit_matrix(3);
  H_ = make_unit_matrix(3);
  R_ = 10 * make_unit_matrix(3);
  Q_ = 100 * make_unit_matrix(3);

  return X_;
}


ColumnVector TECore::g_func(const ColumnVector &X, const ColumnVector &U){
  return (X + dt_ * U);
}


ColumnVector TECore::h_func(const ColumnVector &X){
  return (H_ * X);
}


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

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

  /* カルマンゲイン */
  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_; // 共分散行列

  return X_;
}
