library(microarray)
library(stats)
library(nlme)

options(width=140, digits=3)

cens=.7


#
# This function defines the polynomial-hyperbolic spot shape model
#
# Transformed for stability: Spotintensity
#                            gamma
spotshape.fct.polyhyp <- function(xcord, ycord, background, spotintensity, sigma, gamma, mux, muy, alpha) {

  # Transformations for stability
  gamma <- exp(gamma)+1

  alpha <- exp(alpha)

  # Calculate euclidian diatance from spot centre
  distance <- sqrt((xcord-mux)**2+(ycord-muy)**2)
  distance <- distance/sigma


  xg <- (distance>=gamma)
  
  h <- exp(spotintensity + distance*alpha/gamma**2 + .5*alpha*(1/(gamma-1)**2  - 1/gamma**2  )*distance^2 -
           alpha/(gamma-distance))
  h <- h/(sigma**2)

  h[xg] <- 0
  
  return(background + h)
}


polyhypInit <- function(mCall, LHS, data) {
#  xy <- sortedXyData(mCall[["x"]], LHS, data)  
  y <- eval(LHS, data, parent.frame())
  xcord <- data$xcord
  ycord <- data$ycord

#  if(max(y)>1) {
#    stop("Spot intensities should be standardized before using polyhyp")
#  }
  
  value <- rep(0,7)

  # Set background
  value[1] <- quantile(y, probs=.15)

  # Set spotintensity to
  value[2] <- quantile(y, probs=.85)/value[1]

  # Set sigma to 6
  value[3] <- 6

  # Set gamma to 1.7
  value[4] <- log(.7)
  
  # Set spot centres
  value[5] <- mean(eval(mCall[["xcord"]], data, parent.frame())) #+ rnorm(1, 0, .2)
  value[6] <- mean(eval(mCall[["ycord"]], data, parent.frame())) #+ rnorm(1, 0, .2)

  # Try to find the spot centre by the top from a 2nd degree polynomial
  xval <- 9:17
  
  xxx <- y[ycord==13]
  xxx <- xxx[xval]
  res <- lm(xxx ~ xval + I(xval^2))
  top <- -coef(res)[2]/(2*coef(res)[3])  
  value[5] <- (value[5] + top)/2
  value[5] <- top
  
  xxx <- y[xcord==13]
  xxx <- xxx[xval]
  res <- lm(xxx ~ xval + I(xval^2))
  top <- -coef(res)[2]/(2*coef(res)[3])
  value[6] <- (value[5] + top)/2
  value[6] <- top

  value[5] <- 14.6
  value[6] <- 12.5
  
  value[7] <- .7
#  value[7] <- .2
  value[7] <- log(value[7])


  #
  # let optim run a bit to get optimized (and possible a bit more robust) starting values
  #
  wrap <- function(theta, y, xcord, ycord) {
    res <- -sum(-.5*( log(2*pi*exp(theta[8])) + 1/exp(theta[8])*
                     (y-spotshape.fct.polyhyp(xcord, ycord,
                                              theta[1], theta[2], theta[3],
                                              theta[4], theta[5], theta[6],
                                              theta[7]))**2))
    res
  }
  
  
  #
  # Now run optimize to get a good starting point
  #
  # 28/1-2004 Run optim a few times to select the best starting point by jiggling the starting values a little
  beststart <- value
  startvalue <- value
  bestres   <- 1000000
  for (i in 1:3) {

    opt <- optim(c(value,log(.2)), wrap,
                 control=c(maxit=150), method="BFGS",
    #            control=c(maxit=250), method="L-BFGS-B", lower=c(0, -Inf, 0, -Inf, 0, 0, 0, -Inf), upper=c(1, Inf, Inf, Inf, 25, 25, 2, Inf ),
                 y=y, xcord=xcord, ycord=ycord)
    
    # Set the values to the optimized values
    value <- opt$par[-length(opt$par)]
    
    if (opt$value<bestres) {
      beststart <- value
      bestres <- opt$value      
    }
#    print(opt)

    # Now jiggle some values
    value[c(4,7)] <- rnorm(2, mean=startvalue[c(4,7)], sd=.1)   
  }
  value <- beststart
 
  names(value) <- mCall[c("background", "spotintensity", "sigma", "gamma", "mux", "muy", "alpha")]

  # Fix some values
#  value[7] <- min(value[7], 1.5)
#  value[4] <- min(value[4], -1)

#  print(value)

  value
}



polyhyp <- selfStart(spotshape.fct.polyhyp, initial=polyhypInit,
                     parameters=c("background", "spotintensity", "sigma", "gamma", "mux", "muy", "alpha"))



