// Controlling multiple servo motors, 

#include <slowServo.h>
#include <mechButton.h>

#define MS_DEG 30

// -------------------------------------------------------------

enum           { left, right, turn };
enum actions   { open, close };

const uint8_t lockAngle [][2] = {{90, 30}, {90, 150}, {0, 180}};         // left {open, closed} , right {open, closed} , turn {open, closed}

// -------------------------------------------------------------

enum buttonNames       { turnL, turnR, openA, closeA, openB, closeB };
mechButton button [] = {    13,    10,     3,      2,     8,      9 };   // button pins


// -------------------------------------------------------------

enum servoNames         { TL, TR, AL, AR, BL, BR };            // renamed A1 to AL because of conflict with analog pin A1
slowServo servo    [] = { 12, 11,  5,  4,  6,  7 };            // array of servo objects (pin numbers)
uint8_t servoState [] = {  0,  0,  0,  0,  0,  0 };            // only used for the turn servos

// -------------------------------------------------------------



void f0() {  }
void f1() {  }
void f2() {  }
void f3() {  }

// https://stackoverflow.com/questions/1952175/how-can-i-call-a-function-using-a-function-pointer
typedef void (*func_ptr)();
func_ptr function[4] = {f0, f1, f2, f3};  // array of functions




void turnLock(uint8_t srv)
{
  if (!button[srv].getState())
  {
    servoState[srv] = servoState[srv] == 0 ? 1 : 0;
    servo[srv].setDeg( lockAngle[turn][servoState[srv]] );
  }
}

void turnLeft () {  turnLock(TL); }
void turnRight() {  turnLock(TR); }

// -------------------------------------------------------------

void moveLock(uint8_t srvo, uint8_t action)
{
  servo[srvo    ].setDeg(lockAngle[left ][action]);
  servo[srvo + 1].setDeg(lockAngle[right][action]);
}

void closeLockA() {  moveLock(AL, close); }
void closeLockB() {  moveLock(BL, close); }
void  openLockA() {  moveLock(AL, open ); }
void  openLockB() {  moveLock(BL, open ); }

// -------------------------------------------------------------

void setup()
{
  Serial.begin(115200);

  button[turnL].setCallback(turnLeft );
  button[turnR].setCallback(turnRight);

  for ( uint8_t i = TL; i<=TR; i++)
  {
    servo[i].begin();
    servo[i].setDeg(lockAngle[turn][close]);

    sleep(1000);

    servo[i].setMsPerDeg(MS_DEG);
  }
// -------------------------------------------------------------
  button[ openA].setCallback( openLockA);
  button[closeA].setCallback(closeLockA);
  button[ openB].setCallback( openLockB);
  button[closeB].setCallback(closeLockB);

  closeLockA();    sleep(1000);
  closeLockB();    sleep(1000);

  for ( uint8_t i = AL; i<=BR; i++)
  {
    servo[i].begin();
    servo[i].setMsPerDeg(MS_DEG);
  }
}

void loop()
{
  idle();
}