/*
 * SELMATH2  S-Function for two input elementary math functions.
 * DSP Blockset S-Function for two input elementary math functions.
 * This SIMULINK S-function computes functions of the two input vectors;
 * it is called by some of the two input elementary math blocks in the 
 * DSP Blockset, including atan2, pow, hypot.
 *
 *  Copyright 1995-2000 The MathWorks, Inc.
 *  $Revision: 1.9 $  $Date: 2000/05/05 20:16:09 $
 */

#define S_FUNCTION_NAME selmath2

#include <math.h>
#include <string.h>   /* strcmp() */

#include "simstruc.h"  /* SimStruct, etc */

#define NUMINPUTARGS 1
#define FCN_NAME     ssGetArg(S, 0)

/*
 * hypot_local - compute sqrt(x^2 + y^2) for real x and y inputs
 *
 *      This routine calculates the quantity sqrt(a^2 + b^2)
 *      without underflow or overflow and takes into account
 *      IEEE math conventions.  As long as the result of the
 *      function is representable as a valid real_T, then this
 *      algorithm will eliminate underflow and overflow.
 *
 *      This function is provided because hypot is not ANSI.  Although 
 *      most compilers include it, the MetroWerks PPC and TI C40 
 *      compilers do not.
 */
real_T hypot_local(real_T a, real_T b)
{
/*
 * This code is here for platforms which don't have a hypot or have one
 * which overflows or underflows.
 */
        real_T t;                       /* scales a & b */

   /* Case 1: a > b ==> t = b / a < 1.0 */

        if (fabs(a) > fabs(b)) {
                t = b/a;
                return (fabs(a)*sqrt(1+t*t));

   /* Case 2: a <= b ==> t = a / b <= 1.0 */

        } else {
                if (b == 0.0) return (0.0);
                t = a/b;
                return (fabs(b)*sqrt(1+t*t));
        }
}


static void mdlInitializeSizes(SimStruct *S)
{
#ifdef MATLAB_MEX_FILE
    mexWarnMsgTxt("Obsolete blocks calling the CMEX S-Function 'selmath2' have\n"
                  "been detected.  If you wish to upgrade this model, replace\n"
                  "all elementary math blocks (such as atan2, hypot, and pow) with\n"
                  "new versions found in the block library:\n"
                  "        'dsplib/Math Functions/Elementary Math'\n"
                  "If you wish to disable this warning message, type\n"
                  "'warning off' at the MATLAB command line.\n\n");
#endif
    ssSetNumContStates(    S, 0);                 /* number of continuous states */
    ssSetNumDiscStates(    S, 0);                 /* number of discrete states */
    ssSetNumInputs(        S, DYNAMICALLY_SIZED); /* number of inputs */
    ssSetNumOutputs(       S, DYNAMICALLY_SIZED); /* number of outputs */
    ssSetDirectFeedThrough(S, 1);                 /* direct feedthrough flag */
    ssSetNumSampleTimes(   S, 1);                 /* number of sample times */
    ssSetNumInputArgs(     S, NUMINPUTARGS);      /* number of input arguments */
    ssSetNumRWork(         S, 0);                 /* number of real work vector elements */
    ssSetNumIWork(         S, 0);                 /* number of integer work vector elements */
    ssSetNumPWork(         S, 1);                 /* number of pointer work vector elements */
}


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


static void mdlInitializeConditions(real_T *x0, SimStruct *S)
{
    typedef real_T (*fcn) (const real_T, const real_T);
    fcn *PWork = (fcn *)ssGetPWork(S);
    char fcnName[16];

    /*
     * get the function name from the input argument, bail out
     * if for some reason mxGetString can't convert it
     */

#ifdef MATLAB_MEX_FILE
    if (mxGetString(FCN_NAME, fcnName, sizeof(fcnName)) != 0) {
	mexErrMsgTxt("Error in function name specification.");
    }
#else    
    /* in case of code generation, don't bother with error checking: */
    /* (works around bug in mxGetString as defined in cg_matrx.h)    */
    mxGetString(FCN_NAME, fcnName, sizeof(fcnName));
#endif

    /*
     * translate the name into a function pointer
     */
    if (strcmp(fcnName, "atan2") == 0)
        *PWork = (fcn) atan2;
    else if (strcmp(fcnName, "pow") == 0)
        *PWork = (fcn)pow;
    else if (strcmp(fcnName, "hypot") == 0)
        *PWork = (fcn) hypot_local;
#ifdef MATLAB_MEX_FILE
    else
        mexErrMsgTxt("Error in function name specification.");
#endif
}


static void mdlOutputs(real_T *y, const real_T *x, const real_T *u, 
                       SimStruct *S, int_T tid)
{
    /*
     * get the input width, the function to evaluate, and set up a pointer
     * to point to the second input vector
     */
    int width = ssGetNumInputs(S) / 2;
    typedef real_T (*fcn) (const real_T, const real_T);
    fcn dfcn = ((fcn *)ssGetPWork(S))[0];

    const real_T *u2 = u + width;

    /*
     * evaluate the function given the two inputs
     */
    while(width-- != 0) {
      *y++ = (dfcn)(*u++, *u2++);
    }
}


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