OpenNest/OpenNest.Posts.GravographIS/GravographISWriter.cs

using System;
using System.Collections.Generic;
using System.IO;
using System.Text;
using OpenNest.Geometry;

namespace OpenNest.Posts.GravographIS
{
    /// <summary>
    /// One tool pass: a run of polylines cut at a single feed/depth, optionally
    /// preceded by an operator pause (to swap or adjust the tool). The Gravograph
    /// post builds one pass for engrave and one for cut.
    /// </summary>
    public sealed class GravographPass
    {
        public IEnumerable<IReadOnlyList<Vector>> Polylines { get; set; }

        public int FeedMmPerSec { get; set; }

        public double DepthInches { get; set; }

        /// <summary>When true, park to origin and prompt the operator before this pass.</summary>
        public bool PauseBefore { get; set; }

        public string PauseMessage { get; set; } = "";
    }

    /// <summary>
    /// Encodes polylines (in inches) into the Gravograph IS8000 native "binary HPGL"
    /// wire format. The byte stream is byte-exact against captures from GravoStyle'98.
    ///
    /// Scale: 80 steps/mm = 2032 steps/inch. Y (and Z) are negated on the wire.
    /// Deltas are signed big-endian int16 (max ±32767 steps ≈ ±16 inches per move).
    /// </summary>
    public sealed class GravographISWriter
    {
        // 93-byte preamble — captured from GravoStyle'98 with the trailing
        // job-specific travel block stripped. The VS, VZ and DZ operands are
        // patched by the writer to reflect feed and depth options.
        //
        // The original capture ended with a DR command (FF FD 44 52 00 00)
        // followed by three 8-byte int16 records — same format as PU/PD —
        // that carried a chunked travel from the head's parked position to
        // the original job's first vertex (cumulative ΔX ≈ 1", ΔY ≈ 47").
        // Those frozen deltas have nothing to do with our job geometry, so
        // replaying them sends the head to a fixed point regardless of where
        // the operator set zero. Stripped for the same reason as the captured
        // fixed return-to-home block.
        private static readonly byte[] PreambleTemplate = new byte[]
        {
            0x21, 0x41, 0x53, 0x20, 0x33, 0x38, 0x3b, 0x01, 0x90, 0x01,
            0xf4, 0x01, 0x90, 0x01, 0xf4, 0x01, 0x90, 0x01, 0xf4, 0x00,
            0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
            0x00, 0x00, 0x09, 0x00, 0x00, 0x03, 0xe8, 0x05, 0x06, 0x00,
            0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xfd, 0x32, 0x44, 0x00,
            0x00, 0xff, 0xfd, 0x4d, 0x43, 0x00, 0x01, 0xff, 0xfd, 0x4f,
            0x55, 0xff, 0xfb, 0xff, 0xfd, 0x4f, 0x55, 0xff, 0xfa, 0xff,
            0xfd, 0x50, 0x5a, 0x00, 0x00, 0xff, 0xfd, 0x56, 0x53, 0x00,
            0x23, 0xff, 0xfd, 0x56, 0x5a, 0x00, 0x23, 0xff, 0xfd, 0x44,
            0x5a, 0x01, 0xfc,
        };

        // Stripped 36-byte postamble: lift, aux off, motor off, operator beep,
        // job-finish. The 24-byte return-to-home block that appears in GravoStyle's
        // captured postamble between MC and OP is intentionally OMITTED — those
        // three 8-byte int16 records carry chunked job-specific return deltas
        // (each record is [word1:int16][param:int16][ΔX:int16][ΔY:int16], same
        // format as PU/PD records; the original capture chunked the long Y return
        // across three records because each delta has to fit in int16). Reusing
        // GravoStyle's frozen deltas on different geometry overshoots the X-axis
        // limit. We emit calculated return deltas for the current job instead.
        // The writer now replaces the captured fixed return block with a calculated
        // lift + PU travel to the operator-set origin before these final commands.
        private static readonly byte[] EndJobBytes = new byte[]
        {
            0xff, 0xfd, 0x4f, 0x55, 0xff, 0xfa, // OU 0xFFFA  aux off
            0xff, 0xfd, 0x4f, 0x55, 0xff, 0xfb, // OU 0xFFFB  aux off
            0xff, 0xfd, 0x4d, 0x43, 0x00, 0x00, // MC 0x0000  motor off
            0xff, 0xfd, 0x4f, 0x50, 0x00, 0x00, // OP 0x0000  operator beep
            0xff, 0xfd, 0x4a, 0x46, 0x00, 0x00, // JF 0x0000  job finish
        };

