#include "sVars.h"

void burnStartTimeCalc(struct sVars &, double g);
void thrustSelection(struct sVars &, int t);
void lqrCalc(struct sVars &);
void TVC(struct sVars &, double g);
void vehicleDynamics(struct sVars &, int t);
void write2CSV(struct sVars &, std::fstream &outfile, int t);
double derivative(double x2, double x1, double dt);
double integral(double x2, double x1, double dt);

void sim(struct sVars &Vars) {
  double g = -9.81;

  // defining a few random values here cause I'm lazy
  Vars.burnElapsed = 2000;
  Vars.m = Vars.m0;
  Vars.thrust_prev = 0;

  burnStartTimeCalc(Vars, g);

  // Deleting any previous output file
  if (remove("simOut.csv") != 0)
    perror("Error deleting file");
  else
    puts("File successfully deleted");

  // Define and open output file "simOut.csv"
  std::fstream outfile;
  outfile.open("simOut.csv", std::ios::app);

  // Output file header. These are the variables that we output - useful for
  // debugging
  outfile
      << "t, x, y, z, vx, vy, vz, ax, ay, az, yaw, pitch, roll, yawdot, "
         "pitchdot, rolldot, yawddot, pitchddot, rollddot, I11, I22, I33, "
         "I11dot, I22dot, I33dot, Servo1, Servo2, m, thrust, burnElapsed, Fz, "
         "LQRx, LQRy"
      << std::endl;

  // Start Sim
  for (int t = 0; t < Vars.simTime; t++) {
    thrustSelection(Vars, t);
    lqrCalc(Vars);
    TVC(Vars, g);
    vehicleDynamics(Vars, t);
    write2CSV(Vars, outfile, t);
  }

  outfile.close();
}

void burnStartTimeCalc(struct sVars &Vars, double g) {
  double v = Vars.vz;
  double h = 0;
  double dt = 0.001;
  double a, j, m, thrust;

  for (double i = 0.148; i < 3.450; i = i + dt) {
    m = Vars.m0 - i * Vars.mdot;

    if ((i > 0.147) & (i < 0.420))
      thrust = 65.165 * i - 2.3921;

    else if ((i > 0.419) & (i < 3.383))
      thrust = 0.8932 * pow(i, 6) - 11.609 * pow(i, 5) + 60.739 * pow(i, 4) -
               162.99 * pow(i, 3) + 235.6 * pow(i, 2) - 174.43 * i + 67.17;

    else if ((i > 3.382) & (i < 3.46))
      thrust = -195.78 * i + 675.11;

    v = (((thrust / m) + g) * dt) + v;
    h = v * dt + h;
  }

  Vars.z = h + (pow(v, 2) / (2 * -g)); // starting height
  Vars.vb = v;                         // terminal velocity

  double burnStartTime = Vars.vb / -g;
  Vars.simTime = (Vars.tb + burnStartTime) * 1000;
}

void thrustSelection(struct sVars &Vars, int t) {
  double tol = 0.001; // 0.001 seems to be a nice tolerance

  // Check to see if current velocity is close to the F15's total velocity
  bool b_burnStart = (Vars.vb < (1 + tol) * Vars.vz * -1) &
                     (Vars.vb > (1 - tol) * Vars.vz * -1);

  if (Vars.burnElapsed != 2000) {
    // determine where in the thrust curve we're at based on elapsed burn time
    // as well as current mass
    Vars.burnElapsed = (t - Vars.burnStart) / 1000;
    Vars.m = Vars.m0 - (Vars.mdot * Vars.burnElapsed);
  }

  else if (b_burnStart) {
    // Start burn
    Vars.burnStart = t;
    Vars.burnElapsed = 0;
  }

  else
    Vars.burnElapsed = 2000; // arbitrary number to ensure we don't burn

  if ((Vars.burnElapsed > 0.147) & (Vars.burnElapsed < 0.420))
    Vars.thrust = 65.165 * Vars.burnElapsed - 2.3921;

  else if ((Vars.burnElapsed > 0.419) & (Vars.burnElapsed < 3.383))
    Vars.thrust =
        0.8932 * pow(Vars.burnElapsed, 6) - 11.609 * pow(Vars.burnElapsed, 5) +
        60.739 * pow(Vars.burnElapsed, 4) - 162.99 * pow(Vars.burnElapsed, 3) +
        235.6 * pow(Vars.burnElapsed, 2) - 174.43 * Vars.burnElapsed + 67.17;

  else if ((Vars.burnElapsed > 3.382) & (Vars.burnElapsed < 3.46))
    Vars.thrust = -195.78 * Vars.burnElapsed + 675.11;
}

