#include <cmath>

#include "Camera.h"
#include "Skeleton.h"
#include "misc.h"

extern float multiplier;
extern unsigned int gSourceID[37];
extern float rad2deg;
extern Camera camera;
extern float soundscalefactor;

void Joint::DoConstraint()
{
	if(hasparent){
		//Find midpoint
		XYZ midp = (position + parent->position) / 2;
		// Find vector from midpoint to second vector
		XYZ vel = normalize(parent->position - midp);
		//Apply velocity change
		velocity+=((midp-vel*length/2)-position);
		parent->velocity+=((midp+vel*length/2)-parent->position);
		//Move child point to within certain distance of parent point
		if(!locked)position=midp-vel*length/2;
		if(!parent->locked)parent->position=midp+vel*length/2;
	}
}

void Muscle::DoConstraint(int broken)
{
	float oldlength = this->length;
	float relaxlength = len(parent1->position - parent2->position);

	if(type==boneconnect)strength=1;
	if(type==constraint)strength=0;

	if(strength<0)strength=0;
	if(strength>1)strength=1;

	length-=(length-relaxlength)*(1-strength)*multiplier*multiplier*10000;
	length-=(length-targetlength)*(strength)*multiplier*multiplier*10000;
	if(strength==0)length=relaxlength;

	if((relaxlength-length>0&&relaxlength-oldlength<0)||(relaxlength-length<0&&relaxlength-oldlength>0))length=relaxlength;

	if(length<minlength)length=minlength;
	if(length>maxlength&&!broken)length=maxlength;

	//Find midpoint
	XYZ midp = (parent1->position + parent2->position) / 2;
	// Find vector from midpoint to second vector
	XYZ vel = normalize(parent2->position - midp);
	//Apply velocity change
	XYZ newpoint1 = midp - vel * length / 2;
	XYZ newpoint2 = midp + vel * length / 2;
	parent1->velocity+=(newpoint1-parent1->position);
	parent2->velocity+=(newpoint2-parent2->position);
	//Move child point to within certain distance of parent point
	if(!parent1->locked)parent1->position=newpoint1;
	if(!parent2->locked)parent2->position=newpoint2;
}

void Skeleton::DoConstraints()
{
	for (int i = 0; i < 3; ++i)
		for (auto& joint : joints)
			joint.DoConstraint();
}

void Skeleton::DoConstraints(Model *collide, XYZ *move, float rotation)
{
	move->y += 0.35;
	for (int j = 0; j < 2; j++) {
		for (int i = 0; i < max_joints; i++) {
			if (!joints[i].existing && i != lefthand && i != righthand)
				continue;

			// Length constraints
			joints[i].DoConstraint();
			// Ground constraint
			auto& pos = joints[i].position;
			XYZ overpoint = pos;
			overpoint.y += 10;
			XYZ underpoint = pos;
			XYZ impact;
			int whichtri = segCrossModelTrans(overpoint, underpoint,
				collide, *move, rotation, &impact);
			if (whichtri == -1
			    || collide->faces[whichtri][0].normal.y <= 0.8)
				whichtri = segCrossModelTrans(joints[i].realoldposition, pos,
					collide, *move, rotation, &impact);
			const auto& normal = collide->faces[whichtri][0].normal;

			if (pos.y <= 0 || whichtri != -1) {
				if (whichtri == -1 || normal.y > 0.8) {
					if (whichtri == -1)
						pos.y = 0;
					else
						pos = impact;
					joints[i].velocity.y *= -0.3;
					joints[i].velocity.x *= 0.3;
					joints[i].velocity.z *= 0.3;
				}

				if (whichtri != -1 && normal.y <= 0.8) {
					XYZ normalrotated = rotate(normal, 0, rotation, 0);
					pos = impact + normalrotated;
					reflect(&joints[i].velocity, normalrotated);
					joints[i].velocity *= 0.3;
				}

				if (broken > 1)
					continue;
				XYZ avgvelocity {0};
				for (int k = 0; k < max_joints; k++)
					avgvelocity += joints[k].velocity;
				avgvelocity /= max_joints;

				int landsound = -1;
				switch (joints[i].label) {
				case head:
					landsound = headlandsound;
					break;
				case abdomen:
					landsound = bodylandsound;
					break;
				}
				ALint playing = AL_PLAYING;
				if (landsound >= 0)
					alGetSourcei(gSourceID[landsound],
						AL_SOURCE_STATE, &playing);

				if (playing != AL_PLAYING
				    && (sqrlen(joints[i].velocity) > 2
				        || sqrlen(avgvelocity) > 2)) {
					auto soundpos = pos - camera.position;
					playSound(gSourceID[landsound],
						soundpos.x, soundpos.y, soundpos.z);
				}
			}
		}

		for (int i = 0; i < num_muscles; i++)
			//Length constraints
			muscles[i].DoConstraint(broken);
	}

	for (int i = 0; i < max_joints; i++) {
		joints[i].realoldposition = joints[i].position;
		//Add velocity
		if (joints[i].existing || i == lefthand || i == righthand)
			joints[i].position += joints[i].velocity * multiplier;
	}
}

