using RobotNet10.CustomConfiguration.Events;
using RobotNet10.CustomConfiguration.Models;
using RobotNet10.CustomConfiguration.Services;
using RobotNet10.RobotApp.Services.Navigation;
using RobotNet10.RobotApp.Services.Navigation.CSharp;
using System.Reflection;

namespace RobotNet10.RobotApp.Services.ConfigManager;

/// <summary>
/// Service implementation for managing navigation configurations
/// </summary>
public class NavigationConfig : INavigationConfig
{
    private readonly IConfigManager _configManager;
    private readonly Logger<NavigationConfig> _logger;

    private readonly Lock _lockObject = new();
    
    // Config objects
    private PurePursuitConfig? _purePursuitConfig;
    private VelocitySignalProcessingConfig? _velocitySignalProcessingConfig;
    private VelocityEstimatorConfig? _velocityEstimatorConfig;
    private MotorDynamicsConfig? _motorDynamicsConfig;
    private PIDConfig? _movePidConfig;
    private PIDConfig? _rotatePidConfig;
    private Navigation.NavigationConfig? _navigationConfig;
    private RobotNet10.NavigationTune.Shared.Models.StanleyConfig? _stanleyConfig;
    private DockToConfig? _dockToConfig;
    private DockToConfig? _moveStraightConfig;
    private LocalPlannerConfig? _localPlannerConfig;

    // Load flags
    private bool _purePursuitConfigLoaded = false;
    private bool _velocitySignalProcessingConfigLoaded = false;
    private bool _velocityEstimatorConfigLoaded = false;
    private bool _motorDynamicsConfigLoaded = false;
    private bool _movePidConfigLoaded = false;
    private bool _rotatePidConfigLoaded = false;
    private bool _navigationConfigLoaded = false;
    private bool _stanleyConfigLoaded = false;
    private bool _dockToConfigLoaded = false;
    private bool _moveStraightConfigLoaded = false;
    private bool _localPlannerConfigLoaded = false;

    // Config type constants
    private const string PURE_PURSUIT_CONFIG_TYPE = "PurePursuitConfig";
    private const string VELOCITY_SIGNAL_PROCESSING_CONFIG_TYPE = "VelocitySignalProcessingConfig";
    private const string VELOCITY_ESTIMATOR_CONFIG_TYPE = "VelocityEstimatorConfig";
    private const string MOTOR_DYNAMICS_CONFIG_TYPE = "MotorDynamicsConfig";
    private const string MOVE_PID_CONFIG_TYPE = "MovePidConfig";
    private const string ROTATE_PID_CONFIG_TYPE = "RotatePidConfig";
    private const string NAVIGATION_CONFIG_TYPE = "NavigationConfig";
    private const string STANLEY_CONFIG_TYPE = "StanleyConfig";
    private const string DOCK_TO_CONFIG_TYPE = "DockToConfig";
    private const string MOVE_STRAIGHT_CONFIG_TYPE = "MoveStraightConfig";
    private const string LOCAL_PLANNER_CONFIG_TYPE = "LocalPlannerConfig";

    public NavigationConfig(IConfigManager configManager, Logger<NavigationConfig> logger)
    {
        _configManager = configManager ?? throw new ArgumentNullException(nameof(configManager));
        _logger = logger;
        
        // Subscribe to config changes
        _configManager.ConfigChanged += OnConfigChanged;
    }

    public PurePursuitConfig GetPurepursuitConfig()
    {
        if (!_purePursuitConfigLoaded)
        {
            lock (_lockObject)
            {
                if (!_purePursuitConfigLoaded)
                {
                    LoadPurePursuitConfigAsync().GetAwaiter().GetResult();
                }
            }
        }
        return _purePursuitConfig ?? throw new InvalidOperationException("PurePursuit configuration not loaded");
    }

    public VelocitySignalProcessingConfig GetVelocitySignalProcessingConfig()
    {
        if (!_velocitySignalProcessingConfigLoaded)
        {
            lock (_lockObject)
            {
                if (!_velocitySignalProcessingConfigLoaded)
                {
                    LoadVelocitySignalProcessingConfigAsync().GetAwaiter().GetResult();
                }
            }
        }
        return _velocitySignalProcessingConfig ?? throw new InvalidOperationException("VelocitySignalProcessing configuration not loaded");
    }

