using System;
using System.Collections.Generic;
using System.Globalization;
using System.IO;
using System.Linq;
using System.Text.RegularExpressions;

namespace ToolPathParser
{
    public static class ToolPathProcessor
    {
        // Main processing function
        public static List<ToolPosition> ProcessToolPath(
            string inputPath,
            int totalPointsPerLayer = 50,
            double radialDepth = 1.0,
            double zOrigin = 0.0,
            bool verbose = true,
            double zLayerTol = 0.1)
        {
            var lines = File.ReadAllLines(inputPath)
                            .Select(l => l.Trim())
                            .Where(l => !string.IsNullOrWhiteSpace(l))
                            .ToList();

            var rawPoints = new List<Dictionary<string, object>>();
            double feedrate = 0, spindle = 0;
            for (int i = 0; i < lines.Count; i++)
            {
                string line = lines[i];

                if (line.StartsWith("FEDRAT", StringComparison.OrdinalIgnoreCase))
                {
                    var m = Regex.Match(line, @"[-+]?\d*\.?\d+");
                    if (m.Success) feedrate = double.Parse(m.Value, CultureInfo.InvariantCulture);
                    continue;
                }

                if (line.StartsWith("SPINDL", StringComparison.OrdinalIgnoreCase))
                {
                    var m = Regex.Match(line, @"[-+]?\d*\.?\d+");
                    if (m.Success) spindle = double.Parse(m.Value, CultureInfo.InvariantCulture);
                    continue;
                }

                if (line.StartsWith("GOTO", StringComparison.OrdinalIgnoreCase))
                {
                    var g = ParseGotoRobust(line);
                    if (g != null)
                    {
                        g["feedrate"] = feedrate;
                        g["spindle"] = spindle;
                        rawPoints.Add(g);
                    }

                    // check next line for CIRCLE (ARC)
                    if (i + 1 < lines.Count && lines[i + 1].StartsWith("CIRCLE", StringComparison.OrdinalIgnoreCase))
                    {
                        var c = ParseCircleRobust(lines[i + 1]);
                        if (c != null)
                        {
                            // find the next GOTO as arc end
                            int j = i + 2;
                            Dictionary<string, object> endPoint = null;
                            while (j < lines.Count)
                            {
                                if (lines[j].StartsWith("GOTO", StringComparison.OrdinalIgnoreCase))
                                {
                                    endPoint = ParseGotoRobust(lines[j]);
                                    break;
                                }
                                j++;
                            }
                            if (endPoint != null)
                            {
                                rawPoints.Add(new Dictionary<string, object>
                                {
                                    ["type"] = "ARC",
                                    ["start"] = g["coord"],
                                    ["end"] = endPoint["coord"],
                                    ["center"] = c["center"],
                                    ["axis"] = c["axis"],
                                    ["radius"] = c["radius"],
                                    ["feedrate"] = feedrate,
                                    ["spindle"] = spindle
                                });
                                // advance i to j-1 so outer loop continues correctly
                                i = j - 1;
                            }
                        }
                    }
                }
            }

            if (verbose)
            {
                //Console.WriteLine("=== parsed rawPoints (first 30) ===");
                for (int r = 0; r < Math.Min(30, rawPoints.Count); r++)
                {
                    var rp = rawPoints[r];
                    if (rp["type"].ToString() == "ARC")
                    {
                        var s = (double[])rp["start"];
                        var e = (double[])rp["end"];
                        //Console.WriteLine($"[{r}] ARC start Z={s[2].ToString(CultureInfo.InvariantCulture)} end Z={e[2].ToString(CultureInfo.InvariantCulture)}");
                    }
                    else
                    {
                        var c = (double[])rp["coord"];
                        //Console.WriteLine($"[{r}] GOTO X={c[0].ToString(CultureInfo.InvariantCulture)} Y={c[1].ToString(CultureInfo.InvariantCulture)} Z={c[2].ToString(CultureInfo.InvariantCulture)} FR={rp["feedrate"]} SP={rp["spindle"]}");
                    }
                }
            }

            // cluster into layers by Z using tolerance (zLayerTol)
            var layers = new List<List<Dictionary<string, object>>>();
            double lastZ = double.NaN;
            List<Dictionary<string, object>> current = null;
            foreach (var pt in rawPoints)
            {
                double z = (pt.ContainsKey("type") && pt["type"].ToString() == "ARC")
                    ? ((double[])pt["start"])[2]
                    : ((double[])pt["coord"])[2];

                if (double.IsNaN(lastZ) || Math.Abs(z - lastZ) < zLayerTol)
                {
                    if (current == null) current = new List<Dictionary<string, object>>();
                    current.Add(pt);
                }
                else
                {
                    layers.Add(current);
                    current = new List<Dictionary<string, object>> { pt };
                }
                lastZ = z;
            }
            if (current != null && current.Count > 0) layers.Add(current);

            if (verbose)
            {
                //Console.WriteLine("=== detected layers Z (first 20) ===");
                for (int li = 0; li < Math.Min(20, layers.Count); li++)
                {
                    double zv = (layers[li].First().ContainsKey("type") && layers[li].First()["type"].ToString() == "ARC")
                        ? ((double[])layers[li].First()["start"])[2]
                        : ((double[])layers[li].First()["coord"])[2];
                    //Console.WriteLine($"layer {li} Z ~ {zv.ToString(CultureInfo.InvariantCulture)}  pts={layers[li].Count}");
                }
            }

            // interpolation per layer
            var results = new List<ToolPosition>();
            double prevLayerZ = zOrigin;
            foreach (var layer in layers)
            {
                // build segments: consecutive GOTO -> LINE; include ARC entries as arcs
                var segments = BuildSegments(layer);
                var segLens = segments.Select(seg =>
                {
                    if (seg["type"].ToString() == "LINE")
                        return LineLength((double[])seg["start"], (double[])seg["end"]);
                    else
                        return ArcLength((double[])seg["start"], (double[])seg["end"], (double[])seg["center"], ((double[])seg["axis"])[2]);
                }).ToList();

                double totalLen = segLens.Sum();
                if (totalLen < 1e-9) continue;
                double[] targetS = Enumerable.Range(0, totalPointsPerLayer)
                                             .Select(k => k * totalLen / (totalPointsPerLayer - 1)).ToArray();

                double segStart = 0;
                int ptIdx = 0;
                for (int sIdx = 0; sIdx < segments.Count; sIdx++)
                {
                    var seg = segments[sIdx];
                    double segLen = segLens[sIdx];
                    while (ptIdx < totalPointsPerLayer && targetS[ptIdx] <= segStart + segLen)
                    {
                        double t = (segLen == 0) ? 0.0 : (targetS[ptIdx] - segStart) / segLen;
                        double[] pos;
                        double curvature = 0;
                        double I = 0, J = 0, K = 0, cx = 0, cy = 0, cz = 0;
                        int segType;
                        if (seg["type"].ToString() == "LINE")
                        {
                            pos = InterpolateLine((double[])seg["start"], (double[])seg["end"], t);
                            segType = 1;
                        }
                        else
                        {
                            pos = InterpolateArc((double[])seg["start"], (double[])seg["end"], (double[])seg["center"], (double)seg["radius"], ((double[])seg["axis"])[2], t);
                            curvature = (double)seg["radius"];
                            var axis = (double[])seg["axis"];
                            var c = (double[])seg["center"];
                            I = axis[0]; J = axis[1]; K = axis[2];
                            cx = c[0]; cy = c[1]; cz = c[2];
                            segType = 2;
                        }

                        double axialDepth = Math.Abs(pos[2] - prevLayerZ);
                        results.Add(new ToolPosition
                        {
                            X = pos[0],
                            Y = pos[1],
                            Z = pos[2],
                            FeedRate = (double)seg["feedrate"],
                            SpindleSpeed = (double)seg["spindle"],
                            RadialDepth = radialDepth,
                            AxialDepth = axialDepth,
                            CurvatureRadius = curvature,
                            I = I,
                            J = J,
                            K = K,
                            Cx = cx,
                            Cy = cy,
                            Cz = cz,
                            SegmentType = segType
                        });

                        ptIdx++;
                    }
                    segStart += segLen;
                }

                // update prevLayerZ from layer: use average Z of this layer's original pts (more robust)
                var zs = new List<double>();
                foreach (var p in layer)
                {
                    if (p.ContainsKey("type") && p["type"].ToString() == "ARC")
                    {
                        zs.Add(((double[])p["start"])[2]);
                        zs.Add(((double[])p["end"])[2]);
                    }
                    else
                    {
                        zs.Add(((double[])p["coord"])[2]);
                    }
                }
                if (zs.Count > 0) prevLayerZ = zs.Average();
            }

            return results;
        }

