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

#include <math.h>
//#include "traject.h"
#include "../include/traject/traject.h"

#define XYZ			3

// Forward Declarations
class SPFA_Model;				// 	defines Stewart Platform (SP) model
class JointMatrix;              //  coordinates of the 6 SP joints
class RotationMatrix;           //	rotation matrix, overloaded multiplication


//-------------------------------------------------------------
// sqr(x) = x*x
//-------------------------------------------------------------
template <class Type>
Type sqr(Type x){
	Type temp = x;
	return temp*temp;
};

//-------------------------------------------------------------

class JointMatrix {
	public:
		double m[6][XYZ];								// (x,y,z) coordinates of 6 joints

	protected:
		static class JointMatrix q;						// buffer for temporary storage

	friend JointMatrix& operator -(const JointMatrix& a, const JointMatrix& b);   // subtracts 2 joint matrices and stores result in temp.var. q
	friend JointMatrix &operator *(const RotationMatrix& Rot, const JointMatrix& p);  // multiplies a rotational matrix with a joint matrix and stores result in temp.var. q
	friend class SPFA_Model;							// in order to access temp.var. q

	public:
	// constructors
	JointMatrix( ) {}								 	// default constructor				
	JointMatrix( const double R, const double Theta);	// constructing joints along a circle of radius R
	JointMatrix( const double *matrixPtr);				// construction from a pool of values (a file for instance)

};


//-------------------------------------------------------------

class RotationMatrix {
		double m[4][4];

		friend JointMatrix &operator *(const RotationMatrix& Rot, const JointMatrix& p);
	public:
		class RotationMatrix& RollPitchYaw(const double x[6]);
	RotationMatrix(const double x[6]);
};


//-------------------------------------------------------------
class SPFA_Model {
		class JointMatrix base, platform;
		double links	[6], heights	[6];

		friend JointMatrix &operator *(const RotationMatrix& Rot, const JointMatrix& p);

	public:
		void SPFA_Inverse( const RotationMatrix& Rot, double h[6]);
		void SPFA_Inverse( double const x[6], double h[6]);
		void SPFA_Inverse( const Trajectory inTrajectory,
								 Trajectory outActuators);    // obsolete
		Trajectory& SPFA_Inverse( const Trajectory& inTrajectory);

	// constructors:

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

	// construction from a pool of values (a file for instance)
	SPFA_Model (const double *modelPtr);

};	// class SPFA_Model


#endif	// #ifndef	SPFAMODEL_H