#include "libscl.h"
#include "svmod.h"

using namespace std;
using namespace scl;

void svmod::set_parms(const realmat& theta)
{
  if(theta.size() != 10) error("Error, svmod, wrong dimension for theta");
  a0 = theta[1];
  b0 = theta[2];
  b11 = theta[3];
  b22 = theta[4];
  r11 = theta[5];
  r21 = theta[6];
  r22 = theta[7];
  r31 = theta[8];
  r32 = theta[9];
  r33 = theta[10];
  T(1,1) = r11;
  T(1,2) = 0.0;
  T(2,1) = r21;
  T(2,2) = r22;
  T = inv(T);
}

realmat svmod::get_parms() const
{
  realmat theta(10,1);
  theta[1] = a0;
  theta[2] = b0;
  theta[3] = b11;
  theta[4] = b22;
  theta[5] = r11;
  theta[6] = r21;
  theta[7] = r22;
  theta[8] = r31;
  theta[9] = r32;
  theta[10] = r33;
  return theta;
}

realmat svmod::draw_x0(INT_32BIT& seed) const
{
  realmat xlag(4,1,0.0);
  realmat x;
  for (INTEGER t=1; t<=50; ++t) {
    x = draw_xt(xlag, seed);
    xlag = x;
  }
  return x;
}

realmat svmod::draw_xt(const realmat& xlag, INT_32BIT& seed) const
{
  realmat x(4,1);
  REAL e1 = unsk(seed);
  REAL e2 = unsk(seed);
  x[1] = b0 + b11*xlag[1] + r11*e1;
  x[2] = b0 + b22*xlag[2] + r21*e1 + r22*e2;
  x[3] = xlag[1];
  x[4] = xlag[2];
  return x;
}

REAL svmod::prob_yt(REAL y, realmat x) const
{
  const REAL roottwopi = sqrt(6.283195307179587);
  realmat e(2,1);
  e[1] = x[1] - b0 - b11*x[3];
  e[2] = x[2] - b0 - b22*x[4];
  e = T*e;
  REAL sd = exp(x[3]+x[4])*r33;
  REAL mu = a0 + exp(x[3]+x[4])*(r31*e[1] + r32*e[2]);
  REAL z = (y-mu)/sd;
  return exp(-0.5*z*z)/(roottwopi*sd);
}

sample svmod::draw_sample(INTEGER n, INT_32BIT& seed) const
{
  sample s(n);
  s.x0 = draw_x0(seed);
  realmat xlag = s.x0;
  realmat x;
  realmat e(2,1);
  for (INTEGER t=1; t<=n; ++t) {
    x = draw_xt(xlag, seed);
    e[1] = x[1] - b0 - b11*x[3];
    e[2] = x[2] - b0 - b22*x[4];
    e = T*e;
    REAL u = r31*e[1] + r32*e[2] + r33*unsk(seed);
    REAL y = a0 + exp(x[3]+x[4])*u;
    for (INTEGER i=1; i<=4; ++i) {
      s.X(i,t) = x[i];
    }
    s.y[t] = y;
    xlag = x;
  }
  return s;
}