using System;
using MyParking.Shared;

namespace MultiWheelC.Control.Allocation
{
    /// <summary>
    /// 在对称前后GCP方向命令与车体中心刚体速度之间执行纯几何转换。
    /// </summary>
    public static class GcpKinematics
    {
        private const double ParallelDirectionTolerance = 1e-9;
        private const double StopSpeedDeadbandMetersPerSecond = 1e-6;

        /// <summary>
        /// 将有符号中心速度和前后GCP方向转换为真实车体坐标系中的Twist2D。
        /// </summary>
        public static Twist2D ToBodyTwist(
            GcpMotionCommand command,
            double controlPointRadiusMeters)
        {
            NumericGuard.EnsureFinitePositive(
                controlPointRadiusMeters,
                nameof(controlPointRadiusMeters));

            if (Math.Abs(command.SpeedMetersPerSecond) <=
                StopSpeedDeadbandMetersPerSecond)
            {
                return Twist2D.Zero;
            }

            var frontAngleRadians =
                command.FrontAngleRadians;
            var rearAngleRadians =
                command.RearAngleRadians;
            var directionDeterminant =
                Math.Sin(
                    rearAngleRadians -
                    frontAngleRadians);

            if (Math.Abs(directionDeterminant) <=
                ParallelDirectionTolerance)
            {
                var averageDirectionRadians =
                    Math.Atan2(
                        Math.Sin(frontAngleRadians) +
                        Math.Sin(rearAngleRadians),
                        Math.Cos(frontAngleRadians) +
                        Math.Cos(rearAngleRadians));

                return new Twist2D(
                    command.SpeedMetersPerSecond *
                    Math.Cos(averageDirectionRadians),
                    command.SpeedMetersPerSecond *
                    Math.Sin(averageDirectionRadians),
                    0.0);
            }

            var frontCosine =
                Math.Cos(frontAngleRadians);
            var frontSine =
                Math.Sin(frontAngleRadians);
            var rearCosine =
                Math.Cos(rearAngleRadians);
            var rearSine =
                Math.Sin(rearAngleRadians);

            // 两个GCP速度方向的法线交点就是瞬时旋转中心，坐标位于真实车体系。
            var rotationCenterXMeters =
                controlPointRadiusMeters *
                (frontCosine * rearSine +
                 frontSine * rearCosine) /
                directionDeterminant;
            var rotationCenterYMeters =
                -2.0 *
                controlPointRadiusMeters *
                frontCosine *
                rearCosine /
                directionDeterminant;
            var centerRadiusMeters =
                Math.Sqrt(
                    rotationCenterXMeters *
                    rotationCenterXMeters +
                    rotationCenterYMeters *
                    rotationCenterYMeters);

            if (!NumericGuard.IsFinite(centerRadiusMeters) ||
                centerRadiusMeters <= 0.0)
            {
                throw new InvalidOperationException(
                    "前后GCP方向不能生成有效的车体中心旋转半径。");
            }

            // 用有符号速度决定绕ICR的实际转向，避免倒车时把同一组GCP轴向解释成反向运动。
            var requestedDirectionSign =
                Math.Sign(command.SpeedMetersPerSecond);
            var positiveAngularFrontVelocityXMetersPerSecond =
                rotationCenterYMeters;
            var positiveAngularFrontVelocityYMetersPerSecond =
                controlPointRadiusMeters -
                rotationCenterXMeters;
            var frontDirectionAlignment =
                positiveAngularFrontVelocityXMetersPerSecond *
                requestedDirectionSign *
                frontCosine +
                positiveAngularFrontVelocityYMetersPerSecond *
                requestedDirectionSign *
                frontSine;
            var omegaSign =
                frontDirectionAlignment >= 0.0
                    ? 1.0
                    : -1.0;
            var omegaRadiansPerSecond =
                omegaSign *
                Math.Abs(command.SpeedMetersPerSecond) /
                centerRadiusMeters;

            return new Twist2D(
                omegaRadiansPerSecond *
                rotationCenterYMeters,
                -omegaRadiansPerSecond *
                rotationCenterXMeters,
                omegaRadiansPerSecond);
        }
    }
}