#include <Wire.h>
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27, 16, 2); // Initialize the LCD display

// Define the pins for the stepper motor control
const int stepPin = 9;             // Step pin connected to A4988 STEP pin
const int dirPin = 8;              // Direction pin connected to A4988 DIR pin
const int buttonPinStart = 2;      // Start button pin
const int buttonPinStop = 3;       // Stop button pin
const int buttonPinDirChange = 4;  // Direction change button pin
const int buttonPinMode = 5;       // Mode button pin
const int buttonPinAngleInc = 6;   // Angle increase button pin
const int buttonPinAngleDec = 7;   // Angle decrease button pin
const int buttonPinReset = 10;     // Reset button pin
const int potPinSpeed = A0;        // Potentiometer pin for speed control
const int potPinRest = A1;
const int buttonPinResInc = 11;
const int newButtonPin = 12;

// Define variables
int stepDelay = 10000;          // Initial delay between steps in microseconds
int currentAngle = 0;           // Current angle setting
bool motorRunning = false;      // Flag to track motor state
bool angleMode = false;         // Flag to indicate the current mode
int direction = HIGH;           // Initial direction (clockwise)
int res = 1;
int totalsteps = 0;
unsigned long startTime; // Variable to store the start time of motor operation

void setup() {
  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);
  pinMode(buttonPinStart, INPUT_PULLUP);    // Internal pull-up resistor
  pinMode(buttonPinStop, INPUT_PULLUP);     // Internal pull-up resistor
  pinMode(buttonPinDirChange, INPUT_PULLUP); // Internal pull-up resistor
  pinMode(buttonPinMode, INPUT_PULLUP);     // Internal pull-up resistor
  pinMode(buttonPinAngleInc, INPUT_PULLUP); // Internal pull-up resistor
  pinMode(buttonPinAngleDec, INPUT_PULLUP); // Internal pull-up resistor
  pinMode(buttonPinReset, INPUT_PULLUP);    // Internal pull-up resistor
  pinMode(buttonPinResInc, INPUT_PULLUP);   // Internal pull-up resistor
  pinMode(newButtonPin, INPUT_PULLUP);   // Internal pull-up resistor

  // Initialize the LCD
  lcd.init();
  lcd.backlight();

  // Initialize Serial communication
  Serial.begin(9600);

  // Set initial direction (clockwise)
  digitalWrite(dirPin, direction);

  // Print initial instructions
  Serial.println("Press Mode button to switch between Angle and Speed modes.");
  Serial.println("Press Start button to start the motor.");
  Serial.println("Press Stop button to stop the motor.");
  Serial.println("Press Direction button to change motor direction.");
  Serial.println("Use Angle increase/decrease buttons to set the rotation angle.");
  Serial.println("Press Reset button to undo changes and reset.");
}

void loop() {
  // Read the state of the mode button
  if (digitalRead(buttonPinMode) == LOW) {
    angleMode = !angleMode; // Toggle mode
    Serial.print("Mode changed to: ");
    Serial.println(angleMode ? "Angle" : "Speed");
    delay(500); // debounce delay
  }

  // Read the state of the start button
  if (digitalRead(buttonPinStart) == LOW && !motorRunning) {
    motorRunning = true; // Start motor
    startTime = millis(); // Save the start time
    Serial.println("Motor started");
    delay(500); // debounce delay
  }

  // Read the state of the stop button
  if (digitalRead(buttonPinStop) == LOW && motorRunning) {
    motorRunning = false; // Stop motor
    startTime = 0; // Reset start time
    Serial.println("Motor stopped");
    delay(500); // debounce delay
  }

  // Read the state of the direction button
  if (digitalRead(buttonPinDirChange) == LOW) {
    direction = !direction; // Toggle direction
    digitalWrite(dirPin, direction);
    Serial.println(direction ? "Clockwise" : "Anti-Clockwise");
    delay(500); // debounce delay
  }

  if (digitalRead(buttonPinResInc) == LOW) {
    if (res < 100) {
      res = (res * 10);
    } else {
      res = 1;
      delay(500); // Debounce delay after resetting resolution to 1
    }
    Serial.print("Resolution ");
    Serial.println(res);
    delay(500); // debounce delay
  }

  // Read the state of the angle increase button
  if (digitalRead(buttonPinAngleInc) == LOW && !motorRunning) {
    currentAngle += (1 * res); // Increase angle
    Serial.print("Angle set to: ");
    Serial.println(currentAngle);
    delay(500); // debounce delay
  }

  // Read the state of the angle decrease button
  if (digitalRead(buttonPinAngleDec) == LOW && !motorRunning) {
    currentAngle -= (1 * res); // Decrease angle
    Serial.print("Angle set to: ");
    Serial.println(currentAngle);
    delay(500); // debounce delay
  }

  // Read the state of the reset button
  if (digitalRead(buttonPinReset) == LOW) {
    resetSystem();
    Serial.println("System reset.");
    delay(500); // debounce delay
  }

  // Read the state of a new button to initiate the timed rotation
  if (digitalRead(newButtonPin) == LOW) {
    rotateByAngleWithTime(currentAngle, 10000); // Rotate the motor by 123 degrees in 10 seconds
  }

  int PotValueSpeed = analogRead(potPinSpeed);
  stepDelay = map(PotValueSpeed, 0, 1023, 2000000, 100); // Adjust the range for speed

  // Execute based on current mode
  if (angleMode) {
    // Angle rotation mode
    if (motorRunning) {
      //updateMotorSpeed();
      rotateByAngle(currentAngle);
    }
  } else {
    // Speed control mode
    if (motorRunning) {
      //updateMotorSpeed();
      rotateBySpeed();
    }
  }

  // Calculate the elapsed time in milliseconds
  unsigned long elapsedTime = motorRunning ? millis() - startTime : 0;

  // Update display with speed, angle, and time information
  lcd.setCursor(0, 0); // Move cursor to the first row
  lcd.print("Speed: ");
  lcd.print(stepDelay); // Print the current speed value
  lcd.print(" Time: ");
  lcd.print(elapsedTime / 1000); // Print the elapsed time in seconds

  lcd.setCursor(0, 1); // Move cursor to the second row
  lcd.print("Rotation: ");
  lcd.print(currentAngle/360); // Print the current angle value
}

