/*
 * Copyright 2018 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.Transform;
using  RobotNet10.Shared.Numbers;

namespace CartographerSharp.Sensor;

/// <summary>
/// Stores 3D position of a point observed by a rangefinder sensor.
/// </summary>
public struct RangefinderPoint(Vector3 position)
{
    public Vector3 Position { get; set; } = position;

    public static RangefinderPoint operator *(Rigid3f transform, RangefinderPoint point)
    {
        return new RangefinderPoint(transform.TransformPoint(point.Position));
    }

    public static bool operator ==(RangefinderPoint lhs, RangefinderPoint rhs)
    {
        return lhs.Position == rhs.Position;
    }

    public static bool operator !=(RangefinderPoint lhs, RangefinderPoint rhs)
    {
        return !(lhs == rhs);
    }

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

    public override readonly int GetHashCode()
    {
        return Position.GetHashCode();
    }

    /// <summary>
    /// Converts from proto representation.
    /// </summary>
    public static RangefinderPoint FromProto(RangefinderPoint proto)
    {
        return new RangefinderPoint(proto.Position);
    }

    /// <summary>
    /// Converts to proto representation.
    /// </summary>
    public readonly RangefinderPoint ToProto()
    {
        return new RangefinderPoint(Position);
    }
}

/// <summary>
/// Stores 3D position of a point with its relative measurement time.
/// See PointCloud for more details.
/// </summary>
public struct TimedRangefinderPoint(Vector3 position, double time)
{
    public Vector3 Position { get; set; } = position;
    public double Time { get; set; } = time;

    public static TimedRangefinderPoint operator *(Rigid3f transform, TimedRangefinderPoint point)
    {
        return new TimedRangefinderPoint(transform.TransformPoint(point.Position), point.Time);
    }

    public static bool operator ==(TimedRangefinderPoint lhs, TimedRangefinderPoint rhs)
    {
        return lhs.Position == rhs.Position && lhs.Time == rhs.Time;
    }

    public static bool operator !=(TimedRangefinderPoint lhs, TimedRangefinderPoint rhs)
    {
        return !(lhs == rhs);
    }

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

    public override readonly int GetHashCode()
    {
        return HashCode.Combine(Position, Time);
    }

    /// <summary>
    /// Converts from proto representation.
    /// </summary>
    public static TimedRangefinderPoint FromProto(TimedRangefinderPoint proto)
    {
        return new TimedRangefinderPoint(proto.Position, proto.Time);
    }

    /// <summary>
    /// Converts to proto representation.
    /// </summary>
    public readonly TimedRangefinderPoint ToProto()
    {
        return new TimedRangefinderPoint(Position, Time);
    }

    /// <summary>
    /// Converts to RangefinderPoint (drops time information).
    /// </summary>
    public readonly RangefinderPoint ToRangefinderPoint()
    {
        return new RangefinderPoint(Position);
    }

    /// <summary>
    /// Creates TimedRangefinderPoint from RangefinderPoint with time.
    /// </summary>
    public static TimedRangefinderPoint FromRangefinderPoint(RangefinderPoint point, double time)
    {
        return new TimedRangefinderPoint(point.Position, time);
    }
}