using Microsoft.AspNetCore.Components.Authorization;
using Microsoft.AspNetCore.Identity;
using Microsoft.AspNetCore.Mvc;
using Microsoft.EntityFrameworkCore;
using MudBlazor.Services;
using NLog.Web;
using RobotNet10.CANOpen;
using RobotNet10.CustomConfiguration.Extensions;
using RobotNet10.MapManager.Extensions;
using RobotNet10.NavigationTune.Data;
using RobotNet10.NavigationTune.Execution;
using RobotNet10.NavigationTune.Extensions;
using RobotNet10.NavigationTune.Hubs;
using RobotNet10.RobotApp.Client;
using RobotNet10.RobotApp.Components;
using RobotNet10.RobotApp.Components.Account;
using RobotNet10.RobotApp.Data;
using RobotNet10.RobotApp.Detection;
using RobotNet10.RobotApp.Devices;
using RobotNet10.RobotApp.Hubs;
using RobotNet10.RobotApp.Interfaces;
using RobotNet10.RobotApp.Modules;
using RobotNet10.RobotApp.Motion;
using RobotNet10.RobotApp.Services;
using RobotNet10.RobotApp.Services.Navigation;
using RobotNet10.RobotApp.Services.Robot;
using RobotNet10.RobotApp.Services.NavigationMonitor;
using RobotNet10.RobotApp.Services.Simulation;
using RobotNet10.RobotApp.Navigation;
using RobotNet10.RobotApp.TF3;
using RobotNet10.RobotApp.Xloc;
using RobotNet10.ScriptEngine;
using RobotNet10.ScriptEngine.Helpers;
using System.Text.Json;
using Microsoft.AspNetCore.Http.Features;

// Print Ceres library information at startup
//CeresLibraryInfo.PrintLibraryInfo();

var builder = WebApplication.CreateBuilder(args);

var robotConfig = builder.Configuration["RobotConfig"];
if(string.IsNullOrEmpty(robotConfig) || !File.Exists(robotConfig))
{
    throw new Exception("RobotConfig not found");
}
builder.Configuration.AddJsonFile(robotConfig);

// Add NLog
//builder.Logging.ClearProviders();
//builder.Logging.SetMinimumLevel(LogLevel.Trace);
builder.Host.UseNLog();

// Add services to the container.
builder.Services.AddRazorComponents()
    .AddInteractiveServerComponents()
    .AddInteractiveWebAssemblyComponents()
    .AddAuthenticationStateSerialization();

builder.Services.AddCascadingAuthenticationState();
builder.Services.AddScoped<IdentityRedirectManager>();
builder.Services.AddScoped<AuthenticationStateProvider, IdentityRevalidatingAuthenticationStateProvider>();

builder.Services.AddAuthentication()
    .AddBearerToken(IdentityConstants.BearerScheme);

var connectionString = builder.Configuration.GetConnectionString("DefaultConnection") ?? throw new InvalidOperationException("Connection string 'DefaultConnection' not found.");
builder.Services.AddDbContext<ApplicationDbContext>(options => options.UseSqlite(connectionString, b => b.MigrationsAssembly("RobotNet10.RobotApp").UseQuerySplittingBehavior(QuerySplittingBehavior.SplitQuery)));
builder.Services.AddDatabaseDeveloperPageExceptionFilter();

builder.Services.AddIdentity<ApplicationUser, IdentityRole>(options =>
    {
        options.SignIn.RequireConfirmedAccount = true;
        options.Lockout.AllowedForNewUsers = false;
        options.Password.RequireNonAlphanumeric = false;
        options.Password.RequireUppercase = false;
        options.Password.RequireLowercase = false;
        options.Password.RequireDigit = false;
    })
    .AddEntityFrameworkStores<ApplicationDbContext>()
    .AddSignInManager()
    .AddDefaultTokenProviders();

builder.Services.AddSingleton<IEmailSender<ApplicationUser>, IdentityNoOpEmailSender>();

builder.Services.AddMudServices();
builder.Services.AddNavigationMenu();

builder.Services.Configure<FormOptions>(o =>
{
    o.MultipartBodyLengthLimit = 200L * 1024 * 1024;
});
builder.WebHost.ConfigureKestrel(o =>
{
    o.Limits.MaxRequestBodySize = 200L * 1024 * 1024;
});

// Add HttpClient for Blazor Server components (server-side calls into the same Kestrel app).
// BaseAddress must be loopback: using the browser's WAN/MOXA host from this machine often fails (hairpin NAT).
builder.Services.AddScoped(sp =>
{
    var navigationManager = sp.GetRequiredService<Microsoft.AspNetCore.Components.NavigationManager>();

    var handler = new HttpClientHandler
    {
        ServerCertificateCustomValidationCallback = (message, cert, chain, sslPolicyErrors) => true
    };

    var publicBase = new Uri(navigationManager.BaseUri);
    var loopbackBase = new UriBuilder(publicBase) { Host = "127.0.0.1" }.Uri;

    return new HttpClient(handler) { BaseAddress = loopbackBase };
});

builder.Services.AddControllers();
builder.Services.AddSignalR();
builder.Services.AddSingleton<DeviceHubContext>();
builder.Services.AddSingleton<OdometryHubContext>();
var scriptConnectionString = builder.Configuration.GetConnectionString("ScriptEngineConnection") ?? throw new InvalidOperationException("Connection string 'ScriptEngineConnection' not found.");
builder.Services.AddScriptEngine<ScriptEngineResource>(options => options.UseSqlite(scriptConnectionString, b => b.MigrationsAssembly("RobotNet10.RobotApp")));

var mapConnectionString = builder.Configuration.GetConnectionString("MapEditorConnection") ?? throw new InvalidOperationException("Connection string 'MapEditorConnection' not found.");
builder.Services.AddMapManager(builder.Configuration, options => options.UseSqlite(mapConnectionString, b => b.MigrationsAssembly("RobotNet10.RobotApp").UseQuerySplittingBehavior(QuerySplittingBehavior.SplitQuery)), "LayoutImage");

// var navTuneConnectionString = builder.Configuration.GetConnectionString("NavigationTuneConnection") ?? throw new InvalidOperationException("Connection string 'NavigationTuneConnection' not found.");
// builder.Services.AddNavigationTuningWithRobot(options => options.UseSqlite(navTuneConnectionString, b => b.MigrationsAssembly("RobotNet10.RobotApp").UseQuerySplittingBehavior(QuerySplittingBehavior.SplitQuery)));

// Register adapters for Navigation Tuning
// builder.Services.AddScoped<ILocalizationProvider>(sp =>
// {
//     var localization = sp.GetRequiredService<RobotNet10.RobotApp.Interfaces.ILocalization>();
//     return new LocalizationAdapter(
//         () => localization.X,
//         () => localization.Y,
//         () => localization.Theta
//     );
// });

builder.Services.AddScoped<IVelocityProvider>(sp =>
{
    var velocityController = sp.GetRequiredService<IVelocityController>();
    return new VelocityControllerAdapter(
        () => velocityController.RawVelocity,
        (linear, angular) => velocityController.SetVelocity(linear, angular),
        () => velocityController.GetModelConfidence(),
        acc => velocityController.SetAcceleration(acc),
        dec => velocityController.SetDeceleration(dec)
    );
});

// Add Custom Configuration
builder.Services.AddCustomConfiguration(builder.Configuration, "StorageConfig");

builder.Services.AddCanOpenManager();
builder.Services.AddSingleton<DeviceProvider>();
builder.Services.AddSingleton<IDeviceProvider>(sp => sp.GetRequiredService<DeviceProvider>());
builder.Services.AddHostedService(sp => sp.GetRequiredService<DeviceProvider>());

builder.Services.AddSingleton(typeof(RobotNet10.RobotApp.Services.Logger<>));
builder.Services.AddRobotSimulation();

// Core C API services (TF3 -> XLOC -> Navigation)
builder.Services.AddSingleton<TF3BufferManager>();
builder.Services.AddHostedService(sp => sp.GetRequiredService<TF3BufferManager>());

// XLOC integration is required by RobotLocalization.
builder.Services.AddSingleton<XlocIntegrationService>();
builder.Services.AddHostedService(sp => sp.GetRequiredService<XlocIntegrationService>());
builder.Services.AddHostedService<XlocAutoLocalizationHostedService>();

builder.Services.AddSingleton<NavigationIntegrationService>();
builder.Services.AddHostedService(sp => sp.GetRequiredService<NavigationIntegrationService>());

builder.Services.AddRobot();

// Motion services
builder.Services.AddSingleton<DifferentialDrive>();
builder.Services.AddSingleton<IInverseKinematics>(sp => sp.GetRequiredService<DifferentialDrive>());
builder.Services.AddSingleton<IOdometryEstimator>(sp => sp.GetRequiredService<DifferentialDrive>());
builder.Services.AddHostedService(sp => sp.GetRequiredService<DifferentialDrive>());
builder.Services.AddSingleton<OdometryService>();
builder.Services.AddHostedService(sp => sp.GetRequiredService<OdometryService>());
builder.Services.AddHostedService<OdometryBroadcastService>();

// Rotation Module services
// builder.Services.AddSingleton<RotationModuleService>();
// builder.Services.AddSingleton<IRotationModule>(sp => sp.GetRequiredService<RotationModuleService>());
// builder.Services.AddHostedService(sp => sp.GetRequiredService<RotationModuleService>());

// Lift Module: bật khi Modules:LiftModule:Enable = true
var liftModuleEnabled = builder.Configuration.GetValue<bool>("Modules:LiftModule:Enable");
if (liftModuleEnabled)
{
    builder.Services.AddSingleton<LiftModuleService>();
    builder.Services.AddSingleton<ILiftModule>(sp => sp.GetRequiredService<LiftModuleService>());
    builder.Services.AddHostedService(sp => sp.GetRequiredService<LiftModuleService>());
}
else
{
    builder.Services.AddSingleton<ILiftModule, NoOpLiftModule>();
}

// Rotation Module (vẫn dùng NoOp khi chưa bật)
builder.Services.AddSingleton<IRotationModule, NoOpRotationModule>();

builder.Services.AddSingleton<ManualControlService>();
builder.Services.AddHostedService(sp => sp.GetRequiredService<ManualControlService>());
builder.Services.AddSingleton<PS5ControllerService>();
builder.Services.AddHostedService(sp => sp.GetRequiredService<PS5ControllerService>());

// builder.Services.AddCartographer(builder.Configuration.GetSection("Cartographer"));

// builder.Services.AddSingleton<IMarkerDetector, MarkerDetector>();

// Navigation Monitor (Telemetry & Safety)
// builder.Services.AddSingleton<NavigationMonitorService>();
// builder.Services.AddHostedService(sp => sp.GetRequiredService<NavigationMonitorService>());

