// -*- C++ -*-
/*!
 * @file  NxMotionEstimator.h
 * @brief Sequence InPort component
 *
 */

#ifndef ROT_ESTIMATOR_H
#define ROT_ESTIMATOR_H

#include <iostream>
#include <iomanip>
#include <exception>
#include <stdexcept>
#include <sstream>
#include <boost/bind.hpp>
#include <rtm/Manager.h>
#include <rtm/DataFlowComponentBase.h>
#include <rtm/CorbaPort.h>
#include <rtm/DataInPort.h>
#include <rtm/DataOutPort.h>
#include <rtm/idl/BasicDataTypeSkel.h>
#include <utils/UtilTimer.h>
#include <octave_rtmex/octave_rtmex.h>
#include <BWF/bwf.h>
#include <Extended3DdataTypes.hh>

#include "MotionEstimatorCore.h"

#ifndef PI
#define PI 3.141592653589793238462643383279
#endif


class NxMotionEstimator
  : public RTC::DataFlowComponentBase
{
public:
  NxMotionEstimator(RTC::Manager* manager);
  ~NxMotionEstimator();

  virtual RTC::ReturnCode_t onInitialize();
  virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id);
  virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id);
  virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);

protected:
  /********** Configuration variable declaration **********/
  std::string m_componentconf_file;
  std::string m_config;

  /********** DataInPort declaration **********/
  Extended3DdataTypes::TimedMotion3DSensorData m_inSensor;
  RTC::InPort<Extended3DdataTypes::TimedMotion3DSensorData> m_inSensorPort;

  /********** DataOutPort declaration **********/
  Extended3DdataTypes::TimedEstimatedMotionData m_EstimatedResult;
  RTC::OutPort<Extended3DdataTypes::TimedEstimatedMotionData> m_EstimatedResultPort;

  /********** ServicePort declaration **********/
  // State Service

  // Command Service

private:
  std::string quat_result(const std::string &argv);// Command Function
  bool setState(); // State Configure
  void readSensorPort();
  void getInertiaData();
  void writeEstimationResult();
  void getAvelAverage();

  pthread_mutex_t mutex;
  bool m_first_;

  double dt_;
  int input_packets_;

  /* State Pointer */
  double *in_accs_, *in_gyros_;
  double out_accl_[3], out_avel_[3], out_quat_[20], out_tvel_[4], out_sttc_[1];

  /* Estimator */
  MECore *mtnest_;


  /* Kalman Filter InputData */
  vector<ColumnVector> accs_, gyros_, avel_;
  ColumnVector quat_;
  Matrix Cov_;
  list<ColumnVector> gyro_stck_; // 1/500 * 15 = 3/100
  ColumnVector ave_avel_;
  ColumnVector tvel_;
  bool isStatic_;

  // Timer
  UtilTimer utimer;

  // for reset yaw
  int count_;
};


extern "C"
{
  void NxMotionEstimatorInit(RTC::Manager* manager);
};

#endif // ROT_ESTIMATOR_H
