//ODEHuman.cpp

#include "ODEHuman.h"
#include <math.h>

ODEHuman::ODEHuman(ODEWorld* world, dReal posx, dReal posy, dReal posz, dReal size)
: ODEObject(true, false, "", "", posx, posy, posz)
{
	brain = NULL;

	//create a new space and insert it into the world collision space
	//thisSpaceID = dSimpleSpaceCreate(collisionWorldID);
	
	point3d jointPoint;

	//All dimensions are x,y,z
	dReal torsoDimensions[3] = {1*size, 2*size, 0.5*size};
	dReal upperArmDimensions[3] = {0.5*size, 1.5*size, 0.4*size};
	dReal lowerArmDimensions[3] = {0.5*size, 1.5*size, 0.4*size};
	dReal upperLegDimensions[3] = {0.6*size, 1.7*size, 0.5*size};
	dReal lowerLegDimensions[3] = {0.5*size, 1.7*size, 0.5*size};
	dReal footDimensions[3] = {0.5*size, 0.3*size, 1.0*size};
	dReal headRadius = 0.5*size;
	//maxHeadHeightAboveFeet = footDimensions[1]/2 + lowerLegDimensions[1] + upperLegDimensions[1] + torsoDimensions[1] + headRadius;
	//maxHeadDistanceAheadOfTorso = torsoDimensions[1]/2 + headRadius;

	torso = new ODEBox(world, true, false, "", "", 0, 0, 0, torsoDimensions[0], torsoDimensions[1], torsoDimensions[2]);
	head = new ODESphere(world, true, false, "", "", 0, 0, 0, headRadius);
	leftUpperArm = new ODEBox(world, true, false, "", "", 0, 0, 0, upperArmDimensions[0], upperArmDimensions[1], upperArmDimensions[2]);
	rightUpperArm = new ODEBox(world, true, false, "", "", 0, 0, 0, upperArmDimensions[0], upperArmDimensions[1], upperArmDimensions[2]);
	leftLowerArm = new ODEBox(world, true, false, "", "", 0, 0, 0, lowerArmDimensions[0], lowerArmDimensions[1], lowerArmDimensions[2]);
	rightLowerArm = new ODEBox(world, true, false, "", "", 0, 0, 0, lowerArmDimensions[0], lowerArmDimensions[1], lowerArmDimensions[2]);
	leftUpperLeg = new ODEBox(world, true, false, "", "", 0, 0, 0, upperLegDimensions[0], upperLegDimensions[1], upperLegDimensions[2]);
	rightUpperLeg = new ODEBox(world, true, false, "", "", 0, 0, 0, upperLegDimensions[0], upperLegDimensions[1], upperLegDimensions[2]);
	leftLowerLeg = new ODEBox(world, true, false, "", "", 0, 0, 0, lowerLegDimensions[0], lowerLegDimensions[1], lowerLegDimensions[2]);
	rightLowerLeg = new ODEBox(world, true, false, "", "", 0, 0, 0, lowerLegDimensions[0], lowerLegDimensions[1], lowerLegDimensions[2]);
	leftFoot = new ODEBox(world, true, false, "", "", 0, 0, 0, footDimensions[0], footDimensions[1], footDimensions[2]);
	rightFoot = new ODEBox(world, true, false, "", "", 0, 0, 0, footDimensions[0], footDimensions[1], footDimensions[2]);
	thisBodyID = torso->GetBodyID();
	const dReal* initialQ = dBodyGetQuaternion(torso->GetBodyID());
	initialQuaternion[0] = initialQ[0];
	initialQuaternion[1] = initialQ[1];
	initialQuaternion[2] = initialQ[2];
	initialQuaternion[3] = initialQ[3];

	//Now setting the positions of all body parts.
	//startPoint is starting point for the center of the torso (in world coordinates).
	//point3d startPoint = {rand()%10, rand()%10+10, rand()%10};
	dReal initialTorsoHeightAboveFeet = footDimensions[1] + lowerLegDimensions[1] + upperLegDimensions[1] + torsoDimensions[1]/2;
	//point3d startPoint = {0, initialTorsoHeightAboveFeet, 0};
	point3d startPoint = {posx, posy, posz};
	SetInitialPosition(startPoint);

	//Now create joints between objects

	//UPDATE SOME HINGE JOINTS WITH BALL-AND-SOCKET/AMOTORS

	//Create neck joint
	//neck = dJointCreateUniversal(dynamicsWorldID, 0);
	//dJointAttach(neck, torso->GetBodyID(), head->GetBodyID());
	//jointPoint = torso->GetPosition();
	//jointPoint.y += torso->GetYDimension()/2;
	//dJointSetUniversalAnchor(neck, jointPoint.x, jointPoint.y, jointPoint.z);
	//dJointSetUniversalAxis1(neck, 1, 0, 0);
	//dJointSetUniversalAxis2(neck, 0, 0, 1);
	//dJointSetUniversalParam(neck, dParamLoStop, -PI/3);
	//dJointSetUniversalParam(neck, dParamHiStop, PI/3);
	//dJointSetUniversalParam(neck, dParamLoStop2, -PI/3);
	//dJointSetUniversalParam(neck, dParamHiStop2, PI/3);
	neck = dJointCreateHinge(world->GetWorldID(), 0);
	dJointAttach(neck, torso->GetBodyID(), head->GetBodyID());
	jointPoint = torso->GetPosition();
	jointPoint.y += torso->GetYDimension()/2;
	dJointSetHingeAnchor(neck, jointPoint.x, jointPoint.y, jointPoint.z);
	dJointSetHingeAxis(neck, 1, 0, 0);
	dJointSetHingeParam(neck, dParamLoStop, -PI/3);
	dJointSetHingeParam(neck, dParamHiStop, PI/3);

	//Create left shoulder joint
	//leftShoulder = dJointCreateUniversal(dynamicsWorldID, 0);
	//dJointAttach(leftShoulder, torso->GetBodyID(), leftUpperArm->GetBodyID());
	//jointPoint = torso->GetPosition();
	//jointPoint.y += torso->GetYDimension()/2;
	//jointPoint.x -= torso->GetXDimension()/2;
	//dJointSetUniversalAnchor(leftShoulder, jointPoint.x, jointPoint.y, jointPoint.z);
	//dJointSetUniversalAxis1(leftShoulder, 1, 0, 0);
	//dJointSetUniversalAxis2(leftShoulder, 0, 0, 1);
	//dJointSetUniversalParam(leftShoulder, dParamLoStop, -PI+0.001); //must be > -PI
	//dJointSetUniversalParam(leftShoulder, dParamHiStop, PI-0.001); //must be < PI
	//dJointSetUniversalParam(leftShoulder, dParamLoStop2, 0);
	//dJointSetUniversalParam(leftShoulder, dParamHiStop2, 3*PI/4);
	leftShoulder = dJointCreateHinge(world->GetWorldID(), 0);
	dJointAttach(leftShoulder, torso->GetBodyID(), leftUpperArm->GetBodyID());
	jointPoint = torso->GetPosition();
	jointPoint.y += torso->GetYDimension()/2;
	jointPoint.x -= torso->GetXDimension()/2;
	dJointSetHingeAnchor(leftShoulder, jointPoint.x, jointPoint.y, jointPoint.z);
	dJointSetHingeAxis(leftShoulder, 1, 0, 0);

	//Create right shoulder joint
	//rightShoulder = dJointCreateUniversal(dynamicsWorldID, 0);
	//dJointAttach(rightShoulder, torso->GetBodyID(), rightUpperArm->GetBodyID());
	//jointPoint = torso->GetPosition();
	//jointPoint.y += torso->GetYDimension()/2;
	//jointPoint.x += torso->GetXDimension()/2;
	//dJointSetUniversalAnchor(rightShoulder, jointPoint.x, jointPoint.y, jointPoint.z);
	//dJointSetUniversalAxis1(rightShoulder, 1, 0, 0);
	//dJointSetUniversalAxis2(rightShoulder, 0, 0, 1);
	//dJointSetUniversalParam(rightShoulder, dParamLoStop, -PI+0.001); //must be > -PI
	//dJointSetUniversalParam(rightShoulder, dParamHiStop, PI-0.001); //must be < PI
	//dJointSetUniversalParam(rightShoulder, dParamLoStop2, -3*PI/4);
	//dJointSetUniversalParam(rightShoulder, dParamHiStop2, 0);
	rightShoulder = dJointCreateHinge(world->GetWorldID(), 0);
	dJointAttach(rightShoulder, torso->GetBodyID(), rightUpperArm->GetBodyID());
	jointPoint = torso->GetPosition();
	jointPoint.y += torso->GetYDimension()/2;
	jointPoint.x += torso->GetXDimension()/2;
	dJointSetHingeAnchor(rightShoulder, jointPoint.x, jointPoint.y, jointPoint.z);
	dJointSetHingeAxis(rightShoulder, 1, 0, 0);

	//Create left elbow joint
	leftElbow = dJointCreateHinge(world->GetWorldID(), 0);
	dJointAttach(leftElbow, leftUpperArm->GetBodyID(), leftLowerArm->GetBodyID());
	jointPoint = leftUpperArm->GetPosition();
	jointPoint.y -= leftUpperArm->GetYDimension()/2;
	//jointPoint.z -= leftUpperArm->GetZDimension()/2;
	dJointSetHingeAnchor(leftElbow, jointPoint.x, jointPoint.y, jointPoint.z);
	dJointSetHingeAxis(leftElbow, 1, 0, 0);
	dJointSetHingeParam(leftElbow, dParamLoStop, 0);
	dJointSetHingeParam(leftElbow, dParamHiStop, 4*PI/5);

	//Create right elbow joint
	rightElbow = dJointCreateHinge(world->GetWorldID(), 0);
	dJointAttach(rightElbow, rightUpperArm->GetBodyID(), rightLowerArm->GetBodyID());
	jointPoint = rightUpperArm->GetPosition();
	jointPoint.y -= rightUpperArm->GetYDimension()/2;
	//jointPoint.z -= rightUpperArm->GetZDimension()/2;
	dJointSetHingeAnchor(rightElbow, jointPoint.x, jointPoint.y, jointPoint.z);
	dJointSetHingeAxis(rightElbow, 1, 0, 0);
	dJointSetHingeParam(rightElbow, dParamLoStop, 0);
	dJointSetHingeParam(rightElbow, dParamHiStop, 4*PI/5);

	//Create left hip joint
	//leftHip = dJointCreateUniversal(dynamicsWorldID, 0);
	//dJointAttach(leftHip, torso->GetBodyID(), leftUpperLeg->GetBodyID());
	//jointPoint = leftUpperLeg->GetPosition();
	//jointPoint.y += leftUpperLeg->GetYDimension()/2;
	//dJointSetUniversalAnchor(leftHip, jointPoint.x, jointPoint.y, jointPoint.z);
	//dJointSetUniversalAxis1(leftHip, 1, 0, 0);
	//dJointSetUniversalAxis2(leftHip, 0, 0, 1);
	//dJointSetUniversalParam(leftHip, dParamLoStop, -PI/6);
	//dJointSetUniversalParam(leftHip, dParamHiStop, 3*PI/4);
	//dJointSetUniversalParam(leftHip, dParamLoStop2, 0);
	//dJointSetUniversalParam(leftHip, dParamHiStop2, PI/4);
	leftHip = dJointCreateHinge(world->GetWorldID(), 0);
	dJointAttach(leftHip, torso->GetBodyID(), leftUpperLeg->GetBodyID());
	jointPoint = leftUpperLeg->GetPosition();
	jointPoint.y += leftUpperLeg->GetYDimension()/2;
	dJointSetHingeAnchor(leftHip, jointPoint.x, jointPoint.y, jointPoint.z);
	dJointSetHingeAxis(leftHip, 1, 0, 0);
	dJointSetHingeParam(leftHip, dParamLoStop, -PI/6);
	dJointSetHingeParam(leftHip, dParamHiStop, 3*PI/4);

	//Create right hip joint
	//rightHip = dJointCreateUniversal(dynamicsWorldID, 0);
	//dJointAttach(rightHip, torso->GetBodyID(), rightUpperLeg->GetBodyID());
	//jointPoint = rightUpperLeg->GetPosition();
	//jointPoint.y += rightUpperLeg->GetYDimension()/2;
	//dJointSetUniversalAnchor(rightHip, jointPoint.x, jointPoint.y, jointPoint.z);
	//dJointSetUniversalAxis1(rightHip, 1, 0, 0);
	//dJointSetUniversalAxis2(rightHip, 0, 0, 1);
	//dJointSetUniversalParam(rightHip, dParamLoStop, -PI/6);
	//dJointSetUniversalParam(rightHip, dParamHiStop, 3*PI/4);
	//dJointSetUniversalParam(rightHip, dParamLoStop2, -PI/4);
	//dJointSetUniversalParam(rightHip, dParamHiStop2, 0);
	rightHip = dJointCreateHinge(world->GetWorldID(), 0);
	dJointAttach(rightHip, torso->GetBodyID(), rightUpperLeg->GetBodyID());
	jointPoint = rightUpperLeg->GetPosition();
	jointPoint.y += rightUpperLeg->GetYDimension()/2;
	dJointSetHingeAnchor(rightHip, jointPoint.x, jointPoint.y, jointPoint.z);
	dJointSetHingeAxis(rightHip, 1, 0, 0);
	dJointSetHingeParam(rightHip, dParamLoStop, -PI/6);
	dJointSetHingeParam(rightHip, dParamHiStop, 3*PI/4);

	//Create left knee joint
	leftKnee = dJointCreateHinge(world->GetWorldID(), 0);
	dJointAttach(leftKnee, leftUpperLeg->GetBodyID(), leftLowerLeg->GetBodyID());
	jointPoint = leftUpperLeg->GetPosition();
	jointPoint.y -= leftUpperLeg->GetYDimension()/2;
	jointPoint.z += leftUpperLeg->GetZDimension()/2;
	dJointSetHingeAnchor(leftKnee, jointPoint.x, jointPoint.y, jointPoint.z);
	dJointSetHingeAxis(leftKnee, 1, 0, 0);
	dJointSetHingeParam(leftKnee, dParamLoStop, -PI/2);
	dJointSetHingeParam(leftKnee, dParamHiStop, 0);

	//Create right knee joint
	rightKnee = dJointCreateHinge(world->GetWorldID(), 0);
	dJointAttach(rightKnee, rightUpperLeg->GetBodyID(), rightLowerLeg->GetBodyID());
	jointPoint = rightUpperLeg->GetPosition();
	jointPoint.y -= rightUpperLeg->GetYDimension()/2;
	jointPoint.z += rightUpperLeg->GetZDimension()/2;
	dJointSetHingeAnchor(rightKnee, jointPoint.x, jointPoint.y, jointPoint.z);
	dJointSetHingeAxis(rightKnee, 1, 0, 0);
	dJointSetHingeParam(rightKnee, dParamLoStop, -PI/2);
	dJointSetHingeParam(rightKnee, dParamHiStop, 0);

	//Create left ankle joint
	leftAnkle = dJointCreateHinge(world->GetWorldID(), 0);
	dJointAttach(leftAnkle, leftLowerLeg->GetBodyID(), leftFoot->GetBodyID());
	jointPoint = leftLowerLeg->GetPosition();
	jointPoint.y -= leftLowerLeg->GetYDimension()/2;
	//jointPoint.z += leftLowerLeg->GetZDimension()/2;
	dJointSetHingeAnchor(leftAnkle, jointPoint.x, jointPoint.y, jointPoint.z);
	dJointSetHingeAxis(leftAnkle, 1, 0, 0);
	dJointSetHingeParam(leftAnkle, dParamLoStop, -PI/2);
	dJointSetHingeParam(leftAnkle, dParamHiStop, PI/6);

	//Create right ankle joint
	rightAnkle = dJointCreateHinge(world->GetWorldID(), 0);
	dJointAttach(rightAnkle, rightLowerLeg->GetBodyID(), rightFoot->GetBodyID());
	jointPoint = rightLowerLeg->GetPosition();
	jointPoint.y -= rightLowerLeg->GetYDimension()/2;
	//jointPoint.z += rightLowerLeg->GetZDimension()/2;
	dJointSetHingeAnchor(rightAnkle, jointPoint.x, jointPoint.y, jointPoint.z);
	dJointSetHingeAxis(rightAnkle, 1, 0, 0);
	dJointSetHingeParam(rightAnkle, dParamLoStop, -PI/2);
	dJointSetHingeParam(rightAnkle, dParamHiStop, PI/6);
}

