#include <math.h>
#include <stdio.h>

#define NRANSI
#include "nr.h"				//Numerical Recipes
#include "nrutil.h"			//Numerical Recipes

#include "Matrix3.h"
#include "Vector3.h"

/**************************** CONSTANTS *******************************/

#ifndef M_PI
const double M_PI=3.14159265358979323846;
#endif

#define ROCKET_ENGINE
static const NUMWING=4;

/**************************** VARIABLES *******************************/

double dxsav,*xp,**yp;		//Numerical Recipes
int kmax,kount;				//Numerical Recipes

/*********************** STRUCTURE RigidBody **************************/

struct RigidBody {
  Vector3 T;			// Translation
  Vector3 V;			// Velocity
  double M;				// Total Mass
  double phi,theta,psi;	// Eulerian Angles
  Vector3 omega_ego;	// Egocentric Angular Velocity
  Vector3 Ip;			// Principle Inertia

  // RigidBody(){};
  RigidBody(const double Tx, const double Ty, const double Tz,
	    const double Vx, const double Vy, const double Vz,
	    const double mass,
	    const double a1, const double a2, const double a3,
	    const double omega_ego_x,
	    const double omega_ego_y,
	    const double omega_ego_z,
	    const double Ipx, const double Ipy, const double Ipz
	    )
    :T(Tx,Ty,Tz),
     V(Vx,Vy,Vz),
     M(mass),
     phi(a1), theta(a2), psi(a3),
     omega_ego(omega_ego_x,omega_ego_y,omega_ego_z),
     Ip(Ipx,Ipy,Ipz)
  {};
};

/************************ INLINE FUNCTIONS ****************************/

static inline double RADIAN(double x) { return M_PI * x / 180; }; 
static inline double DEG(double x) { return x / M_PI * 180; };
static inline double DeRedundancy(double x) { return fmod(x,M_PI*2); };


/*********************** FUNCTION DEFINTIONS **************************/

Matrix3 rot_matrix_from_euler (double phi, double theta, double psi)
{
  return Matrix3 ( cos(phi),  -sin(phi),  0,
		   sin(phi),   cos(phi),  0,
		   0,          0,         1)

       * Matrix3 ( cos(theta), 0,         sin(theta),
		   0,          1,         0,
		  -sin(theta), 0,         cos(theta))

       * Matrix3 ( 1,          0,         0,
		   0,          cos(psi), -sin(psi),
		   0,          sin(psi),  cos(psi)) ;
};

static void compute_euler_angular_velocity(
    double &phi_dot, double &theta_dot, double &psi_dot,
    const double phi, const double theta,
    const Vector3& omega)
{

/*
  Consider that the column vector of our angular velocities
  is equal to omega*inverse(the axes of rotation).

  Matlab was used to find the inverse of the axes of rotation:
	>> st = sym('sin(t)');
	>> ct = sym('cos(t)');
	>> sp = sym('sin(p)');
	>> cp = sym('cos(p)');
	----
	>> inv(M)  
 
	ans =
 
	[    cp/ct/(cp^2+sp^2),    sp/ct/(cp^2+sp^2), 0]
	[      -sp/(cp^2+sp^2),       cp/(cp^2+sp^2), 0]
	[ cp*st/ct/(cp^2+sp^2), sp*st/ct/(cp^2+sp^2), 1]

  We need to multiply omega times this matrix.

  The code would have been much cleaner if I had used more 
  extensive matrix and vector classes (multiply vector & matrices, 
  normalize, etc.), but we were asked to only modify this file.  
*/

    Matrix3 m;

	m = Matrix3( cos(phi)/cos(theta), (sin(phi)/cos(theta)), 0,
		         -sin(phi),            cos(phi),             0,
				 (sin(theta)*cos(phi))/cos(theta), 
				 (sin(theta)*sin(phi))/cos(theta), 
				 1);

#   if 1
    psi_dot = m[0][0]*omega[0] + m[0][1]*omega[1] + m[0][2]*omega[2];
	theta_dot = m[1][0]*omega[0] + m[1][1]*omega[1] + m[1][2]*omega[2];
    phi_dot = m[2][0]*omega[0] + m[2][1]*omega[1] + m[2][2]*omega[2];

#   else
#     include "/afs/cs.unc.edu/home/hirota/public/290/rigid/viewer/rbody.bak/answer.h"
#   endif
}


/**********************************************************************/
/******************* CLASS DEF: RigidBodyDynamics *********************/
/**********************************************************************/

class RigidBodyDynamics {
private:
  // For ODE Int
  static int DerivEvalCount;
  static const int StateDim;

  // Rigid Body
  static Vector3 Ip;  // Principle Inertia
  static double M;    // Mass

