/*
 * RKF45
 * 2
 * Eigen
 *
 * by 
 */

#include 
#include 
#include 
#include 
#include 

#include 
#include 

#include 
#include 
#include 

#include 
// 
constexpr double g           = 1.0;//9.8
constexpr double m1          = 1.0;
constexpr double m2          = 1.0;
constexpr double L1          = 1.0;
constexpr double L2          = 0.4;
//
constexpr double theta1_init = 3.0;
constexpr double theta2_init = 3.0;
//
constexpr double omega1_init = 0.;
constexpr double omega2_init = 0.;

//
double dt                    = 10e-3;
constexpr double t_limit     = 10000.0;

constexpr double e_tol = 10e-10;
constexpr double t_min = 10e-6;

//
constexpr int INTV = 40;

//movie
constexpr int sim = 0;

//R^4R^4
template 
Eigen::Matrix func(const Eigen::Matrix& x){
    T theta1 = x(0, 0);
    T eta1   = x(1, 0);
    T theta2 = x(2, 0);
    T eta2   = x(3, 0);

    return Eigen::Matrix {
        eta1,
        (- m1 * g * std::sin(theta1) - m2 * (g * std::sin(theta1) + L2 * eta2 * eta2 * std::sin(theta1 - theta2) + (L1 * eta1 * eta1 * std::sin(theta1 - theta2) - g * std::sin(theta2)) * std::cos(theta1 - theta2))) / (L1 * (m1 + m2 * (std::sin(theta1 - theta2) * (std::sin(theta1 - theta2))))),
        eta2,
        ((m1 + m2) * (L1 * eta1 * eta1 * std::sin(theta1 - theta2) - g * std::sin(theta2) + g * std::sin(theta1) * std::cos(theta1 - theta2)) + m2 * L2 * eta2 * eta2 * std::cos(theta1 - theta2) * std::sin(theta1 - theta2)) / (L2 * (m1 + m2 * std::sin(theta1 - theta2) * std::sin(theta1 - theta2)))
    };
}

template 
T potentialEnergy(Eigen::Matrix& x){
    T theta1 = x(0, 0);
    T eta1   = x(1, 0);
    T theta2 = x(2, 0);
    T eta2   = x(3, 0);

    return 0.5 * m1 * L1 * L1 * eta1 * eta1 + 0.5 * m2 * (L1 * L1 * eta1 * eta1 + L2 * L2 * eta2 * eta2 + 2. * L1 * L2 * eta1 * eta2 * std::cos(theta1 - theta2));
}

template 
T kineticEnergy(Eigen::Matrix& x){
    T theta1 = x(0, 0);
    T theta2 = x(2, 0);

    return - m1 * g * L1 * std::cos(theta1) - m2 * g * (L1 * std::cos(theta1) + L2 * std::cos(theta2));
}

int main(){
    Eigen::Matrix x(theta1_init, omega1_init, theta2_init, omega2_init);
    Eigen::Matrix x4, x5;
    std::cout