/***
 * Switch Signal Controller Demo
 * 10/29/2019
 * Modified by Robby Mossman
 * 11/14/2019
 * 06/03/2021 - Updated Input and Output assignments for Arduino Mega Board
 * 06/04/2021 - Defined what items are outputs and what is an input
 * 06/04/2021 - Updated signal lights so that Green does not come on after switch finishes changing position.  
 *              Yellow still flashes indicating which route switch is moving towards
 *              On power on I enabled each crossing light to turn on and bell to ding in sync with lights turning on
 * 07/05/2021 - Updated changes which include crossing warning activation being separated from switch and signal head actions 
 *              Updated PBR / PBL function to change the signal head lights to all RED on push and on release activate the motor 
 *              and change the lights to prevent motor from running longer than needed.
 *              Enabled Serial.Print so I can monitor what CASE we are in and verify that the appropriate PIN is active
 *              Added Light and Lightoff before motor runs in order to reset lights when PBL or PBR are pushed after crossing clears
 * 07/12/2021 - Added additional logging details to show time between actions in order to try and troubleshoot what appears to be a        
 *              phantom trigger of PBL or PBR
 *              
 * 08/10/2021 - Updated Pin assignments for installation into enclosure
 *              
 * 08/19/2021 - Changed the signal head light so when route is select Green is on until train detected in crossing the Green turns 
 *              off and yellow turns on.  Signal does not change back to Green until train clears switch.  Green also indicates the Push buttons 
 *              are active.
 *              
 * 08/19/2021 - Added additional functionality so when lights get turned off after being idle for a period of time when grade crossing flashers
 *              becomes active it turns the appropriate signal head lights back on to represent the last known condition of signal head lights              
 *
 * 07/07/2023 - Adding Capture button on West loop at Breezeway heading towards yard
 *              Added Track Detection after traveling across crossing from West to East towards yard
 *              
 *
 *              
 ************************************************************************/

/*
 * Assign pin numbers to symbols - wired to digital IO Ports
 * Relay Board 1 - Pins 35 - 49 Odd Numbers
 * Relay Board 2 - Pins 34 - 48 Even Numbers
 */
#define REDL 35    // RED LEFT - Relay Board 1 - Relay 1
#define YELL 37   // YELLOW LEFT - Relay Board 1 - Relay 2
#define GRNL 39    // GREEN LEFT - Relay Board 1 - Relay 3
#define REDR 41    // RED RIGHT - Relay Board 1 - Relay 4
#define YELR 43    // YELLOW RIGHT - Relay Board 1 - Relay 5
#define GRNR 45    // GREEN RIGHT - Relay Board 1 - Relay 6
#define GRDLTL 47  // Left Grade Crossing Light - Relay Board 1 - Relay 7
#define GRDLTR 49  // Right Grade Crossing Light - Relay Board 1 - Relay 8

#define BELL  34   // Grade crossing Bell - Relay Board 2 - Relay 1

/*
 * Switch Motor Output
 *
 */
#define MOTORL 36  // MOTOR MOVING SWITCH TO GO LEFT  - Relay Board 2 - Relay 2
#define MOTORR 38  // MOTOR MOVING SWITCH TO GO RIGHT - Relay Board 2 - Relay 3


/*
 * The following are inputs - wired to digital IO Ports
 */
#define PBL 8     // PUSH BUTTON TO GO LEFT Head toward ByPass (TRKDD)
#define PBR 9     // PUSH BUTTON TO GO RIGHT Head towards Station (TRKDC)
#define PBW 40    // PUSH Button to go from West to East on outter loop towards yard (TRKDE)

#define TRKDA 10  // Train Detection at Grade Crossing
#define TRKDC 11  // #6 Track detection to reactivate Push Buttons and change signal lights to ALL Green - Train heading towards station or Bypass both tracks are wired to share the detection
#define TRKDD 12  // #8 Track detection to reactivate Push Buttons and change signal lights to ALL Green - Train heading towards station or Bypass both tracks are wired to share the detection
#define TRKDE 42  // Track Dection after train crossing grade crossing towards yard
#define OUTTERTRACKWEST 51  //Track detection on west side of double crossing
#define OUTTERTRACKEAST 53  //Track detection on east side of double crossing

/* 
 *  USEFUL SYMBOLS (FOR READABILITY)
 */
#define LEFT 0
#define RIGHT 1

#define PUSHED LOW
#define NOTPUSHED HIGH

#define ON HIGH
#define OFF LOW

#define WRITE 1
#define NOWRITE 0

/*
 * If you expected the motor to take 3 seconds to move the switch you would
 * set MOTORDELAYMS to 3000
 * The BLINKINTERVAL is how long the light will be on or off at a time
 * as it blinks. Again in MS
 * BLINKCOUNT IS CALCULATED FOR YOU BUT BETTER TO NAKE BLINKINTERVAL TO
 * DIVIDE INTO MOTORDELAYMS EVENLY
 */
 
#define MOTORDELAYMS 700
#define BLINKINTERVAL 160
#define BLINKCOUNT (((MOTORDELAYMS)/(BLINKINTERVAL))  / 2)  

#define FLASHINTERVAL 300
#define FLASHCOUNT 20

//  Do this BLINKCOUNT times
//    Light On
//    Delay(BLINKINTERVAL)
//    Light Off
//    Delay(BLINKINTERVAL)                                    

/*********************************************
 * 
 * New Section to add Case Enumeration
 * Bill Leahy / Robby Mossman - 06/28/2021 
 * Updated ENUM with BREEZEWAY Items - 07/07/2023
 *********************************************/

enum{START,
    MOVE_SWITCH_BYPASS,
    MOVE_SWITCH_BYPASS_BREEZEWAY,
    MOVE_SWITCH_STRAIGHT,      
    TRAIN_IN_SWITCH_BYPASS,
    TRAIN_IN_SWITCH_BYPASS_ERROR,
    TRAIN_IN_SWITCH_BYPASS_ERROR_BREEZEWAY,
    TRAIN_IN_SWITCH_STRAIGHT,      
    TRAIN_IN_SWITCH_STRAIGHT_ERROR,  
    TRAIN_PRESENT_BYPASS,
    TRAIN_PRESENT_BYPASS_BREEZEWAY,
    TRAIN_PRESENT_STRAIGHT,      
    WAIT_BYPASS,
    WAIT_BYPASS_BREEZEWAY,
    WAIT_STRAIGHT, 
    TRAIN_CLEAR_STRAIGHT,
    TRAIN_CLEAR_BYPASS,
    TRAIN_CLEAR_BYPASS_BREEZEWAY,
    TRAIN_IN_WRONG_SWITCH_4_STRAIGHT,
    TRAIN_IN_WRONG_SWITCH_4_BYPASS,
    TRAIN_IN_WRONG_SWITCH_4_BYPASS_BREEZEWAY,
    WAIT_STRAIGHT_PBR_RELEASE,
    WAIT_BYPASS_PBL_RELEASE,
    WAIT_BYPASS_BREEZEWAY_PBW_RELEASE,
    TRAIN_IN_SWITCH_BYPASS_BREEZEWAY,
    TRAIN_TRAVELING_WEST_EAST_ON_STRAIGHT,
    TRAIN_TRAVELING_WEST_EAST_ON_STRAIGHT_WAIT,
    TRAIN_CLEAR_BYPASS_BREEZEWAY_ALL_GREEN,
    STRAIGHT_ALL_GREEN,
    STRAIGHT_ALL_GREEN_WAIT,
    BYPASS_ALL_GREEN,
    BYPASS_ALL_GREEN_WAIT,
    TRAIN_TRAVELING_WEST_EAST_ON_BYPASS,
    TRAIN_TRAVELING_WEST_EAST_ON_BYPASS_WAIT,
    TRKDA_CAPTURE_WITHOUT_BUTTON,
    TRKDA_CAPTURE_WITHOUT_BUTTON_TRAIN_PRESENT_STRAIGHT,
    TRKDA_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_LIGHTS,
    TRKDA_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_STRAIGHT,
    TRKDA_CAPTURE_WITHOUT_BUTTON_TRAIN_WAIT,
    TRKDA_CAPTURE_WITHOUT_BUTTON_TRAIN_IN_TRKDC,
    TRKDA_CAPTURE_WITHOUT_BUTTON_TRAIN_IN_TRKDD,
    TRKDA_CAPTURE_WITHOUT_BUTTON_TRAIN_IN_TRKDE,
    TRKDC_CAPTURE_WITHOUT_BUTTON,
    TRKDC_CAPTURE_WITHOUT_BUTTON_TRAIN_PRESENT_STRAIGHT,
    TRKDC_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_LIGHTS,
    TRKDC_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_STRAIGHT,
    TRKDC_CAPTURE_WITHOUT_BUTTON_TRAIN_WAIT,
    TRKDC_CAPTURE_WITHOUT_BUTTON_TRAIN_IN_TRKDA,
    TRKDC_CAPTURE_WITHOUT_BUTTON_TRAIN_IN_TRKDD,
    TRKDC_CAPTURE_WITHOUT_BUTTON_TRAIN_IN_TRKDE,
    TRKDD_CAPTURE_WITHOUT_BUTTON,
    TRKDD_CAPTURE_WITHOUT_BUTTON_TRAIN_PRESENT_STRAIGHT,
    TRKDD_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_LIGHTS,
    TRKDD_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_STRAIGHT,
    TRKDD_CAPTURE_WITHOUT_BUTTON_TRAIN_WAIT,
    TRKDD_CAPTURE_WITHOUT_BUTTON_TRAIN_IN_TRKDA,
    TRKDD_CAPTURE_WITHOUT_BUTTON_TRAIN_IN_TRKDC,
    TRKDD_CAPTURE_WITHOUT_BUTTON_TRAIN_IN_TRKDE,
    TRKDE_CAPTURE_WITHOUT_BUTTON,
    TRKDE_CAPTURE_WITHOUT_BUTTON_TRAIN_PRESENT_STRAIGHT,
    TRKDE_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_LIGHTS,
    TRKDE_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_STRAIGHT,
    TRKDE_CAPTURE_WITHOUT_BUTTON_TRAIN_WAIT,
    TRKDE_CAPTURE_WITHOUT_BUTTON_TRAIN_IN_TRKDA,
    TRKDE_CAPTURE_WITHOUT_BUTTON_TRAIN_IN_TRKDC,
    TRKDE_CAPTURE_WITHOUT_BUTTON_TRAIN_IN_TRKDD,
    PBL_BUTTON_SWITCH_LEFT_CAPTURE,
    PBL_BUTTON_SWITCH_LEFT_CAPTURE_TRAIN_PRESENT_STRAIGHT,
    PBL_BUTTON_SWITCH_LEFT_CAPTURE_TRAIN_CLEAR_LIGHTS,
    PBL_BUTTON_SWITCH_LEFT_CAPTURE_TRAIN_CLEAR_STRAIGHT,
    STARTOUTTERTRACK,
    TRAIN_DETECTED_OUTTER_WAIT,
    TRAIN_DETECTED_OUTTER_WEST,
    TRAIN_DETECTED_OUTTER_WEST_WAIT,
    TRAIN_DETECTED_OUTTER_EAST,
    TRAIN_DETECTED_OUTTER_EAST_WAIT};  
    

