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

//	9.5
//	14
//	9.5

//	10
//	10

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

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

boolean robotStanding = false;

float robotHeigth = 12;

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();
}

/*  //  Animation old.
 PVector Animation(int addition)
 {
 return animationSubSteps(walk1[((animationStep+addition)%6)], walk1[((animationStep+addition+1)%6)]);
 }
 
 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;
 }
 */


PVector []walk1 = new PVector[6];

//  Animation new
int animationStep=0;
int animationSubStep=0;
boolean animationRepeat = false;
boolean animationRepeatComplete = false;
int animationSpeed = 20;
PVector animationOld;

PVector Animation(PVector from, PVector to)
{
  PVector[] newAnimation = new PVector[2];
  newAnimation[0] = from;
  newAnimation[1] = to;
  return Animation(newAnimation, 2, 0);
}

PVector Animation(PVector[] animation, int animationLength, int addition)
{
  PVector newStep = new PVector();

  PVector from = animation[((animationStep+addition) % animationLength)];
  PVector to = animation[((animationStep+addition+1) % animationLength)];

  /*if (!((animationStep >= animationLength - 1) && !animationRepeat))
   {
   if (timerAnimation.delay(animationSpeed))
   {
   animationSubStep++;
   }
   } else
   {
   return animationOld;
   }*/
  if ( (animationRepeatComplete) || ((animationStep >= animationLength - 1) && !animationRepeat) )
  {
    return animationOld;
  } else {
    /*if (timerAnimation.delay(animationSpeed))
     {
     animationSubStep++;
     }*/

    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;

      if ((animationStep >= animationLength - 2) && !animationRepeat)
      {
        animationRepeatComplete = true;
        animationStep = 0;
        animationOld = to;
        return to;
      } else if (!animationRepeatComplete)
      {
        animationStep++;
      }
    }

    return newStep;
  }
}

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

class CustomTimer
{
  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;
  }
}

void DrawFeet()
{
  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 RobotWalk()
{
  if (robotStanding)
  {
    if (!animationRepeatComplete)
      if (timerAnimation.delay(animationSpeed))
        animationSubStep++;

    animationRepeat = true;
    animationSpeed = 20;
    foot1.SetFootCoordinates(Animation(walk1, 6, 2));  //  BULLSHIT BUG
    foot1.SetFootCoordinates(Animation(walk1, 6, 2));
    foot2.SetFootCoordinates(Animation(walk1, 6, 3));
    foot3.SetFootCoordinates(Animation(walk1, 6, 4));//
    foot4.SetFootCoordinates(Animation(walk1, 6, 1));//
    foot5.SetFootCoordinates(Animation(walk1, 6, 0));
    foot6.SetFootCoordinates(Animation(walk1, 6, 5));
  }
}

void DrawGround()
{
  noStroke();  //  Ground
  fill(128);
  beginShape();  
  Vertex(-60, robotHeigth, -60);
  Vertex(60, robotHeigth, -60);
  Vertex(60, robotHeigth, 60);
  Vertex(-60, robotHeigth, 60);
  Vertex(-60, robotHeigth, -60);
  endShape();
}

void DrawTestSpots()
{
  /*drawTestSpot(new PVector((Animation(walk1, 6, 2).x + point1.x), 
   robotHeigth, 
   (Animation(walk1, 6, 2).z - point1.z)), false);
   drawTestSpot(new PVector((Animation(walk1, 6, 3).x + point2.x), 
   robotHeigth, 
   (Animation(walk1, 6, 3).z - point2.z)), false);
   drawTestSpot(new PVector((Animation(walk1, 6, 4).x + point3.x), 
   robotHeigth, 
   (Animation(walk1, 6, 4).z - point3.z)), false);
   drawTestSpot(new PVector((Animation(walk1, 6, 1).x - point4.x), 
   robotHeigth, 
   (Animation(walk1, 6, 1).z - point4.z)), true);
   drawTestSpot(new PVector((Animation(walk1, 6, 0).x - point5.x), 
   robotHeigth, 
   (Animation(walk1, 6, 0).z - point5.z)), true);
   drawTestSpot(new PVector((Animation(walk1, 6, 5).x - point6.x), 
   robotHeigth, 
   (Animation(walk1, 6, 5).z - point6.z)), true);*/
}

void DrawBody()
{
  if (!robotStanding)
  {
    rotationX = 0;
    rotationY = 0;
    rotationZ = 0;
  }
  //  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(); 

  RobotStandup();

  RobotWalk();

  DrawFeet();
}

PVector []standup = new PVector[2];
int standupStep = 0;

void RobotStandup()
{  
  standup[0] = new PVector(5, 6, 0);
  standup[1] = new PVector(10, 6, 0);

  if (!robotStanding)
  {
    if (!animationRepeatComplete)
      if (timerAnimation.delay(animationSpeed))
        animationSubStep++;

    animationRepeat = false;
    animationSpeed = 250;

    switch (standupStep)
    {
    case 0:
      foot1.SetFootCoordinates(Animation(standup, 2, 0));
      foot2.SetFootCoordinates(Animation(standup, 2, 0));
      foot3.SetFootCoordinates(Animation(standup, 2, 0));//
      foot4.SetFootCoordinates(Animation(standup, 2, 0));//
      foot5.SetFootCoordinates(Animation(standup, 2, 0));
      foot6.SetFootCoordinates(Animation(standup, 2, 0));

      if (animationRepeatComplete)
      {
        animationRepeatComplete = false;
        standupStep = 1;
        animationStep = 0;
      }
      break;

    case 1:
      foot1.SetFootCoordinates(Animation(standup[1], walk1[2]));
      foot2.SetFootCoordinates(Animation(standup[1], walk1[3]));
      foot3.SetFootCoordinates(Animation(standup[1], walk1[4]));//
      foot4.SetFootCoordinates(Animation(standup[1], walk1[1]));//
      foot5.SetFootCoordinates(Animation(standup[1], walk1[0]));
      foot6.SetFootCoordinates(Animation(standup[1], walk1[5]));

      if (animationRepeatComplete)
      {
        robotStanding = true;
        animationRepeatComplete = false;
        standupStep = 2;
      }
      break;
    }
  }
}