/* $Revision: 1.16 $ */
/*
 * V34TRANS   A Simulink example for v34 transmission
 *
 *  Syntax:  [sys, x0] = v34trans(t,x,u,flag,rate,ShMp)
 *
 * rate is a 12 element vector, which contains:
 * rate = [rate_s, rate_r,    rate_s_j, rate_s_p, rate_s_n, 
 *         rate_b, rate_q,    rate_K,   rate_M,   rate_M_exp,
 *         rate_L, rate_L_exp];
 * see V34param for the details of the rate parameter.
 *
 * ShMp is a four column matrix
 * ShMp(:, 1) contains g2
 * ShMp(:, 2) contains g4
 * ShMp(:, 3) contains g8
 * ShMp(:, 4) contains z8
 *
 * Wes Wang  Jan. 24, 1996
 * Copyright 1996-2000 The MathWorks, Inc.
 */
#define S_FUNCTION_NAME v34trans

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

#include "simstruc.h"
#include "tmwtypes.h"

/* For RTW */
#if defined(RT) || defined(NRT)  
#undef  mexPrintf
#define mexPrintf printf
#endif

/* Defines for easy access of the input parameters */
#define NUM_ARGS   2
#define RATE     ssGetArg(S,0)
#define ShMp       ssGetArg(S,1)
                        
/*
 * mdlInitializeSizes - called to initialize the sizes array stored in
 *                      the SimStruct.  The sizes array defines the
 *                      characteristics (number of inputs, outputs,
 *                      states, etc.) of the S-Function.
 */

static void mdlInitializeSizes(SimStruct *S)
{
    int_T  rate_s = (int_T) mxGetPr(RATE)[0];
    int_T  rate_r = (int_T) mxGetPr(RATE)[1];
    int_T  rate_j = (int_T) mxGetPr(RATE)[2];
    int_T  rate_p = (int_T) mxGetPr(RATE)[3];
    int_T  rate_b = (int_T) mxGetPr(RATE)[5];
    int_T  rate_q = (int_T) mxGetPr(RATE)[6];
    int_T  rate_K = (int_T) mxGetPr(RATE)[7];
    int_T  rate_M = (int_T) mxGetPr(RATE)[8];

    /*
     * Set-up size information.
     */ 
/*    
if (0) {
    char_T    err_msg[256];
    sprintf(err_msg, "Size initial, rate_s = %d; rate_r = %d; rate_j = %d; rate_p = %d;\n", rate_s, rate_r, rate_j, rate_p);
    mexPrintf(err_msg);
    sprintf(err_msg, "rate_b = %d; rate_q = %d; rate_K = %d; rate_M = %d;\n", rate_b, rate_q, rate_K, rate_M);
    mexPrintf(err_msg);
    sprintf(err_msg, "ShMp_N= %d, ShMp_M=%d;\n", mxGetN(ShMp), mxGetM(ShMp));
    mexPrintf(err_msg);    
}
*/
    if (ssGetNumArgs(S) == NUM_ARGS) {
      ssSetNumContStates(    S, 0);
      ssSetNumDiscStates(    S, 0);
      ssSetNumInputs(        S, rate_b+2);
      ssSetNumOutputs(       S, rate_q + 1 + 3);
      ssSetDirectFeedThrough(S, 1);
      ssSetNumInputArgs(     S, NUM_ARGS);
      ssSetNumSampleTimes(   S, 1);
      ssSetNumRWork(         S, 0);
      /* 1 last updata
       * 1 last trigger output
       * 1 current output counter
       * 23 scramble
       * 8 mijk number
       * scrambled b input data
       */
      ssSetNumIWork(         S, 1 + 1 + 1 + 23 + 8 + rate_b);
      ssSetNumPWork(         S, 0);
    } else {
#ifdef MATLAB_MEX_FILE
      char_T 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, NUM_ARGS + 4);
      mexErrMsgTxt(err_msg);
#endif
    }
}

/*
 * mdlInitializeSampleTimes - initializes the array of sample times stored in
 *                            the SimStruct associated with this S-Function.
 */

