MulticastVoiceApp-BCLO.py

"""
What this thingy contains and does:
- Separate Audio (5007) and Control (5008) channels
- Decentralized Arbitration: PTT requests are arbitrated to prevent collisions
- Busy Channel Lock Out: Prevents transmitting while channel is held
- Codec2 / Opus (Stub) support structure


TODO: Include Web UI ( maybe flask ) and use web browser audio and mic capabilities 

"""

import socket
import struct
import pycodec2 as codec2
import pyaudio
import numpy as np
import threading
import signal
import sys
import time
import random
import keyboard
import argparse
import soxr


# --- CONFIGURATION ---
MULTICAST_GROUP = '239.1.1.1'
PORT_AUDIO = 5007
PORT_CONTROL = 5008
MULTICAST_TTL = 2


CODEC2_SAMPLE_RATE = 8000  # Codec2 only works at 8kHz natively
# Audio Settings
SAMPLE_RATE =  48000 #8000
AUDIO_CHANNELS = 1
CODEC2_MODE = 3200
# Allowed codec2 modes (bps)
CODEC2_MODES = [3200, 2400, 1600, 1400, 1300, 1200, 700]

# Audio device defaults (None -> OS default)
DefaultInputDeviceIndex = 3
DefaultOutputDeviceIndex = None


# Identity
TALKER_ID = 420  # MY Talker ID
GROUP_ID = 10
PRIORITY = 1     # 1=Normal, 10=Emergency

TALKER_NAMES = {
    420: "Desktop",
    101: "Laptop",
    500: "Charlie",
}


# Timers (ms)
TIME_SLOT_MS = 20
REQ_WINDOW_MS = 80       # Time to wait for other requests
HEARTBEAT_MS = 20       # Heartbeat to know if still talking
HOLD_TIMEOUT_MS = 80    # If no heartbeat for X ms, assume channel free 4 bis 8 x hearthbeat time 
RANDOM_BACKOFF_MS = 30 # Max random backoff time for arbitration

# Control Message Types 
MSG_REQ = 1
MSG_GRANT = 2
MSG_RELEASE = 3
MSG_HEARTBEAT = 4

# Header Formats
# Audio: !BBHHH (Version, Group, TalkerID, Mode, Seq)
# Format: version (B), group (B), talker_id (H), codec2_mode (H), seq (H)
# Total size = 1 + 1 + 2 + 2 + 2 = 8 bytes
HEADER_FMT_AUDIO = '!BBHHH'
HEADER_SIZE_AUDIO = struct.calcsize(HEADER_FMT_AUDIO)
HEADER_VERSION_AUDIO = 1

# Control: !B H H I B (Type, TalkerID, Seq, Timestamp, Priority)
# I (unsigned int) is 4 bytes for timestamp
HEADER_FMT_CTRL = '!BHHIB' 
HEADER_SIZE_CTRL = struct.calcsize(HEADER_FMT_CTRL)


ptt = False