censor.spot <- function(data, cens=1) {
  data$oldy <- data$y
  data$y[data$y>=cens] <- cens
  data
}
  


read.spot.file <- function(file, transformation=c("boxcox", "log", "none"), lambda1=10, lambda2=.2) {
  load(file)

  transformation <- match.arg(transformation)

  origy <- spot
  
  xx   <- seq(1,nrow(origy),1)-((nrow(origy)+1)/2)
  distance <- sqrt(outer(xx**2,xx**2, "+"))

  M     <- 2**16-1

  y     <- as.vector(origy)
  x     <- as.vector(distance)

  x1   <- as.vector(distance) - 5
  x1[x1<0] <- 0
  x2   <- as.vector(distance) - 7
  x2[x2<0] <- 0
  x3   <- as.vector(distance) -9
  x3[x3<0] <- 0
  x4   <- as.vector(distance) -11
  x4[x4<0] <- 0
  
  xcord <- rep(1:nrow(origy), nrow(origy))
  ycord <- rep(1:nrow(origy), rep(nrow(origy), nrow(origy)))

  y <- switch(transformation,
              boxcox = ((y+lambda1)**lambda2 -1)/((M+lambda1)**lambda2 -1),
              log = log(y+lambda1)/log(M+lambda1),
              none=y)

  data.frame(y, x, xcord, ycord)
}


plot.spot.model <- function(object, data) {

#  data <- (object$call)$data

  oldpar <- par()  

  size <- sqrt(nrow(data))

  layout(matrix(c(1, 2, 1, 3, 1, 4), ncol=3))

  
  model.distance <- sqrt((data$xcord-(object$coefficients)["mux"])**2 + (data$ycord-(object$coefficients)["muy"])**2 )

  plot(model.distance, data$y,
       xlab="Distance from spot center",
       ylab="Spot intensity",
       col="black",
       ylim=c(0, 1.1))
  newx.2 <- order(model.distance)

  points(model.distance, data$newy, col="green")
  
  lines(model.distance[newx.2], predict(object)[newx.2], lwd=2, col="red", lty=1)
  
  abline(h=(object$coefficients)["background"], col="blue")
  abline(v=(object$coefficients)["sigma"], col="blue")
  abline(h=(object$coefficients)["background"] + exp((object$coefficients)["spotintensity"]), col="blue")


  m <- diag(size)
  m[cbind(data$xcord, data$ycord)] <- data$y

  my.estimate <<- 
    spotshape.fct.polyhyp(data$xcord,data$ycord,
                          object$coefficients["background"],
                          object$coefficients["spotintensity"],
                          object$coefficients["sigma"],
                          object$coefficients["gamma"],
                          object$coefficients["mux"],
                          object$coefficients["muy"],
                          object$coefficients["alpha"]
                          )
  
  attributes(my.estimate) <- NULL

  m2 <- diag(size)
  m2[cbind(data$xcord, data$ycord)] <- my.estimate
  
  contour(1:size, 1:size, m)

  par(mar=c(0,0,0,0) + .1)
  persp(1:size, 1:size, m, phi=35, theta=25, zlim=c(0, max(m, m2)),
        zlab="Intensity")  
  persp(1:size, 1:size, m2, phi=35, theta=25, zlim=c(0, max(m, m2)),
        zlab="Intensity")
  par(oldpar)
    
}



originalParameters <- function(x) {
  if (length(x) != 7)
    stop("Wrong length for parameters")


  # The parameters are
  #
  #  background, spotintensity, sigma, gamma, mux, muy, alpha

  # Spotintensity
  x[2] <- exp(x[2])
  
  # Gamma 
  x[4] <- exp(x[4])+1

  # Gamma 
  x[7] <- exp(x[7])
  
  x
}



iterate.cond.exp <- function(obj, y, data, censoring.level=1) {
  cens <- (y>=censoring.level)
  obs  <- !cens

  


#  imputy.cond.exp <- predict + sigma12 solve(sigma22) (x2-ksi2)
  
  
}


