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

namespace OpenNest.Posts.GravographIS
{
    /// <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));
            if (output == null) throw new ArgumentNullException(nameof(output));

            var preamble = (byte[])PreambleTemplate.Clone();
            PatchOperand(preamble, (byte)'V', (byte)'S', (short)Options.FeedMmPerSec);
            PatchOperand(preamble, (byte)'V', (byte)'Z', (short)Options.FeedMmPerSec);
            PatchOperand(preamble, (byte)'D', (byte)'Z', DepthInStepsAsInt16());
            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;

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

                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;
            }

            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 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 short DepthInStepsAsInt16()
        {
            var steps = (long)System.Math.Round(Options.DepthInches * StepsPerInch, MidpointRounding.AwayFromZero);
            if (steps < short.MinValue || steps > short.MaxValue)
                throw new ArgumentOutOfRangeException(nameof(Options.DepthInches), $"Depth {Options.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.");
        }
    }
}