using System;
using System.Numerics;
using ClumsyCore;
using ClumsyCore.Interfaces;
using ClumsyCore.Pilot;
using FundamentalLib;
using MDCSToolBox.Clumsy.MotionControllers;
using MDCSToolBox.Clumsy.Movements;
using MDCSToolBox.Clumsy.Pilot;

namespace MultiWheelC;

public class ChassisController : MovementDefinition<MultiWheelGeometricController>
{
    public float BaseSpeed = Configuration.conf.basicSpeed;

    private DateTime _sendMotionDbgLast = DateTime.MinValue;

    public override MultiWheelGeometricController Get()
    {
        return new MultiWheelGeometricController
        {
            Chassis = BasicPilotBase.Chassis,
            BaseSpeed = BaseSpeed,
            SlowDistance = PilotDefinition.Conf.SlowDistance,
            SlowingPow = PilotDefinition.Conf.SlowingPow,
            FinishDistance = PilotDefinition.Conf.FinishDistance,
            FinishSpeed = PilotDefinition.Conf.FinishSpeed,
            FirstThAccuracy = PilotDefinition.Conf.FirstThAccuracy,
            FirstRotateSpeedFac = PilotDefinition.Conf.FirstRotateSpeedFac,
            FirstRotateMaxSpeed = PilotDefinition.Conf.FirstRotateMaxSpeed,
            NotContinuousAngle = PilotDefinition.Conf.NotContinuousAngle,
            DebugMode = PilotDefinition.Conf.MotionDebugPrint,
            DebugCurvature = PilotDefinition.Conf.DebugCurvature,
            PowerSteeringLookAhead = PilotDefinition.Conf.PowerSteeringLookAhead,
            SpeedLookAhead = PilotDefinition.Conf.SpeedLookAhead,
            SpeedLookAheadCurveDiff = PilotDefinition.Conf.SpeedLookAheadCurveDiff,
            SpeedLookBackCurveDiff = PilotDefinition.Conf.SpeedLookBackCurveDiff,
            SpeedLimitCurveDiffMin = PilotDefinition.Conf.SpeedLimitCurveDiffMin,
            SpeedLimitCurveMin = PilotDefinition.Conf.SpeedLimitCurveMin,
            MaxRotateSpeed = PilotDefinition.Conf.MaxRotateSpeedCurveLimit,
            MaxRotateAcc = PilotDefinition.Conf.MaxRotateAccCurveLimit,
            GcpThetaThreshold = PilotDefinition.Conf.GcpThetaThreshold,
            DthLinearFac = PilotDefinition.Conf.DthLinearFac,
            DthLinearThreshold = PilotDefinition.Conf.DthLinearThreshold,
            BiasFac = PilotDefinition.Conf.BiasFac,
            BiasThreshold = PilotDefinition.Conf.BiasThreshold,

            MultiVehicleSendMotion = (speed, frontTh, rearTh, idealPos, idealAngle) =>
            {
                var self = PilotDefinition.Self;
                self.MultiVehicleAutoEnabled = true;
                // A: 用固定锁对象（不再锁会被替换的字段引用）。
                int fleetCnt;
                lock (self.FleetLock)
                    fleetCnt = self.MultiVehicleFleet.Count;

                // 诊断（节流 ~300ms）：确认回调被调用、编队是否就绪、是否因数量不符提前 return（导致不下发速度）。
                if ((DateTime.Now - _sendMotionDbgLast).TotalMilliseconds >= 300)
                {
                    _sendMotionDbgLast = DateTime.Now;
                    DLog.Log(
                        $"SENDMOTION speed={speed:0.000} fTh={frontTh:0.0} rTh={rearTh:0.0} " +
                        $"ideal=({idealPos.X:0},{idealPos.Y:0},{idealAngle:0.0}) " +
                        $"editCnt={fleetCnt}/{PilotDefinition.Conf.MultiVehicleFleetNum} " +
                        $"earlyReturn={fleetCnt != PilotDefinition.Conf.MultiVehicleFleetNum}",
                        "FleetCrabDbg");
                }

                if (fleetCnt != PilotDefinition.Conf.MultiVehicleFleetNum)
                    return;

                self.MultiVehicleAutoVx = speed;
                self.MultiVehicleAutoFrontTh = frontTh;
                self.MultiVehicleAutoRearTh = rearTh;
                // D: 透传路径控制器算出的理想车队中心位姿（此前被丢弃），供各车按 layout 做前馈。
                self.MultiVehicleAutoIdealX = idealPos.X;
                self.MultiVehicleAutoIdealY = idealPos.Y;
                self.MultiVehicleAutoIdealTh = idealAngle;
                self.MultiVehicleAutoHasIdeal = true;
                // B: 标记命令新鲜度。路径结束/早退/卡顿不再刷新此时刻 → 主车超时后清零速度，避免滑行。
                self.MultiVehicleAutoCmdTime = DateTime.Now;
            },

            // G: 读取车队中心原子快照，避免跨线程读到撕裂的 x/y/th 组合。
            MultiVehicleGetFleetPos = () =>
            {
                var snap = PilotDefinition.Self.GetFleetCenterSnapshot();
                return new Location
                {
                    x = snap.X,
                    y = snap.Y,
                    th = snap.Th,
                    l_step = 1,
                    tick = DateTime.Now.Ticks
                };
            }
        };
    }
}