#ifndef __PARAMETER_H
#define __PARAMETER_H

#include <math.h>
#include "matrix.h"

template <class T>
class Vector3{
  public:
    enum Index {X_INDEX = 0, Y_INDEX, Z_INDEX};
    
  private:
    T m_X, m_Y, m_Z;
    
  public:
    Vector3() : m_X(0), m_Y(0), m_Z(0){}
    Vector3(const T &x, const T &y, const T &z)
      : m_X(x), m_Y(y), m_Z(z){}
    ~Vector3(){}

    T &operator[](unsigned index){
      switch(index){
        case X_INDEX: return m_X;
        case Y_INDEX: return m_Y;
        case Z_INDEX: return m_Z;
        default: NULL;
      }
    }
    
    void set(unsigned index, const T &value){(*this)[index] = value;}
    void setX(T x){set(X_INDEX, x);}
    void setY(T y){set(Y_INDEX, y);}
    void setZ(T z){set(Z_INDEX, z);}
    
    T get(unsigned index) const{return (*const_cast<Vector3 *>(this))[index];}
    T getX() const{return get(X_INDEX);}
    T getY() const{return get(Y_INDEX);}
    T getZ() const{return get(Z_INDEX);}
    
    Vector3<T> operator*(const T &t) const{
      Vector3<T> result;
      for(int i = 0; i < 3; i++){result[i] = get(i) * t;}
      return result;
    }
    
    Vector3<T> operator/(const T &t) const{
      Vector3<T> result;
      for(int i = 0; i < 3; i++){result[i] = get(i) / t;}
      return result;
    }
    
    Vector3<T> operator+(const Vector3<T> &t) const{
      Vector3<T> result;
      for(int i = 0; i < 3; i++){result[i] = get(i) + t.get(i);}
      return result;
    }
    
    Vector3<T> operator-(const Vector3<T> &t) const{return (*this) + (t * (-1));}

    //Matrix support method
    Vector3(const Matrix<T> &matrix) throw(MatrixException){
      if(matrix.rows() == 3 && matrix.columns() == 1){
        for(int i = 0; i < 3; i++){(*this)[i] = (*const_cast<Matrix<T> *>(&matrix))(i, 0);}
      }else if(matrix.rows() == 1 && matrix.columns() == 3){
        for(int i = 0; i < 3; i++){(*this)[i] = (*const_cast<Matrix<T> *>(&matrix))(0, i);}
      }else{
        throw MatrixException("Operatiorn void!! ; Need Matrix(3, 1) or Matrix(1, 3)");
      }
    }
        
    Matrix<T> toMatrix() const{
      Matrix<T> matrix = Matrix<T>(3, 1);
      for(int i = 0; i < 3; i++){matrix(i, 0) = (*const_cast<Vector3<T> *>(this))[i];}
      return matrix;
    }
};

template <class T>
class Quarternion{
  private:
    T m_q0, m_q1, m_q2, m_q3;
    Quarternion<T> *regularized;
  public:
    Quarternion()
      : m_q0(0), m_q1(0), m_q2(0), m_q3(0){regularized = NULL;}
    Quarternion(T q0, T q1, T q2, T q3)
      : m_q0(q0), m_q1(q1), m_q2(q2), m_q3(q3){regularized = NULL;}
    ~Quarternion(){if(regularized){delete(regularized);}}
    
    T &operator[](unsigned index){
      switch(index){
        case 0: return m_q0;
        case 1: return m_q1;
        case 2: return m_q2;
        case 3: return m_q3;
        default: NULL;
      }
    }
    void set(unsigned index, const T &value){(*this)[index] = value;}
    T get(unsigned index) const{return (*const_cast<Quarternion *>(this))[index];}
    
    T abs() const{
      T result(0);
      for(int i = 0; i < 4; i++){result += pow(get(i), 2);}
      return pow(result, 0.5);
    }
    Quarternion<T> regularize() const{
      if(!regularized){
        const_cast<Quarternion<T> *>(this)->regularized = new Quarternion<T>;
        T denominator = abs();
        for(unsigned i = 0; i < 4; i++){(*regularized)[i] = get(i) / denominator;}
      }
      return *regularized;
    }
    
    T getTheta() const{return acos(regularize()[0]) * 2;}
    Vector3<T> getAxis() const{
      Vector3<T> axis;
      Quarternion<T> r = regularize();
      T theta = r.getTheta();
      for(int i = 0; i < 3; i++){axis[i] = r[i + 1] / sin(theta / 2);}
      return axis;
    }

    Matrix<T> getDCM() const{
      Quarternion<T> r = regularize();
      Matrix<T> dcm = Matrix<T>(3, 3);
      {
        dcm(0, 0) = pow(r[0], 2) + pow(r[1], 2) - pow(r[2], 2) - pow(r[3], 2);
        dcm(1, 0) = (r[1] * r[2] - r[0] * r[3]) * 2;
        dcm(2, 0) = (r[1] * r[3] + r[0] * r[2]) * 2;
     
        dcm(0, 1) = (r[1] * r[2] + r[0] * r[3]) * 2;
        dcm(1, 1) = pow(r[0], 2) - pow(r[1], 2) + pow(r[2], 2) - pow(r[3], 2);
        dcm(2, 1) = (r[2] * r[3] - r[0] * r[1]) * 2;
       
        dcm(0, 2) = (r[1] * r[3] - r[0] * r[2]) * 2;
        dcm(1, 2) = (r[2] * r[3] + r[0] * r[1]) * 2;
        dcm(2, 2) = pow(r[0], 2) - pow(r[1], 2) - pow(r[2], 2) + pow(r[3], 2); 
      }
      return dcm;
    }
    
    void operator=(const Quarternion<T> &q){
      regularized = NULL;
      for(int i = 0; i < 4; i++){set(i, q.get(i));}
    }
    Quarternion<T> operator*(const T &t) const{
      Quarternion<T> result;
      for(int i = 0; i < 4; i++){result[i] = get(i) * t;}
      return result;
    }
    Quarternion<T> operator/(const T &t) const{
      Quarternion<T> result;
      for(int i = 0; i < 4; i++){result[i] = get(i) / t;}
      return result;
    }
    
    Quarternion<T> operator+(const Quarternion<T> &t) const{
      Quarternion<T> result;
      for(int i = 0; i < 4; i++){result[i] = get(i) + t.get(i);}
      return result;
    }
};

#endif /* __PARAMETER_H */