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

void testGimbal(class PWMServo &, class PWMServo &);
void initLoadCells(struct Vehicle &);
void initFile();
void thrustInfo(struct Vehicle &);
void processTVC(struct Vehicle &, class PWMServo &, class PWMServo &);
void write2CSV(struct Vehicle &);
void printSimResults(struct Vehicle &);
void teensyAbort();
double loadCellFilter(double current, double previous);
float yaw_conv(float thetad);
float pitch_conv(float thetad);

// Load cell stuff
HX711 lc0;
HX711 lc1;
HX711 lc2;
HX711 lc3;

void read_lc0();
void read_lc1();
void read_lc2();
void read_lc3();

const int lc_clock = 23;
const int pin_lc0 = 15; // Green
const int pin_lc1 = 14; // Yellow
const int pin_lc2 = 19; // White
const int pin_lc3 = 20; // Black
const int pin_igniter = 7;

double lcGain0 = -107.39;
double lcGain1 = -107.39;
double lcGain2 = -107.39;
double lcGain3 = -107.39;

// SD card stuff
const int chipSelect = BUILTIN_SDCARD;
File dataFile;

void testGimbal(PWMServo &yaw, PWMServo &pitch) {
  int servoTest = 0;

  if ((!yaw.attached()) || (!pitch.attached())) {
    Serial.println("Servo pin assignments are not valid!");
    teensyAbort();
  }

  yaw.write(yaw_conv(servoTest));
  pitch.write(pitch_conv(servoTest));

  delay(1000);

  // Servo 1 Test
  for (servoTest = 0; servoTest < 7; servoTest += 1) {
    yaw.write(yaw_conv(servoTest));
    delay(30);
  }
  for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
    yaw.write(yaw_conv(servoTest));
    delay(30);
  }

  delay(1000);

  // Servo 2 Test
  for (servoTest = 0; servoTest < 7; servoTest += 1) {
    pitch.write(pitch_conv(servoTest));
    delay(30);
  }
  for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
    pitch.write(pitch_conv(servoTest));
    delay(30);
  }

  delay(1000);

  // Servo 1 & 2 Test
  for (servoTest = 0; servoTest < 7; servoTest += 1) {
    yaw.write(yaw_conv(servoTest));
    pitch.write(pitch_conv(servoTest));
    delay(30);
  }
  for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
    yaw.write(yaw_conv(servoTest));
    pitch.write(pitch_conv(servoTest));
    delay(30);
  }

  delay(30);

  yaw.write(yaw_conv(0));
  pitch.write(pitch_conv(0));
}

void initLoadCells(Vehicle &State) {
  // Configure clock pin with high impedance to protect
  // pin (if this doesn't  work, change to OUTPUT)
  pinMode(lc_clock, INPUT);

  // Configure load cell data pins as inputs
  pinMode(pin_lc0, INPUT);
  pinMode(pin_lc1, INPUT);
  pinMode(pin_lc2, INPUT);
  pinMode(pin_lc3, INPUT);
  lc0.begin(pin_lc0, lc_clock);
  lc1.begin(pin_lc1, lc_clock);
  lc2.begin(pin_lc2, lc_clock);
  lc3.begin(pin_lc3, lc_clock);

  Serial.print("Calibrating");
  lc0.set_scale();
  lc1.set_scale();
  lc2.set_scale();
  lc3.set_scale();

  lc0.tare(50);
  Serial.print(".");
  lc1.tare(50);
  Serial.print(".");
  lc2.tare(50);
  Serial.println(".");
  lc3.tare(50);

  // Attach ISRs to load cell data pins to read data when available
  attachInterrupt(digitalPinToInterrupt(pin_lc0), read_lc0, LOW);
  attachInterrupt(digitalPinToInterrupt(pin_lc1), read_lc1, LOW);
  attachInterrupt(digitalPinToInterrupt(pin_lc2), read_lc2, LOW);
  attachInterrupt(digitalPinToInterrupt(pin_lc3), read_lc3, LOW);

  Serial.println("Checking tare");
  double lc0Avg, lc1Avg, lc2Avg, lc3Avg = 0.0;
  double loopSize = 50.0;

  for (int i = 0; i < loopSize; i++) {
    lc0Avg += (State.lc0 / loopSize);
    lc1Avg += (State.lc1 / loopSize);
    lc2Avg += (State.lc2 / loopSize);
    lc3Avg += (State.lc3 / loopSize);
    delay(50);
  }

  if (((lc0Avg > 1000) || (lc1Avg > 1000)) ||
      ((lc2Avg > 1000) || (lc3Avg > 1000))) {
    Serial.println("Load cell tare insufficient!");
    teensyAbort();
  }

  // Initializes State variables
  State.lc0 = lc0.get_value();
  State.lc1 = lc1.get_value();
  State.lc2 = lc2.get_value();
  State.lc3 = lc3.get_value();

  Serial.println("Load Cells Initialized");

  State.lc0_processed = 9.81 * 0.001 * lc0.get_value() / lcGain0;
  State.lc1_processed = 9.81 * 0.001 * lc1.get_value() / lcGain1;
  State.lc2_processed = 9.81 * 0.001 * lc2.get_value() / lcGain2;
  State.lc3_processed = 9.81 * 0.001 * lc3.get_value() / lcGain3;
}

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,lc0,lc1,lc2,lc3,lc0_"
      "processed,lc1_processed,"
      "lc2_processed,lc3_processed");
}

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

    digitalWrite(pin_igniter, HIGH);
    State.thrustFiring = 1;

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

    // Constants based on vehicle
    double r = 3.0;
    double R = 5.0;

    // Vector math to aqcuire thrust vector components
    State.Fz = State.lc0_processed + State.lc1_processed + State.lc2_processed +
               State.lc3_processed + (State.mass * g);
    State.Fx = (State.lc1_processed - State.lc2_processed) * r / R;
    State.Fy = (State.lc0_processed - State.lc3_processed) * r / R;
    State.thrust = sqrt(pow(State.Fz, 2) + pow(State.Fx, 2) + pow(State.Fy, 2));

    if (State.burnElapsed > 4.0) {
      State.thrustFiring = 2;
      digitalWrite(pin_igniter, LOW);
      State.thrust = 0.0;
    }

  } else {
    State.thrust = 0.0;
  }
}