ODEHuman::~ODEHuman()
{
	dJointDestroy(leftElbow);
	dJointDestroy(rightElbow);
	dJointDestroy(leftShoulder);
	dJointDestroy(rightShoulder);
	dJointDestroy(neck);
	dJointDestroy(leftHip);
	dJointDestroy(rightHip);
	dJointDestroy(leftKnee);
	dJointDestroy(rightKnee);
	dJointDestroy(leftAnkle);
	dJointDestroy(rightAnkle);

	//don't delete brain; it will get deleted externally

	delete head;
	delete torso;
	delete leftUpperArm;
	delete rightUpperArm;
	delete leftLowerArm;
	delete rightLowerArm;
	delete leftUpperLeg;
	delete rightUpperLeg;
	delete leftLowerLeg;
	delete rightLowerLeg;
	delete leftFoot;
	delete rightFoot;
}

point3d ODEHuman::GetPosition()
{
	const dReal* bodyPosition;
	bodyPosition = dBodyGetPosition(torso->GetBodyID());
	position.x = bodyPosition[0];
	position.y = bodyPosition[1];
	position.z = bodyPosition[2];
	return position;
}

void ODEHuman::SetInitialPosition(point3d newposition)
{
	position = newposition;

	//position torso
	dBodySetPosition(torso->GetBodyID(), position.x, position.y, position.z);
	dBodySetForce(torso->GetBodyID(), 0, 0, 0);
	dBodySetLinearVel(torso->GetBodyID(), 0, 0, 0);
	dBodySetAngularVel(torso->GetBodyID(), 0, 0, 0);
	dBodySetQuaternion(torso->GetBodyID(), initialQuaternion);

	//position head
	dReal headOffset = torso->GetYDimension()/2 + head->GetRadius();
	dBodySetPosition(head->GetBodyID(), position.x, position.y + headOffset, position.z);
	dBodySetForce(head->GetBodyID(), 0, 0, 0);
	dBodySetLinearVel(head->GetBodyID(), 0, 0, 0);
	dBodySetAngularVel(head->GetBodyID(), 0, 0, 0);
	dBodySetQuaternion(head->GetBodyID(), initialQuaternion);

	//position upper arms
	dReal upperArmOffsetX = torso->GetXDimension()/2 + leftUpperArm->GetXDimension()/2;
	dReal upperArmOffsetY = torso->GetYDimension()/2 - leftUpperArm->GetYDimension()/2;
	dBodySetPosition(leftUpperArm->GetBodyID(), position.x - upperArmOffsetX, position.y + upperArmOffsetY, position.z);
	dBodySetPosition(rightUpperArm->GetBodyID(), position.x + upperArmOffsetX, position.y + upperArmOffsetY, position.z);
	dBodySetForce(leftUpperArm->GetBodyID(), 0, 0, 0);
	dBodySetLinearVel(leftUpperArm->GetBodyID(), 0, 0, 0);
	dBodySetAngularVel(leftUpperArm->GetBodyID(), 0, 0, 0);
	dBodySetQuaternion(leftUpperArm->GetBodyID(), initialQuaternion);
	dBodySetForce(rightUpperArm->GetBodyID(), 0, 0, 0);
	dBodySetLinearVel(rightUpperArm->GetBodyID(), 0, 0, 0);
	dBodySetAngularVel(rightUpperArm->GetBodyID(), 0, 0, 0);
	dBodySetQuaternion(rightUpperArm->GetBodyID(), initialQuaternion);

	//position lower arms
	dReal lowerArmOffsetX = torso->GetXDimension()/2 + leftLowerArm->GetXDimension()/2;
	dReal lowerArmOffsetY = torso->GetYDimension()/2 - leftUpperArm->GetYDimension() - leftLowerArm->GetYDimension()/2;
	dBodySetPosition(leftLowerArm->GetBodyID(), position.x - lowerArmOffsetX, position.y + lowerArmOffsetY, position.z);
	dBodySetPosition(rightLowerArm->GetBodyID(), position.x + lowerArmOffsetX, position.y + lowerArmOffsetY, position.z);
	dBodySetForce(leftLowerArm->GetBodyID(), 0, 0, 0);
	dBodySetLinearVel(leftLowerArm->GetBodyID(), 0, 0, 0);
	dBodySetAngularVel(leftLowerArm->GetBodyID(), 0, 0, 0);
	dBodySetQuaternion(leftLowerArm->GetBodyID(), initialQuaternion);
	dBodySetForce(rightLowerArm->GetBodyID(), 0, 0, 0);
	dBodySetLinearVel(rightLowerArm->GetBodyID(), 0, 0, 0);
	dBodySetAngularVel(rightLowerArm->GetBodyID(), 0, 0, 0);
	dBodySetQuaternion(rightLowerArm->GetBodyID(), initialQuaternion);

	//position upper legs
	dReal upperLegOffsetX = torso->GetXDimension()/2 - leftUpperLeg->GetXDimension()/2;
	dReal upperLegOffsetY = -torso->GetYDimension()/2 - leftUpperLeg->GetYDimension()/2;
	dBodySetPosition(leftUpperLeg->GetBodyID(), position.x - upperLegOffsetX, position.y + upperLegOffsetY, position.z);
	dBodySetPosition(rightUpperLeg->GetBodyID(), position.x + upperLegOffsetX, position.y + upperLegOffsetY, position.z);
	dBodySetForce(leftUpperLeg->GetBodyID(), 0, 0, 0);
	dBodySetLinearVel(leftUpperLeg->GetBodyID(), 0, 0, 0);
	dBodySetAngularVel(leftUpperLeg->GetBodyID(), 0, 0, 0);
	dBodySetQuaternion(leftUpperLeg->GetBodyID(), initialQuaternion);
	dBodySetForce(rightUpperLeg->GetBodyID(), 0, 0, 0);
	dBodySetLinearVel(rightUpperLeg->GetBodyID(), 0, 0, 0);
	dBodySetAngularVel(rightUpperLeg->GetBodyID(), 0, 0, 0);
	dBodySetQuaternion(rightUpperLeg->GetBodyID(), initialQuaternion);

	//position lower legs
	dReal lowerLegOffsetX = torso->GetXDimension()/2 - leftLowerLeg->GetXDimension()/2;
	dReal lowerLegOffsetY = -torso->GetYDimension()/2 - leftUpperLeg->GetYDimension() - leftLowerLeg->GetYDimension()/2;
	dBodySetPosition(leftLowerLeg->GetBodyID(), position.x - lowerLegOffsetX, position.y + lowerLegOffsetY, position.z);
	dBodySetPosition(rightLowerLeg->GetBodyID(), position.x + lowerLegOffsetX, position.y + lowerLegOffsetY, position.z);
	dBodySetForce(leftLowerLeg->GetBodyID(), 0, 0, 0);
	dBodySetLinearVel(leftLowerLeg->GetBodyID(), 0, 0, 0);
	dBodySetAngularVel(leftLowerLeg->GetBodyID(), 0, 0, 0);
	dBodySetQuaternion(leftLowerLeg->GetBodyID(), initialQuaternion);
	dBodySetForce(rightLowerLeg->GetBodyID(), 0, 0, 0);
	dBodySetLinearVel(rightLowerLeg->GetBodyID(), 0, 0, 0);
	dBodySetAngularVel(rightLowerLeg->GetBodyID(), 0, 0, 0);
	dBodySetQuaternion(rightLowerLeg->GetBodyID(), initialQuaternion);

	//position feet
	dReal feetOffsetX = torso->GetXDimension()/2 - leftFoot->GetXDimension()/2;
	dReal feetOffsetY = -torso->GetYDimension()/2 - leftUpperLeg->GetYDimension() - leftLowerLeg->GetYDimension() - leftFoot->GetYDimension()/2;
	dReal feetOffsetZ = leftLowerLeg->GetZDimension()/2;
	dBodySetPosition(leftFoot->GetBodyID(), position.x - feetOffsetX, position.y + feetOffsetY, position.z + feetOffsetZ);
	dBodySetPosition(rightFoot->GetBodyID(), position.x + feetOffsetX, position.y + feetOffsetY, position.z + feetOffsetZ);
	dBodySetForce(leftFoot->GetBodyID(), 0, 0, 0);
	dBodySetLinearVel(leftFoot->GetBodyID(), 0, 0, 0);
	dBodySetAngularVel(leftFoot->GetBodyID(), 0, 0, 0);
	dBodySetQuaternion(leftFoot->GetBodyID(), initialQuaternion);
	dBodySetForce(rightFoot->GetBodyID(), 0, 0, 0);
	dBodySetLinearVel(rightFoot->GetBodyID(), 0, 0, 0);
	dBodySetAngularVel(rightFoot->GetBodyID(), 0, 0, 0);
	dBodySetQuaternion(rightFoot->GetBodyID(), initialQuaternion);
}

