/*
 * Copyright 2016 The Cartographer Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

using CartographerSharp.Common.Math;

namespace CartographerSharp.Mapping.Internal.D2D;

/// <summary>
/// Ray to pixel mask utilities.
/// </summary>
public static class RayToPixelMask
{
    /// <summary>
    /// Compute all pixels that contain some part of the line segment connecting
    /// 'scaled_begin' and 'scaled_end'. 'scaled_begin' and 'scaled_end' are scaled
    /// by 'subpixel_scale'. 'scaled_begin' and 'scaled_end' are expected to be
    /// greater than zero. Return values are in pixels and not scaled.
    /// </summary>
    public static List<Array2i> Compute(
        Array2i scaledBegin,
        Array2i scaledEnd,
        int subpixelScale)
    {
        var result = new List<Array2i>();
        ComputeInto(scaledBegin, scaledEnd, subpixelScale, result);
        return result;
    }

    /// <summary>
    /// Same as Compute but appends results into an existing list (caller must Clear beforehand).
    /// This avoids per-ray List allocation when processing many rays in a loop.
    /// </summary>
    public static void ComputeInto(
        Array2i scaledBegin,
        Array2i scaledEnd,
        int subpixelScale,
        List<Array2i> pixelMask)
    {
        // For simplicity, we order 'scaled_begin' and 'scaled_end' by their x
        // coordinate.
        if (scaledBegin.X > scaledEnd.X)
        {
            ComputeInto(scaledEnd, scaledBegin, subpixelScale, pixelMask);
            return;
        }

        // Match C++ CHECK_GE assertions for all coordinates
        if (scaledBegin.X < 0 || scaledBegin.Y < 0 || scaledEnd.X < 0 || scaledEnd.Y < 0)
        {
            throw new ArgumentException("Scaled coordinates must be non-negative");
        }

        // Track last added element to avoid consecutive duplicates (avoids pixelMask[^1] indexer overhead)
        int lastX = int.MinValue, lastY = int.MinValue;

        // Special case: We have to draw a vertical line in full pixels, as
        // 'scaled_begin' and 'scaled_end' have the same full pixel x coordinate.
        if (scaledBegin.X / subpixelScale == scaledEnd.X / subpixelScale)
        {
            var cx = scaledBegin.X / subpixelScale;
            var cy = Math.Min(scaledBegin.Y, scaledEnd.Y) / subpixelScale;
            pixelMask.Add(new Array2i(cx, cy));
            lastX = cx; lastY = cy;
            var endY = Math.Max(scaledBegin.Y, scaledEnd.Y) / subpixelScale;
            for (; cy <= endY; cy++)
            {
                if (cx != lastX || cy != lastY)
                {
                    pixelMask.Add(new Array2i(cx, cy));
                    lastX = cx; lastY = cy;
                }
            }
            return;
        }

        // Match C++ int64 types to prevent integer overflow
        long dx = (long)scaledEnd.X - scaledBegin.X;
        long dy = (long)scaledEnd.Y - scaledBegin.Y;
        long denominator = 2L * subpixelScale * dx;

        // The current full pixel coordinates. We start at 'scaled_begin'.
        int curX = scaledBegin.X / subpixelScale;
        int curY = scaledBegin.Y / subpixelScale;
        pixelMask.Add(new Array2i(curX, curY));
        lastX = curX; lastY = curY;

        // The center of the subpixel part of 'scaled_begin.y()' assuming the
        // 'denominator', i.e., sub_y / denominator is in (0, 1).
        long subY = (2L * (scaledBegin.Y % subpixelScale) + 1) * dx;

        // The distance from the from 'scaled_begin' to the right pixel border, to be
        // divided by 2 * 'subpixel_scale'.
        long firstPixel = 2L * subpixelScale - 2L * (scaledBegin.X % subpixelScale) - 1;
        // The same from the left pixel border to 'scaled_end'.
        long lastPixel = 2L * (scaledEnd.X % subpixelScale) + 1;

        // The full pixel x coordinate of 'scaled_end'.
        var endX = Math.Max(scaledBegin.X, scaledEnd.X) / subpixelScale;

        // Move from 'scaled_begin' to the next pixel border to the right.
        subY += dy * firstPixel;

        if (dy > 0)
        {
            while (true)
            {
                if (curX != lastX || curY != lastY)
                {
                    pixelMask.Add(new Array2i(curX, curY));
                    lastX = curX; lastY = curY;
                }
                while (subY > denominator)
                {
                    subY -= denominator;
                    curY++;
                    if (curX != lastX || curY != lastY)
                    {
                        pixelMask.Add(new Array2i(curX, curY));
                        lastX = curX; lastY = curY;
                    }
                }
                curX++;
                if (subY == denominator)
                {
                    subY -= denominator;
                    curY++;
                }
                if (curX == endX)
                {
                    break;
                }
                // Move from one pixel border to the next.
                subY += dy * 2L * subpixelScale;
            }
            // Move from the pixel border on the right to 'scaled_end'.
            subY += dy * lastPixel;

            if (curX != lastX || curY != lastY)
            {
                pixelMask.Add(new Array2i(curX, curY));
                lastX = curX; lastY = curY;
            }
            while (subY > denominator)
            {
                subY -= denominator;
                curY++;
                if (curX != lastX || curY != lastY)
                {
                    pixelMask.Add(new Array2i(curX, curY));
                    lastX = curX; lastY = curY;
                }
            }

            // Match C++ CHECK_NE(sub_y, denominator) - subY should not equal denominator
            if (subY == denominator)
            {
                throw new InvalidOperationException("subY should not equal denominator");
            }

            // Match C++ CHECK_EQ(current.y(), scaled_end.y() / subpixel_scale)
            var expectedY = scaledEnd.Y / subpixelScale;
            if (curY != expectedY)
            {
                throw new InvalidOperationException($"Current y should equal scaledEnd.Y / subpixelScale: current.Y={curY}, scaledEnd.Y/subpixelScale={expectedY}, scaledBegin=({scaledBegin.X}, {scaledBegin.Y}), scaledEnd=({scaledEnd.X}, {scaledEnd.Y}), subpixelScale={subpixelScale}");
            }
            return;
        }

        // Same for lines non-ascending in y coordinates.
        while (true)
        {
            if (curX != lastX || curY != lastY)
            {
                pixelMask.Add(new Array2i(curX, curY));
                lastX = curX; lastY = curY;
            }
            while (subY < 0)
            {
                subY += denominator;
                curY--;
                if (curX != lastX || curY != lastY)
                {
                    pixelMask.Add(new Array2i(curX, curY));
                    lastX = curX; lastY = curY;
                }
            }
            curX++;
            if (subY == 0)
            {
                subY += denominator;
                curY--;
            }
            if (curX == endX)
            {
                break;
            }
            subY += dy * 2L * subpixelScale;
        }
        subY += dy * lastPixel;
        if (curX != lastX || curY != lastY)
        {
            pixelMask.Add(new Array2i(curX, curY));
            lastX = curX; lastY = curY;
        }
        while (subY < 0)
        {
            subY += denominator;
            curY--;
            if (curX != lastX || curY != lastY)
            {
                pixelMask.Add(new Array2i(curX, curY));
                lastX = curX; lastY = curY;
            }
        }
        // Match C++: CHECK_NE(sub_y, 0)
        if (subY == 0)
        {
            throw new InvalidOperationException("subY should not equal 0");
        }
        // Match C++: CHECK_EQ(current.y(), scaled_end.y() / subpixel_scale)
        if (curY != scaledEnd.Y / subpixelScale)
        {
            throw new InvalidOperationException($"Current y should equal scaledEnd.y / subpixelScale: current.Y={curY}, scaledEnd.Y/subpixelScale={scaledEnd.Y / subpixelScale}, scaledBegin=({scaledBegin.X}, {scaledBegin.Y}), scaledEnd=({scaledEnd.X}, {scaledEnd.Y}), subpixelScale={subpixelScale}");
        }
    }
}