#if DEBUG
var dllPath = builder.Configuration["ScriptEngine:RuntimeDllFolder"] ?? "dlls";
if(!Directory.Exists(dllPath))
{
    Directory.CreateDirectory(dllPath);
}
var netCoreDir = Path.GetDirectoryName(typeof(object).Assembly.Location);
if (!string.IsNullOrEmpty(netCoreDir))
{
    var coreSrc = Path.Combine(netCoreDir, "System.Private.CoreLib.dll");
    var runtimeSrc = Path.Combine(netCoreDir, "System.Runtime.dll");
    var linqSrc = Path.Combine(netCoreDir, "System.Linq.Expressions.dll");
    var scriptSrc = typeof(RobotNet10.Script.ILogger).Assembly.Location;
    var robotAppSrc = typeof(RobotNet10.RobotApp.Script.IRobot).Assembly.Location;

    var coreDest = Path.Combine(dllPath, "System.Private.CoreLib.dll");
    var runtimeDest = Path.Combine(dllPath, "System.Runtime.dll");
    var linqDest = Path.Combine(dllPath, "System.Linq.Expressions.dll");
    var scriptDest = Path.Combine(dllPath, "RobotNet10.Script.dll");
    var robotAppDest = Path.Combine(dllPath, "RobotNet10.RobotApp.Script.dll");

    if(!File.Exists(coreDest)) File.Copy(coreSrc, coreDest);
    if(!File.Exists(runtimeDest)) File.Copy(runtimeSrc, runtimeDest);
    if(!File.Exists(linqDest)) File.Copy(linqSrc, linqDest);

    File.Copy(scriptSrc, scriptDest, true);
    File.Copy(robotAppSrc, robotAppDest, true);
}
#endif

var app = builder.Build();

await app.Services.SeedApplicationDbAsync();
await app.Services.SeedScriptEngineDbAsync();
await app.Services.SeedMapManagerAsync();

var serviceProbe = app.Services.GetService<IServiceProviderIsService>();
if (serviceProbe?.IsService(typeof(TuningDbContext)) == true)
{
    await app.Services.SeedTuningDbAsync();
}

// Configure the HTTP request pipeline.
if (app.Environment.IsDevelopment())
{
    app.UseWebAssemblyDebugging();
    app.UseMigrationsEndPoint();
}
else
{
    app.UseExceptionHandler("/Error", createScopeForErrors: true);
    // The default HSTS value is 30 days. You may want to change this for production scenarios, see https://aka.ms/aspnetcore-hsts.
    app.UseHsts();
}
app.UseStatusCodePagesWithReExecute("/not-found", createScopeForStatusCodePages: true);
app.UseHttpsRedirection();

app.UseAntiforgery();
app.MapControllers();

app.MapStaticAssets();
app.MapRazorComponents<App>()
    .AddInteractiveServerRenderMode()
    .AddInteractiveWebAssemblyRenderMode()
    .AddAdditionalAssemblies(typeof(RobotNet10.RobotApp.Client._Imports).Assembly);

// Add additional endpoints required by the Identity /Account Razor components.
app.MapAdditionalIdentityEndpoints();
app.MapScriptEngineHubs();
app.MapHub<DeviceHub>("/hubs/devices");
app.MapHub<BatteryHub>("/hubs/devices/battery");
app.MapHub<InertialMeasurementUnitHub>("/hubs/devices/inertialmeasurementunit");
app.MapHub<LidarHub>("/hubs/devices/lidar");
app.MapHub<ModbusTcpHub>("/hubs/devices/modbustcp");
// app.MapHub<RfHandleHub>("/hubs/devices/rfhandle");
app.MapHub<CameraQrHub>("/hubs/devices/cameraqr");
app.MapHub<CiA402ServoHub>("/hubs/devices/cia402servo");
app.MapHub<MotionHub>("/hubs/motion");
app.MapHub<OdometryHub>("/hubs/odometry");
app.MapHub<XlocPoseHub>("/hubs/xloc/pose");
// app.MapHub<SLAMHub>("/hubs/slam");
// app.MapHub<TuningHub>("/tuninghub");
app.MapHub<PlcControllerHub>("/hubs/plc/controller");
// app.MapHub<MarkerDetectorHub>("hubs/marker-detect");
// app.MapHub<NavigationMonitorHub>("/hubs/motion/navigation-monitor");

var logger = app.Services.GetRequiredService<ILogger<Program>>();
app.MapGet("/api/motion/ps5/status", (PS5ControllerService svc) =>
{
    return Results.Json(new { state = svc.State.ToString() });
});

app.MapPost("/api/motion/ps5/enable", async (PS5ControllerService svc) =>
{
    svc.Enable();
    return Results.Ok("PS5 controller enabled");
});

app.MapPost("/api/motion/ps5/disable", (PS5ControllerService svc) =>
{
    svc.Disable();
    return Results.Ok("PS5 controller disabled");
});

// PLC Lift module API (giao diện sửa: homing, velocity, position — 10000 = 0.01m)
var plcApi = app.MapGroup("/api/plc").DisableAntiforgery();
plcApi.MapPost("/lift/homing", (IPlcController plc) =>
{
    plc.LiftHoming();
    return Results.Ok(new { status = "ok", message = "Lift homing triggered" });
});
plcApi.MapPost("/lift/velocity", async (HttpRequest req, IPlcController plc) =>
{
    var body = await req.ReadFromJsonAsync<LiftVelocityRequest>();
    if (body == null) return Results.BadRequest(new { status = "error", message = "Body required: { up: bool, down: bool }" });
    plc.SetLiftVelocity(body.Up, body.Down);
    return Results.Ok(new { status = "ok", up = body.Up, down = body.Down });
});
plcApi.MapPost("/lift/position", async (HttpRequest req, IPlcController plc) =>
{
    var body = await req.ReadFromJsonAsync<LiftPositionRequest>();
    if (body == null) return Results.BadRequest(new { status = "error", message = "Body required: { position: int } (10000 = 0.01m)" });
    plc.SetLiftPositionAndGo(body.Position);
    return Results.Ok(new { status = "ok", position = body.Position });
});

// XLOC Control API
var xlocApi = app.MapGroup("/api/xloc").DisableAntiforgery();

// Mapping control
xlocApi.MapPost("/mapping/start", ([FromServices] RobotNet10.RobotApp.Xloc.XlocIntegrationService service) => {
    return service.StartMapping() 
        ? Results.Ok(new { status = "success", message = "Mapping started" })
        : Results.BadRequest(new { status = "error", message = "Failed to start mapping" });
});

xlocApi.MapPost("/mapping/stop", async (HttpRequest request, [FromServices] RobotNet10.RobotApp.Xloc.XlocIntegrationService service) => {
    var body = await request.ReadFromJsonAsync<Dictionary<string, string>>();
    var mapFile = body?["map_file_path"] ?? "/home/mic-733ao/.local/share/xloc/resources/maps";
    
    return service.StopMapping(mapFile)
        ? Results.Ok(new { status = "success", message = "Mapping stopped", map_file = mapFile })
        : Results.BadRequest(new { status = "error", message = "Failed to stop mapping" });
});

// Localization control
xlocApi.MapPost("/localization/start", ([FromServices] RobotNet10.RobotApp.Xloc.XlocIntegrationService service) => {
    return service.StartLocalization()
        ? Results.Ok(new { status = "success", message = "Localization started" })
        : Results.BadRequest(new { status = "error", message = "Failed to start localization" });
});

xlocApi.MapPost("/localization/stop", ([FromServices] RobotNet10.RobotApp.Xloc.XlocIntegrationService service) => {
    return service.StopLocalization()
        ? Results.Ok(new { status = "success", message = "Localization stopped" })
        : Results.BadRequest(new { status = "error", message = "Failed to stop localization" });
});

// Update map control (only allowed when in localization mode)
xlocApi.MapPost("/update-map/start", ([FromServices] RobotNet10.RobotApp.Xloc.XlocIntegrationService service) => {
    return service.StartUpdateMap()
        ? Results.Ok(new { status = "success", message = "Map update started" })
        : Results.BadRequest(new { status = "error", message = "Failed to start map update. Robot must be in localization mode." });
});

xlocApi.MapPost("/update-map/stop", async (HttpRequest request, [FromServices] RobotNet10.RobotApp.Xloc.XlocIntegrationService service) => {
    var body = await request.ReadFromJsonAsync<Dictionary<string, object>>();
    bool saveUpdatedMap = true;
    if (body != null && body.TryGetValue("save_updated_map", out var saveVal))
    {
        if (saveVal is bool saveBool)
            saveUpdatedMap = saveBool;
        else if (saveVal is JsonElement jsonElem && jsonElem.ValueKind == JsonValueKind.True)
            saveUpdatedMap = true;
        else if (saveVal is JsonElement jsonElem2 && jsonElem2.ValueKind == JsonValueKind.False)
            saveUpdatedMap = false;
    }
    
    return service.StopUpdateMap(saveUpdatedMap)
        ? Results.Ok(new { status = "success", message = "Map update stopped", save_updated_map = saveUpdatedMap })
        : Results.BadRequest(new { status = "error", message = "Failed to stop map update" });
});

// Reset SLAM error
xlocApi.MapPost("/reset-error", ([FromServices] RobotNet10.RobotApp.Xloc.XlocIntegrationService service) => {
    return service.ResetSlamError()
        ? Results.Ok(new { status = "success", message = "SLAM error reset successfully" })
        : Results.BadRequest(new { status = "error", message = "Failed to reset SLAM error" });
});

// Initial pose control
xlocApi.MapPost("/pose/initial", async (HttpRequest request, [FromServices] RobotNet10.RobotApp.Xloc.XlocIntegrationService service) => {
    var body = await request.ReadFromJsonAsync<Dictionary<string, double>>();
    if (body == null)
        return Results.BadRequest(new { status = "error", message = "Invalid request body" });

    if (!body.TryGetValue("x", out var x) || !body.TryGetValue("y", out var y))
        return Results.BadRequest(new { status = "error", message = "x and y are required" });

    var z = body.TryGetValue("z", out var zVal) ? zVal : 0.0;
    var roll = body.TryGetValue("roll", out var rollVal) ? rollVal : 0.0;
    var pitch = body.TryGetValue("pitch", out var pitchVal) ? pitchVal : 0.0;
    var yaw = body.TryGetValue("yaw", out var yawVal) ? yawVal : 0.0;

    return service.SetInitialPose(x, y, z, roll, pitch, yaw)
        ? Results.Ok(new { status = "success", message = "Initial pose set", position = new { x, y, z }, orientation = new { roll, pitch, yaw } })
        : Results.BadRequest(new { status = "error", message = "Failed to set initial pose" });
});

