#include "Screw.h"
#include <windows.h>
#include <GL\gl.h>
#include <GL\glu.h>
#include <GL\glaux.h>
#include <GL\glut.h>
#include <stdlib.h>
#include <math.h>

Pose *pose1;
Pose *pose2;

float sx;
float sy;
float sz;
float px;
float py;
float pz;
float ox;
float oy;
float oz;

float dval;
float b;

void initialize(Pose *p1, Pose *p2)
{
	pose1 = p1;
	pose2 = p2;

	setupParams();
}

float doCross(float e1, float e2, float f1, float f2)
{
	return (e1*f2 - e2*f1);
}

void setupParams()
{
	float ix = pose2->ux - pose1->ux;
	float iy = pose2->uy - pose1->uy;
	float iz = pose2->uz - pose1->uz;
	float jx = pose2->vx - pose1->vx;
	float jy = pose2->vy - pose1->vy;
	float jz = pose2->vz - pose1->vz;
	float kx = pose2->wx - pose1->wx;
	float ky = pose2->wy - pose1->wy;
	float kz = pose2->wz - pose1->wz;

	sx = (doCross(iy, iz, jy, jz) + doCross(jy, jz, ky, kz) + doCross(ky, kz, iy, iz));
	sy = (doCross(iz, ix, jz, jx) + doCross(jz, jx, kz, kx) + doCross(kz, kx, iz, ix));
	sz = (doCross(ix, iy, jx, jy) + doCross(jx, jy, kx, ky) + doCross(kx, ky, ix, iy));

	float mags = sqrt((sx * sx) + (sy * sy) + (sz * sz));
	sx /= mags;
	sy /= mags;
	sz /= mags;

	float siax = doCross(sy, sz, pose1->uy, pose1->uz);
	float siay = doCross(sz, sx, pose1->uz, pose1->ux);
	float siaz = doCross(sx, sy, pose1->ux, pose1->uy);

	float ilen = sqrt((ix * ix) + (iy * iy) + (iz * iz));
	float jlen = sqrt((jx * jx) + (jy * jy) + (jz * jz));
	float sialen = sqrt((siax * siax) + (siay * siay) + (siaz * siaz));

	//Swap them if i is 0
	if((ilen == 0.f) && (jlen != 0.f))
	{
		float tx = ix;
		float ty = iy;
		float tz = iz;
		ix = jx;
		iy = jy;
		iz = jz;
		jx = tx;
		jy = ty;
		jz = tz;
	}
  
	//make sure direction is good
	float siadoti = ((siax * ix) + (siay * iy)+ (siaz * iz));
	if(siadoti < 0.f)
	{
		sx = -1.f * sx;
		sy = -1.f * sy;
		sz = -1.f * sz;
	}
  
	float arc = ilen / (2 * sialen);
	b = 2 * asinf(arc);		
	ox = pose2->ox - pose1->ox;
	oy = pose2->oy - pose1->oy;;
	oz = pose2->oz - pose1->oz;;
	dval = ((sx * ox) + (sy * oy) + (sz * oz));
	px = (pose2->ox + pose1->ox + doCross(sy, sz, oy, oz) / tanf(b / 2.f)) / 2.f;
	py = (pose2->oy + pose1->oy + doCross(sz, sx, oz, ox) / tanf(b / 2.f)) / 2.f;
	pz = (pose2->oz + pose1->oz + doCross(sx, sy, ox, oy) / tanf(b / 2.f)) / 2.f;
}

void displayAt(GLfloat time)
{
	//Initial pose
	float matrix[16] = {pose1->ux, pose1->uy, pose1->uz, 0,
		pose1->vx, pose1->vy, pose1->vz, 0,
		pose1->wx, pose1->wy, pose1->wz, 0, 
		pose1->ox, pose1->oy, pose1->oz, 1};
    
    
	float tmpx = px;
	float tmpy = py;
	float tmpz = pz;

	glLoadIdentity();
	glTranslatef(tmpx, tmpy, tmpz);
	glRotatef(time * (b/3.1415926)*180.f, sx, sy, sz);	
		
	//Trnslate P to origin and move d*t along S
	tmpx = -1 * px + sx * time * dval;
	tmpy = -1 * py + sy * time * dval;
	tmpz = -1 * pz + sz * time * dval;
	glTranslatef(tmpx, tmpy, tmpz);
		
	//Set pose1
	glMultMatrixf(matrix);
}