/*
 * 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_MSStateSpace.h"
#include "dw_matrix_array.h"
#include "dw_error.h"
#include "dw_math.h"
#include "dw_rand.h"
#include "dw_std.h"

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

//#define DEBUG_FILTER
//#define DEBUG_PAUSE

static void AddTranspose(TMatrix X);
static PRECISION IsSymmetric(TMatrix X);

ThetaRoutines* CreateThetaRoutines_MSStateSpace(void)
{
  ThetaRoutines *routines;
  routines=CreateThetaRoutines_empty();

  // sroutines_ps->pLogConditionalLikelihood = logTimetCondLH;     
  // sroutines_ps->pLogPrior = logpriordensity;                                           
  // sroutines_ps->pNumberFreeParametersTheta = NumberOfFreeModelSpecificParameters;               
  // sroutines_ps->pConvertFreeParametersToTheta = ConvertFreeParameters2ModelSpecificParameters;  
  // sroutines_ps->pConvertThetaToFreeParameters = CopyMyFreeParameters2OptimizationParameters; 
  // sroutines_ps->pThetaChanged = tz_thetaChanged;          
  // sroutines_ps->pTransitionMatrixParametersChanged = tz_TransitionMatrixChanged;  

  routines->pLogConditionalLikelihood = LogConditionalLikelihood_MSStateSpace;                                  
  routines->pLogPrior = LogPrior_MSStateSpace;                                           

  routines->pNumberFreeParametersTheta = NumberFreeParametersTheta_MSStateSpace;               
  routines->pConvertFreeParametersToTheta = ConvertFreeParametersToTheta_MSStateSpace;
  routines->pConvertThetaToFreeParameters = ConvertThetaToFreeParameters_MSStateSpace;

  routines->pStatesChanged = StatesChanged_MSStateSpace;
  routines->pThetaChanged = ThetaChanged_MSStateSpace;
  routines->pTransitionMatrixParametersChanged = TransitionMatrixParametersChanged_MSStateSpace;

  routines->pDestructor = (void (*)(void*))Free_MSStateSpace;

  routines->pModelType = ModelType_MSStateSpace;

  return routines;
}

char* ModelType_MSStateSpace(void)
{
  return "T_MSStateSpace";
}

/*
   Assumes:
    ny            : positive integer
    nz            : positive integer
    nu            : non-negative integer
    nepsilon      : non-negative integer
    nbasestates   : positive integer
    nlags_encoded : non-negative integer
    nobs          : positive integer

   Results:
    Allocates memory for T_MSStateSpace structure.
*/
T_MSStateSpace* Create_MSStateSpace(int ny, int nz, int nu, int nepsilon, int nbasestates, int nlags_encoded, int nobs)
{
  T_MSStateSpace *statespace;
  int i, t;

  statespace=(T_MSStateSpace*)dw_malloc(sizeof(T_MSStateSpace));

  // Sizes
  statespace->ny=ny;
  statespace->nz=nz;
  statespace->nu=nu;
  statespace->nepsilon=nepsilon;
  statespace->nbasestates=nbasestates;
  statespace->nlags_encoded=nlags_encoded;
  statespace->nobs=nobs;
  for (statespace->zeta_modulus=1, i=nlags_encoded; i > 0; i--) statespace->zeta_modulus*=nbasestates;
  statespace->nstates=statespace->zeta_modulus*nbasestates;

  // Flags
  statespace->NonZeroG=0;
  statespace->t0=-1;
  statespace->smoothed=0;

  // Create data
  statespace->y=dw_CreateArray_vector(nobs+1);
  for (i=nobs; i >= 0; i--)
    statespace->y[i]=CreateVector(ny);

  // Parameters
  statespace->a=dw_CreateArray_vector(nbasestates);
  statespace->H=dw_CreateArray_matrix(nbasestates);
  statespace->b=dw_CreateArray_vector(nbasestates);
  statespace->F=dw_CreateArray_matrix(nbasestates);
  statespace->R=dw_CreateArray_matrix(nbasestates);
  statespace->V=dw_CreateArray_matrix(nbasestates);
  statespace->G=dw_CreateArray_matrix(nbasestates);
  statespace->Phiy=(nu > 0) ? dw_CreateArray_matrix(nbasestates) : (TMatrix*)NULL;
  statespace->Phiz=(nepsilon > 0) ? dw_CreateArray_matrix(nbasestates) : (TMatrix*)NULL;
  for (i=nbasestates-1; i >= 0; i--)
    {
      statespace->a[i]=CreateVector(ny);
      statespace->H[i]=CreateMatrix(ny,nz);
      statespace->b[i]=CreateVector(nz);
      statespace->F[i]=CreateMatrix(nz,nz);
      statespace->R[i]=CreateMatrix(ny,ny);
      statespace->V[i]=CreateMatrix(nz,nz);
      statespace->G[i]=CreateMatrix(nz,ny);
      if (nu > 0) statespace->Phiy[i]=CreateMatrix(ny,nu);
      if (nepsilon > 0) statespace->Phiz[i]=CreateMatrix(nz,nepsilon);
    }

  // Stored Kalman structure
  statespace->Ez=dw_CreateRectangularArray_vector(nobs+1,statespace->nstates);
  statespace->Ezz=dw_CreateRectangularArray_matrix(nobs+1,statespace->nstates);
  statespace->IEz=dw_CreateRectangularArray_vector(nobs+1,statespace->zeta_modulus);
  statespace->IEzz=dw_CreateRectangularArray_matrix(nobs+1,statespace->zeta_modulus);
  statespace->Ez1=dw_CreateRectangularArray_vector(nobs+1,statespace->nstates);
  statespace->Ezz1=dw_CreateRectangularArray_matrix(nobs+1,statespace->nstates);
  statespace->Ey1=dw_CreateRectangularArray_vector(nobs+1,statespace->nstates);
  statespace->Eyy1=dw_CreateRectangularArray_matrix(nobs+1,statespace->nstates);
  statespace->ey1=dw_CreateRectangularArray_vector(nobs+1,statespace->nstates);
  statespace->L=dw_CreateArray_vector(nobs+1);
  statespace->SEz=dw_CreateRectangularArray_vector(nobs+1,statespace->nstates);
  statespace->ISEz=dw_CreateRectangularArray_vector(nobs+1,statespace->zeta_modulus);
  //statespace->SEu=(nu > 0) ? dw_CreateRectangularArray_vector(nobs+1,statespace->zeta_modulus) : (TVector**)NULL;
  //statespace->SEepsilon=(nepsilon > 0) ? dw_CreateRectangularArray_vector(nobs+1,statespace->zeta_modulus) : (TVector**)NULL;
  for (t=nobs; t >= 0; t--)
    {
      for (i=statespace->nstates-1; i >= 0; i--)
	{
	  statespace->Ez[t][i]=CreateVector(nz);
	  statespace->Ezz[t][i]=CreateMatrix(nz,nz);
	  statespace->Ez1[t][i]=CreateVector(nz);
	  statespace->Ezz1[t][i]=CreateMatrix(nz,nz);
	  statespace->Ey1[t][i]=CreateVector(ny);
	  statespace->Eyy1[t][i]=CreateMatrix(ny,ny);
	  statespace->ey1[t][i]=CreateVector(ny);
	  statespace->SEz[t][i]=CreateVector(nz);
	}
      for (i=statespace->zeta_modulus-1; i >= 0; i--)
	{
	  statespace->IEz[t][i]=CreateVector(nz);
	  statespace->IEzz[t][i]=CreateMatrix(nz,nz);
	  statespace->ISEz[t][i]=CreateVector(nz);
	  //if (nu > 0) statespace->SEu[t][i]=CreateVector(nu);
	  //if (nepsilon > 0) statespace->SEepsilon[t][i]=CreateVector(nepsilon);
	}
      statespace->L[t]=CreateVector(statespace->nstates);
    }

  // Tao's model structure
  statespace->TZstatemodel=(TStateModel*)NULL;
  statespace->MSModel=NULL;
  statespace->gensys_struct=NULL;
  return statespace;
}

void Free_MSStateSpace(T_MSStateSpace *statespace)
{
  if (statespace)
    {
      dw_FreeArray(statespace->y);

      dw_FreeArray(statespace->a);
      dw_FreeArray(statespace->H);
      dw_FreeArray(statespace->b);
      dw_FreeArray(statespace->F);
      dw_FreeArray(statespace->R);
      dw_FreeArray(statespace->V);
      dw_FreeArray(statespace->G);
      dw_FreeArray(statespace->Phiy);
      dw_FreeArray(statespace->Phiz);

      dw_FreeArray(statespace->Ez);
      dw_FreeArray(statespace->Ezz);
      dw_FreeArray(statespace->IEz);
      dw_FreeArray(statespace->IEzz); 
      dw_FreeArray(statespace->Ezz1);
      dw_FreeArray(statespace->Ey1);
      dw_FreeArray(statespace->Eyy1);
      dw_FreeArray(statespace->ey1);
      dw_FreeArray(statespace->L);

      dw_FreeArray(statespace->ISEz);
      dw_FreeArray(statespace->SEz);

      dw_free(statespace);
    }
}

TStateModel* CreateStateModel_MSStateSpace(TStateModel *basemodel, int nlags_encoded)
{
  T_MSStateSpace *statespace=Create_MSStateSpaceFromStateModel(basemodel,nlags_encoded);
  statespace->MSModel = NULL;
  TMarkovStateVariable *sv_new;
  ThetaRoutines *routines;
  TStateModel *model;
  PRECISION *buffer=dw_malloc(NumberFreeParametersQ(basemodel)*sizeof(PRECISION));

  // Create TMarkovStateVariable structure
  sv_new=CreateMarkovStateVariable_lags(nlags_encoded,basemodel->sv);

  // Create theta routines
  routines=CreateThetaRoutines_MSStateSpace();

  // Create TStateVariable
  model=CreateStateModel(statespace->nobs,sv_new,routines,(void*)statespace);

  // Initialize structure
  model->fobs=basemodel->fobs;
  ConvertQToFreeParameters(basemodel,buffer);
  ConvertFreeParametersToQ(model,buffer);
  dw_free(buffer);

  return model;
}