        // 80 steps/mm × 25.4 mm/in
        internal const int StepsPerInch = 2032;

        public GravographISWriterOptions Options { get; }

        public GravographISWriter()
            : this(new GravographISWriterOptions())
        {
        }

        public GravographISWriter(GravographISWriterOptions options)
        {
            Options = options ?? throw new ArgumentNullException(nameof(options));
        }

        /// <summary>
        /// Writes the full byte stream (preamble + geometry + postamble) for the given
        /// polylines. Polyline coordinates are in inches, relative to the operator-set
        /// work origin. The writer emits a leading DR travel to the first polyline
        /// start before lowering for the first cut.
        /// </summary>
        public void Write(IEnumerable<IReadOnlyList<Vector>> polylines, Stream output)
        {
            if (polylines == null) throw new ArgumentNullException(nameof(polylines));

            // A single pass at the configured feed/depth — byte-identical to the
            // original single-group output (no transitions, no pause).
            Write(new[]
            {
                new GravographPass
                {
                    Polylines = polylines,
                    FeedMmPerSec = Options.FeedMmPerSec,
                    DepthInches = Options.DepthInches,
                    PauseBefore = false,
                    PauseMessage = "",
                },
            }, output);
        }

        /// <summary>
        /// Writes the full byte stream for an ordered list of tool passes. The preamble
        /// carries the first pass's feed/depth; each later pass emits an inline feed
        /// (and depth, if changed) and, when <see cref="GravographPass.PauseBefore"/> is
        /// set, parks to the operator origin and emits an operator pause before cutting.
        /// </summary>
        public void Write(IReadOnlyList<GravographPass> passes, Stream output)
        {
            if (passes == null) throw new ArgumentNullException(nameof(passes));
            if (output == null) throw new ArgumentNullException(nameof(output));

            var firstFeed = passes.Count > 0 ? passes[0].FeedMmPerSec : Options.FeedMmPerSec;
            var firstDepth = passes.Count > 0 ? passes[0].DepthInches : Options.DepthInches;

            var preamble = (byte[])PreambleTemplate.Clone();
            PatchOperand(preamble, (byte)'V', (byte)'S', (short)firstFeed);
            PatchOperand(preamble, (byte)'V', (byte)'Z', (short)firstFeed);
            PatchOperand(preamble, (byte)'D', (byte)'Z', DepthInStepsAsInt16(firstDepth));
            output.Write(preamble, 0, preamble.Length);

            // Cumulative head position from the operator-set upper-left origin, in
            // wire steps. The first polyline gets a leading DR travel from this
            // origin before PD lowers for cutting. Used by the envelope guard to
            // catch bad records before they ship to the engraver.
            var headX = 0;
            var headY = 0;
            var envelopeXSteps = (int)System.Math.Round(Options.WorkEnvelopeXMm * StepsPerMm,
                MidpointRounding.AwayFromZero);
            var envelopeYSteps = (int)System.Math.Round(Options.WorkEnvelopeYMm * StepsPerMm,
                MidpointRounding.AwayFromZero);

            var firstPolyline = true;
            var polyIndex = 0;
            var currentFeed = firstFeed;
            var currentDepth = firstDepth;

            for (var p = 0; p < passes.Count; p++)
            {
                var pass = passes[p];

                if (p > 0)
                {
                    if (pass.PauseBefore)
                    {
                        // Park: lift Z, then rapid (pen-up) back to the operator origin so
                        // the head is clear of the work while the tool is swapped.
                        WriteLiftOnly(output);
                        WriteTravel(output, (byte)'P', (byte)'U',
                            checked(-headX), checked(-headY),
                            ref headX, ref headY, envelopeXSteps, envelopeYSteps, polyIndex);
                        WritePauseCore(output, pass.PauseMessage);
                    }

                    if (pass.FeedMmPerSec != currentFeed)
                    {
                        WriteCommand(output, (byte)'V', (byte)'S', (short)pass.FeedMmPerSec);
                        WriteCommand(output, (byte)'V', (byte)'Z', (short)pass.FeedMmPerSec);
                        currentFeed = pass.FeedMmPerSec;
                    }

                    if (pass.DepthInches != currentDepth)
                    {
                        WriteCommand(output, (byte)'D', (byte)'Z', DepthInStepsAsInt16(pass.DepthInches));
                        currentDepth = pass.DepthInches;
                    }
                }

                if (pass.Polylines == null) continue;

                foreach (var poly in pass.Polylines)
                {
                    polyIndex++;
                    if (poly == null || poly.Count < 2)
                        continue;

                    WritePolyline(output, poly, ref firstPolyline, ref headX, ref headY,
                        envelopeXSteps, envelopeYSteps, polyIndex);
                }
            }

            WriteLiftOnly(output);
            if (Options.ReturnToOriginAtEnd && !firstPolyline)
            {
                WriteTravel(output, (byte)'P', (byte)'U',
                    checked(-headX), checked(-headY),
                    ref headX, ref headY, envelopeXSteps, envelopeYSteps, polyIndex);
            }
            output.Write(EndJobBytes, 0, EndJobBytes.Length);
        }

