BQP/srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Sensor/TimedPointCloudData.cs

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

namespace CartographerSharp.Sensor;

/// <summary>
/// Timed point cloud data structure.
/// </summary>
public struct TimedPointCloudData
{
    public long Time { get; set; }
    public Vector3 Origin { get; set; }
    public TimedPointCloud Ranges { get; set; }
    
    /// <summary>
    /// 'intensities' has to be same size as 'ranges', or empty.
    /// </summary>
    public List<double> Intensities { get; set; }

    public TimedPointCloudData(long time, Vector3 origin, TimedPointCloud ranges, List<double>? intensities = null)
    {
        Time = time;
        Origin = origin;
        Ranges = ranges;
        Intensities = intensities ?? [];
        
        if (Intensities.Count > 0 && Intensities.Count != Ranges.Count)
        {
            throw new ArgumentException("Intensities must have the same size as ranges, or be empty.");
        }
    }
}

/// <summary>
/// Timed point cloud origin data structure.
/// </summary>
public struct TimedPointCloudOriginData(long time, List<Vector3> origins, List<TimedPointCloudOriginData.RangeMeasurement> ranges)
{
    /// <summary>
    /// Range measurement with point time, intensity, and origin index.
    /// </summary>
    public struct RangeMeasurement(TimedRangefinderPoint pointTime, double intensity, int originIndex)
    {
        public TimedRangefinderPoint PointTime { get; set; } = pointTime;
        public double Intensity { get; set; } = intensity;
        public int OriginIndex { get; set; } = originIndex;
    }

    public long Time { get; set; } = time;
    public List<Vector3> Origins { get; set; } = origins;
    public List<RangeMeasurement> Ranges { get; set; } = ranges;
}

/// <summary>
/// Operations on TimedPointCloudData.
/// </summary>
public static class TimedPointCloudDataOperations
{
    /// <summary>
    /// Converts 'timed_point_cloud_data' to a proto::TimedPointCloudData.
    /// Note: Time is already in Universal Time Scale ticks (long), so no conversion needed.
    /// </summary>
    public static Models.Sensor.TimedPointCloudData ToProto(TimedPointCloudData timedPointCloudData)
    {
        var pointData = new List<Models.Sensor.TimedRangefinderPoint>(timedPointCloudData.Ranges.Count);
        foreach (var range in timedPointCloudData.Ranges)
        {
            pointData.Add(new Models.Sensor.TimedRangefinderPoint(
                new Vector3f(range.Position.X, range.Position.Y, range.Position.Z),
                range.Time
            ));
        }

        return new Models.Sensor.TimedPointCloudData(
            timedPointCloudData.Time, // Already in Universal Time Scale ticks
            new Vector3f(timedPointCloudData.Origin.X, timedPointCloudData.Origin.Y, timedPointCloudData.Origin.Z),
            pointDataLegacy: null,
            pointData: pointData,
            intensities: timedPointCloudData.Intensities
        );
    }

    /// <summary>
    /// Converts 'proto' to TimedPointCloudData.
    /// Note: Time is already in Universal Time Scale ticks (long), so no conversion needed.
    /// </summary>
    public static TimedPointCloudData FromProto(Models.Sensor.TimedPointCloudData proto)
    {
        var timedPointCloud = new TimedPointCloud();
        
        // Use point_data if available, otherwise fall back to point_data_legacy
        if (proto.PointData != null && proto.PointData.Count > 0)
        {
            timedPointCloud.Capacity = proto.PointData.Count;
            foreach (var protoPoint in proto.PointData)
            {
                timedPointCloud.Add(new TimedRangefinderPoint(
                    new Vector3(protoPoint.Position.X, protoPoint.Position.Y, protoPoint.Position.Z),
                    protoPoint.Time
                ));
            }
        }
        else if (proto.PointDataLegacy != null && proto.PointDataLegacy.Count > 0)
        {
            // Legacy format: Vector4f where T component is time
            timedPointCloud.Capacity = proto.PointDataLegacy.Count;
            foreach (var point4 in proto.PointDataLegacy)
            {
                timedPointCloud.Add(new TimedRangefinderPoint(
                    new Vector3(point4.X, point4.Y, point4.Z),
                    point4.T
                ));
            }
        }

        var intensities = new List<double>(proto.Intensities ?? []);
        
        if (intensities.Count > 0 && intensities.Count != timedPointCloud.Count)
        {
            throw new ArgumentException("Intensities size must match ranges size, or be empty.");
        }

        return new TimedPointCloudData(
            proto.Timestamp, // Already in Universal Time Scale ticks
            new Vector3(proto.Origin.X, proto.Origin.Y, proto.Origin.Z),
            timedPointCloud,
            intensities
        );
    }
}