#include <Servo.h>
struct turnoutStruct {
  int SrvoNum;            //Turnout number
  int srvoPin;            //Arduino Pin for Servo
  int grnDeg;             //Servo degrees for Green (Straight)
  int redDeg;             //Servo degrees for Red (Diverted)
  int grnPin;             //Arduino Pin for Green LED
  int grnLED;             //Green LED Status
  int redPin;             //Arduino Pin for Red LED
  int redLED;             //Red LED Status
  int grnSwtchPin;        //Arduino Pin for Green switch (Straight)
  int grnSwtch;           //Switch Status for Green
  int redSwtchPin;        //Arduino Pin for Red switch (Diverted)
  int redSwtch;           //Switch Status for Red
  int pushBtnPin;          //Arduino Pin for Pushbutton to toggle turnout
};

const int numTurnouts=2; //Number of turnouts
Servo numSrvo[numTurnouts];
int i;

turnoutStruct myTurnout[] = {
  {1, 10, 20, 180, 2, LOW, 3, LOW, 6, LOW, 7, LOW, 12},
  {2, 11, 8, 150, 4, LOW, 5,  LOW, 8, LOW, 9, LOW, 13}
};

void setup() {
  Serial.begin(115200);
  for (i=0; i<numTurnouts;i++) {
    pinMode(myTurnout[i].srvoPin, OUTPUT);
    pinMode(myTurnout[i].grnPin, OUTPUT);
    pinMode(myTurnout[i].redPin, OUTPUT);
    pinMode(myTurnout[i].grnSwtchPin, INPUT);
    pinMode(myTurnout[i].redSwtchPin, INPUT);
    pinMode(myTurnout[i].pushBtnPin, INPUT);
    numSrvo[i].attach(myTurnout[i].srvoPin);
    }
  }

void loop() {
  for (i=0;i<numTurnouts;i++) {
    Serial.print("Turnout #");
    Serial.println(myTurnout[i].SrvoNum); 
    Serial.print("Button Status: ");
    Serial.println(digitalRead(myTurnout[i].pushBtnPin));

//Green Switch Status-------------
//    Serial.print("Green Switch Status (Straight): ");
    myTurnout[i].grnSwtch = digitalRead(myTurnout[i].grnSwtchPin);

//    Serial.println(myTurnout[i].grnSwtch);              
//Red Switch Status---------------
//    Serial.print("Red Switch Status (Divert): ");
    myTurnout[i].redSwtch = digitalRead(myTurnout[i].redSwtchPin);
//    Serial.println(myTurnout[i].redSwtch);
//Move Servo Position
    if (myTurnout[i].grnSwtch == myTurnout[i].redSwtch) {
      numSrvo[i].write(90);
//      Serial.println("Error in turnout switch settings");
      }
      else if(myTurnout[i].grnSwtch) {
      numSrvo[i].write(myTurnout[i].grnDeg);
      }
      else {
      numSrvo[i].write(myTurnout[i].redDeg);
      }
    
//Green LED Status---------------
//    Serial.print("Green LED Status: ");
    myTurnout[i].grnLED = myTurnout[i].grnSwtch;
    digitalWrite(myTurnout[i].grnPin, myTurnout[i].grnSwtch);
//    Serial.println(myTurnout[i].grnLED);                
//Red LED Status--------------
//    Serial.print("Red LED Status: ");
    myTurnout[i].redLED = myTurnout[i].redSwtch;
    digitalWrite(myTurnout[i].redPin, myTurnout[i].redSwtch);
//    Serial.println(myTurnout[i].redLED);                

//Green Switch Pin (Straight)-------------
//    Serial.print("Green Switch Pin: ");
//    Serial.println(myTurnout[i].grnSwtchPin);           
//Red Switch Pin (Diverted)--------------
//    Serial.print("Red Switch Pin: ");
//    Serial.println(myTurnout[i].redSwtchPin);           

//Green LED Pin 
//    Serial.print("Green LED Pin #:");
//    Serial.println(myTurnout[i].grnPin);                
//    digitalWrite(myTurnout[i].grnPin, myTurnout[i].grnSwtchPin);
//Red LED Pin
//    Serial.print("Red LED Pin #:");
//    Serial.println(myTurnout[i].redPin);                

//Servo details-------------
//    Serial.print("Servo Signal Pin #");
//    Serial.println(myTurnout[i].srvoPin); 
//    Serial.print("Green Servo Degrees: ");
//    Serial.println(myTurnout[i].grnDeg); 
//    Serial.print("Red Servo Degrees: ");
//    Serial.println(myTurnout[i].redDeg); 
//    Serial.println("-----------------------------");
    }
  delay(500);
  }