#
# The following is a rudimentary example of a search strategy based on Bayesian
# principles. The primary resource for this material is:
#
# Stone, L., D. (1975). Theory of Optimal Search. Mathematics in Science and
#     Engineering, Vol. 118. New York: Academic Press, Inc.
#
# To get a quick view of what these methods were developed for, see:
#
# The US loss of Nuclear bombs off the coast of Spain:
#     http://en.wikipedia.org/wiki/1966_Palomares_B-52_crash
#
# The US loss of the nuclear sub, USS Scorpion:
#     http://en.wikipedia.org/wiki/USS_Scorpion_%28SSN-589%29
#
# Also strongly recommend the following book which relates the history of Bayes Rule:
#     McGrayne, S. B. (2011). The Theory That Would Not Die: How Bayes' Rule Cracked
#     the Enigma Code, Hunted Down Russian Submarines, and Emerged Triumphant from Two
#     Centuries of Controversy. Yale University Press.
#     http://yalepress.yale.edu/book.asp?isbn=9780300169690
#
################################################################################
################################################################################

# Generate the data; remember to re-size the graphics window so that the 3-D
# perspective plot is large enough to see countour lines changing as the prior
# gets updated.

library(MASS)
set.seed(321)
x.y<-mvrnorm(n=500, mu=c(0,0), Sigma=matrix(c(1,.60,.60,1),
             ncol=2), empirical=T)
x<-x.y[,1]
y<-x.y[,2]
cor(x,y)
plot(x,y)

f<-kde2d(x,y,n=50)

oldpar <- par(oma=c(0,0,3,0), mfrow=c(2,1))
plot(x,y)
persp(f$x,f$y,f$z,shade=.3, theta = 320, phi = 20,
      expand = 0.5, col = "lightblue",ticktype = "detailed")
par(oldpar)

rm(x,x.y,y)
ls()

###############################
###############################

# Loop 'j' is within loop 'i'; run the entire 'i' loop (with the 'j' loop embedded
# in it).

times.found.vec <- as.vector(0)
values.list <- as.list(0)
found.iter <- as.vector(0)
flags.list <- as.list(0)
mean.flags <- as.vector(0)

