/* fileName: biped.c
 * purpose: Biped object to interface to the biped
 * author: Jesse Pepper <peppe-jj@ee.uwa.edu.au>
 * version: v0.00a
 * bugs:
 * notes: 
 */

#include <eyebot.h>

/* Local Headers */
#include "globalDefines.h"
#include "biped.h"
#include "acc.h"

/* Prototypes */
int angleToServo( int servoName, int angle );

extern globals g;

typedef struct tBiped
	{
	ServoHandle       servo[cNumServos];
	int               posture[cNumServos];
	unsigned char     feet;
	bool              servoUsed;
	bool              accUsed;
	} biped;

biped b;

/******************************************************************************/

static int offset[cNumServos] = {128, 128, 128, 134, 122, 128, 128, 128, 128};
static int sFact[cNumServos] = {-423, -512, -512, 133, -133, 133, -133, -133, 133};

/* ok version
static int offset[cNumServos] = {128, 128, 128, 128, 128, 128, 128, 128, 128};
static int sFact[cNumServos] = {-423, -512, -512, 133, -133, 133, -133, -133, 133};
*/

static int maxAngle[cNumServos] = {50, 50, 50, 50, 50, 50, 50, 50, 50};
static int minAngle[cNumServos] = {-50, -50, -50, -50, -50, -50, -50, -50, -50};

/******************************************************************************/

void bipedInit( bool acc, bool servo )
	{
	if (acc == true)
		{
		b.accUsed = true;
		accInit( 1, 0.2 );
		}

	if (g.e != noError)
		return;

	if (servo == true)
		{
		b.servoUsed = true;
		/* Assign servo handles */
		b.servo[torso] = SERVOInit( Torso );
		b.servo[lHipT] = SERVOInit( LHipT );
		b.servo[rHipT] = SERVOInit( RHipT );
		b.servo[lHipB] = SERVOInit( LHipB );
		b.servo[rHipB] = SERVOInit( RHipB ); 
		b.servo[lKnee] = SERVOInit( LKnee );
		b.servo[rKnee] = SERVOInit( RKnee ); 
		b.servo[lAnkle] = SERVOInit( LAnkle );
		b.servo[rAnkle] = SERVOInit( RAnkle ); 
		}
	}

/******************************************************************************/

void bipedSetPosture( int* newPosture )
	{
	int i;

	if (newPosture == NULL)
		{
		g.e = bipedError;
		return;
		}
	
	for (i=0; i<cNumServos; i++)
		{
		if (newPosture[i] > maxAngle[i] || newPosture[i] < minAngle[i])
			{
			}
			b.posture[i] = newPosture[i];
		}

	for (i=0; i<cNumServos; i++)
		{
		if (SERVOSet( b.servo[i], angleToServo( i, newPosture[i] ) ) != 0)
			{
			g.e = bipedError;
			return;
			}
		}
	}

/******************************************************************************/

int angleToServo( int servoName, int angle )
	{
	if (angle > maxAngle[servoName])
		return (maxAngle[servoName] * sFact[servoName])/100 + offset[servoName];
	if (angle < minAngle[servoName])
		return (minAngle[servoName] * sFact[servoName])/100 + offset[servoName];

	return (angle * sFact[servoName])/100 + offset[servoName];
	}

/******************************************************************************/

void bipedGetPosture( int* posture )
	{
	int i;

	if (posture == NULL)
		{
		g.e = bipedError;
		return;
		}

	for (i=0; i<cNumServos; i++)
		posture[i] = b.posture[i];
	}

/******************************************************************************/

float bipedGetAccFB( void )
	{
	return (accGetFB());
	}

/******************************************************************************/

float bipedGetAccLR( void )
	{
	return (accGetLR());
	}

/******************************************************************************/

unsigned char bipedGetFootStatus( void )
	{
	return (b.feet);
	}

/******************************************************************************/