using Flyshot.Core.Config;
using Flyshot.Core.Domain;
using Flyshot.Core.Planning;
using Flyshot.Core.Planning.Sampling;
using Flyshot.Core.Triggering;
using Microsoft.Extensions.Logging;

namespace Flyshot.ControllerClientCompat;

/// <summary>
/// 负责把 ControllerClient 兼容层的轨迹输入转换为规划结果和触发时间轴。
/// </summary>
public sealed class ControllerClientTrajectoryOrchestrator
{
    private readonly ICspPlanner _icspPlanner;
    private readonly SelfAdaptIcspPlanner _selfAdaptIcspPlanner;
    private readonly ShotTimelineBuilder _shotTimelineBuilder = new(new WaypointTimestampResolver());
    private readonly Dictionary<string, PlannedExecutionBundle> _flyshotCache = new(StringComparer.Ordinal);
    private readonly ILogger<ControllerClientTrajectoryOrchestrator>? _logger;

    /// <summary>
    /// 初始化轨迹编排器。
    /// </summary>
    /// <param name="logger">日志记录器；允许 null。</param>
    public ControllerClientTrajectoryOrchestrator(ILogger<ControllerClientTrajectoryOrchestrator>? logger = null)
    {
        _logger = logger;
        _icspPlanner = new(logger: null);
        _selfAdaptIcspPlanner = new(logger: null);
    }

