/*************************************************
 *                   estimate_core.cpp
 *
 * 
 * written by T.Goto
 *************************************************/

#ifndef __FASTSLMA_H__
#define __FASTSLMA_H__

#include <octave/config.h>
#include <octave/Matrix.h>
#include <math.h>
#include <iostream>
#include <vector>
#include <utils/UtilTimer.h>
#include <octave_rtmex/octave_rtmex.h>
#include "VOUtil.h"


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


using namespace std;

namespace rtm{
  class FastSLAM{
  public:
    FastSLAM();
    FastSLAM(double dt);
    ~FastSLAM();

    void init(double dt);
    void makeNewWorldPoint(const ColumnVector &lpos, ColumnVector &wpos, Matrix &Sigma);
    Matrix makeWorldPointCovfromStereo(const ColumnVector &lpos);
    Matrix makeStereoErrorCov(const ColumnVector &lpos);
    Matrix makePointCovfromQuat(const ColumnVector &lpos);
    void setQ(const Matrix &Q);


    ColumnVector estZofAVel();
    void setHofAVel();
    ColumnVector estZofVel();
    void setHofVel();
    ColumnVector estZofLVel();
    void setHofLVel();

    ColumnVector estZofQuat();
    void setHofQuat();
    ColumnVector estZofVertical();
    void setHofVertical();
    ColumnVector estZofPos(int type);
    void setHofPos(int type);
    ColumnVector estZofCoords();
    void setHofCoords();



    void update(const ColumnVector &Z);
    void updatebyPoints(const vector<ColumnVector> &lpos,
			vector<ColumnVector> &wpos, vector<Matrix> &WPCov);
    void predict();
    void predictX();
    void setG();
    void setR();

    void evalEKF();


  private:
    /******** 変数 ********/
    double dt_; // 周期
    int fn_;    // 特徴点数
    int sdim_;  // 状態変数の数
    coords estco_;

    Matrix G_; //状態方程式ヤコビアン
    Matrix H_; //観測方程式ヤコビアン
    Matrix R_; //状態ノイズ共分散
    Matrix invR_;
    Matrix Q_; //誤差分散
    Matrix S_; //状態共分散
    Matrix Sh_;//状態共分散予測
    Matrix K_; //カルマンフィルタ

    ColumnVector X_;  //状態項
    ColumnVector Xh_; //状態項予測
    ColumnVector Z_;  //観測項
    ColumnVector Zh_; //観測項予測
    ColumnVector U_;  //入力項
    ColumnVector d_Z_;//予想と観測の差

    UtilTimer utimer;

  public:
    void setX(const ColumnVector &X){
      X_ = X;
      estco_.init(X_.extract_n(0,7));
    }
    ColumnVector getX(){return X_;}

    void setU(const ColumnVector &U){U_ = U;}
    ColumnVector getU(){return U_;}

    void setS(const Matrix &S){S_ = S;}
    Matrix getS(){return S_;}

    void setZh(const ColumnVector &Zh){Zh_ = Zh;}
    ColumnVector getZh(){return Zh_;}

    void setco(const coords &co){ estco_ = co;}
    coords getco(){return estco_;}
  };
};

#endif


