#include <Servo.h>

/****************************************************************************************************************************************************************
                                                  YASC - Yet Another Servo Controller
             /              Written for a classic Arduino Nano using the Arduino V1.x IDE, but adaptable to any suitable board             \
  __________/         Implements a simple 1:1 input to output control of standard hobby servos for turnout control on Model Railroads       \__________
  _________/          Inputs may be toggle switches, digital signals from other processors, Berrett Hill Touch-Toggles, TTP223 Touch         \_________
  ________/        controls; basically, anything that provides a 1/0 signal to indicate which of two positions the servo should move to       \________
         /         User may modify positions or servo speed on-the-fly using the Arduino Serial Monitor, or any serial Terminal Program        \
  ______/              Settings are saved in non-volatile memory at the request of the user, and automatically retrieved at startup             \______
  _____/           At startup, user is presented with a menu of available commands; the menu may be recalled at any time by entering '?'         \_____
  ____/                N.B. This code uses microseconds for servo settings.  Program development would have been far less convenient,             \____
  ___/                                   had we not had the use of an excellent Arduino Simulator, at Wokwi.com.                                   \___

***** The above header references some features not present in this stripped version, they are added in later *****
*****************************************************************************************************************************************************************
  Version information:  Root is YASC 1.0.6; reduced for Lesson 5
*****************************************************************************************************************************************************************/

//MAIN Section XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX

const bool HOME = 0;           //alleviates having to think about ones and zeros when evaluating logic
const bool THROWN = 1;         //

struct TURNOUTS {         //values that are static and must be stored for use after next reset/power up; serial function writeline(Turnout) must be updated if this structure changes
  uint8_t servoPin;       //where we send the pulse to position the servo
  uint8_t buttonPin;      //where we get the setting for the servo from(i.e. the Touch Toggle connection)
  int16_t homePosition;   //the angle needed for 'homing' the turnout
  int16_t thrownPosition; //the angle needed for 'throwing' the turnout
  int8_t ServoSpeed;      //initially zero, causes ASAP movement; positive values are degrees/update, negative are updates/degree, allowing a broad range of speed
};

struct TURNOUTV {         //values that are volatile, initialized after every reset/power up
  bool state;             //commanded state of the servo (input from digital)
  Servo thisServo;        //a holder for the instance of the servo
};
//I make the distinction of static vs volatile attributes for the turnouts, both as a memory aid(oh, yeah, this one doesn't persist...), and because it makes
//reading and writing that part which should go into the EEPROM, and come from it, dead simple.
struct TURNOUT {          //a struct of structs
  TURNOUTS S;             //static elements
  TURNOUTV V;             //volatile elements
};

//do we need a fn to throw all turnouts to the half way point for setup? Don't think so.  I've just set DEFHOME and DEFTHRN very close to the same number, run the code with all attached
//on the bench (or installed, but with no horns), then install them and connect them one at a time and adjust each at that point?  Seems like that would be the best option.

//N.B. Usually, if there are more than one board to be controlled, we create multiple turnout tables, using an #ifdef to select which one gets compiled; for
//example, in a similar code I've provided to a friend, there are six separate table instances for his six Arduinos; only one code, but six Turnouts[] tables

const uint16_t DEFHOME = 1400;  //starting point for Home position; 10 us equates to a little bit more than one degree
const uint16_t DEFTHRN = 1600;  //starting point for Thrown position
const int8_t ASAP = 0;         //as the word implies, move as quickly as possible to commanded position.  Negative and positive values are all slower than this.

TURNOUT Turnouts[] = {  //items shown are for 8 servos on a classic Nano; N.B. serial function writeline(Turnout) must be updated if TURNOUTS structure changes
  {{ 2,  3,  DEFHOME,  DEFTHRN, ASAP}, {}},    //a servo on pin 2, button on pin 3; default speed ASAP; note the volatile elements don't get initialized
  {{ 4,  5,  DEFHOME,  DEFTHRN, ASAP}, {}},    //a servo on pin 4, button on pin 5
  {{ 6,  7,  DEFHOME,  DEFTHRN, ASAP}, {}},
  {{ 8,  9,  DEFHOME,  DEFTHRN, ASAP}, {}},
  {{10, 11,  DEFHOME,  DEFTHRN, ASAP}, {}},
  {{14, 15,  DEFHOME,  DEFTHRN, ASAP}, {}},  //Skips D12, D13(LED)
  {{16, 17,  DEFHOME,  DEFTHRN, ASAP}, {}},
  {{18, 19,  DEFHOME,  DEFTHRN, ASAP}, {}},  //8 max for Nano/Uno; add rows, change pins as needed for MEGA; N.B. - servo pins must be <64 due to Servo.h limitation
};

