#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

Adafruit_SSD1306 display(128, 32, &Wire, -1);

#define pinA 1
#define pinB 0

#define SDA_PIN 2
#define SCL_PIN 3

#define hourPin 4
#define minPin 5
#define secPin 6
#define startPin 9
#define safePin 10

#define pwmPin 11
#define dirPin 12

#define outPin2 13
#define outPin1 14

#define vinPin 26

#define numRotation 10  // 10 motor rotation
#define speed 128       // 0-255
#define dir true        // direction CW

int hh = 0, mm = 0, ss = 0;
bool start = false;

unsigned long timeNow = 0;
int count = 0;
int newCount = 0;
bool flag = false;

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

  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    while (true) {}
  }
  display.clearDisplay();
  display.display();
  display.setTextSize(2);
  display.setTextColor(WHITE);

  pinMode(pinA, INPUT_PULLUP);
  pinMode(pinB, INPUT_PULLUP);

  pinMode(hourPin, INPUT_PULLUP);
  pinMode(minPin, INPUT_PULLUP);
  pinMode(secPin, INPUT_PULLUP);
  pinMode(startPin, INPUT_PULLUP);
  pinMode(safePin, INPUT_PULLUP);

  pinMode(dirPin, OUTPUT);
  pinMode(pwmPin, OUTPUT);

  pinMode(outPin1, OUTPUT);
  pinMode(outPin2, OUTPUT);

  pinMode(vinPin, INPUT);

  digitalWrite(outPin1, 0);
  digitalWrite(outPin2, 0);
  analogWrite(pwmPin, 0);
}

void loop() {
  if (!digitalRead(hourPin)) {
    hh++;
    if (hh >= 24) hh = 0;
    delay(250);
  }
  if (!digitalRead(minPin)) {
    mm++;
    if (mm >= 60) mm = 0;
    delay(250);
  }
  if (!digitalRead(secPin)) {
    ss++;
    if (ss >= 60) ss = 0;
    delay(250);
  }

  if (!digitalRead(startPin)) {
    start = true;
    delay(250);
  }

  if (millis() % 1000 == 0) {
    display.clearDisplay();
    display.drawRect(0, 0, 128, 32, SSD1306_WHITE);
    display.setCursor(16, 8);
    // Manually format and display time (HH:MM:SS)
    if (hh < 10) display.print("0");
    display.print(hh);
    display.print(":");
    if (mm < 10) display.print("0");
    display.print(mm);
    display.print(":");
    if (ss < 10) display.print("0");
    display.print(ss);
    display.display();
  }

  // when start button is pressed
  while (start) {
    // outputs to a 24v out
    digitalWrite(outPin1, 1);

    // A pre warn countdown from 10 seconds
    for (int i = 10; i >= 0; i--) {
      if (!safe()) break;
      display.clearDisplay();
      display.setCursor(16, 8);
      display.print(i);
      delay(1000);
    }
    // turn off output 1
    digitalWrite(outPin1, 0);

    while (ss != 0 && mm != 0 && hh != 0) {
      if (!safe()) break;
      else {
        // other 24v output is made high
        digitalWrite(outPin2, 1);

        if (!digitalRead(pinA) && !flag) {
          if (count < numRotation) count++;
          flag = true;
        } else
          flag = false;

        if (millis() - timeNow >= 1000) {
          timeNow = millis();

          if (count < numRotation) {
            // Set motor direction and speed
            digitalWrite(dirPin, dir);
            analogWrite(pwmPin, speed);
          } else {
            // turn off motor
            analogWrite(pwmPin, 0);
          }

          // Update the countdown timer
          if (ss > 0) {
            ss--;
          } else if (mm > 0) {
            mm--;
            ss = 59;
          } else if (hh > 0) {
            hh--;
            mm = 59;
            ss = 59;
          }
          display.clearDisplay();
          display.drawRect(0, 0, 128, 32, SSD1306_WHITE);
          display.setCursor(16, 8);
          // display time (HH:MM:SS)
          if (hh < 10) display.print("0");
          display.print(hh);
          display.print(":");
          if (mm < 10) display.print("0");
          display.print(mm);
          display.print(":");
          if (ss < 10) display.print("0");
          display.print(ss);
          display.display();
        }
      }
    }
    // after countdown finished or stopped or safety reasons

    // turn off output 2
    digitalWrite(outPin2, 1);

    // the motor returns ccw
    while (newCount < count) {
      if (!digitalRead(pinA) && !flag) {
        newCount++;
        flag = true;
      } else
        flag = false;

      // Set motor opposite direction and speed
      digitalWrite(dirPin, !dir);
      analogWrite(pwmPin, speed);
    }

    // turn off motor
    analogWrite(pwmPin, 0);
  }
}

bool safe() {
  if (digitalRead(safePin) || !digitalRead(startPin) || !digitalRead(vinPin)) {
    start = false;
    return false;
  } else
    return true;
}