#define M_PI 3.14159265359

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

int BUILTIN_LED = 13;
unsigned long last, initTime;
#endif

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

#if defined(NATIVE) || defined(_WIN32)
#include "native.h"
outVector stateVector;
#elif defined(TEENSY)

#include "LoadCells.h"
LoadCells loadCells;

#include "teensy.h"
const int lc_clock = 23;

const int lc_data_0 = 0;
const int lc_data_1 = 1;
const int lc_data_2 = 2;
const int lc_data_3 = 3;

#endif

Vehicle State;
Vehicle PrevState;

#if defined(NATIVE) || defined(_WIN32)
void setup() {
  init_Vehicle(State);

  // Determine when to burn
  burnStartTimeCalc(State);
}
#elif defined(TEENSY)
void setup() {
  Serial.begin(9600);

  // 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(lc_data_0, INPUT);
  pinMode(lc_data_1, INPUT);
  pinMode(lc_data_2, INPUT);
  pinMode(lc_data_3, INPUT);

  delay(1000);
  init_Vehicle(State);
  Serial.println("Simulated Vehicle Initalized");
  delay(1000);

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

  init_LoadCells(loadCells);

  Serial.println("Load Cells Calibrated");
  delay(1000);
  initFile();
  delay(1000);

  initTime = micros();
}
#endif

#if defined(NATIVE) || defined(_WIN32)
void loop() {
  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);
  }
}
#elif defined(TEENSY)
void loop() {

  last = micros();
  vehicleDynamics(State, PrevState);
  thrustInfo(State);
  pidController(State, PrevState);
  TVC(State, PrevState);
  
  processTVC(State, loadCells);

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

  // Set "prev" values for next timestep
  PrevState = State;
  
  // state2vec(State, PrevState, stateVector);


  State.time += State.stepSize;

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

    closeFile();
    delay(20000);

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

  delay(20 - ((micros() - last) * 1000.0));
}
#endif

#if defined(_WIN32) || defined(NATIVE)
int main() {

  setup();

  do {
    loop();
  } while ((State.z > 0.0) || (State.thrustFiring != 2));

  return 0;
}
#endif