##########################################################################
#Splus functions for automatic image segmentation.                       #
#All functions in this file written by Derek Stanford                    #
#Copyright 1999 Derek Stanford                                           #
#Permission is hereby granted for noncommercial use provided that the    #
#author is appropriately acknowledged.                                   #
#For commercial use, please contact the author at                        #
#isomorphicresearch@yahoo.com                                            #
#for licensing information.                                              #
##########################################################################
#                                                                        #
#  For more information, see "Fast Automatic Unsupervised Image          #
#  Segmentation and Curve Detection in Spatial Point Processes" by       #
#  Derek Stanford, PhD dissertation, University of Washington Statistics #
#  Department, 1999.                                                     #
#                                                                        #
##########################################################################
#
#  autosegment8 takes an image as input and chooses a number of segments 
#       between 1 and 8.  It is essentially a batch job written as a 
#       function; it can be changed to address different numbers of 
#       segments.  The following functions are used.
#
#  segment.marginal does initial segmentation by Ward and then EM fitting
#  ward.initclass does ward's algorithm fitting
#  emfast.alg does EM tailored to 256-level greyscale data.
#  estimate.icm.phi0 refines a segmentation by using ICM with parameter 
#       estimation, with phi constrained to be non-negative
#  icm performs ICM (without parameter estimation)
#  pseudol.unorder computes the final pseudolikelihood
#  estimate.icm is an unconstrained version of estimate.icm.phi0
#  morphit performs greyscale opening-closing (=erode,dilate,dilate,erode)
#  greyreorder.fn reorders the grey values in an image for display
#
#  other functions are subroutines
#
##########################################################################
#
#  Example of use (commands in this section are commented out so that this 
#  file may be sourced into Splus).
#
#  Suppose the image of interest is contained in a matrix called "imagedata",
#  and suppose we wish to segment it into 5 or fewer segments.
#  First, compute a marginal segmentation into 5 segments.
#
#image.marg5<-segment.marginal(imagedata,k=5)
#
#  Next, perform ICM to refine the segmentation.
#
#image.icm5<-estimate.icm.phi0(imagedata,x.init = image.marg5$em.classif)
#
#  Last, compute the pseudolikelihood of the result.
#
#image.pl5<-pseudol.unorder(imagedata,image.icm5$x, phi=image.icm5$phi,
#                           means=image.icm5$means, sds=image.icm5$sds)
#
#  At this point, the marginal segmentation is given by 
#  "image.marg5$em.classif" and the spatial segmentation is given by 
#  "image.icm5$x".  These can be viewed using the Splus "image" function.
#
#  The process can be repeated for other numbers of segments, e.g. 1 to 4, 
#  and then the pseudolikelihood values can be used to choose the optimal 
#  number of segments.  When computing a marginal segmentation for a new 
#  number of segments, the result from a larger number of segments can 
#  be used as a starting value so that less computation is required; the 
#  following line illustrates this.
#
#image.marg4<-segment.marginal(imagedata,k=4,lastinit=image.marg5$ward.last)
#
##########################################################################

