#include <slowServo.h>

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

const byte CLOSE_A1_DEGREES = 30;//actually 150
const byte CLOSE_A2_DEGREES = 150;//actually 30
const byte OPEN_A1_DEGREES = 90;
const byte OPEN_A2_DEGREES = 90;
const byte CLOSE_B1_DEGREES = 30;//actually 150
const byte CLOSE_B2_DEGREES = 150;//actually 30
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 PUMP1_FLOW_SPEED = 192;
const byte PUMP2_FLOW_SPEED = 192;
const byte PUMP3_FLOW_SPEED = 192;
const byte PUMP4_FILL_LOCK_SPEED = 128;
const byte PUMP5_FLOW_SPEED = 192;


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;
  }
};

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

public:
  Debouncer(byte inPin, uint16_t inDebounceDelay)
  {
    pin = inPin;
    debounceDelay = inDebounceDelay;
    pinMode(pin, INPUT_PULLUP);
    state = digitalRead(pin);
  }

  byte GetState() 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 outPin, byte initialSpeed)
  {
    pin = outPin;
    speed = initialSpeed;
    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 outPin, byte initialState = HIGH)
  {
    pin = outPin;
    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();
    }
  }
};

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

// Pump and solenoid control to fill and empty the lock.
Pump pump4(11, PUMP4_FILL_LOCK_SPEED);  // White LED represents DC water Pump 4.
Solenoid solenoid(36, HIGH);           // 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.GetState())
    {
      nextState = States::S01_FLOW_LEFT_UPPER;
    }
    break;

  case States::S01_FLOW_LEFT_UPPER:
    if (!closeGatesAReedSwitch.GetState())
    {
      nextState = States::S02_CLOSE_GATES_A;
      closeGatesATimer.Start(CLOSE_GATES_DELAY);
    }
    break;

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

  case States::S03_EMPTY_LOCK:
    if (!openGatesBReedSwitch.GetState())
    {
      nextState = States::S04_OPEN_GATES_B;
      openGatesBTimer.Start(OPEN_GATES_DELAY);
    }
    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.GetState())
    {
      nextState = States::S06_TURN_LEFT;
      turntableLeftTimer.Start(TURNTABLE_DELAY);
    }
    break;

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

  case States::S07_FLOW_RIGHT_LOWER:
    if (!closeGatesBReedSwitch.GetState())
    {
      nextState = States::S08_CLOSE_GATES_B;
      closeGatesBTimer.Start(CLOSE_GATES_DELAY);
    }
    break;

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

  case States::S09_FILL_LOCK:
    if (!openGatesAReedSwitch.GetState())
    {
      nextState = States::S10_OPEN_GATES_A;
      openGatesATimer.Start(OPEN_GATES_DELAY);
    }
    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.GetState())
    {
      nextState = States::S12_TURN_RIGHT;
      turntableRightTimer.Start(TURNTABLE_DELAY);
    }
    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();
      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();
      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);
      }
      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();
      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();
      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);
      }
      break;

    default:
      break;
    }
  }
}

StateMachine stateMachine(StateMachineCallback);

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

  // 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();
}