const uint8_t NumTurnouts = sizeof(Turnouts) / sizeof(TURNOUT);  //automatically determines the number of turnouts represented in the Turnouts array - user just adds/deletes rows as desired

uint8_t PresTurnout = 0;       //index used to indicate which turnout is presently active in update

void setup() {
  serialStartup();   //opens port, emits hello message
  for (uint8_t i = 0; i < NumTurnouts; i++) {       //Now, for each turnout in the array, do the following actions once
    pinMode(Turnouts[i].S.buttonPin, INPUT_PULLUP); //make the button input pullup to ensure consistant readings, else the attached turnout will flicker if the input isn't connected.
    Turnouts[i].V.state = THROWN;                   //the button will read as 'HOME' in update, so pre-force the state to 'THROWN' to trigger an update
  }
}//end of setup ******************************************************************************************************

void loop() {
  //note how, since loop repeats endlessly, we don't need a for loop in loop.   By this mechanism, loop automatically cycles
  //through updating all turnouts, moving them only if their state input has changed, or if their end limits are user-modified
  if (updateTurnout(PresTurnout)) PresTurnout = (PresTurnout + 1) % NumTurnouts; //if done, move to next turnout to be managed; N.B. in this case, we must wrap around to 0
}//end of loop **************************************************************************************************************

void serialStartup() {
  Serial.begin(115200);  //configures the connection to Serial Monitor, the terminal window for the IDE
  Serial.flush();        //swallows any startup trash
  serialHello();         //report program name, version, date
}//end serialStartup **************************************************************************************************

void serialHello() { //Just a series of Serial.print() statements
  //print Hello banner here
  Serial.println(F("----------------------------------------------------------------"));
  Serial.println(F("                    Touch-Toggle Servo Control"));
  Serial.print(F("   "));             Serial.print(__DATE__);
  Serial.print(F(" File:   "));       Serial.println(__FILE__);
}//end serialHello **************************************************************************************************
//end MAIN section XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX

//UPDATES section XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX

enum DBG {TERSE, NORMAL, VERBOSE} debugLevel = NORMAL; //default level of serial feedback is normal.  Set using +|- keys; change to verbose when developing

bool updateTurnout(uint8_t To) {
  bool retval = false;  //returns true if move completed, or no move occurred. Default to false
  bool oldState = Turnouts[To].V.state;                //save old state for change check
  Turnouts[To].V.state = digitalRead(Turnouts[To].S.buttonPin);  //read new state
  //for clarity, we have expanded the logic for the movement condition
  bool S = (oldState == Turnouts[To].V.state);//Has the state changed?
  bool H = Turnouts[To].V.state == HOME && (Turnouts[To].V.thisServo.readMicroseconds() == Turnouts[To].S.homePosition);  //are we home and actually home?
  bool T = Turnouts[To].V.state == THROWN && (Turnouts[To].V.thisServo.readMicroseconds() == Turnouts[To].S.thrownPosition);//are we thrown and actually thrown?
  if (S && (H || T)) { //IDLE, since input hasn't changed state and position is consistent with state
    if (Turnouts[To].V.thisServo.attached()) waitForPulseEnd(To); //if we're still attached, wait to ensure a pulse has been emitted
    Turnouts[To].V.thisServo.detach();                            //ensures movement stopped, power consumption at minimum
    retval = true;                                               //signal that we can move on to next turnout
  }//end IDLE
  else {//we are MOVING
    if (moveit(To)) //returns true if move completed, i.e. if state position == position, false otherwise
      updateReport(To);                                      //emit report of turnout change complete
  }//end MOVING
  return retval;
} //end of UpdateTurnout **************************************************************************************************

