using CartActivator;
using CommonUsage.Chassis;
using System.Numerics;

namespace MultiWheelM;

[UseManualController(manualController = typeof(Remote))]
[UseLadderLogic(logic = typeof(MotorRoutine), scanInterval = 50)]
public partial class CartDefinition : CartActivator.CartDefinition
{
    [AsLowerIO(desc = "GhostMode")] public bool GhostMode = false;

    [AsUpperIO(desc = "左前轮速度", timeOutReset = true)]
    [Playground]
    public float SpeedLeftFront;

    [AsUpperIO(desc = "左后轮速度", timeOutReset = true)]
    [Playground]
    public float SpeedLeftRear;

    [AsUpperIO(desc = "右前轮速度", timeOutReset = true)]
    [Playground]
    public float SpeedRightFront;

    [AsUpperIO(desc = "右后轮速度", timeOutReset = true)]
    [Playground]
    public float SpeedRightRear;

    [AsLowerIO(desc = "左前轮速度反馈")]
    [Playground]
    public float ActualSpeedLeftFront;

    [AsLowerIO(desc = "左后轮速度反馈")]
    [Playground]
    public float ActualSpeedLeftRear;

    [AsLowerIO(desc = "右前轮速度反馈")]
    [Playground]
    public float ActualSpeedRightFront;

    [AsLowerIO(desc = "右后轮速度反馈")]
    [Playground]
    public float ActualSpeedRightRear;

    [AsUpperIO(desc = "左前轮转向角度", timeOutReset = true)]
    [Playground]
    public float ThLeftFront;

    [AsUpperIO(desc = "左后轮转向角度", timeOutReset = true)]
    [Playground]
    public float ThLeftRear;

    [AsUpperIO(desc = "右前轮转向角度", timeOutReset = true)]
    [Playground]
    public float ThRightFront;

    [AsUpperIO(desc = "右后轮转向角度", timeOutReset = true)]
    [Playground]
    public float ThRightRear;

    [AsLowerIO(desc = "左前轮转向角度反馈")]
    [Playground]
    public float ActualThLeftFront;

    [AsLowerIO(desc = "左后轮转向角度反馈")]
    [Playground]
    public float ActualThLeftRear;

    [AsLowerIO(desc = "右前轮转向角度反馈")]
    [Playground]
    public float ActualThRightFront;

    [AsLowerIO(desc = "右后轮转向角度反馈")]
    [Playground]
    public float ActualThRightRear;

    [AsLowerIO(desc = "遥控模式")]
    public int ManualMode = -1;

    // 车号：在 Medulla 初始化参数面板可视化配置(AsInitParam)，并作为下位信号上报给 Clumsy(AsLowerIO)。
    [AsLowerIO(desc = "车号")]
    [AsInitParam(desc = "车号")]
    public int CarNum = 1;

    // 手动多车联动指令由遥控器在 Medulla 侧产生、上报给 Clumsy，方向为下位->上位，故统一为 AsLowerIO。
    [AsLowerIO(desc = "启用（手动）多车联动")]
    public bool MultiVehicleManualEnabled;

    [AsLowerIO(desc = "（手动）多车联动模式")]
    public int MultiVehicleManualMode;

    [AsLowerIO(desc = "多车联动：遥控器Vx比例")]
    public float MultiVehicleManualVx;

    [AsLowerIO(desc = "多车联动：遥控器Vy比例")]
    public float MultiVehicleManualVy;

    [AsLowerIO(desc = "多车联动：遥控器Vth比例")]
    public float MultiVehicleManualVth;

    [AsLowerIO(desc = "多车联动：遥控暂停")]
    public bool MultiVehicleHold;

    [AsInitParam(desc = "普通手动最大速度(m/s)，车队联动不使用")]
    public float MaxManualSpeed = 0.3f;

    [AsInitParam(desc = "普通手动最大自旋角速度(deg/s)，车队联动不使用")]
    public float MaxManualAngularSpeed = 45f;

