using CartographerSharp.Mapping;
using RobotNet10.RobotApp.SLAM.Cartographer;

namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;

/// <summary>
/// Helper class for trajectory-related operations
/// </summary>
public static class TrajectoryHelper
{
    

    /// <summary>
    /// Build sensor IDs from configuration
    /// </summary>
    /// <param name="config">Cartographer configuration</param>
    /// <returns>HashSet of sensor IDs</returns>
    public static HashSet<ITrajectoryBuilder.SensorId> BuildSensorIds(CartographerConfiguration config)
    {
        var sensorIds = new HashSet<ITrajectoryBuilder.SensorId>();

        // Add Lidar sensor IDs
        foreach (var lidarConfig in config.Sensors.Lidars.Where(l => l.Enabled))
        {
            sensorIds.Add(new ITrajectoryBuilder.SensorId(
                ITrajectoryBuilder.SensorId.SensorType.Range,
                lidarConfig.DeviceId));
        }

        // Add IMU sensor ID
        if (config.Sensors.Imu.Enabled && !string.IsNullOrEmpty(config.Sensors.Imu.DeviceId))
        {
            sensorIds.Add(new ITrajectoryBuilder.SensorId(
                ITrajectoryBuilder.SensorId.SensorType.Imu,
                config.Sensors.Imu.DeviceId));
        }

        // Add Odometry sensor ID
        if (config.UseOdometry)
        {
            sensorIds.Add(new ITrajectoryBuilder.SensorId(
                ITrajectoryBuilder.SensorId.SensorType.Odometry,
                "odometry"));
        }

        return sensorIds;
    }
}