/*
   Next_block_left  |  Far_left  |  Home_left|...>This Control Point(cp)<...|Home_right  |  Far_right  |  Next_block_right

  2 |  1 home_left |...>This Control Point(cp)<...| Home_right  5  |  6

  left is "west" and right is "east"

  0 = triggered logic
  1 = empty
  2 = west/towards
  3 = east/away

  2=rx west
  3=tx west

  4=East/right facing R
  5=East/right facing Y
  6=East/right facing G
  7=West/left facing R
  8=West/left facing Y
  9=West/left facing G

  10=rx east
  11=tx east

  Y  ;//A0/ is next block left occupied
  R  ;//A1/ is far sensor Far_left triggered
  HL ;//A2/ is near sensor Home_left triggered
  HR ;//A3/ is near sensor Home_right triggered
  R  ;//A4/ is far sensor Far_right triggered
  Y  ;//A5/ is next block right occupied
  0, 1, 2, 3/\4 5, 6, 7
*/

# include <SoftwareSerial.h>
SoftwareSerial Ws(2, 3); //rx,tx com to left 2<->11, 3<->10
SoftwareSerial Es(10, 11); // rx,tx com to right 10<->3,11<->2


#define DEBUG 1
#define Turnout 0

#if DEBUG == 1
#define debug(dx) Serial.print(dx)
#define debugln(dx) Serial.println(dx)
#else
#define debug(dx)
#define debugln(dx)
#endif

#define  TO Turnout 
#define  bl00  PIN_A0

char wa;
char ea;

int cp[2];// value must be >= # of rows in sigs[]
int cpfL;// facing west/left (east bond movement)
int cpfR;//facing east/right (west bond movement)

bool termL;// is there another board to comuniate with to the Left/ West? 0 = no, 1 = yes
bool termR;// is there another board to comuniate with to the right/ East?

int O[6];// value must be >= # of rows in bks[]
int HLW;// HOME_LEFT_WEST
int FLW;// FAR_LEFT_WEST
int TLW;// Farther_Left_West
int HRE;// HOME_RIGHT_EAST
int FRE;// FAR_RIGHT_EAST
int TRE;// Farther_Right_East

int BL[8];// place holder array for sensor inputs
int WB[5];// place holder array for serial inputs from west or left
int EB[5];// place holder array for serial inputs from east or right

bool SC;// initial condition varialble

unsigned long CM;
unsigned long Cu[8];// timing variables, counts up time if block not triggered
unsigned long Cd[8];// counts up if block triggered
//unsigned long cpdt[2];// timers for change of signa

unsigned long BT = 0;

bool bState;

enum {
  L04 = 4, L05, L06,
  L07, L08, L09,
  Llast,
};
// ocupancy function---V---
typedef struct {
  const char    *desc;//cp
  int          term;
  int          FHR;// sensor past next cp
  int          NR;//WD entering block from far end
  int          FR;//WHD exiting block at far end
  int          HR;//EHD entering block controlled by signal
  int          HM;//ED home under signal
  int          ocp;// previous occupancy desc.
  int          hmdt;// home delay
  int          hrdt;// home red delay
} bks_t;
//---------signal function------V----------------------------------------
typedef struct {
  const char    *desc;
  int          HB;
  int          NB;
  int          FB;
  int          TB;
  int          term;
  int          pcp;
  int          hmdt;
  int          hrdt;
} sig_t;
//------light head function----V-------------------------------------------
typedef struct {
  const char *desc;
  int        ledR;
  int        ledY;
  int        ledG;
  int        cpst;
  int        hmdt;
  int        hrdt;
} lt_hd;
// User defined functions------------------------------------------------
int block (bks_t * s);
int sig (sig_t * s);
void head (lt_hd * s);
// end user defined functions-------------------------------------------
//----------------------------------------------------------------
void setup() {
  Serial.begin(9600);
  Es.begin(9600);
  Ws.begin(9600);

  SC = 0;
  cpfL = 1;
  cpfR = 1;
  HLW = 1;
  HRE = 1;

  for (int outPin = L04; outPin < Llast; outPin++) {
    pinMode(outPin, OUTPUT);
  }
  Ws.write('A');
  Es.write('A');

}
/* software serial refs
  if (mySerial.available())
    Serial.write(mySerial.read());
  if (Serial.available())
    mySerial.write(Serial.read());
  }*/
