void setup() {
  size(600, 600, P3D);
  background(0);
}

Custom c1 = new Custom();
Custom c2 = new Custom();
Custom c3 = new Custom();
Custom c4 = new Custom();
Custom c5 = new Custom();

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

PVector point1 = new PVector(8, 0, 20);
PVector point2 = new PVector(12, 0, 0);
PVector point3 = new PVector(8, 0, -20);
PVector point4 = new PVector(-8, 0, -20);
PVector point5 = new PVector(-12, 0, 0);
PVector point6 = new PVector(-8, 0, 20);

float rotationX=0;
float rotationY=0;
float rotationZ=0;

int animationStep=0;
int animationSubStep=0;

float i=0;
int i2=0;
int i3=0;
int i4=0;
int i5=0;

void draw()
{  
  background(0);

  translate(width/2, height/2, 0);

  switch(/*i5*/2)
  {
  case 0:
    rotateX(-1*(PI/10)); 
    rotateY(radians(-210));
    break;
  case 1:
    rotateY(i);
    break;
  case 2:
    rotateX(radians(-90));
    rotateY(radians(180));
    break;
  default:
    i5=0;
  }

  float rotationRange = 8;

  //rotationX = ( i3%(rotationRange*4) < (rotationRange*2) ? -rotationRange+i3%(rotationRange*2) : rotationRange+(-1*(i3%(rotationRange*2))) );
  rotationY = ( i4%(rotationRange*4) < (rotationRange*2) ? -rotationRange+i4%(rotationRange*2) : rotationRange+(-1*(i4%(rotationRange*2))) );
  //rotationZ = ( i2%(rotationRange*4) < (rotationRange*2) ? -rotationRange+i2%(rotationRange*2) : rotationRange+(-1*(i2%(rotationRange*2))) );

  if (c2.delay(25))
  {
    i-=0.012f;
    i2++;
  }
  if (c3.delay(50))
  {
    i3++;
  }
  if (c4.delay(75))
  {
    i4++;
  }
  if (c5.delay(5000))
  {
    i5++;
  }

  scale(6);

  noStroke();  //  Ground
  fill(128);
  beginShape();  
  Vertex(-60, 15, -60);
  Vertex(60, 15, -60);
  Vertex(60, 15, 60);
  Vertex(-60, 15, 60);
  Vertex(-60, 15, -60);
  endShape();

  drawTestSpot(new PVector((Animation(2).x + point1.x), 
  15, 
  (Animation(2).z - point1.z)), false);
  drawTestSpot(new PVector((Animation(3).x + point2.x), 
  15, 
  (Animation(3).z - point2.z)), false);
  drawTestSpot(new PVector((Animation(4).x + point3.x), 
  15, 
  (Animation(4).z - point3.z)), false);
  drawTestSpot(new PVector((Animation(1).x - point4.x), 
  15, 
  (Animation(1).z - point4.z)), true);
  drawTestSpot(new PVector((Animation(0).x - point5.x), 
  15, 
  (Animation(0).z - point5.z)), true);
  drawTestSpot(new PVector((Animation(5).x - point6.x), 
  15, 
  (Animation(5).z - point6.z)), true);

  //  Rotate body
  rotateX(radians(-1*rotationX));
  rotateY(radians(-1*rotationY));
  rotateZ(radians(-1*rotationZ));

  stroke(100, 100, 100);  //  Body
  fill(0, 0, 255);
  beginShape();  
  Vertex(point1.x, point1.y, point1.z);
  Vertex(point2.x, point2.y, point2.z);
  Vertex(point3.x, point3.y, point3.z);
  Vertex(point4.x, point4.y, point4.z);
  Vertex(point5.x, point5.y, point5.z);
  Vertex(point6.x, point6.y, point6.z);
  Vertex(point1.x, point1.y, point1.z);
  endShape(); 
  stroke(100, 100, 100);  //  Body top
  fill(0, 255, 0);
  beginShape();  
  Vertex(point1.x, point1.y, point1.z);
  Vertex(point2.x, point2.y, point2.z);
  Vertex(point5.x, point5.y, point5.z);
  Vertex(point6.x, point6.y, point6.z);
  Vertex(point1.x, point1.y, point1.z);
  endShape(); 

  //  Animation
  if (c1.delay(1))
  {
    animationSubStep++;
  }
  
  animationSubStep = 0;  //  REMOVE
  
  foot1.SetFootCoordinates(Animation(2));
  foot2.SetFootCoordinates(Animation(3));
  foot3.SetFootCoordinates(Animation(4));//
  foot4.SetFootCoordinates(Animation(1));//
  foot5.SetFootCoordinates(Animation(0));
  foot6.SetFootCoordinates(Animation(5));

  foot1.Translate(point1);  
  foot1.DrawFoot();

  foot2.Translate(point2);  
  foot2.DrawFoot();

  foot3.Translate(point3);  
  foot3.DrawFoot();

  foot4.Translate(point4);
  foot4.leftLegorRight=true;
  foot4.DrawFoot();

  foot5.Translate(point5);
  foot5.leftLegorRight=true;
  foot5.DrawFoot();

  foot6.Translate(point6);
  foot6.leftLegorRight=true;
  foot6.DrawFoot();
}

