using System;
using System.Collections.Generic;
using System.Numerics;
using ClumsyCore;
using ClumsyCore.Interfaces;
using ClumsyCore.Pilot;
using FundamentalLib;
using CommonUsage.Chassis;
using CommonUsage.Mathematics;
using MDCSToolBox.Clumsy.Movements;
using MDCSToolBox.Clumsy.Pilot;

namespace MultiWheelC;

// ===== 车队联动-自动蟹行动作 =====
// 以当前车队中心为起点，构造指定方向和长度的直线路径；
// 执行侧直接写 MultiVehicleAuto...，由 TickMultiVehicle 自动分支统一下发。
//
// 控制思路参考 MDCSToolbox 几何控制器，但实现收在 MultiWheelC 内：
// 1) 读取主车 Detour 反推车队中心，计算沿直线的进度、横向偏差和车身目标朝向偏差；
// 2) 根据横向偏差给前后 GCP 同向修正，根据车身目标朝向偏差给前后 GCP 反向修正；
// 3) 根据终点距离减速，并发布 ideal fleet center 给从车做前馈。
//
// 前提：在主车(MultiVehicleMasterEndpoint=="/")运行，且主车有 Detour 定位。
public class FleetCrabWalk : MovementDefinition
{
    /// <summary>路径方向相对启动时车队朝向的夹角(deg，逆时针为正)。</summary>
    public float CrabAngleDeg = 45f;

    /// <summary>路径方向相对车身目标朝向的夹角(deg，逆时针为正)。MovementTest 会设为 CrabAngleDeg，以保持启动时车身朝向。</summary>
    public float BodyToPathAngleDeg = 45f;

    /// <summary>路径长度(mm)。</summary>
    public float CrabLengthMm = 2000f;

    /// <summary>行驶速度(m/s)。</summary>
    public float CrabSpeed = 0.2f;

    /// <summary>速度命令加速度限制(m/s^2)，小于等于 0 表示不限制。</summary>
    public float FleetCrabAccel = 0.2f;

    /// <summary>预对齐后正式下发速度前 5 秒加速度限制(m/s^2)，小于等于 0 表示不限制。</summary>
    public float FleetCrabStartAccel = 0.01f;

    /// <summary>末端开始减速距离(mm)。</summary>
    public float FleetCrabSlowDistance = 2000f;

    /// <summary>完成距离(mm)，低于该剩余距离结束动作。</summary>
    public float FleetCrabFinishDistance = 20f;

    /// <summary>末端最低速度(m/s)。</summary>
    public float FleetCrabFinishSpeed = 0.02f;

    /// <summary>末端减速曲线指数。</summary>
    public float FleetCrabSlowingPow = 0.8f;

    /// <summary>前后 GCP 舵角修正上限(deg)。</summary>
    public float GcpThetaThreshold = 95f;

    private bool _stopping;

    private void Cleanup()
    {
        var self = PilotDefinition.Self;
        self.MultiVehicleScriptVx = 0;
        self.MultiVehicleScriptVy = 0;
        self.MultiVehicleScriptVth = 0;
        self.MultiVehicleScriptMode = 0;
        self.MultiVehicleScriptEnabled = false;
        self.MultiVehicleAutoVx = 0;
        self.MultiVehicleAutoFrontTh = 0;
        self.MultiVehicleAutoRearTh = 0;
        self.MultiVehicleAutoHasIdeal = false;
        self.MultiVehicleAutoEnabled = false;
    }

    public void Stop()
    {
        _stopping = true;
        Cleanup();
    }

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

    private static float ClampAbs(float value, float limit)
    {
        var absLimit = Math.Abs(limit);
        if (absLimit <= 0) return value;
        if (value > absLimit) return absLimit;
        if (value < -absLimit) return -absLimit;
        return value;
    }

    private static float Slew(float current, float target, float maxDelta)
    {
        if (maxDelta <= 0) return target;
        if (target > current + maxDelta) return current + maxDelta;
        if (target < current - maxDelta) return current - maxDelta;
        return target;
    }

