/*
 * 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 System.Collections.Generic;
using System.Text.Json.Serialization;
using CartographerSharp.Models.Transform;

namespace CartographerSharp.Models.Mapping;

/// <summary>
/// Protocol buffer representation of a trajectory.
/// </summary>
public struct Trajectory
{
    /// <summary>
    /// Node within a trajectory.
    /// </summary>
    public struct Node
    {
        /// <summary>
        /// Index of this node within its trajectory.
        /// </summary>
        [JsonPropertyName("node_index")]
        public int NodeIndex { get; set; }

        /// <summary>
        /// Timestamp of this node.
        /// </summary>
        [JsonPropertyName("timestamp")]
        public long Timestamp { get; set; }

        /// <summary>
        /// Transform from tracking to global map frame.
        /// </summary>
        [JsonPropertyName("pose")]
        public Rigid3dProto Pose { get; set; }

        public Node(int nodeIndex, long timestamp, Rigid3dProto pose)
        {
            NodeIndex = nodeIndex;
            Timestamp = timestamp;
            Pose = pose;
        }
    }

    /// <summary>
    /// Submap within a trajectory.
    /// </summary>
    public struct Submap
    {
        /// <summary>
        /// Index of this submap within its trajectory.
        /// </summary>
        [JsonPropertyName("submap_index")]
        public int SubmapIndex { get; set; }

        /// <summary>
        /// Transform from submap to global map frame.
        /// </summary>
        [JsonPropertyName("pose")]
        public Rigid3dProto Pose { get; set; }

        public Submap(int submapIndex, Rigid3dProto pose)
        {
            SubmapIndex = submapIndex;
            Pose = pose;
        }
    }

    /// <summary>
    /// ID of this trajectory.
    /// </summary>
    [JsonPropertyName("trajectory_id")]
    public int TrajectoryId { get; set; }

    /// <summary>
    /// Time-ordered sequence of Nodes.
    /// </summary>
    [JsonPropertyName("node")]
    public List<Node> Nodes { get; set; }

    /// <summary>
    /// Submaps associated with the trajectory.
    /// </summary>
    [JsonPropertyName("submap")]
    public List<Submap> Submaps { get; set; }

    public Trajectory(int trajectoryId, List<Node>? nodes = null, List<Submap>? submaps = null)
    {
        TrajectoryId = trajectoryId;
        Nodes = nodes ?? new List<Node>();
        Submaps = submaps ?? new List<Submap>();
    }
}