        private void WritePolyline(Stream output, IReadOnlyList<Vector> poly,
            ref bool firstPolyline, ref int headX, ref int headY,
            int envelopeXSteps, int envelopeYSteps, int polyIndex)
        {
            var (startX, startY) = ToWire(poly[0]);
            WriteTravel(output,
                firstPolyline ? (byte)'D' : (byte)'P',
                firstPolyline ? (byte)'R' : (byte)'U',
                checked(startX - headX), checked(startY - headY),
                ref headX, ref headY, envelopeXSteps, envelopeYSteps, polyIndex);

            // PD command + single records-follow flag, then one record per segment.
            output.WriteByte(0xFF);
            output.WriteByte(0xFD);
            output.WriteByte((byte)'P');
            output.WriteByte((byte)'D');
            output.WriteByte(0x00);
            output.WriteByte(0x00);

            var prevX = startX;
            var prevY = startY;
            for (int i = 1; i < poly.Count; i++)
            {
                var (cx, cy) = ToWire(poly[i]);
                var dx = checked(cx - prevX);
                var dy = checked(cy - prevY);
                EnsureEnvelope(headX + dx, headY + dy, envelopeXSteps, envelopeYSteps,
                    polyIndex, segment: i, isTravel: false);
                WriteRecord(output, dx, dy);
                prevX = cx;
                prevY = cy;
                headX += dx;
                headY += dy;
            }

            firstPolyline = false;
        }

        // The operator pause, minus the leading lift/park which the caller emits.
        // Stops the spindle (MC off), turns off aux, writes the console message, then
        // restarts the spindle (MC on) so the job resumes when the operator presses start.
        private static void WritePauseCore(Stream s, string message)
        {
            WriteCommandRaw(s, (byte)'M', (byte)'C', 0x00, 0x00); // motor off
            WriteCommandRaw(s, (byte)'O', (byte)'U', 0xFF, 0xFB); // aux off
            WriteCommandRaw(s, (byte)'O', (byte)'U', 0xFF, 0xFA); // aux off
            WriteCommandRaw(s, (byte)'L', (byte)'B', 0x00, 0x00); // begin message
            WriteMessagePackets(s, message);
            WriteCommandRaw(s, (byte)'N', (byte)'R', 0x00, 0x01); // line terminator
            WriteCommandRaw(s, (byte)'L', (byte)'B', 0x00, 0x01); // end message
            WriteCommandRaw(s, (byte)'M', (byte)'C', 0x00, 0x01); // motor on
        }