#define B_FLASHERLEFT 7 //Flasher light left
#define B_FLASHERRIGHT 8 //Flasher Light Right
#define B_Bell 9         // Bell

#define L_GRADECROSSING 10
#define R_GRADECROSSING 11

#define L_SWITCHLEFT 12  // PBL buttons disabled until train clears
#define L_SWITCHRIGHT 13 // PBR buttons disabled until train clears

// Function Prototypes (these function are at the bottom of the file)
void all(int side, int dly);                        //  Turn all lights on one side on for dly MS then turn them off
void motor(int side, int action);                   //  Turn the motor actuator on or off in the desired direction
void light(int which);            //  Turn on 'which' light
void lightoff(int which);         //  Turn off 'which' light
int pushed(int button);           //  Return true if button is pushed
void blinker(int count, int which, int duration);   // Blinks which light count times each on or off is duration long

void flasher(int count, int which, int duration);   // Blinks which light count times each on or off is duration long
void crossingProcedure(bool bells,int duration); // Activates the entire crossing setup for a duration with bell sounds on or off
void digitalWriteWrapper(int pin, int val);  //Print PIN information to Serial Monitor


// Set state machine initial starting state
int state = START;
int state2 = STARTOUTTERTRACK;
int long timeout = 0L; //Time out variable for lights to turn off after button has been pushed


/************************************************************************  
 *   
 *  The setup function gets executed once at the beginning 
 *  
 ************************************************************************/

void setup() 
{
  // Tell the Arduino which pins are input and which are output. 
/***************************************************
 * Items below are for signal head lights
****************************************************/
  pinMode(REDL, OUTPUT);
  pinMode(YELL, OUTPUT);
  pinMode(GRNL, OUTPUT);
  pinMode(REDR, OUTPUT);
  pinMode(YELR, OUTPUT);
  pinMode(GRNR, OUTPUT);
/***************************************************
 * Items below are for the crossing lights and bell
****************************************************/
  pinMode(GRDLTL, OUTPUT);
  pinMode(GRDLTR, OUTPUT);
  pinMode(BELL, OUTPUT);
/***************************************************
 * Items below are for the switch motor
****************************************************/
  pinMode(MOTORL, OUTPUT);
  pinMode(MOTORR, OUTPUT);
  
/***************************************************
 * Items below represent push buttons and track detection
 * PULLUP means if not pressed then it's high
 * Pressed will be low
****************************************************/
  pinMode(PBL, INPUT_PULLUP);   // Push button LEFT for Switch Motor
  pinMode(PBR, INPUT_PULLUP);   // Push button Right for Switch Motor
  pinMode(PBW, INPUT_PULLUP);   // Push Button at Breezeway to Capture Block
  
  pinMode(TRKDA, INPUT_PULLUP);   // Track detection for approaching train to man and activate warning signal and bell
  pinMode(TRKDC, INPUT_PULLUP);   // Track detection once train clears switch heading towards station and allows buttons to reactivate and signal heads change to green
  pinMode(TRKDD, INPUT_PULLUP);   // Track detection once train clears switch heading towards bypass and allows buttons to reactivate and signal heads change to green
  pinMode(TRKDE, INPUT_PULLUP);   // Track dection heading towards yard

  // Turn all lights off
  digitalWriteWrapper(REDL, LOW);
  digitalWriteWrapper(YELL, LOW);
  digitalWriteWrapper(GRNL, LOW);
  digitalWriteWrapper(REDR, LOW);
  digitalWriteWrapper(YELR, LOW);
  digitalWriteWrapper(GRNR, LOW);
  digitalWriteWrapper(MOTORL, LOW);
  digitalWriteWrapper(MOTORR, LOW);
  digitalWriteWrapper(GRDLTL, LOW);
  digitalWriteWrapper(GRDLTR, LOW);
  digitalWriteWrapper (BELL, LOW);

  Serial.begin(115200);  //Print all activity from Arduino to Serial port.  This is for troubleshooting.  This is also part of pin 0/1

} // void setup()


/************************************************************************  
 *
 *  Main loop which is called over and over continuously 
 *  
 ************************************************************************/
