/*
 * Robô de Futebol — controle sem fio PS2 + driver TB6612 (Arduino UNO)
 * Lê os joysticks do controle PS2 e move 2 motores com direção diferencial
 * (frente, trás e giro no lugar). (código original; comentários em português)
 *
 * Driver TB6612: os pinos abaixo controlam AIN1/AIN2/PWMA (motor A) e
 * BIN1/BIN2/PWMB (motor B). O pino STBY do TB6612 deve ficar em nível ALTO
 * (ligado no 5V) para o driver funcionar. Os nomes IN/EN abaixo são herdados
 * do código original e mapeiam direto nesses terminais.
 */
#include <PS2X_lib.h>

PS2X ps2x;
int error = 0;
byte vibrate = 0;

// Pinos do driver TB6612  (IN1/IN2->AIN1/AIN2, ENA->PWMA, IN3/IN4->BIN1/BIN2, ENB->PWMB)
const int IN1 = 4;
const int IN2 = 3;
const int IN3 = 8;
const int IN4 = 7;
const int ENA = 9;
const int ENB = 5;

void controlMotores(int velocidadMotorA, int velocidadMotorB);

void setup() {
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  pinMode(ENA, OUTPUT);
  pinMode(ENB, OUTPUT);

  delay(300);
  // config_gamepad(clock A3, command A1, attention A0, data A2)
  error = ps2x.config_gamepad(A3, A1, A0, A2, true, true);
}

void loop() {
  ps2x.read_gamepad(false, vibrate);

  // Ler o joystick direito (frente / trás)
  int joyY = ps2x.Analog(PSS_RY) - 127;
  if (joyY == -128) joyY = -127;

  // Ler o joystick esquerdo (giros)
  int joyX = ps2x.Analog(PSS_LX) - 127;
  if (joyX == -128) joyX = -127;

  // Ler os botões de direção (setas)
  bool flechaIzq = ps2x.Button(PSB_PAD_LEFT);
  bool flechaDer = ps2x.Button(PSB_PAD_RIGHT);

  // Zona morta
  if (abs(joyY) < 15) joyY = 0;
  if (abs(joyX) < 15) joyX = 0;

  // Velocidade base a partir do joystick direito
  int velocidadBase = map(abs(joyY), 0, 127, 0, 255);

  // Giro proporcional a partir do joystick esquerdo
  int giro = map(joyX, -127, 127, -255, 255);

  // Prioridade: se apertar as setas, substitui o giro
  if (flechaIzq) giro = -200;
  else if (flechaDer) giro = 200;

  // Inicializar as velocidades
  int velocidadMotorA = 0;
  int velocidadMotorB = 0;

  if (joyY < 0) {
    // Avançar
    velocidadMotorA = velocidadBase - giro;
    velocidadMotorB = velocidadBase + giro;
  } else if (joyY > 0) {
    // Recuar
    velocidadMotorA = -velocidadBase + giro;
    velocidadMotorB = -velocidadBase - giro;
  } else {
    // Parado: permite girar no lugar
    velocidadMotorA = giro;
    velocidadMotorB = -giro;
  }

  // Limitar os valores
  velocidadMotorA = constrain(velocidadMotorA, -255, 255);
  velocidadMotorB = constrain(velocidadMotorB, -255, 255);

  controlMotores(velocidadMotorA, velocidadMotorB);

  delay(30);
}

void controlMotores(int velocidadMotorA, int velocidadMotorB) {
  // Motor A
  if (velocidadMotorA > 0) {
    digitalWrite(IN1, HIGH);
    digitalWrite(IN2, LOW);
  } else if (velocidadMotorA < 0) {
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, HIGH);
  } else {
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, LOW);
  }

  // Motor B
  if (velocidadMotorB > 0) {
    digitalWrite(IN3, LOW);
    digitalWrite(IN4, HIGH);
  } else if (velocidadMotorB < 0) {
    digitalWrite(IN3, HIGH);
    digitalWrite(IN4, LOW);
  } else {
    digitalWrite(IN3, LOW);
    digitalWrite(IN4, LOW);
  }

  // Velocidade
  analogWrite(ENA, abs(velocidadMotorA));
  analogWrite(ENB, abs(velocidadMotorB));
}