#include "Integratoren.hpp"
constexpr double kBT = 1.0;

void Integratoren::integrateITOEuler_1(Rod2d &rod)
{
  //DLOG_SCOPE_F(1,"Integrateing Rod2d:");
  double movePara = std::sqrt(2.0 * rod.Dpara * rod.deltaT) * rod.dis(rod.generator);
  double moveOrtho = std::sqrt(2.0 * rod.Dortho * rod.deltaT) * rod.dis(rod.generator);

  double tmpx1 = rod.x1 + movePara * rod.e1 - moveOrtho * rod.e2;
  double tmpx2 = rod.x2 + movePara * rod.e2 + moveOrtho * rod.e1;

  double rodOrtho = std::sqrt(2.0 * rod.Drot * rod.deltaT) * rod.dis(rod.generator);
  double tmpe1 = rod.e1 - rodOrtho * rod.e2;
  double tmpe2 = rod.e2 + rodOrtho * rod.e1;

  rod.x1 = tmpx1;
  rod.x2 = tmpx2;
  rod.e1 = tmpe1;
  rod.e2 = tmpe2;
  rod.normalize();
}

void Integratoren::integrateITOHeun_2(Rod2d &rod)
{
  //DLOG_SCOPE_F(1,"Integrateing Rod2d:");

  double movePara = std::sqrt(2.0 * rod.Dpara * rod.deltaT) * rod.dis(rod.generator);
  double moveOrtho = std::sqrt(2.0 * rod.Dortho * rod.deltaT) * rod.dis(rod.generator);

  double tmpx1 = rod.x1 + movePara * rod.e1 - moveOrtho * rod.e2;
  double tmpx2 = rod.x2 + movePara * rod.e2 + moveOrtho * rod.e1;


  double rodOrtho = std::sqrt(2.0 * rod.Drot * rod.deltaT) * rod.dis(rod.generator);
  double e1 = rod.e1 - rodOrtho * rod.e1;
  double e2 = rod.e2 + rodOrtho * rod.e2;
  double norm = std::sqrt(e1 * e1 + e2 * e2);
  e1 /= norm;
  e2 /= norm;

  double rodOrtho1 = std::sqrt(2.0 * rod.Drot * rod.deltaT);
  double rodOrthoRand = rod.dis(rod.generator);
  double tmpe1 = rod.e1 - 0.5 * (rodOrtho1 * rod.e2 + rodOrtho1 * e2) * rodOrthoRand;//TODO Check if 0.5 faktor
  double tmpe2 = rod.e2 + 0.5 * (rodOrtho1 * rod.e1 + rodOrtho1 * e1) * rodOrthoRand;

  rod.x1 = tmpx1;
  rod.x2 = tmpx2;
  rod.e1 = tmpe1;
  rod.e2 = tmpe2;
  rod.normalize();
}


void Integratoren::integrateExact_3(Rod2d &rod)
{
  //DLOG_SCOPE_F(1,"Integrateing Rod2d:");

  double movePara = std::sqrt(2.0 * rod.Dpara * rod.deltaT) * rod.dis(rod.generator);
  double moveOrtho = std::sqrt(2.0 * rod.Dortho * rod.deltaT) * rod.dis(rod.generator);

  double tmpx1 = rod.x1 + movePara * rod.e1 - moveOrtho * rod.e2;
  double tmpx2 = rod.x2 + movePara * rod.e2 + moveOrtho * rod.e1;

  double rodOrtho = std::sqrt(2.0 * rod.Drot * rod.deltaT) * rod.dis(rod.generator);
  double korr = -0.5 * rod.Drot * rod.Drot / kBT / kBT * rod.deltaT;
  double tmpe1 = rod.e1 - rodOrtho * rod.e2 + korr * rod.e1;
  double tmpe2 = rod.e2 + rodOrtho * rod.e1 + korr * rod.e2;

  rod.x1 = tmpx1;
  rod.x2 = tmpx2;
  rod.e1 = tmpe1;
  rod.e2 = tmpe2;
  rod.normalize();
}

void Integratoren::integrateBDAS_4(Rod2d &rod)
{
  double deltaRTpara = std::sqrt(2.0 * rod.Dpara * rod.deltaT) * rod.dis(rod.generator);
  double deltaRTortho = std::sqrt(2.0 * rod.Dortho * rod.deltaT) * rod.dis(rod.generator);
  double tmpx1 = rod.x1 + deltaRTpara * rod.e1 - deltaRTortho * rod.e2;
  double tmpx2 = rod.x2 + deltaRTpara * rod.e2 + deltaRTortho * rod.e1;

  double deltaPhi = std::sqrt(2.0 * rod.Drot * rod.deltaT) * rod.dis(rod.generator);
  double tmpe1 = rod.e1 * std::cos(deltaPhi) - rod.e2 * std::sin(deltaPhi);
  double tmpe2 = rod.e1 * std::sin(deltaPhi) + rod.e2 * std::cos(deltaPhi);

  rod.x1 = tmpx1;
  rod.x2 = tmpx2;
  rod.e1 = tmpe1;
  rod.e2 = tmpe2;
  rod.normalize();
}

void Integratoren::integrateMBD_5(Rod2d &rod)
{
  /*
    double movePara = std::sqrt(2.0*rod.Dpara*rod.deltaT)*rod.dis(rod.generator); // Taken from euler
    double moveOrtho = std::sqrt(2.0*rod.Dortho*rod.deltaT)*rod.dis(rod.generator); // taken from euler
    double x1 = rod.x1 + movePara*rod.e1- moveOrtho*rod.e2;
    double x2 = rod.x2 + movePara*rod.e2+ moveOrtho*rod.e1;
    */
  // unused without force
  double rodOrtho = std::sqrt(2.0 * rod.Drot * rod.deltaT) * rod.dis(rod.generator);
  double e1 = rod.e1 - rodOrtho * rod.e2;
  double e2 = rod.e2 + rodOrtho * rod.e1;
  double norm = std::sqrt(e1 * e1 + e2 * e2);
  e1 /= norm;
  e2 /= norm;

  double movePara1 = std::sqrt(1.0 * rod.Dpara * rod.deltaT) * rod.dis(rod.generator);// If difussion would be time dependend, average of the predicted D and now D
  double moveOrtho1 = std::sqrt(1.0 * rod.Dortho * rod.deltaT) * rod.dis(rod.generator);
  double movePara2 = std::sqrt(1.0 * rod.Dpara * rod.deltaT) * rod.dis(rod.generator);// If difussion would be time dependend, average of the predicted D and now D
  double moveOrtho2 = std::sqrt(1.0 * rod.Dortho * rod.deltaT) * rod.dis(rod.generator);
  double tmpx1 = rod.x1 + (movePara1 * rod.e1 - moveOrtho1 * rod.e2) + (movePara2 * e1 - moveOrtho2 * e2);
  double tmpx2 = rod.x2 + (movePara1 * rod.e2 + moveOrtho1 * rod.e1) + (movePara2 * e2 + moveOrtho2 * e1);


  double rodOrtho1 = std::sqrt(2.0 * rod.Drot * rod.deltaT);
  double rodOrtho2 = rod.dis(rod.generator);

  double tmpe1 = rod.e1 - 0.5 * (rodOrtho1 * rod.e2 + rodOrtho1 * e2) * rodOrtho2;
  double tmpe2 = rod.e2 + 0.5 * (rodOrtho1 * rod.e1 + rodOrtho1 * e1) * rodOrtho2;

  rod.x1 = tmpx1;
  rod.x2 = tmpx2;
  rod.e1 = tmpe1;
  rod.e2 = tmpe2;
  rod.normalize();
}