        // Console label packets carry two ASCII chars each; an odd-length message is
        // space-padded to a whole number of packets so widths stay 2 bytes.
        private static void WriteMessagePackets(Stream s, string message)
        {
            if (string.IsNullOrEmpty(message)) return;

            var chars = Encoding.ASCII.GetBytes(message);
            for (var i = 0; i < chars.Length; i += 2)
            {
                var c0 = chars[i];
                var c1 = (i + 1 < chars.Length) ? chars[i + 1] : (byte)0x20;
                WriteCommandRaw(s, (byte)'L', (byte)'B', c0, c1);
            }
        }

        private static void WriteCommand(Stream s, byte c0, byte c1, short value)
        {
            WriteCommandRaw(s, c0, c1, (byte)((value >> 8) & 0xFF), (byte)(value & 0xFF));
        }

        private static void WriteCommandRaw(Stream s, byte c0, byte c1, byte hi, byte lo)
        {
            s.WriteByte(0xFF);
            s.WriteByte(0xFD);
            s.WriteByte(c0);
            s.WriteByte(c1);
            s.WriteByte(hi);
            s.WriteByte(lo);
        }

        private const double StepsPerMm = 80.0;

        private void EnsureEnvelope(int wireX, int wireY,
            int envXSteps, int envYSteps,
            int polyIndex, int segment, bool isTravel)
        {
            if (!Options.EnvelopeGuardEnabled) return;

            // Wire frame: X is identity to input; Y is negated. With the operator
            // origin set at the upper-left of the work envelope and an OpenNest
            // quadrant-4 plate, valid part coordinates are +X/right and -Y/down:
            //   wireX ∈ [0, +envXSteps]
            //   wireY ∈ [0, +envYSteps]
            if (wireX >= 0 && wireX <= envXSteps && wireY >= 0 && wireY <= envYSteps)
                return;

            var inputX = wireX / (double)StepsPerInch;
            var inputY = -wireY / (double)StepsPerInch;
            var kind = isTravel ? "pen-up travel" : "cut segment";
            throw new InvalidOperationException(
                $"Polyline {polyIndex} {kind} (segment {segment}) would place the head at " +
                $"({inputX:F3}\", {inputY:F3}\"), outside the {Options.WorkEnvelopeXMm}×{Options.WorkEnvelopeYMm} mm " +
                $"work envelope from upper-left origin. Refusing to emit the record.");
        }

        private static short DepthInStepsAsInt16(double depthInches)
        {
            var steps = (long)System.Math.Round(depthInches * StepsPerInch, MidpointRounding.AwayFromZero);
            if (steps < short.MinValue || steps > short.MaxValue)
                throw new ArgumentOutOfRangeException(nameof(depthInches), $"Depth {depthInches} in. → {steps} steps overflows int16.");
            return (short)steps;
        }

        private static (int x, int y) ToWire(Vector v)
        {
            // Inches -> steps. With upper-left origin in OpenNest quadrant 4,
            // negative input Y is down; Y is negated on the wire.
            var x = (int)System.Math.Round(v.X * StepsPerInch, MidpointRounding.AwayFromZero);
            var y = (int)System.Math.Round(-v.Y * StepsPerInch, MidpointRounding.AwayFromZero);
            return (x, y);
        }

