//ODETrebuchet.cpp

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

ODETrebuchet::ODETrebuchet(ODEWorld* world, dReal posx, dReal posy, dReal posz)
: ODEObject(false, false, "", "", posx, posy, posz)
{
	point3d jointPoint;

	//frame = new ODEBox(world, true, true, TREBUCHET_MODELS_DIR, TREBUCHET_FRAME_FILENAME, posx, posy, posz, 0, 0, 0);
	frame = new Base3DObject(false, true, TREBUCHET_MODELS_DIR, TREBUCHET_FRAME_FILENAME, posx, posy, posz);
	shaft = new ODEBox(world, true, true, TREBUCHET_MODELS_DIR, TREBUCHET_SHAFT_FILENAME, posx-6, posy+6.2, posz, 0, 0, 0);
	ballast = new ODEBox(world, true, true, TREBUCHET_MODELS_DIR, TREBUCHET_BALLAST_FILENAME, posx+4.2, posy+4.1, posz, 0, 0, 0);
	guard = new ODEBox(world, true, true, TREBUCHET_MODELS_DIR, TREBUCHET_GUARD_FILENAME, posx-17, posy+7.5, posz, 0, 0, 0);
	//guard->SetOrientation(1, -PI/4);

	//dGeomDestroy(guard->GetGeomID());
	//dBodyDestroy(guard->GetBodyID());
	//dGeomID guardTransform = dCreateGeomTransform(world->GetSpaceID());
	//dGeomID newGeomID = dCreateBox(0, guard->GetXDimension(), guard->GetYDimension(),
	//	guard->GetZDimension());
	//dGeomTransformSetGeom(guardTransform, newGeomID);
	//dGeomSetBody(guardTransform, shaft->GetBodyID());
	//guard->SetGeomID(newGeomID);
	//guard->SetBodyID(shaft->GetBodyID());
	//dQuaternion q;
	//dQFromAxisAndAngle(q, 0, 0, 1, PI/3);
	//dGeomSetQuaternion(newGeomID, q);
	//dGeomSetPosition(newGeomID, -12.5, 0.8, posz);

	shaft->SetMass(2);

	//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

	shaftToFrame = dJointCreateHinge(world->GetWorldID(), 0);
	dJointAttach(shaftToFrame, shaft->GetBodyID(), 0);
	jointPoint = frame->GetPosition();
	jointPoint.x += 0.5;
	jointPoint.y += 6;
	dJointSetHingeAnchor(shaftToFrame, jointPoint.x, jointPoint.y, jointPoint.z);
	dJointSetHingeAxis(shaftToFrame, 0, 0, 1);
	dJointSetHingeParam(shaftToFrame, dParamLoStop, -PI/3);
	dJointSetHingeParam(shaftToFrame, dParamHiStop, PI/2);

	shaftToBallast = dJointCreateHinge(world->GetWorldID(), 0);
	dJointAttach(shaftToBallast, shaft->GetBodyID(), ballast->GetBodyID());
	jointPoint = frame->GetPosition();
	jointPoint.x += 4.2;
	jointPoint.y += 6.2;
	dJointSetHingeAnchor(shaftToBallast, jointPoint.x, jointPoint.y, jointPoint.z);
	dJointSetHingeAxis(shaftToBallast, 0, 0, 1);

	shaftToGuard = dJointCreateFixed(world->GetWorldID(), 0);
	dJointAttach(shaftToGuard, shaft->GetBodyID(), guard->GetBodyID());
	dJointSetFixed(shaftToGuard);

	launched = false;
}

ODETrebuchet::~ODETrebuchet()
{
	dJointDestroy(shaftToFrame);
	dJointDestroy(shaftToBallast);
	dJointDestroy(shaftToGuard);

	delete frame;
	delete shaft;
	delete ballast;
}

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

//void ODETrebuchet::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 ODETrebuchet::Draw()
{
	frame->Draw();
	shaft->Draw();
	ballast->Draw();
	guard->Draw();
}

void ODETrebuchet::MakeDisplayList()
{
	frame->MakeDisplayList();
	shaft->MakeDisplayList();
	ballast->MakeDisplayList();
	guard->MakeDisplayList();
}

void ODETrebuchet::Launch()
{
	shaft->SetMass(10);
	ballast->SetMass(100);
	launched = true;
}

void ODETrebuchet::Reset()
{
	shaft->SetMass(2);
	ballast->SetMass(1);
	launched = false;
}

bool ODETrebuchet::IsLaunched()
{
	return launched;
}