/*
 * Copyright (C) 1996-2011 Daniel Waggoner and Tao Zha
 *
 * This 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 3 of the License, or
 * (at your option) any later version.
 *
 * It 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.
 *
 * If you did not received a copy of the GNU General Public License
 * with this software, see <http://www.gnu.org/licenses/>.
 */

#include "dw_switch.h"
#include "dw_metropolis_theta.h"
#include "dw_array.h"
#include "dw_matrix_array.h"
#include "dw_error.h"
#include "dw_rand.h"
#include "dw_math.h"
#include "dw_std.h"

#include <math.h>
#include <string.h>
#include <stdlib.h>
#include <limits.h>


/*******************************************************************************/
/********************************** QRoutines **********************************/
/*******************************************************************************/
QRoutines* CreateQRoutines_empty(void)
{
  QRoutines* pRoutines=(QRoutines*)dw_malloc(sizeof(QRoutines));
  
  if (pRoutines)
    {
      pRoutines->ComputeTransitionMatrix=NULL;
      pRoutines->pLogPrior=NULL;
      pRoutines->Probability_s0=NULL;

      pRoutines->DrawProposalTransitionMatrixParameters=NULL;
      pRoutines->DrawTransitionMatrixParameters=NULL;
      pRoutines->pDrawTransitionMatrixParametersFromPrior=NULL;

      pRoutines->FreeInfo=NULL;
      pRoutines->DuplicateInfo=NULL;

      pRoutines->WriteSpecification=NULL;

      pRoutines->Update_q_from_BaseTransitionMatrix=NULL;
      pRoutines->DefaultTransitionMatrixParameters=NULL;
    }
  
  return pRoutines;
}

QRoutines* DuplicateQRoutines(QRoutines *routines)
{
  QRoutines* pRoutines=(QRoutines*)NULL;
  if (routines && (pRoutines=(QRoutines*)dw_malloc(sizeof(QRoutines))))
    memcpy(pRoutines,routines,sizeof(QRoutines));
  return pRoutines;
}
/*******************************************************************************/
/*******************************************************************************/
/*******************************************************************************/


/*******************************************************************************/
/******************************** ThetaRoutines ********************************/
/*******************************************************************************/
ThetaRoutines* CreateThetaRoutines_empty(void)
{
  ThetaRoutines* pRoutines=(ThetaRoutines*)dw_malloc(sizeof(ThetaRoutines));
  
  if (pRoutines)
    {
      pRoutines->pLogConditionalLikelihood=NULL;
      pRoutines->pLogPrior=NULL;
      pRoutines->pExpectationSingleStep=NULL;
      pRoutines->pExpectedError=NULL;
      pRoutines->pDestructor=NULL;
      pRoutines->pDrawParameters=NULL;
      pRoutines->pDrawParametersFromPrior=NULL;
      pRoutines->pNumberFreeParametersTheta=NULL;
      pRoutines->pConvertFreeParametersToTheta=NULL;
      pRoutines->pConvertThetaToFreeParameters=NULL;
      pRoutines->pStatesChanged=NULL;
      pRoutines->pThetaChanged=NULL;
      pRoutines->pTransitionMatrixParametersChanged=NULL;
      pRoutines->pValidTheta=NULL;
      pRoutines->pInitializeForwardRecursion=NULL;
      //pRoutines->pPermuteTheta=NULL;
      pRoutines->pModelType=NULL;
      pRoutines->pIsDegenerate=NULL;
      //pGetNormalizingPermutation=NULL;
      pRoutines->pNormalizeRegimes=NULL;
    }
  
  return pRoutines;
}
/*******************************************************************************/
/*******************************************************************************/
/*******************************************************************************/


/*******************************************************************************/
/******************************** Lag Routines *********************************/
/*******************************************************************************/
/*
   base_prob : T x nbaseststates or null pointer
   prob      : T x nstates
   sv        : valid pointer to TMarkovStateVariable structure
*/
TVector* sv_BaseProbabilities(TVector *base_prob, TVector *prob, TMarkovStateVariable *sv)
{
  int k, t, T=dw_DimA(prob);
  if (!base_prob)
    {
      base_prob=dw_CreateArray_vector(T);
      for (t=T-1; t >= 0; t--)
	InitializeVector(base_prob[t]=CreateVector(sv->nbasestates),0);
    }
  else
    for (t=T-1; t >= 0; t--)
      InitializeVector(base_prob[t],0);
  for (t=T-1; t >= 0; t--)
    for (k=sv->nstates-1; k >= 0; k--)
      ElementV(base_prob[t],sv->lag_index[k][0])+=ElementV(prob[t],k);
  return(base_prob);
}

/*
   prob      : T x nstates
   sv        : valid pointer to TMarkovStateVariable structure
*/
TVector* sv_FullBaseProbabilities(TVector *prob, TMarkovStateVariable *sv)
{
  TVector *full_base_prob, *base_prob=sv_BaseProbabilities((TVector*)NULL,prob,sv);
  while (sv->n_state_variables == 1)
    {
      sv=sv->state_variable[0];
      full_base_prob=sv_BaseProbabilities((TVector*)NULL,base_prob,sv);
      dw_FreeArray(base_prob);
      base_prob=full_base_prob;
    }
  return base_prob;
}

/*
   Assumes:
     Q  : m x m matrix where m = n^(nlags_encoded + 1) 
     bQ : n x n matrix
     nlags_encoded : positive integer
   
   Returns:
     One upon success and zero otherwise.

   Results:
     Computes the grand transition matrix Q from the base transition matrix bQ 
     given the number of lags encoded.
*/
void ConvertBaseTransitionMatrix(TMatrix Q, TMatrix bQ, int nlags_encoded)
{
  int n=RowM(bQ), a, b, c, d, e, i, j, k, m;
  PRECISION p;

  for (b=1, i=nlags_encoded-1; i > 0; i--) b*=n;
  a=n*b;
  InitializeMatrix(Q,0.0);
  for (j=0; j < n; j++)
    for (i=0; i < n; i++)
      for (p=ElementM(bQ,i,j), k=0; k < b; k++)
	for (c=b*j+k, d=i*a+c, e=n*c, m=0; m < n; m++)
	  ElementM(Q,d,e+m)=p;
}

/*
   Assumes:
     nbasestates : positive
     nlags       : non-negative
     nstates     : nbasestates^(nlags+1)

   Returns:
     Pointer to lag index upon sucess and null upon failurer.  Returned pointer
     must be freed by calling routine.

   Results:
     Allocates and fills lag index.  Note that for 0 <= k < nstates and 
     0 <= j <= nlags, lag_index[k][j] is equal to s(t-j) given that the time t 
     grand state is k.  The ordering of the lags is

                 0  ...  0   0         
                 0  ...  0   1         
                 .       .   .
                 .       .   .
                 .       .   .
                 0  ...  0  h-1        
                 0  ...  1   0         
                 0  ...  1   1         
                 .       .   .
                 .       .   .
                 .       .   .
                 0  ...  1  h-1        
                 .       .   .
                 .       .   .
                 .       .   .
                h-1 ... h-1  0         
                h-1 ... h-1  1
                 .       .   .
                 .       .   .
                 .       .   .
                h-1 ... h-1 h-1
 
*/
int** CreateLagIndex(int nbasestates, int nlags, int nstates)
{
  int **lag_index;
  int j, k;

  lag_index=dw_CreateRectangularArray_int(nstates,nlags+1);
  for (j=nlags; j >= 0; j--) lag_index[0][j]=0;
  for (k=1; k < nstates; k++)
    {
      for (j=nlags; j >= 0; j--)
	if (lag_index[k-1][j] < nbasestates-1)
	  {
	    lag_index[k][j]=lag_index[k-1][j]+1;
	    break;
	  }
	else
	  lag_index[k][j]=0;
      for (--j; j >= 0; j--) lag_index[k][j]=lag_index[k-1][j];
    }

  return lag_index;
}
/*******************************************************************************/
/*******************************************************************************/
/*******************************************************************************/


/*******************************************************************************/
/**************************** TMarkovStateVariable *****************************/
/*******************************************************************************/

/************* TMarkovStateVariable Constructors and Destructors  **************/
/*
   Assumes:
     sv: valid pointer to TMarkovStateVariable structure or null pointer

   Results:
     Frees all memory allocated to sv.
*/
void FreeMarkovStateVariable(TMarkovStateVariable *sv)
{
  int i;
  if (sv)
    {
      if (sv->nlags_encoded > 0) FreeMatrix(sv->Q);

      if (sv->n_state_variables != 1) FreeMatrix(sv->baseQ);

      for (i=sv->n_state_variables-1; i >= 0; i--) sv->QA[i]=(TMatrix)NULL;
      dw_FreeArray(sv->QA);

      dw_FreeArray(sv->index);

      dw_FreeArray(sv->lag_index);

      if (sv->routines)
	{
	  if (sv->info && sv->routines->FreeInfo) sv->routines->FreeInfo(sv->info);
	  dw_free(sv->routines);
	}

      dw_FreeArray(sv->state_variable);

      dw_free(sv);
    }
}

/*
   Assumes:
     nbasestates and nobs must be positive.  nfree and nlags must be non-
     negative.  routines must be a pointer to a valid QRoutines structure and 
     info must be null or a valid pointer to a structure compatible with 
     routines.

   Returns:
     Returns a pointer to a valid TMarkovStateVariable structure.  Upon failure,
     the program terminates.

   Notes:
     routines and info are owned by the returned TMarkovStateVariable structure,
     which will be responsible for their destruction.  If info is not null and
     the field routines->FreeInfo is not set, a memory leak will occur.
*/
TMarkovStateVariable* CreateMarkovStateVariable_Single(int nbasestates, int nfree, int nlags, QRoutines *routines, void *info)
{
  TMarkovStateVariable *sv;
  int i, terminal_errors;

  //=== Set to terminate on memory error ===
  terminal_errors=dw_SetTerminalErrors(MEM_ERR | ARG_ERR);

  if ((nbasestates <= 0) || (nfree < 0) || (nlags < 0) || (!routines)) dw_Error(ARG_ERR);

  if (!(sv=(TMarkovStateVariable*)dw_malloc(sizeof(TMarkovStateVariable)))) dw_Error(MEM_ERR);

  //=== Set flags ===
  sv->t0=0;
  sv->t1=-1;

  //=== Sizes and lags===
  sv->nlags_encoded=nlags;
  sv->nbasestates=nbasestates;
  for (sv->nstates=nbasestates, i=nlags; i > 0; i--) sv->nstates*=nbasestates;
  sv->nfree=nfree;
  sv->lag_index=CreateLagIndex(sv->nbasestates,sv->nlags_encoded,sv->nstates);

  //=== Routines and info ===
  sv->routines=routines;
  sv->info=info;

  //=== Multiple state variables ===
  sv->parent=(TMarkovStateVariable*)NULL;
  sv->n_state_variables=0;
  sv->state_variable=(TMarkovStateVariable**)NULL; 

  //=== Initialize Index ===
  sv->index=(int**)NULL;

  //=== Transition matrix ===
  sv->baseQ=CreateMatrix(sv->nbasestates,sv->nbasestates);
  sv->Q=(sv->nlags_encoded > 0) ? CreateMatrix(sv->nstates,sv->nstates) : sv->baseQ;

  //=== Initialize QA ===
  sv->QA=(TMatrix*)NULL;

  //=== Free parameters ===
  sv->q=(PRECISION*)NULL;

  //=== Reset terminal errors ===
  dw_SetTerminalErrors(terminal_errors);

  return sv;
}

