// 计算8个驱动电机的目标速度和舵角PID
using CartActivator;
using FundamentalLib;
using MDCSToolBox.Commons;
using System;
using System.Diagnostics;
using static MDCSToolBox.Medulla.Chassis.BasicCartDefinition;

namespace MedullaAdapter
{
    public class MotorRoutine : LadderLogic<DiverCartDefinition>
    {
        private const double MaximumFeedforwardIntervalSeconds = 0.2;
        private bool _wasTransmitterControlling;
        private DateTime _lastMoveTime = DateTime.Now;
        private bool _diffSteerFeedforwardInitialized;
        private long _lastDiffSteerFeedforwardTimestamp;
        private float _previousThLeftFront;
        private float _previousThLeftRear;
        private float _previousThRightFront;
        private float _previousThRightRear;
        private DiverCartDefinition.ManualControlMode?
            _pendingTransmitterControlMode;
        private DateTime _pendingTransmitterControlModeSince =
            DateTime.MinValue;
        public override void Operation(int iteration)
        {
            if (!cart.GhostMode && cart.State == -1) return;
            // SA稳定打开后，使能实体遥控器。
            TriggerOnce(
                cart.TransmitterConnected && cart.Transmitter_SA,
                300,
                () =>
                {
                    cart.TransmitterControlEnable = true;
                    cart.TransmitterLastTime = DateTime.Now;
                });
            // 遥控器断连或SA关闭时，立即撤销遥控使能。
            if (!cart.TransmitterConnected || !cart.Transmitter_SA)
                cart.TransmitterControlEnable = false;
            var transmitterSelected =
                cart.TransmitterConnected &&
                cart.TransmitterControlEnable &&
                cart.Transmitter_SA &&
                cart.Transmitter_SC == cart.CarNum;
            var transmitterControlling =
                transmitterSelected &&
                CartDefinition.testPriority(5, "TransmitterMode");
            if (transmitterControlling)
            {
                TransmitterChassisControl();
                cart.TransmitterLastTime = DateTime.Now;
                cart.CarStatu = "实体遥控器控制";
            }
            else
            {
                // 只在遥控器刚刚退出时发送一次停车，
                // 不能每周期停车，否则会覆盖C层轨迹控制。
                if (_wasTransmitterControlling)
                {
                    cart.ManualControl(
                        cart.TransmitterControlMode,
                        0, 0, 0,
                        cart.TransmitterSpeed,
                        DateTime.Now - cart.TransmitterLastTime);

                    StopClampArms();
                    cart.TransmitterLastTime = DateTime.Now;
                }

                cart.CarStatu = "正常运行";
            }
            _wasTransmitterControlling = transmitterControlling;
            // 当前是否由C层控制。
            cart.ClumsyControl = CartDefinition.currentPriority == 0;
            // 计算四个舵轮PID和8个驱动电机最终速度。
            UpdateDiffSteerWheelSpeeds();
            // 平滑更新硬件速度限制。
            UpdateSendSpeedLimit();
            // 更新红黄绿灯状态。
            UpdateLightMode();
        }
        // M层物理遥控器：只有SB档位连续稳定指定时间后才确认模式切换。
        private bool TryGetStableTransmitterControlMode(
            out DiverCartDefinition.ManualControlMode stableMode)
        {
            stableMode = cart.TransmitterControlMode;

            DiverCartDefinition.ManualControlMode? requestedMode;

            switch (cart.Transmitter_SB)
            {
                case TransmitterState.Mode0:
                    requestedMode =
                        DiverCartDefinition.ManualControlMode.Normal;
                    break;

                case TransmitterState.Mode1:
                    requestedMode =
                        DiverCartDefinition.ManualControlMode.Crab;
                    break;

                case TransmitterState.Mode2:
                    requestedMode =
                        DiverCartDefinition.ManualControlMode.Spin;
                    break;

                default:
                    _pendingTransmitterControlMode = null;
                    _pendingTransmitterControlModeSince =
                        DateTime.MinValue;
                    return false;
            }

            if (_pendingTransmitterControlMode != requestedMode)
            {
                _pendingTransmitterControlMode = requestedMode;
                _pendingTransmitterControlModeSince = DateTime.Now;
                return false;
            }

            var debounceMilliseconds = Math.Max(
                0,
                cart.TransmitterModeDebounceMilliseconds);

            if ((DateTime.Now -
                 _pendingTransmitterControlModeSince)
                .TotalMilliseconds < debounceMilliseconds)
            {
                return false;
            }

            stableMode = requestedMode.Value;
            return true;
        }