autosegment8<-
function(y, trace = F,iterlimit=10)
{
#y is image (matrix)
#this function fits 1 to 8 segments
        lastward <- NA  #marginal segmentation by EM, initialized by Ward
        k <- 8
        seg8 <- segment.marginal(y, k = k, trace = trace, lastinit = lastward)
        lastward <- seg8$ward.last
        k <- 7
        seg7 <- segment.marginal(y, k = k, trace = trace, lastinit = lastward)
        lastward <- seg7$ward.last
        k <- 6
        seg6 <- segment.marginal(y, k = k, trace = trace, lastinit = lastward)
        lastward <- seg6$ward.last
        k <- 5
        seg5 <- segment.marginal(y, k = k, trace = trace, lastinit = lastward)
        lastward <- seg5$ward.last
        k <- 4
        seg4 <- segment.marginal(y, k = k, trace = trace, lastinit = lastward)
        lastward <- seg4$ward.last
        k <- 3
        seg3 <- segment.marginal(y, k = k, trace = trace, lastinit = lastward)
        lastward <- seg3$ward.last
        k <- 2
        seg2 <- segment.marginal(y, k = k, trace = trace, lastinit = lastward)
        lastward <- seg2$ward.last
        k <- 1
        seg1 <- segment.marginal(y, k = k, trace = trace, lastinit = lastward)
        #ICM with parameter estimation
        seg8.icm <- estimate.icm.phi0(y = y, x.init = seg8$em.classif, max.iter
                 = iterlimit, trace = T)
        seg7.icm <- estimate.icm.phi0(y = y, x.init = seg7$em.classif, max.iter
                 = iterlimit, trace = T)
        seg6.icm <- estimate.icm.phi0(y = y, x.init = seg6$em.classif, max.iter
                 = iterlimit, trace = T)
        seg5.icm <- estimate.icm.phi0(y = y, x.init = seg5$em.classif, max.iter
                 = iterlimit, trace = T)
        seg4.icm <- estimate.icm.phi0(y = y, x.init = seg4$em.classif, max.iter
                 = iterlimit, trace = T)
        seg3.icm <- estimate.icm.phi0(y = y, x.init = seg3$em.classif, max.iter
                 = iterlimit, trace = T)
        seg2.icm <- estimate.icm.phi0(y = y, x.init = seg2$em.classif, max.iter
                 = iterlimit, trace = T)
        seg1.icm <- estimate.icm.phi0(y = y, x.init = seg1$em.classif, max.iter
                 = iterlimit, trace = T)       #compute BIC_PL
        seg8.pl <- pseudol.unorder(y, seg8.icm$x, phi = seg8.icm$phi, means = 
                seg8.icm$means, sds = seg8.icm$sds)
        seg7.pl <- pseudol.unorder(y, seg7.icm$x, phi = seg7.icm$phi, means = 
                seg7.icm$means, sds = seg7.icm$sds)
        seg6.pl <- pseudol.unorder(y, seg6.icm$x, phi = seg6.icm$phi, means = 
                seg6.icm$means, sds = seg6.icm$sds)
        seg5.pl <- pseudol.unorder(y, seg5.icm$x, phi = seg5.icm$phi, means = 
                seg5.icm$means, sds = seg5.icm$sds)
        seg4.pl <- pseudol.unorder(y, seg4.icm$x, phi = seg4.icm$phi, means = 
                seg4.icm$means, sds = seg4.icm$sds)
        seg3.pl <- pseudol.unorder(y, seg3.icm$x, phi = seg3.icm$phi, means = 
                seg3.icm$means, sds = seg3.icm$sds)
        seg2.pl <- pseudol.unorder(y, seg2.icm$x, phi = seg2.icm$phi, means = 
                seg2.icm$means, sds = seg2.icm$sds)
        seg1.pl <- pseudol.unorder(y, seg1.icm$x, phi = seg1.icm$phi, means = 
                seg1.icm$means, sds = seg1.icm$sds)
        return(seg8, seg7, seg6, seg5, seg4, seg3, seg2, seg1, seg8.icm, 
                seg7.icm, seg6.icm, seg5.icm, seg4.icm, seg3.icm, seg2.icm, 
                seg1.icm, seg8.pl, seg7.pl, seg6.pl, seg5.pl, seg4.pl, seg3.pl,
                seg2.pl, seg1.pl)
}

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

segment.marginal<-
function(y, k = 3, trace = F, lastinit = NA, iter.limit = 40)
{
#y is an image - a matrix of values
#k is the number of segments 
#trace causes trace info to be printed
#lastinit can contain a segmentation into k+1 (for Ward)
        if(trace) {
                cat(k, "segments.  Ward ")
        }
        maxrows <- dim(y)[1]
        maxcols <- dim(y)[2]
        y.vec <- as.vector(y)   #Check if only 1 segment
        if(k == 1) {
                ward.classif <- rep(0, (maxrows * maxcols))
                ward.last <- NA
                init.classif <- ward.classif
                em.classif <- ward.classif
                means <- mean(y.vec)
                sds <- sqrt(var(y.vec))
                if(sds < 0.5) {
                        sds <- 0.5
                }
                mixprobs <- 1   
        }
        else {
# k>1
#Ward's method on marginal values using my function
#   (slower than mclust, but doesn't crash!)
                ward.classif <- ward.initclass(y, k, x.init = lastinit) 
        #save classif for next loop if using loop
                ward.last <- ward.classif$classif
                ward.classif <- as.vector(ward.classif$classif)
#force zero-indexed integer class values
                temp.vals <- unique(ward.classif)
                temp.classif <- ward.classif
                for(i in 1:length(temp.vals)) {
                        ward.classif[temp.classif == temp.vals[i]] <- (i - 1)
                }
                init.classif <- ward.classif
#EM to estimate parameters and classify (uses mixture likelihood)
#Note: requires zero-indexed init.classif
                this.em <- emfast.alg(y.vec, init.classif, conv.crit = 0.001, 
                        max.iter = iter.limit, small.out = T, trace = F, 
                        sd.bound = 0.5)
                means <- this.em$means
                sds <- this.em$sds
                em.classif <- this.em$classif
                mixprobs <- this.em$segment.probs       
                init.classif <- matrix(init.classif, ncol = dim(y)[2])
                em.classif <- matrix(em.classif, ncol = dim(y)[2])
        }
        if(trace) {
                cat("Done, k=", k, " \n")
        }
        if(trace) {
                cat("Means: ", round(means, dig = 2), " \n")
        }
        if(trace) {
                cat("SDs  : ", round(sds, dig = 2), " \n")
        }
        if(trace) {
                cat("Probs: ", round(mixprobs, dig = 3), " \n\n")
        }
        return(y, init.classif, em.classif, means, sds, ward.last)
}
#######################################

