//////////////////////////////////////////////////////
///////  OWI Robot Navigation and Control Kit  ///////
///////     Version 0.1, December 28, 1999     ///////
///////             Alan Mishchenko            ///////
//////////////////////////////////////////////////////

#include 
#include 
#include 
#include "Navigation.h"
#include "Control.h"

// angular rotation speeds
double OmegaBase[2];
double OmegaShoulder[2];
double OmegaElbow[2];
double OmegaWrist[2];
double OmegaGrip[2];

int Move( CARTESIAN StartPoint, CARTESIAN StopPoint )
{
	CYLINDRIC Start = Cartesian2Cylindric( StartPoint );
	CYLINDRIC Stop  = Cartesian2Cylindric( StopPoint );

	TRIPLE CurPos  = GetRobotParameters( Start );
	TRIPLE NextPos = GetRobotParameters( Stop );

	if ( CurPos.A == 0 && CurPos.B == 0 && CurPos.C == 0 )
		return 1;

	if ( NextPos.A == 0 && NextPos.B == 0 && NextPos.C == 0 )
		return 1;

	double BaseShift = NextPos.A - CurPos.A;
	int BaseDirection = ( BaseShift < 0 );

	double ShoulderShift = NextPos.B - CurPos.B;
	int ShoulderDirection = ( ShoulderShift < 0 );

	double ElbowShift = NextPos.C - CurPos.C;
	int ElbowDirection = ( ElbowShift < 0 );

	double SecondsBase     = fabs(BaseShift)    /OmegaBase[ BaseDirection ];
	double SecondsShoulder = fabs(ShoulderShift)/OmegaShoulder[ ShoulderDirection ];
	double SecondsElbow    = fabs(ElbowShift)   /OmegaElbow[ ElbowDirection ];

	int TimeBase    = int(SecondsBase*1000);
	int TimeShoulder= int(SecondsShoulder*1000);
	int TimeElbow   = int(SecondsElbow*1000);

	if ( StartPoint.Z > 20 )
	{
		SetMotion( 'b', BaseDirection );
		ContinueMotion( TimeBase );

		SetMotion( 's', ShoulderDirection );
		ContinueMotion( TimeShoulder );

		SetMotion( 'e', ElbowDirection );
		ContinueMotion( TimeElbow );

		StopMotion();
	}
	else
	{
		SetMotion( 'e', ElbowDirection );
		ContinueMotion( TimeElbow );

		SetMotion( 's', ShoulderDirection );
		ContinueMotion( TimeShoulder );

		SetMotion( 'b', BaseDirection );
		ContinueMotion( TimeBase );

		StopMotion();
	}


	return 0;
}

void SetMotion( char Joint, int Direction )
{
	int Value;

	switch ( Joint )
	{
		case 'b':
		case 'B':
		{
			Value = Direction? 37: 2;
			_outp( DPORT, Value ); 
			break;
		}
		case 's':
		case 'S':
		{
			Value = Direction? 3: 69;
			_outp( DPORT, Value ); 
			break;
		}
		case 'e':
		case 'E':
		{
			Value = Direction? 21: 1;
			_outp( DPORT, Value ); 
			break;
		}
		case 'w':
		case 'W':
		{
			Value = Direction? 4: 128;
			_outp( DPORT, Value ); 
			break;
		}
		case 'g':
		case 'G':
		{
			Value = Direction? 13: 0;
			_outp( DPORT, Value ); 
			break;
		}
		default:
			ASSERT( 0 );
	}	
}

void StopMotion()
{
	_outp( DPORT, 7 ); 
}

void ContinueMotion( int Time )
{
	Sleep( Time );
}

void Calibrate()
// determine angular speed of rotation of joints 
// (in radians per second)
{
	
	OmegaBase[0] = Pi/25;
	OmegaBase[1] = Pi/25;

	OmegaShoulder[0] = (Pi/2)/25;
	OmegaShoulder[1] = (Pi/2)/35;

	OmegaElbow[0] = (Pi/2)/25;
	OmegaElbow[1] = (Pi/2)/26;

	OmegaWrist[0] = Pi/6;
	OmegaWrist[1] = Pi/6;

	OmegaGrip[0] = (Pi/2)/3;
	OmegaGrip[1] = (Pi/2)/3;
}