#include "InverseKinemetics.h"

#pragma region Core
void InverseKinemetics::RL3 (Data *DataLine)
{
	float x = DataLine->X;
	float z = DataLine->Z;

	if ((x == 0) && (z == 0)) 
		DataLine->l3 = 0;
	else DataLine->l3 = hypot(z, x);

	if (x < 0)
		DataLine->l3 = DataLine->l3 * -1;
}

float InverseKinemetics::O1 (Data *DataLine)
{
	RL3(DataLine);

	float x = DataLine->X;
	float z = DataLine->Z;
	float r;

	if ((x != 0) & (z != 0)){
		double a = (z / x);
		return atan(a);
	}
	else
		return atan(0.0); // Math Error Protection.
}

float InverseKinemetics::T1 (Data *DataLine)
{
	float x = DataLine->X;
	float y = DataLine->Y;
	float z = DataLine->Z;
	float x2 = x * x;
	float y2 = y * y;
	float z2 = z * z;
	float l32 = DataLine->l3 * DataLine->l3;

	DataLine->s = sqrt(x2 + y2 + z2);

	float t1 = 0;

	float s2 = DataLine->s * DataLine->s;

	if ((y == 0) && (DataLine->l3 == 0))
		t1 = 0; // Math Error Protection.
	else
		t1 = (atan2(float(DataLine->l3), float(y)));

	float a = (l1 * l1) - (l2*l2) + s2;
	float b = 2 * l1 * DataLine->s;
	float t2 = acos(a / b);

	float ret = 0;

	ret = t1 + abs( (long long) t2);

	return ret;
}

float InverseKinemetics::T3 (Data *DataLine)
{
	return (acos((float)((l1*l1) + (l2*l2) - (DataLine->s*DataLine->s)) / (2 * l1 * l2)));
}
#pragma endregion Core

#define PI 3.14159265

void InverseKinemetics::setleg (bool Side, Data *DataLine)
{
	error = false;

	if (Side == 0)
	{
		float a = ((float)90 - degrees(O1(DataLine)));
		float b = ((float)180 - degrees(T1(DataLine)));
		float c = degrees(T3(DataLine));

		if ((a >= 0) & (a <= 180)
			& (b >= 0) & (b <= 180)
			& (c >= 0) & (c <= 180))
		{
			DataLine->O1 = a;
			DataLine->O2 = b;
			DataLine->O3 = c;

			float d = sin( (float)b*(PI/180) );
			if( d != 0 )
				DataLine->X2 = d * l1;
			else
				DataLine->X2 = 0;

			d = cos( (float)b*(PI/180) );
			if( d != 0 )
				DataLine->Y2 = d * l1;
			else
				DataLine->Y2 = 0;

			d = sin( (float)(a-90)*(PI/180) );
			if( d != 0 )
				DataLine->Z2 = d * abs( (long long) DataLine->X2 );
			else
				DataLine->Z2 = 0;
		}
		else
		{
			error = true;
		}
	}
	//else
	//{
	//	float a = ((float)90 + degrees(O1(DataLine)));
	//	float b = degrees(T1(DataLine));
	//	float c = ((float)180 - degrees(T3(DataLine)));
	//
	//	/*if ((a >= 0) & (a <= 180)
	//	& (b >= 0) & (b <= 180)
	//	& (c >= 0) & (c <= 180))
	//	{*/
	//	DataLine->O1 = a;
	//	DataLine->O2 = b;
	//	DataLine->O3 = c;
	//	/*}
	//	else
	//	{
	//	error = true;
	//	}*/
	//}
}