using CartActivator;
using CommonUsage.Mathematics;
using FundamentalLib;
using MDCSToolBox.Commons;
using MDCSToolBox.Commons.Controllers;
using System;
using System.Collections.Generic;
using System.Linq;
using static MDCSToolBox.Medulla.Chassis.BasicCartDefinition;

namespace MedullaAdapter
{
    public class MotorRoutine:LadderLogic<DiverCartDefinition>
    {
        private DateTime _lastMoveTime = DateTime.Now;
        private DateTime _transmitterFleetDbgLastLog = DateTime.MinValue;
        private DateTime _transmitterFleetResetLastLog = DateTime.MinValue;

        private void LogTransmitterFleet(string branch, bool force = false)
        {
            var now = DateTime.Now;
            if (!force && (now - _transmitterFleetDbgLastLog).TotalMilliseconds < 200) return;
            _transmitterFleetDbgLastLog = now;
            DLog.Log(
                $"branch={branch} connected={cart.TransmitterConnected} ctrl={cart.TransmitterControlEnable} " +
                $"SA={cart.Transmitter_SA} SC={cart.Transmitter_SC} SB={cart.Transmitter_SB} SD={cart.Transmitter_SD} car={cart.CarNum} " +
                $"joyLx={cart.TransmitterLeftJoystickValX:0.000} joyRy={cart.TransmitterRightJoystickValY:0.000} joyRx={cart.TransmitterRightJoystickValX:0.000} " +
                $"rawLx={cart.TransmitterLeftJoystickValXRaw} rawRy={cart.TransmitterRightJoystickValYRaw} " +
                $"remotePanel={cart.MultiVehicleRemoteManualEnabled} outEn={cart.MultiVehicleManualEnabled} outMode={cart.MultiVehicleManualMode} " +
                $"outVx={cart.MultiVehicleManualVx:0.000} outVy={cart.MultiVehicleManualVy:0.000} outVth={cart.MultiVehicleManualVth:0.000} hold={cart.MultiVehicleHold}",
                "TransmitterFleetDbg");
        }

        private void LogTransmitterFleetReset(string reason)
        {
            var now = DateTime.Now;
            if ((now - _transmitterFleetResetLastLog).TotalMilliseconds < 500) return;
            _transmitterFleetResetLastLog = now;
            DLog.Log(
                $"reset reason={reason} remotePanel={cart.MultiVehicleRemoteManualEnabled} beforeEn={cart.MultiVehicleManualEnabled} " +
                $"beforeMode={cart.MultiVehicleManualMode} beforeVx={cart.MultiVehicleManualVx:0.000} beforeVy={cart.MultiVehicleManualVy:0.000} " +
                $"beforeVth={cart.MultiVehicleManualVth:0.000} hold={cart.MultiVehicleHold}",
                "TransmitterFleetDbg");
        }

        public override void Operation(int iteration)
        {
            void ResetMultiVehicle(string reason)
            {
                if (cart.MultiVehicleRemoteManualEnabled)
                {
                    LogTransmitterFleet("reset-skipped-remote-panel");
                    return;
                }

                if (cart.MultiVehicleManualEnabled || cart.MultiVehicleManualVx != 0 || cart.MultiVehicleManualVy != 0 ||
                    cart.MultiVehicleManualVth != 0 || cart.MultiVehicleHold)
                    LogTransmitterFleetReset(reason);

                cart.MultiVehicleManualEnabled = false;
                cart.MultiVehicleManualVx = 0;
                cart.MultiVehicleManualVy = 0;
                cart.MultiVehicleManualVth = 0;
                cart.MultiVehicleHold = false;
            }

            TriggerOnce(cart.TransmitterConnected && cart.Transmitter_SA, 700,
                () =>
                {
                    Console.WriteLine("遥控器使能!");
                    cart.TransmitterControlEnable = true;
                    cart.TransmitterLastTime = DateTime.Now;
                    LogTransmitterFleet("enable", true);
                });
            TriggerOnce(cart.TransmitterConnected && !cart.Transmitter_SA, 700,
                () =>
                {
                    Console.WriteLine("遥控器断使能!");
                    cart.TransmitterControlEnable = false;
                    LogTransmitterFleet("disable", true);
                });

            if (cart.TransmitterControlEnable && cart.Transmitter_SC == cart.CarNum)
            {
                CartDefinition.testPriority(5, "TransmitterMode");
                ResetMultiVehicle("single-car-control");
                LogTransmitterFleet("single-car-control");
                TransmitterChassisControl();
                cart.TransmitterLastTime = DateTime.Now;
                cart.CarStatu = "Transmitter控制模式使能";
            }
            else if (cart.TransmitterControlEnable && cart.Transmitter_SC == 3)
            {
                cart.CarStatu = "Transmitter双车联动模式使能";
                cart.MultiVehicleManualEnabled = true;
                var fleetMode = 0;
                if (cart.Transmitter_SB == TransmitterState.Mode0) fleetMode = 0;
                else if (cart.Transmitter_SB == TransmitterState.Mode1) fleetMode = 1;
                else if (cart.Transmitter_SB == TransmitterState.Mode2) fleetMode = 2;
                cart.ApplyFleetManualCommand(fleetMode, -cart.TransmitterLeftJoystickValX,
                    cart.TransmitterRightJoystickValY, 1f);

                // hold multi-vehicle sync, make speed 0
                cart.MultiVehicleHold = cart.Transmitter_SD == TransmitterState.Mode1;
                LogTransmitterFleet("fleet-control");
            }
            else
            {
                ResetMultiVehicle("idle-or-sc-mismatch");
                LogTransmitterFleet("idle-or-sc-mismatch");
                cart.CarStatu = "正常运行";
            }

            cart.ClumsyControl = true;
            cart.LeftFrontPid.ChangeParameters(cart.DiffSteerKp, cart.DiffSteerKi, cart.DiffSteerKd, cart.DiffSteerMaxI,
                cart.DiffSteerDeadZone, cart.DiffSteerThresh, cart.DiffSteerSpeedAcc);
            cart.LeftRearPid.ChangeParameters(cart.DiffSteerKp, cart.DiffSteerKi, cart.DiffSteerKd, cart.DiffSteerMaxI,
                cart.DiffSteerDeadZone, cart.DiffSteerThresh, cart.DiffSteerSpeedAcc);
            cart.RightFrontPid.ChangeParameters(cart.DiffSteerKp, cart.DiffSteerKi, cart.DiffSteerKd, cart.DiffSteerMaxI,
                cart.DiffSteerDeadZone, cart.DiffSteerThresh, cart.DiffSteerSpeedAcc);
            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 diffRf = cart.RightFrontPid.GetResponse(cart.ThRightFront, false, false, "RF");
            var diffLr = cart.LeftRearPid.GetResponse(cart.ThLeftRear, false, false, "LR");
            var diffRr = cart.RightRearPid.GetResponse(cart.ThRightRear, false, false, "RR");
            // Hedingben.ToastText($"LF:{diffLf:0.00},LR:{diffLr:0.00},RF:{diffRf:0.00},RR:{diffRr:0.00}", "wheel-pid");
            DLog.Log($"[wheel pid] {cart.ActualThLeftFront:0.00}->{cart.ThLeftFront:0.00} diff:{CommonMath.ThDiff(cart.ThLeftFront, cart.ActualThLeftFront):0.00} pid:{diffLf:0.00}");
            cart.SpeedLFL = cart.SpeedLeftFrontLeft - diffLf;
            cart.SpeedLFR = cart.SpeedLeftFrontRight + diffLf;
            cart.SpeedRFL = cart.SpeedRightFrontLeft - diffRf;
            cart.SpeedRFR = cart.SpeedRightFrontRight + diffRf;
            cart.SpeedLRL = cart.SpeedLeftRearLeft - diffLr;
            cart.SpeedLRR = cart.SpeedLeftRearRight + diffLr;
            cart.SpeedRRL = cart.SpeedRightRearLeft - diffRr;
            cart.SpeedRRR = cart.SpeedRightRearRight + diffRr;
            List<float> wheelSpeed = new float[]
            {
                Math.Abs(cart.SpeedLFL), Math.Abs(cart.SpeedLFR), Math.Abs(cart.SpeedRFL),
                Math.Abs(cart.SpeedRFR), Math.Abs(cart.SpeedLRL),Math.Abs(cart.SpeedLRR), Math.Abs(cart.SpeedRRL),
                Math.Abs(cart.SpeedRRR)
            }.ToList();
            var wheelMaxSpeed = wheelSpeed.Max();
            var speedSign = Math.Sign(cart.ThresSpeed - cart.SendThresSpeed);
            var acc = Math.Abs(cart.ThresSpeed) > Math.Abs(cart.SendThresSpeed) ? cart.Chassis.AccPerSecond : cart.Chassis.DeAccPerSecond;
            cart.SendThresSpeed += speedSign * Math.Min(Math.Abs(cart.ThresSpeed - cart.SendThresSpeed),
                acc * (float)(DateTime.Now - _lastMoveTime).TotalSeconds);
            if (cart.ThresSpeed < wheelMaxSpeed && cart.SendThresSpeed > wheelMaxSpeed)
                cart.SendThresSpeed = Math.Min(cart.SendThresSpeed, wheelMaxSpeed);
            _lastMoveTime = DateTime.Now;

            //灯光控制
            if (cart.AlarmLevel == 2)
            {
                //故障红灯常亮
                cart.LightMode = 2;
            }
            else if (cart.ThresSpeed != 1)
            {
                //避障黄灯常亮
                cart.LightMode = 3;
            }
            else if (!cart.WheelAbleState)
            {
                FlipFlop(ref cart.LightMode, 500, 0, 3);
            }
            else if (cart.Soc < cart.LowBatteryAlarmThreshold && cart.ElectricCurrent >= -10)
            {
                cart.LightMode = 3;
            }
            else if (cart.ElectricCurrent < -10)
            {
                FlipFlop(ref cart.LightMode, 500, 0, 1);
            }
            else
            {
                FlipFlop(ref cart.LightMode, 500, 0, 1);
            }
        }
        public void TransmitterChassisControl()
        {
            TriggerOnce(cart.Transmitter_SB == TransmitterState.Mode0, 100,
                () => cart.TransmitterControlMode = DiverCartDefinition.ManualControlMode.Normal);
            TriggerOnce(cart.Transmitter_SB == TransmitterState.Mode1, 100,
                () => cart.TransmitterControlMode = DiverCartDefinition.ManualControlMode.Crab);
            TriggerOnce(cart.Transmitter_SB == TransmitterState.Mode2, 100,
                () => cart.TransmitterControlMode = DiverCartDefinition.ManualControlMode.Spin);

            Hedingben.ToastText($"X方向速度: {cart.TransmitterLeftJoystickValX}  Y方向速度:  {cart.TransmitterRightJoystickValY}");

            if (cart.Transmitter_SD == TransmitterState.Mode1)
            {
                if(cart.Transmitter_T3 == 1694)
                    cart.WheelReset();
                else if(cart.Transmitter_T3 == 151)
                    cart.WheelDisable();
            }
            if (cart.Transmitter_SD == TransmitterState.Mode1 && cart.TransmitterLeftJoystickValXRaw == 353)
            {
                TriggerOnce(cart.TransmitterRightJoystickValYRaw == 353, 100, () => cart.TransmitterSpeed += 0.1f);
                TriggerOnce(cart.TransmitterRightJoystickValYRaw == 1694, 100, () => cart.TransmitterSpeed -= 0.1f);
            }
            cart.TransmitterSpeed = Math.Max(Math.Min(cart.TransmitterSpeed, cart.TransmitterSpeedUpperLimit), cart.TransmitterSpeedLowerLimit);
            //cart.TransmitterSpeed = Math.Max(cart.TransmitterSpeed, cart.TransmitterSpeedLowerLimit);
            if (!cart.Transmitter_SA)
            {
                cart.ManualControl(cart.TransmitterControlMode, 0,
                    0, 0, cart.TransmitterSpeed,
                    DateTime.Now - cart.TransmitterLastTime);
                cart.SpeedLeftArm = 0;
                cart.SpeedRightArm = 0;
            }
            //摇杆控制底盘
            else if (cart.Transmitter_SD == TransmitterState.Mode0)
            {
                cart.ManualControl(cart.TransmitterControlMode, cart.TransmitterLeftJoystickValX,
                    cart.TransmitterRightJoystickValY, 0, cart.TransmitterSpeed,
                    DateTime.Now - cart.TransmitterLastTime);
                cart.SpeedLeftArm = 0;
                cart.SpeedRightArm = 0;
            }
            //右摇杆同时控制左右夹臂
            else if (cart.Transmitter_SD == TransmitterState.Mode1 && cart.TransmitterLeftJoystickValX == 0)
            {
                cart.ManualControl(cart.TransmitterControlMode, 0,
                    0, 0, cart.TransmitterSpeed,
                    DateTime.Now - cart.TransmitterLastTime);
                cart.SpeedLeftArm = cart.TransmitterRightJoystickValX * cart.ManualArmSpeedFac;
                cart.SpeedRightArm = cart.TransmitterRightJoystickValX * cart.ManualArmSpeedFac;
            }
        }
    }
}