void Skeleton::DoGravity()
{
	for (auto& joint : joints)
		joint.velocity.y += gravity * multiplier;
}

void Skeleton::Draw(int  muscleview)
{
	float jointcolor[4];

	if(muscleview!=2){
		jointcolor[0]=0;
		jointcolor[1]=0;
		jointcolor[2]=.5;
		jointcolor[3]=1;
	}

	if(muscleview==2){
		jointcolor[0]=0;
		jointcolor[1]=0;
		jointcolor[2]=0;
		jointcolor[3]=.5;
	}

	if(muscleview!=2){
		glPointSize(3);
		glBegin(GL_POINTS);
			for(int i=0; i<max_joints; i++){
				if(i!=selected)glColor4f(0,0,.5,1);
				if(i==selected)glColor4f(1,1,0,1);
				if(joints[i].locked&&i!=selected)glColor4f(1,0,0,1);
				glVertex3f(joints[i].position.x,joints[i].position.y,joints[i].position.z);
			}
		glEnd();
	}

	//Set old position to current position
	if(muscleview==2)
	for(int i=0; i<max_joints; i++){
		joints[i].oldposition=joints[i].position;
	}
	glDepthMask(1);
}

void Skeleton::DeleteJoint(int whichjoint)
{
	if (whichjoint < max_joints && whichjoint >= 0) {
		for(int i=0;i<num_muscles;i++){
			while(muscles[i].parent1==&joints[whichjoint]&&i<num_muscles)DeleteMuscle(i);
			while(muscles[i].parent2==&joints[whichjoint]&&i<num_muscles)DeleteMuscle(i);
		}

		for (auto& joint : joints)
			if (joint.parent == &joints[whichjoint])
				joint.hasparent = 0;
		joints[whichjoint].hasparent = 0;
	}
}

void Skeleton::DeleteMuscle(int whichmuscle)
{
	if(whichmuscle<num_muscles){
		muscles[whichmuscle].minlength=muscles[num_muscles-1].minlength;
		muscles[whichmuscle].maxlength=muscles[num_muscles-1].maxlength;
		muscles[whichmuscle].strength=muscles[num_muscles-1].strength;
		muscles[whichmuscle].parent1=muscles[num_muscles-1].parent1;
		muscles[whichmuscle].parent2=muscles[num_muscles-1].parent2;
		muscles[whichmuscle].length=muscles[num_muscles-1].length;
		muscles[whichmuscle].visible=muscles[num_muscles-1].visible;
		muscles[whichmuscle].type=muscles[num_muscles-1].type;
		muscles[whichmuscle].targetlength=muscles[num_muscles-1].targetlength;

		num_muscles--;
	}
}

void Skeleton::FindRotationJoint(int which)
{
	XYZ temppoint1,temppoint2,tempforward;
	float distance;

	temppoint1=joints[which].position;
	temppoint2=joints[which].parent->position;
	distance = len(temppoint1 - temppoint2);
	joints[which].rotate2=asin((temppoint1.y-temppoint2.y)/distance)*rad2deg;
	temppoint1.y=0;
	temppoint2.y=0;
	joints[which].rotate1 = rad2deg * acos((temppoint1.z - temppoint2.z)
		/ len(temppoint1 - temppoint2));
	if(temppoint1.x>temppoint2.x)joints[which].rotate1=360-joints[which].rotate1;
	if(joints[which].label==head)tempforward=specialforward[0];
	else if(joints[which].label==rightshoulder||joints[which].label==rightelbow||joints[which].label==rightwrist||joints[which].label==righthand)tempforward=specialforward[1];
	else if(joints[which].label==leftshoulder||joints[which].label==leftelbow||joints[which].label==leftwrist||joints[which].label==lefthand)tempforward=specialforward[2];
	else if(joints[which].label==righthip||joints[which].label==rightknee||joints[which].label==rightankle)tempforward=specialforward[3];
	else if(joints[which].label==lefthip||joints[which].label==leftknee||joints[which].label==leftankle)tempforward=specialforward[4];
	else if(!joints[which].lower)tempforward=forward;
	else if(joints[which].lower)tempforward=lowforward;
	tempforward=rotate(tempforward,0,joints[which].rotate1-90,0);
	tempforward=rotate(tempforward,0,0,joints[which].rotate2-90);
	tempforward.y=0;
	tempforward = normalize(tempforward);
	joints[which].rotate3=acos(0-tempforward.z)*rad2deg;
	if(0>tempforward.x)joints[which].rotate3=360-joints[which].rotate3;
}

