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

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

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


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

CKinematics2AxisRobot::CKinematics2AxisRobot()
{
	//define linkage geometry
	L = 18;
	R1 = 6;
	S = 6;
	R2 = 6;
	R3 = 6;
	L1 = 12

	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 L, double R1, double S, double A1, double H, double A2, double M2, double R2, double R3, double L1,
double Y1, double Y2, double Y3, double X1, double X2, double X3, double H1, double A3, double L3, double A4, double* Acts, bool NoGeo = false);
{
	// 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((H^2+R1^2-L^2)/(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(L2 ^ 2 + Y2 ^ 2);				//pythagorean theorem to solve for X1
	X2 = X - X1;							//Find base of triangle between M1 and top linkage
	H1 = sqrt(Y2 ^ 2 + X2 ^ 2);				//Find hypotenuse of same angle
	A3 = acos((R2 ^ 2 + H1 ^ 2 - R3 ^ 2) / (2 * R2 * H1));	//Law of Cos to find angle A3
	L3 = sqrt(Y2 ^ 2 + (S + X2) ^ 2);		//Find length of line L3
	A4 = acos((H1 ^ 2 + S ^ 2 - L3 ^ 2) / (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 X, double Y, double L, double R1, double S, double A1, double H, double A2, double M2, double R2, double R3, double L1,
	double Y1, double Y2, double Y3, double X1, double X2, double X3, double H1, double A3, double L3, double A4, double* Acts, bool NoGeo = false);
{
	return InvertTransformCADtoActuators(Acts, xr, yr, zr, ar, br, cr);
}