import time
import sys
import json
import struct
import queue
from datetime import datetime, timezone

import numpy as np
import bgapi


# =============================================================================
# EINSTELLUNGEN: BG22 / CTE
# =============================================================================

COM_PORT = "COM13"
BAUD_RATE = 115200
XAPI_PATH = r"C:\Users\sandr\.silabs\slt\installs\conan\p\simpleca33d691c539\p\bluetooth_le_host\api\sl_bt.xapi"

OUTPUT_FILE = "iq_samples.jsonl"
ANGLE_TABLE_FILE = "winkel_tabelle.json"   # nur Azimut/Elevation, ein Eintrag pro neuem Wert

SLOT_DURATIONS = 2                        # 1 oder 2 Mikrosekunden pro Slot
SWITCHING_PATTERN = bytes(range(0, 16))   # Reihenfolge der Antennen
CTE_COUNT = 0                             # 0 = unbegrenzt weiter empfangen


# =============================================================================
# EINSTELLUNGEN: EIGENER MUSIC-ALGORITHMUS
# =============================================================================

CALIBRATION_FILE = "calibration.json"

FREQUENCY_HZ = 2.44e9
SPEED_OF_LIGHT = 3e8
WAVELENGTH = SPEED_OF_LIGHT / FREQUENCY_HZ

ANTENNA_SPACING = 0.0400   # 40 mm, laut AN1195 (nicht lambda/2)

ARRAY_ROWS = 4
ARRAY_COLS = 4
NUM_ANTENNAS = ARRAY_ROWS * ARRAY_COLS

AZIMUTH_SIGN = 1
ELEVATION_SIGN = 1

REFERENCE_SAMPLES = 8   # feste Referenzperiode am Anfang jedes CTE
NUM_SOURCES = 1         # nur ein Tag sendet gleichzeitig

AZIMUTH_RANGE = np.arange(-180, 181, 1)
ELEVATION_RANGE = np.arange(-90, 91, 1)

ACCUMULATE_PACKETS = 20   # Pakete pro Winkel-Update


# =============================================================================
# GLOBALER ZUSTAND
# =============================================================================

lib = None
output_file = None
sync_handle = None
already_synced = False
pending_actions = queue.Queue()

_cached_positions = None
_cached_offsets = None
_calibration_loaded = False

_snapshot_buffer = []
_latest_stable_angle = (None, None)
_angle_table = []


# =============================================================================
# BLUETOOTH-EVENTS
# =============================================================================

def on_event(event):
    """Wird bei jedem eingehenden Ereignis vom BG22 aufgerufen."""
    global sync_handle

    name = str(event).split("(")[0]

    if name in ("bt_evt_scanner_scan_report",
                "bt_evt_scanner_extended_advertisement_report"):
        look_for_periodic_advertising(event)

    elif name in ("bt_evt_sync_opened",
                  "bt_evt_sync_scanner_opened",
                  "bt_evt_periodic_sync_opened"):
        sync_handle = event.sync
        print(f"Verbindung zum Tag hergestellt (sync={sync_handle}).")
        pending_actions.put(("enable_cte", sync_handle))

    elif name in ("bt_evt_sync_closed",
                  "bt_evt_sync_scanner_closed",
                  "bt_evt_periodic_sync_closed"):
        print("Verbindung zum Tag wurde beendet.")

    elif name in ("bt_evt_cte_receiver_connectionless_iq_report",
                  "bt_evt_periodic_sync_iq_report",
                  "bt_evt_cte_receiver_iq_report"):
        save_iq_report(event)

    elif hasattr(event, "result") and event.result != 0:
        print(f"Fehler vom BG22: {event}")


def look_for_periodic_advertising(event):
    """Prüft ein gefundenes Gerät: sendet es CTE über periodisches Advertising?"""
    global already_synced

    if already_synced:
        return

    periodic_interval = getattr(event, "periodic_interval", None)
    adv_sid = getattr(event, "adv_sid", None)

    if not periodic_interval:
        return

    print(f"Tag gefunden! RSSI={event.rssi} dBm, Interval={periodic_interval} ms")

    already_synced = True
    pending_actions.put(("connect_to_tag", event.address, event.address_type, adv_sid))


# =============================================================================
# BLUETOOTH-BEFEHLE
# =============================================================================

