#Example sequential importance sampling function in Splus 6.1
#Implements analysis described in Thomas, Buckland, Newman and Harwood,
#  "A unified framework for modelling wildlife population dynamics"
#  Australian and New Zealand Journal of Statistics
#Driver routine for this function is in sis.driver

sis.seal.10.1 <- function(useFor=F) 
{
# Seal population dynamics model estimation function.
# 7 seal stages - pups, age 1, ..., age 6+
# Density dependent productivity and movement, density independent survival

# Estimation by SIS with auxilliary particle filtering and resampling of kernel smoothed
#  parameter distributions.  Ref Lui and West 2001.  Algorithm amended by LJT to include
#  adaptive number of particles

# 29 August 2001 - 6 February 2002 KBN 
# 6 February 2002 - New movement routine installed; Delta and Threshold trigger dropped; 
#   movement = f(fidelity, S[j]-S[natal], 1/dist[i,j]) KBN 
# 18 February 2002 - Density dependent productivity; density independent survival; 
#   movement depends on relative productivity - LJT
# 1 March 2002 - Re-wrote to use auxilliary particle filter and removed local resampling
#   Also made code modular. - LJT
# 23 March 2002 - Added adaptive number of particles, and packaged years into a For loop
# 5 July 2002 - Density dependent survival added back in
# 14 November 2002 - Beta and Gamma priors on parameters; multinomial movement model - LJT
# 20 November 2002 - constant CV model for observation process.

# Inputs: 7
#  If useFor is T then uses a For loop; otherwise, uses a for loop and you can get
#   output as the simulation progresses.

#  Assumes the following variables are packaged in the list sis in the working directory:
#  sis$N.ages - number of age classes
#  sis$obs - matrix of pup count estimates - colonies x time
#        Note: could later expand this to include a 3rd dimension of ages, so could
#        have observations on more than just pups
#  sis$distances - square matrix of distances between colonies
#  sis$N - number of particles to follow between time steps, in each iteration
#  sis$N1 - number of particles to follow within auxiliary filtering, in each iteration
#  sis$max.N - maximum number of particles to follow, over all iterations
#  sis$min.absolute.unique - minimum number of the original particles allowed to survive
#  sis$min.N.unique - number of unique particles in a year after auxiliary particle filter
#  sis$min.N.unique - number of unique particles in a year at posterior
#  sis$prior.params.dist - vector of strings indicating which distribution to use
#    to generate priors on each parameter.  Options: "fixed","uniform","beta","gamma"
#    Parameters are in the following order:
#    p.female,Sa,SjMax,aMax,psi,move1,move2,move3,b1...bK
#  sis$prior.params - matrix of prior parameter parameter values
#   2 rows.  For uniform priors, 1st is lower range, 2nd is upper
#            For beta priors, 1st is parameter a, 2nd is parameter b
#            For gamma priors, 1st is shape parameter k, 2nd is scale parameter b
#            For fixed parameters, 1st and 2nd are both the same, and are the fixed
#             parameter value
#  sis$state.inflate - factor to inflate initial state conditions using runif
#    factor to deflate initial state conditions is worked out from this
#    note that state.inflate must be less than 2
#  sis$discount - sets the kernal smoothing parameter 
#  sis$echo - if T then outputs intermediate results
#  sis$true.states - array of known states - age x colonies x years 
#    Note - this is different from, say states output var -- for historical reasons
#  sis$init.with.true.states - if true, and true states is not null, then inititalizes
#    the states matrix using the true states -- otherwise just uses the observed
#    pup counts
#  sis$allow.movement - if F then no movement in simulated populations
#  sis$rdir - name of directory for results
#  sis$pop.process.is.stochastic - if T then pop process is stochastic, if F then deterministic
#  sis$resid.resamp - if T then uses residual resampling, otherwise ordinary resampling with
#    replacement
#  sis$dens.dep.surv - if T then survival is density dependent
#  sis$dens.dep.fecund - if T then fecundity is density dependent
#   NOTE - both dens.dep.surv and dens.dep.fecund can be F - in which case migration does not
#   depend on density -- but both cannot be T - only one can at any one time

# Makes temporary variables starting with ., in which it store temp stuff
# .year, .echo, .N
# Deletes these at the end.

# Outputs:
#  files: states.[year].txt - posterior estimates of states, saved as ages * colonies * N
#         params.[year].txt - posterior estimates of parameters, saved as parameters * N
#         N.txt - gives year, N.prior (total prior particles), N.post (total posterior particles),
#           N.unique.1, N.unique.2 for each year

  #Check for existance of sis var in working folder
  if (!exists("sis",where=1)) stop("List 'sis' does not exist in working folder.")
  
  #Check that dens.dep.surv and dens.dep.fecund are not both T
  if (sis$dens.dep.surv & sis$dens.dep.fecund) 
    stop("sis$dens.dep.surv and sis$dens.dep.fecund cannot both be true.")

  #Make sure we clean up when exiting
  on.error(cat("*** Error in year",.year,"***\n"))
  on.exit({if(exists(".year",where=1)) remove(".year",where=1);
           if(exists(".echo",where=1)) remove(".echo",where=1);
           if(exists(".N",where=1)) remove(".N",where=1)
           par(mfrow=c(1,1))})


  #Initialize year in working folder - start with year 2 as year 1 is used to generate
  # prior states
  assign(".year",2,where=1,immediate=T)
  assign(".data.dir",paste(search()[1],"\\",sis$rdir,sep=""),where=1,immediate=T)

  #set outer margin lines of histo figures   
  par(oma=c(0,0,3,0))    

  #work out how many time periods in total
  T1 <- dim(sis$obs)[[2]]   

  #initialize the database for storing results
  initialize.database(T1)

  #Run the appropriate loop
  if(useFor) {
    #use a For loop - ie do each year in a separate process
    #won't get any interactive output this way, so may as well make echo false, to save memory
    .echo <<- F
    For(2:T1,do.sis.byyear())
  } else {
    .echo <<- sis$echo
    #use a for loop - ie do within this process
    for (i in 2:T1) do.sis.byyear()
  }

  invisible(NULL)
}

#-----------------------------------------------------------------------------------------------#