/*
   state_variable is an array of valid pointers to TMarkovStateVariable 
   structures.  It must have been created via a call of the form
   
    dw_CreateArray_pointer(n_state_variables,(void (*)(void*))FreeMarkovStateVariable);  

   This array will be owned by the created TMarkovStateVariable structure and
   will be destroyed when this structure is destroyed.
*/
TMarkovStateVariable* CreateMarkovStateVariable_Multiple(int nlags, TMarkovStateVariable **state_variable)
{
  int i, k, terminal_errors;
  TMarkovStateVariable *sv;

  //=== Set to terminate only on memory error ===
  terminal_errors=dw_SetTerminalErrors(MEM_ERR | ARG_ERR);

  if ((nlags < 0) || !state_variable) dw_Error(ARG_ERR);

  for (i=0; i < dw_DimA(state_variable)-1; i++)
    if (!state_variable[i]) dw_Error(ARG_ERR);

  if (!(sv=(TMarkovStateVariable*)dw_malloc(sizeof(TMarkovStateVariable)))) dw_Error(MEM_ERR);

  //=== Set flags ===
  sv->t0=0;
  sv->t1=-1;

  //=== Sizes and lags===
  sv->nlags_encoded=nlags;
  for (sv->nbasestates=state_variable[0]->nstates, i=dw_DimA(state_variable)-1; i > 0; i--) sv->nbasestates*=state_variable[i]->nstates;
  for (sv->nstates=sv->nbasestates, i=nlags; i > 0; i--) sv->nstates*=sv->nbasestates;
  for (sv->nfree=state_variable[0]->nfree, i=dw_DimA(state_variable)-1; i > 0; i--) sv->nfree+=state_variable[i]->nfree;
  sv->lag_index=CreateLagIndex(sv->nbasestates,sv->nlags_encoded,sv->nstates);

  //=== Routines and info ===
  sv->routines=(QRoutines*)NULL;
  sv->info=(void*)NULL;

  //=== Multiple state variables ===
  sv->parent=(TMarkovStateVariable*)NULL;
  sv->n_state_variables=dw_DimA(state_variable);
  sv->state_variable=state_variable;
  for (i=dw_DimA(state_variable)-1; i >= 0; i--) state_variable[i]->parent=sv; 

  //=== Initialize Index ===
  sv->index=dw_CreateRectangularArray_int(sv->nbasestates,sv->n_state_variables);
  for (i=sv->nbasestates-1, k=sv->n_state_variables-1; k >= 0; k--)
    sv->index[i][k]=state_variable[k]->nstates-1;
  for (i--; i >= 0; i--)
    {
      memcpy(sv->index[i],sv->index[i+1],sv->n_state_variables*sizeof(int));
      for (k=sv->n_state_variables-1; k >= 0; k--)
	if (--(sv->index[i][k]) >= 0)
	  break;
	else
	  sv->index[i][k]=state_variable[k]->nstates-1;
    }

  //=== Transition matrix ===
  sv->baseQ=(sv->n_state_variables != 1) ? CreateMatrix(sv->nbasestates,sv->nbasestates) : sv->state_variable[0]->Q;
  sv->Q=(sv->nlags_encoded > 0) ? CreateMatrix(sv->nstates,sv->nstates) : sv->baseQ;

  //=== Initialize QA ===
  sv->QA=dw_CreateArray_matrix(sv->n_state_variables);
  for (k=sv->n_state_variables-1; k >= 0; k--)
    sv->QA[k]=state_variable[k]->Q;

  //=== Free parameters ===
  sv->q=(PRECISION*)NULL;

  //=== Reset terminal errors ===
  dw_SetTerminalErrors(terminal_errors);

  return sv;
}

/*
   Creates a TMarkovStateVariable with the same base structure as sv, but with a
   possibly different number of lags encoded.
*/
TMarkovStateVariable* CreateMarkovStateVariable_base(int nlags_encoded, TMarkovStateVariable *sv)
{
  TMarkovStateVariable **sv_array;
  void *dup_info;
  QRoutines *dup_routines;
  int k;
  if (sv->n_state_variables > 0)
    {
      sv_array=(TMarkovStateVariable**)dw_CreateArray_pointer(sv->n_state_variables,(void (*)(void*))FreeMarkovStateVariable); 
      for (k=sv->n_state_variables-1; k >= 0; k--)
	if (!(sv_array[k]=CreateMarkovStateVariable_base(sv->state_variable[k]->nlags_encoded,sv->state_variable[k])))
	  {
	    dw_FreeArray(sv_array);
	    return (TMarkovStateVariable*)NULL;
	  }
      return CreateMarkovStateVariable_Multiple(nlags_encoded,sv_array);
    }
  else
    if (sv->routines->DuplicateInfo  && (dup_info=sv->routines->DuplicateInfo(sv->info)))
      if (dup_routines=DuplicateQRoutines(sv->routines))
	return CreateMarkovStateVariable_Single(sv->nbasestates,sv->nfree,nlags_encoded,dup_routines,dup_info);
      else
	if (sv->routines->FreeInfo) sv->routines->FreeInfo(dup_info);
 
  return (TMarkovStateVariable*)NULL;
}

/*
   Creates a mulitple TMarkovStateVariable structure whose base structure is sv, 
   but with mulitple lags encoded.
*/
TMarkovStateVariable* CreateMarkovStateVariable_lags(int nlags_encoded, TMarkovStateVariable *sv)
{  
  TMarkovStateVariable **sv_array;
  if (sv_array=(TMarkovStateVariable**)dw_CreateArray_pointer(1,(void (*)(void*))FreeMarkovStateVariable))
    if (sv_array[0]=DuplicateMarkovStateVariable(sv))
      return CreateMarkovStateVariable_Multiple(nlags_encoded,sv_array);
  return (TMarkovStateVariable*)NULL;
}

/******************* Base TMarkovStateVariable Routines ********************/
/*
   Assumes:
     q  - a pointer to a real array of length sv->nfree
     sv - a pointer to a properly initialized TMarkovStateVariable structure

   Results:
     Recursively initializies the free parameters.
*/
void sv_SetupFreeParameters(PRECISION *q, TMarkovStateVariable *sv)
{
  int k;
  sv->q=q; 
  for (k=0; k < sv->n_state_variables; q+=sv->state_variable[k]->nfree, k++) 
    sv_SetupFreeParameters(q,sv->state_variable[k]);
} 

/*
   Set sv->t1 to minus one for all state variables in sv, which has the effect of
   making the transition matrices invalid for all t.
*/
void sv_InvalidateTransitionMatrix(TMarkovStateVariable *sv)
{
  int k;
  for (k=sv->n_state_variables-1; k >=0; k--)
    sv_InvalidateTransitionMatrix(sv->state_variable[k]);
  sv->t1=-1;
}

/*
   Assumes:
     t     : 0 <= t <= model->nobs
     sv    : pointer to valid TMarkovStateVariable structure
     model : pointer to valid TStateModel structure

   Returns:
     One upon success and zero on failure.

   Results:
     Upon success, all time t transition matrices are computed.  Sets sv->t0 and 
     sv->t1.

   Note:
     The element of the time t transition matrix in row i and col j is the 
     probability that s[t]=i given that s[t-1]=j, Y[t-1], Z[t-1], theta and q.
*/
int sv_ComputeTransitionMatrix(int t, TMarkovStateVariable *sv, TStateModel *model)
{
  int k;

  if ((sv->t0 <= t) && (t <= sv->t1)) return 1;

  if (sv->n_state_variables > 0)
    {
      sv->t0=0;
      sv->t1=INT_MAX;
      for (k=sv->n_state_variables-1; k >= 0; k--)
	if (!sv_ComputeTransitionMatrix(t,sv->state_variable[k],model))
	  {
	    sv->t1=-1;
	    return 0;
	  }
	else
	  {
	    if (sv->t0 < sv->state_variable[k]->t0) sv->t0=sv->state_variable[k]->t0;
	    if (sv->t1 > sv->state_variable[k]->t1) sv->t1=sv->state_variable[k]->t1;
	  }
      if (sv->n_state_variables > 1) MatrixTensor(sv->baseQ,sv->QA);
      if (sv->nlags_encoded > 0) ConvertBaseTransitionMatrix(sv->Q,sv->baseQ,sv->nlags_encoded);
      return 1;
    }
  else
    if (sv->routines->ComputeTransitionMatrix(t,sv,model)) return 1;

  sv->t1=-1;
  return 0;
}

/*
   Assumes:
     sv : pointer to valid TMarkovStateVariable structure

   Returns:
     The natural logarithm of the properly scaled prior evaluated at sv->q.

   Notes:
     Assumes the priors on the Markov state variables are independent.
*/
PRECISION sv_LogPrior(TMarkovStateVariable *sv)
{
  int k;
  PRECISION log_prior;

  if (sv->n_state_variables > 0)
    {
      for (log_prior=0.0, k=sv->n_state_variables-1; k >= 0; k--) 
	log_prior+=sv_LogPrior(sv->state_variable[k]);
      return log_prior;
    }
  else
    return sv->routines->pLogPrior(sv);
}

/*
   Assumes:
     p  : vector of length sv->nstates
     sv : pointer to valid TMarkovStateVariable structure.

   Returns:
     One upon success and zero upon failure.

   Results:
     The value of p[i] is the probability that s0 is equal to i.

   Notes:
     If sv->nlags_encoded > 0, then it must be the case that t0 <= 0 <= t1.
*/
int sv_Probability_s0(TVector p, TMarkovStateVariable *sv)
{
  int i, k;
  TVector q, r, *u;
  if (sv->n_state_variables > 0)
    {
      if (sv->nlags_encoded == 0)
	r=p;
      else
	if ((sv->t1 < 0) || (sv->t0 > 0))
	  return 0;
	else
	  r=CreateVector(sv->nbasestates);
      InitializeVector(r,1.0);
      for (k=sv->n_state_variables-1; k >= 0; k--)
	if (!sv_Probability_s0(q=CreateVector(sv->state_variable[k]->nstates),sv->state_variable[k]))
	  {
	    FreeVector(q);
	    return 0;
	  }
	else
	  {
	    for (i=sv->nbasestates-1; i >= 0; i--) ElementV(r,i)*=ElementV(q,sv->index[i][k]);
	    FreeVector(q);
	  }

      if (sv->nlags_encoded > 0)
	{
	  u=dw_CreateArray_vector(sv->nlags_encoded+1);
	  u[i=sv->nlags_encoded]=EquateVector((TVector)NULL,r);
	  for (i-- ; i >= 0; i--) u[i]=ProductMV((TVector)NULL,sv->baseQ,u[i+1]);
	  InitializeVector(p,1.0);
	  for (k=sv->nlags_encoded; k >= 0; k--)
	    for (i=sv->nstates-1; i >= 0; i--)
	      ElementV(p,i)*=ElementV(u[k],sv->lag_index[i][k]);
	  dw_FreeArray(u);
	  FreeVector(r);
	}

      PRECISION x=0.0;
      for (i=sv->nstates-1; i >= 0; i--) x+=ElementV(p,i);
      if (fabs(x - 1.0) > 1e-7) { printf("invalid probabilities - %lf",x); getchar(); }    
     
      return 1;
    }
  else
    return sv->routines->Probability_s0(p,sv);
}

