#include <Servo.h>

#define DEBUG
#define DIPSWITCH_VALUE_CLOSE

#define SERVO_FRONT_SX 9
#define SERVO_FRONT_DX 10
#define SERVO_BACK_SX 5
#define SERVO_BACK_DX 6

#define DIP_0 2
#define DIP_1 4
#define DIP_2 8
#define DIP_3 12
#define DIP_4 13

#define BUTTON_PIN 11

#define GREEN_FRONT_LED 21
#define YELLOW_FRONT_LED 20
#define RED_FRONT_LED 17

#define GREEN_BACK_LED 16
#define YELLOW_BACK_LED 15
#define RED_BACK_LED 14

#define OPEN 0
#define REGULATED 90
#ifdef DIPSWITCH_VALUE_CLOSE
  #define CLOSED 180
#else
  #define CLOSED 144
#endif

#define NUMELEMENTS(x) (sizeof(x) / sizeof(x[0]))

const int setup0servoFrontSx[] = {CLOSED, CLOSED, CLOSED, OPEN, OPEN, OPEN, REGULATED, REGULATED, REGULATED};
const int setup0servoFrontDx[] = {CLOSED, CLOSED, CLOSED, OPEN, OPEN, OPEN, REGULATED, REGULATED, REGULATED};
const int setup0servoBackSx[] = {CLOSED, OPEN, REGULATED, OPEN, REGULATED, CLOSED, REGULATED, CLOSED, OPEN};
const int setup0servoBackDx[] = {CLOSED, OPEN, REGULATED, OPEN, REGULATED, CLOSED, REGULATED, CLOSED, OPEN};

const int setup1servoFrontSx[] = {CLOSED, REGULATED, OPEN};
const int setup1servoFrontDx[] = {CLOSED, REGULATED, OPEN};
const int setup1servoBackSx[] = {CLOSED};
const int setup1servoBackDx[] = {CLOSED};

const int setup2servoFrontSx[] = {CLOSED, REGULATED, OPEN, CLOSED, REGULATED, OPEN};
const int setup2servoFrontDx[] = {CLOSED, REGULATED, OPEN, CLOSED, REGULATED, OPEN};
const int setup2servoBackSx[] = {CLOSED, REGULATED, OPEN, CLOSED, REGULATED, OPEN};
const int setup2servoBackDx[] = {CLOSED, REGULATED, OPEN, CLOSED, REGULATED, OPEN};

struct STEPLEDS
{
  int *valueStep;
  int *pinLed;
};

STEPLEDS stepLedServo0[] = {
  {OPEN, GREEN_FRONT_LED},
  {REGULATED, YELLOW_FRONT_LED},
  {CLOSED, RED_FRONT_LED}
};

STEPLEDS stepLedServo1[] = {
  {OPEN, GREEN_BACK_LED},
  {REGULATED, YELLOW_BACK_LED},
  {CLOSED, RED_BACK_LED}
};

struct STEPS
{
  int *stepArray;
  uint16_t numSteps;
  STEPLEDS *stepLedArray;
  uint16_t numStepLeds;
};

STEPS setup0[] = {
  {setup0servoFrontSx, NUMELEMENTS(setup0servoFrontSx), stepLedServo0, NUMELEMENTS(stepLedServo0)},
  {setup0servoFrontDx, NUMELEMENTS(setup0servoFrontDx), stepLedServo0, NUMELEMENTS(stepLedServo0)},
  {setup0servoBackSx, NUMELEMENTS(setup0servoBackSx), stepLedServo1, NUMELEMENTS(stepLedServo1)},
  {setup0servoBackDx, NUMELEMENTS(setup0servoBackDx), stepLedServo1, NUMELEMENTS(stepLedServo1)}
};

