OpenNest/OpenNest.Core/CNC/CutSimulation.cs

using System;
using System.Collections.Generic;
using OpenNest.Geometry;
using OpenNest.Math;

namespace OpenNest.CNC
{
    public enum CutSimulationStepKind
    {
        /// <summary>Head-up travel: a rapid, a gap between programs, or an uncut (display/tab) move.</summary>
        Rapid,

        /// <summary>The dwell before a cut that starts after travel.</summary>
        Pierce,

        /// <summary>Torch-on motion through material (cut, lead-in/out, break).</summary>
        Cut,

        /// <summary>Marking motion that does not cut through.</summary>
        Scribe,
    }

    /// <summary>
    /// One timed tool motion in plate coordinates. Arcs carry a signed sweep
    /// (positive counterclockwise); lines and pierces leave it zero.
    /// </summary>
    public sealed class CutSimulationStep
    {
        public CutSimulationStepKind Kind { get; init; }
        public int PartIndex { get; init; }
        public Vector Start { get; init; }
        public Vector End { get; init; }
        public bool IsArc { get; init; }
        public Vector Center { get; init; }
        public double Radius { get; init; }
        public double StartAngle { get; init; }
        public double Sweep { get; init; }
        public double Length { get; init; }

        /// <summary>The program layer of a cutting or scribing move.</summary>
        public LayerType Layer { get; init; }

        /// <summary>
        /// The contour this move or pierce belongs to, numbered in cutting order; -1 for travel.
        /// </summary>
        public int ContourIndex { get; internal set; } = -1;

        /// <summary>
        /// True when the move is part of an interior contour, timed at the hole feed. Follows the
        /// CI Fiber post's rule: only a closed clockwise contour (or a cut-off) is exterior.
        /// </summary>
        public bool IsHole { get; internal set; }

        /// <summary>Programmed speed in units per second, before acceleration.</summary>
        public double NominalSpeed { get; internal set; }

        public TimeSpan StartTime { get; internal set; }
        public TimeSpan Duration { get; internal set; }
        public TimeSpan EndTime => StartTime + Duration;

        /// <summary>
        /// Planned speed profile in units per second: accelerate from <see cref="EntrySpeed"/>
        /// to <see cref="CruiseSpeed"/>, then decelerate to <see cref="ExitSpeed"/>. A zero
        /// <see cref="Acceleration"/> moves at constant speed for the whole step.
        /// </summary>
        public double EntrySpeed { get; internal set; }
        public double CruiseSpeed { get; internal set; }
        public double ExitSpeed { get; internal set; }
        public double Acceleration { get; internal set; }

        /// <summary>Fraction (0..1) of this step's distance covered <paramref name="elapsed"/> into it.</summary>
        public double ProgressAt(TimeSpan elapsed)
        {
            if (Duration <= TimeSpan.Zero)
                return elapsed >= TimeSpan.Zero ? 1 : 0;
            if (Acceleration <= 0 || Length <= 0)
                return System.Math.Clamp(elapsed / Duration, 0, 1);
            return System.Math.Clamp(DistanceAt(elapsed.TotalSeconds) / Length, 0, 1);
        }

        /// <summary>Head speed in units per second <paramref name="elapsed"/> into this step.</summary>
        public double SpeedAt(TimeSpan elapsed)
        {
            if (Kind == CutSimulationStepKind.Pierce || Duration <= TimeSpan.Zero)
                return 0;
            if (Acceleration <= 0)
                return Length / Duration.TotalSeconds;

            var t = System.Math.Clamp(elapsed.TotalSeconds, 0, Duration.TotalSeconds);
            var (accelEnd, cruiseEnd) = PhaseEnds();
            if (t < accelEnd)
                return EntrySpeed + Acceleration * t;
            if (t < cruiseEnd)
                return CruiseSpeed;
            return System.Math.Max(0, CruiseSpeed - Acceleration * (t - cruiseEnd));
        }

        private (double AccelEnd, double CruiseEnd) PhaseEnds()
        {
            var accelTime = (CruiseSpeed - EntrySpeed) / Acceleration;
            var accelDistance = (CruiseSpeed * CruiseSpeed - EntrySpeed * EntrySpeed) / (2 * Acceleration);
            var decelDistance = (CruiseSpeed * CruiseSpeed - ExitSpeed * ExitSpeed) / (2 * Acceleration);
            var cruise = CruiseSpeed > 0 ? System.Math.Max(0, Length - accelDistance - decelDistance) / CruiseSpeed : 0;
            return (accelTime, accelTime + cruise);
        }

