// Controlling multiple servo motors and pumps using the Arduino Mega.

// Version 1:   https://wokwi.com/projects/369705616834606081
// Version 2:   https://wokwi.com/projects/370198618373197825
// Version 3:   https://wokwi.com/projects/370260137667236865
// Version 4:   https://wokwi.com/projects/370269233463345153  Added separate callback functions for each button.
// Version 4.1: https://wokwi.com/projects/370440521607948289  Added buttons and servos for turntables.
// Version 5:   https://wokwi.com/projects/370269233463345153  Modified turnLeft() and turnRight() to remember the current turn.
// Version 6:   https://wokwi.com/projects/372320472308699137  Added extra buttons to control pumps. Changed to Arduino Mega for extra I/O.
// Version 7:   https://wokwi.com/projects/372320472308699137  Added a state machine.
//                                                             Added a timer.
//                                                             Replaced mechButton with Debouncer to allow 500 ms for debouncing the reed switches.
//                                                             Added blinker class to blink the inbuilt LED.

#include <slowServo.h>

const byte TURN_1_DEGREES = 180;
const byte TURN_2_DEGREES = 0;

// Edit these angles for your mechanical layout.
const byte CLOSE_A1_DEGREES = 30;
const byte CLOSE_A2_DEGREES = 150;
const byte OPEN_A1_DEGREES = 90;
const byte OPEN_A2_DEGREES = 90;
const byte CLOSE_B1_DEGREES = 30;
const byte CLOSE_B2_DEGREES = 150;
const byte OPEN_B1_DEGREES = 90;
const byte OPEN_B2_DEGREES = 90;

const byte MS_PER_DEG = 30;

const uint32_t CLOSE_GATES_DELAY = 3000; // ms.
const uint32_t OPEN_GATES_DELAY = 3000;  // ms.
const uint32_t TURNTABLE_DELAY = 7000;   // ms.

// Edit these delays for your buttons and reed switches.
const uint16_t BUTTON_DEBOUNCE_DELAY = 50;       // ms.
const uint16_t REED_SWITCH_DEBOUNCE_DELAY = 500; // ms.

// Edit these speeds for your pumps.
const byte PUMP_FLOW_SPEED = 192;
const byte PUMP_FILL_LOCK_SPEED = 128;

class Timer
{
private:
  uint32_t timestamp;
  uint32_t timespan;

public:
  Timer()
  {
  }

  void Start(uint32_t timespan)
  {
    this->timespan = timespan;
    timestamp = millis();
  }

  bool Expired()
  {
    return (millis() - timestamp) >= timespan;
  }
};

// Blinker to provide a heartbeat signal.
class Blinker : public idler
{
private:
  Timer timer;
  const uint32_t timespan = 500;
  byte pin;

public:
  Blinker(byte pin)
  {
    this->pin = pin;
  }

  void Begin()
  {
    pinMode(pin, OUTPUT);
    timer.Start(timespan);
  }

  void idle()
  {
    if (timer.Expired())
    {
      digitalWrite(pin, !digitalRead(pin));
      timer.Start(timespan);
    }
  }
};

class Debouncer : public idler
{
private:
  Timer timer;
  uint32_t debounceDelay;
  byte pin;
  byte state;

public:
  Debouncer(byte pin, uint16_t debounceDelay)
  {
    this->pin = pin;
    this->debounceDelay = debounceDelay;
  }

  void Begin()
  {
    pinMode(pin, INPUT_PULLUP);
    state = digitalRead(pin);
  }

  byte State() const
  {
    return state;
  }

  void Update()
  {
    const bool newState = digitalRead(pin);
    // Hysteresis:
    //   If there is no change, reset the debounce timer.
    //   Else, compare the time difference with the debounce delay.
    if (newState == state)
    {
      timer.Start(debounceDelay);
    }
    else if (timer.Expired())
    {
      // Successfully debounced, so reset the debounce timer and update the state.
      state = newState;
      timer.Start(debounceDelay);
    }
  }

  void idle()
  {
    Update();
  }
};

class Pump
{
private:
  byte pin;
  byte speed;

public:
  Pump(byte pin, byte initialSpeed)
  {
    this->pin = pin;
    speed = initialSpeed;
  }

