Dispenser/DispenserCommon/Utils/CoordinateUtil.cs

using Masuit.Tools;

namespace DispenserCommon.Utils;

/// <summary>
///     数组工具类
/// </summary>
public class CoordinateUtil
{
    /// <summary>
    ///     将一个二维坐标数组拆分成两个一维数组
    /// </summary>
    /// <param name="coordinates">二维坐标数组</param>
    /// <param name="xAxis">X轴坐标数组</param>
    /// <param name="yAxis">Y轴坐标数组</param>
    public static void SplitCoordinates(double[][] coordinates, out double[] xAxis, out double[] yAxis)
    {
        var totalPoints = coordinates.GetLength(0);
        xAxis = new double[totalPoints];
        yAxis = new double[totalPoints];

        for (var i = 0; i < totalPoints; i++)
        {
            // 提取x坐标
            xAxis[i] = coordinates[i][0];
            // 提取y坐标
            yAxis[i] = coordinates[i][1];
        }
    }

    public static void SplitCoordinates(double[][] coordinates, out double[] xAxis, out double[] yAxis,
        out double[] depth)
    {
        var totalPoints = coordinates.GetLength(0);
        xAxis = new double[totalPoints];
        yAxis = new double[totalPoints];
        depth = new double[totalPoints];

        for (var i = 0; i < totalPoints; i++)
        {
            // 提取x坐标
            xAxis[i] = coordinates[i][0];
            // 提取y坐标
            yAxis[i] = coordinates[i][1];
            // 提取针刺深度
            depth[i] = coordinates[i][3];
        }
    }

    /// <summary>
    /// 将两个坐标数组合并成一个二维坐标数组
    /// </summary>
    /// <param name="coordinates1"></param>
    /// <param name="coordinates2"></param>
    /// <returns></returns>
    public static double[][] MergeArray(IEnumerable<double> coordinates1, double[] coordinates2)
    {
        List<double[]> merged = [];
        merged.AddRange(coordinates1.Select((t, i) => (double[]) [t, coordinates2[i]]));

        return merged.ToArray();
    }


    /// <summary>
    ///     判断数组是否为空
    /// </summary>
    /// <param name="array"></param>
    /// <returns></returns>
    public static bool IsEmpty(double[]? array)
    {
        return array == null || !array.Any();
    }

    /// <summary>
    ///     判断数组是否为空
    /// </summary>
    /// <param name="array"></param>
    /// <returns></returns>
    public static bool IsEmpty(double[][]? array)
    {
        return array == null || !array.Any();
    }

    /// <summary>
    ///     判断数组是否为空
    /// </summary>
    /// <param name="array"></param>
    /// <returns></returns>
    public static bool IsEmpty(double[,]? array)
    {
        return array == null || array.GetLength(0) == 0 || array.GetLength(1) == 0;
    }

    /// <summary>
    ///     合并相似点
    /// </summary>
    /// <param name="points"></param>
    /// <param name="window">相似划分窗口</param>
    /// <param name="rowSimilarPitchThreshold">行相似值判断阈值</param>
    /// <param name="colSimilarPitchThreshold">列相似值判断阈值</param>
    /// <returns></returns>
    public static List<double[]> MergeSimilarPoints(List<double[]> points, string window,
        double rowSimilarPitchThreshold,
        double colSimilarPitchThreshold)
    {
        var windowConfig = window;
        var windows = windowConfig.Split(",");
        points = windows
            .Select(int.Parse)
            .Aggregate(points,
                (current, windowSize) =>
                    MergeSimilarPoints(current, windowSize, rowSimilarPitchThreshold, colSimilarPitchThreshold));

        return points;
    }


    /// <summary>
    ///     对坐标进行分区计算，提高计算效率
    /// </summary>
    /// <param name="points">合并前的坐标点</param>
    /// <param name="space">分区数</param>
    /// <returns></returns>
    private static List<List<List<double[]>>> DivideIntoRegions(List<double[]> points, int space)
    {
        // 找到坐标范围
        var minX = points.Min(p => p[0]);
        var maxX = points.Max(p => p[0]);
        var minY = points.Min(p => p[1]);
        var maxY = points.Max(p => p[1]);

        // 计算每个区域的尺寸
        var regionWidth = (maxX - minX) / space;
        var regionHeight = (maxY - minY) / space;

        // 初始化区域列表
        var regions = new List<List<List<double[]>>>(space);
        for (var i = 0; i < space; i++)
        {
            regions.Add(new List<List<double[]>>(space));
            for (var j = 0; j < space; j++) regions[i].Add([]);
        }

        // 将点分配到相应的区域
        foreach (var point in points)
        {
            var x = point[0];
            var y = point[1];
            var row = (int)Math.Floor((y - minY) / regionHeight);
            var col = (int)Math.Floor((x - minX) / regionWidth);

            // 防止由于边界值导致的索引超出范围
            row = Math.Min(row, space - 1);
            col = Math.Min(col, space - 1);

            regions[row][col].Add(point);
        }

        return regions;
    }