void ODEHuman::SetTotalVelocity(vector3d velocity)
{
	head->SetVelocity(velocity);
	leftUpperArm->SetVelocity(velocity);
	rightUpperArm->SetVelocity(velocity);
	leftLowerArm->SetVelocity(velocity);
	rightLowerArm->SetVelocity(velocity);
	leftUpperLeg->SetVelocity(velocity);
	rightUpperLeg->SetVelocity(velocity);
	leftLowerLeg->SetVelocity(velocity);
	rightLowerLeg->SetVelocity(velocity);
	leftFoot->SetVelocity(velocity);
	rightFoot->SetVelocity(velocity);
	torso->SetVelocity(velocity);
}

void ODEHuman::AddForce(dReal x, dReal y, dReal z)
{
	dBodyAddForce(head->GetBodyID(), x, y, z);
	dBodyAddForce(leftUpperArm->GetBodyID(), x, y, z);
	dBodyAddForce(rightUpperArm->GetBodyID(), x, y, z);
	dBodyAddForce(leftLowerArm->GetBodyID(), x, y, z);
	dBodyAddForce(rightLowerArm->GetBodyID(), x, y, z);
	dBodyAddForce(leftUpperLeg->GetBodyID(), x, y, z);
	dBodyAddForce(rightUpperLeg->GetBodyID(), x, y, z);
	dBodyAddForce(leftLowerLeg->GetBodyID(), x, y, z);
	dBodyAddForce(rightLowerLeg->GetBodyID(), x, y, z);
	dBodyAddForce(leftFoot->GetBodyID(), x, y, z);
	dBodyAddForce(rightFoot->GetBodyID(), x, y, z);
	dBodyAddForce(torso->GetBodyID(), x, y, z);
}

