using Microsoft.AspNetCore.Authorization;
using Microsoft.AspNetCore.SignalR;
using RobotNet10.RobotApp.Client.Shared.SLAM;
using RobotNet10.RobotApp.Shared;
using RobotNet10.RobotApp.Shared.Enums;
using RobotNet10.RobotApp.SLAM;
using RobotNet10.RobotApp.SLAM.Cartographer.Geometry;
using RobotNet10.Shared.Geometry;
using RobotNet10.Shared.Localization;
using RobotNet10.Shared.Numbers;

namespace RobotNet10.RobotApp.Hubs;

/// <summary>
/// SignalR Hub cho Cartographer/Localization - cung cấp real-time mapping và localization data
/// </summary>
[Authorize]
public class SLAMHub(ISLAMService _slamService, ILogger<SLAMHub> _logger) : Hub
{
    /// <summary>
    /// Called when a client connects to the hub
    /// </summary>
    public override async Task OnConnectedAsync()
    {
        await base.OnConnectedAsync();
        // Clients will poll for occupancy grid using GetOccupancyGridBase() and GetOccupancyGridUpdating() methods
    }

    /// <summary>
    /// Called when a client disconnects from the hub
    /// </summary>
    public override async Task OnDisconnectedAsync(Exception? exception)
    {
        await base.OnDisconnectedAsync(exception);
    }
    /// <summary>
    /// Lấy trạng thái hiện tại của CartographerService
    /// </summary>
    public Task<SLAMState> GetCurrentState()
    {
        var state = _slamService.State;
        return Task.FromResult(MapState(state));
    }

    /// <summary>
    /// Lấy tên map hiện tại đang được sử dụng
    /// </summary>
    public Task<string?> GetCurrentMap()
    {
        return Task.FromResult(_slamService.CurrentMap);
    }

