#include "Arduino.h"

// Definir pines
const int servoPin1 = 18;  // Servo 1
const int servoPin2 = 19;  // Servo 2

const int potPin1 = 34;  // Potenciómetro 1
const int potPin2 = 35;  // Potenciómetro 2

const int motorPin1 = 32;  // Pin para el motor (H-Bridge A)
const int motorPin2 = 33;  // Pin para el motor (H-Bridge B)
const int buttonPin = 25;  // Pin del botón para controlar la dirección del motor (cambia el pin del botón)

// Dirección del motor (false = adelante, true = atrás)
bool motorDirection = false;

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

  // Configurar pines de servos, motor y botón
  pinMode(servoPin1, OUTPUT);
  pinMode(servoPin2, OUTPUT);
  pinMode(motorPin1, OUTPUT);
  pinMode(motorPin2, OUTPUT);
  pinMode(buttonPin, INPUT_PULLUP);  // Configurar el botón con resistencia pull-up interna
}

void loop() {
  // Leer valores de los potenciómetros
  int potValue1 = analogRead(potPin1);
  int potValue2 = analogRead(potPin2);

  // Definir el valor central del joystick y el umbral para considerar apagado
  int middleValue = 2047;  // Valor en el centro de 0-4095
  int threshold = 500;      // Rango alrededor del centro considerado como apagado

  // Convertir valores de los potenciómetros a ángulos (0° a 180°)
  int angle1 = map(potValue1, 0, 4095, 0, 180);
  int angle2 = map(potValue2, 0, 4095, 0, 180);

  // Definir la señal PWM para el servo 1
  int pulseWidth1 = 0;
  if (potValue1 < (middleValue - threshold) || potValue1 > (middleValue + threshold)) {
    // Si el potenciómetro está fuera del rango central, encender el servo
    pulseWidth1 = map(angle1, 0, 180, 500, 2400);
  } else {
    // Si está en el centro, apagar el servo
    pulseWidth1 = 0;
  }

  // Definir la señal PWM para el servo 2
  int pulseWidth2 = 0;
  if (potValue2 < (middleValue - threshold) || potValue2 > (middleValue + threshold)) {
    // Si el potenciómetro está fuera del rango central, encender el servo
    pulseWidth2 = map(angle2, 0, 180, 500, 2400);
  } else {
    // Si está en el centro, apagar el servo
    pulseWidth2 = 0;
  }

  // Enviar señal PWM manualmente a los servos
  moveServo(servoPin1, pulseWidth1);
  moveServo(servoPin2, pulseWidth2);

  // Leer el estado del botón
  static bool lastButtonState = HIGH;
  bool currentButtonState = digitalRead(buttonPin);

  // Detectar el cambio de estado del botón
  if (lastButtonState == HIGH && currentButtonState == LOW) {
    motorDirection = !motorDirection;  // Cambiar dirección del motor
    delay(200);  // Debounce del botón (evitar múltiples lecturas por un solo toque)
  }
  lastButtonState = currentButtonState;

  // Controlar la dirección del motor y velocidad
  int motorSpeed = map(potValue1, 0, 4095, 0, 255);  // Ajustar velocidad según potenciómetro 1 (0 a 255)

  if (motorDirection) {
    // Motor en dirección contraria
    analogWrite(motorPin1, 0);
    analogWrite(motorPin2, motorSpeed);
  } else {
    // Motor en dirección hacia adelante
    analogWrite(motorPin1, motorSpeed);
    analogWrite(motorPin2, 0);
  }

  delay(20);
}

// Generar señal PWM manualmente para los servos
void moveServo(int pin, int pulseWidth) {
  digitalWrite(pin, HIGH);
  delayMicroseconds(pulseWidth);
  digitalWrite(pin, LOW);
  delayMicroseconds(20000 - pulseWidth); // Periodo de 20ms
}