// Map management
// xlocApi.MapPost("/map/activate", async (HttpRequest request, [FromServices] RobotNet10.RobotApp.Xloc.XlocIntegrationService service) => {
//     var body = await request.ReadFromJsonAsync<Dictionary<string, string>>();
//     var mapFile = body?["map_file_path"];
    
//     if (string.IsNullOrEmpty(mapFile))
//         return Results.BadRequest(new { status = "error", message = "map_file_path required" });

//     // UI may send only the map folder name (e.g. "map_20260307_103646").
//     // Resolve it to an absolute pbstream path so native XLOC doesn't depend on HOME defaults (e.g. /root vs /home/...).
//     var resolved = XlocPaths.ResolvePbstreamPath(mapFile);
//     if (resolved == null)
//         return Results.BadRequest(new
//         {
//             status = "error",
//             message = "Failed to resolve pbstream path for map_file_path. " +
//                       "Expected either an absolute .pbstream file, or a map folder name under: " +
//                       XlocPaths.GetMapsDirectory()
//         });

//     return service.ActivateMap(resolved)
//         ? Results.Ok(new { status = "success", message = "Map activated", map_file = resolved })
//         : Results.BadRequest(new { status = "error", message = $"Failed to activate map pbstream: {resolved}" });
// });

xlocApi.MapPost("/map/activate", async (HttpRequest request, [FromServices] RobotNet10.RobotApp.Xloc.XlocIntegrationService service) => {
    var body = await request.ReadFromJsonAsync<Dictionary<string, string>>();
    var mapFile = body?["map_file_path"];
    
    if (string.IsNullOrEmpty(mapFile))
        return Results.BadRequest(new { status = "error", message = "map_file_path required" });
    
    return service.ActivateMap(mapFile)
        ? Results.Ok(new { status = "success", message = "Map activated", map_file = mapFile })
        : Results.BadRequest(new { status = "error", message = "Failed to activate map" });
});


// Grid map endpoints
xlocApi.MapGet("/gridmap/static", ([FromServices] RobotNet10.RobotApp.Xloc.XlocIntegrationService service, [FromServices] ILogger<Program> logger, [FromQuery] bool reload = false) => {
    // Log when map is being requested (to track if it's called after initial pose)
    // logger.LogWarning("🔍 API /gridmap/static called (reload={Reload})", reload);
    
    var grid = service.GetStaticGridMap(reload);
    if (grid == null)
        return Results.NotFound(new { status = "error", message = "No static grid map available" });
    
    // Prefer origin from active map's YAML (Map frame in World frame); native often returns (0,0,0)
    double ox = grid.Origin.X, oy = grid.Origin.Y, oz = grid.Origin.Z;
    var diag = service.GetDiagnostics();
    if (!string.IsNullOrEmpty(diag?.CurrentActiveMap))
    {
        var (yx, yy, yz) = ReadOriginFromMapYaml(diag.CurrentActiveMap);
        ox = yx; oy = yy; oz = yz;
    }
    
    //logger.LogWarning("🔍 API /gridmap/static returning origin: ({X}, {Y}, {Z})", ox, oy, oz);
    
    return Results.Ok(new {
        status = "success",
        resolution = grid.Resolution,
        width = grid.Width,
        height = grid.Height,
        origin = new {
            x = ox,
            y = oy,
            z = oz,
            qx = grid.Origin.Qx,
            qy = grid.Origin.Qy,
            qz = grid.Origin.Qz,
            qw = grid.Origin.Qw
        },
        data = Convert.ToBase64String(grid.Data),
        frameId = grid.FrameId
    });
});

xlocApi.MapGet("/gridmap/online", ([FromServices] RobotNet10.RobotApp.Xloc.XlocIntegrationService service) => {
    var grid = service.GetOnlineGridMap();
    if (grid == null)
        return Results.NotFound(new { status = "error", message = "No online grid map available" });
    
    // CRITICAL: Use origin directly from online grid map to ensure alignment with LIDAR
    // Do NOT override with YAML origin during mapping, as online map origin may change
    // and must match the actual grid data for proper rendering
    double ox = grid.Origin.X, oy = grid.Origin.Y, oz = grid.Origin.Z;
    
    // Only use YAML origin for static maps, not for online maps during mapping
    // Online map origin should come directly from xloc_get_online_grid_map()
    // var diag = service.GetDiagnostics();
    // if (!string.IsNullOrEmpty(diag?.CurrentActiveMap))
    // {
    //     var (yx, yy, yz) = ReadOriginFromMapYaml(diag.CurrentActiveMap);
    //     ox = yx; oy = yy; oz = yz;
    // }
    
    return Results.Ok(new {
        status = "success",
        resolution = grid.Resolution,
        width = grid.Width,
        height = grid.Height,
        origin = new {
            x = ox,
            y = oy,
            z = oz,
            qx = grid.Origin.Qx,
            qy = grid.Origin.Qy,
            qz = grid.Origin.Qz,
            qw = grid.Origin.Qw
        },
        data = Convert.ToBase64String(grid.Data),
        frameId = grid.FrameId
    });
});

// List available maps
xlocApi.MapGet("/maps/list", () => {
    try
    {
        var mapsDir = XlocPaths.GetMapsDirectory();
        if (!Directory.Exists(mapsDir))
            return Results.Ok(new { status = "success", maps = Array.Empty<string>() });

        var maps = Directory.GetDirectories(mapsDir)
            .Select(dir => Path.GetFileName(dir))
            .Where(name => !string.IsNullOrEmpty(name) && name != "tmp")
            .OrderBy(name => name)
            .ToArray();

        return Results.Ok(new { status = "success", maps });
    }
    catch (Exception ex)
    {
        return Results.BadRequest(new { status = "error", message = ex.Message });
    }
});

// Load map from folder
xlocApi.MapGet("/maps/load/{mapName}", (string mapName) => {
    try
    {
        var mapsDir = XlocPaths.GetMapsDirectory();
        var mapFolder = Path.Combine(mapsDir, mapName);
        
        if (!Directory.Exists(mapFolder))
            return Results.NotFound(new { status = "error", message = $"Map '{mapName}' not found" });

        // Find YAML file
        var yamlFiles = Directory.GetFiles(mapFolder, "*.yaml");
        if (yamlFiles.Length == 0)
            return Results.BadRequest(new { status = "error", message = "YAML file not found" });

        var yamlFile = yamlFiles[0];
        var yamlContent = File.ReadAllText(yamlFile);
        
        // Parse YAML (simple parsing)
        float resolution = 0.05f;
        double originX = 0.0, originY = 0.0, originZ = 0.0;
        string? imageFile = null;
        
        foreach (var line in yamlContent.Split('\n'))
        {
            var trimmed = line.Trim();
            if (trimmed.StartsWith("resolution:"))
            {
                if (float.TryParse(trimmed.Substring("resolution:".Length).Trim(), out float res))
                    resolution = res;
            }
            else if (trimmed.StartsWith("origin:"))
            {
                var originStr = trimmed.Substring("origin:".Length).Trim();
                if (originStr.StartsWith("[") && originStr.EndsWith("]"))
                {
                    var coords = originStr.Substring(1, originStr.Length - 2).Split(',');
                    if (coords.Length >= 1) double.TryParse(coords[0].Trim(), out originX);
                    if (coords.Length >= 2) double.TryParse(coords[1].Trim(), out originY);
                    if (coords.Length >= 3) double.TryParse(coords[2].Trim(), out originZ);
                }
            }
            else if (trimmed.StartsWith("image:"))
            {
                imageFile = trimmed.Substring("image:".Length).Trim();
            }
        }

        // Find PGM file - always prefer .pgm over .png from YAML
        string? pgmFile = null;
        
        // First try to find .pgm file directly
        var pgmFiles = Directory.GetFiles(mapFolder, "*.pgm");
        if (pgmFiles.Length > 0)
        {
            pgmFile = pgmFiles[0];
        }
        else if (imageFile != null)
        {
            // If YAML specifies .png, try replacing extension with .pgm
            var pgmFromYaml = Path.Combine(mapFolder, Path.ChangeExtension(imageFile, ".pgm"));
            if (File.Exists(pgmFromYaml))
                pgmFile = pgmFromYaml;
            else
            {
                // Last resort: use image file from YAML (might be .png)
                var imageFromYaml = Path.Combine(mapFolder, imageFile);
                if (File.Exists(imageFromYaml) && imageFile.EndsWith(".pgm", StringComparison.OrdinalIgnoreCase))
                    pgmFile = imageFromYaml;
            }
        }
        
        if (pgmFile == null)
            return Results.BadRequest(new { status = "error", message = "PGM file not found" });

        // Read PGM file and convert to occupancy grid
        var (width, height, data) = ReadPgmFile(pgmFile);
        
        // Convert to base64
        var dataBase64 = Convert.ToBase64String(data);

        return Results.Ok(new {
            status = "success",
            mapName,
            width,
            height,
            resolution,
            origin = new {
                x = originX,
                y = originY,
                z = originZ,
                qx = 0.0,
                qy = 0.0,
                qz = 0.0,
                qw = 1.0
            },
            data = dataBase64
        });
    }
    catch (Exception ex)
    {
        return Results.BadRequest(new { status = "error", message = ex.Message });
    }
});

// Download map folder as .zip (single top-level folder matching map name inside the archive on re-import)
xlocApi.MapGet("/maps/download/{mapName}", (string mapName) => XlocMapFileEndpoints.DownloadMapAsZip(mapName));

// Import map from .zip into maps directory
xlocApi.MapPost("/maps/import", async (HttpRequest request) => await XlocMapFileEndpoints.ImportMapFromZipAsync(request));

// Delete map folder (explicit path avoids conflicting with /maps/list, /maps/load/...)
xlocApi.MapDelete("/maps/delete/{mapName}", (string mapName) => XlocMapFileEndpoints.DeleteMapFolder(mapName));

// Helper: read origin (Map frame in World frame) from active map's YAML
static (double x, double y, double z) ReadOriginFromMapYaml(string mapName)
{
    try
    {
        var mapsDir = XlocPaths.GetMapsDirectory();
        var folderName = Path.GetFileName(mapName.TrimEnd(Path.DirectorySeparatorChar, '/'));
        if (string.IsNullOrEmpty(folderName)) return (0, 0, 0);
        var mapFolder = Path.Combine(mapsDir, folderName);
        if (!Directory.Exists(mapFolder)) return (0, 0, 0);
        var yamlFiles = Directory.GetFiles(mapFolder, "*.yaml");
        if (yamlFiles.Length == 0) return (0, 0, 0);
        var content = File.ReadAllText(yamlFiles[0]);
        double ox = 0, oy = 0, oz = 0;
        foreach (var line in content.Split('\n'))
        {
            var t = line.Trim();
            if (!t.StartsWith("origin:")) continue;
            var s = t.Substring("origin:".Length).Trim();
            if (s.StartsWith("[") && s.EndsWith("]"))
            {
                var parts = s.Substring(1, s.Length - 2).Split(',');
                if (parts.Length >= 1) double.TryParse(parts[0].Trim(), out ox);
                if (parts.Length >= 2) double.TryParse(parts[1].Trim(), out oy);
                if (parts.Length >= 3) double.TryParse(parts[2].Trim(), out oz);
            }
            break;
        }
        return (ox, oy, oz);
    }
    catch { return (0, 0, 0); }
}

