/*
 * 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 System.Threading.Channels;
using CartographerSharp.Models.Mapping;
using CartographerSharp.Sensor;
using CartographerSharp.Transform;
using  RobotNet10.Shared.Numbers;

namespace CartographerSharp.Mapping.D2D;

/// <summary>
/// The first active submap will be created on the insertion of the first range
/// data. Except during this initialization when no or only one single submap
/// exists, there are always two submaps into which range data is inserted: an
/// old submap that is used for matching, and a new one, which will be used for
/// matching next, that is being initialized.
///
/// Once a certain number of range data have been inserted, the new submap is
/// considered initialized: the old submap is no longer changed, the "new" submap
/// is now the "old" submap and is used for scan-to-map matching. Moreover, a
/// "new" submap gets created. The "old" submap is forgotten by this object.
///
/// The front (old) submap is always inserted synchronously since it is used for
/// scan matching immediately. The back (new) submap is inserted asynchronously
/// via a background worker to reduce per-frame blocking latency.
/// </summary>
public class ActiveSubmaps2D(SubmapsOptions2D options) : IDisposable
{
    private const int kInitialSubmapSize = 100;

    private readonly SubmapsOptions2D _options = options;
    private readonly List<Submap2D> _submaps = [];
    private readonly ValueConversionTables _conversionTables = new();
    private IRangeDataInserter? _rangeDataInserter;
    private readonly Lock _insertLock = new(); // Lock to prevent concurrent inserts

    // Async back submap insertion infrastructure.
    // The back submap is queued to a Channel and processed by a single long-running
    // worker task, preserving insertion order (SingleReader). The front submap is
    // always inserted synchronously for immediate scan matching availability.
    private readonly Channel<(RangeData rangeData, Submap2D submap, IRangeDataInserter inserter)>
        _backSubmapChannel = Channel.CreateUnbounded<(RangeData, Submap2D, IRangeDataInserter)>(
            new UnboundedChannelOptions { SingleReader = true });
    private Task? _backSubmapWorker;
    private int _backSubmapExpectedCount;  // Tracks intended NumRangeData of back submap (including queued)
    private int _backSubmapPendingCount;   // Items queued but not yet processed (Interlocked)

    /// <summary>
    /// Inserts 'range_data' into the Submap collection.
    /// Front submap: synchronous (used for scan matching immediately).
    /// Back submap: queued to background worker (fire-and-forget).
    /// </summary>
    public List<Submap2D> InsertRangeData(RangeData rangeData)
    {
        lock (_insertLock)
        {
            var submapOrigin = new Vector2(rangeData.Origin.X, rangeData.Origin.Y);
            var submapPose = new Rigid3d(
                new Vector3(submapOrigin.X, submapOrigin.Y, 0.0),
                Quaternion.Identity);

            // Use _backSubmapExpectedCount instead of back submap's NumRangeData
            // because the back submap's actual count may lag (async insertions).
            if (_submaps.Count == 0 ||
                (_submaps.Count > 0 && _backSubmapExpectedCount == _options.NumRangeData))
            {
                // Drain all pending back submap insertions before submap rotation
                // so the back submap is fully up-to-date when it becomes the front.
                DrainBackSubmapQueue();
                AddSubmap(submapPose);
                _backSubmapExpectedCount = 0;
            }

            _rangeDataInserter ??= CreateRangeDataInserter();

            if (_submaps.Count >= 2)
            {
                // Front submap: SYNCHRONOUS (used for scan matching immediately)
                _submaps[0].InsertRangeData(rangeData, _rangeDataInserter);

                // Back submap: ASYNC (queue to background worker)
                // Increment pending BEFORE writing to channel to ensure drain correctness.
                Interlocked.Increment(ref _backSubmapPendingCount);
                _backSubmapChannel.Writer.TryWrite((rangeData, _submaps[1], _rangeDataInserter));
                _backSubmapWorker ??= Task.Factory.StartNew(
                    ProcessBackSubmapQueue, TaskCreationOptions.LongRunning);
            }
            else
            {
                // Only 1 submap - insert synchronously
                for (int si = 0; si < _submaps.Count; si++)
                {
                    _submaps[si].InsertRangeData(rangeData, _rangeDataInserter);
                }
            }
            _backSubmapExpectedCount++;

            // Finish front submap when it reaches 2x threshold.
            // Front is always up-to-date (synchronous insert).
            if (_submaps.Count > 0 && _submaps[0].NumRangeData == _options.NumRangeData * 2)
            {
                _submaps[0].Finish();
            }

            return [.. _submaps];
        }
    }

    /// <summary>
    /// Forces the current front submap to finish, creates a new submap at the range data origin,
    /// and inserts range data into all active submaps. Used to break the deadlock when the robot
    /// enters genuinely new territory and consecutive hard-limit Ceres failures accumulate.
    /// </summary>
    public List<Submap2D> ForceNewSubmapAndInsert(RangeData rangeData)
    {
        lock (_insertLock)
        {
            // Drain any pending back submap insertions before manipulating submaps.
            DrainBackSubmapQueue();

            var submapOrigin = new Vector2(rangeData.Origin.X, rangeData.Origin.Y);
            var submapPose = new Rigid3d(
                new Vector3(submapOrigin.X, submapOrigin.Y, 0.0),
                Quaternion.Identity);

            if (_submaps.Count == 0)
            {
                // No submaps yet - just create the first one via normal flow
                AddSubmap(submapPose);
            }
            else if (_submaps.Count == 1)
            {
                // FIX: When only 1 submap exists, finish it and REMOVE it before creating the new one.
                // Previously, finishing + AddSubmap would leave [finished, new] and the subsequent
                // InsertRangeData loop would crash on the finished front submap.
                if (!_submaps[0].InsertionFinished)
                {
                    _submaps[0].Finish();
                }
                _submaps.RemoveAt(0);
                AddSubmap(submapPose);
            }
            else
            {
                // 2 submaps: finish front if needed, then AddSubmap removes it and creates new
                if (!_submaps[0].InsertionFinished)
                {
                    _submaps[0].Finish();
                }
                AddSubmap(submapPose);
            }

            // Reset expected count after submap manipulation.
            _backSubmapExpectedCount = 0;

            // Insert range data into all active submaps sequentially.
            // ForceNewSubmap is a rare recovery path (consecutive Ceres failures),
            // so sequential insert is simpler and avoids thread-safety risks.
            _rangeDataInserter ??= CreateRangeDataInserter();

            for (int si = 0; si < _submaps.Count; si++)
            {
                _submaps[si].InsertRangeData(rangeData, _rangeDataInserter);
            }
            _backSubmapExpectedCount++;

            return [.. _submaps];
        }
    }

    /// <summary>
    /// Gets the current active submaps.
    /// </summary>
    public List<Submap2D> Submaps()
    {
        return [.. _submaps];
    }

    /// <summary>
    /// Background worker that sequentially processes queued back submap insertions.
    /// Uses per-item error handling for resilience - a single failed insertion
    /// should not crash the entire SLAM pipeline.
    /// </summary>
    private async Task ProcessBackSubmapQueue()
    {
        await foreach (var (rangeData, submap, inserter) in _backSubmapChannel.Reader.ReadAllAsync())
        {
            try
            {
                submap.InsertRangeData(rangeData, inserter);
            }
            catch (Exception ex)
            {
                Console.WriteLine($"[SUBMAP_ASYNC] Back submap insert error: {ex.Message}");
            }
            Interlocked.Decrement(ref _backSubmapPendingCount);
        }
    }

    /// <summary>
    /// Waits for all pending back submap insertions to complete.
    /// Called before submap rotation (AddSubmap) to ensure the back submap is
    /// fully up-to-date before it becomes the front submap used for scan matching.
    /// Called infrequently (every NumRangeData frames, typically ~90).
    /// </summary>
    private void DrainBackSubmapQueue()
    {
        if (Volatile.Read(ref _backSubmapPendingCount) == 0) return;
        var sw = new SpinWait();
        while (Volatile.Read(ref _backSubmapPendingCount) > 0)
        {
            sw.SpinOnce();
        }
    }

    public void Dispose()
    {
        _backSubmapChannel.Writer.Complete();
        _backSubmapWorker?.GetAwaiter().GetResult();
    }

    private IRangeDataInserter CreateRangeDataInserter()
    {
        // Match C++ logic: switch case with LOG(FATAL) for unknown types
        var options = _options.RangeDataInserterOptions;

        switch (options.RangeDataInserterTypeValue)
        {
            case RangeDataInserterOptions.RangeDataInserterType.ProbabilityGridInserter2D:
                if (options.ProbabilityGridRangeDataInserterOptions2D.HasValue)
                {
                    return new ProbabilityGridRangeDataInserter2D(options.ProbabilityGridRangeDataInserterOptions2D.Value);
                }
                throw new ArgumentException("ProbabilityGridRangeDataInserterOptions2D is required for ProbabilityGrid inserter");

            case RangeDataInserterOptions.RangeDataInserterType.TsdfInserter2D:
                if (options.TsdfRangeDataInserterOptions2D.HasValue)
                {
                    return new TSDFRangeDataInserter2D(options.TsdfRangeDataInserterOptions2D.Value);
                }
                throw new ArgumentException("TSDFRangeDataInserterOptions2D is required for TSDF inserter");

            default:
                throw new ArgumentException($"Unknown RangeDataInserterType: {options.RangeDataInserterTypeValue}");
        }
    }

    private Grid2D CreateGrid(Vector2 origin)
    {
        return CreateGridWithOptions(origin, _options.GridOptions2D);
    }

    private Grid2D? CreateHighResGrid(Vector2 origin)
    {
        if (_options.HighResGridOptions2D == null)
            return null;
        return CreateGridWithOptions(origin, _options.HighResGridOptions2D.Value);
    }

    private Grid2D CreateGridWithOptions(Vector2 origin, GridOptions2D gridOptions)
    {
        var resolution = gridOptions.Resolution;
        // Calculate initial size to cover ±18m (matching MaxRange) at this resolution
        var initialSize = Math.Max(100, (int)(kInitialSubmapSize * _options.GridOptions2D.Resolution / resolution));
        var mapLimits = new MapLimits(
            resolution,
            new Vector2(
                origin.X + 0.5 * initialSize * resolution,
                origin.Y + 0.5 * initialSize * resolution
            ),
            new CellLimits(initialSize, initialSize)
        );

        // Match C++ logic: switch case with LOG(FATAL) for unknown types
        switch (gridOptions.GridTypeValue)
        {
            case GridOptions2D.GridType.ProbabilityGrid:
                return new ProbabilityGrid(mapLimits, _conversionTables);

            case GridOptions2D.GridType.Tsdf:
                // Match C++: Get truncation_distance and maximum_weight from range_data_inserter_options
                // C++: options_.range_data_inserter_options().tsdf_range_data_inserter_options_2d()
                if (_options.RangeDataInserterOptions.TsdfRangeDataInserterOptions2D.HasValue)
                {
                    var tsdfOptions = _options.RangeDataInserterOptions.TsdfRangeDataInserterOptions2D.Value;
                    return new TSDF2D(
                        mapLimits,
                        tsdfOptions.TruncationDistance,
                        tsdfOptions.MaximumWeight,
                        _conversionTables
                    );
                }
                throw new ArgumentException("TSDFRangeDataInserterOptions2D is required for TSDF grid type");

            case GridOptions2D.GridType.InvalidGrid:
                throw new ArgumentException("Invalid grid type specified");

            default:
                throw new ArgumentException($"Unknown grid type: {gridOptions.GridTypeValue}");
        }
    }

    private void AddSubmap(Rigid3d localSubmapPose)
    {
        // Match C++ logic: if (submaps_.size() >= 2) { CHECK(submaps_.front()->insertion_finished()); submaps_.erase(submaps_.begin()); }
        if (_submaps.Count >= 2)
        {
            // This will crop the finished Submap before inserting a new Submap to
            // reduce peak memory usage a bit.
            if (!_submaps[0].InsertionFinished)
            {
                throw new InvalidOperationException("First submap must be finished before adding a new one");
            }
            _submaps.RemoveAt(0);
        }

        // Match C++: Extract origin from pose (C++ passes Vector2f directly, but we have Rigid3d)
        var origin = new Vector2(localSubmapPose.Translation.X, localSubmapPose.Translation.Y);
        var grid = CreateGrid(origin);
        var highResGrid = CreateHighResGrid(origin);

        // Match C++: Submap2D(origin, grid, conversion_tables)
        // C++ constructor takes Vector2f origin, but C# Submap2D takes Rigid3d (which includes origin)
        var submap = new Submap2D(
            localSubmapPose, // Pass the full Rigid3d pose including rotation
            grid,
            _conversionTables,
            highResGrid
        );
        _submaps.Add(submap);
    }
}