#ifndef __VO_CORE_H__
#define __VO_CORE_H__

#include <stdio.h>
#include <string>
#include <vector>
#include <list>
#include <deque>
#include <functional>
#include "Labeling.h"
#include <fstream>
//#include <cv.h>
#include <utils/UtilTimer.h>
#include <octave_rtmex/octave_rtmex.h>
#include "EKFCoords.h"
#include "FastSLAM.h"


#include "DualQuaternionEst.h"
#include "ProbMotionEst.h"

#ifndef MAX_FN
#define MAX_FN (200)
#endif

using namespace std;

namespace rtm{
  struct StereoParams{
    void init(double _f, double _bl, double _su, double _sv, double _sd){
      f = _f; bl = _bl; su = _su; sv = _sv; sd = _sd;
    }

    double f;  /* 焦点 */
    double bl; /* ベースライン */
    double su; /* 水平方向誤差 */
    double sv; /* 垂直方向誤差 */
    double sd; /* 視差誤差 */
  };


  /******************************
   * Visual Odometry Points Data
   ******************************/
  class VOPData {
  public:
    /* 関数 */
    VOPData();
    void reset();
    void init();
    void init(const ColumnVector &lpos, const quaternion &quat,
	      const StereoParams &sprm, const Matrix qcov);
    bool update(const ColumnVector &pre, const ColumnVector &cur, const StereoParams &sprm,
		const quaternion &quat, const quaternion &dquat, const Matrix &DQCov);

    bool update(const ColumnVector &pre, const ColumnVector &cur, const StereoParams &sprm,
		const quaternion &quat, const Matrix &qcov, const quaternion &dquat);


    void updateMSWorldValues(const ColumnVector &lpos, const quaternion &quat, const Matrix qcov);


    /**** 変数 ****/
    int ntrack;   /* 追跡フレーム数 */
    int id;       /* グループID */
    bool isStatic;
    bool isEnvironment;

    /* local座標系 */
    ColumnVector cur_pos; /* 現フレームのローカル座標点 */
    ColumnVector pre_pos; /* 前フレームのローカル座標点 */

    Matrix l_cov;    /* 現フレーム点のステレオ誤差の共分散行列 */
    Matrix l_icov;   /* 現フレーム点のステレオ誤差の共分散逆行列 */

    Matrix SQCov;   /* temporary */
    Matrix iSQCov;  /* temporary */


    /* SLAM世界座標系 */
    ColumnVector worldpos;/* 世界座標点 */
    Matrix worldcov;      /* 共分散行列 */

    /* MS世界座標系(yaw=0) */
    ColumnVector rotpos;  /* 慣性センサで回転し直した点座標(yaw=0) */

    /* MS世界座標系 */
    D33 w_st_cov; /* 世界座標系点のステレオ誤差共分散行列 */
    deque<D3>  w_poss; /* MS世界座標系の位置 */
    deque<D33> w_covs; /* MS世界座標系の共分散(ステレオ誤差＋回転誤差) */
  };

  class OdmtryData{
  public:
    int ntrack;
    deque<D3> w_poss;
    deque<D33> w_covs;
  };


  /******************************
   * Visual Odometry Object Data
   ******************************/
  class VOLData{
  public:
    /**** 変数 *****/
    /*******************************
     * クラウドなくなった直後に別のラベルがはられると受けて側では続いているように見えてしまう
     * ラベルクリアされた直後は必ずその周期はラベルがはられないようにする
     * emptypをupdateのみで更新する
     **********************************/
    bool emptyp; /* このラベルIDが開いているかどうか */
    int ntrack;  /* trackされているフレーム数 */

    list<int> fidx; /* 所属するpointsへのアクセス */
    bool isStatic;

    /******************************
     * rotposを基準にする値
     ******************************/
    ColumnVector centroid; /* 重心 */
    Matrix Cov;   /* 共分散行列 */
    Matrix invCov;
    ColumnVector evals; /* 固有値 */
    Matrix evecs;       /* 固有ベクトル */
    list<int> labeling_ids; /* ラベリング領域での占めている領域のidリスト */


    /* MS世界座標系系での運動 */
    deque<D3> w_poss; /* MS世界座標系での並進変化量のログ push_front(w_poss[0]+ w_Gflow) */
    deque<D33> w_covs;


    /**** 関数 ****/
    VOLData();
    void reset();
    ColumnVector getCentroid(vector<VOPData> &ftrs);
    void getCovariance(vector<VOPData> &ftrs);
    void updateGTraj(vector<VOPData> &ftrs);
    void sortVectorElement(const ColumnVector& src, int* down_list);
  };

  class MotionClassifierCore{
  public:
    MotionClassifierCore();
    MotionClassifierCore(double cycle_dt);
    ~MotionClassifierCore();

    void init(double cycle_dt);
    void initStartOrient(const double* quat);
    void initFirstPosition();
    void convert(const double *flow, const double *avel, const double *quat,
		 const double *tvel, const double *odometry,
		 double *out_coords, double *out_ftrs);

    /**** set data ****/
    void setInputData(const double *flow, const double *avel, const double *quat,
		      const double *tvel, const double *odometry);
    int  setFeatures(const double* flow);
    void updateLabels();
    void initMSValuesofLabel(VOLData &label);
    void updateMSValuesofLabel(VOLData &label);