compareModels <- function(data, spotnum="Unknown ", gain="Unknown ") {

  # First we estimate the parameters without worrying about the censoring 
  
  claus.2  <<- gnls(y ~ polyhyp(xcord, ycord,  background, spotintensity, sigma, gamma, mux, muy, alpha),
                    data=data, control=gnlsControl(maxIter=100), verbose=FALSE)

  #
  # Identify censored observations
  #
  cens.obs <- (data$y>=cens)      # Vector of indicators for the censored
  nocens.obs <- (data$y<cens)     # and uncensored observations
  subset <- data[cens.obs,]       # Copy of the part of the original dataset corresponding to censored observations
  obs.subset <- data[nocens.obs,] # Copy of the part of the original dataset corresponding to observed observations :o/

  obj <- claus.2

  # Start by conditioning on the observed data but without any correlation structure (ie. independence)
  for (i in 1:100) {
    # Estimate the expected mean of the censored data given the observed  
    conditional.mean <- polyhyp(subset$xcord, subset$ycord, coef(obj)[1],coef(obj)[2],coef(obj)[3],coef(obj)[4],coef(obj)[5],coef(obj)[6],coef(obj)[7])

    # Impute new values
    newy <- data$y
    # Insert (for each censored observation) the conditional expectation given the observed data _and_ the fact that the censoring
    # must be larger the level of censoring.
    newy[cens.obs] <- conditional.mean + obj$sigma/sqrt(2*pi)*(exp(-.5*((cens-conditional.mean)/(obj$sigma))**2))/(1-pnorm((cens-conditional.mean)/obj$sigma))
    cat("Update difference : ", sum((newy-data$newy)**2))
    data$newy <- newy
    newobj  <<- gnls(newy ~ polyhyp(xcord, ycord,  background, spotintensity, sigma, gamma, mux, muy, alpha),
                       data=data, control=gnlsControl(maxIter=100), start=coef(obj))
    obj <- newobj
    print(sum((data$newy-data$oldy)**2))
    print(coef(newobj))
    print(newobj)
    plot.spot.model(newobj, data)
  }
#  print(data$newy[cens.obs])

#  print(cbind(data$y,data$newy, data$oldy)[cens.obs,])
 
  cat(spotnum, gain, "None ", logLik(claus.2),  originalParameters(coef(claus.2)),  claus.2$sigma,  "NA   ", sum((data$y-data$oldy)**2), "\n")  
  cat(spotnum, gain, "None ", logLik(newobj),  originalParameters(coef(newobj)),  newobj$sigma,  "NA   ", sum((data$newy-data$oldy)**2), "\n")

  
  obj <- claus.2
#  obj <- newobj
  newobj <<- update(obj, .  ~ . , correlation=corGaus(value=1, form= ~ xcord + ycord, fixed=TRUE), verbose=FALSE)
  print(newobj)
  for (i in 1:100) {
    # First figure out the ...
    rho <- coef(newobj$modelStruct, unconstrained=FALSE)
    cG <- corGaus(rho, form= ~ xcord + ycord)
    cGI <- Initialize(cG, data)    
    Omega <- corMatrix(cGI)
    Omega11 <- Omega[cens.obs,cens.obs]
    Omega12 <- Omega[cens.obs,nocens.obs]
    InvOmega22 <- solve(Omega[nocens.obs,nocens.obs])
    Omega21 <- Omega[nocens.obs,cens.obs]

    Omega1222 <- Omega12%*%InvOmega22

    # Calculate the conditional mean of the censored observations given the observed
    xi1 <- polyhyp(subset$xcord, subset$ycord, coef(obj)[1],coef(obj)[2],coef(obj)[3],coef(obj)[4],coef(obj)[5],coef(obj)[6],coef(obj)[7])
    xi2 <- polyhyp(obs.subset$xcord, obs.subset$ycord, coef(obj)[1],coef(obj)[2],coef(obj)[3],coef(obj)[4],coef(obj)[5],coef(obj)[6],coef(obj)[7])
#    print(dim(Omega12))
#    print(dim(InvOmega22))
#    print(dim(Omega21))
#    print(xi1)
#    print(xi2)
    conditional.mean <- xi1 + Omega1222 %*% (obs.subset$y - xi2)
    conditional.var  <- Omega11 - Omega1222 %*% Omega21
    cens.sd <- obj$sigma*sqrt(diag(conditional.var))

#    print(conditional.var)
#    print(sqrt(diag(conditional.var)))
#    print(cens.sd)

    # XXX husk at fiks sigma!    
    newy <- data$y
    
#    newy[cens.obs] <- conditional.mean + obj$sigma/sqrt(2*pi)*(exp(-.5*((cens-conditional.mean)/(obj$sigma))**2))/(1-pnorm((cens-conditional.mean)/obj$sigma))
    newy[cens.obs] <- conditional.mean + cens.sd/sqrt(2*pi)*(exp(-.5*((cens-conditional.mean)/(cens.sd))**2))/(1-pnorm((cens-conditional.mean)/cens.sd))
    updatediff <- sum((newy-data$newy)**2)
    cat("Update difference : ", updatediff, "\n")
    data$newy <- newy
    newobj <<- update(newobj, newy ~ . , correlation=corGaus(value=1, form= ~ xcord + ycord, fixed=TRUE), start=coef(newobj))
    obj <- newobj
    print(sum((data$newy-data$oldy)**2))
    print(newobj)    
    plot.spot.model(newobj, data)    
  }
  cat("Final model with Gaussian correlation structure:\n")
  plot(newobj)
  
#  tid1 <- system.time(claus.31 <- update(claus.2, . ~ . , correlation=corSpher(value=1.3, form= ~ xcord + ycord)))
  #
  # Først køres denne
  #
#  tid1 <- system.time(claus.31 <<- update(claus.2, . ~ . , correlation=corSpher(value=1.9, form= ~ xcord + ycord)))


#  print(cbind(data$y,data$newy, data$oldy)[cens.obs,])
  
#  tid2 <- system.time(claus.32 <- update(claus.2, . ~ . , correlation=corExp(value=sqrt(.2), form= ~ xcord + ycord)))
#  tid3 <- system.time(claus.33 <<- update(claus.2, . ~ . , correlation=corGaus(value=.6, form= ~ xcord + ycord)))
#  tid4 <- system.time(claus.34 <- update(claus.2, . ~ . , correlation=corRatio(value=.7, form= ~ xcord + ycord)))
  # Der er nogle gang problemer med nedenstående:
#  tid5 <- system.time(claus.35 <- update(claus.2, . ~ . , correlation=corLin(value=sqrt(1.3), form= ~ xcord + ycord)))

  claus.332 <<- update(newobj, . ~ . , correlation=corGaus(value=1, form= ~ xcord + ycord))
  cat(spotnum, gain, "Gauss", logLik(claus.332), originalParameters(coef(claus.332)), claus.332$sigma, coef(claus.332$modelStruct, unconstrained=FALSE), sum((data$newy-data$oldy)**2), "\n")

  # Plot original data
  plot(1:25, data$oldy[data$xcord==13], type="l", ylim=c(0, 1.1))
  myx <- seq(1, 25, .1)
  lines(myx, polyhyp(13, myx, coef(claus.332)[1],coef(claus.332)[2],coef(claus.332)[3],
                      coef(claus.332)[4],coef(claus.332)[5],coef(claus.332)[6],coef(claus.332)[7]))

#  print(rbind(tid1, tid2, tid3, tid4, tid5))
#  print(rbind(tid1))
  
}


