#include <Arduino.h>

const int sampleWindow = 50; // Sample window width in mS (50 mS = 20Hz)
const int sensorPins[] = {A0, A1, A2, A3, A4, A5, A6, A7, A8, A9};
const int numSensors = sizeof(sensorPins) / sizeof(sensorPins[0]);
float peakToPeakValues[numSensors] = {0}; // Array to store the peak-to-peak values

// Constants for sensor array configuration
const float sensorSpacing = 0.103; // Distance between sensors in meters
const float angleSpacing = 7.0;    // Angle spacing between sensors in degrees
const float soundSpeed = 343.0;    // Speed of sound in air in m/s
const float distanceToSource = 1.15; // Distance from the sensor array to the sound source in meters

void setup() {
  Serial.begin(9600); // Serial comms for debugging
}

void loop() {
  int sensorWithMaxValue = 0;
  unsigned int maxPeakToPeak = 0;

  for (int i = 0; i < numSensors; i++) {
    unsigned long startMillis = millis(); // Start of sample window
    unsigned int signalMax = 0;
    unsigned int signalMin = 1024;

    // Collect data for 50 ms
    while (millis() - startMillis < sampleWindow) {
      unsigned int sample = analogRead(sensorPins[i]);
      if (sample < 1024) { // Ensure valid reading
        if (sample > signalMax) {
          signalMax = sample;
        }
        if (sample < signalMin) {
          signalMin = sample;
        }
      }
    }

    // Calculate peak-to-peak for current sensor
    peakToPeakValues[i] = signalMax - signalMin;

    // Check if this sensor has the max value so far
    if(peakToPeakValues[i] > maxPeakToPeak) {
      maxPeakToPeak = peakToPeakValues[i];
      sensorWithMaxValue = i;
    }
  }

  // Calculate the angle based on the sensor with the max value
  float angleFromBasePoint = (sensorWithMaxValue - (numSensors / 2)) * angleSpacing;
  
  // Adjust the angle based on the distance to the sound source
  float adjustedAngle = atan2(distanceToSource * sin(angleFromBasePoint * PI / 180.0), distanceToSource * cos(angleFromBasePoint * PI / 180.0)) * 180.0 / PI;

  // Print the estimated angle to the Serial Monitor
  Serial.print("Estimated angle of arrival: ");
  Serial.print(adjustedAngle);
  Serial.println(" degrees");

  // Delay before the next loop iteration
  delay(500);
}