using CartActivator;
using FundamentalLib;
using MCUSerialBridgeCLR;
using System;
using System.Collections.Generic;

namespace MedullaAdapter
{
    //[LogicRunOnMCU(scanInterval = 20)]
    public class MCURoutine:LadderLogic<DiverCartDefinition>
    {
        private int _lastIteration = 0;
        private int _count = 0;
        private int _operationTime = 0;
        private bool _driversDisabled;
        private bool _canCallbackRegistered;
        private bool _serialCallbackRegistered;
        private byte _resetCode = 0x86;
        private byte _enableCode1 = 0x06;
        private byte _enableCode2 = 0x07;
        private byte _enableCode3 = 0x0F;
        private bool io_bit0 = false;//继电器
        private bool io_bit1 = false;//抱闸
        private bool io_bit2 = false;//红灯
        private bool io_bit3 = false;//绿灯
        private bool io_bit4 = false;//黄灯
        private const byte BatteryPortIndex = 3;
        private static readonly byte[] BatteryRequest = BuildBatteryRequest();

        private static float ConvertMps2Rpm(float mps)
        {
            return (float)(mps / (Math.PI * 85f) * 10.5f * 60f * 1000f);
        }
        private static float ConvertRpm2Mps(float rpm)
        {
            return (float)(rpm / 10.5f / 60f * Math.PI * 85f / 1000f);
        }
        private float ConvertR2MM(float r)
        {
            return (float)(r / 10.5f * Math.PI * 85);
        }
        private static float DecodeRpmFromPayload(byte[] payload, int offset = 4)
        {
            return BitConverter.ToInt32(payload, offset) * 1875f / 512f / 10000f;
        }
        public override void Operation(int iteration)
        {
            if (_lastIteration != iteration)
            {
                _lastIteration = iteration;
                _count = 0;
            }
            else
            {
                _count++;
            }
            if (_count >= 15)
            {
                cart.AlarmLevel = 2;
                Console.WriteLine("mcu lost connection \n");
            }

            if (_count >= 30)
            {
                return;
            }
            if (cart?.Bridge == null)
            {
                return;
            }
            //注册CAN回调
            EnsureCanCallbacksRegistered();
            //注册串口回调
            EnsureSerialCallbacksRegistered();
            PollBatterySerial(iteration);

            #region io部分
            var ioReadBuffer = new byte[4];
            var readInputErr = cart.Bridge.ReadInput(out ioReadBuffer, 20);
            if (readInputErr == MCUSerialBridgeError.OK)
            {
                cart.test = ioReadBuffer[0];
                cart.Start = (ioReadBuffer[0] & (1 << 0)) != 0;
                cart.ResetPressed = (ioReadBuffer[0] & (1 << 1)) != 0;
                cart.ChassisMode = ((ioReadBuffer[0] & (1 << 2)) == 0) ? 0 : 1;
                cart.BrakeEnable = (ioReadBuffer[0] & (1 << 3)) != 0;
                if ((ioReadBuffer[0] & (1 << 4)) == 0)
                {
                    cart.EmergencyPressed = 1;
                }
            }
            else
            {
                Console.WriteLine("IO Read FAILED");
            }

            var ioWriteBuffer = new byte[4];
            io_bit0 = cart.ChargePort ? true : false;
            io_bit1 = cart.EmergencyPressed == 1 ? false : true;
            if (cart.MultiVehicleLightSync == 0)
            {
                io_bit2 = false;
                io_bit3 = false;
                io_bit4 = false;
            }
            else if (cart.MultiVehicleLightSync == 1)
            {
                io_bit2 = false;
                io_bit3 = true;
                io_bit4 = false;
            }
            else
            {
                if (cart.LightMode == 2)
                {
                    io_bit2 = true;//红
                    io_bit3 = false;//绿
                    io_bit4 = false;//黄
                }
                else if (cart.LightMode == 3)
                {
                    io_bit2 = false;
                    io_bit3 = false;
                    io_bit4 = true;
                }
                else if (cart.LightMode == 1)
                {
                    io_bit2 = false;
                    io_bit3 = true;
                    io_bit4 = false;
                }
                else
                {
                    io_bit2 = false;
                    io_bit3 = false;
                    io_bit4 = false;
                }
            }

            ioWriteBuffer[0] = (byte)((io_bit0 ? 1 << 0 : 0) | //继电器
                                    (io_bit1 ? 1 << 1 : 0) | //抱闸
                                    (io_bit2 ? 1 << 2 : 0) | //红灯
                                    (io_bit3 ? 1 << 3 : 0) | //绿灯
                                    (io_bit4 ? 1 << 4 : 0)); //黄灯
            if (cart.Bridge.WriteOutput(ioWriteBuffer, 20) != MCUSerialBridgeError.OK)
            {
                Console.WriteLine("IO Write FAILED");
            }
            #endregion

            #region 驱动部分
            cart.ActualSpeedLeftFront = (cart.ActualSpeedLeftFrontLeft + cart.ActualSpeedLeftFrontRight) / 2;
            cart.ActualSpeedLeftRear = (cart.ActualSpeedLeftRearLeft + cart.ActualSpeedLeftRearRight) / 2;
            cart.ActualSpeedRightFront = (cart.ActualSpeedRightFrontLeft + cart.ActualSpeedRightFrontRight) / 2;
            cart.ActualSpeedRightRear = (cart.ActualSpeedRightRearLeft + cart.ActualSpeedRightRearRight) / 2;
            MCUSerialBridgeError SendCan(byte port, ushort standardId, byte[] payload, bool RTR = false, uint timeout = 2)
            {
                var canSend = new CANMessage
                {
                    ID = standardId,
                    RTR = RTR,
                    DLC = !RTR ? (byte)8 : (byte)0,
                    Payload = payload ?? new byte[] { }
                };
                return cart.Bridge.WriteCAN(port, canSend, timeout);
            }

            var resetPayload = new byte[8] { 0xFD, _resetCode, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
            var enable1Payload = new byte[8] { 0xFD, _enableCode1, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
            var enable2Payload = new byte[8] { 0xFD, _enableCode2, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
            var enable3Payload = new byte[8] { 0xFD, _enableCode3, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };

            Hedingben.ToastText($"_operationTime:{_operationTime}", "OperationTime");
            if (_operationTime == 0)
            {
                SendCan(0, 0x00, new byte[] { 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 });
                _operationTime++;
                return;
            }
            else if (_operationTime == 1)
            {
                if (cart.LeftFrontLeftErrorCode != 0)
                {
                    SendCan(0, 0x201, enable1Payload);
                    SendCan(0, 0x201, resetPayload);
                }
                if (cart.LeftFrontRightErrorCode != 0)
                {
                    SendCan(0, 0x202, enable1Payload);
                    SendCan(0, 0x202, resetPayload);
                }
                if (cart.RightFrontLeftErrorCode != 0)
                {
                    SendCan(0, 0x203, enable1Payload);
                    SendCan(0, 0x203, resetPayload);
                }
                if (cart.RightFrontRightErrorCode != 0)
                {
                    SendCan(0, 0x204, enable1Payload);
                    SendCan(0, 0x204, resetPayload);
                }
                if (cart.LeftRearLeftErrorCode != 0)
                {
                    SendCan(0, 0x205, enable1Payload);
                    SendCan(0, 0x205, resetPayload);
                }
                if (cart.LeftRearRightErrorCode != 0)
                {
                    SendCan(0, 0x206, enable1Payload);
                    SendCan(0, 0x206, resetPayload);
                }
                if (cart.RightRearLeftErrorCode != 0)
                {
                    SendCan(0, 0x207, enable1Payload);
                    SendCan(0, 0x207, resetPayload);
                }
                if (cart.RightRearRightErrorCode != 0)
                {
                    SendCan(0, 0x208, enable1Payload);
                    SendCan(0, 0x208, resetPayload);
                }
                if (cart.LeftArmErrorCode != 0)
                {
                    SendCan(0, 0x209, enable1Payload);
                    SendCan(0, 0x209, resetPayload);
                }
                if (cart.RightArmErrorCode != 0)
                {
                    SendCan(0, 0x20A, enable1Payload);
                    SendCan(0, 0x20A, resetPayload);
                }
                _operationTime++;
                return;
            }
            else if (_operationTime == 2)
            {
                //SendNodeGuardRequests(SendCan);
                //if (AreAllNodesOperational())
                //{
                //    _operationTime++;
                //}
                _operationTime++;
                return;
            }
            //检查错误是否被消除 若没被消除就得重新返回上一步发复位 若消除了就继续往下
            else if (_operationTime == 3)
            {
                if (cart.LeftFrontLeftErrorCode != 0 || cart.LeftFrontRightErrorCode != 0 ||
                cart.LeftRearLeftErrorCode != 0 || cart.LeftRearRightErrorCode != 0
                || cart.RightFrontLeftErrorCode != 0 || cart.RightFrontRightErrorCode != 0 ||
                cart.RightRearLeftErrorCode != 0 || cart.RightRearRightErrorCode != 0 ||
                cart.LeftArmErrorCode != 0 || cart.RightArmErrorCode != 0)
                {
                    _operationTime = 1;
                    return;
                }
                else
                {
                    _operationTime++;
                    return;
                }
            }
            // 没有错误 没有节点保护 就发06 07 0F使能
            else if (_operationTime == 4)
            {
                SendCan(0, 0x201, enable1Payload);
                SendCan(0, 0x202, enable1Payload);
                SendCan(0, 0x203, enable1Payload);
                SendCan(0, 0x204, enable1Payload);
                SendCan(0, 0x205, enable1Payload);
                SendCan(0, 0x206, enable1Payload);
                SendCan(0, 0x207, enable1Payload);
                SendCan(0, 0x208, enable1Payload);
                SendCan(0, 0x209, enable1Payload);
                SendCan(0, 0x20A, enable1Payload);
                _operationTime++;
                return;
            }
            else if (_operationTime == 5)
            {
                SendCan(0, 0x201, enable2Payload);
                SendCan(0, 0x202, enable2Payload);
                SendCan(0, 0x203, enable2Payload);
                SendCan(0, 0x204, enable2Payload);
                SendCan(0, 0x205, enable2Payload);
                SendCan(0, 0x206, enable2Payload);
                SendCan(0, 0x207, enable2Payload);
                SendCan(0, 0x208, enable2Payload);
                SendCan(0, 0x209, enable2Payload);
                SendCan(0, 0x20A, enable2Payload);
                _operationTime++;
                return;
            }
            else if (_operationTime == 6)
            {
                SendCan(0, 0x201, enable3Payload);
                SendCan(0, 0x202, enable3Payload);
                SendCan(0, 0x203, enable3Payload);
                SendCan(0, 0x204, enable3Payload);
                SendCan(0, 0x205, enable3Payload);
                SendCan(0, 0x206, enable3Payload);
                SendCan(0, 0x207, enable3Payload);
                SendCan(0, 0x208, enable3Payload);
                SendCan(0, 0x209, enable3Payload);
                SendCan(0, 0x20A, enable3Payload);
                if (cart.LeftFrontLeftErrorCode != 0 || cart.LeftFrontRightErrorCode != 0 ||
                cart.LeftRearLeftErrorCode != 0 || cart.LeftRearRightErrorCode != 0
                || cart.RightFrontLeftErrorCode != 0 || cart.RightFrontRightErrorCode != 0 ||
                cart.RightRearLeftErrorCode != 0 || cart.RightRearRightErrorCode != 0 ||
                cart.LeftArmErrorCode != 0 || cart.RightArmErrorCode != 0)
                {
                    _operationTime = 1;
                    return;
                }
                else
                {
                    _operationTime++;
                    return;
                }
            }

            if (cart.ResetPressed || cart.ResetFromM || cart.ResetFromC)
            {
                cart.AlarmLevel = -1;
                cart.EmergencyPressed = 0;
                _count = 0;
                _operationTime = 0;
                _driversDisabled = false;
                cart.WheelAbleState = true;
                cart.ResetFromM = false;
            }

            if (cart.DisableFromM || cart.DisableFromC)
            {
                _driversDisabled = true;
                cart.WheelAbleState = false;
                cart.DisableFromM = false;
            }

            var lfl = Math.Sign(cart.SpeedLFL) * Math.Min(Math.Max(0, cart.SendThresSpeed), Math.Abs(cart.SpeedLFL));
            var lfr = Math.Sign(cart.SpeedLFR) * Math.Min(Math.Max(0, cart.SendThresSpeed), Math.Abs(cart.SpeedLFR));
            var rfl = Math.Sign(cart.SpeedRFL) * Math.Min(Math.Max(0, cart.SendThresSpeed), Math.Abs(cart.SpeedRFL));
            var rfr = Math.Sign(cart.SpeedRFR) * Math.Min(Math.Max(0, cart.SendThresSpeed), Math.Abs(cart.SpeedRFR));
            var lrl = Math.Sign(cart.SpeedLRL) * Math.Min(Math.Max(0, cart.SendThresSpeed), Math.Abs(cart.SpeedLRL));
            var lrr = Math.Sign(cart.SpeedLRR) * Math.Min(Math.Max(0, cart.SendThresSpeed), Math.Abs(cart.SpeedLRR));
            var rrl = Math.Sign(cart.SpeedRRL) * Math.Min(Math.Max(0, cart.SendThresSpeed), Math.Abs(cart.SpeedRRL));
            var rrr = Math.Sign(cart.SpeedRRR) * Math.Min(Math.Max(0, cart.SendThresSpeed), Math.Abs(cart.SpeedRRR));

            var v1 = ConvertMps2Rpm(lfl);
            var v2 = ConvertMps2Rpm(-lfr);
            var v3 = ConvertMps2Rpm(rfl);
            var v4 = ConvertMps2Rpm(-rfr);
            var v5 = ConvertMps2Rpm(lrl);
            var v6 = ConvertMps2Rpm(-lrr);
            var v7 = ConvertMps2Rpm(rrl);
            var v8 = ConvertMps2Rpm(-rrr);
            var v9 = ConvertMps2Rpm(cart.SpeedLeftArm);
            var v10 = ConvertMps2Rpm(cart.SpeedRightArm);
            if (cart.AlarmLevel == 2 || cart.WaitStart || cart.PreparaStart)
            {
                v1 = v2 = v3 = v4 = v5 = v6 = v7 = v8 = v9 = v10 = 0;
            }
            if ((cart.ActualPosLeftArm <= cart.LeftArmLowerPos && cart.SpeedLeftArm < 0) || (cart.ActualPosLeftArm >= cart.LeftArmUpperPos && cart.SpeedLeftArm > 0))
            {
                v9 = 0;
            }
            if ((cart.ActualPosRightArm <= cart.RightArmLowerPos && cart.SpeedRightArm < 0) || (cart.ActualPosRightArm >= cart.RightArmUpperPos && cart.SpeedRightArm > 0))
            {
                v10 = 0;
            }

            var sendLFL = BitConverter.GetBytes((int)Math.Round(v1 * 512f * 10000f / 1875f));
            var sendLFR = BitConverter.GetBytes((int)Math.Round(v2 * 512f * 10000f / 1875f));
            var sendRFL = BitConverter.GetBytes((int)Math.Round(v3 * 512f * 10000f / 1875f));
            var sendRFR = BitConverter.GetBytes((int)Math.Round(v4 * 512f * 10000f / 1875f));
            var sendLRL = BitConverter.GetBytes((int)Math.Round(v5 * 512f * 10000f / 1875f));
            var sendLRR = BitConverter.GetBytes((int)Math.Round(v6 * 512f * 10000f / 1875f));
            var sendRRL = BitConverter.GetBytes((int)Math.Round(v7 * 512f * 10000f / 1875f));
            var sendRRR = BitConverter.GetBytes((int)Math.Round(v8 * 512f * 10000f / 1875f));
            var sendLArm = BitConverter.GetBytes((int)Math.Round(v9 * 512f * 10000f / 1875f));
            var sendRArm = BitConverter.GetBytes((int)Math.Round(v10 * 512f * 10000f / 1875f));

            if (iteration % 2 == 0)
            {
                //SendNodeGuardRequests(SendCan);
            }

            if (_driversDisabled)
            {
                SendCan(0, 0x201, enable1Payload);
                SendCan(0, 0x202, enable1Payload);
                SendCan(0, 0x203, enable1Payload);
                SendCan(0, 0x204, enable1Payload);
                SendCan(0, 0x205, enable1Payload);
                SendCan(0, 0x206, enable1Payload);
                SendCan(0, 0x207, enable1Payload);
                SendCan(0, 0x208, enable1Payload);
            }
            else
            {
                SendCan(0, 0x201, new byte[] { 0xFD, _enableCode3, 0x00, sendLFL[0], sendLFL[1], sendLFL[2], sendLFL[3], 0x00 });
                SendCan(0, 0x202, new byte[] { 0xFD, _enableCode3, 0x00, sendLFR[0], sendLFR[1], sendLFR[2], sendLFR[3], 0x00 });
                SendCan(0, 0x203, new byte[] { 0xFD, _enableCode3, 0x00, sendRFL[0], sendRFL[1], sendRFL[2], sendRFL[3], 0x00 });
                SendCan(0, 0x204, new byte[] { 0xFD, _enableCode3, 0x00, sendRFR[0], sendRFR[1], sendRFR[2], sendRFR[3], 0x00 });
                SendCan(0, 0x205, new byte[] { 0xFD, _enableCode3, 0x00, sendLRL[0], sendLRL[1], sendLRL[2], sendLRL[3], 0x00 });
                SendCan(0, 0x206, new byte[] { 0xFD, _enableCode3, 0x00, sendLRR[0], sendLRR[1], sendLRR[2], sendLRR[3], 0x00 });
                SendCan(0, 0x207, new byte[] { 0xFD, _enableCode3, 0x00, sendRRL[0], sendRRL[1], sendRRL[2], sendRRL[3], 0x00 });
                SendCan(0, 0x208, new byte[] { 0xFD, _enableCode3, 0x00, sendRRR[0], sendRRR[1], sendRRR[2], sendRRR[3], 0x00 });
            }
            SendCan(0, 0x209, new byte[] { 0xFD, _enableCode3, 0x00, sendLArm[0], sendLArm[1], sendLArm[2], sendLArm[3], 0x00 });
            SendCan(0, 0x20A, new byte[] { 0xFD, _enableCode3, 0x00, sendRArm[0], sendRArm[1], sendRArm[2], sendRArm[3], 0x00 });

            if (cart.LeftFrontLeftErrorCode != 0 || cart.LeftFrontRightErrorCode != 0 ||
                cart.LeftRearLeftErrorCode != 0 || cart.LeftRearRightErrorCode != 0
                || cart.RightFrontLeftErrorCode != 0 || cart.RightFrontRightErrorCode != 0 ||
                cart.RightRearLeftErrorCode != 0 || cart.RightRearRightErrorCode != 0)
            {
                //_errorCount++;
            }
            
            #endregion

        }

        private static bool IsNodeOperational(byte remoteCode)
        {
            return remoteCode == 5 || remoteCode == 133;
        }

        private bool AreAllNodesOperational()
        {
            return IsNodeOperational(cart.LFLRemoteCode)
                && IsNodeOperational(cart.LFRRemoteCode)
                && IsNodeOperational(cart.RFLRemoteCode)
                && IsNodeOperational(cart.RFRRemoteCode)
                && IsNodeOperational(cart.LRLRemoteCode)
                && IsNodeOperational(cart.LRRRemoteCode)
                && IsNodeOperational(cart.RRLRemoteCode)
                && IsNodeOperational(cart.RRRRemoteCode)
                && IsNodeOperational(cart.LArmRemoteCode)
                && IsNodeOperational(cart.RArmRemoteCode);
        }

        private static void SendNodeGuardRequests(Func<byte, ushort, byte[], bool, uint, MCUSerialBridgeError> sendCan)
        {
            sendCan(0, 0x709, Array.Empty<byte>(), true, 2);
            sendCan(0, 0x70A, Array.Empty<byte>(), true, 2);
            sendCan(0, 0x701, Array.Empty<byte>(), true, 2);
            sendCan(0, 0x702, Array.Empty<byte>(), true, 2);
            sendCan(0, 0x703, Array.Empty<byte>(), true, 2);
            sendCan(0, 0x704, Array.Empty<byte>(), true, 2);
            sendCan(0, 0x705, Array.Empty<byte>(), true, 2);
            sendCan(0, 0x706, Array.Empty<byte>(), true, 2);
            sendCan(0, 0x707, Array.Empty<byte>(), true, 2);
            sendCan(0, 0x708, Array.Empty<byte>(), true, 2);
        }

        private void EnsureCanCallbacksRegistered()
        {
            if (_canCallbackRegistered)
            {
                return;
            }
            if (cart?.Bridge == null)
            {
                return;
            }
            var dispatch = BuildCanDispatchTable();

            var err0 = cart.Bridge.RegisterCANPortCallback(0, msg =>
            {
                if (msg == null) return;
                if (dispatch.TryGetValue(msg.ID, out var handler))
                {
                    handler(msg);
                }
            });
            _canCallbackRegistered = true;
        }

        private void EnsureSerialCallbacksRegistered()
        {
            if (_serialCallbackRegistered)
            {
                return;
            }
            if (cart?.Bridge == null)
            {
                return;
            }
            var err1 = cart.Bridge.RegisterSerialPortCallback(1, msg =>
            {
                if (msg == null) return;
                Hedingben.ToastText($"Callback Serial 1 Callback Received: {BitConverter.ToString(msg)}", "Serial 1");
            });
            Hedingben.ToastText($"Register Serial 1: {err1}", "Register Serial");
            _serialCallbackRegistered = err1 == MCUSerialBridgeError.OK;
        }

        private void PollBatterySerial(int iteration)
        {
            if (cart?.Bridge == null) return;
            if (iteration % 20 != 0) return;

            var writeErr = cart.Bridge.WriteSerial(BatteryPortIndex, BatteryRequest, 60);
            if (writeErr != MCUSerialBridgeError.OK)
            {
                Hedingben.ToastText($"Battery write err: {writeErr}", "Serial 2");
                return;
            }

            var readErr = cart.Bridge.ReadSerial(BatteryPortIndex, out var msg, 40);
            if (readErr == MCUSerialBridgeError.NoData) return;
            if (readErr != MCUSerialBridgeError.OK)
            {
                Hedingben.ToastText($"Battery read err: {readErr}", "Serial 2");
                return;
            }
            if (msg == null || msg.Length == 0) return;

            Hedingben.ToastText($"Serial 2 RX: {BitConverter.ToString(msg)}", "Serial 2");
            TryUpdateBatteryData(msg);
        }

        private void TryUpdateBatteryData(byte[] msg)
        {
            // Modbus RTU response: [id,03,08,data(8),crc(2)]
            if (msg.Length < 13 || msg[0] != 0x01 || msg[1] != 0x03 || msg[2] < 8) return;
            ushort calc = ComputeModbusCrc(msg, msg.Length - 2);
            ushort recv = (ushort)(msg[msg.Length - 2] | (msg[msg.Length - 1] << 8));
            if (calc != recv) return;

            cart.Voltage = ReadInt16BE(msg, 3) * 0.1f;
            cart.Soc = ReadUInt16BE(msg, 5);
            cart.SOH = ReadUInt16BE(msg, 7);
            cart.ElectricCurrent = -ReadInt16BE(msg, 9) * 0.1f;
        }

        private static byte[] BuildBatteryRequest()
        {
            // 01 03 00 03 00 04 CRC (读取寄存器 03~06)
            byte[] req = new byte[] { 0x01, 0x03, 0x00, 0x03, 0x00, 0x04, 0x00, 0x00 };
            ushort crc = ComputeModbusCrc(req, 6);
            req[6] = (byte)(crc & 0xFF);
            req[7] = (byte)((crc >> 8) & 0xFF);
            return req;
        }

        private static ushort ComputeModbusCrc(byte[] data, int length)
        {
            ushort crc = 0xFFFF;
            for (int i = 0; i < length; i++)
            {
                crc ^= data[i];
                for (int j = 0; j < 8; j++)
                {
                    bool lsb = (crc & 0x0001) != 0;
                    crc >>= 1;
                    if (lsb) crc ^= 0xA001;
                }
            }
            return crc;
        }

        private static short ReadInt16BE(byte[] data, int offset)
        {
            return (short)((data[offset] << 8) | data[offset + 1]);
        }

        private static ushort ReadUInt16BE(byte[] data, int offset)
        {
            return (ushort)((data[offset] << 8) | data[offset + 1]);
        }

        private Dictionary<ushort, Action<CANMessage>> BuildCanDispatchTable()
        {
            return new Dictionary<ushort, Action<CANMessage>>
            {
                // 速度/位置反馈 0x281~0x28A
                [0x281] = (msg) =>
                {
                    //Console.WriteLine("Received 0x281 CAN Message");
                    var payload = msg.Payload;
                    if (payload == null || payload.Length < 8) return;
                    var rpm = DecodeRpmFromPayload(payload);
                    cart.ActualSpeedLeftFrontLeft = ConvertRpm2Mps(rpm);
                    cart.LFLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
                },
                [0x282] = (msg) =>
                {
                    //Console.WriteLine("Received 0x282 CAN Message");
                    var payload = msg.Payload;
                    if (payload == null || payload.Length < 8) return;
                    var rpm = DecodeRpmFromPayload(payload);
                    cart.ActualSpeedLeftFrontRight = -ConvertRpm2Mps(rpm);
                    cart.LFRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
                },
                [0x283] = (msg) =>
                {
                    //Console.WriteLine("Received 0x283 CAN Message");
                    var payload = msg.Payload;
                    if (payload == null || payload.Length < 8) return;
                    var rpm = DecodeRpmFromPayload(payload);
                    cart.ActualSpeedRightFrontLeft = ConvertRpm2Mps(rpm);
                    cart.RFLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
                },
                [0x284] = (msg) =>
                {
                    //Console.WriteLine("Received 0x284 CAN Message");
                    var payload = msg.Payload;
                    if (payload == null || payload.Length < 8) return;
                    var rpm = DecodeRpmFromPayload(payload);
                    cart.ActualSpeedRightFrontRight = -ConvertRpm2Mps(rpm);
                    cart.RFRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
                },
                [0x285] = (msg) =>
                {
                    //Console.WriteLine("Received 0x285 CAN Message");
                    var payload = msg.Payload;
                    if (payload == null || payload.Length < 8) return;
                    var rpm = DecodeRpmFromPayload(payload);
                    cart.ActualSpeedLeftRearLeft = ConvertRpm2Mps(rpm);
                    cart.LRLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
                },
                [0x286] = (msg) =>
                {
                    //Console.WriteLine("Received 0x286 CAN Message");
                    var payload = msg.Payload;
                    if (payload == null || payload.Length < 8) return;
                    var rpm = DecodeRpmFromPayload(payload);
                    cart.ActualSpeedLeftRearRight = -ConvertRpm2Mps(rpm);
                    cart.LRRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
                },
                [0x287] = (msg) =>
                {
                    //Console.WriteLine("Received 0x287 CAN Message");
                    var payload = msg.Payload;
                    if (payload == null || payload.Length < 8) return;
                    var rpm = DecodeRpmFromPayload(payload);
                    cart.ActualSpeedRightRearLeft = ConvertRpm2Mps(rpm);
                    cart.RRLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
                },
                [0x288] = (msg) =>
                {
                    //Console.WriteLine("Received 0x288 CAN Message");
                    var payload = msg.Payload;
                    if (payload == null || payload.Length < 8) return;
                    var rpm = DecodeRpmFromPayload(payload);
                    cart.ActualSpeedRightRearRight = -ConvertRpm2Mps(rpm);
                    cart.RRRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
                },
                [0x289] = (msg) =>
                {
                    //Console.WriteLine("Received 0x289 CAN Message");
                    var payload = msg.Payload;
                    if (payload == null || payload.Length < 8) return;
                    var rpm = DecodeRpmFromPayload(payload);
                    cart.ActualSpeedLeftArm = ConvertRpm2Mps(rpm);
                    cart.ActualPosLeftArm = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
                },
                [0x28A] = (msg) =>
                {
                    //Console.WriteLine("Received 0x28A CAN Message");
                    var payload = msg.Payload;
                    if (payload == null || payload.Length < 8) return;
                    var rpm = DecodeRpmFromPayload(payload);
                    cart.ActualSpeedRightArm = ConvertRpm2Mps(rpm);
                    cart.ActualPosRightArm = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
                },
                // 状态/错误码 0x181~0x18A
                [0x181] = (msg) =>
                {
                    //Console.WriteLine("Received 0x181 CAN Message");
                    var payload = msg.Payload;
                    if (payload == null || payload.Length < 4) return;
                    cart.LeftFrontLeftStateCode = BitConverter.ToInt16(payload, 0);
                    cart.LeftFrontLeftErrorCode = BitConverter.ToInt16(payload, 2);
                    cart.LeftFrontLeftElectric = BitConverter.ToInt16(payload, 5) * 0.001f;
                },
                [0x182] = (msg) =>
                {
                    //Console.WriteLine("Received 0x182 CAN Message");
                    var payload = msg.Payload;
                    if (payload == null || payload.Length < 4) return;
                    cart.LeftFrontRightStateCode = BitConverter.ToInt16(payload, 0);
                    cart.LeftFrontRightErrorCode = BitConverter.ToInt16(payload, 2);
                    cart.LeftFrontRightElectric = BitConverter.ToInt16(payload, 5) * 0.001f;
                },
                [0x183] = (msg) =>
                {
                    //Console.WriteLine("Received 0x183 CAN Message");
                    var payload = msg.Payload;
                    if (payload == null || payload.Length < 4) return;
                    cart.RightFrontLeftStateCode = BitConverter.ToInt16(payload, 0);
                    cart.RightFrontLeftErrorCode = BitConverter.ToInt16(payload, 2);
                    cart.RightFrontLeftElectric = BitConverter.ToInt16(payload, 5) * 0.001f;
                },
                [0x184] = (msg) =>
                {
                    //Console.WriteLine("Received 0x184 CAN Message");
                    var payload = msg.Payload;
                    if (payload == null || payload.Length < 4) return;
                    cart.RightFrontRightStateCode = BitConverter.ToInt16(payload, 0);
                    cart.RightFrontRightErrorCode = BitConverter.ToInt16(payload, 2);
                    cart.RightFrontRightElectric = BitConverter.ToInt16(payload, 5) * 0.001f;
                },
                [0x185] = (msg) =>
                {
                    //Console.WriteLine("Received 0x185 CAN Message");
                    var payload = msg.Payload;
                    if (payload == null || payload.Length < 4) return;
                    cart.LeftRearLeftStateCode = BitConverter.ToInt16(payload, 0);
                    cart.LeftRearLeftErrorCode = BitConverter.ToInt16(payload, 2);
                    cart.LeftRearLeftElectric = BitConverter.ToInt16(payload, 5) * 0.001f;
                },
                [0x186] = (msg) =>
                {
                    //Console.WriteLine("Received 0x186 CAN Message");
                    var payload = msg.Payload;
                    if (payload == null || payload.Length < 4) return;
                    cart.LeftRearRightStateCode = BitConverter.ToInt16(payload, 0);
                    cart.LeftRearRightErrorCode = BitConverter.ToInt16(payload, 2);
                    cart.LeftRearRightElectric = BitConverter.ToInt16(payload, 5) * 0.001f;
                },
                [0x187] = (msg) =>
                {
                    //Console.WriteLine("Received 0x187 CAN Message");
                    var payload = msg.Payload;
                    if (payload == null || payload.Length < 4) return;
                    cart.RightRearLeftStateCode = BitConverter.ToInt16(payload, 0);
                    cart.RightRearLeftErrorCode = BitConverter.ToInt16(payload, 2);
                    cart.RightRearLeftElectric = BitConverter.ToInt16(payload, 5) * 0.001f;
                },
                [0x188] = (msg) =>
                {
                    //Console.WriteLine("Received 0x188 CAN Message");
                    var payload = msg.Payload;
                    if (payload == null || payload.Length < 4) return;
                    cart.RightRearRightStateCode = BitConverter.ToInt16(payload, 0);
                    cart.RightRearRightErrorCode = BitConverter.ToInt16(payload, 2);
                    cart.RightRearRightElectric = BitConverter.ToInt16(payload, 5) * 0.001f;
                },
                [0x189] = (msg) =>
                {
                    //Console.WriteLine("Received 0x189 CAN Message");
                    var payload = msg.Payload;
                    if (payload == null || payload.Length < 4) return;
                    cart.LeftArmStateCode = BitConverter.ToInt16(payload, 0);
                    cart.LeftArmErrorCode = BitConverter.ToInt16(payload, 2);
                    cart.LeftArmElectric = BitConverter.ToInt16(payload, 5) * 0.001f;
                },
                [0x18A] = (msg) =>
                {
                    //Console.WriteLine("Received 0x18A CAN Message");
                    var payload = msg.Payload;
                    if (payload == null || payload.Length < 4) return;
                    cart.RightArmStateCode = BitConverter.ToInt16(payload, 0);
                    cart.RightArmErrorCode = BitConverter.ToInt16(payload, 2);
                    cart.RightArmElectric = BitConverter.ToInt16(payload, 5) * 0.001f;
                },

                // 舵角 0x18B~0x18E 
                [0x18B] = (msg) =>
                {
                    //Console.WriteLine("Received 0x18B CAN Message");
                    var payload = msg.Payload;
                    if (payload == null || payload.Length < 4) return;
                    cart.ActualThLeftFront = BitConverter.ToInt32(payload, 0);
                    cart.ActualThLeftFront = cart.ActualThLeftFront >= 16384
                    ? (cart.ActualThLeftFront - 98303) / 4096f / 5f * 360 - cart.ThBiasLeftFront
                    : cart.ActualThLeftFront / 4096f / 5f * 360 - cart.ThBiasLeftFront;
                },
                [0x18C] = (msg) =>
                {
                    var payload = msg.Payload;
                    if (payload == null || payload.Length < 4) return;
                    var raw = BitConverter.ToInt32(payload, 0);
                    cart.ActualThRightFront = raw >= 16384
                        ? (raw - 98303) / 4096f / 5f * 360 - cart.ThBiasRightFront
                        : raw / 4096f / 5f * 360 - cart.ThBiasRightFront;
                    //DLog.Log($"RF raw=0x{raw:X8}({raw}) angle={cart.ActualThRightFront:F2}", "0x18C");
                },
                [0x18D] = (msg) =>
                {
                    //Console.WriteLine($"Received 0x18D CAN Message {DateTime.Now:yyyy-MM-dd HH:mm:ss.ffffff}"); 
                    var payload = msg.Payload;
                    if (payload == null || payload.Length < 4) return;  
                    cart.ActualThLeftRear = BitConverter.ToInt32(payload, 0);
                    cart.ActualThLeftRear = cart.ActualThLeftRear >= 16384
                        ? (cart.ActualThLeftRear - 98303) / 4096f / 5f * 360 - cart.ThBiasLeftRear
                        : cart.ActualThLeftRear / 4096f / 5f * 360 - cart.ThBiasLeftRear;
                },
                [0x18E] = (msg) =>
                {
                    //Console.WriteLine("Received 0x18E CAN Message");
                    var payload = msg.Payload;
                    if (payload == null || payload.Length < 4) return;
                    cart.ActualThRightRear = BitConverter.ToInt32(payload, 0);
                    cart.ActualThRightRear = cart.ActualThRightRear >= 16384
                        ? (cart.ActualThRightRear - 98303) / 4096f / 5f * 360 - cart.ThBiasRightRear
                        : cart.ActualThRightRear / 4096f / 5f * 360 - cart.ThBiasRightRear;
                },

                // 远程帧
                [0x701] = (msg) =>
                {
                    //Console.WriteLine("Received 0x701 CAN Message");
                    var payload = msg.Payload;
                    if (payload == null || payload.Length < 1) return;
                    cart.LFLRemoteCode = payload[0];
                },
                [0x702] = (msg) =>
                {
                    //Console.WriteLine("Received 0x702 CAN Message");
                    var payload = msg.Payload;
                    if (payload == null || payload.Length < 1) return;
                    cart.LFRRemoteCode = payload[0];
                },
                [0x703] = (msg) =>
                {
                    //Console.WriteLine("Received 0x703 CAN Message");
                    var payload = msg.Payload;
                    if (payload == null || payload.Length < 1) return;
                    cart.RFLRemoteCode = payload[0];
                },
                [0x704] = (msg) =>
                {
                    //Console.WriteLine("Received 0x704 CAN Message");
                    var payload = msg.Payload;
                    if (payload == null || payload.Length < 1) return;
                    cart.RFRRemoteCode = payload[0];
                },
                [0x705] = (msg) =>
                {
                    //Console.WriteLine("Received 0x705 CAN Message");
                    var payload = msg.Payload;
                    if (payload == null || payload.Length < 1) return;
                    cart.LRLRemoteCode = payload[0];
                },
                [0x706] = (msg) =>
                {
                    //Console.WriteLine("Received 0x706 CAN Message");
                    var payload = msg.Payload;
                    if (payload == null || payload.Length < 1) return;
                    cart.LRRRemoteCode = payload[0];
                },
                [0x707] = (msg) =>
                {
                    //Console.WriteLine("Received 0x707 CAN Message");
                    var payload = msg.Payload;
                    if (payload == null || payload.Length < 1) return;
                    cart.RRLRemoteCode = payload[0];
                },
                [0x708] = (msg) =>
                {
                    //Console.WriteLine("Received 0x708 CAN Message");
                    var payload = msg.Payload;
                    if (payload == null || payload.Length < 1) return;
                    cart.RRRRemoteCode = payload[0];
                },
                [0x709] = (msg) =>
                {
                    //Console.WriteLine("Received 0x709 CAN Message");
                    var payload = msg.Payload;
                    if (payload == null || payload.Length < 1) return;
                    cart.LArmRemoteCode = payload[0];
                },
                [0x70A] = (msg) =>
                {
                    //Console.WriteLine("Received 0x70A CAN Message");
                    var payload = msg.Payload;
                    if (payload == null || payload.Length < 1) return;
                    cart.RArmRemoteCode = payload[0];
                },
            };
        }

    }
}