    /// <summary>
    /// Bắt đầu localization với map đã lưu
    /// </summary>
    public async Task<bool> StartLocalization(string mapName, PoseDto? initialPoseDto = null)
    {
        try
        {
            Pose? initialPose = initialPoseDto != null ? MapPoseFromDto(initialPoseDto) : null;
            _slamService.StartLocalization(mapName, initialPose);
            return true;
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "CartographerHub: Failed to start localization");
            await Clients.Caller.SendAsync("OnError", new ErrorDto
            {
                Message = ex.Message,
                Timestamp = DateTime.UtcNow
            });
            return false;
        }
    }

    /// <summary>
    /// Dừng localization
    /// </summary>
    public Task StopLocalization()
    {
        _slamService.StopLocalization();
        return Task.CompletedTask;
    }

    /// <summary>
    /// Bắt đầu scan & mapping
    /// </summary>
    public async Task<bool> StartScanMapping(string mapName)
    {
        try
        {
            _slamService.StartScanMapping(mapName);
            return true;
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "CartographerHub: Failed to start scan mapping");
            await Clients.Caller.SendAsync("OnError", new ErrorDto
            {
                Message = ex.Message,
                Timestamp = DateTime.UtcNow
            });
            return false;
        }
    }

    /// <summary>
    /// Lưu map hiện tại.
    /// Note: This is now fire-and-forget. The actual save happens asynchronously.
    /// Monitor state changes to know when save completes.
    /// </summary>
    public async Task<string?> SaveMap()
    {
        try
        {
            _slamService.SaveScanMap();
            // Return null since we no longer wait for completion
            // Clients should monitor state changes instead
            return null;
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "CartographerHub: Failed to save map");
            await Clients.Caller.SendAsync("OnError", new ErrorDto
            {
                Message = ex.Message,
                Timestamp = DateTime.UtcNow
            });
            return null;
        }
    }

    /// <summary>
    /// Liệt kê tất cả maps có sẵn
    /// </summary>
    public MapInfoDto[] ListMaps()
    {
        try
        {
            var maps = _slamService.ListMaps();
            return [.. maps.Select(MapMapInfoToDto)];
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "CartographerHub: Failed to list maps");
            return [];
        }
    }

    /// <summary>
    /// Lấy thông tin map và đăng ký client vào group để nhận cập nhật trạng thái xử lý
    /// </summary>
    /// <param name="mapName">Tên map</param>
    /// <returns>MapInfoDto với trạng thái IsProcessing</returns>
    public async Task<MapInfoDto?> GetMapInfoAndSubscribeProcessing(string mapName)
    {
        try
        {
            // Subscribe client to group with mapName
            await Groups.AddToGroupAsync(Context.ConnectionId, mapName);

            // Get map info
            var mapInfo = _slamService.GetMapInfo(mapName);
            if (mapInfo == null)
            {
                _logger.LogWarning("SLAMHub: Map not found: {MapName}", mapName);
                return null;
            }

            // Get processing status
            var isProcessing = _slamService.GetMapProcessingStatus(mapName);

            // Map to DTO with IsProcessing
            var dto = MapMapInfoToDto(mapInfo);
            dto.IsProcessing = isProcessing;

            return dto;
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "SLAMHub: Failed to get map info and subscribe: {MapName}", mapName);
            return null;
        }
    }

    /// <summary>
    /// Hủy đăng ký client khỏi group nhận cập nhật trạng thái xử lý
    /// </summary>
    /// <param name="mapName">Tên map</param>
    public async Task UnsubscribeMapProcessing(string mapName)
    {
        try
        {
            await Groups.RemoveFromGroupAsync(Context.ConnectionId, mapName);
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "SLAMHub: Failed to unsubscribe from map processing: {MapName}", mapName);
        }
    }

    /// <summary>
    /// Xóa map
    /// </summary>
    public async Task<bool> DeleteMap(string mapName)
    {
        try
        {
            return await _slamService.DeleteMapAsync(mapName);
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "CartographerHub: Failed to delete map");
            return false;
        }
    }

    /// <summary>
    /// Transform map origin to a new pose
    /// </summary>
    /// <param name="mapName">Name of the map to transform</param>
    /// <param name="newOriginDto">New origin pose (position and orientation)</param>
    /// <returns>True if transform was successful</returns>
    public async Task<bool> TransformMapOrigin(string mapName, PoseDto newOriginDto)
    {
        try
        {
            var newOrigin = MapPoseFromDto(newOriginDto);
            return await _slamService.TransformMapOriginAsync(mapName, newOrigin);
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "CartographerHub: Failed to transform map origin");
            await Clients.Caller.SendAsync("OnError", new ErrorDto
            {
                Message = ex.Message,
                Timestamp = DateTime.UtcNow
            });
            return false;
        }
    }

    /// <summary>
    /// Rerender map image files (PNG, JPG, PGM) with custom OccupancyGridConfiguration.
    /// This is an async operation - returns true if processing started successfully.
    /// Clients subscribed to the map group will receive OnMapProcessingChanged notifications.
    /// </summary>
    /// <param name="mapName">Name of the map to rerender</param>
    /// <param name="configDto">Custom occupancy grid configuration</param>
    /// <returns>True if rerender was started successfully</returns>
    public async Task<bool> RerenderMapWithConfig(string mapName, OccupancyGridConfigurationDto configDto)
    {
        try
        {
            return await _slamService.RerenderMapWithConfigAsync(mapName, configDto);
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "SLAMHub: Failed to rerender map with config");
            await Clients.Caller.SendAsync("OnError", new ErrorDto
            {
                Message = ex.Message,
                Timestamp = DateTime.UtcNow
            });
            return false;
        }
    }

    /// <summary>
    /// Set initial pose cho localization
    /// </summary>
    public async Task SetInitialPose(PoseDto poseDto)
    {
        try
        {
            var pose = MapPoseFromDto(poseDto);
            _slamService.SetInitialPose(pose);
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "CartographerHub: Failed to set initial pose");
            await Clients.Caller.SendAsync("OnError", new ErrorDto
            {
                Message = ex.Message,
                Timestamp = DateTime.UtcNow
            });
        }
    }
    
    /// <summary>
    /// Lấy occupancy grid mới hơn thời gian chỉ định
    /// </summary>
    /// <param name="since">Thời gian để so sánh</param>
    /// <returns>OccupancyGridDto nếu có update mới hơn, null nếu không có</returns>
    public OccupancyGridDto? GetOccupancyGrid(DateTime since)
    {
        var lastUpdated = _slamService.LastUpdatedOccupancyGrid;
        var grid = _slamService.GetOccupancyGrid(since);
        var trajectoryNodes = _slamService.GetTrajectoryNodes();

        if (grid == null)
        {
            _logger.LogDebug(
                "SLAMHub.GetOccupancyGrid: No grid available (since={Since}, lastUpdated={LastUpdated}, state={State})",
                since, lastUpdated, _slamService.State);
            return null;
        }

        _logger.LogDebug(
            "SLAMHub.GetOccupancyGrid: Returning grid {W}x{H}, lastUpdated={LastUpdated}",
            grid.Width, grid.Height, lastUpdated);

        return MapOccupancyGridToDto(grid, lastUpdated, lastUpdated, versionTicks: lastUpdated.Ticks, trajectoryNodes);
    }

    /// <summary>
    /// Lấy pose hiện tại của robot
    /// Trả về pose từ ScanMappingService nếu đang ScanMapping, hoặc từ CartographerService nếu đang Localizing
    /// </summary>
    /// <returns>PoseDto nếu có pose, null nếu không có</returns>
    public PoseDto GetCurrentPose()
    {
        var state = _slamService.State;
        
        // Khi đang ScanMapping, lấy pose từ ScanMappingService
        if (state == SLAMState.ScanMapping)
        {
            return MapPoseToDto(_slamService.CurrentPose, 0);
        }
        // Khi đang Localizing, lấy pose từ CartographerService
        else if (state == SLAMState.Localizing)
        {
            return MapPoseToDto(_slamService.CurrentPose, _slamService.LocalizationScore ?? 0, _slamService.PoseCovariance);
        }
        
        return MapPoseToDto(new Pose(), 0);
    }

    /// <summary>
    /// Lấy sample point cloud từ tất cả lidar devices (đã transform sang pose-graph global frame).
    /// Pipeline: LocalTrajectoryBuilder2D → trajectory-with-global-orientation, GlobalTrajectoryBuilder2D → GetLocalToGlobalTransform → global.
    /// </summary>
    /// <returns>Danh sách Point32 trong global (map) frame, empty list nếu không có</returns>
    public Vector3[] GetSamplePointCloud()
    {
        var points = _slamService.GetAggregatedSamplePointCloud();
        return points?.Count > 0 ? [.. points] : [];
    }

    // Mapping methods (convert server types to DTOs)

    private static SLAMState MapState(SLAMState state)
    {
        return state switch
        {
            SLAMState.Idle => SLAMState.Idle,
            SLAMState.Initializing => SLAMState.Initializing,
            SLAMState.Ready => SLAMState.Ready,
            SLAMState.Relocalizing => SLAMState.Relocalizing,
            SLAMState.Localizing => SLAMState.Localizing,
            SLAMState.ScanMapping => SLAMState.ScanMapping,
            SLAMState.SavingMap => SLAMState.SavingMap,
            SLAMState.Error => SLAMState.Error,
            _ => SLAMState.Idle
        };
    }

    private static Pose MapPoseFromDto(PoseDto dto)
    {
        return new Pose
        {
            Position = dto.Position,
            Orientation = dto.Orientation
        };
    }

    private static PoseDto MapPoseToDto(Pose pose, double score, Matrix3x3? covariance = null)
    {
        var dto = new PoseDto
        {
            Position = pose.Position,
            Orientation = pose.Orientation,
            Score = score,
            Timestamp = DateTime.UtcNow
        };

        // Convert covariance matrix to flattened array if available
        if (covariance.HasValue)
        {
            var cov = covariance.Value;
            // Matrix3x3 uses indexer [row, col] where:
            // row 0 = x, row 1 = y, row 2 = theta
            // col 0 = x, col 1 = y, col 2 = theta
            dto.Covariance =
            [
                cov[0, 0], cov[0, 1], cov[0, 2], // XX, XY, XT
                cov[1, 0], cov[1, 1], cov[1, 2], // YX, YY, YT
                cov[2, 0], cov[2, 1], cov[2, 2]  // TX, TY, TT
            ];
        }

        return dto;
    }

    private OccupancyGridDto MapOccupancyGridToDto(OccupancyGrid grid, DateTime lastBaseUpdated, DateTime lastUpdated, long versionTicks, List<(int NodeId, Pose Pose)>? trajectoryNodes = null)
    {
        if (grid == null)
        {
            _logger.LogWarning("CartographerHub: MapOccupancyGridToDto called with null grid");
            throw new ArgumentNullException(nameof(grid));
        }

        // Convert to sparse format: only include known cells (value >= 0)
        // Format: byte[] với mỗi cell = 5 bytes: [index_byte0, index_byte1, index_byte2, index_byte3, value]
        // Index: 4 bytes little-endian (32-bit unsigned integer)
        // Value: 1 byte (0-100)
        var knownCells = new List<byte>();
        
        int knownCellCount = 0;
        for (int i = 0; i < grid.Data.Length; i++)
        {
            var value = grid.Data[i];
            if (value >= 0) // Only include known cells (skip unknown = -1)
            {
                knownCellCount++;
                // Encode index as 4 bytes (little-endian)
                knownCells.Add((byte)(i & 0xFF));           // Byte 0: LSB
                knownCells.Add((byte)((i >> 8) & 0xFF));    // Byte 1
                knownCells.Add((byte)((i >> 16) & 0xFF));   // Byte 2
                knownCells.Add((byte)((i >> 24) & 0xFF));   // Byte 3: MSB
                
                // Encode value as 1 byte (0-100)
                knownCells.Add((byte)value);
            }
        }

        TrajectoryNodeDto[]? trajectoryDtos = null;
        if (trajectoryNodes != null && trajectoryNodes.Count > 0)
        {
            trajectoryDtos = [.. trajectoryNodes.Select(node => new TrajectoryNodeDto
            {
                NodeId = node.NodeId,
                Pose = node.Pose,
                Timestamp = DateTime.UtcNow
            })];
        }
        
        return new OccupancyGridDto
        {
            Resolution = grid.Resolution,
            Width = grid.Width,
            Height = grid.Height,
            Origin = grid.Origin,
            KnownCells = [.. knownCells],
            Version = versionTicks,
            LastBaseUpdated = lastBaseUpdated,
            LastUpdated = lastUpdated,
            TrajectoryNodes = trajectoryDtos
        };
    }

    private static MapInfoDto MapMapInfoToDto(MapInfo mapInfo)
    {
        return new MapInfoDto
        {
            Name = mapInfo.Name,
            CreatedDate = mapInfo.CreatedDate,
            Resolution = mapInfo.Resolution,
            Width = mapInfo.Size.Width,
            Height = mapInfo.Size.Height,
            TrajectoryNodeCount = mapInfo.TrajectoryNodeCount,
            OriginX = mapInfo.Origin.Position.X,
            OriginY = mapInfo.Origin.Position.Y,
        };
    }
}