// 定义车型、上下层IO、参数和MCU初始化
using CartActivator;
using MCUSerialBridgeCLR;
using MDCSToolBox.Medulla.Chassis.MultiWheel;
using Medulla.Types;
using System;
using System.Collections.Generic;
using System.Threading;
using MyParking.Shared;

namespace MedullaAdapter
{
    [UseLadderLogic(logic = typeof(AlarmRoutine), scanInterval = 50)]
    [UseLadderLogic(logic = typeof(MotorRoutine), scanInterval = 50)]
    [UseLadderLogic(logic = typeof(MCURoutine), scanInterval = 20)]
    [UseManualController(manualController = typeof(Remote))]
    public class DiverCartDefinition : MultiWheelCartDefinition
    {
        #region 基本成员
        public MCUSerialBridge Bridge;

        internal enum ManualControlMode
        {
            Normal = 0, // 正常模式
            Crab = 1, // 螃蟹模式
            Spin = 2, // 自旋模式
        }
        internal ManualControlMode TransmitterControlMode = ManualControlMode.Normal;
        internal DateTime TransmitterLastTime = DateTime.Now; // 物理遥控器计算两次实体遥控器指令之间的时间间隔
        private ManualControlMode? _pendingManualMode;
        private ManualControlMode? _activeManualMode;
        #endregion

        #region AsUpperIO
        [AsUpperIO(desc = "从C上复位")] public bool ResetFromC;
        [AsUpperIO(desc = "从C将驱动轮下使能")] public bool DisableFromC;
        [AsUpperIO(desc = "左夹臂下发速度", timeOutReset = true)] public float SpeedLeftArm;
        [AsUpperIO(desc = "右夹臂下发速度", timeOutReset = true)] public float SpeedRightArm;
        [AsUpperIO(desc = "夹臂不同步报警")] public bool ClampOutOfSync;
        #endregion

        #region AsLowerIO
        [AsLowerIO(desc = "左前左轮实际位置")] public float LFLActualPos;
        [AsLowerIO(desc = "左前右轮实际位置")] public float LFRActualPos;
        [AsLowerIO(desc = "右前左轮实际位置")] public float RFLActualPos;
        [AsLowerIO(desc = "右前右轮实际位置")] public float RFRActualPos;
        [AsLowerIO(desc = "左后左轮实际位置")] public float LRLActualPos;
        [AsLowerIO(desc = "左后右轮实际位置")] public float LRRActualPos;
        [AsLowerIO(desc = "右后左轮实际位置")] public float RRLActualPos;
        [AsLowerIO(desc = "右后右轮实际位置")] public float RRRActualPos;
        [AsLowerIO(desc = "左夹臂实际速度")] public float ActualSpeedLeftArm;
        [AsLowerIO(desc = "右夹臂实际速度")] public float ActualSpeedRightArm;
        [AsLowerIO(desc = "左夹臂状态字")] public int LeftArmStateCode;
        [AsLowerIO(desc = "右夹臂状态字")] public int RightArmStateCode;
        [AsLowerIO(desc = "左夹臂错误字")] public int LeftArmErrorCode;
        [AsLowerIO(desc = "右夹臂错误字")] public int RightArmErrorCode;
        [AsLowerIO(desc = "左夹臂电流")] public float LeftArmElectric;
        [AsLowerIO(desc = "右夹臂电流")] public float RightArmElectric;
        [AsLowerIO(desc = "左夹臂实际位置")] public float ActualPosLeftArm;
        [AsLowerIO(desc = "右夹臂实际位置")] public float ActualPosRightArm;
        [AsLowerIO(desc = "驱动轮使能状态")] public bool WheelAbleState = true;
        [AsLowerIO(desc = "电池健康状态")] public float SOH;
        [AsInitParam(desc = "车号")][AsLowerIO] public int CarNum = 1;
        #endregion

