// Kinematics2AxisRobot.cpp: implementation of the CKinematics2AxisRobot class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "Kinematics2AxisRobot.h"

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


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

CKinematics2AxisRobot::CKinematics2AxisRobot()
{
	m_MotionParams.MaxLinearLength = 0.25;  // limit the segment lengs for nonlinear systems
	m_MotionParams.MaxRapidFRO = 1.0;       // limit the increase in Rapid HW FRO
	m_MotionParams.UseOnlyLinearSegments=true;
}

CKinematics2AxisRobot::~CKinematics2AxisRobot()
{

}

int CKinematics2AxisRobot::TransformCADtoActuators(double x, double y, double z, double a, double b, double c, double* Acts, bool NoGeo)
{
	//Defin Intermediate Variables
	double A1, H, A2, Y1, Y2, Y3, X1, X2, H1, A3, L3, A4;
	
	// find angles for each motor
	GeoCorrect(x,y,z,&x,&y,&z);

	//Calculate values necessary to solve for motor 2
	A1 = atan(Y/(X + S));									//pythagorean theorem to solve for A1
	H = y / sin(A1);										//pythagorean theorem to find hypotenuse of same angle
	A2 = acos((sqr(H)+sqr(R1)-sqr(L))/(2*H*R1))				//law of Cosines to find A2

	double r1 = A1+A2;										//Motor position 2 is the sum of the solved angles

	//Calculate values necessary to solve for motor 1
	Y1 = sin(180 - M1) * R1;								//Calculate Y height of Motor 2 Linkage
	Y2 = L1 / L * (Y - Y1) + Y1;							//Find height of center link on L using a ratio of Y and Y1
	Y3 = Y - Y2;											//Find Y height of small trangle
	X1 = sqrt(sqr(L2) + sqr(Y2));							//pythagorean theorem to solve for X1
	X2 = X - X1;											//Find base of triangle between M1 and top linkage
	H1 = sqrt(sqr(Y2)+ sqr(X2));							//Find hypotenuse of same angle
	A3 = acos((sqr(R2) + sqr(H1)- sqr(R3)) / (2 * R2 * H1));	//Law of Cos to find angle A3
	L3 = sqrt(sqr(Y2) + sqr(S + X2));						//Find length of line L3
	A4 = acos((sqr(H1)+ sqr(S) - sqr(L3)) / (f * H1 * S));	//Law cos Cos to find angle A4

	double r0 = 180 - A4 - A3; 								//Motor position 1

	Acts[0] = r0*m_MotionParams.CountsPerInchX;
	Acts[1] = r1*m_MotionParams.CountsPerInchY;

	return 0;
}


// perform Inversion to go the other way

int CKinematics2AxisRobot::TransformActuatorstoCAD(double* Acts, double* xr, double* yr, double* zr, double* ar, double* br, double* cr, bool NoGeo)
{
	return InvertTransformCADtoActuators(Acts, xr, yr, zr, ar, br, cr);
}