  // Rotor Specific Parameters
  static const int NumWing;
  static Vector3 Rotor[NUMWING];
  static double Tilt_Angle;
  static Vector3 WingNormal[NUMWING];

public:
  // member funcs
  static void set_rotor(const int, const Vector3);
  static void set_tilt_angle(const double);
  static RigidBody ODE_Integrate( const RigidBody& rb, 
		const double StartTime,
		const double EndTime);
  static void compute_derivatives(double x,double y[],double dydx[]);
};

/********* DECLARE/INITIALIZE STATIC VARIABLES *********/

int RigidBodyDynamics::DerivEvalCount;
const RigidBodyDynamics::StateDim=12;
Vector3 RigidBodyDynamics::Ip;  // Principle Inertia
double RigidBodyDynamics::M;    // Mass

//Rotor Specific Parameters
const RigidBodyDynamics::NumWing=NUMWING;

Vector3 RigidBodyDynamics::Rotor[NUMWING]={	Vector3(1,0,0),
	Vector3(-1,0,0),Vector3(0,1,0),Vector3(0,-1,0)};

double RigidBodyDynamics::Tilt_Angle=5;
Vector3 RigidBodyDynamics::WingNormal[NUMWING];


/**************** FUNCTION DEFINITIONS *****************/

void RigidBodyDynamics::set_rotor( const int rotor_index, 
								   const Vector3 rotor_position)
{
	if(rotor_index<0||rotor_index>=NumWing) {
		fprintf(stderr,"wrong rotor_index\n");
		return;
	}
	Rotor[rotor_index]=rotor_position;
}


void RigidBodyDynamics::set_tilt_angle(const double a)
{
	Tilt_Angle=a;
}


RigidBody RigidBodyDynamics::ODE_Integrate( const RigidBody& rb,
    const double StartTime, const double EndTime)
{
  Ip = rb.Ip;
  M = rb.M;
  
  for(int i=0; i<NumWing; i++) {
	  const double alpha=RADIAN(Tilt_Angle);
	  Vector3 Up(0,0,1);	
	  Vector3 T=crossproduct(Up,Rotor[i]);
	  T.normalize();
	  WingNormal[i]=-cos(alpha)*Up+sin(alpha)*T;
  }

  // compute rotation matrix
  Matrix3 B = rot_matrix_from_euler(rb.phi,rb.theta,rb.psi); 
  
  // Egocentric Angular Momentum
  Vector3 L_ego = rb.Ip * rb.omega_ego;   
  
  // Rotate L_ego to get L
  Vector3 L = B * L_ego;                  
  
  // Call Numerical Recipes
    
  // intermediate result stored by odeint
  // not important
  xp=dvector(1,200);
  yp=dmatrix(1,20,1,200);

  // intial state values

  // allocate double ystart[1..StateDime]
  double *ystart=dvector(1,StateDim);  
  
  // NOTE: Array's Indices START FROM 1 in Numerical Recipes'
  
  // Set Initial Value of Translation
  ystart[1]=rb.T[0];
  ystart[2]=rb.T[1];
  ystart[3]=rb.T[2];
  
  // Set Initial Value of Velocity
  ystart[4]=rb.V[0];
  ystart[5]=rb.V[1];
  ystart[6]=rb.V[2];
  
  // Set Initial Value of Euler angles
  ystart[6+1]=rb.phi;
  ystart[6+2]=rb.theta;
  ystart[6+3]=rb.psi;
  
  // Set Initial Value of Angular Momentum
  ystart[6+4]=L[0];
  ystart[6+5]=L[1];
  ystart[6+6]=L[2];

  DerivEvalCount=0;

  // maximum number of intermediate values to be saved (can be 0)
  kmax=0; // 100;  
               
  // interval of intermediate value saving
  dxsav=(EndTime-StartTime)/20.0;  

  int nbad,nok;
  double eps=1.0e-5;
  double h1=0.1; 
  double hmin=0.0;
  
  typedef void (*DerivFunc) (double x,double y[],double dydx[]);
  
  DerivFunc derivs = (DerivFunc)RigidBodyDynamics::compute_derivatives;
  odeint(ystart, StateDim, StartTime, EndTime, eps, h1, hmin, &nok, 
	  &nbad, derivs, rkqs);

  
# if 0
  printf("\n%s %13s %3d\n","successful steps:"," ",nok);
  printf("%s %20s %3d\n","bad steps:"," ",nbad);
  printf("%s %9s %3d\n","function evaluations:"," ",DerivEvalCount);
  printf("\n%s %3d\n","stored intermediate values:    ",kount);
  printf("\n");
  {
	  int i;
	  for (i=1;i<=kount;i++)
		printf("%10.4f %16.10f %16.10f %16.10f\n",
			xp[i],
			DEG(yp[1][i]),
			DEG(yp[2][i]),
			DEG(yp[3][i]));
  }
# endif
  
  // Retreive Final Value of Translation
  double Tx=ystart[1];
  double Ty=ystart[2];
  double Tz=ystart[3];
  
  // Retreive Final Value of Velocity
  double Vx=ystart[4];
  double Vy=ystart[5];
  double Vz=ystart[6];
  
  // Retreive Final Value of Eulerian Angles, and Remove Redundant Rotation
  double phi=DeRedundancy(ystart[6+1]);
  double theta=DeRedundancy(ystart[6+2]);
  double psi=DeRedundancy(ystart[6+3]);
  
  // Retreive Final Value of Angular Momentum
  L[0]=ystart[6+4];
  L[1]=ystart[6+5];
  L[2]=ystart[6+6];

  // compute rotation matrix
  B = rot_matrix_from_euler(phi,theta,psi);
  
  // compute egocentric angular velocity
  Vector3 omega_ego
    = Vector3( 1/rb.Ip[0], 1/rb.Ip[1], 1/rb.Ip[2] )
    * ( transpose(B) * L );

  free_dmatrix(yp,1,10,1,200);
  free_dvector(xp,1,200);
  free_dvector(ystart,1,StateDim);

  return RigidBody(Tx,Ty,Tz,
		   Vx,Vy,Vz,
		   rb.M,
		   phi,theta,psi,
		   omega_ego[0],omega_ego[1],omega_ego[2],
		   rb.Ip[0],rb.Ip[1],rb.Ip[2]);
};