double LogPrior_MSStateSpace(TStateModel *model)
{
  T_MSStateSpace *statespace=(T_MSStateSpace*)(model->theta);
  if (statespace->MSModel == NULL)
    {  
      return (statespace->TZstatemodel->routines->pLogPrior) 
	? statespace->TZstatemodel->routines->pLogPrior(statespace->TZstatemodel) : MINUS_INFINITY;
    }
  else 
#ifdef DYNARE_INTERFACE
    return get_log_prior_from_markov_model(statespace->MSModel);
#else
  return MINUS_INFINITY;
#endif
} 

void StatesChanged_MSStateSpace(TStateModel *model)
{
  T_MSStateSpace *statespace=(T_MSStateSpace*)(model->theta);
  if (statespace->MSModel == NULL) 
    {
      Invalidate_MSStateSpace(statespace);
      if (statespace->TZstatemodel->routines->pStatesChanged) 
	statespace->TZstatemodel->routines->pStatesChanged(statespace->TZstatemodel);
    }
#ifdef DYNARE_INTERFACE
  else 
    states_changed_in_markov_model(statespace->MSModel);
#endif
}

void ThetaChanged_MSStateSpace(TStateModel *model)
{
  T_MSStateSpace *statespace=(T_MSStateSpace*)(model->theta);
  Invalidate_MSStateSpace(statespace);
  if (statespace->MSModel == NULL) 
    {
      if (statespace->TZstatemodel->routines->pThetaChanged) 
	statespace->TZstatemodel->routines->pThetaChanged(statespace->TZstatemodel);
    }
#ifdef DYNARE_INTERFACE
  else 
    theta_changed_in_markov_model(statespace->MSModel);
#endif
}

void TransitionMatrixParametersChanged_MSStateSpace(TStateModel *model)
{
  T_MSStateSpace *statespace=(T_MSStateSpace*)(model->theta);
  if (statespace->MSModel == NULL) 
    {
      Invalidate_MSStateSpace(statespace);
      if (statespace->TZstatemodel->routines->pTransitionMatrixParametersChanged) 
	statespace->TZstatemodel->routines->pTransitionMatrixParametersChanged(statespace->TZstatemodel);
    }
#ifdef DYNARE_INTERFACE
  else 
    transition_matrix_parameters_changed_im_markov_model(statespace->MSModel);
#endif
}

void Invalidate_MSStateSpace(T_MSStateSpace *statespace)
{
  statespace->t0=-1;
  statespace->smoothed=0;
}

int NumberFreeParametersTheta_MSStateSpace(TStateModel *model)
{
  T_MSStateSpace *statespace=(T_MSStateSpace*)(model->theta);
  if (statespace->MSModel == NULL)  
    {
      return (statespace->TZstatemodel->routines->pNumberFreeParametersTheta) ? 
	statespace->TZstatemodel->routines->pNumberFreeParametersTheta(statespace->TZstatemodel) : -1;
    } 
  else 
#ifdef DYNARE_INTERFACE
    return number_free_parameters_theta_markov_model(statespace->MSModel);
#else
  return -1;
#endif
}

void ConvertFreeParametersToTheta_MSStateSpace(TStateModel *model, PRECISION *buffer)
{
  T_MSStateSpace *statespace=(T_MSStateSpace*)(model->theta);
  if (statespace->MSModel == NULL) 
    {
      if (statespace->TZstatemodel->routines->pConvertFreeParametersToTheta) 
	statespace->TZstatemodel->routines->pConvertFreeParametersToTheta(statespace->TZstatemodel,buffer);
    } 
#ifdef DYNARE_INTERFACE
  else 
    convert_free_parameter_to_theta_markov_model(statespace->MSModel, buffer);
#endif
}

void ConvertThetaToFreeParameters_MSStateSpace(TStateModel *model, PRECISION *buffer)
{
  T_MSStateSpace *statespace=(T_MSStateSpace*)(model->theta);
  if (statespace->MSModel == NULL) 
    {
      if (statespace->TZstatemodel->routines->pConvertThetaToFreeParameters) 
	statespace->TZstatemodel->routines->pConvertThetaToFreeParameters(statespace->TZstatemodel,buffer);
    }
#ifdef DYNARE_INTERFACE
  else 
    convert_theta_to_free_parameters_markov_model(statespace->MSModel, buffer);
#endif
}


/*
   Computes
     
      P[y(t) | Y(t-1), s(t)=s, theta, q]

   under the assumption that y(t) conditional on Y(t-1), s(t), theta, and q is
   Gaussian with mean statespace->ey1[t][s] and variance statespace->Eyy1[t][s].  The Markov
   process s(t) denotes the grand state in model->sv.

   If t > statespace->t0, calls Filter(t,model).
*/
PRECISION LogConditionalLikelihood_MSStateSpace(int s, int t, TStateModel *model)
{
  T_MSStateSpace *statespace=(T_MSStateSpace*)(model->theta);
  if ((statespace->t0 < t) && !Filter(t,model)) return MINUS_INFINITY;
  return ElementV(statespace->L[t],s);
}

  //=== Computes E[y[t] | Y[t-1], Z[t], theta, q, s[t] = s], works even for t > T, ===
  //=== data[tau] = Y[T+tau+1], assumes exogeneous variable z[t] is deterministic  ===
TVector pEyt_MSStateSpace(TVector y, int T, TVector *data, int s, int t, TStateModel *model)
{
  if ((t <= T) || (T > model->nobs)) return (TVector)NULL;
  if (t == T+1)
    {
    }
  else
    {
    }
  return (TVector)NULL;
}

  //=== Computes E[y[t] | Y[t-1], Z[t], theta, q, s[t] = s, shock(i)], shock(i) is the unit  ===
  //=== value of the model specific ith shock, works even for t > T, data[tau] = Y[T+tau+1], ===
  //=== assumes exogeneous variable z[t] is deterministic                                    ===
  TVector pEyt_i_MSStateSpace(TVector y, int T, TVector *data, int i, int s, int t, TStateModel *model);

  //=== Makes random draw of y[t] given Y[t-1], Z[t], theta, q, s[t] = s, works for t > T, ===
  //=== data[tau] = Y[T+tau+1], assumes exogeneous variable z[t] is deterministic          ===
  TVector pyt_draw_MSStateSpace(TVector y, int T, TVector *data, int s, int t, TStateModel *model);