static void mdlInitializeSampleTimes(SimStruct *S)
{
    /*
     * Note, blocks that are continuous in nature should have a single
     * sample time of 0.0.
     */

    ssSetSampleTimeEvent(S, 0, INHERITED_SAMPLE_TIME);
    ssSetOffsetTimeEvent(S, 0, FIXED_IN_MINOR_STEP_OFFSET);
}

/*
 * mdlInitializeConditions - initializes the states for the S-Function
 */

static void mdlInitializeConditions(real_T *x0, SimStruct *S)
{
    int_T  rate_s = (int_T) mxGetPr(RATE)[0];
    int_T  rate_r = (int_T) mxGetPr(RATE)[1];
    int_T  rate_j = (int_T) mxGetPr(RATE)[2];
    int_T  rate_p = (int_T) mxGetPr(RATE)[3];
    int_T  rate_b = (int_T) mxGetPr(RATE)[5];
    int_T  rate_q = (int_T) mxGetPr(RATE)[6];
    int_T  rate_K = (int_T) mxGetPr(RATE)[7];
    int_T  rate_M = (int_T) mxGetPr(RATE)[8];
    
    int_T *last_updata = ssGetIWork(S);
    int_T *last_output = ssGetIWork(S) + 1;
    int_T *out_count   = ssGetIWork(S) + 2;
    int_T *scramble    = ssGetIWork(S) + 3;
    int_T *mijk        = ssGetIWork(S) + 3 + 23;
    int_T *data_scrm   = ssGetIWork(S) + 3 + 23 + 8;
    
    int_T i;
/*
if (0) {
    char_T    err_msg[256];
    sprintf(err_msg, "Parame initial rate_s = %d; rate_r = %d; rate_j = %d; rate_p = %d;\n", rate_s, rate_r, rate_j, rate_p);
    mexPrintf(err_msg);
    sprintf(err_msg, "rate_b = %d; rate_q = %d; rate_K = %d; rate_M = %d;\n", rate_b, rate_q, rate_K, rate_M);
    mexPrintf(err_msg);
    sprintf(err_msg, "ShMp_N=%d , ShMp_M=%d;\n", mxGetN(ShMp), mxGetM(ShMp));
    mexPrintf(err_msg);    
}
*/
    *last_updata = 0;
    *last_output = 0;
    *out_count   = 0;
    
    for (i = 0; i < 23; i++)
        scramble[i] = 0;
    for (i = 0; i < 8; i++)
        mijk[i] = 0;
    for (i = 0; i < rate_b; i++)
        data_scrm[i] = 0;
}

/*
 * mdlOutputs - computes the outputs of the S-Function
 */

