// Reverse Loop Control
// Norbert Doerry
// December 2019
// Rev of January 2020 -- Fix Switch coil problem
//
//  F0 = C4 (mid) connects to F6 and F4 (+ towards F6)
//  F1 = B4 (mid) connects to F6 and F5 (+ towards F6)
//  F2 = C3 - D3 - (D4)  (back) connects to F6 and F4  (+ towards F4)
//  F3 = A3 - (A4) - B3  (back) connects to F5 and F6  (+ towards F6)
//  F4 = C5 - D5 (front) connects to F2, F0 and F7     (+ towards F7 and F0)
//  F5 = A5 - B5 (front) connects to F3, F1 and F7     (+ towards F3)
//  F6 = B6 - C6 (back) connects to F3, F1, F0 and F2  (+ towards F0 and F2)
//  F7 = B7 - C7 (front) connects to F5 and F4         (+ towards F5)
//
//  If in F1, then S-BR should be set to CURVE and S-BF should be set to CURVE
//  If in F7, then S-BF should be set to STRAIGHT and S-CF should be set to STRAIGHT
//  If in F0, then S-CF should be set to CURVE and S-CR should be set to CURVE
//  If in F2, then S-CR should be set to STRAIGHT
//  If in F3, then S-BR should be set to STRAIGHT
//  If in F4 and Last was F2, then set S-CF based on switch (if always Str or always crv, set independent of Last)
//  If in F2 and Last was F4, then set S-BR based on switch (if always Str or always crv, set independent of Last)
//  If in F3 and Last was F5, then set S-CR based on switch (if always Str or always crv, set independent of Last)
//  If in F5 and Last was F3, then set S-BF based on switch (if always Str or always crv, set independent of Last)
//  If in F1 and Last was F5, then set S-CR based on switch (if always Str or always crv, set independent of Last) 
//  If in F0 and Last was F4, then set S-BR based on switch (if always Str or always crv, set independent of Last) 
// 
//  
//  If in F0, then set F6 polarity to -1 and F4 polarity to 1
//  If in F1, then set F6 polarity to 1 and F5 polarity to -1
//  If in F2, then set F4 polarity to 1 and F6 polarity to 1
//  If in F3, then set F5 polarity to 1 and F6 polarity to 1
//  If in F4, then set F2 polrity to 1, F7 polarity to 1, and F0 polarity to 1
//  If in F5, then set F3 polarity to 1, F1 polarity to -1, and F7 polarity to 1
//  If in F6, then set F3 polarity to 1, F1 polarity to 1, F0 polarity to -1 and F2 polarity to 1
//  If in F7, then set F4 polarity to 1, F5 polarity to 1

//
//  logic for determing which block one is in.
//
//
//  Initially state_this_block = F_UNKOWN and state_last_block = F_UNKNOWN
//   when a block turns on, then state_this_block becomes the ID of the block that turned on
//   state_this_block does not change until its block turns off (F_UNKNOWN is presumed to be off at all times)
//   at tht time, state_last_block is set eaual to state_this_block
//   state_this_block is set equal to a block that is on, or to F_UNKNOWN if no blocks are on.
//        the adjacent blocks are checked first, then non-adjacent blocks 

//  port assignments

//  Block IDs
//
const int F0 = 0;
const int F1 = 1;
const int F2 = 2;
const int F3 = 3;
const int F4 = 4;
const int F5 = 5;
const int F6 = 6;
const int F7 = 7;
const int F_UNKNOWN = 8;

int state_direction[8];  // 0 = A 1 =  B ; use this for logic for the current polarity of blocks
int state_direction_cmd[8]; // use this to comamnd the new polarity of blocks.
int state_this_block = F_UNKNOWN;   // use this for logic for which block the train is in
int state_last_block = F_UNKNOWN;   // use this for logic for which block the train was previously in


const int COIL_PULSE_TIME = 500; // ms   
const unsigned long DI_DEBOUNCE_TIME = 20; // ms


int switch_cmd_str[4];  // use this for logic for commanding switch to straight (set to 1 to send command, set to 0 when command executed)
int switch_cmd_crv[4];  // use this for logic for commanding switch to curve (set to 1 to send command, set to 0 when command executed)
unsigned long switch_coil_str_on_time[4]; // time the straight coil turned on
unsigned long switch_coil_crv_on_time[4]; // time the curve coil turned on
int switch_coil_str[4]; // set to 1 if the coil is on, set to 0 if coil is off
int switch_coil_crv[4]; // set to 1 if the coil is on, set to 0 if coil is off
int switch_coil_block;  // if 1, then wait to turn on coil

int state_switch_raw[4];  // raw value from switch position sensor
unsigned long state_switch_transition_time[4];  // time raw value was first different from debounce value
int state_switch_transition[4]; // = 1 if raw value is differnt than debounce value = 0 if the same
int state_switch_debounce[4]; // value of switch sensor after debouncing
int state_switch_health[4]; //= 0 if indicator agrees with coils for STR and CRV, = 1 if indicator does not agree for last time
int state_switch_logic_dir[4]; // use this for logic, same as debounce

// index for switches
const int S_BF = 0;
const int S_BR = 1;
const int S_CR = 2;
const int S_CF = 3;

// switch positions
const int S_CRV = 1;
const int S_STR = 0;
const int S_UNK = 2;

// rotary switches

// switch_control states

const int SC_UNK = 0; 
const int SC_RANDOM = 1;
const int SC_NC = 2;
const int SC_OPP = 3;
const int SC_STRAIGHT = 4;
const int SC_CURVE = 5;

const unsigned long AI_DEBOUNCE_TIME = 20; // ms

int SC_val[4];   // use this array for logic - after debounding of the switch control rotary switch
int SC_raw[4];   // raw value from sensor
int SC_d_raw[4]; // raw value  converted to one of the switch_control states
int SC_d_tval[4]; // d raw value if different from SC_val
unsigned long SC_transition_time[4]; // Time since d_tval changed

// Polarity push buttons