/*
   Assumes:
     S     : integer array of length model->nobs + 1 satisfying 0 <= S[t] < sv->nstates.
     sv    : pointer to valid TMarkovStateVariable structure
     model : pointer to valid TStateModel structure

   Returns:
     The natural logrithm of  

       p(q_old | Y[T],Z[T],S[T],theta,q_new)/p(q_new | Y[T],Z[T],S[T],theta,q_old)

     where p( | ) is the proposal density, q_old is the value of sv->q upon entry 
     and q_new is the value of sv->q upon exit.

   Results:
     Draws sv->q from the propoal density p(q_new | Y[T], Z[T], S[T], theta, q_old).

   Notes:
     Sets sv->t1 to -1.
*/
PRECISION sv_DrawProposalTransitionMatrixParameters(int *S, TMarkovStateVariable *sv, TStateModel *model)
{
  int t, k, *s;
  PRECISION log_proposal_ratio=0.0;
  sv->t1=-1;
  if (sv->n_state_variables > 0)
    {
      if (!(s=(int*)dw_malloc((model->nobs+1)*sizeof(int)))) return 0;
      for (k=sv->n_state_variables-1; k >= 0; k--)
	{
          for (t=model->nobs; t >= 0; t--) 
	    s[t]=sv->state_variable[k]->lag_index[sv->index[sv->lag_index[S[t]][0]][k]][0];
	  log_proposal_ratio+=sv_DrawProposalTransitionMatrixParameters(s,sv->state_variable[k],model);
	}
      dw_free(s);
      return log_proposal_ratio;
    }
  else
    return sv->routines->DrawProposalTransitionMatrixParameters ? 
      sv->routines->DrawProposalTransitionMatrixParameters(S,sv,model) : MINUS_INFINITY;
}

/*
   Assumes:
     S     : integer array of length model->nobs + 1 satisfying 0 <= S[t] < sv->nstates.
     sv    : pointer to valid TMarkovStateVariable structure
     model : pointer to valid TStateModel structure

   Returns:
     One upon success and zero otherwise.

   Results:
     Draws sv->q from the conditional posterior density 
     p(q | Y[T], Z[T], S[T], theta).

   Notes:
     Sets sv->t1 to -1.
*/
int sv_DrawTransitionMatrixParameters(int *S, TMarkovStateVariable *sv, TStateModel *model)
{
  int t, k, *s;
  sv->t1=-1;
  if (sv->n_state_variables > 0)
    {
      if (!(s=(int*)dw_malloc((model->nobs+1)*sizeof(int)))) return 0;
      for (k=sv->n_state_variables-1; k >= 0; k--)
	{
	  for (t=model->nobs; t >= 0; t--) 
	    s[t]=sv->state_variable[k]->lag_index[sv->index[sv->lag_index[S[t]][0]][k]][0]; 
	  if (!sv_DrawTransitionMatrixParameters(s,sv->state_variable[k],model)) 
	    {
	      dw_free(s);
	      return 0;
	    }
	}
      dw_free(s);
      return 1;
    }
  else
    return sv->routines->DrawTransitionMatrixParameters ? sv->routines->DrawTransitionMatrixParameters(S,sv,model) : 0;
}

/*
   Assumes:
     sv    : pointer to valid TMarkovStateVariable structure

   Returns:
     One upon success and zero otherwise.

   Results:
     Draws sv->q from the prior density p(q).

   Notes:
     Sets sv->t1 to -1.
*/
int sv_DrawTransitionMatrixParametersFromPrior(TMarkovStateVariable *sv)
{
  int k;
  sv->t1=-1;
  if (sv->n_state_variables > 0)
    {
      for (k=sv->n_state_variables-1; k >= 0; k--)
	if (!sv_DrawTransitionMatrixParametersFromPrior(sv->state_variable[k])) return 0;
      return 1;
    }
  else
    return sv->routines->pDrawTransitionMatrixParametersFromPrior ? sv->routines->pDrawTransitionMatrixParametersFromPrior(sv) : 0;
}

/*
   Assumes:
     sv    : pointer to valid TMarkovStateVariable structure
     model : pointer to valid TStateModel structure

   Returns: 
     One upon success and zero upon failure.

   Results:
     Sets sv->q to some default value.  Sets sv->t1 to -1.
*/
int sv_DefaultTransitionMatrixParameters(TMarkovStateVariable *sv, TStateModel *model)
{
  int k;
  sv->t1=-1;
  if (sv->n_state_variables > 0)
    {
      for (k=sv->n_state_variables-1; k >= 0; k--)
	if (!sv_DefaultTransitionMatrixParameters(sv->state_variable[k],model)) return 0;
      return 1;
    }
  else
    return sv->routines->DefaultTransitionMatrixParameters ? sv->routines->DefaultTransitionMatrixParameters(sv,model) : 0;
}

/*
  Assumes:
    sv : pointer to valid TMarkovStateVariable structure

  Returns:
    One if all the required routines have been defined and zero otherwise.
*/
int sv_ValidRoutines(TMarkovStateVariable *sv)
{
  int k;
  if (sv->n_state_variables > 0)
    {
      for (k=sv->n_state_variables-1; k >= 0; k--)
	if (!sv_ValidRoutines(sv->state_variable[k])) return 0;
      return 1;
    }
  else
    return (sv->routines && sv->routines->Probability_s0 && sv->routines->ComputeTransitionMatrix && sv->routines->pLogPrior) ? 1 : 0;
}

/*
  Assumes:
    sv : pointer to valid TMarkovStateVariable structure

  Returns:
    One if all the routines needed for Metropolis-Hasting draws of q have been 
    defined and zero otherwise.
*/
int sv_ValidMetropolisHastings(TMarkovStateVariable *sv)
{
  int k;
  if (sv->n_state_variables > 0)
    {
      for (k=sv->n_state_variables-1; k >= 0; k--)
	if (!sv_ValidMetropolisHastings(sv->state_variable[k])) return 0;
      return 1;
    }
  else
    return (sv->routines && sv->routines->DrawProposalTransitionMatrixParameters) ? 1 : 0;
}

/*
  Assumes:
    sv : pointer to valid TMarkovStateVariable structure

  Returns:
    One if all the routines needed for direct draws of q have been 
    defined and zero otherwise.
*/
int sv_ValidDirectDraw(TMarkovStateVariable *sv)
{
  int k;
  if (sv->n_state_variables > 0)
    {
      for (k=sv->n_state_variables-1; k >= 0; k--)
	if (!sv_ValidDirectDraw(sv->state_variable[k])) return 0;
      return 1;
    }
  else
    return (sv->routines && sv->routines->DrawTransitionMatrixParameters) ? 1 : 0;
}

/*
   Assumes:
     p  : array of integers representing a permutation of dimension nstates.
     sv : pointer to a valid TMarkovStateVariable structure.

   Returns:
     One upon success and zero otherwise.

   Results:
     Permutes the transistion matrix parameters

   Notes:
     The permutation represented by p maps i to p[i].  Furthermore, this 
     permutation must generate valid permutations on the base states arising from
     encoded lags or independent Markov state variables.  This means
       1) sv->lag_index[i][k]=sv->lag_index[j][k] implies p[i]=p[j].  
       2) sv->index[i][k]=sv->index[j][k] implies p[i]=[j].

     Sets sv->t1 to -1.
*/
int sv_NormalizeRegimes(int *p, int t, TMarkovStateVariable *sv, TStateModel *model)
{ 
  int i, k, *q, rtrn=0;
  TPermutation P;
  sv->t1=-1;
  if (sv->n_state_variables > 0)
    if (!(q=dw_malloc(sv->nbasestates*sizeof(int))))
      dw_Error(MEM_ERR);
    else
      {
	if (sv->nbasestates < sv->nstates)
	  for (k=sv->n_state_variables-1; k >= 0; k--)
	    {
	      for (i=sv->nstates-1; i >= 0; i--) q[sv->index[sv->lag_index[i][0]][k]]=sv->index[sv->lag_index[p[i]][0]][k];
	      if (!sv_NormalizeRegimes(q,t,sv->state_variable[k],model)) break;
	    }
	else
	  for (k=sv->n_state_variables-1; k >= 0; k--)
	    {
	      for (i=sv->nstates-1; i >= 0; i--) q[sv->index[i][k]]=sv->index[p[i]][k];
	      if (!sv_NormalizeRegimes(q,t,sv->state_variable[k],model)) break;
	    }
	if (k < 0) rtrn=1;
	dw_free(q);
      }
  else
    if (sv->routines->Update_q_from_BaseTransitionMatrix)
      if (sv->nbasestates < sv->nstates)
	{
	  if (!(q=dw_malloc(sv->nbasestates*sizeof(int))))
	    dw_Error(MEM_ERR);
	  else
	    {
	      for (i=sv->nstates-1; i >= 0; i--) q[sv->lag_index[i][0]]=sv->lag_index[p[i]][0];
	      if (P=InitializePermutation((TPermutation)NULL,q,sv->nbasestates))
		{
		  if (ProductMP(sv->baseQ,TransposeProductPM(sv->baseQ,P,sv->baseQ),P))
		    rtrn=sv->routines->Update_q_from_BaseTransitionMatrix(t,sv,model);
		  FreePermutation(P);
		}
	      else
		dw_Error(MEM_ERR);
	      dw_free(q);
	    }
	}
      else
	if (P=InitializePermutation((TPermutation)NULL,p,sv->nbasestates))
	  {
	    if (ProductMP(sv->baseQ,TransposeProductPM(sv->baseQ,P,sv->baseQ),P))
	      rtrn=sv->routines->Update_q_from_BaseTransitionMatrix(t,sv,model);
	    FreePermutation(P);
	  }
	else
	  dw_Error(MEM_ERR);
  return rtrn;
}
/*******************************************************************************/
/*******************************************************************************/
/*******************************************************************************/

/*******************************************************************************/
/********************************* TStateModel *********************************/
/*******************************************************************************/

/*************************** TStateModel Destructors ***************************/
/*
   Frees TStateModel
*/
void FreeStateModel(TStateModel *model)
{
  if (model)
    {
      if (model->routines)
        {
          if (model->routines->pDestructor) model->routines->pDestructor(model->theta);
          dw_free(model->routines);
        }
      if (model->q) dw_free(model->q);
      FreeMarkovStateVariable(model->sv);
      dw_FreeArray(model->C);
      dw_FreeArray(model->V);
      dw_FreeArray(model->Z);
      FreeVector(model->L);
      dw_FreeArray(model->SP);
      Free_metropolis_theta(model->metropolis_theta);
      dw_free(model);
    }
}
/*******************************************************************************/

