using Microsoft.AspNetCore.Mvc;
using System.Text.Json;
using RobotNet10.RobotApp.Motion;
using RobotNet10.RobotApp.Navigation;

namespace RobotNet10.RobotApp.Xloc;

public static class XlocApiEndpoints
{
    // XLOC Control API
    public static IEndpointRouteBuilder MapXlocApiEndpoints(this IEndpointRouteBuilder app)
    {
        var xlocApi = app.MapGroup("/api/xloc").DisableAntiforgery();

        // Mapping control
        xlocApi.MapPost("/mapping/start", ([FromServices] 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] 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] 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] 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] 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] 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] 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] 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] 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] 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] 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] 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));

        // Map upload endpoint (kept for backward compatibility)
        xlocApi.MapPost("/map/upload", async (HttpRequest request, [FromServices] 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] 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] 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] 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] 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] 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] 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" });
        });
        */

        return app;
    }

    // Helper: read origin (Map frame in World frame) from active map's YAML
    private 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
    private 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);
    }
}