void ODEHuman::DampenVelocity(dReal dampingAmount)
{
	head->DampenVelocity(dampingAmount);
	leftUpperArm->DampenVelocity(dampingAmount);
	rightUpperArm->DampenVelocity(dampingAmount);
	leftLowerArm->DampenVelocity(dampingAmount);
	rightLowerArm->DampenVelocity(dampingAmount);
	leftUpperLeg->DampenVelocity(dampingAmount);
	rightUpperLeg->DampenVelocity(dampingAmount);
	leftLowerLeg->DampenVelocity(dampingAmount);
	rightLowerLeg->DampenVelocity(dampingAmount);
	leftFoot->DampenVelocity(dampingAmount);
	rightFoot->DampenVelocity(dampingAmount);
	torso->DampenVelocity(dampingAmount);
}

void ODEHuman::SetFloats(bool doesFloat)
{
	floats = doesFloat;

	head->SetFloats(doesFloat);
	leftUpperArm->SetFloats(doesFloat);
	rightUpperArm->SetFloats(doesFloat);
	leftLowerArm->SetFloats(doesFloat);
	rightLowerArm->SetFloats(doesFloat);
	leftUpperLeg->SetFloats(doesFloat);
	rightUpperLeg->SetFloats(doesFloat);
	leftLowerLeg->SetFloats(doesFloat);
	rightLowerLeg->SetFloats(doesFloat);
	leftFoot->SetFloats(doesFloat);
	rightFoot->SetFloats(doesFloat);
	torso->SetFloats(doesFloat);
}

