using ClumsyCore;
using ClumsyCore.DTools;
using ClumsyCore.Interfaces;
using ClumsyCore.Pilot;
using CommonUsage.Chassis;
using MyParking.Shared;
using System;
using System.Collections.Generic;
using System.Numerics;

namespace MultiWheelC
{
    // C层单车测试：在可配置的运动坐标系中统一跟踪直线、圆弧或S型曲线。
    public sealed class CrabMotionFrameTracker : MovementDefinition
    {
        public enum ReferencePathKind
        {
            Straight = 0,
            LeftArc = 1,
            SCurve = 2
        }

        public enum ChassisCommandBackend
        {
            SendXYThSpeed = 0,
            SendMotion = 1
        }

        public ReferencePathKind PathKind;
        public ChassisCommandBackend CommandBackend =
            ChassisCommandBackend.SendMotion;
        public Vector2 StartPosition;
        public double InitialBodyYawRadians;
        public float LengthMillimeters = 4000f;
        public float RadiusMillimeters = 2000f;
        public float SCurveLateralOffsetMillimeters = 400f;
        public double ArcSweepRadians = Math.PI / 2.0;
        public float CruiseSpeed = 0.2f;
        public float SlowDistanceMillimeters = 600f;
        public float FinishDistanceMillimeters = 30f;
        public float MinimumSpeed = 0.04f;
        public double LateralGainPerSecond = 0.8;
        public double MaximumLateralCorrection = 0.12;
        public double HeadingGainPerSecond = 1.5;
        public double MaximumAngularSpeedRadiansPerSecond =
            AngleMath.DegreesToRadians(30.0);
        public double MaximumVirtualSteeringRadians =
            AngleMath.DegreesToRadians(30.0);
        public float WheelAlignmentToleranceDegrees = 2f;
        public float WheelAlignmentStableSeconds = 0.3f;
        public float WheelAlignmentTimeoutSeconds = 10f;
        public float TrackingTimeoutSeconds = 60f;
        public Action<float, float, float> CommandObserver;

        // 运动坐标系相对车体坐标系的朝向：普通模式为0，蟹行为π/2。
        public double MotionFrameYawInBodyRadians = Math.PI / 2.0;
        private double _lastSCurveProgress;

