#include "stdafx.h"
#include "KinematicsDanasScara.h"

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


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

CKinematicsDanasScara::CKinematicsDanasScara()
{
	Length_1 = 23.622; // X axis length 23.622 in or 600mm
	Length_2 = 23.622; // Y axis length 23.622 in or 600mm

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

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

CKinematicsDanasScara::~CKinematicsDanasScara()
{

}

int CKinematicsDanasScara::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 distance = sqrt(sqr(_X) + sqr(_Y));  // distance from (x0,y0) to (x2,y2)
												// to simplify calculations
	double distance2 = sqr(_X) + sqr(_Y);

	//the angle between the vector from (0,0) to (x2,y2) and the axis X
	double angle = acos(CorrectAngle(_X / distance));

	// looking for angles in a triangle by the consensus theorem
	double angle1 = acos(CorrectAngle((distance2 + SqrLength_1 - SqrLength_2) / (2 * distance * Length_1)));
	double angle3 = acos(CorrectAngle((distance2 + SqrLength_2 - SqrLength_1) / (2 * distance * Length_2)));
	double angle2 = PI - angle1 - angle3;

	double angleMotor1 = angle + angle1; // angle of rotation of the 1st engine
	double a1 = 180 - angleMotor1 * 180 / PI; //angle of rotation of 1 engine in degrees
	double a2 = 180 - 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 axis

	// counts per rev are set in the KmotionCNC Trajectory planner setup Screen, but details are here for reference
	// X axis is channel 7, at 9600 pulses per rev with a 121 gear reduciton ratio
	// Counts per inch = (9600 * 121) / 360 = 3226.66667 pul/deg
	// Y axis is channel 6, at 9600 pulses per rev with a 105 gear reduciton ratio
	// Counts per inch = (9600 * 105) / 360 = 2800 pul/deg
	// Z axis is channel 5, at 9600 pulses per rev with a 20 mm per turn lead screw
	// Counts per inch = (9600 pul/rev) / (20mm/rev / 25.4mm/in) = 12192 pul/in
	// A axis is channel 4, at 6800 pulses per rev with a 50 gear reduction ratio
	// Counts per inch = (6400 * 50) / 360 deg = 1333.33333 pul/deg

	Acts[0] = a1 * m_MotionParams.CountsPerInchX;
	Acts[1] = a2 * m_MotionParams.CountsPerInchY;
	Acts[2] = z * m_MotionParams.CountsPerInchZ;

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

	return 0;
}

int CKinematicsDanasScara::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 = PI - (a1)* PI / 180;
	double angleMotor2 = PI - (a2)* PI / 180;

	// distance from (0,0) to (x2,y2)
	double distance = sqrt(SqrLength_1 + SqrLength_2 - 2 * Length_1 * Length_2 * cos(angleMotor2));
	// to simplify calculations
	double distance2 = SqrLength_1 + SqrLength_2 - 2 * Length_1 * Length_2 * cos(angleMotor2);

	double t;
	// 1st angle of the triangle
	if (distance != 0)
		t = (SqrLength_1 + distance2 - SqrLength_2) / (2 * Length_1 * distance);
	else
		t = 0;
	double angle1 = acos(CorrectAngle(t));

	// angle between the vector from (0,0) to (x2,y2) and the X axis
	double angle = angleMotor1 - angle1;

	double a = a1 + a2 + a3;


	// find the coordinates (x2,y2) - the end point
	*xr = distance * cos(angle) + (Length_1 + Length_2);
	*yr = distance * sin(angle);

	*zr = Acts[2] / m_MotionParams.CountsPerInchZ;
	*ar = a;
	*br = Acts[4] / m_MotionParams.CountsPerInchB;
	*cr = Acts[5] / m_MotionParams.CountsPerInchC;

	return 0;
}

// Check for out of range for ACos
double CKinematicsDanasScara::CorrectAngle(double _angle)
{
	if (_angle > 1)
		return 1;
	if (_angle < -1)
		return -1;
	return _angle;
}