void loop() 
{ 
  // Examine the state variable and go down to the case line that corresponds to its value
  // If none match, go to "default"

    timeout = timeout + 1L;  // Increments the variable
    if(timeout > 300000000L)  // Have to determine somebignumber experimentally
    {
        printMessage2("******** Turning all lights off ********");
        // Turn off all lights  300000000L
        digitalWriteWrapper(REDL, LOW);
        digitalWriteWrapper(YELL, LOW);
        digitalWriteWrapper(GRNL, LOW);
        digitalWriteWrapper(REDR, LOW);
        digitalWriteWrapper(YELR, LOW);
        digitalWriteWrapper(GRNR, LOW);
        //digitalWriteWrapper(GRDLTL, LOW);  //Added so grade crossing ligths off off when signal lights go off 06/07/2021
        //digitalWriteWrapper(GRDLTR, LOW);  //Added so grade crossing ligths off off when signal lights go off 06/07/2021
        
        printMessage2("******** Lights should now be off ********");

        while(!pushed(PBL) && !pushed(PBR) && !pushed(TRKDA) && !pushed(PBW))   // while not pushing left pb and not pushing right pb, loop (if either button gets pushed it will drop out of loop)
        {
            // Do nothing  // Infinite loop (unless a button is pushed)
        }
        timeout = 0L;

  } 

/****************************************************************************************
 * New Switch & State definitions created with Bill L and Robby M on 6/28/2021
 * 
 * Sections below are for Straight traffic through the man crossing towards the station
 ****************************************************************************************/


/*******************************************
* Added a second state in order to have outter track track detection to be separate from
* inner track detection and more 07/24/2023
*
*******************************************/
while(1)
{
  switch(state2)
  //This state addresses the outter track on the double crossing that activates 
  //signals and bell only
  {
    case STARTOUTTERTRACK:
    printMessage2("Now in Case Start2 - STARTOUTTERTRACK - Train detection for outter loop at doublecrossing");
    state2 = TRAIN_DETECTED_OUTTER_WAIT;
    break;
    
    case TRAIN_DETECTED_OUTTER_WAIT:
    printMessage2("Now in Case Start2 - TRAIN_DETECTED_OUTTER_WAIT - Train detection for outter loop at doublecrossing");
        
        if(pushed(OUTTERTRACKWEST))
          {
            state2 = TRAIN_DETECTED_OUTTER_WEST;
          } 

          //I need to add OUTTERTRACKEAST
    break;

/**********************************************************
* TRAIN traveling West to East at Crossing on Outter Track
* Train capture crossing at West and Releases at East
* 
************************************************************/
    case TRAIN_DETECTED_OUTTER_WEST:
    printMessage2("Now in Case Start2 - TRAIN_DETECTED_OUTTER_WEST - Train detection for outter loop at doublecrossing");
            if(!pushed(OUTTERTRACKWEST))
            {
              crossinglights (2,0,300);
              state2 = TRAIN_DETECTED_OUTTER_WEST_WAIT;
            }
         break;

    case TRAIN_DETECTED_OUTTER_WEST_WAIT:
    printMessage2("Now in Case Start2 - TRAIN_DETECTED_OUTTER_WEST_WAIT - Train detection for outter loop at doublecrossing");

      if(pushed(OUTTERTRACKWEST))
          {
            state2 = TRAIN_DETECTED_OUTTER_WAIT;
          } 
    break;

  }  //Close state2



switch(state)
{
  //Initial Startup checks all lights & Bell
  case START:
    printMessage2("Now in Case Start - Initialize System");
    all(LEFT, 1000);      // Turn all lights on left side on then off
    all(RIGHT, 1000);     // Turn all kights on right side on then off
    //state = MOVE_SWITCH_STRAIGHT;
    state = STRAIGHT_ALL_GREEN;
    break;


case STRAIGHT_ALL_GREEN:
   //Change signals to ALL GREEN.
    lightoff(REDL);
    lightoff(REDR);
    motor(RIGHT, ON);    // Move switch to right  
    delay(MOTORDELAYMS);  
    motor(RIGHT, OFF) ;
    lightoff(YELL);
    lightoff(YELR);
    light(GRNL);
    light(GRNR);    
    state = STRAIGHT_ALL_GREEN_WAIT;
    break;     

case STRAIGHT_ALL_GREEN_WAIT:
  //Wait for next input from either Crossing, PBR, or PBL
    printMessage2("Now in Case STRAIGHT_ALL_GREEN_WAIT - waiting for TRKDA, PBR, PBL, PBW");
    
    if(pushed(PBL))
    {
      state = WAIT_BYPASS_PBL_RELEASE ;
    } 
    if(pushed(PBR)) 
    {
      state = WAIT_STRAIGHT_PBR_RELEASE;
    }     
        if(pushed(PBW))
    {
      state = WAIT_BYPASS_BREEZEWAY_PBW_RELEASE;
    }
    if(pushed(TRKDA)) //ADDED 07/22/2023
    {
      state = TRKDA_CAPTURE_WITHOUT_BUTTON;
    }
    if(pushed(TRKDC)) 
    {
      state = TRKDC_CAPTURE_WITHOUT_BUTTON;
    }    
   if(pushed(TRKDD)) //ADDED 07/22/2023
    {
      state = TRKDD_CAPTURE_WITHOUT_BUTTON;
    }
       if(pushed(TRKDE)) //ADDED 07/22/2023
    {
      state = TRKDE_CAPTURE_WITHOUT_BUTTON;
    }
    break;        

/************************************************************
*   TRKDA
*
*   This section addresses train detection on Track after system restart 
*   and no other action has taken place
*
*************************************************************/

  case TRKDA_CAPTURE_WITHOUT_BUTTON:
    printMessage2("Now in Case Start - TRKDA_CAPTURE_WITHOUT_BUTTON - !!!NO SWITCH ACTION!!! - Lights & Motor to Straight/Right");
    lightoff(YELR);
    lightoff(GRNR);
    lightoff(YELL);
    lightoff(GRNL);
    light(REDL);   
    //motor(RIGHT, ON);    // Move switch to right  
    //delay(MOTORDELAYMS);  
    //motor(RIGHT, OFF) ;
    lightoff(REDR);
    light (YELR);   
    state = TRKDA_CAPTURE_WITHOUT_BUTTON_TRAIN_PRESENT_STRAIGHT;
    break;                  


  case TRKDA_CAPTURE_WITHOUT_BUTTON_TRAIN_PRESENT_STRAIGHT:      
    printMessage2("Now in Case Start - TRKDA_CAPTURE_WITHOUT_BUTTON_TRAIN_PRESENT_STRAIGHT - In Crossing");
    lightoff(GRNR);
    light(YELR); 
    light(REDL); 
    crossinglights (2,0,300);
          if(!pushed(TRKDA)) 
     {
        state = TRKDA_CAPTURE_WITHOUT_BUTTON_TRAIN_WAIT;
      }             
   break;

case TRKDA_CAPTURE_WITHOUT_BUTTON_TRAIN_WAIT:
   printMessage2("Now in Case Start - TRKDA_CAPTURE_WITHOUT_BUTTON_TRAIN_WAIT - In Crossing");
    if(pushed(TRKDA)) 
        {
          state = TRKDA_CAPTURE_WITHOUT_BUTTON;
        }   
    if(pushed(TRKDC))
    {
      crossinglights (2,0,300);
      state = TRKDA_CAPTURE_WITHOUT_BUTTON_TRAIN_IN_TRKDC;
    }
    if(pushed(TRKDD))
    {
      crossinglights (2,0,300);
      state = TRKDA_CAPTURE_WITHOUT_BUTTON_TRAIN_IN_TRKDD;
    }
    if(pushed(TRKDE))
    {
      crossinglights (2,0,300);
      state = TRKDA_CAPTURE_WITHOUT_BUTTON_TRAIN_IN_TRKDE;
    }

   break;


case TRKDA_CAPTURE_WITHOUT_BUTTON_TRAIN_IN_TRKDC:

  if(!pushed(TRKDC))
  {
    state = TRKDA_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_LIGHTS;
  }
  break;

case TRKDA_CAPTURE_WITHOUT_BUTTON_TRAIN_IN_TRKDD:
  if(!pushed(TRKDD))
  {
    state = TRKDA_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_LIGHTS;
  }
  break;

case TRKDA_CAPTURE_WITHOUT_BUTTON_TRAIN_IN_TRKDE:
  if(!pushed(TRKDE))
  {
    state = TRKDA_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_LIGHTS;
  }
  break;

case TRKDA_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_LIGHTS:
     printMessage2("Now in Case Start - TRKDA_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_LIGHTS");
          lightoff(YELR);
          lightoff(REDL);
          light(GRNL);
          light(GRNR);
          state = TRKDA_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_STRAIGHT;
      break;

case TRKDA_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_STRAIGHT:
  
    printMessage2("Now in Case Start - TRKDA_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_STRAIGHT - Exited Crossing");
    if(pushed(TRKDA)) 
        {
          state = TRKDA_CAPTURE_WITHOUT_BUTTON;
        }   
    if(pushed(TRKDC)) 
        {
          state = TRKDC_CAPTURE_WITHOUT_BUTTON;
        }   
    if(pushed(TRKDD)) 
        {
          state = TRKDD_CAPTURE_WITHOUT_BUTTON;
        }   
      if(pushed(TRKDE)) 
        {
          state = TRKDE_CAPTURE_WITHOUT_BUTTON;
        }   

    if(pushed(PBL))
    {
      state = WAIT_BYPASS_PBL_RELEASE ;
    } 
    if(pushed(PBR)) 
    {
      state = WAIT_STRAIGHT_PBR_RELEASE;
    }     
        if(pushed(PBW))
    {
      state = WAIT_BYPASS_BREEZEWAY_PBW_RELEASE;
    }

    break;
 


/*************************************************************
*   TRKDC  
*
*  This section is for TRKDC if trains coming from cross over or inner loop
*
**************************************************************/

case TRKDC_CAPTURE_WITHOUT_BUTTON:
    printMessage2("Now in Case Start - TRKDC_CAPTURE_WITHOUT_BUTTON - MOVE_SWITCH_STRAIGHT - Lights & Motor to Straight/Right");
    lightoff(YELL); 
    lightoff(YELR);
    lightoff(GRNL); 
    lightoff(GRNR); 
    light(REDL);   
    light(REDR);    
    //motor(RIGHT, ON);    // Move switch to right  
    //delay(MOTORDELAYMS);  
    //motor(RIGHT, OFF) ;
     state = TRKDC_CAPTURE_WITHOUT_BUTTON_TRAIN_PRESENT_STRAIGHT;
    break;                  


  case TRKDC_CAPTURE_WITHOUT_BUTTON_TRAIN_PRESENT_STRAIGHT:      
    printMessage2("Now in Case Start - TRKDC_CAPTURE_WITHOUT_BUTTON_TRAIN_PRESENT_STRAIGHT - In Crossing");
    light(REDL); 
    light(REDR); 
    crossinglights (2,0,300);
        if(!pushed(TRKDC)) 
        {
          //state = TRKDC_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_LIGHTS;
          state = TRKDC_CAPTURE_WITHOUT_BUTTON_TRAIN_WAIT;
        }             
    break;

case TRKDC_CAPTURE_WITHOUT_BUTTON_TRAIN_WAIT:
   printMessage2("Now in Case Start - TRKDC_CAPTURE_WITHOUT_BUTTON_TRAIN_WAIT - In Crossing");
    if(pushed(TRKDC)) 
        {
          state = TRKDC_CAPTURE_WITHOUT_BUTTON;
        }   
    if(pushed(TRKDA))
    {
      crossinglights (2,0,300);
      state = TRKDC_CAPTURE_WITHOUT_BUTTON_TRAIN_IN_TRKDA;
    }
    if(pushed(TRKDD))
    {
      crossinglights (2,0,300);
      state = TRKDC_CAPTURE_WITHOUT_BUTTON_TRAIN_IN_TRKDD;
    }
    if(pushed(TRKDE))
    {
      crossinglights (2,0,300);
      state = TRKDC_CAPTURE_WITHOUT_BUTTON_TRAIN_IN_TRKDE;
    }

   break;

case TRKDC_CAPTURE_WITHOUT_BUTTON_TRAIN_IN_TRKDA:

  if(!pushed(TRKDA))
  {
    state = TRKDC_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_LIGHTS;
  }
  break;

case TRKDC_CAPTURE_WITHOUT_BUTTON_TRAIN_IN_TRKDD:
  if(!pushed(TRKDD))
  {
    state = TRKDC_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_LIGHTS;
  }
  break;

case TRKDC_CAPTURE_WITHOUT_BUTTON_TRAIN_IN_TRKDE:
  if(!pushed(TRKDE))
  {
    state = TRKDC_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_LIGHTS;
  }
  break;

case TRKDC_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_LIGHTS:
     printMessage2("Now in Case Start - TRKDC_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_LIGHTS");
          lightoff(REDR);
          lightoff(REDL);
          light(GRNL);
          light(GRNR);
          state = TRKDC_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_STRAIGHT;

case TRKDC_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_STRAIGHT:
  
    printMessage2("Now in Case Start - TRKDC_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_STRAIGHT - Exited Crossing");
    if(pushed(TRKDA)) 
        {
          state = TRKDA_CAPTURE_WITHOUT_BUTTON;
        }   
    if(pushed(TRKDC)) 
        {
          state = TRKDC_CAPTURE_WITHOUT_BUTTON;
        }   
    if(pushed(TRKDD)) 
        {
          state = TRKDD_CAPTURE_WITHOUT_BUTTON;
        }   
      if(pushed(TRKDE)) 
        {
          state = TRKDE_CAPTURE_WITHOUT_BUTTON;
        }   

    if(pushed(PBL))
    {
      state = WAIT_BYPASS_PBL_RELEASE ;
    } 
    if(pushed(PBR)) 
    {
      state = WAIT_STRAIGHT_PBR_RELEASE;
    }     
        if(pushed(PBW))
    {
      state = WAIT_BYPASS_BREEZEWAY_PBW_RELEASE;
    }

    break;

/*****************************************************
*   TRKDD
*
*
*
****************************************************/

case TRKDD_CAPTURE_WITHOUT_BUTTON:
    printMessage2("Now in Case Start - TRKDD_CAPTURE_WITHOUT_BUTTON - MOVE_SWITCH_STRAIGHT - Lights & Motor to Straight/Right");
    lightoff(YELL);
    lightoff(YELR);
    lightoff(GRNL);  //Added 07/22/2023
    lightoff(GRNR);  //Added 8/19
    light(REDR);   //added 07/22/2023
    //motor(RIGHT, ON);    // Move switch to right  
    //delay(MOTORDELAYMS);  
    //motor(RIGHT, OFF) ;
    lightoff(REDL);
    light (YELL);   
    state = TRKDD_CAPTURE_WITHOUT_BUTTON_TRAIN_PRESENT_STRAIGHT;
    break;                  

  case TRKDD_CAPTURE_WITHOUT_BUTTON_TRAIN_PRESENT_STRAIGHT:      
    printMessage2("Now in Case Start - TRKDD_CAPTURE_WITHOUT_BUTTON_TRAIN_PRESENT_STRAIGHT - In Crossing");
    lightoff(GRNR);
    light(YELL); 
    light(REDR); 
    crossinglights (2,0,300);
        if(!pushed(TRKDD)) 
        {
          //state = TRKDD_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_LIGHTS;       
           state = TRKDD_CAPTURE_WITHOUT_BUTTON_TRAIN_WAIT;
        }             
    break;

/*************************************************

************************************************/
case TRKDD_CAPTURE_WITHOUT_BUTTON_TRAIN_WAIT:
   printMessage2("Now in Case Start - TRKDD_CAPTURE_WITHOUT_BUTTON_TRAIN_WAIT - In Crossing");
    if(pushed(TRKDD)) 
        {
          state = TRKDD_CAPTURE_WITHOUT_BUTTON;
        }   
    if(pushed(TRKDA))
    {
      crossinglights (2,0,300);
      state = TRKDD_CAPTURE_WITHOUT_BUTTON_TRAIN_IN_TRKDA;
    }
    if(pushed(TRKDC))
    {
      crossinglights (2,0,300);
      state = TRKDD_CAPTURE_WITHOUT_BUTTON_TRAIN_IN_TRKDC;
    }
    if(pushed(TRKDE))
    {
      crossinglights (2,0,300);
      state = TRKDD_CAPTURE_WITHOUT_BUTTON_TRAIN_IN_TRKDE;
    }

   break;

case TRKDD_CAPTURE_WITHOUT_BUTTON_TRAIN_IN_TRKDA:

  if(!pushed(TRKDA))
  {
    lightoff(REDR);
    lightoff(YELL);
    state = TRKDA_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_LIGHTS;
  }
  break;

case TRKDD_CAPTURE_WITHOUT_BUTTON_TRAIN_IN_TRKDC:
  if(!pushed(TRKDC))
  {
    lightoff(YELL);
    state = TRKDC_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_LIGHTS;
  }
  break;

case TRKDD_CAPTURE_WITHOUT_BUTTON_TRAIN_IN_TRKDE:
  if(!pushed(TRKDE))
  {
    lightoff(YELL);
    state = TRKDE_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_LIGHTS;
  }
  break;

case TRKDD_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_LIGHTS:
     printMessage2("Now in Case Start - TRKDD_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_LIGHTS");
          lightoff(YELL);
          lightoff(REDR);
          light(GRNL);
          light(GRNR);
          state = TRKDD_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_STRAIGHT;

case TRKDD_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_STRAIGHT:
  
    printMessage2("Now in Case Start - TRKDD_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_STRAIGHT - Exited Crossing");
    if(pushed(TRKDD)) 
        {
          state = TRKDD_CAPTURE_WITHOUT_BUTTON;
        }   
    if(pushed(TRKDC)) 
        {
          state = TRKDC_CAPTURE_WITHOUT_BUTTON;
        }   
    if(pushed(TRKDE)) 
        {
          state = TRKDE_CAPTURE_WITHOUT_BUTTON;
        }   
      if(pushed(TRKDA)) 
        {
          state = TRKDA_CAPTURE_WITHOUT_BUTTON;
        }   

    if(pushed(PBL))
    {
      state = WAIT_BYPASS_PBL_RELEASE ;
    } 
    if(pushed(PBR)) 
    {
      state = WAIT_STRAIGHT_PBR_RELEASE;
    }     
        if(pushed(PBW))
    {
      state = WAIT_BYPASS_BREEZEWAY_PBW_RELEASE;
    }

    break;


/****************************************************
*   TRKDE
*
* This section addresses train detection on TRKDE (Track leading towars yard traveling
* West.  This section was originally created to detect train when system first starts up
* and no other condition has changed.
*  
*
*****************************************************/
  
 case TRKDE_CAPTURE_WITHOUT_BUTTON:
    printMessage2("Now in Case Start - TRKDE_CAPTURE_WITHOUT_BUTTON - MOVE_SWITCH_STRAIGHT - Lights & Motor to Straight/Right");
    lightoff(YELL);
    lightoff(YELR);
    lightoff(GRNL);
    lightoff(GRNR);
    lightoff(REDR);
    light(REDL);
    light(YELR);
    state = TRKDE_CAPTURE_WITHOUT_BUTTON_TRAIN_PRESENT_STRAIGHT;
    break;                  


  case TRKDE_CAPTURE_WITHOUT_BUTTON_TRAIN_PRESENT_STRAIGHT:      
    printMessage2("Now in Case Start - TRKDE_CAPTURE_WITHOUT_BUTTON_TRAIN_PRESENT_STRAIGHT - In Crossing");
    crossinglights (2,0,300);
        if(!pushed(TRKDE)) 
        {
          //state = TRKDE_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_LIGHTS;       
            state = TRKDE_CAPTURE_WITHOUT_BUTTON_TRAIN_WAIT;
        }             
    break;

case TRKDE_CAPTURE_WITHOUT_BUTTON_TRAIN_WAIT:
   printMessage2("Now in Case Start - TRKDE_CAPTURE_WITHOUT_BUTTON_TRAIN_WAIT - In Crossing");
    if(pushed(TRKDE)) 
        {
          state = TRKDE_CAPTURE_WITHOUT_BUTTON;
        }   
    if(pushed(TRKDA))
    {
      crossinglights (2,0,300);
      state = TRKDE_CAPTURE_WITHOUT_BUTTON_TRAIN_IN_TRKDA;
    }
    if(pushed(TRKDC))
    {
      crossinglights (2,0,300);
      state = TRKDE_CAPTURE_WITHOUT_BUTTON_TRAIN_IN_TRKDC;
    }
    if(pushed(TRKDD))
    {
      crossinglights (2,0,300);
      state = TRKDE_CAPTURE_WITHOUT_BUTTON_TRAIN_IN_TRKDD;
    }

   break;

case TRKDE_CAPTURE_WITHOUT_BUTTON_TRAIN_IN_TRKDA:

  if(!pushed(TRKDA))
  {
    lightoff(REDR);
    lightoff(YELL);
    state = TRKDA_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_LIGHTS;
  }
  break;

case TRKDE_CAPTURE_WITHOUT_BUTTON_TRAIN_IN_TRKDC:
  if(!pushed(TRKDC))
  {
    lightoff(YELR);
    state = TRKDC_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_LIGHTS;
  }
  break;

case TRKDE_CAPTURE_WITHOUT_BUTTON_TRAIN_IN_TRKDD:
  if(!pushed(TRKDD))
  {
    lightoff(REDL);
    lightoff(YELR);
    state = TRKDD_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_LIGHTS;
  }
  break;


case TRKDE_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_LIGHTS:
     printMessage2("Now in Case Start - TRKDE_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_LIGHTS");
          lightoff(REDL);
          lightoff(REDR);
          lightoff(YELR);
          light(GRNL);
          light(GRNR);
          state = TRKDE_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_STRAIGHT;

case TRKDE_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_STRAIGHT:
  
    printMessage2("Now in Case Start - TRKDE_CAPTURE_WITHOUT_BUTTON_TRAIN_CLEAR_STRAIGHT - Exited Crossing");
    if(pushed(TRKDE)) 
        {
          state = TRKDE_CAPTURE_WITHOUT_BUTTON;
        }   
    if(pushed(TRKDC)) 
        {
          state = TRKDC_CAPTURE_WITHOUT_BUTTON;
        }   
    if(pushed(TRKDD)) 
        {
          state = TRKDD_CAPTURE_WITHOUT_BUTTON;
        }   
      if(pushed(TRKDA)) 
        {
          state = TRKDA_CAPTURE_WITHOUT_BUTTON;
        }   

    if(pushed(PBL))
    {
      state = WAIT_BYPASS_PBL_RELEASE ;
    } 
    if(pushed(PBR)) 
    {
      state = WAIT_STRAIGHT_PBR_RELEASE;
    }     
        if(pushed(PBW))
    {
      state = WAIT_BYPASS_BREEZEWAY_PBW_RELEASE;
    }

    break;


/********************************
*
* This section should address trains moving straight from West to East towards
* inner loop
*
********************************/

  case MOVE_SWITCH_STRAIGHT:      
    //Move switch motor to right (Straight for station), change lights to indicate which direction is safe to travel
    printMessage2("Now in Case Start - Green Button - MOVE_SWITCH_STRAIGHT - Lights & Motor to Right");
    lightoff(YELR);
    lightoff(GRNR);  
    light(REDR);
    motor(RIGHT, ON);    // Move switch to right  
    delay(MOTORDELAYMS);  
    motor(RIGHT, OFF) ;
    light(REDL);
    lightoff(REDR);
    light (GRNR);    
    // light(YELR); 
    lightoff(YELL);
    lightoff(YELR); 
//    printMessage2("Now in Case MOVE_SWITCH_STRAIGHT - Lights should be REDL & GRNR On");
    state = WAIT_STRAIGHT;
    break;                  

  case WAIT_STRAIGHT:           
    //Wait for next input from either Crossing, PBR, or PBL
    printMessage2("Now in Case WAIT_STRAIGHT - waiting for TRKDA, PBR, PBL, PBW");
     if(pushed(PBR)) 
    {
      state = WAIT_STRAIGHT_PBR_RELEASE;
      //Button pushed and move to next action.  
      //Block capture is complete and track dection enabled lights and bell
    }     
    if(pushed(PBL))
    {
      state = WAIT_BYPASS_PBL_RELEASE ;
    } 
    /**************************
    if(pushed(PBW))
    {
      state = WAIT_BYPASS_BREEZEWAY_PBW_RELEASE;
    }
    *****************/
    if(pushed(TRKDA)) 
    {
      state = TRAIN_PRESENT_STRAIGHT;
    }       

/*******************************
       if(pushed(TRKDC))
    {
      state = TRAIN_TRAVELING_WEST_EAST_ON_STRAIGHT;
    }
    if(pushed(TRKDD))
    {
      state = STRAIGHT_ALL_GREEN;
    }
**********************************/
    break;            

  case WAIT_STRAIGHT_PBR_RELEASE:
    //Once PBR is pushed wait until it is released to change lights and move the switch motor
    printMessage2("WAIT_STRAIGHT_PBR_RELEASE");
    lightoff(GRNL);
    lightoff(YELL);
    light(REDL);

  if (!pushed(PBR))
  {
    state = MOVE_SWITCH_STRAIGHT;
  }
  break;
  
  case TRAIN_PRESENT_STRAIGHT:
  //TRAIN Traveling from TRKDA into Crossing traveling West to East
    printMessage2("Now in Case Start - TRAIN_PRESENT_STRAIGHT - In Crossing");
    lightoff(GRNR);
    light(YELR);
    light(REDL);
    crossinglights (2,0,300);
        if(!pushed(TRKDA) && !pushed(TRKDC) && !pushed(TRKDD)) 
        {
          state = TRAIN_CLEAR_STRAIGHT;
        }             
    if(!pushed(TRKDA) && pushed(TRKDC)) 
        {
          state = TRAIN_IN_SWITCH_STRAIGHT;
        }             
    if(!pushed(TRKDA) && pushed(TRKDD)) 
        {
          state = TRAIN_IN_WRONG_SWITCH_4_STRAIGHT;
        }   
    break;






  case TRAIN_CLEAR_STRAIGHT:
    printMessage2("Now in Case Start - TRAIN_CLEAR_STRAIGHT - Exited Crossing");
    if(pushed(TRKDA)) 
        {
          state = TRAIN_IN_SWITCH_STRAIGHT;
        }   
    if(!pushed(TRKDC)) 
        {
          //state = TRAIN_IN_SWITCH_STRAIGHT;
          state = STRAIGHT_ALL_GREEN;
        }   
      if(pushed(TRKDD)) 
        {
          //state = TRAIN_IN_WRONG_SWITCH_4_STRAIGHT;
          state = STRAIGHT_ALL_GREEN;
        }   
    break;
    
  case TRAIN_IN_WRONG_SWITCH_4_STRAIGHT:
    //Train entered straight unexpectedly and once train clears will reset the signal heads and switch motor to straight state
    printMessage2("Now in Case Start - TRAIN_IN_WRONG_SWITCH_4_STRAIGHT");
    if(!pushed(TRKDD)) 
          {
            light(REDR);  //reset signal head so REDR turns on, REDL is already on
            lightoff(YELR);   //reset signal head and turn YELR off.  Tells Engineer to stop and not proceed switch is about to move
            state = MOVE_SWITCH_BYPASS;
          }   
    break; 

  case TRAIN_IN_SWITCH_STRAIGHT:      
    printMessage2("Now in Case Start - TRAIN_IN_SWITCH_STRAIGHT");
    //lightoff(YELR);
    //lightoff(REDR);
    //light(GRNR);
    if (!pushed(TRKDC)) 
    {
        state = WAIT_STRAIGHT;
        //state = STRAIGHT_ALL_GREEN;
    }            
    break;
       
  case TRAIN_IN_SWITCH_STRAIGHT_ERROR:  
    //printMessage("Now in Case Start - TRAIN_IN_SWITCH_STRAIGHT_ERROR",5);
    printMessage2("Now in Case Start - TRAIN_IN_SWITCH_STRAIGHT_ERROR");
    if(!pushed(TRKDD))
    {
      state = MOVE_SWITCH_BYPASS;
    } 
    break;        
    
  /************************************************************
   * TRAIN IN BYPASS SECTION OF TRACK
   * 
   ************************************************************/

case BYPASS_ALL_GREEN:
   //Change signals to ALL GREEN.
    printMessage2("Now in Case Start - BYPASS_ALL_GREEN");
    lightoff(REDL);
    lightoff(YELL);
    lightoff(REDR);
    lightoff(YELR);
    light(GRNL);
    light(GRNR);    
    state = BYPASS_ALL_GREEN_WAIT;
    break;     

case BYPASS_ALL_GREEN_WAIT:
  //Wait for next input from either Crossing, PBR, or PBL
    printMessage2("Now in Case BYPASS_ALL_GREEN_WAIT - waiting for TRKDA, TRKDE, PBR, PBL, PBW");
    if(pushed(TRKDA)) 
    {
      state = TRAIN_PRESENT_STRAIGHT;
    }       
    if(pushed(PBR)) 
    {
      state = WAIT_STRAIGHT_PBR_RELEASE;
    }     
    if(pushed(PBL))
    {
      state = WAIT_BYPASS_PBL_RELEASE ;
    } 
    if(pushed(PBW))
    {
      state = WAIT_BYPASS_BREEZEWAY_PBW_RELEASE;
    }
     if(pushed(TRKDE)) 
    {
      state = MOVE_SWITCH_STRAIGHT;
    }      
      if(pushed(TRKDC)) 
    {
      state = TRAIN_TRAVELING_WEST_EAST_ON_STRAIGHT;
    }      
    break;        

  case MOVE_SWITCH_BYPASS:
    printMessage2("Now in Case Start - MOVE_SWITCH_BYPASS");
    lightoff(YELL);
    lightoff(GRNL);  //added 8/19
    light(REDL);
    motor(LEFT, ON);    // Move switch to left  
    delay(MOTORDELAYMS);  
    motor(LEFT, OFF);
    light(REDL);  //Changed from REDR to REDL  07/21/2023
    lightoff(REDR);  //Changed from REDL to REDR 07/21/2023
    light(GRNR); //Changed from GRNL to GRNR 07/21/2023
    lightoff(YELR); //Changed from YELL to YELR 07/21/2023
    
    state = WAIT_BYPASS;
    break;

  case WAIT_BYPASS:
    printMessage2("Now in Case Start - WAIT_BYPASS - waiting for TRKDA, PBR, PBL");
    if(pushed(TRKDA))
    {
    state = TRAIN_PRESENT_BYPASS;
    }
    if(pushed(PBL)) 
    {
      state = WAIT_BYPASS_PBL_RELEASE; 
    } 
    if(pushed(PBR)) 
    {
    state = WAIT_STRAIGHT_PBR_RELEASE;
    }
    if(pushed(PBW)) 
    {
      state = WAIT_BYPASS_BREEZEWAY_PBW_RELEASE; 
    } 
    if(pushed(TRKDD))
    {
      //state = WAIT_BYPASS_BREEZEWAY;
      state = BYPASS_ALL_GREEN;
    }
    if(pushed(TRKDC))
    {
      //state = TRAIN_TRAVELING_WEST_EAST_ON_STRAIGHT;
      state = TRAIN_TRAVELING_WEST_EAST_ON_BYPASS;
    }
    break;

 case WAIT_BYPASS_PBL_RELEASE:
  printMessage2("Now in Case Start - WAIT_BYPASS_PBL_RELEASE");
    lightoff(GRNR); //Added 8/19
    lightoff(YELR);
    light(REDR);
 
  if (!pushed(PBL))
  {
    state = MOVE_SWITCH_BYPASS;
  }
  break;
  

  case TRAIN_PRESENT_BYPASS:
    printMessage2("Now in Case Start - TRAIN_PRESENT_BYPASS");
    lightoff(GRNL);
    lightoff(GRNR);  //Changed from GRNL to GRNR 07/21/2023
    light(YELR); //Changed from YELL to YELR 07/21/2023
    //light(REDR);  //Changed from REDR to REDL 07/21/2023
    crossinglights (2,0,300);
    //crossinglights (4,0,300);  // crossinglights (2,0,300) to crossinglights (4,0,300)
    if(!pushed(TRKDA) && !pushed(TRKDC) && !pushed(TRKDD)) 
        {
          state = TRAIN_CLEAR_BYPASS;
        }             
    if(!pushed(TRKDA) && pushed(TRKDC)) 
        {
          state = TRAIN_IN_WRONG_SWITCH_4_STRAIGHT;
        }             
    if(!pushed(TRKDA) && pushed(TRKDD)) 
        {
          state = TRAIN_IN_SWITCH_BYPASS;
        }   
   break;

  case TRAIN_CLEAR_BYPASS:
  printMessage2("Now in Case Start - TRAIN_CLEAR_BYPASS");
     if(pushed(TRKDA)) 
        {
          state = TRAIN_IN_SWITCH_BYPASS;
        }   
      if(pushed(TRKDC)) 
        {
          state = TRAIN_IN_WRONG_SWITCH_4_BYPASS;
          //state = STRAIGHT_ALL_GREEN; //added 07/21/2023
        } 
    if(pushed(TRKDD)) 
        {
         // state = TRAIN_IN_SWITCH_BYPASS; 
          state = BYPASS_ALL_GREEN;  //added 07/21/2023
        }   

    break;

    case TRAIN_IN_WRONG_SWITCH_4_BYPASS:
    //Train entered bypass unexpectedly and once train clears will reset the signal heads and switch motor to bypass state
    printMessage2("Now in Case Start - TRAIN_IN_WRONG_SWITCH_4_BYPASS");
    if(!pushed(TRKDC)) 
          {
            light(REDL);  
            //reset signal head so REDL turns on, REDR is already on
            lightoff(YELL);   
            //reset signal head and turn YELL off.  Tells Engineer to stop and not proceed switch is about to move
            state = MOVE_SWITCH_STRAIGHT;
          }   
    break; 


  case TRAIN_IN_SWITCH_BYPASS:
//    printMessage("Now in Case Start - TRAIN_IN_SWITCH_BYPASS",9);
    printMessage2("Now in Case Start - TRAIN_IN_SWITCH_BYPASS");
    lightoff(YELR); //Changed from YELL to YELR 07/21/2023
    lightoff(REDR); //Changed from REDL to REDR 07/21/2023
    light(GRNR);  //Changed GRNL to GRNR 07/21/2023
    if (!pushed(TRKDD))
    //if ((!pushed(TRKDD)) && (!pushed(TRKDC)))
    {
        state = WAIT_BYPASS;
    }
      break;
      
  case TRAIN_IN_SWITCH_BYPASS_ERROR:
//    printMessage("Now in Case Start - TRAIN_IN_SWITCH_BYPASS_ERROR",10);
    printMessage2("Now in Case Start - TRAIN_IN_SWITCH_BYPASS_ERROR");
    if(!pushed(TRKDC)) 
    {
      state = MOVE_SWITCH_STRAIGHT;
    } 
    break;

 /************************************************************
   * TRAIN IN BYPASS_BREEZEWAY SECTION OF TRACK
   * 
   ************************************************************/
  case MOVE_SWITCH_BYPASS_BREEZEWAY:
    printMessage2("Now in Case Start - MOVE_SWITCH_BYPASS_BREEZEWAY");
    lightoff(YELL);
    lightoff(GRNL);  //added 8/19
    light(REDL);
    motor(LEFT, ON);    // Move switch to left  
    delay(MOTORDELAYMS);  
    motor(LEFT, OFF);
    light(REDR);
    lightoff(REDL);
    light(GRNL); 
    lightoff(YELL); //added 8/19
    light(GRNL);  //added 8/19
    state = WAIT_BYPASS_BREEZEWAY;
    break;

  case WAIT_BYPASS_BREEZEWAY:
    printMessage2("Now in Case Start - WAIT_BYPASS_BREEZEWAY - waiting for TRKDD, PBR, PBL, PBW");

  /**********************  
    if(pushed(PBL))
    {
    state = WAIT_BYPASS_PBL_RELEASE;
    }
    if(pushed(PBR)) 
    {
    state = WAIT_STRAIGHT_PBR_RELEASE;
    }
    if(pushed(TRKDC))
    {
      state = TRAIN_TRAVELING_WEST_EAST_ON_STRAIGHT;
    }
  
  *********************************/
  
    if(pushed(TRKDD))   
    //After Capture Button (PBW) pushed wait for TRAIN to enter block.  
    //TRKDD indicates TRAIN has enter block and goes to next state
    
    {
    state = TRAIN_PRESENT_BYPASS_BREEZEWAY;
    }
    if(pushed(TRKDE))
    {
      state = TRAIN_CLEAR_BYPASS_BREEZEWAY_ALL_GREEN;
    }
    break;

 case WAIT_BYPASS_BREEZEWAY_PBW_RELEASE:
  printMessage2("Now in Case Start - WAIT_BYPASS_BREEZEWAY_PBW_RELEASE");
    lightoff(GRNR); //Added 8/19
    lightoff(YELR);
    light(REDR);
 
  if (!pushed(PBW))  //Changed from PBL to PBW
  {
    state = MOVE_SWITCH_BYPASS_BREEZEWAY;
  }
  break;
  

  case TRAIN_PRESENT_BYPASS_BREEZEWAY:
    printMessage2("Now in Case Start - TRAIN_PRESENT_BYPASS_Breezeway");
    lightoff(REDL);
    lightoff(YELL);
    lightoff(GRNL);
    lightoff(YELR);
    lightoff(GRNR);
    light(REDR);
    light(YELL); 
    crossinglights (2,0,300);
    
        if(!pushed(TRKDD))
        {
          state = TRAIN_CLEAR_BYPASS_BREEZEWAY;
        }      
      if(!pushed(TRKDA) && pushed(TRKDC)) 
        {
          state = TRAIN_IN_WRONG_SWITCH_4_STRAIGHT;
        }             
      if(pushed(TRKDE))
       {
      state = TRAIN_CLEAR_BYPASS_BREEZEWAY_ALL_GREEN;
      }

   break;

case TRAIN_CLEAR_BYPASS_BREEZEWAY_ALL_GREEN:
  //Change Signals to ALL GREEN.  Waiting for input from Push Button or Track
    //Added 07/09/2023
    printMessage2("Now in Case Start - TRAIN_CLEAR_BYPASS_BREEZEWAY_ALL_GREEN");
    lightoff(REDL);
    lightoff(YELL);
    lightoff(REDR);
    lightoff(YELR);
    light(GRNL);
    light(GRNR);    

    state = TRAIN_CLEAR_BYPASS_BREEZEWAY;
    break;                  

  case TRAIN_CLEAR_BYPASS_BREEZEWAY:
  printMessage2("Now in Case Start - TRAIN_CLEAR_BYPASS_BREEZEWAY");

    if(pushed(PBR)) 
    {
      state = WAIT_STRAIGHT_PBR_RELEASE;
    }     
    if(pushed(PBW))
    {
      state = WAIT_BYPASS_BREEZEWAY_PBW_RELEASE;
    }        
    if(pushed(TRKDA))
      {
        state = TRAIN_CLEAR_BYPASS_BREEZEWAY_ALL_GREEN;
      }
//    if(pushed(TRKDC)) 
//        {
//          state = TRAIN_IN_WRONG_SWITCH_4_BYPASS_BREEZEWAY;
//        }
     if(pushed(TRKDE))
        {
          state = WAIT_BYPASS_BREEZEWAY;
        }   
    if(pushed(TRKDD)) 
        {
//          state = TRAIN_IN_SWITCH_BYPASS_BREEZEWAY;
//         state = TRAIN_PRESENT_BYPASS_BREEZEWAY;
         state = TRAIN_IN_WRONG_SWITCH_4_BYPASS_BREEZEWAY;
        }   

  
    
    break;

    case TRAIN_IN_WRONG_SWITCH_4_BYPASS_BREEZEWAY:
    //Train entered bypass unexpectedly and once train clears will reset the signal heads and switch motor to bypass state
    printMessage2("Now in Case Start - TRAIN_IN_WRONG_SWITCH_4_BYPASS_BREEZEWAY");
    if(!pushed(TRKDC)) 
          {
            light(REDL);  
            //reset signal head so REDL turns on, REDR is already on
            lightoff(YELL);   
            //reset signal head and turn YELL off.  Tells Engineer to stop and not proceed switch is about to move
            state = MOVE_SWITCH_STRAIGHT;
          }   
    break; 


  case TRAIN_IN_SWITCH_BYPASS_BREEZEWAY:
//    printMessage("Now in Case Start - TRAIN_IN_SWITCH_BYPASS",9);
    printMessage2("Now in Case Start - TRAIN_IN_SWITCH_BYPASS_BREEZEWAY");
    //lightoff(YELL);
    //light(GRNL);
    if (!pushed(TRKDE))
    //if ((!pushed(TRKDD)) && (!pushed(TRKDC)))
    {
//        state = WAIT_BYPASS_BREEZEWAY;
        state = TRAIN_PRESENT_BYPASS_BREEZEWAY;
    }
      break;
      
  case TRAIN_IN_SWITCH_BYPASS_ERROR_BREEZEWAY:
//    printMessage("Now in Case Start - TRAIN_IN_SWITCH_BYPASS_ERROR",10);
    printMessage2("Now in Case Start - TRAIN_IN_SWITCH_BYPASS_ERROR_BREEZEWAY");
    if(!pushed(TRKDC)) 
    {
      state = MOVE_SWITCH_STRAIGHT;
    } 
    break;

/************************************************************
* TRAIN TRAVELING West to East on Straight
* Most likely from crossover
************************************************************/


case TRAIN_TRAVELING_WEST_EAST_ON_BYPASS:      
    //Change Signals to ALL RED.  Trains Traveling West to East on Straight Towards Yard
    //Added 07/09/2023
    printMessage2("Now in Case Start - TRAIN_TRAVELING_WEST_EAST_ON_STRAIGHT");
    lightoff(YELR);
    lightoff(GRNR);  //Added 8/19
    light(REDR);
    lightoff(YELL);
    lightoff(GRNL);  //Added 8/19
    light(REDL);

    state = TRAIN_TRAVELING_WEST_EAST_ON_STRAIGHT_WAIT;
    break;                  

  case TRAIN_TRAVELING_WEST_EAST_ON_BYPASS_WAIT:           
    //Wait for next input from either Crossing, PBR, or PBL
    printMessage2("Now in Case TRAIN_TRAVELING_WEST_EAST_ON_STRAIGHT_WAIT - waiting for TRKDA, TRKDE, PBR, PBL, PBW");
    if(pushed(TRKDA)) 
    {
      state = TRAIN_PRESENT_STRAIGHT;
    }       
    if(pushed(PBR)) 
    {
      state = WAIT_STRAIGHT_PBR_RELEASE;
    }     
    if(pushed(PBL))
    {
      state = WAIT_BYPASS_PBL_RELEASE ;
    } 
    if(pushed(PBW))
    {
      state = WAIT_BYPASS_BREEZEWAY_PBW_RELEASE;
    }
         if(pushed(TRKDE)) 
    {
      //state = MOVE_SWITCH_STRAIGHT;
      state = STRAIGHT_ALL_GREEN;
    }       

    break;      

/************************************************************
* TRAIN TRAVELING West to East on Straight
* Most likely from crossover
************************************************************/


case TRAIN_TRAVELING_WEST_EAST_ON_STRAIGHT:      
    //Change Signals to ALL RED.  Trains Traveling West to East on Straight Towards Yard
    //Added 07/09/2023
    printMessage2("Now in Case Start - TRAIN_TRAVELING_WEST_EAST_ON_STRAIGHT");
    lightoff(YELR);
    lightoff(GRNR);  //Added 8/19
    light(REDR);
    lightoff(YELL);
    lightoff(GRNL);  //Added 8/19
    light(REDL);
    crossinglights (2,0,300);

    state = TRAIN_TRAVELING_WEST_EAST_ON_STRAIGHT_WAIT;
    break;                  

  case TRAIN_TRAVELING_WEST_EAST_ON_STRAIGHT_WAIT:           
    //Wait for next input from either Crossing, PBR, or PBL
    printMessage2("Now in Case TRAIN_TRAVELING_WEST_EAST_ON_STRAIGHT_WAIT - waiting for TRKDA, TRKDE, PBR, PBL, PBW");
    if(pushed(TRKDA)) 
    {
      //state = TRAIN_PRESENT_STRAIGHT;
      state = STRAIGHT_ALL_GREEN;
    }       
    if(pushed(PBR)) 
    {
      state = WAIT_STRAIGHT_PBR_RELEASE;
    }     
    if(pushed(PBL))
    {
      state = WAIT_BYPASS_PBL_RELEASE ;
    } 
    if(pushed(PBW))
    {
      state = WAIT_BYPASS_BREEZEWAY_PBW_RELEASE;
    }
        if(pushed(TRKDE)) 
    {
    //state = MOVE_SWITCH_STRAIGHT;
    state = STRAIGHT_ALL_GREEN;
    }       
    if(pushed(TRKDD))
    {
      state = STRAIGHT_ALL_GREEN; //May need to change this but resets everything to green
    }
    if(pushed(TRKDC))
    {
      state = TRAIN_TRAVELING_WEST_EAST_ON_STRAIGHT; //Detect train
    }
    break;      




      default: // Error condition! Should never get here!!!!
//      printMessage("Now in Case Start - DEFAULT - BAD",11);
      printMessage2("Now in Case Start - DEFAULT - BAD");
      digitalWriteWrapper(YELL, HIGH);
      digitalWriteWrapper(GRNL, HIGH); 
      digitalWriteWrapper(REDR, HIGH);
      digitalWriteWrapper(YELR, HIGH);
      digitalWriteWrapper(GRNR, HIGH);
      break;
} //End of State?

} //End of While?

} // void loop()  

