using PyPlot
using NtToolBox

n = 40;

neigh = [1 -1 0 0; 0 0  1 -1];

boundary = x -> mod(x-1,n)+1

ind2sub1 = k -> Int.([rem(k-1, n)+1; (k - rem(k-1, n) - 1)/n + 1])
sub2ind1 = u -> Int((u[2]-1)*n + u[1])
Neigh = (k,i) -> sub2ind1(boundary(ind2sub1(k) + neigh[:,i]));

println( ind2sub1( sub2ind1([13, 27]) ) )
println( sub2ind1( ind2sub1(134) ) )

W = ones( (n,n) );

x0 = [Base.div(n,2), Base.div(n,2)];

I = [sub2ind1(x0)];

D = ones( n,n ) + Inf
u = ind2sub1(I)
D[u[1],u[2]] = 0;

S = zeros(n,n)
S[u[1],u[2]] = 1;

extract   = (x,I) -> x[I]
extract1d = (x,I) -> extract(vec(x),I);

j = sortperm( extract1d(D,I)  )
if ndims(j)==0
    j = [j]   # make sure that j is a list a not a singleton
end
j = j[1]
i = I[j]         
a = deleteat!(I,j);

u = ind2sub1(i);
S[u[1],u[2]] = -1;

J = [] 
for k in 1:4
    j = Neigh(i,k)
    if extract1d(S,j)!= -1
        # add to the list of point to update
        append!(J,j)           
        if extract1d(S,j)==0
            # add to the front
            u = ind2sub1(j)
            S[u[1],u[2]] = 1
            append!(I,j)
        end
    end
end

imageplot(S)

DNeigh = (D,k) -> extract1d(D,Neigh(j,k))
dx = 0; dy = 0; w = 0
for j in J
    dx = min(DNeigh(D,1), DNeigh(D,2))
    dy = min(DNeigh(D,3), DNeigh(D,4))
    u = ind2sub1(j)
    w = extract1d(W,j);
    D[u[1],u[2]] = min(dx + w, dy + w)
end

include("NtSolutions/fastmarching_0_implementing/exo1.jl")

## Insert your code here.

displ = D -> cos(2*pi*5*D/maximum(D) )
imageplot(displ(D))
set_cmap("jet")

D[u[1],u[2]] = min(dx + w, dy + w);

Delta = 2*w - (dx-dy)^2
if (Delta>=0)
    D[u[1],u[2]] = (dx + dy + sqrt(Delta))/ 2
else
    D[u[1],u[2]] = min(dx + w, dy + w)
    end;

include("NtSolutions/fastmarching_0_implementing/exo2.jl")

## Insert your code here.

imageplot(displ(D))
set_cmap("jet")

n = 100
x = collect(linspace(-1, 1, n))
(Y, X) = meshgrid(x, x)
sigma = .2
W = 1 + 8 * exp(-(X.^2 + Y.^2) / (2*sigma^2));

imageplot(W)

x0 = Int.([round(.1*n); round(.1*n)]);

include("NtSolutions/fastmarching_0_implementing/exo3.jl")

## Insert your code here.

G0 = grad(D);

d = sqrt(sum(G0.^2, 3))
U = zeros(n,n,2)
U[:,:,1] = d
U[:,:,2] = d
G = G0 ./ U;

tau = .8;

x1 = Int(round(.9*n)) + im*Int(round(.88*n))
gamma = [x1];

Geval = (G,x) -> bilinear_interpolate(G[:,:,1], imag(x), real(x) ) + im * bilinear_interpolate(G[:,:,2],imag(x), real(x));

g = Geval(G, gamma[end] )

append!(float(gamma), gamma[end] - tau*g);

include("NtSolutions/fastmarching_0_implementing/exo4.jl");

## Insert your code here.

imageplot(W) 
set_cmap("gray")
h = plot(imag(gamma), real(gamma), ".b", linewidth=2)
h = plot(x0[2], x0[1], ".r", markersize=20)
h = plot(imag(x1), real(x1), ".g", markersize=20);
<script>
  $(document).ready(function(){
      $('div.prompt').hide();
  });
</script>