#include "stick.h"

Stick::Stick() {
    stickQuad=gluNewQuadric();
    gluQuadricDrawStyle(stickQuad, GLU_FILL);
    gluQuadricNormals(stickQuad, GLU_SMOOTH);
    radius=.05;
    circle_points=20;
    length=1;
    orig_center[0]=center[0]=0.0;
    orig_center[1]=center[1]=0.0;
    orig_center[2]=center[2]=.5*length;
    nextDisk=NULL;
    colliding=0;
    
    initDisplayList();
    makePolyhedron();
    
}

Stick::~Stick() {
    
}

void Stick::setLength(double newLength) {
    length=newLength;
    //destroy old display list
    glDeleteLists(stickList, 1);
    initDisplayList();
    //remove old polyTree
    //free(myPoly);
    //free(myTree);
    makePolyhedron();}

void Stick::makePolyhedron() {
    char name[15]="aVert";
    myPoly=new Polyhedron;
    myTree=new PolyTree;
    //add all the vertices to this polyhedron
    Vect3 vec1(-.05, -.05, 0);
    Vect3 vec2(-.05, .05, 0);
    Vect3 vec3(.05, .05, 0);
    Vect3 vec4(.05, -.05, 0);
    Vect3 vec5(-.05, -.05, length);
    Vect3 vec6(-.05, .05, length);
    Vect3 vec7(.05, .05, length);
    Vect3 vec8(.05, -.05, length);
    myPoly->addVertex(name, vec1);
    myPoly->addVertex(name, vec2);
    myPoly->addVertex(name, vec3);
    myPoly->addVertex(name, vec4);
    myPoly->addVertex(name, vec5);
    myPoly->addVertex(name, vec6);
    myPoly->addVertex(name, vec7);
    myPoly->addVertex(name, vec8);
    myPoly->buildHull();
    if(myPoly->check())  cout<<"Error building poly"<<endl;
    myTree->setPoly(myPoly);
}

void Stick::initDisplayList() {
    
    stickList=glGenLists(1);
    glNewList(stickList, GL_COMPILE);
    //draw the stick
    glPushMatrix();
    gluCylinder(stickQuad, radius, radius, length, circle_points, circle_points); //rad, rad, height, slice, stack
    glPopMatrix();
    //draw each end
    glPushMatrix();
    glTranslated(0.0, 0.0, length);
    gluDisk(stickQuad, 0, radius, circle_points, circle_points);
    glPopMatrix();
    glPushMatrix();
    glRotated(180, 0.0, 1.0, 0.0);
    gluDisk(stickQuad, 0, radius, circle_points, circle_points);
    glPopMatrix();
    glEndList();
}

void Stick::draw(int calc) {
    //draw the stick
    glPushMatrix();
    if(calc==0)
    {
	if(colliding==0)
	{
	    glColor3f(0.0, 1.0, 0.0);
	}
	else
	{
	    glColor3f(1.0, 0.0, 0.0);
	}
	glCallList(stickList);
    }
    if(calc==1 || calc==3)
    {
	updatePoints(calc);
    }
    if(nextDisk!=NULL) {
	glTranslated(0.0, 0.0, length-(.5*(nextDisk->thickness)));
	nextDisk->draw(calc);
    }
    
    glPopMatrix();
    
}

void Stick::setNextDisk(Disk* next) {
    nextDisk=next;
}

int Stick::nextExists() {
    return !(nextDisk==NULL);
}

Disk* Stick::getNextDisk() {
    return nextDisk;
}

void Stick::updatePoints(int calc) {
    float mat[16];
    //m0 m4 m8  m12
    //m1 m5 m9  m13
    //m2 m6 m10 m14
    //m3 m7 m11 m15
    static Mat3 mat3;
    static Vect3 vec;
    
    glGetFloatv(GL_MODELVIEW_MATRIX, mat);
    if(calc==1)
    {
	center[0]=orig_center[0];
	center[1]=orig_center[1];
	center[2]=orig_center[2];
	updatePoint(mat, center);    
    }
    else if(calc==3)
    {
	//update stick pose
	mat3[0][0]=mat[0];
	mat3[0][1]=mat[4];
	mat3[0][2]=mat[8];
	mat3[1][0]=mat[1];
	mat3[1][1]=mat[5];
	mat3[1][2]=mat[9];
	mat3[2][0]=mat[2];
	mat3[2][1]=mat[6];
	mat3[2][2]=mat[10];
	vec[0]=mat[12];
	vec[1]=mat[13];
	vec[2]=mat[14];
	pose.set(mat3, vec);
    }
}

Stick* Stick::closestStick(float p[3], float *bestDistance) {
    float dist, dist2;
    Stick *stick2;
    
    dist=distance(p, center);
    stick2=nextDisk->closestStick(p, &dist2);
    if(stick2!=NULL)
    {
	if(dist<dist2)
	{
	    *bestDistance=dist;
	    return this;
	}
	else
	{
	    *bestDistance=dist2;
	    return stick2;
	}
    }
    //else no other to compare with, return self
    *bestDistance=dist;
    return this;
}

int Stick::collideCheck(Stick* newStick, ClosestFeaturesHT myHash,
			slist<stickPair>* collidingList, Stick* base) {
    Real result;
    Vect3 cp1, cp2;   // closest points between bodies, in body (local) frames
    int i;
    stickPair* aPair;
    double m[16];
    Mat3 m3;
    Vect3 v3;
    int collided;
    int return_me;
    return_me=0;
    
    for(i=0;i<8;i++)
    {
	if(nextDisk->nextStick[i]==newStick)
	{
	    return 0;
	}
    }
    if(nextDisk!=NULL)
    {
	for(i=0;i<8;i++)
	{
	    //check if the nextStick collides with newStick
	    if(nextDisk->nextStick[i]!=NULL)
		collided=nextDisk->nextStick[i]->collideCheck(newStick, myHash, collidingList, base);
	    if(collided==1)
	    {
		return_me=1;
	    }
		
	}
    }
    //if got here, newStick is not a successor to us, check
    //if we are colliding with it
    glPushMatrix();
    glLoadIdentity();
    //update the pose of newStick relative to this
    base->draw(3);
    pose.invert();
    //copy inverted pose into modelview matrix
    m3=pose.rot();
    v3=pose.trans();
    m[0]=m3[0][0];
    m[4]=m3[0][1];
    m[8]=m3[0][2];
    m[1]=m3[1][0];
    m[5]=m3[1][1];
    m[9]=m3[1][2];
    m[2]=m3[2][0];
    m[6]=m3[2][1];
    m[10]=m3[2][2];
    m[3]=0;
    m[7]=0;
    m[11]=0;
    m[12]=v3[0];
    m[13]=v3[1];
    m[14]=v3[2];
    m[15]=1;
    glMatrixMode(GL_MODELVIEW);
    glLoadMatrixd(m);
    base->draw(3);
    glPopMatrix();
    
    result=PolyTree::vclip(myTree, newStick->myTree, 
			    newStick->pose, myHash, cp1, cp2);
    if(result<=0)
    {
        //the sticks are in collision
        newStick->colliding=1;
        colliding=1;
	aPair=new stickPair;
	aPair->d1=this;
	aPair->d2=newStick;
	collidingList->push_front(*aPair);
	return_me=1;
    }
    return return_me;
}

Disk* Stick::closestDisk(float p[3], float *bestDistance) {
    
    if(nextDisk!=NULL)
	return nextDisk->closestDisk(p, bestDistance);
    else
    {
	*bestDistance=100;
	return NULL;
    }	
}