using CartActivator;
using MCUSerialBridgeCLR;
using MDCSToolBox.Medulla.Chassis.MultiWheel;
using Medulla.Types;
using System;
using System.Collections.Generic;
using System.Runtime.InteropServices;
using System.Text;
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 partial class DiverCartDefinition : MultiWheelCartDefinition
    {
        public DiverCommunication Embedded;
        public MCUSerialBridge Bridge;
        [AsUpperIO(desc = "转弯半径")] public float Radius;
        #region MultiVehicleCoordination

        [AsInitParam(desc = "遥控器最大线速度")]
        public float MaxManualSpeed = 0.3f;

        [AsInitParam(desc = "遥控器最大角速度")]
        public float MaxManualAngularSpeed = 45f;


        [AsLowerIO(desc = "启用（手动）多车联动")] public bool MultiVehicleManualEnabled = false;
        [AsLowerIO(desc = "（手动）多车联动模式")] public int MultiVehicleManualMode = 0;

        [AsUpperIO(desc = "多车联动：声光同步，-1未启动，0灭，1亮")] public int MultiVehicleLightSync = -1;

        [AsLowerIO(desc = "多车联动模式-暂停")] public bool MultiVehicleHold = false;

        [AsLowerIO(desc = "多车联动：遥控器Vx")] public float MultiVehicleManualVx = 0f;
        [AsLowerIO(desc = "多车联动：蟹行方向比例")] public float MultiVehicleManualVy = 0f;
        [AsLowerIO(desc = "多车联动：遥控器Vth")] public float MultiVehicleManualVth = 0f;

        #endregion
        [IOObjectMonitor(desc = "左前左轮下发速度")] public float SpeedLFL;
        [IOObjectMonitor(desc = "左前右轮下发速度")] public float SpeedLFR;
        [IOObjectMonitor(desc = "右前左轮下发速度")] public float SpeedRFL;
        [IOObjectMonitor(desc = "右前右轮下发速度")] public float SpeedRFR;
        [IOObjectMonitor(desc = "左后左轮下发速度")] public float SpeedLRL;
        [IOObjectMonitor(desc = "左后右轮下发速度")] public float SpeedLRR;
        [IOObjectMonitor(desc = "右后左轮下发速度")] public float SpeedRRL;
        [IOObjectMonitor(desc = "右后右轮下发速度")] public float SpeedRRR;
        //[AsUpperIO(desc = "左前左轮下发速度", timeOutReset = true)] public float SpeedLFL;
        //[AsUpperIO(desc = "左前右轮下发速度", timeOutReset = true)] public float SpeedLFR;
        //[AsUpperIO(desc = "右前左轮下发速度", timeOutReset = true)] public float SpeedRFL;
        //[AsUpperIO(desc = "右前右轮下发速度", timeOutReset = true)] public float SpeedRFR;
        //[AsUpperIO(desc = "左后左轮下发速度", timeOutReset = true)] public float SpeedLRL;
        //[AsUpperIO(desc = "左后右轮下发速度", timeOutReset = true)] public float SpeedLRR;
        //[AsUpperIO(desc = "右后左轮下发速度", timeOutReset = true)] public float SpeedRRL;
        //[AsUpperIO(desc = "右后右轮下发速度", timeOutReset = true)] public float SpeedRRR;

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

        [AsUpperIO(desc = "左夹臂下发速度", timeOutReset = true)] public float SpeedLeftArm;
        [AsUpperIO(desc = "右夹臂下发速度", timeOutReset = true)] public float SpeedRightArm;
        [AsUpperIO(desc = "左夹臂实际速度")] public float ActualSpeedLeftArm;
        [AsUpperIO(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 = "test")] public float test;
        [AsLowerIO(desc = "test1")] public byte test1;
        [AsLowerIO(desc = "test2")] public byte test2;
        [AsInitParam(desc = "test3")] public int test3=24;
        [IOObjectMonitor(desc = "灯光模式")] public int LightMode = 0;
        [AsInitParam(desc = "触发模式")] public int trigger = 0;

        [AsInitParam(desc = "手动控制夹臂速度系数")] public float ManualArmSpeedFac = 1.0f;
        [AsInitParam(desc = "车号")][AsLowerIO] public int CarNum = 1;
        [AsInitParam(desc = "初始音量")] public int MusicVolume = 5;

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

        [AsInitParam(desc = "MCU端口号")] public string MCUPort = "COM4";

        [AsLowerIO(desc = "陀螺仪角度")] public float GyrosTh;
        [AsLowerIO(desc = "电池健康状态")] public float SOH;

        [AsLowerIO(desc = "左前左驱动器远程帧701")] public byte LFLRemoteCode = 0;
        [AsLowerIO(desc = "左前右驱动器远程帧702")] public byte LFRRemoteCode = 0;
        [AsLowerIO(desc = "右前左驱动器远程帧703")] public byte RFLRemoteCode = 0;
        [AsLowerIO(desc = "右前右驱动器远程帧704")] public byte RFRRemoteCode = 0;
        [AsLowerIO(desc = "左后左驱动器远程帧705")] public byte LRLRemoteCode = 0;
        [AsLowerIO(desc = "左后右驱动器远程帧706")] public byte LRRRemoteCode = 0;
        [AsLowerIO(desc = "右后左驱动器远程帧707")] public byte RRLRemoteCode = 0;
        [AsLowerIO(desc = "右后右驱动器远程帧708")] public byte RRRRemoteCode = 0;
        [AsLowerIO(desc = "左夹臂驱动器远程帧709")] public byte LArmRemoteCode = 0;
        [AsLowerIO(desc = "右夹臂驱动器远程帧70A")] public byte RArmRemoteCode = 0;
        [AsUpperIO(desc = "从M上复位")] public bool ResetFromM;
        [AsUpperIO(desc = "从M将驱动轮下使能")] public bool DisableFromM;
        [AsUpperIO(desc = "从C上复位")] public bool ResetFromC;
        [AsUpperIO(desc = "从C将驱动轮下使能")] public bool DisableFromC;
        [AsUpperIO(desc = "夹臂不同步报警")] public bool ClampOutOfSync;

        internal ManualControlMode TransmitterControlMode = ManualControlMode.Normal;

        [IOObjectMonitor] public float TransmitterSpeed = 0.3f;
        [AsLowerIO(desc = "驱动轮使能状态")] public bool WheelAbleState = true;

        [AsInitParam(desc = "遥控器速度上限")] public float TransmitterSpeedUpperLimit = 1.0f;
        [AsInitParam(desc = "遥控器速度下限")] public float TransmitterSpeedLowerLimit = 0.0f;
        internal DateTime TransmitterLastTime;

        [IOObjectUtility]
        public void WheelReset()
        {
            ResetFromM = true;
        }

        [IOObjectUtility]
        public void WheelDisable()
        {
            DisableFromM = true;
        }

        public override void CommunicationInit()
        {
            if (GhostMode) return;

            Bridge = new MCUSerialBridge();
            //Step1:打开指定串口连接
            var err = Bridge.Open(MCUPort, 1000000u);
            if (err != MCUSerialBridgeError.OK)
            {
                Console.WriteLine("MCU Open FAILED: {0}", err.ToDescription());
                return;
            }
            else
            {
                Console.WriteLine("MCU Open OK");
            }

            //Step2:远程复位MCU
            err = Bridge.Reset();
            if (err != MCUSerialBridgeError.OK)
            {
                Console.WriteLine("MCU Reset FAILED: {0}", err.ToDescription());
                return;
            }
            else
            {
                Thread.Sleep(500);
                Console.WriteLine("MCU Reset OK");
            }

            //Step3:GetVersion
            err = Bridge.GetVersion(out var version, 100);
            if (err != MCUSerialBridgeError.OK)
            {
                Console.WriteLine("MCU GetVersion FAILED: {0}", err.ToDescription());
                return;
            }
            else
            {
                Console.WriteLine("MCU GetVersion OK: {0}", version.ToString());
            }

            //Step4:GetState
            err = Bridge.GetState(out var state, 100);
            if (err != MCUSerialBridgeError.OK)
            {
                Console.WriteLine("MCU GetState FAILED: {0}", err.ToDescription());
                return;
            }
            else
            {
                Console.WriteLine("MCU GetState OK: {0}", state.ToString());
            }

            //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 {0}: Serial, Baud={1}, ReceiveFrameMs={2}",
                            i,
                            s.Baud,
                            s.ReceiveFrameMs);
                    else if (ports[i] is CANPortConfig c)
                        Console.WriteLine("Port {0}: CAN, Baud={1}, RetryTimeMs={2}", i, c.Baud, c.RetryTimeMs);
                }

                var ret = Bridge.Configure(ports, 200);
                if (ret != MCUSerialBridgeError.OK)
                {
                    Console.WriteLine("MCU Configure FAILED: {0}", 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: {0}", ex.Message);
            }
            //MCUInterface<DiverCartDefinition>.Start(this);
            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;
            switch (mode)
            {
                case ManualControlMode.Normal:
                    ManualMode = 0;
                    Chassis.DirectionAngle = frontDirection;
                    Chassis.SendMotion(speed, frontTh, rearTh, interval);
                    break;

                case ManualControlMode.Sway:
                    ManualMode = 1;
                    Chassis.DirectionAngle = frontDirection;
                    Chassis.SendMotion(speed, frontTh, -rearTh, interval);
                    break;

                case ManualControlMode.Crab:
                    ManualMode = 2;
                    Chassis.DirectionAngle = 90;
                    Chassis.SendMotion(speed, frontTh, rearTh, interval);
                    break;

                case ManualControlMode.Spin:
                    ManualMode = 3;
                    Chassis.SendRotateMotion(speed * MaxAngularSpeed, interval);
                    break;
            }
        }
       
        internal enum ManualControlMode
        {
            Normal = 0,
            Spin = 1,
            Crab = 2,
            Sway = 3
        }
    }
}
