/*
 * File : flc_invp.c
 * Abstract:
 *       An example C-file S-function for linking Fuzzy Inference System,
 *       uk = fuzzy_controller(ek, dek)
 *
 * See simulink/src/sfuntmpl_doc.c
 *
 * Copyright 1990-2000 The MathWorks, Inc.
 * $Revision: 1.7 $
 * Written by Chou, Penchen, Aug. 3,2001
 * EE Dept., Da-Yeh University.
 * Execute mex flc_invp.c to get flc_invp.dll before Simulink simulation.
 */

#define S_FUNCTION_NAME  flc_invp
#define S_FUNCTION_LEVEL 2

#include "simstruc.h"

/*================*
 * Build checking *
 *================*/
#if !defined(MATLAB_MEX_FILE)
/*
 * This file cannot be used directly with the Real-Time Workshop. However,
 * this S-function does work with the Real-Time Workshop via
 * the Target Language Compiler technology. See 
 * matlabroot/toolbox/simulink/blocks/tlc_c/timestwo.tlc   for the C version
 * matlabroot/toolbox/simulink/blocks/tlc_ada/timestwo.tlc for the Ada version
 */
# error This_file_can_be_used_only_during_simulation_inside_Simulink
#endif

float fuzzy_controller(float, float);                 
#include<dos.h>
#include<math.h>
#include<stdio.h>
#include<stdlib.h>
#include<string.h>

#define MAX(i,j)  (i>=j)?i:j
#define MIN(i,j)  (i<=j)?i:j
#define NO_SECOND_INPUT 0
#define AND_OPERATOR    1
#define  OR_OPERATOR    2
/***************************************************************/

/***************************************************************
**          ==> USER CAN MODIFY THE FOLLOWING:         *********
****************************************************************/
#define N_rules 25   /* Total # of RULES          */
#define DA_bias 0    /* Bias appended to output for D/A conv. 
                        If you do not know, set DA_bias to zero*/
#define IN1_min -2.3562
#define IN1_max  2.3562
#define IN2_min -1.1781
#define IN2_max  1.1781
#define OUT_min -30.0
#define OUT_max  30.0

/*-----------------------------------------------------------*/
/*  Membership Functions */
/*  ===>Triangles with equal sides only<=== */
float x_membership[5][3]=  /* MB of input1 */
   {{-2.3562,-1.5708,-0.7850},
   {-1.5708,-0.7850,0},
   {-0.7850,0,0.7850},
   {0,0.7850,1.5708},
   {0.7850,1.5708,2.3562}};

float y_membership[5][3]=  /* MB of input2 */
   {{-1.1781,-0.7850,-0.3927},
   {-0.7850,-0.3927,0},
   {-0.3927,0,0.3927},
   {0,0.3927,0.7850},
   {0.3927,0.7850,1.1781}};

float z_membership[5][3]=  /* MB of output1 */
   {{-30,-20,-10},
   {-20,-10,0},
   {-10,0,10},
   {0,10,20},
   {10,20,30}};

/* RULES DEFINITION *
-----------------------------------------------------------------*/
/* ===>{1,1,1,2}, the last 2 is OR operation between two inputs.
		  Use 1 for AND.
-----------------------------------------------------------------*/
int Rule[N_rules][4]=
  {{1,1,5,1},
   {1,2,5,1},
   {1,3,5,1},
   {1,4,4,1},
   {1,5,3,1},
   {2,1,5,1},
   {2,2,4,1},
   {2,3,4,1},
   {2,4,3,1},
   {2,5,2,1},
   {3,1,5,1},
   {3,2,4,1},
   {3,3,3,1},
   {3,4,2,1},
   {3,5,1,1},
   {4,1,4,1},
   {4,2,3,1},
   {4,3,2,1},
   {4,4,2,1},
   {4,5,1,1},
   {5,1,2,1},
   {5,2,2,1},
   {5,3,1,1},
   {5,4,1,1},
   {5,5,1,1}};

/******************************************************************
**             [==> NO TOUCH in the following TO THE END.]      **
******************************************************************/

/* Global variables */
/* grade_reg array saves w1 and w2 of input x and y respectively.
   grade stores all min(w1,w2) in N_rules of locations.           */
float grade_reg[2], grade[N_rules];

float *ptr_MB;  /* membership functions (MB) pointer     */

/* Function: mdlInitializeSizes ===============================================
 * Abstract:
 *   Setup sizes of the various vectors.
 */
static void mdlInitializeSizes(SimStruct *S)
{
    ssSetNumSFcnParams(S, 0);
    if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
        return; /* Parameter mismatch will be reported by Simulink */
    }

    if (!ssSetNumInputPorts(S, 2)) return;         /* 2-input */
    ssSetInputPortWidth(S, 0, DYNAMICALLY_SIZED);
    ssSetInputPortDirectFeedThrough(S, 0, 1);
    ssSetInputPortWidth(S, 1, DYNAMICALLY_SIZED);
    ssSetInputPortDirectFeedThrough(S, 1, 1);

    if (!ssSetNumOutputPorts(S,1)) return;
    ssSetOutputPortWidth(S, 0, DYNAMICALLY_SIZED);

    ssSetNumSampleTimes(S, 1);

    /* Take care when specifying exception free code - see sfuntmpl_doc.c */
    ssSetOptions(S, SS_OPTION_EXCEPTION_FREE_CODE |
                 SS_OPTION_USE_TLC_WITH_ACCELERATOR);
}


/* Function: mdlInitializeSampleTimes =========================================
 * Abstract:
 *    Specifiy that we inherit our sample time from the driving block.
 */
