using CartographerSharp.Mapping;
using Microsoft.Extensions.Logging;

namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;

/// <summary>
/// Cache for pose graph constraints of a single trajectory.
/// Used by CartographerService to avoid repeatedly querying constraints when calculating covariance.
/// </summary>
/// <remarks>
/// Creates a constraint cache with the given refresh interval.
/// </remarks>
public sealed class TrajectoryConstraintCache(TimeSpan _updateInterval)
{
    private readonly Lock _lock = new();
    private List<IPoseGraph.Constraint>? _cachedConstraints;
    private DateTime _lastUpdate = DateTime.MinValue;
    private IMapBuilder? _lastMapBuilder;
    private int _lastTrajectoryId = -1;

    /// <summary>
    /// Gets constraints for the given trajectory, refreshing from the pose graph when the cache is stale
    /// or when map builder / trajectory id change.
    /// </summary>
    /// <param name="mapBuilder">Current map builder (may be null)</param>
    /// <param name="trajectoryId">Trajectory id to filter constraints</param>
    /// <param name="logger">Optional logger for warnings</param>
    /// <returns>Cached or freshly fetched constraints, or null if mapBuilder is null or fetch failed</returns>
    public List<IPoseGraph.Constraint>? GetOrUpdate(
        IMapBuilder? mapBuilder,
        int trajectoryId,
        ILogger? logger = null)
    {
        var now = DateTime.UtcNow;
        bool needsUpdate = false;

        lock (_lock)
        {
            if (mapBuilder != _lastMapBuilder || trajectoryId != _lastTrajectoryId)
            {
                needsUpdate = true;
            }
            else if (_cachedConstraints == null || (now - _lastUpdate) > _updateInterval)
            {
                needsUpdate = true;
            }
        }

        if (!needsUpdate)
        {
            lock (_lock)
            {
                return _cachedConstraints;
            }
        }

        if (mapBuilder == null)
        {
            return null;
        }

        try
        {
            var constraints = mapBuilder.PoseGraph.Constraints().Where(c => c.NodeId.TrajectoryId == trajectoryId).ToList();

            lock (_lock)
            {
                _cachedConstraints = constraints;
                _lastUpdate = now;
                _lastMapBuilder = mapBuilder;
                _lastTrajectoryId = trajectoryId;
            }

            return constraints;
        }
        catch (Exception ex)
        {
            logger?.LogWarning(ex, "TrajectoryConstraintCache: Failed to update constraint cache");
            lock (_lock)
            {
                return _cachedConstraints;
            }
        }
    }

    /// <summary>
    /// Clears the cache (e.g. when switching map or trajectory).
    /// </summary>
    public void Invalidate()
    {
        lock (_lock)
        {
            _cachedConstraints = null;
            _lastUpdate = DateTime.MinValue;
            _lastMapBuilder = null;
            _lastTrajectoryId = -1;
        }
    }
}