    public VelocityEstimatorConfig GetVelocityEstimatorConfig()
    {
        if (!_velocityEstimatorConfigLoaded)
        {
            lock (_lockObject)
            {
                if (!_velocityEstimatorConfigLoaded)
                {
                    LoadVelocityEstimatorConfigAsync().GetAwaiter().GetResult();
                }
            }
        }
        return _velocityEstimatorConfig ?? throw new InvalidOperationException("VelocityEstimator configuration not loaded");
    }

    public MotorDynamicsConfig GetMotorDynamicsConfig()
    {
        if (!_motorDynamicsConfigLoaded)
        {
            lock (_lockObject)
            {
                if (!_motorDynamicsConfigLoaded)
                {
                    LoadMotorDynamicsConfigAsync().GetAwaiter().GetResult();
                }
            }
        }
        return _motorDynamicsConfig ?? throw new InvalidOperationException("MotorDynamics configuration not loaded");
    }

    public PIDConfig GetMovePidConfig()
    {
        if (!_movePidConfigLoaded)
        {
            lock (_lockObject)
            {
                if (!_movePidConfigLoaded)
                {
                    LoadMovePidConfigAsync().GetAwaiter().GetResult();
                }
            }
        }
        return _movePidConfig ?? throw new InvalidOperationException("MovePid configuration not loaded");
    }

    public PIDConfig GetRotatePidConfig()
    {
        if (!_rotatePidConfigLoaded)
        {
            lock (_lockObject)
            {
                if (!_rotatePidConfigLoaded)
                {
                    LoadRotatePidConfigAsync().GetAwaiter().GetResult();
                }
            }
        }
        return _rotatePidConfig ?? throw new InvalidOperationException("RotatePid configuration not loaded");
    }

    public Navigation.NavigationConfig GetNavigationConfig()
    {
        if (!_navigationConfigLoaded)
        {
            lock (_lockObject)
            {
                if (!_navigationConfigLoaded)
                {
                    LoadNavigationConfigAsync().GetAwaiter().GetResult();
                }
            }
        }
        return _navigationConfig ?? throw new InvalidOperationException("Navigation configuration not loaded");
    }

    public RobotNet10.NavigationTune.Shared.Models.StanleyConfig GetStanleyConig()
    {
        if (!_stanleyConfigLoaded)
        {
            lock (_lockObject)
            {
                if (!_stanleyConfigLoaded)
                {
                    LoadStanleyConfigAsync().GetAwaiter().GetResult();
                }
            }
        }
        return _stanleyConfig ?? throw new InvalidOperationException("Stanley configuration not loaded");
    }

    private async Task LoadPurePursuitConfigAsync()
    {
        try
        {
            var configFile = await _configManager.GetConfigByTypeAsync(PURE_PURSUIT_CONFIG_TYPE);
            
            if (configFile == null)
            {
                _logger.Warning("PurePursuit configuration not found, using defaults");
                _purePursuitConfig = new PurePursuitConfig
                {
                    LookaheadMin = 0.3,
                    Kdd = 1.0,
                    LookaheadMax = 2.0,
                    CurvatureGain = 1.0,
                    WaypointTolerance = 0.1,
                    MaxAngularVelocity = 1.5,
                    ResolutionSplit = 0.05f
                };
            }
            else
            {
                _purePursuitConfig = MapConfigVariablesToObject<PurePursuitConfig>(configFile.Variables);
                _logger.Info("PurePursuit configuration loaded successfully");
            }
        }
        catch (Exception ex)
        {
            _logger.Error($"Error loading PurePursuit configuration, using defaults: {ex.Message}");
            _purePursuitConfig = new PurePursuitConfig
            {
                LookaheadMin = 0.3,
                Kdd = 1.0,
                LookaheadMax = 2.0,
                CurvatureGain = 1.0,
                WaypointTolerance = 0.1,
                MaxAngularVelocity = 1.5,
                ResolutionSplit = 0.05f
            };
        }
        finally
        {
            _purePursuitConfigLoaded = true;
        }
    }

    private async Task LoadVelocitySignalProcessingConfigAsync()
    {
        try
        {
            var configFile = await _configManager.GetConfigByTypeAsync(VELOCITY_SIGNAL_PROCESSING_CONFIG_TYPE);
            
            if (configFile == null)
            {
                _logger.Warning("VelocitySignalProcessing configuration not found, using defaults");
                _velocitySignalProcessingConfig = new VelocitySignalProcessingConfig
                {
                    AlphaFilter = 0.3,
                    NoiseThreshold = 0.5
                };
            }
            else
            {
                _velocitySignalProcessingConfig = MapConfigVariablesToObject<VelocitySignalProcessingConfig>(configFile.Variables);
                _logger.Info("VelocitySignalProcessing configuration loaded successfully");
            }
        }
        catch (Exception ex)
        {
            _logger.Error($"Error loading VelocitySignalProcessing configuration, using defaults: {ex.Message}");
            _velocitySignalProcessingConfig = new VelocitySignalProcessingConfig
            {
                AlphaFilter = 0.3,
                NoiseThreshold = 0.5
            };
        }
        finally
        {
            _velocitySignalProcessingConfigLoaded = true;
        }
    }

