#include <array>

#ifndef VEHICLE_H
#define VEHICLE_H

struct Vehicle {
  double x, y, z;
  double vx, vy, vz;
  double vxBody, vyBody, vzBody;
  double ax, ay, az;

  double yaw, pitch, roll;
  double phidot, thetadot, psidot;
  double yawdot, pitchdot, rolldot;
  double yawddot, pitchddot, rollddot;

  double mass, massInitial, massPropellant, massBurnout, mdot;
  double vehicleHeight, vehicleRadius, momentArm;

  double burntime;
  double burnVelocity;
  double thrust, burnElapsed, burnStart;
  int thrustFiring = 0; // 0: Pre-burn, 1: Mid-burn, 2: Post-burn

  double PIDx, PIDy, Fx, Fy, Fz;
  double momentX, momentY, momentZ;

  double I11, I22, I33;
  double I11dot, I22dot, I33dot;

  double maxServo;
  double maxServoRate;
  double xServoDegs, yServoDegs;
  double xServoDegsDot, yServoDegsDot;

  double Kp, Ki, Kd;
  double yError, yPrevError;
  double pError, pPrevError;
  double i_yError, i_pError = 0.0;
  double d_yError, d_pError;

  double simTime;
  double stepSize;
  double stepDuration;

  double time = 0.0;
};

void init_Vehicle(Vehicle &State) {
  // PID Gains
  State.Kp = -6.8699;
  State.Ki = 0.0;
  State.Kd = -0.775;

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

  // Initial YPR
  State.yaw = 45.0 * M_PI / 180.0;   // [rad]
  State.pitch = 45.0 * M_PI / 180.0; // [rad]
  State.roll = 0.0 * M_PI / 180.0;   // [rad]

  // Initial YPRdot
  State.yawdot = 1.0 * M_PI / 180.0;    // [rad/s]
  State.pitchdot = -1.0 * M_PI / 180.0; // [rad/s]
  State.rolldot = 0.0 * M_PI / 180.0;   // [rad/s]

  // Servo Limitation
  State.maxServo = 7.0;       // [degs]
  State.maxServoRate = 360.0; // [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.0; // [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]
}

#endif