void RigidBodyDynamics::compute_derivatives(double x,double y[],double dydx[])
////////////////////////////////////////////////////////////////////////////////
// Call back from odeint(Numerical Recipes)
// Input Arguments:
// x    : time
// y[1] : position x
// y[2] : position y
// y[3] : position z
// y[4] : velocity x
// y[5] : velocity y
// y[6] : velocity z
// y[6+1] : phi 
// y[6+2] : theta
// y[6+3] : psi
// y[6+4] : angular momentum x
// y[6+5] : angular momentum y
// y[6+6] : angular momentum z
// Output Arguments
// dydx[] : derivatives of cooresponding parameters in y[]
////////////////////////////////////////////////////////////////////////////////
{
  DerivEvalCount++;
  
  // copy angular momentum
  Vector3 L(y[6+4],y[6+5],y[6+6]);
  
  // compute rotation matrix from Euler Angles
  double phi=y[6+1];
  double theta=y[6+2];
  double psi=y[6+3];
  Matrix3 B = rot_matrix_from_euler(phi,theta,psi);
  
  // compute egocentric angular velocity
  Vector3 omega_ego =
    Matrix3(1/Ip[0], 0, 0,
	      0, 1/Ip[1], 0,
	      0, 0, 1/Ip[2])
    * transpose(B)
    * L ;
  
  // Rotate egocentric angular velocity to get angular velocity
  Vector3 omega = B * omega_ego ;
  
  // compute angular velocity around three axes of Euler angles
  compute_euler_angular_velocity(dydx[6+1],dydx[6+2],dydx[6+3],phi,theta,omega);
  
  // derivatives of position is velocity
  dydx[1] = y[4];
  dydx[2] = y[5];
  dydx[3] = y[6];
  
  // Compute Total Force and Torque
  Vector3 F_ego(0,0,0);  // Total Force (egocentric)
  Vector3 N_ego(0,0,0);  // Total Torque (egocentric)
  
  
  double const g=9.8;

  for(int i=0;i<NumWing;i++) { // for each wing (blade)
	  const double AirDragFactor=2.5;
	  
	  // compute egocentric wing velocity
	  Vector3 WingVelocity=crossproduct(omega_ego,Rotor[i])
		               +(transpose(B)*Vector3(y[4],y[5],y[6]));
	  Vector3 AirDragForce=-AirDragFactor
		  *dotproduct(WingVelocity,WingNormal[i]) // normal component of velocity
		  *WingNormal[i];

	/******** CHANGED **********/
	  //project onto xy plane, normalize - this is the direction of 
	  // force created by the motor for this wing
	  Vector3 motor_dir = Vector3(WingNormal[i][0], WingNormal[i][1], 0);
	  motor_dir.normalize();
 
	  Vector3 MotorForce;
	  MotorForce = (9.8/4)*motor_dir;

	  //the motor forces cancel (which at first confused the heck out of me!)
	  F_ego=F_ego+AirDragForce + MotorForce;
	  //but the forces still affect the torque... 
	  N_ego=N_ego+crossproduct(Rotor[i],AirDragForce)+crossproduct(Rotor[i],MotorForce);
	/**************************/

  }
   
	 
  // rotate the egocentric force and torque to get force and torque
  Vector3 F = B * F_ego; // Total Force
  Vector3 N = B * N_ego; // Total Torque
  
  // Linear Acceleration = Force / Mass
  dydx[4]=1.00*(F[0]/M);
  dydx[5]=1.00*(F[1]/M);
  dydx[6]=1.00*(F[2]/M-g);

  // time derivative of angular momentum = torque
  dydx[6+4]=N[0];
  dydx[6+5]=N[1];
  dydx[6+6]=N[2];
};