int pushbutton_A_raw = 0;
int pushbutton_B_raw = 0;
unsigned long pushbutton_A_transition_time = 0;
unsigned long pushbutton_B_transition_time = 0;
int pushbutton_A_transition; // =1 if in transtion
int pushbutton_B_transition;
int pushbutton_A_val = 0;
int pushbutton_B_val = 0;


// port assignments
//
// Analog Inputs
// Current Sensors
const int AI_F0_CT = A0;
const int AI_F1_CT = A1;
const int AI_F2_CT = A2;
const int AI_F3_CT = A3;
const int AI_F4_CT = A4;
const int AI_F5_CT = A5;
const int AI_F6_CT = A6;
const int AI_F7_CT = A7;
//
// Rotary Switch Position
const int AI_T_BF = A8;
const int AI_T_BR = A9;
const int AI_T_CR = A10;
const int AI_T_CF = A11;
//
// Digital Inputs
//
// switch position input 
const int DI_SP_BF = 24;
const int DI_SP_BR = 25;
const int DI_SP_CF = 26;
const int DI_SP_CR = 27;
//
// Polarity pushbuttons
const int DI_PA = 28;
const int DI_PB = 29;
//  
// ports 30 and 31 reserved for future use
//
// Digital Outputs
//
// Directional control
const int DO_F0_DC = 32;
const int DO_F1_DC = 33;
const int DO_F2_DC = 34;
const int DO_F3_DC = 35;
const int DO_F4_DC = 36;
const int DO_F5_DC = 37;
const int DO_F6_DC = 38;
const int DO_F7_DC = 39;
//
// switch coil power
//
const int DO_S_BF_S = 40;
const int DO_S_BF_C = 41;
const int DO_S_BR_S = 42;
const int DO_S_BR_C = 43;
const int DO_S_CF_S = 44;
const int DO_S_CF_C = 45;
const int DO_S_CR_S = 46;
const int DO_S_CR_C = 47;
//
//
// time

unsigned long now_time;


// display time

unsigned long display_last_update_time = 0;
const unsigned long DISPLAY_UPDATE_TIME = 350; //ms

// analog port data (current sensors)

int state_block[8];  //1 for train present 0 for train not present

int AI_CT_raw[8]; // raw value from analog reading
int AI_CT_nbr_hits[8];  // number of readings outside of "zero band"
int AI_CT_transition[8]; // = 1 if in transition, = 0 otherwise (d_raw not the same as state_block)
int AI_CT_d_raw[8];   // tentative value for the digital value - if CS_MIN_HITS achieved in CS_TIME_WINDOW
unsigned long AI_CT_window_start_time[8]; 
unsigned long AI_CT_trans_time[8];  

// Current Sensor trip parameters

const int CS_MIN_HITS = 10;
const int CS_TIME_WINDOW = 50;  // milliseconds
const int CURRENT_LOW_VALUE  = 492;  //   2.5 volts is no current for sensor = 512 -- 0  = 0 Volts 1023 = 5 volts
const int CURRENT_HIGH_VALUE = 532;  // 

const int CURRENT_TIME_LOW_TO_HIGH = 150;  // number of ms d_raw has to be high before registering a high in state_block
const int CURRENT_TIME_HIGH_TO_LOW = 300;  // number of ms d_raw has to be low before registering a low in state_block

//

void setup() {

 int i;

 unsigned long rseed;
 
 // Directional Control
 
 pinMode(DO_F0_DC,OUTPUT);
 pinMode(DO_F1_DC,OUTPUT);
 pinMode(DO_F2_DC,OUTPUT);
 pinMode(DO_F3_DC,OUTPUT); 
 pinMode(DO_F4_DC,OUTPUT); 
 pinMode(DO_F5_DC,OUTPUT); 
 pinMode(DO_F6_DC,OUTPUT);
 pinMode(DO_F7_DC,OUTPUT); 

 // Switch Coil Power

 pinMode(DO_S_BF_S,OUTPUT);
 pinMode(DO_S_BF_C,OUTPUT);
 pinMode(DO_S_BR_S,OUTPUT);
 pinMode(DO_S_BR_C,OUTPUT);
 pinMode(DO_S_CR_S,OUTPUT);
 pinMode(DO_S_CR_C,OUTPUT);
 pinMode(DO_S_CF_S,OUTPUT);
 pinMode(DO_S_CF_C,OUTPUT);

 // Switch indicator
  
 pinMode(DI_SP_BF,INPUT); 
 pinMode(DI_SP_BR,INPUT);  
 pinMode(DI_SP_CR,INPUT);  
 pinMode(DI_SP_CF,INPUT);  

 // Polarity Pushbutton

 pinMode(DI_PA,INPUT);
 pinMode(DI_PB,INPUT);
 
 // ensure pull up resistor is off for digital inputs
 
 digitalWrite(DI_SP_BF,0);
 digitalWrite(DI_SP_BR,0);
 digitalWrite(DI_SP_CR,0);
 digitalWrite(DI_SP_CF,0);
 
 digitalWrite(DI_PA,0);
 digitalWrite(DI_PB,0);

 // set the direction to A

 digitalWrite(DO_F0_DC,0);
 digitalWrite(DO_F1_DC,0);
 digitalWrite(DO_F2_DC,0);
 digitalWrite(DO_F3_DC,0);
 digitalWrite(DO_F4_DC,0);
 digitalWrite(DO_F5_DC,0);
 digitalWrite(DO_F6_DC,0);
 digitalWrite(DO_F7_DC,0);

 // turn all coils off

 digitalWrite(DO_S_BF_S,0);
 digitalWrite(DO_S_BF_C,0);
 digitalWrite(DO_S_BR_S,0);
 digitalWrite(DO_S_BR_C,0);
 digitalWrite(DO_S_CR_S,0);
 digitalWrite(DO_S_CR_C,0);
 digitalWrite(DO_S_CF_S,0);
 digitalWrite(DO_S_CF_C,0);

 // start the serial port
 
 Serial.begin(9600);
 // clear the screen
 Serial.write(254);
 Serial.write(81);

 // set the random seed

  rseed = analogRead(AI_F0_CT);
  randomSeed(rseed);
  
 // set the default values for the arrays  

 // directional control

 for (i=0 ; i < 8 ; i++)
  {
     state_direction[8] = 0;  // 0 = A 1 =  B ; use this for logic for the current polarity of blocks
     state_direction_cmd[8] = 0; // use this to comamnd the new polarity of blocks.
     state_block[i] = 0;  // assume all blocks are off
  }

 // block detection

 for (i=0 ; i < 8 ; i++)
  {
   AI_CT_raw[i] = 512;  // middle of range or zero
   AI_CT_nbr_hits[i] = 0;
   AI_CT_transition[i] = 0;
   AI_CT_d_raw[i] = 0;
   AI_CT_window_start_time[i] = 0;
   AI_CT_trans_time[i] = 0;
  }

 // switches

 for (i=0 ; i < 4 ; i++)
  {
   switch_cmd_str[i] = 0;  // use this for logic for commanding switch to straight (set to 1 to send command, set to 0 when command executed)
   switch_cmd_crv[i] = 0;  // use this for logic for commanding switch to curve (set to 1 to send command, set to 0 when command executed)
   switch_coil_str_on_time[i] = 0; // time the straight coil turned on
   switch_coil_crv_on_time[i] = 0; // time the curve coil turned on
   switch_coil_str[i] = 0; // set to 1 if the coil is on, set to 0 if coil is off
   switch_coil_crv[i] = 0; // set to 1 if the coil is on, set to 0 if coil is off

   state_switch_raw[i] = S_UNK;  // raw value from switch position sensor
   state_switch_transition_time[i] = 0;  // time since raw value is different from debounced
   state_switch_transition[i] = 0; // = 1 if raw is diffrent from debounced 
   state_switch_debounce[i] = S_UNK; // value of switch sensor after debouncing  
   state_switch_health[i] = 0; // = 0 if indicator agrees with coils for STR and CRV, = 1 if the do not agree
   state_switch_logic_dir[i] = S_UNK; // use this for logic, same as state_switch_debounce
  }

switch_coil_block = 0;
  
}

