// Version 1

#ifndef Foot_H
#define Foot_H

#include "Vector.h";
#include "Global.h"

const double pi = 3.14159265359;

class Foot
{
  public:
    float TranslateX, TranslateY, TranslateZ;
    boolean Invert;

    void SetFootCoordinates(Vector P)
    {
      Xi = P.x;
      Yi = P.y;
      Zi = P.z;
    }

    // ?used to put in position from body?
    void Translate(int x, int y, int z)
    {
      TranslateX = x;
      TranslateY = y;
      TranslateZ = z;
    }

    void Translate(Vector i)
    {
      TranslateX = i.x;
      TranslateY = i.y;
      TranslateZ = i.z;
    }

    //  Inverse Kinemetics Region
    double l3, s, O1, O2, O3, X2, Y2, Z2;
    double l1, l2;
    double Xi, Yi, Zi;
    double X, Y, Z;
    boolean error;

    void RotateX()
    {
      Y = Y * cos(radians(rotationX)) - Z * sin(radians(rotationX));
      Z = Y * sin(radians(rotationX)) + Z * cos(radians(rotationX));
    }

    void RotateY()
    {
      Z = Z * cos(radians(rotationY)) - X * sin(radians(rotationY));
      X = Z * sin(radians(rotationY)) + X * cos(radians(rotationY));
    }

    void RotateZ()
    {
      X = X * cos(radians(rotationZ)) - Y * sin(radians(rotationZ));
      Y = X * sin(radians(rotationZ)) + Y * cos(radians(rotationZ));
    }

    void RL3 ()
    {
      double x = X;
      double z = Z;

      if ((x == 0) && (z == 0))
        l3 = 0;
      else l3 = sqrt((float)(x * x + z * z));

      if (x < 0)
        l3 = l3 * -1;
    }

    float FO1 ()
    {
      RL3();

      double x = X;
      double z = Z;
      double r;

      if ((x != 0) & (z != 0)) {
        double a = (z / x);
        return atan((float)(a));
      }
      else
        return atan(0.0); // Math Error Protection.
    }

    float T1 ()
    {
      float x = X;
      float y = Y;
      float z = Z;
      float x2 = x * x;
      float y2 = y * y;
      float z2 = z * z;
      float l32 = l3 * l3;

      s = sqrt((float)(x2 + y2 + z2));

      float t1 = 0;

      float s2 = s * s;

      if ((y == 0) && (l3 == 0))
        t1 = 0; // Math Error Protection.
      else
        t1 = (atan2((float)(l3), (float)(y)));

      float a = (l1 * l1) - (l2 * l2) + s2;
      float b = 2 * l1 * s;
      float t2 = acos(a / b);

      float ret = 0;

      ret = t1 + abs(t2);

      return ret;
    }

    float T3 ()
    {
      return (acos((float)(((l1 * l1) + (l2 * l2) - (s * s)) / (2 * l1 * l2))));
    }

    void setleg()
    {
      setleg(Invert);
    }

    void setleg(boolean Side)
    {
      l1 = 10;
      l2 = 15;

      X = Xi;
      Y = Yi;
      Z = Zi;

      //  Rotation from body point
      if (Invert)
      { //  Right
        X = (-1 * X) + TranslateX;
        Z = (-1 * Z) + TranslateZ;
      }
      else
      {
        X = Xi + TranslateX;
        Z = (-1 * Z) + TranslateZ;
      }

      RotateY();
      RotateX();
      RotateZ();

      //  Remove gaits
      if (Invert)
      { //  Right
        X = -1 * (X - TranslateX);
        Z = -1 * (Z - TranslateZ);
      }
      else
      {
        X -= TranslateX;
        Z = -1 * (Z - TranslateZ);
      }

      if (Invert)
      { //  right
        double a = (90 - degrees(FO1()));
        double b = (180 - degrees(T1()));
        double c = degrees(T3());

        /*if ((a >= 15) & (a <= 165)
            & (b >= 15) & (b <= 165)
            & (c >= 15) & (c <= 165))
          {*/
        error = false;

        O1 = a;
        O2 = b;
        O3 = c;

        double d = sin((float)(b * (pi / 180)));
        if ( d != 0 )
          X2 = d * l1;
        else
          X2 = 0;

        d = cos((float)(b * (pi / 180)));
        if ( d != 0 )
          Y2 = d * l1;
        else
          Y2 = 0;

        d = sin((float)((a - 90) * (pi / 180)));
        if ( d != 0 )
          Z2 = d * abs((float)(X2));
        else
          Z2 = 0;
        /*}
          else
          {
          error = true;
          }*/
      }
      else
      {
        double a = (90 - degrees((float)(FO1())));
        double b = degrees((float)(T1()));
        double c = 180 - degrees((float)(T3()));

        /*if ((a >= 15) & (a <= 165)
            & (b >= 15) & (b <= 165)
            & (c >= 15) & (c <= 165))
          {*/
        error = false;

        O1 = a;
        O2 = b;
        O3 = c;

        a = (90 - degrees((float)(FO1())));
        b = (180 - degrees((float)(T1())));
        c = degrees((float)(T3()));

        double d = sin((float)(b * (pi / 180)));
        if ( d != 0 )
          X2 = d * l1;
        else
          X2 = 0;

        d = sin((float)(b * (pi / 180)));
        if ( d != 0 )
          Y2 = d * l1;
        else
          Y2 = 0;

        d = sin((float)((a - 90) * (pi / 180)));
        if ( d != 0 )
          Z2 = d * abs((float)(X2));
        else
          Z2 = 0;
        /*}
          else
          {
          error = true;
          Serial.println("ERROR");
          }*/
      }
    }
};

#endif