@page "/localization"
@rendermode InteractiveWebAssemblyNoPrerender
@implements IAsyncDisposable

@using MudBlazor
@using RobotNet10.RobotApp.Client.Clients
@using RobotNet10.RobotApp.Client.Components.SLAM
@using RobotNet10.RobotApp.Client.Dialogs
@using RobotNet10.RobotApp.Client.Shared.SLAM
@using RobotNet10.RobotApp.Shared.Enums
@using RobotNet10.Shared.Detection
@using RobotNet10.Shared.Geometry

<PageTitle>Localization</PageTitle>

<div class="h-100 w-100 d-flex flex-column overflow-hidden">
    <!-- Control Bar -->
    <div class="position-relative d-flex flex-row align-items-center gap-3 flex-wrap px-1" style="background-color: var(--mud-palette-background);">
        <!-- Map Name -->
        <div class="d-flex flex-row align-items-center gap-2" style="min-width: 300px;">
            <MudTextField @bind-Value="MapName"
                          Label="Map Name" ShrinkLabel
                          Variant="Variant.Outlined"
                          Margin="Margin.Dense"
                          Disabled="@(CurrentState != SLAMState.Ready)"
                          Style="max-width: 300px;" />
        </div>

        <!-- Controls -->
        <div class="d-flex flex-row align-items-center gap-2">
            <MudButtonGroup Variant="Variant.Filled" Color="Color.Primary">
                @if (IsLocalizationActive)
                {
                    @* Fit View Button - always visible when map is displayed *@
                    <MudTooltip Text="Fit View">
                        <MudIconButton Color="Color.Default" Variant="Variant.Outlined"
                                       Icon="@Icons.Material.Filled.FitScreen"
                                       OnClick="FitViewAsync" />
                    </MudTooltip>

                    @* Localizing or InitializingLocalizing: Show Stop, Initial Pose, Marker Detect *@
                    <MudTooltip Text="Stop">
                        <MudIconButton Color="Color.Error" Variant="Variant.Filled"
                                       Icon="@Icons.Material.Filled.Stop"
                                       OnClick="StopLocalizationAsync" />
                    </MudTooltip>
                    <MudTooltip Text="Initialize Pose">
                        <MudIconButton Color="Color.Secondary" Variant="Variant.Filled"
                                       Icon="@Icons.Material.Filled.MyLocation"
                                       OnClick="SetInitialPoseAsync" />
                    </MudTooltip>
                    <MudTooltip Text="@(_isDetecting ? "Stop Detect" : "Marker Detect")">
                        <MudIconButton Color="@(_isDetecting? Color.Error: Color.Tertiary)" Variant="Variant.Filled"
                                       Icon="@(_isDetecting ? Icons.Material.Filled.StopCircle : Icons.Material.Filled.Search)"
                                       OnClick="ToggleMarkerDetectAsync"
                                       Disabled="@(CurrentState == SLAMState.Relocalizing)" />
                    </MudTooltip>
                }
                else if (IsScanMappingActive)
                {
                    @* Fit View Button - always visible when map is displayed *@
                    <MudTooltip Text="Fit View">
                        <MudIconButton Color="Color.Default" Variant="Variant.Outlined"
                                       Icon="@Icons.Material.Filled.FitScreen"
                                       OnClick="FitViewAsync" />
                    </MudTooltip>

                    @* ScanMapping or SavingMap: Show Save Map *@
                    <MudTooltip Text="@(CurrentState == SLAMState.SavingMap ? "Saving..." : "Save Map")">
                        <MudIconButton Color="Color.Success" Variant="Variant.Filled"
                                       Icon="@Icons.Material.Filled.Save"
                                       OnClick="SaveMapAsync"
                                       Disabled="@(CurrentState == SLAMState.SavingMap)" />
                    </MudTooltip>
                }
                else
                {
                    @* Ready or other states: Show Localize and Scan Mapping *@
                    <MudTooltip Text="Localize">
                        <MudIconButton Color="Color.Primary" Variant="Variant.Filled"
                                       Icon="@Icons.Material.Filled.LocationOn"
                                       OnClick="StartLocalizationAsync"
                                       Disabled="@(CurrentState != SLAMState.Ready)" />
                    </MudTooltip>
                    <MudTooltip Text="Scan Mapping">
                        <MudIconButton Color="Color.Primary" Variant="Variant.Filled"
                                       Icon="@Icons.Material.Filled.Map"
                                       OnClick="StartScanMappingAsync"
                                       Disabled="@(CurrentState != SLAMState.Ready || string.IsNullOrWhiteSpace(MapName))" />
                    </MudTooltip>
                }
            </MudButtonGroup>
        </div>

        @* Map Save Progress Indicator *@
        @if (CurrentState == SLAMState.SavingMap && _saveProgress >= 0)
        {
            <div class="d-flex flex-row align-items-center gap-2" style="min-width: 300px;">
                <MudProgressLinear Color="Color.Primary"
                                   Value="@_saveProgress"
                                   Class="flex-grow-1"
                                   Striped="true"
                                   Size="Size.Medium" />
                <span class="small fw-medium">@_saveProgress%</span>
                <span class="small">@(_workItemsCompleted)/@(_workItemsAdded)</span>
            </div>
        }
        <div class="flex-grow-1"></div>
        <!-- State Indicator -->
        <div class="d-flex flex-row align-items-center gap-2">
            <span class="status-chip d-inline-flex align-items-center px-3 py-1 rounded-pill small fw-medium" style="background-color: @GetStateChipBackgroundColor(CurrentState); color: @GetStateChipTextColor(CurrentState);">
                @GetStateText(CurrentState ?? SLAMState.Idle)
            </span>
        </div>
        <MudOverlay Visible="@(!CartographerClient.IsConnected)" AutoClose="false" DarkBackground Absolute />
    </div>

    <!-- Map Localization - Fill remaining space -->
    <div class="flex-grow-1 w-100" style="min-height: 0;">
        <MapLocalization @ref="MapLocalizationRef">
            <Elements>
                <MarkerDetectOverlay @ref="MarkerDetectOverlayRef" />
            </Elements>
        </MapLocalization>
    </div>