void loop() {
  // Read the input values
  
  get_raw();
  
  process_switch_dir();

  process_block_detection();

  process_rotary_switches();
  
  process_polarity_pushbuttons();
  
  calc_state();
  
  process_switch_coil();
  
  process_block_direction();
  
  write_to_ports();
 
  write_display();
  
   
}

void get_raw(void)
{
  now_time = millis();


 state_switch_raw[S_BF] = digitalRead(DI_SP_BF);
 state_switch_raw[S_BR] = digitalRead(DI_SP_BR);
 state_switch_raw[S_CR] = digitalRead(DI_SP_CR);
 state_switch_raw[S_CF] = digitalRead(DI_SP_CF);
 
 pushbutton_A_raw = digitalRead(DI_PA);
 pushbutton_B_raw = digitalRead(DI_PB);

 AI_CT_raw[0] = analogRead(AI_F0_CT);
 AI_CT_raw[1] = analogRead(AI_F1_CT);
 AI_CT_raw[2] = analogRead(AI_F2_CT);
 AI_CT_raw[3] = analogRead(AI_F3_CT);
 AI_CT_raw[4] = analogRead(AI_F4_CT);
 AI_CT_raw[5] = analogRead(AI_F5_CT);
 AI_CT_raw[6] = analogRead(AI_F6_CT);
 AI_CT_raw[7] = analogRead(AI_F7_CT);


 SC_raw[S_BF] = analogRead(AI_T_BF);
 SC_raw[S_BR] = analogRead(AI_T_BR);
 SC_raw[S_CR] = analogRead(AI_T_CR);
 SC_raw[S_CF] = analogRead(AI_T_CF);

}


void  process_switch_dir(void)
{
 unsigned long time_diff;
 int i;

 for (i = 0 ; i < 4 ; i++)
  {
   if (state_switch_debounce[i] == S_UNK)  // initialization -> first time through
    {
     state_switch_transition_time[i] = 0;  // time since raw value is different from debounced
     state_switch_transition[i] = 0; // = 1 if raw is diffrent from debounced 
     state_switch_debounce[i] = state_switch_raw[i]; // value of switch sensor after debouncing  
     state_switch_health[i] = 0; // = 0 if indicator agrees with coils for STR and CRV, = 1 if indicator does not agree
     state_switch_logic_dir[i] = state_switch_debounce[i]; // use this for logic, normally same as above, unless equal to S_BROKEN, in which case is last commanded
    }
   else if (state_switch_raw[i] == state_switch_debounce[i])  // no change in raw from debounce, reset transition
    {
     state_switch_transition_time[i] = 0;  // time since raw value is different from debounced
     state_switch_transition[i] = 0; // = 1 if raw is diffrent from debounced 
    }
   else if (state_switch_transition[i] == 0) // have a mismatch, but this is the first one
    {
     state_switch_transition_time[i] = now_time;
     state_switch_transition[i] = 1;
    }
   else   // in transition, see if completed 
    {
     if (now_time <= state_switch_transition_time[i]) // handle case where time rolled over
      {
       time_diff = 0;
       state_switch_transition_time[i] = now_time; 
      }
     else
      time_diff = now_time - state_switch_transition_time[i];

     if (time_diff >= DI_DEBOUNCE_TIME) // transition complete
      {
       state_switch_debounce[i] = state_switch_raw[i];
       state_switch_transition[i] = 0;
       state_switch_transition_time[i] = 0; 
      }
     
    }
 
   state_switch_logic_dir[i] = state_switch_debounce[i];  
  
  }
 
}

// block detection uses a current sensor where the zero value is 512, 
// hence the block is off (0) in a band around 512, it is on above a threshold value and below another thrshold value
// For a.c. current, in a subwindow, the current will fall within the band around 512, hence, a one is scored if a specific
// number of current measurements of high enough magnitude are detected.
// If enough hits are made, this is tranlated to a digital raw value
// If the digital raw value stays constant for long enough, then the actual state value changes.
// The time to transition from on to off is longer than the time to transtion from off to on to ensure another block is ON when
// a block turns off.
  
