let find grid c =
  match Aoc.Grid.idx_from_opt grid 0 c with
  | None -> failwith "find"
  | Some idx -> Aoc.Grid.pos_of_idx grid idx

let input_of_file fname =
  let grid = Aoc.Grid.of_file fname in
  let start_state = (find grid 'S', (1, 0)) in
  (grid, start_state)

let rec dijkstra visit check_end states =
  let compare_costs (lhs, _) (rhs, _) = compare lhs rhs in
  match states with
  | [] -> failwith "dijkstra"
  | (cost, state) :: t ->
      if check_end state then cost
      else
        let new_states =
          visit cost state |> List.sort compare_costs
          |> List.merge compare_costs t
        in
        dijkstra visit check_end new_states

let visited_idx grid (p, (dx, dy)) =
  let add =
    match (dx, dy) with
    | 1, 0 -> 0
    | 0, 1 -> 1
    | -1, 0 -> 2
    | 0, -1 -> 3
    | _ -> failwith "visited_idx"
  in
  (Aoc.Grid.idx_of_pos grid p * 4) + add

let has_visited grid visited_grid state = visited_grid.(visited_idx grid state)

let mark_visited grid visited_grid state =
  visited_grid.(visited_idx grid state) <- true

let visit grid visited_grid cost ((((x, y) as p), (dx, dy)) as state) =
  if has_visited grid visited_grid state then []
  else if Aoc.Grid.get_by_pos grid p = '#' then []
  else (
    Printf.printf "%d %d %d %d %d\n" cost x y dx dy;
    mark_visited grid visited_grid state;
    [
      (cost + 1, ((x + dx, y + dy), (dx, dy)));
      (cost + 1000, (p, (-dy, dx)));
      (cost + 1000, (p, (dy, -dx)));
    ])

let check_end grid (p, _) = Aoc.Grid.get_by_pos grid p = 'E'

let part (grid, start_state) =
  let visited_grid = Array.make (Aoc.Grid.length grid * 4) false in
  dijkstra (visit grid visited_grid) (check_end grid) [ (0, start_state) ]

let _ = Aoc.main input_of_file [ (string_of_int, part) ]