/*
SDS - Shift and Drive System
Bachelor Thesis 2023
Wrexham University
Jan Heidegger
-------------------------------------------------------------------------------------
two Way Hall Sensor Test for one Cassette
V_009 - Final Code
*/

#include <Arduino.h>
#include <U8g2lib.h>

U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0);

const int forwardSwitchPin = 8;
const int backwardSwitchPin = 6;
const int forwardLedPin = 9;
const int backwardLedPin = 7;
const int potentiometerPins[] = {A0, A1, A2, A3, A4, A5};
const int numberOfGears = 6;

int currentGear = 0;
bool isMovingForward = false;
bool isMovingBackward = false;
unsigned long lastDebounceTime = 0;
const unsigned long debounceDelay = 50;

void setup() {
  pinMode(forwardSwitchPin, INPUT);
  pinMode(backwardSwitchPin, INPUT);
  pinMode(forwardLedPin, OUTPUT);
  pinMode(backwardLedPin, OUTPUT);

  for (int i = 0; i < numberOfGears; i++) {
    pinMode(potentiometerPins[i], INPUT);
  }

  u8g2.begin();
  currentGear = findInitialGear();
  updateDisplay(currentGear);
}

void loop() {
  handleSwitches();
  if (isMovingForward || isMovingBackward) {
    checkMovementAndPosition();
  }
  checkForErrorCondition();
}

int findInitialGear() {
  int gearFound = 0;
  for (int i = 0; i < numberOfGears; i++) {
    if (isAtDestination(analogRead(potentiometerPins[i]))) {
      if (gearFound != 0) {
        return -1; // More than one Potentiometer between 0-300 and 700-1023
      }
      gearFound = i + 1;
    }
  }
  return gearFound;
}

bool isAtDestination(int potValue) {
  return potValue <= 300 || potValue >= 700;
}

void updateDisplay(int gear) {
  u8g2.clearBuffer();
  if (gear <= 0 || gear > numberOfGears) {
    u8g2.setFont(u8g2_font_ncenB08_tr);
    u8g2.drawStr(0, 24, "ERROR 001");
    u8g2.drawStr(0, 40, "Restart/Contact MFG");
  } else {
    u8g2.setFont(u8g2_font_logisoso32_tf); 
    u8g2.setCursor((u8g2.getDisplayWidth() - u8g2.getStrWidth(String(gear).c_str())) / 2, 48);
    u8g2.print(gear);
  }
  u8g2.sendBuffer();
}

void enterErrorState() {
  while(true) {
    digitalWrite(forwardLedPin, LOW);
    digitalWrite(backwardLedPin, LOW);
    u8g2.clearBuffer();
    u8g2.setFont(u8g2_font_ncenB08_tr);
    u8g2.drawStr(0, 24, "ERROR 001");
    u8g2.drawStr(0, 40, "Restart/Contact MFG");
    u8g2.sendBuffer();
    delay(1000);
  }
}

void checkForErrorCondition() {
  if (currentGear == -1) {
    enterErrorState();
  }
}

void handleSwitches() {
  int forwardSwitchState = digitalRead(forwardSwitchPin);
  int backwardSwitchState = digitalRead(backwardSwitchPin);

  // Ignore both switches when they are pressed simultaneously
  if (forwardSwitchState == HIGH && backwardSwitchState == HIGH) {
    return;
  }

  // Handle the case where a switch is pressed while the carriage is moving
  if ((millis() - lastDebounceTime) > debounceDelay) {
    if (forwardSwitchState == HIGH && !isMovingForward && !isMovingBackward && currentGear < numberOfGears) {
      currentGear++;
      digitalWrite(forwardLedPin, HIGH);
      isMovingForward = true;
    } else if (backwardSwitchState == HIGH && !isMovingBackward && !isMovingForward && currentGear > 1) {
      currentGear--;
      digitalWrite(backwardLedPin, HIGH);
      isMovingBackward = true;
    }
    lastDebounceTime = millis();
  }
}

void checkMovementAndPosition() {
  int potValue = analogRead(potentiometerPins[currentGear - 1]);
  if (isAtDestination(potValue)) {
    digitalWrite(forwardLedPin, LOW);
    digitalWrite(backwardLedPin, LOW);
    isMovingForward = false;
    isMovingBackward = false;
    updateDisplay(currentGear);
  }
}