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

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; // 物理遥控器计算两次实体遥控器指令之间的时间间隔
        #endregion

        #region AsUpperIO
        [AsUpperIO(desc = "从C上复位")] public bool ResetFromC;
        [AsUpperIO(desc = "从C将驱动轮下使能")] public bool DisableFromC;
        #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 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;

        #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 = "灯光模式")] public int LightMode = 0;
        [IOObjectMonitor(desc = "实体遥控器当前速度倍率")] public float TransmitterSpeed = 0.3f;
        [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;
        #endregion

        #region  操作按钮
        // M层单车硬件：向驱动轮发送复位请求。
        [IOObjectUtility]
        public void WheelReset()
        {
            ResetFromM = true;
        }
        // M层单车硬件：向驱动轮发送下使能请求。
        [IOObjectUtility]
        public void WheelDisable()
        {
            DisableFromM = true;
        }
        #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 speed = speedThreshold * y;
            var thPow = (float)Math.Pow(Math.Abs(x), ManualThetaPow) * Math.Sign(x);
            var frontTh = -thPow * MaxManualTheta;
            var rearTh = thPow * MaxManualTheta;
            ManualMode = (int)mode;
            switch (mode)
            {
                case ManualControlMode.Normal:
                    SetChassisDirection(frontDirection);
                    Chassis.SendMotion(speed, frontTh, rearTh, interval);
                    break;
                case ManualControlMode.Crab:
                    SetChassisDirection(90);
                    Chassis.SendMotion(speed, frontTh, rearTh, interval);
                    break;
                case ManualControlMode.Spin:
                    Chassis.SendRotateMotion(speed * MaxAngularSpeed, interval);
                    break;
                default:
                    ManualMode = -1;
                    Chassis.PredefinedDriveStop();
                    break;
            }
        }

        private void SetChassisDirection(float direction)
        {
            var origin = Chassis.GetOriginBias();
            // 方向没有变化时不重复计算轮组几何关系。
            if (Math.Abs(origin.Z - direction) < 0.001f)
                return;
            Chassis.SetOriginBias(
                origin.X,
                origin.Y,
                direction);
            // 坐标系改变后重新等待四个舵轮对齐。
            Chassis.AfterDirectionChanged();
        }

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

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

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

        // 以下夹臂字段用于兼容原MCU的0x209、0x20A等CAN协议；当前不使用时目标速度保持为0。
        [AsUpperIO(desc = "左夹臂目标速度", timeOutReset = true)]
        public float SpeedLeftArm;

        [AsUpperIO(desc = "右夹臂目标速度", timeOutReset = true)]
        public float SpeedRightArm;

        [AsLowerIO(desc = "左夹臂实际速度")]
        public float ActualSpeedLeftArm;

        [AsLowerIO(desc = "右夹臂实际速度")]
        public float ActualSpeedRightArm;

        [AsLowerIO(desc = "左夹臂实际位置")]
        public float ActualPosLeftArm;

        [AsLowerIO(desc = "右夹臂实际位置")]
        public float ActualPosRightArm;

        [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;

        [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;

        [AsLowerIO(desc = "左夹臂驱动器远程帧709")]
        public byte LArmRemoteCode;

        [AsLowerIO(desc = "右夹臂驱动器远程帧70A")]
        public byte RArmRemoteCode;

        #endregion

    }
}