#pragma once

#include <iostream>
#include <Eigen/Dense>


// Equation of Motion
template <class T> class EoM 
{
  private:
    Eigen::MatrixXd A_;
    Eigen::MatrixXd B_;
    Eigen::VectorXd u_;

  public:
    // Matrix A, B and initial controll input u
    EoM(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B,
        const Eigen::VectorXd &u) :
      A_(A), B_(B), u_(u)
    {
    }

    // Controll input
    void inputControll(const Eigen::VectorXd &u)
    {
      u_ = u;
    }

    // dx/dt = f(x, t)
    Eigen::MatrixXd operator() (const Eigen::VectorXd &x, T t) const 
    {
      return A_ * x + B_ * u_;
    }
};

#include <iostream>
#include <fstream>
#include "integrator.hpp"
#include "plant.hpp"

int main(void)
{
  Eigen::MatrixXd A(2,2);
  Eigen::MatrixXd B(2,1);
  Eigen::VectorXd x(2), dx(2);
  Eigen::VectorXd u(1);

  // System Parameter
  const double D  = 0.01;
  const double M  = 0.30;
  const double Kt = 22.0;

  // Parameters
  A << 0, 1, 0, -D/M;
  B << 0, Kt/M;
  x << 0, 0;
  dx << 0,0;
  u << 0;
  std::cout << "A, B" << std::endl;
  std::cout << A << std::endl;
  std::cout << B << std::endl;

  std::cout << "controll input" << std::endl;
  std::cout << B * u << std::endl;

  EoM<double> eom(A, B, u);

  std::ofstream ofs("out.dat");

  const double dt = 0.01;

  const double dmp = 1.0;
  const double Kp = 100.0;
  const double Kd = 2.0 * sqrt(Kp) * dmp;
  const double Mn = 0.3;
  const double Ktn = 22.0;
  Eigen::VectorXd x_cmd(2);
  x_cmd << 0.0, 0.0;

  for(double t=0.0; t<10.0; t+=dt) {
      if( t>=1.00 ) {
          x_cmd << 1.0, 0.0;
      }
      u[0] = (Mn / Ktn) * (Kp * (x[0] - x_cmd[0]) + Kd * (x[1] - x_cmd[1]) );
      // cout << u << endl;
      eom.inputControll(u);
      x = Euler1st(eom, x, t, dt);
      ofs << t << "," << x[0] << "," << x[1] << std::endl;
  }
  
}

#pragma once

// 1st Euler Method
template <class Function, class V1, class V2>
V1 Euler1st(Function f, V1 x, V2 t, V2 delta){
  return x + f(x, t) * delta;
}

// 2nd Euler Method
template <class Function, class V1, class V2>
V1 Euler2nd(Function f, V1 x, V2 t, V2 delta){
  V1 k1 = f(x, t) * delta;
  V1 k2 = f(x + k1, t + delta) * delta;
  return x + (k1 + k2)/2;
}

// Runge Kutta Method
template <class Function, class V1, class V2>
V1 RungeKutta4th(Function f, V1 x, V2 t, V2 delta){
  V1 k1 = f(x, t) * delta;
  V1 k2 = f(x + k1/2, t + delta/2) * delta;
  V1 k3 = f(x + k2/2, t + delta/2) * delta;
  V1 k4 = f(x + k3, t + delta) * delta;
  return x + (k1 + k2*2 + k3*2 + k4)/6;
}