#include <Button_SL.hpp>
#include <Servo.h>

//////////////////////////////////////////////////
// Class and structure definitions
//////////////////////////////////////////////////

class Interval {
public:
  bool operator()(const uint32_t duration) {
    if (false == isStarted) { return start(false); }
    return (millis() - timeStamp >= duration) ? start(true) : false;
  }
  void reset() { isStarted = false; }

private:
  bool start(bool state = false) {
    isStarted = !state;   // Set the value to true on the first call
    timeStamp = millis();
    return state;
  }

  bool isStarted {false};   // Flag = true if the first Operator() call has been made.
  uint32_t timeStamp {0};
};

class ButtonDelay {
public:
  ButtonDelay(Interval& timer, uint32_t delay_ms) : timer {timer}, delay_ms {delay_ms} {}
  enum class State : uint8_t { released, changed, set };

  State operator()(uint8_t mask) {
    if (!mask) {
      prvMask = 0;
      isSet = State::released;
    } else if (mask != prvMask) {
      timer.reset();
      prvMask = mask;
      isSet = State::changed;
    } else if (isSet != State::set && timer(delay_ms)) {
      isSet = State::set;
    }
    return isSet;
  }

private:
  Interval& timer;
  uint32_t delay_ms;
  State isSet {State::released};
  uint8_t prvMask {0};
};

struct MyServo {
  const uint8_t pin;
  const uint8_t startPos;
  const uint8_t endPos;
  uint8_t actPos;
  const int step;
  const uint32_t delay_ms;
  Servo sv;
};

struct Motor {
  const uint8_t fwPin;
  const uint8_t revPin;
  const uint8_t speedPin;
  const uint8_t speed;
  uint8_t isStarted;
};

//////////////////////////////////////////////////
// Global constants
//////////////////////////////////////////////////
namespace gc {
constexpr uint32_t delay_ms {1000};   // Delay before a button press is accepted

constexpr uint8_t servoVR {0};        // Index servo right
constexpr uint8_t servoVL {1};        // Index servo left

constexpr uint8_t motorVR {0};        // Index motor right
constexpr uint8_t motorVL {1};        // Index motor left
}   // namespace gc

//////////////////////////////////////////////////
// Global variables
//////////////////////////////////////////////////
Interval interval;
Interval svInterval;
Btn::Button buttons[] {{A0}, {A1}, {A2}, {A3}};
ButtonDelay btnDelay {interval, gc::delay_ms};

// { Servo pin, start pos, end pos, actual pos (=start pos), step size, delay between steps }
MyServo servos[] {
    {8, 45, 135, 45, 1, 25},
    {9, 45, 135, 45, 1, 25}
};

// { Forward pin, reverse pin, speed pin, speed, motor state (off = false/ on = true) }
// Pin 255 - Not used
Motor motors[] {
    {5,  255, 6, 200, false},
    {11, 255, 3, 200, false}
};

//////////////////////////////////////////////////
// Functions
//////////////////////////////////////////////////
template <typename T, size_t N> T checkButtons(Btn::Button (&btns)[N]) {
  T btnMask {0};
  for (size_t i {0}; i < N; ++i) {
    (btns[i].tick() == true) ? bitWrite(btnMask, i, 1) : bitWrite(btnMask, i, 0);
  }
  return btnMask;
}

void startMotor(Motor& motor, uint8_t fw) {
  if (motor.isStarted) { return; }
  // Serial.println("Start motor");
  digitalWrite(motor.fwPin, fw);
  analogWrite(motor.speedPin, motor.speed);
  motor.isStarted = true;
}

void stopMotor(Motor& motor) {
  // Serial.println("Stop motor");
  digitalWrite(motor.fwPin, LOW);
  analogWrite(motor.speedPin, 0);
  motor.isStarted = false;
}

void servoForward(MyServo& servo, Interval& wait) {
  if(wait(servo.delay_ms)) {
    if (servo.actPos >= servo.startPos && servo.actPos < servo.endPos) { servo.sv.write(servo.actPos += servo.step); }
  }
}

void servoReverse(MyServo& servo, Interval& wait) {
  if(wait(servo.delay_ms)) {
    if (servo.actPos <= servo.endPos && servo.actPos > servo.startPos) { servo.sv.write(servo.actPos -= servo.step); }
  }
}

void evaluate(uint8_t value) {
  switch (value) {
    case 0b1000:   // Button1
      servoForward(servos[gc::servoVR], svInterval);
      break;
    case 0b0100:   // Button2
      servoReverse(servos[gc::servoVR], svInterval);
      break;
    case 0b0010:   // Button3
      servoForward(servos[gc::servoVL], svInterval);
      break;
    case 0b0001:   // Button4
      servoReverse(servos[gc::servoVL], svInterval);
      break;
    case 0b1100:   // Button 1 + 2: Turn forward with speed x
      startMotor(motors[gc::motorVR], HIGH);
      break;
    case 0b1010:   // Button 1 + 3: Turn forward with speed x
      startMotor(motors[gc::motorVL], HIGH);
      break;
      // case 0b00000101: Serial.println("Button 2 und 4 gedrückt"); break;
      // case 0b00000011: Serial.println("Button 3 und 4 gedrückt"); break;
  }
}

//////////////////////////////////////////////////
// Main
//////////////////////////////////////////////////

void setup() {
  // Serial.begin(115200);
  // Serial.println("Start");
  // Initialize Buttons
  for (auto& btn : buttons) {
    btn.begin();
  }
  // set motor pins
  for (const auto& motor : motors) {
    pinMode(motor.fwPin, OUTPUT);
    pinMode(motor.revPin, OUTPUT);
  }

  for (auto& servo : servos) {
    servo.sv.attach(servo.pin);       // attaches the digital pin to the servo object
    servo.sv.write(servo.startPos);   // tell servo to go to position in variable 'pos'
  }
}

void loop() {
  uint8_t btnMask = checkButtons<uint8_t>(buttons);

  switch (btnDelay(btnMask)) {
    case ButtonDelay::State::released:
    case ButtonDelay::State::changed:
      for (auto& motor : motors) {   // Stop all running motors
        if (motor.isStarted) { stopMotor(motor); }
      }
      return;
    case ButtonDelay::State::set: evaluate(btnMask); break;
  }
}