        private void WriteTravel(Stream s, byte c0, byte c1, int dx, int dy,
            ref int headX, ref int headY,
            int envelopeXSteps, int envelopeYSteps,
            int polyIndex)
        {
            if (dx == 0 && dy == 0)
                return;

            s.WriteByte(0xFF);
            s.WriteByte(0xFD);
            s.WriteByte(c0);
            s.WriteByte(c1);
            s.WriteByte(0x00);
            s.WriteByte(0x00);

            var chunks = System.Math.Max(
                (int)System.Math.Ceiling(System.Math.Abs(dx) / (double)short.MaxValue),
                (int)System.Math.Ceiling(System.Math.Abs(dy) / (double)short.MaxValue));
            if (chunks < 1) chunks = 1;

            var emittedX = 0;
            var emittedY = 0;
            for (var i = 1; i <= chunks; i++)
            {
                var targetX = (int)System.Math.Round(dx * (i / (double)chunks), MidpointRounding.AwayFromZero);
                var targetY = (int)System.Math.Round(dy * (i / (double)chunks), MidpointRounding.AwayFromZero);
                var chunkX = checked(targetX - emittedX);
                var chunkY = checked(targetY - emittedY);

                EnsureEnvelope(headX + chunkX, headY + chunkY, envelopeXSteps, envelopeYSteps,
                    polyIndex, segment: 0, isTravel: true);
                WriteRecord(s, chunkX, chunkY);

                emittedX = targetX;
                emittedY = targetY;
                headX += chunkX;
                headY += chunkY;
            }
        }

        private static void WriteLiftOnly(Stream s)
        {
            s.WriteByte(0xFF);
            s.WriteByte(0xFD);
            s.WriteByte((byte)'P');
            s.WriteByte((byte)'U');
            s.WriteByte(0x00);
            s.WriteByte(0x01);
        }

        private static void WriteCommandWithRecord(Stream s, byte c0, byte c1, int dx, int dy)
        {
            s.WriteByte(0xFF);
            s.WriteByte(0xFD);
            s.WriteByte(c0);
            s.WriteByte(c1);
            // Records-follow flag (0x0000) emitted once per PU/PD packet.
            s.WriteByte(0x00);
            s.WriteByte(0x00);
            WriteRecord(s, dx, dy);
        }

        private static void WriteRecord(Stream s, int dx, int dy)
        {
            if (dx < short.MinValue || dx > short.MaxValue ||
                dy < short.MinValue || dy > short.MaxValue)
            {
                throw new InvalidOperationException(
                    $"Move delta ({dx}, {dy}) steps overflows signed int16 — split moves upstream.");
            }

            int word1;
            int param;

            var absDx = (double)System.Math.Abs(dx);
            var absDy = (double)System.Math.Abs(dy);
            var len = System.Math.Sqrt(absDx * absDx + absDy * absDy);

            if (len < 1.0)
            {
                // Zero-length lift (PU 00 01) is the dedicated form; for a record-carrying
                // packet a true zero-length move shouldn't occur, but stay numerically safe.
                word1 = 16384;
                param = 1;
            }
            else
            {
                var maxAbs = System.Math.Max(absDx, absDy);
                word1 = (int)System.Math.Round(16384.0 * maxAbs / len, MidpointRounding.AwayFromZero);
                param = (int)System.Math.Round(len / 22.4, MidpointRounding.AwayFromZero);
                if (param < 1) param = 1;
                if (param > 180) param = 180;
                if (word1 > 16384) word1 = 16384;
            }

            WriteBigEndianInt16(s, (short)word1);
            WriteBigEndianInt16(s, (short)param);
            WriteBigEndianInt16(s, (short)dx);
            WriteBigEndianInt16(s, (short)dy);
        }

        private static void WriteBigEndianInt16(Stream s, short value)
        {
            s.WriteByte((byte)((value >> 8) & 0xFF));
            s.WriteByte((byte)(value & 0xFF));
        }

        // Locates the operand of a command (FF FD <c0> <c1> <hi> <lo>) and overwrites it.
        // Throws if the command isn't present — that would mean the preamble was mis-edited.
        private static void PatchOperand(byte[] buffer, byte c0, byte c1, short value)
        {
            for (int i = 0; i <= buffer.Length - 6; i++)
            {
                if (buffer[i] == 0xFF && buffer[i + 1] == 0xFD &&
                    buffer[i + 2] == c0 && buffer[i + 3] == c1)
                {
                    buffer[i + 4] = (byte)((value >> 8) & 0xFF);
                    buffer[i + 5] = (byte)(value & 0xFF);
                    return;
                }
            }

            throw new InvalidOperationException(
                $"Command '{(char)c0}{(char)c1}' not found in preamble template.");
        }
    }
}