Micromouse/src/algorithm.zig

const std = @import("std");
const assert = std.debug.assert;

const map = @import("map.zig");
const Point = map.Point;
const Cardinal = map.Cardinal;
const Mouse = @import("mouse.zig").Mouse;

pub const is_robot = @import("builtin").os.tag == .freestanding;

// Doesn't have any state
// It's a hardware abstraction layer
pub const robot = if (is_robot) @import("rp2040-bot.zig") else @import("simulated-bot.zig");

// It's a little weird to do this as a global, but it just returns
//  an instance of Mouse with initial values
var mouse = @import("mouse.zig").initialize();

pub fn setup () void {
    robot.setup();
}

//// Node
const Node = struct {
    p: Point,
    score: usize,

    // Return 0-4 Nodes, by checking if
    //  each of the four nodes around this node a) exists and b) is accessible (no wall)
    // I maybe could've also returned a `[4:0]*Node` (sentinel terminated 4-long array of node pointers)
    pub fn getAdjacentNodes(self: Node) std.BoundedArray(*Node, 4) {
        var nodes = std.BoundedArray(*Node, 4).init(0) catch unreachable; // 0 is < 4

        // I could clean this up with like a loop and a helper function
        // North
        // std.log.debug("North: y + 1 < height {}; not wall: {}", .{self.p.y + 1 < map.height, !isWall(self.p, .north)});
        // std.log.debug("isWall(self.p '{}', .north): {}", .{self.p, isWall(self.p, .north)});
        if (self.p.y + 1 < map.height and !isWall(self.p, .north)) {
            // std.log.debug("Adding node", .{});
            nodes.append(&mazeNodes[self.p.y + 1][self.p.x]) catch unreachable;
        }
        // South
        if (self.p.y > 0 and !isWall(self.p, .south)) {
            nodes.append(&mazeNodes[self.p.y - 1][self.p.x]) catch unreachable;
        }
        // East
        if (self.p.x + 1 < map.width and !isWall(self.p, .east)) {
            nodes.append(&mazeNodes[self.p.y][self.p.x + 1]) catch unreachable;
        }
        // West
        if (self.p.x > 0 and !isWall(self.p, .west)) {
            nodes.append(&mazeNodes[self.p.y][self.p.x - 1]) catch unreachable;
        }

        return nodes;
    }

    pub fn updateScore(self: *Node, newScore: usize) void {
        // newScore should be less than the current score
        assert(newScore <= self.score);

        self.score = newScore;
        // Call updateScore on all adjacent nodes if they have a score which is worse than (newScore + 1)
        // For some reason BoundedArrays aren't indexable
        const adjacentNodes = self.getAdjacentNodes().slice();
        for (adjacentNodes) |node| {
            if (self.score + 1 < node.score) {
                node.updateScore(self.score + 1);
            }
        }

        if (!is_robot) {
            robot.writeCellScore(self.p, self.score);
        }
    }
};


//// Walls

// I tried to come up with a better data structure for this, and it just hurt my brain too much
// So I'm just going to go with the Matthias classic
// With a slight change for our coordinate system
// 11 x 11 list of "wall posts" with each wall post keeping track of the wall to its north and east
const WallPost = struct {
    // Not using Cardinal or Point here to avoid confusing with the maze tile directions
    // These are nullable to represent when we haven't seen them yet
    up: ?bool,
    right: ?bool,
};


// Might be a better way to do this initialization
var walls = init: {
    var initialWalls: [map.height + 1][map.width + 1]WallPost = undefined;
    var row: [map.width + 1]WallPost = undefined;
    @memset(&row, WallPost { .up = null, .right = null });
    @memset(&initialWalls, row);
    break :init initialWalls;
};

fn isEdge(p: Point, direction: Cardinal) bool {
    // std.log.debug("is Edge called with point: {} direction: {}\n", .{p, direction});
    return (
           (p.x == 0 and direction == .west)
        or (p.y == 0 and direction == .south)
        or (p.x == map.width - 1 and direction == .east)
        or (p.y == map.height - 1 and direction == .north)
    );
}

// Used by both isWall and setWall
fn getWall(p: Point, direction: Cardinal) *?bool {
    // Should be handled by the caller
    assert(!isEdge(p, direction));

    // Direction is north, west, or south we look left.
    const wallX = if (direction != .east) p.x else p.x + 1;
    // We look down, unless it's north
    const wallY = if (direction != .north) p.y else p.y + 1;

    const wallPost = &walls[wallY][wallX];

    return &(if (direction == .west or direction == .east) wallPost.up else wallPost.right);
}

