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

namespace MedullaAdapter
{
    public class MotorRoutine : LadderLogic<DiverCartDefinition>
    {
        private bool _wasTransmitterControlling;
        private DateTime _lastMoveTime = DateTime.Now;
        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;
        }

        // M层单车底盘：根据四个舵轮的目标角度和实际角度修正8个驱动电机速度。
        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;
                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);

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

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

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

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

            // 保存四个转向PID的本周期修正量，供M层监控和舵轮响应CSV记录使用。
            cart.DiffSteerOutputLeftFront = diffLf;
            cart.DiffSteerOutputLeftRear = diffLr;
            cart.DiffSteerOutputRightFront = diffRf;
            cart.DiffSteerOutputRightRear = diffRr;

            // 左前腿：左右电机施加方向相反的PID修正量。
            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;
        }

        // 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);
        }

    }
}
