//
// Created by jholder on 21.10.21.
//

#include "Integratoren2d_forceless.h"
#include "vec_trafos.h"

void Integratoren2d_forceless::Set0_deterministic(Rod2d &rod2D,
                                                  Simulation & /*sim*/) {
    auto trans_lab = Eigen::Vector2d({0, 0.01});
    rod2D.setPos(rod2D.getPos() + trans_lab);
    Eigen::Rotation2D<double> rot(0.1);
    rod2D.setE(rot * rod2D.getE());
}

void Integratoren2d_forceless::Set1_Euler(Rod2d &rod2D, Simulation &sim) {
    auto std = sim.getSTD();  // sqrt(2*delta_T)
    Eigen::Vector2d e = rod2D.getE();
    // translation
    Eigen::Vector2d rand = {sim.getNorm(std), sim.getNorm(std)};
    Eigen::Vector2d trans_part = rod2D.getDiff_Sqrt() * rand;
    Eigen::Vector2d trans_lab = rotation_Matrix(e) * trans_part;

    // rotation
    double rot = sim.getNorm(std) * rod2D.getDRot_Sqrt();
    Eigen::Vector2d new_e = e + rot * ortho(e);

    // apply
    rod2D.setPos(rod2D.getPos() + trans_lab);
    rod2D.setE(new_e.normalized());
}

void Integratoren2d_forceless::Set2_Heun(Rod2d &rod2D, Simulation &sim) {
    // Predict with Euler
    auto std = sim.getSTD();
    Eigen::Vector2d e = rod2D.getE();

    Eigen::Vector2d rand = Eigen::Vector2d(sim.getNorm(std), sim.getNorm(std));
    Eigen::Vector2d trans_part =
        rod2D.getDiff_Sqrt() * rand;
    Eigen::Vector2d trans_lab = rotation_Matrix(e) * trans_part;
    Eigen::Vector2d pos_predictor = rod2D.getPos() + trans_lab;

    // rotation
    double rot_predict = sim.getNorm(std) * rod2D.getDRot_Sqrt();
    Eigen::Vector2d delta_e_predict = rot_predict * ortho(e);
    Eigen::Vector2d e_predict = (e + delta_e_predict).normalized();
    double rot = sim.getNorm(std) * rod2D.getDRot_Sqrt();
    Eigen::Vector2d e_integrated =
        e + 0.5 * (rot * ortho(e) + rot * ortho(e_predict));

    // apply
    rod2D.setPos(pos_predictor); // pos with Euler
    rod2D.setE(e_integrated.normalized());
}

void Integratoren2d_forceless::Set3_Exact(Rod2d &rod2D, Simulation &sim) {
    auto std = sim.getSTD();
    Eigen::Vector2d e = rod2D.getE();
    // translation
    Eigen::Vector2d rand = {sim.getNorm(std), sim.getNorm(std)};
    Eigen::Vector2d trans_part = rod2D.getDiff_Sqrt() * rand;
    Eigen::Vector2d trans_lab = rotation_Matrix(e) * trans_part;

    // rotation
    double rot = sim.getNorm(std) * rod2D.getDRot_Sqrt();
    auto correction =
        -0.5 * pow(rod2D.getDRot(), 2)  * sim.getMDeltaT();
    Eigen::Vector2d delta_e = correction * e + rot * ortho(e);

    // apply
    rod2D.setPos(rod2D.getPos() + trans_lab);
    rod2D.setE((e + delta_e).normalized());
}

// this is a slow implementation to keep the same data structure for performance
// analysis this must be altered
// Normalisation should not be necessary if a proper angular representation
// is used. But with vector e it is done in case of numerical errors
void Integratoren2d_forceless::Set4_BDAS(Rod2d &rod2D, Simulation &sim) {
    auto std = sim.getSTD();
    Eigen::Vector2d e = rod2D.getE();
    // translation
    auto rand = Eigen::Vector2d(sim.getNorm(std), sim.getNorm(std));
    auto trans_part =
        rod2D.getDiff_Sqrt() * rand;  // translation vector in Particle System
    Eigen::Vector2d trans_lab = rotation_Matrix(e) * trans_part;

    auto rot =
        Eigen::Rotation2D<double>(sim.getNorm(std) * rod2D.getDRot_Sqrt());
    Eigen::Rotation2Dd rotation(rot);
    auto e_new = (rotation.toRotationMatrix() * e).normalized();

    rod2D.setPos(rod2D.getPos() + trans_lab);
    rod2D.setE(e_new);
}

void Integratoren2d_forceless::Set5_MBD(Rod2d & rod2D,
                                        Simulation & sim) {

  auto std = sim.getSTD();
  Eigen::Vector2d e = rod2D.getE();

  //rotation
  double rot_predict = sim.getNorm(std) *
  rod2D.getDRot_Sqrt();
  auto delta_e_predict = rot_predict * ortho(e);
  auto e_predict = (e + delta_e_predict).normalized();


  Eigen::Matrix2d Diff_combined =  0.5*(rod2D.getDiff() + rotation_Matrix(e).inverse() * rotation_Matrix(e_predict)*rod2D.getDiff());
  Eigen::Matrix2d D_combined_sqrt =  Diff_combined.llt().matrixL();

  auto rand = Eigen::Vector2d(sim.getNorm(std), sim.getNorm(std));
  Eigen::Vector2d pos_integrated =rod2D.getPos() + rotation_Matrix(e) * D_combined_sqrt * rand;

  double rot = sim.getNorm(std) * rod2D.getDRot_Sqrt();
  Eigen::Vector2d e_integrated = e + 0.5 * (rot * ortho(e) + rot * ortho(e_predict));
  //apply
  rod2D.setPos(pos_integrated);
  rod2D.setE(e_integrated.normalized());

}