static void mdlInitializeSampleTimes(SimStruct *S)
{
    ssSetSampleTime(S, 0, INHERITED_SAMPLE_TIME);
    ssSetOffsetTime(S, 0, 0.0);
}


/* Function: mdlOutputs =======================================================
 * Abstract:
 *    y = fuzzy_controller(u1,u2)
 */
static void mdlOutputs(SimStruct *S, int_T tid)
{
    int_T             i;
    InputRealPtrsType uPtrs1 = ssGetInputPortRealSignalPtrs(S,0); /* input #1 */
    InputRealPtrsType uPtrs2 = ssGetInputPortRealSignalPtrs(S,1); /* input #2 */
    real_T            *y    = ssGetOutputPortRealSignal(S,0);
    int_T             width = ssGetOutputPortWidth(S,0);

    for (i=0; i<width; i++) {
        /*
         * This example does not implement complex signal handling.
         * To find out see an example about how to handle complex signal in 
         * S-function, see sdotproduct.c for details.
         */
        *y++ = fuzzy_controller(*uPtrs1[i], *uPtrs2[i]); 
        
    }
}


/* Function: mdlTerminate =====================================================
 * Abstract:
 *    No termination needed, but we are required to have this routine.
 */
static void mdlTerminate(SimStruct *S)
{
}

#if defined(MATLAB_MEX_FILE)
#define MDL_RTW
/* Function: mdlRTW ===========================================================
 * Abstract:
 *	Since we've declared all are parameters as non-tunable, we need
 *	only provide this routine so that they aren't written to the model.rtw
 *	file. The values of the parameters are implicitly encoded in the
 *	sample times.
 */
static void mdlRTW(SimStruct *S)
{
}
#endif /* MDL_RTW */

#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

/*****************************************************************
**                  FUNCTIONS DEFINITIONS                       **
******************************************************************
   [1]. COMPUTE WEIGHT OF INPUT                                 **
******************************************************************/
float compute_grade(float *MB, int set, float input_data)
  {
    float weight;

    if ((input_data< *(MB+3*set+0)) || (input_data>*(MB+3*set+2)))
	  weight=0;
    else
      if (input_data> *(MB+3*set+1))           /* ON THE RIGHT SIDE OF A TRIANGLE */
	     weight=(*(MB+3*set+2)-input_data)/
	     (*(MB+3*set+2)-*(MB+3*set+1));
      else
	     weight=(input_data-*(MB+3*set+0))/    /* ON THE LEFT SIDE OF A TRIANGLE */
		 (*(MB+3*set+1)-*(MB+3*set+0));
    //printf("weight=%.2f\n",weight);
    weight=MAX(0.0,weight);                /* MAKE SURE TO BE WITHIN 0 TO 1 */
    weight=MIN(1.0,weight);
    //printf("weight=%.2f\n",weight);
    return weight;
  }

/**************************************************************
  [2]. COMPUTE WEIGHTED AVERAGE(CENTROID) OF AREA MADE BY 
       OUTPUT FUZZY SET.     
***************************************************************/
float fuzzy_controller(float input1, float input2)
  {
    float accu_num,accu_den,output,signal;
    int which_rule, set;

    /* CLEAR VARIABLES */
    accu_num=0;accu_den=0;output=0;
    for (which_rule=0;which_rule<=N_rules-1;which_rule++)
      {
	    signal=input1;           /* Data in */
	    signal=MIN(signal, IN1_max);
	    signal=MAX(signal, IN1_min);
	    //printf("Data modified=%f\n",signal);
	    set=Rule[which_rule][0]-1; ptr_MB=&x_membership;
	    grade_reg[0]=compute_grade(ptr_MB,set,signal);  /* Save data in grade_reg[0] */
	    //printf("%2d, Grade1=%6.2f, ",n+1,grade_reg[0]);

	    signal=input2;
	    signal=MIN(signal, IN2_max);
	    signal=MAX(signal, IN2_min);
	    set=Rule[which_rule][1]-1; ptr_MB=&y_membership;
	    grade_reg[1]=compute_grade(ptr_MB,set,signal);  /* Save data in grade_reg[1] */
	    //printf("Grade2=%6.2f\n",grade_reg[1]);

	    /* Check and/or operator */
	    /* If the sencond input is empty (i.e. 0), then only one grade */
	    if (Rule[which_rule][1]==NO_SECOND_INPUT)
	       grade[which_rule]=grade_reg[0];
	    else
	       if (Rule[which_rule][3]==AND_OPERATOR)
		  grade[which_rule]=MIN(grade_reg[0],grade_reg[1]);
	       else   /* Otherwise, it is an OR_OPERATOR */
		  grade[which_rule]=MAX(grade_reg[0],grade_reg[1]);

	    /* ACCUMULATED GRADES AND AREA GRADES CALCULATIONS */
	    //printf("grade[%i]=%.2f\n",which_rule,grade[which_rule]);
	    accu_num=accu_num+(grade[which_rule]*z_membership[Rule[which_rule][2]-1][1]);
	    accu_den=accu_den+grade[which_rule];
	    //printf("num[%i]=%5.2f,den[%i]=%5.2f\n",which_rule,accu_num,which_rule,accu_den);
      }
    /* IN CASE, AVOIDING DIVIDED BY ZERO */
    if (fabs(accu_den)>0.001)
       output=accu_num/accu_den;
    else
       output=0.0;
    //printf("output=%f",output);
    output=MIN(output, OUT_max);    /* BOUNDED OUTPUT CHECK POINTS */
    output=MAX(output, OUT_min);

    return (output+DA_bias);
  }
/************************************************************************
	   END of FUNCTIONS in this file.
*************************************************************************/