using CartographerSharp.Mapping;
using RobotNet10.RobotApp.Shared;
using RobotNet10.RobotApp.SLAM.Cartographer;
using RobotNet10.Shared.Localization;
using System.Text.Json;

namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;

/// <summary>
/// Manages occupancy grid loading, updating, and caching
/// Handles throttled updates during ScanMapping using counter-based logic
/// </summary>
public class OccupancyGridManager(CartographerConfiguration _config, ILogger<OccupancyGridManager> _logger)
{
    private readonly Lock _lock = new();

    private OccupancyGrid? _occupancyGrid;
    private OccupancyGrid? _occupancyGridMcl;
    private DateTime _lastUpdatedOccupancyGrid = DateTime.MinValue;
    private volatile int _generationInProgress = 0; // 1 = generation running

    // Version tracking: PoseGraph node insertion version at last successful generation.
    // Compared against the live PoseGraph version to skip redundant regenerations.
    private int _lastGeneratedVersion = -1;

    // Configuration: time-based throttling (~3 seconds between updates)
    private readonly System.Diagnostics.Stopwatch _updateStopwatch = System.Diagnostics.Stopwatch.StartNew();
    private const double UpdateIntervalSeconds = 3.0;

    // Insertion tracking: flag to track if at least one scan was inserted into submap since last update
    // Used in ScanMapping mode to ensure we only update when there's new data
    private volatile bool _hasInsertionSinceLastUpdate = false;

    public OccupancyGrid? OccupancyGrid
    {
        get
        {
            lock (_lock) { return _occupancyGrid; }
        }
    }

    public OccupancyGrid? OccupancyGridMcl
    {
        get
        {
            lock (_lock) { return _occupancyGridMcl; }
        }
    }

    public DateTime LastUpdated
    {
        get
        {
            lock (_lock) { return _lastUpdatedOccupancyGrid; }
        }
    }

    /// <summary>
    /// Get occupancy grid, optionally checking if updated since a given time
    /// </summary>
    public OccupancyGrid? GetGrid(DateTime since = default)
    {
        lock (_lock)
        {
            if (since == DateTime.MinValue)
                return _occupancyGrid;
            return _lastUpdatedOccupancyGrid > since ? _occupancyGrid : null;
        }
    }

    /// <summary>
    /// Signal that a scan was inserted into a submap (InsertionResult.HasValue = true).
    /// Called from ProcessWithTrajectoryBuilder when AddSensorData returns insertion result.
    /// This flag is required for ShouldUpdateGrid to return true.
    /// </summary>
    public void SignalInsertion()
    {
        _hasInsertionSinceLastUpdate = true;
    }

    /// <summary>
    /// Check if grid should be updated based on two conditions:
    /// 1. Enough time has elapsed since last update (3 seconds) OR grid is null
    /// 2. At least one scan was inserted into submap since last update (SignalInsertion was called)
    /// Both conditions must be met for update to proceed.
    /// Thread-safe: prevents concurrent generation via _generationInProgress flag.
    /// </summary>
    public bool ShouldUpdateGrid()
    {
        // Fast check: skip if generation is already in progress
        if (Interlocked.CompareExchange(ref _generationInProgress, 0, 0) == 1)
            return false;

        lock (_lock)
        {
            bool gridIsNull = _occupancyGrid == null;
            bool timeElapsed = _updateStopwatch.Elapsed.TotalSeconds >= UpdateIntervalSeconds;
            bool hasInsertion = _hasInsertionSinceLastUpdate;

            // Require BOTH: (time elapsed OR grid null) AND has insertion
            if ((gridIsNull || timeElapsed) && hasInsertion)
            {
                _logger.LogDebug(
                    "OccupancyGridManager: ShouldUpdateGrid=true, gridIsNull={GridIsNull}, elapsed={Elapsed:F1}s, hasInsertion={HasInsertion}",
                    gridIsNull, _updateStopwatch.Elapsed.TotalSeconds, hasInsertion);
                _updateStopwatch.Restart();
                _hasInsertionSinceLastUpdate = false; // Reset insertion flag after update
                return true;
            }
            return false;
        }
    }

