### This function carries out either Interative Conditional Modes estimation (ICM) [if ICM=TRUE]
### or an MCMC simulation using Metrpolis-Hastings steps [if ICM=FALSE]

metrop<-function(x,mu,nit,beta=1.5,ICM=FALSE){
nx<-dim(x)[1]
ny<-dim(x)[2]
nacc<-0
mu1<-mean(x[mu==0])
sig1<-(var(x[mu==0]))
mu2<-mean(x[mu>0])
sig2<-(var(x[mu>0]))

nprop<-0

print("No. of proposed pixels changes | no of acceptances")

for (it in 1:nit){

######## Note the edge pixels will not be estimated - the program needs adjusting to
######## take this into account

rangei<-sample(1:(nx))
rangej<-sample(1:(ny))


for (i1 in rangei){
for (j1 in rangej){

nprop<-nprop+1
if ((nprop/10000)==trunc(nprop/10000)) {
print(c(nprop,nacc))
image(mu)
}

muold<-mu[i1,j1]
munew<-1-mu[i1,j1]
xpixel<-x[i1,j1]

if (munew==1){
mupixelnew<-mu2
mupixelold<-mu1
sigpixnew<-sig2
sigpixold<-sig1
}
if (munew==0){
mupixelnew<-mu1
mupixelold<-mu2
sigpixnew<-sig1
sigpixold<-sig2
}


## work out the neighbours
if ((i1>=2)&(i1<=(nx-1))){
rangex<-c( (i1-1):(i1+1) )
}
if (i1==1){
rangex<- c( 1:2 )
}
if (i1==nx){
rangex <- c( (nx-1):nx )
}
if ((j1>=2)&(j1<=(ny-1))){
rangey<-c( (j1-1):(j1+1) )
}
if (j1==1){
rangey<- c( 1:2 )
}
if (j1==ny){
rangey <- c( (ny-1):ny )
}

#  count up like and different neighbours
NLikeNeigh<-sum(mu[rangex,rangey]==munew)
NdiffNeigh<-sum(mu[rangex,rangey]!=munew)



prior <- (beta*( NLikeNeigh - NdiffNeigh ))
like <-  log( sigpixold/sigpixnew      )  -1/(2*sigpixnew)*( xpixel - mupixelnew )**2 + 1/(2*sigpixold)*( xpixel - mupixelold )**2

mh<-exp(like+prior)


u<-runif(1)


if (mh>u){
mu[i1,j1]<-munew
nacc<-nacc+1
}

}
}


}
mu
}



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


icm<-function(x,mu,beta=1.5){
nx<-dim(x)[1]
ny<-dim(x)[2]
nacc<-0
mu1<-mean(x[mu==0])
sig1<-(var(x[mu==0]))
mu2<-mean(x[mu>0])
sig2<-(var(x[mu>0]))

nprop<-0

print("No. of proposed pixels changes | no of acceptances")

check<-TRUE

while (check == TRUE){

######## Note the edge pixels will not be estimated - the program needs adjusting to
######## take this into account
check <-FALSE

rangei<-sample(1:(nx))
rangej<-sample(1:(ny))


for (i1 in rangei){
for (j1 in rangej){

nprop<-nprop+1
if ((nprop/225)==trunc(nprop/225)) {
print(c(nprop,nacc))
image(mu)
}

muold<-mu[i1,j1]
munew<-1-mu[i1,j1]
xpixel<-x[i1,j1]

if (munew==1){
mupixelnew<-mu2
mupixelold<-mu1
sigpixnew<-sig2
sigpixold<-sig1
}
if (munew==0){
mupixelnew<-mu1
mupixelold<-mu2
sigpixnew<-sig1
sigpixold<-sig2
}


## work out the neighbours
if ((i1>=2)&(i1<=(nx-1))){
rangex<-c( (i1-1):(i1+1) )
}
if (i1==1){
rangex<- c( 1:2 )
}
if (i1==nx){
rangex <- c( (nx-1):nx )
}
if ((j1>=2)&(j1<=(ny-1))){
rangey<-c( (j1-1):(j1+1) )
}
if (j1==1){
rangey<- c( 1:2 )
}
if (j1==ny){
rangey <- c( (ny-1):ny )
}

#  count up like and different neighbours
NLikeNeigh<-sum(mu[rangex,rangey]==munew)
NdiffNeigh<-sum(mu[rangex,rangey]!=munew)



prior <- (beta*( NLikeNeigh - NdiffNeigh ))
like <-  log( sigpixold/sigpixnew      )  -1/(2*sigpixnew)*( xpixel - mupixelnew )**2 + 1/(2*sigpixold)*( xpixel - mupixelold )**2

mh<-exp(like+prior)

u<-1

if (mh>u){
mu[i1,j1]<-munew
nacc<-nacc+1
check<-TRUE
}

}
}


}

mu
}



####################################################################
#The main program

#First of al a true image is defined (mu) and then some normal noise
#is added with mean 0 and standard deviation sigma to give the observed
#noisy image (x)
#####################################################################

nx<-15
ny<-15

### parameters
sigma<-1
betaprior <- 1.5


mu<-matrix(0,nx,ny)
x<-matrix(0,nx,ny)
for (i in 1:nx){
for (j in 1:ny){

########### specify the true image here

# half and half
if (j > nx/2)
{ mu[i,j] <-1 }

# disk
#if  ( ( (i-7)**2 + (j-7)**2 ) < 5 )  
#{ mu[i,j] <-1 }

# stripes
#if ( (i/2)== trunc(i/2) ){
#{ mu[i,j] <-1 }
#}

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

x[i,j]<-rnorm(1,mu[i,j],sigma)
}
}
par(mfrow=c(2,2))
image(mu)
trumu<-mu
title("The truth")
image(x)
noisyimage<-x
title("Noisy image")


#### Now threshold the observed image to two classes based on whether
#### values are above or below the mean pixel value

mu0<-matrix(0,nx,ny)
hist(x)
mm<-0.5
mu0[x>mm]<-1
mu0[x<=mm]<-0
image(mu0)
title("initial threshold")


print("PRESS RETURN to continue")
ans<-readline()  ### wait until return IS PRESSED

######## run ICM for 1000 iterations


par(mfrow=c(1,1))

print("Carrying out ICM")
muhaticm<-icm(x,mu0,beta=betaprior)
image(muhaticm)
title("ICM estimate")



######## run MCMC for 1000 iterations

print("Carrying out MCMC")
muhatmcmc<-metrop(x,mu0,beta=betaprior,nit=1000)
image(muhatmcmc)
title("MCMC simulated value")


######## display the images and estimates

par(mfrow=c(2,2))
image(trumu)
title("Truth")
image(noisyimage)
title("Noisy image")
image(muhaticm)
title("ICM estimate")
image(muhatmcmc)
title("MCMC simulation")

print("Error measure ICM")
print(sum(c(trumu-muhaticm)**2))
print("Error measure MCMC simulated")
print(sum(c(trumu-muhatmcmc)**2))