/*
   Upon entry to a call, the following must be true:
    
     Parameter values
      If statespace->t0 >= 0, then a, H, b, F, R, V, and G are properly set
  
     Kalman filtering data are properly defined for
      IEz[t][j]  1 <= t <= t0-1, 0 <= j < zeta_modulus
      IEzz[t][j] 1 <= t <= t0-1, 0 <= j < zeta_modulus
      Ez1[t][k]  1 <= t <= t0,   0 <= k < nstates       - Ez1[1][k] is also defined if t0 = 0
      Ezz1[t][k] 1 <= t <= t0,   0 <= k < nstates       - Ezz1[1][k] is also defined if t0 = 0
      Ey1[t][k]  1 <= t <= t0,   0 <= k < nstates
      Eyy1[t][k] 1 <= t <= t0,   0 <= k < nstates
      ey1[t][k]  1 <= t <= t0,   0 <= k < nstates
      Ez[t][k]   1 <= t <= t0,   0 <= k < nstates       
      Ezz[t][k]  1 <= t <= t0,   0 <= k < nstates       
      L[t][k]    1 <= t <= t0,   0 <= k < nstates

     Flags
      If statespace->t0 >= 0, then NonZeroG is properly set 
     
   -----------------------------------------------------------------------------------------------

   After a successful call, with tau >= 0, the following are valid:

     Parameter values
      a, H, b, F, R, V, and G are properly set

     Kalman filtering data are properly defined for
      IEz[t][j]  1 <= t <= tau-1, 0 <= j < zeta_modulus
      IEzz[t][j] 1 <= t <= tau-1, 0 <= j < zeta_modulus
      Ez1[t][k]  1 <= t <= tau,   0 <= k < nstates       - Ez1[1][k] is also defined if tau = 0
      Ezz1[t][k] 1 <= t <= tau,   0 <= k < nstates       - Ezz1[1][k] is also defined if tau = 0
      Ey1[t][k]  1 <= t <= tau,   0 <= k < nstates
      Eyy1[t][k] 1 <= t <= tau,   0 <= k < nstates
      ey1[t][k]  1 <= t <= tau,   0 <= k < nstates
      Ez[t][k]   1 <= t <= tau,   0 <= k < nstates       
      Ezz[t][k]  1 <= t <= tau,   0 <= k < nstates       
      L[t][k]    1 <= t <= tau,   0 <= k < nstates

     Flags
      NonZeroG is properly set
      statespace->t0 = tau

  Returns 1 upon success and 0 otherwise.

  If model->t0 < tau-1, calls ForwardRecursion(t,model) for statespace->t0 <= t <= tau-1.
  IEz and IEzz are only updated through tau-1 because ForwardRecursion(t,model) can only
  be called with t < tau.
    
*/
#define LOG2PI 1.83787706640934548356     // log(2*pi)
int Filter(int tau, TStateModel *model)
{
  int j, k, s, t, zeta, terminal_errors, verbose_errors, rtrn=0;
  PRECISION tmp;
  T_MSStateSpace *statespace=(T_MSStateSpace*)(model->theta);
  TMatrix X, Y, K, T;
  TVector v;

  if (tau <= statespace->t0) return 1;

  if (tau > statespace->nobs) return 0;

  // After a successful call to Initialize_MSStateSpace(), the following hold
  // t0 = 0
  // NonZeroG is properly set
  // a, H, b, F, R, V, and G are properly set
  // Ez1[1][k] and Ezz1[1][k] are properly set for 0 <= k < nstates
  if ((statespace->t0 < 0) && !Initialize_MSStateSpace(model)) return 0;

  X=CreateMatrix(statespace->nz,statespace->nz);
  Y=CreateMatrix(statespace->ny,statespace->nz);
  T=CreateMatrix(statespace->ny,statespace->ny);   
  K=CreateMatrix(statespace->nz,statespace->ny);
  v=CreateVector(statespace->ny);

  terminal_errors=dw_SetTerminalErrors(0);
  verbose_errors=dw_SetVerboseErrors(0);

  for (t=statespace->t0+1; t <= tau; t++)
    {
      if (!ForwardRecursion(t-1,model)) goto CLEAN_UP;

      // Sets IEz[t-1] and IEzz[t-1] 
      // Uses Ez[t-1], Ezz[t-1], and model->V[t-1](i)=P(s[t-1] = i | Y[t-1], Theta)
      if (t > 1)
	{
	  IntegrateStatesSingleV(statespace->IEz[t-1],model->V[t-1],statespace->Ez[t-1],
				 statespace->zeta_modulus,statespace->nbasestates,2);
	  IntegrateStatesSingleM(statespace->IEzz[t-1],model->V[t-1],statespace->Ezz[t-1],
				 statespace->zeta_modulus,statespace->nbasestates,2);
	}

      for (k=statespace->nstates-1; k >= 0; k--)
	{
	  // s = s(t)
	  s=model->sv->lag_index[k][0];

	  // Sets Ez1[t][k] and Ezz1[t][k]
	  // Uses IEz[t-1][k], and IEzz[t-1][k]
	  if (t > 1)
	    {
	      // zeta = zeta(t-1) 
	      zeta=k-s*statespace->zeta_modulus;                               

	      ProductMV(statespace->Ez1[t][k],statespace->F[s],statespace->IEz[t-1][zeta]);
	      AddVV(statespace->Ez1[t][k],statespace->Ez1[t][k],statespace->b[s]);

	      ProductMM(X,statespace->F[s],statespace->IEzz[t-1][zeta]);
	      ProductTransposeMM(statespace->Ezz1[t][k],X,statespace->F[s]);
	      AddMM(statespace->Ezz1[t][k],statespace->Ezz1[t][k],statespace->V[s]);
	      ForceSymmetric(statespace->Ezz1[t][k]);
	    }

	  // Sets Ey1[t][k] and Eyy1[t][k]
	  // Uses Ez1[t][k] and Ezz1[t][k]
	  ProductMV(statespace->Ey1[t][k],statespace->H[s],statespace->Ez1[t][k]);
	  AddVV(statespace->Ey1[t][k],statespace->Ey1[t][k],statespace->a[s]);

	  ProductMM(Y,statespace->H[s],statespace->Ezz1[t][k]);
	  ProductTransposeMM(statespace->Eyy1[t][k],Y,statespace->H[s]);
	  AddMM(statespace->Eyy1[t][k],statespace->Eyy1[t][k],statespace->R[s]);
	  if (statespace->NonZeroG)
	    {
	      ProductMM(T,statespace->H[s],statespace->G[s]);
	      AddTranspose(T);
	      AddMM(statespace->Eyy1[t][k],statespace->Eyy1[t][k],T);
	    }
	  ForceSymmetric(statespace->Eyy1[t][k]);

	  // Sets ey1[t][k]
	  // Uses Ey1[t][k] and y[t]
	  SubtractVV(statespace->ey1[t][k],statespace->y[t],statespace->Ey1[t][k]);

	  // Cholesky decomposition of Eyy1[t][k] (Eyy1[t][k] = T'*T)
	  if (!CholeskyUT(T,statespace->Eyy1[t][k])) goto CLEAN_UP;

	  //dw_PrintMatrix(stdout,statespace->Eyy1[t][k],"%lg ");
	  //terminal_errors=dw_SetTerminalErrors(0);
	  //if (!CholeskyUT(T,statespace->Eyy1[t][k])) 
	  //  {
	  //    MatrixSquareRoot(T,statespace->Eyy1[t][k]);
	  //    if (!QR((TMatrix)NULL,T,T))
	  //	{
	  //	  dw_SetTerminalErrors(terminal_errors);
	  //	  return 0;
	  //	}
	  //  }
	  //dw_SetTerminalErrors(terminal_errors);
	  //printf("%d-%d\n",t,k); getchar();

	  // K = (Ezz1[t][k]*H[s]' + G[s])*Inverse(T)
	  ProductTransposeMM(K,statespace->Ezz1[t][k],statespace->H[s]);
	  if (statespace->NonZeroG) AddMM(K,K,statespace->G[s]);
	  ProductInverseMU(K,K,T);

	  // v = (ey1[t][k]'*Inverse(T))'
	  ProductInverseVU(v,statespace->ey1[t][k],T);

	  // Sets Ez[t][k] and Ezz[t][k]
	  // Uses K, v, Ez1[t][k], and Ezz1[t][k]
	  ProductMV(statespace->Ez[t][k],K,v);
	  AddVV(statespace->Ez[t][k],statespace->Ez[t][k],statespace->Ez1[t][k]);

	  ProductTransposeMM(statespace->Ezz[t][k],K,K);
	  SubtractMM(statespace->Ezz[t][k],statespace->Ezz1[t][k],statespace->Ezz[t][k]);
	  ForceSymmetric(statespace->Ezz[t][k]);

	  // Sets L[t](k)
	  // Uses T and v.  The diagonal elements of T are positive.
	  for (tmp=0.0, j=RowM(T)*RowM(T)-1; j >= 0; j-=RowM(T)+1)
	    tmp-=log(pElementM(T)[j]);
	  ElementV(statespace->L[t],k)=tmp - 0.5*(LOG2PI*statespace->ny + DotProduct(v,v));

///////////////////////////////////////////////////////////////////////////////////////////////////////
#ifdef DEBUG_FILTER

	  TMatrix K1=CreateMatrix(statespace->nz,statespace->ny), K2=CreateMatrix(statespace->nz,statespace->ny);
	  TVector v1=CreateVector(statespace->nz);

          // Alternative computation of statespace->Ez[t][k] and statespace->Ezz[t][k]
	  // Uses statespace->Ezz1[t][k], statespace->Eyy1[t][k], statespace->ey1[t][k]
	  ProductTransposeMM(K1,statespace->Ezz1[t][k],statespace->H[s]);
	  if (statespace->NonZeroG) AddMM(K1,K1,statespace->G[s]);
	  ProductInverseMM(K2,K1,statespace->Eyy1[t][k]);
	  ProductMV(v1,K2,statespace->ey1[t][k]);
	  AddVV(v1,v1,statespace->Ez1[t][k]);
	  if (Norm(SubtractVV(v1,v1,statespace->Ez[t][k])) > 1e-12)
	    {
	      printf("Difference E[z(%d) | Y(%d), s(%d)=%d, theta, q] : %le\n",t,t,t,k,Norm(v1));
#ifdef DEBUG_PAUSE
              printf("Press <Enter> to continue");
	      getchar(); 
#endif
	    }

	  ProductMM(K1,K2,statespace->Eyy1[t][k]);
	  ProductTransposeMM(X,K1,K2);
 	  SubtractMM(X,statespace->Ezz1[t][k],X);
 	  ForceSymmetric(X);
	  SubtractMM(X,X,statespace->Ezz[t][k]);
	  int ii, jj;
	  for (sum=0, ii=0; ii < statespace->nz; ii++)
	    for (jj=0; jj < statespace->nz; jj++)
	      sum+=ElementM(X,ii,jj)*ElementM(X,ii,jj);
	  sum=sqrt(sum);
	  if (sum > 1e-10)
	    {
	      printf("Difference Var[z(%d) | Y(%d), s(%d)=%d, theta, q] : %le\n",t,t,t,k,sum);
#ifdef DEBUG_PAUSE
              printf("Press <Enter> to continue");
	      getchar();
#endif
	    }

	  // An alternative way of computing the loglikelihood
	  InverseProductMV(v,statespace->Eyy1[t][k],statespace->ey1[t][k]);
	  if (fabs(ElementV(statespace->L[t],k) - (tmp - 0.5*(LOG2PI*statespace->ny + DotProduct(statespace->ey1[t][k],v)))) > 1e-13)
	    { 
	      printf("Difference conditional log likelihood: t =  %d  state = %d  %le\n",t,k,
		     ElementV(statespace->L[t],k) - (tmp - 0.5*(LOG2PI*statespace->ny + DotProduct(statespace->ey1[t][k],v))));
#ifdef DEBUG_PAUSE
              printf("Press <Enter> to continue");
	      getchar(); 
#endif
	    }

	  FreeMatrix(K1);
	  FreeMatrix(K2);
	  FreeVector(v1);
#endif
///////////////////////////////////////////////////////////////////////////////////////////////////////
	}

      statespace->t0=t;
    }

  rtrn=1;

  // Clean up
 CLEAN_UP:
  FreeVector(v); 
  FreeMatrix(K);
  FreeMatrix(T); 
  FreeMatrix(Y);
  FreeMatrix(X);

  dw_SetTerminalErrors(terminal_errors);
  dw_SetVerboseErrors(verbose_errors);

  return rtrn;
}
#undef LOG2PI

