#include "stdafx.h"
#include "KinematicsScara.h"

#define sqr(x) ((x)*(x))


//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

CKinematicsScara::CKinematicsScara()
{
	Length_1 = 6.2; // X axis length
	Length_2 = 5.31; // Y axis length

	SqrLength_1 = sqr(Length_1); //X*X
	SqrLength_2 = sqr(Length_2); //Y*Y

								 // copied from gepetto
	//m_MotionParams.MaxLinearLength = 0.05;  // limit the segment lengths for nonlinear systems
	m_MotionParams.MaxAngularChange = 0.5;  // limit the segment angle change for nonlinear systems
	m_MotionParams.MaxRapidFRO = 1.0;       // limit the increase in Rapid HW FRO
	m_MotionParams.UseOnlyLinearSegments = true;
}

CKinematicsScara::~CKinematicsScara()
{

}

int CKinematicsScara::TransformCADtoActuators(double x, double y, double z, double a, double b, double c, double *Acts, bool NoGeo)
{
     double X_= x -(Length_1 + Length_2); // shift the coordinates to match the systems
	 double Y_ = y;
	double aux2 = (sqr(X_) + sqr(Y_) - SqrLength_2 - SqrLength_1) / (2 * Length_2*Length_1);
	double ancoss = sqrt(1 - sqr(aux2));

	// looking for angles in a triangle by the consensus theorem
	double angle2 = atan(ancoss / aux2);
	double angle1 = atan(X_/Y_) - atan((Length_2*ancoss) / (Length_1 + Length_2 * (aux2)));



	double a1 = angle1 * 180 / PI; //angle of rotation of 1 engine in degrees
	double a2 = (angle2) * 180 / PI; //angle of rotation of 2 engine in degrees
	//double a3 = -a1 - a2 + a; //we hold the axis A parallel to the X axi
	Acts[0] = a1 * m_MotionParams.CountsPerInchX;
	Acts[1] = a2 * m_MotionParams.CountsPerInchY;
	Acts[2] = z * m_MotionParams.CountsPerInchZ;

	Acts[3] = a * m_MotionParams.CountsPerInchA;
	Acts[4] = b * m_MotionParams.CountsPerInchB;
	Acts[5] = c * m_MotionParams.CountsPerInchC;

	return 0;
}

int CKinematicsScara::TransformActuatorstoCAD(double *Acts, double *xr, double *yr, double *zr, double *ar, double *br, double *cr, bool NoGeo)
{
	double a1 = Acts[0] / m_MotionParams.CountsPerInchX;
	double a2 = Acts[1] / m_MotionParams.CountsPerInchY;
	//double a3 = Acts[3] / m_MotionParams.CountsPerInchA;

	double angleMotor1 = (a1)* PI / 180;//convertir a radianes
	double angleMotor2 = (a2)* PI / 180;

	

	// find the coordinates (x2,y2) - the end point
	*xr = Length_1 * cos(angleMotor1) + Length_2 * cos(angleMotor1 + angleMotor2);
	*yr = Length_1 * sin(angleMotor1) + Length_2 * sin(angleMotor1 + angleMotor2);
	*zr = 0.0;
	*ar = 0.0;
	*br = Acts[4] / m_MotionParams.CountsPerInchB;
	*cr = Acts[5] / m_MotionParams.CountsPerInchC;
	return 0;
}