#include <stdlib.h>
#include <stdio.h>

#include "data_types.h"
#include "model_parameters.h"
#include "condensation.h"

/* All of the global information is packaged into the following two
   structures. `global' contains information which is constant over a
   run of the algorithm and `data' contains all of the current state
   at a given iteration. */

GlobalData global;
IterationData data;

/* End of global variables */


/* From here on is generic Condensation regardless of the form of
   model or observation used. All of the model-specific routines are
   found in model_specific.c */

/* This is binary search using cumulative probabilities to pick a base
   sample. The use of this routine makes Condensation O(NlogN) where N
   is the number of samples. It is probably better to pick base
   samples deterministically, since then the algorithm is O(N) and
   probably marginally more efficient, but this routine is kept here
   for conceptual simplicity and because it maps better to the
   published literature. */
int pick_base_sample(void)
{
  double choice = uniform_random() * data.largest_cumulative_prob;
  int low, middle, high;

  low = 0;
  high = global.nsamples;

  while (high>(low+1)) {
    middle = (high+low)/2;
    if (choice > data.cumul_prob_array[middle])
      low = middle;
    else high = middle;
  }

  return low;
}

/* This routine computes all of the new (unweighted) sample
   positions. For each sample, first a base is chosen, then the new
   sample position is computed by sampling from the prediction density
   p(x_t|x_t-1 = base). predict_sample_position is obviously
   model-dependent and is found in model_specific.c, but it can be
   replaced by any process model required. */
void predict_new_bases(void)
{
  int n, base;

  for (n=0; n<global.nsamples; ++n) {
    base = pick_base_sample();
    predict_sample_position(n, base);
  }
}

/* Once all the unweighted sample positions have been computed using
   predict_new_bases, this routine computes the weights by evaluating
   the observation density at each of the positions. Cumulative
   probabilities are also computed at the same time, to permit an
   efficient implementation of pick_base_sample using binary
   search. evaluate_observation_density is obviously model-dependent
   and is found in model_specific.c, but it can be replaced by any
   observation model required. */
void calculate_base_weights(void)
{
  int n;
  double cumul_total;

  cumul_total = 0.0;
  for (n=0; n<global.nsamples; ++n) {
    data.sample_weights[n] = evaluate_observation_density(n);
    data.cumul_prob_array[n] = cumul_total;
    cumul_total += data.sample_weights[n];
  }
  data.largest_cumulative_prob = cumul_total;
}

/* Go and output the estimate for this iteration (which is a
   model-dependent routine found in model_specific.c) and then swap
   over the arrays ready for the next iteration. */
void update_after_iterating(int iteration)
{
  Sample *temp;

  display_data(iteration);

  temp = data.new_positions;
  data.new_positions = data.old_positions;
  data.old_positions = temp;
}

/* obtain_observations is model-dependent and can be found in
   model_specific.c */
void run_filter(void)
{
  int i;

  for (i=0; i<global.niterations; ++i) {
    obtain_observations();     /* Go make necessary measurements */
    predict_new_bases();       /* Push previous state through process model */
    calculate_base_weights();  /* Apply Bayesian measurement weighting */
    update_after_iterating(i); /* Tidy up, display output, etc. */
  }
}

/* This routine fills in the data structures with default constant
   values. It could be enhanced by reading informatino from the
   command line to allow e.g. N to be altered without recompiling. */
void initialise_defaults(void)
{
  global.nsamples = NSamples;
  global.niterations = NIterations;

  initialise_model_specific_defaults();
}

/* Create all the arrays, then fill in the prior distribution for the
   first iteration. The prior is model-dependent, so
   set_up_prior_conditions can be found in model_specific.c */
int initialise(int argc, char *argv[])
{
  initialise_defaults();

  data.new_positions = malloc(sizeof(Sample) * global.nsamples);
  data.old_positions = malloc(sizeof(Sample) * global.nsamples);

  data.sample_weights = malloc(sizeof(double) * global.nsamples);
  data.cumul_prob_array = malloc(sizeof(double) * global.nsamples);

  if (!data.new_positions || !data.old_positions ||
      !data.sample_weights || !data.cumul_prob_array) {
    fprintf(stderr, "Failed to allocate memory for sample arrays\n");
    return 0;
  }

  set_up_prior_conditions();

  return 1;
}

/* Tidy up */
void shut_down(void)
{
  free(data.new_positions);
  free(data.old_positions);
  free(data.sample_weights);
  free(data.cumul_prob_array);
}

int main(int argc, char *argv[])
{
  if (!initialise(argc, argv))
    return 1;

  run_filter();

  shut_down();

  return 0;
}