#include "model.h"

void Model::writefile(){
  char str[100];
  sprintf(str, "scrsave /var/tmp/arns/%05d.rgb 0 500 0 500", id);
  system(str);
  //fprintf(stderr, "saving %05d.rgb\n", id++); 
  id++;
}



Model::Model() {
    handQuad=gluNewQuadric();
    gluQuadricDrawStyle(handQuad, GLU_FILL);
    gluQuadricNormals(handQuad, GLU_SMOOTH);
    basePos[0]=0;
    basePos[1]=5;
    basePos[2]=0;
    id=0;
}

Model::~Model() {
    
}

void Model::init() {
    inMotion=0;
    moveMe=baseStick.getNextDisk();
    handList=glGenLists(1);
    glNewList(handList, GL_COMPILE);
    glPushMatrix();
    glColor3f(1, 0, 1);
    gluSphere(handQuad, .25, 10, 10);
    glPopMatrix();
    glEndList();
}

void Model::draw(int calc) {
    //1=calculate, 0=draw
    glPushMatrix();
    glTranslatef(basePos[0], basePos[1], basePos[2]);
    baseStick.draw(calc);
    glPopMatrix();
}

void Model::drawBall() {
    glCallList(handList);
}

Stick* Model::findClosestStick(float p[3]) {
    float bestDistance;
    Stick* currentBest;
    
    currentBest=baseStick.closestStick(p, &bestDistance);
    
    return currentBest;
}

Disk* Model::findClosestDisk(float p[3]) {
    float bestDistance;
    Disk* currentBest;
    
    currentBest=baseStick.closestDisk(p, &bestDistance);
    
    return currentBest;
}

void Model::collideCheckNewStick(Stick* newStick) {
    //we just added newStick, see if it collides with anything
    baseStick.collideCheck(newStick, myHash, &collidingList, &baseStick);
}

int Model::collideCheckStickRecursive(Stick* aStick) {
    int collided, return_me;
    //for the stick we are on, remove anything its colliding
    //with from the list, then recursively check each of
    //the sticks that follow it in the tree for collisions, then
    //check ourself for collisions.
    return_me=0;
    removeColliding(aStick);
    if(aStick->getNextDisk()!=NULL)
    {
	collided=collideCheckRotated(aStick->getNextDisk());
	if(collided==1)
	    return_me=1;
    }
    collided=baseStick.collideCheck(aStick, myHash, &collidingList, &baseStick);
    if(collided==1)
	    return_me=1;
    return return_me;
}

int Model::collideCheckRotated(Disk* aDisk) {
    int i, collide_occured;
    int return_me;
    
    return_me=0;
    if(aDisk->orient==0)
    {
	for(i=1;i<8;i++)
	{
	    if(aDisk->nextStick[i]!=NULL)
	    {
		//check the next stick for collision
		collide_occured=collideCheckStickRecursive(aDisk->nextStick[i]);
		if(collide_occured==1)
		    return_me=1;
	    }
	}
    }
    else
    {
	if(aDisk->nextStick[0]!=NULL)
	{
	    //check the next stick for collision
	    collide_occured=collideCheckStickRecursive(aDisk->nextStick[0]);
	    if(collide_occured==1)
		return_me=1;
	}
	if(aDisk->nextStick[1]!=NULL)
	{
	    //check the next stick for collision
	    collide_occured=collideCheckStickRecursive(aDisk->nextStick[1]);
	    if(collide_occured==1)
		return_me=1;
	}
	for(i=3;i<8;i++)
	{  
	    if(aDisk->nextStick[i]!=NULL)
	    {
		//check the next stick for collision
		collide_occured=collideCheckStickRecursive(aDisk->nextStick[i]);
		if(collide_occured==1)
		    return_me=1;
	    }
	}
    }
    return return_me;
}

void Model::removeColliding(Stick* aStick) {
    slist<stickPair>::iterator myIter;
    slist<stickPair>::iterator tempIter;
    stickPair aPair;
    
    if(collidingList.empty())
	return;
    myIter=collidingList.begin();
    while(myIter!=collidingList.end())
    {
	aPair=*(myIter);
	tempIter=myIter;
	//get next pair
	myIter++;
	//if either Stick pointed to is aStick, then remove
	if(aStick==aPair.d1 || aStick==aPair.d2)
	{
	    aPair.d1->colliding=0;
	    aPair.d2->colliding=0;
	    collidingList.erase(tempIter);
	}
    }
}

void Model::checkList() {
    slist<stickPair>::iterator myIter;
    slist<stickPair>::iterator tempIter;
    Real result;
    Vect3 cp1, cp2;   // closest points between bodies, in body (local) frames
    stickPair aPair;
    double m[16];
    Mat3 m3;
    Vect3 v3;
    if(collidingList.empty())
	return;
    myIter=collidingList.begin();
    while(myIter!=collidingList.end())
    {
	aPair=*(myIter);
	//run vclip on the current pair of sticks to
	//see if they are still colliding
	glPushMatrix();
	glLoadIdentity();
	//update the pose of one stick relative to the other
	baseStick.draw(3);
	aPair.d1->pose.invert();
	//copy inverted pose into modelview matrix
	m3=aPair.d1->pose.rot();
	v3=aPair.d1->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);
	baseStick.draw(3);
	glPopMatrix();
	
	result=PolyTree::vclip(aPair.d1->myTree, aPair.d2->myTree, 
			    aPair.d2->pose, myHash, cp1, cp2);
	tempIter=myIter;
	//get next pair
	myIter++;
	if(result>0)
	{
	    //the sticks are not in collision
	    aPair.d1->colliding=0;
	    aPair.d2->colliding=0;
	    collidingList.erase(tempIter);
	}
	
    }
}

void Model::startMotion() {
    int number, number2;
    
    //pick a random disk to move
    number=number2=5;
    number=(int) 10*drand48();
    while(number!=0)
    {
	number2=(int) 8*drand48();
	if(moveMe->nextStick[number2]->getNextDisk()==NULL)
	    moveMe=baseStick.getNextDisk();
	else
	    moveMe=moveMe->nextStick[number2]->getNextDisk();
	number--;
    }
    //pick a random direction to move
    if(number2%2==0)
	movingDirection=1;
    else
	movingDirection=-1;
    maxRotate=(int) 360*drand48();
    if(moveMe==baseStick.getNextDisk())
	startMotion();
}

void Model::move() {
    int collide_occurred;
    static int moved=0;
    
    //try to move the selected disk a small amount
    //if it collides, move it back and pick a new
    //disk and direction to randomly move
    if(moveMe->orient==0)
    {
	moveMe->orient0_angle=moveMe->orient0_angle+movingDirection;	    
    }
    else
    {
	moveMe->orient1_angle=moveMe->orient1_angle+movingDirection;
    }
    moved++;
    //collision check
    collide_occurred=collideCheckRotated(moveMe);
    if(collide_occurred || moved==maxRotate)
    {
	//reverse the move
	if(moveMe->orient==0)
	{
	    moveMe->orient0_angle=moveMe->orient0_angle-movingDirection;	    
	}
	else
	{
	    moveMe->orient1_angle=moveMe->orient1_angle-movingDirection;
	}
	//redo collision check
	checkList();
	startMotion();
	if(moved==maxRotate)
	    moved=0;
    }
}