//----------------------------------------------------------------
void loop() {
  CM = millis();

  for (int b = 0; b < 8; b++) {
    BL[b] = digitalRead(bl00 + b);//sesnor reads
    (BL[b] == 0) ? (Cd[b] = CM) : CM;// triggered = Cd growing("Time Off"), not triggered = Cu graowing("Time On")
  }

  debugln("home sensors read check");
  debugln(BL[0]);
  debugln(BL[1]);

  //------------------------------------
  if ((CM - BT) > 600) {// BLINKER TIMER VARIABLE
    BT = CM;
    (bState == 1)?(bState = 0):(bState = 1);
  }
  //----------------------------------------------
  if ((CM < 7000) && ((termL == 0) || (termR == 0))) {
    (Ws.available() > 0) ? (termL = 1) : (termL = 0);
    (Es.available() > 0) ? (termR = 1) : (termR = 0);
    // A = active

    Ws.write('A');
    Es.write('A');
    (Ws.available() > 0) ? (wa = 'A') : (wa = 'N');
    (Es.available() > 0) ? (ea = 'A') : (ea = 'N');
   // wa = Ws.read();
   // ea = Es.read();

    debug("wa = ");
    debugln(wa);
    debug("ea = ");
    debugln(ea);
    delay(25);
  }
  //-----------------------------------------------------------
  if ((CM > 7200) || ((termL == 1) && (termR == 1))) {// time more that 7 seconds after statup

    for (int w = 0; w < 5; w++) {
      if (termL == 1) {
        WB[w] = Ws.read();
        debug("WB ");
        debug(w);
        debug(" is ");
        debugln(WB[w]);
      }
      else {
        if(w == 2){
          WB[w] = 3;
        }
        else{
        WB[w] = 1;
        debug("termL is not 1:WB");
        debug(w);
        debug("is <>");
        debugln(WB[w]);
        }
      }
    }
    Ws.flush();

    for (int e = 0; e < 5; e++) {
      if (termR == 1) {
        EB[e] = Es.read();
        debugln(EB[e]);
      }
      else {
        if(e == 2){
          EB[e] = 3;
        }
        else{
        EB[e] = 1;
        debug("termR is not 1:EB");
        debug(e);
        debug("is <>");
        debugln(EB[e]);
        }
      }
    }
    Es.flush();

    //------------------------------------------------
    //  desc   term     FHR     NR     FR      HR    HM    ocp     hmdt      hrdt
    bks_t bks [] = {
      {"HLW", termL,   WB[3],   WB[1], WB[0], BL[0], BL[1], HLW,    1,       0},// block to left of cp
      {"HRE", termR,   EB[3],   EB[1], EB[0], BL[1], BL[0], HRE,    0,       1},// block to right of cp
    };

#define B_SIZE (sizeof(bks)/sizeof(bks_t))
    for (int o = 0; o < B_SIZE; o++) {// occupancy block function
      O[o] = block (& bks[o]);// should = "<" number of rows in table above

    }
    HLW = O[0];
    HRE = O[1];

    //TLW = WB[2];
    FLW = WB[2];//convert bks [] info to HLW-> info
    // need to arrange according to bks[] table above
    // row 1 = O[0], row 2 = O[1]...
    FRE = EB[2];
    //TRE = EB[2];

    if ((HLW != 1) && (HRE != 1)) {
      SC = 1;
    }
    debug("ocupancy checks:(");
    debug(HLW);
    debug(")(");
    debug(HRE);
    debugln(") end ocp transmission");


    //-----------------*********************
    // desc    HB     NB   FB   TB   term   pcp   hmdt   hrdt
    sig_t sigs [] = {
      {"cpfL", BL[0], HRE, FRE, FRE, termR, cpfL,  0,    1},// west to east bond move
      {"cpfR", BL[1], HLW, FLW, FLW, termL, cpfR,  1,    0},// east to west bound move
    };
#define S_SIZE (sizeof(sigs)/sizeof(sig_t))
    for (int l = 0; l < S_SIZE; l++) {// signal function
      cp[l] =  sig (& sigs[l]);
    }

    cpfL = cp[0];
    cpfR = cp[1];
/*
    if ((CM - Cu[1] > 6000) && (cpfL < 3) && (termR = 0)) {
      Cu[1] = CM;
      cpfL++;
    }
    if ((CM - Cu[0] > 6000) && (cpfR < 3) && (termL = 0)) {
      Cu[0] = CM;
      cpfR++;
    }*/
    debugln("cp checks");
    debugln(cpfL);
    debugln(cpfR);
    //********^
    lt_hd lits [] = {
      {"FWest", L07, L08, L09, cpfL, 1, 0 },
      {"Feast", L04, L05, L06, cpfR, 0, 1 },
    };
#define H_SIZE (sizeof(lits)/sizeof(lt_hd))
    for (int h = 0; h < H_SIZE; h++) {// signal head output function
      head (& lits[h]);
    }
    Es.write(BL[1]);
    Es.write(HRE);
    Es.write(cpfR);
    Es.write(BL[0]);
    Ws.write(BL[0]);
    Ws.write(HLW);
    Ws.write(cpfL);
    Ws.write(BL[1]);
  }
}
// END VOID LOOPS
//-------------------------------------------------------
//  desc   term     FHR     NR     FR      HR    HM    ocp     hmdt      hrdt
int block (bks_t * s) {
  int OD;

  if ((s->ocp) == 1) {
    if ((((s->NR) == 3) && ((s->FR) == 0))) {
      Cd[s->hmdt] = 0;
      OD = 2;
      return OD;
    }
    if((((s->HR) == 0))){
      Cd[s->hmdt] = 0;
      OD = 2;
      return OD;
    }
    if (((s->FR) == 0 )) {
      OD = 3;
      return OD;
    }
    if((s->HM) == 0){
      Cu[s->hmdt] = CM;
      Cd[s->hrdt] = 0;
      OD = 3;
      return OD;
    }
    else {
      OD = 1;
      return OD;
    }
  }
  if ((s->ocp) == 3) {// train moving away from cp
    if (((s->HR) == 0) || ((s->HM) == 0) || ((s->FR) == 0)) {// if any sensors triggere, direction stays gree
      OD = 3;
      return OD;
    }
    if((((s->HM) == 1) && (CM - Cd[s->hmdt] > 700) && (Cd[s->hrdt] == 0))){// protects from false sig while crossing cp
      OD = 3;
      return OD;
    } 

    if(((s->term) == 0) && (CM - Cd[s->hrdt] > 2200) && (Cd[s->hrdt] != 0)){// **** set timeout of block if term 0***MUST = 4X TIMEOUT IN SIG
      OD = 1;
      return OD;
    }
    if(((s->term) == 1) && (CM - Cd[s->hrdt] > 700) && (Cd[s->hrdt] != 0)){
      if (((s->FR) == 1) && ((s->HR) == 1) && ((CM - Cd[s->hmdt] > 700))){
        OD = 0;
        return OD;
      }
     if ((((s->FHR) == 0) && ((s->FR) == 1))) {
      OD = 1;
      return OD;
    }
      else{
        OD = 3;
        return OD;
      }
    }
    else {
      OD = 3;
      return OD;
    }
  }
  if ((s->ocp) == 2) {
    if (((s->FR) == 0) || ((s->HR) == 0) || ((s->HM) == 0)) {
      OD = 2;
      return OD;
    }
    if ((CM - Cd[s->hrdt] > 700)) {
     if((CM -Cd[s->hmdt] > 2200) && (Cd[s->hmdt] != 0)){//**** set time to delay drop to green behind train****
      OD = 1;
      return OD;
     }
      if(((s->NR) == 3) && ((s->FR) == 1) && ((s->HR) == 1) && ((s->term) == 1)){
      OD = 0;
      return OD;
     }
     else{
       OD = 2;
       return OD;
     }
    }
    else {
      OD = 2;
      return OD;
    }
  }
  if ((s->ocp) == 0) {
    if(((s->NR) == 3) && ((s->FR) == 0)){
      OD = 2;
      return OD;
    }
    if(((s->NR) != 3) && ((s->FR) == 0)){
      OD = 3;
      return OD;
    }
    if ((s->HR) == 0) {
      OD = 2;
      return OD;
    }
    else {
      OD = 0;
      return OD;
    }
  }
}
//--------------------------------------------
// desc    HB     NB   FB   TB   term   pcp  hmdt  hrdt
int sig (sig_t * s) {
  int cp;
  debugln("sig time check");
  debugln(Cu[0]);
  if((((s->NB) == 3) && (CM - Cu[s->hmdt] > 1100))){
    cp = 0;
    return cp;
  }
  if (((s->NB) == 0)) {// if near block is occupied "not 1, !1" sig is always red
    cp = 0;
    return cp;
  }
  if(((s->NB) == 2)){
    cp = 4;
    return cp;
  }
  if (SC == 0) {// initial condition indications
    if ((s->term) == 0) {// if no next block in direction of signal
      cp = 1;            // initial indication is solid yellow
      return cp;
    }
    if ((s->term) == 1) {// if next block exists
      cp = 2;            // initial indication is blinking yellow
      return cp;
    }
  }
  if (((s->term) == 0) && (CM - Cd[s->hrdt] > 6000) && ((s->pcp) < 3)) {
    Cd[s->hrdt] = CM;
    debug(s->pcp);
    cp = (s->pcp);
    cp++;
    debug(" => this cycle is returning cp =");
    debugln(cp);
    return cp++;
  }
  if(((s->pcp) == 4) && ((CM - Cd[s->hmdt] > 1100)) && (Cd[s->hmdt] !=0)){
    cp = 3;
    return cp;
  }
  if (((s->NB) == 1) && ((s->FB) == 0)) {
    cp = 1;
    return cp;
  }
  if (((s->NB) == 1) && ((s->FB) == 1)) {
    cp = 2;
    return cp;
  }
  if (((s->NB) == 1) && ((s->FB) == 2)) {
    cp = 3;
    return cp;
  }
  else{
    cp = (s->pcp);
    return cp;
  }
}
//-------------------------------------------------
void head (lt_hd * s) {
  digitalWrite((s->ledR), HIGH);
  digitalWrite((s->ledY), HIGH);
  digitalWrite((s->ledG), HIGH);
  if (((s->cpst) == 0) || ((s->cpst) == 4)) {// red
    digitalWrite((s->ledR), LOW);

  }
  if (((s->cpst) == 1) || ((s->cpst) == 5)) {// yellow
    digitalWrite((s->ledY), LOW);

  }
  if ((s->cpst) == 2) {// blinking yellow
    digitalWrite((s->ledY), bState);

  }
  if ((s->cpst) == 3) {//green
    digitalWrite((s->ledG), LOW);
  }
}