#include <GL/glut.h>
#include <iostream.h>
#include "spaceship.h"
#include <math.h>

Spaceship::Spaceship(int shipType) {
  _shipType = shipType;
  position = Math3d::M3d(0.0, 0.0, 0.0);
  
  radius = 10.0;
  
  orientation = Math3d::MQuat(0.0, 0.0, 1.0, 0.0);
  

  velocity = rollinc = pitchinc = yawinc = velinc = 0.0;
  cycler = 0.0;
  velocity = 0.0;
  health = 1.0;
  alive = true;
  maxVelocity = 2.0;
}


Math3d::M4x4 Spaceship::GetRotationMat()  {
  Math3d::M4x4 mat;
  mat.loadRotate(orientation);
  return mat;
}


Math3d::M3d Spaceship::GetDirection()  {
#if 0
  Math3d::M4x4 mat;
  mat.loadRotate(orientation);
  Math3d::M3d direction = Math3d::M3d(0.0, 0.0, 1.0);
  direction = mat * direction;
  return direction;
#endif
  return GetDirection(orientation);
}

Math3d::M3d Spaceship::GetDirection(Math3d::MQuat orient)  {
  Math3d::M4x4 mat;
  mat.loadRotate(orient);
  Math3d::M3d direction = Math3d::M3d(0.0, 0.0, 1.0);
  direction = mat * direction;
  return direction;
}

void Spaceship::UpdateState(float timeElapsed, bool roll) {


  float sensitivitymult = 0.01;
  bool invertpitchcontrol = true;

  cycler += timeElapsed;
  hover = Math3d::M3d((float)sin(cycler*1.23f), (float)sin(cycler*1.55f), (float)sin(cycler*2.19f));
  hover *= 0.1;

  Math3d::M3d direction = GetDirection();
  //  cout << "Dir A:" << direction << _shipType << endl;
  //  cout << axis << endl;
  // update ships position
  //  cout << "Direction: " << direction << endl;
  position += direction * velocity * timeElapsed * 75.0;

  //  cout << "Velinc: " << velinc << endl;
  
  // Update ship's velocity
  if (velocity < maxVelocity) 
    velocity += velinc * timeElapsed * 40.0;


  // Update the ship's heading (yaw)
  if (roll) {
    matRoll.loadRotateZ(-rollinc * sensitivitymult);
    rollQuat = Math3d::MQuat(matRoll);
    orientation = rollQuat * orientation;
  }
  else {
    matYaw.loadRotateY(yawinc * sensitivitymult);
    yawQuat = Math3d::MQuat(matYaw);
    orientation *= yawQuat; // * orientation;
  }

  // Update the ship's heading (pitch)
  if (invertpitchcontrol) 
    matPitch.loadRotateX(-pitchinc * sensitivitymult);
  else 
    matPitch.loadRotateX(pitchinc * sensitivitymult);
  
  pitchQuat = Math3d::MQuat(matPitch);
  orientation *= pitchQuat;// * orientation;

  // Update the ship's roll
  /*

  */

  // originally 4.5
  dampenrotlots = 1.0f - (timeElapsed * 8.0);
  dampenrot = 1.0f - (timeElapsed);
  dampenvel = 1.0f - (timeElapsed);
  
#if 1
  if (health > 0.2f) {  // guidance systems online
    rollinc *= dampenrotlots; // dampen rotation
    pitchinc *= dampenrotlots;
    yawinc *= dampenrotlots;	
  }
  else { // guidance systems offline
    rollinc *= dampenrot; // dampen rotation less
    pitchinc *= dampenrot;
    yawinc *= dampenrot;	
  }
#endif  

  velinc *= dampenvel; // dampen acceleration
  velocity *= dampenvel; // dampen speed

  //  cout << "Velocity: " << velocity << endl;

  
}
  

bool Spaceship::IsCollision(BoundingVol bvol) { 
  float sumr = radius + bvol.radius;

  // calculate the distance b/w the centers
  float x = position[0] - bvol.center[0];
  x *= x;
  float y = position[1] - bvol.center[1];
  y *= y;
  float z = position[2] - bvol.center[2];
  z *= z;

  float dist = sqrt(x+y+z);

  return (dist <= sumr);
}

void Spaceship::Hit(float damage) {
  health -= damage; 
  //  cout << "New Health: " << health << endl;
  if (health <= 0.01)
    alive = false;
}

void Spaceship::Restart() {
  position = Math3d::M3d(0.0, 0.0, 0.0);
    
  orientation = Math3d::MQuat(0.0, 0.0, 1.0, 0.0);
  
  velocity = rollinc = pitchinc = yawinc = velinc = 0.0;
  cycler = 0.0;
  velocity = 0.0;
  health = 1.0;
  alive = true;

}