  void Begin()
  {
    analogWrite(pin, 0);
  }

  void SetSpeed(byte newSpeed)
  {
    speed = newSpeed;
    analogWrite(pin, newSpeed);
  }

  void Start()
  {
    analogWrite(pin, speed);
  }

  void Stop()
  {
    analogWrite(pin, 0);
  }
};

class Solenoid
{
private:
  byte pin;

public:
  Solenoid(byte pin)
  {
    this->pin = pin;
  }

  void Begin(byte initialState = HIGH)
  {
    pinMode(pin, OUTPUT);
    digitalWrite(pin, initialState);
  }

  void Open()
  {
    digitalWrite(pin, LOW);
  }

  void Close()
  {
    digitalWrite(pin, HIGH);
  }
};

class StateMachine : public idler
{
public:
  StateMachine(void (*function)(void))
  {
    Callback = function;
  }

  void (*Callback)(void);

  void idle()
  {
    if (Callback != nullptr)
    {
      Callback();
    }
  }
};

// Blinker.
Blinker blinker(LED_BUILTIN);

// Water pump control for flow speed.
Pump pump1(8, PUMP_FLOW_SPEED);  // Red LED represents DC water Pump 1.
Pump pump2(9, PUMP_FLOW_SPEED);  // Blue LED represent DC water Pump 2.
Pump pump3(10, PUMP_FLOW_SPEED); // Green LED represents DC water Pump 3.
Pump pump5(12, PUMP_FLOW_SPEED); // Yellow LED represents DC water Pump 5.

// Pump and solenoid control to fill and empty the lock.
Pump pump4(11, PUMP_FILL_LOCK_SPEED); // White LED represents DC water Pump 4.
Solenoid solenoid(36);                // Limegreen LED represents 12 V solenoid to empty the lock.

// Sensors and timers to move the boat from the higher water level right side to the lower water level left side.
Debouncer startButton(34, BUTTON_DEBOUNCE_DELAY);
Debouncer closeGatesAReedSwitch(24, REED_SWITCH_DEBOUNCE_DELAY);
Timer closeGatesATimer;
Debouncer openGatesBReedSwitch(26, REED_SWITCH_DEBOUNCE_DELAY);
Timer openGatesBTimer;
Debouncer turntableLeftReedSwitch(30, REED_SWITCH_DEBOUNCE_DELAY);
Timer turntableLeftTimer;

// Sensors and timers to move the boat from the lower water level left side to the higher water level on the right side.
Debouncer closeGatesBReedSwitch(28, REED_SWITCH_DEBOUNCE_DELAY);
Timer closeGatesBTimer;
Debouncer openGatesAReedSwitch(22, REED_SWITCH_DEBOUNCE_DELAY);
Timer openGatesATimer;
Debouncer turntableRightReedSwitch(32, REED_SWITCH_DEBOUNCE_DELAY);
Timer turntableRightTimer;

// Servos to control the lock gates and turntables.
slowServo gatesA1Servo(5);
slowServo gatesA2Servo(4);
slowServo gatesB1Servo(6);
slowServo gatesB2Servo(7);
slowServo turntableLeftServo(3);
slowServo turntableRightServo(2);

void TurnServo(slowServo &servo, byte turn)
{
  Serial.println("TurnServo()");
  int deg = turn == 1 ? TURN_1_DEGREES : TURN_2_DEGREES;
  servo.setDeg(deg);
}

void CloseGatesA()
{
  Serial.println("CloseGatesA()");
  gatesA1Servo.setDeg(CLOSE_A1_DEGREES);
  gatesA2Servo.setDeg(CLOSE_A2_DEGREES);
}

void CloseGatesB()
{
  Serial.println("CloseGatesB()");
  gatesB1Servo.setDeg(CLOSE_B1_DEGREES);
  gatesB2Servo.setDeg(CLOSE_B2_DEGREES);
}

void OpenGatesA()
{
  Serial.println("OpenGatesA()");
  gatesA1Servo.setDeg(OPEN_A1_DEGREES);
  gatesA2Servo.setDeg(OPEN_A2_DEGREES);
}

void OpenGatesB()
{
  Serial.println("OpenGatesB()");
  gatesB1Servo.setDeg(OPEN_B1_DEGREES);
  gatesB2Servo.setDeg(OPEN_B2_DEGREES);
}