/*
   0 <= t0 <= t1 < T

   y     - T x ny with y[t][i] null for data that is missing.

   Ez_i  - Ez_i(k) ~ E[z(t0-1) | Y(t0-1), xi(t0-1)=k, theta, q]
 
   Ezz_i - Ezz_i(k) ~ E[(z(t0-1) - Ez_i(k))(z(t0-1) - Ez_i(k))' | Y(t0-1), xi(t0-1)=k, theta, q]

   Pxi_i - Pxi_i(k) ~ P(xi(t0-1)=k | Y(t0-1), theta, q)

   Q     - transition matrix.  Q(i,j) = P(s[t+1]=i | s[t]=j)

   Pxi   - Pxi[t](k) ~ P(xi(t)=k | Y(t), theta, q)

   Pxi1  - Pxi1[t](k) ~ P(xi(t)=k | Y(t-1), theta, q)

   IEz   - IEz[t](j) ~ E[z(t) | Y(t), zeta(t)=j, theta, q]

   IEzz  - IEzz[t](j) ~ E[(z(t) - IEz[t](j))(z(t) - IEz[t](j))' | Y(t), zeta(t)=j, theta, q] 

   Ez1   - Ez1[t](k) ~ E[z(t) | Y(t-1), xi(t)=k, theta, q]

   Ezz1  - Ezz1[t](k) ~ E[(z(t) - Ez1[t](k))(z(t) - Ez1[t](k))' | Y(t-1), xi(t)=k, theta, q] 
 
   Ez    - Ez[t](k) ~ E[z(t) | Y(t), xi(t)=k, theta, q]

   Ezz   - Ezz[t](k) ~ E[(z(t) - Ez[t](k))(z(t) - Ez[t](t))' | Y(t), xi(t)=k, theta, q] 
   
   Pxi, Pxi1, IEz, IEzz, Ez1, Ezz1, Ez, and Ezz are all matrix or vector arrays 
   of length T.  Upon exit, Pxi[t], Pxi1[t], IEz[t], IEzz[t], Ez1[t], Ezz1[t], 
   Ez[t], and Ezz[t] are all set for t0 <= t <= t1.

   In general the transition matrix Q could depend on t, however, in this 
   application we assume that it is independent of t.
*/
#define LOG2PI 1.83787706640934548356     // log(2*pi)
int ConditionalFilter(int t0, int t1, PRECISION ***y, TVector *Ez_i, TMatrix *Ezz_i, TVector Pxi_i, TMatrix Q, 
		      TVector *Pxi, TVector *Pxi1, TVector **IEz, TMatrix **IEzz, TVector **Ez1, TMatrix **Ezz1, 
		      TVector **Ez, TMatrix **Ezz, T_MSStateSpace *statespace)
{
  int i, j, k, m, n, xi, s, t, zeta, terminal_errors, verbose_errors, n_xi=statespace->nstates, 
    n_zeta=statespace->zeta_modulus, n_s=statespace->nbasestates, nz=statespace->nz, ny=statespace->ny;
  PRECISION scale, sum;
  TMatrix X, Y, K, T, *H, *R, *G=(TMatrix*)NULL, *IEzz_i, Eyy1;
  TVector v, cy, *a, *IEz_i, Ey1, L;
  
  IEz_i=dw_CreateArray_vector(n_xi);
  IEzz_i=dw_CreateArray_matrix(n_xi);
  a=dw_CreateArray_vector(n_s);
  H=dw_CreateArray_matrix(n_s);
  R=dw_CreateArray_matrix(n_s);
  if (statespace->NonZeroG) G=dw_CreateArray_matrix(n_s);
  L=CreateVector(n_xi);
  X=CreateMatrix(nz,nz);

  // Resetting terminal and verbose errors for calls to CholeskyLT()
  terminal_errors=dw_SetTerminalErrors(0);
  verbose_errors=dw_SetVerboseErrors(0);

  for (t=t0; t <= t1; t++)
    {
      if (t > t0)
	{
	  // Sets IEz[t-1] and IEzz[t-1] 
	  // Uses Ez[t-1], Ezz[t-1], and Pxi[t-1]
	  IntegrateStatesSingleV(IEz[t-1],Pxi[t-1],Ez[t-1],n_zeta,n_s,2);
	  IntegrateStatesSingleM(IEzz[t-1],Pxi[t-1],Ezz[t-1],n_zeta,n_s,2);

	  // s=s(t)
	  for (s=n_s-1; s >= 0; s--)
	    // zeta=zeta(t-1)
	    for (zeta=n_zeta-1; zeta >= 0; zeta--)
	      {
		// xi=xi(t)
		xi=s*n_zeta+zeta;

		// Sets Ez1[t][xi] and Ezz1[t][xi]
		// Uses IEz[t-1][xi], and IEzz[t-1][xi]
		ProductMV(Ez1[t][xi],statespace->F[s],IEz[t-1][zeta]);
		AddVV(Ez1[t][xi],Ez1[t][xi],statespace->b[s]);

		ProductMM(X,statespace->F[s],IEzz[t-1][zeta]);
		ProductTransposeMM(Ezz1[t][xi],X,statespace->F[s]);
		AddMM(Ezz1[t][xi],Ezz1[t][xi],statespace->V[s]);
		ForceSymmetric(Ezz1[t][xi]);
	      }

	  // Sets Pxi1[t] 
	  // Uses Pxi[t-1]
	  ProductMV(Pxi1[t],Q,Pxi[t-1]);
	}
      else
	{
	  // Sets IEz[t-1] and IEzz[t-1] 
	  // Uses Ez[t-1], Ezz[t-1], and Pxi[t-1]
	  IntegrateStatesSingleV(IEz_i,Pxi_i,Ez_i,n_zeta,n_s,2);
	  IntegrateStatesSingleM(IEzz_i,Pxi_i,Ezz_i,n_zeta,n_s,2);

	  // s=s(t)
	  for (s=n_s-1; s >= 0; s--)
	    // zeta=zeta(t-1)
	    for (zeta=n_zeta-1; zeta >= 0; zeta--)
	      {
		// xi=xi(t)
		xi=s*n_zeta+zeta;
 
		// Sets Ez1[t][xi] and Ezz1[t][xi]
		// Uses IEz[t-1][xi], and IEzz[t-1][xi]
		ProductMV(Ez1[t][xi],statespace->F[s],IEz_i[zeta]);
		AddVV(Ez1[t][xi],Ez1[t][xi],statespace->b[s]);

		ProductMM(X,statespace->F[s],IEzz_i[zeta]);
		ProductTransposeMM(Ezz1[t][xi],X,statespace->F[s]);
		AddMM(Ezz1[t][xi],Ezz1[t][xi],statespace->V[s]);
		ForceSymmetric(Ezz1[t][xi]);
	      }

	  // Sets Pxi1[t] 
	  // Uses Pxi[t-1]
	  ProductMV(Pxi1[t],Q,Pxi_i);
	}

      // Determine the dimension of y[t]
      for (k=0, i=ny-1; i >= 0; i--) if (y[t][i]) k++;

      if (k > 0)
	{
	  Ey1=CreateVector(k);
	  Eyy1=CreateMatrix(k,k);
	  cy=CreateVector(k);
	  v=CreateVector(k);
	  Y=CreateMatrix(k,nz);
	  T=CreateMatrix(k,k);   
	  K=CreateMatrix(nz,k);

	  for (i=j=0; i < ny; i++)
	    if (y[t][i]) ElementV(cy,j++)=*(y[t][i]);

	  for (s=n_s-1; s >= 0; s--)
	    {
	      a[s]=CreateVector(k);
	      H[s]=CreateMatrix(k,nz);
	      R[s]=CreateMatrix(k,k);

	      for (m=k, i=ny-1; i >= 0; i--)
		if (y[t][i])
		  {
		    m--;
		    ElementV(a[s],m)=ElementV(statespace->a[s],i);
		    for (j=nz-1; j >= 0; j--)
		      ElementM(H[s],m,j)=ElementM(statespace->H[s],i,j);
		    for (n=k, j=ny-1; j >= 0; j--)
		      if (y[t][j]) ElementM(R[s],m,--n)=ElementM(statespace->R[s],i,j);
		  }
	    }

	  if (statespace->NonZeroG)
	    for (s=n_s-1; s >= 0; s--)
	      {
		G[s]=CreateMatrix(nz,k);
		for (n=k, j=ny-1; j >= 0; j--)
		  if (y[t][j])
		    for (n--, i=nz-1; i >= 0; i--)
		      ElementM(G[s],i,n)=ElementM(statespace->G[s],i,j);
	      }

	  for (xi=n_xi-1; xi >= 0; xi--)
	    {
	      // s = s(t)
	      s=xi/n_zeta;

	      // Sets Ey1[t][xi] and Eyy1[t][xi]
	      // Uses Ez1[t][xi] and Ezz1[t][xi]
	      ProductMV(Ey1,H[s],Ez1[t][xi]);
	      AddVV(Ey1,Ey1,a[s]);

	      ProductMM(Y,H[s],Ezz1[t][xi]);
	      ProductTransposeMM(Eyy1,Y,H[s]);
	      AddMM(Eyy1,Eyy1,R[s]);
	      if (statespace->NonZeroG)
		{
		  ProductMM(T,H[s],G[s]);
		  AddTranspose(T);
		  AddMM(Eyy1,Eyy1,T);
		}
	      ForceSymmetric(Eyy1);

	      // Cholesky decomposition of Eyy1[t][xi] (Eyy1[t][xi] = T'*T)
	      if (!CholeskyUT(T,Eyy1))
		{
		  dw_SetTerminalErrors(terminal_errors);
		  dw_SetVerboseErrors(verbose_errors);
		  return 0;
		}
	      else
		{
		  // K = (Ezz1[t][xi]*H[s]' + G[s])*Inverse(T)
		  ProductTransposeMM(K,Ezz1[t][xi],H[s]);
		  if (statespace->NonZeroG) AddMM(K,K,G[s]);
		  ProductInverseMU(K,K,T);

		  // v = ((cy - Ey1)'*Inverse(T))'
		  SubtractVV(v,cy,Ey1);
		  ProductInverseVU(v,v,T);

		  // Sets Ez[t][xi] and Ezz[t][xi]
		  // Uses K, v, Ez1[t][xi], and Ezz1[t][xi]
		  ProductMV(Ez[t][xi],K,v);
		  AddVV(Ez[t][xi],Ez[t][xi],Ez1[t][xi]);

		  ProductTransposeMM(Ezz[t][xi],K,K);
		  SubtractMM(Ezz[t][xi],Ezz1[t][xi],Ezz[t][xi]);
		  ForceSymmetric(Ezz[t][xi]);

		  // Sets L[t](xi)
		  // Uses T and v.  The diagonal elements of T are positive.
		  for (sum=0.0, j=k*k-1; j >= 0; j-=k+1) sum-=log(pElementM(T)[j]);
		  ElementV(L,xi)=sum - 0.5*(LOG2PI*k + DotProduct(v,v));
		}
	    }

	  // Free data storage 
	  if (statespace->NonZeroG)
	    for (s=n_s-1; s >= 0; s--)
	      { FreeMatrix(G[s]); G[s]=(TMatrix)NULL; }

	  for (s=n_s-1; s >= 0; s--)
	    {
	      FreeMatrix(R[s]); R[s]=(TMatrix)NULL;
	      FreeMatrix(H[s]); H[s]=(TMatrix)NULL;
	      FreeVector(a[s]); a[s]=(TVector)NULL;
	    }

	  FreeMatrix(K);
	  FreeMatrix(T); 
	  FreeMatrix(Y);
	  FreeVector(v); 
	  FreeVector(cy);
	  FreeMatrix(Eyy1);
	  FreeVector(Ey1);
	}
      else
	for (xi=n_xi-1; xi >= 0; xi--)
	  {
	    EquateVector(Ez[t][xi],Ez1[t][xi]);
	    EquateMatrix(Ezz[t][xi],Ezz1[t][xi]);
	    ElementV(L,xi)=0.0;
	  }

      // Set Pxi[t]
      // Uses Pxi1[t]
      scale=MINUS_INFINITY;
      for (xi=n_xi-1; xi >= 0; xi--)
	{
	  if (ElementV(Pxi1[t],xi) > 0.0)
	    {
	      scale=AddScaledLogs(1.0,scale,ElementV(Pxi1[t],xi),ElementV(L,xi));
	      ElementV(Pxi[t],xi)=log(ElementV(Pxi1[t],xi)) + ElementV(L,xi);
	    }
	  else
	    ElementV(Pxi[t],xi)=MINUS_INFINITY;
	}

      for (xi=n_xi-1; xi >= 0; xi--)
	if (ElementV(Pxi[t],xi) != MINUS_INFINITY)
	  ElementV(Pxi[t],xi)=exp(ElementV(Pxi[t],xi) - scale);
	else
	  ElementV(Pxi[t],xi)=0.0;
    }

  // Sets IEz[t1] and IEzz[t1] 
  // Uses Ez[t1], Ezz[t1], and Pxi[t1]
  IntegrateStatesSingleV(IEz[t1],Pxi[t1],Ez[t1],n_zeta,n_s,2);
  IntegrateStatesSingleM(IEzz[t1],Pxi[t1],Ezz[t1],n_zeta,n_s,2);

  // Clean up
  FreeMatrix(X);
  FreeVector(L);
  if (statespace->NonZeroG) dw_FreeArray(G);
  dw_FreeArray(R);
  dw_FreeArray(H);
  dw_FreeArray(a);
  dw_FreeArray(IEzz_i);
  dw_FreeArray(IEz_i);

  dw_SetTerminalErrors(terminal_errors);
  dw_SetVerboseErrors(verbose_errors);

  return 1;
}
#undef LOG2PI

