/*************************************************
 *                   MotionEstimatorCore.cpp
 *
 *
 *************************************************/

#ifndef ESTIMATE_CORE_H
#define ESTIMATE_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>

#include "TrueValueEstimatorCore.h"
#include "RotEstimatorCore.h"
#include "TransEstimatorCore.h"

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


using namespace std;
using namespace rtm;

class MECore{
 public:
  MECore();
  ~MECore();
  MECore(double in_dt);
  void init(double in_dt);
  /* オドメトリ無 */
  void convert(const ColumnVector &accs, const ColumnVector &gyros, ColumnVector &quat,
	       Matrix &Cov, ColumnVector &avel, ColumnVector &tvel, bool &isStatic);
  /* オドメトリ有 */
  void convert(const ColumnVector &accs, const ColumnVector &gyros,
	       const ColumnVector &ref_tvel, const ColumnVector &ref_var,
	       ColumnVector &quat, Matrix &Cov,
	       ColumnVector &avel, ColumnVector &tvel, bool &isStatic);

  bool initFirstPose(const ColumnVector &accs, const ColumnVector &gyros);

 private:
  RECore *rotest_;
  TVECore *tvest_;
  TECore *trsest_;

  double dt_;
  ColumnVector accs_, gyros_;
  ColumnVector avel_;
  bool isStatic_;
  ColumnVector quat_;
  Matrix Cov_;
  ColumnVector tvel_;
};




#endif