# --- FLOOOR CONTROL MANAGER ---
class FloorManager:
    """
    Handles the distributed state machine for PTT arbitration.
    States: IDLE, REQUESTING, TRANSMITTING, REMOTE_HOLD
    """
    IDLE = 0
    REQUESTING = 1
    TRANSMITTING = 2
    REMOTE_HOLD = 3

    def __init__(self, sock_tx, talker_id, group_id):
        self.sock_tx = sock_tx
        self.talker_id = talker_id
        self.group_id = group_id
        
        self.state = self.IDLE
        self.seq = 0
        
        # holds current Floor holder Info
        self.holder_id = None
        self.holder_expiry = 0
        
        # Request Arbitration
        self.req_ts = 0  # TImestamp when we started requesting
        self.req_priority = PRIORITY
        self.wins_arbitration = False

    def get_time_ms(self):
        return int(time.time() * 1000)

    def send_ctrl_msg(self, msg_type):
        """Packs and send a control message."""
        self.seq = (self.seq + 1) & 0xFFFF # wraparound seq to 16 bits
        ts = self.get_time_ms() & 0xFFFFFFFF #  wraparund to keep only 4 bytes
        
        # Packet: Type | TalkerID | Seq | Timestamp | Priority
        payload = struct.pack(HEADER_FMT_CTRL, msg_type, self.talker_id, self.seq, ts, PRIORITY)
        
        try:
            self.sock_tx.sendto(payload, (MULTICAST_GROUP, PORT_CONTROL))
        except Exception as e:
            print(f"[CTRL] Send Error: {e}")

    # --- PTT REQUEST ACTIONS ---
    def request_floor(self):
        """User pressed PTT Start arbitration."""
        if self.state == self.REMOTE_HOLD:
            # Simple BCLO check # Later Add Priotity Feature
            print("[BCLO] Channel Busy! Request Denied.")
            return False

        if self.state == self.IDLE:
            self.state = self.REQUESTING
            self.req_ts = self.get_time_ms()
            self.wins_arbitration = True # Assume we win until proven otherwise
            
            # Send Request
            self.send_ctrl_msg(MSG_REQ)
            
            # Start arbitration timer thread
            threading.Timer(REQ_WINDOW_MS / 1000.0, self._finalize_arbitration).start()
            return True
        return False

    def release_floor(self):
        """User released PTT."""
        if self.state == self.TRANSMITTING:
            self.send_ctrl_msg(MSG_RELEASE)
        
        self.state = self.IDLE
        self.holder_id = None
        print("\n[PTT] Released.")

    def _finalize_arbitration(self):
        """Called after REQ_WINDOW_MS to see if we won."""
        if self.state == self.REQUESTING:
            if self.wins_arbitration:
                self.state = self.TRANSMITTING
                self.send_ctrl_msg(MSG_GRANT)
                print("\n[PTT] Floor Granted! Transmitting...")
                # Start Heartbeat loop
                self._heartbeat_loop()
            else:
                self.state = self.IDLE
                print("\n[PTT] Arbitration Lost. Backing off.")

    def _heartbeat_loop(self):
        """Sends periodic heartbeats while transmitting."""
        if self.state == self.TRANSMITTING:
            self.send_ctrl_msg(MSG_HEARTBEAT)
            threading.Timer(HEARTBEAT_MS / 1000.0, self._heartbeat_loop).start()

    # --- PACKET PROCESSING ---
    def handle_packet(self, data):
        """Process incoming control packets."""
        if len(data) < HEADER_SIZE_CTRL: return # if len less than header size, ignore

        m_type, sender_id, seq, ts, prio = struct.unpack(HEADER_FMT_CTRL, data) #  unpack control header into variables
        
        if sender_id == self.talker_id: return # Ignore own echo 

        now = self.get_time_ms() # current time in ms

        # 1. HANDLE REQUESTS (Abitration) 
        if m_type == MSG_REQ:
            if self.state == self.TRANSMITTING:
                # I am talking, other requested to talk . 
                # IF PRIO is bigger than mine (> mine), YIELD and relese
                #todo: implement priority yielding
                pass 
            
            elif self.state == self.REQUESTING:
                # Collision scenario. Compare timestamps.
                # Lower timestamp wins. If tie, lower ID wins.
                opponent_wins = False
                if ts < self.req_ts: opponent_wins = True
                elif ts == self.req_ts and sender_id < self.talker_id: opponent_wins = True
                
                if opponent_wins:
                    self.wins_arbitration = False
                    print(f"[ARB] Lost race to {sender_id}")

        # 2. HANDLE GRANTS / HEARTBEATS (Remote Hold)
        elif m_type in [MSG_GRANT, MSG_HEARTBEAT]: # if message is GRANT or HEARTBEAT 
            self.holder_id = sender_id # set holder to sender id that sent the message
            self.holder_expiry = now + HOLD_TIMEOUT_MS  # set expiry time to now + timeout
            
            if self.state != self.TRANSMITTING:  # if we are not transmitting ourselves
                if self.state != self.REMOTE_HOLD: # and not already in remote hold
                    name = TALKER_NAMES.get(sender_id, sender_id) # get name or use id
                    print(f"\n[BUSY] Channel held by {name}") # print busy message
                self.state = self.REMOTE_HOLD # set state to remote hold

        # 3. HANDLE RELEASE
        elif m_type == MSG_RELEASE: # if message is RELEASE
            if self.holder_id == sender_id: # if the sender is the current holder
                self.state = self.IDLE # set state to IDLE
                self.holder_id = None # clear holder id
                print("\n[FREE] Channel Clear.") 

    def check_timeouts(self):
        """Called periodically to clear stale locks."""
        if self.state == self.REMOTE_HOLD: # if in remote hold state 
            if self.get_time_ms() > self.holder_expiry: # if current time is greater than holder expiry time aka no hearthbeat recived in time
                print("\n[FREE] Remote lock timed out.") 
                self.state = self.IDLE # set state to IDLE
                self.holder_id = None