void process_block_detection(void)
{
   unsigned long  time_diff;  
   int i;

   // convert to digital raw;

   for (i=0 ; i < 8 ; i++)
    {
      if (AI_CT_window_start_time[i] == 0)
       {
         AI_CT_window_start_time[i] = now_time;
         AI_CT_nbr_hits[i] = 0;
       }
      if (AI_CT_raw[i] <= CURRENT_LOW_VALUE || AI_CT_raw[i] >= CURRENT_HIGH_VALUE)
       AI_CT_nbr_hits[i]++;

      // calcuate time since start of sub-window 

      if (now_time <= AI_CT_window_start_time[i])
       {
        time_diff = 0;
        AI_CT_window_start_time[i] = now_time; 
       }
      else
       time_diff = now_time - AI_CT_window_start_time[i];

      // see if sub-window is complete  

      if (time_diff >= CS_TIME_WINDOW)
       {
         if (AI_CT_nbr_hits[i] >= CS_MIN_HITS)
          AI_CT_d_raw[i] = 1;
         else
          AI_CT_d_raw[i] = 0; 

         AI_CT_window_start_time[i] = 0;  
       }

     // see if the transition window has not started 

     if (AI_CT_d_raw[i] == state_block[i])
      {
       AI_CT_transition[i] = 0;
       AI_CT_trans_time[i] = 0;
      }
     else if (AI_CT_transition[i] == 0)  // start possible transition
      {
       AI_CT_transition[i] = 1;
       AI_CT_trans_time[i] = now_time;
      }
     else // in transition window
      {
       if (now_time <= AI_CT_trans_time[i])
        {
         time_diff = 0;
         AI_CT_trans_time[i] = now_time;
        }
       else
        time_diff = now_time - AI_CT_trans_time[i];

       if (state_block[i] == 0 && time_diff > CURRENT_TIME_LOW_TO_HIGH)
        {
         state_block[i] = 1;
         AI_CT_transition[i] = 0;
         AI_CT_trans_time[i] = 0;
        }

       else if (state_block[i] == 1 && time_diff > CURRENT_TIME_HIGH_TO_LOW)
        { 
         state_block[i] = 0;
         AI_CT_transition[i] = 0;
         AI_CT_trans_time[i] = 0;
        }
      }
    
    }
 
}

// decode the rotary switch position 
// 1. Random (0 to 1 volt)  = 1024 / 5 =  205
// 2. No change (1 to 2 volts) = 2 * 1024 / 5 = 410
// 3 Always Opposite (2 to 3 volts) = 3 * 1024 / 5 = 614
// 4. Always Straight (3 to 4 volts) = 4 * 1024 / 5 = 819
// 5. Always Curve (4 to 5 volts)  = 1024

  
void process_rotary_switches(void)
{
  unsigned long time_diff;
  int i;

  for (i = 0 ; i < 4 ; i++)
   {
     if (SC_raw[i] < 205) 
      SC_d_raw[i] = SC_RANDOM;
     else if (SC_raw[i] < 410)
      SC_d_raw[i] = SC_NC;
//       else if (SC_raw[i] < 670)   // does not work on all of them (near limit
//     else if (SC_raw[i] < 640)   // works
      else if (SC_raw[i] < 614)   // works
       SC_d_raw[i] = SC_OPP;
      else if (SC_raw[i] < 750)   // works adjusted becuase would not discriminate betweeen SC_STRAIGHT and SC_CURVE
//       else if (SC_raw[i] < 700)   // works
//       else if (SC_raw[i] < 770)   // works
//     else if (SC_raw[i] < 790)   // does not work
      SC_d_raw[i] = SC_STRAIGHT;
//     else if (SC_raw[i] < 819)   // does not work
//      SC_d_raw[i] = SC_STRAIGHT;
     else if (SC_raw[i] <= 1024)
      SC_d_raw[i] = SC_CURVE;
     else
      SC_d_raw[i] = SC_UNK;

     // see if the same as the existing value -- if so, reset transition

     if (SC_d_raw[i] == SC_val[i])
      {
        SC_transition_time[i] = 0;
        SC_d_tval[i] = SC_val[i];
      }
     else if (SC_d_raw[i] != SC_d_tval[i])  // seee if differnt from last reading
      {
       SC_transition_time[i] = now_time;
       SC_d_tval[i] = SC_d_raw[i];
      }
     else  // see if at the new value long enough
      {
       // calculate time_diff
       if (now_time <= SC_transition_time[i])
        { 
         time_diff = 0;
         SC_transition_time[i]  = now_time;
        }
       else
        time_diff = now_time - SC_transition_time[i];

       if (time_diff > AI_DEBOUNCE_TIME)
        {
         SC_transition_time[i] = 0;
         SC_val[i] =  SC_d_raw[i];
        }

      }
   
   }
 

}

void process_polarity_pushbuttons(void)
{
 unsigned long time_diff;

 if (pushbutton_A_raw == pushbutton_A_val)  // not transitioning
  {
    pushbutton_A_transition_time = 0;
    pushbutton_A_transition = 0;
  }
 else if (pushbutton_A_transition == 0)  // start of transition
  {
   pushbutton_A_transition_time = now_time;
   pushbutton_A_transition = 1;
  }
 else  // in transiition, check to see if done
  {
   if (now_time <= pushbutton_A_transition_time)
    { 
     time_diff = 0;
     pushbutton_A_transition_time  = now_time;
    }
   else
    time_diff = now_time - pushbutton_A_transition_time;

   if (time_diff >= DI_DEBOUNCE_TIME)
    {
      pushbutton_A_val = pushbutton_A_raw;
      pushbutton_A_transition = 0;
      pushbutton_A_transition_time = 0;
    }
  }

 if (pushbutton_B_raw == pushbutton_B_val)  // not transitioning
  {
    pushbutton_B_transition_time = 0;
    pushbutton_B_transition = 0;
  }
 else if (pushbutton_B_transition == 0)  // start of transition
  {
   pushbutton_B_transition_time = now_time;
   pushbutton_B_transition = 1;
  }
 else  // in transiition, check to see if done
  {
   if (now_time <= pushbutton_B_transition_time)
    { 
     time_diff = 0;
     pushbutton_B_transition_time  = now_time;
    }
   else
    time_diff = now_time - pushbutton_B_transition_time;

   if (time_diff >= DI_DEBOUNCE_TIME)
    {
      pushbutton_B_val = pushbutton_B_raw;
      pushbutton_B_transition = 0;
      pushbutton_B_transition_time = 0;
    }
  }

}

