// https://wokwi.com/projects/366606203975163905
// https://forum.arduino.cc/t/approach-island-approach-rr-crossing/1121055

# include "track.h"

// inputs
#define NORTH_APPROACH 5
#define ISLAND_CIRCUIT 6
#define SOUTH_APPROACH 7

// outputs
#define LIGHT_RIGHT 10
#define LIGHT_LEFT 9
#define BELL_RELAY 11

unsigned long now;    // current time for all

enum {
  OFF = 0,
  NAP,
  SAP,
  NISL,
  SISL,
  NCLR,
  SCLR,
};

byte xState;

enum {OFF_, ON_};

byte flashQQ;  // are we flashing or no?

# define ACTIVE LOW // my switches pulled high. OMG tangle of sense and == LOW ARGH! LED sense FIX

byte northActive, islandActive, southActive;

void setup() {
  Serial.begin(115200);
  Serial.println("\napproach - island - approach world!\n");

  initTrack();

  pinMode(NORTH_APPROACH, INPUT_PULLUP);  //Track circuit one. 60ft/39.5m long. Primary circuit the train activates 90% of the time.
  pinMode(ISLAND_CIRCUIT, INPUT_PULLUP);  //Island circuit. 30ft/9m long. Circuit travels through a #5 Right hand Turnout that diverges.
  pinMode(SOUTH_APPROACH, INPUT_PULLUP);  //Track circuit two. Same length as number one, but needs to be able to activate the crossing from the other direction.

  pinMode(BELL_RELAY, OUTPUT);            //RR Bell Coil/SoundByte Speaker.
  pinMode(LIGHT_LEFT, OUTPUT);            //Left Flashing Light.
  pinMode(LIGHT_RIGHT, OUTPUT);           //Right Flashing Light.

  report();   // first report
}
  
void loop() {

// just to test the output function switch it on and off with the island value
//  flashQQ = digitalRead(ISLAND_CIRCUIT) ? ON_ : OFF_;

  outputFSM();

  static unsigned long lastTime;
  now = millis();

  if (now - lastTime < 50) return;   // loop throttle 20 Hz also will debounce switches automatically 
  lastTime = now;

  updateTrain();

  northActive = digitalRead(NORTH_APPROACH) == ACTIVE;
  islandActive = digitalRead(ISLAND_CIRCUIT) == ACTIVE;
  southActive = digitalRead(SOUTH_APPROACH) == ACTIVE;  // not NORTH!

  report();

//  theFSM();

    theFSM();
}

void theFSM()
{
  switch (xState) {

  case OFF :
    if (northActive) xState = NAP;
    if (southActive) xState = SAP;
    flashQQ = xState == OFF ? OFF_ : ON_;
    break;

  case NAP :
    if (islandActive) xState = NISL;
    if (!northActive) xState = OFF;
    break;

  case SAP :
    if (islandActive) xState = SISL;
    if (!southActive) xState = OFF;
    break;

  case NISL :
    flashQQ = ON_;
    if (islandActive)
      break;        // island is active! stay in state

    if (northActive) {
      flashQQ = OFF_;
      xState = NAP;
    }
    if (southActive) xState = SCLR;
    break;

  case SISL :
    flashQQ = ON_;
    if (islandActive)
      break;        // island is active! stay in state

    if (southActive) {
      flashQQ = OFF_;
      xState = SAP;
    }
    if (northActive) xState = NCLR;
    break;

  case NCLR :
    if (islandActive) xState = SISL;
    if (!islandActive) flashQQ = OFF_;
    if (!northActive) xState = OFF;
    break;

  case SCLR :
    if (islandActive) xState = NISL;
    if (!islandActive) flashQQ = OFF_;
    if (!southActive) xState = OFF;
    break;
  }
}

char *stateTags[] = {
  "OFF_",
  "NAP_",
  "SAP_",
  "NISL",
  "SISL",
  "NCLR",
  "SCLR",
};

void report()
{
  byte needTo = 0;

  static byte printedInputs;
  static byte printedState;

  byte inputs = 4 * northActive + 2 * islandActive + southActive;

  if (xState != printedState) {
    needTo = 1;
    printedState = xState;
  }
  
  if (inputs != printedInputs) {
    needTo = 1;
    printedInputs = inputs;
  }

  if (needTo) {
    Serial.print(stateTags[xState]); Serial.print("  ");
    Serial.print(northActive ? "NORTH" : "     "); Serial.print("   ");
    Serial.print(islandActive ? "ISLAND" : "      "); Serial.print("   ");
    Serial.print(southActive ? "SOUTH" : "     "); Serial.print("   ");

    Serial.println();
  }
}

// change for wiring differences
# define LED_OFF    LOW
# define LED_ON     HIGH
# define RELAY_ON   HIGH
# define RELAY_OFF  LOW

void outputFSM()
{
  static int phase;
  static unsigned long lastTime;

  if (now - lastTime < 777) return;

  lastTime = now;

  if (flashQQ == OFF_) {
    digitalWrite(LIGHT_LEFT, LED_OFF);
    digitalWrite(LIGHT_RIGHT, LED_OFF);
    digitalWrite(BELL_RELAY, RELAY_OFF);
 
    phase = 0;
    return;
  }

// for now, just a level. comment and change the if/else
  digitalWrite(BELL_RELAY, RELAY_ON);

  if (!phase) {
    digitalWrite(LIGHT_LEFT, LED_ON);
    digitalWrite(LIGHT_RIGHT,LED_OFF);
 //   digitalWrite(BELL_RELAY, RELAY_ON);
  }
  else {
    digitalWrite(LIGHT_LEFT, LED_OFF);
    digitalWrite(LIGHT_RIGHT, LED_ON);
//    digitalWrite(BELL_RELAY, RELAY_OFF);
  }

  phase = !phase;
}