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

#include "octave_rtmex.h"

using namespace std;
using namespace rtm;

/******************** クォータニオン ********************/

quaternion::quaternion(){
  t=1;
  V=ColumnVector(3,0);
}


quaternion::quaternion(const ColumnVector& a){
  t=1;
  V=ColumnVector(3,0);
  init(a);
}

quaternion::quaternion(const double& rad, const ColumnVector& axis){
  t=1;
  V=ColumnVector(3,0);
  init(rad, axis);
}

string quaternion::to_string() const{
  ostringstream s;
  s << t << V.transpose();
  //s << t << " " << V.transpose();
  return s.str();
}

/*
 * cout << quat << endl;の様に仕える様に
 */
ostream& operator<<(ostream&s, const quaternion& x){
  return s << x.to_string();
}


ostream& operator<<(ostream&s, const coords& x){
  return s << x.to_string();
}


quaternion& quaternion::operator=(const quaternion& q){
  if(this != &q){
    t = q.t;
    V = q.V;
  }
  return *this;
}

quaternion
quaternion::operator+(const quaternion& q) const{
  quaternion ans;
  ans.t = t + q.t;
  ans.V = V + q.V;
  return ans;
}


quaternion
quaternion::operator-(const quaternion& q) const{
  quaternion ans;
  ans.t = t - q.t;
  ans.V = V - q.V;
  return ans;
}


quaternion
quaternion::operator*(const quaternion& q) const{
  quaternion ans;
  ans.t = t * q.t - vector_ip(V, q.V);
  ans.V = t * q.V + q.t * V + vector_op(V,q.V);
  return ans;
}


void
quaternion::init(const ColumnVector& q){
  if(q.rows() == 4){
    ColumnVector tmp = q;
    normalize_vector(tmp);
    t = tmp(0);
    V = tmp.extract(1,3);
  }else{
    printf("Error! Rows is not 4\n");
  }
}


void
quaternion::init(const double& rad, const ColumnVector& axis){
  if(axis.rows() != 3){
    printf("Error! axis dum is not 3\n");
  }
  else{
    double ang = rad / 2;
    ColumnVector tmp = axis;
    normalize_vector(tmp);
    t = cos(ang);
    V = tmp * sin(ang);
  }
}


void
quaternion::reset(){
  t = 1.0;
  V = ColumnVector(3,0);
}


void
quaternion::normalize(){
  normalize_quat(*this);
}

quaternion
quaternion::inverse(){
  quaternion ans;
  double norm2 = this->t * this->t + this->V(0) * this->V(0) + this->V(1) * this->V(1) + this->V(2) * this->V(2);
  ans.t = this->t / norm2;
  for(int i = 0; i < 3; i++)ans.V(i) = -this->V(i) / norm2;
  return ans;
}

ColumnVector
quaternion::rotate_vector(const ColumnVector& vec) const{
  quaternion ans, couple;
  couple.t = t;
  couple.V = -V;
  ans.t = 0;
  ans.V = vec;

  ans = *this * ans * couple;
  return ans.V;
}


ColumnVector
quaternion::inverse_rotate_vector(const ColumnVector& vec) const{
  quaternion ans, couple;
  couple.t = t;
  couple.V = -V;
  ans.t = 0;
  ans.V = vec;

  ans = couple * ans * *this;
  return ans.V;
}



ColumnVector quaternion::to_vector() const{
  ColumnVector ans(4,0);
  ans(0) = t;
  ans.insert(V,1);
  return ans;
}


/******************** coords ********************/
coords::coords(){
  ColumnVector init_pos(3,0);
  worldpos = init_pos;

  ColumnVector init_quat(4,0);
  init_quat(0) = 1;
  quat.init(init_quat);
}

coords::coords(const ColumnVector& incoords){
  init(incoords);
}

coords::coords(const ColumnVector& pos, const quaternion& rotation){
  init(pos, rotation);
}

coords::coords(const ColumnVector& pos, const ColumnVector& rotation){
  init(pos, rotation);
}

