#include "libscl.h"
#include "svmod.h"
using namespace scl; 
using namespace std;

int main(int argc, char** argp, char** envp)
{
  INTEGER n = 100;
  INTEGER N = 25000;
  INT_32BIT seed = 780695;
  
  svmod m;

  sample s = m.draw_sample(n,seed);

  vector<realmat> smooth(N);
  vector<realmat> draws;
  REAL weights[N];

  // Initialization

  realmat y(n+1,1);
  y[1] = 0.0;
  for (INTEGER t=2; t<=n+1; t++) {
    y[t] = s.y[t-1];
  }

  realmat x(n+1,1);
  for (INTEGER i=0; i<N; ++i) {
    x[1] = m.draw_x0(seed);
    smooth[i] = x;
  }

  draws = smooth;
  
  for (INTEGER t=2; t<=n+1; ++t) {

    // Importance sampling step

    REAL sum = 0.0;
    for (INTEGER i=0; i<N; ++i) {
      draws[i][t] = m.draw_xt(draws[i][t-1],seed);
      sum += weights[i] = m.prob_yt(y[t],draws[i][t]);
    }
  
    weights[0] /= sum;
    for (INTEGER i=1; i<N; ++i) {
      weights[i] /= sum;
      weights[i] += weights[i-1];
    }
    weights[N-1] = 1.0;

    // Selection step

    for (INTEGER i=0; i<N; ++i) {
      REAL u = ran(seed);
      INTEGER j = 0;
      while(weights[j] <= u) ++j;
      smooth[i] = draws[j];
    }

    draws = smooth;
  }  
    
  realmat mean(n+1,1,0.0);
  for (INTEGER i=0; i<N; ++i) {
    mean += smooth[i];
  }
  mean = mean/N;

  realmat sdev(n+1,1,0.0);
  for (INTEGER i=0; i<N; ++i) {
    realmat z = smooth[i] - mean;
    for (INTEGER t=1; t<=n+1; ++t) sdev[t] += z[t]*z[t];
  }
  for (INTEGER t=1; t<=n+1; ++t) sdev[t] = sqrt(sdev[t]/REAL(N-1));
  
  ofstream fout("smooth.csv");
  if (!fout) error("Error, smooth, cannot open fout");
  
  fout << "mean, sdev, x, y" << '\n';
  fout << mean[1] <<','<< sdev[1] <<','<< s.x0 <<','<< 0 <<'\n';
  for (INTEGER t=2; t<=n+1; ++t) {
    fout << mean[t] <<','<< sdev[t] <<','<< s.x[t-1] <<','<< s.y[t-1] <<'\n';
  }

  return 0;
}