    private static float AverageAngle(float frontTh, float rearTh)
    {
        var diff = (float)CommonMath.ThDiff(frontTh, rearTh);
        return (float)CommonMath.RoundTh(rearTh + diff / 2f);
    }

    private static void ResolveCrabDriveEquivalent(float speed, float rawFrontTh, float rawRearTh, float steerLimit,
        out float driveSpeed, out float frontTh, out float rearTh, out bool reverseEquivalent, out float rawBaseTh)
    {
        var limit = Math.Min(179f, Math.Max(1f, Math.Abs(steerLimit)));
        rawBaseTh = AverageAngle(rawFrontTh, rawRearTh);
        driveSpeed = speed;
        frontTh = rawFrontTh;
        rearTh = rawRearTh;
        reverseEquivalent = false;

        if (rawBaseTh > limit)
        {
            frontTh = (float)CommonMath.RoundTh(frontTh - 180f);
            rearTh = (float)CommonMath.RoundTh(rearTh - 180f);
            driveSpeed = -driveSpeed;
            reverseEquivalent = true;
        }
        else if (rawBaseTh < -limit)
        {
            frontTh = (float)CommonMath.RoundTh(frontTh + 180f);
            rearTh = (float)CommonMath.RoundTh(rearTh + 180f);
            driveSpeed = -driveSpeed;
            reverseEquivalent = true;
        }

        frontTh = ClampAbs(frontTh, limit);
        rearTh = ClampAbs(rearTh, limit);
    }

    private static float ProbeSpeed(float speed)
    {
        return Math.Abs(speed) > 1e-4f ? speed : 1f;
    }

    private static bool TryGetMotionYawSign(float frontTh, float rearTh, float driveSpeed, float controlRadius,
        out float yawSign)
    {
        yawSign = 0f;
        if (Math.Abs(CommonMath.ThDiff(frontTh, rearTh)) <= 1e-3f)
            return false;

        var radius = Math.Max(1f, Math.Abs(controlRadius));
        Vector2 pFront = new(radius, 0), pRear = new(-radius, 0),
            normFront = CommonMath.Transform2D(pFront, frontTh + 90f, Vector2.UnitX),
            normRear = CommonMath.Transform2D(pRear, rearTh + 90f, Vector2.UnitX);
        var (intersect, center) = CommonMath.TwoLinesIntersection(pFront, normFront, pRear, normRear);
        if (!intersect)
            return false;

        // Match MultiWheelChassis.SendMotion: the tangent side is selected by
        // rotCenter.Y > 1, and reverse-equivalent motion flips the yaw direction.
        var tangentSign = center.Y > 1f ? 1f : -1f;
        var speedSign = driveSpeed >= 0f ? 1f : -1f;
        yawSign = speedSign * tangentSign;
        return true;
    }

    private static float GetYawSplitSign(float baseTh, float speed, float steerLimit, float controlRadius)
    {
        const float probeDth = 1f;
        ResolveCrabDriveEquivalent(ProbeSpeed(speed), baseTh + probeDth, baseTh - probeDth, steerLimit,
            out var probeSpeed, out var probeFrontTh, out var probeRearTh, out _, out _);
        return TryGetMotionYawSign(probeFrontTh, probeRearTh, probeSpeed, controlRadius, out var yawSign)
            ? yawSign
            : 1f;
    }

    private static float EstimateLateralVelocity(float bodyTh, float frontTh, float rearTh, float driveSpeed,
        Vector2 pathLeft)
    {
        var motionTh = (float)CommonMath.RoundTh(bodyTh + AverageAngle(frontTh, rearTh));
        var rad = motionTh / 180f * Math.PI;
        var dir = new Vector2((float)Math.Cos(rad), (float)Math.Sin(rad));
        if (driveSpeed < 0f)
            dir = -dir;
        return Vector2.Dot(dir, pathLeft);
    }

