/*-----------------------------------------------------------------------------

Copyright (C) 2012, 2013

A. Ronald Gallant
Post Office Box 659
Chapel Hill NC 27514-0659
USA   

This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License along
with this program; if not, write to the Free Software Foundation, Inc.,
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.

-----------------------------------------------------------------------------*/

#define USE_JACOBIAN
//#undef USE_JACOBIAN

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

using namespace std;
using namespace scl;


void svmod::set_parms(const realmat& theta)
{
  if(theta.size() != 4) error("Error, svmod, wrong dimension for theta");
  rho = theta[1];
  phi = theta[2];
  sigma = theta[3];
  beta = theta[4];
}

realmat svmod::get_parms() const
{
  realmat theta(4,1);
  theta[1] = rho;
  theta[2] = phi;
  theta[3] = sigma;
  theta[4] = beta;
  return theta;
}

REAL svmod::draw_x0(INT_32BIT& seed) const
{
  return (sigma/sqrt(1.0-phi*phi))*unsk(seed);
}

REAL svmod::draw_xt(REAL xlag, INT_32BIT& seed) const
{
  return phi*xlag + sigma*unsk(seed);
}

REAL svmod::prob_yt(REAL yt, REAL ytlag, REAL xt) const
{
  const REAL roottwopi = sqrt(6.283195307179587);
  REAL sd = beta*exp(xt);
  REAL z = (yt-rho*ytlag)/sd;
  return exp(-0.5*z*z)/(roottwopi*sd);
}

REAL svmod::prob_yt(INTEGER t, realmat* part)
{
  denval dv = likelihood(t, part);
  return exp(dv.log_den);
}

denval svmod::likelihood(INTEGER t, realmat* part)
{
  mf->set_particle(part);
  mf->set_sample_size(t);

  realmat theta = get_parms();

  gmmobj->set_sample_size(t);

  realmat m;
  realmat W;
  REAL logdetW;
  INTEGER rankW;
  realmat S;

  REAL obj = (*gmmobj)(theta, m, W, logdetW, rankW, S);

  INTEGER d = W.ncol();
  const REAL logoneontwopi = log(1.0/6.283195307179587);

  REAL log_likelihood = -0.5*REAL(t)*obj + 0.5*REAL(d)*logoneontwopi;

  #if defined USE_JACOBIAN
    log_likelihood += 0.5*logdetW;
  #endif

  return denval(true, log_likelihood);
}

sample svmod::draw_sample(INTEGER n, INT_32BIT& seed) const
{
  const INTEGER spin = 500;
  sample s(n);
  REAL xlag = draw_x0(seed);
  REAL ylag = 0.0;
  for (INTEGER t=1; t<=spin; ++t) {
    REAL x = draw_xt(xlag, seed);
    REAL y = rho*ylag + beta*exp(x)*unsk(seed);
    xlag = x;
    ylag = y;
  }
  s.x0 = xlag;
  s.y0 = ylag;
  for (INTEGER t=1; t<=n; ++t) {
    s.x[t] = draw_xt(xlag, seed);
    s.y[t] = rho*ylag + beta*exp(s.x[t])*unsk(seed);
    xlag = s.x[t];
    ylag = s.y[t];
  }
  return s;
}

