/*
 * 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 pose graph.
/// </summary>
public struct PoseGraph
{
    /// <summary>
    /// Submap ID.
    /// </summary>
    public struct SubmapId
    {
        [JsonPropertyName("trajectory_id")]
        public int TrajectoryId { get; set; }

        /// <summary>
        /// Submap index in the given trajectory.
        /// </summary>
        [JsonPropertyName("submap_index")]
        public int SubmapIndex { get; set; }

        public SubmapId(int trajectoryId, int submapIndex)
        {
            TrajectoryId = trajectoryId;
            SubmapIndex = submapIndex;
        }
    }

    /// <summary>
    /// Node ID.
    /// </summary>
    public struct NodeId
    {
        [JsonPropertyName("trajectory_id")]
        public int TrajectoryId { get; set; }

        /// <summary>
        /// Node index in the given trajectory.
        /// </summary>
        [JsonPropertyName("node_index")]
        public int NodeIndex { get; set; }

        public NodeId(int trajectoryId, int nodeIndex)
        {
            TrajectoryId = trajectoryId;
            NodeIndex = nodeIndex;
        }
    }

    /// <summary>
    /// Constraint in the pose graph.
    /// </summary>
    public struct Constraint
    {
        /// <summary>
        /// Differentiates between intra-submap (where the range data was inserted
        /// into the submap) and inter-submap constraints (where the range data was
        /// not inserted into the submap).
        /// </summary>
        public enum Tag
        {
            IntraSubmap = 0,
            InterSubmap = 1
        }

        /// <summary>
        /// Differentiates between enable state (condition to add to optimization)
        /// and disable state (do not use to optimize).
        /// </summary>
        public enum State
        {
            Enabled = 0,
            Disabled = 1
        }

        /// <summary>
        /// Submap ID.
        /// </summary>
        [JsonPropertyName("submap_id")]
        public SubmapId SubmapId { get; set; }

        /// <summary>
        /// Node ID.
        /// </summary>
        [JsonPropertyName("node_id")]
        public NodeId NodeId { get; set; }

        /// <summary>
        /// Pose of the node relative to submap, i.e. taking data from the node frame
        /// into the submap frame.
        /// </summary>
        [JsonPropertyName("relative_pose")]
        public Rigid3dProto RelativePose { get; set; }

        /// <summary>
        /// Weight of the translational part of the constraint.
        /// </summary>
        [JsonPropertyName("translation_weight")]
        public double TranslationWeight { get; set; }

        /// <summary>
        /// Weight of the rotational part of the constraint.
        /// </summary>
        [JsonPropertyName("rotation_weight")]
        public double RotationWeight { get; set; }

        /// <summary>
        /// Tag indicating constraint type.
        /// </summary>
        [JsonPropertyName("tag")]
        public Tag ConstraintTag { get; set; }

        /// <summary>
        /// Score of the constraint.
        /// </summary>
        [JsonPropertyName("score")]
        public double Score { get; set; }

        /// <summary>
        /// State of the constraint (enabled or disabled).
        /// </summary>
        [JsonPropertyName("state")]
        public State ConstraintState { get; set; }

        public Constraint(
            SubmapId submapId,
            NodeId nodeId,
            Rigid3dProto relativePose,
            double translationWeight,
            double rotationWeight,
            Tag constraintTag,
            double score = 0.0,
            State constraintState = State.Enabled)
        {
            SubmapId = submapId;
            NodeId = nodeId;
            RelativePose = relativePose;
            TranslationWeight = translationWeight;
            RotationWeight = rotationWeight;
            ConstraintTag = constraintTag;
            Score = score;
            ConstraintState = constraintState;
        }
    }

    /// <summary>
    /// Landmark pose in the pose graph.
    /// </summary>
    public struct LandmarkPose
    {
        [JsonPropertyName("landmark_id")]
        public string LandmarkId { get; set; }

        [JsonPropertyName("global_pose")]
        public Rigid3dProto GlobalPose { get; set; }

        public LandmarkPose(string landmarkId, Rigid3dProto globalPose)
        {
            LandmarkId = landmarkId;
            GlobalPose = globalPose;
        }
    }

    /// <summary>
    /// Accurate submap in the pose graph.
    /// </summary>
    public struct AccurateSubmap
    {
        [JsonPropertyName("submap_id")]
        public SubmapId SubmapId { get; set; }

        [JsonPropertyName("parent_id")]
        public SubmapId ParentId { get; set; }

        [JsonPropertyName("pose")]
        public Rigid2dProto Pose { get; set; }

        [JsonPropertyName("parent_relative_pose")]
        public Rigid2dProto ParentRelativePose { get; set; }

        /// <summary>
        /// Stop station in the accurate submap.
        /// </summary>
        public struct StopStation
        {
            public enum StopStationType
            {
                StartPoint = 0,
                StopPoint = 1
            }

            [JsonPropertyName("pose")]
            public Rigid2dProto Pose { get; set; }

            [JsonPropertyName("type")]
            public StopStationType Type { get; set; }

            public StopStation(Rigid2dProto pose, StopStationType type)
            {
                Pose = pose;
                Type = type;
            }
        }

        [JsonPropertyName("stop_stations")]
        public List<StopStation> StopStations { get; set; }

        [JsonPropertyName("is_shelf")]
        public bool IsShelf { get; set; }

        public AccurateSubmap(
            SubmapId submapId,
            SubmapId parentId,
            Rigid2dProto pose,
            Rigid2dProto parentRelativePose,
            List<StopStation>? stopStations = null,
            bool isShelf = false)
        {
            SubmapId = submapId;
            ParentId = parentId;
            Pose = pose;
            ParentRelativePose = parentRelativePose;
            StopStations = stopStations ?? new List<StopStation>();
            IsShelf = isShelf;
        }
    }

    [JsonPropertyName("constraint")]
    public List<Constraint> Constraints { get; set; }

    [JsonPropertyName("trajectory")]
    public List<Trajectory> Trajectories { get; set; }

    [JsonPropertyName("landmark_poses")]
    public List<LandmarkPose> LandmarkPoses { get; set; }

    [JsonPropertyName("accurate_submaps")]
    public List<AccurateSubmap> AccurateSubmaps { get; set; }

    [JsonPropertyName("transform_to_map")]
    [JsonIgnore(Condition = JsonIgnoreCondition.WhenWritingNull)]
    public Rigid3dProto? TransformToMap { get; set; }

    public PoseGraph(
        List<Constraint>? constraints = null,
        List<Trajectory>? trajectories = null,
        List<LandmarkPose>? landmarkPoses = null,
        List<AccurateSubmap>? accurateSubmaps = null,
        Rigid3dProto? transformToMap = null)
    {
        Constraints = constraints ?? new List<Constraint>();
        Trajectories = trajectories ?? new List<Trajectory>();
        LandmarkPoses = landmarkPoses ?? new List<LandmarkPose>();
        AccurateSubmaps = accurateSubmaps ?? new List<AccurateSubmap>();
        TransformToMap = transformToMap;
    }
}