    /**** Objects Separate ****/
    void separateObjects();
    void trackLabels();
    bool trackLabel(VOLData& label);
    bool evalLabel(VOLData& label, bool rmftrs);
    int removeNoRigidCore(VOLData& label,bool rmfewp);
    bool GrantRigid(VOLData& label, list<double>& median_gap);
    void entry2Labels();
    int getNewLabels();
    void calcKMeans(const double* features, int fnum, int cluster_count, int* label_list,
		    vector<ColumnVector>& label_centers, int attempt);
    void fusionLabels();
    void removeLabelEntry(VOLData& label, bool rmfeatures);
    int removeNotInElipCore(VOLData& label);
    void resetAllLabel();
    //void labelFeatures(VOLData &label);
    int getLabelingIDfromPos(const ColumnVector &rotpos);
    int getLabelingIDfromPos(const ColumnVector &rotpos, const double &size,
			     const ColumnVector &stpos, const int &nx, const int &ny);
    bool checkAdj(list<int> ids, int tid);
    ColumnVector getLabelMovement(VOLData &label, int nback);
    bool evalMovingObjectIdentity(VOLData &label, double size);
    bool setLabelData(VOLData &label);
    void removeOutlierbyFlow(VOLData& label);


    void reupdateLabels();
    void reupdateLabel(VOLData &label);

    /* 静止物体評価 */
    bool evalStaticObjects(VOLData &label);
    bool evalStaticObjectsFlow(VOLData &label, int nback, double mthre,
			       int &fn, int &err_fn, double &med_mhl);
    void removeStaticObjectsBadFlow(VOLData &label, int nback, double mthre);
    double getMhlDistanceGFlowandFFlow(VOLData &label, VOPData &point, int nback);


    /**** Detect Moving Objects ****/
    void detectMovingObjects();
    int detectMaxVarLabel(const double &min_var_thre, const int &min_fn);
    int getMedianNTrack(VOLData &label, const int &min_fn);
    int detectEnvironment();
    void detectMObyEnv(int env_idx);

    double getAveDisGap(vector<ColumnVector> &pres, vector<ColumnVector> &curs);

    bool detectMOAmongLabels();
    bool evalSameRigidLabels(VOLData &src, VOLData &dst, int nback);
    void detectMObyCameraFlowWithEnv(int env_idx);

    void updateIndependLabel(VOLData &label);
    void regardDynamicLabel(int lidx);

    /**** Motion Estimation ****/
    bool estimateMotion(coords &mco, Matrix &Cov);
    //void estimateMotion(const ColumnVector &omega, coords &em_ans);
    void makeEMFeatures(vector<ColumnVector> &pres, vector<ColumnVector> &curs, int &fn);
    void estimateMotionfromOmega(const ColumnVector &omega, coords &em_ans);
    int makeEstimateDatas();

    /**** EKF Coords ****/
    quaternion klmnRotation(const quaternion &quat, const quaternion &dquat);

    ColumnVector klmnTranslation(const quaternion &quat, const ColumnVector &d_pos);
    ColumnVector makeTransEKFObserveData(const quaternion &quat, list<int> &fidx_list);
    void reflectTransEKFResult(list<int> &fidx_list);


    void ekfilterCoords(const coords &d_coords, const quaternion &quat);
    ColumnVector makeEKFObserveData(const quaternion &quat, list<int> &fidx_list);
    void reflectEKFResult(list<int> &fidx_list);
    int updateEstimateDatas();

    /// .......
    void removeErrorWorldMatch(vector<VOPData>& ftrs, vector<double>& eval_gaps,
			       vector<int>& ftrs_idx);

    void updateOdometryData(const double* odometry);

    /**** utils ****/
    void writeResultCoords(double *out_coords);
    void writeResultFeatures(double *out_ftrs);
    //void writeResult(double *out_coords, double *out_ftrs);
    void setEstimationOptions(int kalman_type, bool separatep);
    double getDistanceFromPntToPln(const ColumnVector &point, const ColumnVector &plane);

  private:
    /* Converter */
    ProbMotionEst pme_;      /* 運動推定 */
    DualQuaternionEst estmt_;/* 運動推定 */
    FastSLAM fslam_;         /* FASTSLAM */

    vector<VOPData> ftrs_;   /* 特徴点の実体 */
    vector<VOLData> labels_; /* ラベルの実体 */


    /* Parameter */
    const static int max_fn_ = MAX_FN; /* 最大特徴点数 */
    double dt_;  /* 周期 */

    int fn_;           /* OpticalFlowで発見された特徴点数 */
    quaternion cquat_; /* 現フレーム入力姿勢角 */
    quaternion pquat_; /* 前フレーム入力姿勢角 */
    quaternion dquat_; /* 姿勢角差分 */
    Matrix QCov_;      /* 現フレーム慣性姿勢角の共分散行列 */
    ColumnVector avel_;/* 慣性センサからの角速度 */
    ColumnVector pre_avel_;


    coords em_ans_;/* 運動推定結果 */
    coords estco_; /* SLAM結果 */
    Matrix estCov_;

    pthread_mutex_t mutex;
    bool separatep_ ;

    bool odometryp_;

    StereoParams sprm_; /* ステレオカメラパラム情報 */
    OdmtryData odm_data_;

    UtilTimer utimer_;

    const static int clst_maxn_ = 30;
    const static int LabelFrameSize_ = 100;

    /******************************
     * 運動推定の入力用のデータ
     ******************************/
    vector<ColumnVector> pres_, curs_;
    vector<Matrix> SCovs_, iSCovs_, iSQCovs_;
    vector<ColumnVector> wposs_;
    vector<Matrix> WCovs_;
  };
};

// 使い方
//MotionClassifierCore->InputData
//MotionClassifierCore->Convert
//MotionClassifierCore->GetResult

#endif // __VO_CORE_H__
