/*
    POC Weiche, Fahrstraße, Zieltaste
    https://forum.arduino.cc/t/schattenbahnhofsteuerung/1208510/46
    Simulation: https://wokwi.com/projects/386439244339395585

    by noiasca
    2024-01-09 Grundgerüst
    2024-01-11 MCP23S17 eingebunden, Debug Meldungen, erstes Time Management
    2024-02-15 fixed class WeicheMCP  https://forum.arduino.cc/t/schattenbahnhofsteuerung/1208510/63
    2024-03-03 fixed class WeicheMCP, 4 turnouts
    2024-03-05 Fahrstraße
    2024-03-21 cleanup
    2024-03-21 route sets one turnout after another
    by schanderl
    2024-03-31 8 turnouts instead of 4; 10 waysA-J;  missing turnout 9 has to be connected to Mega2560 directly; Missing trackbuttons A to J & routeleds connected to Mega2560
    2024-04-01 10 routes
    2024-04-01 cleanup without MCP - connections directly to mega
    2024-04-02 10 trackbutton, linked to Routes A to J
    by noiasca
    2024-04-07 introduce yard as global array
*/

#include <Streaming.h>                 // Streaming by Peter Polidoro (boardmanager) - macht das Serial.print einfacher.

constexpr uint8_t buttonPinA {31};     // trackbutton for Route A
constexpr uint8_t buttonPinB {30};     // Route B
constexpr uint8_t buttonPinC {29};
constexpr uint8_t buttonPinD {28};
constexpr uint8_t buttonPinE {27};
constexpr uint8_t buttonPinF {26};
constexpr uint8_t buttonPinG {25};
constexpr uint8_t buttonPinH {24};
constexpr uint8_t buttonPinI {23};
constexpr uint8_t buttonPinJ {22};

// prototype
int yardUnset(int yardId);

// Yard
int yardSetRoute[1] {0};                 // stores the currently used Route (currently the indicatorPin) or 0 if unused. Currently only yardRoute[0] is needed

// turnout (switch)  - Weichenklasse
class Turnout {
    const uint8_t pinThrough;          // Output Pin uneven; through route, main, Tangent Route (gerade)
    const uint8_t pinDiverging;        // Output Pin even; diverging route (abbiegen)
    static const uint16_t interval = 1000;       // hard off of outputs (it is const for all turnouts - so lets make it static. Helps the compiler to optimize this variable and reduces needed SRAM)
    uint32_t previousMillis = 0;       // time management
    bool isActive = false;             // coil is on (true) or off (false)
    uint8_t direction = 0;             // 0 through (gerade), 1 diverging(abbiegen)
  public:
    Turnout (uint8_t pinThrough, uint8_t pinDiverging) :
      pinThrough {pinThrough},
      pinDiverging {pinDiverging} {}

    // to be called in setup() (could also be used to read back values from EEPROM)
    void begin() {
      pinMode(pinThrough, OUTPUT);
      pinMode(pinDiverging, OUTPUT);
    }

    // set the turnout 0 straight or 1 curve
    int set(byte newDirection) {
      if (isActive) return -1;         // command not accepted
      if (newDirection == 0) {
        digitalWrite(pinThrough, HIGH);
        direction = 0;
        Serial << "through pin " << pinThrough << '\n';
      }
      else {
        digitalWrite(pinDiverging, HIGH);
        direction = 1;
        Serial << "diverging pin " << pinDiverging << '\n';
      }
      isActive = true;
      previousMillis = millis();
      return 0; // OK
    }

    // set the turnout to through (Geradeausfahrt)
    int through() {
      return set(0);
    }

    // set the turnout to diverging (abbiegen)
    int diverging() {
      return set(1);
    }

    // returns the currenct direction of the turnout
    int getDirection() {
      return direction;
    };

    // returns if the turnout is currently active
    bool getActive() {
      return isActive;
    }

    // run function. Call this in setup()
    void update(uint32_t currentMillis = millis()) {
      if (isActive && currentMillis - previousMillis > interval) {
        digitalWrite(pinThrough, LOW);
        digitalWrite(pinDiverging, LOW);
        isActive = false;
        Serial << "off " << pinThrough << '/' << pinDiverging << '\n';
      }
    }
};

Turnout turnout[] {
  {49, 48},    // create one instance of a turnout
  {47, 46},    // onother turnout ( but different pins)
  {45, 44},
  {43, 42},
  {41, 40},
  {39, 38},
  {37, 36},
  {35, 34},
  {33, 32},
};

// a waypoint consists of a turnout and a direction
struct Way {
  Turnout &turnout;
  const byte direction;
};

// each way can consist of several waypoints
Way wayA[] {
  {turnout[0], 1}
};

Way wayB[] {
  {turnout[0], 0},
  {turnout[1], 1},
  {turnout[2], 1}
};