initialize.database <- function(T1) {
#deletes the files generated by these routines - called when starting a new run
#also checks that the results directory exists, and if not creates it

  if (!is.dir(sis$rdir)) mkdir (sis$rdir)
  attach(.data.dir,pos=2,name="sis.db")
  remove(objects(where="sis.db"),where="sis.db")

  #make a new meta var
  assign("meta",matrix(0,T1,5),where="sis.db")
  detach("sis.db")
}

#-----------------------------------------------------------------------------------------------#

do.sis.byyear <- function() 
#Called once for each year of data -- sets up the prior params and states, calls the particle
# filter function, and then saves the posterior estimates to file
# Keeps looping until a minimum number of unique particles have been generated (or the
#  max number of particles has been reached)
{
  attach(.data.dir,pos=2,name="sis.db")
  on.exit({detach("sis.db")})
  
  #tracks total number of unique particles
  #number unique in this year after auxiliary particle filtering
  total.unique.1 <- 0
  #number unique in this year in the posterior
  total.unique.2 <- 0
  #particle indexes of particles unique from the beginning
  unique.particle.indx <- NULL
  #tracks the iterations
  i <- 0
  #maximum number of iterations
  max.i <- floor(sis$max.N/sis$N)
  #number of prior particles sampled
  pre.N <- 0
  
  #create array space, markers   
  # K is number of colonies   
  K <- dim(sis$obs)[[1]]   
  #number of parameters
  N.params <- dim(sis$prior.params)[[2]]

  #number of global (as opposed to colony specific) parameters
  N.g.params <- N.params - K

  #index to position of global parameters in params matrix
  # (colony specific beta params go after this)
  p <- list (1,2,3,4,5,6,7,8)
  names(p) <- c("pFemale","Sa","SjMax","aMax","psi","move1","move2","move3")
  #parameters either get a log or a logit transform when kernal smoothing
  #if params.islogit is T then it's logit
  params.islogit <- rep(F,N.params)
  params.islogit[c(p$pFemale,p$Sa,p$SjMax,p$aMax)] <- T

  #work out which parameters are fixed
  params.fixed <- (sis$prior.params.dist=="fixed")

  #used in plots later
  par(mfrow=c(3,sum(!params.fixed[1:N.g.params])))

  #set kernal smoothing mixing rate   
  a <- (3*sis$discount-1)/(2*sis$discount) 
  h.sq <- 1-a^2

  if (.echo) cat ("\n*****************\n*** Year = ",.year,
   "*** \n*****************\n\n")

  #loop over particle.filter function until you have enough unique particles
  repeat {

    i <- i+1

    if (.echo) cat ("\n**********************\n*** Iteration = ",i,
     "*** \n**********************\n\n")

    #get priors for this year
    if (.year==2) {
      #generate sis$N initial parameters and states
      # will generate a fresh set of params and states on each iteration
      # prior.params is N.params x N
      pre.params <- initialize.params(sis$N,sis$prior.params,sis$prior.params.dist,.echo)
      # prior.states is N x K x n.ages
      pre.states <- initialize.states(pre.params,p,sis$obs,sis$state.inflate,
        sis$N.ages,sis$dens.dep.surv,sis$dens.dep.fecund,sis$true.states,
        sis$init.with.true.states,.echo)
      # prior.particles is an N vector.  Number gets larger with each iteration.
      pre.particle.indx <- (1:sis$N)+(i-1)*sis$N
    } else {
        priors <- get.priors(K,N.params,.year,i,
          params.fixed,params.islogit,a,h.sq,.echo)
        pre.params <- priors$params
        pre.states <- priors$states
        pre.particle.indx <- priors$particle.indx
    }
    #write the priors out to file
    if (.echo) cat ("Writing priors to database.\n")
    assign(paste("pre.particle.indx.year",.year,".rep",i,sep=""),pre.particle.indx,where="sis.db")
    assign(paste("pre.params.year",.year,".rep",i,sep=""),pre.params,where="sis.db")
    assign(paste("pre.states.year",.year,".rep",i,sep=""),pre.states,where="sis.db")

    #run the particle filter - returns a list of posterior states and
    # parameters, and number of unique particles sampled
    if(is.null(sis$true.states)) {
      out <- filter.particles(sis$obs[,.year], sis$distances, pre.particle.indx,
        pre.params, pre.states,
        sis$N,sis$N1,a,h.sq,.echo,NULL,sis$allow.movement,
        p,params.fixed,params.islogit,sis$pop.process.is.stochastic,
        sis$resid.resamp,sis$dens.dep.surv,sis$dens.dep.fecund,.year,i)
    } else {
      out <- filter.particles(sis$obs[,.year], sis$distances, pre.particle.indx,
        pre.params, pre.states,
        sis$N,sis$N1,a,h.sq,.echo,sis$true.states[,,.year],sis$allow.movement,
        p,params.fixed,params.islogit,sis$pop.process.is.stochastic,
        sis$resid.resamp,sis$dens.dep.surv,sis$dens.dep.fecund,.year,i)
    }

    #write out the results
    if (.echo) cat ("Writing posteriors to database.\n")
    assign(paste("post.particle.indx.year",.year,".rep",i,sep=""),out$particle.indx,where="sis.db")
    assign(paste("post.params.year",.year,".rep",i,sep=""),out$params,where="sis.db")
    assign(paste("post.states.year",.year,".rep",i,sep=""),out$states,where="sis.db")

    #see if enough unique yet, or if you've reached the max i
    total.unique.1 <- total.unique.1 + out$n.unique.1
    total.unique.2 <- total.unique.2 + out$n.unique.2
    unique.particle.indx <- unique(c(unique.particle.indx,out$particle.indx))
    total.unique.3 <- length(unique.particle.indx)
    if (.echo) cat ("End of iteration ",i,
       "\nTotal.unique.1:",total.unique.1,
       "\nTotal.unique.2:",total.unique.2,
       "\nTotal.absolute.unique:",total.unique.3,
       "\n")
    if (((total.unique.1 >= sis$min.N.unique)&
         (total.unique.2 >= sis$min.N1.unique)&
         (total.unique.3 >= sis$min.absolute.unique))| i==max.i) break
    
  }

  #save number of particles generated, so you know how many to read from file next year
  .meta <- get("meta",where="sis.db")
  .meta[.year,] <- c(i,i*sis$N,total.unique.1,total.unique.2,total.unique.3)
  assign("meta",.meta,where="sis.db")
  .meta <- NULL
  
  #increment the year counter, ready for next time
  .year <<- .year+1

  invisible(NULL)
}