void lqrCalc(struct sVars &Vars) {

  Vars.I11 = Vars.m * ((1 / 12) * pow(Vars.vehicleHeight, 2) +
                       pow(Vars.vehicleRadius, 2) / 4);
  Vars.I22 = Vars.m * ((1 / 12) * pow(Vars.vehicleHeight, 2) +
                       pow(Vars.vehicleRadius, 2) / 4);
  Vars.I33 = Vars.m * 0.5 * pow(Vars.vehicleRadius, 2);

  /*

  double n = sqrt(398600 / pow(6678, 3));
  double k1 = (Vars.I22 - Vars.I33) / Vars.I11;
  double k2 = (Vars.I11 - Vars.I33) / Vars.I22;
  double k3 = (Vars.I22 - Vars.I11) / Vars.I33;

  double R11 = pow(10, 2);
  double R22 = pow(10, 2);
  double R33 = pow(10, 2);

  double F11 = -2 * pow(n, 2) * 4 * k1;
  double F22 = -2 * pow(n, 2) * 3 * k2;
  double F33 = -2 * pow(n, 2) * k3;

  double G31 = n * (1 - k1);
  double G13 = n * (k3 - 1);

  double d = 0.5;
  /*
    The following calculations are based on output of LQR.m to avoid working
   with matrices in C++ Please see .m file for details on calculation.
   Algorithm taken from LQR wikipedia page:
   https://en.wikipedia.org/wiki/Algebraic_Riccati_equation#Solution
*/
  /*
  double gain = 0.25;

  double K11 =
      -1 * gain *
      (Vars.I33 * pow(abs(F33), 2) * pow(abs(Vars.I33), 4) * pow(abs(R33), 2) +
       R33 * pow(abs(G31), 2) * pow(abs(Vars.I33), 2) + Vars.I33) /
      (R11 * d * Vars.I11 *
       (pow(abs(F33), 2) * pow(abs(Vars.I33), 4) * pow(abs(R33), 2) + 1));
  double K22 = -1 * gain * Vars.I33 / (R22 * d * Vars.I22);
  double K33 =
      -1 * gain *
      (Vars.I33 *
       (Vars.I33 * pow(abs(F11), 2) * pow(abs(Vars.I11), 4) * pow(abs(R11), 2) +
        R11 * pow(abs(G13), 2) * pow(abs(Vars.I11), 2) + Vars.I33)) /
      (R33 * d * pow(abs(Vars.I33), 2) *
       (pow(abs(F11), 2) * pow(abs(Vars.I11), 4) * pow(abs(R11), 2) + 1));
  double K34 =
      gain * (R11 * pow(abs(Vars.I11), 2) * G13) /
      (R33 * Vars.I33 *
       (pow(abs(F11), 2) * pow(abs(Vars.I11), 4) * pow(abs(R11), 2) + 1));
  double K16 =
      gain * (R33 * pow(abs(Vars.I33), 2) * G31) /
      (R11 * Vars.I11 *
       (pow(abs(F33), 2) * pow(abs(Vars.I33), 4) * pow(abs(R33), 2) + 1));

  // Matrix Multiply K with [YPR/2; w123] column vector and divide by moment arm
  Vars.LQRx = (K11 * Vars.yaw / 2 + K16 * Vars.rolldot) / Vars.momentArm;
  Vars.LQRy = (K22 * Vars.pitch / 2) / Vars.momentArm;

  */

  // Paste in Values from gainCalc.m
  double K11 = 39.54316;
  double K12 = 0.00000;
  double K13 = -0.00000;
  double K14 = 39.55769;
  double K15 = 0.00000;
  double K16 = 0.00000;
  double K21 = 0.00000;
  double K22 = 39.54316;
  double K23 = 0.00000;
  double K24 = 0.00000;
  double K25 = 39.55769;
  double K26 = 0.00000;
  double K31 = 0.00000;
  double K32 = 0.00000;
  double K33 = 39.54316;
  double K34 = 0.00000;
  double K35 = 0.00000;
  double K36 = 39.54394;

  double gain =
      0.25 *
      pow(10, -4); // changing exponenet drastically changes results of LQR

  // Matrix Multiply K with [YPR/2; w123] column vector and divide by moment arm
  Vars.LQRx =
      gain *
      ((K12 * Vars.pitch) / 2 + K15 * Vars.pitchdot + (K13 * Vars.roll) / 2 +
       K16 * Vars.rolldot + (K11 * Vars.yaw) / 2 + K14 * Vars.yawdot) /
      -Vars.momentArm;
  Vars.LQRy =
      gain *
      ((K22 * Vars.pitch) / 2 + K25 * Vars.pitchdot + (K23 * Vars.roll) / 2 +
       K26 * Vars.rolldot + (K21 * Vars.yaw) / 2 + K24 * Vars.yawdot) /
      -Vars.momentArm;

  // LQR Force limiter X
  if (Vars.LQRx > Vars.thrust)
    Vars.LQRx = Vars.thrust;
  else if (Vars.LQRx < -1 * Vars.thrust)
    Vars.LQRx = -1 * Vars.thrust;

  // LQR Force limiter Y
  if (Vars.LQRy > Vars.thrust)
    Vars.LQRy = Vars.thrust;
  else if (Vars.LQRy < -1 * Vars.thrust)
    Vars.LQRy = -1 * Vars.thrust;
}

