# hdgl_enviro.py
import numpy as np
import threading
import time
import struct
from queue import Queue
from rtlsdr import RtlSdr
from hackrf import HackRF
from scipy.signal import resample

# -----------------------------
# Config
# -----------------------------
STRANDS, SLOTS, NODE_COUNT = 8, 4, 4
ALPHA = 0.3        # Smith-graph resonance factor
RIDE_FACTOR = 0.5  # Environmental riding
SAMPLE_RATE = 2.048e6
IQ_LEN = 2048
TX_FREQ = 915e6
TICK_INTERVAL = 0.05

# -----------------------------
# Shared State
# -----------------------------
nodes = {i: np.zeros((STRANDS, SLOTS)) for i in range(NODE_COUNT)}
node_lock = threading.Lock()
env_signal = np.zeros(IQ_LEN)
env_lock = threading.Lock()
tick_count = 0

# -----------------------------
# Node RX Thread
# -----------------------------
def node_rx(queue: Queue):
    global nodes
    while True:
        try:
            # Example: receive a packet from RAK4630 LoRa node
            # Replace with your LoRa RX code
            packet = queue.get()
            flat = list(struct.unpack("B"*STRANDS*SLOTS + "B", packet))
            lattice = np.array(flat[:-1]).reshape((STRANDS, SLOTS)) / 50.0
            node_idx = flat[0] % NODE_COUNT  # map to node index
            with node_lock:
                nodes[node_idx] = lattice
        except Exception as e:
            print("[Node RX Error]", e)

# -----------------------------
# Environmental RX Thread
# -----------------------------
def env_rx():
    global env_signal
    sdr = RtlSdr()
    sdr.sample_rate = SAMPLE_RATE
    sdr.center_freq = TX_FREQ
    sdr.gain = 'auto'
    while True:
        try:
            samples = sdr.read_samples(IQ_LEN)
            with env_lock:
                env_signal = samples.astype(np.complex64)
        except Exception as e:
            print("[Env RX Error]", e)
        time.sleep(TICK_INTERVAL)

# -----------------------------
# Smith-graph resonance
# -----------------------------
def smith_resonance():
    with node_lock:
        node_list = list(nodes.values())
        blended_nodes = {}
        for i, lattice in enumerate(node_list):
            resonance = np.zeros_like(lattice)
            for j, other in enumerate(node_list):
                if i == j: continue
                resonance += other
            resonance /= (len(node_list)-1)
            blended_nodes[i] = (1-ALPHA)*lattice + ALPHA*resonance
        # Update nodes
        for i in blended_nodes:
            nodes[i] = blended_nodes[i]

# -----------------------------
# Lattice aggregation
# -----------------------------
def aggregate_lattice():
    with node_lock:
        lattices = np.array(list(nodes.values()))
        return np.mean(lattices, axis=0)

# -----------------------------
# Lattice -> IQ
# -----------------------------
def lattice_to_iq(lattice, carrier=None, length=IQ_LEN):
    t = np.arange(length) / SAMPLE_RATE
    sig = np.zeros(length)
    for s in range(STRANDS):
        weight = np.mean(lattice[s])
        freq = 1e3*(s+1)
        sig += weight * np.sin(2*np.pi*freq*t)

    if carrier is not None:
        if len(carrier) != length:
            carrier = resample(carrier, length)
        sig = (1-RIDE_FACTOR)*carrier + RIDE_FACTOR*sig

    sig /= np.max(np.abs(sig)) + 1e-12
    return sig.astype(np.complex64)

# -----------------------------
# HackRF TX Thread
# -----------------------------
def tx_loop():
    global tick_count
    hackrf = HackRF()
    hackrf.setup()
    hackrf.sample_rate = SAMPLE_RATE
    hackrf.center_freq = TX_FREQ
    print("[+] HackRF TX initialized...")
    while True:
        smith_resonance()
        agg = aggregate_lattice()
        with env_lock:
            carrier = env_signal.copy()
        iq = lattice_to_iq(agg, carrier)
        hackrf.tx(iq.tobytes())
        tick_count += 1
        print(f"[Tick {tick_count}] Aggregated Lattice: {agg.mean():.3f}")
        time.sleep(TICK_INTERVAL)

# -----------------------------
# Main
# -----------------------------
def main():
    # Placeholder queue for LoRa packets
    packet_queue = Queue()

    threading.Thread(target=node_rx, args=(packet_queue,), daemon=True).start()
    threading.Thread(target=env_rx, daemon=True).start()
    threading.Thread(target=tx_loop, daemon=True).start()

    # Keep main alive
    while True:
        time.sleep(1)

if __name__ == "__main__":
    main()