        public override IEnumerable<bool> Get()
        {
            ValidateParameters();

            var chassis =
                PilotDefinition.Chassis as MultiWheelChassis;
            if (chassis == null)
                throw new InvalidOperationException(
                    "当前底盘不是MultiWheelChassis，无法执行运动坐标系轨迹测试。");

            var adapter = new MultiWheelChassisAdapter(
                chassis,
                PilotDefinition.Self.CarNum);
            adapter.ResetToBodyFrame();

            var lastCommandTime = DateTime.Now;

            try
            {
                // 模式切换阶段只转舵轮，驱动速度始终保持为零。
                var alignmentStarted = DateTime.Now;
                DateTime? stableSince = null;
                while (true)
                {
                    if (!adapter.PrepareParallelDirection(
                            MotionFrameYawInBodyRadians))
                        throw new InvalidOperationException(
                            "无法生成运动坐标系对应的舵轮准备姿态。");

                    var aligned =
                        adapter.AreParallelWheelsAligned(
                            MotionFrameYawInBodyRadians,
                            AngleMath.DegreesToRadians(
                                WheelAlignmentToleranceDegrees));

                    if (aligned)
                    {
                        if (stableSince == null)
                            stableSince = DateTime.Now;

                        if ((DateTime.Now - stableSince.Value)
                                .TotalSeconds >=
                            WheelAlignmentStableSeconds)
                            break;
                    }
                    else
                    {
                        stableSince = null;
                    }

                    if ((DateTime.Now - alignmentStarted)
                            .TotalSeconds >
                        WheelAlignmentTimeoutSeconds)
                        throw new TimeoutException(
                            "舵轮在限定时间内未稳定到达运动坐标系初始方向。");

                    yield return true;
                }

                if (CommandBackend ==
                    ChassisCommandBackend.SendMotion)
                {
                    // 舵轮已按真实机械角度完成预对齐；
                    // 现在由Shared适配层激活SendMotion虚拟运动坐标系。
                    adapter.ActivateMotionFrame(
                        MotionFrameYawInBodyRadians);
                }

                var trackingStarted = DateTime.Now;
                while (true)
                {
                    if ((DateTime.Now - trackingStarted)
                            .TotalSeconds >
                        TrackingTimeoutSeconds)
                        throw new TimeoutException(
                            "蟹行轨迹在限定时间内未完成。");

                    var location =
                        DetourInterface.getCartLocation();
                    if (!IsFinite(location.x) ||
                        !IsFinite(location.y) ||
                        !IsFinite(location.th))
                        throw new InvalidOperationException(
                            "蟹行轨迹测试期间Detour位姿无效。");

                    var currentPosition = new Vector2(
                        (float)location.x,
                        (float)location.y);
                    var currentBodyYaw =
                        AngleMath.DegreesToRadians(location.th);

                    CalculateReference(
                        currentPosition,
                        out var tangentYaw,
                        out var referencePoint,
                        out var remainingMillimeters,
                        out var referenceCurvature);

                    if (remainingMillimeters <=
                        FinishDistanceMillimeters)
                        break;

                    var speed =
                        CalculateSpeed(remainingMillimeters);
                    var tangent = new Vector2(
                        (float)Math.Cos(tangentYaw),
                        (float)Math.Sin(tangentYaw));
                    var leftNormal = new Vector2(
                        -tangent.Y,
                        tangent.X);
                    var positionError =
                        currentPosition - referencePoint;
                    var lateralErrorMeters =
                        Vector2.Dot(
                            positionError,
                            leftNormal) / 1000.0;
                    var normalCorrection =
                        Limit(
                            -LateralGainPerSecond *
                            lateralErrorMeters,
                            MaximumLateralCorrection);

                    // 先在世界坐标中组合切向速度与横向纠偏速度。
                    var worldVx =
                        tangent.X * speed +
                        leftNormal.X * (float)normalCorrection;
                    var worldVy =
                        tangent.Y * speed +
                        leftNormal.Y * (float)normalCorrection;

                    // 将世界速度表达为当前蟹行运动坐标系速度。
                    var motionYaw =
                        currentBodyYaw +
                        MotionFrameYawInBodyRadians;
                    var motionCos = Math.Cos(motionYaw);
                    var motionSin = Math.Sin(motionYaw);
                    var vxInMotion =
                        motionCos * worldVx +
                        motionSin * worldVy;
                    var vyInMotion =
                        -motionSin * worldVx +
                        motionCos * worldVy;

                    var desiredBodyYaw =
                        tangentYaw -
                        MotionFrameYawInBodyRadians;
                    var headingError =
                        AngleMath.ShortestDifferenceRadians(
                            desiredBodyYaw,
                            currentBodyYaw);
                    var omega =
                        speed * referenceCurvature +
                        HeadingGainPerSecond * headingError;
                    omega = Limit(
                        omega,
                        MaximumAngularSpeedRadiansPerSecond);

                    var now = DateTime.Now;
                    var interval = now - lastCommandTime;
                    lastCommandTime = now;

                    bool commandAccepted;
                    Twist2D bodyTwist;
                    if (CommandBackend ==
                        ChassisCommandBackend.SendMotion)
                    {
                        // 运动坐标系相对车体系旋转+90°：
                        // 运动系正向速度会转换成车体系+Y速度。
                        bodyTwist =
                            FrameTransform2D
                                .TransformTwistAtSamePoint(
                                    new Pose2D(
                                        0.0,
                                        0.0,
                                        MotionFrameYawInBodyRadians),
                                    new Twist2D(
                                        vxInMotion,
                                        vyInMotion,
                                        omega));

                        // 将运动坐标系原点和前后几何控制点处的速度，
                        // 转换为SendMotion需要的前后轴方向。
                        var controlPointRadiusMeters =
                            Math.Max(
                                chassis.ControlPointRadius /
                                1000.0,
                                0.001);
                        var frontVelocityY =
                            vyInMotion +
                            omega *
                            controlPointRadiusMeters;
                        var rearVelocityY =
                            vyInMotion -
                            omega *
                            controlPointRadiusMeters;
                        var frontSteeringRadians =
                            Math.Atan2(
                                frontVelocityY,
                                vxInMotion);
                        var rearSteeringRadians =
                            Math.Atan2(
                                rearVelocityY,
                                vxInMotion);

                        // 蟹行测试绕过M层ManualControl并直接调用SendMotion，
                        // 因此需要在C层同步应用蟹行虚拟几何比例和转向符号。
                        if (IsCrabMotionFrame())
                        {
                            var geometryRatio =
                                adapter.HalfTrackWidthMeters /
                                adapter.HalfWheelBaseMeters;

                            frontSteeringRadians =
                                ConvertToCrabSteering(
                                    frontSteeringRadians,
                                    geometryRatio);
                            rearSteeringRadians =
                                ConvertToCrabSteering(
                                    rearSteeringRadians,
                                    geometryRatio);
                        }

                        var frontThetaDegrees =
                            (float)AngleMath.RadiansToDegrees(
                                frontSteeringRadians);
                        var rearThetaDegrees =
                            (float)AngleMath.RadiansToDegrees(
                                rearSteeringRadians);
                        var motionSpeed =
                            (float)Math.Sqrt(
                                vxInMotion * vxInMotion +
                                vyInMotion * vyInMotion);

                        commandAccepted =
                            chassis.SendMotion(
                                motionSpeed,
                                frontThetaDegrees,
                                rearThetaDegrees,
                                interval);
                    }
                    else if (CommandBackend ==
                             ChassisCommandBackend
                                 .SendXYThSpeed)
                    {
                        // 安全XYTh后端根据舵角误差统一压低驱动轮速。
                        bodyTwist =
                            FrameTransform2D
                                .TransformTwistAtSamePoint(
                                    new Pose2D(
                                        0.0,
                                        0.0,
                                        MotionFrameYawInBodyRadians),
                                    new Twist2D(
                                        vxInMotion,
                                        vyInMotion,
                                        omega));
                        var command = new ChassisCommand(
                            PilotDefinition.Self.CarNum,
                            bodyTwist);
                        commandAccepted =
                            adapter.Send(
                                command,
                                interval);
                    }
                    else
                    {
                        throw new InvalidOperationException(
                            $"不支持的底盘命令后端：{CommandBackend}。");
                    }

                    if (!commandAccepted)
                        throw new InvalidOperationException(
                            "运动坐标系轨迹底盘解算失败：" +
                            chassis
                                .LastMotionDecomposeFailureReason);

                    CommandObserver?.Invoke(
                        (float)bodyTwist.VxMetersPerSecond,
                        (float)bodyTwist.VyMetersPerSecond,
                        (float)bodyTwist
                            .OmegaRadiansPerSecond);

                    yield return true;
                }
            }
            finally
            {
                adapter.StopImmediately();
                if (CommandBackend ==
                    ChassisCommandBackend.SendMotion)
                {
                    // 测试退出后恢复真实车体坐标系，避免影响后续测试。
                    adapter.ResetToBodyFrame();
                }
                CommandObserver?.Invoke(0f, 0f, 0f);
            }

            yield return false;
        }