        private double DistanceAt(double t)
        {
            var (accelEnd, cruiseEnd) = PhaseEnds();
            var accelDistance = EntrySpeed * accelEnd + Acceleration * accelEnd * accelEnd / 2;
            if (t < accelEnd)
                return EntrySpeed * t + Acceleration * t * t / 2;
            if (t < cruiseEnd)
                return accelDistance + CruiseSpeed * (t - accelEnd);
            var decel = t - cruiseEnd;
            return accelDistance + CruiseSpeed * (cruiseEnd - accelEnd) + CruiseSpeed * decel - Acceleration * decel * decel / 2;
        }

        /// <summary>Unit direction of travel at the start (0) or end (1) of the step.</summary>
        internal Vector TangentAt(double fraction)
        {
            if (IsArc)
            {
                var angle = StartAngle + Sweep * fraction;
                var sign = System.Math.Sign(Sweep);
                return new Vector(-System.Math.Sin(angle) * sign, System.Math.Cos(angle) * sign);
            }
            return Length > 0 ? new Vector((End.X - Start.X) / Length, (End.Y - Start.Y) / Length) : Vector.Zero;
        }

        /// <summary>The tool position after <paramref name="fraction"/> (0..1) of this step.</summary>
        public Vector PointAt(double fraction)
        {
            fraction = System.Math.Clamp(fraction, 0, 1);

            if (IsArc)
            {
                var angle = StartAngle + Sweep * fraction;
                return new Vector(
                    Center.X + Radius * System.Math.Cos(angle),
                    Center.Y + Radius * System.Math.Sin(angle));
            }

            return new Vector(
                Start.X + (End.X - Start.X) * fraction,
                Start.Y + (End.Y - Start.Y) * fraction);
        }
    }

    /// <summary>
    /// A replayable timeline of a plate's tool motion in cutting order. Cut and display-layer
    /// moves cut (as the posts emit them), at the hole or outside feed. Suppressed moves and,
    /// with <see cref="CutParameters.SkipScribe"/>, etch-only contours are left out. Adds
    /// <see cref="CutParameters.Motion"/> acceleration when set (see <see cref="MotionPlanner"/>).
    /// It does not model jerk, controller feed tables or clamp removal. See docs/cut-time-estimates.md.
    /// </summary>
    public sealed class CutSimulation
    {
        private readonly List<CutSimulationStep> steps;
        private readonly List<TimeSpan> contourStarts = new();
        private readonly List<TimeSpan> partStarts = new();

        private CutSimulation(List<CutSimulationStep> steps, int partCount)
        {
            this.steps = steps;
            PartCount = partCount;
            TotalTime = steps.Count == 0 ? TimeSpan.Zero : steps[^1].EndTime;

            // A contour (a cutout or a part's perimeter) begins where the torch goes down after travel.
            var lastPart = -1;
            var lastContour = -1;
            foreach (var step in steps)
            {
                if (step.ContourIndex < 0 || step.ContourIndex == lastContour)
                    continue;
                lastContour = step.ContourIndex;

                contourStarts.Add(step.StartTime);
                if (step.PartIndex != lastPart)
                {
                    partStarts.Add(step.StartTime);
                    lastPart = step.PartIndex;
                }
            }
        }

        public IReadOnlyList<CutSimulationStep> Steps => steps;
        public int PartCount { get; }
        public TimeSpan TotalTime { get; }

        /// <summary>When each contour's first cut (or its pierce) starts, in cutting order.</summary>
        public IReadOnlyList<TimeSpan> ContourStarts => contourStarts;

        /// <summary>When each part's first contour starts, in cutting order.</summary>
        public IReadOnlyList<TimeSpan> PartStarts => partStarts;

        public TimeSpan NextContour(TimeSpan time) => Next(contourStarts, time);
        public TimeSpan PreviousContour(TimeSpan time) => Previous(contourStarts, time);
        public TimeSpan NextPart(TimeSpan time) => Next(partStarts, time);
        public TimeSpan PreviousPart(TimeSpan time) => Previous(partStarts, time);

        // The first start after the playhead, or the end once none is left.
        private TimeSpan Next(List<TimeSpan> starts, TimeSpan time)
        {
            foreach (var start in starts)
            {
                if (start > time)
                    return start;
            }
            return TotalTime;
        }