</div>

<MudThemeProvider IsDarkMode />
<MudPopoverProvider />
<MudDialogProvider />
<MudSnackbarProvider />

@code {
    private SLAMState? CurrentState { get; set; }
    private bool IsLocalizationActive => CurrentState == SLAMState.Localizing || CurrentState == SLAMState.Relocalizing;
    private bool IsScanMappingActive => CurrentState == SLAMState.ScanMapping || CurrentState == SLAMState.SavingMap;
    private MapLocalization? MapLocalizationRef { get; set; }
    private MarkerDetectOverlay? MarkerDetectOverlayRef { get; set; }

    [Inject]
    private SLAMClient CartographerClient { get; set; } = null!;

    [Inject]
    private ISnackbar Snackbar { get; set; } = null!;

    [Inject]
    private IDialogService DialogService { get; set; } = null!;

    [Inject]
    private MarkerDetectorHubClient MarkerDetectorClient { get; set; } = null!;

    // Marker detection state
    private Guid? _detectSessionId;
    private MarkersSearchRequest? _detectRequest;
    private System.Timers.Timer? _detectTimer;
    private bool _isDetecting;
    private string MapName = "";

    // Map save progress state
    private int _saveProgress = -1;
    private int _workItemsAdded;
    private int _workItemsCompleted;

    protected override async Task OnAfterRenderAsync(bool firstRender)
    {
        if (firstRender)
        {
            await CartographerClient.StartAsync();

            // Subscribe to events
            CartographerClient.StateChanged += OnStateChanged;
            CartographerClient.MapSaveProgressChanged += OnMapSaveProgressChanged;

            // Get initial state
            CurrentState = await CartographerClient.GetCurrentStateAsync();

            // If currently in localization or scan mapping state, get the current map name
            if (CurrentState == SLAMState.Relocalizing ||
                CurrentState == SLAMState.Localizing ||
                CurrentState == SLAMState.ScanMapping)
            {
                var currentMap = await CartographerClient.GetCurrentMapAsync();
                if (!string.IsNullOrEmpty(currentMap))
                {
                    MapName = currentMap;
                }
            }

            StateHasChanged();
        }
    }

    private void OnStateChanged(SLAMState state)
    {
        CurrentState = state;

        // Reset progress when not saving
        if (state != SLAMState.SavingMap)
        {
            _saveProgress = -1;
            _workItemsAdded = 0;
            _workItemsCompleted = 0;
        }

        StateHasChanged();
    }

    private void OnMapSaveProgressChanged(int workItemsAdded, int workItemsCompleted, int percentComplete)
    {
        _workItemsAdded = workItemsAdded;
        _workItemsCompleted = workItemsCompleted;
        _saveProgress = percentComplete;
        StateHasChanged();
    }

    private async Task StartLocalizationAsync()
    {
        try
        {
            if (!CartographerClient.IsConnected)
            {
                Snackbar.Add("Not connected to server", Severity.Warning);
                return;
            }

            // Get list of available maps
            var maps = await CartographerClient.ListMapsAsync();
            if (maps == null || maps.Length == 0)
            {
                Snackbar.Add("No maps available. Please create a map first.", Severity.Warning);
                return;
            }

            // Show dialog to select map
            var parameters = new DialogParameters<SelectMapDialog>
            {
                { x => x.Maps, maps }
            };

            var options = new DialogOptions
            {
                CloseOnEscapeKey = true,
                MaxWidth = MaxWidth.Small,
                FullWidth = true
            };

            var dialog = await DialogService.ShowAsync<SelectMapDialog>("Select Map for Localization", parameters, options);
            var result = await dialog.Result;

            if (result != null && !result.Canceled && result.Data is string mapName)
            {
                // Start localization with selected map
                var success = await CartographerClient.StartLocalizationAsync(mapName);
                if (success)
                {
                    MapName = mapName;
                    Snackbar.Add($"Started localization with map: {mapName}", Severity.Success);
                    StateHasChanged();
                }
                else
                {
                    Snackbar.Add($"Failed to start localization with map: {mapName}", Severity.Error);
                }
            }
        }
        catch (Exception ex)
        {
            Snackbar.Add($"Failed to start localization: {ex.Message}", Severity.Error);
        }
    }

    private async Task StopLocalizationAsync()
    {
        try
        {
            await CartographerClient.StopLocalizationAsync();
            MapName = "";
            StateHasChanged();
        }
        catch (Exception ex)
        {
            Snackbar.Add($"Failed to stop localization: {ex.Message}", Severity.Error);
        }
    }

    private async Task StartScanMappingAsync()
    {
        try
        {
            if (!CartographerClient.IsConnected)
            {
                Snackbar.Add("Not connected to server", Severity.Warning);
                return;
            }
            if (string.IsNullOrWhiteSpace(MapName))
            {
                Snackbar.Add("Please enter a map name.", Severity.Warning);
                return;
            }
            var success = await CartographerClient.StartScanMappingAsync(MapName);
            if (success)
                Snackbar.Add("Scan mapping started", Severity.Success);
            else
                Snackbar.Add("Failed to start scan mapping", Severity.Error);
        }
        catch (Exception ex)
        {
            Snackbar.Add($"Failed to start scan mapping: {ex.Message}", Severity.Error);
        }
    }

    private async Task SaveMapAsync()
    {
        try
        {
            _saveProgress = 0;
            _workItemsAdded = 0;
            _workItemsCompleted = 0;
            StateHasChanged();

            var mapPath = await CartographerClient.SaveMapAsync();
            if (!string.IsNullOrEmpty(mapPath))
                Snackbar.Add($"Map saved successfully: {mapPath}", Severity.Success);
            else
                Snackbar.Add("Map save initiated. Monitor progress above.", Severity.Info);
        }
        catch (Exception ex)
        {
            Snackbar.Add($"Failed to save map: {ex.Message}", Severity.Error);
        }
    }

    private async Task SetInitialPoseAsync()
    {
        try
        {
            var pose = MapLocalizationRef?.GetGoalPoseDto();
            if (pose == null)
            {
                Snackbar.Add("No goal pose set. Right-click on map to set robot goal, then click Initial Pose.", Severity.Warning);
                return;
            }
            await CartographerClient.SetInitialPoseAsync(pose);
            Snackbar.Add("Initial pose set from goal.", Severity.Success);
        }
        catch (Exception ex)
        {
            Snackbar.Add($"Failed to set initial pose: {ex.Message}", Severity.Error);
        }
    }

    private async Task FitViewAsync()
    {
        if (MapLocalizationRef != null)
        {
            await MapLocalizationRef.FitViewAsync();
        }
    }

    private async Task ToggleMarkerDetectAsync()
    {
        if (_isDetecting)
        {
            await StopMarkerDetectAsync();
        }
        else
        {
            await OpenMarkersSearchRequestDialogAsync();
        }
    }

    private async Task OpenMarkersSearchRequestDialogAsync()
    {
        var options = new DialogOptions
        {
            CloseOnEscapeKey = true,
            MaxWidth = MaxWidth.Medium,
            FullWidth = true
        };

        var dialog = await DialogService.ShowAsync<MarkersSearchRequestDialog>("Create Markers Search Request", options);
        var result = await dialog.Result;

        if (result is not { Canceled: false, Data: MarkersSearchRequest request })
            return;

        try
        {
            // Stop previous detection if running
            if (_isDetecting)
                await StopMarkerDetectAsync();

            // Connect and create session
            await MarkerDetectorClient.StartAsync();
            var createResult = await MarkerDetectorClient.CreateSessionAsync(request);
            if (!createResult.IsSuccess)
            {
                Snackbar.Add($"Failed to create session: {createResult.Message}", Severity.Error);
                return;
            }

            _detectSessionId = createResult.Data;
            _detectRequest = request;
            _isDetecting = true;

            // Start polling for goal
            _detectTimer = new System.Timers.Timer(1000);
            _detectTimer.Elapsed += OnDetectTimerElapsed;
            _detectTimer.Start();

            Snackbar.Add($"Marker detection started (session: {_detectSessionId})", Severity.Success);
        }
        catch (Exception ex)
        {
            Snackbar.Add($"Failed to start marker detection: {ex.Message}", Severity.Error);
        }
    }

    private async Task StopMarkerDetectAsync()
    {
        if (_detectTimer != null)
        {
            _detectTimer.Stop();
            _detectTimer.Elapsed -= OnDetectTimerElapsed;
            _detectTimer.Dispose();
            _detectTimer = null;
        }

        _isDetecting = false;
        _detectSessionId = null;
        _detectRequest = null;

        MarkerDetectOverlayRef?.Clear();

        try
        {
            await MarkerDetectorClient.StopAsync();
        }
        catch { }

        Snackbar.Add("Marker detection stopped.", Severity.Info);
    }

    private async void OnDetectTimerElapsed(object? sender, System.Timers.ElapsedEventArgs e)
    {
        if (!_detectSessionId.HasValue)
            return;

        try
        {
            var result = await MarkerDetectorClient.GetGoalAsync(_detectSessionId.Value);
            Console.WriteLine($"goal = [{result.Data.Position.X}, {result.Data.Position.Y}, {result.Data.Position.Z}] [{result.Data.Orientation.X}, {result.Data.Orientation.Y}, {result.Data.Orientation.Z}, {result.Data.Orientation.W}]");

            if (result.IsSuccess && result.Data is Pose goal && (goal.Orientation.W != 0 || goal.Orientation.Z != 0))
            {
                var points = ComputeReferencePointsWorld(goal);
                await InvokeAsync(() => MarkerDetectOverlayRef?.Update(goal, points));
            }
            else
            {
                Console.WriteLine($"reject MarkerDetectorClient.GetGoalAsync");
            }
        }
        catch (Exception ex)
        {
            Console.WriteLine(ex);
        }
    }

    private List<(double X, double Y)> ComputeReferencePointsWorld(Pose goal)
    {
        var points = new List<(double X, double Y)>();
        if (_detectRequest is not MarkersSearchRequest req)
            return points;

        var yaw = goal.Orientation.ToYawRadian();
        var cos = Math.Cos(yaw);
        var sin = Math.Sin(yaw);

        foreach (var entry in req.MarkerSearchRequests)
        {
            foreach (var pt in entry.ReferencePoints)
            {
                var wx = goal.Position.X + pt.X * cos - pt.Y * sin;
                var wy = goal.Position.Y + pt.X * sin + pt.Y * cos;
                points.Add((wx, wy));
            }
        }
        return points;
    }

    private string GetStateChipBackgroundColor(SLAMState? state)
    {
        return state switch
        {
            SLAMState.Ready => "#4caf50",
            SLAMState.Relocalizing => "#2196f3",
            SLAMState.Localizing => "#2196f3",
            SLAMState.ScanMapping => "#9c27b0",
            SLAMState.SavingMap => "#ff9800",
            SLAMState.Error => "#f44336",
            _ => "#9e9e9e"
        };
    }

    private string GetStateChipTextColor(SLAMState? state)
    {
        return "white";
    }

    private string GetStateText(SLAMState state)
    {
        return state switch
        {
            SLAMState.Idle => "Idle",
            SLAMState.Initializing => "Initializing",
            SLAMState.Ready => "Ready",
            SLAMState.Relocalizing => "Relocalizing",
            SLAMState.Localizing => "Localizing",
            SLAMState.ScanMapping => "Scan Mapping",
            SLAMState.SavingMap => "Saving Map",
            SLAMState.Error => "Error",
            _ => "Unknown"
        };
    }

    public async ValueTask DisposeAsync()
    {
        // Unsubscribe from events
        CartographerClient.StateChanged -= OnStateChanged;
        CartographerClient.MapSaveProgressChanged -= OnMapSaveProgressChanged;

        // Cleanup marker detection timer
        if (_detectTimer != null)
        {
            _detectTimer.Stop();
            _detectTimer.Elapsed -= OnDetectTimerElapsed;
            _detectTimer.Dispose();
            _detectTimer = null;
        }

        // Stop marker detector client
        if (_isDetecting)
        {
            try
            {
                await MarkerDetectorClient.StopAsync();
            }
            catch { }
        }

        _isDetecting = false;
        _detectSessionId = null;
        _detectRequest = null;
    }
}