        // 判断当前运动坐标系是否为车体左侧朝前的蟹行坐标系。
        private bool IsCrabMotionFrame()
        {
            return Math.Abs(
                       AngleMath.ShortestDifferenceRadians(
                           Math.PI / 2.0,
                           MotionFrameYawInBodyRadians)) <
                   1e-6;
        }

        // 按车体几何比例缩小蟹行转角。
        // +90°运动坐标系已经完成方向映射，此处不能再次反号。
        private double ConvertToCrabSteering(
            double normalSteeringRadians,
            double geometryRatio)
        {
            var crabSteeringRadians =
                Math.Atan(
                    geometryRatio *
                    Math.Tan(
                        normalSteeringRadians));

            return Limit(
                crabSteeringRadians,
                MaximumVirtualSteeringRadians);
        }

        // 计算当前点在直线或圆弧上的参考点、切线和剩余距离。
        private void CalculateReference(
            Vector2 currentPosition,
            out double tangentYaw,
            out Vector2 referencePoint,
            out float remainingMillimeters,
            out double curvaturePerMeter)
        {
            var initialMotionYaw =
                InitialBodyYawRadians +
                MotionFrameYawInBodyRadians;

            if (PathKind == ReferencePathKind.Straight)
            {
                var tangent = new Vector2(
                    (float)Math.Cos(initialMotionYaw),
                    (float)Math.Sin(initialMotionYaw));
                var relative = currentPosition - StartPosition;
                var progress =
                    Vector2.Dot(relative, tangent);
                var clampedProgress =
                    Math.Max(
                        0f,
                        Math.Min(progress, LengthMillimeters));

                tangentYaw = initialMotionYaw;
                referencePoint =
                    StartPosition +
                    tangent * clampedProgress;
                remainingMillimeters =
                    Math.Max(
                        0f,
                        LengthMillimeters - progress);
                curvaturePerMeter = 0.0;
                return;
            }

            if (PathKind == ReferencePathKind.SCurve)
            {
                CalculateSCurveReference(
                    currentPosition,
                    initialMotionYaw,
                    out tangentYaw,
                    out referencePoint,
                    out remainingMillimeters,
                    out curvaturePerMeter);
                return;
            }

            var center = GetArcCenter();
            var startRadialYaw =
                initialMotionYaw - Math.PI / 2.0;
            var radial = currentPosition - center;
            var currentRadialYaw =
                Math.Atan2(radial.Y, radial.X);
            var progressRadians =
                AngleMath.NormalizeRadians(
                    currentRadialYaw - startRadialYaw);

            // 测试圆弧只有+90°，起点附近的轻微负噪声按0处理。
            if (progressRadians < 0.0)
                progressRadians = 0.0;

            var clampedProgressRadians =
                Math.Min(
                    progressRadians,
                    ArcSweepRadians);
            var referenceRadialYaw =
                startRadialYaw +
                clampedProgressRadians;
            referencePoint = center + new Vector2(
                RadiusMillimeters *
                    (float)Math.Cos(referenceRadialYaw),
                RadiusMillimeters *
                    (float)Math.Sin(referenceRadialYaw));
            tangentYaw =
                referenceRadialYaw + Math.PI / 2.0;
            remainingMillimeters =
                (float)Math.Max(
                    0.0,
                    (ArcSweepRadians - progressRadians) *
                    RadiusMillimeters);
            curvaturePerMeter =
                1000.0 / RadiusMillimeters;
        }

