## Implementation of a parameterized particle filter.

## Data consists of three matrixes, all all three dimensions, with the
## third dimension representing time, the second (columns) representing
## variables, and the first (rows) representing replications (subjects).

## <obs> Represents a collection of observations made about the
## student at each time point.  Missing values should be allowed (but
## I haven't developed handers yet).

## <act> Represents a series of actions taken by a decision maker.
## (In particular, a lesson plan).  This can be NULL if this
## information is not used by the filter.

## <suc> Represents the success/failure of each action.  This can be
## NULL if success/failure is not observed.


## <params> is a list of parameter objects.  This is passed to the
## other functions which should be able to extract values by name.

## <nparticles> controls the number of particles, and
## <resample> controls the number of cycles between
## bootstrap resampling of the particles.

## <cycletime> is the time length of a cycle 
## (relative to the other rate parameters)

## Thes arguments should be functions (or function 
## names) which calculate the given quantities.
## <obsLike> is the likelihood function for the 
## benchmark test
## <successProb> is the probability of success for the
## chosen action 
## <advanceData> is the forward step generation function of the
## stochastic process
## <advanceParam> is the forward step generation function of the
## stochastic process
## <initialData> is the population prior distribution for the Data
## <initialParam> is the population prior distribution for the Data
paramFilter <- function (filter,data,resample=0) {

  
  ncycle <- ncycle(data)
  ncases <- nsubjects(data)


  ##Initial setup
  ## The goal is to as much as possible use implicit 
  ## looping functions for operations across particles 
  ## and explicit looping functions for operations
  ## within a particle.
  param <- initParam(filter)
  est <- initData(filter,param,ncases)
  weight <- obsLlike(filter,param,data@obs[,,1],est)

  record <- new("BowtieFilterOutput",
                param = list(param), est=list(est),
                weight=list(weight), map=list(),
                  suc = list(), data=data, filter=filter)
  
  ## Loop over time points
  for (icycle in 1:ncycle) {

    ## Resample if necessary
    if (resample >0 && icycle %% resample == 0) {
      ## Resample
      cat("Resampling at cycle",icycle,"\n")
      map <- wlsample(nparticles(filter),weight)
      est <- est[,,map]
      cat("Ave weight before =",mean(weight),
          "; after=", mean(weight[map]),"\n")
      weight <- rep(0,length(weight))
    } else {
      ## identity map
      map <- 1:nparticles(filter)
    }
    record@map <-lappend(record@map,map)

    ## Handle the success variable.  If it is observed, it
    ## provides evidence for the current proficiency.  If it
    ## is unobserved, we need to simulate it.

    if (filter@usesSuccess) {
      if(data@successObserved) {
        success <- data@suc[,,icycle]
        weight <- weight + evalSuccess(filter,param,latent,action,success)
      } else {
        success <- drawSuccess(filter,param,latent,action)
      }
      record@suc <- lappend(record@suc,success)
    } else {
      success <- NULL
    }
      
    ## Advance position of particles
    if (filter@paramAdvances) {
      param <- advanceParam(filter,param,time=getCycletime(data,icycle))
      record@param <- lappend(record@param,param)
    }

    est <- advanceData(filter,param,est,data@act[,,icycle],success,
                       getCycletime(data,icycle))
    record@est <- lappend(record@est,est)

    ## Evaluate new observations
    weight <- weight + obsLlike(filter,param,data@obs[,,icycle+1],est)
    record@weight <- lappend(record@weight,weight)
  }
  record
}


paramSimulator <-
  function (filter, nsubjects, ntimepoints, estimator, policy,
  cycletime=1)
{
  ## Just one particle for simulation.
  nparticles(filter) <- 1
  if (missing(ntimepoints)) {
    if (length(cycletime)>1)
      ntimepoints <- length(cycletime)
  }
  if (length(cycletime ==1)) {
    cycletime <- rep(cycletime,ntimepoints)
  }
  ## Last cycle is stub only
  cycletime <- c(cycletime,0)

  nvar <- nlatent(filter)
  nobs <- nobserved(filter)
  nsuc <- nsuccess(filter)
  nact <- naction(filter)

  result <- new("BowtieSimulationData",
                obs   = array(NA,c(nsubjects,nobs,ntimepoints+1)),
                act   = array(NA,c(nsubjects,nact,ntimepoints+1)),
                suc   = array(NA,c(nsubjects,nsuc,ntimepoints+1)),
                successObserved = filter@usesSuccess,
                cycletime=cycletime,
                truth = array(NA,c(nsubjects,nvar,ntimepoints+1)),
                est   = array(NA,c(nsubjects,nvar,ntimepoints+1)),
                params = list(),
                paramsFollowTime=filter@paramsFollowTime)
  
  param <- initParam(filter)
  result@params <- list(param)

  latent <- initData(filter,param,nsubjects)
  result@truth[,,1] <- latent
  observed <- obsGen(filter,param,latent)
  result@obs[,,1] <- observed
  
  for (icycle in 1:ntimepoints) {
    
    estimate <- do.call(estimator,list(result))
    result@est[,,icycle] <- estimate
    action <- do.call(policy,list(estimate))
    result@act[,,icycle] <- action

    if (filter@usesSuccess) {
      success <- drawSuccess(filter,param,latent,action)
      result@suc[,,icycle] <- success
    } else {
      success <- NULL
    }
    
    if (filter@paramsFollowTime) {
      param <- advanceParamGen(filter,param,cycletime[icycle])
      result@params <- c(result@params,param)
    }

    latent <- advanceData(filter,param,latent,action,success,
                          cycletime[icycle])
    result@truth[,,icycle+1] <- latent

    observed <- obsGen(filter,param,latent)
    result@obs[,,icycle+1] <- observed

  }
  result
}

#####
## Helper functions



## Returns a sample of indexes (1:length(weights)) given
## size using weights as weights.  Assumes that the weights are given
## on the log scale.   
wlsample <- function (size, weights) {
  w <- cumsum(exp(weights-log(sum(exp(weights)))))
  ## Outer produces an array of true and false values,
  ## apply counts number of thresholds exceeded.
  apply(outer(runif(size),w,">"),1,sum)+1
}


## Because sometimes S just gets it wrong
lappend <- function (lst,val) {c(lst,list(val))}