drivePath  <- function(nturns = 10, nblocks = 50, prob = c(1/3,1/3,1/3)) {
#' This is a simulation of a car driving in a city and taking random
#' turns left, right, or not turn at all, with equal probability. If
#' we do not turn, we continue in the same direction we came from.
#' @param nturns The number of turns and no-turns
#' @param nblock The Number of blocks in each direction
#' @return The path, 2-by-nturns matrix where each column has x-y
#'     coordinates of the intersection.
    n <- nturns
    nb <- nblocks
    ## Sample n-1 from directions left=L, right=R, straight=S
    dirs <- sample(c("L","R","S"), n-1, replace=T, prob = prob)
    ## Initial position
    pos <- c(1, 0)
    ## Initialize path to contain the first segment
    path <- cbind(pos)
    ## Initial displacement
    displ <- c(1,0)
    for(i in 1:length(dirs)) {
        dir <- dirs[i];

        ## Compute new displacement
        if( all.equal(displ, c(1,0)) == TRUE) {
            if (dir == "L") {
                displ <- c(0,1)
            } else if(dir=="R") {
                displ <- c(0,-1)
            }
        } else if( all.equal(displ, c(0,1)) == TRUE) {
            if (dir == "L") {
                displ <- c(-1,0)
            } else if(dir=="R") {
                displ <- c(1,0)
            }
        } else if( all.equal(displ, c(-1,0)) == TRUE) {
            if (dir == "L") {
                displ <- c(0,-1)
            } else if(dir=="R") {
                displ <- c(0,1)
            }
        } else if( all.equal(displ, c(0,-1)) == TRUE) {
            if (dir == "L") {
                displ <- c(1,0)
            } else if(dir=="R")
            {
                displ <- c(-1,0)
            }
        }
        ## Add new displacement to position
        pos <- pos + displ
        ## Update path
        path <- cbind(path, pos)
    }
    return(path);
}

drive  <- function(nturns = 10, nblocks = 50, prob = c(1/3,1/3,1/3)) {
#' Plots the path of a car driving according to function drivePath.
#' @param nturns The number of turns and no-turns.
#' @param nblock The Number of blocks in each direction.
    n <- nturns
    nb <- nblocks
    path <- drivePath(nturns, nblocks, prob)
    ## Begin the plot
    plot(NULL,type=NULL,xlim=c(-nb,nb),ylim=c(-nb,nb),xlab='x',ylab='y',asp=1)
    myColors <- palette(rainbow(n))

    ## Plot initial position
    pos  <- path[,0]
    points(pos[1], pos[2], col=myColors[0],pch=19)
    
    for(i in 1:dim(path)[2]) { 
        pos  <- path[,i]
        ## Plot current position in the i-th color
        points(pos[1], pos[2], col=myColors[i], pch=19)
    }

    ## Plot the path
    lines(t(path),type="l",lwd=2,col="magenta")

    ##grid(nx=2*n, ny=2*n)
    ## Draw grid manually
    for (x in -nb:nb)   lines(c(x,x),c(-nb,nb), col="gray",lty=2)
    for (y in -nb:nb)   lines(c(-nb,nb),c(y,y), col="gray",lty=2)
}