# =============================================================================
# DRONE CONFIGURATION
# =============================================================================

DRONE = {
    # Physical properties
    "mass": 1.0,              # kg
    "size": (0.3, 0.3, 0.1),  # (width, depth, height) in meters
    "color": (0.8, 0.1, 0.1, 1.0),  # RGBA (red)
    
    # Starting position
    "start_position": (0.0, 0.0, 5.0),  # (x, y, z) in meters
    
    # Control limits
    "max_thrust": 1.0,   # Maximum thrust command (-1 to 1)
    "max_pitch": 0.5,    # Maximum pitch command
    "max_roll": 0.5,     # Maximum roll command
    "max_yaw": 1.0,      # Maximum yaw command
    
    # Thrust/torque scaling
    "thrust_scale": 15.0,      # Force multiplier for thrust
    "pitch_torque_scale": 2.0, # Torque multiplier for pitch
    "roll_torque_scale": 2.0,  # Torque multiplier for roll
    "yaw_torque_scale": 1.0,   # Torque multiplier for yaw
}

# =============================================================================
# ROVER (LANDING PAD) CONFIGURATION
# =============================================================================

ROVER = {
    # Physical properties
    "size": (1.0, 1.0, 0.3),  # (width, depth, height) in meters
    "color": (0.2, 0.6, 0.2, 1.0),  # RGBA (green)
    
    # Starting position
    "start_position": (0.0, 0.0, 0.15),  # (x, y, z) in meters
    
    # Movement patterns (for RoverController)
    "default_pattern": "stationary",  # stationary, linear, circular, square, random
    "default_speed": 0.5,             # m/s
    "default_amplitude": 2.0,         # meters (radius for circular, half-width for others)
}

# =============================================================================
# CAMERA CONFIGURATION
# =============================================================================

CAMERA = {
    # Camera sensor settings (for simulation)
    "width": 320,
    "height": 240,
    "fov": 60.0,           # Field of view in degrees
    "near_clip": 0.1,      # Near clipping plane
    "far_clip": 100.0,     # Far clipping plane
    "jpeg_quality": 70,    # JPEG compression quality (1-100)
    
    # Camera viewer settings (for camera_viewer.py)
    "viewer_fps": 30,                      # Display framerate
    "viewer_topic": "/drone/camera",       # ROS 2 topic for Gazebo
    "viewer_topic_ardupilot": "/camera/image_raw",  # Topic for ArduPilot
    "show_info_overlay": True,             # Show FPS and frame count
}

# =============================================================================
# PHYSICS CONFIGURATION
# =============================================================================

PHYSICS = {
    "gravity": -9.81,            # m/s² (negative = downward)
    "timestep": 1.0 / 240.0,     # Physics timestep (240 Hz)
    "telemetry_rate": 24,        # Telemetry updates per second
}

# =============================================================================
# NETWORK CONFIGURATION
# =============================================================================

NETWORK = {
    "host": "0.0.0.0",     # Listen on all interfaces
    "command_port": 5555,  # Port for receiving commands
    "telemetry_port": 5556,# Port for sending telemetry
}

# =============================================================================
# LANDING DETECTION
# =============================================================================

LANDING = {
    "success_distance": 0.5,    # Maximum horizontal distance from pad center (m)
    "success_velocity": 0.3,    # Maximum landing velocity (m/s)
    "height_threshold": 0.5,    # Height considered "landed" (m)
}

# =============================================================================
# CONTROLLER GAINS (PD Controller)
# =============================================================================

CONTROLLER = {
    # Altitude control
    "Kp_z": 0.5,    # Proportional gain for altitude
    "Kd_z": 0.3,    # Derivative gain for altitude
    
    # Horizontal control
    "Kp_xy": 0.3,   # Proportional gain for horizontal position
    "Kd_xy": 0.2,   # Derivative gain for horizontal position
    
    # Control rate
    "rate": 50,     # Control loop frequency (Hz)
}

# =============================================================================
# ARDUPILOT ROS 2 CONFIGURATION
# =============================================================================

ARDUPILOT = {
    # Vehicle type
    "vehicle": "ArduCopter",      # ArduCopter, ArduPlane, APMrover2
    "frame": "iris",              # Gazebo model (iris, wildthumper)
    
    # ROS 2 workspace (set by install_ardupilot.sh)
    "workspace": "~/ardu_ws",
    
    # Simulation worlds
    "default_world": "runway",    # runway, maze, sitl
    
    # MAVProxy connection (for GCS features)
    "mavproxy_port": 14550,       # MAVProxy UDP port
    
    # DDS configuration
    "dds_enable": True,           # Enable DDS for native ROS 2 topics
    "dds_domain_id": 0,           # Must match ROS_DOMAIN_ID
}

# ArduPilot ROS 2 Topics (read-only reference)
ARDUPILOT_TOPICS = {
    # State topics (subscribe)
    "pose": "/ap/pose/filtered",           # PoseStamped
    "geopose": "/ap/geopose/filtered",     # GeoPoseStamped
    "twist": "/ap/twist/filtered",         # TwistStamped
    "imu": "/ap/imu/filtered",             # Imu
    "battery": "/ap/battery",              # BatteryState
    "navsat": "/ap/navsat",                # NavSatFix
    
    # Command topics (publish) - via MAVLink
    "cmd_vel": "/cmd_vel",                 # Twist (velocity commands)
}

# MAVLink configuration (for pymavlink/mavproxy)
MAVLINK = {
    # MAVLink system IDs
    "system_id": 255,             # GCS system ID
    "component_id": 190,          # MAV_COMP_ID_MISSIONPLANNER
    
    # Target system (the autopilot)
    "target_system": 1,
    "target_component": 1,
    
    # Connection - SITL provides multiple ports:
    # - TCP 5760: MAVProxy uses this (SERIAL0)
    # - TCP 5762: Available for additional connections (SERIAL1)
    # - TCP 5763: Available for additional connections (SERIAL2)
    "connection_string": "tcp:127.0.0.1:5762",
    
    # Timeouts (seconds)
    "heartbeat_timeout": 5.0,
    "connection_timeout": 30.0,
}