        // robust GOTO parse: accept "GOTO/-99.0092,6.5793,-0.9667" or "GOTO -99.0092,6.5793,-0.9667"
        private static Dictionary<string, object> ParseGotoRobust(string line)
        {
            // find substring after "GOTO"
            int idx = line.IndexOf("GOTO", StringComparison.OrdinalIgnoreCase);
            if (idx < 0) return null;
            string tail = line.Substring(idx + 4).TrimStart(new char[] { '/', ' ', '\t' });
            // tail like "-99.0092,6.5793,-0.9667" possibly with extra commas or fields
            var parts = tail.Split(new[] { ',', ' ' }, StringSplitOptions.RemoveEmptyEntries);
            // find first three numeric tokens
            var nums = new List<double>();
            foreach (var p in parts)
            {
                if (double.TryParse(p, NumberStyles.Float, CultureInfo.InvariantCulture, out double v))
                {
                    nums.Add(v);
                    if (nums.Count == 3) break;
                }
            }
            if (nums.Count < 3) return null;
            return new Dictionary<string, object> { ["type"] = "GOTO", ["coord"] = nums.ToArray() };
        }

        // robust CIRCLE parse: accept "CIRCLE/x,y,z,i,j,k,r,..." where first 7 numbers are center(i,j,k)radius...
        private static Dictionary<string, object> ParseCircleRobust(string line)
        {
            int idx = line.IndexOf("CIRCLE", StringComparison.OrdinalIgnoreCase);
            if (idx < 0) return null;
            string tail = line.Substring(idx + 6).TrimStart(new char[] { '/', ' ', '\t' });
            var parts = tail.Split(new[] { ',', ' ' }, StringSplitOptions.RemoveEmptyEntries);
            var nums = new List<double>();
            foreach (var p in parts)
            {
                if (double.TryParse(p, NumberStyles.Float, CultureInfo.InvariantCulture, out double v))
                {
                    nums.Add(v);
                }
            }
            if (nums.Count < 7) return null;
            double[] center = nums.Take(3).ToArray();
            double[] axis = nums.Skip(3).Take(3).ToArray();
            double radius = nums[6];
            return new Dictionary<string, object>
            {
                ["type"] = "CIRCLE",
                ["center"] = center,
                ["axis"] = axis,
                ["radius"] = radius
            };
        }