/************************************************************************  
 *
 *  Turn all lights on one side on for dly MS, then turn them off 
 *  
 ************************************************************************/
void all(int side, int dly)
{
  if(side == LEFT)
  {
    
    digitalWriteWrapper(REDL, HIGH);
    digitalWriteWrapper(YELL, HIGH);
    digitalWriteWrapper(GRNL, HIGH);
    digitalWriteWrapper(GRDLTL, HIGH);
    digitalWriteWrapper(BELL, HIGH);
    delay(dly);
    digitalWriteWrapper(REDL, LOW);
    digitalWriteWrapper(YELL, LOW);
    digitalWriteWrapper(GRNL, LOW);
    digitalWriteWrapper(GRDLTL, LOW);
    digitalWriteWrapper(BELL, LOW);
  }
  if(side == RIGHT)
  {
    digitalWriteWrapper(REDR, HIGH);
    digitalWriteWrapper(YELR, HIGH);
    digitalWriteWrapper(GRNR, HIGH);
    digitalWriteWrapper(GRDLTR, HIGH);
    digitalWriteWrapper(BELL, HIGH);
    delay(dly);
    digitalWriteWrapper(REDR, LOW);
    digitalWriteWrapper(YELR, LOW);
    digitalWriteWrapper(GRNR, LOW);
    digitalWriteWrapper(GRDLTR, LOW);
    digitalWriteWrapper(BELL, LOW);

  }
} // void all(int side, int dly)

