/*
 * Robô de Futebol PRO — ESP32 + controle PS4 (Bluetooth) + driver TB6612
 * Tração diferencial estilo tanque: joystick direito (frente/trás) e
 * joystick esquerdo (giros). PWM por hardware do ESP32 (LEDC).
 * (código original; comentários em português)
 *
 * Pareamento do controle PS4: o ESP32 anuncia um endereço Bluetooth (MAC).
 * Troque a MAC abaixo pela do seu ESP32 e pareie o controle PS4 a essa MAC
 * com a ferramenta SixaxisPairTool (incluída na pasta do projeto).
 */
#include <PS4Controller.h>   // Biblioteca para usar o controle PS4 com o ESP32

// -------------------------
// Pinos do driver de motores (TB6612)
// -------------------------
#define PWMA 4     // PWM do motor A
#define PWMB 5     // PWM do motor B
#define AIN1 18    // Direção do motor A
#define AIN2 19
#define BIN1 21    // Direção do motor B
#define BIN2 22

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

  // Pinos de direção como saída
  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);
  pinMode(BIN1, OUTPUT);
  pinMode(BIN2, OUTPUT);

  // PWM por hardware do ESP32 (LEDC): canal 0 = motor A, canal 1 = motor B
  ledcAttachPin(PWMA, 0);
  ledcAttachPin(PWMB, 1);
  ledcSetup(0, 1000, 8);   // canal 0: 1 kHz, 8 bits
  ledcSetup(1, 1000, 8);   // canal 1: 1 kHz, 8 bits

  // Conexão com o controle PS4 via Bluetooth (troque pela MAC do seu ESP32)
  PS4.begin("CC:DB:A7:3F:97:36");
  Serial.println("Esperando conexao com o controle PS4...");
}

void loop() {
  // Só executa o controle se o controle estiver conectado
  if (PS4.isConnected()) {

    // Joystick direito (RStickY) -> frente / trás
    int ry = PS4.RStickY();
    // Joystick esquerdo (LStickX) -> giros
    int lx = PS4.LStickX();

    // Mapear a sensibilidade dos joysticks para velocidade PWM
    int speed_forward = map(abs(ry), 0, 127, 0, 255);
    int speed_turn    = map(abs(lx), 0, 127, 0, 255);

    // Frente / trás (com zona morta para evitar movimento falso)
    if (abs(ry) > 30) {
      if (ry < 0) { motorA_forward(speed_forward); motorB_forward(speed_forward); }
      else        { motorA_backward(speed_forward); motorB_backward(speed_forward); }
    }
    // Giro estilo tanque (com o joystick esquerdo)
    else if (abs(lx) > 30) {
      if (lx > 0) { motorA_forward(speed_turn); motorB_backward(speed_turn); }   // direita
      else        { motorA_backward(speed_turn); motorB_forward(speed_turn); }   // esquerda
    }
    // Sem movimento: parar
    else {
      motors_stop();
    }
  }
}

// ===== Funções de controle dos motores (TB6612) =====
void motorA_forward(int speed)  { digitalWrite(AIN1, HIGH); digitalWrite(AIN2, LOW);  ledcWrite(0, speed); }
void motorA_backward(int speed) { digitalWrite(AIN1, LOW);  digitalWrite(AIN2, HIGH); ledcWrite(0, speed); }
void motorB_forward(int speed)  { digitalWrite(BIN1, HIGH); digitalWrite(BIN2, LOW);  ledcWrite(1, speed); }
void motorB_backward(int speed) { digitalWrite(BIN1, LOW);  digitalWrite(BIN2, HIGH); ledcWrite(1, speed); }
void motors_stop() {
  ledcWrite(0, 0); ledcWrite(1, 0);
  digitalWrite(AIN1, LOW); digitalWrite(AIN2, LOW);
  digitalWrite(BIN1, LOW); digitalWrite(BIN2, LOW);
}