Denso/srcs/RobotNet10/Commons/RobotNet10.NavigationTune/Services/ParameterManager.cs

using Microsoft.EntityFrameworkCore;
using RobotNet10.NavigationTune.Data;
using RobotNet10.NavigationTune.Shared.Interfaces;
using RobotNet10.NavigationTune.Shared.Models;

namespace RobotNet10.NavigationTune.Services;

/// <summary>
/// Parameter manager implementation
/// </summary>
public class ParameterManager(TuningDbContext context) : IParameterManager
{
    private readonly TuningDbContext _context = context;

    public async Task<NavigationParameterSet?> GetByNameAsync(string name)
    {
        return await _context.ParameterSets
            .FirstOrDefaultAsync(p => p.Name == name);
    }

    public async Task<NavigationParameterSet?> GetByIdAsync(Guid id)
    {
        return await _context.ParameterSets.FindAsync(id);
    }

    public async Task<List<NavigationParameterSet>> GetAllAsync()
    {
        return await _context.ParameterSets
            .OrderByDescending(p => p.CreatedAt)
            .ToListAsync();
    }

    public async Task<Guid> SaveAsync(NavigationParameterSet parameterSet)
    {
        if (parameterSet.Id == Guid.Empty)
            parameterSet.Id = Guid.NewGuid();

        parameterSet.CreatedAt = DateTime.UtcNow;
        _context.ParameterSets.Add(parameterSet);
        await _context.SaveChangesAsync();
        return parameterSet.Id;
    }

    public async Task UpdateAsync(NavigationParameterSet parameterSet)
    {
        parameterSet.UpdatedAt = DateTime.UtcNow;
        _context.ParameterSets.Update(parameterSet);
        await _context.SaveChangesAsync();
    }

    public async Task DeleteAsync(Guid id)
    {
        var parameterSet = await GetByIdAsync(id);
        if (parameterSet != null)
        {
            _context.ParameterSets.Remove(parameterSet);
            await _context.SaveChangesAsync();
        }
    }

    public ValidationResult Validate(NavigationParameterSet parameterSet)
    {
        var result = new ValidationResult { IsValid = true };

        // Validate PID bounds
        if (parameterSet.MovePidConfig.Kp < 0.1 || parameterSet.MovePidConfig.Kp > 5.0)
            result.AddError($"Move PID Kp must be between 0.1 and 5.0, got {parameterSet.MovePidConfig.Kp}");

        if (parameterSet.MovePidConfig.Ki < 0 || parameterSet.MovePidConfig.Ki > 2.0)
            result.AddError($"Move PID Ki must be between 0 and 2.0, got {parameterSet.MovePidConfig.Ki}");

        if (parameterSet.MovePidConfig.Kd < 0 || parameterSet.MovePidConfig.Kd > 1.0)
            result.AddError($"Move PID Kd must be between 0 and 1.0, got {parameterSet.MovePidConfig.Kd}");

        // Validate Pure Pursuit
        if (parameterSet.PurePursuitConfig.LookaheadMax <= parameterSet.PurePursuitConfig.LookaheadMin)
            result.AddError("LookaheadMax must be greater than LookaheadMin");

        if (parameterSet.PurePursuitConfig.Kdd < 0.3 || parameterSet.PurePursuitConfig.Kdd > 2.0)
            result.AddError($"Kdd must be between 0.3 and 2.0, got {parameterSet.PurePursuitConfig.Kdd}");

        // Validate velocity limits
        if (parameterSet.NavigationConfig.MaxLinearVelocity > 2.0)
            result.AddWarning("MaxLinearVelocity > 2.0 m/s may be unsafe");

        // Validate blend ratios
        if (parameterSet.EstimatorConfig.GoodTrackingBlend < parameterSet.EstimatorConfig.PoorTrackingBlend)
            result.AddError("GoodTrackingBlend must be greater than PoorTrackingBlend (trust encoder more when tracking is good)");

        return result;
    }