// Create some states for the state machine.
enum class States : byte
{
  S00_IDLE,
  S01_FLOW_LEFT_UPPER,
  S02_CLOSE_GATES_A,
  S03_EMPTY_LOCK,
  S04_OPEN_GATES_B,
  S05_FLOW_LEFT_LOWER,
  S06_TURN_LEFT,
  S07_FLOW_RIGHT_LOWER,
  S08_CLOSE_GATES_B,
  S09_FILL_LOCK,
  S10_OPEN_GATES_A,
  S11_FLOW_RIGHT_UPPER,
  S12_TURN_RIGHT
};

// The state machine logic that is periodically called by the idler update.
void StateMachineCallback()
{
  static States presentState = States::S00_IDLE;

  //
  // Next state logic.
  //

  // The next state defaults to the present state;
  States nextState = presentState;

  switch (presentState)
  {
  case States::S00_IDLE:
    if (!startButton.State())
    {
      nextState = States::S01_FLOW_LEFT_UPPER;
    }
    break;

  case States::S01_FLOW_LEFT_UPPER:
    if (!closeGatesAReedSwitch.State())
    {
      nextState = States::S02_CLOSE_GATES_A;
    }
    break;

  case States::S02_CLOSE_GATES_A:
    if (closeGatesATimer.Expired())
    {
      nextState = States::S03_EMPTY_LOCK;
    }
    break;

  case States::S03_EMPTY_LOCK:
    if (!openGatesBReedSwitch.State())
    {
      nextState = States::S04_OPEN_GATES_B;
    }
    break;

  case States::S04_OPEN_GATES_B:
    if (openGatesBTimer.Expired())
    {
      nextState = States::S05_FLOW_LEFT_LOWER;
    }
    break;

  case States::S05_FLOW_LEFT_LOWER:
    if (!turntableLeftReedSwitch.State())
    {
      nextState = States::S06_TURN_LEFT;
    }
    break;

  case States::S06_TURN_LEFT:
    if (turntableLeftTimer.Expired())
    {
      nextState = States::S07_FLOW_RIGHT_LOWER;
    }
    break;

  case States::S07_FLOW_RIGHT_LOWER:
    if (!closeGatesBReedSwitch.State())
    {
      nextState = States::S08_CLOSE_GATES_B;
    }
    break;

  case States::S08_CLOSE_GATES_B:
    if (closeGatesBTimer.Expired())
    {
      nextState = States::S09_FILL_LOCK;
    }
    break;

  case States::S09_FILL_LOCK:
    if (!openGatesAReedSwitch.State())
    {
      nextState = States::S10_OPEN_GATES_A;
    }
    break;

  case States::S10_OPEN_GATES_A:
    if (openGatesATimer.Expired())
    {
      nextState = States::S11_FLOW_RIGHT_UPPER;
    }
    break;

  case States::S11_FLOW_RIGHT_UPPER:
    if (!turntableRightReedSwitch.State())
    {
      nextState = States::S12_TURN_RIGHT;
    }
    break;

  case States::S12_TURN_RIGHT:
    if (turntableRightTimer.Expired())
    {
      nextState = States::S00_IDLE;
    }
    break;

  default:
    break;
  }

  if (nextState != presentState)
  {
    // Update the present state with the next state.
    presentState = nextState;

    //
    // Output logic.
    //

    switch (presentState)
    {
    case States::S00_IDLE:
      Serial.println("S00_IDLE");
      // Switch everything off.
      break;

    case States::S01_FLOW_LEFT_UPPER:
      Serial.println("S01_FLOW_LEFT_UPPER");
      pump1.Start();
      break;

    case States::S02_CLOSE_GATES_A:
      Serial.println("S02_CLOSE_GATES_A");
      pump1.Stop();
      CloseGatesA();
      closeGatesATimer.Start(CLOSE_GATES_DELAY);
      break;

    case States::S03_EMPTY_LOCK:
      Serial.println("S03_EMPTY_LOCK");
      solenoid.Open();
      break;

    case States::S04_OPEN_GATES_B:
      Serial.println("S04_OPEN_GATES_B");
      solenoid.Close();
      OpenGatesB();
      openGatesBTimer.Start(OPEN_GATES_DELAY);
      break;

    case States::S05_FLOW_LEFT_LOWER:
      Serial.println("S05_FLOW_LEFT_LOWER");
      pump2.Start();
      break;

    case States::S06_TURN_LEFT:
      Serial.println("S06_TURN_LEFT");
      pump2.Stop();
      {
        static byte turn = 1;
        turn = turn == 1 ? 2 : 1;
        TurnServo(turntableLeftServo, turn);
        turntableLeftTimer.Start(TURNTABLE_DELAY);
      }
      break;

    case States::S07_FLOW_RIGHT_LOWER:
      Serial.println("S07_FLOW_RIGHT_LOWER");
      pump3.Start();
      break;

    case States::S08_CLOSE_GATES_B:
      Serial.println("S08_CLOSE_GATES_B");
      pump3.Stop();
      CloseGatesB();
      closeGatesBTimer.Start(CLOSE_GATES_DELAY);
      break;

    case States::S09_FILL_LOCK:
      Serial.println("S09_FILL_LOCK");
      pump4.Start();
      break;

    case States::S10_OPEN_GATES_A:
      Serial.println("S10_OPEN_GATES_A");
      pump4.Stop();
      OpenGatesA();
      openGatesATimer.Start(OPEN_GATES_DELAY);
      break;

    case States::S11_FLOW_RIGHT_UPPER:
      Serial.println("S11_FLOW_RIGHT_UPPER");
      pump5.Start();
      break;

    case States::S12_TURN_RIGHT:
      Serial.println("S12_TURN_RIGHT");
      pump5.Stop();
      {
        static byte turn = 1;
        turn = turn == 1 ? 2 : 1;
        TurnServo(turntableRightServo, turn);
        turntableRightTimer.Start(TURNTABLE_DELAY);
      }
      break;

    default:
      break;
    }
  }
}

