#include <stdlib.h>
#include <stdio.h>  
#include <GL/gl.h>
#include <GL/glu.h>
#include <GL/glut.h>

#include <math.h>

double A1x,A1y,A1z,T1;
double A2x,A2y,A2z,T2;
double Akx,Aky,Akz;
double K;
double inc = 0.0;
double Bx,By,Bz;
double THETA;
double PHI;


static void _InitMaterial(void)
{
  /* define material properties */
  GLfloat material_ambient[] = {0.25f, 0.25f, 0.25f};
  GLfloat material_diffuse[] = {0.90f, 0.90f, 0.90f};
  GLfloat material_specular[] = {0.90f, 0.90f, 0.90f};
  GLfloat material_shininess = 25.0f;

  /* load material properties */
  glMaterialfv(GL_FRONT, GL_AMBIENT, material_ambient);
  glMaterialfv(GL_FRONT, GL_DIFFUSE, material_diffuse);
  glMaterialfv(GL_FRONT, GL_SPECULAR, material_specular);
  glMaterialf(GL_FRONT, GL_SHININESS, material_shininess);
}

static void _InitLights(void)
{
  /* define light properties */
  GLfloat light0_diffuse[] = {0.5, 0.5, 0.5, 1.0};
  GLfloat light0_position[] = {-1.0, 0.0, 1.0, 0.0};
  GLfloat light1_diffuse[] = {0.3, 0.3, 0.3, 1.0};
  GLfloat light1_position[] = {1.0, 0.0, 1.0, 0.0};

  /* enable lights 0 and 1*/
  glEnable(GL_LIGHTING);
  glEnable(GL_LIGHT0);
  glEnable(GL_LIGHT1);

  /* load light properties */
  glLightfv(GL_LIGHT0, GL_DIFFUSE, light0_diffuse);
  glLightfv(GL_LIGHT0, GL_POSITION, light0_position);
  glLightfv(GL_LIGHT1, GL_DIFFUSE, light1_diffuse);
  glLightfv(GL_LIGHT1, GL_POSITION, light1_position);
}

static void _Init(void)
{  
  /* set the clear colour (R, G, B, A) */
  glClearColor(0.0, 0.0, 0.0, 1.0f);

  /* enable depth testing */
  glEnable(GL_DEPTH_TEST);

  /* enabled lighting */
  glEnable(GL_LIGHTING);

  /* initialise materials */
  _InitMaterial();

  /* initialise lights */
  _InitLights();

  /* position and orient the camera */ 
  gluLookAt(0.0, 0.0, 10.0, /* eye point */
	    0.0, 0.0, 0.0,  /* reference point */
	    0.0, 1.0, 0.0); /* up vector */

   Bx = A1y*A2z - A1z*A2y;
   By = A1z*A2x - A1x*A2z;
   Bz = A1x*A2y - A1y*A2x;

   double lA1 = sqrt(A1x*A1x+A1y*A1y+A1z*A1z);
   double lA2 = sqrt(A2x*A2x+A2y*A2y+A2z*A2z);
  
   PHI = acos((A1x*A2x+A1y*A2y+A1z*A2z)/(lA1*lA2)); 
   printf("PHI=%f\n",PHI);
   printf("BX=%f BY=%f BZ=%f\n",Bx,By,Bz);
}

static void RotateAboutAxisP(double x, double y, double z, double theta, double *px, double *py, double *pz)
{
	//printf("%f %f %f %f %f %f %f\n",x,y,z,theta,*px,*py,*pz);
	//printf("%f\n",theta);

	/* theta in radians */
	double rotMatrix[16];
	double length = sqrt(x*x+y*y+z*z);
	x = x/length;
	y = y/length;
	z = z/length;

	double t = 1-cos(theta);
	double s = sin(theta);
	double c = cos(theta);

	double tempx, tempy, tempz;

	rotMatrix[0] = t*x*x+c;
	rotMatrix[1] = t*x*y-s*z;
	rotMatrix[2] = t*x*z+s*y;
	rotMatrix[3] = 0;
	rotMatrix[4] = t*x*y+s*z;
	rotMatrix[5] = t*y*y+c;
	rotMatrix[6] = t*z*y-s*x;
	rotMatrix[7] = 0;
	rotMatrix[8] = t*x*z-s*y;
	rotMatrix[9] = t*y*z+s*x;
	rotMatrix[10] = t*z*z+c;
	rotMatrix[11] = 0;
	rotMatrix[12] = 0;
	rotMatrix[13] = 0;
	rotMatrix[14] = 0;
	rotMatrix[15] = 1;


	tempx = *px;	
	tempy = *py;	
	tempz = *pz;	

	(*px) = tempx*rotMatrix[0] + tempy*rotMatrix[4] + tempz*rotMatrix[8] + rotMatrix[12];
	(*py) = tempx*rotMatrix[1] + tempy*rotMatrix[5] + tempz*rotMatrix[9] + rotMatrix[13];
	(*pz) = tempx*rotMatrix[2] + tempy*rotMatrix[6] + tempz*rotMatrix[10] + rotMatrix[14];
	//printf("%f %f %f %f %f %f %f\n",x,y,z,theta,*px,*py,*pz);

}

