#include "parameter.h"

#ifndef __EQUATION_H
#define __EQUATION_H

using namespace std;

template <class T>
class EulerEoM{
  private:
    Vector3<T> m_Torque;
    Vector3<T> m_ProductI;
  public:
    EulerEoM(){}
    EulerEoM(const Vector3<T> &torque, const Vector3<T> &productI)
      : m_Torque(torque), m_ProductI(productI){}
    ~EulerEoM(){}
    
    void setTorque(const Vector3<T> &torque){m_Torque = torque;}
    void setProductI(const Vector3<T> &productI){m_ProductI = productI;}
    void set(const Vector3<T> &torque, const Vector3<T> &productI){
      setTorque(torque);
      setProductI(productI);
    }
    
    Vector3<T> operator()(T t, const Vector3<T> &omega) const{
      Vector3<T> result;
            
      for(int i = omega.X_INDEX; i <= omega.Z_INDEX; i++){
         result[i] = (m_Torque.get(i)
            - (m_ProductI.get((i + 2) % 3) - m_ProductI.get((i + 1) % 3))
            * (omega.get((i + 1) % 3) * omega.get((i + 2) % 3)))
            / m_ProductI.get(i);
      }
      return result;
    }
};

template <class T>
class QuarternionEoM{
  private:
    Vector3<T> m_Omega;
  public:
    QuarternionEoM(){}
    QuarternionEoM(const Vector3<T> &omega) : m_Omega(omega) {}
    ~QuarternionEoM(){}
    
    void setOmega(const Vector3<T> &omega){m_Omega = omega;}
    
    Quarternion<T> operator()(T t, const Quarternion<T> &q) const{
      Quarternion<T> result;
      result[0] = (-q.get(1) * m_Omega.get(0) + -q.get(2) * m_Omega.get(1) + -q.get(3) * m_Omega.get(2)) / 2;
      result[1] = ( q.get(0) * m_Omega.get(0) + -q.get(3) * m_Omega.get(1) +  q.get(2) * m_Omega.get(2)) / 2;
      result[2] = ( q.get(3) * m_Omega.get(0) +  q.get(0) * m_Omega.get(1) + -q.get(1) * m_Omega.get(2)) / 2;
      result[3] = (-q.get(2) * m_Omega.get(0) +  q.get(1) * m_Omega.get(1) +  q.get(0) * m_Omega.get(2)) / 2;
      return result;
    }
    
};

#endif /* __EQUATION_H */