StateMachine stateMachine(StateMachineCallback);

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

  startButton.Begin();
  blinker.Begin();
  closeGatesAReedSwitch.Begin();
  closeGatesBReedSwitch.Begin();
  openGatesAReedSwitch.Begin();
  openGatesBReedSwitch.Begin();
  turntableLeftReedSwitch.Begin();
  turntableRightReedSwitch.Begin();
  pump1.Begin();
  pump2.Begin();
  pump3.Begin();
  pump4.Begin();
  pump5.Begin();
  solenoid.Begin();

  // Add the blinker to the idler update.
  blinker.hookup();

  // Add the start button to the idler update.
  startButton.hookup();

  // Add the reed switches to the idler update.
  closeGatesAReedSwitch.hookup();
  openGatesBReedSwitch.hookup();
  turntableLeftReedSwitch.hookup();
  closeGatesBReedSwitch.hookup();
  openGatesAReedSwitch.hookup();
  turntableRightReedSwitch.hookup();

  // Add the state machine to the idler update.
  stateMachine.hookup();
  Serial.println("S00_IDLE");

  turntableLeftServo.begin();
  turntableRightServo.begin();
  turntableLeftServo.setDeg(TURN_1_DEGREES);
  turntableRightServo.setDeg(TURN_1_DEGREES);
  sleep(1000);
  turntableLeftServo.setMsPerDeg(MS_PER_DEG);
  turntableRightServo.setMsPerDeg(MS_PER_DEG);

  gatesA1Servo.begin();
  gatesA2Servo.begin();
  gatesA1Servo.setDeg(OPEN_A1_DEGREES);
  gatesA2Servo.setDeg(OPEN_A2_DEGREES);
  sleep(1000);
  gatesA1Servo.setMsPerDeg(MS_PER_DEG);
  gatesA2Servo.setMsPerDeg(MS_PER_DEG);

  gatesB1Servo.begin();
  gatesB2Servo.begin();
  gatesB1Servo.setDeg(CLOSE_B1_DEGREES);
  gatesB2Servo.setDeg(CLOSE_B2_DEGREES);
  sleep(1000);
  gatesB1Servo.setMsPerDeg(MS_PER_DEG);
  gatesB2Servo.setMsPerDeg(MS_PER_DEG);
}

void loop()
{
  idle();
}