/************************** TStateModel Constructors ***************************/
/*
   Creates TStateModel structure.  The structures sv, routines, and theta become the
   property of the the TStateModel structure.  The structures sv and routines will 
   be destroyed by the TStateModel destructor.  If the pDestructor field of the 
   routines structure has been set, then the structure theta will be destroyed by 
   TStateModel destructor.
*/
TStateModel* CreateStateModel(int nobs, TMarkovStateVariable *sv, ThetaRoutines *routines, void *theta)
{
  int t, n;
  TStateModel *model;

  if ((nobs < 1) || !sv || !routines )
    {
      printf("CreateStateModel():  Invalid arguments\n");
      dw_exit(0);
    }

  if (!sv_ValidRoutines(sv))
    {
      printf("CreateStateModel():  Invalid TMarkovStateVariable routines\n");
      dw_exit(0);
    }

  if (!(routines->pNumberFreeParametersTheta))
    {
      printf("CreateStateModel():  routines->pNumberFreeParametersTheta() must be supplied\n");
      dw_exit(0);
    }

  // Uncomment if simulation will be used
  if (!sv_ValidMetropolisHastings(sv) && !sv_ValidDirectDraw(sv))
    {
      printf("CreateStateModel():  Simulation not available\n");
      dw_exit(0);
    }

  if (!(model=(TStateModel*)dw_malloc(sizeof(TStateModel))))
    {
      printf("CreateStateModel():  Out of memory\n");
      dw_exit(0);
    }
    
  //=== Markov state variable ===
  model->q=(PRECISION*)dw_malloc(sv->nfree*sizeof(PRECISION));
  sv_SetupFreeParameters(model->q,sv);
  model->sv=sv;
  model->use_metropolis_hasting=sv_ValidDirectDraw(sv) ? 0 : 1;
  model->metropolis_draws_q=0;
  model->metropolis_jumps_q=0;

  //=== Model specific parameters and routines ===
  model->theta=theta;
  model->routines=routines;

  //=== State vector ===
  model->fobs=1;
  model->nobs=nobs;
  dw_InitializeArray_int(model->S=dw_CreateArray_int(nobs+1),0);

  //=== Notification flags ===
  model->ForwardRecursionFailed=0;

  //=== Common work space ===
  model->t0=-1;
  model->SP_Computed=0;
  model->C=dw_CreateArray_vector(nobs+1);
  model->Z=dw_CreateArray_vector(nobs+1);
  model->V=dw_CreateArray_vector(nobs+1);
  model->SP=dw_CreateArray_vector(nobs+1);
  for (t=nobs; t >= 0; t--)
    {
      model->C[t]=CreateVector(sv->nstates);
      model->Z[t]=CreateVector(sv->nstates);
      model->V[t]=CreateVector(sv->nstates);
      model->SP[t]=CreateVector(sv->nstates);
    }
  model->L=CreateVector(nobs+1);

  //== Metropolis infomation for theta
  model->metropolis_theta=Create_metropolis_theta(NumberFreeParametersTheta(model));

  //== Simulation workspace
  model->max_log_posterior=MINUS_INFINITY;
  n=NumberFreeParametersTheta(model);
  model->max_posterior_theta=(n > 0) ? CreateVector(n) : (TVector)NULL;
  model->max_posterior_q=(sv->nfree > 0) ? CreateVector(sv->nfree) : (TVector)NULL;

  //=== Degenerate draws of states
  model->reject_degenerate_draws=0;
  model->n_degenerate_draws=0;
  model->total_degenerate_draws=0;

  //== Set control variables
  model->NormalizeStates=0;
  
  //=== Set Transition matrix to prior mean ===
  DefaultTransitionMatrixParameters(model);
 
  return model;
}
/*******************************************************************************/

/************************** TStateModel Notifications **************************/
void StatesChanged(TStateModel *model)
{
  if (model->routines->pStatesChanged) model->routines->pStatesChanged(model);
}

void TransitionMatrixParametersChanged(TStateModel *model)
{
  if (model->routines->pTransitionMatrixParametersChanged) model->routines->pTransitionMatrixParametersChanged(model);
  sv_InvalidateTransitionMatrix(model->sv);
  model->t0=-1;
  model->SP_Computed=0;
  model->ForwardRecursionFailed=0;
}

void ThetaChanged(TStateModel *model)
{
  if (model->routines->pThetaChanged) model->routines->pThetaChanged(model);
  model->t0=-1;
  model->SP_Computed=0;
  model->ForwardRecursionFailed=0;
}
/*******************************************************************************/

/**************************** TStateModel Functions ****************************/
/*
   Assumes:
     model is a pointer to a properly initialized TStateModel.

   Returns:
     One upon success or zero upon failure.

   Results:
     Computes the following:

       C[t][i] = ln(P(y[t] | Y[t-1], Z[t], s[t]=i, theta, q))  for  0 <  t <= tau  and  0 <= i < nstates
       V[t][i] = P(s[t]=i | Y[t], Z[t], theta, q)              for  0 <= t <= tau  and  0 <= i < nstates
       Z[t][i] = P(s[t]=i | Y[t-1], Z[t-1], theta, q)          for  0 <  t <= tau  and  0 <= i < nstates
       L[t]    = ln(P(Y[t] | Z[t], theta, q))                  for  0 <= t <= tau

     Sets model->t0 to tau upon success.  Upon failure, model->ForwardRecursionFailed is set.

   Notes:
     Because z is exogenous, P(z[u] | Y[u-1], Z[u-1], S[u], theta, q) = P(z[u] | Z[u-1]) and  P(Z[t] | theta, q) = P(Z[t]). So 

       P(Y[t] | Z[t], theta, q) = P(Y[t], Z[t] | theta, q)/P(Z[t] | theta, q)

           = Product(P(y[u], z[u] | Y[u-1], Z[u-1], theta, q), 0 < u <= t)/P(Z[t])

           = Product(Sum(P(y[u], z[u], s[u] | Y[u-1], Z[u-1], theta, q), 0 <= s[u] < nstates), 0 < u <= t)/P(Z[t])

           = Product(Sum(P(y[u] | Y[u-1], Z[u], s[u], theta, q)*P(z[u] | Y[u-1], Z[u-1], s[u], theta, q)
                             *P(s[u] | Y[u-1], Z[u-1], theta, q), 0 <= s[u] < nstates), 0 < u <= t)/P(Z[t])

           = Product(P(z[u] | Z[u-1])*Sum(P(y[u] | Y[u-1], Z[u], s[u], theta, q)
                                              *P(s[u] | Y[u-1], Z[u-1], theta, q), 0 <= s[u] < nstates), 0 < u <= t)/P(Z[t])

           = Product(Sum(P(y[u] | Y[u-1], Z[u], s[u], theta, q)*P(s[u] | Y[u-1], Z[u-1], theta, q), 0 <= s[u] < nstates), 0 < u <= t)

     So L[t] is computed using this formula.
*/
int ForwardRecursion(int tau, TStateModel *model)
{
  int s, t;
  PRECISION scale;
  TMarkovStateVariable *sv=model->sv;

  //====== If tau <= t0, then we are done ======//
  if (tau <= model->t0) return 1;

  //====== If ForwardRecursion() has already failed, do not try again ======//
  if (model->ForwardRecursionFailed) return 0;
  
  //====== Initializes forward recursion if t0 < 0  ======//
  if (model->t0 < 0)
    {
      if (model->routines->pInitializeForwardRecursion) 
	if (!model->routines->pInitializeForwardRecursion(model))
	  {
	    model->ForwardRecursionFailed=1;
	    return 0;
	  }
      ElementV(model->L,0)=0;
      if (!Probability_s0(model->V[0],model)) 
	{
	  model->ForwardRecursionFailed=1;
	  return 0;
	}
      model->t0=0;
    }

  //====== forward recursion ======//
  for (t=model->t0+1; t <= tau; t++)
    {
      //------ compute Q(t) if necessary ------
      if (((t < sv->t0) || (sv->t1 < t)) && !sv_ComputeTransitionMatrix(t,model->sv,model))
	{
	  model->ForwardRecursionFailed=1;
	  return 0;
	}

      //------ compute Z[t] ------
      ProductMV(model->Z[t],sv->Q,model->V[t-1]);

      //------ compute log conditional probabilities and scale ------
      scale=MINUS_INFINITY;
      for (s=sv->nstates-1; s >= 0; s--)
	{
	  if (ElementV(model->Z[t],s) > 0.0)
	    {
	      scale=AddScaledLogs(1.0,scale,ElementV(model->Z[t],s),ElementV(model->C[t],s)=LogConditionalLikelihood(s,t,model));
	      ElementV(model->V[t],s)=log(ElementV(model->Z[t],s)) + ElementV(model->C[t],s);
	    }
	  else
	    ElementV(model->V[t],s)=ElementV(model->C[t],s)=MINUS_INFINITY;
	}

      //------ update L ------
      ElementV(model->L,t)=ElementV(model->L,t-1)+scale;

      //------ scale V[t] ------
      for (s=sv->nstates-1; s >= 0; s--)
	if (ElementV(model->V[t],s) != MINUS_INFINITY)
	  ElementV(model->V[t],s)=exp(ElementV(model->V[t],s) - scale);
	else
	  ElementV(model->V[t],s)=0.0;

      //------ valid forward recursion through t ------
      model->t0=t;
    }

  return 1;
}

int ComputeTransitionMatrix(int t, TStateModel *model) 
{
  return sv_ComputeTransitionMatrix(t,model->sv,model);
}

int Probability_s0(TVector p, TStateModel *model)
{
  return ComputeTransitionMatrix(0,model) ? sv_Probability_s0(p,(model)->sv) : 0;
}

/*
   Assumes:
     model : pointer to valid TStateModel

   Returns:
     Returns one upon success and zero upon failure.

   Results:
     Fills SP for 0 <= t <= nobs and 0 <= i < nstates.

          SP[t][i] = P(s[t] = i | Y[T], Z[T], theta, q)

   Notes:
     Calls ForwardRecursion() if necessary.  The time parameter t is base one.
     The computations use the following facts:

      P(s[t]=k | Y[T], Z[T], theta, q) 
              = Sum(P(s[t]=k, s[t+1]=i | Y[T], Z[T], theta, q), 0 <= i < nstates)

      P(s[t]=k, s[t+1]=i | Y[T], Z[T], theta, q) 
                    = P(s[t]=k | Y[T], Z[T], theta, q, s[t+1]=i)  
                                             * P(s[t+1]=i | Y[T], Z[T], theta, q)
     
      P(s[t]=k | Y[T], Z[T], theta, q, s[t+1]=i)
                                     = P(s[t]=k | Y[t], Z[t], theta, q, s[t+1]=i) 
                                     
      P(s[t]=k | Y[t], Z[t], theta, q, s[t+1]=i) = Q[i][k] 
          * P(s[t]=k | Y[t], Z[t], theta, q) / P(s[t+1]=i | Y[t], Z[t], theta, q)

     The third of these facts follows from

       P(s[t] | Y[t], Z[t], theta, t, s[t+1]) 
                                       = P(s[t] | Y[T], Z[T], theta, t, S(t+1,T))

     where S(t+1,T)={s[t+1],...,s[T]}.  This is proven in the related paper.

*/
int ComputeSmoothedProbabilities(TStateModel *model)
{
  int nstates=model->sv->nstates, i, k, t;

  //====== Smoothed probabilities have already been computed ======
  if (model->SP_Computed) return 1;

  //====== Check for valid forward recursion through time t ======
  if ((model->t0 < model->nobs) && !ForwardRecursion(model->nobs,model)) return 0;

  //====== Compute SP via backward recursion ======
  EquateVector(model->SP[model->nobs],model->V[model->nobs]);
  for (t=model->nobs-1; t >= 0; t--)
    {
      //------ compute Q(t) if necessary ------
      if (((t < model->sv->t0) || (model->sv->t1 < t)) && !sv_ComputeTransitionMatrix(t+1,model->sv,model)) return 0;

      // s[t] = k and s[t+1] = i
      for (k=nstates-1; k >= 0; k--)
	{
	  for (ElementV(model->SP[t],k)=0.0, i=nstates-1; i >= 0; i--)
	    if (ElementV(model->Z[t+1],i) > 0.0)
	      ElementV(model->SP[t],k)+=ElementV(model->SP[t+1],i)*ElementM(model->sv->Q,i,k)/ElementV(model->Z[t+1],i);
	  ElementV(model->SP[t],k)*=ElementV(model->V[t],k);
	}
    }

  model->SP_Computed=1;
  return 1;
}

