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

namespace CartographerSharp.Mapping;

/// <summary>
/// Constant pose data (time and local pose).
/// </summary>
public struct ConstantPoseData
{
    public long Time { get; set; } // Universal Time Scale ticks
    public Rigid3d LocalPose { get; set; }
}

/// <summary>
/// Trajectory node pose information.
/// </summary>
public struct TrajectoryNodePose
{
    /// <summary>
    /// The node pose in the global SLAM frame.
    /// </summary>
    public Rigid3d GlobalPose { get; set; }

    /// <summary>
    /// Optional constant pose data.
    /// </summary>
    public ConstantPoseData? ConstantPoseData { get; set; }
}

/// <summary>
/// Trajectory node containing sensor data and pose information.
/// </summary>
public class TrajectoryNode
{
    /// <summary>
    /// Constant data associated with a trajectory node.
    /// </summary>
    public class Data
    {
        public long Time { get; set; } // Universal Time Scale ticks

        /// <summary>
        /// Transform to approximately gravity align the tracking frame as
        /// determined by local SLAM.
        /// </summary>
        public Quaternion GravityAlignment { get; set; }

        /// <summary>
        /// Used for loop closure in 2D: voxel filtered returns in the
        /// 'gravity_alignment' frame.
        /// </summary>
        public PointCloud? FilteredGravityAlignedPointCloud { get; set; }

        /// <summary>
        /// Used for loop closure in 3D.
        /// </summary>
        public PointCloud? HighResolutionPointCloud { get; set; }
        public PointCloud? LowResolutionPointCloud { get; set; }
        public double[]? RotationalScanMatcherHistogram { get; set; }

        /// <summary>
        /// The node pose in the local SLAM frame.
        /// </summary>
        public Rigid3d LocalPose { get; set; }
    }

    /// <summary>
    /// Gets the time of this node.
    /// </summary>
    public long Time => ConstantData?.Time ?? 0;

    /// <summary>
    /// This must be a shared reference. If the data is used for visualization while the
    /// node is being trimmed, it must survive until all use finishes.
    /// </summary>
    public Data? ConstantData { get; set; }

    /// <summary>
    /// The node pose in the global SLAM frame.
    /// </summary>
    public Rigid3d GlobalPose { get; set; }

    /// <summary>
    /// Creates a shallow clone of this TrajectoryNode.
    /// The ConstantData reference is shared (not deep cloned) since it's immutable after creation.
    /// Only GlobalPose is independent in the clone.
    /// Used for Copy-on-Write optimization to avoid data races.
    /// </summary>
    public TrajectoryNode ShallowClone()
    {
        return new TrajectoryNode
        {
            ConstantData = this.ConstantData,  // Shared reference (immutable)
            GlobalPose = this.GlobalPose       // Value type copy
        };
    }
}

/// <summary>
/// Conversion utilities for TrajectoryNode.
/// </summary>
public static class TrajectoryNodeOperations
{
    /// <summary>
    /// Converts to proto representation.
    /// </summary>
    public static TrajectoryNodeData ToProto(TrajectoryNode.Data constantData)
    {
        var proto = new TrajectoryNodeData(
            constantData.Time,
            (Quaterniond)constantData.GravityAlignment,
            (Rigid3dProto)constantData.LocalPose,
            constantData.FilteredGravityAlignedPointCloud != null
                ? new Sensor.CompressedPointCloud(constantData.FilteredGravityAlignedPointCloud).ToProto()
                : null,
            constantData.HighResolutionPointCloud != null
                ? new Sensor.CompressedPointCloud(constantData.HighResolutionPointCloud).ToProto()
                : null,
            constantData.LowResolutionPointCloud != null
                ? new Sensor.CompressedPointCloud(constantData.LowResolutionPointCloud).ToProto()
                : null,
            constantData.RotationalScanMatcherHistogram?.ToList()
        );

        return proto;
    }

    /// <summary>
    /// Creates from proto representation.
    /// </summary>
    public static TrajectoryNode.Data FromProto(TrajectoryNodeData proto)
    {
        return new TrajectoryNode.Data
        {
            Time = proto.Timestamp,
            GravityAlignment = (Quaternion)proto.GravityAlignment,
            LocalPose = (Rigid3d)proto.LocalPose,
            FilteredGravityAlignedPointCloud = proto.FilteredGravityAlignedPointCloud.HasValue
                ? Sensor.CompressedPointCloud.FromProto(proto.FilteredGravityAlignedPointCloud.Value).Decompress()
                : null,
            HighResolutionPointCloud = proto.HighResolutionPointCloud.HasValue
                ? Sensor.CompressedPointCloud.FromProto(proto.HighResolutionPointCloud.Value).Decompress()
                : null,
            LowResolutionPointCloud = proto.LowResolutionPointCloud.HasValue
                ? Sensor.CompressedPointCloud.FromProto(proto.LowResolutionPointCloud.Value).Decompress()
                : null,
            RotationalScanMatcherHistogram = proto.RotationalScanMatcherHistogram?.ToArray()
        };
    }
}