coords& coords::operator=(const coords& co){
  if(this != &co){
    worldpos = co.worldpos;
    quat = co.quat;
  }
  return *this;
}


void coords::init(const ColumnVector& incoords){
  if(incoords.rows()==7){
    worldpos = incoords.extract(0,2);
    quat.init(incoords.extract(3,6));
  }else{
    printf("coords init error! coords rows is not 7\n");
  }
}


void coords::init(const ColumnVector& pos, const quaternion& rotation){
  initWorldpos(pos);
  quat = rotation;
}

void coords::init(const ColumnVector&pos, const ColumnVector&rotation){
  initWorldpos(pos);
  initRotation(rotation);
}

void coords::init(const ColumnVector& pos, const double& rad, const ColumnVector& axis){
  initWorldpos(pos);
  initRotation(rad,axis);
}


void
coords::reset(){
  worldpos = ColumnVector(3,0);
  quat.reset();
}


void coords::initWorldpos(const ColumnVector& pos){
  if(pos.rows()!=3)
    printf("Error! pos dim is not 3\n");
  else
    worldpos = pos;
}

void coords::initRotation(const ColumnVector& rotation){
  quat.init(rotation);
}

void coords::initRotation(const double& rad, const ColumnVector& axis){
  quat.init(rad, axis);
}


ColumnVector
coords::transform_vector(ColumnVector vec) const{
  return worldpos + quat.rotate_vector(vec);
}


ColumnVector
coords::inverse_transform_vector(ColumnVector vec) const{
  return quat.inverse_rotate_vector(vec - worldpos);
}


void
coords::translate(const ColumnVector& pos, bool world){
  if(pos.rows()!=3)
    printf("can't translate vector is not 3dim\n");
  else {
    ColumnVector trans;
    if(world)
      trans = pos;
    else
      trans = quat.rotate_vector(pos);

    worldpos += trans;
  }
}


void
coords::rotate(quaternion& rotation, bool world){
  if(world)
    quat = rotation * quat;
  else
    quat = quat * rotation;
  normalize_quat(quat);
}


void
coords::invrotate(quaternion& rotation, bool world){
  quaternion rot = rotation;
  rot.t = - rot.t;
  if(world)
    quat = rot * quat;
  else
    quat = quat * rot;
  normalize_quat(quat);
}

void
coords::transform(const coords& co, bool world){
  if(world){
    worldpos = co.quat.rotate_vector(worldpos) + co.worldpos;
    quat = co.quat * quat;
  }
  else{
    worldpos = worldpos + quat.rotate_vector(co.worldpos);
    quat = quat * co.quat;
  }
}

void
coords::inverse_transform(const coords& co, bool world){
  if(world){
    quaternion cq = co.quat;
    worldpos = cq.inverse().rotate_vector(worldpos - co.worldpos);
    quat = cq.inverse() * quat;
  }
  else{
    quaternion cq = co.quat;
    worldpos = worldpos - (quat * cq.inverse()).rotate_vector(co.worldpos);
    quat = quat * cq.inverse();
  }
}


string coords::to_string() const{
  ostringstream s;
  s << worldpos.transpose() << " " << quat;
  return s.str();
}


ColumnVector coords::to_vector() const{
  ColumnVector ans(7,0);
  ans.insert(worldpos,0);
  ans(3) = quat.t;
  ans.insert(quat.V, 4);
  return ans;
}


/******************** ベクトル ********************/
//内積
double rtm::vector_ip(ColumnVector v1, ColumnVector v2){
  if(v1.rows()!=v2.rows()){
    printf("inp.error dimensions are not the same\n");
    return 0;
  }else{
    double inp = 0;
    for(int i=0; i<v1.rows(); i++)
      inp = inp + v1(i)*v2(i);
    return inp;
  }
}

//外積
ColumnVector rtm::vector_op(ColumnVector v1, ColumnVector v2){
  ColumnVector v3(3);
  v3(0) = v1(1) * v2(2) - v1(2) * v2(1);
  v3(1) = v1(2) * v2(0) - v1(0) * v2(2);
  v3(2) = v1(0) * v2(1) - v1(1) * v2(0);
  return v3;
}