        #region  初始参数
        [AsInitParam(desc = "MCU端口号")] public string MCUPort = "COM4";
        [AsInitParam(desc = "遥控器速度上限")] public float TransmitterSpeedUpperLimit = 1.0f;
        [AsInitParam(desc = "遥控器速度下限")] public float TransmitterSpeedLowerLimit = 0.0f;
        [AsInitParam(desc = "实体遥控器SB模式防抖时间，单位ms")]
        public int TransmitterModeDebounceMilliseconds = 200;
        [AsInitParam(desc = "手动控制夹臂速度系数")] public float ManualArmSpeedFac = 1.0f;
        [AsInitParam(desc = "遥控转弯舵角同步限速宽度，单位为度")]
        public float ManualSteeringAlignmentSigmaDegrees = 8.0f;
        [AsInitParam(desc = "自转最大角速度，单位deg/s")]
        public float MaxSpinAngularSpeedDegreesPerSecond = 30f;
        [AsInitParam(desc = "轮速诊断日志相对目录")]
        public string WheelSpeedDiagnosticDirectory =
            @"logs\wheel-speed";
        [AsInitParam(desc = "左夹臂低限位")][AsLowerIO] public int LeftArmLowerPos = -10000;
        [AsInitParam(desc = "左夹臂高限位")][AsLowerIO] public int LeftArmUpperPos = 5927610;
        [AsInitParam(desc = "右夹臂低限位")][AsLowerIO] public int RightArmLowerPos = -17295;
        [AsInitParam(desc = "右夹臂高限位")][AsLowerIO] public int RightArmUpperPos = 5927610;


        #endregion

        #region 监控参数
        [IOObjectMonitor(desc = "从M上复位")] public bool ResetFromM;
        [IOObjectMonitor(desc = "从M将驱动轮下使能")] public bool DisableFromM;
        [IOObjectMonitor(desc = "左前左轮PID修正后速度")] public float SpeedLFL;
        [IOObjectMonitor(desc = "左前右轮PID修正后速度")] public float SpeedLFR;
        [IOObjectMonitor(desc = "右前左轮PID修正后速度")] public float SpeedRFL;
        [IOObjectMonitor(desc = "右前右轮PID修正后速度")] public float SpeedRFR;
        [IOObjectMonitor(desc = "左后左轮PID修正后速度")] public float SpeedLRL;
        [IOObjectMonitor(desc = "左后右轮PID修正后速度")] public float SpeedLRR;
        [IOObjectMonitor(desc = "右后左轮PID修正后速度")] public float SpeedRRL;
        [IOObjectMonitor(desc = "右后右轮PID修正后速度")] public float SpeedRRR;
        [IOObjectMonitor(desc = "左前舵轮转向PID输出")] public float DiffSteerOutputLeftFront;
        [IOObjectMonitor(desc = "左后舵轮转向PID输出")] public float DiffSteerOutputLeftRear;
        [IOObjectMonitor(desc = "右前舵轮转向PID输出")] public float DiffSteerOutputRightFront;
        [IOObjectMonitor(desc = "右后舵轮转向PID输出")] public float DiffSteerOutputRightRear;
        [IOObjectMonitor(desc = "灯光模式")] public int LightMode = 0;
        [IOObjectMonitor(desc = "实体遥控器当前速度倍率")] public float TransmitterSpeed = 0.3f;
        [IOObjectMonitor(desc = "轮速诊断记录已启用")]
        public bool WheelSpeedDiagnosticEnabled;
        [IOObjectMonitor(desc = "轮速诊断记录状态")]
        public string WheelSpeedDiagnosticStatus = "未启动";
        [IOObjectMonitor(desc = "左前左驱动器远程帧701")] public byte LFLRemoteCode = 0;
        [IOObjectMonitor(desc = "左前右驱动器远程帧702")] public byte LFRRemoteCode = 0;
        [IOObjectMonitor(desc = "右前左驱动器远程帧703")] public byte RFLRemoteCode = 0;
        [IOObjectMonitor(desc = "右前右驱动器远程帧704")] public byte RFRRemoteCode = 0;
        [IOObjectMonitor(desc = "左后左驱动器远程帧705")] public byte LRLRemoteCode = 0;
        [IOObjectMonitor(desc = "左后右驱动器远程帧706")] public byte LRRRemoteCode = 0;
        [IOObjectMonitor(desc = "右后左驱动器远程帧707")] public byte RRLRemoteCode = 0;
        [IOObjectMonitor(desc = "右后右驱动器远程帧708")] public byte RRRRemoteCode = 0;
        [IOObjectMonitor(desc = "左夹臂驱动器远程帧709")] public byte LArmRemoteCode = 0;
        [IOObjectMonitor(desc = "右夹臂驱动器远程帧70A")] public byte RArmRemoteCode = 0;
        #endregion

