#pragma once
#include "PhysicsManager.h"

void InitPhysics(VPhysics * p) {
  p->Gravity.x = 0.0;
  p->Gravity.y = -0.0001;
  p->Gravity.z = 0.0;
  p->WindResistance = 0.01;
  p->MaxSpeed = 2.0;
  p->Momentum.x = 0.0;
  p->Momentum.y = 0.0;
  p->Momentum.z = 0.0;
}

void CleanUpPhysics(VPhysics * p) {
  #pragma unused(p)
}

void PushShip(VPhysics * p, Vector * push) {
  float length;
  p->Momentum.x += push->x;
  p->Momentum.y += push->y;
  p->Momentum.z += push->z;
  length = VectorMagnitudeSquared(&p->Momentum);
  if (length > (p->MaxSpeed * p->MaxSpeed)) {
    NormalizeVector(&p->Momentum);
    p->Momentum.x *= p->MaxSpeed;
    p->Momentum.y *= p->MaxSpeed;
    p->Momentum.z *= p->MaxSpeed;
  }
}

void TurnShip(VPhysics * p, int direction, float amount, Vector * front, Vector * right, Vector * up) {
#pragma unused(p)
  Vector upAdjustment;
  
  switch (direction) {
    case LEFT:
      upAdjustment.x = (-1.0 * amount);
      upAdjustment.y = 0.0000001; /* In case up is {1.0, 0.0, 0.0} */
      upAdjustment.z = 0.0;
      break;
    case RIGHT:
      upAdjustment.x = (1.0 * amount);
      upAdjustment.y = 0.0000001; /* In case up is {-1.0, 0.0, 0.0} */
      upAdjustment.z = 0.0;
      break;
    case STATIONARY:
      upAdjustment.x = 0.0;
      upAdjustment.y = (1.0 * amount);
      upAdjustment.z = 0.0000001; /* In case up is {0.0, -1.0, 0.0} */
      break;
    case DOWN:
      upAdjustment.x = 0.0;
      upAdjustment.y = 0.0000001; /* In case up is {0.0, 0.0, 1.0} */
      upAdjustment.z = (-1.0 * amount);
      break;
    case UP:
      upAdjustment.x = 0.0;
      upAdjustment.y = 0.0000001; /* In case up is {0.0, 0.0, -1.0} */
      upAdjustment.z = (1.0 * amount);
      break;
    case (LEFT | DOWN):
      upAdjustment.x = (-1.0 * amount);
      upAdjustment.y = 0.0;
      upAdjustment.z = (-1.0 * amount);
      break;
    case (LEFT | UP):
      upAdjustment.x = (-1.0 * amount);
      upAdjustment.y = 0.0;
      upAdjustment.z = (1.0 * amount);
      break;
    case (RIGHT | UP):
      upAdjustment.x = (1.0 * amount);
      upAdjustment.y = 0.0;
      upAdjustment.z = (1.0 * amount);
      break;
    case (RIGHT | DOWN):
      upAdjustment.x = (1.0 * amount);
      upAdjustment.y = 0.0;
      upAdjustment.z = (-1.0 * amount);
      break;
  }
  AddVector(up, &upAdjustment, up);
  NormalizeVector(up);
  CrossProduct(up, front, right);
  CrossProduct(right, up, front); /* For some reason I find this line of code rather amusing... */
  NormalizeVector(right);
  NormalizeVector(front);
}

void RunPhysics(VPhysics * p, Vector * position, Vector * front, Vector * right, Vector * up, float interval) {
#pragma unused(interval) /* FIX LATER */
  float momentumMag, gravityMag;
  Vector axis = {0.0, 1.0, 0.0};
  Quaternion quat;
  
  p->Momentum.x *= (1.0 - p->WindResistance);
  p->Momentum.y *= (1.0 - p->WindResistance);
  p->Momentum.z *= (1.0 - p->WindResistance);
  momentumMag = fabs(VectorMagnitudeSquared(&p->Momentum));
  gravityMag = fabs(VectorMagnitudeSquared(&p->Gravity));
  if (momentumMag < (gravityMag * NOGRAVITYSPEEDMULTIPLIER)) {
    Vector adjustment;
    
    adjustment = p->Gravity;
    adjustment.x *= ((1.0 * momentumMag) / gravityMag);
    adjustment.y *= ((1.0 * momentumMag) / gravityMag);
    adjustment.z *= ((1.0 * momentumMag) / gravityMag);
    AddVector(&p->Momentum, &adjustment, &p->Momentum);
  }
  QuaternionRotateVector(-(atan2(up->x, up->z) * SHIPTURNING), interval, &axis, front, &quat);
  front->x = quat.v.x;
  front->y = quat.v.y;
  front->z = quat.v.z;
  NormalizeVector(front);
  CrossProduct(front, right, up);
  NormalizeVector(up);
  CrossProduct(up, front, right);
  NormalizeVector(right);
  AddVector(&p->Momentum, position, position);
}