import sounddevice as sd
import numpy as np
import serial
import time
from scipy.fftpack import fft

# Serial port configuration
ser = serial.Serial('COM11', 9600, timeout=1)  # Adjust COM port
time.sleep(2)

# Audio settings
SAMPLE_RATE = 44100
DURATION = 0.02  # seconds per analysis frame

# Movement thresholds
VOLUME_THRESHOLD = 0.1 
FREQ_THRESHOLD = 1000 

FORWARD = False
LEFT = False

def get_dominant_freq(audio_data):
    fft_data = np.abs(fft(audio_data))
    freqs = np.fft.fftfreq(len(fft_data), 1/SAMPLE_RATE)
    pos_mask = freqs > 0
    fft_data = fft_data[pos_mask]
    freqs = freqs[pos_mask]
    dominant_freq = freqs[np.argmax(fft_data)]
    return dominant_freq

def decide_action(volume, freq):
    global FORWARD
    global LEFT
    if volume < VOLUME_THRESHOLD:
        FORWARD = not FORWARD
        LEFT = not LEFT
        return 's'  # stop
    else:
        if freq < FREQ_THRESHOLD:
            if LEFT:
                return 'l'
            else:
                return 'r'  
        else:
            if FORWARD:
                return 'f'  
            else:
                return 'b'

def callback(indata, frames, time_info, status):
    audio_data = indata[:, 0]
    volume = np.linalg.norm(audio_data)
    freq = get_dominant_freq(audio_data)

    action = decide_action(volume, freq)
    ser.write(action.encode())
    print(f"Volume: {volume:.3f}, Freq: {freq:.1f} Hz -> Action: {action}")

# Start streaming and analyzing
with sd.InputStream(callback=callback, channels=1, samplerate=SAMPLE_RATE, blocksize=int(SAMPLE_RATE * DURATION)):
    print("Dancing to music... Press Ctrl+C to stop.")
    while True:
        pass