// -*- C++ -*-
#ifndef __MOTION_CLASSIFIER_H__
#define __MOTION_CLASSIFIER_H__

#include <iostream>
#include <iomanip>
#include <sstream>
#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 <rtm/idl/InterfaceDataTypesSkel.h>
#include <utils/UtilTimer.h>
#include <boost/bind.hpp>
#include <Extended3DdataTypes.hh>

#include <octave_rtmex/octave_rtmex.h>
#include "MotionClassifierCore.h"

#ifndef COORDS_N
#define COORDS_N (7)
#endif
#ifndef FEATURE_P_N
#define FEATURE_P_N (9)
#endif

using namespace std;
using namespace rtm;


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

  // rtm function
  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;

  /* InPort */
  /* flow */
  RTC::TimedFloatSeq m_inFlow;
  RTC::InPort<RTC::TimedFloatSeq> m_inFlowPort;

  /* inertia data */
  Extended3DdataTypes::TimedEstimatedMotionData m_inInertia;
  RTC::InPort<Extended3DdataTypes::TimedEstimatedMotionData> m_inInertiaPort;

  /* DataOutPort declaration */
  /* coords data for ego-motion */
  Extended3DdataTypes::TimedWorldQuatCoords m_outCoords;
  RTC::OutPort<Extended3DdataTypes::TimedWorldQuatCoords> m_outCoordsPort;

  /* staticpcl data */
  Extended3DdataTypes::TimedLabeledPCL m_outStaticPCL;
  RTC::OutPort<Extended3DdataTypes::TimedLabeledPCL> m_outStaticPCLPort;

  /* movingpcl data */
  Extended3DdataTypes::TimedLabeledPCL m_outMovingPCL;
  RTC::OutPort<Extended3DdataTypes::TimedLabeledPCL> m_outMovingPCLPort;

private:
  void writePCL();
  void info2Color(int ntr, int lbl, RTC::RGBColour &col);
  bool setState();
  void readInertiaPort();
  void readFlowPort();

  /**** Converter ****/
  MotionClassifierCore vo_;

  /**** state ****/
  double in_flow_[MAX_FN*COORDS_N], in_height_[3], in_odometry_[COORDS_N];
  double in_accl_[3], in_avel_[3], in_quat_[20],  in_tvel_[3], in_isStatic_[1];
  double out_coords_[COORDS_N], out_ftrs_[MAX_FN*FEATURE_P_N];

  bool orientp_; /* 初期姿勢取得フラグ */
  int count_;
  bool heightp_;

  /* Timer */
  UtilTimer utimer_;
};


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

#endif // __MOTION_CLASSIFIER_H__