/*
   Assumes:
     IR    : horizon x (nvar*nshocks) matrix
     S     : integer array of length horizon
     model : valid pointer to TStateModel/T_MSStateSpace structure
     flag  : equal to IR_STATES or IR_OBSERVABLES

   Returns:
     One upon success and zero otherwise. 

   Results:
     Fills IR with the impulse responses of the variables with respect to the 
     shocks.

   Notes:
     The current value of the parameters implicit in model are used to form the
     impulse responses.  The element in position (t,i+j*nvar) is the response of
     the ith state variable to the jth shock at horizon t.  Horizon 0 is the
     contemporaneous response.

     If flag is equal to IR_STATES then the variables are the state variables and
     the shocks are the fundamental shocks.  If flag is equal to IR_OBSERVABLES
     then the variables are the observables and the shocks are both the 
     fundamental shocks and the measurement error.  The fundamental shocks are 
     ordered first.
*/
int dw_state_space_impulse_response(TMatrix IR, int *S, TStateModel *model, int flag)
{
  TVector *z, y=(TVector)NULL, e;
  T_MSStateSpace *statespace=(T_MSStateSpace*)(model->theta);
  int i, j, t;

  // check that there are shocks and variables
  if (flag & IR_OBSERVABLES)
    { if ((statespace->nepsilon + statespace->nu <= 0) || (statespace->ny <= 0)) return 0; }
  else
    { if ((statespace->nepsilon <= 0) || (statespace->nz <= 0)) return 0; }

  // initialize structures
  if ((statespace->t0 < 0) && !Initialize_MSStateSpace(model)) return 0;
  InitializeMatrix(IR,0.0);

  // create observable vector if necessary
  if (flag & IR_OBSERVABLES) y=CreateVector(statespace->ny);

  // contemporaneous response to fundamental shocks
  if (statespace->nepsilon > 0)
    {
      z=dw_CreateArray_vector(statespace->nepsilon);
      InitializeVector(e=CreateVector(statespace->nepsilon),0.0);
      for (j=statespace->nepsilon-1; j >= 0; j--)
	{
	  ElementV(e,j)=1.0;
	  z[j]=ProductMV((TVector)NULL,statespace->Phiz[S[0]],e);
	  if (flag & IR_STATES)
	    {
	      for (i=statespace->nz-1; i >= 0; i--)
		ElementM(IR,0,i+j*statespace->nz)=ElementV(z[j],i);
	    }
	  else
	    {
	      ProductMV(y,statespace->H[S[0]],z[j]);
	      for (i=statespace->ny-1; i >= 0; i--)
		ElementM(IR,0,i+j*statespace->ny)=ElementV(y,i);
	    }
	  ElementV(e,j)=0.0;
	}
      FreeVector(e);
     }

  // contemporaneous response to measument error shocks
  if ((flag & IR_OBSERVABLES) && (statespace->nu > 0))
    {
      InitializeVector(e=CreateVector(statespace->nu),0.0);
      for (j=statespace->nu-1; j >= 0; j--)
	{
	  ElementV(e,j)=1.0;
	  ProductMV(y,statespace->Phiy[S[0]],e);
	  for (i=statespace->ny-1; i >= 0; i--)
	    ElementM(IR,0,i+(statespace->nepsilon+j)*statespace->ny)=ElementV(y,i);
	  ElementV(e,j)=0.0;
	}
      FreeVector(e);
    }

  // horizon k response to fundamental shocks
  if (statespace->nepsilon > 0)
    {
      for (t=1; t < RowM(IR); t++)
	for (j=statespace->nepsilon-1; j >= 0; j--)
	  {
	    ProductMV(z[j],statespace->F[S[t]],z[j]);
	    if (flag & IR_STATES)
	      {
		for (i=statespace->nz-1; i >= 0; i--)
		  ElementM(IR,t,i+j*statespace->nz)=ElementV(z[j],i);
	      }
	    else
	      {
		ProductMV(y,statespace->H[S[t]],z[j]);
		for (i=statespace->ny-1; i >= 0; i--)
		  ElementM(IR,t,i+j*statespace->ny)=ElementV(y,i);
	      }
	  }
      dw_FreeArray(z);
    }

  // free memory and return success
  FreeVector(y);
  return 1;
}

/*
   Assumes
     VD    : horizon x (nvar*nshocks) matrix
     S     : integer array of length horizon
     model : valid pointer to TStateModel/T_MSStateSpace structure
     flag  : equal to IR_STATES or IR_OBSERVABLES

   Returns:
     One upon success and zero otherwise. 

   Results:
     Fills VD with the variance decompositions of the variables with respect to the 
     shocks.
     
   Notes:
     The element in position (t,j+i*nshocks) of VD is the cummulative contributions
     to the total variance of ith variable to the jth shock at horizon t. Horizon 
     zero is the contemporaneous contribution.

     If flag is equal to IR_STATES then the variables are the state variables and
     the shocks are the fundamental shocks.  If flag is equal to IR_OBSERVABLES
     then the variables are the observables and the shocks are both the 
     fundamental shocks and the measurement error.  The fundamental shocks are 
     ordered first.
*/
int dw_state_space_variance_decomposition(TMatrix VD, int *S,  TStateModel *model, int flag)
{
  T_MSStateSpace *statespace=(T_MSStateSpace*)(model->theta);
  int t, i, j, rtrn=0, nvars, nshocks;
  PRECISION *pIR;
  TMatrix IR=CreateMatrix(RowM(VD),ColM(VD));
  if (rtrn=dw_state_space_impulse_response(IR,S,model,flag))
    {
      // square impulse responses
      for (pIR=pElementM(IR), j=ColM(IR)*RowM(IR)-1; j >= 0; j--)
	pIR[j]*=pIR[j];

      // accumulate square impulse responses
      for (t=1; t < RowM(IR); t++)
	for (j=ColM(IR)-1; j >= 0; j--)
	  ElementM(IR,t,j)+=ElementM(IR,t-1,j);

      // reorders columns
      if (flag & IR_STATES)
	{
	  nvars=statespace->nz;
	  nshocks=statespace->nepsilon;
	}
      else
	{
	  nvars=statespace->ny;
	  nshocks=statespace->nepsilon + statespace->nu;
	}
      for (t=RowM(VD)-1; t >= 0; t--)
	for (i=nvars-1; i >= 0; i--)
	  for (j=nshocks-1; j >= 0; j--)
	    ElementM(VD,t,j+i*nshocks)=ElementM(IR,t,i+j*nvars);
    }
  FreeMatrix(IR);
  return rtrn;
}

/*
   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_MSStateSpace(TMatrix Q, TVector *Pxi, TVector *Pxi1, int *S)
{
  int nstates=RowM(Q), T=dw_DimA(Pxi), t, i, j;
  PRECISION scale, u, s;

  //====== Backward recursion ======
  if ((u=dw_uniform_rnd()) >= (s=ElementV(Pxi[t=T-1],i=nstates-1)))
    while (--i > 0)
      if (u < (s+=ElementV(Pxi[t],i))) break;
  for (t--; t >= 0; t--)
    {
      scale=1.0/ElementV(Pxi1[t+1],j=i);
      i=nstates-1;
      if ((u=dw_uniform_rnd()) >= (s=ElementV(Pxi[t],i)*ElementM(Q,j,i)*scale))
	while (--i > 0)
	  if (u < (s+=ElementV(Pxi[t],i)*ElementM(Q,j,i)*scale)) break;
    }
    
  return 1;
}

/*
   Assumes:
    f        : pointer to valid FILE structure or null pointer
    filename : pointer to null terminated string or null
    model    : pointer to valid TStateModel structure.

   Returns:
    One upon success and zero upon failure.

   Results:
    Writes P(s(t) = i | Y(t), theta, q) - which is the element in row t and 
    column i.

   Notes:
    One of f or filename should be non-null. 
*/
int WriteFilteredProbabilities(FILE *f, char *filename, TStateModel *model)
{
  FILE *f_out;
  int i, t;
  if (!Filter(model->nobs,model)) return 0;
  f_out=f ? f : filename ? fopen(filename,"wt") : (FILE*)NULL;
  if (f_out)
    {
      for (t=1; t < model->nobs; t++)
	{
	  for (i=0; i < model->sv->nbasestates; i++)
	    fprintf(f_out,"%lf ",ProbabilityBaseStateConditionalCurrent(i,t,model));
	  fprintf(f_out,"\n");
	}
      if (f != f_out) fclose(f_out);
      return 1;
    }
  else
    return 0;
}