    private async Task LoadVelocityEstimatorConfigAsync()
    {
        try
        {
            var configFile = await _configManager.GetConfigByTypeAsync(VELOCITY_ESTIMATOR_CONFIG_TYPE);
            
            if (configFile == null)
            {
                _logger.Warning("VelocityEstimator configuration not found, using defaults");
                _velocityEstimatorConfig = new VelocityEstimatorConfig
                {
                    MinBlendRatio = 0.15f,
                    MaxBlendRatio = 0.8f,
                    DefaultBlendRatio = 0.6,
                    GoodTrackingThreshold = 0.12f,
                    ModerateTrackingThreshold = 0.3,
                    GoodTrackingBlend = 0.7,
                    ModerateTrackingBlend = 0.5,
                    PoorTrackingBlend = 0.25f,
                    ConfidenceDecayRate = 0.95f,
                    MinConfidence = 0.3
                };
            }
            else
            {
                _velocityEstimatorConfig = MapConfigVariablesToObject<VelocityEstimatorConfig>(configFile.Variables);
                _logger.Info("VelocityEstimator configuration loaded successfully");
            }
        }
        catch (Exception ex)
        {
            _logger.Error($"Error loading VelocityEstimator configuration, using defaults: {ex.Message}");
            _velocityEstimatorConfig = new VelocityEstimatorConfig
            {
                MinBlendRatio = 0.15f,
                MaxBlendRatio = 0.8f,
                DefaultBlendRatio = 0.6,
                GoodTrackingThreshold = 0.12f,
                ModerateTrackingThreshold = 0.3,
                GoodTrackingBlend = 0.7,
                ModerateTrackingBlend = 0.5,
                PoorTrackingBlend = 0.25f,
                ConfidenceDecayRate = 0.95f,
                MinConfidence = 0.3
            };
        }
        finally
        {
            _velocityEstimatorConfigLoaded = true;
        }
    }

    private async Task LoadMotorDynamicsConfigAsync()
    {
        try
        {
            var configFile = await _configManager.GetConfigByTypeAsync(MOTOR_DYNAMICS_CONFIG_TYPE);
            
            if (configFile == null)
            {
                _logger.Warning("MotorDynamics configuration not found, using defaults");
                _motorDynamicsConfig = new MotorDynamicsConfig
                {
                    Tau = 0.3,
                    Delta = 0.05f
                };
            }
            else
            {
                _motorDynamicsConfig = MapConfigVariablesToObject<MotorDynamicsConfig>(configFile.Variables);
                _logger.Info("MotorDynamics configuration loaded successfully");
            }
        }
        catch (Exception ex)
        {
            _logger.Error($"Error loading MotorDynamics configuration, using defaults: {ex.Message}");
            _motorDynamicsConfig = new MotorDynamicsConfig
            {
                Tau = 0.3,
                Delta = 0.05f
            };
        }
        finally
        {
            _motorDynamicsConfigLoaded = true;
        }
    }

    private async Task LoadMovePidConfigAsync()
    {
        try
        {
            var configFile = await _configManager.GetConfigByTypeAsync(MOVE_PID_CONFIG_TYPE);
            
            if (configFile == null)
            {
                _logger.Warning("MovePid configuration not found, using defaults");
                _movePidConfig = new PIDConfig
                {
                    Kp = 1.0,
                    Ki = 0.0001,
                    Kd = 0.6
                };
            }
            else
            {
                _movePidConfig = MapConfigVariablesToObject<PIDConfig>(configFile.Variables);
                _logger.Info("MovePid configuration loaded successfully");
            }
        }
        catch (Exception ex)
        {
            _logger.Error($"Error loading MovePid configuration, using defaults: {ex.Message}");
            _movePidConfig = new PIDConfig
            {
                Kp = 1.0,
                Ki = 0.0001,
                Kd = 0.6
            };
        }
        finally
        {
            _movePidConfigLoaded = true;
        }
    }

