#ifndef __EST_UTIL_H__
#define __EST_UTIL_H__

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

using namespace std;

namespace rtm{
  class D3{
  public:
    /* constructor */
    D3(double init=0.0);
    D3(double x, double y, double z);
    D3(double v[]);
    D3(const ColumnVector &a);
    ~D3();

    /* operator */
    const double &operator()(int i) const{ return vec[i];}
    double &operator()(int i) { return vec[i];}
    D3 &operator=(const D3 &a);
    D3 operator+(const D3 &a) const;
    D3 operator-(const D3 &a) const;

    friend D3 operator*(const D3 &a, double b);
    friend D3 operator*(double b, const D3 &a);
    friend D3 operator/(const D3 &a, double b);


    /* function */
    double norm();

    double vec[3];
  };



  class D33{
  public:
    /* constructor */
    D33(double init = 0.0);
    D33(const Matrix& a);

    /* operator */
    const double &operator()(int i,int j) const{ return mat[i][j];}
    double &operator()(int i, int j) { return mat[i][j];}
    D33 &operator=(const D33 &a);
    D33 operator+(const D33 &a) const;
    D33 operator-(const D33 &a) const;
    D3 operator*(const D3 &a) const;
    D33 operator*(const D33 &a) const;

    friend D33 operator*(const D33 &a, double b);
    friend D33 operator*(double b, const D33 &a);

    D33 transpose() const;
    D33 inverse() const;

    double mat[3][3];
  };

  /* */
  ColumnVector D32ColumnVector(const D3 &a);
  Matrix D332Matrix(const D33 &a);

  ColumnVector makeColumnVectorfromD3(const D3 &a);
  Matrix makeMatrixfromD33(const D33 &a);
  double getMhlnbsDistance(const D33 &mat, const D3 &vec);
  D33 makeStereoErrorD33(const D3 &lpos,
			 double b, double f, double sgm_u, double sgm_v, double sgm_d);
  Matrix inverseMatrix(const Matrix &a);


  ColumnVector getMaxValEigVector(Matrix& mat);
  void getRandamList(int srcn, int dstn, list<int> &result);
  double calc3DEquation(double a, double b, double c, double d, int N, double x0);

  /* インライン */
  //template<class T> T getAverage(vector<T> srcs);
  template<class T> T getAverage(vector<T> srcs){
    int n = static_cast<int>(srcs.size());
    T ans = srcs[0];
    for(int i=1; i<n; i++){
      ans = ans + srcs[i];
    }
    ans = ans / n;
    return ans;
  }


  /* 角速度<=>姿勢角 */
  ColumnVector dquat2avel(const quaternion &dquat, const double &dt);
  ColumnVector dquat2avel(const ColumnVector &dquat, const double &dt);
  quaternion avel2dquat(const ColumnVector &avel, const double &dt);
  quaternion getDQuat(const quaternion &pre_quat, const quaternion &cur_quat);

  /* 共分散行列の作成 */
  Matrix makeDQuatCov(const quaternion &quat);
  Matrix makeAVelCov(const ColumnVector &avel);

  /********************
   *
   * 偏微分(ヤコビアン)
   *
   ********************/
  /* world特徴点 */
  Matrix dpWorldPointbyQuat(const ColumnVector &lpos, const ColumnVector &quat);

  /* local特徴点 */
  Matrix dpLocalPointbyQuat(const ColumnVector &wpos, const quaternion &quat);
  Matrix dpLocalPointbyWorldPoint(const ColumnVector &wpos, const ColumnVector &co);
  Matrix dpLocalPointbyCoords(const ColumnVector &wpos, const ColumnVector &co);
  Matrix dpLocalPointbyQuat(const ColumnVector &wpos, const ColumnVector &co);

  /* クォータニオン */
  Matrix dpQuatbyAVel(const ColumnVector &quat, const double &dt);

  /* ステレオ誤差 */


  /********************
   * srcからslistに入っている順番の物をdstに入れる．(インライン)
   ********************/
  template <class T> void selectContents(const vector<T> &src, list<int> &slist, vector<T> &dst){
    /* 目標の初期化 */
    dst.clear();
    dst.reserve(slist.size());

    list<int>::iterator it = slist.begin();
    for( ; it!=slist.end(); it++ ){
      dst.push_back(src[*it]);
    }
  }

  /**** 3x3逆行列計算 ****/
  bool calcInverseMatrix(double *src, double *dst);
};

std::ostream &operator << (std::ostream &s, const rtm::D3 &v);
std::ostream &operator << (std::ostream &s, const rtm::D33 &m);

#endif
