// Hexapod FireFly

#include "hexapod.h"
#include "Data.h"
#include "Converter.h"
#include "InverseKinemetics.h"
#include "Servos.h"
#include "Watchdog.h"
#include "Ultrasone.h"

Data *DataLine = new Data;
Converter Con;
InverseKinemetics IK;
Servos SSC32;
Watchdog WD;
Ultrasone US;

void setup()
{
  Serial.begin(38400);
  Serial1.begin(38400);
  pinMode(13, OUTPUT);  
  DataLine->WalkTime = 100;
  DataLine->FireFly.FireFly();
}

int a=0;
int b=6;
int tt;
byte Sit;
byte SitC;

// Main loop.
void loop()
{
  int d = DataLine->FireFly.GetNewCmd();

  US.GetRadar(DataLine);

  /*if(Sit == 1)
   if (SitC <= millis())
   {
   Sit = 0;
   DataLine->WalkTime = tt;
   } */

  if (d != 0)
  {
    switch(d)
    {
    case power:
      DataLine->ServoData[0](5, 3, 8);
      DataLine->ServoData[1](8, 3, 0);
      DataLine->ServoData[2](5, 3, -8);
      DataLine->ServoData[3](5, 3, 8);
      DataLine->ServoData[4](8, 3, 0);
      DataLine->ServoData[5](5, 3, -8); 
      break;
    case up:  
      Serial.println("UP");
      for (int i=a; i<=b; i++)
        DataLine->ServoData[i].y -= 1;
      break;
    case down:  
      Serial.println("DOWN");
      for (int i=a; i<=b; i++)
        DataLine->ServoData[i].y += 1;
      break;
    case left:  
      Serial.println("LEFT");
      for (int i=a; i<=b; i++)
      {
        if (i<3)
          DataLine->ServoData[i].x += 1;
        else
          DataLine->ServoData[i].x -= 1;
      }
      break;
    case right:  
      Serial.println("RIGHT");
      for (int i=a; i<=b; i++)
      {
        if (i<3)
          DataLine->ServoData[i].x -= 1;
        else
          DataLine->ServoData[i].x += 1;
      }
      break;
    case forward:  
      Serial.println("forward");
      for (int i=a; i<=b; i++)
        DataLine->ServoData[i].z -= 1;
      break;
    case back:  
      Serial.println("back");
      for (int i=a; i<=b; i++)
        DataLine->ServoData[i].z += 1;
      break;
    case ok:  
      Serial.println("OK");

      tt = DataLine->WalkTime;
      DataLine->WalkTime = 3000;

      DataLine->ServoData[0](5, 3, 8);
      DataLine->ServoData[1](8, 3, 0);
      DataLine->ServoData[2](5, 3, -8);
      DataLine->ServoData[3](5, 3, 8);
      DataLine->ServoData[4](8, 3, 0);
      DataLine->ServoData[5](5, 3, -8);

      IK.IK(DataLine);
      Con.Convert(DataLine);
      DataLine->Send = 1;
      SSC32.Send(DataLine);

      Sit = 1;
      break;
    case IR_0:  
      Serial.println("0");
      a = 0;
      b = 5;
      break;
    case IR_1:  
      Serial.println("1");
      a = 0;
      b = 0;
      break;
      /*case IR_2:  
       Serial.println("OK");
       a = ;
       b = ;
       break;*/
    case IR_3:  
      Serial.println("3");
      a = 3;
      b = 3;
      break;
    case IR_4:  
      Serial.println("4");
      a = 1;
      b = 1;
      break;
      /*case IR_5:  
       Serial.println("OK");
       a = ;
       b = ;
       break;*/
    case IR_6:  
      Serial.println("6");
      a = 4;
      b = 4;
      break;
    case IR_7:  
      Serial.println("7");
      a = 2;
      b = 2;
      break;
      /*case IR_8:  
       Serial.println("OK");
       a = ;
       b = ;
       break;*/
    case IR_9:  
      Serial.println("9");
      a = 5;
      b = 5;
      break;
    default:  
      Serial.println(d);
      break;
    }
  }
  IK.IK(DataLine);
  Con.Convert(DataLine);
  WD.CheckTime(DataLine);

  if (Sit){
    if ((DataLine->Send) & (SitC))
    {
      DataLine->WalkTime = tt;
      SitC = 0;
      Sit = 0;
    }
    else if (DataLine->Send)
      SitC = 1;
  }

  SSC32.Send(DataLine);
}