The TDOA logic in this code doesn't seem right. The microphone shown as the closest is weird: import numpy as np
import sounddevice as sd
import cv2
from scipy.signal import correlate
import matplotlib.pyplot as plt
from matplotlib.colors import ListedColormap

# Configuration parameters
SAMPLE_RATE = 10000  # Sampling rate in Hertz
BUFFER_DURATION = 0.1  # Duration of the audio buffer in seconds
BUFFER_SIZE = int(SAMPLE_RATE * BUFFER_DURATION)
SOUND_SPEED = 343.2  # Speed of sound in m/s
MIC_DISTANCE = 1  # Distance between the two mics in meters (adjust accordingly)

# Get list of input devices
devices = sd.query_devices()
input_devices = [dev for dev in devices if dev['max_input_channels'] > 0]

print("Available input devices:")
for i, dev in enumerate(input_devices):
    print(f"{i}: {dev['name']}")

# Select the two microphones
mic_indices = []
for i in range(2):
    device_index = int(input(f"Select microphone {i+1}: "))
    mic_indices.append(input_devices[device_index]['index'])

# Prepare buffers for each microphone
audio_buffers = [[] for _ in mic_indices]

# Function to create individual callbacks for each microphone
def create_callback(mic_index):
    def callback(indata, frames, time, status):
        if status:
            print(status)
        # Append the data to the corresponding buffer
        audio_buffers[mic_index].append(indata.copy())
    return callback

# Open audio streams for both microphones
streams = []
for i, mic_index in enumerate(mic_indices):
    stream = sd.InputStream(device=mic_indices[i], channels=1, samplerate=SAMPLE_RATE,
                            blocksize=BUFFER_SIZE, callback=create_callback(i))
    streams.append(stream)

# Start the audio streams
for stream in streams:
    stream.start()

# Initialize video capture
cap = cv2.VideoCapture(0)

# Heatmap setup
cmap = plt.get_cmap('jet')
heatmap_colors = (cmap(np.arange(cmap.N))[:, :3] * 255).astype(np.uint8)
heatmap_cmap = ListedColormap(cmap(np.linspace(0, 1, 256)))

try:
    while True:
        # Ensure both buffers have data
        if any(len(buf) == 0 for buf in audio_buffers):
            continue

        # Get and remove the oldest audio buffers
        audio_data_left = audio_buffers[0].pop(0)
        audio_data_right = audio_buffers[1].pop(0)

        # Extract audio samples
        audio_left = audio_data_left[:, 0]
        audio_right = audio_data_right[:, 0]

        # Compute cross-correlation
        corr = correlate(audio_left, audio_right, mode='full')
        lag = np.argmax(np.abs(corr)) - (len(audio_left) - 1)
        tdoa = lag / SAMPLE_RATE  # Time difference in seconds

        # Estimate angle of arrival
        max_tdoa = MIC_DISTANCE / SOUND_SPEED
        if abs(tdoa) > abs(max_tdoa):
            angle = 0  # Out of bounds
        else:
            angle = np.arcsin(tdoa / max_tdoa)  # Angle in radians

        # Convert angle to degrees
        angle_deg = np.degrees(angle)

        # Determine which microphone is closer
        if tdoa > 0:
            closer_mic = "Mic 1"
        elif tdoa < 0:
            closer_mic = "Mic 2"
        else:
            closer_mic = "Equidistant"

        # Read frame from camera
        ret, frame = cap.read()
        if not ret:
            print("Failed to grab frame")
            break

        # Generate heatmap overlay
        height, width = frame.shape[:2]
        heatmap = np.zeros((height, width), dtype=np.float32)

        # Map angle to position on the frame
        x_pos = int((angle_deg + 90) / 180 * width)
        x_pos = max(0, min(width - 1, x_pos))  # Ensure x_pos is within frame bounds
        cv2.line(heatmap, (x_pos, 0), (x_pos, height), (1,), thickness=5)

        # Apply colormap to heatmap
        heatmap_color = cv2.applyColorMap((heatmap * 255).astype(np.uint8), cv2.COLORMAP_JET)

        # Blend heatmap with the frame
        overlay = cv2.addWeighted(frame, 0.7, heatmap_color, 0.3, 0)

        # Add text indicator for closer microphone
        font = cv2.FONT_HERSHEY_SIMPLEX
        text = f"Closer to: {closer_mic}"
        cv2.putText(overlay, text, (10, height - 10), font, 1, (0, 255, 0), 2, cv2.LINE_AA)

        # Display the result
        cv2.imshow('Audio Heatmap Overlay', overlay)

        # Break on 'q' key press
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break

except KeyboardInterrupt:
    print('Interrupted by user')

finally:
    # Stop and close audio streams
    for stream in streams:
        stream.stop()
        stream.close()
    cap.release()
    cv2.destroyAllWindows()