static void RotateAboutAxis(double x, double y, double z, double theta)
{
	/* theta in radians */
	double rotMatrix[16];
	double length = sqrt(x*x+y*y+z*z);
	x = x/length;
	y = y/length;
	z = z/length;

	double t = 1-cos(theta);
	double s = sin(theta);
	double c = cos(theta);

	rotMatrix[0] = t*x*x+c;
	rotMatrix[1] = t*x*y-s*z;
	rotMatrix[2] = t*x*z+s*y;
	rotMatrix[3] = 0;
	rotMatrix[4] = t*x*y+s*z;
	rotMatrix[5] = t*y*y+c;
	rotMatrix[6] = t*z*y-s*x;
	rotMatrix[7] = 0;
	rotMatrix[8] = t*x*z-s*y;
	rotMatrix[9] = t*y*z+s*x;
	rotMatrix[10] = t*z*z+c;
	rotMatrix[11] = 0;
	rotMatrix[12] = 0;
	rotMatrix[13] = 0;
	rotMatrix[14] = 0;
	rotMatrix[15] = 1;

	glMultMatrixd(rotMatrix);
	
}

static void _Draw(void)
{
  Akx = A1x;
  Aky = A1y;
  Akz = A1z;

  RotateAboutAxisP(Bx,By,Bz,inc*PHI,&Akx,&Aky,&Akz);

  THETA = (1-inc)*T1+inc*T2;

  /* clear the color and depth buffers */
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

  glLoadIdentity();
  gluLookAt(0.0, 0.0, 10.0, /* eye point */
	    0.0, 0.0, 0.0,  /* reference point */
	    0.0, 1.0, 0.0); /* up vector */

  /* preserve modelview matrix */
  glPushMatrix(); 

  RotateAboutAxis(Akx,Aky,Akz,THETA);

  /* render a solid teapot */
  glutSolidTeapot(2.0f);

  /* return to previous modelview matrix */
  glPopMatrix();

  /* swap front and back buffers */
  glutSwapBuffers();
}

static void _Reshape(int width, int height)
{
  /* set the viewport to the window width and height */
  glViewport(0, 0, width, height);

  /* load a projection matrix that matches the window aspect ratio */
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  gluPerspective(45.0, (double)width/(double)height, 1.0, 100.0);

  /* return to modelview matrix mode*/
  glMatrixMode(GL_MODELVIEW);
}

static void _Key(unsigned char key, int x, int y)
{
  switch (key)
  {
  case 27: /* escape key */
    exit(0);
    break;
  }
}

static void _Timer(int value)
{
  inc+=K;
  //printf("Ax=%f   Ay=%f   Az=%f   Theta=%f\n",Akx,Aky,Akz,THETA);
  if (inc>1.0) inc=0;
  /* send redisplay event */
  glutPostRedisplay();

  /* call this function again in 10 milliseconds */
  glutTimerFunc(10, _Timer, 0);
}

int main(int argc, char *argv[])
{

  if (argc!=10)
  {
	printf("Usage: angleAxisInterpolation <A1x> <A1y> <A1z> <T1> <A2x> <A2y> <A2z> <T2> <k>");
	exit(1);
  }

  A1x = atof(argv[1]);
  A1y = atof(argv[2]);
  A1z = atof(argv[3]);
  T1 = atof(argv[4]);
  A2x = atof(argv[5]);
  A2y = atof(argv[6]);
  A2z = atof(argv[7]);
  T2 = atof(argv[8]);
  K = atof(argv[9]);

  glutInitWindowSize(400, 400);                   /* set window size */
  glutInitDisplayMode(GLUT_RGBA | 
		      GLUT_DOUBLE | 
		      GLUT_DEPTH);                /* set display mode */
  glutCreateWindow("Angle and Axis Interpolation");    /* create a window */

  glutReshapeFunc(_Reshape);   /* register callback for window resize */
  glutDisplayFunc(_Draw);      /* register callback for window redraw */
  glutKeyboardFunc(_Key);      /* register callback for keyboard input */

  glutTimerFunc(10, _Timer, 0); /* register callback for a timer */

  _Init(); /* set initial OpenGL state and initialise data */

  glutMainLoop(); /* start the main event loop */

  return 1;
}