#
### version 2 is starting to fit B
#
#  2b-easy still trying to fit B .....   2a-easy is a stable (if a bit dissatisfying) version

myim2 <- function(X){
 nrow <- dim(X)[1]
 image(t(X[nrow:1,]), col=gray(seq(0,1,0.01)), xaxt="n", yaxt="n")
}

myim <- function(X){
 nrow <- dim(X)[1]
 image(t(X[nrow:1,]), col=gray(seq(1,0,-0.01)), xaxt="n", yaxt="n")
}


regty <- function(H){   # regularuty of the array H 
  n1 <- dim(H)[1]
  n2 <- dim(H)[2]
  reg <- sum((H[1:(n1-1),] - H[2:n1,])^2)
  reg <- reg + sum((H[,1:(n2-1)] - H[,2:n2])^2)
  sqrt(reg/(n2*(n1-1) + n1*(n2-1)))
} 
 


 A <- as.matrix(read.csv('c:/Oleg/deblur/cat3.csv', header=FALSE))
  
 nrow <- dim(A)[1]
 ncol <- dim(A)[2]
 N <- nrow*ncol


 img <- A[1:120,261:400]                 
 img <- max(img)-img     # made so that the boundary is white
 Ni <- dim(img)[1]
 Nj <- dim(img)[2]

m <- 4     # blur order
m2 <- 2*m+1 
B <- matrix(1/m2^2, m2,m2)      # uniform blur

Btrue <- B

 
  G <- img          # this is the source for lkhd
   
  F <- img[(1+m):(Ni-m), (1+m):(Nj-m)]      # will be the restored image, edges removed
    
  NiF <-  Ni-2*m 
  NjF <-  Nj -2*m 

  Gcen <- G[(2*m+1):(NiF), (2*m+1):(NjF)]      # for future sampling B
    Lmat <- matrix(0,m2,m2) 
    Lvec <- rep(0, m2^2)

  Gf <- G*0     # this is the smoothing of F, to be iterated to perfection
  for (i in 1:NiF)
   for(j in 1:NjF){
     Gf[i:(i+2*m), j:(j+2*m)] <-  Gf[i:(i+2*m), j:(j+2*m)] + F[i,j] * B
  }

  
  regty(F)   # actually it's estimate of irregularity, that is sigma
  s <- 5    #  a bit higher than noise in F 
  se <- 1  # some "small" number, also tunable 
  
   # 5 and 1 give an ok result for m = 2

  Mask <- 0*G
  Mask[(1+2*m):(Ni-4*m), (1+2*m):(Nj-4*m)] <- 1    # to avoid edge effects       	

  lkhdif <- function(i,j,fold,fnew){   # gives loglkhd difference, to be used in Metropolis 
     mask <- Mask[i:(i+2*m),j:(j+2*m)] 
     Gw <- G[i:(i+2*m),j:(j+2*m)]   # affected window
     Gfw <- Gf[i:(i+2*m),j:(j+2*m)]
     Gwn <- Gfw + (fnew-fold)*B      # this is how the new window will look like
   
     loglik <- sum(mask*(Gfw  - Gw)^2)  - sum(mask*(Gwn  - Gw)^2)    
                                           # changes in lkhd from change in f
 
     nhb <- c(F[i+1,j], F[i-1,j], F[i,j+1], F[i,j-1])
     logprior <- sum((fold-nhb)^2) - sum((fnew-nhb)^2)     # changes in regularity from change in f
     (loglik/(se^2) + logprior/(s^2))/2
   }

  MC <- 50
  nskip <- 20000
  delf <- 3
  Nacc <- 0
lkhds <- rep(0,MC)
 lkhd <- 0
 meanF <- F*0
 fitB <- TRUE

  for(mc in 1:MC){
   for (skip in 1:nskip){
    i <- sample(2:(NiF-1),1)    # 1-margin taken off to avoid edge effects
    j <- sample(2:(NjF-1),1)
    fold <- F[i,j]
    fnew <- fold + rnorm(1,0,delf)  
    ldif <- lkhdif(i,j,fold,fnew)  

    accprob <- exp(min(ldif,0))
    u <- runif(1)
    if (u < accprob){                  # Metropolis step
     F[i,j] <- fnew
      Nacc <- Nacc + 1
      lkhd <- lkhd + ldif
      Gf[i:(i+2*m),j:(j+2*m)] <-  Gf[i:(i+2*m),j:(j+2*m)] + (fnew-fold)*B  
    }
  } # end skip
  # output the new pic
   myim(F)
   lkhds[mc] <- lkhd
   meanF <- meanF + F/MC

  # fitting s 
  dfs <- (NiF*(NjF-1) + NjF*(NiF-1))
  ssq <- regty(F)^2*dfs
  s <- sqrt((ssq + 100)/rchisq(1,dfs+5))    # includes prior var and prior df
  
  sse <- sum(((Gf-G)*Mask)^2)
  dfe <- sum(Mask)
 # se <- sqrt((sse + 1)/rchisq(1,dfe+50)) 


if (fitB){
    # linear part 

    Cmat <- Lmat*0
 for (i in 1:m2)
     for (j in 1:m2){ 
       Cmat[i,j] <- cor(as.numeric(F[(m2+1-i):(NiF +1-i), (m2+1-j):(NjF +1-j)]), as.numeric(Gcen) ) 
   }   
   B <- Cmat/sum(Cmat)

    Gf <- G*0     # update the Gf
    for (i in 1:NiF)
     for(j in 1:NjF){
       Gf[i:(i+2*m), j:(j+2*m)] <-  Gf[i:(i+2*m), j:(j+2*m)] + F[i,j] * B
    }
   }  # B switch on/off


 } # end of Gibbs

 (acc.pct <- Nacc/(MC*nskip))
plot(lkhds)
Fprep <- pmin(max(img) + 0*F,pmax(meanF,0*F))
myim(Fprep)



sqrt(sum(((Gf-G)*Mask)^2)/sum(Mask))

 11.17
  3.24 (!)

 3.0293  (for m=1, with se = 1 and s = 10)
 


  # resampling B

  # F <-  img[(1+2*m):(Ni-2*m), (1+2*m):(Nj-2*m)]    # cheating

  

 
  ## naive fitting of B

 Cmat <- Lmat*0
 for (i in 1:m2)
     for (j in 1:m2){ 
       Cmat[i,j] <- cor(as.numeric(F[(m2+1-i):(NiF +1-i), (m2+1-j):(NjF +1-j)]), as.numeric(Gcen) ) 
   }