    private async Task LoadRotatePidConfigAsync()
    {
        try
        {
            var configFile = await _configManager.GetConfigByTypeAsync(ROTATE_PID_CONFIG_TYPE);
            
            if (configFile == null)
            {
                _logger.Warning("RotatePid configuration not found, using defaults");
                _rotatePidConfig = new PIDConfig
                {
                    Kp = 0.4,
                    Ki = 0.0,
                    Kd = 0.05,
                    IntegralZone = 0.087 // ~5° in radians
                };
            }
            else
            {
                _rotatePidConfig = MapConfigVariablesToObject<PIDConfig>(configFile.Variables);
                _logger.Info("RotatePid configuration loaded successfully");
            }
        }
        catch (Exception ex)
        {
            _logger.Error($"Error loading RotatePid configuration, using defaults: {ex.Message}");
            _rotatePidConfig = new PIDConfig
            {
                Kp = 0.4,
                Ki = 0.0,
                Kd = 0.05
            };
        }
        finally
        {
            _rotatePidConfigLoaded = true;
        }
    }

    private async Task LoadNavigationConfigAsync()
    {
        try
        {
            var configFile = await _configManager.GetConfigByTypeAsync(NAVIGATION_CONFIG_TYPE);
            
            if (configFile == null)
            {
                _logger.Warning("Navigation configuration not found, using defaults");
                _navigationConfig = new Navigation.NavigationConfig
                {
                    MaxLinearVelocity = 0.5,
                    MaxAngularVelocity = 0.5,
                    MinLinearVelocity = 0.1,
                    RotateAngularVelocity = 0.5,
                    Acceleration = 0.5,
                    Deceleration = 0.5,
                    HeadingTolerance = 2,
                    ReachedRadius = 0.05,
                    InitialRotationThreshold = 5,
                    DockToRotateSpeed = 0.05,
                    LoadedMaxLinearVelocity = 0.3,
                    LoadedHeadingErrorThresholdDegrees = 10.0,
                };
            }
            else
            {
                _navigationConfig = MapConfigVariablesToObject<Navigation.NavigationConfig>(configFile.Variables);
                _logger.Info("Navigation configuration loaded successfully");
            }
        }
        catch (Exception ex)
        {
            _logger.Error($"Error loading Navigation configuration, using defaults: {ex.Message}");
            _navigationConfig = new Navigation.NavigationConfig
            {
                MaxLinearVelocity = 0.5,
                MaxAngularVelocity = 1.0,
                MinLinearVelocity = 0.1,
                RotateAngularVelocity = 0.5,
                DockToRotateSpeed = 0.05,
                LoadedMaxLinearVelocity = 0.3,
                LoadedHeadingErrorThresholdDegrees = 10.0,
            };
        }
        finally
        {
            _navigationConfigLoaded = true;
        }
    }

    private async Task LoadStanleyConfigAsync()
    {
        try
        {
            var configFile = await _configManager.GetConfigByTypeAsync(STANLEY_CONFIG_TYPE);

            if (configFile == null)
            {
                _logger.Warning("Stanley configuration not found, using defaults");
                _stanleyConfig = new RobotNet10.NavigationTune.Shared.Models.StanleyConfig
                {
                    K = 2.5,
                    Ks = 0.1,
                    WheelBase = 0.6,
                    MaxSteeringAngle = 0.5,
                    EnableCurvatureFeedforward = true,
                    KCurvatureFF = 1.0,
                    GoalTolerance = 0.05,
                    HeadingTolerance = 5.0,
                    GoalApproachDistance = 1.0,
                    GoalGainMultiplier = 2.0,
                    LowSpeedThreshold = 0.3,
                    LowSpeedAngularGain = 1.5,
                    ResolutionSplit = 0.05
                };
            }
            else
            {
                _stanleyConfig = MapConfigVariablesToObject<RobotNet10.NavigationTune.Shared.Models.StanleyConfig>(configFile.Variables);
                _logger.Info("Stanley configuration loaded successfully");
            }
        }
        catch (Exception ex)
        {
            _logger.Error($"Error loading Stanley configuration, using defaults: {ex.Message}");
            _stanleyConfig = new RobotNet10.NavigationTune.Shared.Models.StanleyConfig
            {
                K = 2.5,
                Ks = 0.1,
                WheelBase = 0.6,
                MaxSteeringAngle = 0.5,
                EnableCurvatureFeedforward = true,
                KCurvatureFF = 1.0,
                GoalTolerance = 0.05,
                HeadingTolerance = 5.0,
                GoalApproachDistance = 1.0,
                GoalGainMultiplier = 2.0,
                LowSpeedThreshold = 0.3,
                LowSpeedAngularGain = 1.5,
                ResolutionSplit = 0.05
            };
        }
        finally
        {
            _stanleyConfigLoaded = true;
        }
    }

