#define M_PI 3.14159265359

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

#include "sVars.h"
#include "sim.h"

void sim(struct sVars &);

int main() {
  sVars Vars;

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

  // Initial YPR
  Vars.yaw = 0 * M_PI / 180;   // [rad]
  Vars.pitch = 0 * M_PI / 180; // [rad]
  Vars.roll = 0 * M_PI / 180;  // [rad]

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

  // Servo Limitation
  Vars.maxServo = 15; // [degs]

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

  // Sim Step Size
  // TODO: use dt instead?
  Vars.stepSize = 1; // [ms]

  // Other Properties
  Vars.massPropellant = 0.06;                                // [kg]
  Vars.massBurnout = Vars.massInitial - Vars.massPropellant; // [kg]
  Vars.burntime = 3.45 - 0.148;                              // [s]
  Vars.mdot = Vars.massPropellant / Vars.burntime;           // [kg/s]

  sim(Vars);

  std::cout << "Finished";
  std::cin.get();

  return 0;
}