#include <Arduino.h>
#include "constants.h"
#include "ultrasonic.h"
#include "buzzer.h"
#include "utils.h"

Ultrasonic ultrasonic;
Buzzer buzzer;

void setup() {
  Serial.begin(9600);
  ultrasonic.begin();
  buzzer.begin();
}

void loop() {
  // Read the distances from the sensors
  float distance_left = ultrasonic.readDistance(ULTRASONIC_LEFT_TRIG_PIN, ULTRASONIC_LEFT_ECHO_PIN);
  float distance_center = ultrasonic.readDistance(ULTRASONIC_CENTER_TRIG_PIN, ULTRASONIC_CENTER_ECHO_PIN);
  float distance_right = ultrasonic.readDistance(ULTRASONIC_RIGHT_TRIG_PIN, ULTRASONIC_RIGHT_ECHO_PIN);

  // Find the closest obstacle among the three sensors
  int closest_sensor = findClosestSensor(distance_left, distance_center, distance_right);

  // Play the scan beat
  buzzer.playScanBeat();

  // Check if an obstacle was detected
  if (closest_sensor != -1 && distance_left < MAX_DISTANCE && distance_center < MAX_DISTANCE && distance_right < MAX_DISTANCE) {
    // Calculate the position of the obstacle within the FOV
    float position = mapValue(closest_sensor, 0, 2, -FOV_ANGLE / 2, FOV_ANGLE / 2);
    
    // Play the obstacle beat with a tone based on the distance
    float distance = (closest_sensor == 0) ? distance_left : ((closest_sensor == 1) ? distance_center : distance_right);
    int tone = mapValue(distance, 0, MAX_DISTANCE, 0, 3);
    buzzer.playObstacleBeat(tone, position);
  }

  // Wait for the next scan
  delay(SCAN_DELAY);
}