#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;

  // PID Gains
  State.Kp = -6.8699;
  State.Ki = 0;
  State.Kd = -0.775;

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

  // Initial YPR

  State.yaw = 75 * M_PI / 180;   // [rad]
  State.pitch = 30 * M_PI / 180; // [rad]
  State.roll = 0 * M_PI / 180;   // [rad]

  // Initial YPRdot
  State.yawdot = 1 * M_PI / 180;    // [rad/s]
  State.pitchdot = -1 * M_PI / 180; // [rad/s]
  State.rolldot = 0 * M_PI / 180;   // [rad/s]

  // Servo Limitation
  State.maxServo = 7;   // [degs]
  State.maxServoRate = 360; // [degs/sec]

  // Vehicle Properties
  State.massInitial = 1.2;       // [kg]
  State.vehicleHeight = 0.5318;  // [m]
  State.vehicleRadius = 0.05105; // [m]
  State.momentArm = 0.145;       // [m]

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

  // Other Properties
  State.massPropellant = 0.06;                                  // [kg]
  State.massBurnout = State.massInitial - State.massPropellant; // [kg]
  State.burntime = 3.45 - 0.148;                                // [s]
  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 << std::endl << "Simulation Complete 🚀" << std::endl;
  //                                            ^^^
  // 50% chance this makes Mattys linux crash
  return 0;
}