        #region  操作按钮
        // M层单车硬件：向驱动轮发送复位请求。
        [IOObjectUtility]
        public void WheelReset()
        {
            ResetFromM = true;
        }
        // M层单车硬件：向驱动轮发送下使能请求。
        [IOObjectUtility]
        public void WheelDisable()
        {
            DisableFromM = true;
        }

        // M层诊断：请求开始保存CAN轮速事件和底盘周期快照。
        [IOObjectUtility]
        public void StartWheelSpeedDiagnostic()
        {
            WheelSpeedDiagnosticEnabled = true;
            WheelSpeedDiagnosticStatus = "等待创建日志文件";
        }

        // M层诊断：请求停止轮速记录并刷新CSV文件。
        [IOObjectUtility]
        public void StopWheelSpeedDiagnostic()
        {
            WheelSpeedDiagnosticEnabled = false;
            WheelSpeedDiagnosticStatus = "等待停止并刷新日志";
        }
        #endregion

        public override void CommunicationInit()
        {
            if (GhostMode) return;
            State = -1;
            Bridge = new MCUSerialBridge();
            //Step1:打开指定串口连接
            var err = Bridge.Open(MCUPort, 1000000u);
            if (err != MCUSerialBridgeError.OK)
            {
                Console.WriteLine($"MCU Open FAILED: {err.ToDescription()}");
                return;
            }
            else
            {
                Console.WriteLine("MCU Open OK");
            }
            //Step2:远程复位MCU
            err = Bridge.Reset();
            if (err != MCUSerialBridgeError.OK)
            {
                Console.WriteLine($"MCU Reset FAILED: {err.ToDescription()}");
                return;
            }
            else
            {
                Thread.Sleep(500);
                Console.WriteLine("MCU Reset OK");
            }
            //Step3:获取MCU版本号
            err = Bridge.GetVersion(out var version, 100);
            if (err != MCUSerialBridgeError.OK)
            {
                Console.WriteLine($"MCU GetVersion FAILED: {err.ToDescription()}");
                return;
            }
            else
            {
                Console.WriteLine($"MCU GetVersion OK: {version}");
            }
            //Step4:获取MCU状态
            err = Bridge.GetState(out var state, 100);
            if (err != MCUSerialBridgeError.OK)
            {
                Console.WriteLine($"MCU GetState FAILED: {err.ToDescription()}");
                return;
            }
            else
            {
                Console.WriteLine($"MCU GetState OK: {state}");
            }
            //Step5:串口/CAN配置
            try
            {
                var ports = new List<PortConfig>();
                for (int i = 0; i < 1; i++)
                    ports.Add(new CANPortConfig(500000, 10));
                for (int i = 0; i < 3; i++)
                    ports.Add(new SerialPortConfig(9600, 10));
                Console.WriteLine("=== Port Configuration ===");
                for (int i = 0; i < ports.Count; i++)
                {
                    if (ports[i] is SerialPortConfig s)
                        Console.WriteLine($"Port {i}: Serial, Baud={s.Baud}, ReceiveFrameMs={s.ReceiveFrameMs}");
                    else if (ports[i] is CANPortConfig c)
                        Console.WriteLine($"Port {i}: CAN, Baud={c.Baud}, RetryTimeMs={c.RetryTimeMs}");
                }

                var ret = Bridge.Configure(ports, 200);
                if (ret != MCUSerialBridgeError.OK)
                {
                    Console.WriteLine($"MCU Configure FAILED: {ret.ToDescription()}");
                    return;
                }
                else
                {
                    Console.WriteLine("MCU Configure OK");
                }
                Console.WriteLine("MCU Configure {0}", ret == MCUSerialBridgeError.OK ? "OK" : $"FAILED: 0x{(uint)ret:X8}");
            }
            catch (Exception ex)
            {
                Console.WriteLine($"Configure Exception: {ex.Message}");
                return;
            }
            State = 0;
        }

