using CartographerSharp.Mapping;
using Microsoft.AspNetCore.SignalR;
using RobotNet10.RobotApp.Client.Shared.SLAM;
using RobotNet10.RobotApp.Devices;
using RobotNet10.RobotApp.Hubs;
using RobotNet10.RobotApp.Shared;
using RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
using RobotNet10.Shared.Geometry;
using RobotNet10.Shared.Localization;
using RobotNet10.Shared.Numbers;

namespace RobotNet10.RobotApp.SLAM.Cartographer;

public partial class CartographerService
{
    #region Map Listing and Info

    /// <summary>
    /// Liệt kê các maps có sẵn trong thư mục maps
    /// </summary>
    public IReadOnlyList<MapInfo> ListMaps()
    {
        var mapsDirectory = Path.GetFullPath(_config.MapStorage.Directory);
        if (!Directory.Exists(mapsDirectory))
            return [];

        try
        {
            var maps = new List<MapInfo>();
            var mapDirectories = Directory.GetDirectories(mapsDirectory);

            foreach (var mapDir in mapDirectories)
            {
                try
                {
                    var metadataPath = MapPathHelper.ResolveMetadataPath(mapDir);
                    if (metadataPath != null)
                    {
                        var jsonContent = File.ReadAllText(metadataPath);
                        var metadata = System.Text.Json.JsonSerializer.Deserialize<MapInfo>(jsonContent);
                        if (metadata != null)
                        {
                            metadata = MapNameHelper.EnsureMapSize(metadata);
                            maps.Add(metadata);
                        }
                    }
                    else
                    {
                        // Create basic metadata from directory name
                        var mapName = Path.GetFileName(mapDir);
                        maps.Add(new MapInfo
                        {
                            Name = mapName,
                            FolderPath = mapDir,
                            CreatedDate = Directory.GetCreationTime(mapDir),
                            Resolution = _config.MapStorage.OccupancyGridResolution
                        });
                    }
                }
                catch (Exception ex)
                {
                    _logger.LogWarning(ex, "CartographerService: Failed to read metadata for map in: {MapDir}", mapDir);
                }
            }

            return maps;
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "CartographerService: Failed to list maps");
            return [];
        }
    }

    /// <summary>
    /// Lấy thông tin chi tiết của một map theo tên
    /// </summary>
    public MapInfo? GetMapInfo(string mapName)
    {
        try
        {
            var mapPath = MapPathHelper.GetMapPath(_config.MapStorage.Directory, mapName);

            if (!Directory.Exists(mapPath))
            {
                _logger.LogWarning("CartographerService: Map directory not found: {MapPath}", mapPath);
                return null;
            }

            var metadataPath = MapPathHelper.ResolveMetadataPath(mapPath);

            if (metadataPath == null)
            {
                // Create basic metadata from directory name
                var sanitizedMapName = MapNameHelper.Sanitize(mapName);
                return new MapInfo
                {
                    Name = sanitizedMapName,
                    FolderPath = mapPath,
                    CreatedDate = Directory.GetCreationTime(mapPath),
                    Resolution = _config.MapStorage.OccupancyGridResolution
                };
            }

            var jsonContent = File.ReadAllText(metadataPath);
            var metadata = System.Text.Json.JsonSerializer.Deserialize<MapInfo>(jsonContent);
            if (metadata == null)
                return null;

            metadata = MapNameHelper.EnsureMapSize(metadata);

            return metadata;
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "CartographerService: Failed to get map info: {MapName}", mapName);
            return null;
        }
    }

    /// <summary>
    /// Lấy trạng thái xử lý của map
    /// </summary>
    /// <param name="mapName">Tên map</param>
    /// <returns>True nếu map đang được xử lý, false nếu không</returns>
    public bool GetMapProcessingStatus(string mapName)
    {
        return _mapProcessingStatus.TryGetValue(mapName, out var isProcessing) && isProcessing;
    }

    #endregion

    #region Map Operations

    /// <summary>
    /// Lấy đường dẫn đến file ảnh PNG của map
    /// </summary>
    public string? GetMapImagePath(string mapName)
    {
        try
        {
            var mapPath = MapPathHelper.GetMapPath(_config.MapStorage.Directory, mapName);

            if (!Directory.Exists(mapPath))
            {
                _logger.LogWarning("CartographerService: Map directory not found: {MapPath}", mapPath);
                return null;
            }

            var imagePath = MapPathHelper.ResolveImagePath(mapPath);
            if (imagePath != null)
                return imagePath;

            _logger.LogWarning("CartographerService: Map image not found for: {MapName}", mapName);
            return null;
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "CartographerService: Failed to get map image path: {MapName}", mapName);
            return null;
        }
    }

    /// <summary>
    /// Xóa map folder
    /// </summary>
    public Task<bool> DeleteMapAsync(string mapName, CancellationToken cancellationToken = default)
    {
        try
        {
            var mapPath = MapPathHelper.GetMapPath(_config.MapStorage.Directory, mapName);

            if (!Directory.Exists(mapPath))
            {
                _logger.LogWarning("CartographerService: Map directory not found: {MapPath}", mapPath);
                return Task.FromResult(false);
            }

            Directory.Delete(mapPath, recursive: true);
            return Task.FromResult(true);
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "CartographerService: Failed to delete map: {MapName}", mapName);
            return Task.FromResult(false);
        }
    }

    /// <summary>
    /// Transform map để chọn lại gốc tọa độ.
    /// Kiểm tra nếu map đang được xử lý thì return false.
    /// Nếu không, bắt đầu xử lý trên thread riêng và return true ngay lập tức.
    /// </summary>
    public Task<bool> TransformMapOriginAsync(string mapName, Pose newOrigin, CancellationToken cancellationToken = default)
    {
        // Check if map is already being processed
        if (_mapProcessingStatus.TryGetValue(mapName, out var isProcessing) && isProcessing)
        {
            _logger.LogWarning("CartographerService: Map {MapName} is already being processed", mapName);
            return Task.FromResult(false);
        }

        // Set processing status to true
        _mapProcessingStatus[mapName] = true;

        // Notify clients that processing has started
        _ = _hubContext.Clients.Group(mapName).SendAsync("OnMapProcessingChanged", mapName, true, cancellationToken);

        // Start processing on a separate thread
        _ = Task.Run(async () =>
        {
            try
            {
                await TransformMapOriginInternalAsync(mapName, newOrigin, cancellationToken);
            }
            finally
            {
                // Set processing status to false
                _mapProcessingStatus[mapName] = false;

                // Notify clients that processing has completed
                await _hubContext.Clients.Group(mapName).SendAsync("OnMapProcessingChanged", mapName, false, cancellationToken);
            }
        }, cancellationToken);

        // Return true immediately to indicate processing has started
        return Task.FromResult(true);
    }

    /// <summary>
    /// Internal method that performs the actual map transformation.
    /// Based on xloc.cc ChangeMapOrigin logic: updates TransformToMap in the PoseGraph proto
    /// while preserving all other data (trajectories, submaps, options, etc.).
    /// </summary>
    private async Task<bool> TransformMapOriginInternalAsync(string mapName, Pose newOrigin, CancellationToken cancellationToken = default)
    {
        try
        {
            var mapPath = MapPathHelper.GetMapPath(_config.MapStorage.Directory, mapName);
            var metadataPath = MapPathHelper.ResolveMetadataPath(mapPath);
            var pbstreamPath = MapPathHelper.ResolvePbstreamPath(mapPath);

            if (metadataPath == null)
            {
                _logger.LogError("TransformMapOrigin: Metadata file not found in: {MapPath}", mapPath);
                return false;
            }

            if (pbstreamPath == null)
            {
                _logger.LogError("TransformMapOrigin: Pbstream file not found in: {MapPath}", mapPath);
                return false;
            }

            // Load metadata
            var jsonContent = await File.ReadAllTextAsync(metadataPath, cancellationToken);
            var metadata = System.Text.Json.JsonSerializer.Deserialize<MapCartographerInfo>(jsonContent);

            if (metadata == null)
            {
                _logger.LogError("TransformMapOrigin: Failed to deserialize metadata");
                return false;
            }

            // === Update pbstream file using shared helper ===
            // This preserves all original data (AllTrajectoryBuilderOptions, submaps, nodes, etc.)
            PbstreamTransformHelper.TransformPbstreamOrigin(pbstreamPath, newOrigin, _logger);

            // Reload transformed map to regenerate occupancy grid and image files
            var transformedLoadResult = LoadMapFromDirectory(mapName);
            if (transformedLoadResult == null)
            {
                _logger.LogError("TransformMapOrigin: Failed to load transformed map for file regeneration");
                return false;
            }

            MapBuilder transformedMapBuilder;
            try
            {
                transformedMapBuilder = MapBuilderHelper.CreateMapBuilderFromLoadResult(
                    transformedLoadResult,
                    _config,
                    _logger,
                    loadFrozenState: false);
            }
            catch (Exception ex)
            {
                _logger.LogError(ex, "TransformMapOrigin: Failed to create MapBuilder from transformed pbstream");
                return false;
            }

            try
            {
                // Use MapSaveProcessor to regenerate occupancy grid files and update metadata
                if (_mapSaveProcessor == null)
                {
                    _logger.LogError("TransformMapOrigin: MapSaveProcessor is not available");
                    return false;
                }

                await _mapSaveProcessor.SaveMapMetadataAsync(
                    mapName,
                    transformedMapBuilder,
                    mapPath,
                    cancellationToken);

                return true;
            }
            finally
            {
                transformedMapBuilder.Dispose();
            }
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "TransformMapOrigin: Failed for map {MapName}", mapName);
            return false;
        }
    }

    /// <summary>
    /// Rerender map image files (PNG, JPG, PGM) with custom OccupancyGridConfiguration.
    /// Kiểm tra nếu map đang được xử lý thì return false.
    /// Nếu không, bắt đầu xử lý trên thread riêng và return true ngay lập tức.
    /// </summary>
    public Task<bool> RerenderMapWithConfigAsync(string mapName, OccupancyGridConfigurationDto configDto, CancellationToken cancellationToken = default)
    {
        // Check if map is already being processed
        if (_mapProcessingStatus.TryGetValue(mapName, out var isProcessing) && isProcessing)
        {
            _logger.LogWarning("CartographerService: Map {MapName} is already being processed", mapName);
            return Task.FromResult(false);
        }

        // Set processing status to true
        _mapProcessingStatus[mapName] = true;

        // Notify clients that processing has started
        _ = _hubContext.Clients.Group(mapName).SendAsync("OnMapProcessingChanged", mapName, true, cancellationToken);

        // Start processing on a separate thread
        _ = Task.Run(async () =>
        {
            try
            {
                await RerenderMapWithConfigInternalAsync(mapName, configDto, cancellationToken);
            }
            finally
            {
                // Set processing status to false
                _mapProcessingStatus[mapName] = false;

                // Notify clients that processing has completed
                await _hubContext.Clients.Group(mapName).SendAsync("OnMapProcessingChanged", mapName, false, cancellationToken);
            }
        }, cancellationToken);

        // Return true immediately to indicate processing has started
        return Task.FromResult(true);
    }

    /// <summary>
    /// Internal method that performs the actual map rerendering.
    /// Loads pbstream, generates occupancy grid with custom config, and saves image files.
    /// </summary>
    private async Task<bool> RerenderMapWithConfigInternalAsync(string mapName, OccupancyGridConfigurationDto configDto, CancellationToken cancellationToken = default)
    {
        try
        {
            var mapPath = MapPathHelper.GetMapPath(_config.MapStorage.Directory, mapName);
            var pbstreamPath = MapPathHelper.ResolvePbstreamPath(mapPath);

            if (pbstreamPath == null)
            {
                _logger.LogError("RerenderMapWithConfig: Pbstream file not found in: {MapPath}", mapPath);
                return false;
            }

            // Load map from pbstream
            var loadResult = LoadMapFromDirectory(mapName);
            if (loadResult == null)
            {
                _logger.LogError("RerenderMapWithConfig: Failed to load map from directory");
                return false;
            }

            MapBuilder mapBuilder;
            try
            {
                mapBuilder = MapBuilderHelper.CreateMapBuilderFromLoadResult(
                    loadResult,
                    _config,
                    _logger,
                    loadFrozenState: false);
            }
            catch (Exception ex)
            {
                _logger.LogError(ex, "RerenderMapWithConfig: Failed to create MapBuilder from pbstream");
                return false;
            }

            try
            {
                // === DEBUG: Log PoseGraph state after loading ===
                var poseGraph = mapBuilder.PoseGraph;
                var transformToMap = poseGraph.GetTransformToMap();
                _logger.LogWarning(
                    "RerenderMapWithConfig DEBUG: TransformToMap = T[{TX:F4}, {TY:F4}, {TZ:F4}], R[{RW:F4}, {RX:F4}, {RY:F4}, {RZ:F4}]",
                    transformToMap.Translation.X, transformToMap.Translation.Y, transformToMap.Translation.Z,
                    transformToMap.Rotation.W, transformToMap.Rotation.X, transformToMap.Rotation.Y, transformToMap.Rotation.Z);

                var allSubmapData = poseGraph.GetAllSubmapData();
                _logger.LogWarning("RerenderMapWithConfig DEBUG: Total submaps = {Count}", allSubmapData?.Count ?? 0);

                // Log LocalToGlobalTransform for each trajectory
                try
                {
                    var trajectoryStates = poseGraph.GetTrajectoryStates();
                    _logger.LogWarning("RerenderMapWithConfig DEBUG: Found {Count} trajectories", trajectoryStates?.Count ?? 0);
                    if (trajectoryStates != null)
                    {
                        foreach (var trajState in trajectoryStates)
                        {
                            var trajId = trajState.Key;
                            var localToGlobal = poseGraph.GetLocalToGlobalTransform(trajId);
                            _logger.LogWarning(
                                "RerenderMapWithConfig DEBUG: Trajectory[{TrajId}] State={State}, LocalToGlobalTransform = " +
                                "T[{TX:F4},{TY:F4},{TZ:F4}], R[{RW:F4},{RX:F4},{RY:F4},{RZ:F4}]",
                                trajId, trajState.Value,
                                localToGlobal.Translation.X, localToGlobal.Translation.Y, localToGlobal.Translation.Z,
                                localToGlobal.Rotation.W, localToGlobal.Rotation.X, localToGlobal.Rotation.Y, localToGlobal.Rotation.Z);
                        }
                    }
                }
                catch (Exception ex)
                {
                    _logger.LogWarning("RerenderMapWithConfig DEBUG: Failed to get trajectory states: {Error}", ex.Message);
                }

                int submapIdx = 0;
                foreach (var idDataRef in allSubmapData)
                {
                    var submapId = idDataRef.Id;
                    var submapData = idDataRef.Data;
                    var submapPose = submapData.Pose;

                    // Calculate globalPose as OccupancyGridGenerator does
                    var transformToMapInverse = transformToMap.Inverse();
                    var globalPose = transformToMapInverse * submapPose;

                    if (submapData.Submap is CartographerSharp.Mapping.D2D.Submap2D submap2D)
                    {
                        var localPose = submap2D.LocalPose;
                        var grid = submap2D.Grid;
                        var gridInfo = grid != null
                            ? $"cells={grid.Limits.CellLimits.NumXCells}x{grid.Limits.CellLimits.NumYCells}, res={grid.Limits.Resolution:F4}"
                            : "null";

                        // Calculate yaw from quaternion components
                        static double GetYawDegreesFromComponents(double qw, double qx, double qy, double qz)
                        {
                            var siny_cosp = 2.0 * (qw * qz + qx * qy);
                            var cosy_cosp = 1.0 - 2.0 * (qy * qy + qz * qz);
                            return Math.Atan2(siny_cosp, cosy_cosp) * 180.0 / Math.PI;
                        }

                        var localYaw = GetYawDegreesFromComponents(
                            localPose.Rotation.W, localPose.Rotation.X, localPose.Rotation.Y, localPose.Rotation.Z);
                        var submapPoseYaw = GetYawDegreesFromComponents(
                            submapPose.Rotation.W, submapPose.Rotation.X, submapPose.Rotation.Y, submapPose.Rotation.Z);
                        var globalPoseYaw = GetYawDegreesFromComponents(
                            globalPose.Rotation.W, globalPose.Rotation.X, globalPose.Rotation.Y, globalPose.Rotation.Z);

                        _logger.LogWarning(
                            "RerenderMapWithConfig DEBUG: Submap[{Idx}] Id={TrajId}:{SubIdx}, " +
                            "LocalPose=T[{LPX:F4},{LPY:F4}] Yaw={LPYaw:F2}°, " +
                            "SubmapData.Pose=T[{SPX:F4},{SPY:F4}] Yaw={SPYaw:F2}°, " +
                            "GlobalPose=T[{GPX:F4},{GPY:F4}] Yaw={GPYaw:F2}°, " +
                            "Grid={GridInfo}",
                            submapIdx, submapId.TrajectoryId, submapId.SubmapIndex,
                            localPose.Translation.X, localPose.Translation.Y, localYaw,
                            submapPose.Translation.X, submapPose.Translation.Y, submapPoseYaw,
                            globalPose.Translation.X, globalPose.Translation.Y, globalPoseYaw,
                            gridInfo);
                    }
                    submapIdx++;
                }
                // === END DEBUG ===

                // Convert DTO to OccupancyGridConfiguration
                var config = ConvertDtoToConfig(configDto);

                // Generate occupancy grid with custom config
                var resolution = _config.MapStorage.OccupancyGridResolution;
                var padding = _config.MapStorage.MapPadding;
                var occupancyGrid = OccupancyGridGenerator.Generate(mapBuilder, resolution, padding, _logger, config);

                if (occupancyGrid == null)
                {
                    _logger.LogError("RerenderMapWithConfig: Failed to generate occupancy grid");
                    return false;
                }

                // === DEBUG: Log OccupancyGrid result ===
                _logger.LogWarning(
                    "RerenderMapWithConfig DEBUG: OccupancyGrid size={W}x{H}, resolution={Res:F4}, " +
                    "Origin=T[{OX:F4},{OY:F4}]",
                    occupancyGrid.Width, occupancyGrid.Height, occupancyGrid.Resolution,
                    occupancyGrid.Origin.Position.X, occupancyGrid.Origin.Position.Y);
                // === END DEBUG ===

                // Save all image formats (PGM, PNG, JPG, YAML)
                await OccupancyGridFileHelper.SaveAllFormatsAsync(occupancyGrid, mapPath, cancellationToken, _logger);

                _logger.LogInformation("RerenderMapWithConfig: Successfully rerendered map {MapName} with custom config", mapName);
                return true;
            }
            finally
            {
                mapBuilder.Dispose();
            }
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "RerenderMapWithConfig: Failed for map {MapName}", mapName);
            return false;
        }
    }

    /// <summary>
    /// Convert OccupancyGridConfigurationDto to OccupancyGridConfiguration
    /// </summary>
    private static OccupancyGridConfiguration ConvertDtoToConfig(OccupancyGridConfigurationDto dto)
    {
        return new OccupancyGridConfiguration
        {
            MergeStrategy = dto.MergeStrategy switch
            {
                SubmapMergeStrategyDto.PorterDuff => SubmapMergeStrategy.PorterDuff,
                SubmapMergeStrategyDto.LogOddsSum => SubmapMergeStrategy.LogOddsSum,
                SubmapMergeStrategyDto.MaxProbability => SubmapMergeStrategy.MaxProbability,
                _ => SubmapMergeStrategy.LogOddsSum
            },
            LogOddsClamp = dto.LogOddsClamp,
            UseLogOddsAverage = dto.UseLogOddsAverage,
            FreeSpaceThreshold = dto.FreeSpaceThreshold,
            OccupiedSpaceThreshold = dto.OccupiedSpaceThreshold,
            UseBinaryOutput = dto.UseBinaryOutput,
            EnableWallThinning = dto.EnableWallThinning,
            WallThinningIterations = dto.WallThinningIterations,
            MinWallThicknessPixels = dto.MinWallThicknessPixels,
            AmbiguousCellValue = dto.AmbiguousCellValue,
            AmbiguousRangeLower = dto.AmbiguousRangeLower,
            AmbiguousRangeUpper = dto.AmbiguousRangeUpper,
            EnableMedianFilter = dto.EnableMedianFilter,
            MedianFilterKernelSize = dto.MedianFilterKernelSize
        };
    }

    #endregion

    #region Map Loading

    /// <summary>
    /// Load map files directly from map directory
    /// </summary>
    private MapCartographerLoadResult? LoadMapFromDirectory(string mapName)
    {
        try
        {
            var mapPath = MapPathHelper.GetMapPath(_config.MapStorage.Directory, mapName);

            if (!Directory.Exists(mapPath))
            {
                _logger.LogError("CartographerService: Map directory not found: {MapPath}", mapPath);
                return null;
            }

            // Find pbstream file (map.pbstream or legacy)
            var pbstreamPath = MapPathHelper.ResolvePbstreamPath(mapPath);
            if (pbstreamPath == null)
            {
                _logger.LogError("CartographerService: No pbstream file found in: {MapPath}", mapPath);
                return null;
            }

            if (Path.GetFileName(pbstreamPath) != "map.pbstream")
            {
                _logger.LogWarning("CartographerService: Using legacy pbstream file: {Path}", pbstreamPath);
            }

            // Load metadata to get saved config (map.json)
            MapCartographerConfigSnapshot? savedConfig = null;
            var metadataPath = MapPathHelper.ResolveMetadataPath(mapPath);
            if (metadataPath != null)
            {
                if (Path.GetFileName(metadataPath) == "metadata.json")
                {
                    _logger.LogWarning("CartographerService: Using legacy metadata.json file");
                }

                try
                {
                    var jsonContent = File.ReadAllText(metadataPath);
                    var metadata = System.Text.Json.JsonSerializer.Deserialize<MapCartographerInfo>(jsonContent);
                    savedConfig = metadata?.MapConfig;
                }
                catch (Exception ex)
                {
                    _logger.LogWarning(ex, "CartographerService: Failed to load metadata, will use current config");
                }
            }

            return new MapCartographerLoadResult
            {
                PbstreamPath = pbstreamPath,
                MapPath = mapPath,
                SavedConfig = savedConfig
            };
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "CartographerService: Failed to load map: {MapName}", mapName);
            return null;
        }
    }

    private bool MapExists(string mapName)
    {
        try
        {
            var mapPath = MapPathHelper.GetMapPath(_config.MapStorage.Directory, mapName);
            return Directory.Exists(mapPath);
        }
        catch
        {
            return false;
        }
    }

    #endregion

    #region Wall Alignment

    /// <summary>
    /// Calculate wall-aligned pose by detecting walls from lidar scans.
    /// This method should be called AFTER sensors are resumed to ensure fresh lidar data.
    /// The aligned pose should be set on LocalTrajectoryBuilder2D using SetInitialPose()
    /// BEFORE resuming sensors for scan mapping.
    /// </summary>
    /// <returns>Wall-aligned pose if successful, null otherwise</returns>
    private Pose CalculateWallAlignedPoseAsync()
    {
        _logger.LogInformation("CartographerService: Starting wall alignment calculation");

        // Step 1: Get all lidar devices
        var allDevices = _deviceProvider.GetDevicesByType(RobotNet10.RobotApp.Client.Shared.Devices.DeviceType.Lidar);
        var lidarList = allDevices.OfType<ILidar>().ToList();

        if (lidarList.Count == 0)
        {
            _logger.LogWarning("CartographerService: No lidar devices found for wall alignment");
            return new Pose();
        }

        _logger.LogInformation("CartographerService: Found {Count} lidar device(s)", lidarList.Count);

        // Step 2: Collect point clouds from all lidars and transform to base_link frame
        var allPoints = new List<Vector3>();

        foreach (var lidar in lidarList)
        {
            try
            {
                var deviceId = (lidar as DeviceBase)?.DeviceId;
                if (string.IsNullOrEmpty(deviceId))
                {
                    _logger.LogWarning("CartographerService: Lidar device has no ID, skipping");
                    continue;
                }

                var laserScan = lidar.CurrentMeasurementData;
                if (!laserScan.HasValue)
                {
                    _logger.LogWarning("CartographerService: No scan data from lidar {DeviceId}", deviceId);
                    continue;
                }

                // Get sensor configuration and transform
                var sensorConfig = _config.Sensors.Lidars
                    .FirstOrDefault(cfg => cfg.DeviceId == deviceId && cfg.Enabled);

                if (sensorConfig == null)
                {
                    _logger.LogWarning("CartographerService: No configuration found for lidar {DeviceId}", deviceId);
                    continue;
                }

                var scan = laserScan.Value;

                // Convert LaserScan to point cloud in base_link frame
                var (baseLink, _, _, _, _) = SensorDataTransformHelper.ToTimedPointCloudDataBaseAndSensorFrame(
                    scan.Header.Stamp,
                    scan,
                    sensorConfig.Transform,
                    sensorConfig.AngleMin,
                    sensorConfig.AngleMax
                );

                // Extract points in base_link frame (use .Ranges instead of .Points)
                int pointsAdded = 0;
                foreach (var point in baseLink.Ranges)
                {
                    allPoints.Add(point.Position);
                    pointsAdded++;
                }

                _logger.LogInformation(
                    "CartographerService: Collected {Count} points from lidar {DeviceId}",
                    pointsAdded, deviceId);
            }
            catch (Exception ex)
            {
                _logger.LogError(ex, "CartographerService: Error processing lidar data for wall alignment");
            }
        }

        if (allPoints.Count == 0)
        {
            _logger.LogWarning("CartographerService: No points collected for wall alignment");
            return new Pose();
        }

        _logger.LogInformation("CartographerService: Total points collected: {Count}", allPoints.Count);

        // Step 3: Detect wall and calculate compensation angle
        var compensationAngle = WallAlignmentHelper.DetectWallAndCalculateCompensation(allPoints, _logger);

        if (!compensationAngle.HasValue)
        {
            _logger.LogWarning("CartographerService: Could not detect wall for alignment");
            return new Pose();
        }

        // Step 4: Calculate wall-aligned pose
        // Start with identity pose (robot at origin, facing along X-axis)
        // Then apply the compensation angle to align with detected wall
        var alignedOrientation = WallAlignmentHelper.CreateQuaternionFromYaw(compensationAngle.Value);

        var alignedPose = new Pose
        {
            Position = Vector3.Zero,          // Start at origin
            Orientation = alignedOrientation   // Oriented to align map with wall
        };

        _logger.LogInformation(
            "CartographerService: Calculated wall-aligned pose - " +
            "Position: [{X:F3}, {Y:F3}, {Z:F3}], Yaw: {Yaw:F3}rad ({YawDeg:F1}°), " +
            "Compensation angle: {CompAngle:F3}rad ({CompAngleDeg:F1}°)",
            alignedPose.Position.X,
            alignedPose.Position.Y,
            alignedPose.Position.Z,
            WallAlignmentHelper.GetYawFromQuaternion(alignedPose.Orientation),
            WallAlignmentHelper.GetYawFromQuaternion(alignedPose.Orientation) * 180.0 / Math.PI,
            compensationAngle.Value,
            compensationAngle.Value * 180.0 / Math.PI);

        return alignedPose;
    }

    #endregion
}