#include <LiquidCrystal_I2C.h>

#define DIR1_PIN 4
#define STEP1_PIN 5
#define POT1_PIN A0

#define DIR2_PIN 2
#define STEP2_PIN 3
#define POT2_PIN A1

#define BUTTON1_PIN 7
#define BUTTON2_PIN 6

#define I2C_ADDR    0x27
#define LCD_COLUMNS 16
#define LCD_LINES   2
LiquidCrystal_I2C lcd(I2C_ADDR, LCD_COLUMNS, LCD_LINES);

#define MIN_RAW 0
#define MAX_RAW 1023
#define MIN_REVOLUTION 1
#define MAX_REVOLUTION 1000
#define STEPS_PER_REVOLUTION 200
#define DEFAULT_REVOLUTION 60
#define INTERVAL_LCD_PRINT 250

enum direction_t {
  CCW,
  CW
};

enum state_t {
  NONE,
  ENABLE,
  DIRECTION,
  REVOLUTION,
  POSITION
};

typedef struct {
  uint8_t id;
  state_t state;
} EventArgs;

typedef void (*CallbackFunction)(EventArgs e);

typedef struct {
  private:
    uint8_t id;
    uint8_t stepPin;
    uint8_t dirPin;
    EventArgs eventArgs;

    void internalStateChanged(state_t state) {
      if (eventArgs.state != state) {
        eventArgs.state = state;
        if (callbackFunction) {
          callbackFunction(eventArgs);
        }
      }
      eventArgs.state = NONE;
    }
  public:
    bool direction;
    int revolution;
    int position;
    bool enable;
    bool changed;

    CallbackFunction callbackFunction;

    unsigned long startTime;
    unsigned long timeDelay;

    void para(uint8_t step, uint8_t dir, uint8_t motorId = 0, uint8_t motorDirection = CW) {
      eventArgs.id = motorId;
      stepPin = step;
      dirPin = dir;
      direction = motorDirection;
      eventArgs.state = NONE;
      pinMode(stepPin, OUTPUT);
      pinMode(dirPin, OUTPUT);
      digitalWrite(stepPin, LOW);
      digitalWrite(dirPin, init);
      rpm(DEFAULT_REVOLUTION);
    }

    void onChanged(CallbackFunction onChangeFunction) {
      callbackFunction = onChangeFunction;
    }

    // per minute
    void rpm(int rpm) {
      int prevRPM = revolution;
      revolution = rpm;
      unsigned long div = (unsigned long)revolution * (unsigned long)STEPS_PER_REVOLUTION;
      timeDelay = 60000000UL / div;
      if (rpm != prevRPM) {
        internalStateChanged(REVOLUTION);
      }
    }

    void run(bool motorEnable, direction_t motorDirection = CW) {
      unsigned long currentTime = micros();

      if (motorEnable) {
        if (!enable) {
          startTime = currentTime;
          enable = motorEnable;
          internalStateChanged(ENABLE);
        }
        unsigned long elapsedTime = currentTime - startTime;
        if (elapsedTime >= timeDelay) {
          startTime = currentTime;
          digitalWrite(dirPin, direction);
          digitalWrite(stepPin, HIGH);
          digitalWrite(stepPin, LOW);
          if (motorDirection == CW) {
            position++;
          } else {
            position--;
          }
          internalStateChanged(POSITION);
        }
      } else {
        if (enable) {
          enable = motorEnable;
          internalStateChanged(ENABLE);
        }
      }
      if (direction != motorDirection) {
        direction = motorDirection;
        internalStateChanged(DIRECTION);
      }
      enable = motorEnable;
    }
} motor_t;

#define DEFAULT_DEBOUNCE_PERIOD 10UL  //  Debounce Delay 10 milliseconds
typedef struct {
  private:
    bool input;
    unsigned long startTime;

  public:
    bool state;

    bool debounce(bool bounce, const unsigned long timeDelay = DEFAULT_DEBOUNCE_PERIOD) {
      unsigned long currentTime = millis();
      bool prevState = state;
      if (bounce) {
        state = true;
      } else {
        if (state) {
          if (input) {
            startTime = currentTime;
          }
          unsigned long elapsedTime = currentTime - startTime;
          if (elapsedTime >= timeDelay) {
            state = false;
          }
        }
      }
      input = bounce;
      return state != prevState & state == true;
    }
} debounce_t;


const uint8_t numberOfMotors = 2;
const uint8_t dirPins[numberOfMotors] = {DIR1_PIN, DIR2_PIN};
const uint8_t stepPins[numberOfMotors] = {STEP1_PIN, STEP2_PIN};
const uint8_t buttonPins[numberOfMotors] = {BUTTON1_PIN, BUTTON2_PIN};
const uint8_t potPins[numberOfMotors] = {POT1_PIN, POT2_PIN};

debounce_t buttons[numberOfMotors];
motor_t motors[numberOfMotors];

bool oneSecond;

void MotorEnable();
void MotorSpeed();
void LCDPrint(bool manual = false);

void OnMotorChanged(EventArgs e);

void setup() {
  Serial.begin(115200);
  lcd.init();
  lcd.backlight();

  for (uint8_t index = 0; index < numberOfMotors; index++) {
    lcd.setCursor(0, index);
    lcd.print("M" + String(index + 1) + ":");
    lcd.setCursor(7, index);
    lcd.print("RPM:");
    lcd.setCursor(13, index);

    motors[index].para(stepPins[index], dirPins[index], index, CW);
    motors[index].onChanged(OnMotorChanged);
  }

  MotorSpeed();
  MotorEnable();
  LCDPrint(true);
}

void loop() {
  MotorSpeed();
  MotorEnable();
}

void MotorEnable() {
  for (uint8_t index = 0; index < numberOfMotors; index++) {
    bool pressed = buttons[index].debounce(!digitalRead(buttonPins[index]));
    motors[index].run(buttons[index].state);
  }
}

void MotorSpeed() {
  for (uint8_t index = 0; index < numberOfMotors; index++) {
    int rawValue = analogRead(potPins[index]);
    motors[index].rpm(map(rawValue, MIN_RAW, MAX_RAW, MIN_REVOLUTION, MAX_REVOLUTION));
  }
}

void OnMotorChanged(EventArgs e) {
  switch (e.state) {
    case   ENABLE...REVOLUTION:
      LCDPrint(true);
      break;
  }
  if (e.state == POSITION) {
    //Serial.println(motors[e.id].position);
  }
}

void LCDPrint(bool manual) {
  for (uint8_t index = 0; index < LCD_LINES; index++) {
    if (manual) {
      lcd.setCursor(3, index);
      lcd.print(motors[index].enable ? "ENA" : "DIS");
      lcd.setCursor(11, index);
      if (motors[index].revolution < 10) lcd.print("   ");
      else if (motors[index].revolution < 100) lcd.print("  ");
      else if (motors[index].revolution < 1000) lcd.print(' ');
      lcd.print(String(motors[index].revolution));
    }
  }
}