#include <math.h>    
#include "trigfun.h"  
/* 
 *  
 *            Syntax  [sys, x0] = dbl_pend(t,x,u,flag,X0,m1,l1,m2,l2,gc) 
*/ 
/* 
* The following #define is used to specify the name of this S-Function. 
*/ 
#define S_FUNCTION_NAME dbl_pend 
/* 
* need to include simstruc.h for the definition of the SimStruct and 
* its associated macro definitions. 
*/ 
#include "simstruc.h" 
  
/* Defines for work arrays */ 
#define Br(j) RWork[j] 
#define Ar(i,j) RWork[2+ i+j*2] 
#define Cr(i,j) RWork[6+ i+j*2] 
/* 
 * 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; 
{ 
    ssSetNumContStates(    S, 4);      /* number of continuous states */ 
    ssSetNumDiscStates(    S, 0);      /* number of discrete states */ 
    ssSetNumInputs(        S, 2 );      /* number of inputs */ 
    ssSetNumOutputs(       S, 2 );      /* number of outputs */ 
    ssSetDirectFeedThrough(S, 0);      /* direct feedthrough flag */ 
    ssSetNumSampleTimes(   S, 1);      /* number of sample times */ 
    ssSetNumInputArgs(     S, 6);      /* number of input arguments */ 
    ssSetNumRWork(         S, 10);      /* number of real work vector elements */ 
    ssSetNumIWork(         S, 0);      /* number of integer work vector elements */ 
    ssSetNumPWork(         S, 0);      /* number of pointer work vector elements */ 
     /* 
     * if there aren't the correct number of parameters, just return, simulink.c will 
     * flag the error 
     */ 
    if (ssGetNumArgs(S) != 6) 
       return; 
    if (mxGetM(ssGetArg(S,0)) != 4  || mxGetN(ssGetArg(S,0)) !=1)  { 
        ssSetErrorStatus( S, "X0 must have dimensions: 4 by 1"); 
        return; 
    } 
    if (mxGetM(ssGetArg(S,1)) != 1  || mxGetN(ssGetArg(S,1)) !=1)  { 
        ssSetErrorStatus( S, "m1 must have dimensions: 1 by 1"); 
        return; 
    } 
    if (mxGetM(ssGetArg(S,2)) != 1  || mxGetN(ssGetArg(S,2)) !=1)  { 
        ssSetErrorStatus( S, "l1 must have dimensions: 1 by 1"); 
        return; 
    } 
    if (mxGetM(ssGetArg(S,3)) != 1  || mxGetN(ssGetArg(S,3)) !=1)  { 
        ssSetErrorStatus( S, "m2 must have dimensions: 1 by 1"); 
        return; 
    } 
    if (mxGetM(ssGetArg(S,4)) != 1  || mxGetN(ssGetArg(S,4)) !=1)  { 
        ssSetErrorStatus( S, "l2 must have dimensions: 1 by 1"); 
        return; 
    } 
    if (mxGetM(ssGetArg(S,5)) != 1  || mxGetN(ssGetArg(S,5)) !=1)  { 
        ssSetErrorStatus( S, "gc must have dimensions: 1 by 1"); 
        return; 
    } 
} 
/* 
 * 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) 
#ifdef MATLAB_MEX_FILE    /* Is this file being compiled as a MEX-file? */ 
    double *x0; 
    SimStruct *S; 
{ 
    int i,imax; 
    double *X0pr; 
/* Set initial value for state vector */ 
    X0pr = mxGetPr(ssGetArg(S,0));  
    imax = mxGetM(ssGetArg(S,0)); 
    for(i=0;i<imax;i++){ 
        x0[i] = X0pr[i]; 
    } 
} 
#else    /* compiled for dSpace board? */ 
    real_T *x0; 
    SimStruct *S; 
{ 
    int_T i,imax; 
    real_T *X0pr; 
/* Set initial value for state vector */ 
    X0pr = mxGetPr(ssGetArg(S,0));  
    imax = mxGetM(ssGetArg(S,0)); 
    for(i=0;i<imax;i++){ 
        x0[i] = X0pr[i]; 
    } 
} 
#endif 
/* 
 * 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) 
#ifdef MATLAB_MEX_FILE    /* Is this file being compiled as a MEX-file? */ 
    double *dx, *x, *u; 
    SimStruct *S; 
    int tid; 
{  
    int n; 
    double s1, s2, s3, s4, s5, s6, s7, s8, s9, s10; 
    double t1, t2, t3, t4, t5, t6, t7, t8; 
    double *RWork = ssGetRWork(S); 
    int i,j; 
    char s='L'; 
    int nrhs, LDA, info; 
    double *m1pr = mxGetPr(ssGetArg(S,1)); 
    double *l1pr = mxGetPr(ssGetArg(S,2)); 
    double *m2pr = mxGetPr(ssGetArg(S,3)); 
    double *l2pr = mxGetPr(ssGetArg(S,4)); 
    double *gcpr = mxGetPr(ssGetArg(S,5)); 
#else    /* compiled for dSpace board? */ 
    real_T *dx; 
  const real_T *x, *u; 
    SimStruct *S; 
    int_T tid; 
{  
    int_T n; 
    real_T s1, s2, s3, s4, s5, s6, s7, s8, s9, s10; 
    real_T t1, t2, t3, t4, t5, t6, t7, t8; 
    real_T *RWork = ssGetRWork(S); 
    int_T i,j; 
    char s='L'; 
    int_T nrhs, LDA, info; 
    real_T *m1pr = mxGetPr(ssGetArg(S,1)); 
    real_T *l1pr = mxGetPr(ssGetArg(S,2)); 
    real_T *m2pr = mxGetPr(ssGetArg(S,3)); 
    real_T *l2pr = mxGetPr(ssGetArg(S,4)); 
    real_T *gcpr = mxGetPr(ssGetArg(S,5)); 
#endif 
    n = 2; 
    t1=pow(l2pr[0],2.); 
    t2=pow(l1pr[0],2.); 
    t3=cos(x[3]); 
    t4=sin(x[3]); 
    t5=sin(x[2]); 
    t6=pow(t4,2.); 
    t7=pow(t3,2.); 
    t8=sin(x[2]+x[3]); 
    Ar(0,0)=m1pr[0]*t2+m2pr[0]*(t1+2.0000000*l1pr[0]*l2pr[0]*t3+t2*(t6+t7)); 
    Ar(1,0)=m2pr[0]*(t1+l1pr[0]*l2pr[0]*t3); 
    Ar(1,1)=m2pr[0]*t1; 
    Br(0)=-(gcpr[0]*(l1pr[0]*(m1pr[0]*t5+m2pr[0]*t5)+l2pr[0]*m2pr[0]*t8))+u[0]; 
    Br(1)=-(gcpr[0]*l2pr[0]*m2pr[0]*t8)+u[1]; 
    Cr(0,1)=-(l1pr[0]*l2pr[0]*m2pr[0]*t4*(-2.0000000*x[0]-x[1])); 
    Cr(1,0)=-(l1pr[0]*l2pr[0]*m2pr[0]*t4*(x[0]+0.5000000*x[1])); 
    Cr(1,1)=0.5000000*l1pr[0]*l2pr[0]*m2pr[0]*t4*x[0]; 
/* Compute C*p + F */ 
    for (i=0;i<=1;++i){ 
        for(j=0;j<=1;++j){ 
               Br(i) +=  Cr(i,j)*x[j]; 
       } 
  } 
    /* Solve linear system A*X = C*P + F  */ 
    nrhs = 1; 
    LDA = n; 
    dposv_(&s,&n,&nrhs,&Ar(0,0),&LDA,&Br(0),&LDA,&info); 
    /* reassign results */ 
    dx[0] = Br(0); 
    dx[1] = Br(1); 
    dx[2] =     x[0]; 
    dx[3] =     x[1]; 
} 
/* 
 * 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) 
#ifdef MATLAB_MEX_FILE    /* Is this file being compiled as a MEX-file? */ 
    double *y, *x, *u; 
    SimStruct *S; 
    int tid; 
{ 
    double s1, s2, s3, s4, s5, s6, s7, s8, s9, s10; 
    double t1, t2, t3, t4; 
    double *m1pr = mxGetPr(ssGetArg(S,1)); 
    double *l1pr = mxGetPr(ssGetArg(S,2)); 
    double *m2pr = mxGetPr(ssGetArg(S,3)); 
    double *l2pr = mxGetPr(ssGetArg(S,4)); 
    double *gcpr = mxGetPr(ssGetArg(S,5)); 
#else    /* compiled for dSpace board? */ 
    real_T *y; 
   const real_T *x, *u; 
    SimStruct *S; 
    int_T tid; 
{ 
    real_T s1, s2, s3, s4, s5, s6, s7, s8, s9, s10; 
    real_T t1, t2, t3, t4; 
    real_T *m1pr = mxGetPr(ssGetArg(S,1)); 
    real_T *l1pr = mxGetPr(ssGetArg(S,2)); 
    real_T *m2pr = mxGetPr(ssGetArg(S,3)); 
    real_T *l2pr = mxGetPr(ssGetArg(S,4)); 
    real_T *gcpr = mxGetPr(ssGetArg(S,5)); 
#endif 
    t1=cos(x[3]); 
    t2=cos(x[2]); 
    t3=sin(x[3]); 
    t4=sin(x[2]); 
    y[0]=l1pr[0]*t4+l2pr[0]*(t2*t3+t1*t4); 
    y[1]=-(l1pr[0]*t2)+l2pr[0]*(-(t1*t2)+t3*t4); 
} 
/* 
 * 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) 
#ifdef MATLAB_MEX_FILE    /* Is this file being compiled as a MEX-file? */ 
    double  *x, *u; 
    SimStruct *S; 
    int tid; 
#else    /* compiled for dSpace board? */ 
    real_T *x; 
   const real_T *u; 
    SimStruct *S; 
    int_T tid; 
#endif 
{ 
} 
/* 
 * 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; 
{ 
} 
#include "linsolv.c"    /* includes linsolve routine dposv_ linear equation solver*/ 
#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 