        internal void ManualControl(
            ManualControlMode mode,
            float x,
            float y,
            float frontDirection,
            float speedThreshold,
            TimeSpan? interval = null)
        {
            if (Chassis == null) return;

            var adapter = GetChassisAdapter();
            if (adapter == null) return;

            // 模式变化时先停车并下发舵轮准备角度；
            // 在实际舵角到位之前，不开放驱动速度。
            if (!EnsureManualModeReady(mode, interval))
            {
                adapter.StopImmediately();
                return;
            }

            var speed = speedThreshold * y;
            var normalizedSteering =
                (float)Math.Pow(
                    Math.Abs(x),
                    ManualThetaPow) *
                Math.Sign(x);
            var steeringDegrees =
                -normalizedSteering * MaxManualTheta;
            var frontTh = steeringDegrees;
            var rearTh = -steeringDegrees;
            ManualMode = (int)mode;

            switch (mode)
            {
                case ManualControlMode.Normal:
                    // 普通模式统一使用车体速度命令：
                    // X向前，行驶中连续改变角速度时舵轮边转、车辆边走。
                    // SendBodyCommand(
                    //     vx: speed,
                    //     vy: 0.0,
                    //     omegaRadiansPerSecond: omega,
                    //     interval);
                    Chassis.SendMotion(
                                speed,
                                frontTh,
                                rearTh,
                                interval);
                    break;
                case ManualControlMode.Crab:
                    // 舵轮机械范围为[-120°,120°]。
                    // 蟹行后虚拟轴距由原车宽度决定，比正常模式轴距短。
                    // 按几何比例缩小转角，使相同摇杆输入获得接近一致的曲率。
                    var normalSteeringRadians =
                        AngleMath.DegreesToRadians(steeringDegrees);
                    var geometryRatio =
                        adapter.HalfTrackWidthMeters /
                        adapter.HalfWheelBaseMeters;

                    // +90°运动坐标系已经把虚拟左侧映射为车体后方，
                    // 此处保持普通模式的转向符号，避免再次取反导致左右颠倒。
                    var crabSteeringRadians =
                        Math.Atan(
                            geometryRatio *
                            Math.Tan(
                                normalSteeringRadians));

                    // 蟹行转角最终限制为±30°，为±120°机械舵角保留余量。
                    var maximumCrabSteeringRadians =
                        AngleMath.DegreesToRadians(30.0);
                    crabSteeringRadians = Math.Max(
                        -maximumCrabSteeringRadians,
                        Math.Min(
                            maximumCrabSteeringRadians,
                            crabSteeringRadians));

                    // 将车体左侧作为虚拟阿克曼车头，并在该运动坐标系中
                    // 复用与普通模式相同的SendMotion前后控制点解算。
                    if (!adapter.SendVirtualAckermannMotion(
                            motionDirectionRadians:
                                Math.PI / 2.0,
                            speedMetersPerSecond:
                                speed,
                            steeringRadians:
                                crabSteeringRadians,
                            interval))
                    {
                        adapter.StopImmediately();

                        Console.WriteLine(
                            "蟹行SendMotion命令分解失败，车辆已经停车：" +
                            adapter.LastFailureReason);
                    }
                    break;
                case ManualControlMode.Spin:
                    // 摇杆处于中位时只清零驱动速度，保持已经准备好的
                    // 自转舵角；下次推动摇杆时仍会重新检查实际舵角。
                    if (Math.Abs(speed) < 1e-6f)
                    {
                        adapter
                            .StopXYThDrivePreserveSteeringState();
                        break;
                    }

                    // 自转时speed表示最外侧舵轮中心的目标切向速度，
                    // 根据v=omega*r换算为SendXYThSpeed需要的角速度。
                    var requestedSpinOmegaRadiansPerSecond =
                        speed /
                        adapter.MaximumWheelRadiusMeters;

                    // 对半径换算结果做正负对称限幅，防止遥控速度参数误设后自转过快。
                    var maximumSpinOmegaRadiansPerSecond =
                        AngleMath.DegreesToRadians(
                            Math.Max(
                                0f,
                                MaxSpinAngularSpeedDegreesPerSecond));

                    var spinOmegaRadiansPerSecond =
                        Math.Max(
                            -maximumSpinOmegaRadiansPerSecond,
                            Math.Min(
                                maximumSpinOmegaRadiansPerSecond,
                                requestedSpinOmegaRadiansPerSecond));

                    // 普通安全版SendXYThSpeed只下发角速度，
                    // 四轮实际舵角未到位时不会开放驱动速度。
                    if (!adapter.Send(
                            new ChassisCommand(
                                CarNum,
                                new Twist2D(
                                    0.0,
                                    0.0,
                                    spinOmegaRadiansPerSecond)),
                            interval))
                    {
                        adapter.StopImmediately();

                        Console.WriteLine(
                            "SendXYThSpeed原地自转命令分解失败，车辆已经停车：" +
                            adapter.LastFailureReason);
                    }
                    break;
                default:
                    ManualMode = -1;
                    adapter.StopImmediately();
                    break;
            }
        }

