#include "SPFA.h"

//#include <stl> 			// Standard Template Library seems to be missing
//-------------------------------------------------------------
// sqr(x) = x*x
//-------------------------------------------------------------
template <class T>
T sqr(T x){
	return x*x;
};


/****************************************************************************
* SPFAModel::SPFAModel - constructor. 
*          along circles with radii Rb and Rc.
*     Inputs: Rb, Rp - Base and platform joints are positioned 
*                      along circles with radii Rb and Rc.
*             ThetaBase, ThetaPlatform - angles between first two joints
*             Height - Height of the platform when actuators are at zero
*/
SPFAModel::SPFAModel
			(	Radius Rb, Radius Rp					// base & platform radii
			,	Angle ThetaBase, Angle ThetaPlatform	// angles between joints
			,	double Height ) 						// initial platform height
	:	base(Rb, ThetaBase) 
	,	platform (Rp, ThetaPlatform)
{
	int i;

	// the lengths of the links are equal to the distances between the joints
	for( i = 0; i < ACTUATOR_NUM; i++ ) {
		linkLenghts[i] = sqrt(
			sqr(platform[X][i] - base[X][i])
		+	sqr(platform[Y][i] - base[Y][i])
		+	sqr(Height         - base[Z][i])
		);
	}

}

/****************************************************************************
* SPFAModel::inv - calculates inverse kinematics for the manipulator
*     Inputs: Rot - desired pose of the manipulator end-effector
*     Outputs:    - positions of the actuators coresponding to the pose
*     Side-effects- none
*/
ActuatorPositions SPFAModel::
inv( const TransformationMatrix& Rot )
		throw(ManipulatorException)
{

	JointMatrix q = Rot * platform - base;	// distances between platform and base joints  
	ActuatorPositions heights;
	
	for( int i=0; i < ACTUATOR_NUM; ++i )  {
		double ssquared = sqr(linkLenghts[i]) - sqr(q[X][i]) - sqr(q[Y][i]);
		if( ssquared < 0.0 )	throw ManipulatorException();
		heights[i] = q[Z][i] - sqrt( ssquared );
	}

	return heights;
}



/****************************************************************************
* JointMatrix::JointMatrix - constructor of joints for SPFA base or platform 
**     Inputs: R - Joints are positioned on a circle of this radius 
*              Theta - angle between first two joints
*/
JointMatrix::JointMatrix( const Radius R, const Angle Theta)
	: Points3D(ACTUATOR_NUM) 				// initialize parent class
{
	Angle lambda[ACTUATOR_NUM];
	int i;

	lambda[0] = -2*M_PI/3.0 - Theta/2.0;
	lambda[1] = -2*M_PI/3.0 + Theta/2.0;
	lambda[2] =             - Theta/2.0;
	lambda[3] =             + Theta/2.0;
	lambda[4] = +2*M_PI/3.0 - Theta/2.0;
	lambda[5] = +2*M_PI/3.0 + Theta/2.0;

	for( i = 0; i < ACTUATOR_NUM; i++ ) {
		(*this)[X][i] = R * cos(lambda[i]);
		(*this)[Y][i] = R * sin(lambda[i]);
		(*this)[Z][i] = 0.0;
		(*this)[W][i] = 1.0;
	}
}

//------------------------------------------------------------------------------
// Operators 
//
JointMatrix operator * ( const TransformationMatrix& R, const JointMatrix& p ) {
	return * JointMatrix::matrixToJointMatrix(&(R*p.toMatrix()));			// convert Matrix_d to points
}
JointMatrix operator + ( const JointMatrix& p, const JointMatrix& q ) {
	return * JointMatrix::matrixToJointMatrix(&(p.toMatrix() + q.toMatrix()));
}
JointMatrix operator - ( const JointMatrix& p, const JointMatrix& q ) {
	return * JointMatrix::matrixToJointMatrix(&(p.toMatrix() - q.toMatrix()));
}