#include <stdio.h>
#include "ActuatorProvider.h"


ActuatorProvider::ActuatorProvider(Channel *chan, btHingeConstraint *joint, int orient)
: XProvider(chan)
{
	m_joint = joint;
	m_orient = orient;
}

ActuatorProvider::~ActuatorProvider()
{
}

btHingeConstraint *ActuatorProvider::getConstraint()
{
	return m_joint;
}

int ActuatorProvider::getOrient()
{
	return m_orient;
}

void ActuatorProvider::updatePost()
{
	float angle;
	float speed = 200.0;
	float force = 1000.0;
	m_chan->read(&angle, MPI_FLOAT, 1);
	m_joint->enableAngularMotor(true, computeVelocity(m_joint->getHingeAngle(), btScalar(angle)) * speed, btScalar(force));
}

void ActuatorProvider::packData(channelData *data, int id)
{
	data->angle = m_joint->getHingeAngle();
	data->maxAngle = m_joint->getUpperLimit();
	data->minAngle = m_joint->getLowerLimit();
	data->orient = m_orient;
	data->type = ACTUATOR;
}

btScalar ActuatorProvider::computeVelocity(btScalar currentAngle, btScalar targetAngle) {

	btScalar deltaAngle = targetAngle - currentAngle;

	if(deltaAngle < 0)
		deltaAngle += 2 * M_PI;

	if(deltaAngle > M_PI)
		deltaAngle -= 2 * M_PI;

	return deltaAngle;
}