// Function to rotate the stepper motor by the specified angle
void rotateByAngle(int targetAngle) {
  // Calculate the number of steps required for the specified angle
  int steps = targetAngle / 1.8;
  if (direction == HIGH) {
    totalsteps += steps;
  } else {
    totalsteps -= steps;
  }

  // Rotate the stepper motor
  for (int i = 0; i < steps; i++) {
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(stepDelay); // Use the global stepDelay variable
    digitalWrite(stepPin, LOW);
    delayMicroseconds(stepDelay); // Use the global stepDelay variable
  }

  // Stop the motor after reaching the target angle
  motorRunning = false;
  currentAngle = 0;
  startTime = 0; // Reset start time
  Serial.println("Motor stopped after rotation.");
}



// Function to reset the system and return the motor to initial position
void resetSystem() {
  int stepDifference = totalsteps % 200; // Calculate the angle difference from current position to initial position
  Serial.print("stepDifference set to: ");
  Serial.println(stepDifference);
  direction = !direction; // Toggle direction
  digitalWrite(dirPin, direction);
  rotateByAngle(stepDifference * 1.8); // Rotate the motor to return to initial position (angle 0)
  Serial.println("Motor returned to initial position.");
  stepDelay = 10000;
  currentAngle = 0;
  motorRunning = false;
  angleMode = false;
  direction = HIGH;
  res = 1;
  totalsteps = 0;
  startTime = 0; // Reset start time
}

// Function to rotate the stepper motor with the current speed
void rotateBySpeed() {
  // Rotate the stepper motor continuously with the current speed
  digitalWrite(stepPin, HIGH);
  delayMicroseconds(stepDelay); // Use the global stepDelay variable
  digitalWrite(stepPin, LOW);
  delayMicroseconds(stepDelay); // Use the global stepDelay variable
}

// New function to update the motor speed
void updateMotorSpeed() {
  static unsigned long lastStepTime = 0;
  unsigned long currentTime = micros();
  if (currentTime - lastStepTime >= stepDelay) {
    digitalWrite(stepPin, !digitalRead(stepPin));
    lastStepTime = currentTime;
  }
}

// New function to rotate the motor by a specific angle in a specified time
void rotateByAngleWithTime(int targetAngle, unsigned long targetTime) {
  unsigned long startTime = millis();
  unsigned long elapsedTime = 0;
  int steps = targetAngle / 1.8;
  int totalSteps = 0;

  while (elapsedTime < targetTime && totalSteps < steps) {
    unsigned long currentTime = millis();
    elapsedTime = currentTime - startTime;
    float progress = (float)elapsedTime / targetTime;
    int stepsToRotate = steps * progress;

    // Calculate incremental steps to rotate
    int stepsIncrement = stepsToRotate - totalSteps;
    if (stepsIncrement > 0) {
      for (int i = 0; i < stepsIncrement; i++) {
        digitalWrite(stepPin, HIGH);
        delayMicroseconds(stepDelay);
        digitalWrite(stepPin, LOW);
        delayMicroseconds(stepDelay);
      }
      totalSteps += stepsIncrement; // Update total steps rotated
    }
  }

  // Stop the motor and update variables
  motorRunning = false;
  currentAngle = 0;
  Serial.println("Motor stopped after rotation.");
}