// Hexapod FireFly

#include "Header.h"
#include "Wire.h" // Just dont work's, if you put it in the Header file.

// Setup.
void setup()
{
  chuck.begin();
  chuck.update();
  smin[0] = 600;
  smin[1] = 960;
  smin[2] = 600;
  smax[0] = 2400;
  smax[1] = 2100;
  smax[2] = 2400;
  l1 = 10;
  l2 = 15;
  pinMode(13, OUTPUT); // Led L
  Serial.begin(38400);
  body.setup();
  FireFly();
}

// Position.
int Position=1;
int PosMax=15;
int BeginPos=0;
int Directory=0;

void UpdatePosition(int i=0)
{
  if (i == 0)
  {
    if ((Position < PosMax) && (Directory == 0))
      Position++;
    else if (Directory == 0)
      Directory=1;
    else if ((Position > 1) && (Directory == 1))
      Position--;
    else
      Directory=0;
  }
  else
  {
    if (Position < PosMax)
      Position++;
    else
      Position = BeginPos;

    if (Directory == 1)
    {
      Directory = 2;
      Position = BeginPos; 
    }
  }
}

/*==============================================/
 
 Go Use Class::operator=, To give Data From IK Class To Rotation or SSC32 Class.
 
 /==============================================*/

// Variables.
double past = millis();
int Watchdog=0;

// Main loop.
void loop()
{
  if (millis() < past) // to handle te exception of 50 day overflow of millis()
    past = millis();

  while ( ((millis() - past) <  walktime) || (Watchdog == 0))
  {
    /* chuck.update();
     ojx = chuck.readJoyX();
     ojy = chuck.readJoyY();
     oax = chuck.readAccelX();
     oay = chuck.readAccelY();
     
     // Mode Head Servo.
     body.MoveHead(chuck.readRoll());*/

    if (Watchdog == 0)
    {
      walktime = 300;

      if (Directory==0)
      {
        UpdatePosition();

        for (int i=0; i<6; i++)
        {
          COR_2_IK[0+(i*3)] = 10; //x
          COR_2_IK[1+(i*3)] = Position;  //y
          COR_2_IK[2+(i*3)] = 0;  //z
        }
        COR_2_IK[2+(0*3)] = -10;// x side body
        COR_2_IK[2+(2*3)] = 10;///
        COR_2_IK[2+(3*3)] = -10;//
        COR_2_IK[2+(5*3)] = 10;///
        COR_2_IK[0+(1*3)] = 10;// z side body
        COR_2_IK[0+(4*3)] = 10;
      }
      else
      {
        PosMax = 7;
        UpdatePosition(1);
        for (int i2=0; i2<18; i2++)
        {
          COR_2_IK[i2] = TriPoD[Position][i2];
          COR_2_IK[i2] = TriPoD[Position][i2];
        }
        for(int i=0; i<6; i++)
        {
          // zij, laag, achter
          // 10, 15, -10,   15, 15, 0,   10, 15, 10,   10, 15, -10,   15, 15, 0,   10, 15, 10;
          int cor[6] = {  -5, 0, 5, -5, 0, 5    };
          COR_2_IK[2+(i*3)] += cor[i]; 
        }
      }

      /* orbit.getAngle(90 + (chuck.readRoll()/3), 90);// 90 + (chuck.readPitch()/3));
       orbit.getData(x, y, z);
       orbit.calculate();*/

      //                  x, y, z
      // x, z is above the leg, z is depth
      // y, l3 is side leg, y is height
      ik.setlegs();
      body.Render();  

      Serial.println("Done Rendering");
      /*if (ik.CheckForErrorCor() != 0)
      {
        warning();
        Serial.println("Coordinate Error. ");
        Serial.println(ik.CheckForErrorCor_print());
      }
      else*/
        Watchdog = 1;
    }
  }

  past = millis(); // gifs an overflow after 50 day run.
  Serial.println("Out of loop");

  if (Watchdog == 1)
  {
    if (ik.CheckForErrorAng() == 0) // check if there is no error.
    {
      Serial.println("Sending");
      body.toServos();
    }
    else
    {
      warning();
      Serial.println("Axis Error.");
    }
    Watchdog = 0;
  }
}