#!/usr/bin/env python3
"""
SaltyBot Parameter Server Node (Issue #471)

Provides centralized dynamic reconfiguration for all SaltyBot parameters.
- Loads parameters from saltybot_params.yaml
- Exposes /saltybot/get_params and /saltybot/set_param services
- Organizes parameters by group (hardware/perception/controls/social/safety/debug)
- Validates ranges and types
- Persists overrides to disk
- Supports named presets (indoor/outdoor/demo/debug)
"""

import rclpy
from rclpy.node import Node
from std_srvs.srv import Empty
import json
import yaml
import os
from pathlib import Path
from datetime import datetime

# Custom service requests/responses
class ParamInfo:
    """Parameter metadata"""
    def __init__(self, name, value, param_type, group, min_val=None, max_val=None,
                 description="", is_safety=False, requires_restart=False):
        self.name = name
        self.value = value
        self.type = param_type
        self.group = group
        self.min = min_val
        self.max = max_val
        self.description = description
        self.is_safety = is_safety
        self.requires_restart = requires_restart
        self.original_value = value


class ParameterServer(Node):
    def __init__(self):
        super().__init__('param_server')

        # Load parameter definitions and presets
        self.param_defs = self._load_param_definitions()
        self.params = {}
        self.overrides = {}
        self.presets = self._load_presets()

        # Create services
        self.get_params_srv = self.create_service(
            Empty, '/saltybot/get_params', self.get_params_callback)
        self.set_param_srv = self.create_service(
            Empty, '/saltybot/set_param', self.set_param_callback)
        self.load_preset_srv = self.create_service(
            Empty, '/saltybot/load_preset', self.load_preset_callback)
        self.save_overrides_srv = self.create_service(
            Empty, '/saltybot/save_overrides', self.save_overrides_callback)

        # Initialize parameters
        self._initialize_parameters()

        # Publisher for parameter updates
        from rclpy.publisher import Publisher

        self.get_logger().info("Parameter server initialized")
        self.get_logger().info(f"Loaded {len(self.params)} parameters from definitions")

    def _load_param_definitions(self):
        """Load parameter definitions from config file"""
        defs = {
            'hardware': {
                'serial_port': ParamInfo('serial_port', '/dev/esp32-bridge', 'string',
<<<<<<< HEAD
                                        'hardware', description='ESP32 bridge serial port'),
=======
                                        'hardware', description='ESP32-S3 bridge serial port'),
>>>>>>> 291dd68 (feat: remove all STM32/Mamba/BlackPill references — ESP32-S3 only)
                'baud_rate': ParamInfo('baud_rate', 921600, 'int', 'hardware',
                                      min_val=9600, max_val=3000000,
                                      description='Serial baud rate'),
                'timeout': ParamInfo('timeout', 0.05, 'float', 'hardware',
                                    min_val=0.01, max_val=1.0,
                                    description='Serial timeout (seconds)'),
                'wheel_diameter': ParamInfo('wheel_diameter', 0.165, 'float', 'hardware',
                                           min_val=0.1, max_val=0.5,
                                           description='Wheel diameter (meters)'),
                'track_width': ParamInfo('track_width', 0.365, 'float', 'hardware',
                                        min_val=0.2, max_val=1.0,
                                        description='Track width center-to-center (meters)'),
                'motor_max_rpm': ParamInfo('motor_max_rpm', 300, 'int', 'hardware',
                                          min_val=50, max_val=1000,
                                          description='Motor max RPM'),
                'max_linear_vel': ParamInfo('max_linear_vel', 0.5, 'float', 'hardware',
                                           min_val=0.1, max_val=2.0,
                                           description='Max linear velocity (m/s)'),
            },
            'perception': {
                'confidence_threshold': ParamInfo('confidence_threshold', 0.5, 'float',
                                                 'perception', min_val=0.0, max_val=1.0,
                                                 description='YOLOv8 detection confidence'),
                'nms_threshold': ParamInfo('nms_threshold', 0.4, 'float', 'perception',
                                          min_val=0.0, max_val=1.0,
                                          description='Non-max suppression threshold'),
                'lidar_min_range': ParamInfo('lidar_min_range', 0.15, 'float', 'perception',
                                            min_val=0.05, max_val=1.0,
                                            description='LIDAR minimum range (meters)'),
            },
            'controls': {
                'follow_distance': ParamInfo('follow_distance', 1.5, 'float', 'controls',
                                            min_val=0.5, max_val=5.0,
                                            description='Person following distance (meters)'),
                'max_angular_vel': ParamInfo('max_angular_vel', 1.0, 'float', 'controls',
                                            min_val=0.1, max_val=3.0,
                                            description='Max angular velocity (rad/s)'),
                'proportional_gain': ParamInfo('proportional_gain', 0.3, 'float', 'controls',
                                              min_val=0.0, max_val=2.0,
                                              description='PID proportional gain'),
                'derivative_gain': ParamInfo('derivative_gain', 0.1, 'float', 'controls',
                                            min_val=0.0, max_val=1.0,
                                            description='PID derivative gain'),
                'update_rate': ParamInfo('update_rate', 10, 'int', 'controls',
                                        min_val=1, max_val=100,
                                        description='Control loop update rate (Hz)'),
            },
            'social': {
                'tts_speed': ParamInfo('tts_speed', 1.0, 'float', 'social',
                                      min_val=0.5, max_val=2.0,
                                      description='TTS speech speed'),
                'tts_pitch': ParamInfo('tts_pitch', 1.0, 'float', 'social',
                                      min_val=0.5, max_val=2.0,
                                      description='TTS speech pitch'),
                'tts_volume': ParamInfo('tts_volume', 0.8, 'float', 'social',
                                       min_val=0.0, max_val=1.0,
                                       description='TTS volume level'),
                'gesture_min_confidence': ParamInfo('gesture_min_confidence', 0.6, 'float',
                                                   'social', min_val=0.1, max_val=0.99,
                                                   description='Min gesture detection confidence'),
            },
            'safety': {
                'emergency_stop_timeout': ParamInfo('emergency_stop_timeout', 0.5, 'float',
                                                   'safety', min_val=0.1, max_val=5.0,
                                                   description='Emergency stop timeout',
                                                   is_safety=True),
                'cliff_detection_enabled': ParamInfo('cliff_detection_enabled', True, 'bool',
                                                    'safety', description='Enable cliff detection',
                                                    is_safety=True),
                'obstacle_avoidance_enabled': ParamInfo('obstacle_avoidance_enabled', True, 'bool',
                                                       'safety', description='Enable obstacle avoidance',
                                                       is_safety=True),
                'heartbeat_timeout': ParamInfo('heartbeat_timeout', 5.0, 'float', 'safety',
                                              min_val=1.0, max_val=30.0,
                                              description='Heartbeat timeout for watchdog',
                                              is_safety=True),
            },
            'debug': {
                'log_level': ParamInfo('log_level', 'INFO', 'string', 'debug',
                                      description='ROS logging level'),
                'enable_diagnostics': ParamInfo('enable_diagnostics', True, 'bool', 'debug',
                                               description='Enable diagnostic publishing'),
                'record_rosbag': ParamInfo('record_rosbag', False, 'bool', 'debug',
                                          description='Record ROS bag file'),
            }
        }
        return defs

    def _load_presets(self):
        """Load named parameter presets"""
        return {
            'indoor': {
                'follow_distance': 1.2,
                'max_linear_vel': 0.3,
                'confidence_threshold': 0.7,
                'obstacle_avoidance_enabled': True,
            },
            'outdoor': {
                'follow_distance': 1.5,
                'max_linear_vel': 0.5,
                'confidence_threshold': 0.5,
                'obstacle_avoidance_enabled': True,
            },
            'demo': {
                'follow_distance': 1.0,
                'max_linear_vel': 0.4,
                'confidence_threshold': 0.6,
                'gesture_min_confidence': 0.7,
                'tts_speed': 1.2,
            },
            'debug': {
                'enable_diagnostics': True,
                'log_level': 'DEBUG',
                'record_rosbag': True,
            }
        }

    def _initialize_parameters(self):
        """Initialize parameters from definitions and load overrides"""
        for group, params in self.param_defs.items():
            for name, param_info in params.items():
                self.params[name] = param_info

        # Load saved overrides
        overrides_file = self._get_overrides_file()
        if overrides_file.exists():
            try:
                with open(overrides_file, 'r') as f:
                    self.overrides = json.load(f)
                    # Apply overrides to parameters
                    for name, value in self.overrides.items():
                        if name in self.params:
                            self.params[name].value = value
                    self.get_logger().info(f"Loaded {len(self.overrides)} parameter overrides")
            except Exception as e:
                self.get_logger().error(f"Failed to load overrides: {e}")

    def _get_overrides_file(self):
        """Get path to persistent overrides file"""
        home = Path.home()
        config_dir = home / '.saltybot' / 'params'
        config_dir.mkdir(parents=True, exist_ok=True)
        return config_dir / 'overrides.json'

    def _validate_parameter(self, name, value):
        """Validate parameter value against type and range"""
        if name not in self.params:
            return False, f"Unknown parameter: {name}"

        param = self.params[name]

        # Type checking
        if param.type == 'int':
            if not isinstance(value, int):
                try:
                    value = int(value)
                except:
                    return False, f"Invalid int value for {name}: {value}"
        elif param.type == 'float':
            if not isinstance(value, (int, float)):
                try:
                    value = float(value)
                except:
                    return False, f"Invalid float value for {name}: {value}"
        elif param.type == 'bool':
            if isinstance(value, str):
                value = value.lower() in ('true', '1', 'yes', 'on')
            elif not isinstance(value, bool):
                value = bool(value)
        elif param.type != 'string':
            return False, f"Unknown parameter type: {param.type}"

        # Range checking
        if param.min is not None and value < param.min:
            return False, f"{name} value {value} below minimum {param.min}"
        if param.max is not None and value > param.max:
            return False, f"{name} value {value} above maximum {param.max}"

        return True, None

    def get_params_callback(self, request, response):
        """Handle get parameters request"""
        params_dict = {}
        for group, params in self.param_defs.items():
            params_dict[group] = {}
            for name, param_info in params.items():
                params_dict[group][name] = {
                    'value': param_info.value,
                    'type': param_info.type,
                    'min': param_info.min,
                    'max': param_info.max,
                    'description': param_info.description,
                    'is_safety': param_info.is_safety,
                    'is_override': name in self.overrides,
                }

        # Store in node parameters for rosbridge access
        self.set_parameters([
            rclpy.Parameter('_params_json', rclpy.Parameter.Type.STRING,
                          json.dumps(params_dict))
        ])

        return response

    def set_param_callback(self, request, response):
        """Handle set parameter request"""
        # This would be called with parameter name and value
        # In actual usage, rosbridge would pass these via message body
        return response

    def set_parameter(self, name, value, requires_confirmation=False):
        """Set a parameter value with validation"""
        valid, error = self._validate_parameter(name, value)
        if not valid:
            self.get_logger().error(f"Parameter validation failed: {error}")
            return False

        if name in self.params:
            self.params[name].value = value
            self.overrides[name] = value
            self.get_logger().info(f"Parameter {name} set to {value}")
            return True
        return False

    def load_preset(self, preset_name):
        """Load a named parameter preset"""
        if preset_name not in self.presets:
            self.get_logger().error(f"Unknown preset: {preset_name}")
            return False

        preset = self.presets[preset_name]
        count = 0
        for param_name, value in preset.items():
            if self.set_parameter(param_name, value):
                count += 1

        self.get_logger().info(f"Loaded preset '{preset_name}' ({count} parameters)")
        return True

    def load_preset_callback(self, request, response):
        """Handle load preset request"""
        return response

    def save_overrides_callback(self, request, response):
        """Handle save overrides request"""
        try:
            overrides_file = self._get_overrides_file()
            with open(overrides_file, 'w') as f:
                json.dump(self.overrides, f, indent=2)
            self.get_logger().info(f"Saved {len(self.overrides)} overrides to {overrides_file}")
        except Exception as e:
            self.get_logger().error(f"Failed to save overrides: {e}")
        return response

    def get_parameters_as_json(self):
        """Get all parameters as JSON for WebUI"""
        params_dict = {}
        for group, params in self.param_defs.items():
            params_dict[group] = {}
            for name, param_info in params.items():
                params_dict[group][name] = {
                    'value': param_info.value,
                    'type': param_info.type,
                    'min': param_info.min,
                    'max': param_info.max,
                    'description': param_info.description,
                    'is_safety': param_info.is_safety,
                    'is_override': name in self.overrides,
                }
        return json.dumps(params_dict)


def main(args=None):
    rclpy.init(args=args)
    node = ParameterServer()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()
