options(warn=-1) # turns off warnings, to turn on: "options(warn=0)"


library(imager)
library(png)

for (f in list.files(path="nt_toolbox/toolbox_general/", pattern="*.R")) {
    source(paste("nt_toolbox/toolbox_general/", f, sep=""))
}

for (f in list.files(path="nt_toolbox/toolbox_signal/", pattern="*.R")) {
    source(paste("nt_toolbox/toolbox_signal/", f, sep=""))
}

source("nt_toolbox/toolbox_wavelet_meshes/meshgrid.R")

source("nt_toolbox/toolbox_graph/perform_dijstra_fm.R")

options(repr.plot.width=3.5, repr.plot.height=3.5)

n <- 40

neigh <- array(c(1, 0, -1, 0, 0, 1, 0, -1), c(2,4))

boundary <- function(x){ mod(x,n) }

ind2sub1 <- function(k){ c(as.integer( (k-1-mod(k-1,n))/n + 1 ), mod(k-1,n) + 1) }
sub2ind1 <- function(u){ as.integer( (u[1]-1)*n + (u[2]-1) + 1 ) }
Neigh <- function(k,i){ sub2ind1(boundary(ind2sub1(k) + neigh[,i])) }

print( ind2sub1( sub2ind1(c(13, 27)) ) )
print( sub2ind1( ind2sub1(134) ) )

W <- array(1, c(n,n))

x0 <- c(n/2 + 1 , n/2 + 1)

I <- c(sub2ind1(x0))

D <- array(1, c(n,n)) + Inf
u <- ind2sub1(I)
D[u[1],u[2]] <- 0

S <- array(0, c(n,n))
S[u[1],u[2]] <- 1

extract   <- function(x,I){ x[I] }
extract1d <- function(x,I){ extract(as.vector(t(x)),I) }

j <- order( extract1d(D,I)  )
j <- j[1]
i <- I[j]       
a <-  I[j]
I <- I[-c(j)]

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

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

imageplot(S)

DNeigh <- function(D,k){ extract1d(D,Neigh(j,k)) }
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)
}

options(repr.plot.width=5, repr.plot.height=5)

source("nt_solutions/fastmarching_0_implementing/exo1.R")
D <- exo1(x0,W)

## Insert your code here.

displ <- function(D){ cos(2*pi*5*D/max(D) ) }

# color map function
cmap_jet <- function(v){ return( rgb(v, (sin(v*2*pi)+1)/2, (cos(v*2*pi)+1)/2) ) }

options(repr.plot.width=3.5, repr.plot.height=3.5)

plot(as.cimg(displ(D)), colourscale=cmap_jet, interpolate = FALSE, axes = FALSE)

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 }  
if (Delta<0){
    D[u[1],u[2]] <- min(dx + w, dy + w) }

options(repr.plot.width=5, repr.plot.height=5)

source("nt_solutions/fastmarching_0_implementing/exo2.R")
D <- exo2(x0,W)

## Insert your code here.

options(repr.plot.width=3.5, repr.plot.height=3.5)

plot(as.cimg(displ(D)), colourscale=cmap_jet, interpolate = FALSE, axes = FALSE)

n <- 100
x <- seq(-1, 1, length=n)
grid <- meshgrid_2d(x, x)
Y <- grid$X ; X <- grid$Y
sigma <- 0.2
W <- 1 + 8 * exp(-(X**2 + Y**2)/ (2*sigma**2))

imageplot(W)

x0 <- c(round(.1*n)+1, round(.1*n)+1)

options(repr.plot.width=3.5, repr.plot.height=3.5)

source("nt_solutions/fastmarching_0_implementing/exo3.R")
D <- exo3(x0,W)

## Insert your code here.

G0 <- grad(D)

d <- sqrt(apply(G0**2, c(1,2), sum))
U <- array(0, c(n,n,2))
U[,,1] <- d
U[,,2] <- d
G <- G0 / U

tau <- .8

x1 <- round(.9*n) + 1i*round(.88*n)
gamma <- c(x1)

Geval <- function(G,x){ bilinear_interpolate(G[,,1], Im(x), Re(x) ) + 1i * bilinear_interpolate(G[,,2],Im(x), Re(x)) }

g <- Geval(G, gamma[length(gamma)])

gamma <- c(gamma, gamma[length(gamma)] - tau*g)

source("nt_solutions/fastmarching_0_implementing/exo4.R")
gamma <- exo4(tau,x0,x1,G)

## Insert your code here.

imageplot(W)
lines(Im(gamma), Re(gamma), col="blue", lwd=3)
points(x0[2], x0[1], col="red", pch=19, lwd=4)
points(Im(x1), Re(x1), col="green", pch=19, lwd=4)
<script>
  $(document).ready(function(){
      $('div.prompt').hide();
  });
</script>