using CartActivator;

namespace MultiWheelM;

public class Remote : ManualController<CartDefinition>
{
    [AsControlItem(name = "速度摇杆", keyboard = "Up,Down,Left,Right", joystick = "Axis0,Axis1")]
    public Stick SpeedPad;

    [AsControlItem(name = "速度比例")]
    public Throttle SpeedRatio;

    [AsControlItem(name = "旋转模式")]
    public Switch SpinMode;

    [AsControlItem(name = "横移模式")]
    public Switch CrabMode;

    [AsControlItem(name = "斜行模式")]
    public Switch SwayMode;

    [AsControlItem(name = "车头方向")]
    public DThrottle FrontDirection;

    public override void Operation()
    {
        cart.MultiVehicleManualEnabled = false;
        cart.MultiVehicleManualVx = cart.MultiVehicleManualVy = cart.MultiVehicleManualVth = 0;

        var speedThreshold = Math.Clamp(SpeedRatio.val, 0, 1) * Math.Max(0, cart.MaxManualSpeed);
        var speed = Math.Clamp(SpeedPad.y, -1, 1) * speedThreshold;
        var steerInput = Math.Clamp(SpeedPad.x, -1, 1);
        var steering = (float)Math.Pow(Math.Abs(steerInput), cart.ManualThetaPow) * Math.Sign(steerInput);

        var steerLimit = ComputeSafeSteerLimit();
        var axleTheta = Math.Clamp(steering * cart.MaxManualTheta, -steerLimit, steerLimit);
        var frontTh = -axleTheta;
        var rearTh = axleTheta;
        var frontDirection = Math.Clamp(FrontDirection.dval, -1, 1) * 180;

        if (SpinMode.on)
        {
            var direction = steerInput == 0 ? Math.Sign(speed) : steerInput;
            cart.Chassis.SendRotateMotion(direction * cart.MaxManualAngularSpeed * Math.Clamp(SpeedRatio.val, 0, 1));
            cart.ManualMode = 1;
            statusText = $"spin, w={direction * cart.MaxManualAngularSpeed:0.0}deg/s";
            return;
        }

        if (CrabMode.on)
        {
            cart.Chassis.SetOriginBias(0, 0, 90);
            cart.Chassis.SendMotion(speed, frontTh, rearTh);
            cart.ManualMode = 2;
            statusText = $"crab, v={speed:0.000}, front={frontTh:0.0}, rear={rearTh:0.0}";
            return;
        }

        if (SwayMode.on)
        {
            cart.Chassis.SetOriginBias(0, 0, frontDirection);
            cart.Chassis.SendMotion(speed, frontTh, -rearTh);
            cart.ManualMode = 3;
            statusText = $"sway, v={speed:0.000}, front={frontTh:0.0}, rear={-rearTh:0.0}";
            return;
        }

        cart.Chassis.SetOriginBias(0, 0, frontDirection);
        cart.Chassis.SendMotion(speed, frontTh, rearTh);
        cart.ManualMode = 0;
        statusText = $"normal, v={speed:0.000}, front={frontTh:0.0}, rear={rearTh:0.0}";
    }

    private float ComputeSafeSteerLimit()
    {
        var radius = Math.Max(1f, cart.ChassisControlPointRadius);
        var halfTrack = Math.Max(1f, cart.WheelTrackHalfWidth);
        var singularTheta = (float)(Math.Atan2(radius, halfTrack) * 180.0 / Math.PI);
        var ratio = Math.Clamp(cart.ManualSteerSafetyRatio, 0.1f, 1f);
        return Math.Min(cart.MaxManualTheta, singularTheta * ratio);
    }
}