#include <Arduino.h>

// Pin Definitions for L298 Motor Driver
#define MOTOR1_IN1_PIN 14    // Motor 1 IN1 (Direction pin 1)
#define MOTOR1_IN2_PIN 12    // Motor 1 IN2 (Direction pin 2)
#define MOTOR1_EN_PIN  13    // Motor 1 EN (Enable pin)

#define MOTOR2_IN1_PIN 27    // Motor 2 IN1 (Direction pin 1)
#define MOTOR2_IN2_PIN 26    // Motor 2 IN2 (Direction pin 2)
#define MOTOR2_EN_PIN  25    // Motor 2 EN (Enable pin)

// Pin Definitions for Buttons
#define BUTTON1_PIN  32    // Program selection button (A/B)
#define BUTTON2_PIN  33    // Motor selection button (1/2)

// Program modes
#define PROGRAM_A  1
#define PROGRAM_B  2

// Motor selection
#define MOTOR_1    1
#define MOTOR_2    2

volatile int currentProgram = PROGRAM_A;  // Default to Program A
volatile int currentMotor = MOTOR_1;     // Default to Motor 1

// Timing Variables
unsigned long motorStartTime = 0;
unsigned long motorStopTime = 0;
unsigned long motorInterval = 20000;  // Motor run time (20 sec)
unsigned long stopInterval = 10000;   // Motor stop time (10 sec)
bool motorRunning = false;             // Flag to track motor state

// Debounce time for button press
#define DEBOUNCE_TIME 200

// Interrupt Service Routine (ISR) for Button1 (Program selection)
void IRAM_ATTR button1_ISR() {
  static unsigned long lastInterruptTime = 0;
  unsigned long interruptTime = millis();
  // Debounce check
  if (interruptTime - lastInterruptTime > DEBOUNCE_TIME) {
    currentProgram = (currentProgram == PROGRAM_A) ? PROGRAM_B : PROGRAM_A;
    Serial.print("Program selected: ");
    Serial.println(currentProgram == PROGRAM_A ? "A" : "B");
    lastInterruptTime = interruptTime;

    // Reset timing when the program is switched
    motorStartTime = millis();
  }
}

// Interrupt Service Routine (ISR) for Button2 (Motor selection)
void IRAM_ATTR button2_ISR() {
  static unsigned long lastInterruptTime = 0;
  unsigned long interruptTime = millis();
  // Debounce check
  if (interruptTime - lastInterruptTime > DEBOUNCE_TIME) {
    currentMotor = (currentMotor == MOTOR_1) ? MOTOR_2 : MOTOR_1;
    Serial.print("Motor selected: ");
    Serial.println(currentMotor == MOTOR_1 ? "1" : "2");
    lastInterruptTime = interruptTime;

    // Reset motor start time when the motor is switched
    motorStartTime = millis();
  }
}

// Function to start a motor
void startMotor(int motor) {
  if (motor == MOTOR_1) {
    digitalWrite(MOTOR1_IN1_PIN, HIGH);
    digitalWrite(MOTOR1_IN2_PIN, LOW);
    digitalWrite(MOTOR1_EN_PIN, HIGH);  // Enable motor 1
    Serial.println("Motor 1 started");
  } else if (motor == MOTOR_2) {
    digitalWrite(MOTOR2_IN1_PIN, HIGH);
    digitalWrite(MOTOR2_IN2_PIN, LOW);
    digitalWrite(MOTOR2_EN_PIN, HIGH);  // Enable motor 2
    Serial.println("Motor 2 started");
  }
}

// Function to stop a motor
void stopMotor(int motor) {
  if (motor == MOTOR_1) {
    digitalWrite(MOTOR1_EN_PIN, LOW);  // Disable motor 1
    Serial.println("Motor 1 stopped");
  } else if (motor == MOTOR_2) {
    digitalWrite(MOTOR2_EN_PIN, LOW);  // Disable motor 2
    Serial.println("Motor 2 stopped");
  }
}

// Function to run Program A (Motor 1 for 20 sec, then stop)
void runProgramA() {
  // Start the selected motor
  startMotor(currentMotor);
  motorStartTime = millis();
  motorRunning = true;
  motorInterval = 20000;  // Motor runs for 20 seconds
  motorStopTime = motorStartTime + motorInterval;
}

// Function to run Program B (Motor 2 for 20 sec, then stop)
void runProgramB() {
  // Start the selected motor
  startMotor(currentMotor);
  motorStartTime = millis();
  motorRunning = true;
  motorInterval = 20000;  // Motor runs for 20 seconds
  motorStopTime = motorStartTime + motorInterval;
}

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

  // Pin Mode Setup
  pinMode(BUTTON1_PIN, INPUT_PULLUP);
  pinMode(BUTTON2_PIN, INPUT_PULLUP);
  pinMode(MOTOR1_IN1_PIN, OUTPUT);
  pinMode(MOTOR1_IN2_PIN, OUTPUT);
  pinMode(MOTOR1_EN_PIN, OUTPUT);

  pinMode(MOTOR2_IN1_PIN, OUTPUT);
  pinMode(MOTOR2_IN2_PIN, OUTPUT);
  pinMode(MOTOR2_EN_PIN, OUTPUT);

  // Attach interrupts for buttons
  attachInterrupt(digitalPinToInterrupt(BUTTON1_PIN), button1_ISR, FALLING);
  attachInterrupt(digitalPinToInterrupt(BUTTON2_PIN), button2_ISR, FALLING);
  
  // Initially, stop motors
  stopMotor(MOTOR_1);
  stopMotor(MOTOR_2);
  
  // Start Program A by default
  runProgramA();
}

void loop() {
  unsigned long currentMillis = millis();

  if (motorRunning) {
    // Check if it's time to stop the motor based on the program
    if (currentMillis - motorStartTime >= motorInterval) {
      // Stop the selected motor
      stopMotor(currentMotor);

      motorRunning = false;
      motorStopTime = currentMillis + stopInterval;  // Wait for 10 seconds before restart
    }
  }

  // Handle stop interval and repeat pattern
  if (!motorRunning && currentMillis - motorStopTime >= stopInterval) {
    if (currentProgram == PROGRAM_A) {
      runProgramA();  // Restart Program A after stop interval
    } else if (currentProgram == PROGRAM_B) {
      runProgramB();  // Restart Program B after stop interval
    }
  }
}