#include "PhysicsManager.h"
#include "GameManager.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 = 0.25;
  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 axis = {0.0, 0.0, 0.0};
  Quaternion quat;
  
  switch (direction) {
    case LEFT:
      axis = *front;
      amount *= -1;
      break;
    case RIGHT:
      axis = *front;
      break;
    case STATIONARY:
      up->y += 0.02;
      amount = 0.0;
      break;
    case DOWN:
      axis = *right;
      break;
    case UP:
      axis = *right;
      amount *= -1;
      break;
    case (LEFT | DOWN):
      SubtractVector(&axis, front, &axis);
      AddVector(&axis, right, &axis);
      NormalizeVector(&axis);
      break;
    case (LEFT | UP):
      SubtractVector(&axis, right, &axis);
      SubtractVector(&axis, front, &axis);
      NormalizeVector(&axis);
      break;
    case (RIGHT | UP):
      SubtractVector(&axis, right, &axis);
      AddVector(&axis, front, &axis);
      NormalizeVector(&axis);
      break;
    case (RIGHT | DOWN):
      AddVector(right, front, &axis);
      NormalizeVector(&axis);
      break;
  }
  if (amount != 0.0) {
    QuaternionRotateVector(amount, 1.0, &axis, up, &quat);
    up->x = quat.v.x;
    up->y = quat.v.y;
    up->z = quat.v.z;
  }
  if (up->y < 0.0) up->y = 0.0;
  NormalizeVector(up);
  CrossProduct(up, front, right);
  NormalizeVector(right);
  CrossProduct(right, up, front); /* For some reason I find this line of code rather amusing... */
  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}, absoluteUp = {0.0, 0.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);
  /* Gravity? Nahh... */
  /*
  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);
  }
  */
  SubtractVector(&absoluteUp, &p->Gravity, &absoluteUp);
  NormalizeVector(&absoluteUp);
  if (right->y != 0.0) {
    QuaternionRotateVector(((SHIPTURNING * (up->y - absoluteUp.y)) * ((right->y > 0.0) ? 1.0 : -1.0)), 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);
}