The heatmap should be a line on the camera: import sounddevice as sd
import numpy as np
import cv2
import time
from scipy.signal import correlate
from queue import Queue
import threading
import matplotlib.pyplot as plt

# Parameters
SAMPLE_RATE = 44100  # Sampling rate in Hz
BUFFER_DURATION = 0.05  # Duration of each audio buffer in seconds (e.g., 50 ms)
CHANNELS = [1]  # We'll use one channel per device
DEVICE_IDS = [1,2]  # Replace with actual device IDs

# Define microphone positions (in meters)
# For example, microphones are 0.5 meters apart along x-axis
mic_positions = [(0.0, 0.0), (0.5, 0.0)]

# Initialize queues for audio data
audio_queue1 = Queue()
audio_queue2 = Queue()

# Initialize the queue to communicate position data
position_queue = Queue()

# Initialize video capture
video_capture = cv2.VideoCapture(0)  # Adjust the index if multiple cameras

if not video_capture.isOpened():
    print("Error: Could not open video device")
    exit(1)

def create_heatmap(frame, position, delta_d, mic_positions):
    # Create an empty heatmap
    heatmap = np.zeros((frame.shape[0], frame.shape[1]), dtype=np.float32)

    # Map the estimated position to pixel coordinates
    # Assuming the frame represents the area between microphones
    x_pixels = np.interp(position[0], [mic_positions[0][0], mic_positions[1][0]], [0, frame.shape[1]])
    y_pixels = frame.shape[0] / 2  # For simplicity, assuming y is centered

    # Draw a circle representing the source position
    cv2.circle(heatmap, (int(x_pixels), int(y_pixels)), radius=50, color=1.0, thickness=-1)

    # Normalize heatmap to range [0, 255]
    heatmap = cv2.normalize(heatmap, None, alpha=0, beta=255, norm_type=cv2.NORM_MINMAX)
    heatmap = heatmap.astype(np.uint8)

    # Apply a color map
    heatmap_color = cv2.applyColorMap(heatmap, cv2.COLORMAP_JET)

    # Overlay the heatmap onto the frame
    overlay = cv2.addWeighted(frame, 0.7, heatmap_color, 0.3, 0)
    return overlay

def calculate_tdoa(signal_a, signal_b, sample_rate):
    corr = correlate(signal_a, signal_b, 'full')
    lags = np.arange(-len(signal_a) + 1, len(signal_a))
    lag = lags[np.argmax(np.abs(corr))]
    tdoa = lag / sample_rate

    return tdoa

def estimate_position(tdoa, mic_positions, sound_speed=343):
    x1, y1 = mic_positions[0]
    x2, y2 = mic_positions[1]

    # Distance difference between microphones
    delta_d = tdoa * sound_speed

    # Distance between the two microphones
    mic_dist = np.hypot(x2 - x1, y2 - y1)

    # Check if delta_d is within valid range
    if abs(delta_d) > mic_dist:
        print("Warning: Calculated delta_d exceeds the distance between microphones.")
        delta_d = np.sign(delta_d) * mic_dist  # Limit to mic distance

    # Calculate angle of arrival
    try:
        theta = np.arcsin(delta_d / mic_dist)
    except ValueError:
        # Handle numerical errors due to slight overflows
        theta = np.pi / 2 if delta_d >= 0 else -np.pi / 2

    # Unit vector along the line connecting the microphones
    mic_vector = np.array([x2 - x1, y2 - y1]) / mic_dist

    # Rotate the mic_vector by theta to get the direction to the source
    rotation_matrix = np.array([
        [np.cos(theta), -np.sin(theta)],
        [np.sin(theta),  np.cos(theta)]
    ])
    source_direction = rotation_matrix @ mic_vector

    # Midpoint between the two microphones
    mid_point = np.array([(x1 + x2) / 2, (y1 + y2) / 2])

    # Estimate source position at a certain distance 'L' from the midpoint
    # You may adjust 'L' based on context or additional information
    L = 1  # Example distance
    estimated_position = mid_point + L * source_direction

    return estimated_position, delta_d

