using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Numerics;
using System.Threading;
using ClumsyCore;
using ClumsyCore.DTools;
using ClumsyCore.Pilot;
using ClumsyCore.Utilities;
using ClumsyDance.ClumsyDance.Detectors;
using ClumsyDance.ClumsyWalk.Detectors;
using FundamentalLib;
using LineSegment = ClumsyCore.Utilities.LineSegment;

namespace MultiWheelC;

/// <summary>
/// 2腿检测：用单线激光雷达识别两腿托盘/轮胎，按上一帧结果作为下一帧猜测做闭环检测。
/// 从 StandardMultiWheelLifter 移植；参数全部走 PilotConfig（Fields 面板），雷达选择改为配置项而非阻塞输入。
/// </summary>
[MovementTest(name = "多舵轮-2腿检测")]
public class TwoLegDetect : MovementTest
{
    private Painter _painter;
    private bool _running = true;

    public override void TestStop() => _running = false;

    public override void Test()
    {
        _running = true;
        _painter = UI.GetPainter("MultiWheelTwoLegDetect", false);
        _painter.Clear();

        var lidar = PilotDefinition.Conf.TwoLegLidarName;
        var lastDetectX = PilotDefinition.Conf.TwoLegGuessX;
        var lastDetectY = 0f;

        while (_running)
        {
            var ld = Detect(lidar, lastDetectX, SetFilters(lastDetectX, lastDetectY));
            if (ld == null)
            {
                Thread.Sleep(100);
                continue;
            }

            var center = (ld.Src + ld.Dst) / 2;
            var distanceToCarOrigin = Vector2.Distance(Vector2.Zero, center);

            _painter.Clear();
            _painter.DrawLine(Color.Cyan, Vector2.Zero, center, width: 2);
            _painter.DrawText(Color.Yellow, $"{distanceToCarOrigin:F1}", center.X / 2, center.Y / 2);

            Hedingben.ToastText(
                $"[Test检测] lidar:{lidar} guess x:{lastDetectX:F0} y:{lastDetectY:F0} | " +
                $"中心 x:{center.X:F0} y:{center.Y:F0} dist:{distanceToCarOrigin:F0}",
                "MultiWheelTwoLegDetect-test");

            // 用本帧中心作为下一帧猜测，实现闭环跟踪
            lastDetectX = center.X;
            lastDetectY = center.Y;
            Thread.Sleep(100);
        }

        _painter.Clear();
    }

    /// <summary>在车体坐标系下，按猜测位置检测两腿，返回连接两腿的线段（车体系）。</summary>
    public static LineSegment Detect(string lidarName, float guessX, List<DetectFilter> filters)
    {
        var conf = PilotDefinition.Conf;
#pragma warning disable CS0612, CS0618
        var detector = new Lidar2dDetect2LegTray
        {
            BlobDist = conf.TwoLegBlobDist,
            BlobPtCount = conf.TwoLegBlobPtCount,
            BlobSize = conf.TwoLegBlobSize,
            CenterChange = Tuple.Create(conf.TwoLegCenterChangeX, 0f, 0f),
            LegWidth = conf.TwoLegWidth,
            LegWidthErr = conf.TwoLegWidthErr,
            Padding = conf.TwoLegPadding,
            PillarFindingScope = conf.TwoLegPillarFindingScope,
            SgnDir = conf.TwoLegSgnDir,
        };
#pragma warning restore CS0612, CS0618

        var result = detector.DetectWithGuess(
            lidarName,
            new LineSegment(new Vector2(guessX, 0), Vector2.Zero),
            guessCoordinateSystem: CoordinateSystem.Car2D,
            outCoordinateSystem: CoordinateSystem.Car2D,
            filters);
        return ApplyOutputBias(result);
    }

    private static LineSegment ApplyOutputBias(LineSegment result)
    {
        if (result == null) return null;

        var conf = PilotDefinition.Conf;
        if (Math.Abs(conf.TwoLegOutputBiasX) < 1e-6f && Math.Abs(conf.TwoLegOutputBiasY) < 1e-6f)
            return result;

        var bias = new Vector2(conf.TwoLegOutputBiasX, conf.TwoLegOutputBiasY);
        return new LineSegment(result.Src + bias, result.Dst + bias);
    }

    /// <summary>在猜测中心周围构造一个矩形 ROI，过滤掉框外点云，降低误识别。</summary>
    public static List<DetectFilter> SetFilters(float guessCenterX, float guessCenterY)
    {
        var conf = PilotDefinition.Conf;
        var painter = UI.GetPainter("MultiWheelTwoLegDetect.Filter", false);
        painter.Clear();

        var box = new[]
        {
            new Vector2(guessCenterX - conf.TwoLegFilterLength / 2, guessCenterY - conf.TwoLegFilterWidth / 2),
            new Vector2(guessCenterX + conf.TwoLegFilterLength / 2, guessCenterY - conf.TwoLegFilterWidth / 2),
            new Vector2(guessCenterX + conf.TwoLegFilterLength / 2, guessCenterY + conf.TwoLegFilterWidth / 2),
            new Vector2(guessCenterX - conf.TwoLegFilterLength / 2, guessCenterY + conf.TwoLegFilterWidth / 2),
        };

        for (var i = 0; i < box.Length; ++i)
            painter.DrawLine(Color.DarkOliveGreen, box[i], box[(i + 1) % 4]);

        // 点云滤波在车体坐标系下进行
        return new List<DetectFilter>
        {
            new(CoordinateSystem.Car2D,
                p => LessMath.IsPointInPolygon4(
                    box.Select(v => new PointF(v.X, v.Y)).ToArray(), new PointF(p.X, p.Y))),
        };
    }
}