def start_scan():
    """Startet die Suche nach Bluetooth-Geräten in der Umgebung."""
    print("Suche nach einem Tag ...")
    lib.bt.scanner.set_parameters(1, 160, 160)
    lib.bt.scanner.start(1, 2)


def connect_to_tag(address, address_type, adv_sid):
    """Beendet die Suche und verbindet sich mit dem gefundenen Tag."""
    lib.bt.scanner.stop()
    lib.bt.sync_scanner.open(address, address_type, adv_sid)


def enable_cte(sync):
    """Aktiviert den CTE-Empfang auf der bestehenden Synchronisation."""
    lib.bt.cte_receiver.enable_connectionless_cte(
        sync, SLOT_DURATIONS, CTE_COUNT, SWITCHING_PATTERN
    )
    print("CTE-Empfang läuft. Warte auf IQ-Daten ...\n")


# =============================================================================
# EIGENER MUSIC-ALGORITHMUS (2D, Azimut + Elevation, 4x4 URA)
# =============================================================================

def antenna_positions():
    """(x, y)-Position jeder Antenne im 4x4-Raster."""
    positions = []
    for row in range(ARRAY_ROWS):
        for col in range(ARRAY_COLS):
            positions.append((row * ANTENNA_SPACING, col * ANTENNA_SPACING))
    return np.array(positions)


def steering_vector(azimuth_deg, elevation_deg, positions):
    """Theoretischer Steering-Vector für einen Winkel."""
    az = np.radians(azimuth_deg) * AZIMUTH_SIGN
    el = np.radians(elevation_deg) * ELEVATION_SIGN
    u = np.cos(el) * np.sin(az)
    v = np.sin(el)
    phase = (2 * np.pi / WAVELENGTH) * (positions[:, 0] * u + positions[:, 1] * v)
    return np.exp(-1j * phase)


def extract_snapshots(i_values, q_values):
    """Wandelt IQ-Rohwerte in Antennen-Snapshots um."""
    complex_samples = np.array(i_values) + 1j * np.array(q_values)
    switching_samples = complex_samples[REFERENCE_SAMPLES:]

    num_full_cycles = len(switching_samples) // NUM_ANTENNAS
    if num_full_cycles == 0:
        return None

    usable = switching_samples[: num_full_cycles * NUM_ANTENNAS]
    return usable.reshape(num_full_cycles, NUM_ANTENNAS).T


def circular_mean(angles):
    """Mittelwert von Winkeln, robust gegen den Sprung bei ±π."""
    return np.angle(np.mean(np.exp(1j * angles)))


def music_spectrum(snapshots, positions):
    """MUSIC-Pseudospektrum, gibt den stärksten Peak zurück."""
    covariance = snapshots @ snapshots.conj().T / snapshots.shape[1]
    eigenvalues, eigenvectors = np.linalg.eigh(covariance)
    noise_subspace = eigenvectors[:, : NUM_ANTENNAS - NUM_SOURCES]

    best_azimuth, best_elevation, best_power = 0.0, 0.0, -np.inf
    for az in AZIMUTH_RANGE:
        for el in ELEVATION_RANGE:
            a = steering_vector(az, el, positions)
            denom = a.conj().T @ noise_subspace @ noise_subspace.conj().T @ a
            power = 1.0 / np.abs(denom)
            if power > best_power:
                best_power = power
                best_azimuth, best_elevation = az, el

    return int(best_azimuth), int(best_elevation)


def load_calibration():
    """Lädt gespeicherte Phasenoffsets, falls vorhanden."""
    try:
        with open(CALIBRATION_FILE, "r", encoding="utf-8") as f:
            data = json.load(f)
        return np.array(data["phasenoffsets_rad"])
    except FileNotFoundError:
        return None


def apply_calibration(snapshots, offsets):
    """Entfernt bekannte Phasenoffsets aus den Rohdaten."""
    correction = np.exp(-1j * offsets).reshape(-1, 1)
    return snapshots * correction


def add_packet_and_maybe_estimate(i_values, q_values):
    """Sammelt Snapshots; berechnet alle ACCUMULATE_PACKETS Pakete einen Winkel."""
    global _cached_positions, _cached_offsets, _calibration_loaded
    global _snapshot_buffer, _latest_stable_angle

    if _cached_positions is None:
        _cached_positions = antenna_positions()

    if not _calibration_loaded:
        _cached_offsets = load_calibration()
        _calibration_loaded = True

    snapshots = extract_snapshots(i_values, q_values)
    if snapshots is not None:
        _snapshot_buffer.append(snapshots)

    if len(_snapshot_buffer) < ACCUMULATE_PACKETS:
        return _latest_stable_angle

    combined = np.concatenate(_snapshot_buffer, axis=1)
    _snapshot_buffer = []

    if _cached_offsets is not None:
        combined = apply_calibration(combined, _cached_offsets)

    _latest_stable_angle = music_spectrum(combined, _cached_positions)
    return _latest_stable_angle