    private async Task LoadDockToConfigAsync()
    {
        try
        {
            var configFile = await _configManager.GetConfigByTypeAsync(DOCK_TO_CONFIG_TYPE);

            if (configFile == null)
            {
                _logger.Warning("DockTo configuration not found, using defaults");
                _dockToConfig = new DockToConfig
                {
                    K = 2.5,
                    Ks = 0.1,
                    WheelBase = 0.6,
                    MaxSteeringAngle = 0.5,
                    GoalApproachDistance = 1.0,
                    GoalGainMultiplier = 2.0,
                    ReachedRadius = 0.03,
                    ResolutionSplit = 0.05,
                    DockToLength = 3,
                    FinePositioningTimeoutMs = 6000,
                    FinePositioningMaxRetries = 3,
                    MaxGoalPositionShift = 0.5,
                    MaxGoalAngleShiftDegrees = 15.0
                };
            }
            else
            {
                _dockToConfig = MapConfigVariablesToObject<DockToConfig>(configFile.Variables);
                _logger.Info("DockTo configuration loaded successfully");
            }
        }
        catch (Exception ex)
        {
            _logger.Error($"Error loading DockTo configuration, using defaults: {ex.Message}");
            _dockToConfig = new DockToConfig
            {
                K = 2.5,
                Ks = 0.1,
                WheelBase = 0.6,
                MaxSteeringAngle = 0.5,
                GoalApproachDistance = 1.0,
                GoalGainMultiplier = 2.0,
                ReachedRadius = 0.03,
                ResolutionSplit = 0.05,
                DockToLength = 3,
                FinePositioningTimeoutMs = 6000,
                FinePositioningMaxRetries = 3,
                MaxGoalPositionShift = 0.5,
                MaxGoalAngleShiftDegrees = 15.0
            };
        }
        finally
        {
            _dockToConfigLoaded = true;
        }
    }

    private async Task LoadMoveStraightConfigAsync()
    {
        try
        {
            var configFile = await _configManager.GetConfigByTypeAsync(MOVE_STRAIGHT_CONFIG_TYPE);

            if (configFile == null)
            {
                _logger.Warning("MoveStraight configuration not found, using defaults");
                _moveStraightConfig = new DockToConfig
                {
                    DockToDirection = RobotNet10.RobotApp.Shared.Enums.RobotDirection.FORWARD,
                    K = 2.5,
                    Ks = 0.1,
                    WheelBase = 0.6,
                    MaxSteeringAngle = 0.5,
                    GoalApproachDistance = 1.0,
                    GoalGainMultiplier = 2.0,
                    ReachedRadius = 0.05,
                    MaxAngularVelocity = 0.05,
                    ResolutionSplit = 0.05,
                    DecelerationDistance = 5.0,
                    DockingOvershootDetectionRadius = 0.5,
                    DockToLength = 3,
                    FinePositioningTimeoutMs = 6000,
                    FinePositioningMaxRetries = 3,
                };
            }
            else
            {
                _moveStraightConfig = MapConfigVariablesToObject<DockToConfig>(configFile.Variables);
                _moveStraightConfig.DockToDirection = RobotNet10.RobotApp.Shared.Enums.RobotDirection.FORWARD;
                _logger.Info("MoveStraight configuration loaded successfully");
            }
        }
        catch (Exception ex)
        {
            _logger.Error($"Error loading MoveStraight configuration, using defaults: {ex.Message}");
            _moveStraightConfig = new DockToConfig
            {
                DockToDirection = RobotNet10.RobotApp.Shared.Enums.RobotDirection.FORWARD,
                K = 2.5,
                Ks = 0.1,
                WheelBase = 0.6,
                MaxSteeringAngle = 0.5,
                GoalApproachDistance = 1.0,
                GoalGainMultiplier = 2.0,
                ReachedRadius = 0.05,
                MaxAngularVelocity = 0.05,
                ResolutionSplit = 0.05,
                DecelerationDistance = 5.0,
                DockingOvershootDetectionRadius = 0.5,
                DockToLength = 3,
                FinePositioningTimeoutMs = 6000,
                FinePositioningMaxRetries = 3,
            };
        }
        finally
        {
            _moveStraightConfigLoaded = true;
        }
    }