void TVC(struct sVars &Vars, double g) {
  if (Vars.thrust < 1) {
    // Define forces and moments for t = 0
    Vars.Fx = 0;
    Vars.Fy = 0;
    Vars.Fz = g * Vars.m0;

    Vars.momentX = 0;
    Vars.momentY = 0;
    Vars.momentZ = 0;
  }

  else {
    // Convert servo position to degrees for comparison to max allowable
    Vars.xServoDegs = (180 / 3.1416) * asin(Vars.LQRx / Vars.thrust);

    // Servo position limiter
    if (Vars.xServoDegs > Vars.maxServo)
      Vars.xServoDegs = Vars.maxServo;
    else if (Vars.xServoDegs < -1 * Vars.maxServo)
      Vars.xServoDegs = -1 * Vars.maxServo;

    // Convert servo position to degrees for comparison to max allowable
    Vars.yServoDegs = (180 / 3.1416) * asin(Vars.LQRy / Vars.thrust);

    // Servo position limiter
    if (Vars.yServoDegs > Vars.maxServo)
      Vars.yServoDegs = Vars.maxServo;
    else if (Vars.yServoDegs < -1 * Vars.maxServo)
      Vars.yServoDegs = -1 * Vars.maxServo;

    // Vector math to aqcuire thrust vector components
    Vars.Fx = Vars.thrust * sin(Vars.xServoDegs * (3.1416 / 180));
    Vars.Fy = Vars.thrust * sin(Vars.yServoDegs * (3.1416 / 180));
    Vars.Fz = sqrt(pow(Vars.thrust, 2) - (pow(Vars.Fx, 2) + pow(Vars.Fy, 2))) +
              (Vars.m * g);

    // Calculate moment created by Fx and Fy
    Vars.momentX = Vars.Fx * Vars.momentArm;
    Vars.momentY = Vars.Fy * Vars.momentArm;
    Vars.momentZ = 0;
  }
}

