/*
 * 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 CartographerSharp.Sensor;
using CartographerSharp.Transform;

namespace CartographerSharp.Mapping;

/// <summary>
/// This interface is used for both 2D and 3D SLAM. Implementations wire up a
/// global SLAM stack, i.e. local SLAM for initial pose estimates, scan matching
/// to detect loop closure, and a sparse pose graph optimization to compute
/// optimized pose estimates.
/// </summary>
public interface ITrajectoryBuilder
{
    /// <summary>
    /// Result of inserting data into submaps.
    /// </summary>
    public struct InsertionResult(NodeId nodeId, TrajectoryNode.Data? constantData, List<Submap> insertionSubmaps)
    {
        public NodeId NodeId { get; set; } = nodeId;
        public TrajectoryNode.Data? ConstantData { get; set; } = constantData;
        public List<Submap> InsertionSubmaps { get; set; } = insertionSubmaps ?? [];
    }

    /// <summary>
    /// Result of matching sensor data (scan matching, localization, etc.)
    /// Returned when range data accumulation is completed.
    /// Match C++: MatchingResult{time, pose_estimate, range_data_in_local, insertion_result, pose_confidence, ceres_score}
    /// </summary>
    public struct MatchingResult(
        int trajectoryId,
        long time,
        Rigid3d localPose,
        RangeData rangeDataInLocal,
        InsertionResult? insertionResult,
        double poseConfidence = -1.0,
        double ceresScore = -1.0,
        PointCloud? samplePointCloudGlobal = null)
    {
        public int TrajectoryId { get; set; } = trajectoryId;
        public long Time { get; set; } = time;
        public Rigid3d LocalPose { get; set; } = localPose;
        public RangeData RangeDataInLocal { get; set; } = rangeDataInLocal;
        public InsertionResult? InsertionResult { get; set; } = insertionResult;
        /// <summary>
        /// Pose confidence score from real-time correlative scan matcher.
        /// Match C++: pose_confidence field in MatchingResult.
        /// Value is -1.0 if confidence score is not provided.
        /// </summary>
        public double PoseConfidence { get; set; } = poseConfidence;
        /// <summary>
        /// Ceres solver final cost from scan matching.
        /// Match C++: ceres_score tracked in ScanMatch function.
        /// Value is -1.0 if ceres score is not available.
        /// </summary>
        public double CeresScore { get; set; } = ceresScore;
        /// <summary>
        /// Sample point cloud for visualization/debugging.
        /// IMPORTANT: When returned from LocalTrajectoryBuilder2D, this is in LOCAL trajectory frame.
        /// GlobalTrajectoryBuilder2D transforms it to GLOBAL map frame before returning to the caller.
        /// After GlobalTrajectoryBuilder2D processing, this IS in global frame as the name suggests.
        /// </summary>
        public PointCloud? SamplePointCloudGlobal { get; set; } = samplePointCloudGlobal;
    }

    /// <summary>
    /// Sensor identifier.
    /// </summary>
    public struct SensorId(SensorId.SensorType type, string id) : IEquatable<SensorId>, IComparable<SensorId>
    {
        public enum SensorType
        {
            Range = 0,
            Imu,
            Odometry,
            FixedFramePose,
            Landmark,
            LocalSlamResult
        }

        public SensorType Type { get; set; } = type;
        public string Id { get; set; } = id;

        public readonly bool Equals(SensorId other)
        {
            return Type == other.Type && Id == other.Id;
        }

        public readonly override bool Equals(object? obj)
        {
            return obj is SensorId other && Equals(other);
        }

        public readonly override int GetHashCode()
        {
            return HashCode.Combine(Type, Id);
        }

        public readonly int CompareTo(SensorId other)
        {
            var typeComparison = Type.CompareTo(other.Type);
            if (typeComparison != 0) return typeComparison;
            return string.Compare(Id, other.Id, StringComparison.Ordinal);
        }

        public static bool operator ==(SensorId left, SensorId right)
        {
            return left.Equals(right);
        }

        public static bool operator !=(SensorId left, SensorId right)
        {
            return !left.Equals(right);
        }

        public static bool operator <(SensorId left, SensorId right)
        {
            return left.CompareTo(right) < 0;
        }

        public static bool operator <=(SensorId left, SensorId right)
        {
            return left.CompareTo(right) <= 0;
        }

        public static bool operator >(SensorId left, SensorId right)
        {
            return left.CompareTo(right) > 0;
        }

        public static bool operator >=(SensorId left, SensorId right)
        {
            return left.CompareTo(right) >= 0;
        }
    }

    /// <summary>
    /// Adds timed point cloud data from a sensor.
    /// Returns MatchingResult when range data accumulation is completed, otherwise null.
    /// </summary>
    MatchingResult? AddSensorData(string sensorId, TimedPointCloudData timedPointCloudData);

    /// <summary>
    /// Adds IMU data from a sensor.
    /// </summary>
    void AddSensorData(string sensorId, ImuData imuData);

    /// <summary>
    /// Adds odometry data from a sensor.
    /// </summary>
    void AddSensorData(string sensorId, OdometryData odometryData);

    /// <summary>
    /// Adds fixed frame pose data from a sensor.
    /// </summary>
    void AddSensorData(string sensorId, FixedFramePoseData fixedFramePoseData);

    /// <summary>
    /// Adds landmark data from a sensor.
    /// </summary>
    void AddSensorData(string sensorId, LandmarkData landmarkData);

    /// <summary>
    /// Allows to directly add local SLAM results to the 'PoseGraph'. Note that it
    /// is invalid to add local SLAM results for a trajectory that has a
    /// 'LocalTrajectoryBuilder2D/3D'.
    /// </summary>
    void AddLocalSlamResultData(LocalSlamResultData localSlamResultData);

    /// <summary>
    /// Tries to get the current pose from the internal extrapolator at the given time.
    /// Returns null when there is no local trajectory builder / extrapolator, or when
    /// the requested time is before the last pose time. Allows callers (e.g. CartographerService)
    /// to always read a live extrapolated pose instead of only the last scan-matched pose.
    /// </summary>
    /// <param name="time">Time in ticks (e.g. DateTime.UtcNow.Ticks) to extrapolate to.</param>
    /// <returns>Extrapolated pose in trajectory local frame, or null if not available.</returns>
    Rigid3d? TryGetExtrapolatedPose(long time);

    /// <summary>
    /// Tries to get the current pose with low-pass filter to reduce jitter during direction changes.
    /// Match C++: ExtrapolatePose_filter - should be used for publishing pose to external systems.
    /// </summary>
    /// <param name="time">Time in ticks (e.g. DateTime.UtcNow.Ticks) to extrapolate to.</param>
    /// <returns>Filtered extrapolated pose in trajectory local frame, or null if not available.</returns>
    Rigid3d? TryGetExtrapolatedPoseFilter(long time);
}

/// <summary>
/// Local SLAM result data.
/// </summary>
public class LocalSlamResultData(
    int trajectoryId,
    long time,
    Rigid3d localPose,
    RangeData rangeData,
    ITrajectoryBuilder.InsertionResult? insertionResult = null)
{
    public int TrajectoryId { get; set; } = trajectoryId;
    public long Time { get; set; } = time;
    public Rigid3d LocalPose { get; set; } = localPose;
    public RangeData RangeData { get; set; } = rangeData;
    public ITrajectoryBuilder.InsertionResult? InsertionResult { get; set; } = insertionResult;

    /// <summary>
    /// Adds this local SLAM result to the pose graph.
    /// This is used when running in pure localization mode or when replaying data.
    /// </summary>
    /// <param name="trajectoryId">The trajectory ID to add the result to</param>
    /// <param name="poseGraph">The pose graph to add the result to</param>
    public void AddToPoseGraph(int trajectoryId, IPoseGraph poseGraph)
    {
        if (InsertionResult == null)
        {
            // No insertion result, nothing to add to pose graph
            return;
        }

        var insertionResult = InsertionResult.Value;
        
        // Add node to pose graph
        // Note: The actual AddNode implementation depends on whether we're using 2D or 3D
        // For now, we'll need to cast to the specific pose graph type
        if (poseGraph is Mapping.Internal.D2D.PoseGraph2D poseGraph2D)
        {
            // For 2D, we need to convert Submap to Submap2D
            var submaps2D = insertionResult.InsertionSubmaps
                .OfType<Mapping.D2D.Submap2D>()
                .ToList();
            
            if (submaps2D.Count != insertionResult.InsertionSubmaps.Count)
            {
                throw new InvalidOperationException("Cannot add 3D submaps to 2D pose graph");
            }

            if(insertionResult.ConstantData is null)
                throw new NullReferenceException(nameof(insertionResult.ConstantData));

            // Add node to pose graph
            poseGraph2D.AddNode(insertionResult.ConstantData, trajectoryId, submaps2D);
        }
        else if (poseGraph is Mapping.Internal.D3D.PoseGraph3D poseGraph3D)
        {
            // For 3D, we need to convert Submap to Submap3D
            var submaps3D = insertionResult.InsertionSubmaps
                .OfType<Mapping.D3D.Submap3D>()
                .ToList();
            
            if (submaps3D.Count != insertionResult.InsertionSubmaps.Count)
            {
                throw new InvalidOperationException(
                    "Cannot add 2D submaps to 3D pose graph");
            }

            if (insertionResult.ConstantData is null)
                throw new NullReferenceException(nameof(insertionResult.ConstantData));

            poseGraph3D.AddNode(insertionResult.ConstantData, trajectoryId, submaps3D);
        }
        else
        {
            throw new InvalidOperationException(
                $"Unknown pose graph type: {poseGraph.GetType().Name}");
        }
    }
}

/// <summary>
/// Conversion utilities for SensorId.
/// </summary>
public static class SensorIdOperations
{
    /// <summary>
    /// Converts to proto representation.
    /// </summary>
    public static Models.Mapping.SensorId ToProto(ITrajectoryBuilder.SensorId sensorId)
    {
        var type = sensorId.Type switch
        {
            ITrajectoryBuilder.SensorId.SensorType.Range => Models.Mapping.SensorId.SensorType.Range,
            ITrajectoryBuilder.SensorId.SensorType.Imu => Models.Mapping.SensorId.SensorType.Imu,
            ITrajectoryBuilder.SensorId.SensorType.Odometry => Models.Mapping.SensorId.SensorType.Odometry,
            ITrajectoryBuilder.SensorId.SensorType.FixedFramePose => Models.Mapping.SensorId.SensorType.FixedFramePose,
            ITrajectoryBuilder.SensorId.SensorType.Landmark => Models.Mapping.SensorId.SensorType.Landmark,
            ITrajectoryBuilder.SensorId.SensorType.LocalSlamResult => Models.Mapping.SensorId.SensorType.LocalSlamResult,
            _ => throw new ArgumentException($"Unknown sensor type: {sensorId.Type}")
        };

        return new Models.Mapping.SensorId(type, sensorId.Id);
    }

    /// <summary>
    /// Creates from proto representation.
    /// </summary>
    public static ITrajectoryBuilder.SensorId FromProto(Models.Mapping.SensorId sensorIdProto)
    {
        var type = sensorIdProto.Type switch
        {
            Models.Mapping.SensorId.SensorType.Range => ITrajectoryBuilder.SensorId.SensorType.Range,
            Models.Mapping.SensorId.SensorType.Imu => ITrajectoryBuilder.SensorId.SensorType.Imu,
            Models.Mapping.SensorId.SensorType.Odometry => ITrajectoryBuilder.SensorId.SensorType.Odometry,
            Models.Mapping.SensorId.SensorType.FixedFramePose => ITrajectoryBuilder.SensorId.SensorType.FixedFramePose,
            Models.Mapping.SensorId.SensorType.Landmark => ITrajectoryBuilder.SensorId.SensorType.Landmark,
            Models.Mapping.SensorId.SensorType.LocalSlamResult => ITrajectoryBuilder.SensorId.SensorType.LocalSlamResult,
            _ => throw new ArgumentException($"Unknown sensor type: {sensorIdProto.Type}")
        };

        return new ITrajectoryBuilder.SensorId(type, sensorIdProto.Id);
    }
}