fn isWall(p: Point, direction: Cardinal) bool {
    // Boundaries are definitely there
    if (isEdge(p, direction)) {
        // std.log.debug("forcing true return from isWall for point {} and direction {}", .{p, direction});
        return true;
    }

    const wall = getWall(p, direction);

    // Now, because floodfill is optimistic, I'm going to default to assuming there's not a wall if we haven't seen one
    return wall.* orelse false;
}

pub fn setWall(p: Point, direction: Cardinal, value: bool) void {
    // We should maybe have a better way of handling this if it actually happens to the robot
    // If that direction is an outer wall, assert that we saw a wall
    // TODO: cleanup
    if (isEdge(p, direction)) {
        // std.log.debug("Hello? {} {} {} {}", .{p, direction, isEdge(p, direction), value});
        assert(value);
        // print("Hello? {} {} {} {}", .{p, direction, isEdge(p, direction), value});
        // std.debug.panic("Outside of maze is ");
    }
    // assert(!(isEdge(p, direction) and !value));

    if (!is_robot and value) {
        robot.showWall(p, direction);
    }

    if (isEdge(p, direction)) {
        return;
    }

    const wall = getWall(p, direction);
    // wall.* is a ?bool and value is a bool, but we can do type coercion
    wall.* = value;
}

//// Maze
var mazeNodes = init: {
    var nodes: [map.width][map.height]Node = undefined;
    for (&nodes, 0..) |*row, y| {
        for (row, 0..) |*n, x| {
            n.* = Node {
                .p = Point{ .x = x, .y = y },
                .score = std.math.maxInt(@TypeOf(n.score))
            };
        }
    }
    break :init nodes;
};

test {
    // y, x; and we start in the bottom left
    const s = mazeNodes[0][0];
        // @compileLog(s.p.x);
        // @compileLog(s.p.y);
        // @compileLog(map.start.x);
        // @compileLog(map.start.y);
    assert(s.p.x == map.start.x);
    assert(s.p.y == map.start.y);
}


//// Algorithm
fn recalcScores(dest: []const Point) void {
    // Reset every node to a high score
    for (&mazeNodes) |*row| {
        for (row) |*n| {
            n.score = std.math.maxInt(@TypeOf(n.score));
        }
    }

    // Create destination nodes for every destination point, with a score of 0
    // Starting at each of the destination(s) and working out, give each node an increasing score
    // updateScore handles updating the scores for all other nodes in the maze
    for (dest) |p| {
        const goalNode = &mazeNodes[p.y][p.x];
        goalNode.updateScore(0);
    }
}

// Just a helper function
fn hasPoint(l: []const Point, needle: Point) bool {
    for (l) |p| {
        if (needle.eq(p)) {
            return true;
        }
    }
    return false;
}

// Takes a list of destination points, and a mouse
// Blocks until the mouse is in one of the destination points
pub fn floodFill(dest: []const Point) void {
    if (dest.len == 4) {
        assert(hasPoint(dest, map.goals[0]));
        assert(hasPoint(dest, map.goals[3]));
    }

    mouse.updateWalls();

    // Calculate optimistic distances from the center
    recalcScores(dest);

    for (dest) |p| {
        const goalNode = &mazeNodes[p.y][p.x];
        std.log.debug("goal nodes: {}\n", .{ goalNode });
    }

    // While we aren't at dest,
    while (!hasPoint(dest[0..], mouse.position)) {
        const currentSquare = &mazeNodes[mouse.position.y][mouse.position.x];

        // 1. get the square adjacent to the mouse which has the lowest score
        var minAdjacent: ?*Node = null;
        std.log.debug("currentSquare {} score: {}", .{ currentSquare.p, currentSquare.score });
        const adjacentNodes = currentSquare.getAdjacentNodes().slice();
        std.log.debug("adjacentNodes {}", .{adjacentNodes.len});
        for (adjacentNodes) |n| {
            std.log.debug("Node at ({}, {}) has score {}", .{ n.p.x, n.p.y, n.score });
            assert(@TypeOf(n) == *Node);
            if (minAdjacent == null or n.score < minAdjacent.?.score) {
                minAdjacent = n;
            }
        }
        assert(minAdjacent != null);

        // 2. if that square has a value the same or higher than the current score, then we re-do score calculation and go to 1
        if (minAdjacent.?.score >= currentSquare.score) {
            recalcScores(dest);

            continue;
        }

        // 3. otherwise, move the mouse there
        mouse.moveAdjacent(minAdjacent.?.p);

        // 4. update the sensors on the mouse
        mouse.updateWalls();
    }

    assert(hasPoint(dest, mouse.position));
    if (dest.len == 4) {
        // This isn't always true because sometimes dest isn't the goal
        assert(hasPoint(map.goals[0..], mouse.position));
    }
}