    private async Task LoadLocalPlannerConfigAsync()
    {
        try
        {
            var configFile = await _configManager.GetConfigByTypeAsync(LOCAL_PLANNER_CONFIG_TYPE);

            if (configFile == null)
            {
                _logger.Warning("LocalPlanner configuration not found, using defaults");
                _localPlannerConfig = new LocalPlannerConfig();
            }
            else
            {
                _localPlannerConfig = MapConfigVariablesToObject<LocalPlannerConfig>(configFile.Variables);
                _logger.Info("LocalPlanner configuration loaded successfully");
            }
        }
        catch (Exception ex)
        {
            _logger.Error($"Error loading LocalPlanner configuration, using defaults: {ex.Message}");
            _localPlannerConfig = new LocalPlannerConfig();
        }
        finally
        {
            _localPlannerConfigLoaded = true;
        }
    }

    private void OnConfigChanged(object? sender, ConfigChangedEventArgs e)
    {
        // Reload config if it's one of our config types
        lock (_lockObject)
        {
            switch (e.ConfigType)
            {
                case PURE_PURSUIT_CONFIG_TYPE:
                    _purePursuitConfigLoaded = false;
                    _purePursuitConfig = null;
                    _logger.Info("PurePursuit configuration changed, will reload on next access");
                    break;
                case VELOCITY_SIGNAL_PROCESSING_CONFIG_TYPE:
                    _velocitySignalProcessingConfigLoaded = false;
                    _velocitySignalProcessingConfig = null;
                    _logger.Info("VelocitySignalProcessing configuration changed, will reload on next access");
                    break;
                case VELOCITY_ESTIMATOR_CONFIG_TYPE:
                    _velocityEstimatorConfigLoaded = false;
                    _velocityEstimatorConfig = null;
                    _logger.Info("VelocityEstimator configuration changed, will reload on next access");
                    break;
                case MOTOR_DYNAMICS_CONFIG_TYPE:
                    _motorDynamicsConfigLoaded = false;
                    _motorDynamicsConfig = null;
                    _logger.Info("MotorDynamics configuration changed, will reload on next access");
                    break;
                case MOVE_PID_CONFIG_TYPE:
                    _movePidConfigLoaded = false;
                    _movePidConfig = null;
                    _logger.Info("MovePid configuration changed, will reload on next access");
                    break;
                case ROTATE_PID_CONFIG_TYPE:
                    _rotatePidConfigLoaded = false;
                    _rotatePidConfig = null;
                    _logger.Info("RotatePid configuration changed, will reload on next access");
                    break;
                case NAVIGATION_CONFIG_TYPE:
                    _navigationConfigLoaded = false;
                    _navigationConfig = null;
                    _logger.Info("Navigation configuration changed, will reload on next access");
                    break;
                case STANLEY_CONFIG_TYPE:
                    _stanleyConfigLoaded = false;
                    _stanleyConfig = null;
                    _logger.Info("Stanley configuration changed, will reload on next access");
                    break;
                case DOCK_TO_CONFIG_TYPE:
                    _dockToConfigLoaded = false;
                    _dockToConfig = null;
                    _logger.Info("DockTo configuration changed, will reload on next access");
                    break;
                case MOVE_STRAIGHT_CONFIG_TYPE:
                    _moveStraightConfigLoaded = false;
                    _moveStraightConfig = null;
                    _logger.Info("MoveStraight configuration changed, will reload on next access");
                    break;
                case LOCAL_PLANNER_CONFIG_TYPE:
                    _localPlannerConfigLoaded = false;
                    _localPlannerConfig = null;
                    _logger.Info("LocalPlanner configuration changed, will reload on next access");
                    break;
            }
        }
    }

    private T MapConfigVariablesToObject<T>(List<ConfigVariable> variables) where T : new()
    {
        var obj = new T();
        var properties = typeof(T).GetProperties(BindingFlags.Public | BindingFlags.Instance | BindingFlags.SetProperty);

        foreach (var property in properties)
        {
            // Find variable by exact name match (case-sensitive)
            var variable = variables.FirstOrDefault(v => v.Name == property.Name);
            
            if (variable != null && variable.Value != null)
            {
                try
                {
                    var convertedValue = ConvertValue(variable.Value, property.PropertyType, variable.Type);
                    property.SetValue(obj, convertedValue);
                }
                catch (Exception ex)
                {
                    _logger.Warning($"Failed to set property {property.Name} from variable {variable.Name}: {ex.Message}");
                }
            }
        }

        return obj;
    }

