#ifndef S_FUNCTION_NAME
# define S_FUNCTION_NAME
#endif

#ifndef S_FUNCTION_LEVEL
# define S_FUNCTION_LEVEL 2
#endif

#include "simstruc.h"

#ifndef __STDC__
# define __STDC__ 1
#endif

/*
  * Copyright 1994-2005 The MathWorks, Inc.
 */
#ifndef __FIS__
# define __FIS__

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

/***********************************************************************
 Macros and definitions
 **********************************************************************/
/* Define portable printf and double */
#if defined(MATLAB_MEX_FILE)
# define PRINTF mexPrintf
# define DOUBLE real_T
#elif defined(__SIMSTRUC__)
# define PRINTF ssPrintf
# define DOUBLE real_T
#else
# define PRINTF printf
# define DOUBLE double
#endif

#ifndef ABS
# define ABS(x)   ( (x) > (0) ? (x): (-(x)) )
#endif
#ifndef MAX
# define MAX(x,y) ( (x) > (y) ? (x) : (y) )
#endif
#ifndef MIN
# define MIN(x,y) ( (x) < (y) ? (x) : (y) )
#endif
#define MF_PARA_N 4
#define STR_LEN 500
#define MF_POINT_N 101

/* debugging macros */
/*
#define PRINT(expr) printf(#expr " = %g\n", (double)expr)
#define PRINTMAT(mat,m,n) printf(#mat " = \n"); fisPrintMatrix(mat,m,n)
#define FREEMAT(mat,m) printf("Free " #mat " ...\n"); fisFreeMatrix(mat,m)
#define FREEARRAY(array) printf("Free " #array " ...\n"); free(array)
*/

#if (defined(MATLAB_MEX_FILE) && !defined(__SIMSTRUC__))
# define FREE mxFree
#else
# define FREE free
#endif

#define FREEMAT(mat,m) fisFreeMatrix(mat,m)
#define FREEARRAY(array) FREE(array)

/***********************************************************************
 Data types
 **********************************************************************/

typedef struct fis_node {
	int handle;
	int load_param;
	char name[STR_LEN];
	char type[STR_LEN];
	char andMethod[STR_LEN];
	char orMethod[STR_LEN];
	char impMethod[STR_LEN];
	char aggMethod[STR_LEN];
	char defuzzMethod[STR_LEN];
	int userDefinedAnd;
	int userDefinedOr;
	int userDefinedImp;
	int userDefinedAgg;
	int userDefinedDefuzz;
	int in_n;
	int out_n;
	int rule_n;
	int **rule_list;
	DOUBLE *rule_weight;
	int *and_or;	/* AND-OR indicator */
	DOUBLE *firing_strength;
	DOUBLE *rule_output;
	/* Sugeno: output for each rules */
	/* Mamdani: constrained output MF values of rules */
	struct io_node **input;
	struct io_node **output;
	DOUBLE (*andFcn)(DOUBLE, DOUBLE);
	DOUBLE (*orFcn)(DOUBLE, DOUBLE);
	DOUBLE (*impFcn)(DOUBLE, DOUBLE);
	DOUBLE (*aggFcn)(DOUBLE, DOUBLE);
	DOUBLE (*defuzzFcn)();
	DOUBLE *BigOutMfMatrix;	/* used for Mamdani system only */
    DOUBLE *BigWeightMatrix;/* used for Mamdani system only */
	DOUBLE *mfs_of_rule;	/* MF values in a rule */

	DOUBLE *bias; /*bias, to be tuned when no rules are fired*/
	int isbias;

	struct fis_node *next;
} FIS;



typedef struct io_node {
	char name[STR_LEN];
	int mf_n;
	DOUBLE bound[2];
	DOUBLE value;
	struct mf_node **mf;
} IO;



typedef struct mf_node {
	char label[STR_LEN];	/* MF name */
	char type[STR_LEN];		/* MF type */
	int nparams;			/* length of params field */
	DOUBLE *params;			/* MF parameters */
	int userDefined;		/* 1 if the MF is user-defined */
	DOUBLE (*mfFcn)(DOUBLE, DOUBLE *); /* pointer to a mem. fcn */ 
	DOUBLE value;		    /* for Sugeno only */
	DOUBLE *value_array;	/* for Mamdani only, array of MF values */
} MF;

#endif /* __FIS__ */
/***********************************************************************
 File, arrays, matrices operations 
 **********************************************************************/
/* Copyright 1994-2002 The MathWorks, Inc.  */
/* $Revision: $  $Date: $  */

/* display error message and exit */
static void fisError(char *msg)
{
#ifdef MATLAB_MEX_FILE
	mexErrMsgTxt(msg);
#else
	PRINTF("%s\n",msg);
    exit(1);
#endif
}

#ifndef NO_PRINTF         /*in case for rtw and dSPACE use */

