#include "SSC32.h"

void SSC32::setup()
{
  Serial1.begin(38400);
  Serial.begin(38400);
  time = 0;
  walktime = 0;
}

void SSC32::MoveHead (int angel_from_mid)
{
  Serial1.print(" #"); 
  Serial1.print(9); 
  Serial1.print(" P");
  Serial1.print(ConverttoMS((90 + angel_from_mid), smin[0], smax[0]));
  Serial1.print(" T");
  Serial1.println(100);
}

void SSC32::Render()
{
  for (int i=0; i<6; i++)
  {
    RAW_2_SERVO[0+(i*3)] = ConverttoMS(DEG_2_CON[0+(i*3)], smin[0], smax[0]);
    RAW_2_SERVO[1+(i*3)] = ConverttoMS(DEG_2_CON[1+(i*3)], smin[1], smax[1]);
    RAW_2_SERVO[2+(i*3)] = ConverttoMS(DEG_2_CON[2+(i*3)], smin[2], smax[2]);
	
    if (i != 0)
      Serial.print(", ");
    else
      Serial.print("Ang: ");
	  
    Serial.print(DEG_2_CON[0+(i*3)]);
    Serial.print(", ");
    Serial.print(DEG_2_CON[1+(i*3)]);
    Serial.print(", ");
    Serial.print(DEG_2_CON[2+(i*3)]);
  }
  Serial.println(";");
}

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

void SSC32::toServos ()
{
  // puss to servos

    // var.    SSC32.
  // 0 | 3  // 16 | 0
  // 1 | 4  //  | | |
  // 2 | 5  // 24 | 8

  for (int i=0; i<9; i++)
  {
    Serial1.print(" #"); 
    Serial1.print(16+i);// 18 22
    Serial1.print(" P");
    Serial1.print(RAW_2_SERVO[9+i]);
  }

  for (int i=0; i<9; i++)
  {
    Serial1.print(" #"); 
    Serial1.print(i); 
    Serial1.print(" P");
    Serial1.print(RAW_2_SERVO[i]);
  }

  Serial1.print(" T");
  Serial1.println(walktime);
}