STEPS setup1[] = {
  {setup1servoFrontSx, NUMELEMENTS(setup1servoFrontSx), stepLedServo0, NUMELEMENTS(stepLedServo0)},
  {setup1servoFrontDx, NUMELEMENTS(setup1servoFrontDx), stepLedServo0, NUMELEMENTS(stepLedServo0)},
  {setup1servoBackSx, NUMELEMENTS(setup1servoBackSx), stepLedServo1, NUMELEMENTS(stepLedServo1)},
  {setup1servoBackDx, NUMELEMENTS(setup1servoBackDx), stepLedServo1, NUMELEMENTS(stepLedServo1)}
};

STEPS setup2[] = {
  {setup2servoFrontSx, NUMELEMENTS(setup2servoFrontSx), stepLedServo0, NUMELEMENTS(stepLedServo0)},
  {setup2servoFrontDx, NUMELEMENTS(setup2servoFrontDx), stepLedServo0, NUMELEMENTS(stepLedServo0)},
  {setup2servoBackSx, NUMELEMENTS(setup2servoBackSx), stepLedServo1, NUMELEMENTS(stepLedServo1)},
  {setup2servoBackDx, NUMELEMENTS(setup2servoBackDx), stepLedServo1, NUMELEMENTS(stepLedServo1)}
};

struct SETUPS
{
  STEPS *servoArray;
};

SETUPS setupArray[] =
{
  {setup0},
  {setup1},
  {setup2}
};

const uint8_t numSetups = sizeof(setupArray) / sizeof(setupArray[0]);
const int NumServos = 4;
const int NumSwitches = 5;
const int ServoPins[NumServos] = {SERVO_FRONT_SX, SERVO_FRONT_DX, SERVO_BACK_SX, SERVO_BACK_DX};
const int SwitchPins[NumSwitches] = {DIP_0, DIP_1, DIP_2, DIP_3, DIP_4};
Servo ServoArray[NumServos];
bool error = false;
int setupId = 0;
int counterStepServo[NumServos] = {0, 0, 0, 0};

// Variables will change:
int buttonState;            // the current reading from the input pin
int lastButtonState = LOW;  // the previous reading from the input pin

// the following variables are unsigned longs because the time, measured in
// milliseconds, will quickly become a bigger number than can be stored in an int.
unsigned long lastDebounceTime = 0;  // the last time the output pin was toggled
unsigned long debounceDelay = 50;    // the debounce time; increase if the output flickers

void setup() {
  #ifdef DEBUG
  Serial.begin(9600);
  #endif
  pinMode(BUTTON_PIN, INPUT);
  pinMode(GREEN_FRONT_LED, OUTPUT);
  pinMode(YELLOW_FRONT_LED, OUTPUT);
  pinMode(RED_FRONT_LED, OUTPUT);
  pinMode(GREEN_BACK_LED, OUTPUT);
  pinMode(YELLOW_BACK_LED, OUTPUT);
  pinMode(RED_BACK_LED, OUTPUT);
  pinMode(LED_BUILTIN, OUTPUT);
  for (int i = 0; i < NumSwitches; i++) {
    pinMode(SwitchPins[i], INPUT_PULLUP);
    byte switchState = digitalRead(SwitchPins[i]);
    setupId += (switchState << i);
  }
  setupId = setupId & 0x0f ^ 0xf;

  #ifdef DIPSWITCH_VALUE_CLOSE
    #ifdef DEBUG
    Serial.print("Setup: "); Serial.print(0); Serial.println();
    #endif
    for (int i = 0; i < NumServos; i++) {
      ServoArray[i].attach(ServoPins[i]);
      configureServoWithValue(i, setupId, counterStepServo[i]);
    }
  #else
    if (setupId >= numSetups) {
      digitalWrite(LED_BUILTIN, HIGH);
      error = true;
      return;
    }
    #ifdef DEBUG
    Serial.print("Setup: "); Serial.print(setupId); Serial.println();
    #endif
    for (int i = 0; i < NumServos; i++) {
      ServoArray[i].attach(ServoPins[i]);
      configureServo(i, setupId, counterStepServo[i]);
    }
  #endif
}