ward.initclass<-
function(x, k, x.init = NA, trace = F, longout = F)
{
#Inputs:
#x is an image (matrix)
#k is the number of groups
#x.init is an initial classification to start from, same size as x
#if trace is T, information will be printed during execution.
#if longout is T, x will be included in the output returned
#
#Output:
#classif, a matrix the same size as x, with the classification
#k, same as input
#x, same as input (if longout=T)
#
#This function is written to do an initial segmentation of a greyscale
#image (matrix) by applying Ward's method as a clustering criterion on
#the marginal distribution of pixel values (no regard to spatial
#information).  This initial segmentation could then be passed to a
#more sophisticated algorithm (such as ICM) for further processing.
#
#Example of use:
#thisimage<-matrix(scan("imagefile"),byrow=T,ncol=200)
#thisimage.5<-ward.initclass(thisimage,k=5)
#thisimage.4<-ward.initclass(thisimage,k=4,x.init=thisimage.5$classif)
        if(trace) {
                cat("Initializing.\n")
        }
        xlevels <- unique(x)
        levelorder <- order(xlevels)
        xhist <- rep(0, length(xlevels))
        for(i in 1:length(xlevels)) {
                xhist[i] <- sum(x == xlevels[i])
        }
        xlevels <- xlevels[levelorder]
        xhist <- xhist[levelorder]
        if(is.na(x.init[1])) {
                histlabels <- c(1:length(xlevels))
        }
        else {
                histlabels <- c(1:length(xlevels))
                for(i in 1:length(xlevels)) {
                        histlabels[i] <- unique(x.init[x == xlevels[i]])
                }
        }
        if(length(unique(histlabels)) <= k) {
                stop("Need larger K\n")
        }
        done <- F
        if(trace) {
                cat("Beginning clustering.\n")
        }
        while(done == F) {
#iterate over all possible adjacent merges
                nonmergess <- rep(NA, (length(unique(histlabels))))
                for(i in 1:(length(unique(histlabels)))) {
#compute ss of group i
                        groupasizes <- xhist[histlabels == (unique(histlabels)[
                                i])]
                        if(length(groupasizes) == 1) {
                                nonmergess[i] <- 0
                        }
                        else {
                                groupavalues <- xlevels[histlabels == (unique(
                                  histlabels)[i])]
                                thismean <- (groupasizes %*% groupavalues)/sum(
                                  groupasizes)
                                thisdev <- (groupavalues - thismean)
                                thisdevsqr <- thisdev^2
                                thisss <- thisdevsqr %*% (groupasizes)
                                nonmergess[i] <- thisss
                        }
                }
#iterate over all possible adjacent merges
                mergess <- rep(NA, (length(unique(histlabels)) - 1))
                for(i in 1:(length(unique(histlabels)) - 1)) {
#compute ss of merge of i and i+1
                        groupasizes <- xhist[histlabels == (unique(histlabels)[
                                i])]
                        groupavalues <- xlevels[histlabels == (unique(
                                histlabels)[i])]
                        groupbsizes <- xhist[histlabels == (unique(histlabels)[
                                i + 1])]
                        groupbvalues <- xlevels[histlabels == (unique(
                                histlabels)[i + 1])]
                        thismean <- ((groupasizes %*% groupavalues) + (
                                groupbsizes %*% groupbvalues))/sum(c(
                                groupasizes, groupbsizes))
                        thisdev <- c((groupavalues - thismean), (groupbvalues - 
                                thismean))
                        thisdevsqr <- thisdev^2
                        thismerge <- thisdevsqr %*% c(groupasizes, groupbsizes)
                        if(i > 1) {
                                if(i < (length(nonmergess) - 1)) {
                                  otherss <- sum(nonmergess[1:(i - 1)]) + sum(
                                    nonmergess[(i + 2):(length(nonmergess))])
                                }
                                else {
                                  otherss <- sum(nonmergess[1:(i - 1)])
                                }
                        }
                        else otherss <- sum(nonmergess[(i + 2):(length(
                                  nonmergess))])
                        mergess[i] <- thismerge + otherss
                }
#end of for
#choose merge to make
                bestmerge <- c(1:(length(unique(histlabels)) - 1))[mergess == 
                        min(mergess)]
                if(length(bestmerge) > 1) {
                        bestmerge <- bestmerge[1]
                }
#update histlabels
#(unique(histlabels)[bestmerge]) with (unique(histlabels)[bestmerge+1])
      histlabels[histlabels == (unique(histlabels)[bestmerge + 1])] <-
                        unique(histlabels)[bestmerge]
                if(trace) {
                        cat("Current clusters: ", length(unique(histlabels)), 
                                "\n")
                }
#check if we are done
                if(length(unique(histlabels)) == k) {
                        done <- T
                }
                if(length(unique(histlabels)) < k) {
                        done <- T
                        warning("Too few groups \n")
                }
        }
#end of main while loop 
        classif <- x
        for(i in 1:length(xlevels)) {
                classif[x == xlevels[i]] <- histlabels[i]
        }
        if(longout) {
                return(x, classif, k)
        }
        else {
                return(classif, k)
        }
}