// Helper function to read PGM file
static (uint width, uint height, byte[] data) ReadPgmFile(string pgmPath)
{
    // Read entire file as bytes first
    var allBytes = File.ReadAllBytes(pgmPath);
    
    // Parse header manually to avoid StreamReader buffering issues
    int pos = 0;
    
    // Read magic number (P5\n)
    string magic = "";
    while (pos < allBytes.Length && allBytes[pos] != '\n')
    {
        magic += (char)allBytes[pos];
        pos++;
    }
    pos++; // Skip newline
    
    magic = magic.Trim();
    if (magic != "P5")
        throw new InvalidDataException($"Not a P5 PGM file, got: {magic}");
    
    // Skip comments and read dimensions
    uint width = 0, height = 0;
    bool gotDimensions = false;
    
    while (pos < allBytes.Length && !gotDimensions)
    {
        // Skip whitespace
        while (pos < allBytes.Length && (allBytes[pos] == ' ' || allBytes[pos] == '\t' || allBytes[pos] == '\r' || allBytes[pos] == '\n'))
            pos++;
        
        // Check for comment
        if (pos < allBytes.Length && allBytes[pos] == '#')
        {
            // Skip comment line
            while (pos < allBytes.Length && allBytes[pos] != '\n')
                pos++;
            continue;
        }
        
        // Read dimensions line
        string line = "";
        while (pos < allBytes.Length && allBytes[pos] != '\n')
        {
            line += (char)allBytes[pos];
            pos++;
        }
        pos++; // Skip newline
        
        var parts = line.Trim().Split(new[] { ' ', '\t' }, StringSplitOptions.RemoveEmptyEntries);
        if (parts.Length >= 2 && uint.TryParse(parts[0], out width) && uint.TryParse(parts[1], out height))
        {
            gotDimensions = true;
        }
    }
    
    if (!gotDimensions || width == 0 || height == 0)
        throw new InvalidDataException($"Invalid PGM dimensions: {width}x{height}");
    
    // Read max value
    while (pos < allBytes.Length && (allBytes[pos] == ' ' || allBytes[pos] == '\t' || allBytes[pos] == '\r' || allBytes[pos] == '\n'))
        pos++;
    
    string maxValStr = "";
    while (pos < allBytes.Length && allBytes[pos] != '\n')
    {
        maxValStr += (char)allBytes[pos];
        pos++;
    }
    pos++; // Skip newline
    
    if (!int.TryParse(maxValStr.Trim(), out int maxVal))
        throw new InvalidDataException($"Invalid max value in PGM: {maxValStr}");
    
    // Read binary data
    int dataSize = (int)(width * height);
    if (pos + dataSize > allBytes.Length)
        throw new InvalidDataException($"Not enough data: expected {dataSize} bytes at position {pos}, file size {allBytes.Length}");
    
    var rawData = new byte[dataSize];
    Array.Copy(allBytes, pos, rawData, 0, dataSize);
    
    // Convert grayscale to occupancy grid format (int8_t):
    // - 0 (black/occupied) -> 100
    // - 255 (white/free) -> 0  
    // - 205 (unknown) -> -1 (255 in unsigned byte)
    // IMPORTANT: Flip Y axis because PGM has row 0 at top, but ROS map has cell (0,0) at bottom-left
    var occupancyData = new byte[dataSize];
    for (int row = 0; row < height; row++)
    {
        for (int col = 0; col < width; col++)
        {
            // PGM index: row 0 = top
            int pgmIndex = row * (int)width + col;
            // ROS index: cell (x=col, y=0) at bottom → flip Y
            int rosIndex = ((int)height - 1 - row) * (int)width + col;
            
            var pixel = rawData[pgmIndex];
            if (pixel >= 205) // Unknown (typically 205 in ROS maps)
                occupancyData[rosIndex] = 255; // Will be converted to -1 in JS
            else if (pixel < 128) // Dark = occupied
                occupancyData[rosIndex] = 100; // Occupied
            else // Light = free
                occupancyData[rosIndex] = 0; // Free
        }
    }
    
    return (width, height, occupancyData);
}

// Map upload endpoint (kept for backward compatibility)
xlocApi.MapPost("/map/upload", async (HttpRequest request, [FromServices] RobotNet10.RobotApp.Xloc.XlocIntegrationService service) => {
    try
    {
        if (!request.HasFormContentType)
            return Results.BadRequest(new { status = "error", message = "Request must be multipart/form-data" });

        var form = await request.ReadFormAsync();
        var file = form.Files["mapFile"];
        if (file == null || file.Length == 0)
            return Results.BadRequest(new { status = "error", message = "No file uploaded" });

        // Validate PNG
        if (!file.FileName.EndsWith(".png", StringComparison.OrdinalIgnoreCase))
            return Results.BadRequest(new { status = "error", message = "File must be a PNG image" });

        // Parse origin and resolution from form
        if (!float.TryParse(form["resolution"], out float resolution))
            resolution = 0.05f; // default 5cm

        if (!double.TryParse(form["originX"], out double originX))
            originX = 0.0;
        if (!double.TryParse(form["originY"], out double originY))
            originY = 0.0;
        if (!double.TryParse(form["originZ"], out double originZ))
            originZ = 0.0;
        if (!double.TryParse(form["originQx"], out double originQx))
            originQx = 0.0;
        if (!double.TryParse(form["originQy"], out double originQy))
            originQy = 0.0;
        if (!double.TryParse(form["originQz"], out double originQz))
            originQz = 0.0;
        if (!double.TryParse(form["originQw"], out double originQw))
            originQw = 1.0;

        // Save file to temp directory
        var uploadsDir = Path.Combine(Path.GetTempPath(), "xloc_maps");
        Directory.CreateDirectory(uploadsDir);
        var filePath = Path.Combine(uploadsDir, file.FileName);
        
        using (var stream = File.Create(filePath))
        {
            await file.CopyToAsync(stream);
        }

        // Activate map with origin
        var originPose = new { x = originX, y = originY, z = originZ, qx = originQx, qy = originQy, qz = originQz, qw = originQw };
        
        // Note: xloc may need the map in a specific format, so we save the PNG and metadata
        // The actual activation might need conversion to xloc's map format
        var metadata = new {
            filePath,
            resolution,
            origin = originPose
        };

        return Results.Ok(new {
            status = "success",
            message = "Map uploaded successfully",
            filePath,
            resolution,
            origin = originPose
        });
    }
    catch (Exception ex)
    {
        return Results.BadRequest(new { status = "error", message = ex.Message });
    }
});

// Pose data
xlocApi.MapGet("/pose/current", ([FromServices] RobotNet10.RobotApp.Xloc.XlocIntegrationService service) => {
    var pose = service.GetCurrentPose();
    return pose.HasValue 
        ? Results.Ok(new { 
            status = "success",
            pose = new {
                position = new { x = pose.Value.x, y = pose.Value.y, z = pose.Value.z },
                orientation = new { x = pose.Value.qx, y = pose.Value.qy, z = pose.Value.qz, w = pose.Value.qw }
            }
        })
        : Results.NotFound(new { status = "error", message = "No pose available" });
});

xlocApi.MapGet("/pose/current2d", ([FromServices] RobotNet10.RobotApp.Xloc.XlocIntegrationService service) => {
    var pose = service.GetCurrentPose2D();
    return pose.HasValue
        ? Results.Ok(new {
            status = "success",
            pose = new { x = pose.Value.x, y = pose.Value.y, yaw = pose.Value.yaw }
        })
        : Results.NotFound(new { status = "error", message = "No pose available" });
});

// Diagnostics data
xlocApi.MapGet("/diagnostics", ([FromServices] RobotNet10.RobotApp.Xloc.XlocIntegrationService service) => {
    var diagnostics = service.GetDiagnostics();
    return diagnostics != null
        ? Results.Ok(new {
            status = "success",
            diagnostics = new {
                header = new {
                    seq = diagnostics.HeaderSeq,
                    stamp = new {
                        sec = diagnostics.HeaderStampSec,
                        nsec = diagnostics.HeaderStampNsec
                    },
                    frameId = diagnostics.HeaderFrameId
                },
                xlocState = diagnostics.XlocState,
                stateString = diagnostics.StateString,
                currentActiveMap = diagnostics.CurrentActiveMap,
                reliability = diagnostics.Reliability,
                matchingScore = diagnostics.MatchingScore
            }
        })
        : Results.NotFound(new { status = "error", message = "No diagnostics available" });
});

// Laser scan data (sampled at 1 degree intervals)
xlocApi.MapGet("/laser/scan", ([FromServices] RobotNet10.RobotApp.Xloc.XlocIntegrationService service) => {
    var laserData = service.GetSampledLaserScan();
    if (laserData == null || laserData.Count == 0)
        return Results.NotFound(new { status = "error", message = "No laser scan available" });
    
    var points = laserData.Select(p => new { angle = p.angle, range = p.range }).ToArray();
    
    return Results.Ok(new {
        status = "success",
        pointCount = points.Length,
        points = points,
        timestamp = DateTime.UtcNow
    });
});

// All 3 lidars combined with mode support (full = all points, minimal = 120 sampled points)
xlocApi.MapGet("/laser/scan/all", (
    [FromServices] RobotNet10.RobotApp.Xloc.XlocIntegrationService service,
    [FromQuery] string? mode1 = "minimal",
    [FromQuery] string? mode2 = "minimal",
    [FromQuery] string? mode3 = "minimal") => {
    
    // Get laser data based on mode (full = all points, minimal = ~120 sampled points)
    var laser1 = mode1 == "full" ? service.GetFullLaserScan() : service.GetSampledLaserScan();
    var laser2 = mode2 == "full" ? service.GetFullLaserScan2() : service.GetSampledLaserScan2();
    var laser3 = mode3 == "full" ? service.GetFullLaserScan3() : service.GetSampledLaserScan3();
    
    return Results.Ok(new {
        status = "success",
        timestamp = DateTime.UtcNow,
        lidar1 = laser1 != null ? new {
            pointCount = laser1.Count,
            points = laser1.Select(p => new { angle = p.angle, range = p.range }).ToArray()
        } : null,
        lidar2 = laser2 != null ? new {
            pointCount = laser2.Count,
            points = laser2.Select(p => new { angle = p.angle, range = p.range }).ToArray()
        } : null,
        lidar3 = laser3 != null ? new {
            pointCount = laser3.Count,
            points = laser3.Select(p => new { angle = p.angle, range = p.range }).ToArray()
        } : null
    });
});

