#define CUSTOM_SETTINGS
#define INCLUDE_GAMEPAD_MODULE
#include <DabbleESP32.h>

// Motor A pins
int motor1Pin1 = 27;
int motor1Pin2 = 26;
int enable1Pin = 14;

// Motor B pins
int motor2Pin1 = 25;
int motor2Pin2 = 33;
int enable2Pin = 32;

// PWM properties
const int freq = 30000;
const int pwmChannelA = 0;
const int pwmChannelB = 1;
const int resolution = 8;
int dutyCycle = 200;   // 0–255 (speed control)

void setup() {
  Serial.begin(115200);
  Dabble.begin("BattleBotSci"); // Bluetooth device name

  // Motor pins
  pinMode(motor1Pin1, OUTPUT);
  pinMode(motor1Pin2, OUTPUT);
  pinMode(motor2Pin1, OUTPUT);
  pinMode(motor2Pin2, OUTPUT);

  // Configure PWM channels (new)
  ledcAttach(enable1Pin, freq, 8);
  ledcAttach(enable2Pin, freq, 8);

  // Configure PWM channels (old)
  //ledcSetup(pwmChannelA, freq, resolution);
  //ledcAttachPin(enable1Pin, pwmChannelA);
  //ledcSetup(pwmChannelB, freq, resolution);
  //ledcAttachPin(enable2Pin, pwmChannelB);
}

void loop() {
  if (GamePad.isUpPressed()) Serial.println("Up pressed");
  if (GamePad.isDownPressed()) Serial.println("Down pressed");
  if (GamePad.isLeftPressed()) Serial.println("Left pressed");
  if (GamePad.isRightPressed()) Serial.println("Right pressed");
  Dabble.processInput();

  if (GamePad.isUpPressed()) {
    // Forward
    digitalWrite(motor1Pin1, HIGH);
    digitalWrite(motor1Pin2, LOW);
    digitalWrite(motor2Pin1, HIGH);
    digitalWrite(motor2Pin2, LOW);

    // Change the channel to the pin:
    // ledcWriteTone(0, freq);
    //ledcWriteTone(BUZZ_L, freq);
    // ledcWriteTone(1, freq);
    //ledcWriteTone(BUZZ_R, freq);

    ledcWrite(enable1Pin, dutyCycle);
    ledcWrite(enable2Pin, dutyCycle);
  }
  else if (GamePad.isDownPressed()) {
    // Backward
    digitalWrite(motor1Pin1, LOW);
    digitalWrite(motor1Pin2, HIGH);
    digitalWrite(motor2Pin1, LOW);
    digitalWrite(motor2Pin2, HIGH);
    ledcWrite(enable1Pin, dutyCycle);
    ledcWrite(enable2Pin, dutyCycle);
  }
  else if (GamePad.isLeftPressed()) {
    // Turn left
    digitalWrite(motor1Pin1, LOW);
    digitalWrite(motor1Pin2, HIGH);
    digitalWrite(motor2Pin1, HIGH);
    digitalWrite(motor2Pin2, LOW);
    ledcWrite(enable1Pin, dutyCycle);
    ledcWrite(enable2Pin, dutyCycle);
  }
  else if (GamePad.isRightPressed()) {
    // Turn right
    digitalWrite(motor1Pin1, HIGH);
    digitalWrite(motor1Pin2, LOW);
    digitalWrite(motor2Pin1, LOW);
    digitalWrite(motor2Pin2, HIGH);
    ledcWrite(enable1Pin, dutyCycle);
    ledcWrite(enable2Pin, dutyCycle);
  }
  else {
    // Stop if no button pressed
    digitalWrite(motor1Pin1, LOW);
    digitalWrite(motor1Pin2, LOW);
    digitalWrite(motor2Pin1, LOW);
    digitalWrite(motor2Pin2, LOW);
    ledcWrite(enable1Pin, 0);
    ledcWrite(enable2Pin, 0);
  }
}