/* an friendly interface to fopen() */
static FILE *fisOpenFile(char *file, char *mode)
{
	FILE *fp, *fopen();

	if ((fp = fopen(file, mode)) == NULL){
		PRINTF("The file %s cannot be opened.", file);
		fisError("\n");
	}
	return(fp);
}

#endif


/* define a standard memory access function with error checking */
void *fisCalloc(int num_of_x, int size_of_x)
{
	void *ptr;

#if (defined(MATLAB_MEX_FILE) &&  !defined(__SIMSTRUC__))
	/* datstruc.c ln325 requires ptr = NULL when it supplies num_of_x = 0 */
	if (num_of_x == 0) 
            ptr = NULL; /* mxCalloc returns a NULL pointer if num_of_x or size_of_x = 0 */
	else {
            ptr = mxCalloc(num_of_x, size_of_x);
            /* however we still need to check that memory was allocated successfully,
               exclude the case when num_of_x = 0, and if unsuccessful issue an error */
            if (ptr == NULL)
                fisError("Could not allocate memory in mxCalloc function call.");}
#else /* a Simulink file (defined(__SIMSTRUC__)), or standalone is being created */
	if (num_of_x == 0) 
            ptr = NULL; /* calloc returns a NULL pointer if num_of_x or size_of_x = 0 */
	else {
            ptr = calloc(num_of_x, size_of_x);
            /* however we still need to check that memory was allocated successfully,
               exclude the case when num_of_x = 0, and if unsuccessful issue an error */
            if (ptr == NULL)
                fisError("Could not allocate memory in calloc function call.");}
#endif

        return(ptr);
}


char **fisCreateMatrix(int row_n, int col_n, int element_size)
{
	char **matrix;
	int i;

	if (row_n == 0 && col_n == 0)
		return(NULL);
	matrix = (char **)fisCalloc(row_n, sizeof(char *));
	if (matrix == NULL)
		fisError("Calloc error in fisCreateMatrix!");
	for (i = 0; i < row_n; i++) { 
		matrix[i] = (char *)fisCalloc(col_n, element_size);
		if (matrix[i] == NULL)
			fisError("Calloc error in fisCreateMatrix!");
	}
	return(matrix);
}


/* won't complain if given matrix is already freed */
static void fisFreeMatrix(void **matrix, int row_n)
{
	int i;
	if (matrix != NULL) {
		for (i = 0; i < row_n; i++) {
			FREE(matrix[i]);
		}
		FREE(matrix);
	}
}


static DOUBLE**fisCopyMatrix(DOUBLE **source, int row_n, int col_n)
{
	DOUBLE **target;
	int i, j;

	target = (DOUBLE **)fisCreateMatrix(row_n, col_n, sizeof(DOUBLE));
	for (i = 0; i < row_n; i++)
		for (j = 0; j < col_n; j++)
			target[i][j] = source[i][j];
	return(target);
}


#ifndef NO_PRINTF        /* not available for RTW and dSPACE */

static void fisPrintMatrix(DOUBLE **matrix, int row_n, int col_n)
{
	int i, j;
	for (i = 0; i < row_n; i++) {
		for (j = 0; j < col_n; j++)
			PRINTF("%.3f ", matrix[i][j]);
		PRINTF("\n");
	}
}

static void fisPrintArray(DOUBLE *array, int size)
{
	int i;
	for (i = 0; i < size; i++)
		PRINTF("%.3f ", array[i]);
	PRINTF("\n");
}

static void
fisPause()
{
	PRINTF("Hit RETURN to continue ...\n");
	getc(stdin);
}

#endif
/***********************************************************************
 Parameterized membership functions
 **********************************************************************/
/* Copyright 1994-2002 The MathWorks, Inc.  */
/* $Revision: $  $Date: $  */

/* Triangular membership function */
static DOUBLE fisTriangleMf(DOUBLE x, DOUBLE *params)
{
	DOUBLE a = params[0], b = params[1], c = params[2];

	if (a>b)
		fisError("Illegal parameters in fisTriangleMf() --> a > b");
	if (b>c)
		fisError("Illegal parameters in fisTriangleMf() --> b > c");

	if (a == b && b == c)
		return(x == a);
	if (a == b)
		return((c-x)/(c-b)*(b<=x)*(x<=c));
	if (b == c)
		return((x-a)/(b-a)*(a<=x)*(x<=b));
	return(MAX(MIN((x-a)/(b-a), (c-x)/(c-b)), 0));
}