/*
   Notes:
       xi(t+1) = (s(t+1),zeta(t)) = (zeta(t+1),s(t+1-nlags_encoded))
     and 
       xi(t+1) = s(t+1)*zeta_modulus + zeta(t)
               = zeta(t+1)*nbasestates + s(t+1-nlags_encoded)
*/
int Smooth_MSStateSpace(TStateModel *model)
{
  T_MSStateSpace *statespace=(T_MSStateSpace*)(model->theta);

  if (statespace->smoothed) return 1;

  if (!Filter(statespace->nobs,model) || !ComputeSmoothedProbabilities(model)) return 0;

  // Compute IEz[nobs][zeta]
  IntegrateStatesSingleV(statespace->IEz[statespace->nobs],model->V[statespace->nobs],statespace->Ez[statespace->nobs],
			 statespace->zeta_modulus,statespace->nbasestates,2);

  SmoothMean_MSStateSpace(1,statespace->nobs,statespace->SEz,statespace->ISEz,statespace->Ez1,statespace->Ezz1, 
			  statespace->IEz,statespace->IEzz,model->SP,statespace);

  statespace->smoothed=1;

  return 1;  
}

/*
   Assumes:
     t0   - T > t1 >= t0 >= 0
     t1   - T > t1 >= t0 >= 0
     SPxi - Vector array of length T
     Pxi  - Vector array of length T, Pxi[t](k) ~ P(xi(t)=k | Y(t), theta, q)
     Pxi1 - Vector array of length T, Pxi1[t](k) ~ P(xi(t)=k | Y(t-1), theta, q)
     Q    - nstates x nstates transition matrix

   Returns:
     Returns one upon success and zero upon failure.

   Results:
     Fills SPxi for t0 <= t <= t1 and 0 <= k < nstates.

          SPxi[t][k]=P(xi(t)=k | Y[t0], theta, q)

   Notes:
     Q(i,j) is the probability that xi(t)=i given that xi(t-1)=j.  In general,
     Q could depend on t-1 information.  In this function, we do not allow this.
*/
int SmoothProbabilities_MSStateSpace(int t0, int t1, TVector *SPxi, TVector *Pxi, TVector *Pxi1, TMatrix Q)
{
  int nstates=RowM(Q), t=t1, k;

  //====== Compute Smoothed Probabilities via backward recursion ======
  EquateVector(SPxi[t],Pxi[t]);
  for (t--; t >= t0; t--)
    {
      for (k=nstates-1; k >= 0; k--)
	ElementV(SPxi[t],k)=(ElementV(Pxi1[t+1],k) > 0.0) ? ElementV(SPxi[t+1],k)/ElementV(Pxi1[t+1],k) : 0.0;

      ProductVM(SPxi[t],SPxi[t],Q);

      for (k=nstates-1; k >= 0; k--)
	ElementV(SPxi[t],k)*=ElementV(Pxi[t],k);
    }

  return 1;
}

/*
   Computes 

      SEz[t][k]  ~ E[z(t) | Y(t1), xi(t+1)=k, theta, q] for t0 <= t <  t1
      ISEz[t][j] ~ E[z(t) | Y(t1), zeta(t)=j, theta, q] for t0 <= t <= t1
   
   using

      Ez1(t,xi(t))    ~ E[z(t) | Y(t-1), xi(t), theta, q]                                             t0 <  t <= t1

      Ezz1(t,xi(t))   ~ E[(z(t) - Ezl(t,xi(t)))(z(t) - Ezl(t,xi(t)))' | Y(t-1), xi(t), theta, q]      t0 <  t <= t1

      IEz(t,zeta(t))  ~ E[z(t) | Y(t), zeta(t), theta, q]                                             t0 <= t <= t1

      IEzz(t,zeta(t)) ~ E[(z(t) - Ez(t,zeta(t)))(z(t) - Ez(t,zeta(t)))' | Y(t), zeta(t), theta, q]    t0 <= t <  t1
*/
void SmoothMean_MSStateSpace(int t0, int t1, TVector **SEz, TVector **ISEz, TVector **Ez1, TMatrix **Ezz1, 
			     TVector **IEz, TMatrix **IEzz, TVector *SPxi, T_MSStateSpace *statespace)
{
  TVector z;
  TMatrix G;
  int xi, s, zeta, zeta_t, t=t1;

  // Computes ISEz[t1][zeta]
  // zeta = zeta(t1)
  for (zeta=statespace->zeta_modulus-1; zeta >= 0; zeta--)
    EquateVector(ISEz[t1][zeta],IEz[t1][zeta]);

  z=CreateVector(statespace->nz);
  G=CreateMatrix(statespace->nz,statespace->nz);

  for (t--; t >= t0; t--)
    {
      // xi = xi(t+1)
      for (xi=statespace->nstates-1; xi >= 0; xi--)
	{
	  // s = s(t+1)
	  s=xi/statespace->zeta_modulus;

	  // zeta = zeta(t+1)
	  zeta=xi/statespace->nbasestates;

	  // zeta_t = zeta(t)
	  zeta_t=xi - s*statespace->zeta_modulus;

	  SubtractVV(z,ISEz[t+1][zeta],Ez1[t+1][xi]);

	  // Use GeneralizedInverse(G,Ezz1[t+1][xi]) instead of
	  // InverseProductMV(z,Ezz1[t+1][xi],z) because 
	  // statespace->Ezz1[t+1][xi] can be singular.
	  InverseProductMV(z,Ezz1[t+1][xi],z);
	  //GeneralizedInverse(G,Ezz1[t+1][xi]);
	  //ProductMV(z,G,z);


	  /* Debugging code that prints the condition number of Ezz1[t+1][xi] */
	  /* GeneralizedInverse(G,Ezz1[t+1][xi]); */
	  /* ProductMM(G,G,Ezz1[t+1][xi]); */
	  /* int i; */
	  /* for (i=0; i < statespace->nz; i++) ElementM(G,i,i)-=1.0; */
          /* printf("generalized invers: t=%d  zi=%d  norm=%le\n",t+1,xi,MatrixNorm(G)); */
	  /* InverseProductMM(G,Ezz1[t+1][xi],Ezz1[t+1][xi]); */
	  /* printf("Inverse Product: t=%d  zi=%d  norm=%le\n",t+1,xi,MatrixNorm(G)); */
	  /* ProductInverseMM(G,Ezz1[t+1][xi],Ezz1[t+1][xi]); */
	  /* printf("Product Inverse: t=%d  zi=%d  norm=%le\n",t+1,xi,MatrixNorm(G)); */
	  /* TVector d=CreateVector(statespace->nz); */
	  /* SVD((TMatrix)NULL,d,(TMatrix)NULL,Ezz1[t+1][xi]); */
	  /* printf("t=%d  zi=%d  condition=%le/%le\n",t+1,xi,ElementV(d,0),ElementV(d,statespace->nz-1)); */
	  /* FreeVector(d); */

	  TransposeProductMV(z,statespace->F[s],z);
	  ProductMV(z,IEzz[t][zeta_t],z);
	  AddVV(SEz[t][xi],IEz[t][zeta_t],z);	      
	}

      IntegrateStatesSingleV(ISEz[t],SPxi[t+1],SEz[t],statespace->nbasestates,statespace->zeta_modulus,1);
    }
  
  FreeVector(z);
  FreeMatrix(G);
}

/*
   Ez[t][v] = E[z(t) | I(t), v(t)=v]     for 1 <= t <= T
   Ez1[t][v] = E[z(t) | I(t+1), v(t+1)=v]  for 1 <= t <= T-1
   a = nbasestates^(k+1)

   where 
 
     v(t) = (s(t), s(t-1), ... , s(t-k)) 
          = s(t)*h^k + s(t-1)*h^(k-1) + ... + s(t-(k-1))*h + s(t-k)

   for some k >= 0 and h=nbasestates.  I(t) is the time t information set.  If 
   I(t) = Y[t], then the result will be the filtered shocks.  If I(t) = Y[T], 
   then the result will be the smoothed shocks.  The information I(t) is 
   determined by the values of the arguments Ez and Ez1.

   Returns

     Eepsilon[t][v] = E[epsilon(t) | I(t), v(t)=v] for 2 <= t <= T
*/
TVector** ComputeFundamentalShocks(TStateModel *model, TVector **Ez, TVector **Ez1, int a)
{
  T_MSStateSpace *statespace=(T_MSStateSpace*)(model->theta);
  TMatrix *I;
  TVector z, **Eepsilon=(TVector**)NULL;
  int s, v, t, b=a/statespace->nbasestates;
  if (statespace->nepsilon > 0)
    {
      z=CreateVector(statespace->nz);
      I=dw_CreateArray_matrix(statespace->nbasestates);
      for (s=statespace->nbasestates-1; s >= 0; s--)
	I[s]=GeneralizedInverse((TMatrix)NULL,statespace->Phiz[s]);

      Eepsilon=dw_CreateRectangularArray_vector(statespace->nobs+1,a);
      for (t=statespace->nobs; t > 1; t--)
	// v = v(t)
	for (v=a-1; v >= 0; v--)
	  {
	    // s = s(t)
	    s=v/b;
	    ProductMV(z,statespace->F[s],Ez1[t-1][v]);
	    AddVV(z,z,statespace->b[s]);
	    SubtractVV(z,Ez[t][v],z);
	    Eepsilon[t][v]=ProductMV((TVector)NULL,I[s],z);

	    // test projection
            /* UpdateProductMV(1.0,z,-1.0,statespace->Phiz[s],Eepsilon[t][v]); */
	    /* if (Norm(z) > 0.0000001) */
	    /*   { */
	    /* 	printf("Projection error:  t=%d  v=%d  norm=%lf\n",t,v,Norm(z)); */
	    /* 	if (t < statespace->nobs) printf("norm(Ez[t][v] - Ez1[t][v]) = %lf\n",Norm(SubtractVV(z,Ez[t][v],Ez1[t][v]))); */
	    /* 	TVector y=ProductMV((TVector)NULL,statespace->H[0],Ez[t][0]); */
            /*     AddVV(y,statespace->a[0],y); */
            /*     SubtractVV(y,statespace->y[t],y); */
	    /* 	printf("norm(y(t) - a - H*Ez[t]) = %lf\n",Norm(y)); */
	    /* 	FreeVector(y); */
	    /* 	ProductMV(z,statespace->F[0],Ez[t][0]); */
	    /* 	AddVV(z,statespace->b[0],z); */
	    /* 	if (t < statespace->nobs) */
	    /* 	  { */
	    /* 	    UpdateProductMV(1.0,z,1.0,statespace->Phiz[0],Eepsilon[t+1][0]); */
	    /* 	    printf("norm(Ez[t+1] - b - F*Ez[t] - Phyz*Eepsilon[t]) = %lf\n",Norm(SubtractVV(z,Ez[t+1][0],z))); */
	    /* 	  } */
	    /* 	getchar(); */
	    /*   } */
	  }
      dw_FreeArray(I);
      FreeVector(z);
    }
  return Eepsilon;
}