#-----------------------------------------------------------------------------------------------#

initialize.params <- function(N,prior.params,prior.params.dist,echo)
#Simulates N parameter values from the prior.parameters
#Inputs:
# N - number of sims
# prior.params - a 2xN.params matrix of upper and lower bounds for parameters
# prior.params.dist - a char vector: "fixed", "uniform", "gamma", "beta"
# echo - whether to write out summary of priors
#Output:
# Returns a matrix of initial parameter values, N.params * N
{
  N.params <- dim(prior.params)[[2]]
  initial.params <- matrix(0,N.params,N)

  for (i in 1:N.params) {
    initial.params[i,] <- switch(prior.params.dist[i],
      fixed=rep(prior.params[1,i],N),
      uniform=runif(N,prior.params[1,i],prior.params[2,i]),
      gamma=rgamma(N,prior.params[1,i],scale=prior.params[2,i]),
      beta=rbeta(N,prior.params[1,i],prior.params[2,i]))
    if(is.null(initial.params[i,1])) stop("One or more elements of prior.params.dist are not set correctly.") 
    if(echo) {
      cat(names(prior.params.dist[i]),prior.params.dist[i],
        " mean=", round(mean(initial.params[i,]),7),
        ", sd=",round(sqrt(var(initial.params[i,])),7),"\n")
    }
  }
  if(echo) cat("\n")
  return(initial.params)
}

#-----------------------------------------------------------------------------------------------#

initialize.states <- function(initial.params,p,obs,state.inflate,N.ages,
  dens.dep.surv,dens.dep.fecund,true.states=NULL,init.with.true.states=F,echo)
#Simulates N parameter values from the prior.parameters
#Inputs:
# initial.params - an N.params x N matrix of initial parameter values
# p - list containing positions of parameters in initial.params
# obs - matrix of pup count estimates - K x T1 
# state.inflate - factor to inflate initial state conditions using runif
#    factor to deflate initial state conditions is worked out from this
#    note that state.inflate must be less than 2
# N.ages - number of age classes
# p.female ... move.2 are the position of global parameters
# dens.dep.surv - if T, have density dependent survival
# dens.dep.fecund - if T, have density depenent fecundity
# true.states - array of known states - colonies x age x years 
# init.with.true.states - if T and true.states not NULL then
#  uses the true states to inititalize the states (with some error) - 
#  otherwise just uses the pup counts
# echo - if T then get printed summary of initial states
#Output:
# Returns an array of initial states, age x K x N
{

  K <- dim(obs)[[1]]
  N <- dim(initial.params)[[2]]
  N.params <- dim(initial.params)[[1]]
  N.g.params <- N.params - K
  states.available <- (!is.null(true.states))

  #working out state.deflate in this manner keeps the same mean in runif
  if(state.inflate>=2) stop("state.inflate must be < 2")
  state.deflate <- 1/(2-state.inflate)

  initial.states <- array(0,c(N.ages,K,N))

  for(colony in 1:K) {         

    # Generating pups     
    yhat <- obs[colony,1]     
    if (states.available & init.with.true.states) {
      initial.states[1,colony,] <- true.states[1,colony,1]
    } else {
      if(!is.na(yhat) & yhat > 0) {       
        # there are pups observed, so generate pups assuming normal around       
        #  observed, with constant cv=psi       
        initial.states[1,colony,]  <- rnorm(N,yhat,yhat*initial.params[p$psi,])       
        # where this generates values <0, set to 0       
        initial.states[1,colony,(initial.states[1,colony,]<0)] <- 0
      } else {       
        if(is.na(yhat)) {         
          # no measurement that year, so use minimum observed value over all years         
          temp <- obs[colony,]         
          temp <- temp[!is.na(temp)]
          temp <- min(temp)
          if (temp > 0) {
            initial.states[1,colony,] <- rnorm(N,temp,temp*initial.params[p$psi,])         
            # where this generates values <0, set to 0         
            initial.states[1,colony,(initial.states[,colony,1]<0)] <- 0
          } else {
            initial.states[1,colony,] <- 0
          }
        } else {           
          # 0 pups observed so just set to 0         
          initial.states[1,colony,] <- rep(0,N)       
        }     
      }     
    }
    # Add extra variability into pup numbers, and round
    temp <- (initial.states[1,colony,]>0)
    initial.states[1,colony,temp] <- round(runif(sum(temp),
      initial.states[1,colony,temp]/state.deflate, 
      initial.states[1,colony,temp]*state.inflate))
    if (echo) {
      # Print pup counts and truth, if truth is known       
      cat("\nInitialization Colony=",colony,         
        "\npup sim summary=",summary(initial.states[1,colony,]),"\n")     
      if(states.available)       
        cat(" truth=",true.states[1,colony,1])      
      cat (" est pups=",yhat,"\n")
    }
    # Generating age 1 up to age 6
    # Based on numbers of pups, and known survival rates, with
    #  added variability given by state.inflate and state.deflate
    # Work out juvenile survival based on number of pups in the
    #  current year (rather than last year, as there is no last year!)
    Sj <- get.colony.Sj(initial.states[1,colony,],initial.params,p,K,colony,dens.dep.surv)
    # Note - age here is 1 more than adult age -- ie age2 == an age 1 adult
    for(age in 2:N.ages) {                                        
      #age 1 adults
      if (age==2) {         
        if(states.available & init.with.true.states) {
           initial.states[age,colony,] <- true.states[age,colony,1]
        } else {
          initial.states[age,colony,] <- rbinom.0(N,initial.states[1,colony,],
            initial.params[p$pFemale,]*Sj)
        }
      } else {
       #age 6+s
       if (age==N.ages) {
           if(states.available & init.with.true.states) {
             initial.states[age,colony,] <- true.states[age,colony,1]
           } else {
             if(dens.dep.surv) {
               #use number of pups and probability that a female has a pup to
               #back-calculate the number of breeders.
               #number of females not having a pup has a negative binomial
               #distribution
               initial.states[age,colony,] <- rnbinom.0(N,initial.states[1,colony,],
                  initial.params[p$aMax,])
               #so need to add on the number of females who did have a pup == the
               #number of pups, as one pup per female
               initial.states[age,colony,] <- initial.states[age,colony,]+
                 initial.states[1,colony,]
             } else {
               #dens.dep.fecundity -- so can't work out the number of breeders
               #in a simple manner.  Need to think about this more...
               initial.states[age,colony,] <- rbinom.0(N,initial.states[1,colony,],
                 (initial.params[p$pFemale,]*Sj*initial.params[p$Sa,]^5)/
                 (1-initial.params[p$Sa,]))
             }
           }
        #other ages
        } else {
          if(states.available & init.with.true.states) {
             initial.states[age,colony,] <- true.states[age,colony,1]
          } else {
            initial.states[age,colony,] <- rbinom.0(N,initial.states[age-1,colony,],
              initial.params[p$Sa,])
          }
        }
      }
      #Add extra variability, and round
      temp <- (initial.states[age,colony,]>0)
      initial.states[age,colony,temp] <- round(runif(sum(temp),
        initial.states[age,colony,temp]/state.deflate,
        initial.states[age,colony,temp]*state.inflate))
      if (echo) {
        #Print ages and truth, if truth is known       
        cat("age ",age-1," sim summary=",summary(initial.states[age,colony,]),"\n")       
        if(states.available)         
          cat(" truth=",true.states[age,colony,1],"\n")     
      }
    }   
  } 
  if (echo) cat("\n")

  return(initial.states)
}