void Skeleton::FindRotationMuscle(int which)
{
	XYZ temppoint1,temppoint2,tempforward;
	float distance;

	temppoint1=muscles[which].parent1->position;
	temppoint2=muscles[which].parent2->position;
	distance = len(temppoint1 - temppoint2);
	muscles[which].rotate2=asin((temppoint1.y-temppoint2.y)/distance)*rad2deg;
	temppoint1.y=0;
	temppoint2.y=0;
	muscles[which].rotate1 = rad2deg * acos((temppoint1.z - temppoint2.z)
		/ len(temppoint1 - temppoint2));
	if(temppoint1.x>temppoint2.x)muscles[which].rotate1=360-muscles[which].rotate1;
	if(muscles[which].parent1->label==head)tempforward=specialforward[0];
	else if(muscles[which].parent1->label==rightshoulder||muscles[which].parent1->label==rightelbow||muscles[which].parent1->label==rightwrist)tempforward=specialforward[1];
	else if(muscles[which].parent1->label==leftshoulder||muscles[which].parent1->label==leftelbow||muscles[which].parent1->label==leftwrist)tempforward=specialforward[2];
	else if(muscles[which].parent1->label==righthip||muscles[which].parent1->label==rightknee||muscles[which].parent1->label==rightankle)tempforward=specialforward[3];
	else if(muscles[which].parent1->label==lefthip||muscles[which].parent1->label==leftknee||muscles[which].parent1->label==leftankle)tempforward=specialforward[4];
	else if(!muscles[which].parent1->lower)tempforward=forward;
	else if(muscles[which].parent1->lower)tempforward=lowforward;
	tempforward=rotate(tempforward,0,muscles[which].rotate1-90,0);
	tempforward=rotate(tempforward,0,0,muscles[which].rotate2-90);
	tempforward.y=0;
	tempforward = normalize(tempforward);
	muscles[which].rotate3=acos(0-tempforward.z)*rad2deg;
	for (auto& joint : joints)
		if (&joint == muscles[which].parent1) {
			joint.rotate1=muscles[which].rotate1;
			joint.rotate2=muscles[which].rotate2;
			joint.rotate3=muscles[which].rotate3;
		}
	if(0>tempforward.x)muscles[which].rotate3=360-muscles[which].rotate3;
}

extern Skeleton testskeleton;