        // 停车后切换模式：先预转舵轮，实际角度到位后才允许发送运动命令。
        private bool EnsureManualModeReady(
            ManualControlMode mode,
            TimeSpan? interval)
        {
            var adapter = GetChassisAdapter();
            if (adapter == null)
                return false;

            // 当前模式已经完成准备，可以直接接受运动命令。
            if (_activeManualMode == mode &&
                _pendingManualMode == null)
            {
                return true;
            }

            // 第一次收到新模式时，停车并下发一次舵轮准备姿态。
            if (_pendingManualMode != mode)
            {
                adapter.StopImmediately();
                // 所有模式的准备角度均按真实机械舵角表达；
                // 先退出上一模式的虚拟运动坐标系，再执行预对齐。
                adapter.ResetToBodyFrame();
                _activeManualMode = null;

                var preparationAccepted = mode switch
                {
                    ManualControlMode.Normal =>
                        adapter.PrepareParallelDirection(0.0),

                    ManualControlMode.Crab =>
                        adapter.PrepareParallelDirection(
                            Math.PI / 2.0),

                    ManualControlMode.Spin =>
                        adapter.PrepareSpin(interval),

                    _ => false
                };

                if (!preparationAccepted)
                {
                    _pendingManualMode = null;
                    return false;
                }

                _pendingManualMode = mode;
                return false;
            }

            // 后续控制周期保持停车，并读取实际舵角判断是否到位。
            adapter.StopImmediately();

            var toleranceRadians =
                AngleMath.DegreesToRadians(2.0);

            bool aligned;

            if (mode == ManualControlMode.Spin)
            {
                // 自转的四个舵轮目标角不同，等待期间持续刷新其目标。
                var preparationAccepted =
                    adapter.PrepareSpin(interval);

                aligned =
                    preparationAccepted &&
                    adapter.AreSpinWheelsAligned;
            }
            else
            {
                var targetDirection = mode ==
                    ManualControlMode.Crab
                        ? Math.PI / 2.0
                        : 0.0;

                aligned =
                    adapter.AreParallelWheelsAligned(
                        targetDirection,
                        toleranceRadians);
            }

            if (!aligned)
                return false;

            if (mode == ManualControlMode.Spin)
            {
                // 四轮实际舵角确认到位后只交接一次，保留PrepareSpin
                // 选定的机械舵角和轮速方向，避免首条XYTh命令重新选角。
                if (!adapter.AdoptPreparedSpinForXYTh(
                        toleranceRadians))
                {
                    return false;
                }
            }
            // 蟹行轮子在真实车体系中到达机械+90°后，
            // 再将车体左侧激活为SendMotion的虚拟X正方向。
            else if (mode == ManualControlMode.Crab)
            {
                adapter.ActivateMotionFrame(
                    Math.PI / 2.0);
            }
            else
            {
                adapter.ResetToBodyFrame();
            }

            _activeManualMode = mode;
            _pendingManualMode = null;
            return true;
        }

        private MultiWheelChassisAdapter _chassisAdapter;

        private MultiWheelChassisAdapter GetChassisAdapter()
        {
            if (Chassis == null)
                return null;

            if (_chassisAdapter == null ||
                _chassisAdapter.VehicleId != CarNum)
            {
                _chassisAdapter =
                    new MultiWheelChassisAdapter(Chassis, CarNum);
            }

            _chassisAdapter.SteeringAlignmentSigmaDegrees =
                Math.Max(
                    ManualSteeringAlignmentSigmaDegrees,
                    0.1f);

            return _chassisAdapter;
        }

        #region MCURoutine兼容参数（暂保留原硬件协议）

        // 保存MCU读取到的原始输入字节，供M层监控和硬件排查使用。
        [AsLowerIO(desc = "MCU原始输入字节")]
        public float test;

        // 保留原MCU灯光分支；单车默认值-1表示使用本车LightMode。
        [AsUpperIO(desc = "多车灯光同步兼容值，-1使用本车灯光")]
        public int MultiVehicleLightSync = -1;

        #endregion

    }
}