using RobotNet10.RobotApp.Client.Shared.SLAM;
using RobotNet10.RobotApp.Shared;
using RobotNet10.RobotApp.Shared.Enums;
using RobotNet10.RobotApp.SLAM.Cartographer.Geometry;
using RobotNet10.Shared.Geometry;
using RobotNet10.Shared.Localization;
using RobotNet10.Shared.Numbers;

namespace RobotNet10.RobotApp.SLAM;

/// <summary>
/// Unified interface for Cartographer SLAM service including localization, scan mapping, and occupancy grid
/// </summary>
public interface ISLAMService
{
    // ==================== State and Properties ====================
    
    /// <summary>
    /// Gets the current state
    /// </summary>
    SLAMState State { get; }

    /// <summary>
    /// Gets the current pose (from localization or scan mapping depending on current state)
    /// </summary>
    Pose CurrentPose { get; }

    /// <summary>
    /// Gets the covariance of pose estimate (only available during localization)
    /// </summary>
    Matrix3x3? PoseCovariance { get; }

    // ==================== Localization Operations ====================

    string? CurrentMap { get; }

    double? LocalizationScore { get; }

    /// <summary>
    /// Start localization with a saved map.
    /// Validates state, stores pending data, and fires StartLocalization trigger.
    /// The actual localization setup is handled asynchronously by the state machine.
    /// </summary>
    /// <param name="mapName">Name of the map to load</param>
    /// <param name="initialPose">Optional initial pose: base_link in map frame. No transform to sensor frame needed.</param>
    void StartLocalization(string mapName, Pose? initialPose = null);

    /// <summary>
    /// Set initial pose for localization (must be called after StartLocalization).
    /// Pose must be base_link in map frame (e.g. from map click). No transform to sensor frame is needed; MCL uses base_link + baseLink2Laser internally.
    /// Validates state, stores pending data, and fires trigger. Actual work handled by state machine.
    /// </summary>
    void SetInitialPose(Pose pose);

    /// <summary>
    /// Stop localization and cleanup resources.
    /// Fires StopLocalization trigger. State machine handles the actual cleanup.
    /// </summary>
    void StopLocalization();
    
    // ==================== Scan Mapping Control ====================

    /// <summary>
    /// Start scan mapping with the specified map name.
    /// Validates state, stores map name, and fires StartScanMapping trigger.
    /// The actual scan mapping setup is handled by the state machine.
    /// </summary>
    /// <param name="mapName">Map name to be used when saving</param>
    void StartScanMapping(string mapName);

    /// <summary>
    /// Save current map and stop scan mapping.
    /// Validates state and fires SaveMap trigger. The full save workflow (finish trajectory,
    /// optimization, save to storage) is handled asynchronously by the state machine.
    /// </summary>
    void SaveScanMap();
    
    /// <summary>
    /// Get trajectory nodes from pose graph (dùng nội bộ khi build OccupancyGridDto).
    /// Trả về nodes khi Localizing (từ map đã load) hoặc ScanMapping (từ pose graph hiện tại).
    /// </summary>
    List<(int NodeId, Pose Pose)>? GetTrajectoryNodes();

    /// <summary>
    /// Get aggregated sample point cloud from lidar (map/global frame).
    /// Returns points from AddSensorData SamplePointCloudGlobal when available; otherwise empty list.
    /// </summary>
    /// <returns>Read-only list of Point32 in global frame</returns>
    IReadOnlyList<RobotNet10.Shared.Numbers.Vector3> GetAggregatedSamplePointCloud();

    // ==================== Occupancy Grid ====================

    /// <summary>
    /// Lấy occupancy grid hiện tại
    /// Trả về:
    /// - OccupancyGrid từ file PGM nếu đang ở chế độ Localizing
    /// - OccupancyGrid được generate từ tất cả submaps nếu đang ở chế độ ScanMapping
    /// </summary>
    OccupancyGrid? GetOccupancyGrid();

    /// <summary>
    /// Lấy occupancy grid mới hơn thời gian chỉ định
    /// Trả về OccupancyGrid nếu LastUpdated > since, ngược lại trả về null
    /// </summary>
    /// <param name="since">Thời gian để so sánh</param>
    /// <returns>OccupancyGrid nếu có update mới hơn, null nếu không có</returns>
    OccupancyGrid? GetOccupancyGrid(DateTime since);

    /// <summary>
    /// Thời gian cuối cùng occupancy grid được cập nhật
    /// </summary>
    DateTime LastUpdatedOccupancyGrid { get; }

    // ==================== Map Management ====================

    /// <summary>
    /// Liệt kê các maps có sẵn trong thư mục maps
    /// </summary>
    IReadOnlyList<MapInfo> ListMaps();

    /// <summary>
    /// Lấy thông tin chi tiết của một map theo tên
    /// </summary>
    /// <param name="mapName">Tên map</param>
    /// <returns>MapInfo nếu tìm thấy, null nếu không</returns>
    MapInfo? GetMapInfo(string mapName);

    /// <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>
    bool GetMapProcessingStatus(string mapName);

    /// <summary>
    /// Lấy đường dẫn đến file ảnh PNG của map
    /// </summary>
    /// <param name="mapName">Tên map</param>
    /// <returns>Đường dẫn đến file ảnh PNG nếu tồn tại, null nếu không</returns>
    string? GetMapImagePath(string mapName);

    /// <summary>
    /// Xóa map folder
    /// </summary>
    /// <param name="mapName">Tên map cần xóa</param>
    /// <param name="cancellationToken">Cancellation token</param>
    /// <returns>True nếu xóa thành công</returns>
    Task<bool> DeleteMapAsync(string mapName, CancellationToken cancellationToken = default);

    /// <summary>
    /// Transform map để chọn lại gốc tọa độ
    /// </summary>
    /// <param name="mapName">Tên map cần transform</param>
    /// <param name="newOrigin">Gốc tọa độ mới</param>
    /// <param name="cancellationToken">Cancellation token</param>
    /// <returns>True nếu transform thành công</returns>
    Task<bool> TransformMapOriginAsync(string mapName, Pose newOrigin, CancellationToken cancellationToken = default);

    /// <summary>
    /// Rerender map image files (PNG, JPG, PGM) with custom OccupancyGridConfiguration.
    /// Reload pbstream, regenerate occupancy grid với config mới, và lưu lại các file ảnh.
    /// </summary>
    /// <param name="mapName">Tên map cần rerender</param>
    /// <param name="configDto">Cấu hình OccupancyGrid tùy chỉnh</param>
    /// <param name="cancellationToken">Cancellation token</param>
    /// <returns>True nếu rerender được bắt đầu thành công</returns>
    Task<bool> RerenderMapWithConfigAsync(string mapName, OccupancyGridConfigurationDto configDto, CancellationToken cancellationToken = default);
}