/*************************************************
 *                   octave_rtmex.h
 *
 *
 * written by T.Goto
 *************************************************/

#ifndef __MYOCTAVE_H__
#define __MYOCTAVE_H__


#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <math.h>
#include <octave/config.h>
#include <octave/Matrix.h>

using namespace std;

namespace rtm {
  /********** 型宣言 **********/
  // Initは ColumnVector(4) onlyならばquaternionの生の値
  // double + ColumnVector(3) ならば ang + axisとする．
  class quaternion{
  public:
    quaternion();
    quaternion(const ColumnVector& a);
    quaternion(const double& rad, const ColumnVector& axis);

    ////
    quaternion& operator=(const quaternion& q);
    quaternion operator+(const quaternion& q) const;
    quaternion operator-(const quaternion& q) const;
    quaternion operator*(const quaternion& q) const;

    void init(const ColumnVector& q); //注意 四元数の実際の値を代入する
    void init(const double& rad, const ColumnVector& axis);
    void reset();
    void normalize();
    quaternion inverse();

    ColumnVector rotate_vector(const ColumnVector& vec) const;
    ColumnVector inverse_rotate_vector(const ColumnVector& vec) const;

    string to_string() const; // operator<< overwirte用
    ColumnVector to_vector() const;

    /**** 変数 ****/
    double t;      //実部
    ColumnVector V;//虚部
  };
  


  class coords{
  public:
    coords();
    coords(const ColumnVector& incoords);
    coords(const ColumnVector& pos, const quaternion& rotation);
    coords(const ColumnVector& pos, const ColumnVector& rotation);

    coords& operator=(const coords& co);

    void init(const ColumnVector& incoords);
    void init(const ColumnVector& pos, const quaternion& rotation);
    void init(const ColumnVector& pos, const ColumnVector& rotation);
    void init(const ColumnVector& pos, const double& rad, const ColumnVector& axis);
    void reset();

    void initWorldpos(const ColumnVector& pos);
    void initRotation(const ColumnVector& rotation);
    void initRotation(const double& rad, const ColumnVector& axis);

    ColumnVector transform_vector(ColumnVector vec) const;
    ColumnVector inverse_transform_vector(ColumnVector vec) const;

    void translate(const ColumnVector& pos, bool world=true);
    void rotate(quaternion& rotation, bool world=true);
    void invrotate(quaternion& rotation, bool world=true);
    void transform(const coords& co, bool world=true);
    void inverse_transform(const coords& co, bool world=true);

    string to_string() const; // operator<< overwirte用
    ColumnVector to_vector() const;


    ColumnVector worldpos;
    quaternion quat;
  };



  /********** 関数 **********/
  /**** vector ****/
  // 内積
  double vector_ip(ColumnVector v1, ColumnVector v2);
  // 外積
  ColumnVector vector_op(ColumnVector v1, ColumnVector v2);
  void normalize_vector(ColumnVector& v1);
  double vector_angle(const ColumnVector& v1, const ColumnVector& v2);

  /**** matrix ****/
  Matrix make_unit_matrix(int i);
  Matrix remove_row(const Matrix& a, int r);
  Matrix remove_rows(const Matrix& a, int r, int n);
  Matrix remove_column(const Matrix& a, int i);
  Matrix remove_columns(const Matrix& a, int c, int n);
  Matrix remove_matrix(const Matrix& a, int c, int r, int nc, int nr);
  Matrix add_matrix(const Matrix& a,const Matrix& b);
  Matrix makeSkewMatixfromVector(const ColumnVector& vec);

  /**** quaternion ****/
  quaternion quat_cross(const quaternion& Q1, const quaternion& Q2); //quatrnion内部のoperator*推奨
  void normalize_quat(quaternion& quat);
  /*
   * ４元数はq=-qなので，どちらが正しいか常に意識した方がよい．
   * tが十分に大きいときはtを信用して，tが小さいときは一番大きい値を信じる．
   */
  void normalize_quat_sgn(quaternion &quat);
  void syncro_quats_sgn(quaternion &src, const quaternion &reference);

  /**** 回転表式変換 ****/
  Matrix quat2matrix(const quaternion &quat);
  quaternion matrix2quat(const Matrix &mat);
  ColumnVector matrix2rpy(const Matrix &mat);
  Matrix rpy2matrix(const ColumnVector &rpy);
  Matrix rpy2matrix(double roll, double pitch, double yaw);
  ColumnVector quat2rpy(const quaternion &quat);
  quaternion rpy2quat(const ColumnVector &rpy);

  /**** 角度変換 ****/
  double rad2deg(double rad);
  double deg2rad(double deg);

  ColumnVector quat2avel(const ColumnVector &dquat, const double &dt);
  quaternion avel2quat(const ColumnVector &avel, const double &dt);

  /**** その他 ****/
  Matrix makeStereoErrorCov(const ColumnVector &lpos,
			    double b, double f, double sgm_u, double sgm_v, double sgm_d);
  template<class T> T get_sgn(T in);
  template<class T> ColumnVector makeColumnVectorfromArray(const int &dim, const T *array){
    ColumnVector ans(dim,0);
    for(int i=0; i<dim; i++) ans(i) = static_cast<double>(array[i]);
    return ans;
  }

};

ostream& operator<<(ostream&s, const rtm::coords& x);
ostream& operator<<(ostream&s, const rtm::quaternion& x);
#endif // __MYOCTAVE_H__
