#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <mex.h>

#include "matrix.h"
#include "version.h"
#include "ltpda_ssmsim.h"

#define DEBUG 0
/*
 * A mex file to propagate an input signal with the given SS model
 *
 * M Hewitson  19-08-10
 *
 * $Id: ltpda_ssmsim.c,v 1.4 2010/08/23 11:56:09 ingo Exp $
 */


/* 
 * function [y,lx] = ltpda_ssmsim(lastX, A.', Coutputs.', Cstates.', Baos.', Daos.', input);
 */
void  mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
  
  /* outputs */
  double  *y, *yptr;
  
  /* inputs */
  double *input, *iptr;
  double *Daos, *Dptr;
  double *Baos, *Bptr;
  double *Cstates;
  double *Coutputs, *Coptr;
  double *A, *Aptr;
  double *lastX;
  double *SSini;
  double *tmpX;
  
  mwSize Ninputs, Nsamples, Nstates, Nstatesout, Noutputs;
  int kk,jj,ll;
  int mm,nn;
  int ki;  
  
  
  /* parse input functions */
  
  if( (nrhs == 0) || (nlhs == 0) )
  {
    print_usage(VERSION);
  }
  
  if( (nrhs == 7) && (nlhs == 2) )/* let's go */
  {    
    /*----------------- set inputs*/
    SSini    = mxGetPr(prhs[0]);
    A        = mxGetPr(prhs[1]);
    Coutputs = mxGetPr(prhs[2]);
    Cstates  = mxGetPr(prhs[3]);
    Baos     = mxGetPr(prhs[4]);
    Daos     = mxGetPr(prhs[5]);
    input    = mxGetPr(prhs[6]); 
    
    Ninputs    = mxGetM(prhs[4]);
    Nsamples   = mxGetN(prhs[6]);
    Nstates    = mxGetM(prhs[1]);
    Nstatesout = mxGetM(prhs[3]);
    Noutputs   = mxGetN(prhs[2]);    

    #if DEBUG
    mexPrintf("Ninputs: %d\n", Ninputs);
    mexPrintf("Nsamples: %d\n", Nsamples);
    mexPrintf("Nstates: %d\n", Nstates);
    mexPrintf("Nstatesout: %d\n", Nstatesout);
    mexPrintf("Noutputs: %d\n", Noutputs);
    
    mexPrintf("N Coutputs: %d\n", mxGetNumberOfElements(prhs[2]));  
    
    mexPrintf("input: %dx%d\n", mxGetM(prhs[6]), mxGetN(prhs[6]));
    mexPrintf("D: %dx%d\n", mxGetN(prhs[5]), mxGetM(prhs[5]));    
    #endif
            
    /* output y */
    plhs[0] = mxCreateDoubleMatrix(Noutputs, Nsamples, mxREAL);
    y = mxGetPr(plhs[0]);
    
    /* output state vector*/
    plhs[1] = mxCreateDoubleMatrix(Nstates, 1, mxREAL);
    lastX = mxGetPr(plhs[1]);
    
    tmpX  = (double*)calloc(Nstates, sizeof(double));
    memcpy(lastX, SSini, Nstates*sizeof(double));
    
    /* do the business */
    yptr = &(y[0]);
    for (kk=0; kk<Nsamples; kk++) {
      
      ki = kk*Ninputs;
      
      /* observation equation */
      Coptr = &(Coutputs[0]);
      Dptr  = &(Daos[0]);
      for (jj=0; jj<Noutputs; jj++) {
        *yptr = 0.0;
        for (ll=0; ll<Nstates; ll++) {
          *yptr += *Coptr * lastX[ll] ;
          Coptr++;
        }        
        iptr = &(input[ki]);
        for (ll=0; ll<Ninputs; ll++) {
          *yptr += *Dptr * (*iptr);
          Dptr++;
          iptr++;
        }
        yptr++;
      }  
            
      /* state propagation */
      memcpy(tmpX, lastX, Nstates*sizeof(double));
      Bptr = &(Baos[0]);
      Aptr = &(A[0]);
      for (jj=0; jj<Nstates; jj++) {      
        lastX[jj] = 0;
        for (ll=0; ll<Nstates; ll++) {
          lastX[jj] += *Aptr * tmpX[ll]; 
          Aptr++;
        }        
        iptr = &(input[ki]);
        for (ll=0; ll<Ninputs; ll++) {
          lastX[jj] += *Bptr * (*iptr);
          Bptr++;
          iptr++;
        }
      }  
      
      
    } /* end sample loop */
    
    
    free(tmpX);
    
    
  }
  else
  {
    print_usage(VERSION);
  }
}

void print_usage(char *version)
{
  mexPrintf("ltpda_ssmsim version %s\n", version);
  mexPrintf("  usage:    [y,lx] = ltpda_ssmsim(lastX, A.', Coutputs.', Cstates.', Baos.', Daos.', input);");
  mexErrMsgTxt("### incorrect usage");
}