    private static float ScoreBiasSign(float baseTh, float bodyTh, float speed, float steerLimit, Vector2 pathLeft,
        float lateral, float biasProbe)
    {
        ResolveCrabDriveEquivalent(ProbeSpeed(speed), baseTh + biasProbe, baseTh + biasProbe, steerLimit,
            out var probeSpeed, out var probeFrontTh, out var probeRearTh, out _, out _);
        var lateralVelocity = EstimateLateralVelocity(bodyTh, probeFrontTh, probeRearTh, probeSpeed, pathLeft);
        return -Math.Sign(lateral) * lateralVelocity;
    }

    private static float GetLateralBiasSign(float baseTh, float bodyTh, float speed, float steerLimit, Vector2 pathLeft,
        float lateral)
    {
        if (Math.Abs(lateral) <= 1e-3f)
            return 1f;

        const float probeBias = 1f;
        var positiveScore = ScoreBiasSign(baseTh, bodyTh, speed, steerLimit, pathLeft, lateral, probeBias);
        var negativeScore = ScoreBiasSign(baseTh, bodyTh, speed, steerLimit, pathLeft, lateral, -probeBias);
        return positiveScore >= negativeScore ? 1f : -1f;
    }

    private static bool TryGetControlFleetCenter(PilotDefinition self, out float centerX, out float centerY,
        out float centerTh, out string source)
    {
        if (self.TryGetFleetCenterFromMembers(out centerX, out centerY, out centerTh))
        {
            source = "fleet";
            return true;
        }

        if (self.TryGetFleetCenterFromSlam(out centerX, out centerY, out centerTh))
        {
            source = "slam";
            return true;
        }

        source = "none";
        return false;
    }