//２つのベクトルの成す角度
double rtm::vector_angle(const ColumnVector& v1, const ColumnVector& v2){
  double cos_th;
  double v1_size = v1.norm(2.0);
  double v2_size = v2.norm(2.0);

  if(v1_size > 0 && v2_size > 0){
    cos_th = vector_ip(v1,v2) / (v1_size * v2_size);
    if(cos_th < 1) return (acos(cos_th));
    else return 0.0;
  }else{
    printf("error! vector norm is 0\n");
    return 0.0;
  }
}

//ベクトルの正規化
void rtm::normalize_vector(ColumnVector& v1){
  double size = v1.norm(2.0);
  v1 = v1 / size;
}

// クォータニオンの掛け合わせ
// dqを求める際など答えのnormは必ずしも１ではないので注意
quaternion rtm::quat_cross(const quaternion& Q1, const quaternion& Q2){
  quaternion Q3;
  ColumnVector tmp(3,0);
  Q3.t = Q1.t * Q2.t - vector_ip(Q1.V, Q2.V);

  tmp = (Q1.t * Q2.V) + (Q2.t * Q1.V);
  Q3.V = tmp + vector_op(Q1.V, Q2.V);
  return Q3;
}


//クォータニオンから回転行列作成
Matrix rtm::quat2matrix(const quaternion &quat){
  Matrix rot(3,3);
  double q0,q1,q2,q3;
  q0=quat.t; q1=quat.V(0);
  q2=quat.V(1); q3=quat.V(2);

  rot(0,0) = q0*q0 + q1*q1 - q2*q2 - q3*q3;
  rot(0,1) = 2*(q1*q2 - q0*q3);
  rot(0,2) = 2*(q1*q3 + q0*q2);

  rot(1,0) = 2*(q1*q2 + q0*q3);
  rot(1,1) = q0*q0 - q1*q1 + q2*q2 - q3*q3;
  rot(1,2) = 2*(q2*q3 - q0*q1);

  rot(2,0) = 2*(q1*q3 - q0*q2);
  rot(2,1) = 2*(q2*q3 + q0*q1);
  rot(2,2) = q0*q0 - q1*q1 - q2*q2 + q3*q3;
  return rot;
}

// クォータニオン正規化
void rtm::normalize_quat(quaternion& quat){
  // norm
  double q0,q1,q2,q3;
  q0 = quat.t;
  q1 = quat.V(0);
  q2 = quat.V(1);
  q3 = quat.V(2);

  if(q0 == 0 && q1 == 0 && q2 == 0 && q3 == 0)
    return;

  double norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
  quat.t = quat.t / norm;
  quat.V = quat.V / norm;
}


void rtm::normalize_quat_sgn(quaternion &quat){
  const static double kThre = 0.3;
  if( abs(quat.t) > kThre ){ // tを使って正規化
    if(quat.t < 0){
      quat.t = - quat.t;
      quat.V = - quat.V;
    }
  } else { // max_value を使って正規化
    ColumnVector quat_vec = quat.to_vector();
    int max_idx = -1;
    double max_val = -1;
    for(int i=0; i<4; i++){
      if(abs(quat_vec(i)) > max_val){
	max_idx = i;
	max_val = abs(quat_vec(i));
      }}

    if(quat_vec(max_idx)< 0){
      quat.t = - quat.t;
      quat.V = - quat.V;
    }}
}

void rtm::syncro_quats_sgn(quaternion &src, const quaternion &reference){
  const static double kThre = 0.3;
  if( abs(reference.t) > kThre ){ // tを使って同期

    if( src.t * reference.t < 0 ){
      src.t = - src.t;
      src.V = - src.V;
    }
  } else { // max valueを使って同期
    ColumnVector ref_vec = reference.to_vector();
    ColumnVector src_vec = src.to_vector();

    int max_idx = -1;
    double max_val = -1;
    for(int i=0; i<4; i++){
      if(abs(ref_vec(i)) > max_val){
	max_idx = i;
	max_val = abs(ref_vec(i));
      }}

    if(ref_vec(max_idx)*src_vec(max_idx) < 0){
      src.t = - src.t;
      src.V = - src.V;
    }}
}


