/*************************************************
 *                   RotEstimatorCore.cpp
 *
 *
 *************************************************/

#ifndef __ROT_ESTIMATOR_CORE_H__
#define __ROT_ESTIMATOR_CORE_H__

#include <octave/config.h>
#include <octave/Matrix.h>
#include <math.h>
#include <iostream>
#include <octave_rtmex/octave_rtmex.h>
#include <BWF/bwf.h>

//define
#ifndef PI
#define PI 3.141592653589793238462643383279
#endif


using namespace std;
using namespace rtm;

class RECore{
 public:
  RECore();
  ~RECore();
  RECore(double in_dt);
  void init(double in_dt);
  bool initFirstPose(const ColumnVector &accs);
  void convert(const ColumnVector &accs, const ColumnVector &avel,
	       ColumnVector &quat, Matrix &Cov);
  void resetYawAngle(int axis);

 private:
  ColumnVector lowpass(const ColumnVector &raw_accs);

  float dt_;
  BWF lp_bwf_[3];

  ColumnVector accs_, waccs_, lp_accs_, avel_;



  /*****************************
   //
   //カルマンフィルタ
   //
   *****************************/
 private:
  /**** 予測と更新 ****/
  void updateKalmanFilter(const ColumnVector &U, const ColumnVector &Z);

  /**** 状態方程式 ****/
  ColumnVector g_func(const ColumnVector &X, const ColumnVector &U);
  Matrix calcM();
  Matrix calcV();
  Matrix calcR();//by U
  Matrix calcG();//by X

  /**** 観測方程式 *****/
  ColumnVector h_func(const ColumnVector &Xh);
  Matrix calcH();//by Xh
  Matrix calcQ();//by Z


  ColumnVector U_, X_, Xh_;
  ColumnVector Z_, Zh_, d_Z_;
  Matrix G_, H_;
  Matrix R_, Q_, S_, Sh_;
  Matrix K_;

 public:
  /**** アクセスを明示的に ****/
  ColumnVector getU(){ return U_;}
  ColumnVector getX(){ return X_;}
  ColumnVector getXh(){ return Xh_;}
  ColumnVector getZ(){ return Z_;}
  ColumnVector getZh(){ return Zh_;}
  Matrix getG(){ return G_;}
  Matrix getH(){ return H_;}
  Matrix getS(){ return S_;}
  Matrix getSh(){ return Sh_;}

  void setU(const ColumnVector &U){ U_ = U;}
  void setX(const ColumnVector &X){ X_ = X;}
  void setXh(const ColumnVector &Xh){ Xh_ = Xh;}
  void setZ(const ColumnVector &Z){ Z_ = Z;}
  void setZh(const ColumnVector &Zh){ Zh_ = Zh;}
  void setG(const Matrix &G){ G_ = G;}
  void setH(const Matrix &H){ H_ = H;}
  void setS(const Matrix &S){ S_ = S;}
  void setSh(const Matrix &Sh){ Sh_ = Sh;}
};




#endif