void ODEHuman::Draw()
{
	//float matAmbientAndDiffuseRed[4] = {0.5, 0, 0, 1};
	//float matAmbientAndDiffuseGreen[4] = {0, 0.3, 0, 1};
	//float matAmbientAndDiffuseGray[4] = {.5, .5, .5, 1};
	//float matAmbientAndDiffuseYellow[4] = {.5, .5, 0, 1};
	//float matAmbientAndDiffuseWhite[4] = {.7, .7, .7, 1};
	//glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, matAmbientAndDiffuseRed);

	head->Draw();
	leftUpperArm->Draw();
	rightUpperArm->Draw();
	leftLowerArm->Draw();
	rightLowerArm->Draw();
	leftUpperLeg->Draw();
	rightUpperLeg->Draw();
	leftLowerLeg->Draw();
	rightLowerLeg->Draw();
	leftFoot->Draw();
	rightFoot->Draw();
	torso->Draw();
}

void ODEHuman::UseBrain()
{
	if (brain == NULL)
		return;

	float inputs[NUM_BRAIN_INPUTS];
	float outputs[NUM_BRAIN_OUTPUTS];

	//compiling inputs from body
	const dReal* headVelocity = dBodyGetLinearVel(head->GetBodyID());
	inputs[0] = headVelocity[0];
	inputs[1] = headVelocity[1];
	inputs[2] = headVelocity[2];
	inputs[3] = HeadHeightAboveFeet(); //estimate of height
	inputs[4] = dJointGetHingeAngle(leftShoulder);
	inputs[5] = dJointGetHingeAngle(rightShoulder);
	inputs[6] = dJointGetHingeAngle(leftElbow);
	inputs[7] = dJointGetHingeAngle(rightElbow);
	inputs[8] = dJointGetHingeAngle(leftHip);
	inputs[9] = dJointGetHingeAngle(rightHip);
	inputs[10] = dJointGetHingeAngle(leftKnee);
	inputs[11] = dJointGetHingeAngle(rightKnee);
	inputs[12] = dJointGetHingeAngle(leftAnkle);
	inputs[13] = dJointGetHingeAngle(rightAnkle);
	inputs[14] = dJointGetHingeAngle(neck);

	//pass inputs to brain and get outputs
	brain->Compute(inputs, outputs);

	dReal scalar=700;

	//controlling body with outputs
	dJointAddHingeTorque(leftShoulder, outputs[0]*scalar);
	dJointAddHingeTorque(rightShoulder, outputs[1]*scalar);
	dJointAddHingeTorque(leftElbow, outputs[2]*scalar);
	dJointAddHingeTorque(rightElbow, outputs[3]*scalar);
	dJointAddHingeTorque(leftHip, outputs[4]*scalar*5);
	dJointAddHingeTorque(rightHip, outputs[5]*scalar*5);
	dJointAddHingeTorque(leftKnee, outputs[6]*scalar*10);
	dJointAddHingeTorque(rightKnee, outputs[7]*scalar*10);
	dJointAddHingeTorque(leftAnkle, outputs[8]*scalar*2);
	dJointAddHingeTorque(rightAnkle, outputs[9]*scalar*2);
	dJointAddHingeTorque(neck, outputs[10]*scalar);
}