def audio_callback(indata, frames, time_info, status, audio_buffer):
    if status:
        print(f"Status: {status}")
    audio_buffer.append(indata.copy())

def audio_stream_thread(device_id, audio_queue):
    audio_buffer = []
    event = threading.Event()

    def callback(indata, frames, time_info, status):
        audio_callback(indata, frames, time_info, status, audio_buffer)
        event.set()

    with sd.InputStream(samplerate=SAMPLE_RATE,
                        device=device_id,
                        channels=1,
                        dtype='float32',
                        callback=callback,
                        blocksize=int(BUFFER_DURATION * SAMPLE_RATE)):
        while True:
            event.wait()  # Wait for audio callback to set the event
            event.clear()
            if len(audio_buffer) > 0:
                data = audio_buffer.pop(0)
                audio_queue.put(data)
            # Check for exit condition (e.g., a signal to stop the thread)
            if stop_event.is_set():
                break

def process_audio():
    buffer1 = np.array([], dtype=np.float32)
    buffer2 = np.array([], dtype=np.float32)

    while not stop_event.is_set():
        # Collect audio data from both queues
        try:
            data1 = audio_queue1.get(timeout=0.1)
            buffer1 = np.concatenate((buffer1, data1.flatten()))
        except:
            pass
        try:
            data2 = audio_queue2.get(timeout=0.1)
            buffer2 = np.concatenate((buffer2, data2.flatten()))
        except:
            pass

        # Process audio if enough data is accumulated
        min_length = min(len(buffer1), len(buffer2))
        if min_length >= int(BUFFER_DURATION * SAMPLE_RATE):
            # Extract synchronized chunks
            chunk_size = int(BUFFER_DURATION * SAMPLE_RATE)
            signal1 = buffer1[:chunk_size]
            signal2 = buffer2[:chunk_size]
            print(signal1,signal2)

            # Remove the processed part from the buffers
            buffer1 = buffer1[chunk_size:]
            buffer2 = buffer2[chunk_size:]

            # Process the audio signals
            tdoa = calculate_tdoa(signal1, signal2, SAMPLE_RATE)
            print(tdoa)

            # Estimate source position
            position, delta_d = estimate_position(tdoa, mic_positions)
            print(position, delta_d)

            # Put the position into the queue
            position_queue.put((position, delta_d))

def video_display():
    print("Real-time audio processing started. Press 'q' to quit.")
    while not stop_event.is_set():
        # Capture video frame
        ret, frame = video_capture.read()
        if not ret:
            print("Error: Could not read video frame")
            break

        # Check if there is a new position to display
        if not position_queue.empty():
            position, delta_d = position_queue.get()

            # Overlay heatmap on frame
            output_frame = create_heatmap(frame, position, delta_d, mic_positions)
        else:
            output_frame = frame

        # Display the frame
        cv2.imshow('Audio-Visual Synchronization', output_frame)

        # Check for 'q' key press to exit
        if cv2.waitKey(1) & 0xFF == ord('q'):
            stop_event.set()
            break

        # Sleep briefly to reduce CPU usage
        time.sleep(0.01)

    # Release resources
    video_capture.release()
    cv2.destroyAllWindows()
    print("Real-time audio processing stopped.")

# Event to signal threads to stop
stop_event = threading.Event()

# Threads for audio streams
thread1 = threading.Thread(target=audio_stream_thread, args=(DEVICE_IDS[0], audio_queue1))
thread2 = threading.Thread(target=audio_stream_thread, args=(DEVICE_IDS[1], audio_queue2))

# Thread for audio processing
processing_thread = threading.Thread(target=process_audio)

# Start threads
thread1.start()
thread2.start()
processing_thread.start()

# Start video display in the main thread
try:
    video_display()
except KeyboardInterrupt:
    stop_event.set()

# Wait for threads to finish
thread1.join()
thread2.join()
processing_thread.join()