void Animation::load(const char* name)
{
	auto data = loadAnimation(name);
	numframes = data.len;
	for (size_t i = 0; i < numframes; ++i) {
		for (size_t j = 0; j < max_joints; ++j) {
			position[j][i].x = data.ptr[i].joints[j].x;
			position[j][i].y = data.ptr[i].joints[j].y;
			position[j][i].z = data.ptr[i].joints[j].z;
		}
		speed[i] = data.ptr[i].speed;
	}
	free(data.ptr);

	for(int j=0;j<numframes;j++){
		for (int i = 0; i < max_joints; ++i)
			testskeleton.joints[i].position = position[i][j];
		//Find forward vectors
		testskeleton.forward = normalize(crossProduct(testskeleton.joints[testskeleton.forwardjoints[1]].position
			- testskeleton.joints[testskeleton.forwardjoints[0]].position,
			testskeleton.joints[testskeleton.forwardjoints[2]].position
			- testskeleton.joints[testskeleton.forwardjoints[0]].position));

		testskeleton.lowforward = normalize(crossProduct(testskeleton.joints[testskeleton.lowforwardjoints[1]].position
			- testskeleton.joints[testskeleton.lowforwardjoints[0]].position,
			testskeleton.joints[testskeleton.lowforwardjoints[2]].position
			- testskeleton.joints[testskeleton.lowforwardjoints[0]].position));

		//Special forwards
		testskeleton.specialforward[0]=testskeleton.forward;

		testskeleton.specialforward[1]=testskeleton.joints[rightshoulder].position+testskeleton.joints[rightwrist].position;
		testskeleton.specialforward[1]=testskeleton.joints[rightelbow].position-testskeleton.specialforward[1]/2;
		testskeleton.specialforward[1]+=testskeleton.forward*.2;
		testskeleton.specialforward[1] = normalize(testskeleton.specialforward[1]);
		testskeleton.specialforward[2]=testskeleton.joints[leftshoulder].position+testskeleton.joints[leftwrist].position;
		testskeleton.specialforward[2]=testskeleton.joints[leftelbow].position-testskeleton.specialforward[2]/2;
		testskeleton.specialforward[2]+=testskeleton.forward*.2;
		testskeleton.specialforward[2] = normalize(testskeleton.specialforward[2]);

		testskeleton.specialforward[3]=testskeleton.joints[righthip].position+testskeleton.joints[rightankle].position;
		testskeleton.specialforward[3]=testskeleton.specialforward[3]/2-testskeleton.joints[rightknee].position;
		testskeleton.specialforward[3]+=testskeleton.lowforward*.2;
		testskeleton.specialforward[3] = normalize(testskeleton.specialforward[3]);
		testskeleton.specialforward[4]=testskeleton.joints[lefthip].position+testskeleton.joints[leftankle].position;
		testskeleton.specialforward[4]=testskeleton.specialforward[4]/2-testskeleton.joints[leftknee].position;
		testskeleton.specialforward[4]+=testskeleton.lowforward*.2;
		testskeleton.specialforward[4] = normalize(testskeleton.specialforward[4]);

		//Find joint rotations
		for (int i = 0; i < max_joints; ++i)
			if (testskeleton.joints[i].hasparent
			    && testskeleton.joints[i].visible) {
				testskeleton.FindRotationJoint(i);
				rotate1[i][j]=testskeleton.joints[i].rotate1;
				rotate2[i][j]=testskeleton.joints[i].rotate2;
				rotate3[i][j]=testskeleton.joints[i].rotate3;
				if(j!=0&&rotate3[i][j]>rotate3[i][j-1]+180)rotate3[i][j]-=360;
				if(j!=0&&rotate3[i][j]<rotate3[i][j-1]-180)rotate3[i][j]+=360;
				if(j!=0&&rotate2[i][j]>rotate2[i][j-1]+180)rotate2[i][j]-=360;
				if(j!=0&&rotate2[i][j]<rotate2[i][j-1]-180)rotate2[i][j]+=360;
				if(j!=0&&rotate1[i][j]>rotate1[i][j-1]+180)rotate1[i][j]-=360;
				if(j!=0&&rotate1[i][j]<rotate1[i][j-1]-180)rotate1[i][j]+=360;
			}

		for (int i = 0; i < max_muscles; ++i)
			if (testskeleton.muscles[i].visible) {
				testskeleton.FindRotationMuscle(i);
				mrotate1[i][j]=testskeleton.muscles[i].rotate1;
				mrotate2[i][j]=testskeleton.muscles[i].rotate2;
				mrotate3[i][j]=testskeleton.muscles[i].rotate3;
				if(j!=0&&mrotate3[i][j]>mrotate3[i][j-1]+180)mrotate3[i][j]-=360;
				if(j!=0&&mrotate3[i][j]<mrotate3[i][j-1]-180)mrotate3[i][j]+=360;
				if(j!=0&&mrotate2[i][j]>mrotate2[i][j-1]+180)mrotate2[i][j]-=360;
				if(j!=0&&mrotate2[i][j]<mrotate2[i][j-1]-180)mrotate2[i][j]+=360;
				if(j!=0&&mrotate1[i][j]>mrotate1[i][j-1]+180)mrotate1[i][j]-=360;
				if(j!=0&&mrotate1[i][j]<mrotate1[i][j-1]-180)mrotate1[i][j]+=360;
			}
	}

	for(int k=0;k<2;k++)
	for(int j=0;j<numframes;j++){
		for (int i = 0; i < max_muscles; ++i)
			if(testskeleton.muscles[i].visible) {
				if(j!=0&&mrotate3[i][j]>mrotate3[i][j-1]+180)mrotate3[i][j]-=360;
				if(j!=0&&mrotate3[i][j]<mrotate3[i][j-1]-180)mrotate3[i][j]+=360;
				if(j!=0&&mrotate2[i][j]>mrotate2[i][j-1]+180)mrotate2[i][j]-=360;
				if(j!=0&&mrotate2[i][j]<mrotate2[i][j-1]-180)mrotate2[i][j]+=360;
				if(j!=0&&mrotate1[i][j]>mrotate1[i][j-1]+180)mrotate1[i][j]-=360;
				if(j!=0&&mrotate1[i][j]<mrotate1[i][j-1]-180)mrotate1[i][j]+=360;

				if(j==0&&mrotate3[i][j]>mrotate3[i][numframes-1]+180)mrotate3[i][j]-=360;
				if(j==0&&mrotate3[i][j]<mrotate3[i][numframes-1]-180)mrotate3[i][j]+=360;
				if(j==0&&mrotate2[i][j]>mrotate2[i][numframes-1]+180)mrotate2[i][j]-=360;
				if(j==0&&mrotate2[i][j]<mrotate2[i][numframes-1]-180)mrotate2[i][j]+=360;
				if(j==0&&mrotate1[i][j]>mrotate1[i][numframes-1]+180)mrotate1[i][j]-=360;
				if(j==0&&mrotate1[i][j]<mrotate1[i][numframes-1]-180)mrotate1[i][j]+=360;
			}

		for (int i = 0; i < max_joints; ++i)
			if(testskeleton.joints[i].hasparent&&testskeleton.joints[i].visible) {
				if(j!=0&&rotate3[i][j]>rotate3[i][j-1]+180)rotate3[i][j]-=360;
				if(j!=0&&rotate3[i][j]<rotate3[i][j-1]-180)rotate3[i][j]+=360;
				if(j!=0&&rotate2[i][j]>rotate2[i][j-1]+180)rotate2[i][j]-=360;
				if(j!=0&&rotate2[i][j]<rotate2[i][j-1]-180)rotate2[i][j]+=360;
				if(j!=0&&rotate1[i][j]>rotate1[i][j-1]+180)rotate1[i][j]-=360;
				if(j!=0&&rotate1[i][j]<rotate1[i][j-1]-180)rotate1[i][j]+=360;

				if(j==0&&rotate3[i][j]>rotate3[i][numframes-1]+180)rotate3[i][j]-=360;
				if(j==0&&rotate3[i][j]<rotate3[i][numframes-1]-180)rotate3[i][j]+=360;
				if(j==0&&rotate2[i][j]>rotate2[i][numframes-1]+180)rotate2[i][j]-=360;
				if(j==0&&rotate2[i][j]<rotate2[i][numframes-1]-180)rotate2[i][j]+=360;
				if(j==0&&rotate1[i][j]>rotate1[i][numframes-1]+180)rotate1[i][j]-=360;
				if(j==0&&rotate1[i][j]<rotate1[i][numframes-1]-180)rotate1[i][j]+=360;
			}
	}
}

