#define M_PI 3.14159265359

#include <cmath>
#include <fstream>
#include <iostream>
#include <stdexcept> // std::runtime_error
#include <stdio.h>
#include <string>
#include <vector>

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

bool sim(struct Vehicle &);

int main() {
  Vehicle State;
  Vehicle PrevState;

  // Create an input filestream
  std::ifstream inFile("input.csv");

  // Make sure the file is open
  if (!inFile.is_open())
    throw std::runtime_error("Could not open file");

  std::vector<double> varValueVec = std::vector<double>(17, 0.0);
  std::string varName, varValue, varUnits;

  for (int i; i < 20; i++) {
    std::getline(inFile, varName, ',');
    std::getline(inFile, varValue, ',');
    varValueVec[i] = stod(varValue);
    std::getline(inFile, varUnits);
  }

  // Initial Velocity
  State.vx = varValueVec[0]; // [m/s]
  State.vy = varValueVec[1]; // [m/s]
  State.vz = varValueVec[2]; // [m/s]

  // Initial YPR
  State.yaw = varValueVec[3] * M_PI / 180;   // [rad]
  State.pitch = varValueVec[4] * M_PI / 180; // [rad]
  State.roll = varValueVec[5] * M_PI / 180;  // [rad]

  // Initial YPRdot
  State.yawdot = varValueVec[6] * M_PI / 180;   // [rad/s]
  State.pitchdot = varValueVec[7] * M_PI / 180; // [rad/s]
  State.rolldot = varValueVec[8] * M_PI / 180;  // [rad/s]

  // Servo Limitation
  State.maxServo = varValueVec[9]; // [degs]

  // Vehicle Properties
  State.massInitial = varValueVec[10];   // [kg]
  State.vehicleHeight = varValueVec[13]; // [m]
  State.vehicleRadius = varValueVec[14]; // [m]
  State.momentArm = varValueVec[15];     // [m]

  // Sim Step Size
  State.stepSize = varValueVec[16]; // [ms]

  // PID Gains
  State.Kp = varValueVec[17];
  State.Ki = varValueVec[18];
  State.Kd = varValueVec[19];

  std::cout << State.Kd << "\n";

  // Other Properties
  State.burntime = varValueVec[12];                             // [s]
  State.massPropellant = varValueVec[11];                       // [kg]
  State.massBurnout = State.massInitial - State.massPropellant; // [kg]
  State.mdot = State.massPropellant / State.burntime;           // [kg/s]
  State.mass = State.massInitial;                               // [kg]
  State.burnElapsed = 2000;                                     // [s]
  PrevState.thrust = 0;                                         // [N]

  bool outcome = sim(State, PrevState);

  std::cout << "Finished"
            << "\n";

  if (outcome == 1) {
    std::cout << "Sim Result = Success!";
    return 0;
  } else if (outcome == 0) {
    std::cout << "Sim Result = Failed!";

    // return 1; Until I figure out how to make CI/CD continue even when run
    // fails.
    return 0;
  }
}