StandardSence/StandardScene.Magnetic/Coders/MagneticTrackCoder.cs

using CommonUsage.Mathematics;
using LessokajiWeaverUtilities.Utilities;
using SimpleCore;
using SimpleCore.Compiler;
using SimpleCore.Library;
using SimpleCore.PropType;
using Simple3.Props;
using StandardScene.CarTypes;
using System;
using System.Linq;
using System.Numerics;
using Track = SimpleCore.PropType.Track;

namespace StandardScene.Magnetic.Coders
{
    /// <summary>
    /// 唯一磁循迹器（会话1：Kiva 版与 MWL 版去重；NaiveMagGo 采用含 ${track.Speed} 版本，用户确认）。
    /// C2 物理迁出：原暂置 Kiva.cs（StandardScene.CarTypes），现归 scene.mag 平台插件。
    /// 触发条件：track.Magnet=true 且非倒车段；分叉/汇流路口按顺时针序计算 magSelect。
    /// </summary>
    public class MagneticTrackCoder : ITrackCoder
    {
        public bool Code(SegmentPlan plan, Track track, Site src, Site dst, int i)
        {
            Diagnosis.Post("enter code", "MagTrackCoder", true);
            if (!track.fields.TryGetValue("Magnet", out var mStr) || !bool.TryParse(mStr, out var mBool) ||
                !mBool) return false;

            // do not use magnet navigation for reversing
            if (track.fields.TryGetValue("ReverseDst", out var reverseStr) && int.TryParse(reverseStr, out var rId) &&
                rId == dst.id) return false;

            Diagnosis.Post($">>>> checking {track.id}", "MagTrackCoder", true);
            var magSelect = -1;
            // choose forking/converging path in clockwise order.
            // i.e. 0 is the left-most forking. -1 means no choosing

            float CalculateTangent(Track myTrack, bool checkSrc, int segId)
            {
                var anchorSite = (Site)(checkSrc ? plan.segments[segId - 1] : plan.segments[segId + 1]);

                if (myTrack is UICircularArcTrack arcTrack)
                {
                    var angleStart = arcTrack.Arc.AngleStart;
                    var direction = 1;
                    if (Vector2.Distance(anchorSite.v2, arcTrack.Arc.PointEnd) <
                        Vector2.Distance(anchorSite.v2, arcTrack.Arc.PointStart))
                    {
                        angleStart = arcTrack.Arc.AngleEnd;
                        direction = -1;
                    }

                    angleStart = CommonMath.RoundTh(angleStart);
                    var myAngle = angleStart + direction * 5;
                    var arcTh = CommonMath.RoundTh(myAngle + 90 * direction);
                    Console.WriteLine($">>>> {checkSrc} id: {myTrack.id} th: {arcTh:0.0}");
                    return arcTh;
                }

                if (myTrack is UIBezierTrack bTrack)
                {
                    var add180 = 0f;
                    var controlPoints = bTrack.BezierCurve.ControlPoints;
                    if (Vector2.Distance(anchorSite.v2, controlPoints[0]) >
                        Vector2.Distance(anchorSite.v2, controlPoints.Last())) add180 = 180;
                    var TangentPoint = bTrack.BezierCurve.QueryTangentPoint(anchorSite.v2);
                    Console.WriteLine($">>>>Bezier {checkSrc} id: {myTrack.id} th: {TangentPoint.Angle:0.0} add:{add180} queryid:{anchorSite.id}");
                    return TangentPoint.Angle + add180;
                }

                if (myTrack is UINurbsTrack nTrack)
                {

                }

                // line track
                var myDst = SimpleLib.GetSite(myTrack.GetOther(anchorSite.id));
                var lineTh = (float)(Math.Atan2(myDst.y - anchorSite.y, myDst.x - anchorSite.x) / Math.PI * 180);
                Console.WriteLine($">>>> {checkSrc} id: {myTrack.id} th: {lineTh:0.0}");
                return lineTh;
            }

            bool TestMagSelect(bool checkSrc)
            {
                var anchorSite = checkSrc ? src : dst;
                var myTh = CalculateTangent(track, checkSrc, i);
                var allTracks = anchorSite.relatedTracks.Select(ii => SimpleLib.GetTrack(ii)).ToList();
                if (allTracks.Count <= 2) return false;

                var tracksAndThs = allTracks.Select(myTrack => (myTrack, CalculateTangent(myTrack, checkSrc, i))).ToList();
                var sameHalfTracks = tracksAndThs.Where(item => Math.Abs(LessMath.thDiff(item.Item2, myTh)) < 90)
                    .ToList();
                if (sameHalfTracks.Count > 1)
                {
                    sameHalfTracks = checkSrc
                        ? sameHalfTracks.OrderByDescending(item => LessMath.thDiff(item.Item2, myTh)).ToList()
                        : sameHalfTracks.OrderBy(item => LessMath.thDiff(item.Item2, myTh)).ToList();
                    Console.WriteLine(
                        $">>>> {checkSrc} {track.id} ({string.Join(", ", sameHalfTracks.Select(tt => tt.myTrack.id))})");
                    magSelect = sameHalfTracks.FindIndex(tt => tt.myTrack.id == track.id);
                    return true;
                }

                // check if adjacent to forking/converging path
                Track adjacentTrack;
                int adjacentSegId;
                if (checkSrc)
                {
                    if (i <= 1) return false;
                    adjacentTrack = (Track)plan.segments[i - 2];
                    adjacentSegId = i - 2;
                }
                else
                {
                    if (i >= plan.segments.Count - 2) return false;
                    adjacentTrack = (Track)plan.segments[i + 2];
                    adjacentSegId = i + 2;
                }
                var otherHalfTracks = tracksAndThs.Where(item => Math.Abs(LessMath.thDiff(item.Item2, myTh)) >= 90)
                    .ToList();
                var adjacentTh = CalculateTangent(adjacentTrack, !checkSrc, adjacentSegId);
                otherHalfTracks = checkSrc
                    ? otherHalfTracks.OrderBy(item => LessMath.thDiff(item.Item2, adjacentTh)).ToList()
                    : otherHalfTracks.OrderByDescending(item => LessMath.thDiff(item.Item2, adjacentTh)).ToList();
                Console.WriteLine($">>>> use adjacentTrack  {checkSrc} {adjacentTrack.id} ({string.Join(", ", otherHalfTracks.Select(tt => tt.myTrack.id))})");
                magSelect = otherHalfTracks.FindIndex(tt => tt.myTrack.id == adjacentTrack.id);
                return true;
            }

            if (!TestMagSelect(true)) TestMagSelect(false);

            Console.WriteLine($">>>> {track.id} select {magSelect}");
            var engine = ProgramCoderHelper.PrepareTrackEngine(plan, track, src, dst, i,
                carFields: typeof(BasicCarFields),
                siteFields: typeof(BasicSiteFields),
                trackFields: typeof(BasicTrackFields),
                planFields: typeof(BasicPlanFields));
            var tStr = "agv.MagGo(${src.x},${src.y},${src.id},${dst.x},${dst.y},${dst.id},${track.id}," +
                       $"{magSelect}," +
                       "${track.Speed},${track.Reverse || (track.ReverseDst == dst.id)}," +
                       "${track.CarDirectionBias},${track.EnableCarAbsoluteDirection},${track.CarAbsoluteDirection});";

            if (track.fields.TryGetValue("NaiveMagnet", out var naiveStr) && bool.TryParse(naiveStr, out var naive) &&
                naive)
            {
                var slowDown = i == plan.segments.Count - 2;
                if (slowDown && (!dst.fields.TryGetValue("TagValue", out var tagValStr) ||
                                  !int.TryParse(tagValStr, out _)))
                    throw new Exception($"Destination site {dst.id} must have valid TagValue field!");
                Console.WriteLine($">>>> Naive magnet!");
                tStr = "agv.NaiveMagGo(${src.id},${dst.id},${track.id}," +
                       $"{magSelect}," + "${track.Speed}," +
                       $"{slowDown.ToString().ToLower()}," +
                       "${dst.TagValue}," +
                       "${src.x},${src.y},${dst.x},${dst.y},${track.typeInfo});";
            }

            plan.codeArr[i] += (string)engine.ExecuteExpression($"`{tStr}`", default);

            return true;
        }

        public bool toBlock()
        {
            return true;
        }
    }
}