#include <Wire.h>
#include <LiquidCrystal_I2C.h>

// Set up the I2C LCD display (address, columns, rows)
LiquidCrystal_I2C lcd(0x27, 16, 2);

const int encoderPinA = 2; // Encoder pin A
const int encoderPinB = 3; // Encoder pin B
const int buttonPin = 4; // Button pin

const int stepPins[4] = {8, 9, 10, 11}; // Stepper motor step pins
const int dirPins[4] = {12, 13, 14, 15}; // Stepper motor direction pins

const char* motorNames[4] = {"Base", "Shoulder", "Elbow", "Wrist"};

volatile long encoderPos = -2;
long prevEncoderPos = 0;
int encoderState;
int lastEncoderState;
int activeMotor = 0; // Currently active motor

bool buttonState;
bool lastButtonState;

const float stepAngle = 1.8; // Step angle of the stepper motor

void setup() {
  pinMode(encoderPinA, INPUT_PULLUP);
  pinMode(encoderPinB, INPUT_PULLUP);
  pinMode(buttonPin, INPUT_PULLUP);

  for (int i = 0; i < 4; i++) {
    pinMode(stepPins[i], OUTPUT);
    pinMode(dirPins[i], OUTPUT);
  }

  encoderState = readEncoder();
  lastEncoderState = encoderState;
  lastButtonState = digitalRead(buttonPin);

  // Initialize the LCD
  lcd.init();
  lcd.backlight();
  lcd.clear();
  updateLCD();
}

void loop() {
  encoderState = readEncoder();
  buttonState = digitalRead(buttonPin);

  if (buttonState != lastButtonState) {
    if (buttonState == LOW) {
      activeMotor = (activeMotor + 1) % 4; // Switch to the next motor
      updateLCD();
    }
    delay(5); // Debounce delay
  }

  if (encoderState != lastEncoderState) {
    if (encoderState > lastEncoderState) {
      digitalWrite(dirPins[activeMotor], HIGH); // Clockwise
      encoderPos++;
    } else {
      digitalWrite(dirPins[activeMotor], LOW); // Counterclockwise
      encoderPos--;
    }

    digitalWrite(stepPins[activeMotor], HIGH);
    delayMicroseconds(500); // Adjust delay for desired stepper motor speed
    digitalWrite(stepPins[activeMotor], LOW);
    delayMicroseconds(500); // Adjust delay for desired stepper motor speed

    lastEncoderState = encoderState;
  }

  lastButtonState = buttonState;
}

int readEncoder() {
  int pinAVal = digitalRead(encoderPinA);
  int pinBVal = digitalRead(encoderPinB);

  if (pinAVal && !pinBVal) {
    return 1;
  } else if (pinAVal && pinBVal) {
    return 2;
  } else if (!pinAVal && pinBVal) {
    return 3;
  } else {
    return 0;
    }
}

float getAngle() {
return encoderPos * stepAngle;
}

void updateLCD() {
lcd.clear();
lcd.setCursor(0, 0);
lcd.print(motorNames[activeMotor]);
lcd.print(" Motor");

lcd.setCursor(0, 1);
lcd.print("Pos: ");
lcd.print(getAngle(),1);
lcd.print((char)223); // Degree symbol
}