        // 通过离散最近点和解析导数计算两段三次贝塞尔S曲线的参考状态。
        private void CalculateSCurveReference(
            Vector2 currentPosition,
            double initialMotionYaw,
            out double tangentYaw,
            out Vector2 referencePoint,
            out float remainingMillimeters,
            out double curvaturePerMeter)
        {
            const int nearestPointSamples = 200;
            var searchStart =
                Math.Max(
                    0.0,
                    _lastSCurveProgress - 0.02);
            var bestProgress = _lastSCurveProgress;
            var bestDistanceSquared = double.MaxValue;

            for (var i = 0;
                 i <= nearestPointSamples;
                 i++)
            {
                var progress =
                    searchStart +
                    (1.0 - searchStart) *
                    i / nearestPointSamples;
                EvaluateSCurve(
                    progress,
                    out var localPoint,
                    out _,
                    out _);
                var worldPoint =
                    LocalPathPointToWorld(
                        localPoint,
                        initialMotionYaw);
                var distanceSquared =
                    Vector2.DistanceSquared(
                        currentPosition,
                        worldPoint);

                if (distanceSquared <
                    bestDistanceSquared)
                {
                    bestDistanceSquared =
                        distanceSquared;
                    bestProgress = progress;
                }
            }

            // 轨迹进度不允许因定位噪声倒退，防止控制目标跳回上一段曲线。
            _lastSCurveProgress =
                Math.Max(
                    _lastSCurveProgress,
                    bestProgress);
            EvaluateSCurve(
                _lastSCurveProgress,
                out var bestLocalPoint,
                out var firstDerivative,
                out var secondDerivative);
            referencePoint =
                LocalPathPointToWorld(
                    bestLocalPoint,
                    initialMotionYaw);
            tangentYaw =
                initialMotionYaw +
                Math.Atan2(
                    firstDerivative.Y,
                    firstDerivative.X);

            var derivativeMagnitude =
                Math.Sqrt(
                    firstDerivative.X *
                    firstDerivative.X +
                    firstDerivative.Y *
                    firstDerivative.Y);
            if (derivativeMagnitude < 1e-6)
            {
                curvaturePerMeter = 0.0;
            }
            else
            {
                // 导数单位为mm，乘1000后将曲率从1/mm转换成1/m。
                curvaturePerMeter =
                    (firstDerivative.X *
                     secondDerivative.Y -
                     firstDerivative.Y *
                     secondDerivative.X) *
                    1000.0 /
                    Math.Pow(
                        derivativeMagnitude,
                        3.0);
            }

            remainingMillimeters =
                ApproximateSCurveRemainingLength(
                    _lastSCurveProgress);
        }

