#include <Adafruit_NeoPixel.h>

// ---------------- Pines ----------------
#define AI1 11    ///< Pin de control 1 del motor A
#define AI2 3    ///< Pin de control 2 del motor A
#define BI1 5    ///< Pin de control 1 del motor B
#define BI2 6     ///< Pin de control 2 del motor B
#define SW1 4    ///< Botón de prueba de motores
#define SW2 2    ///< Botón de prueba de LEDs
#define LED_PIN 13 ///< Pin de salida hacia la tira NeoPixel
#define NUM_LEDS 2 ///< Cantidad de LEDs NeoPixel

// ---------------- NeoPixel ----------------
Adafruit_NeoPixel leds(NUM_LEDS, LED_PIN, NEO_GRB + NEO_KHZ800);


void motorControl(uint8_t pin1, uint8_t pin2, int vel);
void motorA(int vel);
void motorB(int vel);
void motors(int velB, int velA);
void stopMotors();
void setLed(uint8_t index, uint8_t r, uint8_t g, uint8_t b);
void testMotorSequence();
void blinkLeds();
void handleSw1();
void handleSw2();

// ---------------- Función motores ----------------
void motorControl(uint8_t pin1, uint8_t pin2, int vel) {
  vel = constrain(vel, -255, 255);
  if (vel > 0) {
    analogWrite(pin1, vel);
    analogWrite(pin2, 0);
  } else if (vel < 0) {
    analogWrite(pin1, 0);
    analogWrite(pin2, -vel);
  } else {
    analogWrite(pin1, 0);
    analogWrite(pin2, 0);
  }
}

inline void motorA(int vel) { motorControl(AI1, AI2, vel); }
inline void motorB(int vel) { motorControl(BI1, BI2, vel); }

void motors(int velB, int velA) {
  motorA(velA);
  motorB(velB);
}

void stopMotors() {
  analogWrite(AI1, 0);
  analogWrite(AI2, 0);
  analogWrite(BI1, 0);
  analogWrite(BI2, 0);
}

// ---------------- Funciones LED ----------------
void setLed(uint8_t index, uint8_t r, uint8_t g, uint8_t b) {
  leds.setPixelColor(index, leds.Color(r, g, b));
  leds.show();
}

void blinkLeds() {
  for (int i = 0; i < 3; i++) leds.setPixelColor(i, leds.Color(255, 255, 255));
  leds.show();
  delay(500);
  leds.clear();
  leds.show();
  delay(500);
}

// ---------------- Funciones de prueba ----------------
void testMotorSequence() {
  // Motores adelante
  motors(60, 60);
  setLed(0, 255, 0, 0);
  delay(1000);
  leds.clear();

  // Motores atrás
  motors(-60, -60);
  setLed(0, 0, 0, 255);
  delay(1000);
  leds.clear();

  // Motor derecho adelante
  motors(0, 60);
  setLed(1, 255, 0, 0);
  delay(1000);
  leds.clear();

  // Motor derecho atrás
  motors(0, -60);
  setLed(1, 0, 0, 255);
  delay(1000);
  leds.clear();

  // Motor izquierdo adelante
  motors(60, 0);
  setLed(2, 255, 0, 0);
  delay(1000);
  leds.clear();

  // Motor izquierdo atrás
  motors(-60, 0);
  setLed(2, 0, 0, 255);
  delay(1000);
  leds.clear();

  stopMotors();
  leds.show();
  delay(1000);
}

void handleSw1() {
  if (digitalRead(SW1) == LOW) {
    testMotorSequence();
  }
}

void handleSw2() {
  if (digitalRead(SW2) == LOW) {
    blinkLeds();
  }
}

// ---------------- Configuración ----------------
void setup() {
  leds.begin();
  leds.show();

  pinMode(SW1, INPUT_PULLUP);
  pinMode(SW2, INPUT_PULLUP);

  pinMode(AI1, OUTPUT);
  pinMode(AI2, OUTPUT);
  pinMode(BI1, OUTPUT);
  pinMode(BI2, OUTPUT);
}

// ---------------- Loop principal ----------------
void loop() {
  handleSw1();
  handleSw2();
}