#include <Servo.h> // include the Servo library

// servo controlled turnouts
//   using a button switch to toggle the position of the switch, and
//   3 shift registers to drive LEDs
//   servos move 1 deg/70 msec

// ---------------------------------------------------------
// user defined struct describing sw[] table entries
struct Switch {
    byte    PinBut;         // pin # of button switch
    byte    PinServo;       // pin # of servo

    byte    AngMain;        // servo angle for switch aligned for main path
    byte    AngDiv;         // servo angle for switch aligned for divering path

    // shift register bits, 0-23 (see 74595 datasheet)
    // bits in ledBits[] shift MSB first starting with
    // ledBits[2].    bit 0 is Q0 chip connected to PinData
    byte    BitMainGr;      // main green panel LED
    byte    BitMainRd;      // main read panel LED
    byte    BitDivGr;       // diverging red panel LED
    byte    BitDivRd;       // diverging red panel LED

    // variables - uninitialized, zero by default
    byte    align;          // desired alignment Main/Div
    byte    ang;            // current angle
    byte    angTarg;        // target angle
    byte    butState;       // previous button state
    bool    ledsOn;         // state of LEDs On/Off - off while moving
    Servo   servo;          // servo library object
};

enum { Main, Div };     // align

Switch sw [] = {
//      pins      angle    shift-register bits
//   --------  --------- ----------------------
//   pinB pinS angM angD mainG mainR  divG, divR
    {  A0,   2,  45, 135,    0,    1,    2,    3 },
    {  A1,   3, 135,  45,    4,    5,    6,    7 },
    {  A2,   4,  93, 117,    8,    9,   10,   11 },
    {  A3,   5,  93, 117,   12,   13,   14,   15 },
    {  A4,   6,  93, 117,   16,   17,   18,   19 }
};
const int Nsw = sizeof(sw)/sizeof(Switch);

const unsigned long DegPmsec = 70;      // deg/msec

// ---------------------------------------------------------
// shift register parameters
const int  NshiftReg = 3;
const byte PinLatch  = 7;
const byte PinClock  = 8;
const byte PinData   = 9;

byte ledBits  [NshiftReg];

enum { LedOn = HIGH, LedOff = LOW };

char s [90];

// -----------------------------------------------------------------------------
void setup ()
{
    Serial.begin(115200);

    pinMode (PinLatch, OUTPUT);
    pinMode (PinClock, OUTPUT);
    pinMode (PinData,  OUTPUT);

    for (int i = 0; i < Nsw; i++){
        pinMode (sw [i].PinBut, INPUT_PULLUP);
        sw [i].servo.attach (sw [i].PinServo);
        sw [i].servo.write  (sw [i].AngMain);
        sw [i].angTarg = sw [i].AngMain;
        sw [i].ang     = sw [i].AngMain;
    }
}

// -----------------------------------------------------------------------------
void loop ()
{
    for (int i = 0; i < Nsw; i++)  {
        byte but = digitalRead (sw [i].PinBut);
        if (sw [i].butState != but) {           // is button press/released?
            sw [i].butState  = but;             // update state

            if (LOW == but) {                   // button pressed
                sprintf (s, "button pressed %d", i);
                Serial.println (s);

                if (Main == sw [i].align) {     // currently aligned Main
                    sw [i].align   = Div;           // change to Diverging
                    sw [i].angTarg = sw [i].AngDiv;
                }
                else {
                    sw [i].align   = Main;          // change to Main
                    sw [i].angTarg = sw [i].AngMain;
                }

                setLedBits (i, LedOff, LedOff);     // turn off LEDs
            }
        }

        // update servo angle
        if (sw [i].ang < sw [i].angTarg)            // less than target
            sw [i].servo.write (++sw [i].ang);

        else if (sw [i].ang > sw [i].angTarg)       // greater than target
            sw [i].servo.write (--sw [i].ang);

        else if (! sw [i].ledsOn) {   // turn LEDs on when target reached
            sprintf (s, "turnout %d alligned", i);
            Serial.println (s);
            if (Main == sw [i].align)
                setLedBits (i, LedOn,  LedOff);
            else
                setLedBits (i, LedOff, LedOn);
        }
    }

    delay (DegPmsec);
}


// -----------------------------------------------------------------------------
void setLedBits (
    int id,
    int mainClr,
    int divClr )
{
    sprintf (s, "setLedBits: %d, main %d, div %d", id, mainClr, divClr);
    Serial.println (s);

    // if Main LEDs clear, set Div LEDs stop, or visa versa
    setLedBit (sw [id].BitMainGr, mainClr);
    setLedBit (sw [id].BitDivRd,  mainClr);

    // if Div LEDs clear, set Main LEDs stop, or visa versa
    setLedBit (sw [id].BitDivGr,  divClr);
    setLedBit (sw [id].BitMainRd, divClr);

    shiftLedBits ();

    if (! mainClr && ! divClr)    // or possibly all LEDs off
        sw [id].ledsOn = false;
    else
        sw [id].ledsOn  = true;
}

// -----------------------------------------------------------------------------
void setLedBit (
    int   id, 
    byte state)
{
    bitWrite (ledBits [id/8], id%8, state);
}


// -----------------------------------------------------------------------------
void shiftLedBits ()
{
    digitalWrite (PinLatch, LOW);

    for (int i = (NshiftReg - 1); i>=0; i--)
        shiftOut (PinData, PinClock, MSBFIRST, ledBits[i]);

    digitalWrite (PinLatch, HIGH);
}