/*
   Computes
       
       p[t][s] = P(s[t] = s | Y[tau], Z[tau], theta, q)
*/
TVector* RealTimeSmoothedProbabilities(TVector *p, int tau, TStateModel *model)
{
  int nstates=model->sv->nstates, i, k, t;

  if (!p)
    {
      p=dw_CreateArray_vector(tau+1);
      for (t=tau; t >= 0; t--)
	p[t]=CreateVector(model->sv->nstates);
    }
  else
    if (dw_DimA(p) <= tau)
      {
	dw_Error(SIZE_ERR);
	return (TVector*)NULL;
      }

  //====== Check for valid forward recursion through time tau ======
  if ((model->t0 < tau) && !ForwardRecursion(tau,model)) return 0;

  //====== Compute smoothed probabilities via backward recursion ======
  EquateVector(p[tau],model->V[tau]);
  for (t=tau-1; t >= 0; t--)
    {
      //------ compute Q(t) if necessary ------
      if (((t < model->sv->t0) || (model->sv->t1 < t)) && !sv_ComputeTransitionMatrix(t+1,model->sv,model)) return 0;

      // s[t] = k and s[t+1] = i
      for (k=nstates-1; k >= 0; k--)
	{
	  for (ElementV(p[t],k)=0.0, i=nstates-1; i >= 0; i--)
	    if (ElementV(model->Z[t+1],i) > 0.0)
	      ElementV(p[t],k)+=ElementV(p[t+1],i)*ElementM(model->sv->Q,i,k)/ElementV(model->Z[t+1],i);
	  ElementV(p[t],k)*=ElementV(model->V[t],k);
	}
    }

  return p;
}

/*
   Assumes:
     model is a pointer to a properly initialized TStateModel.

   Returns:
     1 upon success and 0 otherwise.

   Results:
     Draws the discrete Markov state variable from the posterior distribution, 
     conditional on q and theta.  The state variable values are stored in 
     model->S.

   Notes:
     Calls ForwardRecursion() if necessary.
*/
int DrawStates(TStateModel *model)
{
  int i, j, t, k=0, rtrn=1;
  PRECISION scale, u, s;
  TMarkovStateVariable *sv=model->sv;
  int *regime_counts=(int*)dw_malloc(sv->nstates*sizeof(int));

  //====== Check for valid forward recursion through time model->nobs  ======
  if ((model->t0 < model->nobs) && !ForwardRecursion(model->nobs,model)) return 0;

  //====== Backward recursion ======
 BACKWARD_RECURSION:
  for (i=sv->nstates-1; i >=0; i--) regime_counts[i]=0;

  if ((u=dw_uniform_rnd()) >= (s=ElementV(model->V[t=model->nobs],i=sv->nstates-1)))
    while (--i > 0)
      if (u < (s+=ElementV(model->V[t],i))) break;

  //------ track count of states ------
  regime_counts[model->S[t]=i]++;

  for (t--; t >= 0; t--)
    {
      //------ compute Q(t+1) if necessary ------
      if (((t < sv->t0) || (sv->t1 < t)) && !sv_ComputeTransitionMatrix(t+1,model->sv,model)) return 0;

      scale=1.0/ElementV(model->Z[t+1],j=i);
      i=sv->nstates-1;
      if ((u=dw_uniform_rnd()) >= (s=ElementV(model->V[t],i)*ElementM(sv->Q,j,i)*scale))
	while (--i > 0)
	  if (u < (s+=ElementV(model->V[t],i)*ElementM(sv->Q,j,i)*scale)) break;

      //------ track count of states ------
      regime_counts[model->S[t]=i]++;
    }

  //------ degenerate states ------
  if (IsDegenerate(regime_counts,model) && (++k < model->reject_degenerate_draws)) goto BACKWARD_RECURSION;

  if (k > 0)
    {
      model->n_degenerate_draws++;
      model->total_degenerate_draws+=k;
      if (model->reject_degenerate_draws && (k == model->reject_degenerate_draws))
	{
	  rtrn=0;
	  printf("DrawStates(): Reached max iteration count (%d)\n",k);
	}
    }
    
  //====== State change notification ======
  StatesChanged(model);

  //====== Free memory ======
  dw_free(regime_counts);

  return rtrn;
}


/*
   Assumes:
     model is a pointer to a properly initialized TStateModel structure.

   Returns:
     1 upon success and 0 otherwise

   Results:
     Draws the vector of states S from the distribution defined by the transition
     matrices.  The transition matrices depend on model->q and the data.
*/
int DrawStatesFromTransitionMatrices(TStateModel *model)
{   
  int rtrn=0, s, t;
  TMarkovStateVariable *sv=model->sv;
  TVector p;

  if (p=CreateVector(sv->nstates))
    {
      if (sv->routines->Probability_s0(p,sv))
	{
	  model->S[0]=s=DrawDiscrete(p);
	  if (sv->routines->ComputeTransitionMatrix)
	    {
	      rtrn=1;
	      for (t=1; t <= model->nobs; t++)
		if (!sv->routines->ComputeTransitionMatrix(t,sv,model))
		  {
		    rtrn=0;
		    break;
		  }
		else
		  {
		    ColumnVector(p,sv->Q,s);
		    model->S[t]=s=DrawDiscrete(p);
		  }
	    }
	}
      FreeVector(p);
    }

  //===== State change notification ======
  StatesChanged(model); 

  return rtrn;
}

void ResetMetropolisHastingsCounts_q(TStateModel *model)
{
  model->metropolis_jumps_q=model->metropolis_draws_q=0;
}

/*
   Assumes:
     model:  pointer to a properly initialized TStateModel structure

   Returns:
     One upon success and zero upon failure.  Failure indicates a problem in 
     obtaining the proposal draw.

   Results:
     Draws the transition matrix from the posterior distribution, conditional on 
     the states and theta.
*/
int DrawTransitionMatrixParameters_MetropolisHastings(TStateModel *model)
{
  PRECISION log_proposal_ratio, log_posterior_old, log_posterior_new;
  PRECISION *q;

  //== Save old transition matrix parameters
  q=(PRECISION*)dw_malloc(model->sv->nfree*sizeof(PRECISION));
  memcpy(q,model->q,model->sv->nfree*sizeof(PRECISION));

  //== Compute P(q | Y, Z, S, theta), which is proportional to the posterior.
  log_posterior_old=LogPosterior(model);

  //== Draw proposal and call TransitionMatrixParametersChanged()
  log_proposal_ratio=sv_DrawProposalTransitionMatrixParameters(model->S,model->sv,model);
  TransitionMatrixParametersChanged(model);

  //== Compute new posteriorkernel
  log_posterior_new=LogPosterior(model);

  //== Reject draw?
  if (log(dw_uniform_rnd()) > log_posterior_new - log_posterior_old + log_proposal_ratio)
    {
      memcpy(model->q,q,model->sv->nfree*sizeof(PRECISION));
      TransitionMatrixParametersChanged(model);
    }
  else
    model->metropolis_jumps_q++;

  //== Increment number of metropolis draws
  model->metropolis_draws_q++;

  // Clean up
  dw_free(q);

  return 1;
}

int DrawTransitionMatrixParameters(TStateModel *model)
{
  if (model->use_metropolis_hasting)
    return DrawTransitionMatrixParameters_MetropolisHastings(model);
  else
    return sv_DrawTransitionMatrixParameters(model->S,model->sv,model);
}

/*
   Assumes:
    model:  pointer to a properly initialized TStateModel structure

   Results:
    Draws the transition matrix sv->Q from the prior distribution.
*/
int DrawTransitionMatrixParametersFromPrior(TStateModel *model)
{
  int rtrn; 

  //====== Draw transition matrix parameters using recursive call ======
  rtrn=sv_DrawTransitionMatrixParametersFromPrior(model->sv);
  
  //====== Transition matrix change notification ======
  TransitionMatrixParametersChanged(model); 

  return rtrn;
}

/*
   Assumes:
    model:  pointer to a properly initialized TStateModel structure

   Results:
    Draws the transition matrix sv->Q from the prior distribution.
*/
int DefaultTransitionMatrixParameters(TStateModel *model)
{
  int rtrn; 

  //====== Set default transition matrix parameters using recursive call ======
  rtrn=sv_DefaultTransitionMatrixParameters(model->sv,model);
  
  //====== Transition matrix change notification ======
  TransitionMatrixParametersChanged(model); 

  return rtrn;
}

void DrawTheta(TStateModel *model)
{
  //====== Draw theta ======
  if (model->routines->pDrawParameters)
    {
      model->routines->pDrawParameters(model);
      ThetaChanged(model);
    }
  else
    Draw_metropolis_theta(model);
}

void DrawAll(TStateModel *model)
{
  PRECISION tmp;

  if (!DrawStates(model)) dw_UserError("DrawAll(): Error drawing path of regimes");
  DrawTransitionMatrixParameters(model);
  DrawTheta(model);

  if ((tmp=LogPosterior_StatesIntegratedOut(model)) > model->max_log_posterior)
    {
      model->max_log_posterior=tmp;
      if (model->max_posterior_theta) ConvertThetaToFreeParameters(model,pElementV(model->max_posterior_theta));
      if (model->max_posterior_q) ConvertQToFreeParameters(model,pElementV(model->max_posterior_q));
    }
}
/*******************************************************************************/
/*******************************************************************************/
/*******************************************************************************/


/*******************************************************************************/
/**************************** Regime Normalization *****************************/
/*******************************************************************************/
/*
   The integer pointer permutation is the normalizing permutation or a null 
   pointer.  Returns one if the parameters are not normalized and zero otherwise.

   The normalized parameters in regime k are equal to the un-normalized 
   parameters in regime permutation[k]  
*/
int AreRegimesNormalized(TPermutation p, TStateModel *model)
{
  int rtrn;
  if (!p) 
    {
      p=CreatePermutation(model->sv->nstates*sizeof(int));
      rtrn=ComputeNormalizingPermutation(p,model);
      FreePermutation(p);
      return rtrn;
    }
  else
    if (UseP(p) > 0) return 0;
  return 1;
}

