// Converter.cpp
//  V3.0

#include "Converter.h"

void Converter::setup()
{
  //Serial.begin(9600);
}

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

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

#ifdef DEBUGS
  for (int i=0; i<6; i++)
  {
    if (i != 0)
      Serial.print(", ");
    else
      Serial.print("### Servos->mSend ServosAngles Are: ");

    Serial.print(DataLine->ServoData[i].o1);
    Serial.print(", ");
    Serial.print(DataLine->ServoData[i].o2);
    Serial.print(", ");
    Serial.print(DataLine->ServoData[i].o3);
  }
  Serial.println(";");
#endif
}

void Converter::ConvertHead(struct Data *DataLine)
{
  DataLine->HeadPosition = ConverttoMS((90 + DataLine->HeadAngle), smin3, smax3);

#ifdef DEBUGC
  Serial.print("### Converter->HeadPosition is: ");
  Serial.println(DataLine->HeadPosition);
#endif
}

void Converter::Convert(Data *DataLine)
{
#ifdef DEBUGC
  Serial.println("### Converter->Converter ###");
#endif

  ConvertServos(DataLine);
}