//単位行列を作る
Matrix rtm::make_unit_matrix(int i){
  Matrix ans(i,i,0);
  for(int j=0; j<i; j++)
    ans(j,j)=1;
  return ans;
}


//指定した行を除去
Matrix rtm::remove_row(const Matrix& a, int r){
  return (remove_rows (a,r,1));
}

Matrix rtm::remove_rows(const Matrix& a, int r, int n){
  if (r+n > a.rows()){
    cerr << "@remove_rows size is smaller than required" << endl;
    return Matrix(0,0);
  }

  Matrix ans(a.rows() - n, a.cols());
  ans.insert(a.extract_n(0,0,r,a.cols()),0,0);
  if( r+n != a.rows())
    ans.insert(a.extract(r+n,0,a.rows()-1,a.cols()-1),r,0);
  return ans;
}


//指定した列を除去
Matrix rtm::remove_column(const Matrix &a, int c){
  return (remove_columns(a, c,1));
}


Matrix rtm::remove_columns(const Matrix& a, int c, int n){
  if (c+n > a.cols()){
    cerr << "@remove_rolumns size is smaller than required" << endl;
    return Matrix(0,0);
  }

  Matrix ans(a.rows(), a.cols()-n);
  ans.insert(a.extract_n(0,0,a.rows(),c),0,0);
  if( c+n != a.cols())
    ans.insert(a.extract(0,c+n,a.rows()-1,a.cols()-1),0,c);
  return ans;
}


// 指定した行列を除去
Matrix rtm::remove_matrix(const Matrix& a, int r, int c, int nr, int nc){
  return (remove_rows(remove_columns(a, c, nc), r, nr));
}

//行列を対角方向に足す
Matrix rtm::add_matrix(const Matrix& a,const Matrix& b){
  Matrix ans,tmp(b.rows(),a.cols(),0),tmp2(a.rows(),b.cols(),0);
  tmp2 = tmp2.stack(b);
  ans = a;
  ans = ans.stack(tmp);
  ans = ans.append(tmp2);
  return ans;
}


Matrix rtm::makeStereoErrorCov(const ColumnVector &lpos,
			       double b, double f, double sgm_u, double sgm_v, double sgm_d){
  double bb = pow(b, 2);
  double ff = pow(f, 2);
  double sgm_uu = pow(sgm_u, 2);
  double sgm_vv = pow(sgm_v, 2);
  double sgm_dd = pow(sgm_d, 2);

  double x = lpos(0);
  double y = lpos(1);
  double z = lpos(2);
  Matrix S(3,3,0);

  S(0,0) = sgm_dd * x * x / bb;
  S(1,1) = sgm_uu + sgm_dd * y * y / bb;
  S(2,2) = sgm_vv + sgm_dd * z * z / bb;

  S(0,1) = S(1,0) = sgm_dd * x * y / bb;
  S(0,2) = S(2,0) = sgm_dd * x * z / bb;
  S(1,2) = S(2,1) = sgm_dd * y * z / bb;

  S = (x*x/ff) * S;

  return S;
}

template<class T> T rtm::get_sgn(T in){
  T ans;
  if(in > 0) ans = 1;
  else if (in == 0) ans = 0;
  else ans = -1;
  return ans;
}