void ODEHuman::SetBrain(NeuralNet* newBrain)
{
	brain = newBrain;
}

//The following functions are to help the evolution system.

point3d ODEHuman::GetHeadPosition()
{
	return (head->GetPosition());
}

dReal ODEHuman::HeadHeightAboveFeet()
{
	dReal headHeightAboveFeet = head->GetPosition().y - leftFoot->GetPosition().y;
	return headHeightAboveFeet;
}

dReal ODEHuman::ForwardDistanceOfFeet()
{
	dReal distance = leftFoot->GetPosition().z + rightFoot->GetPosition().z;
	return distance;
}

dReal ODEHuman::ZDistanceBetweenFeet()
{
	dReal distance = abs(leftFoot->GetPosition().z - rightFoot->GetPosition().z);
	return distance;
}

dReal ODEHuman::CombinedFeetHeight()
{
	dReal height = leftFoot->GetPosition().y + rightFoot->GetPosition().y;
	return height;
}

dReal ODEHuman::LeftFootHeight()
{
	dReal height = leftFoot->GetPosition().y;
	return height;
}

dReal ODEHuman::RightFootHeight()
{
	dReal height = rightFoot->GetPosition().y;
	return height;
}

bool ODEHuman::LeftFootMovingDownward()
{
	if (dBodyGetLinearVel(leftFoot->GetBodyID())[1] < 0)
	{
		return true;
	}
	else
	{
		return false;
	}
}

bool ODEHuman::RightFootMovingDownward()
{
	if (dBodyGetLinearVel(rightFoot->GetBodyID())[1] < 0)
	{
		return true;
	}
	else
	{
		return false;
	}
}