#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> buffer1(N);
  vector<realmat> buffer2(N);

  vector<realmat>* smooth = &buffer1;
  vector<realmat>* draws = &buffer2;

  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)[i] = x;
  }

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

    vector<realmat>* save = draws;
    draws = smooth;
    smooth = save;

    // 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];
    }
  }  
    
  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;
}