// Velocity data (OdomVel and CmdVel)
xlocApi.MapGet("/velocity", ([FromServices] OdometryService odometryService, [FromServices] ManualControlService manualControlService, [FromServices] PS5ControllerService ps5ControllerService, [FromServices] RobotNet10.RobotApp.Navigation.NavigationIntegrationService navigationService) => {
    try
    {
        // Get odometry velocity directly from OdometryService (raw encoder odometry, not EKF filtered)
        var odom = odometryService.CurrentOdometry;
        double odomLinearVel = odom.Twist.Twist.Linear.X;
        double odomAngularVel = odom.Twist.Twist.Angular.Z;
        
        // Get command velocity from the active control service
        // Priority: PS5Controller > ManualControl > Navigation twist
        double cmdLinearVel = 0.0;
        double cmdAngularVel = 0.0;
        
        // Check PS5Controller first (if active)
        if (ps5ControllerService.State == PS5ControllerState.Active)
        {
            var ps5Twist = ps5ControllerService.CurrentTwist;
            cmdLinearVel = ps5Twist.Linear.X;
            cmdAngularVel = ps5Twist.Angular.Z;
        }
        // Otherwise check ManualControl (if active)
        else if (manualControlService.State == ManualControlState.Active)
        {
            var manualTwist = manualControlService.CurrentTwist;
            cmdLinearVel = manualTwist.Linear.X;
            cmdAngularVel = manualTwist.Angular.Z;
        }
        // Otherwise try to get navigation twist (from GetTwist in navigation system)
        else
        {
            var navTwist = navigationService.GetTwist();
            if (navTwist.HasValue)
            {
                cmdLinearVel = navTwist.Value.x;
                cmdAngularVel = navTwist.Value.theta;
            }
            else
            {
                // Fallback: last known value from ManualControl
                var manualTwist = manualControlService.CurrentTwist;
                cmdLinearVel = manualTwist.Linear.X;
                cmdAngularVel = manualTwist.Angular.Z;
            }
        }
        
        return Results.Ok(new {
            status = "success",
            odomVel = new {
                linear = odomLinearVel,
                angular = odomAngularVel
            },
            cmdVel = new {
                linear = cmdLinearVel,
                angular = cmdAngularVel
            }
        });
    }
    catch (Exception ex)
    {
        return Results.BadRequest(new { status = "error", message = ex.Message });
    }
});

// Diagnostics endpoint
// DISABLED: GetDiagnostics method removed from XlocIntegrationService
/*
app.MapGet("/api/xloc/diagnostics", ([FromServices] XlocIntegrationService xlocService) =>
{
    var diagnostics = xlocService.GetDiagnostics();
    if (diagnostics.HasValue)
    {
        return Results.Ok(new { status = "success", diagnostics = new { 
            state = diagnostics.Value.state,
            map_name = diagnostics.Value.mapName,
            reliability = diagnostics.Value.reliability,
            matching_score = diagnostics.Value.matchingScore
        }});
    }
    return Results.Ok(new { status = "error", message = "No diagnostics available" });
});
*/

// Register XLOC SignalR Hub
// app.MapHub<RobotNet10.RobotApp.Hubs.XlocHub>("/hubs/xloc");

// Navigation Control API
var navApi = app.MapGroup("/api/navigation").DisableAntiforgery();

// Navigation control - Move to goal
navApi.MapPost("/move_to", async (HttpRequest request, [FromServices] RobotNet10.RobotApp.Navigation.NavigationIntegrationService service) => {
    try
    {
        var body = await request.ReadFromJsonAsync<JsonElement>();
        if (body.ValueKind != JsonValueKind.Object)
            return Results.BadRequest(new { status = "error", message = "Invalid request body" });

        if (!body.TryGetProperty("x", out var xProp) || !body.TryGetProperty("y", out var yProp))
            return Results.BadRequest(new { status = "error", message = "x and y are required" });

        double x = xProp.GetDouble();
        double y = yProp.GetDouble();
        double z = body.TryGetProperty("z", out var zProp) ? zProp.GetDouble() : 0.0;
        
        // Orientation (quaternion or euler angles)
        double qx = 0.0, qy = 0.0, qz = 0.0, qw = 1.0;
        if (body.TryGetProperty("qx", out var qxProp) && body.TryGetProperty("qy", out var qyProp) && 
            body.TryGetProperty("qz", out var qzProp) && body.TryGetProperty("qw", out var qwProp))
        {
            qx = qxProp.GetDouble();
            qy = qyProp.GetDouble();
            qz = qzProp.GetDouble();
            qw = qwProp.GetDouble();
        }
        else if (body.TryGetProperty("yaw", out var yawProp))
        {
            // Convert yaw to quaternion
            double yaw = yawProp.GetDouble();
            qw = Math.Cos(yaw / 2.0);
            qz = Math.Sin(yaw / 2.0);
        }

        string frameId = body.TryGetProperty("frame_id", out var frameIdProp) ? frameIdProp.GetString() ?? "map" : "map";
        double xyTolerance = body.TryGetProperty("xy_tolerance", out var xyTolProp) ? xyTolProp.GetDouble() : 0.1;
        double yawTolerance = body.TryGetProperty("yaw_tolerance", out var yawTolProp) ? yawTolProp.GetDouble() : 0.1;

        // Wrap synchronous native calls in Task.Run to avoid blocking the request thread
        bool result = await Task.Run(() => service.MoveTo(x, y, z, qx, qy, qz, qw, frameId, xyTolerance, yawTolerance));
        return result
            ? Results.Ok(new { status = "success", message = "Move to goal sent", goal = new { x, y, z, qx, qy, qz, qw, frameId } })
            : Results.BadRequest(new { status = "error", message = "Failed to send move to goal" });
    }
    catch (Exception ex)
    {
        return Results.BadRequest(new { status = "error", message = $"Failed to parse request: {ex.Message}" });
    }
});

