import sys
import os
import numpy as np
import time

# Add project root and ISOLATE paths to sys.path
project_root = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
sys.path.append(project_root)

# Import the model wrapper
try:
    from scripts.ISOLATE.model_wrapper import ShenronRadarModel
    print(">>> Successfully imported ShenronRadarModel")
except ImportError as e:
    print(f">>> Failed to import ShenronRadarModel: {e}")
    sys.exit(1)

def run_smoke_test():
    print("\n" + "="*50)
    print("SHENRON MODEL SMOKE TEST")
    print("="*50)
    
    success = True
    
    try:
        # 1. Initialize the Model
        print("\n[Step 1] Initializing ShenronRadarModel...")
        start_time = time.time()
        model = ShenronRadarModel(radar_type='radarbook')
        print(f"Done in {time.time() - start_time:.2f}s")
        
        # 2. Prepare Synthetic Semantic LiDAR Data
        # Format: [x, y, z, intensity, cos_inc_angle, object_idx, semantic_tag]
        print("\n[Step 2] Preparing synthetic semantic LiDAR data...")
        # Create a "vehicle" (tag 10) 15 meters ahead, slightly to the left
        num_points = 100
        data = np.zeros((num_points, 7))
        
        # Geometry
        data[:, 0] = np.random.uniform(14.5, 15.5, num_points) # x (forward)
        data[:, 1] = np.random.uniform(-2.0, -1.0, num_points) # y (left)
        data[:, 2] = np.random.uniform(0.5, 1.5, num_points)   # z (up)
        
        # Physical properties
        data[:, 3] = 0.5   # intensity
        data[:, 4] = 0.9   # cos_inc_angle
        data[:, 5] = 123   # object_idx
        data[:, 6] = 10    # semantic_tag (Vehicle)
        
        print(f"Created {num_points} points representing a vehicle at 15m.")

        # 3. Process data through the model
        print("\n[Step 3] Running Shenron Radar Simulation (FMCW + DSP)...")
        start_time = time.time()
        rich_pcd = model.process(data)
        process_time = time.time() - start_time
        print(f"Done in {process_time:.2f}s")
        
        # 4. Validate output
        print("\n[Step 4] Validating output...")
        print(f"Output shape: {rich_pcd.shape}")
        
        if rich_pcd.shape[0] > 0:
            print(f"Detected {rich_pcd.shape[0]} radar targets.")
            print("Format: [x, y, z, velocity, magnitude]")
            # Check for expected coordinates (roughly 15m ahead)
            avg_x = np.mean(rich_pcd[:, 0])
            avg_y = np.mean(rich_pcd[:, 1])
            print(f"Average Detection Position: x={avg_x:.2f}, y={avg_y:.2f}")
            
            if 10 < avg_x < 20:
                print("SUCCESS: Detections are in the expected range.")
            else:
                print("WARNING: Detections are outside expected range.")
                
            print("\nSample Detections:")
            print(rich_pcd[:5])
        else:
            print("ERROR: No detections found from valid input data.")
            success = False
            
    except Exception as e:
        print(f"\n!!! CRITICAL ERROR during smoke test: {e}")
        import traceback
        traceback.print_exc()
        success = False

    print("\n" + "="*50)
    if success:
        print("RESULT: SMOKE TEST PASSED")
    else:
        print("RESULT: SMOKE TEST FAILED")
    print("="*50)
    
    return success

if __name__ == "__main__":
    run_smoke_test()