######################################
emfast.alg<-function(y, init.class, conv.crit = 0.001, max.iter = 20, 
small.out = F, trace= T, sd.bound = 0.5  ) 
{
#
#Written by Derek Stanford 1998.
#
#y is the data
#init.class is the initial classification (must be consistent)
#conv.crit is the convergence criterion (change in loglikelihood)
#max.iter is the maximum number of iterations allowed
#if small.out=T, fewer items will be returned in output
#trace controls display of infor during execution
#sd.bound is a lower bound imposed on sd estimates
#
#
#This function uses EM to fit a mixture of Gaussians to the 
#marginal distribution of y, using the number of classes
#present in init.class.
#
#Since the prob of any pixel depends only on its value, we need not
#iterate over the whole dataset.  We only need to iterate over unique 
#values of in y.
        k <- length(unique(init.class)) #number of classes
        classes <- unique(init.class)   #index of classes (defines order)
        n <- length(y)
        values <- sort(unique(y))
        n.values <- length(values)
        values.hist <- table(y)
        dimnames(values.hist) <- NULL   #values.hist<-rep(0,n.values)
        if(trace) {
                cat("Starting. \n")
        }
        means <- rep(0, k)
        sds <- rep(0, k)
        segment.probs <- rep(0, k)   #initialize pixel.probs and segment.probs
        pixel.probs <- matrix(rep(0, n.values * k), ncol = k)
        for(i in 1:n) {
                for(j in 1:k) {
                        if(classes[j] == init.class[i]) {
                                pixel.probs[values == y[i], j] <- 1 + 
                                  pixel.probs[values == y[i], j]
                        }
                }
        }
        for(i in 1:n.values) {
                pixel.probs[i,  ] <- pixel.probs[i,  ]/sum(pixel.probs[i,  ])
        }
        for(j in 1:k) {
                segment.probs[j] <- sum(pixel.probs[, j])
        }
        segment.probs.total <- sum(segment.probs)
        if(segment.probs.total != n.values) {
                warning("Error 1 in segment probs.")
        }
        segment.probs <- segment.probs/segment.probs.total
        if(trace) {
                cat("Initial SegProbs ", round(segment.probs, dig = 5), " \n")
        }
        done <- F
        iters <- 0
        this.loglikelihood <- ( - Inf)
        while(!done) {
#start of main while loop
                iters <- iters + 1
#M-step: maximum likelihood to estimate parameters given current 
#pixel.probs
#update mu and sd
                for(j in 1:k) {
                        mu.numer <- 0
                        mu.denom <- 0
                        for(i in 1:n.values) {
                             mu.numer <- mu.numer + (values[i] * pixel.probs[
                                  i, j] * values.hist[i])
                                mu.denom <- mu.denom + (pixel.probs[i, j] * 
                                  values.hist[i])
                        }
                        means[j] <- mu.numer/mu.denom
                }
                for(j in 1:k) {
                        sd.numer <- 0
                        sd.denom <- 0
                        for(i in 1:n.values) {

                          sd.numer <- sd.numer + (((values[i] - means[j])^
                                  2) * pixel.probs[i, j] * values.hist[i])
                                sd.denom <- sd.denom + (pixel.probs[i, j] * 
                                  values.hist[i])
                        }
                        sds[j] <- sqrt(sd.numer/sd.denom)
                }
#impose lower bound on SDs
                sds[sds < sd.bound] <- sd.bound #update segment.probs
                for(j in 1:k) {
                  segment.probs[j] <- sum(pixel.probs[, j] * values.hist)/n
                }
                if(round(sum(segment.probs), dig = 2) != 1) {
                        stop("Error 2 in segment probs.\n")
                }
#E-step
#update pixel.probs using Gaussian likelihoods
                denom <- rep(0, n.values)
                for(i in 1:n.values) {
                        for(j in 1:k) {
                             denom[i] <- denom[i] + segment.probs[j] * dnorm(
                                  values[i], means[j], sds[j])
                        }
                }
                pixel.probs <- matrix(rep(0, n.values * k), ncol = k)
                for(i in 1:n.values) {
                        for(j in 1:k) {
                                if(denom[i] > 0) {
                                  pixel.probs[i, j] <- (segment.probs[j] * 
                                 dnorm(values[i], means[j], sds[j]))/denom[i]
                                }
                                else {
                                  pixel.probs[i, j] <- 0
                                }
                        }
                }
#compute overall mixture likelihood, sum_i log( sum_j Lij*Pj )
                last.loglikelihood <- this.loglikelihood
                this.loglikelihood <- sum(values.hist * log(denom))
                if(trace) {
                        cat("This l=", this.loglikelihood, ", Last l=", 
                                last.loglikelihood, " \n")
                }
#check for convergence
                if(abs(this.loglikelihood - last.loglikelihood) <= conv.crit) 
                        {
                        done <- T
                }
                if(iters == max.iter) {
                        done <- T
                }
                if(trace) {
                        cat("EM ", iters, ", Loglike ", round(
                                this.loglikelihood, dig = 4), "\n Means ", 
                                round(means, dig = 5), "\n")
                        cat("   SDS ", round(sds, dig = 5), "\n SegProbs ", 
                                round(segment.probs, dig = 5), " \n")
                }
# end of if
        }
#end of while loop
#classify based on max likelihood (useful for plotting)
        classif <- rep(0, n.values)
        for(i in 1:n.values) {
                this.max <- max(pixel.probs[i,  ])
                choice <- (pixel.probs[i,  ] == this.max)
                temp <- c(1:k)[choice]
                if(length(temp) > 1) {
                        classif[i] <- temp[1]
                }
                else {
                        classif[i] <- temp
                }
        }
        classif.val <- classif
        classif <- rep(0, n)
        for(i in 1:n.values) {
                classif[y == values[i]] <- classif.val[i]
        }
        if(!small.out) {
                return(pixel.probs, means, sds, segment.probs, 
                      last.loglikelihood, this.loglikelihood, classif, iters, 
                        values.hist, values)
        }
        if(small.out) {
                return(means, sds, segment.probs, last.loglikelihood, 
                        this.loglikelihood, classif, iters)
        }
}




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