#-----------------------------------------------------------------------------------------------#

get.priors <- function(K,N.params,year,i,params.fixed,params.islogit,a,h.sq,echo)
#Returns a list containing priors for this year 
# $states (N.ages x K x N), $params (N.params x N), $particle.indx (N vector)
{
  #get number of iterations last year
  prev.i <- get.iterations(year-1)

  pre.particle.indx <- rep(0,sis$N)
  pre.states <- array(0,c(sis$N.ages,K,sis$N))
  pre.params <- matrix(0,N.params,sis$N)

  if (prev.i == 1) {

    #just read previous year's posterior states and params straight in
    name <- paste("post.particle.indx.year",year-1,".rep1",sep="")
    pre.particle.indx <- get(name,where="sis.db")
    name <- paste("post.states.year",year-1,".rep1",sep="")
    pre.states <- get(name,where="sis.db")
    name <- paste("post.params.year",year-1,".rep1",sep="")
    pre.params <- get(name,where="sis.db")

  } else {

    #Get a sample size N of previous year's posterior states and params
    #Do this by getting N/prev.i from each iteration from prev year
    N.per.file <- floor(sis$N/prev.i)
    for (i in 1:prev.i) {
      #make sure you get just the right number of sampled in total
      if (i == prev.i) {
        N.to.sample <- sis$N - N.per.file*(prev.i-1)
      } else {
        N.to.sample <- N.per.file
      }
      samp <- sample(1:sis$N,N.to.sample,replace=F)
      name <- paste("post.particle.indx.year",year-1,".rep1",sep="")
      temp.particle.indx <- get(name,where="sis.db")
      pre.particle.indx[(1:N.to.sample)+(N.per.file*(i-1))] <- temp.particle.indx[samp]
      name <- paste("post.states.year",year-1,".rep",i,sep="")
      temp.states <- get(name,where="sis.db")
      pre.states[,,(1:N.to.sample)+(N.per.file*(i-1))] <- temp.states[,,samp]
      name <- paste("post.params.year",year-1,".rep",i,sep="")
      temp.params <- get(name,where="sis.db")
      pre.params[,(1:N.to.sample)+(N.per.file*(i-1))] <- temp.params[,samp]
    }
  }

  if (i > prev.i) {
    
    #you've already had more repititions this year than last time, so try
    # some kernal smoothing of parameters to see if that helps
    #using 1:sis$N ensures that there will be no resampling, just smoothing
    pre.params <- sample.params((1:sis$N),pre.params,params.fixed,params.islogit,a,h.sq,echo)
    
  }

  return(list(particle.indx=pre.particle.indx,params=pre.params,states=pre.states))

}

#-----------------------------------------------------------------------------------------------#

get.iterations <- function(year)
#Returns the number of iterations used in given year 
{
  meta <- get("meta",where="sis.db")
  iterations <- c(meta[year,1])
  return(iterations)
}

#-----------------------------------------------------------------------------------------------#

filter.particles <- function(obs=NULL,distances=NULL,pre.particle.indx=NULL,
  pre.params=NULL,pre.states=NULL,
  N=100,N1=N,a,h.sq,echo=T,true.states=NULL,allow.movement=T,p,params.fixed,params.islogit,
  pop.process.is.stochastic,resid.resamp,dens.dep.surv,dens.dep.fecund,year,iteration) 
