/*
 * SMMULTR   A SIMULINK real matrix multiply block.
 * DSP Blockset S-Function to perform a real matrix multiply.
 * This SIMULINK S-function multiplies two matrices;
 * it is called by the Matrix Multiply blocks in the DSP Blockset.
 * This is called a "Vector-Matrix Multiply" because the inputs are
 * matrices passed in as column vectors.
 *
 * Syntax:  [sys, x0] = sbuffer(t,x,u,flag,Asize,Bcols)
 * 
 *  The calling sequence is:
 *      [sys, x0] = sfunmul(t,x,u,flag,Asize,Bcols)
 *  t, x, y, and flag are the standard SIMULINK S-function parameters.
 *  Asize = [rows cols] is the size of the FIRST (A) matrix.  Bcols is
 *  the number of cols of the SECOND (B) matrix.  The number of rows
 *  of B is determined by the input size.
 *
 *  When FLAG = 3, the input u is assumed to be both matrices A and B
 *  concatenated together ([A(:); B(:)] in MATLAB syntax).
 *  The function output is A*B.
 *
 *  Copyright 1995-2000 The MathWorks, Inc.
 *  $Revision: 1.13 $  $Date: 2000/06/14 14:28:00 $
 */

#define S_FUNCTION_NAME smmultr

#include "dsp_sim.h"

#define ASIZE    (ssGetArg(S,0))
#define BCOLS    (ssGetArg(S,1))
#define NUM_ARGS (2)

#ifdef MATLAB_MEX_FILE
#define MDL_CHECK_PARAMETERS
static void mdlCheckParameters(SimStruct *S) {
    const char *msg = NULL;

    if ((mxGetM(ASIZE) != 1) || (mxGetN(ASIZE) != 2)) {
        msg = "Size of U must be specified as a 2-element row vector";
        goto ERROR_EXIT;
    }
    if ((mxGetM(BCOLS) != 1) || (mxGetN(BCOLS) != 1)) {
        msg = "Columns in V must be specified as a scalar";
        goto ERROR_EXIT;
    }

ERROR_EXIT:
    if (msg != NULL) {
        ssSetErrorStatus(S,msg);
    }
}
#endif


static void mdlInitializeSizes(SimStruct *S)
{
    ssSetNumSFcnParams(S, NUM_ARGS);

#if defined(MATLAB_MEX_FILE)
    if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) {
        mdlCheckParameters(S);
        if (ssGetErrorStatus(S) != NULL) {
            return;
        }
    } else {
        return; /* Simulink will report a parameter mismatch error */
    }
#endif

    {
        const int_T ii = (int_T)(mxGetPr(ASIZE)[0]);  /* Arows */
        const int_T jj = (int_T)(mxGetPr(ASIZE)[1]);  /* Acols = Brows */
        const int_T kk = (int_T)(mxGetPr(BCOLS)[0]);  /* Bcols */

        ssSetNumInputs(        S, jj*(ii+kk)); /* # of elements in A and B matrices */
        ssSetNumOutputs(       S, ii*kk);      /* # of elements in C=A*B matrix */
        ssSetDirectFeedThrough(S, 1);
        ssSetNumSampleTimes(   S, 1);
        ssSetOptions(          S, SS_OPTION_EXCEPTION_FREE_CODE |
                                  SS_OPTION_USING_ssGetUPtrs);
    }
}


static void mdlInitializeSampleTimes(SimStruct *S)
{
    /*
     * Blocks that are continuous in nature have a single
     * sample time of 0.0
     */
    ssSetSampleTimeEvent(S, 0, INHERITED_SAMPLE_TIME);
    ssSetOffsetTimeEvent(S, 0, 0.0);
}


static void mdlInitializeConditions(real_T *x0, SimStruct *S)
{
#ifdef MATLAB_MEX_FILE
    const int_T ii = (int_T) (mxGetPr(ASIZE)[0]); /* Arows */
    const int_T jj = (int_T) (mxGetPr(ASIZE)[1]); /* Acols = Brows */
    const int_T kk = (int_T) (mxGetPr(BCOLS)[0]); /* Bcols */
    /*
     *  Check that input matrix size is appropriate:
     *    Input is pair of columnized matrices concatenated together,
     *    u = [A(:) B(:)], with size (Arows*Acols+Brows*Bcols)-by-1
     */
    if(ssGetNumInputs(S) != jj*(ii+kk)) {
        ssSetErrorStatus(S, "Size of input is not consistent with block parameters.");
        return;
    }
#endif
}


static void mdlOutputs(real_T *y, const real_T *x, const real_T *u, 
                       SimStruct *S, int_T tid)
{
  /* Inputs are u, Outputs are y */
    const int_T ii   = (int_T)(mxGetPr(ASIZE)[0]);  /* Arows */
    const int_T jj   = (int_T)(mxGetPr(ASIZE)[1]);  /* Acols = Brows */
    const int_T kk   = (int_T)(mxGetPr(BCOLS)[0]);  /* Bcols */
    int_T       k;

    /* Setup input matrix pointers: */
    UPtrsType A = ssGetUPtrs(S);          /* Start of A matrix */
    UPtrsType B = A + ii*jj;  /* Start of B matrix */

      /* Compute real matrix multiply.
       *  Leaves A intact.  Destroys B and C.
       *  Algorithm initializes output memory to zero
       *    before adding to it.
       */
    for(k=kk; k-- != 0; B+=jj) {
        UPtrsType A1 = A;
        int_T     i;

        for(i=ii; i-- != 0; A1++, y++) {
            UPtrsType A2 = A1;
            UPtrsType B1 = B;
            int_T     j;

            for(*y=0, j=jj; j-- != 0; A2+=ii) {
                *y += (**A2) * (**(B1++));
            }
        }
    }
}


static void mdlUpdate(real_T *x, const real_T *u, SimStruct *S, int_T tid)
{
}

static void mdlDerivatives(real_T *dx, const real_T *x, const real_T *u, 
                           SimStruct *S, int_T tid)
{
}

static void mdlTerminate(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