// Version 1

#ifndef HEX
#define HEX

#include "Arduino.h"
#include "SerD.h"
#include <QueueList.h>

// Angle to Pulse Length converter.
#define smin0 600
#define smin1 960
#define smin2 600
#define smin3 600
#define smax0 2400
#define smax1 2100
#define smax2 2400
#define smax3 2400

// Task Pipeline
#define H_TASK	001
#define H_IK	002
#define	H_IKER	003
#define H_ATPC	004

// ProgramControl
#define PC_EMPTY	101
#define PC_NEEDDATA	102

//    LAYOUT
//      _U_
//  0==/   \==3
//     >***<
// 1===|---|===4
//     >***<
//  2==\___/==5

class Hexapod
{
private:
	// Task Pipeline.
	QueueList<int> TaskQueue;
	QueueList<int> Parameter;

	// Program Control
	int PcCommand;

	// Timing.
	int OldTime;
	int TimeBetweenNewPosition;

	/* Servo Data
	# Including coordinate, Angle and Pulse Length.
	*/
	boolean DataReady;
	SerD ServoData[6];

	// Inverse Kinemetics.
	float s;
	float l3;
#define l2 15
#define l1 10

	void	RL3 (int i)
	{
		float x = ServoData[i].x;
		float z = ServoData[i].z;

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

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

	float	O1 (int i)
	{
		RL3(i);

		float x = ServoData[i].x;
		float z = ServoData[i].z;
		float r;

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

	float	T1 (int i)
	{
		float x = ServoData[i].x;
		float y = ServoData[i].y;
		float z = ServoData[i].z;
		float x2 = x * x;
		float y2 = y * y;
		float z2 = z * z;
		float l32 = l3 * l3;

		s = sqrt(x2 + y2 + z2);

		float t1 = 0;

		float s2 = s * s;

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

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

		float ret = 0;

		ret = t1 + abs(t2);

		return ret;
	}

	float	T3 ()
	{
		return (acos(((l1*l1) + (l2*l2) - (s*s)) / (2 * l1 * l2)));
	}

	void	setleg (int i)
	{
		if (i < 3)
		{
			float a = ((float)90 - degrees(O1(i)));
			float b = ((float)180 - degrees(T1(i)));
			float c = degrees(T3());

			if ((a >= 0) & (a <= 180)
				& (b >= 0) & (b <= 180)
				& (c >= 0) & (c <= 180))
			{
				ServoData[i].o1 = a;
				ServoData[i].o2 = b;
				ServoData[i].o3 = c;
			}
			else
			{
				TaskQueue.push(H_IKER);
				Parameter.push(i);
			}
		}
		else
		{
			float a = ((float)90 + degrees(O1(i)));
			float b = degrees(T1(i));
			float c = ((float)180 - degrees(T3()));

			if ((a >= 0) & (a <= 180)
				& (b >= 0) & (b <= 180)
				& (c >= 0) & (c <= 180))
			{
				ServoData[i].o1 = a;
				ServoData[i].o2 = b;
				ServoData[i].o3 = c;
			}
			else
			{
				TaskQueue.push(H_IKER);
				Parameter.push(i);
			}
		}
	}

	void	IK()
	{
		for (int i=0; i<6; i++)
			setleg(i);
	}

	// Angle to Pulse Length converter.

	int ConverttoMS(int Position, int low, int high)
	{
		int tmp = ( Position * ( ( high - low ) / 180 ) );
		return low + tmp;
	}

	void ConvertAngleToPulseLength()
	{
		for (int i=0; i<6; i++)
		{
			ServoData[i].t1 = ConverttoMS(ServoData[i].o1, smin0, smax0);
			ServoData[i].t2 = ConverttoMS(ServoData[i].o2, smin1, smax1);
			ServoData[i].t3 = ConverttoMS(ServoData[i].o3, smin2, smax2);
		}
	}

	void ProgramControl( int Command )
	{
		// Drop task from queue
		TaskQueue.pop();

		// Check Command.
		switch (Command)
		{
		case PC_EMPTY:
			{

				break;
			}

		case PC_NEEDDATA:
			{
				/*
				Queue = {
				Get Move
				Do IK
				Convert To MS
				Send }
				*/
				break;
			}

		default:
			break;
		}
	}

public:
	// Task Pipeline.
	void	loop()
	{
		PcCommand = 0;
		do{
			/*
			Runmonitoing methods and
			Timingdependent methods.
			*/
			if (millis() >= (OldTime + TimeBetweenNewPosition))
			{
				OldTime = millis();
			}

			//Add Task toqueue
			TaskQueue.push(H_TASK);

			//task queue.

			switch ( TaskQueue.peek() )
			{

			case H_TASK:
				{

					break;
				}

			case H_IK:          // Do InverseKinemetics.
				{
					IK();
					break;
				}

			case H_IKER: // Inverse Kinemetics Error.
				{
					while ( !Parameter.isEmpty() )
					{
						// Go troughtparameters.
						/*  
						#      W
						#      I
						#      P
						*/
					}

					break;
				}

			case H_ATPC: // Angel to pulse length converter.
				{
					ConvertAngleToPulseLength();
					DataReady = true;
					break;
				}

			case H_SEND:
				{
					if (DataReady)
					{
						// Send
						DataReady = false;
						// Get New Move
					}
					else
					{
						PcCommand = PC_NEEDDATA;
					}
					break;
				}

			default:     // TaskList was empty.
				{
					// Do something when there is no task.
					PcCommand = PC_EMPTY;
					break;
				}
			}


			//Program Control.
			ProgramControl( PcCommand );
		} while (1);
	}
};

#endif