#Driver routine for doing auxiliary particle filtering for a year's data
#Inputs:
# obs - vector of pup count estimates for this year - colonies
# distances - square matrix of distances between colonies
# pre.particle.indx - N vector giving the number of each particle (all start as unique)
# pre.params - n.params x N.pre matrix of prior parameter estimates for particles (note -
#  N.pre isn't necessarily the same as the input N or N1)
# pre.states - N.ages x K x N.pre array of prior states for particles
# N - number of particles to return
# N1 - number of auxiliary particles to sample
# a and h.sq - used in kernal smoothing
# true.states - array of known states for this year - age x colonies
# allow.movement - if F then no movement in simlated populations
# p - indexes params
# params.fixed - tells which are fixed
# params.islogit - tells what kind of transform is needed
# pop.process.is.stochastic - if T then pop process is stochastic, if F then deterministic
# resid.resamp - if T then uses residual resampling, otherwise ordinary resampling with
#  replacement
# dens.dep.surv - if T then survival is density dependent
# dens.dep.fecund - if T then fecundity is density dependent 
#  (NOTE: both surv and fecund cannot both be density dependent)
# year - used in plotting results
# iteration - used in plotting results
# 
{
  
  N.ages <- dim(pre.states)[[1]]
  K <- dim(pre.states)[[2]]
  N.params <- dim(pre.params)[[1]]
  N.g.params <- N.params - K
  
  #temp arrays for storing states and params when auxiliary filtering
  aux.states <- array(0,c(N.ages,K,N1))
  aux.params <- matrix(0,N.params,N1)
  #results arrays
  particle.indx <- rep(0,N)
  states <- array(0,c(N.ages,K,N))
  params <- matrix(0,N.params,N)

  #work out expected pup counts, given current states and params
  #expected.pups is an array of expected pup counts - matrix K x N.pre
  expected.pups <- get.expected.pups(pre.states,pre.params,p,N.ages,dens.dep.fecund)

  #get weights for these expected pup counts, given observations
  if (echo) cat ("--- Weights1 ---\n")
  weights.1 <- get.weights(obs,expected.pups,pre.params,p,echo)

  #resample according to these weights
  k <- resample(N1,weights.1,resid.resamp,echo)
  n.unique.1 <- length(unique(k))

  #sample new parameters, and kernal smooth
  aux.params <- sample.params(k,pre.params,params.fixed,
    params.islogit,a,h.sq,echo)
  #sample from state evolution process - note that the
  #k in the pre.states[,,k] means that we're passing in resampled states
  aux.states <- pop.process(aux.params,p,pre.states[,,k],
   distances,allow.movement,true.states,obs,echo,pop.process.is.stochastic,
   dens.dep.surv,dens.dep.fecund)

  #get weights for the new pup counts, given observations
  if (echo) cat ("--- Weights2 ---\n")
  weights.2 <- get.weights(obs,aux.states[1,,],aux.params,p,echo)

  #work out overall weight, and resample to give final posterior approximation
  if (echo) cat ("--- Weights3 ---\n")
  weights.3 <- get.final.weights(weights.1[k],weights.2,echo)
  l <- resample(N,weights.3,resid.resamp,echo)
  params <- aux.params[,l]
  states <- aux.states[,,l]
  n.unique.2 <- length(unique(k[l]))

  #update the particle index
  particle.indx <- pre.particle.indx[k[l]]

  #plot intermediate results
  if (echo) 
    plot.params(pre.params,aux.params,params,
      params.fixed,p,N.g.params,year,iteration,length(unique(particle.indx)))

   return(particle.indx,params,states,n.unique.1,n.unique.2)
}

#-----------------------------------------------------------------------------------------------#

get.a <- function(n.breeders,current.params,p,dens.dep.fecund)
#Returns KxN matrix of fecundity parameters for all colonies
# For fecundity params for an individual colony, see next function
#Inputs:
# n.breeders KxN matrix of breeders at current time 
#  (doesn't need to be integer -- can be expected num breeders)
# current.params n.paramsxN matrix of params at current time
# p indexes params
# dens.dep.fecund is T if fecundity is density dependent
{
  N <- dim(n.breeders)[[2]]
  K <- dim(n.breeders)[[1]]
  N.params <- dim(current.params)[[1]]
  N.g.params <- N.params - K

  aMax <- current.params[p$aMax,]

  if(dens.dep.fecund) {
    b <- matrix(current.params[(N.g.params+1):N.params,],K,N)
    a <- aMax/(1+b*n.breeders)
  } else {
    #fecundity is not density dependent, so fecundity is aMax
    a <- matrix(aMax,K,N)
  }
  return(a)
}

#-----------------------------------------------------------------------------------------------#

get.colony.a <- function(n.breeders,current.params,p,K,colony,
  dens.dep.fecund)
#Returns N vector of fecundity parameters for a colony
#Inputs:
# n.breeders N vector of breeders at current time in that colony
#  (doesn't need to be integer -- can be expected num breeders)
# current.params n.paramsxN matrix of params at current time
# p indexes params
# K is total number of colonies
# colony is the colony number you want fecundity params for
# dens.dep.fecund is true if fecundity is density dependent
{
  N <- count.rows(n.breeders)
  N.params <- dim(current.params)[[1]]
  N.g.params <- N.params - K

  aMax <- current.params[p$aMax,]
  if (dens.dep.fecund) {
    b <- current.params[N.g.params+colony,]
    a <- aMax/(1+b*n.breeders)
  } else {
    #fecundity is not density dependent, so fecundity is aMax
    a <- aMax
  }
  return(a)
}
#-----------------------------------------------------------------------------------------------#

get.expected.pups <- function(current.states,current.params,p,N.ages,dens.dep.fecund)
#Returns KxN matrix of expected pups, given current states and params
{
  N <- dim(current.states)[[3]]
  K <- dim(current.states)[[2]]
  N.params <- dim(current.params)[[1]]
  N.g.params <- N.params - K

  expected.n.breeders <- t(t(current.states[N.ages-1,,]+current.states[N.ages,,])*current.params[p$Sa,])
  expected.a <- get.a(expected.n.breeders,current.params,p,dens.dep.fecund)
  expected.pups <- expected.n.breeders*expected.a
  return(expected.pups)
}
#-----------------------------------------------------------------------------------------------#

get.Sj <- function(n.pups,current.params,p,dens.dep.surv)
#Returns KxN matrix of juvenile survival parameters for all colonies
# For survival params for an individual colony, see next function
#Inputs:
# n.pups KxN matrix of pups produced in previous year 
# current.params n.paramsxN matrix of params at current time
# p indexes params
# dens.dep.surv is T if survival is density dependent
{
  N <- dim(n.pups)[[2]]
  K <- dim(n.pups)[[1]]
  N.params <- dim(current.params)[[1]]
  N.g.params <- N.params - K

  SjMax <- current.params[p$SjMax,]

  if(dens.dep.surv) {
    b <- matrix(current.params[(N.g.params+1):N.params,],K,N)
    Sj <- SjMax/(1+b*n.pups)
  } else {
    #juvenile survival is not density dependent, so fecundity is SjMax
    Sj <- matrix(SjMax,K,N)
  }
  return(Sj)
}

#-----------------------------------------------------------------------------------------------#

get.colony.Sj <- function(n.pups,current.params,p,K,colony,
  dens.dep.surv)
#Returns N vector of juvenile survival parameters for a colony
#Inputs:
# n.pups N vector of pups produced in previous time in that colony
# current.params n.paramsxN matrix of params at current time
# p indexes params
# K is total number of colonies
# colony is the colony number you want fecundity params for
# dens.dep.fecund is true if fecundity is density dependent
{
  N <- count.rows(n.pups)
  N.params <- dim(current.params)[[1]]
  N.g.params <- N.params - K

  SjMax <- current.params[p$SjMax,]
  if (dens.dep.surv) {
    b <- current.params[N.g.params+colony,]
    Sj <- SjMax/(1+b*n.pups)
  } else {
    #survival is not density dependent, so fecundity is aMax
    Sj <- SjMax
  }
  return(Sj)
}