/************************************************************************  
 *
 *  Turn the motor actuator on or off in the desired direction 
 *  
 ************************************************************************/
void motor(int side, int action)
{
  if(side == LEFT)
  {
    digitalWriteWrapper(MOTORL, action);
  }
  if(side == RIGHT)
  {
    digitalWriteWrapper(MOTORR, action);
  }
} // void motor(int side, int action)

/************************************************************************  
 *
 *  Turn on 'which' light 
 *  
 ************************************************************************/
void light(int which)
{
  digitalWriteWrapper(which, HIGH);
} // void light(int which)

/************************************************************************  
 *
 *  Turn off 'which' light 
 *  
 ************************************************************************/
void lightoff(int which)
{
  digitalWriteWrapper(which, LOW);
} // void lightoff(int which)

/************************************************************************  
 *
 *  Is button pushed return true or false 
 *  
 ************************************************************************/
int pushed(int button)
{
  return (digitalRead(button) == PUSHED);
} // int pushed(int button)

 /************************************************************************  
 *
 *  Blinks which light count times each on or off is duration long 
 *  
 ************************************************************************/
void blinker(int count, int which, int duration) 
{
  for(int i=0; i<count; i++)  // Blink code
  {
    light(which);
    delay(duration);
    lightoff(which);
    delay(duration);

  }
} // void blinker(int count, int which, int duration)