void processTVC(Vehicle &State, PWMServo &yaw, PWMServo &pitch) {
  /*
   State.Fx = State.thrust * sin(State.xServoDegs * (M_PI / 180.0));
   State.Fy = State.thrust * sin(State.yServoDegs * (M_PI / 180.0));
   State.Fz = sqrt(pow(State.thrust, 2) - pow(State.Fx, 2) - pow(State.Fy, 2)) +
              (State.mass * g);
 */

  yaw.write(yaw_conv(State.xServoDegs));
  pitch.write(pitch_conv(State.yServoDegs));
}

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(",");

  dataFile.print(String(State.lc0, 5));
  dataFile.print(",");
  dataFile.print(String(State.lc1, 5));
  dataFile.print(",");
  dataFile.print(String(State.lc2, 5));
  dataFile.print(",");
  dataFile.print(String(State.lc3, 5));
  dataFile.print(",");

  dataFile.print(String(State.lc0_processed, 5));
  dataFile.print(",");
  dataFile.print(String(State.lc1_processed, 5));
  dataFile.print(",");
  dataFile.print(String(State.lc2_processed, 5));
  dataFile.print(",");
  dataFile.print(String(State.lc3_processed, 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 < 1.0) {
    Serial.print("Landing Velocity  < 1.0 m/s      | PASS | ");
  } else {
    Serial.print("Landing Velocity  < 1.0 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);
  }
}

// ISRs to print data to serial monitor
void read_lc0() {
  State.lc0 = lc0.get_value();
  State.lc0_processed = 0.00981 * State.lc0 / lcGain0;
  State.lc0_processed =
      loadCellFilter(State.lc0_processed, PrevState.lc0_processed);
  PrevState.lc0_processed = State.lc0_processed;
}
void read_lc1() {
  State.lc1 = lc1.get_value();
  State.lc1_processed = 0.00981 * State.lc1 / lcGain1;
  State.lc1_processed =
      loadCellFilter(State.lc1_processed, PrevState.lc1_processed);
  PrevState.lc1_processed = State.lc1_processed;
}
void read_lc2() {
  State.lc2 = lc2.get_value();
  State.lc2_processed = 0.00981 * State.lc2 / lcGain2;
  State.lc2_processed =
      loadCellFilter(State.lc2_processed, PrevState.lc2_processed);
  PrevState.lc2_processed = State.lc2_processed;
}
void read_lc3() {
  State.lc3 = lc3.get_value();
  State.lc3_processed = 0.00981 * State.lc3 / lcGain3;
  State.lc3_processed =
      loadCellFilter(State.lc3_processed, PrevState.lc3_processed);
  PrevState.lc3_processed = State.lc3_processed;
}
double loadCellFilter(double current, double previous) {
  if (abs(current - previous > 60.0)) {
    return previous;
  } else {
    return current;
  }
}

// Desired TVC angle to servo input conversion
float yaw_conv(float thetad) {
  float h = .2875;
  float H = 1.6;

  float theta = thetad * M_PI / 180.0;

  return 90 + (2 * asin((H * sin(theta / 2)) / h)) * 180.0 / M_PI;
}
float pitch_conv(float thetad) {
  float h = .2875;
  float H = 1.2;

  float theta = thetad * M_PI / 180.0;

  return 90 + (2 * asin((H * sin(theta / 2)) / h)) * 180.0 / M_PI;
}