estimate.icm.phi0<-
function(y, x.init, phi.init = 1.4, max.iter=10, trace = F)
{
#phi constrained to be nonnegative
#y is the observed data in a matrix
#x.init is the initial estimate of true scene in a matrix
#initial values of theta are not needed
#phi.init is the initial value of phi
#max.iter is maximum number of iterations
#1. begin with observed data y and initial estimate x.init 
#2. estimate theta by maximizing l(y|x,theta)
#3. estimate phi by maximizing logpseudolikelihood:
#     log (prod(i) [p(xi|xdi, phi)])           excluding boundary pixels
#4. update x by one cycle of ICM, given theta and phi
#5. repeat until max.iter or convergence of x
#we assume that f(yi|xi) is normal with mean xi and sd theta
#theta is c(means,sds)
#phi is beta
#we use an 8-pixel neighborhood
        n.iter <- 0
        rows <- length(y[, 1])
        cols <- length(y[1,  ])
        levels <- unique(x.init)
        num.levels <- length(levels)
        x <- x.init
        phi <- phi.init #initial value
        n.pixels <- rows * cols
        theta <- rep(0, num.levels * 2)
        if(num.levels == 1) {
                phi <- NA
                means <- mean(y)
                sds <- sqrt(var(as.vector(y)))
                theta <- c(means, sds)
                segment.probs <- 1
        }
        else {
#num.levels>1
                neglogpseudolikelihood <- function(beta)
                {
                        result <- 0
                        if(T) {
                                cat("loglik eval \n")
                        }
                        for(i in 2:(rows - 1)) {
                                for(j in 2:(cols - 1)) {
#find number of neighbors with same value x.init[i,j]
                                  neighbors <- c(x[i + 1, j], x[i + 1, j + 1], 
                                    x[i + 1, j - 1], x[i, j + 1], x[i, j - 1], 
                                    x[i - 1, j + 1], x[i - 1, j], x[i - 1, j - 
                                    1])
                                  n.same <- sum(neighbors == x[i, j])   
#result is sum over all pixels of log pseudolikelihood
sames<-rep(0,length(levels))
for (this.level in 1:length(levels)) {
sames[this.level]<-sum(neighbors==levels[this.level])
}

  result <- result + (beta*n.same) - (log(sum(exp(beta*sames))))

                                }
                        }
                        result <- ( - result)
                        result
                }
#
                done <- F
                while((n.iter < max.iter) && (done == F)) {
                        x.old <- x
                        phi.old <- phi
                        n.iter <- n.iter + 1    
        # estimate theta by maximizing l(y|x,theta)
                        for(i in 1:num.levels) {
                                theta[i] <- mean(y[x == levels[i]])
                        }
                        for(i in 1:num.levels) {
                                theta[num.levels + i] <- sqrt(sum((y[x == 
                                  levels[i]] - theta[i])^2)/(sum(x == levels[i]
                                  ) - 1))
                 if (is.na(theta[num.levels + i])) {theta[num.levels + i]<-.5}
                 if (theta[num.levels + i]<.5) {theta[num.levels + i]<-.5}
                        }
                        if(trace) {
                                cat("Iteration ", n.iter, "  phi ", phi, 
                                  " theta ", theta, "\n")
                        }
#estimate phi by maximizing pseudolikelihood:
                        assign("rows", rows, frame = 1)
                        assign("cols", cols, frame = 1)
                        assign("x", x, frame = 1)
                        assign("levels", levels, frame = 1)
                        assign("y", y, frame = 1)
                        phi.est <- nlmin(neglogpseudolikelihood, phi, max.iter
                                 = 2)
                        phi <- phi.est$x
                        if(phi < 0) {
                                phi <- 0
                        }
#one step of ICM
                        x <- icm(y, x, theta, phi, 1)   #check convergence
                        if(sum(x == x.old) == n.pixels) {
                                done <- T
                        }
                }
# end of main while loop
#order theta in the intuitive way
                means <- theta[c(1:num.levels)]
                sds <- theta[c((num.levels + 1):(2 * num.levels))]
                means <- means[order(levels)]
                sds <- sds[order(levels)]
                theta <- c(means, sds)  #compute segment.probs from the final x
                segment.probs <- rep(0, length(levels))
                for(i in 1:length(levels)) {
                        segment.probs[i] <- sum(x == levels[i])
                }
                total.sum <- sum(segment.probs)
                segment.probs <- segment.probs/total.sum
                segment.probs <- segment.probs[order(levels)]
        }
#end of num.levels=1
        return(x, means, sds, theta, phi, segment.probs)
}

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