/**********************************************************
 * 
 * Section to cause grade crossing lights to flash
 * 
 * ********************************************************/
void flasher(int count, int which, int duration) 
{
  for(int i=0; i<count; i++)  // Blink code
  {
    light(GRDLTL);
    delay(duration);
    lightoff(GRDLTL);
    delay(duration);
    light(GRDLTR);
    delay(duration);
    lightoff(GRDLTR);
    delay(duration);
  }
} // void blinker(int count, int which, int duration)


/*********************************************************
 * Section to actuate bell
 * 6/21/2021- Added Lights and Bell together so they are in sync
 *********************************************************/

void crossinglights(int count, int which, int duration)
{
  for(int i=0; i<count; i++)  // Blink code
  {
/****Temporary green light on / off for grass crossing simulation
    light(GRNL);
    delay(duration);
    lightoff(GRNL);
    light(GRNR);
    delay(duration);
    lightoff(GRNR);
    light(GRNL);
    delay(duration);
    lightoff(GRNL);
    light(GRNR);
    delay(duration);
    lightoff(GRNR);
    ***************/
    
    
    
    digitalWriteWrapper (BELL, HIGH);
    light(GRDLTL);
    delay(duration);
    digitalWriteWrapper (BELL, LOW);
    lightoff(GRDLTL);
    light(GRDLTR);
    delay(duration);
    digitalWriteWrapper (BELL, HIGH);
    light(GRDLTL);
    lightoff(GRDLTR);
    delay(duration);
    digitalWriteWrapper (BELL, LOW);
    lightoff(GRDLTL);
    light(GRDLTR);
    delay(duration);
    lightoff(GRDLTR);
  }
} // void blinker(int count, int which, int duration)

