let findIndex2D (p:'A -> bool) (a:'A [,]) =
  a |> Array2D.mapi (fun x y v -> x,y,v)
    |> Seq.cast<int*int*'A>
    |> Seq.pick (fun (x,y,v) -> if p v then Some (x,y)  else None)

let totalManhattanDistance board goal =
    let manhattanDistance (x1, y1) (x2, y2) =
        abs(x1 - x2) + abs (y1 - y2)
    board |> Array2D.mapi (fun x y v -> manhattanDistance (x, y)
                                        <| findIndex2D ((=) v) goal)
          |> Seq.cast<int>         // flatten
          |> Seq.sum

let swap (arr : int[,]) i1 i2 =
    let map i j v =
        match (i,j) with
        | t when t = i1 -> arr.[fst i2, snd i2]
        | u when u = i2 -> arr.[fst i1, snd i1]
        | _ -> v
    arr |> Array2D.mapi map

let neighbors board (zeroX, zeroY) =
    let (x,y) = findIndex2D ((=) 0) board
    [if x > 0 then yield swap board (zeroX,zeroY) (zeroX - 1, zeroY)
     if x < Array2D.length1 board - 1 then yield swap board (zeroX,zeroY) (zeroX + 1, zeroY)
     if y > 0 then yield swap board (zeroX,zeroY) (zeroX, zeroY - 1)
     if y < Array2D.length2 board - 1 then yield swap board (zeroX,zeroY) (zeroX, zeroY + 1)]

let notContainsTuple x list = not (List.forall (fun (b,_,_) ->
                                                    not (b = x)) list)

let notContainsList x list = not (List.exists ((=) x) list)

let fst3 (x,_,_) = x


let Astar board goal =
    let gScore = 0
    let fScore = gScore + totalManhattanDistance board goal
    let startState = (board, gScore, fScore)
    let openSet = [startState]
    let closedSet = []

    let rec find currentBoard CS OS GS =
        if currentBoard = goal then GS
        else
            let zeroInd = findIndex2D ((=) 0) currentBoard
            let removedCurrent = List.tail OS
            let adjacentBoards = List.filter (fun elem ->
                                                 (notContainsList elem CS) &&
                                                 (notContainsTuple elem removedCurrent))
                                             (neighbors currentBoard zeroInd)

            let adjacentStates = List.map (fun x ->
                                                    let newGS = GS + 1
                                                    let newFS = newGS + totalManhattanDistance x goal
                                                    (x, newGS, newFS)) adjacentBoards

            let newCS = currentBoard::CS
            let newOS = List.sortBy (fun (b,g,f) -> f)
                            (List.append removedCurrent adjacentStates)

            find (fst3 (List.head newOS) newCS newOS (GS + 1)

    find board closedSet openSet gScore

[<EntryPoint>]
let main argv =
    let startingBoard = array2D [|[|1; 4; 7|];
                                [|6; 3; 5|];
                                [|0; 8; 2|]|]

    let goal =  array2D [|[|1; 2; 3|];
                        [|4; 5; 6|];
                        [|7; 8; 0|]|]

    printfn "%d" (Astar startingBoard goal)

    0 // return an integer exit code