        // 计算与普通4m S型测试完全一致的三段三次贝塞尔完整S曲线。
        private void EvaluateSCurve(
            double progress,
            out Vector2 point,
            out Vector2 firstDerivative,
            out Vector2 secondDerivative)
        {
            progress =
                Math.Max(
                    0.0,
                    Math.Min(progress, 1.0));

            Vector2 p0;
            Vector2 p1;
            Vector2 p2;
            Vector2 p3;
            double t;

            if (progress <= 0.25)
            {
                t = progress * 4.0;
                p0 = new Vector2(0f, 0f);
                p1 = new Vector2(
                    LengthMillimeters / 12f,
                    0f);
                p2 = new Vector2(
                    LengthMillimeters / 6f,
                    SCurveLateralOffsetMillimeters);
                p3 = new Vector2(
                    LengthMillimeters * 0.25f,
                    SCurveLateralOffsetMillimeters);
            }
            else if (progress <= 0.75)
            {
                t = (progress - 0.25) * 2.0;
                p0 = new Vector2(
                    LengthMillimeters * 0.25f,
                    SCurveLateralOffsetMillimeters);
                p1 = new Vector2(
                    LengthMillimeters / 3f,
                    SCurveLateralOffsetMillimeters);
                p2 = new Vector2(
                    LengthMillimeters * 2f / 3f,
                    -SCurveLateralOffsetMillimeters);
                p3 = new Vector2(
                    LengthMillimeters * 0.75f,
                    -SCurveLateralOffsetMillimeters);
            }
            else
            {
                t = (progress - 0.75) * 4.0;
                p0 = new Vector2(
                    LengthMillimeters * 0.75f,
                    -SCurveLateralOffsetMillimeters);
                p1 = new Vector2(
                    LengthMillimeters * 5f / 6f,
                    -SCurveLateralOffsetMillimeters);
                p2 = new Vector2(
                    LengthMillimeters * 11f / 12f,
                    0f);
                p3 = new Vector2(
                    LengthMillimeters,
                    0f);
            }

            var oneMinusT = 1.0 - t;
            point =
                p0 * (float)(
                    oneMinusT *
                    oneMinusT *
                    oneMinusT) +
                p1 * (float)(
                    3.0 *
                    oneMinusT *
                    oneMinusT *
                    t) +
                p2 * (float)(
                    3.0 *
                    oneMinusT *
                    t *
                    t) +
                p3 * (float)(t * t * t);
            firstDerivative =
                (p1 - p0) *
                    (float)(
                        3.0 *
                        oneMinusT *
                        oneMinusT) +
                (p2 - p1) *
                    (float)(
                        6.0 *
                        oneMinusT *
                        t) +
                (p3 - p2) *
                    (float)(3.0 * t * t);
            secondDerivative =
                (p2 - 2f * p1 + p0) *
                    (float)(6.0 * oneMinusT) +
                (p3 - 2f * p2 + p1) *
                    (float)(6.0 * t);
        }