# =============================================================================
# IQ-DATEN VERARBEITEN
# =============================================================================

def decode_samples(raw_bytes):
    """Wandelt die rohen Bytes in I- und Q-Wertelisten um."""
    values = list(struct.unpack(f"{len(raw_bytes)}b", raw_bytes))
    return values[0::2], values[1::2]


def add_to_angle_table(zeit, counter, azimuth, elevation):
    """Fügt einen neuen Winkel-Eintrag hinzu und schreibt die Tabelle neu."""
    _angle_table.append({
        "zeit": zeit, "counter": counter,
        "azimut": azimuth, "elevation": elevation,
    })
    with open(ANGLE_TABLE_FILE, "w", encoding="utf-8") as f:
        json.dump(_angle_table, f, indent=2, ensure_ascii=False)


def save_iq_report(event):
    """Dekodiert, berechnet den Winkel, zeigt an, speichert."""
    i_values, q_values = decode_samples(event.samples)

    azimuth, elevation = add_packet_and_maybe_estimate(i_values, q_values)

    print(f"IQ-Paket #{event.event_counter}  Kanal={event.channel}  "
          f"RSSI={event.rssi} dBm  {len(i_values)} Samples  "
          f"({len(_snapshot_buffer)}/{ACCUMULATE_PACKETS} gesammelt)")
    print(f"   Rohbytes:   {event.samples[:8]} ...")
    print(f"   I/Q-Werte:  {list(zip(i_values[:4], q_values[:4]))} ...")
    if azimuth is not None:
        print(f"   Azimut={azimuth}°  Elevation={elevation}°")
        if not _angle_table or (azimuth, elevation) != (
            _angle_table[-1]["azimut"], _angle_table[-1]["elevation"]
        ):
            add_to_angle_table(
                datetime.now(timezone.utc).isoformat(),
                event.event_counter, azimuth, elevation,
            )
    else:
        print("   sammle noch Pakete ...")

    entry = {
        "zeit": datetime.now(timezone.utc).isoformat(),
        "counter": event.event_counter,
        "kanal": event.channel,
        "rssi": event.rssi,
        "slot_dauer_us": SLOT_DURATIONS,
        "antennen_pattern": list(SWITCHING_PATTERN),
        "i": i_values,
        "q": q_values,
        "azimut": azimuth,
        "elevation": elevation,
    }
    output_file.write(json.dumps(entry) + "\n")
    output_file.flush()


# =============================================================================
# HAUPTPROGRAMM
# =============================================================================

def connect_to_bg22():
    """Öffnet die serielle Verbindung, stellt sauberen Zustand her."""
    connector = bgapi.SerialConnector(COM_PORT, baudrate=BAUD_RATE)
    module = bgapi.BGLib(connector, XAPI_PATH, on_event)
    module.open()
    print(f"Verbunden mit {COM_PORT}.")

    module.bt.system.reboot()
    time.sleep(2.0)

    module.bt.system.hello()
    print("BG22 antwortet, alles bereit.")
    return module


def process_pending_actions():
    """Führt Aktionen aus, die nicht direkt im Event-Handler laufen dürfen."""
    while True:
        try:
            action, *details = pending_actions.get(timeout=0.1)
        except queue.Empty:
            continue

        if action == "connect_to_tag":
            connect_to_tag(*details)
        elif action == "enable_cte":
            enable_cte(*details)


def main():
    global lib, output_file

    output_file = open(OUTPUT_FILE, "a", encoding="utf-8")
    print(f"IQ-Samples werden gespeichert in: {OUTPUT_FILE}")

    lib = connect_to_bg22()
    start_scan()
    process_pending_actions()


if __name__ == "__main__":
    try:
        main()
    except KeyboardInterrupt:
        print("\nBeendet.")
    except Exception as error:
        print(f"Es ist ein Fehler aufgetreten: {error}")
        sys.exit(1)
    finally:
        if lib is not None:
            lib.close()
        if output_file is not None:
            output_file.close()