int NormalizeTransitionMatrices(int *p, TStateModel *model, PRECISION *buffer)
{
  PRECISION *f=(PRECISION*)NULL;
  int rtrn=0;
  if (buffer)
    {
      f=(PRECISION*)dw_malloc(NumberFreeParametersQ(model)*sizeof(PRECISION));
      ConvertQToFreeParameters(model,f);
    }
  if (sv_ComputeTransitionMatrix(0,model->sv,model))
    rtrn=sv_NormalizeRegimes(p,0,model->sv,model);
  if (buffer)
    {
      ConvertQToFreeParameters(model,buffer);
      ConvertFreeParametersToQ(model,f);
      dw_free(f);
    }
  return rtrn;
}

/*
   Normalizes the parameters.  The integer pointer
*/
int NormalizeRegimes(int *p, TStateModel *model, PRECISION *buffer)
{
  int rtrn=0, *q=p;
  if (model->routines->pNormalizeRegimes)
    {
      if (q || (rtrn=ComputeNormalizingPermutation(q=dw_malloc(model->sv->nstates*sizeof(int)),model)))
	if (model->routines->pNormalizeRegimes(q,model,buffer) && NormalizeTransitionMatrices(q,model,buffer))
	  rtrn=1;
      if (p != q) dw_free(q);
    }
  return rtrn;
}
/*******************************************************************************/
/*******************************************************************************/
/*******************************************************************************/


/*******************************************************************************/
/*********************** Impulse responses and forecasts ***********************/
/*******************************************************************************/
/*
*/
TMatrix ImpluseResponse(TMatrix IR, int i, int T, int horizon, TStateModel *model)
{
  TVector y, *Y1, *Y2, p;
  int s, t;

  if (!model->routines->pEyt || !model->routines->pEyt_i || (T > model->nobs)) return (TMatrix)NULL;

  if (model->t0 < T) ForwardRecursion(T,model);
  p=EquateVector((TVector)NULL,model->V[T]);
  ComputeTransitionMatrix(T,model);
  ProductMV(p,model->sv->Q,p);

  Y1=dw_CreateArray_vector(horizon);
  Y2=dw_CreateArray_vector(horizon);

  Y1[0]=ProductVS((TVector)NULL,y=model->routines->pEyt_i((TVector)NULL,T,(TVector*)NULL,i,0,T+1,model),ElementV(p,0));
  Y2[0]=ProductVS((TVector)NULL,model->routines->pEyt(y,T,(TVector*)NULL,0,T+1,model),ElementV(p,0));
  for (s=model->sv->nstates-1; s > 0; s--)
    {
      UpdateV(1.0,Y1[0],ElementV(p,s),model->routines->pEyt_i(y,T,(TVector*)NULL,i,s,T+1,model));
      UpdateV(1.0,Y2[0],ElementV(p,s),model->routines->pEyt(y,T,(TVector*)NULL,s,T+1,model));
    }

  if (!IR)
    IR=CreateMatrix(horizon,DimV(y));
  else
    if ((RowM(IR) < horizon) || (ColM(IR) != DimV(y))) goto EXIT;

  SubtractVV(y,Y1[0],Y2[0]);
  InsertRowVector(IR,y,0);

  for (t=1; t < horizon; t++)
    {
      ProductMV(p,model->sv->Q,p);
      Y1[t]=ProductVS((TVector)NULL,model->routines->pEyt(y,T,Y1,s,T+1+t,model),ElementV(p,0));
      Y2[t]=ProductVS((TVector)NULL,model->routines->pEyt(y,T,Y2,s,T+1+t,model),ElementV(p,0));
      for (s=model->sv->nstates-1; s > 0; s--)
	{
	  UpdateV(1.0,Y1[t],ElementV(p,s),model->routines->pEyt(y,T,Y1,s,T+1+t,model));
	  UpdateV(1.0,Y2[t],ElementV(p,s),model->routines->pEyt(y,T,Y2,s,T+1+t,model));
	}
      SubtractVV(y,Y1[t],Y2[t]);
      InsertRowVector(IR,y,t);
    }

 EXIT:
  dw_FreeArray(Y2);
  FreeVector(y);
  dw_FreeArray(Y1);
  FreeVector(p);

  return IR;
 }

TMatrix ForecastMean(TMatrix F, int T, int horizon, TStateModel *model)
{
  TVector y, *Y, p;
  int s, t;

  if (!model->routines->pEyt || (T > model->nobs)) return (TMatrix)NULL;

  if (model->t0 < T) ForwardRecursion(T,model);
  p=EquateVector((TVector)NULL,model->V[T]);
  ComputeTransitionMatrix(T,model);
  ProductMV(p,model->sv->Q,p);

  Y=dw_CreateArray_vector(horizon);

  Y[0]=ProductVS((TVector)NULL,y=model->routines->pEyt((TVector)NULL,T,(TVector*)NULL,0,T+1,model),ElementV(p,0));
  for (s=model->sv->nstates-1; s > 0; s--)
    UpdateV(1.0,Y[0],ElementV(p,s),model->routines->pEyt(y,T,(TVector*)NULL,s,T+1,model));

  if (!F)
    F=CreateMatrix(horizon,DimV(y));
  else
    if ((RowM(F) < horizon) || (ColM(F) != DimV(y))) goto EXIT;

  InsertRowVector(F,Y[0],0);

  for (t=1; t < horizon; t++)
    {
      ProductMV(p,model->sv->Q,p);
      Y[t]=ProductVS((TVector)NULL,model->routines->pEyt(y,T,Y,s,T+1+t,model),ElementV(p,0));
      for (s=model->sv->nstates-1; s > 0; s--)
	UpdateV(1.0,Y[t],ElementV(p,s),model->routines->pEyt(y,T,Y,s,T+1+t,model));
      InsertRowVector(F,Y[t],t);
    }

 EXIT:
  FreeVector(y);
  dw_FreeArray(Y);
  FreeVector(p);

  return F;
}

TMatrix ForecastRandom(TMatrix F, int T, int horizon, TStateModel *model)
{
  return (TMatrix)NULL;
}
/*******************************************************************************/
/*******************************************************************************/
/*******************************************************************************/


/*******************************************************************************/
/******************************* Normalization  ********************************/
/*******************************************************************************/
/* int SetStateNormalizationMethod(int (*pGetNormalization)(int*, struct TStateModel_tag*),int (*pPermuteTheta)(int*, struct TStateModel_tag*),TStateModel *model) */
/* { */
/*   if (pGetNormalization && pPermuteTheta) */
/*     { */
/*       model->NormalizeStates=1; */
/*       model->routines->pGetNormalization=pGetNormalization; */
/*       model->routines->pPermuteTheta=pPermuteTheta; */
/*     } */
/*   else */
/*     { */
/*       model->NormalizeStates=0; */
/*       model->routines->pGetNormalization=NULL; */
/*       model->routines->pPermuteTheta=NULL; */
/*     } */
/*   return 1; */
/* } */

/*
   Returns 1 if states successfully normalized and zero otherwise.
*/
/* int NormalizeStates(TStateModel *model) */
/* { */
/*   int *p, rtrn=0; */

/*   if (!(model->NormalizeStates)) return 1; */

/*   if (p=(int*)dw_malloc(model->sv->nstates)) */
/*     { */
/*       if (model->routines->pGetNormalization(p,model)) */
/* 	if (Permute_SV(p,model->sv)) */
/* 	  if (model->routiens->pPermuteTheta(p,model)) */
/* 	    rtrn=1; */
/*       dw_free(p); */
/*     } */

/*   return rtrn; */
/*   return 1; */
/* } */


/*******************************************************************************/
/**************************** Compute Probabilities ****************************/
/*******************************************************************************/
/*
   Assumes:
     t : 1 <= t <= nobs
     model : pointer to valid TStateModel

   Returns:
     Returns the natural logrithm of 

                        P(y[t] | Y[t-1], Z[t], theta, q).

     If forward recursion cannot be completed through time t, returns minus 
     infinity.  Note that minus infinity is also a valid return value.

   Notes:
     Calls ForwardRecursion() if necessary.  The time parameter t is base one.  
     Computation is based on the following:

      P(y[t] | Y[t-1], Z[t], theta, q) =
         Sum(P(s[t]=s | Y[t-1], Z[t], theta, q) 
                * P(y[t] | Y[t-1], Z[t], s[t], theta, q), 0 <= s[t] < nstates)

     and

      P(s[t]=s | Y[t-1], Z[t], theta, q) = P(s[t]=s | Y[t-1], Z[t-1], theta, q)
*/
PRECISION LogConditionalLikelihood_StatesIntegratedOut(int t, TStateModel *model)
{
  int s;
  PRECISION u, x;

  //====== Check for valid forward recursion through time t  ======
  if ((model->t0 < t) && !ForwardRecursion(t,model)) return MINUS_INFINITY;

  if ((u=ElementV(model->Z[t],0)) > 0.0)
    x=ElementV(model->C[t],0) + log(u);
  else
    x=MINUS_INFINITY;

  for (s=model->sv->nstates-1; s > 0; s--)
    if ((u=ElementV(model->Z[t],s)) > 0.0)
      x=AddLogs(x,ElementV(model->C[t],s) + log(u));

  return x;
}

/*
   Assumes:
     t : 1 <= t <= nobs
     model : pointer to valid TStateModel

   Returns:
     Returns 

                        E[y[t] | Y[t-1], Z[t], theta, Q]

     If forward recursion cannot be completed through time t, returns minus 
     infinity.  Note that minus infinity is also a valid return value.

   Notes:
     Calls ForwardRecursion() if necessary.  The time parameter t is base one.  
     Computation is based on the following:

      P(y[t] | Y[t-1], Z[t], theta, Q) =
         Sum(P(s[t]=s | Y[t-1], Z[t], theta, Q) 
                * P(y[t] | Y[t-1], Z[t], theta, Q, s), 0 <= s < nstates)

     and

      P(s[t]=s | Y[t-1], Z[t], theta, Q) = P(s[t]=s | Y[t-1], Z[t-1], theta, Q)
*/
TVector ExpectationSingleStep_StatesIntegratedOut(TVector y, int t, TStateModel *model)
{
  int s;
  TVector y_tmp;

  //====== Check for valid forward recursion through time t  ======
  if ((model->t0 < t) && !ForwardRecursion(t,model)) return (TVector)NULL;

  y=ExpectationSingleStep(y,0,t,model);
  ProductVS(y,y,ElementV(model->Z[t],0));

  y_tmp=CreateVector(DimV(y));
  for (s=model->sv->nstates-1; s > 0; s--)
    {
      ExpectationSingleStep(y_tmp,s,t,model);
      ProductVS(y_tmp,y_tmp,ElementV(model->Z[t],s));
      AddVV(y,y,y_tmp);
    }
  FreeVector (y_tmp);

  return y;
}

TVector ExpectedError(TVector epsilon, int t, TStateModel *model)
{


  return epsilon;
}

