/*
Two yellow LEDs on top represent motor1 dir1 & dir2.
Two green LEDs on buttom represent motor 2 dir1 & dir2.
Blue LED represent left motor.
Red LED represent right motor.
Push button used to change forward & backward direction of the vehicle.
Slide bars used to control the motors speed. The top slide bar is used to control BLUE LED (left motor)
The bottom slide bar is used to control RED LED (right motor)
*/

// Define constants for motor control pins and direction
#define motor_instruction_pin_num 3
#define num_of_motor 2
#define DIR1 0
#define DIR2 1
#define PWM 2

// Define push button pin for changing direction
#define sw1 4   //'0' for forward, '1' for backward

/*
Define pins for motor control
DIR1, DIR2, PWM pins connected from ESP32
*/
uint8_t motor_instruction_pin[num_of_motor][motor_instruction_pin_num] = {{23, 22, 33}, {21, 19, 25}};

// Define pins for slide bars
const uint8_t slidepin1 = 35; //control left wheel speed
const uint8_t slidepin2 = 32; //control right wheel speed

// Define PWM frequency and resolution
const int PWM_Freq = 1000;
const int PWM_RES = 12;

// Define boolean variable for motor direction
bool direction = true;  //true: forward, false: backward

// Define MOTOR class for motor control
class MOTOR {
  private:
    uint8_t pin[motor_instruction_pin_num]; // Pins to control motor
    uint8_t pwm_ch; // PWM channel to control motor's speed

  public:
    MOTOR(uint8_t p[motor_instruction_pin_num], uint8_t pwm_ch) {
      this -> pwm_ch = pwm_ch;

      // Initialize motor pins as outputs
      for (int i = 0; i < motor_instruction_pin_num - 1; i++) {
        this -> pin[i] = p[i];
        pinMode(pin[i], OUTPUT);
        Serial.begin(115200);
      }

      // Set up PWM for motor control
      ledcSetup(pwm_ch, PWM_Freq, PWM_RES);
      ledcAttachPin(p[PWM], pwm_ch);
    }

    // Method to rotate motor clockwise
    void rotate_cw(unsigned int DC) {
      digitalWrite(pin[DIR1], HIGH);
      digitalWrite(pin[DIR2], LOW);
      ledcWrite(pwm_ch, DC);      
    }

    // Method to rotate motor counter-clockwise
    void rotate_ccw(unsigned int DC) {
      digitalWrite(pin[DIR1], LOW);
      digitalWrite(pin[DIR2], HIGH);
      ledcWrite(pwm_ch, DC);        
    }    
};

// Define CAR class for controlling the left and right motors together
class CAR {
  private:
    MOTOR Left_Motor;
    MOTOR Right_Motor;

  public:
    // Constructor to initialize the left and right motors
    CAR(uint8_t pin[num_of_motor][motor_instruction_pin_num]): Left_Motor(pin[0], 0), Right_Motor(pin[1], 1) {}
    
    // Method to move the car forward with given left and right motor duty cycles
    void FORWARD(unsigned int LDC, unsigned int RDC) { 
      Left_Motor.rotate_ccw(LDC);
      Right_Motor.rotate_cw(RDC);
    }

    // Method to move the car backward with given left and right motor duty cycles
    void BACKWARD(unsigned int LDC, unsigned int RDC) {
      Left_Motor.rotate_cw(LDC);
      Right_Motor.rotate_ccw(RDC);         
    }
};

// Create instance of CAR class
CAR mycar(motor_instruction_pin);

// Interrupt Service Routine for push button to change direction
void IRAM_ATTR push_button_ISR(){
  direction = !direction;
}

void setup() {  
  pinMode(slidepin1, INPUT);    // set slidepin1 as input
  pinMode(slidepin2, INPUT);    // set slidepin2 as input
  digitalPinToInterrupt(sw1);   // configure digital pin to interrupt sw1
  attachInterrupt(sw1, push_button_ISR, RISING);    // attach ISR to sw1 pin, triggers on rising edge
}

void loop() {  
  if (direction == true) {  
    /*
    if direction is forward
    move car forward with left and right wheel speeds based on the analog 
    values read from slidepin1 and slidepin2
    */
    mycar.FORWARD(analogRead(slidepin1), analogRead(slidepin2));              
  } else {  
    /*
    if direction is backward
    move car backward with left and right wheel speeds based on the analog 
    values read from slidepin1 and slidepin2
    */
    mycar.BACKWARD(analogRead(slidepin1), analogRead(slidepin2));    
  }
}