import type { Plan } from "./plan.ts";
import type { Point2 } from "./types.ts";

const ROUTE_RADIUS_M = 0.28;
const PORTAL_OFFSET_M = 0.48;

export interface RobotRoute {
  levelId: string;
  waypoints: Point2[];
  lengthM: number;
}

export interface RobotRouter {
  route(levelId: string, start: Point2, goal: Point2): RobotRoute | null;
}

interface Node {
  id: string;
  point: Point2;
}

/**
 * Build a deterministic visibility graph from Plan's passable openings. The
 * graph never links storeys: offices do not author lifts or stairs as routes.
 */
export function createRobotRouter(plan: Plan): RobotRouter {
  const portals = new Map<string, readonly Node[]>();
  for (const level of plan.levels) {
    const nodes: Node[] = [];
    for (const opening of level.openings) {
      if (!opening.passable) continue;
      const normal = { x: Math.sin(opening.yaw), z: Math.cos(opening.yaw) };
      const candidates = [
        opening.center,
        {
          x: opening.center.x + normal.x * PORTAL_OFFSET_M,
          z: opening.center.z + normal.z * PORTAL_OFFSET_M,
        },
        {
          x: opening.center.x - normal.x * PORTAL_OFFSET_M,
          z: opening.center.z - normal.z * PORTAL_OFFSET_M,
        },
      ];
      candidates.forEach((point, index) => {
        if (!plan.blocked(level.id, point, point, ROUTE_RADIUS_M)) {
          nodes.push({ id: `${opening.id}:${index}`, point: copy(point) });
        }
      });
    }
    portals.set(level.id, Object.freeze(nodes.sort((a, b) => a.id.localeCompare(b.id))));
  }

  return {
    route(levelId, start, goal) {
      const level = plan.level(levelId);
      if (!level || !finite(start) || !finite(goal)) return null;
      if (!plan.roomAt(levelId, start) || !plan.roomAt(levelId, goal)) return null;
      if (plan.blocked(levelId, start, start, ROUTE_RADIUS_M)) return null;
      if (plan.blocked(levelId, goal, goal, ROUTE_RADIUS_M)) return null;
      if (!plan.blocked(levelId, start, goal, ROUTE_RADIUS_M)) {
        return { levelId, waypoints: [copy(goal)], lengthM: distance(start, goal) };
      }

      const nodes: Node[] = [
        { id: "$start", point: copy(start) },
        ...(portals.get(levelId) ?? []),
        { id: "$goal", point: copy(goal) },
      ];
      const startIndex = 0;
      const goalIndex = nodes.length - 1;
      const costs = nodes.map(() => Number.POSITIVE_INFINITY);
      const previous = nodes.map(() => -1);
      const open = new Set<number>([startIndex]);
      costs[startIndex] = 0;

      while (open.size > 0) {
        let current = -1;
        for (const index of open) {
          if (
            current < 0 || costs[index]! < costs[current]! - 1e-9 ||
            (Math.abs(costs[index]! - costs[current]!) <= 1e-9 && nodes[index]!.id < nodes[current]!.id)
          ) current = index;
        }
        if (current === goalIndex) break;
        open.delete(current);
        for (let next = 0; next < nodes.length; next += 1) {
          if (next === current || next === startIndex) continue;
          const from = nodes[current]!.point;
          const to = nodes[next]!.point;
          if (plan.blocked(levelId, from, to, ROUTE_RADIUS_M)) continue;
          const candidate = costs[current]! + distance(from, to);
          if (
            candidate < costs[next]! - 1e-9 ||
            (Math.abs(candidate - costs[next]!) <= 1e-9 && current < previous[next]!)
          ) {
            costs[next] = candidate;
            previous[next] = current;
            open.add(next);
          }
        }
      }
      if (!Number.isFinite(costs[goalIndex])) return null;
      const reversed: Point2[] = [];
      for (let cursor = goalIndex; cursor !== startIndex; cursor = previous[cursor]!) {
        if (cursor < 0) return null;
        reversed.push(copy(nodes[cursor]!.point));
      }
      reversed.reverse();
      return { levelId, waypoints: compact(reversed), lengthM: costs[goalIndex]! };
    },
  };
}

function compact(points: readonly Point2[]): Point2[] {
  return points.filter((point, index) => index === points.length - 1 || distance(point, points[index + 1]!) > 0.08);
}

function finite(point: Point2): boolean {
  return Number.isFinite(point.x) && Number.isFinite(point.z);
}

function distance(a: Point2, b: Point2): number {
  return Math.hypot(a.x - b.x, a.z - b.z);
}

function copy(point: Point2): Point2 {
  return { x: point.x, z: point.z };
}
