#include "PhysicsManager.h"
#include "GameManager.h"

void InitPhysics(VPhysics * p) {
  p->Up.x = 0.0;
  p->Up.y = 1.0;
  p->Up.z = 0.0;
  p->WindResistance = 0.0003;
  p->MaxSpeed = 0.00625;
  p->Momentum.x = 0.0;
  p->Momentum.y = 0.0;
  p->Momentum.z = 0.0;
  p->Rotation.x = 0.0;
  p->Rotation.y = 0.0;
  p->Rotation.z = 0.0;
	p->SpeedMultiplier = 1.0;
}

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

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

void StopShip(VPhysics * p, float deceleration) {
  Vector direction;
  
  direction = p->Momentum;
  NormalizeVector(&direction);
  if (p->Momentum.x > 0.0) {
    p->Momentum.x -= (deceleration * direction.x);
    if (p->Momentum.x < 0.0) p->Momentum.x = 0.0;
  } else if (p->Momentum.x < 0.0) {
    p->Momentum.x -= (deceleration * direction.x);
    if (p->Momentum.x > 0.0) p->Momentum.x = 0.0;
  }
  if (p->Momentum.y > 0.0) {
    p->Momentum.y -= (deceleration * direction.y);
    if (p->Momentum.y < 0.0) p->Momentum.y = 0.0;
  } else if (p->Momentum.y < 0.0) {
    p->Momentum.y -= (deceleration * direction.y);
    if (p->Momentum.y > 0.0) p->Momentum.y = 0.0;
  }
  if (p->Momentum.z > 0.0) {
    p->Momentum.z -= (deceleration * direction.z);
    if (p->Momentum.z < 0.0) p->Momentum.z = 0.0;
  } else if (p->Momentum.z < 0.0) {
    p->Momentum.z -= (deceleration * direction.z);
    if (p->Momentum.z > 0.0) p->Momentum.z = 0.0;
  }
}

void TurnShip(VPhysics * p, int direction, float interval) {
  if (direction & LEFT) {
    p->Rotation.y += (SHIPTURNING / interval);
    if (p->Rotation.y > 1.0) p->Rotation.y = 1.0;
  } else if (direction & RIGHT) {
    p->Rotation.y -= (SHIPTURNING / interval);
    if (p->Rotation.y < -1.0) p->Rotation.y = -1.0;
  } else {
    p->Rotation.y *= (1.0 - (0.01 / interval));
  }
  if (direction & DOWN) {
    p->Rotation.x += (SHIPTURNING / interval);
    if (p->Rotation.x > 1.0) p->Rotation.x = 1.0;
  } else if (direction & UP) {
    p->Rotation.x -= (SHIPTURNING / interval);
    if (p->Rotation.x < -1.0) p->Rotation.x = -1.0;
  } else {
    p->Rotation.x *= (1.0 - (0.01 / interval));
  }
}

void RunPhysics(VPhysics * p, Vector * position, Vector * front, Vector * right, Vector * up, float interval) {
  Vector axis;
  Quaternion quat;
  
  if (p->Rotation.y != 0.0) {  
    axis = p->Up;
    QuaternionRotateVector((SHIPTURNING * p->Rotation.y), interval, &axis, front, &quat);
    front->x = quat.v.x;
    front->y = quat.v.y;
    front->z = quat.v.z;
    NormalizeVector(front);
    QuaternionRotateVector((SHIPTURNING * p->Rotation.y), interval, &axis, up, &quat);
    up->x = quat.v.x;
    up->y = quat.v.y;
    up->z = quat.v.z;
    NormalizeVector(up);
    if (up->y < 0.0) {
      p->Rotation.x = 0.0;
      up->y = 0.0;
      NormalizeVector(up);
      CrossProduct(right, up, front);
      NormalizeVector(front);
    }
    CrossProduct(up, front, right);
    NormalizeVector(right);
  }
  
  if (p->Rotation.x != 0.0) {  
    axis = *right;
    QuaternionRotateVector((SHIPTURNING * p->Rotation.x), interval, &axis, front, &quat);
    front->x = quat.v.x;
    front->y = quat.v.y;
    front->z = quat.v.z;
    NormalizeVector(front);
    QuaternionRotateVector((SHIPTURNING * p->Rotation.x), interval, &axis, up, &quat);
    up->x = quat.v.x;
    up->y = quat.v.y;
    up->z = quat.v.z;
    NormalizeVector(up);
    if (up->y < 0.0) {
      p->Rotation.x = 0.0;
      up->y = 0.0;
      NormalizeVector(up);
      CrossProduct(right, up, front);
      NormalizeVector(front);
    }
    CrossProduct(up, front, right);
    NormalizeVector(right);
  }
  
  p->Momentum.x *= (1.0 - (p->WindResistance / interval));
  p->Momentum.y *= (1.0 - (p->WindResistance / interval));
  p->Momentum.z *= (1.0 - (p->WindResistance / interval));
  position->x += (p->Momentum.x / interval);
  position->y += (p->Momentum.y / interval);
  position->z += (p->Momentum.z / interval);
}