/*
 * 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.
 */

namespace CartographerSharp.Mapping;

/// <summary>
/// Uniquely identifies a trajectory node using a combination of a unique
/// trajectory ID and a zero-based index of the node inside that trajectory.
/// </summary>
public struct NodeId(int trajectoryId, int nodeIndex) : IEquatable<NodeId>, IComparable<NodeId>, IIdType
{
    public int TrajectoryId { get; set; } = trajectoryId;
    public int NodeIndex { get; set; } = nodeIndex;

    public readonly int GetTrajectoryId() => TrajectoryId;
    public readonly int GetIndex() => NodeIndex;

    public readonly bool Equals(NodeId other)
    {
        return TrajectoryId == other.TrajectoryId && NodeIndex == other.NodeIndex;
    }

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

    public readonly override int GetHashCode()
    {
        return HashCode.Combine(TrajectoryId, NodeIndex);
    }

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

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

    public readonly int CompareTo(NodeId other)
    {
        var trajectoryComparison = TrajectoryId.CompareTo(other.TrajectoryId);
        if (trajectoryComparison != 0)
        {
            return trajectoryComparison;
        }
        return NodeIndex.CompareTo(other.NodeIndex);
    }

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

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

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

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

    /// <summary>
    /// Converts to proto representation.
    /// </summary>
    public readonly NodeId ToProto()
    {
        return new NodeId(TrajectoryId, NodeIndex);
    }

    public readonly override string ToString()
    {
        return $"({TrajectoryId}, {NodeIndex})";
    }
}

/// <summary>
/// Uniquely identifies a submap using a combination of a unique trajectory ID
/// and a zero-based index of the submap inside that trajectory.
/// </summary>
public struct SubmapId(int trajectoryId, int submapIndex) : IEquatable<SubmapId>, IComparable<SubmapId>, IIdType
{
    public int TrajectoryId { get; set; } = trajectoryId;
    public int SubmapIndex { get; set; } = submapIndex;

    public readonly int GetTrajectoryId() => TrajectoryId;
    public readonly int GetIndex() => SubmapIndex;

    public readonly bool Equals(SubmapId other)
    {
        return TrajectoryId == other.TrajectoryId && SubmapIndex == other.SubmapIndex;
    }

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

    public readonly override int GetHashCode()
    {
        return HashCode.Combine(TrajectoryId, SubmapIndex);
    }

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

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

    public readonly int CompareTo(SubmapId other)
    {
        var trajectoryComparison = TrajectoryId.CompareTo(other.TrajectoryId);
        if (trajectoryComparison != 0)
        {
            return trajectoryComparison;
        }
        return SubmapIndex.CompareTo(other.SubmapIndex);
    }

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

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

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

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

    /// <summary>
    /// Converts to proto representation.
    /// </summary>
    public readonly SubmapId ToProto()
    {
        return new SubmapId(TrajectoryId, SubmapIndex);
    }

    public readonly override string ToString()
    {
        return $"({TrajectoryId}, {SubmapIndex})";
    }
}