void loop() {
  if (!error) {
    // read the state of the switch into a local variable:
    int reading = digitalRead(BUTTON_PIN);

    // check to see if you just pressed the button
    // (i.e. the input went from LOW to HIGH), and you've waited long enough
    // since the last press to ignore any noise:

    // If the switch changed, due to noise or pressing:
    if (reading != lastButtonState) {
      // reset the debouncing timer
      lastDebounceTime = millis();
    }

    if ((millis() - lastDebounceTime) > debounceDelay) {
      // whatever the reading is at, it's been there for longer than the debounce
      // delay, so take it as the actual current state:

      // if the button state has changed:
      if (reading != buttonState) {
        buttonState = reading;

        // only toggle the LED if the new button state is HIGH
        if (buttonState == HIGH) {
          for (int i = 0; i < NumServos; i++) {
            counterStepServo[i]++;
            #ifdef DIPSWITCH_VALUE_CLOSE
              if (counterStepServo[i] == setupArray[0].servoArray[i].numSteps)
                counterStepServo[i] = 0;
              configureServoWithValue(i, setupId, counterStepServo[i]);
            #else
              if (counterStepServo[i] == setupArray[setupId].servoArray[i].numSteps)
                counterStepServo[i] = 0;
              configureServo(i, setupId, counterStepServo[i]);
            #endif
          }
        }
      }
    }
    // save the reading. Next time through the loop, it'll be the lastButtonState:
    lastButtonState = reading;
  } else {
    blinkLed();
  }
}

void configureServo(int i, int setupId, int counterStepServo) {
  int valueStep = setupArray[setupId].servoArray[i].stepArray[counterStepServo];
  ServoArray[i].write(valueStep);
  for (int a = 0; a < setupArray[setupId].servoArray[i].numStepLeds; a++) {
    if (valueStep == setupArray[setupId].servoArray[i].stepLedArray[a].valueStep)
      digitalWrite(setupArray[setupId].servoArray[i].stepLedArray[a].pinLed, HIGH);
    else
      digitalWrite(setupArray[setupId].servoArray[i].stepLedArray[a].pinLed, LOW);
  }
  #ifdef DEBUG
  Serial.print("Servo: "); Serial.print(i); Serial.print(" - ");
  Serial.print("Step: "); Serial.print(counterStepServo); Serial.print(" - ");
  Serial.print("Value: "); Serial.print(valueStep); Serial.println();
  #endif
}

void configureServoWithValue(int i, int setupId, int counterStepServo) {
  int valueStep = setupArray[0].servoArray[i].stepArray[counterStepServo];
  if (valueStep == CLOSED)
    valueStep = valueStep/3*2+setupId*4;
  ServoArray[i].write(valueStep);
  for (int a = 0; a < setupArray[0].servoArray[i].numStepLeds; a++) {
    int valueCompare = setupArray[0].servoArray[i].stepLedArray[a].valueStep;
    valueCompare = valueCompare/3*2+setupId*4;
    if (valueStep == setupArray[0].servoArray[i].stepLedArray[a].valueStep && setupArray[0].servoArray[i].stepArray[counterStepServo] != CLOSED || valueStep == valueCompare)
      digitalWrite(setupArray[0].servoArray[i].stepLedArray[a].pinLed, HIGH);
    else
      digitalWrite(setupArray[0].servoArray[i].stepLedArray[a].pinLed, LOW);
  }
  #ifdef DEBUG
  Serial.print("Servo: "); Serial.print(i); Serial.print(" - ");
  Serial.print("Step: "); Serial.print(counterStepServo); Serial.print(" - ");
  Serial.print("Value: "); Serial.print(valueStep); Serial.println();
  #endif
}

void blinkLed() {
  digitalWrite(LED_BUILTIN, (millis() / 1000) % 2);
}