// Version 1

#ifndef Hexapod_H
#define Hexapod_H

#include "FootClass.h";
#include "Vector.h";
#include "CustomTimer.h";
#include "Global.h"
#include <Streaming.h>

Foot foot1 = Foot();
Foot foot2 = Foot();
Foot foot3 = Foot();
Foot foot4 = Foot();
Foot foot5 = Foot();
Foot foot6 = Foot();

//  Spaces from body to feet
//	9.5
//	14
//	9.5

//	10
//	10

//    LAYOUT HARDWARE
//      _U_
//  1==/   \==4
//     >***<
// 2===|---|===5
//     >***<
//  3==\___/==6

//    LAYOUT SOFTWARE
//      _U_
//  1==/   \==6
//     >***<
// 2===|---|===5
//     >***<
//  3==\___/==4

Vector point1 = Vector(4.75, 0, 10);
Vector point2 = Vector(7, 0, 0);
Vector point3 = Vector(4.75, 0, -10);
Vector point4 = Vector(-4.75, 0, -10);
Vector point5 = Vector(-7, 0, 0);
Vector point6 = Vector(-4.75, 0, 10);

Vector fpoint1 = Vector(10, 5, 0);
Vector fpoint2 = Vector(10, 5, 0); //  y negative is down from body.
Vector fpoint3 = Vector(10, 5, 0);
Vector fpoint4 = Vector(10, 5, 0);
Vector fpoint5 = Vector(10, 5, 0);
Vector fpoint6 = Vector(10, 5, 0);

Vector* standup = new Vector[2];

//  Animation new
int animationSpeed = 20;

int ConverttoMS(double Position, int low, int high, int _min = 15, int _max = 165, int correction = 0)
{
  //Position += 90;

  if (Position < _min | Position > _max)
  {
    Position = 90;
    Serial.println("ERROR");
  }

  int tmp = ( Position * ( ( high - low ) / 180 ) );

  //Serial1.println(Position);

  return low + tmp + correction;
}

void mSend(Foot f1, Foot f2, Foot f3, Foot f4, Foot f5, Foot f6)
{
  int smin0 = 600;
  int smin1 = 960;
  int smin2 = 600;
  int smin3 = 600;
  int smax0 = 2400;
  int smax1 = 2100;
  int smax2 = 2400;
  int smax3 = 2400;

  Serial1.print(" #");
  Serial1.print(0);
  Serial1.print(" P");
  Serial1.print(ConverttoMS(f4.O1, smin0, smax0));
  Serial1.print(" #");
  Serial1.print(1);
  Serial1.print(" P");
  Serial1.print(ConverttoMS(f4.O2, smin1, smax1));
  Serial1.print(" #");
  Serial1.print(2);
  Serial1.print(" P");
  Serial1.print(ConverttoMS(f4.O3, smin2, smax2));
  Serial1.print(" #");
  Serial1.print(3);
  Serial1.print(" P");
  Serial1.print(ConverttoMS(f5.O1, smin0, smax0));
  Serial1.print(" #");
  Serial1.print(4);
  Serial1.print(" P");
  Serial1.print(ConverttoMS(f5.O2, smin1, smax1));
  Serial1.print(" #");
  Serial1.print(5);
  Serial1.print(" P");
  Serial1.print(ConverttoMS(f5.O3, smin2, smax2));
  Serial1.print(" #");
  Serial1.print(6);
  Serial1.print(" P");
  Serial1.print(ConverttoMS(f6.O1, smin0, smax0));
  Serial1.print(" #");
  Serial1.print(7);
  Serial1.print(" P");
  Serial1.print(ConverttoMS(f6.O2, smin1, smax1));
  Serial1.print(" #");
  Serial1.print(8);
  Serial1.print(" P");
  Serial1.print(ConverttoMS(f6.O3, smin2, smax2));

  Serial1.print(" #");
  Serial1.print(9);
  Serial1.print(" P");
  Serial1.print(ConverttoMS(90, smin3, smax3));

  Serial1.print(" #");
  Serial1.print(16);
  Serial1.print(" P");
  Serial1.print(ConverttoMS(f1.O1, smin0, smax0));
  Serial1.print(" #");
  Serial1.print(17);
  Serial1.print(" P");
  Serial1.print(ConverttoMS(f1.O2, smin1, smax1));
  Serial1.print(" #");
  Serial1.print(18);
  Serial1.print(" P");
  Serial1.print(ConverttoMS(f1.O3, smin2, smax2));
  Serial1.print(" #");
  Serial1.print(19);
  Serial1.print(" P");
  Serial1.print(ConverttoMS(f2.O1, smin0, smax0));
  Serial1.print(" #");
  Serial1.print(20);
  Serial1.print(" P");
  Serial1.print(ConverttoMS(f2.O2, smin1, smax1));
  Serial1.print(" #");
  Serial1.print(21);
  Serial1.print(" P");
  Serial1.print(ConverttoMS(f2.O3, smin2, smax2));
  Serial1.print(" #");
  Serial1.print(22);
  Serial1.print(" P");
  Serial1.print(ConverttoMS(f3.O1, smin0, smax0));
  Serial1.print(" #");
  Serial1.print(23);
  Serial1.print(" P");
  Serial1.print(ConverttoMS(f3.O2, smin1, smax1));
  Serial1.print(" #");
  Serial1.print(24);
  Serial1.print(" P");
  Serial1.print(ConverttoMS(f3.O3, smin2, smax2));

  Serial1.print(" T");
  Serial1.println(0);



  /*Serial << " #";
    Serial << 0;
    Serial << " P";
    Serial << f4.O1;
    Serial << " #";
    Serial << 1;
    Serial << " P";
    Serial << f4.O2;
    Serial << " #";
    Serial << 2;
    Serial << " P";
    Serial << f4.O3;
    Serial << " #";
    Serial << 3;
    Serial << " P";
    Serial << f5.O1;
    Serial << " #";
    Serial << 4;
    Serial << " P";
    Serial << f5.O2;
    Serial << " #";
    Serial << 5;
    Serial << " P";
    Serial << f5.O3;
    Serial << " #";
    Serial << 6;
    Serial << " P";
    Serial << f6.O1;
    Serial << " #";
    Serial << 7;
    Serial << " P";
    Serial << f6.O2;
    Serial << " #";
    Serial << 8;
    Serial << " P";
    Serial << f6.O3;

    Serial << " #";
    Serial << 16;
    Serial << " P";
    Serial << f1.O1;
    Serial << " #";
    Serial << 17;
    Serial << " P";
    Serial << f1.O2;
    Serial << " #";
    Serial << 18;
    Serial << " P";
    Serial << f1.O3;
    Serial << " #";
    Serial << 19;
    Serial << " P";
    Serial << f2.O1;
    Serial << " #";
    Serial << 20;
    Serial << " P";
    Serial << f2.O2;
    Serial << " #";
    Serial << 21;
    Serial << " P";
    Serial << f2.O3;
    Serial << " #";
    Serial << 22;
    Serial << " P";
    Serial << f3.O1;
    Serial << " #";
    Serial << 23;
    Serial << " P";
    Serial << f3.O2;
    Serial << " #";
    Serial << 24;
    Serial << " P";
    Serial << f3.O3;

    Serial << " T";
    Serial << 0 << "\n\r";*/
}
//////////////////////////////////////////////////

