#define M_PI 3.14159265359

#include <cmath>
#include <fstream>
#include <iostream>
#include <stdio.h>

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

bool sim(struct Vehicle &);

int main() {
  Vehicle State;
  Vehicle PrevState;

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

  // Initial YPR
  State.yaw = 10 * M_PI / 180;  // [rad]
  State.pitch = 5 * 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 = 15; // [degs]

  // 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 << "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;
  }
}