#ifndef __VO_SIMULATION_H__
#define __VO_SIMULATION_H__

#include <vector>
#include <deque>
#include <octave_rtmex/octave_rtmex.h>
#include <stdlib.h>
#include "VOUtil.h"


using namespace std;
using namespace rtm;

/******************************
 *
 * MotionClassifierのシミュレータ
 *
 ******************************/
class VOSimulation {
 public:
  VOSimulation();
  ~VOSimulation();


  void init(double bsln, double fcs);
  void initFeatures(int fn, const ColumnVector &cnt, const ColumnVector &dsize);
  void addFeatures(int fn, const  ColumnVector &cnt, const ColumnVector &dsize);
  void update(const coords &dco);
  void getIdealFeatures(vector<ColumnVector> &pres, vector<ColumnVector> &curs);
  void getRealFeatures(vector<ColumnVector> &pres, vector<ColumnVector> &curs);
  coords getCoords(){ return co_;}
  int getFN(){ return fn_;}
  void print();


 private:
  void makeNewFeatures(int fn, const ColumnVector &cnt, const ColumnVector &dsize,
		       vector<ColumnVector> &points);
  void initNTrack();
  void initLocalPositions();
  void updateLocalPositions();
  void updateNTrack();
  void update3DFlow();
  ColumnVector getQuantizationPostion(const ColumnVector &pos);

  /******************************
   * 変数
   ******************************/
  coords co_, dco_;
  vector<ColumnVector> points_; /* 世界座標系での値 */
  vector<ColumnVector> prvs_lpos_;
  vector<ColumnVector> crnt_lpos_;
  vector<int> ntrack_;
  int fn_; /* ntrackが１以上の点数 */


  /* MotionClassifier用の情報 (ntrack >= 1) */
  vector<ColumnVector> pres_;
  vector<ColumnVector> curs_;

  /* Camera Paramer */
  double bsln_; /* ベースライン */
  double fcs_;  /* 焦点距離 */

};

#endif