void vehicleDynamics(struct sVars &Vars, int t) {
  // Idot
  if (t < 1) {
    Vars.I11dot = 0;
    Vars.I22dot = 0;
    Vars.I33dot = 0;
  } else {
    Vars.I11dot = derivative(Vars.I11, Vars.I11prev, Vars.stepSize);
    Vars.I22dot = derivative(Vars.I22, Vars.I22prev, Vars.stepSize);
    Vars.I33dot = derivative(Vars.I33, Vars.I33prev, Vars.stepSize);
  }

  // pdot, qdot, rdot
  Vars.yawddot = (Vars.momentX - Vars.I11dot * Vars.yawdot +
                  Vars.I22 * Vars.pitchdot * Vars.rolldot -
                  Vars.I33 * Vars.pitchdot * Vars.rolldot) /
                 Vars.I11;
  Vars.pitchddot = (Vars.momentY - Vars.I22dot * Vars.pitchdot -
                    Vars.I11 * Vars.rolldot * Vars.yawdot +
                    Vars.I33 * Vars.rolldot * Vars.yawdot) /
                   Vars.I22;
  Vars.rollddot = (Vars.momentZ - Vars.I33dot * Vars.rolldot +
                   Vars.I11 * Vars.pitchdot * Vars.yawdot -
                   Vars.I22 * Vars.pitchdot * Vars.yawdot) /
                  Vars.I33;

  if (t < 1) {
    Vars.x = 0;
    Vars.y = 0;

    Vars.ax = 0;
    Vars.ay = 0;
    Vars.az = Vars.Fz / Vars.m0;
  }

  else {
    // p, q, r
    Vars.yawdot = integral(Vars.yawddot, Vars.yawdotPrev, Vars.stepSize);
    Vars.pitchdot = integral(Vars.pitchddot, Vars.pitchdotPrev, Vars.stepSize);
    Vars.rolldot = integral(Vars.rollddot, Vars.rolldotPrev, Vars.stepSize);

    // ax ay az
    Vars.ax =
        (Vars.Fx / Vars.m) + (Vars.pitchdot * Vars.vz - Vars.rolldot * Vars.vy);
    Vars.ay =
        (Vars.Fy / Vars.m) + (Vars.rolldot * Vars.vx - Vars.vz * Vars.yawdot);
    Vars.az =
        (Vars.Fz / Vars.m) + (Vars.vy * Vars.yawdot - Vars.pitchdot * Vars.vx);

    // vx vy vz in Body frame
    Vars.vx = integral(Vars.ax, Vars.vxPrev, Vars.stepSize);
    Vars.vy = integral(Vars.ay, Vars.vyPrev, Vars.stepSize);
    Vars.vz = integral(Vars.az, Vars.vzPrev, Vars.stepSize);

    // Xe
    Vars.x = integral(Vars.vx, Vars.xPrev, Vars.stepSize);
    Vars.y = integral(Vars.vy, Vars.yPrev, Vars.stepSize);
    Vars.z = integral(Vars.vz, Vars.zPrev, Vars.stepSize);

    // Euler Angles
    Vars.phidot = Vars.yawdot + (Vars.pitchdot * sin(Vars.yaw) +
                                 Vars.rolldot * cos(Vars.yaw)) *
                                    (sin(Vars.pitch) / cos(Vars.pitch));
    Vars.thetadot =
        Vars.pitchdot * cos(Vars.yaw) - Vars.rolldot * sin(Vars.pitch);
    Vars.psidot =
        (Vars.pitchdot * sin(Vars.yaw) + Vars.rolldot * cos(Vars.yaw)) /
        cos(Vars.pitch);

    Vars.yaw = integral(Vars.phidot, Vars.yawPrev, Vars.stepSize);
    Vars.pitch = integral(Vars.thetadot, Vars.pitchPrev, Vars.stepSize);
    Vars.roll = integral(Vars.psidot, Vars.rollPrev, Vars.stepSize);
  }

  // Set "prev" values for next timestep
  Vars.I11prev = Vars.I11;
  Vars.I22prev = Vars.I22;
  Vars.I33prev = Vars.I33;

  Vars.yawPrev = Vars.yaw;
  Vars.pitchPrev = Vars.pitch;
  Vars.rollPrev = Vars.roll;

  Vars.yawdotPrev = Vars.yawdot;
  Vars.pitchdotPrev = Vars.pitchdot;
  Vars.rolldotPrev = Vars.rolldot;

  Vars.yawddotPrev = Vars.yawddot;
  Vars.pitchddotPrev = Vars.pitchddot;
  Vars.rollddotPrev = Vars.rollddot;

  Vars.axPrev = Vars.ax;
  Vars.ayPrev = Vars.ay;
  Vars.azPrev = Vars.az;

  Vars.vxPrev = Vars.vx;
  Vars.vyPrev = Vars.vy;
  Vars.vzPrev = Vars.vz;

  Vars.xPrev = Vars.x;
  Vars.yPrev = Vars.y;
  Vars.zPrev = Vars.z;
}

void write2CSV(struct sVars &Vars, std::fstream &outfile, int t) {
  // writing to output file
  outfile << t << ", ";

  outfile << Vars.x << ", ";
  outfile << Vars.y << ", ";
  outfile << Vars.z << ", ";

  outfile << Vars.vx << ", ";
  outfile << Vars.vy << ", ";
  outfile << Vars.vz << ", ";

  outfile << Vars.ax << ", ";
  outfile << Vars.ay << ", ";
  outfile << Vars.az << ", ";

  outfile << Vars.yaw * 180 / 3.1416 << ", ";
  outfile << Vars.pitch * 180 / 3.1416 << ", ";
  outfile << Vars.roll * 180 / 3.1416 << ", ";

  outfile << Vars.yawdot * 180 / 3.1416 << ", ";
  outfile << Vars.pitchdot * 180 / 3.1416 << ", ";
  outfile << Vars.rolldot * 180 / 3.1416 << ", ";

  outfile << Vars.yawddot * 180 / 3.1416 << ", ";
  outfile << Vars.pitchddot * 180 / 3.1416 << ", ";
  outfile << Vars.rollddot * 180 / 3.1416 << ", ";

  outfile << Vars.I11 << ", ";
  outfile << Vars.I22 << ", ";
  outfile << Vars.I33 << ", ";

  outfile << Vars.I11dot << ", ";
  outfile << Vars.I22dot << ", ";
  outfile << Vars.I33dot << ", ";

  outfile << Vars.xServoDegs << ", ";
  outfile << Vars.yServoDegs << ", ";

  outfile << Vars.m << ", ";
  outfile << Vars.thrust << ", ";
  outfile << Vars.burnElapsed << ", ";
  outfile << Vars.Fz << ", ";

  outfile << Vars.LQRx << ", ";
  outfile << Vars.LQRy << std::endl;
}

double derivative(double x2, double x1, double dt) {
  double dxdt = (x2 - x1) / (dt / 1000);
  return dxdt;
}

double integral(double x, double y, double dt) {
  double integ = (x * dt / 1000) + y;
  return integ;
}