void drawTestSpot(PVector p, boolean leftLegorRight)
{
  beginShape();
  stroke(0, 0, 255);
  if (leftLegorRight)
  {
    Vertex((-1*p.x)-1, p.y, (-1*p.z)-1);
    Vertex((-1*p.x)-1, p.y, (-1*p.z)+1);
    Vertex((-1*p.x)+1, p.y, (-1*p.z)+1);
    Vertex((-1*p.x)+1, p.y, (-1*p.z)-1);
    Vertex((-1*p.x)-1, p.y, (-1*p.z)-1);
  } else
  {
    Vertex(p.x-1, p.y, (-1*p.z)-1);
    Vertex(p.x-1, p.y, (-1*p.z)+1);
    Vertex(p.x+1, p.y, (-1*p.z)+1);
    Vertex(p.x+1, p.y, (-1*p.z)-1);
    Vertex(p.x-1, p.y, (-1*p.z)-1);
  }
  endShape();
}

PVector []walk1 = new PVector[6];

PVector Animation(int addition)
{
  walk1[0] = new PVector(12, 15, -5);
  walk1[1] = new PVector(12, 15, 0);
  walk1[2] = new PVector(12, 15, 5);
  walk1[3] = new PVector(12, 10, 5);
  walk1[4] = new PVector(12, 10, 0);
  walk1[5] = new PVector(12, 10, -5);

  PVector P = new PVector();

  P = animationSubSteps(walk1[((animationStep+addition)%6)], walk1[((animationStep+addition+1)%6)]);

  if (false)
  {
    P.x=10;
    P.y=15;
    P.z=0;
  }

  return P;
}

PVector animationSubSteps(PVector from, PVector to)
{
  PVector newStep = new PVector();
  int steps=5;

  if (animationSubStep < steps)
  {
    float differenceX = (to.x - from.x) / steps;
    float differenceY = (to.y - from.y) / steps;
    float differenceZ = (to.z - from.z) / steps;

    newStep.x = from.x + ( differenceX * animationSubStep );
    newStep.y = from.y + ( differenceY * animationSubStep );
    newStep.z = from.z + ( differenceZ * animationSubStep );
  } else
  {
    animationSubStep=0;
    animationStep++;
    newStep = to;
  }

  return newStep;
}

void Vertex(double x, double y, double z)
{
  vertex((float)x, (float)y, (float)z);
}

class Custom
{
  int time;
  boolean start=true;
  public boolean delay(int delay)
  {
    if (start)
    {
      time = millis();
      start = false;
    }
    if (millis() - time >= delay)
    {
      start = true;
      return true;
    } else
      return false;
  }
}

class Foot
{
  float TranslateX, TranslateY, TranslateZ;
  boolean leftLegorRight=false;

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

  void Translate(int x, int y, int z)
  {
    TranslateX = x;  
    TranslateY = y;
    TranslateZ = z;
  }

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