    /// <summary>
    /// Update occupancy grid from MapBuilder submaps during ScanMapping
    /// Only generates Display grid for visualization (MCL not used during mapping)
    /// </summary>
    public void UpdateFromMapBuilder(IMapBuilder mapBuilder)
    {
        // Prevent concurrent generation
        if (Interlocked.CompareExchange(ref _generationInProgress, 1, 0) != 0)
        {
            _logger.LogDebug("OccupancyGridManager: Skipping - generation already in progress");
            return;
        }

        try
        {
            if (mapBuilder == null)
            {
                _logger.LogWarning("OccupancyGridManager: UpdateFromMapBuilder called with null mapBuilder");
                return;
            }

            var resolution = _config.MapStorage.OccupancyGridResolution;
            var padding = _config.MapStorage.MapPadding;
            var strategy = _config.OccupancyGrid?.MergeStrategy ?? SubmapMergeStrategy.LogOddsSum;

            var sw = System.Diagnostics.Stopwatch.StartNew();

            // Generate display grid using configured merge strategy.
            // Pass the last generated version so the generator can skip if no new nodes
            // have been inserted (avoids redundant regeneration of identical data).
            var displayGrid = OccupancyGridGenerator.Generate(
                mapBuilder, resolution, padding,
                Volatile.Read(ref _lastGeneratedVersion),
                out var snapshotVersion,
                _logger, _config.OccupancyGrid);

            sw.Stop();

            if (displayGrid == null)
            {
                lock (_lock)
                {
                    _occupancyGrid = null;
                    _occupancyGridMcl = null;
                    _lastUpdatedOccupancyGrid = DateTime.MinValue;
                }
                return;
            }

            // Count cell statistics for debugging
            int freeCells = 0, occupiedCells = 0, unknownCells = 0;
            for (int i = 0; i < displayGrid.Data.Length; i++)
            {
                if (displayGrid.Data[i] < 0) unknownCells++;
                else if (displayGrid.Data[i] == 0) freeCells++;
                else occupiedCells++;
            }

            lock (_lock)
            {
                _occupancyGrid = displayGrid;
                // Keep existing mclGrid (if any) - don't regenerate during mapping
                _lastUpdatedOccupancyGrid = DateTime.UtcNow;
            }

            // Cache the version so the next cycle can skip if no new data was added.
            Volatile.Write(ref _lastGeneratedVersion, snapshotVersion);
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "OccupancyGridManager: Failed to update grid from MapBuilder");
        }
        finally
        {
            Interlocked.Exchange(ref _generationInProgress, 0);
        }
    }

    /// <summary>
    /// Load occupancy grid from PGM file for localization
    /// </summary>
    public void LoadFromPgm(string mapName)
    {
        try
        {
            var mapPath = Path.Combine(_config.MapStorage.Directory, mapName);
            var pgmPath = Path.Combine(mapPath, "map.pgm");

            if (!File.Exists(pgmPath))
            {
                var pgmFiles = Directory.GetFiles(mapPath, "*.pgm", SearchOption.TopDirectoryOnly);
                if (pgmFiles.Length == 0)
                {
                    _logger.LogWarning("OccupancyGridManager: No PGM file found for map: {MapName}", mapName);
                    Clear();
                    return;
                }
                pgmPath = pgmFiles[0];
            }

            // Load grid once and clone for MCL (Fix 2: avoid loading PGM file twice)
            // UNIFIED CONVENTION: Both use ROS convention (row 0 = world BOTTOM, Y-axis pointing UP)
            var displayGrid = PgmLoader.LoadFromPgm(pgmPath, logger: _logger);
            OccupancyGrid? mclGrid = null;
            if (displayGrid != null)
            {
                mclGrid = new OccupancyGrid
                {
                    Resolution = displayGrid.Resolution,
                    Width = displayGrid.Width,
                    Height = displayGrid.Height,
                    Origin = displayGrid.Origin,
                    Data = (sbyte[])displayGrid.Data.Clone()
                };
            }

            // Load origin from map.json if available
            if (displayGrid != null && mclGrid != null)
            {
                var mapJsonPath = Path.Combine(mapPath, "map.json");
                if (File.Exists(mapJsonPath))
                {
                    try
                    {
                        var jsonContent = File.ReadAllText(mapJsonPath);
                        var metadata = JsonSerializer.Deserialize<MapInfo>(jsonContent);
                        if (metadata?.Origin != null)
                        {
                            displayGrid.Origin = metadata.Origin;
                            mclGrid.Origin = metadata.Origin;
                        }
                    }
                    catch (Exception ex)
                    {
                        _logger.LogWarning(ex, "OccupancyGridManager: Could not read origin from map.json");
                    }
                }
            }

            lock (_lock)
            {
                _occupancyGrid = displayGrid;
                _occupancyGridMcl = mclGrid;
                _lastUpdatedOccupancyGrid = DateTime.UtcNow;
            }
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "OccupancyGridManager: Failed to load PGM for map: {MapName}", mapName);
        }
    }

    /// <summary>
    /// Clear grids for new scanning session
    /// </summary>
    public void Clear()
    {
        lock (_lock)
        {
            _occupancyGrid = null;
            _occupancyGridMcl = null;
            _lastUpdatedOccupancyGrid = DateTime.MinValue;
            _updateStopwatch.Restart();
        }
        _hasInsertionSinceLastUpdate = false;
        Volatile.Write(ref _lastGeneratedVersion, -1);
    }

    /// <summary>
    /// Reset timer and insertion flag for new state (ScanMapping/Localizing)
    /// </summary>
    public void ResetCounter()
    {
        lock (_lock)
        {
            _updateStopwatch.Restart();
        }
        _hasInsertionSinceLastUpdate = false;
        Volatile.Write(ref _lastGeneratedVersion, -1);
    }

    /// <summary>
    /// Dispose resources
    /// </summary>
    public void Dispose()
    {
        lock (_lock)
        {
            _occupancyGrid = null;
            _occupancyGridMcl = null;
        }
    }
}