/*
   ISEz[t][v] = E[z(t) | I(t), v(t)=v]   for 1 <= t <= T
   a = nbasestates^(k+1)

   where 
 
     v(t) = (s(t), s(t-1), ... , s(t-k)) 
          = s(t)*h^k + s(t-1)*h^(k-1) + ... + s(t-(k-1))*h + s(t-k)

   for some k >= 0 and h=nbasestates. I(t) is some information set which could 
   depend on t.

   Returns

     Eu[t][v] = E[u(t) | I(t), v(t)=v]
*/
TVector** ComputeMeasurmentErrors(TStateModel *model, TVector **Ez, int a)
{
  T_MSStateSpace *statespace=(T_MSStateSpace*)(model->theta);
  TMatrix *I;
  TVector y, **Eu=(TVector**)NULL;
  int s, v, t, b=a/statespace->nbasestates;
  if (statespace->nu > 0)
    {
      y=CreateVector(statespace->ny);
      I=dw_CreateArray_matrix(statespace->nbasestates);
      for (s=statespace->nbasestates-1; s >= 0; s--)
	I[s]=GeneralizedInverse((TMatrix)NULL,statespace->Phiy[s]);
	  
      Eu=dw_CreateRectangularArray_vector(statespace->nobs+1,a);
      for (t=statespace->nobs; t > 0; t--)
	// v = v(t)
	for (v=a-1; v >= 0; v--)
	  {
	    // s = s(t)
	    s=v/b;
	    ProductMV(y,statespace->H[s],Ez[t][v]);
	    AddVV(y,y,statespace->a[s]);
	    SubtractVV(y,statespace->y[t],y);
	    ProductMV(Eu[t][v],I[s],y);
	  }
      dw_FreeArray(I);
      FreeVector(y);
    }
  return Eu;
}

/*******************************************************************************/
/********************************** Utilities **********************************/
/*******************************************************************************/
/*
   Assumes:
     X : m x m matrix

   Results:
     X = X + X'
*/
static void AddTranspose(TMatrix X)
{
  int j, k, m;
  PRECISION *pX;
  if (!X || (RowM(X) != ColM(X))) return;
  for (m=RowM(X), pX=pElementM(X), j=m*m-2; j > 0; j+=k-1)
    for (k=j-m+1; k >= 0; j--, k-=m)
      pX[j]=pX[k]=pX[j]+pX[k];
} 

/*
   Assumes:
     X : n x m matrix

   Returns:
     -1 if X is not square and max(fabs(X(i,j) - X(j,i))) otherwise.
*/
static PRECISION IsSymmetric(TMatrix X)
{
  int i, j;
  PRECISION max;
  if (!X || (RowM(X) != ColM(X))) return -1.0;

  for (max=0.0, i=RowM(X)-1; i > 0; i--)
    for (j=i-1; j >= 0; j--)
      if (max < fabs(ElementM(X,i,j) - ElementM(X,j,i)))
	max=fabs(ElementM(X,i,j) - ElementM(X,j,i));

  return max;
}

/*******************************************************************************/
/******************************** Test Routines ********************************/
/*******************************************************************************/
void Check_FilteredInfo(TStateModel *model, PRECISION tol)
{
  T_MSStateSpace *statespace=(T_MSStateSpace*)(model->theta);
  int s, zeta, xi, t, tmax, smax, zetamax, ximax;
  TVector y, **Eu;
  PRECISION max, norm;

  if (!Filter(statespace->nobs,model))
    {
      printf("Unable to filter\n");
      return;
    }

  y=CreateVector(statespace->ny);

  // Check a[s(t)] + H[s(t)]*Ez[t][xi(t)] + Phiy[s(t)]*Eu[t][xi(t)] = y[t] 
  printf("checking a[s(t)] + H[s(t)]*Ez[t][xi(t)] + Phiy[s(t)]*Eu[t][xi(t)] = y[t]\n");
  max=0.0;
  Eu=ComputeMeasurmentErrors(model,statespace->Ez,statespace->nstates);
  for (t=1; t <= statespace->nobs; t++)
    {
      for (xi=statespace->nstates-1; xi >= 0; xi--)
	{
	  s=model->sv->lag_index[xi][0];
	  SubtractVV(y,statespace->a[s],statespace->y[t]);
	  UpdateProductMV(1.0,y,1.0,statespace->H[s],statespace->Ez[t][xi]);
	  if (statespace->nu > 0) UpdateProductMV(1.0,y,1.0,statespace->Phiy[s],Eu[t][xi]);
	  if ((norm=Norm(y)) > max)
	    {
	      max=norm;
	      tmax=t;
	      ximax=xi;
	      smax=s;
	    }
	  if (norm > tol)
	    {
	      printf("difference = %le  (t=%d  xi(t)=%d  s(t)=%d)\n",norm,t,xi,s);
#ifdef DEBUG_PAUSE
              printf("Press <Enter> to continue");
	      getchar();
#endif
	    }
	}
    }
  printf("Max difference = %le  (t=%d  xi(t)=%d  s(t)=%d)\n",max,tmax,ximax,smax);
  dw_FreeArray(Eu);

  // Check a[s(t)] + H[s(t)]*IEz[t][zeta(t)] = y[t] 
  printf("checking a[s(t)] + H[s(t)]*IEz[t][zeta(t)] + Phiy[s(t)]*Eu[t][zeta(t)] = y[t]\n");
  max=0.0;
  // Compute IEz[nobs][zeta]
  IntegrateStatesSingleV(statespace->IEz[statespace->nobs],model->V[statespace->nobs],statespace->Ez[statespace->nobs],
			 statespace->zeta_modulus,statespace->nbasestates,2);
  Eu=ComputeMeasurmentErrors(model,statespace->IEz,statespace->zeta_modulus);
  for (t=1; t <= statespace->nobs; t++)
    {
      for (xi=statespace->nstates-1; xi >= 0; xi--)
	{
	  s=model->sv->lag_index[xi][0];
	  zeta=xi/statespace->nbasestates;
	  SubtractVV(y,statespace->a[s],statespace->y[t]);
	  UpdateProductMV(1.0,y,1.0,statespace->H[s],statespace->IEz[t][zeta]);
	  if (statespace->nu > 0) UpdateProductMV(1.0,y,1.0,statespace->Phiy[s],Eu[t][zeta]);
	  if ((norm=Norm(y)) > max)
	    {
	      max=norm;
	      tmax=t;
	      zetamax=xi;
	      smax=s;
	    }
	  if (norm > tol)
	    {
	      printf("difference = %le  (t=%d  zeta(t)=%d  s(t)=%d)\n",norm,t,zeta,s);
#ifdef DEBUG_PAUSE
              printf("Press <Enter> to continue");
	      getchar();
#endif
	    }
	}
    }
  printf("Max difference = %le  (t=%d  zeta(t)=%d  s(t)=%d)\n",max,tmax,zetamax,smax);
  dw_FreeArray(Eu);

  FreeVector(y);
}