/*
   Assumes:
     s : 0 <= s < nstates
     t : 0 <= t <= nobs
     model : pointer to valid TStateModel

   Returns:
     Upon success, returns

            P(s[t] = s | Y[t], Z[t], theta, q) 

     and -1 otherwise.

   Notes:
     Calls ForwardRecursion() if necessary.  The time parameter t is base one.  
*/
PRECISION ProbabilityStateConditionalCurrent(int s, int t, TStateModel *model)
{
  //====== Check for valid forward recursion through time t  ======
  if ((model->t0 < t) && !ForwardRecursion(t,model)) return -1.0;

  return ElementV(model->V[t],s);
}

/*
   Assumes:
     s : 0 <= s < nbasestates
     t : 0 <= t <= nobs
     model : pointer to valid TStateModel

   Returns:
     Upon success, returns the probability that the base state is s given Y[t],
     Z[t], theta, and q.  Upon failure returns -1.  Note that this probability
     is the sum over all k such that lag_index[k][0]=s of

                     P(s[t] = k | Y[t], Z[t], theta, q)

   Notes:
     Calls ForwardRecursion() if necessary.  The time parameter t is base one.  
*/
PRECISION ProbabilityBaseStateConditionalCurrent(int s, int t, TStateModel *model)
{
  PRECISION p;
  int i, m;
  //====== Check for valid forward recursion through time t  ======
  if ((model->t0 < t) && !ForwardRecursion(t,model)) return -1.0;

  if (model->sv->nlags_encoded == 0)
    return ElementV(model->V[t],s);
  else
    {
      i=(int)pow(model->sv->nbasestates,model->sv->nlags_encoded);
      for (p=0.0, m=(s+1)*i, i*=s; i < m; i++)
	p+=ElementV(model->V[t],i);
      return p; 
    }
}

/*
   Assumes:
     s : 0 <= s < nstates
     t : 1 <= t <= nobs
     model : pointer to valid TStateModel

   Returns:
     Upon success, returns

          P(s[t] | Y[t-1], Z[t-1], theta, q)

     and -1 otherwize.

   Notes:
     Calls ForwardRecursion() if necessary.  The time parameter t is base one.
*/
PRECISION ProbabilityStateConditionalPrevious(int s, int t, TStateModel *model)
{
  //====== Check for valid forward recursion through time t  ======
  if ((model->t0 < t) && !ForwardRecursion(t,model)) return -1.0;

  return ElementV(model->Z[t],s);
}

/*
   Assumes:
     p : vector of length model->sv->nobs+1 or null pointer
     s : 0 <= s < nstates
     model : pointer to valid TStateModel

   Returns:
     Upon success, the vector P is returned and upon failure a null pointer is 
     returned. If P is initially the null pointer, P is created.

   Results:

          P[t] = P(s[t] = s | Y[T], Z[T], theta, Q)

     for 0 <= t <= T.

   Notes:
     Calls ForwardRecursion() if necessary.  The time parameter t is base one.
     The computations use the following facts:

      P(s[t]=k | Y[T], Z[T], theta, Q) 
              = Sum(P(s[t]=k, s[t+1]=i | Y[T], Z[T], theta, Q), 0 <= i < nstates)

      P(s[t]=k, s[t+1]=i | Y[T], Z[T], theta, Q) 
                    = P(s[t]=k | Y[T], Z[T], theta, Q, s[t+1]=i)  
                                             * P(s[t+1]=i | Y[T], Z[T], theta, Q)
     
      P(s[t]=k | Y[t], Z[t], theta, Q, s[t+1]=i) 
                                     = P(s[t]=k | Y[T], Z[T], theta, Q, s[t+1]=i)

      P(s[t]=k | Y[t], Z[t], theta, Q, s[t+1]=i) = Q[i][k] 
          * P(s[t]=k | Y[t], Z[t], theta, Q) / P(s[t+1]=i | Y[t], Z[t], theta, Q)

     The third of these facts follows from

       P(s[t] | Y[t], Z[t], theta, Q, s[t+1]) 
                                       = P(s[t] | Y[T], Z[T], theta, Q, S(t+1,T))

     where S(t+1,T)={s[t+1],...,s[T]}.  This is proven in the related paper.

*/
TVector ProbabilitiesState(TVector p, int s, TStateModel *model)
{
  int t;

  if (!model->SP_Computed && !ComputeSmoothedProbabilities(model)) return (TVector)NULL;

  if (!p)
    { if (!(p=CreateVector(model->nobs+1))) return (TVector)NULL; }
  else
    if (DimV(p) != model->nobs+1) 
      {
	dw_Error(SIZE_ERR);
	return (TVector)NULL;
      }

  for (t=model->nobs; t >= 0; t--) ElementV(p,t)=ElementV(model->SP[t],s);

  return p;
}

/*
   Assumes:
     S     : integer array of length model->sv->nobs+1 
     model : pointer to valid TStateModel

   Returns:
     If the transition matrix is valid, returns the natural logrithm of
     P(S[T] | Y[T], Z[T], theta, Q).  If the transition matrix is invalid, return
     minus infinity.  Note that minus infinity is also a valid return value.

   Notes:
     Calls ForwardRecursion() if necessary.  The time parameter t is base one.
     The computations use the following facts:

      P(S[T] | Y[T], Z[T], theta, Q) 
                  = Product(P(s[t] | Y[T], Z[T], theta, Q, S(t+1,T)),1 <= t <= T)
     
      P(s[t] | Y[t], Z[t], theta, Q, s[t+1]) 
                                       = P(s[t] | Y[T], Z[T], theta, Q, S(t+1,T))

      P(s[t] | Y[t], Z[t], theta, Q, s[t+1]) = Q[s(t+1)][s(t)] 
              * P(s[t] | Y[t], Z[t], theta, Q) / P(s[t+1] | Y[t], Z[t], theta, Q)

     where S(t+1,T)={s[t+1],...,s[T]}.  The second of these facts is proven in
     the related paper.
*/
PRECISION LogProbabilityStates(int *S, TStateModel *model)
{
  printf("LogProbabilityStates() not implemented\n");
  dw_exit(0);

/**
  PRECISION rtrn;
  TMarkovStateVariable *sv=model->sv;
  TMatrix Q=sv->Q;
  int t=model->nobs;

  //====== Check for valid forward recursion through time t  ======
  if ((model->t0 < model->nobs) && !ForwardRecursion(model->nobs,model)) return MINUS_INFINITY;

  //====== Log probability ======
  rtrn=log(ElementV(model->V[t],S[t]));
  while (--t >= 0)
    if (ElementV(model->Z[t+1],S[t+1]) > 0.0)
      rtrn+=log(ElementM(Q,S[t+1],S[t])*ElementV(model->V[t],S[t])/ElementV(model->Z[t+1],S[t+1]));
    else
      return MINUS_INFINITY;

  return rtrn;

  //====== Probability ======
*   rtrn=ElementV(model->V[t],S[t]); */
/*   while (--t >= 0) */
/*     rtrn*=ElementM(Q,S[t+1],S[t])*ElementV(model->V[t],S[t])/ElementV(model->Z[t+1],S[t+1]); */
}

/*
   Assumes
     model:  pointer to valid TStateModel structure

   Returns
     The natural logrithm of 

                   P(S[T] | Theta, Q)
*/
PRECISION LogPrior_S(TStateModel *model)
{
  //int t, *S=model->S;
  //TMatrix Q=model->sv->Q;
  //PRECISION p=-log(model->sv->nstates);  // This assumes that P(s0 = i) = P(s0 = j) for all i and j.
  //for (t=model->nobs; t > 0; t--)
  //  p+=log(ElementM(Q,S[t],S[t-1]));
  //return p;

  PRECISION p;
  int t, *S=model->S;
  if ((model->t0 < 0) && !Probability_s0(model->V[0],model)) return MINUS_INFINITY;
  p=log(ElementV(model->V[0],S[0]));
  for (t=1; t <= model->nobs; t++)
    {
      if (!sv_ComputeTransitionMatrix(t,model->sv,model)) return MINUS_INFINITY;
      p+=log(ElementM(model->sv->Q,S[t],S[t-1]));
    } 
  return p;
}

/*
   Assumes:
     model : pointer to valid TStateModel structure

   Returns:
     The natural logrithm of

        P(Y[T] | Z[T], S[T], theta, q)

   Notes:
     The data is assumed to be the time t observations for fobs <= t <= nobs. 

*/
PRECISION LogLikelihood(TStateModel *model)
{
  int t, t0=model->t0, *S=model->S;
  PRECISION loglikelihood=0.0;
  for (t=model->fobs; t <= t0; t++) 
    loglikelihood+=(ElementV(model->Z[t],S[t]) > 0) ? ElementV(model->C[t],S[t]) : LogConditionalLikelihood(S[t],t,model);
  for ( ; t <= model->nobs; t++)
    loglikelihood+=LogConditionalLikelihood(S[t],t,model);
  return loglikelihood;
}

/*
   Assumes:
     model : pointer to valid TStateModel structure

   Returns:
     Upon sucess, returns the natural logrithm of

                        P(Y[T] | Z[t], theta, Q)

     and minus infinity otherwise.  Note that minus infinity is also a valid 
     return value.

   Notes:
     Calls ForwardRecursion() if necessary.  The time parameter t is base one.  
     The data is assumed to be the time t observations for fobs <= t <= nobs. 
*/
PRECISION LogLikelihood_StatesIntegratedOut(TStateModel *model)
{
  //====== Check for valid forward recursion through time t  ======
  if ((model->t0 < model->nobs) && !ForwardRecursion(model->nobs,model)) return MINUS_INFINITY;

  return ElementV(model->L,model->nobs) - ElementV(model->L,model->fobs - 1);
}

/*
   Assumes:
     model : pointer to valid TStateModel structure

   Returns:
     Upon sucess, returns the natural logrithm of

                        P(Y[T] | Z[t], theta, Q)

     and minus infinity otherwise.  Note that minus infinity is also a valid 
     return value.

   Notes:
     Calls ForwardRecursion() if necessary.  The time parameter t is base one.
*/
PRECISION LogPosterior_StatesIntegratedOut(TStateModel *model)
{
  //====== Check for valid forward recursion through time t  ======
  if ((model->t0 < model->nobs) && !ForwardRecursion(model->nobs,model)) return MINUS_INFINITY;

  return ElementV(model->L,model->nobs) - ElementV(model->L,model->fobs - 1) + LogPrior(model);
}
/*******************************************************************************/
/*******************************************************************************/
/*******************************************************************************/


/*******************************************************************************/
/************************** Free parameters routines ***************************/
/*******************************************************************************/
void ConvertFreeParametersToQ(TStateModel *model, PRECISION *f)
{
  if (model->sv->nfree > 0)
    {
      memcpy(model->q,f,model->sv->nfree*sizeof(PRECISION));
      TransitionMatrixParametersChanged(model);
    }
}

void ConvertQToFreeParameters(TStateModel *model, PRECISION *f)
{
  if (model->sv->nfree > 0) memcpy(f,model->q,model->sv->nfree*sizeof(PRECISION));
}

