#include <Adafruit_GFX.h>
#include <Adafruit_NeoMatrix.h>
#include <Adafruit_NeoPixel.h>
#include <Servo.h>

#define PIN 6 // Pin de datos para la pantalla de LEDs

const int trigPin = 10;
const int echoPin = 11;
const int servoPin = 12;

long duration;
int distance;
Servo myServo;

Adafruit_NeoMatrix matrix = Adafruit_NeoMatrix(16, 16, PIN,
  NEO_MATRIX_TOP + NEO_MATRIX_LEFT +
  NEO_MATRIX_COLUMNS + NEO_MATRIX_ZIGZAG,
  NEO_GRB + NEO_KHZ800);

void setup() {
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  Serial.begin(9600);
  myServo.attach(servoPin);
  matrix.begin();
  matrix.setTextWrap(false);
  matrix.setBrightness(255);
  matrix.fillScreen(0);
  matrix.show();
}

void loop() {
  // Rota el servomotor de 15 a 165 grados
  for (int angle = 15; angle <= 165; angle++) {
    myServo.write(angle);
    delay(30);
    distance = calculateDistance();
    drawObject(angle, distance);
    Serial.print(angle);
    Serial.print(",");
    Serial.print(distance);
    Serial.print(".");
  }
  // Rota el servomotor de 165 a 15 grados
  for (int angle = 165; angle > 15; angle--) {
    myServo.write(angle);
    delay(30);
    distance = calculateDistance();
    drawObject(angle, distance);
    Serial.print(angle);
    Serial.print(",");
    Serial.print(distance);
    Serial.print(".");
  }
}

// Función para calcular la distancia medida por el sensor de ultrasonido
int calculateDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH);
  int distance = duration * 0.034 / 2;
  return distance;
}

// Función para dibujar el objeto en la pantalla de LEDs
void drawObject(int angle, int distance) {
  matrix.fillScreen(0);

  int x = map(angle, 15, 165, 0, 15); // Mapea el ángulo al ancho de la pantalla (0-15)
  int y = map(distance, 0, 200, 15, 0); // Mapea la distancia al alto de la pantalla (0-15)

  if (distance <= 200) {
    matrix.drawPixel(x, y, matrix.Color(255, 0, 0)); // Dibuja el objeto en la pantalla
  }

  matrix.show();
  delay(100);
}