    /// <summary>
    ///     合并相似点
    /// </summary>
    /// <param name="points">合并前的坐标点集合</param>
    /// <param name="space">区域数</param>
    /// <param name="rowSimilarPitchThreshold">行相似值判断阈值</param>
    /// <param name="colSimilarPitchThreshold">列相似值判断阈值</param>
    /// <returns></returns>
    private static List<double[]> MergeSimilarPoints(List<double[]> points, int space, double rowSimilarPitchThreshold,
        double colSimilarPitchThreshold)
    {
        // 将整个区域划分为 若干个区域
        var regions = DivideIntoRegions(points, space);

        var merged = new List<double[]>();
        // 遍历每个区域，对每个区域进行相似点合并
        foreach (var region in regions)
        foreach (var col in region)
            merged.AddRange(MergePoints(col, rowSimilarPitchThreshold, colSimilarPitchThreshold));

        return merged;
    }

    /// <summary>
    ///     对区域内的点进行合并处理
    /// </summary>
    /// <param name="points">待合并区域</param>
    /// <param name="rowSimilarPitchThreshold">行相似值判断阈值</param>
    /// <param name="colSimilarPitchThreshold">列相似值判断阈值</param>
    /// <returns></returns>
    private static List<double[]> MergePoints(List<double[]> points, double rowSimilarPitchThreshold,
        double colSimilarPitchThreshold)
    {
        if (points.Count == 0)
            return new List<double[]>();

        // 如果两个两个点的 abs(x2-x1) <= 0.2 && abs(y2-y1) <= 0.35, 那么可以认为这两个点是相似的,只保留第一个点即可
        var merged = new List<double[]>();
        foreach (var point in points)
            // 同当前区域的其他点进行比较
            if (merged.Count == 0)
            {
                merged.Add(point);
            }
            else
            {
                var similar = false;
                foreach (var other in merged)
                    if (IsSimilar(point, other, 0.1, 0.1))
                    {
                        Console.WriteLine(
                            $"{point[0]},{point[1]} 与 {other[0]},{other[1]} 相似, X差值{point[0] - other[0]}， y差值{point[1] - other[1]}");
                        similar = true;
                        break;
                    }

                if (!similar) merged.Add(point);
            }

        return merged;
    }

    /// <summary>
    ///     判断两个点是否相似
    /// </summary>
    /// <param name="p1">第一个点</param>
    /// <param name="p2">第二个点</param>
    /// <param name="rowSimilarPitchThreshold">行相似值判断阈值</param>
    /// <param name="colSimilarPitchThreshold">列相似值判断阈值</param>
    /// <returns></returns>
    private static bool IsSimilar(double[] p1, double[] p2, double rowSimilarPitchThreshold,
        double colSimilarPitchThreshold)
    {
        return Math.Abs(p2[0] - p1[0]) <= rowSimilarPitchThreshold &&
               Math.Abs(p2[1] - p1[1]) <= colSimilarPitchThreshold;
    }


    /// <summary>
    ///     从原数组中根据匹配点数组查找相似点
    /// </summary>
    /// <param name="referencePoints"></param>
    /// <param name="comparisonPoints"></param>
    /// <param name="rowSimilarPitchThreshold">行相似值判断阈值</param>
    /// <param name="colSimilarPitchThreshold">列相似值判断阈值</param>
    /// <returns></returns>
    public static List<double[]> FindSimilarPoints(List<double[]> referencePoints, List<double[]> comparisonPoints,
        double rowSimilarPitchThreshold,
        double colSimilarPitchThreshold)
    {
        // 从原数组中根据匹配点数组查找相似点
        return referencePoints.AsParallel().Where(sourcePoint =>
            comparisonPoints.Any(matchPoint =>
                IsSimilar(sourcePoint, matchPoint, rowSimilarPitchThreshold, colSimilarPitchThreshold))
        ).ToList();
    }

