// Servos.cpp
//  V3.0

#include "Servos.h"

void Servos::MoveHead (Data *DataLine)
{  
Serial.begin(38400);
	Serial1.begin(38400);

#ifdef DEBUGHS
	Serial.print("### Servos->Head Time/Position: ");
	Serial.print(DataLine->HeadTime);
	Serial.print(", ");
	Serial.println(DataLine->HeadPosition);
#endif

	Serial1.print(" #"); 
	Serial1.print(9); 
	Serial1.print(" P");
	Serial1.print(DataLine->HeadPosition);
	Serial1.print(" T");
	Serial1.println(DataLine->HeadTime);
}

void mSend(Data *DataLine)
{
	int s=3;

	for (int i=0; i<9; i += 3){
		Serial1.print(" #"); 
		Serial1.print(i);
		Serial1.print(" P");
		Serial1.print(DataLine->ServoData[s].t1);
		Serial1.print(" #"); 
		Serial1.print(i+1);
		Serial1.print(" P");
		Serial1.print(DataLine->ServoData[s].t2);
		Serial1.print(" #"); 
		Serial1.print(i+2);
		Serial1.print(" P");
		Serial1.print(DataLine->ServoData[s].t3);
		s++;
	}

	s = 0;

	for (int i=16; i<25; i += 3){
		Serial1.print(" #"); 
		Serial1.print(i);
		Serial1.print(" P");
		Serial1.print(DataLine->ServoData[s].t1);
		Serial1.print(" #"); 
		Serial1.print(i+1);
		Serial1.print(" P");
		Serial1.print(DataLine->ServoData[s].t2);
		Serial1.print(" #"); 
		Serial1.print(i+2);
		Serial1.print(" P");
		Serial1.print(DataLine->ServoData[s].t3);
		s++;
	}

	Serial1.print(" T");
	Serial1.println(DataLine->WalkTime);

#ifdef DEBUGC
	for (int i=0; i<6; i++)
	{
		if (i != 0)
			Serial.print(", ");
		else
		{
			Serial.print("### Converter->ServosAnglesTime Are: ");
		}

		Serial.print(DataLine->ServoData[i].t1);
		Serial.print(", ");
		Serial.print(DataLine->ServoData[i].t2);
		Serial.print(", ");
		Serial.print(DataLine->ServoData[i].t3);
	}
	Serial.println(";");
#endif
}

void Servos::Send(Data *DataLine)
{
#ifdef DEBUGS
	Serial.println("### Servos->Servos ###");
	Serial.println("### Servos Send ###");
	digitalWrite(13, HIGH);
	delay(10);
	digitalWrite(13, LOW);
#endif

#ifndef Freeze
	mSend(DataLine);
#endif
}