        // 物理遥控器设置
        public void TransmitterChassisControl()
        {
            var interval = DateTime.Now - cart.TransmitterLastTime;

            if (!TryGetStableTransmitterControlMode(
                    out var stableControlMode))
            {
                // SB处于中间档或尚未稳定时立即停车，
                // 保持当前模式，不下发新的模式目标角。
                cart.ManualControl(
                    cart.TransmitterControlMode,
                    0, 0, 0,
                    cart.TransmitterSpeed,
                    interval);
                StopClampArms();
                return;
            }

            cart.TransmitterControlMode = stableControlMode;

            // SA关闭后立即停车。
            if (!cart.Transmitter_SA)
            {
                cart.ManualControl(
                    cart.TransmitterControlMode,
                    0, 0, 0,
                    cart.TransmitterSpeed,
                    interval);
                StopClampArms();
                return;
            }
            // 限制实体遥控器的最大速度。
            cart.TransmitterSpeed = Math.Max(
                cart.TransmitterSpeedLowerLimit,
                Math.Min(
                    cart.TransmitterSpeed,
                    cart.TransmitterSpeedUpperLimit));
            // SD的Mode0作为底盘驾驶档。
            if (cart.Transmitter_SD == TransmitterState.Mode0)
            {
                // 底盘驾驶档不允许保留上一周期的夹臂速度。
                StopClampArms();

                cart.ManualControl(
                    cart.TransmitterControlMode,
                    cart.TransmitterLeftJoystickValX,
                    cart.TransmitterRightJoystickValY,
                    0,
                    cart.TransmitterSpeed,
                    interval);

                return;
            }
            if (cart.Transmitter_SD == TransmitterState.Mode1)
            {
                // 切换到夹臂档时，先确保底盘停止。
                cart.ManualControl(
                    cart.TransmitterControlMode,
                    0, 0, 0,
                    cart.TransmitterSpeed,
                    interval);

                var armSpeed =
                    cart.TransmitterRightJoystickValX *
                    cart.ManualArmSpeedFac;

                cart.SpeedLeftArm = armSpeed;
                cart.SpeedRightArm = armSpeed;
                return;
            }
            // 非驾驶档必须主动停车，防止上一条运动指令残留。
            cart.ManualControl(
                cart.TransmitterControlMode,
                0, 0, 0,
                cart.TransmitterSpeed,
                interval);
            StopClampArms();
        }

        // M层单车夹臂安全：清除物理遥控器留下的左右夹臂速度命令。
        private void StopClampArms()
        {
            cart.SpeedLeftArm = 0;
            cart.SpeedRightArm = 0;
        }