static void mdlOutputs(real_T *y, const real_T *x, const real_T *u, SimStruct *S, int_T tid)
{
    int_T  rate_s = (int_T) mxGetPr(RATE)[0];
    int_T  rate_r = (int_T) mxGetPr(RATE)[1];
    int_T  rate_j = (int_T) mxGetPr(RATE)[2];
    int_T  rate_p = (int_T) mxGetPr(RATE)[3];
    int_T  rate_b = (int_T) mxGetPr(RATE)[5];
    int_T  rate_q = (int_T) mxGetPr(RATE)[6];
    int_T  rate_K = (int_T) mxGetPr(RATE)[7];
    int_T  rate_M = (int_T) mxGetPr(RATE)[8];
    
    int_T *last_updata = ssGetIWork(S);
    int_T *last_output = ssGetIWork(S) + 1;
    int_T *out_count   = ssGetIWork(S) + 2;
    int_T *scramble    = ssGetIWork(S) + 3;
    int_T *mijk        = ssGetIWork(S) + 3 + 23;
    int_T *data_scrm   = ssGetIWork(S) + 3 + 23 + 8;

    if ((u[rate_b] > 0) && (*last_updata == 0)) {
        /* begin the major updating work. */
        real_T  *g2 = mxGetPr(ShMp);
        real_T  *g4 = mxGetPr(ShMp) + 8 *(rate_M-1)+1;
        real_T  *g8 = mxGetPr(ShMp) + 16*(rate_M-1)+2;
        real_T  *z8 = mxGetPr(ShMp) + 24*(rate_M-1)+3;

        long R[8];
        long A, B, C, D, E, F, G, H, R_bef, R_now, V_bef, V_now;
        int_T  i, j;
#ifdef MATLAB_MEX_FILE
if (0) {
    char_T    err_msg[256];
    sprintf(err_msg, "Output rate_s = %d; rate_r = %d; rate_j = %d; rate_p = %d;\n", rate_s, rate_r, rate_j, rate_p);
    mexPrintf(err_msg);
    sprintf(err_msg, "rate_b = %d; rate_q = %d; rate_K = %d; rate_M = %d;\n", rate_b, rate_q, rate_K, rate_M);
    mexPrintf(err_msg);
    sprintf(err_msg, "ShMp_N=%d, ShMp_M=%d;\n", mxGetN(ShMp), mxGetM(ShMp));
    mexPrintf(err_msg);
    sprintf(err_msg, "\n\n\n");
    mexPrintf(err_msg);        
}
#endif
        /* define the maximum size vector */
      if (1) {
        /*scramble */
        for (i = 0; i < rate_b; i++) {
            data_scrm[i] = ((int_T)u[i] + scramble[17] + scramble[22]) % 2;
            for (j = 22; j > 0; j--)
                scramble[j] = scramble[j-1];
            scramble[0] = data_scrm[i];
		}
	  } else {
        for (i = 0; i < rate_b; i++)
            data_scrm[i] = (int_T)u[i];	    
	  }
		/*parser */
        /* data_scrm[0:K-1] Si1 -- SiK                  -- Six
         * data_scrm[K:K+2] I1i0, I2i0, I3i0            -- Ixij
         * data_scrm[K+3:K+3+q-1] Qi001, ..., Qi00q     -- Qijkx
         * data_scrm[K+3+q:K+3+2*q-1] Qi011, ..., Qi01q -- Qijkx
         * data_scrm[K+3+2*q:K+3+2*q+2] I1i1, I2i1, I3i1
         * data_scrm[K+6+2*q:K+6+3*q-1] Qi101, ..., Qi10q
         * data_scrm[K+6+3*q:K+6+4*q-1] Qi111, ..., Qi11q
         * data_scrm[K+6+4*q:K+6+4*q+2] I1i2, I2i2, I3i2         
         * data_scrm[K+9+4*q:K+9+5*q-1] Qi201, ..., Qi20q
         * data_scrm[K+9+5*q:K+9+6*q-1] Qi211, ..., Qi21q
         * data_scrm[K+9+6*q:K+9+6*q+2] I1i3, I2i3, I3i3
         * data_scrm[K+12+6*q:K+12+7*q-1] Qi301, ..., Qi30q
         * data_scrm[K+12+7*q:K+12+8*q-1] Qi311, ..., Qi31q
         */
        
        /* Shell Mapper */
        /* calculate A, B, C, D, E, F, G, H. */
        for (i = 0; i< 8; i++) {
           R[i] = 0;
        }
        A = 0; B = 0; C = 0; D = 0; E = 0; F = 0; G = 0;;
         
        /* A*/
        R[0] = data_scrm[rate_K - 1];
        i = rate_K - 2;
        while (i >= 0) {
            R[0] = R[0] * 2 + data_scrm[i];
            i--;
        }
        A = 0;
        while ((int_T)z8[A] <= R[0]) {
            A = A + 1;;
        }
        A = A - 1;
#ifdef MATLAB_MEX_FILE
if (0) {
    char_T    err_msg[256];
    sprintf(err_msg, "A = %d; ", A);
    mexPrintf(err_msg);
}
#endif
        /* B */
        V_bef = 0;
        R_bef = (int_T)z8[A];
        V_now = 0;
        R_now = R_bef;
        while ((R[0] - R_now >= 0) & (A - V_now >= 0)) {
            R_bef = R_now;
            V_bef = V_now;
            V_now = V_now + 1;;
            R_now = (int_T)z8[A];
            for (i = 0; i < V_now; i++) {
                R_now = R_now + (int_T)(g4[i] * g4[A - i]);
            }
        }
        if (A < V_now) {
            R[1] = R[0] - R_bef;
            B = V_bef;            
        } else if (R[0] - R_now >= 0) {
            R[1] = R[0] - R_now;
            B = V_now;
        } else {
            R[1] = R[0] - R_bef;
            B = V_bef;
        }
#ifdef MATLAB_MEX_FILE
if (0) {
    char_T    err_msg[256];
    sprintf(err_msg, "B = %d; ", B);
    mexPrintf(err_msg);
}
#endif
        /* C */
        R[2] = R[1] % (int_T)g4[B];
        R[3] = (R[1] - R[2]) / (int_T)g4[B];

        V_bef = 0;
        R_bef = 0;
        V_now = 0;
        R_now = R_bef;
        while ((R[2] - R_now >= 0) & (B - V_now >= 0)) {
            R_bef = R_now;
            V_bef = V_now;
            V_now = V_now + 1;;
            R_now = 0;
            for (i = 0; i < V_now; i++) {
                R_now = R_now + (int_T)(g2[i] * g2[B - i]);
            }
        }
        if (B < V_now) {
            R[4] = R[2] - R_bef;
            C = V_bef;            
		} else if (R[2] - R_now >= 0) {
            R[4] = R[2] - R_now;
            C = V_now;
        } else {
            R[4] = R[2] - R_bef;
            C = V_bef;
        }
#ifdef MATLAB_MEX_FILE
if (0) {
    char_T    err_msg[256];
    sprintf(err_msg, "C = %d; ", C);
    mexPrintf(err_msg);
}
#endif
		/* D */
        V_bef = 0;
        R_bef = 0;
        V_now = 0;
        R_now = R_bef;
        while ((R[3] - R_now >= 0) & (A - B - V_now >= 0)) {
            R_bef = R_now;
            V_bef = V_now;
            V_now = V_now + 1;
            R_now = 0;
            for (i = 0; i < V_now; i++) {
                R_now = R_now + (int_T)(g2[i] * g2[A - B - i]);
            }
        }
        if (A-B < V_now) {
            R[5] = R[3] - R_bef;
            D = V_bef;
		} else if (R[3] - R_now >= 0) {
            R[5] = R[3] - R_now;
            D = V_now;
        } else {
            R[5] = R[3] - R_bef;
            D = V_bef;
        }
#ifdef MATLAB_MEX_FILE
if (0) {
    char_T    err_msg[256];
    sprintf(err_msg, "D = %d; ", D);
    mexPrintf(err_msg);
}		
#endif		
        /* E */
        E = R[4] % (int_T)g2[C];

        /* F */
        F = (R[4] - E) / (int_T)g2[C];

        /* G */
        G = R[5] % (int_T)g2[D];

        /* H */
        H = (R[5] - G) / (int_T)g2[D];

        /* calculate mijk[0-7] */
        /* mi0k */
        if (C < rate_M) {
            mijk[0] = E;
            mijk[1] = C - mijk[0];
        } else {
            mijk[1] = rate_M - 1 - E;
            mijk[0] = C - mijk[1];
        }

        /* mi1k */
        if ((B - C) < rate_M) {
            mijk[2] = F;
            mijk[3] = B - C - mijk[2];
        } else {
            mijk[3] = rate_M - 1 - F;
            mijk[2] = B - C - mijk[3];
        }

        /* mi2k */        
        if (D < rate_M) {
            mijk[4] = G;
            mijk[5] = D - mijk[4];
        } else {
            mijk[5] = rate_M - 1 - G;
            mijk[4] = D - mijk[5];
        }

        /* mi3k */
        if ((A - B - D) < rate_M) {
            mijk[6] = H;
            mijk[7] = A - B - D - mijk[6];
        } else {
            mijk[7] = rate_M - 1 - H;
            mijk[6] = A - B - D - mijk[7];
        }
#ifdef MATLAB_MEX_FILE
if (0) {
    char_T    err_msg[256];
    sprintf(err_msg, "Caller\n");
    mexPrintf(err_msg);    
    sprintf(err_msg, "A = %d; B = %d; C = %d; D = %d; E = %d; F = %d; G = %d; H = %d \n",A, B, C, D, E, F, G, H );
    mexPrintf(err_msg);
    sprintf(err_msg, "R[0]=%d; R[1]=%d; R[2]=%d; R[3]=%d; R[4]=%d; R[5]=%d \n",R[0],R[1],R[2],R[3],R[4],R[5]);
    mexPrintf(err_msg);
    sprintf(err_msg, "mijk[0]=%d; mijk[1]=%d; mijk[2]=%d; mijk[3]=%d; mijk[4]=%d; mijk[5]=%d; mijk[6]=%d; mijk[7]=%d \n",mijk[0],mijk[1],mijk[2],mijk[3],mijk[4],mijk[5],mijk[6],mijk[7]);
    mexPrintf(err_msg);        
}
#endif
        *last_updata = 1;
        *out_count = 0;
if (1) {
    char_T    err_msg[256];
    V_bef = 0;
    for (i = 0; i < 8; i++) {
        V_bef = mijk[i] > V_bef ? mijk[i] : V_bef;
    }
#ifdef MATLAB_MEX_FILE
    if (V_bef > 11) {
        sprintf(err_msg, "A=%d; B=%d; C=%d; D=%d; E=%d; F=%d; G=%d; H=%d \n", A, B, C, D, E, F, G, H );
        mexPrintf(err_msg);
        sprintf(err_msg, "R(0)=%d; R(1)=%d; R(2)=%d; R(3)=%d; R(4)=%d; R(5)=%d \n",R[0],R[1],R[2],R[3],R[4],R[5]);
        mexPrintf(err_msg);            
        for (i = 0; i < 8; i++) {
            sprintf(err_msg, "mijk(%d)=%d; ", i, mijk[i]);
            mexPrintf(err_msg);
        }
        sprintf(err_msg, "\n ");
        mexPrintf(err_msg);
    }
#endif
}        
    } else if (u[rate_b] <= 0) {
        *last_updata = 0;
    }

    if ((u[rate_b+1] > 0) && (*last_output == 0)) {
        if (*out_count < 8) {
            long base_count, k, tmp, i;

            k = *out_count % 2;
            base_count = (*out_count / 2) * (3 + 2 * rate_q) + rate_K;
            
            tmp = base_count + 3 + k * rate_q;
            for (i = 0; i < rate_q; i++) {
                y[i] = data_scrm[tmp + i];
            }
            y[rate_q] = mijk[*out_count];
            for (i = 0; i < 3; i++) {
                y[i + rate_q + 1] = data_scrm[base_count + i];
            }
            y[rate_q + 4] = k;
#ifdef MATLAB_MEX_FILE
if (0) {
    char_T    err_msg[256];
    sprintf(err_msg, "Time=%f; y[0]=%f; base_count = %d;  \n", ssGetT(S), y[rate_q], *out_count );
    mexPrintf(err_msg);
}            
#endif
        }
        /* at the very end */
        *out_count = *out_count + 1;
        *last_output = 1;
    } else {
        if (u[rate_b + 1] <= 0)
           *last_output = 0;
        if (ssGetT(S) == 0){
            int_T i;
            /* the initial condition before anything is out. */
            for (i = 0; i < rate_q + 5; i++) {
                y[i] = 0;
            }
        }
    }
}

/*
 * mdlUpdate - computes the discrete states of the S-Function
 */

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

/*
 * mdlDerivatives - computes the derivatives of the S-Function
 */

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

/*
 * mdlTerminate - called at termination of model execution.
 */

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