  void DrawFoot()
  {
    setleg(leftLegorRight);

    pushMatrix();

    translate(TranslateX, TranslateY, TranslateZ);

    stroke(!error?255:0, !error?0:255, 0);
    noFill();
    beginShape();

    Vertex(0, 0, 0);
    Vertex(0, 0, 0);
    if (leftLegorRight)
      Vertex(-1*X2, -1*Y2, Z2);
    else
      Vertex(X2, -1*Y2, Z2);

    endShape(); 

    beginShape();

    if (leftLegorRight)
    {
      Vertex(-1*X2, -1*Y2, Z2);
      Vertex(-1*X2, -1*Y2, Z2);
      Vertex(-1*X, Y, -1*Z);
    } else
    {
      Vertex(X2, -1*Y2, Z2);
      Vertex(X2, -1*Y2, Z2);
      Vertex(X, Y, -1*Z);
    }

    endShape();       

    popMatrix();
  }

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

  void RotateX()
  {
    if (leftLegorRight)
    {
      double yt = Y + TranslateY;
      double zt = Z + TranslateZ;

      double NY = yt * cos(radians(rotationX)) - zt * sin(radians(rotationX));
      double NZ = yt * sin(radians(rotationX)) + zt * cos(radians(rotationX));

      Y = NY - TranslateY;
      Z = NZ - TranslateZ;
    } else
    {
      double yt = Yi + TranslateY;
      double zt = Zi + TranslateZ;

      double NY = yt * cos(radians(rotationX)) - zt * sin(radians(rotationX));
      double NZ = yt * sin(radians(rotationX)) + zt * cos(radians(rotationX));

      Y = NY - TranslateY;
      Z = NZ - TranslateZ;
    }
  }

  void RotateY()
  {
    if (leftLegorRight)
    {  //  Right
      /*double xt = (-1*X) + TranslateX;
      double zt = -1*(Z + TranslateZ);

      double NZ = zt * cos(radians(rotationY)) - xt * sin(radians(rotationY));
      double NX = zt * sin(radians(rotationY)) + xt * cos(radians(rotationY));

      X = -1 * (NX - TranslateX);
      Z = -1*(NZ + TranslateZ);*/
      double xt = (-1*X) + TranslateX;
      double zt = (-1*Z) + TranslateZ;

      double NZ = zt * cos(radians(rotationY)) - xt * sin(radians(rotationY));
      double NX = zt * sin(radians(rotationY)) + xt * cos(radians(rotationY));

      X = -1 * (NX - TranslateX);
      Z = -1 * (NZ - TranslateZ);
    } else
    {
      double xt = X + TranslateX;
      double zt = Z + TranslateZ;

      double NZ = zt * cos(radians(rotationY)) - xt * sin(radians(rotationY));
      double NX = zt * sin(radians(rotationY)) + xt * cos(radians(rotationY));

      X = NX - TranslateX;
      Z = NZ - TranslateZ;
    }
  }

  void RotateZ()
  {
    if (leftLegorRight)
    {
      double xt = (-1*X) + TranslateX;
      double yt = Y + TranslateY;

      double NX = xt * cos(radians(rotationZ)) - yt * sin(radians(rotationZ));
      double NY = xt * sin(radians(rotationZ)) + yt * cos(radians(rotationZ));

      X = -1 * (NX - TranslateX);
      Y = NY - TranslateY;
    } else
    {
      double xt = X + TranslateX;
      double yt = Y + TranslateY;

      double NX = xt * cos(radians(rotationZ)) - yt * sin(radians(rotationZ));
      double NY = xt * sin(radians(rotationZ)) + yt * cos(radians(rotationZ));

      X = NX - TranslateX;
      Y = NY - TranslateY;
      Z = Zi;
    }
  }

  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 O1 ()
  {
    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.
  }

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

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

    double t1 = 0;

    double s2 = s * s;

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

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

    double ret = 0;

    ret = t1 + abs((float)(t2));

    return ret;
  }

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

  double PI = 3.14159265;

  void setleg (boolean Side)
  {
    X = Xi;
    Y = Yi;
    Z = Zi;

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

    double a = (90 - degrees((float)(O1())));
    double b = (180 - degrees((float)(T1())));
    double c = degrees((float)(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;
    }
  }
}