    public NavigationParameterSet GetDefaultPreset()
    {
        return new NavigationParameterSet
        {
            Name = "Default",
            Description = "Default parameter set",
            IsDefault = true,
            ControllerType = PathFollowingController.PurePursuit, // Default to Pure Pursuit
            MovePidConfig = new PIDConfig { Kp = 1.0, Ki = 0.0001, Kd = 0.6 },
            RotatePidConfig = new PIDConfig { Kp = 10.0, Ki = 0.01, Kd = 0.1 },
            PurePursuitConfig = new PurePursuitConfig
            {
                LookaheadMin = 0.3,
                Kdd = 1.0,
                LookaheadMax = 2.0,
                MaxAngularVelocity = 1.5,
                ResolutionSplit = 0.05f,
                FinalApproachThreshold = 0.2,
                HeadingTolerance = 3.0,
                GoalRegionDistance = 1.5,
                KCurvature = 2.0,
                MinLookaheadTimeRatio = 0.3,
                MaxLookaheadTimeRatio = 2.0
            },
            StanleyConfig = new StanleyConfig
            {
                K = 2.5,
                Ks = 0.1,
                WheelBase = 0.5,
                MaxSteeringAngle = 0.5,
                EnableCurvatureFeedforward = true,
                KCurvatureFF = 1.0,
                GoalTolerance = 0.05,
                HeadingTolerance = 5.0,
                ResolutionSplit = 0.05,
                GoalApproachDistance = 1.0,
                GoalGainMultiplier = 2.0,
                LowSpeedThreshold = 0.3,
                LowSpeedAngularGain = 1.5
            },
            EstimatorConfig = new VelocityEstimatorConfig(),
            SignalConfig = new VelocitySignalProcessingConfig(),
            MotorDynamicsConfig = new MotorDynamicsConfig { Tau = 0.3, Delta = 0.05f },
            NavigationConfig = new NavigationConfig
            {
                MaxLinearVelocity = 1.5,
                MaxAngularVelocity = 6.0,
                MinLinearVelocity = 0.1,
                RotateAngularVelocity = 1.0,
                ReachedRadius = 0.015,
                InitialRotationThreshold = 5.0,
                Acceleration = 0.5,
                Deceleration = 0.5
            }
        };
    }

    public NavigationParameterSet GetAggressivePreset()
    {
        var preset = GetDefaultPreset();
        preset.Name = "Aggressive";
        preset.Description = "Aggressive tuning for fast response";
        preset.MovePidConfig.Kp = 1.5;
        preset.MovePidConfig.Ki = 0.2;
        preset.MovePidConfig.Kd = 0.02;
        preset.PurePursuitConfig.Kdd = 0.8f;
        return preset;
    }

    public NavigationParameterSet GetSmoothPreset()
    {
        var preset = GetDefaultPreset();
        preset.Name = "Smooth";
        preset.Description = "Smooth tuning for gentle motion";
        preset.MovePidConfig.Kp = 0.6;
        preset.MovePidConfig.Ki = 0.05;
        preset.MovePidConfig.Kd = 0.3;
        preset.PurePursuitConfig.Kdd = 1.5;
        preset.SignalConfig.AlphaFilter = 0.2;
        return preset;
    }

    public NavigationParameterSet GetStanleyPreset()
    {
        var preset = GetDefaultPreset();
        preset.Name = "Stanley";
        preset.Description = "Stanley controller for high-speed path tracking";
        preset.ControllerType = PathFollowingController.Stanley;

        // Stanley-specific tuning
        preset.StanleyConfig.K = 2.5;
        preset.StanleyConfig.Ks = 0.1;
        preset.StanleyConfig.WheelBase = 0.6;
        preset.StanleyConfig.MaxSteeringAngle = 0.5;
        preset.StanleyConfig.EnableCurvatureFeedforward = true;
        preset.StanleyConfig.KCurvatureFF = 1.0;
        preset.StanleyConfig.GoalTolerance = 0.05;
        preset.StanleyConfig.HeadingTolerance = 5.0;
        preset.StanleyConfig.GoalApproachDistance = 1.0;
        preset.StanleyConfig.GoalGainMultiplier = 2.0;
        preset.StanleyConfig.LowSpeedThreshold = 0.3;
        preset.StanleyConfig.LowSpeedAngularGain = 1.5;

        return preset;
    }
}