        /// <summary>
        /// 根据四个舵轮的目标角速度前馈和实际角度反馈修正八个驱动电机速度。
        /// </summary>
        private void UpdateDiffSteerWheelSpeeds()
        {
            if (cart.LeftFrontPid == null ||
                cart.LeftRearPid == null ||
                cart.RightFrontPid == null ||
                cart.RightRearPid == null)
            {
                cart.SpeedLFL = 0;
                cart.SpeedLFR = 0;
                cart.SpeedRFL = 0;
                cart.SpeedRFR = 0;
                cart.SpeedLRL = 0;
                cart.SpeedLRR = 0;
                cart.SpeedRRL = 0;
                cart.SpeedRRR = 0;
                ResetDiffSteerRateFeedforward();
                return;
            }

            // 更新左前舵轮PID参数。
            cart.LeftFrontPid.ChangeParameters(
                cart.DiffSteerKp,
                cart.DiffSteerKi,
                cart.DiffSteerKd,
                cart.DiffSteerMaxI,
                cart.DiffSteerDeadZone,
                cart.DiffSteerThresh,
                cart.DiffSteerSpeedAcc);

            // 更新左后舵轮PID参数。
            cart.LeftRearPid.ChangeParameters(
                cart.DiffSteerKp,
                cart.DiffSteerKi,
                cart.DiffSteerKd,
                cart.DiffSteerMaxI,
                cart.DiffSteerDeadZone,
                cart.DiffSteerThresh,
                cart.DiffSteerSpeedAcc);

            // 更新右前舵轮PID参数。
            cart.RightFrontPid.ChangeParameters(
                cart.DiffSteerKp,
                cart.DiffSteerKi,
                cart.DiffSteerKd,
                cart.DiffSteerMaxI,
                cart.DiffSteerDeadZone,
                cart.DiffSteerThresh,
                cart.DiffSteerSpeedAcc);

            // 更新右后舵轮PID参数。
            cart.RightRearPid.ChangeParameters(
                cart.DiffSteerKp,
                cart.DiffSteerKi,
                cart.DiffSteerKd,
                cart.DiffSteerMaxI,
                cart.DiffSteerDeadZone,
                cart.DiffSteerThresh,
                cart.DiffSteerSpeedAcc);

            // 根据实际舵角计算四条腿的PID反馈修正量。
            var feedbackLf = cart.LeftFrontPid.GetResponse(
                cart.ThLeftFront, false, false, "LF");

            var feedbackLr = cart.LeftRearPid.GetResponse(
                cart.ThLeftRear, false, false, "LR");

            var feedbackRf = cart.RightFrontPid.GetResponse(
                cart.ThRightFront, false, false, "RF");

            var feedbackRr = cart.RightRearPid.GetResponse(
                cart.ThRightRear, false, false, "RR");

            CalculateDiffSteerRateFeedforward(
                out var feedforwardLf,
                out var feedforwardLr,
                out var feedforwardRf,
                out var feedforwardRr);

            var diffLf = feedbackLf + feedforwardLf;
            var diffLr = feedbackLr + feedforwardLr;
            var diffRf = feedbackRf + feedforwardRf;
            var diffRr = feedbackRr + feedforwardRr;

            // 分别保留PID、前馈和合成差速，便于独立标定与诊断。
            cart.DiffSteerOutputLeftFront = feedbackLf;
            cart.DiffSteerOutputLeftRear = feedbackLr;
            cart.DiffSteerOutputRightFront = feedbackRf;
            cart.DiffSteerOutputRightRear = feedbackRr;
            cart.DiffSteerRateFeedforwardLeftFront = feedforwardLf;
            cart.DiffSteerRateFeedforwardLeftRear = feedforwardLr;
            cart.DiffSteerRateFeedforwardRightFront = feedforwardRf;
            cart.DiffSteerRateFeedforwardRightRear = feedforwardRr;
            cart.DiffSteerTotalOutputLeftFront = diffLf;
            cart.DiffSteerTotalOutputLeftRear = diffLr;
            cart.DiffSteerTotalOutputRightFront = diffRf;
            cart.DiffSteerTotalOutputRightRear = diffRr;

            // 左前腿：左右电机施加方向相反的合成差速修正量。
            cart.SpeedLFL = cart.SpeedLeftFrontLeft - diffLf;
            cart.SpeedLFR = cart.SpeedLeftFrontRight + diffLf;

            // 左后腿。
            cart.SpeedLRL = cart.SpeedLeftRearLeft - diffLr;
            cart.SpeedLRR = cart.SpeedLeftRearRight + diffLr;

            // 右前腿。
            cart.SpeedRFL = cart.SpeedRightFrontLeft - diffRf;
            cart.SpeedRFR = cart.SpeedRightFrontRight + diffRf;

            // 右后腿。
            cart.SpeedRRL = cart.SpeedRightRearLeft - diffRr;
            cart.SpeedRRR = cart.SpeedRightRearRight + diffRr;
        }

