/*
** Program :  cifuzz.c
**
** Abstract:  This program allows for static or dynamic fuzzy
**   systems, either in the form of standard or Suegeno-Takagi
**   models.  This package allows for membership selection for
**   the input membership functions, but assumes that the output
**   membership functions are symmetric and triangular for
**   computational efficiency.  The fuzzy implication may be
**   computed through either the min or product operators and
**   defuzzification may be found through either centroid or
**   COG techniques. 
**  
**   It should be noted that if Takagi-Sugeno systems are to be
**   used, then centroid defuzzification is typically used.
*/
# include <stdlib.h>
# include <math.h>
# include "cifuzz.h"

/****************************************************/
/***** Initialize the Fuzzy System             ******/
void initfuzz(F_SYS *fuzzy, int which)
{
  int i, j;
  int w;                /* number of mem fcns for input #2 */
	int NUMIN = 1;        /* number of inputs */
	int NUMTH = 4;        /* number of extra T-S inputs */
	int NUMRULE = 5;      /* number of rules */
	int NUMMEM = 5;       /* number of input membership fcns */

	int intype[5] = {LEFTTRI, TRI, TRI, TRI, RIGHTTRI};

	double incent[5] = {-1.0, -0.5, 0.0, 0.5, 1.0};

	double inwdth[5] = {0.5, 0.5, 0.5, 0.5, 0.5};

# ifdef NOUK
	double outcentalfa[25] = {0.0, 0.0, 0.0, 0.0, 0.0,
				    0.0, 0.0, 0.0, 0.0, 0.0,
				    48.25, 48.25, 48.25, 48.25, 48.25,
				    73.41, 73.41, 73.41, 73.41, 73.41,
				    -2.29, -2.29, -2.29, -2.29, -2.29};
	double outcentbeta[25] = {-109.37, -109.37, -109.37, -109.37, -109.37,
				    0.0, 0.0, 0.0, 0.0, 0.0,
				    0.0, 0.0, 0.0, 0.0, 0.0,
				    0.0, 0.0, 0.0, 0.0, 0.0,
				    0.0, 0.0, 0.0, 0.0, 0.0};
# endif

# ifdef YESUK
	double outcentalfa[25] = {0.0, 0.0, 0.0, 0.0, 0.0,
				  0.0, 0.0, 0.0, 0.0, 0.0,
				  0.0, 0.0, 0.0, 0.0, 0.0,
				  0.0, 0.0, 0.0, 0.0, 0.0,
				  0.0, 0.0, 0.0, 0.0, 0.0};

	double outcentbeta[25] = {0.0, 0.0, 0.0, 0.0, 0.0,
				  0.0, 0.0, 0.0, 0.0, 0.0,
				  0.0, 0.0, 0.0, 0.0, 0.0,
				  0.0, 0.0, 0.0, 0.0, 0.0,
				  0.0, 0.0, 0.0, 0.0, 0.0};
# endif

  if (!(fuzzy->size = (int *) malloc(4*sizeof(int)))) {
    printf("Error allocating memory");
    exit(1);
  }
  if (!(fuzzy->invar = (int *) malloc((NUMIN+1)*sizeof(int)))) {
    printf("Error allocating memory");
    exit(1);
  }
  if (!(fuzzy->inmem = (IN_MEM *) malloc(sizeof(IN_MEM)))) {
    printf("Error allocating memory");
    exit(1);
  }
  if (!(fuzzy->outmem = (OUT_MEM *) malloc(sizeof(OUT_MEM)))) {
    printf("Error allocating memory");
    exit(1);
  }
  if (!(fuzzy->rules = (int *) malloc(NUMIN*NUMRULE*sizeof(int)))) {
    printf("Error allocating memory");
    exit(1);
  }
  if (!(fuzzy->type = (int *) malloc(2*sizeof(int)))) {
    printf("Error allocating memory");
    exit(1);
  }
  if (!(fuzzy->dom = (double *) malloc(NUMMEM*sizeof(double)))) {
    printf("Error allocating memory");
    exit(1);
  }
  if (!(fuzzy->imdom = (double *) malloc(NUMRULE*sizeof(double)))) {
    printf("Error allocating memory");
    exit(1);
  }
  if (!(fuzzy->inmem->type = (int *) malloc(NUMMEM*sizeof(int)))) {
    printf("Error allocating memory");
    exit(1);
  }
  if (!(fuzzy->inmem->cent = (double *) malloc(NUMMEM*sizeof(double)))) {
    printf("Error allocating memory");
    exit(1);
  }
  if (!(fuzzy->inmem->wdth = (double *) malloc(NUMMEM*sizeof(double)))) {
    printf("Error allocating memory");
    exit(1);
  }
  if (!(fuzzy->outmem->cent = (double *) malloc(NUMRULE*(NUMIN+NUMTH)*sizeof(double)))) {
    printf("Error allocating memory");
    exit(1);
  }
  if (!(fuzzy->outmem->wdth = (double *) malloc(NUMRULE*sizeof(double)))) {
    printf("Error allocating memory");
    exit(1);
  }

  /* define fuzzy system size */
  fuzzy->size[0] = NUMIN;     /* number of inputs                */
  fuzzy->size[1] = NUMTH;     /* number of T-S functions         */
  fuzzy->size[2] = NUMMEM;    /* total number of input membership fcns */
  fuzzy->size[3] = NUMRULE;   /* number of rules                 */

  /*define locations of input membership fcns */
  fuzzy->invar[0] = 0;
  fuzzy->invar[1] = 5;

  /*define the fuzzy input membership types, centers, and widths */
  for (i=0; i<NUMMEM; i++) {
    fuzzy->inmem->type[i] = intype[i];
    fuzzy->inmem->cent[i] = incent[i];
    fuzzy->inmem->wdth[i] = inwdth[i];
  }


  /*define the output membership centers and widths */
  if (!which)                /* which == 0 -> alfa, 1 -> beta */
    for (i=0; i<NUMRULE; i++) {
      fuzzy->outmem->cent[i] = outcentalfa[i];
      fuzzy->outmem->cent[i+5] = outcentalfa[i+5];
      fuzzy->outmem->cent[i+10] = outcentalfa[i+10];
      fuzzy->outmem->cent[i+15] = outcentalfa[i+15];
      fuzzy->outmem->cent[i+20] = outcentalfa[i+20];
      fuzzy->outmem->wdth[i] = 0.5;
    }
  else
    for (i=0; i<NUMRULE; i++) {
      fuzzy->outmem->cent[i] = outcentbeta[i];
      fuzzy->outmem->cent[i+5] = outcentbeta[i+5];
      fuzzy->outmem->cent[i+10] = outcentbeta[i+10];
      fuzzy->outmem->cent[i+15] = outcentbeta[i+15];
      fuzzy->outmem->cent[i+20] = outcentbeta[i+20];
      fuzzy->outmem->wdth[i] = 0.5;
    }

  /*define the rule base */

  for (i=0; i<5; i++)
    fuzzy->rules[i] = i;

  /*define the type of fuzzy system to be used */
  fuzzy->type[0] = PRODTNORM;
  fuzzy->type[1] = CENTROID;
}