void waitForPulseEnd(uint8_t to) { //monitors the servo output, watching for pulse end, must only be called for a servo instance that is 'attached' or we hang forever
  //must wait for high, then wait for low; could potentially delay software for 20 ms, as we can't sync to the interrupt; good thing nothing else is happening!
  while (!digitalRead(Turnouts[to].S.servoPin));//waits while signal low; will fall through if already high.
  while (digitalRead(Turnouts[to].S.servoPin));//waits while signal high
}//end of waitForPulseEnd *************************************************************************************************************

bool moveit(uint8_t To) {
  bool retval = false;//default reply is 'not done yet'
  Turnouts[To].V.thisServo.attach(Turnouts[To].S.servoPin);  //we know we're not where we're supposed to be, so ensure servo is enabled
  if (Turnouts[To].V.state == HOME) {//HOME
    Turnouts[To].V.thisServo.writeMicroseconds(Turnouts[To].S.homePosition);
  }//end HOME
  else {//THROWN
    Turnouts[To].V.thisServo.writeMicroseconds(Turnouts[To].S.thrownPosition);
  }  //end THROWN
  delay(abs(Turnouts[To].S.homePosition - Turnouts[To].S.thrownPosition)); //delay proportional to the distance
  if (debugLevel == VERBOSE) reportTurnout(To);//reports every pass through update, very noisy!
  if (Turnouts[To].V.state == HOME) {//if position = home, signal we're done
    if (Turnouts[To].S.homePosition == Turnouts[To].V.thisServo.readMicroseconds()) retval = true;
  }//endif
  else {//if position = thrown, signal we're done
    if (Turnouts[To].S.thrownPosition == Turnouts[To].V.thisServo.readMicroseconds()) retval = true;
  }//endif
  return retval;
} //end moveit ********************************************************************************************************

void updateReport(uint8_t to) {  //emit single line report of turnout update action
  //of the form "Turnout NN moved to HOME/THROWN (NNN)" where NNN is the angle being set per the Turnout table entry
  if (debugLevel != TERSE) {
    Serial.print(F("Turnout "));  Serial.print(to);  Serial.print(F(" moved to "));
    if (Turnouts[to].V.state == HOME) Serial.print(F(" HOME "));  else Serial.print(F("THROWN"));
    Serial.print(F(" ("));  Serial.print(Turnouts[to].V.thisServo.readMicroseconds());  Serial.print(F(")\n"));
  }//end debug level check
}//end updateReport **************************************************************************************************

void  reportTurnout(uint8_t tt) {
  Serial.print(F(" Turnout "));        sp(2, tt);                                                                  //remind the user which turnout this config is for
  Serial.print(F(" Servo Pin: "));     sp(2, Turnouts[tt].S.servoPin);                                             //print the servo pin
  Serial.print(F(" Buttn Pin: "));    sp(2, Turnouts[tt].S.buttonPin); //Serial.println();                         //print the button pin
  Serial.print(F(" Home A: "));    sp(4, Turnouts[tt].S.homePosition);                                             //print the home angle
  Serial.print(F(" Thrown A: "));  sp(4, Turnouts[tt].S.thrownPosition); //Serial.println();                       //print the thrown angle
  Serial.print(F(" State: "));  Turnouts[tt].V.state == HOME ? Serial.print(F(" HOME  ")) : Serial.print(F("THROWN "));   //print the state
  Serial.print(F(" Position: "));  sp(4, Turnouts[tt].V.thisServo.readMicroseconds()); Serial.println();           //print the current position
}//end reportTurnout **********************************************************************************************

void sp(uint8_t width, int16_t value) { //print integer with a consistant width; very brute-force, but not called often, and serial rate-limited anyway
  if (width >= 5 && value < 10000)       Serial.print(' ');
  if (width >= 4 && value <  1000)       Serial.print(' ');
  if (width >= 3 && value  <  100)       Serial.print(' ');
  if (width >= 2 && value  <   10)       Serial.print(' ');
  Serial.print(value);
}//end sp *********************************************************************************************

//end UPDATES section XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX