ParkingRobot/MedullaAdapter/FleetRemote.cs

using System;
using CartActivator;
using CycleGUI;
using CycleGUI.API;
using FundamentalLib;
using Medulla.Types;

namespace MedullaAdapter
{
    public partial class DiverCartDefinition
    {
        private bool _fleetRemoteActive;
        private DateTime _fleetDiagLastStick = DateTime.MinValue;

        private void FleetDiag(string msg)
        {
            DLog.Log($"car{CarNum} {msg}", "FleetDiagMedulla");
        }

        private void ZeroFleetManualFields()
        {
            MultiVehicleManualEnabled = false;
            MultiVehicleManualMode = 0;
            MultiVehicleManualVx = 0;
            MultiVehicleManualVy = 0;
            MultiVehicleManualVth = 0;
        }

        public float ApplyFleetManualCommand(int mode, float px, float py, float speedRatio)
        {
            var ratio = Clamp(speedRatio, 0, 1);
            px = Clamp(px, -1, 1);
            py = Clamp(py, -1, 1);

            if (mode < 0 || mode > 2) mode = 0;
            MultiVehicleManualMode = mode;

            if (mode == 2)
            {
                MultiVehicleManualVx = 0;
                MultiVehicleManualVy = 0;
                MultiVehicleManualVth = px * MaxManualAngularSpeed * ratio;
            }
            else if (mode == 1)
            {
                MultiVehicleManualVx = py * MaxManualSpeed * ratio;
                MultiVehicleManualVy = px * MaxManualSpeed * ratio;
                MultiVehicleManualVth = 0;
            }
            else
            {
                MultiVehicleManualVx = py * MaxManualSpeed * ratio;
                MultiVehicleManualVy = 0;
                MultiVehicleManualVth = px * MaxManualAngularSpeed * ratio;
            }

            return ratio;
        }

        [IOObjectUtility]
        public void FleetRemote()
        {
            if (_fleetRemoteActive)
            {
                return;
            }

            _fleetRemoteActive = true;
            var fleetOn = false;
            var crabOn = false;
            var rotateOn = false;
            var speedRatio = 1f;
            UseGesture manip = null;

            int CurrentMode()
            {
                if (crabOn) return 1;
                if (rotateOn) return 2;
                return 0;
            }

            void ApplyMode()
            {
                MultiVehicleManualMode = fleetOn ? CurrentMode() : 0;
            }

            void CleanupFleetRemote()
            {
                if (manip != null)
                {
                    manip.End();
                    manip = null;
                }

                fleetOn = false;
                crabOn = false;
                rotateOn = false;
                ZeroFleetManualFields();
                _fleetRemoteActive = false;
            }

            manip = new UseGesture();

            manip.AddWidget(new UseGesture.StickWidget
            {
                name = "fleet_stick",
                text = "速度摇杆",
                position = "37.5%+10px, 18%+10px",
                size = "37.5%-10px, 32%-10px",
                bounceBack = true,
                keyboard = "Up,Down,Left,Right",
                joystick = "Axis0,Axis1",
                OnValue = (pos, manipulating) =>
                {
                    if (!fleetOn)
                    {
                        MultiVehicleManualVx = 0;
                        MultiVehicleManualVy = 0;
                        MultiVehicleManualVth = 0;
                        if ((DateTime.Now - _fleetDiagLastStick).TotalMilliseconds >= 200)
                        {
                            _fleetDiagLastStick = DateTime.Now;
                            FleetDiag($"STICK(ignored,fleetOff) pos=({pos.X:0.00},{pos.Y:0.00}) manip={manipulating}");
                        }
                        return;
                    }

                    var mode = CurrentMode();
                    var ratio = ApplyFleetManualCommand(mode, pos.X, pos.Y, speedRatio);

                    if ((DateTime.Now - _fleetDiagLastStick).TotalMilliseconds >= 200)
                    {
                        _fleetDiagLastStick = DateTime.Now;
                        FleetDiag($"STICK mode={mode} pos=({pos.X:0.00},{pos.Y:0.00}) manip={manipulating} ratio={ratio:0.00} " +
                                  $"-> Vx={MultiVehicleManualVx:0.000} Vy={MultiVehicleManualVy:0.000} Vth={MultiVehicleManualVth:0.0} en={MultiVehicleManualEnabled}");
                    }
                }
            });

            manip.AddWidget(new UseGesture.ToggleWidget
            {
                name = "fleet_crab",
                text = "横移模式",
                position = "12.5%+10px, 52%+10px",
                size = "37.5%-10px, 10%-10px",
                OnValue = b =>
                {
                    crabOn = b;
                    ApplyMode();
                    FleetDiag($"TOGGLE crab={b} -> mode={MultiVehicleManualMode}");
                }
            });

            manip.AddWidget(new UseGesture.ToggleWidget
            {
                name = "fleet_rotate",
                text = "原地旋转",
                position = "50%+10px, 52%+10px",
                size = "37.5%-10px, 10%-10px",
                OnValue = b =>
                {
                    rotateOn = b;
                    ApplyMode();
                    FleetDiag($"TOGGLE rotate={b} -> mode={MultiVehicleManualMode}");
                }
            });

            manip.AddWidget(new UseGesture.ToggleWidget
            {
                name = "fleet_enable",
                text = "车队联动",
                position = "12.5%+10px, 64%+10px",
                size = "37.5%-10px, 10%-10px",
                OnValue = b =>
                {
                    fleetOn = b;
                    MultiVehicleManualEnabled = b;
                    ApplyMode();
                    if (!b) ZeroFleetManualFields();
                    FleetDiag($"TOGGLE fleetOn={b} -> ManualEnabled={MultiVehicleManualEnabled} mode={MultiVehicleManualMode}");
                }
            });

            manip.AddWidget(new UseGesture.ThrottleWidget
            {
                name = "fleet_speed_ratio",
                text = "速度比例",
                position = "50%+10px, 64%+10px",
                size = "37.5%-10px, 10%-10px",
                bounceBack = false,
                OnValue = (val, _) => speedRatio = Clamp(val, 0, 1)
            });

            manip.AddWidget(new UseGesture.ButtonWidget
            {
                name = "fleet_stop",
                text = "急停/归零",
                position = "31.25%+10px, 76%+10px",
                size = "37.5%-10px, 10%-10px",
                OnPressed = pressed =>
                {
                    if (!pressed) return;
                    MultiVehicleManualVx = 0;
                    MultiVehicleManualVy = 0;
                    MultiVehicleManualVth = 0;
                    FleetDiag("BUTTON stop/zero");
                }
            });

            manip.ChangeState(new SetAppearance { drawGuizmo = false });
            manip.Start();

            GUI.PromptOrBringToFront(pb =>
            {
                if (pb.Closing())
                {
                    CleanupFleetRemote();
                    pb.Panel.Exit();
                    return;
                }

                pb.Panel.TopMost(true)
                    .SetDefaultDocking(Panel.Docking.None)
                    .ShowTitle("车队联动遥控")
                    .InitSize(340, 190)
                    .InitPos(false, -32, 32, 1, 0, 1, 0);

                pb.SeparatorText("状态");
                var modeName = MultiVehicleManualMode == 2 ? "原地旋转" : MultiVehicleManualMode == 1 ? "横移" : "常规";
                pb.Label($"车队联动: {MultiVehicleManualEnabled}  模式: {modeName}");
                pb.Label($"速度比例: {speedRatio:0.00}");
                pb.Label($"Vx={MultiVehicleManualVx:0.000} Vy={MultiVehicleManualVy:0.000} m/s, Vth={MultiVehicleManualVth:0.0}");
                pb.Label($"优先级: {CartActivator.CartDefinition.currentPriority} ({CartActivator.CartDefinition.currentPriorityDesc})");
                pb.Label("请勿同时打开手动控制面板");
                pb.Panel.Repaint();
            }, instancingObject: this);
        }

        private static float Clamp(float value, float min, float max)
        {
            if (value < min) return min;
            if (value > max) return max;
            return value;
        }
    }
}