+#include <limits.h>
 #include <Servo.h>
 
-#define DELAY 3
+#define DELAY 2
 
 struct Joint
 {
-    const unsigned int pin;
-    const unsigned int posMin;
-    const unsigned int posMiddle;
-    const unsigned int posMax;
-    const unsigned int distMinToMiddle;
-    const unsigned int distMaxToMiddle;
+    enum Position
+    {
+        POS_MIN,
+        POS_MIDDLE,
+        POS_MAX
+    };
+    
+    const int pin;
+    const int posMin;
+    const int posMiddle;
+    const int posMax;
     Servo servo;
 
-    unsigned int currentPos;
-
     Joint(
-        const unsigned int pin, 
-        const unsigned int posMin, 
-        const unsigned int posMiddle, 
-        const unsigned int posMax) :
+        const int pin, 
+        const int posMin, 
+        const int posMiddle, 
+        const int posMax) :
         pin(pin), 
         posMin(posMin), 
         posMiddle(posMiddle), 
-        posMax(posMax),
-        distMinToMiddle(posMiddle-posMin),
-        distMaxToMiddle(posMax-posMiddle),
-        currentPos(0)
+        posMax(posMax)
     {
     }
 

    {
        this->servo.attach(this->pin);
        this->servo.writeMicroseconds(this->posMiddle);
        this->currentPos = this->posMiddle;
    }

    void dec(unsigned int steps = 1)
    {
        this->currentPos -= steps;
        this->servo.writeMicroseconds(this->currentPos);
    }

    void inc(unsigned int steps = 1)
    {
        this->currentPos += steps;
        this->servo.writeMicroseconds(this->currentPos);
    }
};

Joint rightAnkle(5, 810, 1510, 1860);
Joint leftAnkle(4, 1140, 1490, 2190);
Joint rightHip(3, 950, 1300, 1650);
Joint leftHip(2, 1150, 1500, 1850);

/*
Joint rightAnkle(5, 950, 1510, 1860);
Joint leftAnkle(4, 1140, 1490, 2190);
Joint rightHip(3, 950, 1290, 1650);
Joint leftHip(2, 1100, 1500, 1930);
*/

void stepForward();

void setup() {
    rightAnkle.init();

    leftHip.init();
}

bool started = false;

void loop()
{
    int i = 0;

    if (!started)
    {
        Move startLeanLeft[] = { 
            Move(&rightAnkle, Joint::POS_MIDDLE, Joint::POS_MIN), 
            Move(&leftAnkle, Joint::POS_MIDDLE, Joint::POS_MIN)
        };

        delay(5000);

        moveJoints(2, startLeanLeft);

        started = true;
    }

    stepForward();
}