/********************************************************/
/* clear the memory used to store the fuzzy system      */

void clearfuzz(F_SYS *fuzzy)
{
  free(fuzzy->inmem->type);
  free(fuzzy->inmem->cent);
  free(fuzzy->inmem->wdth);
  free(fuzzy->outmem->cent);
  free(fuzzy->outmem->wdth);
  free(fuzzy->size);
  free(fuzzy->invar);
  free(fuzzy->inmem);
  free(fuzzy->outmem);
  free(fuzzy->rules);
  free(fuzzy->type);
  free(fuzzy->dom);
  free(fuzzy->imdom);
}

/****************************************************************/
/* determine the input degree of membership */

void finddom(double *u, F_SYS *fuzzy)
     
{
  int i;             /* loop counter */
  int nummem;        /* number of input membership functions */
  int innum;         /* input membership index */
  double inval;      /* crisp input value */
  double typ;        /* type of input membership function */
  double cntr;       /* center of the input membership function */
  double wdth;       /* width of the membership function */
  
  nummem = fuzzy->size[2];
  innum = 0;
  inval = u[0];                     /* current input value */
  for (i=0; i<nummem; i++) {
    if (fuzzy->invar[innum + 1] <= i) {
      innum ++;
      inval = u[innum];
    }
    typ = fuzzy->inmem->type[i];
    cntr = fuzzy->inmem->cent[i];
    wdth = fuzzy->inmem->wdth[i];
      
    if (typ == LEFTTRI) {
      if (inval > cntr)
	fuzzy->dom[i] = max(0.0, (1 - (inval - cntr)/wdth));
      else
	fuzzy->dom[i] = 1.0;
    }
    else if (typ == RIGHTTRI) {
      if (inval > cntr)
	fuzzy->dom[i] = 1.0;
      else
	fuzzy->dom[i] = max(0.0, (1 - (cntr - inval)/wdth));
    }
    else if (typ == TRI) {
      fuzzy->dom[i] = max(0.0, (1 - abs(cntr - inval)/wdth));
    }
    else if (typ == LEFTGAUSS) {
      if (inval > cntr)
	fuzzy->dom[i] = exp(-sqr((inval - cntr)/wdth));
      else
	fuzzy->dom[i] = 1.0;
    }
    else if (typ == RIGHTGAUSS) {
      if (inval > cntr)
	fuzzy->dom[i] = 1.0;
      else
	fuzzy->dom[i] = exp(-sqr((inval - cntr)/wdth));
    }
    else if (typ == GAUSSIAN) {
      fuzzy->dom[i] = exp(-sqr((inval - cntr)/wdth));
    }
    else if ((typ == LEFTCOS)||(typ == LEFTCOS2)) {
      if (inval > cntr)
	if (inval < (cntr + wdth))
	  fuzzy->dom[i] = cos(PI*(inval - cntr)/(2*wdth));
	else
	  fuzzy->dom[i] = 0.0;
      else
	fuzzy->dom[i] = 1.0;
      if (typ == LEFTCOS2)
	fuzzy->dom[i] *= fuzzy->dom[i];
    }
    else if ((typ == RIGHTCOS)||(typ == RIGHTCOS2)) {
      if (inval > cntr)
	fuzzy->dom[i] = 1.0;
      else
	if (inval > (cntr - wdth))
	  fuzzy->dom[i] = cos(PI*(inval - cntr)/(2*wdth));
	else
	  fuzzy->dom[i] = 0.0;
      if (typ == RIGHTCOS2)
	fuzzy->dom[i] *= fuzzy->dom[i];
    }
    else if ((typ == COS)||(typ == COS2)) {
      if (abs(inval - cntr) > wdth)
	fuzzy->dom[i] = 0.0;
      else
	fuzzy->dom[i] = cos(PI*(inval - cntr)/(2*wdth));
      if (typ == COS2)
	fuzzy->dom[i] *= fuzzy->dom[i];
    }
  }
}


