#include "Vehicle.h"

#include <iostream>

void thrustInfo(struct Vehicle &);
void processTVC(struct Vehicle &);
void write2CSV(struct outVector &, struct Vehicle &);
void printSimResults(struct Vehicle &);

void thrustInfo(Vehicle &State) {
  if ((std::abs(State.burnVelocity + State.vz) < 1.03) &&
      (State.thrustFiring == 0)) {
    // Start burn
    State.burnStart = State.time;
    State.burnElapsed = 0.0;
    State.thrustFiring = 1;

    getThrust(State);

  } else if (State.thrustFiring == 1) {
    State.burnElapsed = (State.time - State.burnStart) / 1000.0;
    State.mass = State.massInitial - (State.mdot * State.burnElapsed);

    getThrust(State);

  } else {
    State.thrust = 0.0;
  }
}

void processTVC(Vehicle &State) {
  // Vector math to aqcuire thrust vector components
  State.Fx = State.thrust * sin(State.xServoDegs * (M_PI / 180));
  State.Fy = State.thrust * sin(State.yServoDegs * (M_PI / 180));
  State.Fz = sqrt(pow(State.thrust, 2) - pow(State.Fx, 2) - pow(State.Fy, 2)) +
             (State.mass * g);

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

void write2CSV(outVector &stateVector, Vehicle &State) {

  // Deleting any previous output file
  if (remove("simOut.csv") != 0)
    perror("No file deletion necessary");
  else
    puts("Previous output 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,"
             "Servo1,Servo2,thrustFiring,PIDx,PIDy,thrust"
          << std::endl;

  // writing to output file
  for (int i = 0; i < State.time; i += State.stepSize) {
    outfile << i << ", ";

    outfile << stateVector.x[i] << ",";
    outfile << stateVector.y[i] << ",";
    outfile << stateVector.z[i] << ",";

    outfile << stateVector.vx[i] << ",";
    outfile << stateVector.vy[i] << ",";
    outfile << stateVector.vz[i] << ",";

    outfile << stateVector.ax[i] << ",";
    outfile << stateVector.ay[i] << ",";
    outfile << stateVector.az[i] << ",";

    outfile << stateVector.yaw[i] * 180 / M_PI << ",";
    outfile << stateVector.pitch[i] * 180 / M_PI << ",";
    outfile << stateVector.roll[i] * 180 / M_PI << ",";

    outfile << stateVector.yawdot[i] * 180 / M_PI << ",";
    outfile << stateVector.pitchdot[i] * 180 / M_PI << ",";
    outfile << stateVector.rolldot[i] * 180 / M_PI << ",";

    outfile << stateVector.servo1[i] << ",";
    outfile << stateVector.servo2[i] << ",";

    outfile << stateVector.thrustFiring[i] << ",";

    outfile << stateVector.PIDx[i] << ",";
    outfile << stateVector.PIDy[i] << ",";

    outfile << stateVector.thrust[i] << std::endl;
  }

  outfile.close();
  std::cout << "simOut.csv created successfully.\n" << std::endl;
}

void printSimResults(Vehicle &State) {
  State.yaw = State.yaw * 180 / M_PI;
  State.pitch = State.pitch * 180 / M_PI;
  State.roll = State.roll * 180 / M_PI;

  double landing_angle =
      pow(State.yaw * State.yaw + State.pitch * State.pitch, .5);

  double landing_velocity =
      pow(State.vx * State.vx + State.vy * State.vy + State.vz * State.vz, .5);

  if (landing_angle < 5.0) {
    std::cout << "Landing Angle     < 5.0 degrees  | PASS | ";
  } else {
    std::cout << "Landing Angle     < 5.0 degrees  | FAIL | ";
  }
  std::cout << "Final Angles:   [" << State.yaw << ", " << State.pitch << "]"
            << std::endl;

  if (landing_velocity < 0.5) {
    std::cout << "Landing Velocity  < 0.5 m/s      | PASS | ";
  } else {
    std::cout << "Landing Velocity  < 0.5 m/s      | FAIL | ";
  }
  std::cout << "Final Velocity: [" << State.vx << ", " << State.vy << ", "
            << State.vz << "]" << std::endl;

  std::cout << std::endl << "Simulation Complete\n" << std::endl;
}