void ConvertFreeParametersToTheta(TStateModel *model, PRECISION *f)
{
  if (model->routines->pConvertFreeParametersToTheta)
    {
      model->routines->pConvertFreeParametersToTheta(model,f);
      ThetaChanged(model);
    }
}
/*******************************************************************************/
/*******************************************************************************/
/*******************************************************************************/


/*******************************************************************************/
/****************************** Utility Routines *******************************/
/*******************************************************************************/
/*
   Assumes:
     v : An n vector or null pointer
     P : An n x n transition matrix.  It must be the case that P(i,j) >= 0 for
         every i and j and P(0,j) + ... + P(n-1,j) = 1 for every j.

   Results:
     Computes the ergodic distribution of the transition matrix P if it exists.
     The ergodic distribution will exist if there is a unique vector v such that
     the elements of v are non-negative, sum to one and (P - I)v = 0.    

   Notes:
     If w is the n dimensional row vector of ones, then w(P - I) = 0.  This
     implies that there exists a non-zero v such that (P - I)v = 0.  It is easy
     to show that if (P - I)v = 0 and v1 is the positive component of v and v2 is 
     the negative component of v, then both (P - I)v1 = 0 and (P - I)v2 = 0.  So 
     there is always exists a v such that the elements of v are non-negative, sum 
     to one, (P - I)v = 0.  However, a unique such element might not exist.  

     This routine does not check for uniqueness.

     This version uses the LU decomposition, which is fast and fairly stable.
*/
TVector Ergodic(TVector v, TMatrix P)
{
  TVector rtrn;
  TMatrix IP;
  TPermutation S;
  PRECISION sum;
  int i, j, k;

  if (!P)
    {
      dw_Error(NULL_ERR);
      return (TVector)NULL;
    }
  if (RowM(P) != ColM(P))
    {
      dw_Error(SIZE_ERR);
      return (TVector)NULL;
    }
  if (!v)
    {
      if (!(rtrn=CreateVector(RowM(P))))
	return (TVector)NULL;
    }
  else
    if (RowM(P) != DimV(v))
      {
	dw_Error(SIZE_ERR);
	return (TVector)NULL;
      }
    else
      rtrn=v;

  IP=EquateMatrix((TMatrix)NULL,P);
  for (i=RowM(P)-1; i >= 0; i--)
    ElementM(IP,i,i)-=1.0;
  S=CreatePermutation(RowM(P));

  if (LU(S,IP,IP))
    {
      for (k=0; k < RowM(P); k++)
	if (fabs(ElementM(IP,k,k)) < SQRT_MACHINE_EPSILON) break;

      if (k < RowM(P))
	{
	  for (i=DimV(rtrn)-1; i > k; i--) 
	    ElementV(rtrn,i)=0.0;
	  ElementV(rtrn,i--)=1.0;
	  for ( ; i >= 0; i--)
	    {
	      for (sum=-ElementM(IP,i,k), j=k-1; j > i; j--) sum-=ElementM(IP,i,j)*ElementV(rtrn,j);
	      ElementV(rtrn,i)=sum/ElementM(IP,i,i);
	    }

	  for (sum=1.0, i=k-1; i >= 0; i--)
	    if (ElementV(rtrn,i) < 0.0)
	      ElementV(rtrn,i)=0.0;
	    else
	      sum+=ElementV(rtrn,i);

	  ProductSV(rtrn,1.0/sum,rtrn);
	  FreePermutation(S);
	  FreeMatrix(IP);
	  return rtrn;
	}
    }

  FreePermutation(S);
  FreeMatrix(IP);

  if (!v) FreeVector(rtrn);
  return (TVector) NULL;
}

/*
   Assumes:
    There are DimV(p) states, denoted by 0, ... , Dim(p)-1.  The probability of
    state i occuring is given by p[i].  It must be the case that the sum of the
    p[i] is equal to one.

   Returns:
    A random draw of one of the states.
*/
int DrawDiscrete(TVector p)
{
  int i=DimV(p)-1;
  PRECISION u=dw_uniform_rnd(), s=ElementV(p,i);
  while (i > 0)
    if (u < s)
      return i;
    else
      s+=ElementV(p,--i);
  return 0;
}

/*
   Y : null pointer or vector array of dimension m.
   P : vector array of dimension a*b.
   X : vector array of dimension a*b.  Each vector is of dimension n.
   a : size of first variable. 
   b : size of second variable.
   c : which variable to integrate out (1 or 2) 

   Assumes that 

     s = (u,v) = u*b + v

   P(s) gives the probablity of state s occuring.   X[s] is the value of a random 
   vector given that state s occured.  

   If c is one, then m=b and u is integrated out and if c is two, then m=a and v 
   is integrated out.  If Y is null it is created and if Y[] is null, it is 
   created.                                           
*/
TVector* IntegrateStatesSingleV(TVector *Y, TVector P, TVector *X, int a, int b, int c)
{
  int s, u, v, n=DimV(X[0]);
  PRECISION sum, p;
  if (c == 1)
    {
      if (!Y) Y=dw_CreateArray_vector(b);
      for (v=b-1; v >= 0; v--)
	{
	  if (!Y[v]) Y[v]=CreateVector(n);
	  InitializeVector(Y[v],0.0);
	  for (sum=0.0,s=(a-1)*b+v; s >= 0; s-=b)
	    {
	      sum+=(p=ElementV(P,s));
	      LinearCombinationV(Y[v],1.0,Y[v],p,X[s]);
	    }
	  if (sum > 0) ProductVS(Y[v],Y[v],1.0/sum);
	}
    }
  else
    {
      if (!Y) Y=dw_CreateArray_vector(a);
      for (u=a-1; u >= 0; u--)
	{
	  if (!Y[u]) Y[u]=CreateVector(n);
	  InitializeVector(Y[u],0.0);
	  for (sum=0.0, v=b-1, s=u*b+v; v >= 0; v--, s--)
	    {
	      sum+=(p=ElementV(P,s));
	      LinearCombinationV(Y[u],1.0,Y[u],p,X[s]);
	    }
	  if (sum > 0) ProductVS(Y[u],Y[u],1.0/sum);
	}
    }
  return Y;
}

/*
   Y : null pointer or vector array of dimension m.
   P : vector array of dimension a*b.
   X : vector array of dimension a*b.  Each vector is of dimension n.
   a : size of first variable. 
   b : size of second variable.
   c : which variable to integrate out (1 or 2) 

   Assumes that 

     s = (u,v) = u*b + v

   P(s) gives the probablity of state s occuring.   X[s] is the value of a random 
   vector given that state s occured.  

   If c is one, then m=b and u is integrated out and if c is two, then m=a and v 
   is integrated out.  If Y is null it is created and if Y[] is null, it is 
   created.                                           
*/
TMatrix* IntegrateStatesSingleM(TMatrix *Y, TVector P, TMatrix *X, int a, int b, int c)
{
  int s, u, v, m=RowM(X[0]), n=ColM(X[0]);
  PRECISION sum, p;
  if (c == 1)
    {
      if (!Y) Y=dw_CreateArray_matrix(b);
      for (v=b-1; v >= 0; v--)
	{
	  if (!Y[v]) Y[v]=CreateMatrix(m,m);
	  InitializeMatrix(Y[v],0.0);
	  for (sum=0.0,s=(a-1)*b+v; s >= 0; s-=b)
	    {
	      sum+=(p=ElementV(P,s));
	      LinearCombinationM(Y[v],1.0,Y[v],p,X[s]);
	    }
	  if (sum > 0) ProductMS(Y[v],Y[v],1.0/sum);
	}
    }
  else
    {
      if (!Y) Y=dw_CreateArray_matrix(a);
      for (u=a-1; u >= 0; u--)
	{
	  if (!Y[u]) Y[u]=CreateMatrix(m,n);
	  InitializeMatrix(Y[u],0.0);
	  for (sum=0.0, v=b-1, s=u*b+v; v >= 0; v--, s--)
	    {
	      sum+=(p=ElementV(P,s));
	      LinearCombinationM(Y[u],1.0,Y[u],p,X[s]);
	    }
	  if (sum > 0) ProductMS(Y[u],Y[u],1.0/sum);
	}
    }
  return Y;
}

/*
   Y : null pointer or vector array of dimension T x m, where m is a or b.
   P : vector array of dimension T x (a*b).
   X : vector array of dimension a*b.  Each vector is of dimension n.
   a : size of first variable. 
   b : size of second variable.
   c : which variable to integrate out (1 or 2) 

   Assumes that 

     s = (u,v) = u*b + v

   P[t](s) gives the probablity of state s occuring at time t.   X[t][s] is the 
   value of a random vector given that state s occured at time t.  

   If c is one, then m=b and u is integrated out and if c is two, then m=a and v 
   is integrated out.  If Y is null, it is created.                            
*/
TVector** IntegrateStatesV(TVector **Y, TVector* P, TVector **X, int a, int b, int c)
{
  int t, T=dw_DimA(P);
  if (!Y) Y=dw_CreateRectangularArray_vector(T,(c == 1) ? b : a);
  for (t=T-1; t >= 0; t--) Y[t]=IntegrateStatesSingleV(Y[t],P[t],X[t],a,b,c);
  return Y;
}

/*
   Q : null pointer or vector of dimension m.
   P : vectorof dimension a*b.
   a : size of first variable. 
   b : size of second variable.
   c : which variable to integrate out (1 or 2) 

   Assumes that 

     s = (u,v) = u*b + v

   P[t](s) gives the probablity of state s occuring at time t. 

   If c is one, then m=b and u is integrated out and if c is two, then m=a and v 
   is integrated out.  If Y is null, it is created.                                  
*/
TVector IntegrateStatesSingle(TVector Q, TVector P, int a, int b, int c)
{
  int u, v, s;
  if (c == 1)
    {
      if (!Q) Q=CreateVector(b);
      InitializeVector(Q,0.0);
      for (v=b-1; v >= 0; v--)
	for (s=(a-1)*b+v; s >= 0; s-=b)
	  ElementV(Q,v)+=ElementV(P,s);
    }
  else
    {
      if (!Q) Q=CreateVector(a);
      InitializeVector(Q,0.0);
      for (u=a-1; u >= 0; u--)
	for (v=b-1, s=u*b+v; v >= 0; v--, s--)
	  ElementV(Q,u)+=ElementV(P,s);
    }
  return Q;
}

/*
   Q : null pointer or vector array of dimension T x m
   P : vector array of dimension T x (a*b).
   a : size of first variable. 
   b : size of second variable.
   c : which variable to integrate out (1 or 2) 

   Assumes that 

     s = (u,v) = u*b + v

   P[t](s) gives the probablity of state s occuring at time t.

   If c is one, then m=b and u is integrated out and if c is two, then m=a and v 
   is integrated out.  If Q is null, it is created.                                  
*/
TVector* IntegrateStates(TVector *Q, TVector* P, int a, int b, int c)
{
  int t, T=dw_DimA(P);
  if (!Q) Q=dw_CreateArray_vector(T);
  for (t=T-1; t >= 0; t--) Q[t]=IntegrateStatesSingle(Q[t],P[t],a,b,c);
  return Q;
}
/*******************************************************************************/
/*******************************************************************************/
/*******************************************************************************/