/* calculate the output of from the rule base */

double evalrules(double *u, F_SYS *fuzzy)

{
  int i, j;        /* loop counters              */
  int numin;       /* number of input variables  */
  int numrules;    /* number of rules            */
  int numth;       /* number of T-S inputs       */
  double memval;   /* current dom                */
  double atmp;     /* temp area                  */
  double area;     /* total area                 */
  double mom;      /* total moment               */
  double fval;     /* function T-S value         */

  numin = fuzzy->size[0];
  numth = fuzzy->size[1];
  numrules = fuzzy->size[3];
  area = 0.0;
  mom = 0.0;

  for(i = 0; i<numrules; i++) {
    fuzzy->imdom[i] = -1.0;         /* no initial implied degree of mem */
    for(j=0; j<numin; j++) {
      if (fuzzy->rules[i + j*numrules] >= 0) {
	memval = fuzzy->dom[fuzzy->invar[j] + fuzzy->rules[i + j*numrules]];
	if (fuzzy->imdom[i] < 0)                       /* first time */
	  fuzzy->imdom[i] = memval;
	else if (fuzzy->type[0] == MINTNORM)           /* t-norm is min */
	  fuzzy->imdom[i] = min(fuzzy->imdom[i], memval);
	else if (fuzzy->type[0] == PRODTNORM)          /* t-norm is prod */
	  fuzzy->imdom[i] *= memval;
      }
    }
    if (fuzzy->type[1] == CENTROID)
      atmp = fuzzy->imdom[i];      /* centroid defuzz */
    else
      atmp = fuzzy->outmem->wdth[i]*(1.0 - sqr(1.0 - fuzzy->imdom[i]))/2;
    area += atmp;
    fval = fuzzy->outmem->cent[i];
    for (j=0; j < numth; j++)
      fval += fuzzy->outmem->cent[i + numrules*(j+1)]*u[numin + j];
    mom += atmp*fval;
  }
  fuzzy->area = area;
  if (area == 0.0)        /* this should never occur if fuzzy */
    return(0.0);          /*    system is defined properly    */
  else
    return(mom/area);
}


/********************************************************/
/* define the update laws for the fuzzy system */

void fuzzupdate(double *u, F_SYS *fuzzy)

{
  int i, j;           /* loop counters */
  int numrules;       /* number of rules */
  int numth;          /* number of basis functions */
  int numin;          /* number of inputs */
  double adjust;      /* adaptation function */

  numrules = fuzzy->size[3];
  numth = fuzzy->size[1];
  numin = fuzzy->size[0];
  adjust = u[numin+numth]/fuzzy->area;    /* normalize dom */

  for(i=0; i<numrules; i++)
    for(j=0; j<=numth; j++)
      if (j == 0)
	fuzzy->outmem->cent[i] += 0.01*(fuzzy->imdom[i])*adjust;
      else
	fuzzy->outmem->cent[i+numrules*j] += 
                       0.01*(fuzzy->imdom[i])*u[j]*adjust;
}

void dumpmatrix(F_SYS *fuzzy)
{
  int i,j,numrules,numth;

  numrules = fuzzy->size[3];
  numth = fuzzy->size[1];
  for (i=0;i<numrules; i++)  {
    for (j=0; j<=numth; j++) {
      printf("%f  ", fuzzy->outmem->cent[j+numrules*i]);
    }
    printf("\n");
  }
}


/****************************************************************/
/*  This routine is used to pull the other functions together   */
/*  to make a comlete fuzzy system                              */

double fuzzsys(double *u, F_SYS *fuzzy)
     
{
  finddom(u,fuzzy);              /* find input degrees of membership */
  return(evalrules(u,fuzzy));    /* calculate the output COG */
}