    [AsInitParam(desc = "普通手动最大转向角度(deg)，车队联动不使用")]
    public float MaxManualTheta = 45f;

    [AsInitParam(desc = "遥控转向输入幂数")]
    public float ManualThetaPow = 2f;

    [AsInitParam(desc = "遥控转向安全系数(相对奇异转角的比例,0~1)")]
    public float ManualSteerSafetyRatio = 0.8f;

    [AsInitParam(desc = "舵轮前后半轴距(mm)")]
    public float WheelBaseHalfLength = 525f;

    [AsInitParam(desc = "舵轮左右半轮距(mm)")]
    public float WheelTrackHalfWidth = 200f;

    [AsInitParam(desc = "舵轮最小角度(deg)")]
    public float WheelAngleLowerLimit = -120f;

    [AsInitParam(desc = "舵轮最大角度(deg)")]
    public float WheelAngleUpperLimit = 120f;

    [AsInitParam(desc = "底盘速度加速度(m/s^2)")]
    public float ChassisAccPerSecond = 0.3f;

    [AsInitParam(desc = "底盘速度减速度(m/s^2)")]
    public float ChassisDeAccPerSecond = 0.5f;

    [AsInitParam(desc = "转向最低速度系数")]
    public float ChassisMinTurnSpeedFac = 0.25f;

    [AsInitParam(desc = "几何控制点半径(mm)")]
    public float ChassisControlPointRadius = 500f;

    [AsInitParam(desc = "几何控制点转角速度(deg/s)")]
    public float ChassisGcpThetaPerSecond = 10f;

    [AsInitParam(desc = "阿克曼最小转向角(deg)")]
    public float ChassisMinimumTurningAngleForAckermann = 60f;

    public MultiWheelChassis Chassis = null!;

    public override void Init()
    {
        Chassis = new MultiWheelChassis
        {
            MaxSpeed = MaxManualSpeed,
            AccPerSecond = ChassisAccPerSecond,
            DeAccPerSecond = ChassisDeAccPerSecond,
            MinTurnSpeedFac = ChassisMinTurnSpeedFac,
            ControlPointRadius = ChassisControlPointRadius,
            GcpThetaPerSecond = ChassisGcpThetaPerSecond,
            MinimumTurningAngleForAckermann = ChassisMinimumTurningAngleForAckermann
        };

        Chassis.AddWheel(new SteerWheel(
            new Vector2(WheelBaseHalfLength, WheelTrackHalfWidth),
            WheelAngleLowerLimit,
            WheelAngleUpperLimit,
            speed => SpeedLeftFront = speed,
            () => ActualSpeedLeftFront,
            angle => ThLeftFront = angle,
            () => ActualThLeftFront));

        Chassis.AddWheel(new SteerWheel(
            new Vector2(-WheelBaseHalfLength, WheelTrackHalfWidth),
            WheelAngleLowerLimit,
            WheelAngleUpperLimit,
            speed => SpeedLeftRear = speed,
            () => ActualSpeedLeftRear,
            angle => ThLeftRear = angle,
            () => ActualThLeftRear));

        Chassis.AddWheel(new SteerWheel(
            new Vector2(WheelBaseHalfLength, -WheelTrackHalfWidth),
            WheelAngleLowerLimit,
            WheelAngleUpperLimit,
            speed => SpeedRightFront = speed,
            () => ActualSpeedRightFront,
            angle => ThRightFront = angle,
            () => ActualThRightFront));

        Chassis.AddWheel(new SteerWheel(
            new Vector2(-WheelBaseHalfLength, -WheelTrackHalfWidth),
            WheelAngleLowerLimit,
            WheelAngleUpperLimit,
            speed => SpeedRightRear = speed,
            () => ActualSpeedRightRear,
            angle => ThRightRear = angle,
            () => ActualThRightRear));

        Chassis.Initialize();

        Console.WriteLine("[MultiWheelM] Minimal multi-wheel Medulla cart initialized with CommonUsage chassis geometry.");
    }
}