        /// <summary>
        /// 根据四个机械目标舵角的实际变化率计算本周期差速轮线速度前馈。
        /// </summary>
        private void CalculateDiffSteerRateFeedforward(
            out float leftFront,
            out float leftRear,
            out float rightFront,
            out float rightRear)
        {
            leftFront = 0f;
            leftRear = 0f;
            rightFront = 0f;
            rightRear = 0f;

            var currentTimestamp = Stopwatch.GetTimestamp();

            if (_diffSteerFeedforwardInitialized)
            {
                var deltaTimeSeconds =
                    (currentTimestamp -
                     _lastDiffSteerFeedforwardTimestamp) /
                    (double)Stopwatch.Frequency;

                if (deltaTimeSeconds > 0.0 &&
                    deltaTimeSeconds <=
                    MaximumFeedforwardIntervalSeconds)
                {
                    leftFront = CalculateDiffSteerRateFeedforward(
                        cart.ThLeftFront,
                        _previousThLeftFront,
                        deltaTimeSeconds);
                    leftRear = CalculateDiffSteerRateFeedforward(
                        cart.ThLeftRear,
                        _previousThLeftRear,
                        deltaTimeSeconds);
                    rightFront = CalculateDiffSteerRateFeedforward(
                        cart.ThRightFront,
                        _previousThRightFront,
                        deltaTimeSeconds);
                    rightRear = CalculateDiffSteerRateFeedforward(
                        cart.ThRightRear,
                        _previousThRightRear,
                        deltaTimeSeconds);
                }
            }

            _previousThLeftFront = cart.ThLeftFront;
            _previousThLeftRear = cart.ThLeftRear;
            _previousThRightFront = cart.ThRightFront;
            _previousThRightRear = cart.ThRightRear;
            _lastDiffSteerFeedforwardTimestamp = currentTimestamp;
            _diffSteerFeedforwardInitialized = true;
        }

        /// <summary>
        /// 将单个机械目标舵角变化率转换为带限幅的左右轮差速线速度前馈。
        /// </summary>
        private float CalculateDiffSteerRateFeedforward(
            float targetAngleDegrees,
            float previousTargetAngleDegrees,
            double deltaTimeSeconds)
        {
            var gain = cart.DiffSteerRateFeedforwardGain;
            var wheelDistanceMillimeters =
                cart.DiffSteerWheelDistanceMillimeters;
            var maximumSpeed =
                cart.DiffSteerRateFeedforwardMaximumSpeed;

            if (!IsFinite(gain) || gain <= 0f ||
                !IsFinite(wheelDistanceMillimeters) ||
                wheelDistanceMillimeters <= 0f ||
                !IsFinite(maximumSpeed) || maximumSpeed <= 0f ||
                !IsFinite(targetAngleDegrees) ||
                !IsFinite(previousTargetAngleDegrees))
            {
                return 0f;
            }

            // 机械舵角受限，必须使用直接差值而不是圆周最短角差。
            var targetRateRadiansPerSecond =
                (targetAngleDegrees - previousTargetAngleDegrees) *
                Math.PI / 180.0 /
                deltaTimeSeconds;
            var wheelDistanceMeters =
                wheelDistanceMillimeters / 1000.0;
            var feedforwardSpeed =
                0.5 *
                wheelDistanceMeters *
                targetRateRadiansPerSecond *
                gain;

            return (float)Math.Clamp(
                feedforwardSpeed,
                -maximumSpeed,
                maximumSpeed);
        }