/*************************************************
 * Print to monitor to see what section is active
 * 
 *************************************************/
 void printMessage(char *message, int msgnumber)
{
    static int oldmsgnumber = 0;
    if (msgnumber != oldmsgnumber)
    {
        Serial.println(message);
        oldmsgnumber = msgnumber;
    }
}

/************************************************* 
 * Print to monitor to see what section is active
 * 
 *************************************************/
 void printMessage2(char *message)
{
  static char oldmsg[256] ={0};     // Create a 256 character buffer space and set it to empty

    if (strcmp(message, oldmsg) != 0) // If the new message is not the same as the old message
    {
        Serial.print("Elapsed time since last action: ");
        unsigned long elapsedtime = millis()/1000L;  // Convert to seconds
        unsigned long minutes = elapsedtime / 60L; // Extract minutes
        unsigned long seconds = elapsedtime % 60L; // Extract leftover seconds (% is modulus or remainder operator)
        Serial.print(minutes);
        Serial.print(":");
        Serial.print(seconds);
        Serial.print(" ==> ");
        Serial.println(message);      // Print it   
        strcpy(oldmsg, message);    // Copy it as old message
    }
} 

void digitalWriteWrapper(int pin, int val)
{
  Serial.print("Digital Write:");
  Serial.print(pin);
  Serial.print(" ");
  Serial.println(val);
  digitalWrite(pin, val);
}