quaternion rtm::matrix2quat(const Matrix &mat){
  quaternion ans;
  if(mat.rows()!=3 || mat.cols()!=3){
    cout << "ERROR!! mat is not 3x3" << endl;
    return ans;
  }

  double q0,q1,q2,q3;
  double r11, r12,r13,r21,r22,r23,r31,r32,r33;
  r11=mat(0,0);r12=mat(0,1);r13=mat(0,2);
  r21=mat(1,0);r22=mat(1,1);r23=mat(1,2);
  r31=mat(2,0);r32=mat(2,1);r33=mat(2,2);

  q0 = (  r11 + r22 + r33 + 1.0) / 4.0;
  q1 = (  r11 - r22 - r33 + 1.0) / 4.0;
  q2 = (- r11 + r22 - r33 + 1.0) / 4.0;
  q3 = (- r11 - r22 + r33 + 1.0) / 4.0;

  if(q0 < 0 || q1 < 0 || q2 < 0 || q3 < 0){
    cout << "ERROR!! mat is not good" << endl;
    return ans;
  }

  q0 = sqrt(q0);
  q1 = sqrt(q1);
  q2 = sqrt(q2);
  q3 = sqrt(q3);

  q0 *= 1.0;
  q1 *= get_sgn(r32 - r23);
  q2 *= get_sgn(r13 - r31);
  q3 *= get_sgn(r21 - r12);


  ans.t = q0;
  ans.V(0) = q1;
  ans.V(1) = q2;
  ans.V(2) = q3;
  normalize_quat(ans);

  return ans;
}


ColumnVector rtm::quat2rpy(const quaternion &quat){
  Matrix rot = quat2matrix(quat);
  ColumnVector rpy = matrix2rpy(rot);
  return rpy;
}


quaternion rtm::rpy2quat(const ColumnVector &rpy){
  Matrix rot = rpy2matrix(rpy);
  quaternion quat = matrix2quat(rot);
  return quat;
}


ColumnVector rtm::matrix2rpy(const Matrix &mat){
  double roll,pitch,yaw;
  roll = atan2(mat(2,1), mat(2,2));
  pitch = atan2(-mat(2,0), sqrt(mat(2,1)*mat(2,1) + mat(2,2)*mat(2,2)));
  yaw = atan2(mat(1,0), mat(0,0));

  ColumnVector ans(3,0);
  ans(0) = roll;
  ans(1) = pitch;
  ans(2) = yaw;

  return ans;
}


Matrix rtm::rpy2matrix(double roll, double pitch, double yaw){
  Matrix m1(3,3,0),m2(3,3,0),m3(3,3,0),tmp, ans;
  // roll
  m1(0,0)=1;
  m1(1,1) = m1(2,2) = cos(roll);
  m1(2,1) = sin(roll);
  m1(1,2) = -sin(roll);

  //pitch
  m2(1,1)=1;
  m2(0,0) = m2(2,2) = cos(pitch);
  m2(0,2) = sin(pitch);
  m2(2,0) = -sin(pitch);

  //yaw
  m3(2,2)=1;
  m3(0,0) = m3(1,1) = cos(yaw);
  m3(1,0) = sin(yaw);
  m3(0,1) = -sin(yaw);

  tmp = m2 * m1;
  ans = m3 * tmp;
  return ans;
}


Matrix rtm::rpy2matrix(const ColumnVector &rpy){
  if(rpy.rows()!=3){
    cout << "ERROR! rpy vector dim is not 3" << endl;
    return make_unit_matrix(3);
  }
  return (rpy2matrix(rpy(0),rpy(1),rpy(2)));
}


double rtm::rad2deg(double rad){
  const double kPi = 3.14159265358979323846;
  return  ((rad * 180) / kPi);
}


double rtm::deg2rad(double deg){
  const double kPi = 3.14159265358979323846;
  return ((deg * kPi) / 180.0);
}


ColumnVector rtm::quat2avel(const ColumnVector &dquat, const double &dt){
  return (2 * dquat.extract(1,3) / dt);
}


quaternion rtm::avel2quat(const ColumnVector &avel, const double &dt){
  quaternion tmp;
  tmp.V = (dt / 2) * avel;
  tmp.normalize();
  return tmp;
}


Matrix rtm::makeSkewMatixfromVector(const ColumnVector& vec){
  if(vec.rows()!=3){
    cout << "Error vector is not 3 dim. can't make skew mat" << endl;
    return Matrix(3,3,0);
  }
  Matrix skew(3,3,0);
  skew(0,1) = -vec(2);
  skew(0,2) =  vec(1);
  skew(1,2) = -vec(0);

  skew(1,0) = -skew(0,1);
  skew(2,0) = -skew(0,2);
  skew(2,1) = -skew(1,2);
  return skew;
}