// calc state does ...
// 1. determine which block the train is in
// 2.  If in a new block, activate switches as necessary
// 3.  based on pushbuttons, establish the polarity of the block the train is in
// 4.  Set the polarity of all the other blocks based on polarity of the block the train is in.
  
void calc_state(void)
{
  int new_block;
  int switch_dir;
  int i;
  int dir;
 
  // see if a new  block 

  if (state_this_block < 8) // not unknown)
   {
     if (state_block[state_this_block] == 0)
      new_block = 1;  // see if train has exited state_this_block
     else
      new_block = 0;  // train still in state_this_block
   }
  else
   new_block = 1; // if unknown then always a new block

  // if have a new block ... save the old block

  if (new_block == 1)
   state_last_block =  state_this_block;

 // figure out which block one is in

 if (new_block == 0)
  {
  // don't need to find a new block
  }
 else if (state_this_block == 0)
  {
    if (state_block[4] != 0)
     state_this_block = 4;
    else if (state_block[6] != 0)
     state_this_block = 6;
    else if (state_block[3] != 0)
     state_this_block = 3;
    else if (state_block[2] != 0)
     state_this_block = 2;
    else if (state_block[1] != 0)
     state_this_block = 1;
    else if (state_block[5] != 0)
     state_this_block = 5;
    else if (state_block[7] != 0)
     state_this_block = 7;
    else
     state_this_block = 8;
  }
 else if (state_this_block == 1)
  {
    if (state_block[5] != 0)
     state_this_block = 5;
    else if (state_block[6] != 0)
     state_this_block = 6;
    else if (state_block[3] != 0)
     state_this_block = 3;
    else if (state_block[2] != 0)
     state_this_block = 2;
    else if (state_block[0] != 0)
     state_this_block = 0;
    else if (state_block[4] != 0)
     state_this_block = 4;
    else if (state_block[7] != 0)
     state_this_block = 7;
    else
     state_this_block = 8;
  }
 else if (state_this_block == 2)
  {
    if (state_block[6] != 0)
     state_this_block = 6;
    else if (state_block[4] != 0)
     state_this_block = 4;
    else if (state_block[0] != 0)
     state_this_block = 0;
    else if (state_block[7] != 0)
     state_this_block = 7;
    else if (state_block[1] != 0)
     state_this_block = 1;
    else if (state_block[3] != 0)
     state_this_block = 3;
    else if (state_block[5] != 0)
     state_this_block = 5;
    else
     state_this_block = 8;

  }
 else if (state_this_block == 3)
  {
    if (state_block[5] != 0)
     state_this_block = 5;
    else if (state_block[6] != 0)
     state_this_block = 6;
    else if (state_block[1] != 0)
     state_this_block = 1;
    else if (state_block[2] != 0)
     state_this_block = 2;
    else if (state_block[0] != 0)
     state_this_block = 0;
    else if (state_block[7] != 0)
     state_this_block = 7;
    else if (state_block[4] != 0)
     state_this_block = 4;
    else
     state_this_block = 8;
  }
 else if (state_this_block == 4)
  {
    if (state_block[2] != 0)
     state_this_block = 2;
    else if (state_block[0] != 0)
     state_this_block = 0;
    else if (state_block[7] != 0)
     state_this_block = 7;
    else if (state_block[6] != 0)
     state_this_block = 6;
    else if (state_block[5] != 0)
     state_this_block = 5;
    else if (state_block[1] != 0)
     state_this_block = 1;
    else if (state_block[3] != 0)
     state_this_block = 3;
    else
     state_this_block = 8;
  }
 else if (state_this_block == 5)
  {
    if (state_block[3] != 0)
     state_this_block = 3;
    else if (state_block[1] != 0)
     state_this_block = 1;
    else if (state_block[7] != 0)
     state_this_block = 7;
    else if (state_block[6] != 0)
     state_this_block = 6;
    else if (state_block[0] != 0)
     state_this_block = 0;
    else if (state_block[2] != 0)
     state_this_block = 2;
    else if (state_block[4] != 0)
     state_this_block = 4;
    else
     state_this_block = 8;
  }
 else if (state_this_block == 6)
  {
    if (state_block[3] != 0)
     state_this_block = 3;
    else if (state_block[1] != 0)
     state_this_block = 1;
    else if (state_block[0] != 0)
     state_this_block = 0;
    else if (state_block[2] != 0)
     state_this_block = 2;
    else if (state_block[5] != 0)
     state_this_block = 5;
    else if (state_block[4] != 0)
     state_this_block = 4;
    else if (state_block[7] != 0)
     state_this_block = 7;
    else
     state_this_block = 8;
  }
 else if (state_this_block == 7)
  {
    if (state_block[5] != 0)
     state_this_block = 5;
    else if (state_block[4] != 0)
     state_this_block = 4;
    else if (state_block[3] != 0)
     state_this_block = 3;
    else if (state_block[2] != 0)
     state_this_block = 2;
    else if (state_block[6] != 0)
     state_this_block = 6;
    else if (state_block[1] != 0)
     state_this_block = 1;
    else if (state_block[0] != 0)
     state_this_block = 0;
    else
     state_this_block = 8;
  }
 else
  {
    if (state_block[0] != 0)
     state_this_block = 0;
    else if (state_block[1] != 0)
     state_this_block = 1;
    else if (state_block[2] != 0)
     state_this_block = 2;
    else if (state_block[3] != 0)
     state_this_block = 3;
    else if (state_block[4] != 0)
     state_this_block = 4;
    else if (state_block[5] != 0)
     state_this_block = 5;
    else if (state_block[6] != 0)
     state_this_block = 6;
    else if (state_block[7] != 0)
     state_this_block = 7;
    else
     state_this_block = 8;

  }

// activate switches as necessary if a new block

if (new_block == 0)
 {
  // don't need to throw any switches
 }
else if (state_this_block == 0)
 {
   switch_cmd_crv[S_CF] = 1;
   switch_cmd_crv[S_CR] = 1;

   if (state_last_block == 4)
    {
      switch_dir = determine_switch_position(SC_val[S_BR], state_switch_logic_dir[S_BR]);
      if (switch_dir == S_STR)
        switch_cmd_str[S_BR] = 1;
       else
        switch_cmd_crv[S_BR] = 1;
    }
 }
else if (state_this_block == 1)
 {
   switch_cmd_crv[S_BF] = 1;
   switch_cmd_crv[S_BR] = 1;

   if (state_last_block == 5)
    {
      switch_dir = determine_switch_position(SC_val[S_CR], state_switch_logic_dir[S_CR]);
      if (switch_dir == S_STR)
        switch_cmd_str[S_CR] = 1;
       else
        switch_cmd_crv[S_CR] = 1;
    }

 }
else if (state_this_block == 2)
 {
   switch_cmd_str[S_CR] = 1;

   if (state_last_block == 4)
    {
      switch_dir = determine_switch_position(SC_val[S_BR], state_switch_logic_dir[S_BR]);
      if (switch_dir == S_STR)
        switch_cmd_str[S_BR] = 1;
       else
        switch_cmd_crv[S_BR] = 1;
    }

   else if (state_last_block == 6)
    {
      switch_dir = determine_switch_position(SC_val[S_CF], state_switch_logic_dir[S_CF]);
      if (switch_dir == S_STR)
        switch_cmd_str[S_CF] = 1;
       else
        switch_cmd_crv[S_CF] = 1;
    }

 
 }
else if (state_this_block == 3)
 {
   switch_cmd_str[S_BR] = 1;

   if (state_last_block == 5)
    {
      switch_dir = determine_switch_position(SC_val[S_CR], state_switch_logic_dir[S_CR]);
      if (switch_dir == S_STR)
        switch_cmd_str[S_CR] = 1;
       else
        switch_cmd_crv[S_CR] = 1;
    }

   else if (state_last_block == 6)
    {
      switch_dir = determine_switch_position(SC_val[S_BF], state_switch_logic_dir[S_BF]);
      if (switch_dir == S_STR)
        switch_cmd_str[S_BF] = 1;
       else
        switch_cmd_crv[S_BF] = 1;
    }


 }
else if (state_this_block == 4)
 {
  // don't need to throw any switches
 }
else if (state_this_block == 5)
 {
  // don't need to throw any switches
 }
else if (state_this_block == 6)
 {
 // don't need to throw any switches
 }
else if (state_this_block == 7)
 {
   switch_cmd_str[S_BF] = 1;
   switch_cmd_str[S_CF] = 1;
 }

// set the polarity of the current block based on the pushbuttons (if pressed)

if(state_this_block < 8)
 {
   state_direction_cmd[state_this_block] = state_direction[state_this_block];
   if (pushbutton_A_val != 0)
    state_direction_cmd[state_this_block] = 0;
   if (pushbutton_B_val != 0)
    state_direction_cmd[state_this_block] = 1;
 }

// set the polarity of all the blocks to the pushbutton if there is no current block
// if no pushbutton, leave all alone

if (state_this_block > 7)
 {
   if (pushbutton_A_val != 0)
    {
     for (i=0 ; i < 8 ; i++)
      state_direction_cmd[i] = 0;
    }

   if (pushbutton_B_val != 0)
    {
     for (i=0 ; i < 8 ; i++)
      state_direction_cmd[i] = 1;
    }
 } // otherwise the polarity depends on the current block
else if (state_this_block == 0)
 {
   if(state_direction_cmd[state_this_block] > 0)
    dir = 1;
   else
    dir = 0;
      
   state_direction_cmd[1] = 1 - dir;
   state_direction_cmd[2] = dir;
   state_direction_cmd[3] = 1 - dir;
   state_direction_cmd[4] = dir;
   if (state_switch_logic_dir[S_BR] == S_STR)
    state_direction_cmd[5] = 1 - dir;
   else
    state_direction_cmd[5] = dir;
   state_direction_cmd[6] = 1 - dir;
   state_direction_cmd[7] = 1 - dir;
 }
else if (state_this_block == 1)
 {
   if(state_direction_cmd[state_this_block] > 0)
    dir = 1;
   else
    dir = 0;
      
   state_direction_cmd[0] = 1 - dir;
   state_direction_cmd[2] = dir;
   state_direction_cmd[3] = 1 - dir;
   if (state_switch_logic_dir[S_CR] == S_STR)
    state_direction_cmd[4] = dir;
   else
    state_direction_cmd[4] = 1 - dir;
   state_direction_cmd[5] = 1 - dir;
   state_direction_cmd[6] = dir;
   state_direction_cmd[7] = dir;
 }
else if (state_this_block == 2)
 {
   if(state_direction_cmd[state_this_block] > 0)
    dir = 1;
   else
    dir = 0;
      
   state_direction_cmd[0] = dir;
   state_direction_cmd[1] = dir;
   state_direction_cmd[3] = dir;
   state_direction_cmd[4] = dir;
   if (state_switch_logic_dir[S_BR] == S_STR)
    state_direction_cmd[5] = dir;
   else
    state_direction_cmd[5] = 1 - dir;
   state_direction_cmd[6] = dir;
   state_direction_cmd[7] = dir;
 }
else if (state_this_block == 3)
 {
   if(state_direction_cmd[state_this_block] > 0)
    dir = 1;
   else
    dir = 0;
      
   state_direction_cmd[0] = 1 - dir;
   state_direction_cmd[1] = 1 - dir;
   state_direction_cmd[2] = dir;
   if (state_switch_logic_dir[S_CR] == S_STR)
    state_direction_cmd[4] = dir;
   else
    state_direction_cmd[4] = 1 - dir;
   state_direction_cmd[5] = dir;
   state_direction_cmd[6] = dir;
   state_direction_cmd[7] = dir;

 }
else if (state_this_block == 4)
 {
   if(state_direction_cmd[state_this_block] > 0)
    dir = 1;
   else
    dir = 0;
      
   state_direction_cmd[0] = dir; 
   state_direction_cmd[1] = 1 - dir;
   state_direction_cmd[2] = dir;
   if (state_switch_logic_dir[S_CF] == S_STR)
    state_direction_cmd[3] = dir;
   else
    state_direction_cmd[3] = 1 - dir;
   state_direction_cmd[5] = dir;
   if (state_last_block == 2) 
    state_direction_cmd[6] = 1- dir;
   else  
    state_direction_cmd[6] = dir;
   state_direction_cmd[7] = dir;
 }
else if (state_this_block == 5)
 {
   if(state_direction_cmd[state_this_block] > 0)
    dir = 1;
   else
    dir = 0;
      
   state_direction_cmd[0] = dir;
   state_direction_cmd[1] = 1 - dir;
   if (state_switch_logic_dir[S_BF] == S_STR)
    state_direction_cmd[2] = dir;
   else
    state_direction_cmd[2] = 1 - dir;
   state_direction_cmd[3] = dir;
   state_direction_cmd[4] = dir;
   if (state_last_block == 3) 
    state_direction_cmd[6] = 1- dir;
   else  
    state_direction_cmd[6] = dir;
   state_direction_cmd[7] = dir;
 }
else if (state_this_block == 6)
 {
   if(state_direction_cmd[state_this_block] > 0)
    dir = 1;
   else
    dir = 0;
      
   state_direction_cmd[0] = 1 - dir;
   state_direction_cmd[1] = dir;
   state_direction_cmd[2] = dir;
   state_direction_cmd[3] = dir;
   if (state_switch_logic_dir[S_CR] == S_STR)
    state_direction_cmd[4] = dir;
   else
    state_direction_cmd[4] = 1 - dir;
   if (state_switch_logic_dir[S_BR] == S_STR)
    state_direction_cmd[5] = dir;
   else
    state_direction_cmd[5] = 1 -dir;
   state_direction_cmd[7] = dir;
 }
else if (state_this_block == 7)
 {
   if(state_direction_cmd[state_this_block] > 0)
    dir = 1;
   else
    dir = 0;
      
   state_direction_cmd[0] = 1 - dir;
   state_direction_cmd[1] = dir;
   state_direction_cmd[2] = dir;
   state_direction_cmd[3] = dir;
   state_direction_cmd[4] = dir;
   state_direction_cmd[5] = dir;
   state_direction_cmd[6] = dir;
 }


  
}
  
