/* SPFA.h
*/
#ifndef	SPFAMODEL_H
#define SPFAMODEL_H

#include <math.h>

#include "..\Transform\Transformation.h"
#include "..\Transform\Points3D.h"
#include "..\OMSUtil\ActuatorPositions.h"

typedef double Radius;
typedef double Angle;

// Forward decleration
class ActuatorPositions;
class JointMatrix;
class SPFAModel;
class ActuatorPositions;


//-------------------------------------------------------------
class JointMatrix : public Points3D 
{
	public: 
		JointMatrix() : Points3D(ACTUATOR_NUM) {
		}
		// constructing joints along a circle of radius R
		JointMatrix( const Radius R, const Angle Theta);
		virtual ~JointMatrix() {
		}

		
		static JointMatrix* matrixToJointMatrix(Matrix_d* mptr){
			return (JointMatrix*)(mptr);		// converts Matrix pointer to Points3D pointer
		}                                   // used to inherit all matrix operators

};

JointMatrix operator * ( const TransformationMatrix& R, const JointMatrix& p );
JointMatrix operator + ( const JointMatrix& p, const JointMatrix& q );
JointMatrix operator - ( const JointMatrix& p, const JointMatrix& q );





//-------------------------------------------------------------
#include "ManipulatorModel.h"

class SPFAModel 
	:	ManipulatorModel<ActuatorPositions>

{
	private:
		class JointMatrix base, platform;
		double linkLenghts[ACTUATOR_NUM];				// lengths of the links between actuators and platform 

	public:
		ActuatorPositions inv( const TransformationMatrix& Rot ) 	// inverse kinematics
				throw(ManipulatorException);
	// constructors:

	// SP model defined by base and platform shape,
	//		and the distance between them
		SPFAModel
			(	Radius Rb, Radius Rp					// base & platform radii
			,	Angle ThetaBase, Angle ThetaPlatform	// angles between joints
			,	double Height ); 						// initial platform height

};	





#ifndef M_PI							// For Visual C++
	#define		M_PI	3.1415926535 
#endif

#endif	// #ifndef	SPFAMODEL_H