void Skeleton::reload()
{
	broken = 0;
	JointData joints_data[max_joints];
	loadJoints(joints_data);
	for (int i = 0; i < max_joints; ++i) {
		joints[i].label = i;
		joints[i].position.x = joints_data[i].x;
		joints[i].position.y = joints_data[i].y;
		joints[i].position.z = joints_data[i].z;
		joints[i].length = joints_data[i].length;
		joints[i].modelnum = joints_data[i].model;
		joints[i].visible = joints_data[i].visible;
		joints[i].lower = joints_data[i].lower;

		if (joints_data[i].parent < 0) {
			joints[i].hasparent = false;
			joints[i].parent = NULL;
		} else {
			joints[i].hasparent = true;
			joints[i].parent = joints + joints_data[i].parent;
		}

		joints[i].existing = true;
		joints[i].locked = false;
		joints[i].velocity = {};
		joints[i].oldposition = joints[i].position;
	}

	forwardjoints[0] = leftshoulder;
	forwardjoints[1] = abdomen;
	forwardjoints[2] = rightshoulder;
	lowforwardjoints[0] = righthip;
	lowforwardjoints[1] = abdomen;
	lowforwardjoints[2] = lefthip;

	num_muscles = 29;
	MuscleData muscles_data[29];
	loadMuscles(muscles_data);
	for (int i = 0; i < num_muscles; ++i) {
		muscles[i].length = muscles_data[i].length;
		muscles[i].targetlength = muscles_data[i].initlen;
		muscles[i].minlength = muscles_data[i].minlen;
		muscles[i].maxlength = muscles_data[i].maxlen;
		muscles[i].type = muscles_data[i].flag;
		muscles[i].visible = muscles_data[i].visible;
		muscles[i].parent1 = joints + muscles_data[i].parent1;
		muscles[i].parent2 = joints + muscles_data[i].parent2;
		muscles[i].strength = !muscles[i].type;
	}
}