    /// <summary>
    ///     晶环和pcb坐标长度对齐
    /// </summary>
    /// <param name="pcbCoordinates">原始pcb坐标</param>
    /// <param name="waferCoordinates">原始晶环坐标</param>
    /// <param name="pcbAlignedCoordinates">对齐后的pcb坐标</param>
    /// <param name="waferAlignedCoordinates">对齐后的晶环坐标</param>
    public static void CoordinateAlign(List<double[]> pcbCoordinates, List<double[]> waferCoordinates,
        out List<double[]> pcbAlignedCoordinates, out List<double[]> waferAlignedCoordinates)
    {
        pcbAlignedCoordinates = [];
        waferAlignedCoordinates = [];
        var pcbStartIndex = 0;
        var waferStartIndex = 0;
        var pStart = 0;
        var wStart = 0;
        while (pcbStartIndex <= pcbCoordinates.Count && waferStartIndex <= waferCoordinates.Count)
            if ((IsReal(pcbCoordinates, pcbStartIndex) && IsReal(waferCoordinates, waferStartIndex)) ||
                (!IsReal(pcbCoordinates, pcbStartIndex) && !IsReal(waferCoordinates, waferStartIndex)))
            {
                pcbAlignedCoordinates[pStart++] = pcbCoordinates[pcbStartIndex++];
                waferAlignedCoordinates[wStart++] = waferCoordinates[waferStartIndex++];
            }
            else if (IsReal(pcbCoordinates, pcbStartIndex) && !IsReal(waferCoordinates, waferStartIndex))
            {
                var pcbCoordinate = new double[3];
                Array.Copy(pcbCoordinates[pcbStartIndex], pcbCoordinate, 3);
                pcbCoordinate[2] = 0;
                pcbAlignedCoordinates[pStart++] = pcbCoordinate;
                waferAlignedCoordinates[wStart++] = waferCoordinates[waferStartIndex++];
            }
            else
            {
                pcbAlignedCoordinates[pStart++] = pcbCoordinates[pcbStartIndex++];
                var waferCoordinate = new double[3];
                Array.Copy(waferCoordinates[waferStartIndex], waferCoordinate, 3);
                waferCoordinate[2] = 0;
                waferAlignedCoordinates[wStart++] = waferCoordinate;
            }
    }

    /// <summary>
    ///     判断是否是真实坐标
    /// </summary>
    /// <param name="pcbCoordinates"></param>
    /// <param name="pcbStartIndex"></param>
    /// <returns></returns>
    private static bool IsReal(IReadOnlyList<double[]> pcbCoordinates, int pcbStartIndex)
    {
        return pcbCoordinates[pcbStartIndex][2] != 0;
    }

    /// <summary>
    ///     从插值路径中提取动打坐标信息
    /// </summary>
    /// <param name="coordinates">插值后的坐标数组</param>
    /// <param name="startPoint">开始的坐标</param>
    /// <param name="endPoint">借宿的坐标</param>
    public static List<double[]> SubCurrentBatchCoordinate(IEnumerable<double[]> coordinates, double[] startPoint,
        double[] endPoint)
    {
        // 保存当前截取后的新坐标
        List<double[]> newCoordinates = [];
        var target = false;

        foreach (var coordinate in coordinates)
        {
            if (Equals(coordinate, startPoint))
            {
                target = true;
            }
            else if (Equals(coordinate, endPoint))
            {
                newCoordinates.Add(coordinate);
                break;
            }

            if (target)
                // 都是目标值
                newCoordinates.Add(coordinate);
        }

        return newCoordinates;
    }

    public static bool Equals(double[] a, double[] b)
    {
        if (IsEmpty(a) || IsEmpty(b)) return false;

        return Math.Abs(a[0] - b[0]) < 0.00001 && Math.Abs(a[1] - b[1]) < 0.00001;
    }

    public static double[][] FillDefaultFlagIfNotExist(double[][] path, double defaultFlag = 1)
    {
        if (path.IsNullOrEmpty()) return path;
        if (path[0].Length == 2) return path.Select(v => new[] { v[0], v[1], defaultFlag }).ToArray();

        return path;
    }

    public static bool IsRealPoint(double flag)
    {
        return Math.Abs(Math.Round(flag, MidpointRounding.ToEven) - 1) < 0.0001;
    }

    public static double GetTowCoordinatesDistinct(double[] p1, double[] p2)
    {
        return Math.Sqrt(Math.Pow(p1[0] - p2[0], 2) + Math.Pow(p1[1] - p2[1], 2));
    }

    public static double[][] SubPcbPath(double[][] path, int rowAmount, int startColumn, int pathRowAmount)
    {
        List<double[]> result = new();

        if ((startColumn + pathRowAmount - 1) > rowAmount)
        {
            pathRowAmount = rowAmount - startColumn + 1;
        }

        var rows = RowBy(path.ToList(), rowAmount);

        for (var i = 0; i < rows.Count; i++)
        {
            var row = rows[i];
            List<double[]> chunkRow;
            if (i % 2 == 0)
            {
                chunkRow = row.Skip(startColumn - 1).Take(pathRowAmount).ToList();
            }
            else
            {
                chunkRow = row.Skip(rowAmount - (pathRowAmount + startColumn - 1)).Take(pathRowAmount).ToList();
            }

            result.AddRange(chunkRow);
        }

        return result.ToArray();
    }