        // 通过分段采样估算从当前S曲线进度到终点的实际弧长。
        private float ApproximateSCurveRemainingLength(
            double startProgress)
        {
            const int lengthSamples = 100;
            EvaluateSCurve(
                startProgress,
                out var previousPoint,
                out _,
                out _);
            var length = 0f;

            for (var i = 1;
                 i <= lengthSamples;
                 i++)
            {
                var progress =
                    startProgress +
                    (1.0 - startProgress) *
                    i / lengthSamples;
                EvaluateSCurve(
                    progress,
                    out var point,
                    out _,
                    out _);
                length +=
                    Vector2.Distance(
                        previousPoint,
                        point);
                previousPoint = point;
            }

            return length;
        }

        // 将以初始蟹行方向为X轴的局部路径点转换到Detour世界坐标。
        private Vector2 LocalPathPointToWorld(
            Vector2 localPoint,
            double initialMotionYaw)
        {
            var cos =
                (float)Math.Cos(initialMotionYaw);
            var sin =
                (float)Math.Sin(initialMotionYaw);

            return StartPosition + new Vector2(
                localPoint.X * cos -
                localPoint.Y * sin,
                localPoint.X * sin +
                localPoint.Y * cos);
        }

        // 获取蟹行左转圆弧圆心；它位于初始运动方向的左侧。
        public Vector2 GetArcCenter()
        {
            var initialMotionYaw =
                InitialBodyYawRadians +
                MotionFrameYawInBodyRadians;
            return StartPosition + new Vector2(
                -RadiusMillimeters *
                    (float)Math.Sin(initialMotionYaw),
                RadiusMillimeters *
                    (float)Math.Cos(initialMotionYaw));
        }

        // 获取圆弧测试的理论终点。
        public Vector2 GetArcDestination()
        {
            var initialMotionYaw =
                InitialBodyYawRadians +
                MotionFrameYawInBodyRadians;
            var startRadialYaw =
                initialMotionYaw - Math.PI / 2.0;
            var endRadialYaw =
                startRadialYaw + ArcSweepRadians;
            var center = GetArcCenter();

            return center + new Vector2(
                RadiusMillimeters *
                    (float)Math.Cos(endRadialYaw),
                RadiusMillimeters *
                    (float)Math.Sin(endRadialYaw));
        }

        // 根据剩余路径长度生成终点减速速度。
        private float CalculateSpeed(
            float remainingMillimeters)
        {
            if (remainingMillimeters >=
                SlowDistanceMillimeters)
                return CruiseSpeed;

            var ratio =
                remainingMillimeters /
                Math.Max(
                    SlowDistanceMillimeters,
                    1f);
            return Math.Max(
                MinimumSpeed,
                CruiseSpeed * ratio);
        }

        private void ValidateParameters()
        {
            if (CruiseSpeed <= 0f ||
                !IsFinite(CruiseSpeed) ||
                LengthMillimeters <= 0f ||
                !IsFinite(LengthMillimeters) ||
                RadiusMillimeters <= 0f ||
                !IsFinite(RadiusMillimeters) ||
                SCurveLateralOffsetMillimeters <= 0f ||
                !IsFinite(
                    SCurveLateralOffsetMillimeters) ||
                ArcSweepRadians <= 0.0 ||
                !IsFinite(ArcSweepRadians) ||
                SlowDistanceMillimeters <= 0f ||
                !IsFinite(SlowDistanceMillimeters) ||
                FinishDistanceMillimeters < 0f ||
                !IsFinite(FinishDistanceMillimeters) ||
                 TrackingTimeoutSeconds <= 0f ||
                 !IsFinite(TrackingTimeoutSeconds) ||
                 MaximumVirtualSteeringRadians <= 0.0 ||
                 MaximumVirtualSteeringRadians >=
                 Math.PI / 2.0 ||
                 !IsFinite(
                     MaximumVirtualSteeringRadians))
                throw new ArgumentOutOfRangeException(
                    "蟹行轨迹测试参数无效。");
        }

        private static double Limit(
            double value,
            double absoluteLimit)
        {
            return Math.Max(
                -absoluteLimit,
                Math.Min(value, absoluteLimit));
        }

        private static bool IsFinite(double value)
        {
            return
                !double.IsNaN(value) &&
                !double.IsInfinity(value);
        }
    }
}