/*
 * STSPACE   Example mex file S-function for state-space system.
 *
 *           Implements a set of state-space equations.
 *           You can turn this into a new block by using the
 *           S-function block and Mask facility.
 *
 *           This example MEX-file performs the same function
 *           as the built-in State-space block. This is an
 *           example of a MEX-file where the number of inputs,
 *           outputs, and states is dependent on the parameters
 *           passed in from the workspace.
 *
 *           Use this as a template for other MEX-file systems.
 *
 *           Syntax  [sys, x0] = stspace(t,x,u,flag,A,B,C,D,X0)
 *
 * Andrew Grace  Oct 11, 1990
 * Copyright (c) 1990-94 The MathWorks, Inc.
 * All Rights Reserved
 */

/*
 * The following #define is used to specify the name of this S-Function.
 */

#define S_FUNCTION_NAME stspace

#include <stdio.h>    /* needed for declaration of sprintf */

#ifdef MATLAB_MEX_FILE
#include "mex.h"      /* needed for declaration of mexErrMsgTxt */
#endif

/*
 * need to include simstruc.h for the definition of the SimStruct and
 * its associated macro definitions.
 */

#include "simstruc.h"

/*
 * Defines for easy access to the A,B,C,D matrices that are passed in
 */

#define A	ssGetArg(S,0)
#define B	ssGetArg(S,1)
#define C	ssGetArg(S,2)
#define D	ssGetArg(S,3)
#define X0	ssGetArg(S,4)


/*
 * mdlInitializeSizes - initialize the sizes array
 *
 * The sizes array is used by SIMULINK to determine the S-function block's
 * characteristics (number of inputs, outputs, states, etc.).
 */

static void mdlInitializeSizes(S)
    SimStruct *S;
{
    /*
     * Set-up size information, only do this if there are 5 input args
     */
    
#define NSTATES		mxGetM(A)
#define NINPUTS		mxGetN(B)
#define NOUTPUTS	mxGetM(C)
    
    if (ssGetNumArgs(S) == 5) {
	ssSetNumContStates(    S, NSTATES);    /* number of continuous states */
	ssSetNumDiscStates(    S, 0);          /* number of discrete states */
	ssSetNumInputs(        S, NINPUTS);    /* number of inputs */
	ssSetNumOutputs(       S, NOUTPUTS);   /* number of outputs */
	ssSetDirectFeedThrough(S, 0);          /* direct feedthrough flag */
	ssSetNumSampleTimes(   S, 1);          /* number of sample times */
	ssSetNumInputArgs(     S, 5);          /* number of input arguments */
	ssSetNumRWork(         S, 0);          /* number of real work vector elements */
	ssSetNumIWork(         S, 0);          /* number of integer work vector elements */
	ssSetNumPWork(         S, 0);          /* number of pointer work vector elements */
	
	/*
	 * some defensive programming
	 */
	
	if (mxGetN(A) != NSTATES) {
#ifdef MATLAB_MEX_FILE
	    mexErrMsgTxt("A matrix must be square");
#endif
	}
	
	if (mxGetM(B) != NSTATES) {
#ifdef MATLAB_MEX_FILE
	    mexErrMsgTxt("B matrix not dimensioned correctly");
#endif
	}
	
	if (mxGetN(C) != NSTATES) {
#ifdef MATLAB_MEX_FILE
	    mexErrMsgTxt("C matrix not dimensioned correctly");
#endif
	}
	
	if (mxGetM(D) != NOUTPUTS || mxGetN(D) != NINPUTS) {
#ifdef MATLAB_MEX_FILE
	    mexErrMsgTxt("D matrix not dimensioned correctly");
#endif
	}
	
	if ((mxGetM(X0) != 0) && (mxGetM(X0) != NSTATES)) {
#ifdef MATLAB_MEX_FILE
	    mexErrMsgTxt("X0 matrix not dimensioned correctly");
#endif
	}
	
#undef NSTATES
#undef NINPUTS
#undef NOUTPUTS
    } else {
#ifdef MATLAB_MEX_FILE
	char err_msg[256];
	sprintf(err_msg, "Wrong number of input arguments passed to S-function MEX-file.\n\
%d input arguments were passed in when expecting %d input arguments.\n", ssGetNumArgs(S) + 4, 5 + 4);
	mexErrMsgTxt(err_msg);
#endif
    }
}

