#include "StdAfx.h"


#include <ode/ode.h>

#include "jointhinge2.h"


namespace ODEManaged
{
	//Constructors

	JointHinge2::JointHinge2(void) : Joint(){}

	JointHinge2::JointHinge2(World &world)
	{
		if(this->_id) dJointDestroy(this->_id);
		_id = dJointCreateHinge2(world.Id(),0);
	}
	
	JointHinge2::JointHinge2(World &world, JointGroup &jointGroup)
	{
		if(this->_id) dJointDestroy(this->_id);
		_id = dJointCreateHinge2(world.Id(), jointGroup.Id());
	}

	//Destructor

	JointHinge2::~JointHinge2(void){}

	//CreateHinge2 (overload 1)

	void JointHinge2::Create(World &world, JointGroup &jointGroup)
	{
		if(this->_id) dJointDestroy(this->_id);
		_id = dJointCreateHinge2(world.Id(), jointGroup.Id());
	}

	//CreateHinge2 (overload 2)

	void JointHinge2::Create(World &world)
	{
		if(this->_id) dJointDestroy(this->_id);
		_id = dJointCreateHinge2(world.Id(),0);
	}

	//SetAnchor1

	void JointHinge2::SetAnchor (double x, double y ,double z)
	{
		dJointSetHinge2Anchor(_id, x,y,z);
	}

	//GetAnchor1

	Vector3 JointHinge2::GetAnchor()
	{
		Vector3 retVal;
		dVector3 temp;
		dJointGetHinge2Anchor(_id,temp);
		retVal.x = temp[0];
		retVal.y = temp[1];
		retVal.z = temp[2];
		return retVal;
	}

	//SetAxis1

	void JointHinge2::SetAxis1 (double x, double y ,double z)
	{
		dJointSetHinge2Axis1(_id, x,y,z);
	}

	//GetAxis1

	Vector3 JointHinge2::GetAxis1()
	{
		Vector3 retVal;
		dVector3 temp;
		dJointGetHinge2Axis1(_id,temp);
		retVal.x = temp[0];
		retVal.y = temp[1];
		retVal.z = temp[2];
		return retVal;
	}

	//SetAxis2

	void JointHinge2::SetAxis2 (double x, double y ,double z)
	{
		dJointSetHinge2Axis2(_id, x,y,z);
	}

	//GetAxis2

	Vector3 JointHinge2::GetAxis2()
	{
		Vector3 retVal;
		dVector3 temp;
		dJointGetHinge2Axis2(_id,temp);
		retVal.x = temp[0];
		retVal.y = temp[1];
		retVal.z = temp[2];
		return retVal;
	}

	//GetAngle1

	double JointHinge2::GetAngle1 ()
	{
		return dJointGetHinge2Angle1(this->_id);
	}

	//GetAngle1Rate

	double JointHinge2::GetAngle1Rate ()
	{
		return dJointGetHinge2Angle1Rate(this->_id);
	}

	////GetAngle hmm, this doesn't exist

	//double JointHinge2::GetAngle2 ()

	//{

	//	return dJointGetHinge2Angle2(this->_id);

	//}


	//GetAngle2Rate

	double JointHinge2::GetAngle2Rate ()
	{
		return dJointGetHinge2Angle2Rate(this->_id);
	}


	//Attach (overload 1)

	void JointHinge2::Attach (Body &body1, Body &body2)
	{
		dJointAttach(_id, body1.Id(),body2.Id());
	}

	//Attach (overload 2) 

	//TODO: possibly add an overload that takes anchor as a param also.

	void JointHinge2::Attach (Body &body1)
	{
		dJointAttach(_id, body1.Id(),0);
	}


}