#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 = 1505;  //starting point for Home position; 10 us equates to a little bit more than one degree
const uint16_t DEFTHRN = 1495;  //starting point for Thrown position
const int8_t MAXSPEED = 100;   //upper limit for speed setting; 20 indicates that, on every servo update, the angle will change by 20 degrees, or 1000 degrees/second (in theory)
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.   
const int8_t MINSPEED = -10;   //lower limit for speed setting; -50 indicates changing the angle by 1 degree every 50 servo pulse updates, or 1 degree per second aggregate

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 *************************************************************************************************************

static int8_t passcount = 0;  //used in case of slow movement, to track how many updates have happened between changes of position.

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
  switch (Turnouts[To].S.ServoSpeed) {
    case 0:   //MOVE ASAP
      //move ASAP - zero indicates a move immediate to opposite side
      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; with pos in us, removed factor of 10
      //end ASAP
      break;
    case MINSPEED ... -1: //move 1 degree every n pulse ends
      /* pass counter value cycles between 0 and speed value, which is negative.  Logic is:
         if pass counter < 0, add 1 and exit; else by defn pass counter is 0, so we must calculate new position,
         only if not already arrived,(this should never happen, why guard against it?)
            set pass counter to (speed + 1),
            wait for negative transition, and write new value
      */
      if (passcount < 0) passcount = passcount + 1;
      else {//can only get here if passcount = 0 and we're in negative speed
        int16_t pos = Turnouts[To].V.thisServo.readMicroseconds();                   //get current position; needs to be local variable
        int16_t target = Turnouts[To].V.state == HOME ? Turnouts[To].S.homePosition : Turnouts[To].S.thrownPosition;  //determine target position
        if (pos != target) {//need to move
          pos =  pos > target ? pos - 1 : pos + 1;
          /*  constrain pos to within the included limits home to thrown.
               if we're going home then if home > thrown ensure pos <= home else ensure pos >= home
                                        else if home > thrown ensure pos >= thrown else ensure pos <= thrown
          */
          if (Turnouts[To].V.state == HOME) {//if we're going home
            if (Turnouts[To].S.homePosition > Turnouts[To].S.thrownPosition) { //we're going up
              if (pos > Turnouts[To].S.homePosition) pos = Turnouts[To].S.homePosition;
            }//end going up
            else {  //going down
              if (pos < Turnouts[To].S.homePosition) pos = Turnouts[To].S.homePosition;
            }//end going down
          }
          else {//state is thrown
            if (Turnouts[To].S.thrownPosition > Turnouts[To].S.homePosition) {//we're moving up
              if (pos > Turnouts[To].S.thrownPosition) pos = Turnouts[To].S.thrownPosition;
            }//end going up
            else {  //going down
              if (pos < Turnouts[To].S.thrownPosition) pos = Turnouts[To].S.thrownPosition;
            }//end going down
          }//end THROWN
          passcount = Turnouts[To].S.ServoSpeed + 1;       //set up passcounter for next countdown
          waitForPulseEnd(To);                             //wait to ensure pulse has been emitted
          Turnouts[To].V.thisServo.writeMicroseconds(pos); //change position by N degrees
        }//end need to move
      }//end move 1 every n updates
      break;
    case 1 ... MAXSPEED:  //move by n degrees after every end of pulse
      { /* Calculate new position, wait until we see negative edge, then update the position by N degrees
           First, we need to determine what the next value should be for position.
           - read back current position - call that position
           - determine where we are going based on the home/thrown status flag, and the relevant position; call that target
           - know how much to add/subtract from that position - call that delta
           only if not already there, (this should never happen, why guard against it?)
              if target < delta, pos - delta; else pos + delta
              constrain(position, min(home,thrown), max(home,thrown))
              wait for negative edge, then write result
        */
        int16_t pos = Turnouts[To].V.thisServo.readMicroseconds();                   //get current position; needs to be local variable
        int16_t target = Turnouts[To].V.state == HOME ? Turnouts[To].S.homePosition : Turnouts[To].S.thrownPosition;  //determine target position
        if (pos != target) {//need to move
          pos =  pos > target ? pos - Turnouts[To].S.ServoSpeed : pos + Turnouts[To].S.ServoSpeed;            //calc new position
          if (Turnouts[To].V.state == HOME) {//if we're going home
            if (Turnouts[To].S.homePosition > Turnouts[To].S.thrownPosition) { //we're going up
              if (pos > Turnouts[To].S.homePosition) pos = Turnouts[To].S.homePosition;
            }//end going up
            else {  //going down
              if (pos < Turnouts[To].S.homePosition) pos = Turnouts[To].S.homePosition;
            }//end going down
          }//end HOME
          else {//state is thrown
            if (Turnouts[To].S.thrownPosition > Turnouts[To].S.homePosition) {//we're moving up
              if (pos > Turnouts[To].S.thrownPosition) pos = Turnouts[To].S.thrownPosition;
            }//end going up
            else {  //going down
              if (pos < Turnouts[To].S.thrownPosition) pos = Turnouts[To].S.thrownPosition;
            }//end going down
          }//end THROWN
          waitForPulseEnd(To);                                        //wait to ensure pulse has been emitted
          Turnouts[To].V.thisServo.writeMicroseconds(pos);                         //change position by N degrees
        }  //end need to move
      }//end move by n
      break;
    default: //any non-valid speed gets limited
      if (Turnouts[To].S.ServoSpeed < 0) {//limit to min
        Turnouts[To].S.ServoSpeed = MINSPEED;
      }//end limit to min
      else {//limit to max
        Turnouts[To].S.ServoSpeed = MAXSPEED;
      }//end limit to max
      break;
  }//end of switch selector
  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(" Speed: "));  sp(3, Turnouts[tt].S.ServoSpeed); //Serial.println();                              //print the speed setting
  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