// Navigation control - Move to goal with order
navApi.MapPost("/move_to_order", async (HttpRequest request, [FromServices] RobotNet10.RobotApp.Navigation.NavigationIntegrationService service, [FromServices] ILogger<Program> logger) => {
    try
    {
        logger.LogInformation("Received move_to_order request");
        var body = await request.ReadFromJsonAsync<JsonElement>();
        if (body.ValueKind != JsonValueKind.Object)
            return Results.BadRequest(new { status = "error", message = "Invalid request body" });

        if (!body.TryGetProperty("x", out var xProp) || !body.TryGetProperty("y", out var yProp))
            return Results.BadRequest(new { status = "error", message = "x and y are required" });

        // Order handle - can be provided as:
        // 1. order_handle: IntPtr value (number or hex string)
        // 2. order_data: OrderData object (will be converted to OrderHandle)
        IntPtr orderHandle = IntPtr.Zero;
        OrderHandle? orderHandleWrapper = null;

        if (body.TryGetProperty("order_handle", out var orderHandleProp))
        {
            if (orderHandleProp.ValueKind == JsonValueKind.String)
            {
                string orderHandleStr = orderHandleProp.GetString() ?? "0";
                // Try parse as hex if starts with 0x, otherwise as decimal
                if (orderHandleStr.StartsWith("0x", StringComparison.OrdinalIgnoreCase))
                {
                    orderHandle = new IntPtr(Convert.ToInt64(orderHandleStr, 16));
                }
                else
                {
                    orderHandle = new IntPtr(Convert.ToInt64(orderHandleStr));
                }
            }
            else if (orderHandleProp.ValueKind == JsonValueKind.Number)
            {
                orderHandle = new IntPtr(orderHandleProp.GetInt64());
            }

            // Validate order handle is a reasonable pointer value (not too small)
            // Typical valid pointers on 64-bit systems are > 0x1000
            if (orderHandle != IntPtr.Zero && orderHandle.ToInt64() < 0x1000)
            {
                return Results.BadRequest(new { 
                    status = "error", 
                    message = $"Invalid order_handle value: {orderHandle.ToInt64()}. Order handle must be a valid pointer to an Order object created by the navigation system. Use a value > 0x1000 or provide order_data instead.",
                    order_handle = orderHandle.ToInt64()
                });
            }

            if (orderHandle != IntPtr.Zero)
            {
                orderHandleWrapper = new OrderHandle(orderHandle);
            }
        }
        else if (body.TryGetProperty("order_data", out var orderDataProp))
        {
            // Try to deserialize OrderData from JSON
            try
            {
                var orderData = JsonSerializer.Deserialize<OrderData>(orderDataProp.GetRawText());
                if (orderData != null)
                {
                    // Note: Currently, we cannot create OrderHandle from OrderData without C API support
                    // For now, return error suggesting to use order_handle
                    return Results.BadRequest(new { 
                        status = "error", 
                        message = "order_data is provided but OrderHandle creation from OrderData is not yet supported. Please use order_handle (IntPtr) instead, or implement C API function to create OrderHandle from Order data."
                    });
                }
            }
            catch (Exception ex)
            {
                return Results.BadRequest(new { 
                    status = "error", 
                    message = $"Failed to parse order_data: {ex.Message}"
                });
            }
        }

        if (orderHandle == IntPtr.Zero && (orderHandleWrapper == null || !orderHandleWrapper.IsValid))
            return Results.BadRequest(new { status = "error", message = "Either order_handle or order_data is required" });

        double x = xProp.GetDouble();
        double y = yProp.GetDouble();
        double z = body.TryGetProperty("z", out var zProp) ? zProp.GetDouble() : 0.0;
        
        // Orientation (quaternion or euler angles)
        double qx = 0.0, qy = 0.0, qz = 0.0, qw = 1.0;
        if (body.TryGetProperty("qx", out var qxProp) && body.TryGetProperty("qy", out var qyProp) && 
            body.TryGetProperty("qz", out var qzProp) && body.TryGetProperty("qw", out var qwProp))
        {
            qx = qxProp.GetDouble();
            qy = qyProp.GetDouble();
            qz = qzProp.GetDouble();
            qw = qwProp.GetDouble();
        }
        else if (body.TryGetProperty("yaw", out var yawProp))
        {
            // Convert yaw to quaternion
            double yaw = yawProp.GetDouble();
            qw = Math.Cos(yaw / 2.0);
            qz = Math.Sin(yaw / 2.0);
        }

        string frameId = body.TryGetProperty("frame_id", out var frameIdProp) ? frameIdProp.GetString() ?? "map" : "map";
        double xyTolerance = body.TryGetProperty("xy_tolerance", out var xyTolProp) ? xyTolProp.GetDouble() : 0.1;
        double yawTolerance = body.TryGetProperty("yaw_tolerance", out var yawTolProp) ? yawTolProp.GetDouble() : 0.1;

        try
        {
            // Wrap synchronous native calls in Task.Run to avoid blocking the request thread
            bool result;
            long orderHandleValue = orderHandle.ToInt64();
            
            if (orderHandleWrapper != null && orderHandleWrapper.IsValid)
            {
                try
                {
                    logger.LogInformation("Calling MoveToOrder with OrderHandle wrapper: {OrderHandle}, goal: ({X}, {Y}, {Z}), frame: {FrameId}", 
                        orderHandleWrapper.Handle.ToInt64(), x, y, z, frameId);
                    result = await Task.Run(() => service.MoveToOrder(orderHandleWrapper, x, y, z, qx, qy, qz, qw, frameId, xyTolerance, yawTolerance));
                    orderHandleValue = orderHandleWrapper.Handle.ToInt64();
                    logger.LogInformation("MoveToOrder completed with result: {Result}, order_handle: {OrderHandle}", result, orderHandleValue);
                }
                catch (AccessViolationException avex)
                {
                    return Results.BadRequest(new { 
                        status = "error", 
                        message = $"Access violation: Order handle {orderHandleValue} (0x{orderHandleValue:X}) is not a valid pointer to an Order object. The order handle must be created by the navigation system.", 
                        order_handle = orderHandleValue,
                        error_type = "AccessViolationException"
                    });
                }
                catch (ArgumentException aex)
                {
                    return Results.BadRequest(new { 
                        status = "error", 
                        message = aex.Message, 
                        order_handle = orderHandleValue,
                        error_type = "ArgumentException"
                    });
                }
            }
            else if (orderHandle != IntPtr.Zero)
            {
                try
                {
                    logger.LogInformation("Calling MoveToOrder with IntPtr: {OrderHandle}, goal: ({X}, {Y}, {Z}), frame: {FrameId}", 
                        orderHandleValue, x, y, z, frameId);
                    result = await Task.Run(() => service.MoveToOrder(orderHandle, x, y, z, qx, qy, qz, qw, frameId, xyTolerance, yawTolerance));
                    logger.LogInformation("MoveToOrder completed with result: {Result}, order_handle: {OrderHandle}", result, orderHandleValue);
                }
                catch (AccessViolationException avex)
                {
                    return Results.BadRequest(new { 
                        status = "error", 
                        message = $"Access violation: Order handle {orderHandleValue} (0x{orderHandleValue:X}) is not a valid pointer to an Order object. The order handle must be created by the navigation system.", 
                        order_handle = orderHandleValue,
                        error_type = "AccessViolationException"
                    });
                }
                catch (ArgumentException aex)
                {
                    return Results.BadRequest(new { 
                        status = "error", 
                        message = aex.Message, 
                        order_handle = orderHandleValue,
                        error_type = "ArgumentException"
                    });
                }
            }
            else
            {
                return Results.BadRequest(new { status = "error", message = "Invalid order handle" });
            }
            
            return result
                ? Results.Ok(new { status = "success", message = "Move to goal with order sent", goal = new { x, y, z, qx, qy, qz, qw, frameId }, order_handle = orderHandleValue })
                : Results.BadRequest(new { status = "error", message = "Failed to send move to goal with order. Order handle may be invalid or navigation system rejected the command.", order_handle = orderHandleValue });
        }
        catch (AccessViolationException avex)
        {
            return Results.BadRequest(new { 
                status = "error", 
                message = $"Access violation: Order handle {orderHandle.ToInt64()} (0x{orderHandle.ToInt64():X}) is not a valid pointer to an Order object.", 
                order_handle = orderHandle.ToInt64(),
                error_type = "AccessViolationException"
            });
        }
        catch (ArgumentException ex)
        {
            return Results.BadRequest(new { 
                status = "error", 
                message = ex.Message, 
                order_handle = orderHandle.ToInt64(),
                error_type = "ArgumentException"
            });
        }
        catch (Exception ex)
        {
            return Results.BadRequest(new { 
                status = "error", 
                message = $"Internal error: {ex.Message}", 
                order_handle = orderHandle.ToInt64(),
                error_type = ex.GetType().Name,
                stack_trace = ex.StackTrace
            });
        }
    }
    catch (Exception ex)
    {
        return Results.BadRequest(new { status = "error", message = $"Failed to parse request: {ex.Message}" });
    }
});

// Navigation control - Move to goal with OrderData (JSON)
navApi.MapPost("/move_to_order_data", async (HttpRequest request, [FromServices] RobotNet10.RobotApp.Navigation.NavigationIntegrationService service, [FromServices] ILogger<Program> logger) => {
    try
    {
        logger.LogInformation("Received move_to_order_data request");
        var body = await request.ReadFromJsonAsync<JsonElement>();
        if (body.ValueKind != JsonValueKind.Object)
            return Results.BadRequest(new { status = "error", message = "Invalid request body" });

        // Parse goal position
        if (!body.TryGetProperty("x", out var xProp) || !body.TryGetProperty("y", out var yProp))
            return Results.BadRequest(new { status = "error", message = "x and y are required" });

        double x = xProp.GetDouble();
        double y = yProp.GetDouble();
        double z = body.TryGetProperty("z", out var zProp) ? zProp.GetDouble() : 0.0;
        
        // Orientation (quaternion or euler angles)
        double qx = 0.0, qy = 0.0, qz = 0.0, qw = 1.0;
        if (body.TryGetProperty("qx", out var qxProp) && body.TryGetProperty("qy", out var qyProp) && 
            body.TryGetProperty("qz", out var qzProp) && body.TryGetProperty("qw", out var qwProp))
        {
            qx = qxProp.GetDouble();
            qy = qyProp.GetDouble();
            qz = qzProp.GetDouble();
            qw = qwProp.GetDouble();
        }
        else if (body.TryGetProperty("yaw", out var yawProp))
        {
            // Convert yaw to quaternion
            double yaw = yawProp.GetDouble();
            qw = Math.Cos(yaw / 2.0);
            qz = Math.Sin(yaw / 2.0);
        }

        string frameId = body.TryGetProperty("frame_id", out var frameIdProp) ? frameIdProp.GetString() ?? "map" : "map";
        double xyTolerance = body.TryGetProperty("xy_tolerance", out var xyTolProp) ? xyTolProp.GetDouble() : 0.1;
        double yawTolerance = body.TryGetProperty("yaw_tolerance", out var yawTolProp) ? yawTolProp.GetDouble() : 0.1;

        // Parse OrderData from "order" field
        if (!body.TryGetProperty("order", out var orderProp))
            return Results.BadRequest(new { status = "error", message = "order is required" });

        OrderData? orderData = null;
        try
        {
            orderData = JsonSerializer.Deserialize<OrderData>(orderProp.GetRawText(), new JsonSerializerOptions
            {
                PropertyNameCaseInsensitive = true
            });
            
            if (orderData == null)
                return Results.BadRequest(new { status = "error", message = "Failed to parse order data" });

            logger.LogInformation("Parsed OrderData: OrderId={OrderId}, Nodes={NodeCount}, Edges={EdgeCount}", 
                orderData.OrderId, orderData.Nodes?.Count ?? 0, orderData.Edges?.Count ?? 0);
        }
        catch (Exception ex)
        {
            logger.LogError(ex, "Failed to deserialize OrderData");
            return Results.BadRequest(new { 
                status = "error", 
                message = $"Failed to parse order: {ex.Message}"
            });
        }

        try
        {
            // Wrap synchronous native calls in Task.Run to avoid blocking the request thread
            bool result = await Task.Run(() => service.MoveToOrder(orderData, x, y, z, qx, qy, qz, qw, frameId, xyTolerance, yawTolerance));
            
            if (result)
            {
                logger.LogInformation("MoveToOrder successfully sent with OrderData: {OrderId}", orderData.OrderId);
                return Results.Ok(new { 
                    status = "success", 
                    message = "Move to goal with order sent", 
                    goal = new { x, y, z, qx, qy, qz, qw, frameId },
                    order = new {
                        orderId = orderData.OrderId,
                        orderUpdateId = orderData.OrderUpdateId,
                        nodeCount = orderData.Nodes?.Count ?? 0,
                        edgeCount = orderData.Edges?.Count ?? 0
                    }
                });
            }
            else
            {
                logger.LogWarning("MoveToOrder failed for OrderData: {OrderId}", orderData.OrderId);
                return Results.BadRequest(new { 
                    status = "error", 
                    message = "Failed to send move to goal with order. Navigation system rejected the command.",
                    orderId = orderData.OrderId
                });
            }
        }
        catch (Exception ex)
        {
            logger.LogError(ex, "Error in MoveToOrder with OrderData: {OrderId}", orderData?.OrderId ?? "null");
            return Results.BadRequest(new { 
                status = "error", 
                message = $"Internal error: {ex.Message}", 
                error_type = ex.GetType().Name
            });
        }
    }
    catch (Exception ex)
    {
        logger.LogError(ex, "Failed to parse move_to_order_data request");
        return Results.BadRequest(new { status = "error", message = $"Failed to parse request: {ex.Message}" });
    }
});