        // Like a track's previous button: back to the start of the current item, or the one
        // before when the playhead is already there.
        private static TimeSpan Previous(List<TimeSpan> starts, TimeSpan time)
        {
            for (var i = starts.Count - 1; i >= 0; i--)
            {
                if (starts[i] < time)
                    return starts[i];
            }
            return TimeSpan.Zero;
        }

        public static CutSimulation Build(Plate plate, CutParameters parameters)
        {
            ArgumentNullException.ThrowIfNull(plate);
            ArgumentNullException.ThrowIfNull(parameters);

            var builder = new Builder();

            for (var i = 0; i < plate.Parts.Count; i++)
            {
                var part = plate.Parts[i];
                if (part.Program == null)
                    continue;

                builder.PartIndex = i;
                var mode = Mode.Absolute;
                var pos = part.Location;
                builder.AddProgram(DisplayPerimeterTail.WithoutTail(part.Program), part.Location, ref mode, ref pos);
            }

            var steps = ContourTiming.Finish(builder.Steps, plate, parameters);
            return new CutSimulation(steps, plate.Parts.Count);
        }

        /// <summary>
        /// Index of the step running at <paramref name="time"/>; <see cref="Steps"/>.Count
        /// once the timeline has finished, -1 when it is empty.
        /// </summary>
        public int IndexAt(TimeSpan time)
        {
            if (steps.Count == 0)
                return -1;
            if (time >= TotalTime)
                return steps.Count;

            int lo = 0, hi = steps.Count - 1;
            while (lo < hi)
            {
                var mid = (lo + hi + 1) / 2;
                if (steps[mid].StartTime <= time)
                    lo = mid;
                else
                    hi = mid - 1;
            }
            return lo;
        }

        /// <summary>
        /// Fraction (0..1) of step <paramref name="index"/> completed at <paramref name="time"/>:
        /// distance for motion, elapsed time for a pierce.
        /// </summary>
        public double FractionAt(int index, TimeSpan time)
        {
            var step = steps[index];
            if (step.Kind == CutSimulationStepKind.Pierce)
                return step.Duration <= TimeSpan.Zero
                    ? time >= step.StartTime ? 1 : 0
                    : System.Math.Clamp((time - step.StartTime) / step.Duration, 0, 1);
            return step.ProgressAt(time - step.StartTime);
        }

        /// <summary>
        /// When step <paramref name="index"/> is <paramref name="fraction"/> complete: the inverse
        /// of <see cref="FractionAt"/>, so a retimed timeline can find the same tool position.
        /// </summary>
        public TimeSpan TimeAt(int index, double fraction)
        {
            var step = steps[index];
            if (step.Duration <= TimeSpan.Zero || fraction <= 0)
                return step.StartTime;
            if (fraction >= 1)
                return step.EndTime;

            // Progress never decreases through a step, so bisect its duration.
            long lo = 0, hi = step.Duration.Ticks;
            while (lo < hi)
            {
                var mid = lo + (hi - lo) / 2;
                if (step.ProgressAt(TimeSpan.FromTicks(mid)) < fraction)
                    lo = mid + 1;
                else
                    hi = mid;
            }
            return step.StartTime + TimeSpan.FromTicks(lo);
        }

        public Vector ToolPositionAt(TimeSpan time)
        {
            var index = IndexAt(time);
            if (index < 0)
                return Vector.Zero;
            if (index >= steps.Count)
                return steps[^1].End;
            return steps[index].PointAt(FractionAt(index, time));
        }

        private sealed class Builder
        {
            private Vector tool = Vector.Zero;
            private bool torchOn;


            public List<CutSimulationStep> Steps { get; } = new();
            public int PartIndex { get; set; }

