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

#include <math.h>
#include "Navigation.h"
#include "Control.h"

CARTESIAN Cylindric2Cartesian( CYLINDRIC Point )
{
	CARTESIAN Result;

	Result.X = Point.Ro * cos( Point.Fi );
	Result.Y = Point.Ro * sin( Point.Fi );
	Result.Z = Point.Hi;

	return Result;
}

CYLINDRIC Cartesian2Cylindric( CARTESIAN Point )
{
	CYLINDRIC Result;

	if ( Point.X==0 ) // the case of right angle
		Result.Fi = (Point.Y>0)*Pi/2;
	else
	    Result.Fi = atan( Point.Y/Point.X );

	if ( Result.Fi < 0 ) // normalization
		Result.Fi += Pi;

	Result.Ro = sqrt( Point.X*Point.X + Point.Y*Point.Y );
	Result.Hi = Point.Z;

	return Result;
}

TRIPLE GetRobotParameters( CYLINDRIC Point )
// takes Point in cylindric coordinates
// return three values of robotic arm parameters
// (base angle, shoulder angle, elbow angle)
{
	TRIPLE Result;
	Result.A = 0;
	Result.B = 0;
	Result.C = 0;

	// sanity check
	if ( Point.Ro == 0 && Point.Hi == 0 )
		return Result; // abnormal situation

	//setting the center of coords into the shoulder rotation point
	Point.Hi -= Base;

	// the distance from Point to the center of coordinates
	double Dist = sqrt( (Point.Ro)*(Point.Ro) + (Point.Hi)*(Point.Hi) );

	// reachability check
	if ( Dist > Shoulder + Elbow )
		return Result; // abnormal situation

	// the angle to the Point from the center of coordinates
	double Gamma;
	if ( Point.Ro == 0 )
		Gamma = (Point.Hi > 0)*Pi/2;
	else
		Gamma = atan( (Point.Hi)/(Point.Ro) );

	// the first angle of the triangle formed by the direction 
	// to the point and the two dimensions (shoulder and elbow)
	// of the robot - found using the cosine theorem
	double Temp1 = Shoulder*Shoulder + Dist*Dist - Elbow*Elbow;
	double Angle1 = acos( Temp1/2/Shoulder/Dist );

	// compute the shoulder angle
	Result.B = Pi/2 - Angle1 - Gamma;

	// the second angle of the triangle formed by the direction 
	// to the point and the two dimensions (shoulder and elbow) 
	// of the robot - found using the cosine theorem
	double Temp2 = Shoulder*Shoulder + Elbow*Elbow - Dist*Dist;
	double Angle2 = acos( Temp2/2/Shoulder/Elbow );

	// compute the elbow angle
	Result.C = Pi - Angle2;

	// compute the base angle
	Result.A = Point.Fi;

	return Result;
}