#!/usr/bin/env python3

import sys
import os
sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__))))

from src.config_loader import ConfigLoader

def test_config_loader():
    print("Testing ConfigLoader...")
    
    # Test singleton pattern
    loader1 = ConfigLoader()
    loader2 = ConfigLoader()
    assert loader1 is loader2, "ConfigLoader should be singleton"
    print("✓ Singleton pattern works")
    
    # Test config loading
    config = loader1.get_full_config()
    assert 'robot' in config, "Config should contain robot section"
    assert 'wall' in config, "Config should contain wall section"
    assert 'hole' in config, "Config should contain hole section"
    assert 'task_points' in config, "Config should contain task_points section"
    assert 'transport_object' in config, "Config should contain transport_object section"
    assert 'simulation' in config, "Config should contain simulation section"
    print("✓ All required config sections present")
    
    # Test robot config
    robot_config = loader1.get_robot_config()
    assert robot_config['model_path'] == "models/manual_robot.urdf", "Robot model path should be correct"
    assert len(robot_config['base_position']) == 3, "Robot base position should have 3 coordinates"
    assert len(robot_config['base_orientation']) == 3, "Robot base orientation should have 3 angles"
    print("✓ Robot config validation passed")
    
    # Test wall config
    wall_config = loader1.get_wall_config()
    assert len(wall_config['position']) == 3, "Wall position should have 3 coordinates"
    assert 'dimensions' in wall_config, "Wall should have dimensions"
    assert wall_config['dimensions']['width'] > 0, "Wall width should be positive"
    assert wall_config['dimensions']['height'] > 0, "Wall height should be positive"
    assert wall_config['dimensions']['thickness'] > 0, "Wall thickness should be positive"
    print("✓ Wall config validation passed")
    
    # Test hole config
    hole_config = loader1.get_hole_config()
    assert len(hole_config['position_relative_to_wall']) == 3, "Hole position should have 3 coordinates"
    assert 'dimensions' in hole_config, "Hole should have dimensions"
    assert hole_config['dimensions']['width'] > 0, "Hole width should be positive"
    assert hole_config['dimensions']['height'] > 0, "Hole height should be positive"
    print("✓ Hole config validation passed")
    
    # Test task points
    task_points = loader1.get_task_points()
    assert 'point_A' in task_points, "Should have point_A"
    assert 'point_B' in task_points, "Should have point_B"
    assert len(task_points['point_A']['position']) == 3, "Point A should have 3 coordinates"
    assert len(task_points['point_B']['position']) == 3, "Point B should have 3 coordinates"
    print("✓ Task points validation passed")
    
    # Test transport object config
    obj_config = loader1.get_transport_object_config()
    assert len(obj_config['initial_position']) == 3, "Object initial position should have 3 coordinates"
    assert 'dimensions' in obj_config, "Object should have dimensions"
    assert obj_config['mass'] > 0, "Object mass should be positive"
    print("✓ Transport object config validation passed")
    
    # Test simulation config
    sim_config = loader1.get_simulation_config()
    assert sim_config['timestep'] > 0, "Timestep should be positive"
    assert len(sim_config['gravity']) == 3, "Gravity should have 3 components"
    print("✓ Simulation config validation passed")
    
    print("\n✅ All tests passed! ConfigLoader is working correctly.")
    
    # Print sample data
    print("\nSample configuration data:")
    print(f"Robot model: {robot_config['model_path']}")
    print(f"Wall position: {wall_config['position']}")
    print(f"Wall dimensions: {wall_config['dimensions']}")
    print(f"Hole dimensions: {hole_config['dimensions']}")
    print(f"Point A: {task_points['point_A']['position']}")
    print(f"Point B: {task_points['point_B']['position']}")
    print(f"Object mass: {obj_config['mass']} kg")
    print(f"Simulation timestep: {sim_config['timestep']} s")

if __name__ == "__main__":
    test_config_loader()