import numpy as np
import os
import sys
from pathlib import Path

# Add project root and ISOLATE paths
project_root = Path(__file__).parent.parent.parent
sys.path.append(str(project_root))
sys.path.append(str(project_root / 'scripts' / 'ISOLATE'))

try:
    from scripts.ISOLATE.model_wrapper import ShenronRadarModel
    print("✅ Model Import: SUCCESS")
except Exception as e:
    print(f"❌ Model Import: FAILED ({e})")
    sys.exit(1)

def verify_signal_flow(frame_idx=190):
    print(f"\n--- 🛰️ Radar Lab Verification (Frame {frame_idx}) ---")
    
    # 1. Initialize Engine
    try:
        model = ShenronRadarModel(radar_type='awrl1432')
        print(f"✅ Engine Init: awrl1432 (Gain: {model.radar_obj.gain}dB)")
    except Exception as e:
        print(f"❌ Engine Init: FAILED ({e})")
        return

    # 2. Load Real LiDAR Data
    lidar_path = project_root / 'Shenron_debug' / 'logs' / 'lidar' / f"frame_{frame_idx:06d}.npy"
    if not lidar_path.exists():
        print(f"❌ Lidar Data: NOT FOUND at {lidar_path}")
        return
    
    data = np.load(lidar_path)
    # Pad to 7-col [x,y,z,int,cos,obj,tag] if old 6-col format
    if data.shape[1] == 6:
        padded = np.zeros((data.shape[0], 7), dtype=np.float32)
        padded[:, 0:3] = data[:, 0:3]
        padded[:, 4:7] = data[:, 3:6]
        data = padded
    print(f"✅ Lidar Data Loaded: {data.shape[0]} points")

    # 3. Process Frame
    try:
        pcd = model.process(data)
        print(f"✅ Signal Process: SUCCESS ({len(pcd)} detections)")
    except Exception as e:
        print(f"❌ Signal Process: FAILED ({e})")
        return

    # 4. Verify Metrology Extraction
    try:
        met = model.get_last_metrology()
        keys = met.keys()
        expected = ["rd_heatmap", "ra_heatmap", "threshold_matrix"]
        
        status = True
        for k in expected:
            if k not in keys:
                print(f"❌ Metrology Key Missing: {k}")
                status = False
            else:
                val = met[k]
                if np.max(val) <= 0:
                    print(f"❌ Metrology Data Null: {k} (Max: {np.max(val)})")
                    status = False
        
        if status:
            print("✅ Metrology Extraction: ALL KEYS VALID")
            print(f"   -> RD Heatmap: {met['rd_heatmap'].shape} (Peak Power: {np.max(met['rd_heatmap']):.2e})")
            print(f"   -> RA Heatmap: {met['ra_heatmap'].shape} (Peak Power: {np.max(met['ra_heatmap']):.2e})")
            print(f"   -> CFAR Threshold: {met['threshold_matrix'].shape} (Mean: {np.mean(met['threshold_matrix']):.2e})")
            
    except Exception as e:
        print(f"❌ Metrology Extraction: FAILED ({e})")

    # 5. Verify Signal Metrics
    try:
        metrics = model.get_signal_metrics()
        print(f"✅ Signal Metrics: {metrics}")
        if metrics['peak_snr_db'] <= 0:
            print("⚠️ WARNING: Very low SNR detected (< 0dB). Check gain.")
    except Exception as e:
        print(f"❌ Signal Metrics: FAILED ({e})")

if __name__ == "__main__":
    verify_signal_flow(190)