/*
 * mdlInitializeSampleTimes - initialize the sample times array
 *
 * This function is used to specify the sample time(s) for your S-function.
 * If your S-function is continuous, you must specify a sample time of 0.0.
 * Sample times must be registered in ascending order.
 */

static void mdlInitializeSampleTimes(S)
    SimStruct *S;
{
    ssSetSampleTimeEvent(S, 0, 0.0);
    ssSetOffsetTimeEvent(S, 0, 0.0);
}

/*
 * mdlInitializeConditions - initialize the states
 *
 * In this function, you should initialize the continuous and discrete
 * states for your S-function block.  The initial states are placed
 * in the x0 variable.  You can also perform any other initialization
 * activities that your S-function may require.
 */

static void mdlInitializeConditions(x0, S)
    double *x0;
    SimStruct *S;
{
    int i, nstates;

    /*
     * if the initial condition parameter is not an empty matrix,
     * then use it to set up the initial conditions, otherwise,
     * set the intial conditions to all 0.0
     */
    
    nstates = ssGetNumContStates(S);
    if (mxGetM(X0) != 0) {
	double *pr;
	
	pr = mxGetPr(X0);
	for (i = 0; i < nstates; i++)
	    *x0++ = *pr++;
    } else {
	for (i = 0; i < nstates; i++)
	    *x0++ = 0.0;
    }
}

/*
 * mdlOutputs - compute the outputs
 *
 * In this function, you compute the outputs of your S-function
 * block.  The outputs are placed in the y variable.
 */

static void mdlOutputs(y, x, u, S, tid)
    double *y, *x, *u;
    SimStruct *S;
    int tid;
{
    int i,j; 
    double accum;
    double *cpr, *dpr;
    int nstates, ninputs, noutputs;
    
    /*
     * Matrix Multiply:	y = Cx + Du
     */
    
    cpr = mxGetPr(C);
    dpr = mxGetPr(D);
    nstates = ssGetNumContStates(S);
    ninputs = ssGetNumInputs(S);
    noutputs = ssGetNumOutputs(S);
    
    for (i = 0; i < noutputs; i++) {
	accum = 0.0;
	
	/* Cx */
	for (j = 0; j < nstates; j++)
	    accum += cpr[i + noutputs*j] * x[j]; 
	
	/* Du */
	for (j = 0; j < ninputs; j++)
	    accum += dpr[i + noutputs*j] * u[j];
	
	y[i] = accum;
    }
}

/*
 * mdlUpdate - perform action at major integration time step
 *
 * This function is called once for every major integration time step.
 * Discrete states are typically updated here, but this function is useful
 * for performing any tasks that should only take place once per integration
 * step.
 */

static void mdlUpdate(x, u, S, tid)
    double *x, *u;
    SimStruct *S;
    int tid;
{
}

/*
 * mdlDerivatives - compute the derivatives
 *
 * In this function, you compute the S-function block's derivatives.
 * The derivatives are placed in the dx variable.
 */

static void mdlDerivatives(dx, x, u, S, tid)
    double *dx, *x, *u;
    SimStruct *S;
    int tid;
{
    int i, j;
    double accum;
    double *apr, *bpr;
    int nstates, ninputs;
    
    apr = mxGetPr(A);
    bpr = mxGetPr(B);
    nstates = ssGetNumContStates(S);
    ninputs = ssGetNumInputs(S);
    
    /*
     * Matrix Multiply:	dx = Ax + Bu
     */
    
    for (i = 0; i < nstates; i++) {
	accum = 0.0;
	
	/* Ax */
	for (j = 0; j < nstates; j++)
	    accum += apr[i + nstates*j] * x[j]; 
	
	/* Bu */
	for (j = 0; j < ninputs; j++)
	    accum += bpr[i + nstates*j] * u[j];
	
	dx[i] = accum;
    }
}

/*
 * mdlTerminate - called when the simulation is terminated.
 *
 * In this function, you should perform any actions that are necessary
 * at the termination of a simulation.  For example, if memory was allocated
 * in mdlInitializeConditions, this is the place to free it.
 */

static void mdlTerminate(S)
    SimStruct *S;
{
}

#ifdef	MATLAB_MEX_FILE    /* Is this file being compiled as a MEX-file? */
#include "simulink.c"      /* MEX-file interface mechanism */
#else
#include "cg_sfun.h"       /* Code generation registration function */
#endif