    private object? ConvertValue(object? value, Type targetType, ConfigVariableType variableType)
    {
        if (value == null)
            return targetType.IsValueType ? Activator.CreateInstance(targetType) : null;

        // If value is already of the correct type, return it
        if (targetType.IsInstanceOfType(value))
            return value;

        try
        {
            // Handle nullable types
            var underlyingType = Nullable.GetUnderlyingType(targetType) ?? targetType;

            // Convert based on target type
            if (underlyingType == typeof(string))
            {
                return value.ToString() ?? string.Empty;
            }
            else if (underlyingType == typeof(int))
            {
                if (value is int i) return i;
                if (int.TryParse(value.ToString(), out var parsed)) return parsed;
                if (value is double d) return (int)d;
                throw new InvalidCastException($"Cannot convert {value.GetType()} to int");
            }
            else if (underlyingType == typeof(double))
            {
                if (value is double d) return d;
                if (double.TryParse(value.ToString(), System.Globalization.NumberStyles.Any, System.Globalization.CultureInfo.InvariantCulture, out var parsed)) return parsed;
                if (value is int i) return i;
                throw new InvalidCastException($"Cannot convert {value.GetType()} to double");
            }
            else if (underlyingType == typeof(bool))
            {
                if (value is bool b) return b;
                if (bool.TryParse(value.ToString(), out var parsed)) return parsed;
                // Handle numeric values: 0/1, "0"/"1", etc.
                if (value.ToString()?.Trim() == "0" || value.ToString()?.Trim().ToLower() == "false") return false;
                if (value.ToString()?.Trim() == "1" || value.ToString()?.Trim().ToLower() == "true") return true;
                throw new InvalidCastException($"Cannot convert {value.GetType()} to bool");
            }
            else if (underlyingType.IsEnum)
            {
                var enumValue = Enum.Parse(underlyingType, value.ToString() ?? "", true);
                return enumValue;
            }
            // Handle Dictionary types (e.g., Dictionary<SafetySpeed, double>)
            else if (targetType.IsGenericType && targetType.GetGenericTypeDefinition() == typeof(Dictionary<,>))
            {
                return ConvertToDictionary(value, targetType);
            }
            else
            {
                // Try standard conversion
                return Convert.ChangeType(value, underlyingType);
            }
        }
        catch (Exception ex)
        {
            _logger.Warning($"Error converting value {value} of type {value.GetType()} to {targetType}: {ex.Message}");
            throw;
        }
    }

    /// <summary>
    /// Convert value to Dictionary<TKey, TValue>
    /// Handles conversion from Dictionary<string, object> (JSON) to typed dictionaries
    /// Supports enum keys (e.g., Dictionary<SafetySpeed, double>)
    /// </summary>
    private object ConvertToDictionary(object value, Type targetDictionaryType)
    {
        // Get key and value types from Dictionary<TKey, TValue>
        var genericArgs = targetDictionaryType.GetGenericArguments();
        var keyType = genericArgs[0];
        var valueType = genericArgs[1];

        // Create dictionary instance
        var dictionaryType = typeof(Dictionary<,>).MakeGenericType(keyType, valueType);
        var dictionary = Activator.CreateInstance(dictionaryType);
        var addMethod = dictionaryType.GetMethod("Add") ?? throw new InvalidOperationException("Cannot find Add method on Dictionary");