Way wayC[] {
  {turnout[0], 0},
  {turnout[1], 1},
  {turnout[2], 0}
};

Way wayD[] {
  {turnout[0], 0},
  {turnout[1], 0},
  {turnout[3], 1},
  {turnout[4], 1},
  {turnout[5], 0},
};

Way wayE[] {
  {turnout[0], 0},
  {turnout[1], 0},
  {turnout[3], 1},
  {turnout[4], 1},
  {turnout[5], 1},
};

Way wayF[] {
  {turnout[0], 0},
  {turnout[1], 0},
  {turnout[3], 1},
  {turnout[4], 0},
};

Way wayG[] {
  {turnout[0], 0},
  {turnout[1], 0},
  {turnout[3], 0},
  {turnout[6], 1},
  {turnout[7], 1},
  {turnout[8], 1},
};

Way wayH[] {
  {turnout[0], 0},
  {turnout[1], 0},
  {turnout[3], 0},
  {turnout[6], 1},
  {turnout[7], 1},
  {turnout[8], 0},
};

Way wayI[] {
  {turnout[0], 0},
  {turnout[1], 0},
  {turnout[3], 0},
  {turnout[6], 1},
  {turnout[7], 0},
};

Way wayJ[] {
  {turnout[0], 0},
  {turnout[1], 0},
  {turnout[3], 0},
  {turnout[6], 0},
};

template<size_t size>
class Route {
    const uint8_t indicatorPin;        // a LED to be enlighted after all turnouts are set
    const uint8_t yardId;              // assigns the route to one specific yard
    Way *way;                          // pointer to an array of waypoints
    uint16_t actualTurnout = 0;        // index of actual turnout (according to the way definition
    uint8_t state = 0;                 // 0 off / 1 running
  public:
    Route (Way (&way)[size], uint8_t indicatorPin = 255, uint8_t yardId = 0) : way(way), indicatorPin(indicatorPin), yardId(yardId) {}

    void begin() {
      pinMode(indicatorPin, OUTPUT);
    }

    // all turnouts at the same time (older version)
    void setForced() {
      Serial << "size:" << size << endl;
      for (size_t i = 0; i < size; i++) {
        if (way[i].direction == 0)
          way[i].turnout.through();
        else
          way[i].turnout.diverging();
      }
    }

    void set() {
      if (!state) {
        yardUnset(yardId);                      // will delete any other indicator LED
        yardSetRoute[yardId] = indicatorPin;    // "block" yard for this route
        state = 1;
        actualTurnout = 0;
        way[actualTurnout].turnout.set(way[actualTurnout].direction);
      }
      else
        Serial << "E: Route is in action." << endl;
    }

    // release route
    int unset() {
      digitalWrite(indicatorPin, LOW);
      if (yardSetRoute[yardId] == indicatorPin) yardSetRoute[yardId] = 0; // "unblock" yard for this route;              // "unblock" yard for this routeyardSetRoute[yardId] = 0;              // "unblock" yard for this route
      state = 0; // optional: interrupt current route
      return 0; // assume everything is ok
    }

    // FSM to go through all turnouts
    void update() {
      if (state) {
        if (way[actualTurnout].turnout.getActive() == false) {
          if (actualTurnout < size - 1) {
            actualTurnout++; // next turnout
            way[actualTurnout].turnout.set(way[actualTurnout].direction);
          }
          else {
            state = 0;
            digitalWrite(indicatorPin, HIGH);
            yardSetRoute[yardId] = 0;              // "unblock" yard for this route
            // Serial << "Gleis" << track/n;
            Serial << "I: route set." << endl;
          }
        }
      }
    }
};

// assign the array of waypoints to generate a route
// *INDENT-OFF*
Route <sizeof(wayA) / sizeof(wayA[0])> routeA(wayA, 2);
Route <sizeof(wayB) / sizeof(wayB[0])> routeB(wayB, 3);
Route <sizeof(wayC) / sizeof(wayC[0])> routeC(wayC, 4);
Route <sizeof(wayD) / sizeof(wayD[0])> routeD(wayD, 5);
Route <sizeof(wayE) / sizeof(wayE[0])> routeE(wayE, 6);
Route <sizeof(wayF) / sizeof(wayF[0])> routeF(wayF, 7);
Route <sizeof(wayG) / sizeof(wayG[0])> routeG(wayG, 8);
Route <sizeof(wayH) / sizeof(wayH[0])> routeH(wayH, 9);
Route <sizeof(wayI) / sizeof(wayI[0])> routeI(wayI, 10);
Route <sizeof(wayJ) / sizeof(wayJ[0])> routeJ(wayJ, 11);
// another fiddle yard 
// Route <sizeof(wayZ) / sizeof(wayZ[0])> routeZ(wayZ, 13, 1);  // see the additional parameter 1


// *INDENT-ON*