icm<-
function(y, x.init, theta, phi, max.iter, trace = F)
{
#Written by Derek Stanford 1998.
#y is the data (a matrix)
#x.init is the initial classification (a matrix)
#theta is c(means,sds)
#phi is the Potts model parameter ( = beta in some of Besag's papers)
#we use an 8-pixel neighborhood
        n.iter <- 0
        rows <- length(y[, 1])
        cols <- length(y[1,  ])
        levels <- unique(x.init)
        num.levels <- length(levels)
        x.old <- x.init
        x.new <- x.old
        done <- F
        if(length(theta) != (2 * num.levels)) {
                stop("theta should be a vector of means and sds")
        }
        while((n.iter < max.iter) && (done == F)) {
                n.iter <- n.iter + 1
                if(trace) {
                        cat("Iteration ", n.iter, " \n")
                }
#exclude boundary pixels
                for(i in 2:(rows - 1)) {
                        if(trace) {
                                cat(i, " \n")
                        }
                        for(j in 2:(cols - 1)) {
                                current.probs <- rep(NA, num.levels)
                                for(k in 1:num.levels) {
                               logfyi <- log(dnorm(y[i, j], mean = theta[k], 
                                    sd = theta[k + num.levels]))        
        #find number of neighbors with same value levels[k]
                                  neighbors <- c(x.old[i + 1, j], x.old[i + 1, 
                                    j + 1], x.old[i + 1, j - 1], x.old[i, j + 1
                                    ], x.old[i, j - 1], x.old[i - 1, j + 1], 
                                    x.old[i - 1, j], x.old[i - 1, j - 1])
                                  n.same <- sum(neighbors == levels[k])
                                  logpxi <- (phi * n.same)
                                  if(is.na(logfyi)) {
                                    cat("Fyi is NA: i,j ", i, j, "\n")
                                  }
                                  if(is.na(logpxi)) {
                                    cat("Pxi is NA: i,j ", i, j, "\n")
                                  }
                                  current.probs[k] <- logfyi + logpxi
                                  if(is.na(current.probs[k])) {
                     cat("curr.probs is NA: i,j,f,p,phi,k,mean,sd ", i, j, 
                  logfyi, logpxi, phi, k, theta[k],theta[k + num.levels],"\n")
                                  }
                                }
#current.probs is actually log(current.probs)
                                x.new[i, j] <- max(levels[current.probs == max(
                                  current.probs)])
                        }
                }
#check for convergence
                if(sum(is.na(x.new)) > 0) {
                        cat("NAs in x.new \n")
                        cat("Phi ", phi, "  theta ", theta, "\n")
                        cat(x.new, "\n")
                        stop()
                }
                if(sum(x.old == x.new) == length(x.new)) {
                        done <- T
                }
                else {
                        x.old <- x.new
                }
        }
#end of main while loop
        x.new
}


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