        private static List<Dictionary<string, object>> BuildSegments(List<Dictionary<string, object>> layerPts)
        {
            var segs = new List<Dictionary<string, object>>();
            int i = 0;
            while (i < layerPts.Count)
            {
                var p = layerPts[i];
                if (p.ContainsKey("type") && p["type"].ToString() == "GOTO")
                {
                    int j = i + 1;
                    while (j < layerPts.Count && layerPts[j].ContainsKey("type") && layerPts[j]["type"].ToString() == "GOTO") j++;
                    for (int k = i; k < j - 1; k++)
                    {
                        segs.Add(new Dictionary<string, object>
                        {
                            ["type"] = "LINE",
                            ["start"] = (double[])layerPts[k]["coord"],
                            ["end"] = (double[])layerPts[k + 1]["coord"],
                            ["feedrate"] = layerPts[k + 1]["feedrate"],
                            ["spindle"] = layerPts[k + 1]["spindle"]
                        });
                    }
                    i = j;
                }
                else if (p.ContainsKey("type") && p["type"].ToString() == "ARC")
                {
                    segs.Add(p); // keep ARC entries as-is
                    i++;
                }
                else i++;
            }
            return segs;
        }

        private static double LineLength(double[] a, double[] b)
            => Math.Sqrt(Math.Pow(b[0] - a[0], 2) + Math.Pow(b[1] - a[1], 2) + Math.Pow(b[2] - a[2], 2));

        private static double ArcLength(double[] s, double[] e, double[] c, double axisZ)
        {
            double r = Math.Sqrt(Math.Pow(s[0] - c[0], 2) + Math.Pow(s[1] - c[1], 2));
            double ang1 = Math.Atan2(s[1] - c[1], s[0] - c[0]);
            double ang2 = Math.Atan2(e[1] - c[1], e[0] - c[0]);
            double d = ang2 - ang1;
            // normalize
            if (Math.Abs(d) > Math.PI) d -= Math.Sign(d) * 2 * Math.PI;
            return Math.Abs(d) * r;
        }

        private static double[] InterpolateLine(double[] a, double[] b, double t)
            => new double[] { a[0] + (b[0] - a[0]) * t, a[1] + (b[1] - a[1]) * t, a[2] + (b[2] - a[2]) * t };

        private static double[] InterpolateArc(double[] s, double[] e, double[] c, double r, double k, double t)
        {
            double ang1 = Math.Atan2(s[1] - c[1], s[0] - c[0]);
            double ang2 = Math.Atan2(e[1] - c[1], e[0] - c[0]);
            double d = ang2 - ang1;
            if (Math.Abs(d) > Math.PI) d -= Math.Sign(d) * 2 * Math.PI;
            double ang = ang1 + d * t;
            double x = c[0] + r * Math.Cos(ang);
            double y = c[1] + r * Math.Sin(ang);
            double z = s[2] + (e[2] - s[2]) * t;
            return new double[] { x, y, z };
        }
    }
}