#if defined(_WIN32) || defined(NATIVE)
#include <cmath>
#include <fstream>
#include <iostream>
#include <stdexcept> // std::runtime_error
#include <stdio.h>
#include <string>
#include <vector>
#elif defined(TEENSY)
#include <Arduino.h>
#endif

#define M_PI 3.14159265359

#include "Vehicle.h"
#include "sim.h"

Vehicle State;
Vehicle PrevState;

#if defined(TEENSY)

int BUILTIN_LED = 13;
unsigned long last, initTime;

#include "teensy.h"
#include <HX711.h>
#include <PWMServo.h>

const int lc_clock = 23;

const int pin_lc0 = 14;
const int pin_lc1 = 15;
const int pin_lc2 = 19;
const int pin_lc3 = 20;

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

HX711 lc0;
HX711 lc1;
HX711 lc2;
HX711 lc3;

PWMServo servo1;
PWMServo servo2;

const int pin_servo1 = 33;
const int pin_servo2 = 29;

int servoPos = 0; // variable to store the servo position;

void setup() {
  Serial.begin(9600);

  delay(5000);
  Serial.println("Simulation Countdown:");
  for (int i = 0; i < 10; i++) {
    Serial.println(10 - i);
    delay(1000);
  }

  init_loadCells();
  init_Vehicle(State);
  State.lc0 = lc0.get_value();
  State.lc1 = lc1.get_value();
  State.lc2 = lc2.get_value();
  State.lc3 = lc3.get_value();

  servo1.attach(pin_servo1);
  servo2.attach(pin_servo2);
  testGimbal(servo1, servo2);
  Serial.println("Servos Tested");

  delay(3000);
  Serial.println("Simulated Vehicle Initalized");

  // Determine when to burn
  burnStartTimeCalc(State);
  Serial.println("Starting Height Calculated");

  initFile();

  initTime = micros();
}

void loop() {
  last = micros();

  vehicleDynamics(State, PrevState);
  thrustInfo(State);
  pidController(State, PrevState);
  TVC(State, PrevState);
  processTVC(State, servo1, servo2);

  State.stepDuration = micros() - last;
  write2CSV(State);

  // Set previous values for next timestep
  PrevState = State;
  State.time += State.stepSize;

  if ((State.z < 0.0) && (State.thrustFiring == 2)) {
    printSimResults(State);
    Serial.println("Run duration:" + String((micros() - initTime) / 1000000.0) +
                   " seconds");

    closeFile();
    delay(3000);

    Serial.println("SUCCESS");
    Serial.println("Exiting Sim");
    teensyAbort();
  }

  delay(State.stepSize - ((micros() - last) / 1000.0));
}

void init_loadCells() {
  // 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);

  // 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("Load Cells Initialized");

  Serial.print("Calibrating");
  lc0.tare();
  Serial.print(".");
  lc1.tare();
  Serial.print(".");
  lc2.tare();
  Serial.print(".");
  lc3.tare();
  Serial.println(".");
  Serial.println("Load Cells Calibrated");
}

// ISRs to print data to serial monitor
void read_lc0() { State.lc0 = lc0.get_value(); }
void read_lc1() { State.lc1 = lc1.get_value(); }
void read_lc2() { State.lc2 = lc2.get_value(); }
void read_lc3() { State.lc3 = lc3.get_value(); }
#endif

#if defined(_WIN32) || defined(NATIVE)

#include "native.h"
outVector stateVector;

int main() {

  init_Vehicle(State);

  // Determine when to burn
  burnStartTimeCalc(State);

  do {
    vehicleDynamics(State, PrevState);
    thrustInfo(State);
    pidController(State, PrevState);
    TVC(State, PrevState);
    processTVC(State);
    state2vec(State, PrevState, stateVector);

    // Set "prev" values for next timestep
    PrevState = State;
    State.time += State.stepSize;

    if ((State.z < 0.0) && (State.thrustFiring == 2)) {
      write2CSV(stateVector, State);
      printSimResults(State);
      init_Vehicle(State);
    }
  } while ((State.z > 0.0) || (State.thrustFiring != 2));

  return 0;
}
#endif