    static List<List<double[]>> RowBy(List<double[]> source, int chunkSize)
    {
        List<List<double[]>> result = new List<List<double[]>>();

        for (int i = 0; i < source.Count; i += chunkSize)
        {
            result.Add(source.GetRange(i, Math.Min(chunkSize, source.Count - i)));
        }

        return result;
    }

    public static double[] AffineTransform(double[,] matrix, IReadOnlyList<double> point)
    {
        if (matrix.GetLength(0) != 3 || matrix.GetLength(1) != 3)
            throw new ArgumentException("Matrix must be a 3x3 array.");

        return new double[]
        {
            matrix[0, 0] * point[0] + matrix[0, 1] * point[1] + matrix[0, 2] * point[2],
            matrix[1, 0] * point[0] + matrix[1, 1] * point[1] + matrix[1, 2] * point[2]
        };
    }

    public static double[][] Convert2DArrayToJaggedArray(double[,] twoDArray)
    {
        int rows = twoDArray.GetLength(0); // 获取第一维度的长度，即行数
        int cols = twoDArray.GetLength(1); // 获取第二维度的长度，即列数

        double[][] jaggedArray = new double[rows][]; // 创建外层数组

        for (int i = 0; i < rows; i++)
        {
            jaggedArray[i] = new double[cols]; // 每一行创建一个新的内层数组
            for (int j = 0; j < cols; j++)
            {
                jaggedArray[i][j] = twoDArray[i, j]; // 复制元素
            }
        }

        return jaggedArray;
    }

    public static List<List<double[]>> GroupPointsIntoRows(double[][] pointsArray, double yThreshold)
    {
        // 对点数组进行排序
        var pointsArraySorted = pointsArray.OrderBy(p => p[0]).ToArray();
        List<List<double[]>> rows = new List<List<double[]>>();

        foreach (var point in pointsArraySorted)
        {
            bool placed = false;
            foreach (var row in rows)
            {
                if (Math.Abs(row.Last()[1] - point[1]) <= yThreshold && point[0] > row.Last()[0])
                {
                    row.Add(point);
                    placed = true;
                    break;
                }
            }

            if (!placed)
            {
                rows.Add(new List<double[]> { point });
            }
        }

        return rows.OrderBy(p => p[0][1]).ToList();
    }

    public static List<double[]> ConvertMatrixToList(double[,] matrix)
    {
        int rows = matrix.GetLength(0);
        int columns = matrix.GetLength(1);
        var list = new List<double[]>();

        for (int i = 0; i < rows; i++)
        {
            double[] rowArray = new double[columns];
            for (int j = 0; j < columns; j++)
            {
                rowArray[j] = matrix[i, j];
            }

            list.Add(rowArray);
        }

        return list;
    }


    /// <summary>
    /// 将 坐标数组转为 矩阵
    /// </summary>
    /// <param name="array">坐标数组</param>
    /// <param name="cols">标准分列数</param>
    public static double[,][] Array2Matrix(double[][] array, int cols)
    {
        // 总的行数
        var rows = array.Length / cols;

        var matrix = new double[rows, cols][];

        for (var i = 0; i < rows; i++)
        {
            for (var j = 0; j < cols; j++)
            {
                matrix[i, j] = array[i * cols + j];
            }
        }

        return matrix;
    }

    /// <summary>
    /// 从矩阵的中心位置将其一分为二
    /// </summary>
    /// <param name="array"></param>
    /// <returns></returns>
    public static (double[,][], double[,][]) SplitMatrixByHalf(double[,][] array)
    {
        var rows = array.GetLength(0);
        var cols = array.GetLength(1);

        var middle = cols / 2;

        var part1 = new double[rows, middle][];
        var part2 = new double[rows, cols - middle][];

        for (var i = 0; i < rows; i++)
        {
            for (var j = 0; j < cols; j++)
            {
                if (j < middle)
                    part1[i, j] = array[i, j];
                else
                    part2[i, j - middle] = array[i, j];
            }
        }

        return (part1, part2);
    }

    /// <summary>
    /// 将矩阵数组转为弓字形数组
    /// </summary>
    /// <param name="matrix"></param>
    /// <returns></returns>
    public static double[][] ConvertMatrix2BowShapeArray(double[,][] matrix)
    {
        var rows = matrix.GetLength(0);
        var cols = matrix.GetLength(1);
        var result = new double[rows * cols][];
        var index = 0;

        for (var d = 0; d < rows + cols - 1; d++)
        {
            if (d % 2 == 0) // 从左下角到右上角
            {
                for (var i = Math.Min(d, rows - 1); i >= Math.Max(0, d - cols + 1); i--)
                {
                    result[index++] = matrix[i, d - i];
                }
            }
            else // 从右上角到左下角
            {
                for (var i = Math.Max(0, d - cols + 1); i <= Math.Min(d, rows - 1); i++)
                {
                    result[index++] = matrix[i, d - i];
                }
            }
        }

        return result;
    }
}