void process_switch_coil(void)
{
  
 unsigned long time_diff;
 int i;


// a command to energize a coil when another coil is energized will be saved until no coils are being energized

for (i = 0 ; i < 4 ; i++)
 {

   // see if commanded to turn the straight coil on -- make sure coils not already on --= and see if already straight
   
   if (switch_cmd_str[i] != 0 && switch_coil_str[i] == 0)
    {
//      if (state_switch_logic_dir[i] == S_STR)  // already straight - clear flag
//       {
//        switch_cmd_str[i] = 0;
//       }
      
//      else
      if (switch_coil_block == 0)  // make sure another coil isn't on 
       {
        switch_coil_str_on_time[i]  = now_time;
        switch_coil_str[i] = 1;  // turn coil on
        switch_coil_block = 1;  // keep other coils from turning on until this one is done

       } 
    }   

   // check for crv coil

   if (switch_cmd_crv[i] != 0  && switch_coil_crv[i] == 0)
    {

//     if (state_switch_logic_dir[i] == S_CRV)  // already curve - clear flag
//       {
//        switch_cmd_crv[i] = 0;
//       }

//      else 
      if (switch_coil_block == 0)
       {
        switch_coil_crv_on_time[i] = now_time;
        switch_coil_crv[i] = 1; // turn coil on
        switch_coil_block = 1;  // keep other coils from turning on until this one is done
        switch_coil_str[i] = 0; // make sure straight coil is turned off
       }
    }

  // see if straight coil has been on long enough


   if (switch_coil_str[i] != 0)
    {
       // calculate time_diff
      if (now_time <= switch_coil_str_on_time[i])
       { 
        time_diff = 0;
        switch_coil_str_on_time[i]  = now_time;
       }
      else
       time_diff = now_time - switch_coil_str_on_time[i];

      if (time_diff >=   COIL_PULSE_TIME) // see if coil on long enough
       {
         switch_coil_str[i] = 0;  // turn coil off
         switch_cmd_str[i] = 0;   // clear the command
         switch_coil_block = 0;   // allow other coils to be energized

         if (state_switch_debounce[i] == S_STR) // switch sensor and switch coil match
          {
           state_switch_health[i] = 0;  // the same
          }
         else  // they don't match so health of straight sensor is bad
          {
           state_switch_health[i] = 1;  // not the same
          }
       }
    }
    
  // see if curve coil has been on long enough

   if (switch_coil_crv[i] != 0)
    {
       // calculate time_diff
      if (now_time <= switch_coil_crv_on_time[i])
       { 
        time_diff = 0;
        switch_coil_crv_on_time[i]  = now_time;
       }
      else
       time_diff = now_time - switch_coil_crv_on_time[i];

      if (time_diff >=   COIL_PULSE_TIME) // see if coil on long enough
       {
         switch_coil_crv[i] = 0;  // turn coil off
         switch_cmd_crv[i] = 0;   // clear the command
         switch_coil_block = 0;   // allow other coils to be energized
 
         if (state_switch_debounce[i] == S_CRV) // switch sensor and switch coil match
          {
           state_switch_health[i] = 0;  // the same
          }
         else  // they don't match so health of straight sensor is bad
          {
           state_switch_health[i] = 1;  // not the same
          }
       }
    }

  }
}
  