void DrawFeet()
{
  if (TimerFPS_SERVO.Delay(1000 / FPS_SERVO))
  {
    foot1.Translate(point1);
    foot1.SetFootCoordinates(fpoint1);
    foot1.Invert = true;
    foot1.setleg();

    foot1.Translate(point2);
    foot2.SetFootCoordinates(fpoint2);
    foot2.Invert = true;
    foot2.setleg();

    foot1.Translate(point3);
    foot3.SetFootCoordinates(fpoint3);
    foot3.Invert = true;
    foot3.setleg();

    foot1.Translate(point4);
    foot4.SetFootCoordinates(fpoint4);
    foot4.Invert = false;
    foot4.setleg();

    foot1.Translate(point5);
    foot5.SetFootCoordinates(fpoint5);
    foot5.Invert = false;
    foot5.setleg();

    foot1.Translate(point6);
    foot6.SetFootCoordinates(fpoint6);
    foot6.Invert = false;
    foot6.setleg();

    mSend(foot1, foot2, foot3, foot4, foot5, foot6);
  }
}

/*void RobotWalk()
  {
  if (robotStanding)
  {
    //animationRepeat = true;
    animationSpeed = 20;

    if (!animationRepeatComplete)
      if (timerAnimation.Delay(animationSpeed))
        animationSubStep++;

    Serial1.println("robot walk animation");

    foot1.SetFootCoordinates(Animation(walk1, 8, 0));  //  BULLSHIT BUG
    foot1.SetFootCoordinates(Animation(walk1, 8, 0));
    foot2.SetFootCoordinates(Animation(walk1, 8, 4));
    foot3.SetFootCoordinates(Animation(walk1, 8, 0));//
    foot4.SetFootCoordinates(Animation(walk1, 8, 4));//
    foot5.SetFootCoordinates(Animation(walk1, 8, 0));
    foot6.SetFootCoordinates(Animation(walk1, 8, 4));
  }
  }*/

float _animationProgress = 0;
bool rotation = false;

void RobotStandup()
{
  standup[0] = Vector(10, 5, 0);
  standup[1] = Vector(10, 15, 0);

  if (timerAnimation.Delay(1000 / FPS))
  {
    Serial.println("robot standup animation");

    fpoint1.Lerp(standup[0], standup[1], _animationProgress);
    fpoint2.Lerp(standup[0], standup[1], _animationProgress);
    fpoint3.Lerp(standup[0], standup[1], _animationProgress);
    fpoint4.Lerp(standup[0], standup[1], _animationProgress);
    fpoint5.Lerp(standup[0], standup[1], _animationProgress);
    fpoint6.Lerp(standup[0], standup[1], _animationProgress);

    Serial.println(point1.Print());

    int _speed = 2;

    _animationProgress = _animationProgress < 1.0f ? (_animationProgress + (1.0f / (FPS * _speed))) : 1.0f;

    /*if (_animationProgress == 1)
      {
      int uhh = (millis() / 100) % 100;
      if (rotation == true)
      {
        Serial.println("rotate up");
        rotationZ = map((millis() / 100) % 100, 0, 99, -25, 25);
        if (uhh >= 99)
        {
          rotation = true;
          uhh++;
        }
      }
      else
      {
        Serial.println("rotate upside down");
        rotationZ = map((millis() / 100) % 100, 0, 99, 25, -25);
        if (uhh >= 99)
        {
          rotation = false;
          uhh++;
      }
      }
      }*/ 
  }
}

void DrawBody()
{
  RobotStandup();

  //RobotWalk();

  DrawFeet();
}

#endif