            // Sub-program offsets are relative to the calling frame's origin, as ConvertProgram reads them.
            public void AddProgram(Program program, Vector frameOrigin, ref Mode mode, ref Vector pos)
            {
                mode = program.Mode;

                for (var i = 0; i < program.Length; i++)
                {
                    switch (program[i])
                    {
                        case SubProgramCall { Program: { } sub } call:
                            var savedMode = mode;
                            var subOrigin = frameOrigin + call.Offset;
                            pos = subOrigin;
                            AddProgram(sub, subOrigin, ref mode, ref pos);
                            mode = savedMode;
                            break;

                        case RapidMove rapid:
                            var rapidEnd = Resolve(rapid.EndPoint, mode, pos, frameOrigin);
                            AddTravel(tool, rapidEnd);
                            torchOn = false; // A rapid in place still ends the cut.
                            pos = rapidEnd;
                            break;

                        case Motion { Suppressed: true } suppressed:
                            // Posts leave suppressed moves out; the next move starts from here.
                            pos = Resolve(suppressed.EndPoint, mode, pos, frameOrigin);
                            break;

                        case LinearMove linear:
                            var lineEnd = Resolve(linear.EndPoint, mode, pos, frameOrigin);
                            AddMotion(linear.Layer, pos, lineEnd, isArc: false, default, default);
                            pos = lineEnd;
                            break;

                        case ArcMove arc:
                            var arcEnd = Resolve(arc.EndPoint, mode, pos, frameOrigin);
                            var center = Resolve(arc.CenterPoint, mode, pos, frameOrigin);
                            AddMotion(arc.Layer, pos, arcEnd, isArc: true, center, arc.Rotation);
                            pos = arcEnd;
                            break;
                    }
                }
            }

            private static Vector Resolve(Vector point, Mode mode, Vector pos, Vector frameOrigin) =>
                mode == Mode.Incremental ? point + pos : point + frameOrigin;

            private static CutSimulationStepKind? KindOf(LayerType layer) =>
                layer switch
                {
                    // Cutting strategies emit contour moves on Display; the posts cut them.
                    LayerType.Cut or LayerType.Display or LayerType.Leadin or LayerType.Leadout or LayerType.Break
                        => CutSimulationStepKind.Cut,
                    LayerType.Scribe => CutSimulationStepKind.Scribe,
                    _ => null, // Tab spans are never cut.
                };

            private void AddMotion(LayerType layer, Vector start, Vector end, bool isArc, Vector center, RotationType rotation)
            {
                if (KindOf(layer) is not { } kind)
                {
                    AddTravel(tool, end);
                    return;
                }

                // A program that begins cutting away from the tool reaches its start head-up.
                AddTravel(tool, start);

                if (!torchOn && kind == CutSimulationStepKind.Cut)
                {
                    Add(new CutSimulationStep
                    {
                        Kind = CutSimulationStepKind.Pierce,
                        PartIndex = PartIndex,
                        Start = start,
                        End = start,
                    });
                }

                torchOn = kind == CutSimulationStepKind.Cut;

                if (isArc)
                    AddArc(kind, layer, start, end, center, rotation);
                else
                    Add(new CutSimulationStep
                    {
                        Kind = kind,
                        Layer = layer,
                        PartIndex = PartIndex,
                        Start = start,
                        End = end,
                        Length = start.DistanceTo(end),
                    });
            }

            private void AddArc(CutSimulationStepKind kind, LayerType layer, Vector start, Vector end, Vector center, RotationType rotation)
            {
                var radius = center.DistanceTo(start);
                var startAngle = center.AngleTo(start);
                var endAngle = center.AngleTo(end);
                var ccw = rotation == RotationType.CCW;

                // Coincident ends are a full circle, as ConvertProgram reads them.
                var sweep = ccw
                    ? Angle.NormalizeRad(endAngle - startAngle)
                    : -Angle.NormalizeRad(startAngle - endAngle);
                if (System.Math.Abs(sweep) < Tolerance.Epsilon)
                    sweep = ccw ? Angle.TwoPI : -Angle.TwoPI;

                var length = System.Math.Abs(sweep) * radius;
                Add(new CutSimulationStep
                {
                    Kind = kind,
                    Layer = layer,
                    PartIndex = PartIndex,
                    Start = start,
                    End = end,
                    IsArc = true,
                    Center = center,
                    Radius = radius,
                    StartAngle = startAngle,
                    Sweep = sweep,
                    Length = length,
                });
            }

            private void AddTravel(Vector start, Vector end)
            {
                var distance = start.DistanceTo(end);
                if (distance <= Tolerance.Epsilon)
                {
                    tool = end;
                    return;
                }

                torchOn = false;

                Add(new CutSimulationStep
                {
                    Kind = CutSimulationStepKind.Rapid,
                    PartIndex = PartIndex,
                    Start = start,
                    End = end,
                    Length = distance,
                });
            }

            private void Add(CutSimulationStep step)
            {
                tool = step.End;
                Steps.Add(step);
            }
        }
    }
}