void process_block_direction(void)
{
 int i;

 for (i = 0 ; i < 8 ; i++)
  {
   state_direction[i] = state_direction_cmd[i];
   
  }
}  

void  write_to_ports(void)
{
 // Directional control 

 digitalWrite(DO_F0_DC,state_direction[F0]);
 digitalWrite(DO_F1_DC,state_direction[F1]);
 digitalWrite(DO_F2_DC,state_direction[F2]);
 digitalWrite(DO_F3_DC,state_direction[F3]);
 digitalWrite(DO_F4_DC,state_direction[F4]);
 digitalWrite(DO_F5_DC,state_direction[F5]);
 digitalWrite(DO_F6_DC,state_direction[F6]);
 digitalWrite(DO_F7_DC,state_direction[F7]);

 // switch coils -- ensure only one coil is energized - straight if both commanded

 digitalWrite(DO_S_BF_S,switch_coil_str[S_BF]);
 if (switch_coil_str[S_BF] == 0)
  digitalWrite(DO_S_BF_C,switch_coil_crv[S_BF]);
 else
  digitalWrite(DO_S_BF_C,0);
  
 digitalWrite(DO_S_BR_S,switch_coil_str[S_BR]);
 if (switch_coil_str[S_BR] == 0)
  digitalWrite(DO_S_BR_C,switch_coil_crv[S_BR]);
 else
  digitalWrite(DO_S_BR_C,0);
 
 digitalWrite(DO_S_CR_S,switch_coil_str[S_CR]);
 if (switch_coil_str[S_CR] == 0)
  digitalWrite(DO_S_CR_C,switch_coil_crv[S_CR]);
 else 
  digitalWrite(DO_S_CR_C,0);
 
 digitalWrite(DO_S_CF_S,switch_coil_str[S_CF]);
 if (switch_coil_str[S_CF] == 0)
  digitalWrite(DO_S_CF_C,switch_coil_crv[S_CF]);
 else
  digitalWrite(DO_S_CF_C,0);
 
}