/* Trapezpoidal membership function */
static DOUBLE fisTrapezoidMf(DOUBLE x, DOUBLE *params)
{
	DOUBLE a = params[0], b = params[1], c = params[2], d = params[3];
	DOUBLE y1 = 0, y2 = 0;

	if (a>b) {
		PRINTF("a = %f, b = %f, c = %f, d = %f\n", a, b, c, d);
		fisError("Illegal parameters in fisTrapezoidMf() --> a > b");
	}
        if (b>c)
         {
                PRINTF("a = %f, b = %f, c = %f, d = %f\n", a, b, c, d);      
                fisError("Illegal parameters in fisTrapezoidMf() --> b > c");
         }
	if (c>d) {
		PRINTF("a = %f, b = %f, c = %f, d = %f\n", a, b, c, d);
		fisError("Illegal parameters in fisTrapezoidMf() --> c > d");
	}

	if (b <= x)
		y1 = 1;
	else if (x < a)
		y1 = 0;
	else if (a != b)
		y1 = (x-a)/(b-a);

	if (x <= c)
		y2 = 1;
	else if (d < x)
		y2 = 0;
	else if (c != d)
		y2 = (d-x)/(d-c);

	return(MIN(y1, y2));
	/*
	if (a == b && c == d)
		return((b<=x)*(x<=c));
	if (a == b)
		return(MIN(1, (d-x)/(d-c))*(b<=x)*(x<=d));
	if (c == d)
		return(MIN((x-a)/(b-a), 1)*(a<=x)*(x<=c));
	return(MAX(MIN(MIN((x-a)/(b-a), 1), (d-x)/(d-c)), 0));
	*/
}

/* Gaussian membership function */
static DOUBLE fisGaussianMf(DOUBLE x, DOUBLE *params)
{
	DOUBLE sigma = params[0], c = params[1];
	DOUBLE tmp;

	if (sigma==0)
		fisError("Illegal parameters in fisGaussianMF() --> sigma = 0");
	tmp = (x-c)/sigma;
	return(exp(-tmp*tmp/2));
}

/* Extended Gaussian membership function */
static DOUBLE fisGaussian2Mf(DOUBLE x, DOUBLE *params)
{
	DOUBLE sigma1 = params[0], c1 = params[1];
	DOUBLE sigma2 = params[2], c2 = params[3];
	DOUBLE tmp1, tmp2;

	if ((sigma1 == 0) || (sigma2 == 0))
		fisError("Illegal parameters in fisGaussian2MF() --> sigma1 or sigma2 is zero");

	tmp1 = x >= c1? 1:exp(-pow((x-c1)/sigma1, 2.0)/2);
	tmp2 = x <= c2? 1:exp(-pow((x-c2)/sigma2, 2.0)/2);
	return(tmp1*tmp2);
}

/* Sigmoidal membership function */
static DOUBLE fisSigmoidMf(DOUBLE x, DOUBLE *params)
{
	DOUBLE a = params[0], c = params[1];
	return(1/(1+exp(-a*(x-c))));
}

/* Product of two sigmoidal functions */
static DOUBLE fisProductSigmoidMf(DOUBLE x, DOUBLE *params)
{
	DOUBLE a1 = params[0], c1 = params[1], a2 = params[2], c2 = params[3];
	DOUBLE tmp1 = 1/(1+exp(-a1*(x-c1)));
	DOUBLE tmp2 = 1/(1+exp(-a2*(x-c2)));
	return(tmp1*tmp2);
}

/* Absolute difference of two sigmoidal functions */
static DOUBLE fisDifferenceSigmoidMf(DOUBLE x, DOUBLE *params)
{
	DOUBLE a1 = params[0], c1 = params[1], a2 = params[2], c2 = params[3];
	DOUBLE tmp1 = 1/(1+exp(-a1*(x-c1)));
	DOUBLE tmp2 = 1/(1+exp(-a2*(x-c2)));
	return(fabs(tmp1-tmp2));
}

/* Generalized bell membership function */
static DOUBLE fisGeneralizedBellMf(DOUBLE x, DOUBLE *params)
{
	DOUBLE a = params[0], b = params[1], c = params[2];
	DOUBLE tmp;
	if (a==0)
		fisError("Illegal parameters in fisGeneralizedBellMf() --> a = 0");
	tmp = pow((x-c)/a, 2.0);
	if (tmp == 0 && b == 0)
		return(0.5);
	else if (tmp == 0 && b < 0)
		return(0.0);
	else
		return(1/(1+pow(tmp, b)));
}

/* S membership function */
static DOUBLE fisSMf(DOUBLE x, DOUBLE *params)
{
	DOUBLE a = params[0], b = params[1];
	DOUBLE out;

	if (a >= b)
		return(x >= (a+b)/2);

	if (x <= a)
		out = 0;
	else if (x <= (a + b)/2)
		out = 2*pow((x-a)/(b-a), 2.0);
	else if (x <= b)
		out = 1-2*pow((b-x)/(b-a), 2.0);
	else
		out = 1;
	return(out);
}

/* Z membership function */
static DOUBLE fisZMf(DOUBLE x, DOUBLE *params)
{
	DOUBLE a = params[0], b = params[1];
	DOUBLE out;

	if (a >= b)
		return(x <= (a+b)/2);

	if (x <= a)
		out = 1;
	else if (x <= (a + b)/2)
		out = 1 - 2*pow((x-a)/(b-a), 2.0);
	else if (x <= b)
		out = 2*pow((b-x)/(b-a), 2.0);
	else