#include "Vehicle.h"
#include <Arduino.h>
#include <SD.h>
#include <SPI.h>

double loadCellCalibrate();
void initFile();
void thrustInfo(struct Vehicle &);
void processTVC(struct Vehicle &);
void write2CSV(struct Vehicle &);
void printSimResults(struct Vehicle &);
void teensyAbort();

const int chipSelect = BUILTIN_SDCARD;
File dataFile;

double loadCellCalibrate() {
  // place code to calibrate load cells in here
  double loadTotal = 0.0;
  for (double t = 0.0; t == 10.0; ++t) {
    loadTotal += 1.0;
    delay(15);
  }
  return loadTotal / 10.0;
}

void initFile() {
  Serial.print("Initializing SD card...");

  // see if the card is present and can be initialized:
  if (!SD.begin(chipSelect)) {
    Serial.println("Card failed, or not present. \n\nABORTING SIMULATION");
    teensyAbort();
  }
  Serial.println("Card initialized.");

  int i = 1;
  const char *fileName;

  if (SD.exists("simOut.csv")) {
    while (i > 0.0) {
      fileName = ("simOut_" + String(i) + ".csv").c_str();

      if (!SD.exists(fileName)) {
        // Open simOut_i.csv
        dataFile = SD.open(fileName, FILE_WRITE);

        if (dataFile) {
          Serial.println("File successfully opened!");

        } else {
          // if the file didn't open, print an error:
          Serial.println("Error opening output file. \n\nABORTING SIMULATION");
          teensyAbort();
        }
        i = 0.0;

      } else {
        i++;
      }
    }
  } else {
    // Open simOut.csv
    dataFile = SD.open("simOut.csv", FILE_WRITE);
    if (dataFile) {
      Serial.println("File successfully opened!");

    } else {
      // if the file didn't open, print an error:
      Serial.println("Error opening output file. \n\nABORTING SIMULATION");
      teensyAbort();
    }
  }

  // File Header
  dataFile.println(
      "t,x,y,z,vx,vy,vz,ax,ay,az,yaw,pitch,roll,yawdot,pitchdot,rolldot,"
      "Servo1,Servo2,thrustFiring,thrust,simResponse");
}

void thrustInfo(Vehicle &State) {
  if (State.time == 0) {
    Serial.println("WARNING: thrustInfo not implemented for TEENSY");
  }

  if ((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, LoadCells &loadCells) {
  if (State.time == 0) {
    Serial.println("WARNING: processTVC not implemented for TEENSY");
  }

  // Vector math to aqcuire thrust vector components
  // PLACEHOLDER PLACEHOLDERPLACEHOLDER PLACEHOLDERPLACEHOLDER
  // PLACEHOLDERPLACEHOLDER PLACEHOLDERPLACEHOLDER PLACEHOLDER
  State.Fx = loadCells.lc1Value + loadCells.lc2Value;
  State.Fy = loadCells.lc0Value + loadCells.lc3Value;
  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.0;
}

void write2CSV(Vehicle &State) {
  dataFile.print(String(State.time, 5));
  dataFile.print(",");

  dataFile.print(String(State.x, 5));
  dataFile.print(",");
  dataFile.print(String(State.y, 5));
  dataFile.print(",");
  dataFile.print(String(State.z, 5));
  dataFile.print(",");

  dataFile.print(String(State.vx, 5));
  dataFile.print(",");
  dataFile.print(String(State.vy, 5));
  dataFile.print(",");
  dataFile.print(String(State.vz, 5));
  dataFile.print(",");

  dataFile.print(String(State.ax, 5));
  dataFile.print(",");
  dataFile.print(String(State.ay, 5));
  dataFile.print(",");
  dataFile.print(String(State.az, 5));
  dataFile.print(",");

  dataFile.print(String(State.yaw * 180.0 / M_PI, 5));
  dataFile.print(",");
  dataFile.print(String(State.pitch * 180.0 / M_PI, 5));
  dataFile.print(",");
  dataFile.print(String(State.roll * 180.0 / M_PI, 5));
  dataFile.print(",");

  dataFile.print(String(State.yawdot * 180.0 / M_PI, 5));
  dataFile.print(",");
  dataFile.print(String(State.pitchdot * 180.0 / M_PI, 5));
  dataFile.print(",");
  dataFile.print(String(State.rolldot * 180.0 / M_PI, 5));
  dataFile.print(",");

  dataFile.print(String(State.xServoDegs, 5));
  dataFile.print(",");
  dataFile.print(String(State.yServoDegs, 5));
  dataFile.print(",");

  dataFile.print(String(State.thrustFiring, 5));
  dataFile.print(",");
  dataFile.print(String(State.thrust, 5));
  dataFile.print(",");

  dataFile.print(String(State.stepDuration, 5));
  dataFile.print("\n");

}

void printSimResults(Vehicle &State) {
  State.yaw = State.yaw * 180.0 / M_PI;
  State.pitch = State.pitch * 180.0 / M_PI;
  State.roll = State.roll * 180.0 / 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) {
    Serial.print("Landing Angle     < 5.0 degrees  | PASS | ");
  } else {
    Serial.print("Landing Angle     < 5.0 degrees  | FAIL | ");
  }
  Serial.print("Final Angles:   [" + String(State.yaw) + ", " +
               String(State.pitch) + "]\n");

  if (landing_velocity < 0.5) {
    Serial.print("Landing Velocity  < 0.5 m/s      | PASS | ");
  } else {
    Serial.print("Landing Velocity  < 0.5 m/s      | FAIL | ");
  }
  Serial.print("Final Velocity: [" + String(State.vx) + ", " +
               String(State.vy) + ", " + String(State.vz) + "]\n");

  Serial.print("\nSimulation Complete\n");
}

void closeFile() {
  // close the file:
  dataFile.close();
  Serial.println("File closed\n");
}

void teensyAbort() {
  while (1) {
    digitalWrite(BUILTIN_LED, HIGH);
    delay(1000);
    digitalWrite(BUILTIN_LED, LOW);
    delay(1000);
  }
}