// Initialize buttons
void buttonBegin() {
  pinMode(buttonPinA, INPUT_PULLUP);
  pinMode(buttonPinB, INPUT_PULLUP);
  pinMode(buttonPinC, INPUT_PULLUP);
  pinMode(buttonPinD, INPUT_PULLUP);
  pinMode(buttonPinE, INPUT_PULLUP);
  pinMode(buttonPinF, INPUT_PULLUP);
  pinMode(buttonPinG, INPUT_PULLUP);
  pinMode(buttonPinH, INPUT_PULLUP);
  pinMode(buttonPinI, INPUT_PULLUP);
  pinMode(buttonPinJ, INPUT_PULLUP);
}

// read buttons and start action
void buttonRead() {
  /* if (digitalRead(buttonPinA) == LOW) turnout[0].through();
    if (digitalRead(buttonPinB) == LOW) turnout[0].diverging();
    if (digitalRead(buttonPinC) == LOW) turnout[1].through();
    if (digitalRead(buttonPinD) == LOW) turnout[1].diverging();
  */
  if (digitalRead(buttonPinA) == LOW) routeA.set();
  if (digitalRead(buttonPinB) == LOW) routeB.set();
  if (digitalRead(buttonPinC) == LOW) routeC.set();
  if (digitalRead(buttonPinD) == LOW) routeD.set();
  if (digitalRead(buttonPinE) == LOW) routeE.set();
  if (digitalRead(buttonPinF) == LOW) routeF.set();
  if (digitalRead(buttonPinG) == LOW) routeG.set();
  if (digitalRead(buttonPinH) == LOW) routeH.set();
  if (digitalRead(buttonPinI) == LOW) routeI.set();
  if (digitalRead(buttonPinJ) == LOW) routeJ.set();
}

// read Serial and start action
void serialRead() {
  int c = Serial.read();
  switch (c) {
    case -1 : break;
    case '1': turnout[0].through(); break;
    case '2': turnout[0].diverging(); break;
    case '3': turnout[1].through(); break;
    case '4': turnout[1].diverging(); break;
    case '5': turnout[2].through(); break;
    case '6': turnout[2].diverging(); break;
    case '7': turnout[3].through(); break;
    case '8': turnout[3].diverging(); break;
    case '9': turnout[4].through(); break;
    case '!': turnout[4].diverging(); break;
    case '\"': turnout[5].through(); break;
    case '§': turnout[5].diverging(); break;
    case '$': turnout[6].through(); break;
    case '%': turnout[6].diverging(); break;
    case '&': turnout[7].through(); break;
    case '/': turnout[7].diverging(); break;
    case '(': turnout[8].through(); break;
    case ')': turnout[8].diverging(); break;

    case 'a': routeA.set(); break; //track = 1
    case 'b': routeB.set(); break;
    case 'c': routeC.set(); break;
    case 'd': routeD.set(); break;
    case 'e': routeE.set(); break;
    case 'f': routeF.set(); break;
    case 'g': routeG.set(); break;
    case 'h': routeH.set(); break;
    case 'i': routeI.set(); break;
    case 'j': routeJ.set(); break;
  }
}

// release all routes of one yard
int yardUnset(int yardId) {
  int result = 0;  // 0 no error
  switch (yardId) {
    case 0 :
      result += routeA.unset();
      result += routeB.unset();
      result += routeC.unset();
      result += routeD.unset();
      result += routeE.unset();
      result += routeF.unset();
      result += routeG.unset();
      result += routeH.unset();
      result += routeI.unset();
      result += routeJ.unset();
      return result;
    default :
      Serial << F("unknown yardId ") << yardId << endl;
      return 0xE0; // Error 225
  }
}

// because routes are not in an array ;-(
void routeBegin() {
      routeA.begin();
      routeB.begin();
      routeC.begin();
      routeD.begin();
      routeE.begin();
      routeF.begin();
      routeG.begin();
      routeH.begin();
      routeI.begin();
      routeJ.begin();
}

// because routes are not in an array ;-(
void routeUpdate() {
      routeA.update();
      routeB.update();
      routeC.update();
      routeD.update();
      routeE.update();
      routeF.update();
      routeG.update();
      routeH.update();
      routeI.update();
      routeJ.update();
}

void setup() {
  Serial.begin(115200);
  Serial << "Weichensteuerung\n";
  delay(500);

  for (auto &i : turnout) i.begin();   // initialize each turnout (jede Weiche initialisieren)
  routeBegin();
  buttonBegin();
  Serial << "ende setup\n";
}

void loop() {
  buttonRead();    // act on button presses
  // serial << "Route " << buttonpin[] << '/n';
  // Serial << "off " << pinThrough << '/' << pinDiverging << '\n';
  serialRead();    // act on commands from Serial monitor

  // background activities
  for (auto &i : turnout) i.update();  // call run function for each turnout (jeder Weiche etwas Zeit für Hintergrundaktivitäten geben)

  routeUpdate();
}
//