//
// FX-FY-D-CCCCCCCC
// SMc-SMc-SMc-SMc
//
// X = block currentlint in (0-7 or U)
// Y = block last in (0-7 or U)
// D = Direction (A,B)
// C = Direction of eadch block (a,A,b,B) cap = CT on, low = CT off
// S = Direction of Switch (S,C) or if sensor error (s,c)
// M = Switch Conttrol Mode (R,U,O,S,C)
// c = Coil Energized (c, s, x, blank) (x = both)
//
//


void write_display(void)
{
   unsigned long time_diff;
   int i;
   int ch;
 
  if (now_time <= display_last_update_time)
   {
    display_last_update_time = now_time;
    time_diff = 0;
   }
  else
   time_diff = now_time - display_last_update_time;
   
  if (time_diff >= DISPLAY_UPDATE_TIME)
   {
     display_last_update_time = now_time; 
    
     // home the cursor
     
     Serial.write(254);
     Serial.write(70);
     
     // Send the block currently in 

     Serial.write("F");

     if (state_this_block < F_UNKNOWN)
      {
        ch = 48 + state_this_block;
        Serial.write(ch);
      }
     else
      {
        Serial.write("U");
      }

     // send the bloock last in 

     Serial.write("-F");
     
     if (state_last_block < F_UNKNOWN)
      {
        ch = 48 + state_last_block;
        Serial.write(ch);
      }
     else
      {
        Serial.write("U");
      }

     // send the direction (A, B, or U)

     if (state_this_block < F_UNKNOWN)
      {
        if (state_direction[state_this_block] == 0)
         Serial.write("-A-");
        else
         Serial.write("-B-");
      }
    else
      {
        Serial.write("-U-");
      }
    
    for (i=0 ; i < F_UNKNOWN ; i++)
     {
       if (state_direction[i] == 0)
        {
          if (state_block[i] == 0)
           Serial.write("a");
          else
           Serial.write("A");
        }
       else
        {
          if (state_block[i] == 0)
           Serial.write("b");
          else
           Serial.write("B");
        }
 
     }


     // move the cursor to the next line
    
     Serial.write(254);
     Serial.write(69);
     Serial.write(64);
    
    for (i=0 ; i < 4 ; i++)
     {
      if (i > 0)
       Serial.write(" ");

      if (state_switch_health[i] != 0)
       {
        if (state_switch_logic_dir[i] == S_CRV)
         Serial.write("c");
        else if (state_switch_logic_dir[i] == S_STR)
         Serial.write("s");
        else
         Serial.write("u");
       }
      else
       {
        if (state_switch_logic_dir[i] == S_CRV)
         Serial.write("C");
        else if (state_switch_logic_dir[i] == S_STR)
         Serial.write("S");
        else
         Serial.write("U");
       }

      if (SC_val[i] == SC_UNK)
        Serial.write("U");
      else if (SC_val[i] == SC_RANDOM)
        Serial.write("R");
      else if (SC_val[i] == SC_NC)
        Serial.write("N");
      else if (SC_val[i] == SC_OPP)
        Serial.write("O");
      else if (SC_val[i] == SC_STRAIGHT)
        Serial.write("S");
      else if (SC_val[i] == SC_CURVE)
        Serial.write("C");
      else
        Serial.write("?");

      if (switch_coil_str[i] > 0)
       {
        if (switch_coil_crv[i] > 0)
         Serial.write("x");
        else
         Serial.write("s");
       }
      else
       {
        if (switch_coil_crv[i] > 0)
         Serial.write("c");
        else
         Serial.write(" ");
       }
     }
   
   }
  
}


int determine_switch_position(int switch_command, int current_direction)
{
 long rnum;
  
 if (switch_command == SC_CURVE) return S_CRV;
 if (switch_command == SC_STRAIGHT) return S_STR;
 if (switch_command == SC_OPP && current_direction == S_STR) return S_CRV;
 if (switch_command == SC_OPP) return S_STR;
 if (switch_command == SC_NC && current_direction == S_STR) return S_STR;
 if (switch_command == SC_NC) return S_CRV;

 // return random direction
 
 rnum = random(0,100);

 if (rnum < 50) return S_CRV;

 return S_STR;
 
}