/*
 * Copyright 2018 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;
using CartographerSharp.Mapping.Internal.D2D;
using CartographerSharp.Models.Mapping;
using CartographerSharp.Sensor;
using System.Runtime.InteropServices;
using  RobotNet10.Shared.Numbers;

namespace CartographerSharp.Mapping.D2D;

/// <summary>
/// Range data inserter for TSDF grids in 2D.
/// </summary>
public class TSDFRangeDataInserter2D : IRangeDataInserter
{
    private const int kSubpixelScale = 1000;
    private const double kMinRangeMeters = 1e-6;
    private static readonly double kSqrtTwoPi = Math.Sqrt(2.0 * Math.PI);

    private readonly TSDFRangeDataInserterOptions2D _options;

    public TSDFRangeDataInserter2D(TSDFRangeDataInserterOptions2D options)
    {
        _options = options;
    }

    /// <summary>
    /// Inserts 'range_data' into 'grid'.
    /// </summary>
    public void Insert(RangeData rangeData, IGrid grid)
    {
        if (grid is not TSDF2D tsdf)
        {
            throw new ArgumentException("Grid must be a TSDF2D", nameof(grid));
        }

        // Match C++: No FinishUpdate() before GrowAsNeeded()
        var truncationDistance = _options.TruncationDistance;
        GrowAsNeeded(rangeData, truncationDistance, tsdf);

        // Compute normals if needed
        bool scaleUpdateWeightAngleScanNormalToRay =
            _options.UpdateWeightAngleScanNormalToRayKernelBandwidth != 0.0;
        RangeData sortedRangeData = rangeData;
        List<double> normals = [];

        if (_options.ProjectSdfDistanceToScanNormal || scaleUpdateWeightAngleScanNormalToRay)
        {
            // Sort range data by angle from origin
            var returns = new List<RangefinderPoint>(rangeData.Returns.Points);
            returns.Sort(new RangeDataSorter(rangeData.Origin));
            
            sortedRangeData = new RangeData(
                rangeData.Origin,
                new PointCloud(returns),
                rangeData.Misses);

            normals = NormalEstimation2D.EstimateNormals(
                sortedRangeData,
                _options.NormalEstimationOptions);
        }

        var origin = new Vector2(sortedRangeData.Origin.X, sortedRangeData.Origin.Y);
        // Reusable buffer for ray computation - avoids per-hit List<Array2i> allocation
        var rayBuffer = new List<Array2i>(512);
        for (int hitIndex = 0; hitIndex < sortedRangeData.Returns.Count; hitIndex++)
        {
            var hitPoint = sortedRangeData.Returns.Points[hitIndex];
            var hit = new Vector2(hitPoint.Position.X, hitPoint.Position.Y);
            var normal = normals.Count > 0 ? normals[hitIndex] : double.NaN;
            InsertHit(hit, origin, normal, tsdf, rayBuffer);
        }

        tsdf.FinishUpdate();
    }

    private void InsertHit(Vector2 hit, Vector2 origin, double normal, TSDF2D tsdf, List<Array2i> rayBuffer)
    {
        var ray = hit - origin;
        var range = ray.Length();
        var truncationDistance = _options.TruncationDistance;

        if (range < truncationDistance) return;

        var truncationRatio = truncationDistance / range;
        var rayBegin = _options.UpdateFreeSpace
            ? origin
            : origin + (1.0 - truncationRatio) * ray;
        var rayEnd = origin + (1.0 + truncationRatio) * ray;

        var superscaledRay = SuperscaleRay(rayBegin, rayEnd, tsdf);
        rayBuffer.Clear();
        RayToPixelMask.ComputeInto(
            superscaledRay.Item1, superscaledRay.Item2, kSubpixelScale, rayBuffer);

        // Precompute weight factors
        double weightFactorAngleRayNormal = 1.0;
        if (_options.UpdateWeightAngleScanNormalToRayKernelBandwidth != 0.0)
        {
            var negativeRay = -ray;
            double angleRayNormal = MathUtils.NormalizeAngleDifference(
                normal - Math.Atan2(negativeRay.Y, negativeRay.X));
            weightFactorAngleRayNormal = GaussianKernel(
                angleRayNormal,
                _options.UpdateWeightAngleScanNormalToRayKernelBandwidth);
        }

        double weightFactorRange = 1.0;
        if (_options.UpdateWeightRangeExponent != 0)
        {
            weightFactorRange = ComputeRangeWeightFactor(
                range, _options.UpdateWeightRangeExponent);
        }

        // Update cells using Span for bounds-check-free iteration
        var raySpan = CollectionsMarshal.AsSpan(rayBuffer);
        for (int i = 0; i < raySpan.Length; i++)
        {
            var cellIndex = raySpan[i];
            if (tsdf.CellIsUpdated(cellIndex)) continue;

            var cellCenter = tsdf.Limits.GetCellCenter(cellIndex);
            double distanceCellToOrigin = (cellCenter - origin).Length();
            double updateTSD = range - distanceCellToOrigin;

            // Match C++: if (options_.project_sdf_distance_to_scan_normal()) {
            // No NaN check in C++
            if (_options.ProjectSdfDistanceToScanNormal)
            {
                double normalOrientation = normal;
                var normalVector = new Vector2(
                    Math.Cos(normalOrientation),
                    Math.Sin(normalOrientation));
                updateTSD = Vector2.Dot(cellCenter - hit, normalVector);
            }

            updateTSD = Math.Clamp(updateTSD, -truncationDistance, truncationDistance);

            double updateWeight = weightFactorRange * weightFactorAngleRayNormal;
            if (_options.UpdateWeightDistanceCellToHitKernelBandwidth != 0.0)
            {
                updateWeight *= GaussianKernel(
                    updateTSD,
                    _options.UpdateWeightDistanceCellToHitKernelBandwidth);
            }

            UpdateCell(cellIndex, updateTSD, updateWeight, tsdf);
        }
    }

    private void UpdateCell(Array2i cell, double updateSdf, double updateWeight, TSDF2D tsdf)
    {
        if (updateWeight == 0.0) return;

        var (currentTSD, currentWeight) = tsdf.GetTSDAndWeight(cell);
        double updatedWeight = currentWeight + updateWeight;
        double updatedSDF = (currentTSD * currentWeight + updateSdf * updateWeight) / updatedWeight;
        updatedWeight = Math.Min(updatedWeight, _options.MaximumWeight);
        
        tsdf.SetCell(cell, updatedSDF, updatedWeight);
    }

    private static void GrowAsNeeded(RangeData rangeData, double truncationDistance, TSDF2D tsdf)
    {
        // Match C++: Eigen::AlignedBox2f bounding_box(range_data.origin.head<2>());
        // Then only extend end_position, not hit.position
        var origin2D = new Vector2(rangeData.Origin.X, rangeData.Origin.Y);
        var boundingBoxMin = origin2D;
        var boundingBoxMax = origin2D;

        foreach (var hit in rangeData.Returns.Points)
        {
            var hit2D = new Vector2(hit.Position.X, hit.Position.Y);
            var direction = Vector2.Normalize(hit2D - origin2D);
            var endPosition = hit2D + truncationDistance * direction;
            
            // Match C++: bounding_box.extend(end_position.head<2>());
            // Only extend end_position, not hit.position
            boundingBoxMin.X = Math.Min(boundingBoxMin.X, endPosition.X);
            boundingBoxMin.Y = Math.Min(boundingBoxMin.Y, endPosition.Y);
            boundingBoxMax.X = Math.Max(boundingBoxMax.X, endPosition.X);
            boundingBoxMax.Y = Math.Max(boundingBoxMax.Y, endPosition.Y);
        }

        const double kPadding = 1e-6;
        tsdf.GrowLimits(boundingBoxMin - new Vector2(kPadding, kPadding));
        tsdf.GrowLimits(boundingBoxMax + new Vector2(kPadding, kPadding));
    }

    private static (Array2i, Array2i) SuperscaleRay(
        Vector2 begin, Vector2 end, TSDF2D tsdf)
    {
        // Match C++: const MapLimits superscaled_limits(
        //     superscaled_resolution, limits.max(), ...);
        // Use limits.max() directly, no calculation
        var limits = tsdf.Limits;
        var superscaledResolution = limits.Resolution / kSubpixelScale;
        var superscaledCellLimits = new CellLimits(
            limits.CellLimits.NumXCells * kSubpixelScale,
            limits.CellLimits.NumYCells * kSubpixelScale);
        var superscaledLimits = new MapLimits(
            superscaledResolution, limits.Max, superscaledCellLimits);

        var superscaledBegin = superscaledLimits.GetCellIndex(begin);
        var superscaledEnd = superscaledLimits.GetCellIndex(end);
        
        // Match C++: return std::make_pair(superscaled_begin, superscaled_end);
        // No multiplication by kSubpixelScale - GetCellIndex already returns superscaled indices
        return (superscaledBegin, superscaledEnd);
    }

    // Match C++: No sigma == 0 check
    private static double GaussianKernel(double x, double sigma)
    {
        return 1.0 / (kSqrtTwoPi * sigma) * Math.Exp(-0.5 * x * x / (sigma * sigma));
    }

    private static double ComputeRangeWeightFactor(double range, int exponent)
    {
        if (Math.Abs(range) <= kMinRangeMeters) return 0.0;
        return 1.0 / Math.Pow(range, exponent);
    }

    /// <summary>
    /// Sorts range data points by angle from origin.
    /// </summary>
    private class RangeDataSorter : IComparer<RangefinderPoint>
    {
        private readonly Vector2 _origin;

        public RangeDataSorter(Vector3 origin)
        {
            _origin = new Vector2(origin.X, origin.Y);
        }

        public int Compare(RangefinderPoint lhs, RangefinderPoint rhs)
        {
            var deltaLhs = Vector2.Normalize(
                new Vector2(lhs.Position.X, lhs.Position.Y) - _origin);
            var deltaRhs = Vector2.Normalize(
                new Vector2(rhs.Position.X, rhs.Position.Y) - _origin);

            if ((deltaLhs.Y < 0.0) != (deltaRhs.Y < 0.0))
            {
                return deltaLhs.Y < 0.0 ? -1 : 1;
            }
            else if (deltaLhs.Y < 0.0)
            {
                return deltaLhs.X < deltaRhs.X ? -1 : (deltaLhs.X > deltaRhs.X ? 1 : 0);
            }
            else
            {
                return deltaLhs.X > deltaRhs.X ? -1 : (deltaLhs.X < deltaRhs.X ? 1 : 0);
            }
        }
    }
}