    public override IEnumerable<bool> Get()
    {
        var self = PilotDefinition.Self;
        var conf = PilotDefinition.Conf;
        var chassis = BasicPilotBase.Chassis as MultiWheelChassis;
        if (chassis == null)
        {
            DLog.Log("ABORT: FleetCrabWalk requires MultiWheelChassis.", "FleetCrabDbg");
            yield break;
        }
        _stopping = false;

        DLog.Log(
            $"ENTER master?={conf.MultiVehicleMasterEndpoint == "/"} endpoint={conf.MultiVehicleMasterEndpoint} " +
            $"fleetNum={conf.MultiVehicleFleetNum} useDetect={conf.MultiVehicleUseDetect} " +
            $"syncUseDetour={conf.MultiVehicleSyncUseDetour} useIdealCenter={conf.MultiVehicleAutoUseIdealCenter} " +
            $"autoFields=true pathMode=relative pathAngle={CrabAngleDeg:0.0} " +
            $"bodyToPath={BodyToPathAngleDeg:0.0} gcpLimit={GcpThetaThreshold:0.0} " +
            $"biasFac={conf.BiasFac:0.00} fleetCrabDthFac={conf.FleetCrabDthLinearFac:0.00}",
            "FleetCrabDbg");

        if (conf.MultiVehicleMasterEndpoint != "/")
        {
            DLog.Log($"ABORT: 非主车 (endpoint={conf.MultiVehicleMasterEndpoint})", "FleetCrabDbg");
            Hedingben.ToastText("车队蟹行需在主车(主车端点=\"/\")运行", "FleetCrab");
            yield break;
        }

        // 注意：getCartLocation() 在无有效 Detour 定位时会阻塞——若卡在这里且后面看不到 CENTER 日志，即定位未就绪。
        DLog.Log("主车校验通过，开始读取车队中心 (getCartLocation 无定位会阻塞)…", "FleetCrabDbg");
        if (!TryGetControlFleetCenter(self, out var x0, out var y0, out var theta, out var initialCenterSource))
        {
            DLog.Log("ABORT: TryGetFleetCenterFromSlam 返回 false (无定位)", "FleetCrabDbg");
            Hedingben.ToastText("车队蟹行需要主车 Detour 定位", "FleetCrab");
            yield break;
        }
        DLog.Log($"CENTER 车队中心=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");

        DLog.Log($"CENTER_SOURCE source={initialCenterSource} center=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");

        var pathStart = new Vector2(x0, y0);
        var pathLengthMm = CrabLengthMm;
        var phi = CommonMath.RoundTh(theta + CrabAngleDeg);
        var dst = CommonMath.Transform2D(pathStart, phi, new Vector2(pathLengthMm, 0));
        var targetBodyTh = CommonMath.RoundTh(phi - BodyToPathAngleDeg);
        var phiRad = phi / 180.0 * Math.PI;
        var pathDir = new Vector2((float)Math.Cos(phiRad), (float)Math.Sin(phiRad));
        var pathLeft = new Vector2(-pathDir.Y, pathDir.X);

        DLog.Log(
            $"START center=({x0:0},{y0:0},{theta:0.0}) pathMode=relative " +
            $"src=({pathStart.X:0},{pathStart.Y:0}) pathAngle={CrabAngleDeg:0.0} bodyToPath={BodyToPathAngleDeg:0.0} " +
            $"phi={phi:0.0} targetBody={targetBodyTh:0.0} " +
            $"len={pathLengthMm:0} dst=({dst.X:0},{dst.Y:0}) speed={CrabSpeed:0.000} startAccel={FleetCrabStartAccel:0.000} accel={FleetCrabAccel:0.000} " +
            $"slow={FleetCrabSlowDistance:0} finishDist={FleetCrabFinishDistance:0} " +
            $"finishSpeed={FleetCrabFinishSpeed:0.000} slowingPow={FleetCrabSlowingPow:0.00}",
            "FleetCrabDbg");

        var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold));
        var controlRadius = Math.Max(1f, Math.Abs(conf.TestCarSyncDistance) / 2f);
        ResolveCrabDriveEquivalent(0f, (float)CommonMath.ThDiff(phi, theta),
            (float)CommonMath.ThDiff(phi, theta), gcpLimit, out _, out var holdFrontTh, out var holdRearTh,
            out _, out _);
        var warmStart = DateTime.Now;
        var warmSeqBaseline = self.BeginFleetMotionWarmup();

        self.MultiVehicleScriptEnabled = false;
        self.MultiVehicleScriptMode = 0;
        self.MultiVehicleScriptVx = 0;
        self.MultiVehicleScriptVy = 0;
        self.MultiVehicleScriptVth = 0;
        self.MultiVehicleAutoEnabled = true;
        self.MultiVehicleAutoVx = 0;
        self.MultiVehicleAutoFrontTh = holdFrontTh;
        self.MultiVehicleAutoRearTh = holdRearTh;
        self.MultiVehicleAutoIdealX = pathStart.X;
        self.MultiVehicleAutoIdealY = pathStart.Y;
        self.MultiVehicleAutoIdealTh = targetBodyTh;
        self.MultiVehicleAutoHasIdeal = true;
        self.MultiVehicleAutoCmdTime = DateTime.Now;
        self.PrimeMasterAutoFromSlam();
        DLog.Log(
            $"WARMUP auto fields enabled, waiting for fleet startup sync seqBase={warmSeqBaseline} " +
            $"hold=({holdFrontTh:0.00},{holdRearTh:0.00})",
            "FleetCrabDbg");

        var warmEnd = warmStart.AddSeconds(Math.Max(1.0f, conf.FleetCrabStartSyncTimeoutSec));
        var warmIter = 0;
        var warmReady = false;
        var warmDetail = "";
        while (!_stopping && DateTime.Now < warmEnd)
        {
            warmIter++;
            self.MultiVehicleScriptEnabled = false;
            self.MultiVehicleScriptMode = 0;
            self.MultiVehicleAutoEnabled = true;
            self.MultiVehicleAutoVx = 0;
            self.MultiVehicleAutoFrontTh = holdFrontTh;
            self.MultiVehicleAutoRearTh = holdRearTh;
            self.MultiVehicleAutoIdealX = pathStart.X;
            self.MultiVehicleAutoIdealY = pathStart.Y;
            self.MultiVehicleAutoIdealTh = targetBodyTh;
            self.MultiVehicleAutoHasIdeal = true;
            self.MultiVehicleAutoCmdTime = DateTime.Now;
            self.PrimeMasterAutoFromSlam();
            var snap = self.GetFleetCenterSnapshot();
            int cnt;
            lock (self.FleetLock) cnt = self.MultiVehicleFleet.Count;
            if (warmIter % 5 == 0)
                DLog.Log(
                    $"WARMUP#{warmIter} 快照=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) tick={snap.Tick} " +
                    $"autoEn={self.MultiVehicleAutoEnabled} scriptEn={self.MultiVehicleScriptEnabled} cnt={cnt}/{conf.MultiVehicleFleetNum} " +
                    $"detail={warmDetail}",
                    "FleetCrabDbg");
            if (self.IsFleetMotionWarmupReady(warmStart, warmSeqBaseline,
                    conf.TestCarSyncTh, conf.TestCarSyncDistance, out warmDetail))
            {
                warmReady = true;
                DLog.Log(
                    $"WARMUP done iter={warmIter} 快照=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) cnt={cnt} detail={warmDetail}",
                    "FleetCrabDbg");
                break;
            }
            yield return true;
        }
        if (!warmReady)
        {
            DLog.Log($"WARMUP timeout: fleet startup sync failed, abort action. detail={warmDetail}",
                "FleetCrabDbg");
            Hedingben.ToastText("车队蟹行启动同步超时，已取消", "FleetCrab");
            Cleanup();
            yield break;
        }

        Hedingben.ToastText($"车队蟹行 路径{phi:0.0}° 车身夹角{BodyToPathAngleDeg:0.0}° 长度{pathLengthMm:0}mm", "FleetCrab");

        if (warmReady && self.TryGetFleetCenterFromMembers(out var warmX, out var warmY, out var warmTh))
        {
            x0 = warmX;
            y0 = warmY;
            theta = warmTh;
            pathStart = new Vector2(x0, y0);
            phi = CommonMath.RoundTh(theta + CrabAngleDeg);
            dst = CommonMath.Transform2D(pathStart, phi, new Vector2(pathLengthMm, 0));
            targetBodyTh = CommonMath.RoundTh(phi - BodyToPathAngleDeg);
            phiRad = phi / 180.0 * Math.PI;
            pathDir = new Vector2((float)Math.Cos(phiRad), (float)Math.Sin(phiRad));
            pathLeft = new Vector2(-pathDir.Y, pathDir.X);
            self.MultiVehicleAutoIdealX = pathStart.X;
            self.MultiVehicleAutoIdealY = pathStart.Y;
            self.MultiVehicleAutoIdealTh = targetBodyTh;
            self.MultiVehicleAutoCmdTime = DateTime.Now;
            DLog.Log(
                $"WARMUP_REBASE source=fleet center=({x0:0},{y0:0},{theta:0.0}) phi={phi:0.0} targetBody={targetBodyTh:0.0} dst=({dst.X:0},{dst.Y:0})",
                "FleetCrabDbg");
        }

        var iter = 0;
        var lastLog = DateTime.MinValue;
        var finishDistance = Math.Max(0f, FleetCrabFinishDistance);
        var slowDistance = Math.Max(finishDistance + 1f, FleetCrabSlowDistance);
        var baseSpeed = Math.Abs(CrabSpeed);
        var finishSpeed = Math.Min(baseSpeed, Math.Abs(FleetCrabFinishSpeed));
        var slowingPow = Math.Max(0.01f, FleetCrabSlowingPow);
        var accel = Math.Abs(FleetCrabAccel);
        var startAccel = Math.Abs(FleetCrabStartAccel);
        var cmdSpeed = 0f;
        var lastTick = DateTime.Now;
        var speedRampStart = DateTime.Now;
        var stopReason = "done";

        while (!_stopping)
        {
            iter++;

            if (!TryGetControlFleetCenter(self, out var cx, out var cy, out var cth, out var centerSource))
            {
                stopReason = "fleet center invalid";
                DLog.Log("ABORT: TryGetControlFleetCenter returned false during auto crab.", "FleetCrabDbg");
                break;
            }
            var delta = new Vector2(cx - pathStart.X, cy - pathStart.Y);
            var along = Vector2.Dot(delta, pathDir);
            var lateral = Vector2.Dot(delta, pathLeft);
            var remain = pathLengthMm - along;
            if (remain <= finishDistance)
                break;

            var targetSpeed = baseSpeed;
            var slowRatio = 1f;
            if (remain < slowDistance)
            {
                slowRatio = (float)Math.Pow(Clamp(Math.Max(0, remain) / slowDistance, 0f, 1f), slowingPow);
                targetSpeed = slowRatio * (baseSpeed - finishSpeed) + finishSpeed;
            }
            var now = DateTime.Now;
            var dt = Math.Max(0.001f, (float)(now - lastTick).TotalSeconds);
            lastTick = now;
            var rampElapsed = (now - speedRampStart).TotalSeconds;
            var activeAccel = rampElapsed < 5.0 ? startAccel : accel;
            var speed = activeAccel > 0 ? Slew(cmdSpeed, targetSpeed, activeAccel * dt) : targetSpeed;
            cmdSpeed = speed;

            var baseCrabTh = (float)CommonMath.ThDiff(phi, cth);
            var headingErr = (float)CommonMath.ThDiff(targetBodyTh, cth);
            var headingErrReverse = (float)CommonMath.ThDiff(cth, targetBodyTh);
            var targetBodyToPath = (float)CommonMath.ThDiff(phi, targetBodyTh);
            var rawBiasMagnitude = (float)(Math.Atan(conf.BiasFac * Math.Abs(lateral) / 1000f /
                Math.Max(speed, 0.3f)) / Math.PI * 180.0);
            var biasSign = GetLateralBiasSign(baseCrabTh, cth, speed, gcpLimit, pathLeft, lateral);
            var rawBiasItem = rawBiasMagnitude * biasSign;
            var biasItem = ClampAbs(rawBiasItem, conf.BiasThreshold);
            var yawSplitSign = GetYawSplitSign(baseCrabTh + biasItem, speed, gcpLimit, controlRadius);
            var rawDthItem = conf.FleetCrabDthLinearFac * headingErr * yawSplitSign;
            var dthItem = ClampAbs(rawDthItem, conf.FleetCrabDthLinearThreshold);
            var rawFrontTh = baseCrabTh + biasItem + dthItem;
            var rawRearTh = baseCrabTh + biasItem - dthItem;
            ResolveCrabDriveEquivalent(speed, rawFrontTh, rawRearTh, gcpLimit, out var driveSpeed,
                out var frontTh, out var rearTh, out var reverseEquivalent, out var rawBaseTh);
            holdFrontTh = frontTh;
            holdRearTh = rearTh;
            var idealAlong = Clamp(along, 0f, pathLengthMm);
            var ideal = pathStart + pathDir * idealAlong;

            self.MultiVehicleScriptEnabled = false;
            self.MultiVehicleScriptMode = 0;
            self.MultiVehicleScriptVx = 0;
            self.MultiVehicleScriptVy = 0;
            self.MultiVehicleScriptVth = 0;
            self.MultiVehicleAutoEnabled = true;
            self.MultiVehicleAutoVx = driveSpeed;
            self.MultiVehicleAutoFrontTh = frontTh;
            self.MultiVehicleAutoRearTh = rearTh;
            self.MultiVehicleAutoIdealX = ideal.X;
            self.MultiVehicleAutoIdealY = ideal.Y;
            self.MultiVehicleAutoIdealTh = targetBodyTh;
            self.MultiVehicleAutoHasIdeal = true;
            self.MultiVehicleAutoCmdTime = DateTime.Now;

            if ((DateTime.Now - lastLog).TotalMilliseconds >= 300)
            {
                lastLog = DateTime.Now;
                var snap = self.GetFleetCenterSnapshot();
                int fleetCnt;
                lock (self.FleetLock) fleetCnt = self.MultiVehicleFleet.Count;
                DLog.Log(
                    $"ITER#{iter} centerSrc={centerSource} center=({cx:0},{cy:0},{cth:0.0}) snap=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) " +
                    $"along={along:0} lateral={lateral:0} remain={remain:0} headingErr={headingErr:0.0} " +
                    $"baseTh={baseCrabTh:0.0} bias={biasItem:0.0} dth={dthItem:0.0} " +
                    $"slowRatio={slowRatio:0.000} targetV={targetSpeed:0.000} rampT={rampElapsed:0.0} accel={activeAccel:0.000} auto=(vx:{driveSpeed:0.000},fTh:{frontTh:0.0},rTh:{rearTh:0.0}) " +
                    $"ideal=({ideal.X:0},{ideal.Y:0},{targetBodyTh:0.0}) scriptEn={self.MultiVehicleScriptEnabled} " +
                    $"cnt={fleetCnt}/{conf.MultiVehicleFleetNum}",
                    "FleetCrabDbg");
                DLog.Log(
                    $"CTRL iter={iter} centerSrc:{centerSource} phi:{phi:0.00} targetBody:{targetBodyTh:0.00} startTheta:{theta:0.00} " +
                    $"cth:{cth:0.00} crabAngle:{CrabAngleDeg:0.00} bodyToPathCfg:{BodyToPathAngleDeg:0.00} " +
                    $"targetBodyToPath:{targetBodyToPath:0.00} bodyToPathNow:{baseCrabTh:0.00} " +
                    $"headingErr(target-current):{headingErr:0.00} reverse(current-target):{headingErrReverse:0.00} yawSign:{yawSplitSign:0} " +
                    $"fleetCrabDthFac:{conf.FleetCrabDthLinearFac:0.000} rawDth:{rawDthItem:0.00} dth:{dthItem:0.00} dthLimit:{conf.FleetCrabDthLinearThreshold:0.00} " +
                    $"lateral:{lateral:0.0} biasFac:{conf.BiasFac:0.000} biasSign:{biasSign:0} rawBias:{rawBiasItem:0.00} bias:{biasItem:0.00} biasLimit:{conf.BiasThreshold:0.00} " +
                    $"baseTh:{baseCrabTh:0.00} rawBase:{rawBaseTh:0.00} rawOut(f:{rawFrontTh:0.00},r:{rawRearTh:0.00}) " +
                    $"out(f:{frontTh:0.00},r:{rearTh:0.00}) gcpLimit:{gcpLimit:0.00} revEq:{reverseEquivalent} " +
                    $"speedRaw:{speed:0.000} speed:{driveSpeed:0.000} rampT:{rampElapsed:0.0} accel:{activeAccel:0.000} along:{along:0.0} remain:{remain:0.0} ideal=({ideal.X:0.0},{ideal.Y:0.0},{targetBodyTh:0.00})",
                    "FleetCrabHeadingDbg");
            }
            yield return true;
        }

        if (_stopping)
            stopReason = "stop";

        self.MultiVehicleAutoVx = 0;
        self.MultiVehicleAutoFrontTh = holdFrontTh;
        self.MultiVehicleAutoRearTh = holdRearTh;
        self.MultiVehicleAutoCmdTime = DateTime.Now;
        DLog.Log(
            $"STOP_HOLD iter={iter} reason={stopReason} hold=(fTh:{holdFrontTh:0.0},rTh:{holdRearTh:0.0}) cmdSpeed={cmdSpeed:0.000}",
            "FleetCrabDbg");
        var settleEnd = DateTime.Now.AddMilliseconds(Math.Max(100, conf.MultiVehicleSyncInterval * 3));
        while (!_stopping && DateTime.Now < settleEnd)
        {
            self.MultiVehicleScriptEnabled = false;
            self.MultiVehicleScriptMode = 0;
            self.MultiVehicleAutoEnabled = true;
            self.MultiVehicleAutoVx = 0;
            self.MultiVehicleAutoFrontTh = holdFrontTh;
            self.MultiVehicleAutoRearTh = holdRearTh;
            self.MultiVehicleAutoCmdTime = DateTime.Now;
            yield return true;
        }

        Cleanup();
        Hedingben.ToastText("车队蟹行完成", "FleetCrab");
        DLog.Log($"DONE iter={iter} reason={stopReason}", "FleetCrabDbg");
    }
}