/*
 * 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.Sensor;
using System.Globalization;

namespace CartographerSharp.Mapping.Internal;

/// <summary>
/// Synchronizes TimedPointCloudData from different sensors. Input needs only be
/// monotonous in 'TimedPointCloudData::time', output is monotonous in per-point
/// timing. Up to one message per sensor is buffered, so a delay of the period of
/// the slowest sensor may be introduced, which can be alleviated by passing
/// subdivisions.
/// </summary>
public class RangeDataCollator(IEnumerable<string> expectedRangeSensorIds)
{
    private const double kDefaultIntensityValue = 0.0;

    private readonly HashSet<string> _expectedSensorIds = [.. expectedRangeSensorIds];
    private readonly Dictionary<string, TimedPointCloudData> _idToPendingData = [];
    private long _currentStart = long.MinValue; // Universal Time Scale ticks
    private long _currentEnd = long.MinValue; // Universal Time Scale ticks

    // Debug: Track per-sensor timestamps to detect out-of-order data
    private readonly Dictionary<string, long> _lastSensorTimestamp = [];
    private static readonly object _collatorLogLock = new();
    private static readonly string _collatorLogPath = "collator.log";
    private long _lastOutputTime = long.MinValue;

    private static void LogCollator(string message)
    {
        lock (_collatorLogLock)
        {
            try
            {
                var timestamp = DateTime.Now.ToString("yyyy-MM-dd HH:mm:ss.fff", CultureInfo.InvariantCulture);
                var line = $"{timestamp}|{message}";
                File.AppendAllText(_collatorLogPath, line + Environment.NewLine);
            }
            catch { /* Ignore logging errors */ }
        }
    }

    /// <summary>
    /// If timed_point_cloud_data has incomplete intensity data, we will fill the
    /// missing intensities with kDefaultIntensityValue.
    /// </summary>
    public TimedPointCloudOriginData AddRangeData(string sensorId, TimedPointCloudData timedPointCloudData)
    {
        if (!_expectedSensorIds.Contains(sensorId))
        {
            throw new ArgumentException($"Unexpected sensor ID: {sensorId}", nameof(sensorId));
        }

        // DEBUG: Timestamp validation - check if input is monotonic per sensor
        var currentTime = timedPointCloudData.Time;
        var tickMs = currentTime / TimeSpan.TicksPerMillisecond;
        if (_lastSensorTimestamp.TryGetValue(sensorId, out var lastTime))
        {
            if (currentTime < lastTime)
            {
                var diffMs = (lastTime - currentTime) / (double)TimeSpan.TicksPerMillisecond;
                LogCollator($"WARNING|sensor={sensorId}|TIME_REVERSAL|prev_tick={lastTime / TimeSpan.TicksPerMillisecond}|curr_tick={tickMs}|diff={diffMs:F3}ms");
            }
            else
            {
                var deltaMs = (currentTime - lastTime) / (double)TimeSpan.TicksPerMillisecond;
                // Log normal data flow (can comment out for less verbose logging)
                // LogCollator($"INFO|sensor={sensorId}|tick={tickMs}|delta={deltaMs:F3}ms|points={timedPointCloudData.Ranges.Count}");
            }
        }
        _lastSensorTimestamp[sensorId] = currentTime;

        // Fill missing intensities
        // Match C++: timed_point_cloud_data.intensities.resize(
        //     timed_point_cloud_data.ranges.size(), kDefaultIntensityValue);
        // This resizes to exactly ranges.size(), filling with kDefaultIntensityValue if needed,
        // or truncating if intensities is larger than ranges
        if (timedPointCloudData.Intensities.Count != timedPointCloudData.Ranges.Count)
        {
            var intensities = new List<double>(timedPointCloudData.Intensities);
            // Resize to match ranges.Count exactly
            if (intensities.Count < timedPointCloudData.Ranges.Count)
            {
                // Fill missing with default value
                while (intensities.Count < timedPointCloudData.Ranges.Count)
                {
                    intensities.Add(kDefaultIntensityValue);
                }
            }
            else if (intensities.Count > timedPointCloudData.Ranges.Count)
            {
                // Truncate if larger
                intensities.RemoveRange(timedPointCloudData.Ranges.Count, intensities.Count - timedPointCloudData.Ranges.Count);
            }
            timedPointCloudData.Intensities = intensities;
        }

        if (_idToPendingData.TryGetValue(sensorId, out TimedPointCloudData value))
        {
            _currentStart = _currentEnd;
            _currentEnd = value.Time;
            var result = CropAndMerge();
            _idToPendingData[sensorId] = timedPointCloudData;
            return result;
        }

        _idToPendingData[sensorId] = timedPointCloudData;
        
        if (_expectedSensorIds.Count != _idToPendingData.Count)
        {
            return new TimedPointCloudOriginData(0, [], []);
        }

        _currentStart = _currentEnd;
        // We have messages from all sensors, move forward to oldest.
        var oldestTimestamp = _idToPendingData.Values.Min(d => d.Time);
        _currentEnd = oldestTimestamp;
        return CropAndMerge();
    }

    private TimedPointCloudOriginData CropAndMerge()
    {
        var result = new TimedPointCloudOriginData(_currentEnd, [], []);

        // DEBUG: Check if output time is monotonic
        var outputTickMs = _currentEnd / TimeSpan.TicksPerMillisecond;
        if (_lastOutputTime != long.MinValue && _currentEnd < _lastOutputTime)
        {
            var diffMs = (_lastOutputTime - _currentEnd) / (double)TimeSpan.TicksPerMillisecond;
            LogCollator($"WARNING|OUTPUT_TIME_REVERSAL|prev_output={_lastOutputTime / TimeSpan.TicksPerMillisecond}|curr_output={outputTickMs}|diff={diffMs:F3}ms|start={_currentStart / TimeSpan.TicksPerMillisecond}");
        }
        _lastOutputTime = _currentEnd;

        var warnedForDroppedPoints = false;

        // Use ToList() to create a snapshot for iteration, but we'll modify _idToPendingData during iteration
        var sensorIds = _idToPendingData.Keys.ToList();
        
        foreach (var sensorId in sensorIds)
        {
            if (!_idToPendingData.TryGetValue(sensorId, out var data))
            {
                continue; // Already removed
            }
            
            var ranges = data.Ranges;
            var intensities = data.Intensities;

            // Find overlap range (matching C++ line 69-80)
            var overlapBegin = 0;
            while (overlapBegin < ranges.Count)
            {
                // Convert seconds to ticks: use double for precision, then round to long
                // This matches C++: data.time + common::FromSeconds((*overlap_begin).time)
                var pointTime = data.Time + (long)Math.Round(ranges[overlapBegin].Time * TimeSpan.TicksPerSecond);
                if (pointTime >= _currentStart)
                {
                    break;
                }
                overlapBegin++;
            }

            var overlapEnd = overlapBegin;
            while (overlapEnd < ranges.Count)
            {
                // Convert seconds to ticks: use double for precision, then round to long
                // This matches C++: data.time + common::FromSeconds((*overlap_end).time)
                var pointTime = data.Time + (long)Math.Round(ranges[overlapEnd].Time * TimeSpan.TicksPerSecond);
                if (pointTime > _currentEnd)
                {
                    break;
                }
                overlapEnd++;
            }

            if (overlapBegin > 0 && !warnedForDroppedPoints)
            {
                // Log warning about dropped points (matching C++ line 81-84)
                warnedForDroppedPoints = true;
            }

            // Copy overlapping range (matching C++ line 88-106)
            if (overlapBegin < overlapEnd)
            {
                var originIndex = result.Origins.Count;
                result.Origins.Add(data.Origin);
                
                // CRITICAL FIX: Apply time correction to point_time.time (match C++ line 91-103)
                // C++: const double time_correction = static_cast<double>(common::ToSeconds(data.time - current_end_));
                // C++: point.point_time.time += time_correction;
                // Time correction converts the difference between data.Time and currentEnd from ticks to seconds
                var timeCorrection = ((data.Time - _currentEnd) / 10_000_000.0); // Convert ticks to seconds (10 million ticks per second)
                
                for (int i = overlapBegin; i < overlapEnd; i++)
                {
                    // Apply time correction to point time
                    // Create new TimedRangefinderPoint with corrected time
                    var correctedPointTime = ranges[i].Time + timeCorrection;
                    var correctedPoint = new TimedRangefinderPoint(
                        ranges[i].Position,
                        correctedPointTime);
                    
                    var rangeMeasurement = new TimedPointCloudOriginData.RangeMeasurement(
                        correctedPoint,
                        intensities[i],
                        originIndex);
                    result.Ranges.Add(rangeMeasurement);
                }
            }

            // CRITICAL FIX: Drop buffered points until overlap_end (matching C++ line 108-121)
            // This prevents reprocessing of already-processed points
            if (overlapEnd == ranges.Count)
            {
                // All points processed, remove entry
                _idToPendingData.Remove(sensorId);
            }
            else if (overlapEnd == 0)
            {
                // No points processed, keep entry as is
                // Continue to next sensor
            }
            else
            {
                // Some points processed, keep only unprocessed points
                var remainingRanges = new TimedPointCloud();
                var remainingIntensities = new List<double>();
                
                for (int i = overlapEnd; i < ranges.Count; i++)
                {
                    remainingRanges.Add(ranges[i]);
                    remainingIntensities.Add(intensities[i]);
                }
                
                _idToPendingData[sensorId] = new TimedPointCloudData(
                    data.Time,
                    data.Origin,
                    remainingRanges,
                    remainingIntensities
                );
            }
        }

        // CRITICAL FIX: Sort ranges by time (match C++ line 124-128)
        // C++: std::sort(result.ranges.begin(), result.ranges.end(),
        //                [](const auto& a, const auto& b) { return a.point_time.time < b.point_time.time; });
        // This ensures output is monotonous in per-point timing as documented
        if (result.Ranges.Count > 0)
        {
            result.Ranges = [.. result.Ranges.OrderBy(r => r.PointTime.Time)];
        }

        return result;
    }
}