for (i in 1:100){

# Define the amount which will degrade the probability for a location which has
# been visited (and if desired, also increase the probabilities for locations
# not visited) at a particular iteration.

tweak <- 1/(50*50)
tweak

# Define the target; either as the maximum or located relative to the maximum.

max(f$z)
target <- which(f$z == max(f$z), arr.ind = TRUE)
target[1] <- target[1] + 3
target[2] <- target[2] + 2
target

# Setting prior probabilties (i.e. global map normalization).

probs <- f$z/sum(f$z)

# Define the original starting position.

orig.x <- min(f$x) # Also known as "f$x[1]".
orig.y <- min(f$y) # Also known as "f$y[1]".

oldpar <- par(oma = c(0,0,3,0), mfrow = c(2,1))
plot(orig.y, orig.x, xlim = c(-3,3), ylim = c(-3,3),
     xlab = "X", ylab = "Y",
     col = "blue", pch = 1)
persp(f$x, f$y, probs, shade=.3, theta = 320,
      phi = 20, expand = 0.5, col = "lightblue", ticktype = "detailed")
par(oldpar)

# Defining the SubScript starting points (subscripts for the matrix).

x.ss <- 2
y.ss <- 2

probs[y.ss, x.ss]
probs[2,2]

# Creating the data frame which will store information from each iteration and
# setting the initial conditions.

values <- data.frame(0)
N <- 1250

# Setting the initial 'decision' probabilities (each direction is equally
# weighted at 1/8th).

ul.prob <- .125
u.prob <- .125
ur.prob <- .125
l.prob <- .125
r.prob <- .125
dl.prob <- .125
d.prob <- .125
dr.prob <- .125
decision.probs <- data.frame(ul.prob, u.prob, ur.prob, l.prob, r.prob, dl.prob, d.prob,
                             dr.prob)

# Creating a counter for the number of times the search visits the target's location.

times.found <- 0

###############################

# Initiate the search (loop 'j').

for (j in 1: N){

# Marking the boundaries.

if(y.ss <= 2){y.ss <- 2}
if(x.ss <= 2){x.ss <- 2}
if(y.ss >= 48){y.ss <- 48}
if(x.ss >= 48){x.ss <- 48}

# Getting the surrounding values (i.e. defining the direction possibilities).

up.left    <- probs[y.ss - 1, x.ss - 1]
up         <- probs[y.ss - 1, x.ss]
up.right   <- probs[y.ss - 1, x.ss + 1]
left       <- probs[y.ss, x.ss - 1]
right      <- probs[y.ss, x.ss + 1]
down.left  <- probs[y.ss + 1, x.ss - 1]
down       <- probs[y.ss + 1, x.ss]
down.right <- probs[y.ss + 1, x.ss + 1]
directions <- data.frame(up.left, up, up.right, left, right, down.left, down, down.right)

# Update (decrease) the value for the location you were at (and if desired: increase
# all other values -- which speeds up the process slightly). The third choice is to
# drop the value at the current location to "tweak" (which here with 1/(50X50) is 0.0004).

probs[y.ss,x.ss] <- probs[y.ss,x.ss] - (tweak * probs[y.ss,x.ss])
probs[-y.ss,-x.ss] <- probs[-y.ss,-x.ss] + (tweak * probs[-y.ss,-x.ss])
# probs[y.ss,x.ss] <- .0001

# RE-normalize the global values.

probs <- probs/sum(probs)

# Getting the *NEW* surrounding values (i.e. defining the direction possibilities).

new.up.left    <- probs[y.ss - 1, x.ss - 1]
new.up         <- probs[y.ss - 1, x.ss]
new.up.right   <- probs[y.ss - 1, x.ss + 1]
new.left       <- probs[y.ss, x.ss - 1]
new.right      <- probs[y.ss, x.ss + 1]
new.down.left  <- probs[y.ss + 1, x.ss - 1]
new.down       <- probs[y.ss + 1, x.ss]
new.down.right <- probs[y.ss + 1, x.ss + 1]
new.directions <- data.frame(new.up.left, new.up, new.up.right, new.left, new.right, 
                             new.down.left, new.down, new.down.right)

# Local normalization.

new.directions <- new.directions/sum(new.directions)

##### Some options for fixing the way the search progresses.

# Several strategies can be specified for whether a directed step (toward the maximum)
# or non directed step should take place. (Some of these are commented out, because only one
# flag strategy can be specified for a run).
# The best choice is the one not commented out (i.e. 60% chance favoring a directed step).

# flag <- 1                                  # Fully directed search
# flag <- rbinom(1, 1, prob = .50))          # Oscilation between Fully directed & Partial directed.
flag <- rbinom(1, 1, prob = .60)           # Oscilation between Fully directed & Partial directed.
# flag <- rbinom(1, 2, prob = 1/3)           # Oscilation between all three.
# flag <- 0                                  # Partially directed search
# flag <- 2                                  # Fully random search.

#####

# Below, "d" is directed; "p" is partially directed ("r" is fully random; if used).

dir.max <- max(new.directions)
d <- 1 - dir.max
p <- 1 - d
# p <- (1 - d) * .75
# r <- (1 - d) * .25
# bob <- c(1, 0, 2)
# flag <- sample(bob, 1, prob = c(d,p,r))
# flag <- rbinom(1, 1, prob = c(d,p))

# Normalize and then combine (old) Directions with Decision Probabilities.

directions <- directions/sum(directions)

dir.probs <- directions * decision.probs

# Normalize those.

dir.probs <- dir.probs/sum(dir.probs)

### Move and update the position (subscripts).

# If flag = 1, then directed maximum choice (i.e. best.choice).

if(flag == 1){
  choice.1 <- dir.max
  if(choice.1 == new.directions[1]){new.y.ss <- y.ss - 1; new.x.ss <- x.ss - 1}
  if(choice.1 == new.directions[2]){new.y.ss <- y.ss - 1; new.x.ss <- x.ss}
  if(choice.1 == new.directions[3]){new.y.ss <- y.ss - 1; new.x.ss <- x.ss}
  if(choice.1 == new.directions[4]){new.y.ss <- y.ss; new.x.ss <- x.ss - 1}
  if(choice.1 == new.directions[5]){new.y.ss <- y.ss; new.x.ss <- x.ss + 1}
  if(choice.1 == new.directions[6]){new.y.ss <- y.ss + 1; new.x.ss <- x.ss - 1}
  if(choice.1 == new.directions[7]){new.y.ss <- y.ss + 1; new.x.ss <- x.ss}
  if(choice.1 == new.directions[8]){new.y.ss <- y.ss + 1; new.x.ss <- x.ss + 1}
  }

if(flag == 0 | flag == 2){

# If flag == 0, then a somewhat random choice is made (choice made with a sample
# function of the weighted directional probabilities).

if(flag == 0){
  choice <- sample(dir.probs, 1, prob = dir.probs)
  }

# If flag == 2; then a completely random choice.

if(flag == 2){
  choice <- sample(dir.probs, 1, .125)
  }

if(choice == dir.probs[1]){new.y.ss <- y.ss - 1; new.x.ss <- x.ss - 1}
if(choice == dir.probs[2]){new.y.ss <- y.ss - 1; new.x.ss <- x.ss}
if(choice == dir.probs[3]){new.y.ss <- y.ss - 1; new.x.ss <- x.ss}
if(choice == dir.probs[4]){new.y.ss <- y.ss; new.x.ss <- x.ss - 1}
if(choice == dir.probs[5]){new.y.ss <- y.ss; new.x.ss <- x.ss + 1}
if(choice == dir.probs[6]){new.y.ss <- y.ss + 1; new.x.ss <- x.ss - 1}
if(choice == dir.probs[7]){new.y.ss <- y.ss + 1; new.x.ss <- x.ss}
if(choice == dir.probs[8]){new.y.ss <- y.ss + 1; new.x.ss <- x.ss + 1}

}

# Convert new stuff to old stuff for next loop.

decision.probs <- dir.probs

x.ss <- new.x.ss
y.ss <- new.y.ss

orig.x <- f$x[x.ss]
orig.y <- f$y[y.ss]

# Plotting.

oldpar <- par(oma = c(0,0,3,0), mfrow = c(2,1))
plot(orig.y, orig.x, xlim = c(-3,3), ylim = c(-3,3),
     xlab = "X", ylab = "Y",
     col = "blue", pch = 1)
persp(f$x, f$y, probs, shade=.3, theta = 320,
      phi = 20, expand = 0.5, col = "lightblue", ticktype = "detailed")
par(oldpar)

# Saving information from the iteration.

values[j,1] <- x.ss
values[j,2] <- y.ss
values[j,3] <- probs[y.ss, x.ss]
values[j,4] <- flag

# Break out criterion (i.e. found criterion = 85% chance of 'seeing' it when
# search arrives at the target's location).

if(y.ss == target[1] & x.ss == target[2]){
   times.found <- times.found + 1
   found <- rbinom(1, 1, .85)
     if(found == 1){break}
   }

}

# End of search (loop 'j').

###############################

times.found.vec[i] <- times.found
values.list[[i]] <- values
found.iter[i] <- nrow(values)
flags.list[[i]] <- values$V4
mean.flags[i] <- mean(values$V4)

print(i)
print(j)
print(times.found)

}

# End of loop 'i'.

###############################
###############################

summary(found.iter)
sd(found.iter)
hist(found.iter)

mean(times.found.vec)
iter.found <- data.frame(found.iter, times.found.vec)
head(iter.found)

flags.list[[5]]
mean(mean.flags)
values.list[[5]]

# End, June 13, 2011.