// Navigation control - Dock to marker
navApi.MapPost("/dock_to", async (HttpRequest request, [FromServices] RobotNet10.RobotApp.Navigation.NavigationIntegrationService service) => {
    try
    {
        var body = await request.ReadFromJsonAsync<JsonElement>();
        if (body.ValueKind != JsonValueKind.Object)
            return Results.BadRequest(new { status = "error", message = "Invalid request body" });

        if (!body.TryGetProperty("marker", out var markerProp))
            return Results.BadRequest(new { status = "error", message = "marker is required" });

        string marker = markerProp.GetString() ?? "";
        if (string.IsNullOrEmpty(marker))
            return Results.BadRequest(new { status = "error", message = "marker cannot be empty" });

        if (!body.TryGetProperty("x", out var xProp) || !body.TryGetProperty("y", out var yProp))
            return Results.BadRequest(new { status = "error", message = "x and y are required" });

        double x = xProp.GetDouble();
        double y = yProp.GetDouble();
        double z = body.TryGetProperty("z", out var zProp) ? zProp.GetDouble() : 0.0;
        
        double qx = 0.0, qy = 0.0, qz = 0.0, qw = 1.0;
        if (body.TryGetProperty("qx", out var qxProp) && body.TryGetProperty("qy", out var qyProp) && 
            body.TryGetProperty("qz", out var qzProp) && body.TryGetProperty("qw", out var qwProp))
        {
            qx = qxProp.GetDouble();
            qy = qyProp.GetDouble();
            qz = qzProp.GetDouble();
            qw = qwProp.GetDouble();
        }
        else if (body.TryGetProperty("yaw", out var yawProp))
        {
            double yaw = yawProp.GetDouble();
            qw = Math.Cos(yaw / 2.0);
            qz = Math.Sin(yaw / 2.0);
        }

        string frameId = body.TryGetProperty("frame_id", out var frameIdProp) ? frameIdProp.GetString() ?? "map" : "map";
        double xyTolerance = body.TryGetProperty("xy_tolerance", out var xyTolProp) ? xyTolProp.GetDouble() : 0.05;
        double yawTolerance = body.TryGetProperty("yaw_tolerance", out var yawTolProp) ? yawTolProp.GetDouble() : 0.05;

        // Wrap synchronous native calls in Task.Run to avoid blocking the request thread
        bool result = await Task.Run(() => service.DockTo(marker, x, y, z, qx, qy, qz, qw, frameId, xyTolerance, yawTolerance));
        return result
            ? Results.Ok(new { status = "success", message = "Dock to goal sent", marker, goal = new { x, y, z, qx, qy, qz, qw, frameId } })
            : Results.BadRequest(new { status = "error", message = "Failed to send dock to goal" });
    }
    catch (Exception ex)
    {
        return Results.BadRequest(new { status = "error", message = $"Failed to parse request: {ex.Message}" });
    }
});

// Navigation control - Move straight by distance (meters) in current direction
navApi.MapPost("/move_straight_to", async (HttpRequest request, [FromServices] RobotNet10.RobotApp.Navigation.NavigationIntegrationService service) => {
    try
    {
        var body = await request.ReadFromJsonAsync<JsonElement>();
        if (body.ValueKind != JsonValueKind.Object)
            return Results.BadRequest(new { status = "error", message = "Invalid request body" });

        if (!body.TryGetProperty("distance", out var distProp))
            return Results.BadRequest(new { status = "error", message = "distance is required" });
        double distance = distProp.GetDouble();

        // Wrap synchronous native calls in Task.Run to avoid blocking the request thread
        bool result = await Task.Run(() => service.MoveStraightTo(distance));
        return result
            ? Results.Ok(new { status = "success", message = "Move straight to goal sent", distance = distance })
            : Results.BadRequest(new { status = "error", message = "Failed to send move straight to goal" });
    }
    catch (Exception ex)
    {
        return Results.BadRequest(new { status = "error", message = $"Failed to parse request: {ex.Message}" });
    }
});

// Navigation control - Rotate to orientation
navApi.MapPost("/rotate_to", async (HttpRequest request, [FromServices] RobotNet10.RobotApp.Navigation.NavigationIntegrationService service) => {
    try
    {
        var body = await request.ReadFromJsonAsync<JsonElement>();
        if (body.ValueKind != JsonValueKind.Object)
            return Results.BadRequest(new { status = "error", message = "Invalid request body" });

        // For rotation, we need current position and target orientation
        double x = 0.0, y = 0.0, z = 0.0;
        if (body.TryGetProperty("x", out var xProp)) x = xProp.GetDouble();
        if (body.TryGetProperty("y", out var yProp)) y = yProp.GetDouble();
        if (body.TryGetProperty("z", out var zProp)) z = zProp.GetDouble();
        
        double qx = 0.0, qy = 0.0, qz = 0.0, qw = 1.0;
        if (body.TryGetProperty("qx", out var qxProp) && body.TryGetProperty("qy", out var qyProp) && 
            body.TryGetProperty("qz", out var qzProp) && body.TryGetProperty("qw", out var qwProp))
        {
            qx = qxProp.GetDouble();
            qy = qyProp.GetDouble();
            qz = qzProp.GetDouble();
            qw = qwProp.GetDouble();
        }
        else if (body.TryGetProperty("yaw", out var yawProp))
        {
            double yaw = yawProp.GetDouble();
            qw = Math.Cos(yaw / 2.0);
            qz = Math.Sin(yaw / 2.0);
        }
        else
        {
            return Results.BadRequest(new { status = "error", message = "yaw or quaternion (qx, qy, qz, qw) is required" });
        }

        string frameId = body.TryGetProperty("frame_id", out var frameIdProp) ? frameIdProp.GetString() ?? "map" : "map";
        double yawTolerance = body.TryGetProperty("yaw_tolerance", out var yawTolProp) ? yawTolProp.GetDouble() : 0.1;

        // Wrap synchronous native calls in Task.Run to avoid blocking the request thread
        bool result = await Task.Run(() => service.RotateTo(x, y, z, qx, qy, qz, qw, frameId, yawTolerance));
        return result
            ? Results.Ok(new { status = "success", message = "Rotate to goal sent", goal = new { x, y, z, qx, qy, qz, qw, frameId } })
            : Results.BadRequest(new { status = "error", message = "Failed to send rotate to goal" });
    }
    catch (Exception ex)
    {
        return Results.BadRequest(new { status = "error", message = $"Failed to parse request: {ex.Message}" });
    }
});

// Navigation control - Pause
navApi.MapPost("/pause", async ([FromServices] RobotNet10.RobotApp.Navigation.NavigationIntegrationService service) => {
    // Wrap synchronous native calls in Task.Run to avoid blocking the request thread
    await Task.Run(() => service.Pause());
    return Results.Ok(new { status = "success", message = "Navigation paused" });
});

// Navigation control - Resume
navApi.MapPost("/resume", async ([FromServices] RobotNet10.RobotApp.Navigation.NavigationIntegrationService service) => {
    // Wrap synchronous native calls in Task.Run to avoid blocking the request thread
    await Task.Run(() => service.Resume());
    return Results.Ok(new { status = "success", message = "Navigation resumed" });
});

// Navigation control - Cancel
navApi.MapPost("/cancel", async ([FromServices] RobotNet10.RobotApp.Navigation.NavigationIntegrationService service) => {
    // Wrap synchronous native calls in Task.Run to avoid blocking the request thread
    await Task.Run(() => service.Cancel());
    return Results.Ok(new { status = "success", message = "Navigation cancelled" });
});

// Navigation control - Set linear twist
navApi.MapPost("/twist/linear", async (HttpRequest request, [FromServices] RobotNet10.RobotApp.Navigation.NavigationIntegrationService service) => {
    try
    {
        var body = await request.ReadFromJsonAsync<JsonElement>();
        if (body.ValueKind != JsonValueKind.Object)
            return Results.BadRequest(new { status = "error", message = "Invalid request body" });

        double linearX = body.TryGetProperty("x", out var xProp) ? xProp.GetDouble() : 0.0;
        double linearY = body.TryGetProperty("y", out var yProp) ? yProp.GetDouble() : 0.0;
        double linearZ = body.TryGetProperty("z", out var zProp) ? zProp.GetDouble() : 0.0;

        // Wrap synchronous native calls in Task.Run to avoid blocking the request thread
        bool result = await Task.Run(() => service.SetTwistLinear(linearX, linearY, linearZ));
        return result
            ? Results.Ok(new { status = "success", message = "Linear twist set", twist = new { x = linearX, y = linearY, z = linearZ } })
            : Results.BadRequest(new { status = "error", message = "Failed to set linear twist" });
    }
    catch (Exception ex)
    {
        return Results.BadRequest(new { status = "error", message = $"Failed to parse request: {ex.Message}" });
    }
});

// Navigation control - Set angular twist
navApi.MapPost("/twist/angular", async (HttpRequest request, [FromServices] RobotNet10.RobotApp.Navigation.NavigationIntegrationService service) => {
    try
    {
        var body = await request.ReadFromJsonAsync<JsonElement>();
        if (body.ValueKind != JsonValueKind.Object)
            return Results.BadRequest(new { status = "error", message = "Invalid request body" });

        double angularX = body.TryGetProperty("x", out var xProp) ? xProp.GetDouble() : 0.0;
        double angularY = body.TryGetProperty("y", out var yProp) ? yProp.GetDouble() : 0.0;
        double angularZ = body.TryGetProperty("z", out var zProp) ? zProp.GetDouble() : 0.0;

        // Wrap synchronous native calls in Task.Run to avoid blocking the request thread
        bool result = await Task.Run(() => service.SetTwistAngular(angularX, angularY, angularZ));
        return result
            ? Results.Ok(new { status = "success", message = "Angular twist set", twist = new { x = angularX, y = angularY, z = angularZ } })
            : Results.BadRequest(new { status = "error", message = "Failed to set angular twist" });
    }
    catch (Exception ex)
    {
        return Results.BadRequest(new { status = "error", message = $"Failed to parse request: {ex.Message}" });
    }
});

// Navigation data - Current pose
navApi.MapGet("/pose/current", ([FromServices] RobotNet10.RobotApp.Navigation.NavigationIntegrationService service) => {
    var pose = service.GetRobotPose();
    return pose.HasValue 
        ? Results.Ok(new { 
            status = "success",
            pose = new {
                position = new { x = pose.Value.x, y = pose.Value.y, z = pose.Value.z },
                orientation = new { x = pose.Value.qx, y = pose.Value.qy, z = pose.Value.qz, w = pose.Value.qw },
                frameId = pose.Value.frameId
            }
        })
        : Results.NotFound(new { status = "error", message = "No pose available" });
});

// Navigation data - Current pose 2D
navApi.MapGet("/pose/current2d", ([FromServices] RobotNet10.RobotApp.Navigation.NavigationIntegrationService service) => {
    var pose = service.GetRobotPose2D();
    return pose.HasValue
        ? Results.Ok(new {
            status = "success",
            pose = new { x = pose.Value.x, y = pose.Value.y, theta = pose.Value.theta }
        })
        : Results.NotFound(new { status = "error", message = "No pose available" });
});

// Navigation data - Current twist
navApi.MapGet("/twist", ([FromServices] RobotNet10.RobotApp.Navigation.NavigationIntegrationService service) => {
    var twist = service.GetTwist();
    return twist.HasValue
        ? Results.Ok(new {
            status = "success",
            twist = new {
                linear = new { x = twist.Value.x, y = twist.Value.y },
                angular = new { z = twist.Value.theta },
                frameId = twist.Value.frameId
            }
        })
        : Results.NotFound(new { status = "error", message = "No twist available" });
});

