astar.jl

using DataStructures: heappop!, heappush!, isheap, heapify!
import Base: ==, .==, show, isequal, isless
using Base.Test

const Coord = Tuple{Int,Int}
abstract type AbstractNode end
struct StartNode <: AbstractNode end
struct GoalNode <: AbstractNode end
struct UnknownNode <: AbstractNode end
mutable struct MatrixNode <: AbstractNode
    c::Coord
    f::Float64
    g::Float64
    cost::Float64
    parent::AbstractNode
end
show(io::IO,n::MatrixNode) = print(io,"MatrixNode(c=",n.c,", f,g=",round(n.f,3),",",round(n.g,3),")")

==(a::MatrixNode,b::MatrixNode) = a.c == b.c
.==(a::MatrixNode,b::MatrixNode) = a.c == b.c
isless(a::MatrixNode,b::MatrixNode) = isless(a.f, b.f)
distance(a,b) = b.cost + √((a.c[1]-b.c[1])^2 + (a.c[2]-b.c[2])^2)
function neighbors(current::MatrixNode,G)::Vector{MatrixNode}
    N = size(G,1)
    i,j = current.c
    irange = max(1,i-1):min(N,i+1)
    jrange = max(1,j-1):min(N,j+1)
    nodes = G[irange,jrange] |> vec
    return nodes
end

function reconstruct_path(current, path)
    push!(path,current)
    if current.parent == StartNode()
        return reverse(path)
    else
        return reconstruct_path(current.parent, path)
    end
end
reconstruct_path(current) = reconstruct_path(current, typeof(current)[])

# Generate graph (matrix)


function astar{T}(G::Matrix{T},startc,goalc)
    time0 = tic()
    start = G[startc...]
    start.c = startc
    start.f = heuristic(startc,goalc)
    start.g = 0.
    start.parent = StartNode()

    goal = G[goalc...]
    goal.c = goalc
    goal.f = Inf
    goal.g = Inf
    goal.parent = GoalNode()

    open = Vector{T}();  sizehint!(open,length(G)÷10)
    closed = Set{T}();   sizehint!(closed,length(G)÷10)
    heappush!(open,start)
    while !isempty(open)
        current = heappop!(open)
        if current == goal
            path = reconstruct_path(current)
            # println("A* found the optimal path, length: $(length(path)), cost: ",path[end].g, " , time: ", toq())
            return path
        end
        push!(closed, current)
        for neighbor in neighbors(current,G)
            if neighbor ∈ closed
                continue
            end
            g_new = current.g + distance(current, neighbor)
            if g_new >= neighbor.g
                continue
            end
            neighbor.parent = current
            neighbor.g = g_new
            neighbor.f = neighbor.g + heuristic(neighbor.c, goal.c)
            if neighbor ∉ open
                heappush!(open, neighbor)
                # @assert isheap(open)
            else
                heapify!(open) # This has to be done here since f was updated above.
                # TODO: this can probably be changed for percolate_up!, since f can only be changed for the better
            end
        end
    end
    println("A* failed to find a feasible path :(")
    return nothing
end

function assign_costs!{T<:AbstractNode}(G::AbstractMatrix{T},costs)
    for (g,c) in zip(G,costs)
        g.cost = c
    end
end

function run_astar(N)
    G = MatrixNode[MatrixNode((i,j),Inf,Inf,rand(1:50), UnknownNode()) for i = 1:N, j=1:N]
    nf = 5
    costs = conv2(Float64.(abs.(randn(N,N))), ones(nf,nf)/nf^2)[nf÷2:end-nf÷2,nf÷2:end-nf÷2]
    costs = costs[1:N,1:N]

    # costs[20:22,10:40] = 50
    # for i = 2:N-1
    #     costs[i,i-1] *= 0.9
    # end
    # for i = N÷2:N-10
    #     costs[i,i+10] *= 0.8
    # end
    assign_costs!(G,costs)

    startc = (1,1)
    goalc = (N-1,N)
    t = @elapsed path = astar(G,startc,goalc)

    t
end


function test_and_plot(label)
    t     = run_astar(100)
    Nvec  = round.(Int, logspace(1, log10(400), 20) )
    Nmat  = Nvec
    times = map(run_astar,Nmat)

    Alog = Float64.(Nmat.^(0:1)')
    Alog[:,2:end] = log.(Alog[:,2:end])
    xlog = Alog\log.(times)

    scatter!(Nmat,times, lab=label, yscale=:log10, xscale=:log10,
    xlabel="Problem size", ylabel="Execution time [s]")
    # plot!(Nmat, exp.(Alog*xlog), lab="Log-Model fit")
    gui()
end



plot()
heuristic(a,b)::Float64 = √((a[1]-b[1])^2 + (a[2]-b[2])^2)
test_and_plot("L2")

heuristic(a,b)::Float64 = 0.5*√((a[1]-b[1])^2 + (a[2]-b[2])^2)
test_and_plot("0.5 L2")

heuristic(a,b)::Float64 = 0.1*√((a[1]-b[1])^2 + (a[2]-b[2])^2)
test_and_plot("0.1 L2")

heuristic(a,b)::Float64 = ((a[1]-b[1])^2 + (a[2]-b[2])^2)^(1/4)
test_and_plot("sqrt(L2)")

heuristic(a,b)::Float64 = log(abs(a[1]-b[1])) + log(abs(a[2]-b[2]))
test_and_plot("log-manhattan")



# costs = [n.cost for n in G]
# pathx = [n.c[1] for n in path]
# pathy = [n.c[2] for n in path]


# heatmap(log(costs))
# scatter!(pathy,pathx, c=:red)
# # scatter!(pathx,pathy, c=:blue)
# gui()





# Tests ========================================================================
# @test path[1].parent == StartNode()
# @test all(isa(node.parent, MatrixNode) for node in path[2:end])
# @test all(path[i].f <= path[i+1].f for i = 1:length(path)-1)
# @test all(path[i].g <= path[i+1].g for i = 1:length(path)-1)
# @test all(n.f >= n.g for n in path)
# @test all(abs(pathx[i] - pathx[i+1]) <= 1 for i = 1:length(pathx)-1)
# @test all(abs(pathy[i] - pathy[i+1]) <= 1 for i = 1:length(pathy)-1)