void Check_SmoothedInfo(TStateModel *model, PRECISION tol)
{
  T_MSStateSpace *statespace=(T_MSStateSpace*)(model->theta);
  int i, k, t, tmax, zetamax, smax, ximax, s, zeta, xi;
  TVector y, z, **ISEz1, **SEepsilon, **SEu, q1, q2;
  PRECISION max, norm;

  if (!Smooth_MSStateSpace(model) || !ComputeSmoothedProbabilities(model))
    {
      printf("Unable to smooth\n");
      return;
    }

  // Compute ISEz1[t][zeta] = E[z(t) | Y(T), zeta(t+1)=zeta]  1 <= t < nobs
  ISEz1=dw_CreateRectangularArray_vector(statespace->nobs+1,statespace->zeta_modulus);
  for (t=statespace->nobs-1; t > 0; t--)
    IntegrateStatesSingleV(ISEz1[t],model->SP[t+1],statespace->SEz[t],statespace->zeta_modulus,statespace->nbasestates,2);

  // Compute smoothed fundamental shocks
  SEepsilon=ComputeFundamentalShocks(model,statespace->ISEz,ISEz1,statespace->zeta_modulus);

  // Compute smoothed measurment errors
  SEu=ComputeMeasurmentErrors(model,statespace->ISEz,statespace->zeta_modulus);

  // Allocate workspace
  z=CreateVector(statespace->nz);
  y=CreateVector(statespace->ny);

  ///////////////////////////////////////////////////////////////////////////////
  // Tests xi(t) = s(t-k) + h*s(t-k+1) + ... + (h^k)*s(t)
  printf("checking lagged encoding\n");
  for (xi=statespace->nstates-1; xi >= 0; xi--)
    {
      for (k=0, i=statespace->nlags_encoded; i >= 0; i--)
	k+=model->sv->lag_index[xi][i]*(int)pow(statespace->nbasestates,statespace->nlags_encoded-i);
      if (k != xi) 
	printf("Error: %d = xi(t) != s(t-k) + h*s(t-k+1) + ... + (h^k)*s(t) = %d\n",xi,k);
    }
  ///////////////////////////////////////////////////////////////////////////////

  ///////////////////////////////////////////////////////////////////////////////
  // Test integeration of states
  q1=CreateVector(statespace->zeta_modulus);
  q2=CreateVector(statespace->zeta_modulus);
  printf("testing integration of SP[t+1] against SP[t]\n");
  max=0.0;
  for (t=statespace->nobs-1; t >= 0; t--)
    {
      IntegrateStatesSingle(q1,model->SP[t+1],statespace->nbasestates,statespace->zeta_modulus,1);
      IntegrateStatesSingle(q2,model->SP[t],statespace->zeta_modulus,statespace->nbasestates,2);
      SubtractVV(q1,q1,q2);
      if ((norm=Norm(q1)) > max)
	{
	  max=norm;
	  tmax=t;
	}
      if (norm > tol)
	{
	  printf("difference = %le  (t=%d)",norm,t);
	}
    }
  printf("Max difference = %le  (t=%d)\n",max,tmax);
  FreeVector(q2);
  FreeVector(q1);
  ///////////////////////////////////////////////////////////////////////////////

  if (statespace->nu == 0)
    {
      if (statespace->nlags_encoded > 0)
	{
	  ///////////////////////////////////////////////////////////////////////////////
	  // Check y[t] = a[s(t)] + H[s(t)]*ISEz[t][zeta(t)]
	  printf("checking y[t] = a[s(t)] + H[s(t)]*ISEz[t][zeta(t)]\n");
	  max=0.0;
	  for (t=1; t <= statespace->nobs; t++)
	    // zeta = zeta(t)
	    for (zeta=statespace->zeta_modulus-1; zeta >= 0; zeta--)
	      {
		// s = s(t)
		s=model->sv->lag_index[statespace->nbasestates*zeta][0];
		SubtractVV(y,statespace->a[s],statespace->y[t]);
		UpdateProductMV(1.0,y,1.0,statespace->H[s],statespace->ISEz[t][zeta]);
		if ((norm=Norm(y)) > max)
		  {
		    max=norm;
		    tmax=t;
		    zetamax=zeta;
		    smax=s;
		  }
		if (norm > tol)
		  {
		    printf("difference = %le (t=%d  zeta(t)=%d  s(t)=%d)\n",norm,t,zeta,s);
#ifdef DEBUG_PAUSE
		    printf("Press <Enter> to continue");
		    getchar();
#endif
		  }
	      }
	  printf("Max difference = %le  (t=%d  zeta(t)=%d  s(t)=%d)\n",max,tmax,zetamax,smax);
	  ///////////////////////////////////////////////////////////////////////////////

	  ///////////////////////////////////////////////////////////////////////////////
	  // Check y[t] = a[s(t)] + H[s(t)]*SEz[t][xi(t+1)]
	  printf("checking y[t] = a[s(t)] + H[s(t)]*SEz[t][xi(t+1)]\n");
	  max=0.0;
	  for (t=1; t < statespace->nobs; t++)
	    // xi = xi(t+1)
	    for (xi=statespace->nstates-1; xi >= 0; xi--)
	      {
		// s = s(t)
		s=model->sv->lag_index[xi][1];
		SubtractVV(y,statespace->a[s],statespace->y[t]);
		UpdateProductMV(1.0,y,1.0,statespace->H[s],statespace->SEz[t][xi]);
		if ((norm=Norm(y)) > max)
		  {
		    max=norm;
		    tmax=t;
		    ximax=xi;
		    smax=s;
		  }
		if (norm > tol)
		  {
		    printf("difference = %le (t=%d  xi(t+1)=%d  s(t)=%d)\n",norm,t,xi,s);
#ifdef DEBUG_PAUSE
		    printf("Press <Enter> to continue");
		    getchar();
#endif
		  }
	      }
	  printf("Max difference = %le  (t=%d  xi(t+1)=%d  s(t)=%d)\n",max,tmax,ximax,smax);
	  ///////////////////////////////////////////////////////////////////////////////
	}

    }
  else
    {
      ///////////////////////////////////////////////////////////////////////////////
      // Check y[t] = a[s(t)] + H[s(t)]*ISEz[t][zeta(t)] + Phiy[s(t)]*SEu[t][zeta(t)]
      printf("checking y[t] = a[s(t)] + H[s(t)]*ISEz[t][zeta(t)] + Phiy[s(t)]*SEu[t][zeta(t)]\n");
      max=0.0;
      for (t=1; t <= statespace->nobs; t++)
	// zeta = zeta(t)
	for (zeta=statespace->zeta_modulus-1; zeta >= 0; zeta--)
	  {
	    // s = s(t)
	    s=model->sv->lag_index[statespace->nbasestates*zeta][0];
	    SubtractVV(y,statespace->a[s],statespace->y[t]);
	    UpdateProductMV(1.0,y,1.0,statespace->H[s],statespace->ISEz[t][zeta]);
	    if (statespace->nu > 0) UpdateProductMV(1.0,y,1.0,statespace->Phiy[s],SEu[t][zeta]);
	    if ((norm=Norm(y)) > max)
	      {
		max=norm;
		tmax=t;
		zetamax=zeta;
		smax=s;
	      }
	    if (norm > tol)
	      {
		printf("difference = %le (t=%d  zeta(t)=%d  s(t)=%d)\n",norm,t,zeta,s);
#ifdef DEBUG_PAUSE
		printf("Press <Enter> to continue");
		getchar();
#endif
	      }
	  }
      printf("Max difference = %le  (t=%d  zeta(t)=%d  s(t)=%d)\n",max,tmax,zetamax,smax);
      ///////////////////////////////////////////////////////////////////////////////
    }

  ///////////////////////////////////////////////////////////////////////////////
  // Check y[t] = a[s(t)] + Phiy[s(t)]*SEu[t][zeta(t)]
  //                 + H[s(t)]*(b[s(t)] + F[s(t)]*ISEz[t-1][zeta(t)] + Phiz[s(t)]*SEepsilon[t][zeta(t)]) 
  printf("checking y[t] = a[s(t)] + Phiy[s(t)]*SEu[t][zeta(t)]\n");
  printf("                   + H[s(t)]*(b[s(t)] + F[s(t)]*ISEz[t-1][zeta(t)] + Phiz[s(t)]*SEepsilon[t][zeta(t)])\n");
  max=0.0;
  for (t=2; t <= statespace->nobs; t++)
    // zeta = zeta(t)
    for (zeta=statespace->zeta_modulus-1; zeta >= 0; zeta--)
      {
	// s = s(t)
	s=model->sv->lag_index[statespace->nbasestates*zeta][0];
	ProductMV(z,statespace->F[s],ISEz1[t-1][zeta]);
	AddVV(z,z,statespace->b[s]);
	if (statespace->nepsilon > 0) UpdateProductMV(1.0,z,1.0,statespace->Phiz[s],SEepsilon[t][zeta]);
	ProductMV(y,statespace->H[s],z);
	AddVV(y,y,statespace->a[s]);
	if (statespace->nu > 0) UpdateProductMV(1.0,y,1.0,statespace->Phiy[s],SEu[t][zeta]);
	SubtractVV(y,y,statespace->y[t]);
	if ((norm=Norm(y)) > max)
	  {
	    max=norm;
	    tmax=t;
	    zetamax=zeta;
	    smax=s;
	  }
	if (norm > tol)
	  {
	    printf("dif = %le  %% difference = %le (t=%d  zeta(t)=%d  s(t)=%d)\n",norm,norm/Norm(statespace->y[t]),t,zeta,s);
#ifdef DEBUG_PAUSE
	    printf("Press <Enter> to continue");
	    getchar();
#endif
	  }
      }
  printf("Max difference = %le  (t=%d  zeta(t)=%d  s(t)=%d)\n",max,tmax,zetamax,smax);
  ///////////////////////////////////////////////////////////////////////////////

  ///////////////////////////////////////////////////////////////////////////////
  // Check ISEz[t][zeta(t)] = b[s(t)] + F[s(t)]*ISEz1[t-1][zeta(t)] + Phiz[s(t)]*SEepsilon[t][zeta(t)]
  printf("checking ISEz[t][zeta(t)] = b[s(t)] + F[s(t)]*ISEz1[t-1][zeta(t)] + Phiz[s(t)]*SEepsilon[t][zeta(t)]\n");
  max=0.0;
  for (t=2; t <= statespace->nobs; t++)
    // zeta = zeta(t)
    for (zeta=statespace->zeta_modulus-1; zeta >= 0; zeta--)
      {
	// s = s(t)
	s=model->sv->lag_index[statespace->nbasestates*zeta][0];
	SubtractVV(z,statespace->b[s],statespace->ISEz[t][zeta]);
	UpdateProductMV(1.0,z,1.0,statespace->F[s],ISEz1[t-1][zeta]);
	if (statespace->nepsilon > 0) UpdateProductMV(1.0,z,1.0,statespace->Phiz[s],SEepsilon[t][zeta]);
	if ((norm=Norm(z)) > max)
	  {
	    max=norm;
	    tmax=t;
	    zetamax=zeta;
	    smax=s;
	  }
	if (norm > tol)
	  {
	    printf("dif = %le  %% difference = %le (t=%d  zeta(t)=%d  s(t)=%d)\n",norm,
		   norm/Norm(statespace->ISEz[t][zeta]),t,zeta,s);
#ifdef DEBUG_PAUSE
	    printf("Press <Enter> to continue");
	    getchar();
#endif
	  }
      }
  printf("Max difference = %le  (t=%d  zeta(t)=%d  s(t)=%d)\n",max,tmax,zetamax,smax);
  ///////////////////////////////////////////////////////////////////////////////

  FreeVector(y);
  FreeVector(z);
  dw_FreeArray(ISEz1);
  dw_FreeArray(SEepsilon);
  dw_FreeArray(SEu);
}