    /// <summary>
    /// 对普通轨迹执行 ICSP 规划。
    /// </summary>
    /// <param name="robot">当前机器人配置。</param>
    /// <param name="waypoints">普通轨迹关节路点。</param>
    /// <returns>包含规划轨迹、空触发时间轴和执行结果的结果包。</returns>
    public PlannedExecutionBundle PlanOrdinaryTrajectory(
        RobotProfile robot,
        IReadOnlyList<IReadOnlyList<double>> waypoints,
        TrajectoryExecutionOptions? options = null)
    {
        ArgumentNullException.ThrowIfNull(robot);
        ArgumentNullException.ThrowIfNull(waypoints);
        options ??= new TrajectoryExecutionOptions();

        _logger?.LogInformation(
            "PlanOrdinaryTrajectory 开始: 路点数={WaypointCount}, method={Method}",
            waypoints.Count, options.Method);

        var program = CreateProgram(
            name: "ordinary-trajectory",
            waypoints: waypoints,
            shotFlags: Enumerable.Repeat(false, waypoints.Count),
            offsetValues: Enumerable.Repeat(0, waypoints.Count),
            addressGroups: Enumerable.Range(0, waypoints.Count).Select(static _ => Array.Empty<int>()));

        var method = ParseOrdinaryMethod(options.Method);
        var request = new TrajectoryRequest(
            robot: robot,
            program: program,
            method: method,
            saveTrajectoryArtifacts: options.SaveTrajectory);

        var plannedTrajectory = PlanByMethod(request, method);
        var shotTimeline = new ShotTimeline(Array.Empty<ShotEvent>(), Array.Empty<TrajectoryDoEvent>());
        var result = CreateResult(plannedTrajectory, shotTimeline, usedCache: false);

        _logger?.LogInformation(
            "PlanOrdinaryTrajectory 完成: 时长={Duration}s, 采样点数={SampleCount}",
            result.Duration.TotalSeconds,
            result.DenseJointTrajectory?.Count ?? 0);

        return new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result);
    }

    /// <summary>
    /// 对已经上传的飞拍轨迹执行自适应 ICSP 规划并生成触发时间轴。
    /// </summary>
    /// <param name="robot">当前机器人配置。</param>
    /// <param name="uploaded">兼容层保存的上传轨迹。</param>
    /// <returns>包含规划轨迹、触发时间轴和执行结果的结果包。</returns>
    public PlannedExecutionBundle PlanUploadedFlyshot(
        RobotProfile robot,
        ControllerClientCompatUploadedTrajectory uploaded,
        FlyshotExecutionOptions? options = null,
        CompatibilityRobotSettings? settings = null)
    {
        ArgumentNullException.ThrowIfNull(robot);
        ArgumentNullException.ThrowIfNull(uploaded);
        options ??= new FlyshotExecutionOptions();
        settings ??= CreateDefaultRobotSettings();

        _logger?.LogInformation(
            "PlanUploadedFlyshot 开始: name={Name}, waypoints={WaypointCount}, method={Method}, useCache={UseCache}",
            uploaded.Name, uploaded.Waypoints.Count, options.Method, options.UseCache);

        var program = CreateProgram(
            name: uploaded.Name,
            waypoints: uploaded.Waypoints,
            shotFlags: uploaded.ShotFlags,
            offsetValues: uploaded.OffsetValues,
            addressGroups: uploaded.AddressGroups);

        var method = ParseFlyshotMethod(options.Method);
        var cacheKey = CreateFlyshotCacheKey(robot, uploaded, options, settings);
        if (options.UseCache && _flyshotCache.TryGetValue(cacheKey, out var cachedBundle))
        {
            _logger?.LogInformation("PlanUploadedFlyshot 命中缓存: name={Name}, cacheKey={CacheKey}", uploaded.Name, cacheKey);
            // 命中缓存时只替换 TrajectoryResult 的 usedCache 标志，规划轨迹和触发时间轴保持不可变复用。
            return new PlannedExecutionBundle(
                cachedBundle.PlannedTrajectory,
                cachedBundle.ShotTimeline,
                CreateResult(cachedBundle.PlannedTrajectory, cachedBundle.ShotTimeline, usedCache: true));
        }

        var request = new TrajectoryRequest(
            robot: robot,
            program: program,
            method: method,
            moveToStart: options.MoveToStart,
            saveTrajectoryArtifacts: options.SaveTrajectory,
            useCache: options.UseCache);

        var plannedTrajectory = PlanByMethod(request, method, settings);
        var shotTimeline = _shotTimelineBuilder.Build(
            plannedTrajectory,
            holdCycles: settings.IoKeepCycles,
            samplePeriod: robot.ServoPeriod,
            useDo: settings.UseDo);
        var result = CreateResult(plannedTrajectory, shotTimeline, usedCache: false);
        var bundle = new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result);

        _logger?.LogInformation(
            "PlanUploadedFlyshot 完成: name={Name}, 时长={Duration}s, 触发事件数={TriggerCount}, 采样点数={SampleCount}",
            uploaded.Name, result.Duration.TotalSeconds, result.TriggerTimeline.Count, result.DenseJointTrajectory?.Count ?? 0);

        if (options.UseCache)
        {
            _flyshotCache[cacheKey] = bundle;
        }

        return bundle;
    }

    /// <summary>
    /// 按普通轨迹执行接口约束解析 method 参数。
    /// </summary>
    /// <param name="method">旧 SDK 传入的方法名。</param>
    /// <returns>领域层规划方法。</returns>
    private static PlanningMethod ParseOrdinaryMethod(string method)
    {
        var normalized = NormalizeMethod(method);
        return normalized switch
        {
            "icsp" => PlanningMethod.Icsp,
            "doubles" => PlanningMethod.Doubles,
            _ => throw new ArgumentException($"Unsupported ExecuteTrajectory method: {method}", nameof(method))
        };
    }

    /// <summary>
    /// 按飞拍轨迹执行接口约束解析 method 参数。
    /// </summary>
    /// <param name="method">旧 SDK 传入的方法名。</param>
    /// <returns>领域层规划方法。</returns>
    private static PlanningMethod ParseFlyshotMethod(string method)
    {
        var normalized = NormalizeMethod(method);
        return normalized switch
        {
            "icsp" => PlanningMethod.Icsp,
            "self-adapt-icsp" => PlanningMethod.SelfAdaptIcsp,
            "doubles" => PlanningMethod.Doubles,
            _ => throw new ArgumentException($"Unsupported ExecuteFlyShotTraj method: {method}", nameof(method))
        };
    }

    /// <summary>
    /// 按领域枚举分派到当前已经落地的规划器。
    /// </summary>
    /// <param name="request">规划请求。</param>
    /// <param name="method">规划方法。</param>
    /// <returns>规划轨迹。</returns>
    private PlannedTrajectory PlanByMethod(TrajectoryRequest request, PlanningMethod method, CompatibilityRobotSettings? settings = null)
    {
        return method switch
        {
            PlanningMethod.Icsp => _icspPlanner.Plan(request),
            PlanningMethod.SelfAdaptIcsp => _selfAdaptIcspPlanner.Plan(request, settings?.AdaptIcspTryNum ?? 5),
            PlanningMethod.Doubles => throw new NotSupportedException("doubles 轨迹规划尚未落地。"),
            _ => throw new ArgumentOutOfRangeException(nameof(method), method, "未知轨迹规划方法。")
        };
    }

    /// <summary>
    /// 归一化旧 SDK 的 method 字符串。
    /// </summary>
    /// <param name="method">原始方法名。</param>
    /// <returns>小写短横线方法名。</returns>
    private static string NormalizeMethod(string method)
    {
        if (string.IsNullOrWhiteSpace(method))
        {
            return "icsp";
        }

        return method.Trim().ToLowerInvariant();
    }

    /// <summary>
    /// 为已上传飞拍轨迹构造包含参数和轨迹内容的缓存键，避免同名覆盖后误用旧规划结果。
    /// </summary>
    /// <param name="robot">机器人配置。</param>
    /// <param name="uploaded">上传轨迹。</param>
    /// <param name="options">执行参数。</param>
    /// <returns>缓存键。</returns>
    private static string CreateFlyshotCacheKey(
        RobotProfile robot,
        ControllerClientCompatUploadedTrajectory uploaded,
        FlyshotExecutionOptions options,
        CompatibilityRobotSettings settings)
    {
        var hash = new HashCode();
        hash.Add(robot.Name, StringComparer.Ordinal);
        hash.Add(uploaded.Name, StringComparer.Ordinal);
        hash.Add(NormalizeMethod(options.Method), StringComparer.Ordinal);
        hash.Add(options.MoveToStart);
        hash.Add(options.SaveTrajectory);
        hash.Add(settings.UseDo);
        hash.Add(settings.IoKeepCycles);
        hash.Add(settings.AdaptIcspTryNum);

        foreach (var waypoint in uploaded.Waypoints)
        {
            foreach (var value in waypoint)
            {
                hash.Add(value);
            }
        }

        foreach (var flag in uploaded.ShotFlags)
        {
            hash.Add(flag);
        }

        foreach (var offset in uploaded.OffsetValues)
        {
            hash.Add(offset);
        }

        foreach (var group in uploaded.AddressGroups)
        {
            foreach (var address in group)
            {
                hash.Add(address);
            }
        }

        return hash.ToHashCode().ToString("X8");
    }

    /// <summary>
    /// 构造编排器直接调用时的默认兼容配置，保持既有单元测试中的 DO 生成行为。
    /// </summary>
    /// <returns>默认机器人兼容配置。</returns>
    private static CompatibilityRobotSettings CreateDefaultRobotSettings()
    {
        return new CompatibilityRobotSettings(
            useDo: true,
            ioAddresses: Array.Empty<int>(),
            ioKeepCycles: 0,
            accLimitScale: 1.0,
            jerkLimitScale: 1.0,
            adaptIcspTryNum: 5);
    }

    /// <summary>
    /// 把兼容层输入数组转换成领域层 FlyshotProgram。
    /// </summary>
    /// <param name="name">轨迹名称。</param>
    /// <param name="waypoints">关节路点。</param>
    /// <param name="shotFlags">拍照标志。</param>
    /// <param name="offsetValues">偏移周期。</param>
    /// <param name="addressGroups">IO 地址组。</param>
    /// <returns>领域层飞拍程序。</returns>
    private static FlyshotProgram CreateProgram(
        string name,
        IEnumerable<IReadOnlyList<double>> waypoints,
        IEnumerable<bool> shotFlags,
        IEnumerable<int> offsetValues,
        IEnumerable<IReadOnlyList<int>> addressGroups)
    {
        return new FlyshotProgram(
            name: name,
            waypoints: waypoints.Select(static waypoint => new JointWaypoint(waypoint)).ToArray(),
            shotFlags: shotFlags.ToArray(),
            offsetValues: offsetValues.ToArray(),
            addressGroups: addressGroups.Select(static group => new IoAddressGroup(group)).ToArray());
    }

    /// <summary>
    /// 从规划轨迹和触发时间轴构造运行时可消费的稳定结果对象。
    /// </summary>
    /// <param name="plannedTrajectory">规划后的轨迹。</param>
    /// <param name="shotTimeline">触发时间轴。</param>
    /// <returns>运行时执行结果描述。</returns>
    private static TrajectoryResult CreateResult(PlannedTrajectory plannedTrajectory, ShotTimeline shotTimeline, bool usedCache)
    {
        var denseJointTrajectory = TrajectorySampler.SampleJointTrajectory(
            plannedTrajectory,
            samplePeriod: plannedTrajectory.Robot.ServoPeriod.TotalSeconds);

        return new TrajectoryResult(
            programName: plannedTrajectory.OriginalProgram.Name,
            method: plannedTrajectory.Method,
            isValid: true,
            duration: TimeSpan.FromSeconds(plannedTrajectory.WaypointTimes[^1]),
            shotEvents: shotTimeline.ShotEvents,
            triggerTimeline: shotTimeline.TriggerTimeline,
            artifacts: Array.Empty<TrajectoryArtifact>(),
            failureReason: null,
            usedCache: usedCache,
            originalWaypointCount: plannedTrajectory.OriginalWaypointCount,
            plannedWaypointCount: plannedTrajectory.PlannedWaypointCount,
            denseJointTrajectory: denseJointTrajectory);
    }
}