#-----------------------------------------------------------------------------------------------#

get.weights <- function(obs.pups,state.pups,current.params,p,echo)
#Returns N array of weights, given observed pups and simulated pup numbers, and current
# observation error parameter values
#Inputs:
# obs.pups - K vector of observed pups (can be 0 or na)
# state.pups - KxN vector of simulated pup states (can be non-integer if expected pups)
# current.params n.paramsxN matrix of params at current time
# p indexes params
{
  K <- dim(state.pups)[[1]]
  N <- dim(state.pups)[[2]]
  
  #set states that are 0 to 1 to avoid numerical problems with dnorm
  # will set density to 0 later
  pup.sim <- state.pups
  pup.sim[pup.sim==0] <- 1

  #index missing observations
  missing.pup <- is.na(obs.pups)
  num.set <- (K - sum(missing.pup))

  if(num.set==0) {

    prob.mat <- matrix(1,K,N)

  } else {

    yhat <- matrix(obs.pups,K,N,byrow=F)
    #constant CV model
    prob.vec <- dnorm(as.vector(yhat),as.vector(pup.sim),
      as.vector(t( t(pup.sim)*current.params[p$psi,] )))

    #where states were 0, now set density to 0
    prob.vec[(as.vector(state.pups))==0] <- 0
    prob.mat <- matrix(prob.vec,K,N)

    #where missing observations, set density to 1
    prob.mat[missing.pup,] <- 1
    #scale by a constant to prevent underflow when multiply them together
    #note - calculate the constant each time weights is called as psi vector
    # may have changed
    constant <- get.scaling(obs.pups,current.params,p,echo)
    prob.mat <- prob.mat*constant

  }

  #weight is product of colony probabilities
  weights <- apply(prob.mat,2,prod)
  if (sum(weights)==0) {
    weights.std <- rep(1/N,N)
    if(echo) cat ("** Warning - all weights are 0, so set to 1/N\n")
  } else {
    weights.std <- weights/sum(weights)
  }

  if (echo) {
    cat("Summary of weights: \n")
    print(summary(weights.std))
    hi.prob.pos <- order(-weights.std)[1:5]
    cat("Global highest 5 weights",weights.std[hi.prob.pos],"\n")
  }

  return(weights.std)
}

#-----------------------------------------------------------------------------------------------#

get.scaling <- function(obs.pups,current.params,p,echo)
#Returns scaling constant for multiplying weights
#Scaling constant geo mean of 1/prob that observation matches simulated values
# (with a fudge factor to deal with 0 observations)
#Inputs:
# obs.pups - K vector of observed pups (can be 0 or na)
{

  K <- count.rows(obs.pups)
  #index missing observations
  missing.pup <- is.na(obs.pups)
  num.set <- (K - sum(missing.pup))
  
  temp <- obs.pups
  temp[temp==0] <- 0.1
  # constant CV model
  prob.scaling <- dnorm(obs.pups[!missing.pup],temp[!missing.pup],
    temp[!missing.pup])*mean(current.params[p$psi,])

  if (echo) {
    cat("Scaling constant: \n")
    cat(" num colonies observed this year=",num.set,"\n")
    cat(" summary probs:","\n")
    print(summary(prob.scaling))
    cat(" product probs=",prod(prob.scaling),"\n")
  }
  #scale by inverse of geometric mean
  constant <- 1/((prod(prob.scaling))^(1/num.set))  
  if (echo) cat(" inflator=",constant,"\n")
  #set to 1 if 0, inf or na
  if(is.finite(constant)) {
    if (constant == 0) {
      if (echo) cat ("  inflator 0, so set to 1\n")
      constant <- 1.0
    }
  } else {
     if (echo) cat ("  inflator not finite, so set to 1\n")
     constant <- 1.0
  }
  return(constant)
}

#-----------------------------------------------------------------------------------------------#

resample <- function(N.out,weights,resid.resamp=F,echo=T)
#Returns vector of integers, resampled with replacement according to weights.  
#  Weights assumed to sum to 1.
#  N.out can be different
#  from length of weights
#
#Inputs:
# N.out - number of resamples required
# weights - vector of weights
# resid.resamp - if true, get residual resampling - see Lui&Chen, 1998, JASA
# echo - if true, get output
{

  N <- count.rows(weights)
  if (resid.resamp) {
    sure.samp.reps <- floor(weights*N.out)
    N.sure.samp <- sum(sure.samp.reps)
    resid.weights <- weights-(sure.samp.reps/N.out)
    resid.weights <- resid.weights/sum(resid.weights)
    #sample consists of the sure samples, plus the residual part sampled
    # randomly, with replacement
    samp <- c(rep(1:N,sure.samp.reps),
      sample(1:N,N.out-N.sure.samp,replace=T,prob=resid.weights))
  } else {
    samp <- sample(1:N,N.out,replace=T,prob=weights)
  }
  if (echo) {
    cat(ifelse(resid.resamp,"Residual","Ordinary"), "resampling, ",N.out,
      " samples.  Num unique =",length(unique(samp)),"\n")
  }
  return(samp)
}

#-----------------------------------------------------------------------------------------------#