# --- MAIN APP ---
class MulticastVoiceApp:
    def __init__(self,interface_ip=None):
        self.p = pyaudio.PyAudio()
        self.codec2Modes = CODEC2_MODE
        self.codec2 = codec2.Codec2(CODEC2_MODE)
        self.running = False 
        self.seq = 0
        self.lastSeqID = {}
        self.interface_ip = interface_ip

        #Audio Device Setup
        self.input_device_index = DefaultInputDeviceIndex
        self.output_device_index = DefaultOutputDeviceIndex
        
        # Sockets
        self.sock_audio_rx = None
        self.sock_audio_tx = None
        self.sock_ctrl_rx = None
        self.sock_ctrl_tx = None # socket for TX? 
        
        
        self.floor = None # hold FloorManager instance

    def setup_sockets(self):
        # 1. AUDIO RX ( Port 5007)
        self.sock_audio_rx = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        self.sock_audio_rx.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        try:
            self.sock_audio_rx.bind(('', PORT_AUDIO))
        except:
            self.sock_audio_rx.bind((MULTICAST_GROUP, PORT_AUDIO))


        #check if interface ip is given for multicast join
        if self.interface_ip:
            mreq = struct.pack('4s4s', socket.inet_aton(MULTICAST_GROUP), socket.inet_aton(self.interface_ip))
        else:
            mreq = struct.pack('4sl', socket.inet_aton(MULTICAST_GROUP), socket.INADDR_ANY)





        # Join Multicast Group
        #mreq = struct.pack('4sl', socket.inet_aton(MULTICAST_GROUP), socket.INADDR_ANY)
        #if self.interface_ip:
        #    mreq = struct.pack('4s4s', socket.inet_aton(MULTICAST_GROUP), socket.inet_aton(self.interface_ip))
        #else:
        #    mreq = struct.pack('4sl', socket.inet_aton(MULTICAST_GROUP), socket.INADDR_ANY)

        self.sock_audio_rx.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq)
        
        # 2. CONTROL RX (Port 5008)
        self.sock_ctrl_rx = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        self.sock_ctrl_rx.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        try:
            self.sock_ctrl_rx.bind(('', PORT_CONTROL))
        except:
            self.sock_ctrl_rx.bind((MULTICAST_GROUP, PORT_CONTROL))
        self.sock_ctrl_rx.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq)

        # 3. TX SOCKET (Shared for Audio/Ctrl send)
        self.sock_tx = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        self.sock_tx.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_TTL, MULTICAST_TTL)
        self.sock_tx.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_LOOP, 0) # Don't hear self

        # Initialize Floor Manager
        self.floor = FloorManager(self.sock_tx, TALKER_ID, GROUP_ID)

        #get info which ip the multicast joins on
        host = socket.gethostname()
        local_ip = socket.gethostbyname(host)
        print(f"[INFO] Multicast joined on local IP: {local_ip}")

        


    def _print_available_audio_devices(self):
        try:
            host_api = self.p.get_host_api_info_by_index(0)
            numdevices = host_api.get('deviceCount')
        except Exception:
            numdevices = self.p.get_device_count()

        print("[INFO] Available input devices:")
        for i in range(0, numdevices):
            try:
                info = self.p.get_device_info_by_host_api_device_index(0, i)
            except Exception:
                try:
                    info = self.p.get_device_info_by_index(i)
                except Exception:
                    continue
            if info.get('maxInputChannels', 0) > 0:
                print(f"  id {i}: {info.get('name')}")
        print("[INFO] Available output devices:")
        for i in range(0, numdevices):
            try:
                info = self.p.get_device_info_by_host_api_device_index(0, i)
            except Exception:
                try:
                    info = self.p.get_device_info_by_index(i)
                except Exception:
                    continue
            if info.get('maxOutputChannels', 0) > 0:
                print(f"  id {i}: {info.get('name')}")

    def setup_audio_devices(self):
        # If None, pick default
        if self.input_device_index is None:
            try:
                _InputDeviceIndex = self.p.get_default_input_device_info().get('index')
                self.InputDeviceIndex = _InputDeviceIndex
                print(f"[INFO] Using default input device id {self.InputDeviceIndex}",end=' ')
                print("--> SAMPLE RATE "+ str(self.p.get_device_info_by_index(self.InputDeviceIndex).get('defaultSampleRate')))
                
            except Exception as e:
                print(f"[WARN] Couldn't get default input device: {e}")
                self.InputDeviceIndex = None
        else:
            print(f"[INFO] Using input device id {self.input_device_index}",end=' ')
            print("--> SAMPLE RATE "+str(self.p.get_device_info_by_index(self.input_device_index).get('defaultSampleRate')))

        if self.output_device_index is None:
            try:
                _OutputDeviceIndex = self.p.get_default_output_device_info().get('index')
                self.OutputDeviceIndex = _OutputDeviceIndex
                print(f"[INFO] Using default output device id {self.OutputDeviceIndex}",end=' ')

                print("--> SAMPLE RATE "+ str(self.p.get_device_info_by_index(self.OutputDeviceIndex).get('defaultSampleRate')))

            except Exception as e:
                print(f"[WARN] Couldn't get default output device: {e}")
                self.OutputDeviceIndex = None        
        else:
            print(f"[INFO] Using output device id {self.output_device_index}",end=' ')
            print("--> SAMPLE RATE "+ str(self.p.get_device_info_by_index(self.output_device_index).get('defaultSampleRate')))


        #check sample rate compatibility

    def start(self):
        self.setup_sockets()
        self.running = True

        self._print_available_audio_devices()
        self.setup_audio_devices()
        
        threading.Thread(target=self.rx_audio_loop,name="rx audio thread", daemon=True).start()
        threading.Thread(target=self.rx_control_loop,name="rx Controll channel thread", daemon=True).start()
        threading.Thread(target=self.tx_loop,name="tx_thread", daemon=True).start()
        
        print(f"System Ready. Talker ID: {TALKER_ID}")

    def stop(self):
        self.running = False
        self.p.terminate()

    # --- THREAD: Control Receiver ---
    def rx_control_loop(self):
        while self.running:
            try:
                # Check for timeouts periodically
                self.floor.check_timeouts()
                
                # Non-blocking check or short timeout
                self.sock_ctrl_rx.settimeout(0.1)
                try:
                    data, _ = self.sock_ctrl_rx.recvfrom(1024) # Recive Control package . Max Size 1024 bytes . could shrink it more to save bandwidth later 
                    self.floor.handle_packet(data)
                except socket.timeout:
                    pass
            except Exception as e:
                print(f"[ERR CTRL] {e}")

    # --- THREAD: Audio Receiver ---
    def rx_audio_loop(self):
        samples_per_frame = self.codec2.samples_per_frame()
        out_frame= samples_per_frame * (SAMPLE_RATE // CODEC2_SAMPLE_RATE)

        rx_stream = self.p.open(format=pyaudio.paInt16, channels=AUDIO_CHANNELS, rate=SAMPLE_RATE,output_device_index=self.output_device_index, frames_per_buffer=out_frame,output=True)
        while self.running:
            try:
                try:
                    data, _ = self.sock_audio_rx.recvfrom(2048)
                except socket.timeout:
                    print("[WARN] Audio RX timeout",end='')
                    continue
                
                # Basic Header Check
                if len(data) < HEADER_SIZE_AUDIO:
                    print(f"[WARN] Audio packet too short: {len(data)} bytes",end='')
                    continue

                try:
                    ver, grp, tid, mode, seq = struct.unpack(HEADER_FMT_AUDIO, data[:HEADER_SIZE_AUDIO])
                except struct.error as e:
                    print(f"[WARN] Audio header unpack error: {e}",end='')
                    continue

                if ver != HEADER_VERSION_AUDIO: 
                    print(f"[WARN] Unknown audio header version: {ver}")
                    continue


                # Filter
                if grp != GROUP_ID: continue
                if tid == TALKER_ID: continue
                
                # Only play if we believe they hold the floor (optional strictness)
                # For now, just decode and play

                if TALKER_ID not in self.lastSeqID:
                    self.lastSeqID[TALKER_ID] = (seq - 1) & 0xFFFF # init last seq for this talker 0xFFFF is for wraparound 

                #check if packages got lost on the way
                expectedSeq = (self.lastSeqID[TALKER_ID] + 1) & 0xFFFF
                if seq != expectedSeq:
                    lost = (seq - expectedSeq) & 0xFFFF
                    print(f"[WARN] Lost {lost} audio packets from Talker-{tid}",end='')
                self.lastSeqID[TALKER_ID] = seq



                #todo for later check sender codec2 mode and maintain per speaker codec2 instance ( How MEMORY heavy ? keine ahnung)
                payload = data[HEADER_SIZE_AUDIO:]
                pcm8k = self.codec2.decode(payload)


                try:
                    senderName = TALKER_NAMES.get(tid, f"Talker-{talker_id}")
                except Exception:
                    senderName = f"Talker-{tid}"
                #print(f"\n[RX] Audio from {senderName} (Seq: {seq})",end='')
                

                # Upsample to system sample rate
                pcm48 = soxr.resample(pcm8k.astype(np.int16), CODEC2_SAMPLE_RATE, SAMPLE_RATE, quality='HQ')
                pcm48 = np.clip(pcm48, -32768, 32767).astype(np.int16)
                rx_stream.write(pcm48.tobytes())


            except Exception as e:
                print(f"[ERR RX] {e}")

    # ---  THREAD: Audio Transmitter ---
    def tx_loop(self):
        # We assume 1 frame per packet for simplicity, can pack more later
        samples_per_frame = self.codec2.samples_per_frame() # 160
        in_frame= samples_per_frame * (SAMPLE_RATE // CODEC2_SAMPLE_RATE) # 48000/8000 = 6  | 6*160 = 960 samples per frame
        stream = self.p.open(format=pyaudio.paInt16, channels=AUDIO_CHANNELS, rate=SAMPLE_RATE,input_device_index=self.input_device_index, input=True, frames_per_buffer=in_frame)


        seq = 0
        
        while self.running:
            # only Transmit if FloorManager state = TRANSMITTING
            if self.floor.state == FloorManager.TRANSMITTING:
                try:
                    raw_data = stream.read(in_frame, exception_on_overflow=False)
                    pcm48 = np.frombuffer(raw_data, dtype=np.int16)
                    # Downsample 48k->8k
                    pcm8k = soxr.resample(pcm48, SAMPLE_RATE, CODEC2_SAMPLE_RATE, quality='HQ')
                    # Ensure length matches codec2 frame from samples_per_frame
                    if pcm8k.shape[0] != samples_per_frame:
                        # Pad or truncate
                        if pcm8k.shape[0] < samples_per_frame:
                            pcm8k = np.pad(pcm8k, (0, samples_per_frame - pcm8k.shape[0]))
                        else:
                            pcm8k = pcm8k[:samples_per_frame]
                    pcm8k = np.clip(pcm8k, -32768, 32767).astype(np.int16)

                    encoded = self.codec2.encode(pcm8k)
                    
                    header = struct.pack(HEADER_FMT_AUDIO, HEADER_VERSION_AUDIO, GROUP_ID, TALKER_ID, CODEC2_MODE, seq)
                    seq = (seq + 1) & 0xFFFF
                    
                    self.sock_tx.sendto(header + encoded, (MULTICAST_GROUP, PORT_AUDIO))
                except Exception as e:
                    print(f"[TX ERR] {e}")
            else:
                time.sleep(0.02) # Sleep to save CPU when not talking
    



def ptt_toggle(app):
    """Toggle PTT state."""
    if app.floor.state == FloorManager.IDLE:
        app.floor.request_floor()
    elif app.floor.state == FloorManager.TRANSMITTING:
        app.floor.release_floor()
    elif app.floor.state == FloorManager.REMOTE_HOLD:
        print("[BCLO] Channel Busy!")

                                  
def on_press(event):
    #print(event)
    global ptt
    if event.name == "umschalt" and event.event_type == "down" and not ptt:
        ptt = True
        #print("Space pressed and held (callback once)")
        ptt_toggle(app)
        
    if event.name == "umschalt" and event.event_type == "up" and ptt:
        ptt = False
        #print("Space released after hold (callback once)")
        ptt_toggle(app)



# --- CLI ---
if __name__ == "__main__":
    parser = argparse.ArgumentParser(description="MulticastVoiceApp")
    parser.add_argument("--ip", help="Interface IP to join/use for multicast (e.g. 192.168.178.30)")
    parser.add_argument("--dummyAudio", help="Plays dummy audio in repeat instead of using audio devices")
    args = parser.parse_args()

    app = MulticastVoiceApp(interface_ip=args.ip)


    #app = MulticastVoiceApp()

    app.start()


    
    
    

    
    print("Commands: [ENTER] = PTT, 'q' = Quit")
    #hook= keyboard.hook_key('shift', on_press, suppress=False)
    

    while True:
        cmd = input()
        if cmd == 'q': break
        ptt_toggle(app)
        #if keyboard.is_pressed('Enter'):
        #    ptt_toggle(app)
        







"""
    try:
        while True:
            cmd = input()
            if cmd == 'q': break
            
            # Toggle PTT logic
            if app.floor.state == FloorManager.IDLE:
                app.floor.request_floor()
            elif app.floor.state == FloorManager.TRANSMITTING:
                app.floor.release_floor()
            elif app.floor.state == FloorManager.REMOTE_HOLD:
                print("[BCLO] Channel Busy!")
                
    except KeyboardInterrupt:
        pass
    app.stop()

"""