//NOTE: serialMonitor in diagram.json must have "newline" property set to "none".
//      This prevents it from appending an EOL character to input.
#include <Servo.h>
struct swtchStruct {
  int grnSwtchPin;
  int redSwtchPin;
  int grnLEDPin;
  int redLEDPin;
  int buttonPin;
  int servoPin;
  int grnLED;
  int redLED;
  int grnBright;
  int redBright;
  int grnSwtch;
  int redSwtch;
  int servoPosGrn;          //Green position - servo UP
  int servoPosRed;          //Red position - servo DOWN
  int buttonToggle;         //0 = Green; 1 = Red
  int buttonPress;          //0 = Pressed; 1 = Unpressed
  };
int waitTime = 500;         //loop delay time
int i;                      //counter
const int numTurnouts=2;    //Number of turnouts
Servo servo[numTurnouts];

swtchStruct swtchNum[] = {
  {2, 3, 4, 5, 6, 7, LOW, LOW, 255, 255, LOW, HIGH, 60, 120, LOW, LOW},     //Blue
  {8, 9, 10, 11, 12, 13, LOW, LOW, 140, 100, LOW, HIGH, 40, 150, LOW, LOW}  //Yellow
  };

void setup() {
  Serial.begin(115200);
  for ( i = 0; i < numTurnouts; i++) {              //Configure each turnout info
    servo[i].attach(swtchNum[i].servoPin);
    pinMode(swtchNum[i].buttonPin, INPUT_PULLUP);
    pinMode(swtchNum[i].grnLEDPin, OUTPUT);
    pinMode(swtchNum[i].redLEDPin, OUTPUT);
    pinMode(swtchNum[i].grnSwtchPin, INPUT_PULLUP);
    pinMode(swtchNum[i].redSwtchPin, INPUT_PULLUP);
    swtchNum[i].grnLED = !digitalRead(swtchNum[i].grnSwtchPin); //Get current Grn servo position
    swtchNum[i].redLED = !digitalRead(swtchNum[i].redSwtchPin); //Get current Red servo position
    analogWrite(swtchNum[i].grnLEDPin, swtchNum[i].grnLED * swtchNum[i].grnBright);  //Set Grn LED
    analogWrite(swtchNum[i].redLEDPin, swtchNum[i].redLED * swtchNum[i].redBright);  //Set Red LED
    if (swtchNum[i].grnLED) {                                   //If turnout status is Grn
      servo[i].write(swtchNum[i].servoPosGrn);                  //Move servo to Grn position
      swtchNum[i].buttonToggle = swtchNum[i].grnSwtch;          //Set ButtonToggle to Grn
      }
    else {                                                      //Else turnout status is Red
      servo[i].write(swtchNum[i].servoPosRed);                  //Move servo to Red position
      swtchNum[i].buttonToggle = swtchNum[i].redSwtch;          //Set ButtonToggle to Red
      }
    }
  }

void loop() {
  i = 0;
  while (!digitalRead(swtchNum[i].buttonPin) == false) {    //Scan for a button pressed
    i++;
    if (i == numTurnouts) {
      i = 0;
      }
    }
  if (swtchNum[i].buttonToggle == swtchNum[i].redSwtch) { //Button pressed and switch is Red
    swtchNum[i].buttonToggle = LOW;
    swtchNum[i].grnLED = HIGH;
    swtchNum[i].redLED = LOW;
    analogWrite(swtchNum[i].grnLEDPin, swtchNum[i].grnLED * swtchNum[i].grnBright);
    analogWrite(swtchNum[i].redLEDPin, swtchNum[i].redLED * swtchNum[i].redBright);
    servo[i].write(swtchNum[i].servoPosGrn);
    Serial.print("Green LED (Red -> Grn): "); Serial.println(swtchNum[i].grnLED * swtchNum[i].grnBright);
    Serial.print("Red LED (Red -> Grn): "); Serial.println(swtchNum[i].redLED * swtchNum[i].redBright);
    }
  else if (swtchNum[i].buttonToggle == swtchNum[i].grnSwtch) {  //Button pressed and switch is Green
    swtchNum[i].buttonToggle = HIGH;
    swtchNum[i].grnLED = LOW;
    swtchNum[i].redLED = HIGH;
    analogWrite(swtchNum[i].grnLEDPin, swtchNum[i].grnLED * swtchNum[i].grnBright);
    analogWrite(swtchNum[i].redLEDPin, swtchNum[i].redLED * swtchNum[i].redBright);
    servo[i].write(swtchNum[i].servoPosRed);
    Serial.print("Green LED (Grn -> Red): "); Serial.println(swtchNum[i].grnLED * swtchNum[i].grnBright);
    Serial.print("Red LED (Grn -> Red): "); Serial.println(swtchNum[i].redLED * swtchNum[i].redBright);
    }
  delay(waitTime);
}