sample.params <- function(k,old.params,params.fixed,params.islogit,a,h.sq,echo)
#Returns matrix of parameter values, resampled from input params using indicators k, and kernal
# smoothed.  k vector can be of different length than number of rows in params -- 
# in which case the output matrix contains k rows.
#Inputs:
# k - array of indicator variables, indicating which sets of params to resample
# old.params - matrix of params - n.params x N
# params.fixed - vector of length n.params -- T if parameter is fixed (no need to resample
#  or smooth)
# params.islogit - if T then use a logit transform
# a, h.sq - smoothing parameters for kernal smooth
{

  N <- dim(old.params)[[2]]
  N.out <- count.rows(k)
  n.params <- dim(old.params)[[1]]
  n.free <- n.params - sum(params.fixed)

  #note - theta.star is Nxparams, while old.params is paramsxN
  theta.star <- matrix(0,N,n.free)
  #make a mapping for columns of old.params to theta.star
  mapping <- rep(0,n.free)
  j <- 1
  for (i in 1:n.params) {
    if(!params.fixed[i]) {
      mapping[j] <- i
      j <- j+1
    }
  }
  #transform parameters and put in theta.star
  for (i in 1:n.free) {
     if (params.islogit[mapping[i]]) {
        theta.star[,i] <- log(old.params[mapping[i],]/(1-old.params[mapping[i],]))
     } else {
        theta.star[,i] <- log(old.params[mapping[i],])
     }
  }

  mean.theta.star <- apply(theta.star,2,mean)
  mean.theta.star.mat <- matrix(mean.theta.star,N,n.free,byrow=T)
  sigma.theta.star <- var(theta.star)

  if (echo) {
    cat ("\n--- Kernel smoothing of states--- \n\n")
    cat ("Mean theta* before resampling\n")
    print (mean.theta.star)
    cat ("Sigma theta* before resampling\n")
    print (sigma.theta.star)
  }

  #kernal smoothing - see Liu and West
  m.matrix <- a*theta.star + (1-a)*mean.theta.star.mat
  epsilon <- rmvnorm(N.out,mean=rep(0,n.free),cov=h.sq*sigma.theta.star)
  #resample
  theta.star.star <- m.matrix[k,] + epsilon

  #unpack back to parameters
  new.params <- old.params[,k]
  for (i in 1:n.free) {
     if (params.islogit[mapping[i]]) {
       temp <- exp(theta.star.star[,i])
       new.params[mapping[i],] <- temp/(1+temp)
     } else {
       new.params[mapping[i],] <- exp(theta.star.star[,i])
     }
  }
  
  return(new.params)
}

#-----------------------------------------------------------------------------------------------#

pop.process <- function(params,p,old.states,distances,allow.movement,
   true.states,obs,echo,is.stochastic=T,dens.dep.surv,dens.dep.fecund)
#Simulates the population process.
#Returns an array of states(agesxKxN) projected probabilistically one step further forward
# in time.
#Inputs:
# params - n.paramsxN matrix of current parameter estimates
# p - list giving locations of parameters in params
# old.states - agesxKxN array of states at time t
# distances - matrix of distances between colonies
# true.states - agesxK matrix of true states
# obs - pup counts -- K vector
# echo - if true, get output to command window
# is.stochastic - if true, pop process is stochastic, if F, deterministic
# dens.dep.surv - if T then survival is density dependent
# dens.dep.fecund - if T then fecundity is density dependent 
#  (NOTE: both surv and fecund cannot both be density dependent)
{
  N.ages <- dim(old.states)[[1]]
  K <- dim(old.states) [[2]]
  N <- dim(old.states) [[3]]
  new.states <- array(0,c(N.ages,K,N))
  states.available <- (!is.null(true.states))
  if (echo) cat("\n --- Population dynamics model --- \n")

  #--- Loop over colony ---
  if (echo) cat("\n --- Begin colony survival and breeding ---\n")
  for(colony in 1:K) {
  
    if (echo) cat("\n --- Colony=",colony," ---\n")

    #---Survival and age incrementation ----
    Sj <- get.colony.Sj(old.states[1,colony,],params,p,K,colony,dens.dep.surv)
    #age 1
    if (is.stochastic) {
      new.states[2,colony,] <- rbinom.0(N,old.states[1,colony,],
        params[p$pFemale,]*Sj)
    } else {
      new.states[2,colony,] <- round(old.states[1,colony,]*
        params[p$pFemale,]*Sj)
    }
    if(echo) {
      cat("age= 1 :",summary(new.states[2,colony,]))
      if(states.available) 
        cat(" true=",true.states[2,colony],":",
           round(sum(new.states[2,colony,] < true.states[2,colony])/N*100,2),"%")
      cat("\n")
    }

    #age 2 to 5, where 5 is pre-movement
    for (age in 3:(N.ages-1)) {
      if (is.stochastic) {
        new.states[age,colony,] <- rbinom.0(N,old.states[age-1,colony,],params[p$Sa,])
      } else {
        new.states[age,colony,] <- round(old.states[age-1,colony,]*params[p$Sa,])
      }
      if (echo) {
        cat("age=",age-1,":",summary(new.states[age,colony,]))
        if(states.available)
           cat(" true =",true.states[age,colony],":",
             round(sum(new.states[age,colony,] < true.states[age,colony])/N*100,2),"%")
        cat("\n")
      }
    }

    #Age 6
    if (is.stochastic) {
      new.states[N.ages,colony,] <- rbinom.0(N,old.states[N.ages-1,colony,],params[p$Sa,]) +
        rbinom.0(N,old.states[N.ages,colony,],params[p$Sa,])
    } else {
      new.states[N.ages,colony,] <- round(old.states[N.ages-1,colony,]*params[p$Sa,] +
        old.states[N.ages,colony,]*params[p$Sa,])
    }
    if (echo) {
      cat("age=",N.ages-1,"+:",summary(new.states[N.ages,colony,]))
      if(states.available)
         cat(" true =",true.states[N.ages,colony],":",
           round(sum(new.states[N.ages,colony,] < true.states[N.ages,colony])/N*100,2),"%")
      cat("\n")
    }

    #-----Births -------------
    a <- get.colony.a(new.states[N.ages,colony,],params,p,K,colony,dens.dep.fecund)
    if (is.stochastic) {
      new.states[1,colony,] <- rbinom.0(N,new.states[N.ages,colony,],a)
    } else {
      new.states[1,colony,] <- round (new.states[N.ages,colony,]*a)
    }
    if (echo) {
      cat("est pups=",obs[colony],"\npups :",summary(new.states[1,colony,]))
      if (states.available) {
        cat(" true =",true.states[1,colony],":", 
          round(sum(new.states[1,colony,] < true.states[1,colony])/N*100,2),"% \n")
      }
      cat ("\n")
    }
  } #end of loop over colonies
  
  #---------------------------------Movement ----------------------#
  #note - there will be no movement if there is no density dependence
  # so save time by not even calling the movement routine
  if(allow.movement & (dens.dep.surv|dens.dep.fecund)) {
    #Work out potential fecundity
    a <- get.a(new.states[N.ages,,],params,p,dens.dep.fecund)
    #Do movement
    if(states.available) {
      new.states[N.ages-1,,] <- move.seals(new.states[N.ages-1,,],
        rbind(params[p$move1,],params[p$move2,],params[p$move3,]),
        a,distances,echo,is.stochastic,true.states[N.ages-1,])
    } else {
      new.states[N.ages-1,,] <- move.seals(new.states[N.ages-1,,],
        rbind(params[p$move1,],params[p$move2,],params[p$move3,]),
        a,distances,echo,is.stochastic)
    }
  }

  return(new.states)
}

