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

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

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

  INTEGER d = s.X.nrow();
  
  vector<realmat> particles(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(d,n+1);
  for (INTEGER i=0; i<N; ++i) {
    realmat x0 = m.draw_x0(seed);
    for (INTEGER k=1; k<=d; ++k) {
      X(k,1) = x0[k];
    }
    particles[i] = X;
  }

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

    // Importance sampling step

    realmat xlag(d,1);
    REAL sum = 0.0;
    for (INTEGER i=0; i<N; ++i) {
      for (INTEGER k=1; k<=d; ++k) {
        xlag[k] = draws[i](k,t-1);
      }
      realmat xt = m.draw_xt(xlag,seed);
      for (INTEGER k=1; k<=d; ++k) {
        draws[i](k,t) = xt[k];
      }
      sum += weights[i] = m.prob_yt(y[t],xt);
    }
  
    for (INTEGER i=1; i<N; ++i)  weights[i] += weights[i-1];
    for (INTEGER i=0; i<N; ++i)  weights[i] /= sum;
    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;
      particles[i] = draws[j];
    }

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

  realmat sdev(d,n+1,0.0);
  for (INTEGER i=0; i<N; ++i) {
    realmat z = particles[i] - mean;
    for (INTEGER t=1; t<=n+1; ++t) {
      for (INTEGER k=1; k<=d; ++k) {
        sdev(k,t) += z(k,t)*z(k,t);
      }
    }
  }
  for (INTEGER t=1; t<=n+1; ++t) {
    for (INTEGER k=1; k<=d; ++k) {
      sdev(k,t) = sqrt(sdev(k,t)/REAL(N-1));
    }
  }
  
  ofstream fout("particle.csv");
  if (!fout) error("Error, particle, cannot open fout");

  for (INTEGER k=1; k<=d; ++k) fout << "mean" << k << ',';
  for (INTEGER k=1; k<=d; ++k) fout << "sdev" << k << ',';
  for (INTEGER k=1; k<=d; ++k) fout << "x" << k << ',';
  fout << "y" << '\n';
  
  for (INTEGER k=1; k<=d; ++k) fout << mean(k,1) << ',';
  for (INTEGER k=1; k<=d; ++k) fout << sdev(k,1) << ',';
  for (INTEGER k=1; k<=d; ++k) fout << s.x0[k] << ',';
  fout << 0 << '\n';

  for (INTEGER t=2; t<=n+1; ++t) {
    for (INTEGER k=1; k<=d; ++k) fout << mean(k,t) << ',';
    for (INTEGER k=1; k<=d; ++k) fout << sdev(k,t) << ',';
    for (INTEGER k=1; k<=d; ++k) fout << s.X(k,t-1)  << ',';
    fout << s.y[t-1]<< '\n';
  }

  return 0;
}