pseudol.unorder<-
function(y, x, phi = NA, means = NA, sds = NA)
{
#y is image (matrix)
#x is classification (matrix)
#phi is Potts parameter
#means, sds are parameter vectors - must be in same order as x.levels
#exclude boundaries
        nrows <- dim(y)[1]
        ncols <- dim(y)[2]
        x.levels <- unique(x)
        n.levels <- length(x.levels)    
        #if only one level, this reduces to simple Gaussian
        if(n.levels == 1) {
                y.vec <- as.vector(y)
                y.mean <- mean(y.vec)
                y.sd <- sqrt(var(y.vec))
                result <- 0     
        #need to exclude boundary to be compatible with ICM results
                for(i in 2:(nrows - 1)) {
                        for(j in 2:(ncols - 1)) {
                                result <- result + log(dnorm(y[i, j], y.mean, 
                                  y.sd))
                        }
                }
        }
        else {
# result is log of product (over all pixels i) of the sum (over the
# groups k) of prob(y_i | x_i) prob(x_i | x_nbhd)
                result <- matrix(rep(0, nrows * ncols), ncol = ncols)
                for(i in 2:(nrows - 1)) {
                        for(j in 2:(ncols - 1)) {
                                this.pix <- 0
                                this.block <- x[c((i - 1):(i + 1)), c((j - 1):(
                                  j + 1))]
                                for(k in 1:n.levels) {
                                  this.block[2, 2] <- x.levels[k]
     this.pix <- this.pix + (dnorm(y[i, j], means[
                        k], sds[k]) * pseudol.pxi(this.block, phi,x.levels))
                                }
                                result[i, j] <- log(this.pix)
                        }
                }
        }
        temp <- sum(result)
        temp
}

##########

pseudol.pxi<- function(x.block, phi,levels)
{
#x.block is a 3x3 set of estimated states
#phi is estimated parameter for the Potts model
        num.levels <- length(unique(levels))
        x.vec <- as.vector(x.block)
        x.levels <- unique(levels)
        n.same <- rep(0, num.levels)
        for(i in 1:num.levels) {
                n.same[i] <- sum(x.vec[-5] == x.levels[i])
        }
        numer <- exp(phi * sum(x.vec[-5] == x.vec[5]))
        denom <- sum(exp(phi * n.same))
        result <- numer/denom
        result
}


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

estimate.icm<-
function(y, x.init, phi.init = 1.4, max.iter, trace = F)
{
#for binary case 
#y is the observed data in a matrix
#x.init is the initial estimate of true scene in a matrix
#initial values of theta are not needed
#phi.init is the initial value of phi
#max.iter is maximum number of iterations
#1. begin with observed data y and initial estimate x.init 
#2. estimate theta by maximizing l(y|x,theta)
#3. estimate phi by maximizing logpseudolikelihood:
#     log (prod(i) [p(xi|xdi, phi)])           excluding boundary pixels
#4. update x by one cycle of ICM, given theta and phi
#5. repeat until max.iter or convergence of x
#we assume that f(yi|xi) is normal with mean xi and sd theta
#theta is c(means,sds)
#phi is beta
#we use an 8-pixel neighborhood
        n.iter <- 0
        rows <- length(y[, 1])
        cols <- length(y[1,  ])
        levels <- unique(x.init)
        num.levels <- length(levels)
        x <- x.init
        phi <- phi.init #initial value
        n.pixels <- rows * cols
        theta <- rep(0, num.levels * 2)
        neglogpseudolikelihood <- function(beta)
        {
                result <- 0
                if(T) {
                        cat("loglik eval \n")
                }
                for(i in 2:(rows - 1)) {
                        for(j in 2:(cols - 1)) {
#find number of neighbors with same value x.init[i,j]
                              neighbors <- c(x[i + 1, j], x[i + 1, j + 1], x[
                              i + 1, j - 1], x[i, j + 1], x[i, j - 1], x[i - 
                                  1, j + 1], x[i - 1, j], x[i - 1, j - 1])
                                n.same <- sum(neighbors == x[i, j])     
        #result is sum over all pixels of log pseudolikelihood
                                result <- result + (beta * n.same) - (log(exp(
                                  beta * n.same) + exp(beta * (8 - n.same))))
                        }
                }
                result <- ( - result)
                result
        }
#
        done <- F
        while((n.iter < max.iter) && (done == F)) {
                x.old <- x
                phi.old <- phi
                n.iter <- n.iter + 1    
        # estimate theta by maximizing l(y|x,theta)
                for(i in 1:num.levels) {
                        theta[i] <- mean(y[x == levels[i]])
                }
                for(i in 1:num.levels) {
                        theta[num.levels + i] <- sqrt(sum((y[x == levels[i]] - 
                                theta[i])^2)/(sum(x == levels[i]) - 1))
                }
                if(trace) {
                        cat("Iteration ", n.iter, "  phi ", phi, " theta ", 
                                theta, "\n")
                }
#estimate phi by maximizing pseudolikelihood:
                assign("rows", rows, frame = 1)
                assign("cols", cols, frame = 1)
                assign("x", x, frame = 1)
                assign("y", y, frame = 1)
                phi.est <- nlmin(neglogpseudolikelihood, phi, max.iter = 2)
                phi <- phi.est$x        #one step of ICM
                x <- icm(y, x, theta, phi, 1)   #check convergence
                if(sum(x == x.old) == n.pixels) {
                        done <- T
                }
        }
# end of main while loop
#order theta in the intuitive way
        means <- theta[c(1:num.levels)]
        sds <- theta[c((num.levels + 1):(2 * num.levels))]
        means <- means[order(levels)]
        sds <- sds[order(levels)]
        theta <- c(means, sds)  #compute segment.probs from the final x
        segment.probs <- rep(0, length(levels))
        for(i in 1:length(levels)) {
                segment.probs[i] <- sum(x == levels[i])
        }
        total.sum <- sum(segment.probs)
        segment.probs <- segment.probs/total.sum
        segment.probs <- segment.probs[order(levels)]
        return(x, theta, phi, segment.probs)
}

