#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <ESP32Servo.h>
#include <HCSR04.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
#define NUM_ARCS 4
#define TRIGGER_PIN 4 // Pin del trigger
#define ECHO_PIN 5 // Pin del echo

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
Servo radarServo;
UltraSonicDistanceSensor sensor(TRIGGER_PIN, ECHO_PIN);

int angle = 0; // Variable global para el ángulo

void setup() {
  Serial.begin(115200);
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.display();
  delay(2000);
  radarServo.attach(5); // Ajusta el pin del servo aquí
}

void loop() {
  int distancia = sensor.measureDistanceCm();

  display.clearDisplay();
  drawRadarArcs();
  moveRadar();
  drawRadar(angle);
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(10, 0);
  display.println("Distancia: " + String(distancia) + " cm");
  display.display();
}

void moveRadar() {
  angle++;
  angle %= 360;
  radarServo.write(angle);
  delay(10); // Ajusta el delay para la velocidad deseada
}

void drawRadar(int angle) {
  angle = constrain(angle, 0, 180);

  int startX = SCREEN_WIDTH / 2;
  int startY = SCREEN_HEIGHT;
  int endX = startX + cos(angle * PI / 180.0) * 64; // Longitud de la línea
  int endY = startY - sin(angle * PI / 180.0) * 64; // Longitud de la línea
  int startXX = SCREEN_WIDTH / 2;
  int startYY = SCREEN_HEIGHT - 1;
  int endXX = startXX + cos(angle * PI / 180.0) * 64;
  int endYY = startYY - sin(angle * PI / 180.0) * 64;
  int startXXX = SCREEN_WIDTH / 2;
  int startYYY = SCREEN_HEIGHT - 2;
  int endXXX = startXXX + cos(angle * PI / 180.0) * 64; // Longitud de la línea
  int endYYY = startYYY - sin(angle * PI / 180.0) * 64; // Longitud de la línea

  display.drawLine(startXX, startYY, endXX, endYY, SSD1306_WHITE);
  display.drawLine(startX, startY, endX, endY, SSD1306_WHITE);
  display.drawLine(startXXX, startYYY, endXXX, endYYY, SSD1306_WHITE);
}

// Función para dibujar un arco
void drawArc(int x, int y, int radius, float startAngle, float endAngle, uint16_t color) {
  float angleStep = 0.1; // Ajusta la precisión del arco
  float angle = startAngle;

  while (angle < endAngle) {
    float x1 = x + cos(angle * PI / 180.0) * radius;
    float y1 = y - sin(angle * PI / 180.0) * radius;
    display.drawPixel(x1, y1, color);
    angle += angleStep;
  }
}

// Función para dibujar los arcos del radar
void drawRadarArcs() {
  int centerX = SCREEN_WIDTH / 2;
  int centerY = SCREEN_HEIGHT;
  int radius = 64;

    // Dibuja un semiarco simulando con circulos
    display.drawCircle(64,64,64, SSD1306_WHITE);
    display.drawCircle(64,64,32, SSD1306_WHITE);
    display.drawCircle(64,64,16, SSD1306_WHITE);
    display.drawCircle(64,64,8, SSD1306_WHITE);
  }