        // Handle source Dictionary<string, object> from JSON
        if (value is Dictionary<string, object> sourceDict)
        {
            foreach (var kvp in sourceDict)
            {
                // Convert key (usually from string to enum)
                object convertedKey;
                if (keyType.IsEnum)
                {
                    // Parse enum from string
                    convertedKey = Enum.Parse(keyType, kvp.Key, ignoreCase: true);
                }
                else if (keyType == typeof(string))
                {
                    convertedKey = kvp.Key;
                }
                else
                {
                    convertedKey = Convert.ChangeType(kvp.Key, keyType);
                }

                // Convert value
                object? convertedValue = ConvertDictionaryValue(kvp.Value, valueType);

                // Add to dictionary
                addMethod.Invoke(dictionary, [convertedKey, convertedValue]);
            }

            return dictionary ?? new Dictionary<object, object>();
        }
        // Handle JsonElement (alternative JSON representation)
        else if (value is System.Text.Json.JsonElement jsonElement && jsonElement.ValueKind == System.Text.Json.JsonValueKind.Object)
        {
            foreach (var property in jsonElement.EnumerateObject())
            {
                // Convert key
                object convertedKey;
                if (keyType.IsEnum)
                {
                    convertedKey = Enum.Parse(keyType, property.Name, ignoreCase: true);
                }
                else if (keyType == typeof(string))
                {
                    convertedKey = property.Name;
                }
                else
                {
                    convertedKey = Convert.ChangeType(property.Name, keyType);
                }

                // Convert value from JsonElement
                object? convertedValue = ConvertJsonElementValue(property.Value, valueType);

                // Add to dictionary
                addMethod.Invoke(dictionary, [convertedKey, convertedValue]);
            }

            return dictionary ?? new Dictionary<object, object>();
        }
        else
        {
            throw new InvalidCastException($"Cannot convert {value.GetType()} to {targetDictionaryType}");
        }
    }

    /// <summary>
    /// Convert dictionary value to target type
    /// </summary>
    private static object? ConvertDictionaryValue(object? value, Type valueType)
    {
        if (value == null)
            return valueType.IsValueType ? Activator.CreateInstance(valueType) : null;

        if (valueType == typeof(double))
        {
            if (value is double d) return d;
            if (double.TryParse(value.ToString(), out var parsed)) return parsed;
            if (value is int i) return (double)i;
            return Convert.ChangeType(value, valueType);
        }
        else if (valueType == typeof(int))
        {
            if (value is int i) return i;
            if (int.TryParse(value.ToString(), out var parsed)) return parsed;
            if (value is double d) return (int)d;
            return Convert.ChangeType(value, valueType);
        }
        else if (valueType == typeof(string))
        {
            return value.ToString() ?? string.Empty;
        }
        else if (valueType == typeof(bool))
        {
            if (value is bool b) return b;
            if (bool.TryParse(value.ToString(), out var parsed)) return parsed;
            return Convert.ChangeType(value, valueType);
        }
        else if (valueType.IsEnum)
        {
            return Enum.Parse(valueType, value.ToString() ?? "", ignoreCase: true);
        }
        else
        {
            return Convert.ChangeType(value, valueType);
        }
    }

    /// <summary>
    /// Convert JsonElement value to target type
    /// </summary>
    private static object? ConvertJsonElementValue(System.Text.Json.JsonElement element, Type valueType)
    {
        if (element.ValueKind == System.Text.Json.JsonValueKind.Null)
            return valueType.IsValueType ? Activator.CreateInstance(valueType) : null;

        if (valueType == typeof(double))
        {
            return element.GetDouble();
        }
        else if (valueType == typeof(int))
        {
            return element.GetInt32();
        }
        else if (valueType == typeof(string))
        {
            return element.GetString();
        }
        else if (valueType == typeof(bool))
        {
            return element.GetBoolean();
        }
        else if (valueType.IsEnum)
        {
            return Enum.Parse(valueType, element.GetString() ?? "", ignoreCase: true);
        }
        else
        {
            var rawValue = element.GetRawText();
            return System.Text.Json.JsonSerializer.Deserialize(rawValue, valueType);
        }
    }

    public DockToConfig GetDockToConfig()
    {
        if (!_dockToConfigLoaded)
        {
            lock (_lockObject)
            {
                if (!_dockToConfigLoaded)
                {
                    LoadDockToConfigAsync().GetAwaiter().GetResult();
                }
            }
        }
        return _dockToConfig ?? throw new InvalidOperationException("DockTo configuration not loaded");
    }

    public DockToConfig GetMoveStraightConfig()
    {
        if (!_moveStraightConfigLoaded)
        {
            lock (_lockObject)
            {
                if (!_moveStraightConfigLoaded)
                {
                    LoadMoveStraightConfigAsync().GetAwaiter().GetResult();
                }
            }
        }
        return _moveStraightConfig ?? throw new InvalidOperationException("MoveStraight configuration not loaded");
    }

    public LocalPlannerConfig GetLocalPlannerConfig()
    {
        if (!_localPlannerConfigLoaded)
        {
            lock (_lockObject)
            {
                if (!_localPlannerConfigLoaded)
                {
                    LoadLocalPlannerConfigAsync().GetAwaiter().GetResult();
                }
            }
        }
        return _localPlannerConfig ?? throw new InvalidOperationException("LocalPlanner configuration not loaded");
    }
}