        /// <summary>
        /// 清除差速转舵前馈历史和监控输出，避免恢复控制时使用过期目标角。
        /// </summary>
        private void ResetDiffSteerRateFeedforward()
        {
            _diffSteerFeedforwardInitialized = false;
            _lastDiffSteerFeedforwardTimestamp = 0;
            cart.DiffSteerRateFeedforwardLeftFront = 0f;
            cart.DiffSteerRateFeedforwardLeftRear = 0f;
            cart.DiffSteerRateFeedforwardRightFront = 0f;
            cart.DiffSteerRateFeedforwardRightRear = 0f;
            cart.DiffSteerTotalOutputLeftFront = 0f;
            cart.DiffSteerTotalOutputLeftRear = 0f;
            cart.DiffSteerTotalOutputRightFront = 0f;
            cart.DiffSteerTotalOutputRightRear = 0f;
        }

        /// <summary>
        /// 判断单精度参数是否可安全参与底盘控制计算。
        /// </summary>
        private static bool IsFinite(float value)
        {
            return !float.IsNaN(value) &&
                   !float.IsInfinity(value);
        }

        // M层单车限速：按照加速度和减速度平滑更新实际下发速度上限。
        private void UpdateSendSpeedLimit()
        {
            if (cart.Chassis == null)
            {
                cart.SendThresSpeed = 0;
                return;
            }

            var now = DateTime.Now;
            var elapsedSeconds = (float)(now - _lastMoveTime).TotalSeconds;
            _lastMoveTime = now;

            // 防止调试暂停或线程卡顿后，一次产生过大的速度跳变。
            elapsedSeconds = Math.Clamp(elapsedSeconds, 0f, 0.2f);

            // 限速值不允许小于零。
            var targetLimit = Math.Max(0f, cart.ThresSpeed);
            var currentLimit = Math.Max(0f, cart.SendThresSpeed);

            // 增大速度上限时用加速度，减小时用减速度。
            var speedChangingRate =
                targetLimit > currentLimit
                    ? cart.Chassis.AccPerSecond
                    : cart.Chassis.DeAccPerSecond;

            speedChangingRate = Math.Max(0f, speedChangingRate);

            var maxChange = speedChangingRate * elapsedSeconds;
            var speedDifference = targetLimit - currentLimit;

            if (Math.Abs(speedDifference) <= maxChange)
            {
                currentLimit = targetLimit;
            }
            else
            {
                currentLimit += Math.Sign(speedDifference) * maxChange;
            }

            // 计算当前8个电机目标速度中的最大绝对值。
            var wheelMaxSpeed = 0f;

            wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedLFL));
            wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedLFR));
            wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedRFL));
            wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedRFR));
            wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedLRL));
            wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedLRR));
            wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedRRL));
            wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedRRR));

            // 没有必要让平滑限速值高于当前所有车轮需要的速度。
            if (targetLimit < wheelMaxSpeed &&
                currentLimit > wheelMaxSpeed)
            {
                currentLimit = wheelMaxSpeed;
            }

            cart.SendThresSpeed = currentLimit;
        }

        // M层单车灯光：根据报警、驱动器、限速和电量状态生成红黄绿灯模式。
        private void UpdateLightMode()
        {
            // 二级报警：红灯常亮。
            if (cart.AlarmLevel == 2)
            {
                cart.LightMode = 2;
                return;
            }

            // 一级报警：黄灯常亮。
            if (cart.AlarmLevel == 1)
            {
                cart.LightMode = 3;
                return;
            }

            // 驱动轮未使能：黄灯闪烁。
            if (!cart.WheelAbleState)
            {
                FlipFlop(ref cart.LightMode, 500, 0, 3);
                return;
            }

            // C层正在进行限速：黄灯常亮。
            if (Math.Abs(cart.ThresSpeed - 1f) > 0.001f)
            {
                cart.LightMode = 3;
                return;
            }

            // 电量低于预警值：黄灯常亮。
            if (cart.Soc <= cart.LowBatteryAlarmThreshold)
            {
                cart.LightMode = 3;
                return;
            }

            // 正常运行：绿灯闪烁。
            FlipFlop(ref cart.LightMode, 500, 0, 1);
        }

    }
}