################################
morphit<-function(mat) {
#mat is a matrix of integer greyscale values
newmat<-mat
newmat<-morphit.e(newmat)
newmat<-morphit.d(newmat)
newmat<-morphit.d(newmat)
newmat<-morphit.e(newmat)

return(newmat)
}
################################
morphit.e<-function(mat) {
#mat is a matrix of integer greyscale values
ncols<-dim(mat)[2]
nrows<-dim(mat)[1]
newmat<-mat

#erode

#corners
i<-1
j<-1
newmat[i,j]<-min(as.vector(mat[c((1):(i+1)), c((j):(j+1))]))

i<-nrows
j<-1
newmat[i,j]<-min(as.vector(mat[c((i-1):(i)), c((j):(j+1))]))

i<-1
j<-ncols
newmat[i,j]<-min(as.vector(mat[c((i):(i+1)), c((j-1):(j))]))

i<-nrows
j<-ncols
newmat[i,j]<-min(as.vector(mat[c((i-1):(i)), c((j-1):(j))]))

#sides
j<-1
for (i in 2:(nrows-1)) {
this.nbhd<-as.vector(mat[c((i-1):(i+1)), c((j):(j+1))])
newmat[i,j]<-min(this.nbhd)
}

j<-ncols
for (i in 2:(nrows-1)) {
this.nbhd<-as.vector(mat[c((i-1):(i+1)), c((j-1):(j))])
newmat[i,j]<-min(this.nbhd)
}

i<-1
for (j in 2:(nrows-1)) {
this.nbhd<-as.vector(mat[c((i):(i+1)), c((j-1):(j+1))])
newmat[i,j]<-min(this.nbhd)
}

i<-nrows
for (j in 2:(nrows-1)) {
this.nbhd<-as.vector(mat[c((i-1):(i)), c((j-1):(j+1))])
newmat[i,j]<-min(this.nbhd)
}

#interior
for (i in 2:(nrows-1)) {
for (j in 2:(ncols-1)) {
this.nbhd<-as.vector(mat[c((i-1):(i+1)), c((j-1):(j+1))])
newmat[i,j]<-min(this.nbhd)
}}

return(newmat)
}

################################
morphit.d<-function(mat) {
#mat is a matrix of integer greyscale values
ncols<-dim(mat)[2]
nrows<-dim(mat)[1]
newmat<-mat

#erode

#corners
i<-1
j<-1
newmat[i,j]<-max(as.vector(mat[c((1):(i+1)), c((1):(j+1))]))

i<-nrows
j<-1
newmat[i,j]<-max(as.vector(mat[c((i-1):(i)), c((1):(j+1))]))

i<-1
j<-ncols
newmat[i,j]<-max(as.vector(mat[c((i):(i+1)), c((j-1):(j))]))

i<-nrows
j<-ncols
newmat[i,j]<-max(as.vector(mat[c((i-1):(i)), c((j-1):(j))]))

#sides
j<-1
for (i in 2:(nrows-1)) {
this.nbhd<-as.vector(mat[c((i-1):(i+1)), c((j):(j+1))])
newmat[i,j]<-max(this.nbhd)
}

j<-ncols
for (i in 2:(nrows-1)) {
this.nbhd<-as.vector(mat[c((i-1):(i+1)), c((j-1):(j))])
newmat[i,j]<-max(this.nbhd)
}

i<-1
for (j in 2:(nrows-1)) {
this.nbhd<-as.vector(mat[c((i):(i+1)), c((j-1):(j+1))])
newmat[i,j]<-max(this.nbhd)
}

i<-nrows
for (j in 2:(nrows-1)) {
this.nbhd<-as.vector(mat[c((i-1):(i)), c((j-1):(j+1))])
newmat[i,j]<-max(this.nbhd)
}

#interior
for (i in 2:(nrows-1)) {
for (j in 2:(ncols-1)) {
this.nbhd<-as.vector(mat[c((i-1):(i+1)), c((j-1):(j+1))])
newmat[i,j]<-max(this.nbhd)
}}

return(newmat)
}

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

greyreorder.fn<-function(x,neword) {
#x is a matrix (image)
#neword is the reordering of the grey values

curord<-sort(unique(x))
if(length(curord)!=length(neword)) {stop("Blarg.\n")}
tempim<-x
for (i in 1:length(neword)) {
thisval<-curord[i]
newval<-neword[i]
tempim[x==thisval]<-newval
}
tempim
}