#-----------------------------------------------------------------------------------------------#

move.seals <- function(age5.mat,move.mat,
  dd.mat,distances.mat,details=F,
  is.stochastic=T,true.age5=NULL)
# Simulates movement of age 5 seals among colonies
# Inputs:
#  age5.mat - matrix of age 5 seals by colony,sim
#  move.mat - matrix of movement parameters by parameter,sim
#  dd.mat - matrix of density dependent parameter, by colony, sim
#    if survival is density dependent then dd.mat is a matrix of survival
#    if fecundity is density dependent, then dd.mat is a matrix of fecundities
#    if neither is density dependent, then dd.mat must be null
#  distances.mat - square matrix of distances between colonies
#  details - if T then output summary of simulated movements
#  is.stochastic - if true, pop process is stochastic, if F, deterministic
#  true.age5 - array containing true states, indexed by colony
#   If not null and details==T then
#   you get true age 5 distribution as well as simulated movements
# Outputs:
#  matrix of age 5 seals by sim, colony after movement
{
  # K=num colonies; N=num sims
  N <- dim(age5.mat)[[2]]
  K <- dim(age5.mat)[[1]]
  # moving.N5 is going to store the seals after movement
  moving.N5.mat <- matrix(0,K,N)

  if(details)
    cat("\n --- Begin Movement ---\n")

  for(colony in (1:K)) {                                                         

    #Probability of moving to each colony 
    if (is.null(dd.mat)) {
      #no density dependence - so no movement
      aj.ai <- matrix(0,K,N)
    } else {
      #density dependence, so movement depends on differences in the
      # density depenent parameter among colonies
      aj.ai <- t(t(dd.mat) - dd.mat[colony,])
      aj.ai[aj.ai < 0] <- 0
    }
    #3 components of the movement model
    comp.1 <- comp.2 <- comp.3 <- matrix(0,K,N)
    #comp 1 density dependence
    if (!is.null(dd.mat)) {
      comp.1[-colony,] <- t(move.mat[1,]*t(aj.ai[-colony,]))
    }
    #comp 2 movement param 2 * dist
    comp.2[-colony,] <- t(move.mat[2,]%*%t(distances.mat[colony,-colony]))
    #comp 3 is movement parameter 3, only for the colony
    comp.3[colony,] <- move.mat[3,]
    theta.set <- exp(comp.1+comp.2+comp.3)
    #note - move.prob is NxK, rather than KxN
    move.prob <- t(theta.set)/apply(theta.set,2,sum)

    if (is.stochastic) {
       allocations.mat <- t(rmultinom(N,age5.mat[colony,],move.prob))
    } else {
       allocations.mat <- t(age.5.mat[colony,]*move.prob)
       #need to round the allocations
       allocations.mat <- round(allocations.mat)
       #so you don't "loose" any seals in the rounding
       # make the number remaining age5.mat[colony,]-
       # sum(rounded allocations to all other colonies)
       allocations.mat[colony,] <- age5.mat[colony,] - apply(allocations.mat[-colony,],2,sum)
    }

    moving.N5.mat <- moving.N5.mat + allocations.mat
    if (details) {
      cat("Movement for Colony=",colony,
      " mean(a)=",round(mean(dd.mat[colony,]),2), 
      " mean(movers) = ", round(mean(age5.mat[colony,]),2),"\n")
      cat(" mean(Pr(move))=",round(apply(move.prob,2,mean),2),"\n")
      cat(" mean(Allocation)=",round(apply(allocations.mat,1,mean),2),"\n")

    }
  }

  #output true values if passed in
  if (details) cat ("\n")
  if (details & !is.null(true.age5)) {
    cat("Age 5s after movement: \n")
    for(colony in (1:K)) {                                                         
      cat("Colony=", colony, " truth=", true.age5[colony], 
        "summary(simulated)=",summary(moving.N5.mat[colony,]),"\n")
    }
  }

  if(details)
    cat("\n --- End Movement ---\n\n")

  return (moving.N5.mat)
}

#-----------------------------------------------------------------------------------------------#
get.final.weights <- function(weights.1,weights.2,echo)
#Returns standardized ratio weights.2/weights.1
{

  weights <- weights.2/weights.1
  weights.std <- weights/sum(weights)
  
  if (echo) {
   cat("Final weights\nSummary of weights: \n")
   print(summary(weights.std))
   hi.prob.pos <- order(-weights.std)[1:5]
   cat("Final highest 5 weights",weights.std[hi.prob.pos],"\n")
  }
  
  return(weights.std)
}

#-----------------------------------------------------------------------------------------------#

plot.params <- function(old.params,aux.params,new.params,
  params.fixed,p,N.g.params,year,iteration,n.absolute.unique)
#Plots the global parameters for this year's iteration
{
  #get ranges
  ranges <- matrix(0,N.g.params,2)
  for(i in 1:N.g.params) {
    ranges[i,] <- range(c(old.params[i,],
      aux.params[i,],new.params[i,]))
  }
  #plot old params
  for(i in 1:N.g.params) {
    if(!params.fixed[i])
      hist(old.params[i,],main=paste("old", names(p)[i], " N.u=", length(unique(old.params[i,]))),
        cex=0.6,xlim=ranges[i,])
  }
  #plot aux params
  for(i in 1:N.g.params) {
    if(!params.fixed[i])
      hist(aux.params[i,],main=paste("aux", names(p)[i], " N.u=", length(unique(aux.params[i,]))),
        cex=0.6,xlim=ranges[i,])
  }
  #plot aftr resamp params
  for(i in 1:N.g.params) {
    if(!params.fixed[i])
      hist(new.params[i,],main=paste("new", names(p)[i], " N.u=", length(unique(new.params[i,]))),
        cex=0.6,xlim=ranges[i,])
  }

  mytitle(paste("Year=",as.character(year),"Iteration=",as.character(iteration),
    "#Unique=",as.character(n.absolute.unique)))

  return(invisible())
}

#-----------------------------------------------------------------------------------------------#