#
#
#
#
#


GetSpot <- function(image, datafile, number, method="square") {
  get.spot(image,
           round(datafile$X.Coord[number]),
           round(datafile$Y.Coord[number]),
           12,
#           (datafile$Diameter[number])/2 + 5,
           method)
}




#img.30 <- read.tiff.file("../../artikel2/R/020725_cy3_wt_l30_g30_avg4.tif")
#img.40 <- read.tiff.file("../../artikel2/R/020725_cy3_wt_l30_g40_avg4.tif")
#img.50 <- read.tiff.file("../../artikel2/R/020725_cy3_wt_l30_g50_avg4.tif")
img.60 <- read.tiff.file("../../artikel2/R/020725_cy3_wt_l30_g60_avg4.tif")

data.file <- read.table("../../artikel2/R/020725_cy3wtcy53x8.txt",
                        header=T,
                        sep="\t",
                        strip.white=T,
                        fill=T,
                        comment.char="",                        
                        row.names=NULL)


spotlist <- c(98,100,102,104,107,108,109,110,111,112,113,242,244,249,278,307,352,518,564,633,647,758,787,824, 97)

spotlist <- c(352, 787, 242)


spotlist <- c(352)
#spotlist <- c(242)
#spotlist <- c(787)


cat("Starting calculations:\n")
for (indexx in spotlist) {
  cat ("Spot: ", indexx, "\n")

#  spot <- GetSpot(img.30, data.file, 960 + indexx)
#  save(spot, file="tmpspotfile.dat")
#  spot30 <- read.spot.file("tmpspotfile.dat")

#  spot <- GetSpot(img.40, data.file, 960 + indexx)
#  save(spot, file="tmpspotfile.dat")
#  spot40 <- read.spot.file("tmpspotfile.dat")
  
#  spot <- GetSpot(img.50, data.file, 960 + indexx)
#  save(spot, file="tmpspotfile.dat")
#  spot50 <- read.spot.file("tmpspotfile.dat")

  spot <- GetSpot(img.60, data.file, 960 + indexx)
  save(spot, file="tmpspotfile.dat")
  spot60 <- read.spot.file("tmpspotfile.dat")
  spot60 <- censor.spot(spot60, cens=cens)
  


#  compareModels(spot30, indexx, 30)
#  compareModels(spot40, indexx, 40)
#  compareModels(spot50, indexx, 50)
  compareModels(spot60, indexx, 60)
  system("rm tmpspotfile.dat")
}

cat("Calculations ended.\n")