#include <Servo.h>
const bool HOME = 0;      //alleviates having to think about ones and zeros when evaluating logic
const bool THROWN = 1;    //

/* Lesson 5a; restructuring data and code; see comments in accompanying discussion thread for explanations */

struct TurnoutS { //values that are static and must be stored for use after next reset/power up
  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)
  uint16_t homePosition;     //the angle needed for 'homing' the turnout
  uint16_t thrownPosition;   //the angle needed for 'throwing' the turnout
  bool oldState;             //where we store the previous state of the Touch Toggle
  int8_t servoSpeed;         //how quickly the servo moves from position to position; see notes on speed in updateTurnout code
};

struct TurnoutV { //values that are volatile, initialized after every reset/power up
  uint16_t presPosition;  //position the servo was last set to during a move(used during slow motion)
  bool state;                //commanded state of the servo (Touch Toggle
  Servo ourServo;            //a holder for the instance of the servo
};

struct Turnout { //a compound structure that allows us to have a single array of turnout-specific data, with persistent and volatile components
  TurnoutS S;
  TurnoutV V;
};

Turnout turnouts[] = { //one row per turnout; we init the static values only, hence the volatile components are empty
  {{2, 3, 45, 135, 0, HOME}, {}},  //the values correspond to the variables in TurnoutS; the empty {} signify that we are not initializing TurnoutV
  {{4, 5, 40, 140, 0, HOME}, {}},
  {{6, 7, 35, 145, 0, HOME}, {}},
};

const uint8_t numTurnouts = sizeof(turnouts) / sizeof(turnouts[0]); //auto-calculates the number of turnouts in the array during compile
uint8_t presTurnout = 0;      //variable (counter) to hold the 'in focus' turnout number; will always run from 0 to numTurnouts - 1

void setup() {
  Serial.begin(115200);
  for (;;) {
    Serial.print('.');
    //loop through turnout structs, init each one; presTurnout already created
    initTurnout();  //initialize turnout; note that this for loop consists of single statement, so we don't need {}!
    if (presTurnout == 0) break;
  }
  // presTurnout = 0;  //restore turnout counter to first turnout, so serialInit() begins in the right place(if it uses it)
} //end of setup

void loop() {    //implementing function calls makes loop simple, yet clear what activities are done
  updateTurnout(); //update present turnout by reading input, moving if needed, else move index to next turnout
} //end of loop

void initTurnout() {
  /*
    In order for turnouts to agree with buttons, we need to do an initial read of the buttons, and set the servos accordingly
    if the layout was shut down with a lot of turnouts in the non-default position, this will take a while, as we must assume each servo
    must move it's maximum amount - because we have no feedback to tell us where the servos really are; knowing where they were at shutdown
    would only be somewhat helpful, due to the everpresent risk of the user moving the servo(or replacing it, for that matter) when power is off.
    To effect the move as fast as possible, we save the speed setting in a local variable, then overwrite the speed setting with 0, which causes the fastest move. We
    restore the speed variable when done.
  */
  int8_t spd = turnouts[presTurnout].S.servoSpeed;   //save speed setting for present turnout
  turnouts[presTurnout].S.servoSpeed = 0;            //and set to 0 to force immediate jump
  turnouts[presTurnout].V.state = digitalRead(turnouts[presTurnout].S.buttonPin); //read what position the TT is in
  turnouts[presTurnout].S.oldState = !turnouts[presTurnout].V.state;              //set oldState to the opposite, so the move code will move the servo
  updateTurnout();                                                                //moves the present turnout to the state indicated by the TT
  turnouts[presTurnout].S.servoSpeed = spd;
} //end of initTurnout

/*
    logic for moving presTurnout is as follows:
    if TT hasn't changed and our position is consistent with one of the states, don't move
    if TT has changed, initiate moving towards new state
    if TT hasn't changed, but our position is not consistent with either state, (continue) move towards new state
*/
/*  determine what our state is
    if state not changed and position == stateposition
      IDLE         so just detach, in case we came from movement previous pass
      else     (either TT changed OR position is inconsistent)
      if TT state changed
        BEGIN       attach servo, change position dependent on speed
      else
        MOVING      change position dependent on speed (detail - only get here if speed not 0)
*/

void updateTurnout() {  //Okay, let's start the routine; check TT input
  turnouts[presTurnout].S.oldState = turnouts[presTurnout].V.state;                //save old state for change check
  turnouts[presTurnout].V.state = digitalRead(turnouts[presTurnout].S.buttonPin);  //read new state
  //for clarity, expanded logic for the movement condition
  bool S = turnouts[presTurnout].S.oldState == turnouts[presTurnout].V.state;
  bool H = turnouts[presTurnout].V.state == HOME && (turnouts[presTurnout].V.presPosition == turnouts[presTurnout].S.homePosition);
  bool T = turnouts[presTurnout].V.state == THROWN && (turnouts[presTurnout].V.presPosition == turnouts[presTurnout].S.thrownPosition);
  if (S && (H || T)) { //IDLE, since TT hasn't changed state and position is consistent with state
    turnouts[presTurnout].V.ourServo.detach();   //ensures movement stopped, power consumption at minimum
    nextTurnout();
  }//end IDLE
  else {//we are MOVING
    Serial.print("moving turnout: ");
    Serial.println(presTurnout);
    moveit();
    turnouts[presTurnout].S.oldState = turnouts[presTurnout].V.state;  //update oldState
  }//end MOVING
} //end of UpdateTurnout


void moveit() {
  turnouts[presTurnout].V.ourServo.attach(turnouts[presTurnout].S.servoPin);
  if (turnouts[presTurnout].S.servoSpeed > 0) {      //negative signifies that we move every N servo pulses
    /* monitor servo output, and count positive edges; when we've seen N, update the position by 1 degree, until we arrive at final
       no need for delay, as we presume this is slow enough
    */
  }
  else if (turnouts[presTurnout].S.servoSpeed < 0) { //positive signifies that we move N degrees every servo pulse
    /* monitor servo output and when we see a positive edge, update the position by N degrees
       may require a delay scaled by the number of degrees to move each 1/50 second
    */
  }
  else  {//move ASAP - zero indicates a move immediate to opposite side
    if (turnouts[presTurnout].V.state == HOME) {//HOME
      turnouts[presTurnout].V.presPosition = turnouts[presTurnout].S.homePosition;
      turnouts[presTurnout].V.ourServo.write(turnouts[presTurnout].S.homePosition);
    }//end HOME
    else {//THROWN
      turnouts[presTurnout].V.presPosition = turnouts[presTurnout].S.thrownPosition;
      turnouts[presTurnout].V.ourServo.write(turnouts[presTurnout].S.thrownPosition);
    }  //end THROWN
  }//end ASAP
  uint32_t dly;
  dly = abs((int)turnouts[presTurnout].S.homePosition - (int)turnouts[presTurnout].S.thrownPosition) * 10; //delay proportional to the distance; 10 results in 500 ms for a 50 degree move
  //Serial.print(turnouts[presTurnout].S.homePosition);
  //Serial.print(' ');
  //Serial.print(turnouts[presTurnout].S.thrownPosition);
  //Serial.print(' ');
  Serial.println(dly);
  delay(dly);
}//end moveit


void nextTurnout() {
  presTurnout = (presTurnout + 1) % numTurnouts; //increment presTurnout; % (aka modulo) arithmetic treturns value to 0 if we get to numTurnouts, which is what's needed
  //Serial.print(presTurnout);
}