// Navigation data - Feedback
navApi.MapGet("/feedback", ([FromServices] RobotNet10.RobotApp.Navigation.NavigationIntegrationService service) => {
    var feedback = service.GetFeedback();
    return feedback != null
        ? Results.Ok(new {
            status = "success",
            feedback = new {
                navigationState = feedback.NavigationState,
                stateString = feedback.StateString,
                feedbackString = feedback.FeedbackString,
                currentPose = new {
                    x = feedback.CurrentPose.X,
                    y = feedback.CurrentPose.Y,
                    theta = feedback.CurrentPose.Theta
                },
                goalChecked = feedback.GoalChecked,
                isReady = feedback.IsReady
            }
        })
        : Results.NotFound(new { status = "error", message = "No feedback available" });
});

// Navigation data - Global path (navigation_get_global_data)
navApi.MapGet("/global_data/path", async ([FromServices] RobotNet10.RobotApp.Navigation.NavigationIntegrationService service) => {
    var globalPath = await Task.Run(() => service.GetGlobalPathData());
    if (globalPath == null)
    {
        // Return OK with "no_data" status instead of NotFound to prevent log spam
        return Results.Ok(new { status = "no_data", message = "No global planner data available yet" });
    }

    return Results.Ok(new
    {
        status = "success",
        source = "navigation_get_global_data",
        plan = new
        {
            frameId = globalPath.FrameId,
            pointCount = globalPath.Points.Count,
            points = globalPath.Points.Select(point => new
            {
                x = point.X,
                y = point.Y,
                theta = point.Theta
            })
        }
    });
});

// Navigation data - Local path (navigation_get_local_data)
navApi.MapGet("/local_data/path", async (
    [FromServices] RobotNet10.RobotApp.Navigation.NavigationIntegrationService service,
    [FromServices] RobotNet10.RobotApp.Xloc.XlocIntegrationService xlocService,
    [FromServices] OdometryService odometryService) => {
    var localPath = await Task.Run(() => service.GetLocalPathData());
    if (localPath == null)
    {
        // Return OK with "no_data" status instead of NotFound to prevent log spam
        return Results.Ok(new { status = "no_data", message = "No local planner path data available yet" });
    }

    // Transform local path to map frame so it is anchored to robot on the map view.
    // Local path is typically in "odom" (or base frame). Map view is in "map".
    var maybeMapPose = xlocService.GetCurrentPose2D(); // map->base pose
    var odom = odometryService.CurrentOdometry;        // odom->base pose
    var oq = odom.Pose.Pose.Orientation;
    var odomYaw = Math.Atan2(
        2.0 * (oq.W * oq.Z + oq.X * oq.Y),
        1.0 - 2.0 * (oq.Y * oq.Y + oq.Z * oq.Z)
    );

    double mapBaseX = maybeMapPose?.x ?? 0.0;
    double mapBaseY = maybeMapPose?.y ?? 0.0;
    double mapBaseYaw = maybeMapPose?.yaw ?? 0.0;

    // odom->base
    double odomBaseX = odom.Pose.Pose.Position.X;
    double odomBaseY = odom.Pose.Pose.Position.Y;
    double odomBaseYaw = odomYaw;

    // Compute map->odom = map->base ⊕ inverse(odom->base)
    // 2D composition:
    // inv(odom->base): (-R^T t, -yaw)
    // map->odom translation:
    //   t_mo = t_mb + R(yaw_mb) * (-R(yaw_ob)^T * t_ob)
    // yaw_mo = yaw_mb - yaw_ob
    double yawMapOdom = mapBaseYaw - odomBaseYaw;
    double cosOb = Math.Cos(odomBaseYaw);
    double sinOb = Math.Sin(odomBaseYaw);
    // -R^T * t_ob
    double invX = -(cosOb * odomBaseX + sinOb * odomBaseY);
    double invY = -(-sinOb * odomBaseX + cosOb * odomBaseY);
    double cosMb = Math.Cos(mapBaseYaw);
    double sinMb = Math.Sin(mapBaseYaw);
    double mapOdomX = mapBaseX + (cosMb * invX - sinMb * invY);
    double mapOdomY = mapBaseY + (sinMb * invX + cosMb * invY);

    // Helper: transform point from odom to map using map->odom
    static (double x, double y) TransformOdomToMap(double mapOdomX, double mapOdomY, double yawMapOdom, double xOdom, double yOdom)
    {
        double c = Math.Cos(yawMapOdom);
        double s = Math.Sin(yawMapOdom);
        return (mapOdomX + c * xOdom - s * yOdom, mapOdomY + s * xOdom + c * yOdom);
    }

    // Helper: transform point from base to map using map->base
    static (double x, double y) TransformBaseToMap(double mapBaseX, double mapBaseY, double yawMapBase, double xBase, double yBase)
    {
        double c = Math.Cos(yawMapBase);
        double s = Math.Sin(yawMapBase);
        return (mapBaseX + c * xBase - s * yBase, mapBaseY + s * xBase + c * yBase);
    }

    string originalFrameId = localPath.FrameId;
    var pointsOut = localPath.Points.Select(point =>
    {
        double x = point.X;
        double y = point.Y;
        double theta = point.Theta;

        // If planner reports odom frame, convert to map frame.
        // If planner reports base frame, convert using map->base.
        if (string.Equals(originalFrameId, "odom", StringComparison.OrdinalIgnoreCase))
        {
            var (xm, ym) = TransformOdomToMap(mapOdomX, mapOdomY, yawMapOdom, x, y);
            return new { x = xm, y = ym, theta = theta + yawMapOdom };
        }
        if (string.Equals(originalFrameId, "base_link", StringComparison.OrdinalIgnoreCase) ||
            string.Equals(originalFrameId, "base_footprint", StringComparison.OrdinalIgnoreCase))
        {
            var (xm, ym) = TransformBaseToMap(mapBaseX, mapBaseY, mapBaseYaw, x, y);
            return new { x = xm, y = ym, theta = theta + mapBaseYaw };
        }

        // Unknown frame: return as-is.
        return new { x, y, theta };
    }).ToList();

    return Results.Ok(new
    {
        status = "success",
        source = "navigation_get_local_data",
        plan = new
        {
            frameId = "map",
            originalFrameId = originalFrameId,
            pointCount = pointsOut.Count,
            points = pointsOut
        }
    });
});

// Navigation data - Local cost map (navigation_get_local_data)
navApi.MapGet("/local_data/costmap", async ([FromServices] RobotNet10.RobotApp.Navigation.NavigationIntegrationService service, [FromServices] OdometryService odometryService) => {
    var costMap = await Task.Run(() => service.GetCostMapData());
    if (costMap == null)
    {
        // Return OK with "no_data" status instead of NotFound to prevent log spam
        return Results.Ok(new { status = "no_data", message = "No local planner cost map data available yet" });
    }

    var odom = odometryService.CurrentOdometry;
    var q = odom.Pose.Pose.Orientation;
    var odomYaw = Math.Atan2(
        2.0 * (q.W * q.Z + q.X * q.Y),
        1.0 - 2.0 * (q.Y * q.Y + q.Z * q.Z)
    );

    return Results.Ok(new
    {
        status = "success",
        source = "navigation_get_local_data",
        hasFullMap = costMap.HasFullMap,
        isCostmapUpdated = costMap.IsCostmapUpdated,
        odometry = new
        {
            x = odom.Pose.Pose.Position.X,
            y = odom.Pose.Pose.Position.Y,
            yaw = odomYaw
        },
        costmap = new
        {
            frameId = costMap.FrameId,
            resolution = costMap.Resolution,
            width = costMap.Width,
            height = costMap.Height,
            origin = new
            {
                x = costMap.OriginX,
                y = costMap.OriginY,
                theta = costMap.OriginTheta
            },
            dataSize = costMap.HasFullMap ? costMap.Data.Length : 0,
            data = costMap.HasFullMap ? Convert.ToBase64String(costMap.Data) : null
        },
        costmapUpdate = new
        {
            frameId = costMap.UpdateFrameId,
            x = costMap.UpdateX,
            y = costMap.UpdateY,
            width = costMap.UpdateWidth,
            height = costMap.UpdateHeight,
            dataSize = costMap.IsCostmapUpdated ? costMap.UpdateData.Length : 0,
            data = costMap.IsCostmapUpdated ? Convert.ToBase64String(costMap.UpdateData) : null
        }
    });
});

// Navigation data - Robot footprint configuration
navApi.MapGet("/robot_footprint", ([FromServices] RobotNet10.RobotApp.Navigation.NavigationIntegrationService service) => {
    var footprint = service.GetRobotFootprint();
    if (footprint == null || footprint.Length == 0)
    {
        return Results.NotFound(new { status = "error", message = "Robot footprint not configured" });
    }

    return Results.Ok(new
    {
        status = "success",
        footprint = footprint.Select(point => new
        {
            x = point.X,
            y = point.Y,
            z = point.Z
        }).ToArray()
    });
});

// Marker Detection Control API
var markerApi = app.MapGroup("/api/marker-detection").DisableAntiforgery();

// Enable or disable marker detection
markerApi.MapPost("/detection/enable", async (HttpRequest request, [FromServices] RobotNet10.RobotApp.MarkerDetection.MarkerDetectionIntegrationService service) => {
    try
    {
        var body = await request.ReadFromJsonAsync<Dictionary<string, bool>>();
        var enable = body?["enable"] ?? true;
        
        service.SetEnableDetection(enable);
        
        return Results.Ok(new { 
            status = "success", 
            message = $"Marker detection {(enable ? "enabled" : "disabled")}",
            enabled = enable
        });
    }
    catch (Exception ex)
    {
        return Results.BadRequest(new { status = "error", message = ex.Message });
    }
});

// Get current marker pose
markerApi.MapGet("/pose/current", ([FromServices] RobotNet10.RobotApp.MarkerDetection.MarkerDetectionIntegrationService service) => {
    try
    {
        var pose = service.GetMarkerPose();
        
        if (pose == null)
            return Results.NotFound(new { status = "error", message = "No marker pose available" });
        
        return Results.Ok(new {
            status = "success",
            pose = new {
                header = new {
                    seq = pose.Header.Seq,
                    stamp = pose.Header.Stamp,
                    frameId = pose.Header.FrameId
                },
                position = new {
                    x = pose.Pose.Position[0],
                    y = pose.Pose.Position[1],
                    z = pose.Pose.Position[2]
                },
                orientation = new {
                    x = pose.Pose.Orientation[0],
                    y = pose.Pose.Orientation[1],
                    z = pose.Pose.Orientation[2],
                    w = pose.Pose.Orientation[3]
                }
            }
        });
    }
    catch (Exception ex)
    {
        return Results.BadRequest(new { status = "error", message = ex.Message });
    }
});

app.Run();

// DTOs for PLC Lift API
public record LiftVelocityRequest(bool Up, bool Down);
public record LiftPositionRequest(int Position);