/*                          unh_cmac.c	Copyright c 1989, 1990, 1994, 1995, 1996 The University of New 	Hampshire. All rights reserved.	This module is a C language implementation of a multiple	CMAC driver. It includes multiple (and programmable) designs for	the receptive field lattice and the receptive field sensitivity	functions. It was developed in the Robotics Laboratory of the 	Department of Electrical and Computer Engineering at the 	University of New Hampshire. This C source code is not available 	for sale from UNH, and can not be resold. You may use it, and 	give it away freely to others, as long as you, and those you 	give it to, agree to acknowledge the UNH Robotics Laboratory in 	any project reports, manuscripts, software manuals, etc., 	which result from projects which utilize this code.	The code is generic C, but has only been tested at UNH using	Microsoft C compilers for the IBM-PC family. It assumes 32 bit	integer data types. You should check the following functions for 	compatibility with your compiler:	    malloc() - allocate data region of size specified in bytes.	    free()   - deallocate data region.	    rand()   - return random integer in range 0 - 32767.    Also, you should check the list of header files <*.h> since	these may be different for your environment.    MAJOR MODIFICATIONS    ** Modified 7-3-92 to handle compiler variations in modulo (%)    function for negative values.        ** Modified 6-xx-94 to handle various receptive field lattices and     function shapes, collision avoidance hashing, and weight     normalization.    */#include <stdlib.h>#include <fcntl.h>#include <io.h>#include <sys\stat.h>#include <malloc.h>#include "unh_cmac.h"#define TRUE  1#define FALSE  0/* *************************************************************************************************************************************************The following definitions affect static data storage size, but NOT execution time.**************************************************************************//* Set this to the maximum number of independent CMACs you will need */#define NUM_CMACS 8/* Set this to the maximum number of input dimensions you will need */#define MAX_STATE_SIZE 64 /* Set this to the maximum number of output dimensions you will need */#define MAX_RESPONSE_SIZE 8 /* Set this to the maximum number of layers of receptive fields you will need */#define MAX_GEN_SIZE 256 /* Set this to the size of the receptive field function table you will use */#define RF_TABLE_SIZE 128 /* ************************************************************************/************************************************************************** The following store the major specs and pointers for the current CMAC**************************************************************************/static int first_call = TRUE; 	/* is set to FALSE after init      */static int in_learn = FALSE;	/* is set to TRUE while in learn  */static int *ap_a[NUM_CMACS+1]; /* pointers to all cmac memories   */static int *ap;                /* pointer to current cmac memory  */static int ap_size[NUM_CMACS+1]; /* sizes in bytes of cmac memories */static int rsp_size;          	/* dimension of response vector    */static int gen_size;          	/* generalization parameter        */static int st_size;           	/* dimension of input state vector */static int mem_size;          	/* # of response vectors in memory */ static int rf_shape;			/* code for the receptive field shape */static int collide;			/* TRUE/FALSE control of hash collisions */static int *qnt;               /* pointer to input quantization   */static int *disp;				/* pointer to the displacement vector */static int *rfieldt;			/* pointer to the rfield function table */static unsigned int rndseq[2048]; /* Random number table for hashing */static unsigned int rndseq2[2048]; /* Random number table for hashing */static int indexes[MAX_GEN_SIZE]; /* CMAC mapping indexes            */static long rfield[MAX_GEN_SIZE]; /* RF function magnitudes */static long sum_rfield  = 0;	/* summed RF function magnitudes */static long sum2_rfield = 0;   /* summed squared RF function magnitudes *//* ********************************************************************* *//**************************************************************************The following functions are used internally by the CMAC code and can notbe called externally! **************************************************************************//*************************************************************************** *      stoap()                                                            * *                                                                         * *      Input:          state[]                                            * *      Output:         indexes[]                                          * *                                                                         * *      Purpose:        Map from input vector, state[], to corrosponding   * *                      output cells in ap[]. The active output cells      * *                      are indexed by indexes[]                           * *                                                                         * ***************************************************************************/static void stoap(int *state){	int i, j, k;	long index, min_d, test_d, rf_index;	long sum;	int hash_tag;	static long base[MAX_STATE_SIZE], qstate[MAX_STATE_SIZE];	/* the following code is for constant RF functions */	if ((rf_shape == ALBUS) || (rf_shape == RECTANGULAR)) {			/* get quantized input */				for (i = 0; i < st_size; i++) {			if (state[i] >= 0) {				qstate[i] = state[i] / qnt[i];			} else {				qstate[i] = (state[i] - qnt[i] + 1) / qnt[i];			}			base[i] = 0; /* first RF layer aligned with origen */		}		/* compute a new hashed RF index for each RF layer */	    for (j = 0; j < gen_size; j++) {					sum = 0;			hash_tag = 0;			 			/* loop over all dimensions of this RF */		    for(i = 0; i < st_size; i++) {		    	/* find corner coordinate of excited RF in quantized space */ 		    	if (qstate[i] >= base[i])	        		index = qstate[i] - ((qstate[i] - base[i]) % gen_size);	        	else    				index = (qstate[i] + 1) + ((base[i] - (qstate[i] + 1)) % gen_size) - gen_size;	        	/* add random table offset for this dimension and wrap around */			 	index += (449 * i);	            index %= 2048;	            while (index < 0) index += 2048;						/* add selected random number to sum */			 	sum += (long)rndseq[(int)index];			 	if (!collide) hash_tag += (int)rndseq2[(int)index];				 				 	/* compute displacement of next RF layer in quantized space */			 	base[i] += disp[i];			}			if (hash_tag == 0) hash_tag = 1;				    /* compute psuedo-random index */		    indexes[j] = (int)(sum % mem_size);		    while (indexes[j] < 0) indexes[j] += mem_size; 		    		    /* now search for hash tag match or open slot */		    if (!collide) {				--indexes[j];				k = 0;			    i = (indexes[j] * (rsp_size + 1)) + rsp_size;			    do {			    	if (++indexes[j] >= mem_size) {			    		indexes[j] = 0;			    		i = rsp_size;			    	} else {			    		i += rsp_size + 1;			    	}				} while ((ap[i] != hash_tag) && (ap[i] != 0) && (++k < mem_size));			} else {			    i = (indexes[j] * (rsp_size + 1)) + rsp_size;			}			/* mark used memory location */	    			if ((in_learn) && (ap[i] == 0)) ap[i] = hash_tag;  	    		}		return;	}	/* the following code is for tapered RF functions */	/* get quantized input */		for (i = 0; i < st_size; i++) {		if (state[i] >= 0) {			qstate[i] = (long)(state[i] / qnt[i]);		} else {			qstate[i] = (long)((state[i] - qnt[i] + 1) / qnt[i]);		}		base[i] = 0; /* first RF layer aligned with origen */	}	/* init RF function sums */		sum_rfield = 0;	sum2_rfield = 0;	/* compute a new hashed RF index for each RF layer */    for (j = 0; j < gen_size; j++) {			sum = 0;		hash_tag = 0;		min_d = (long)1 << ((8 * sizeof(long)) - 2);				/* loop over all dimensions of this RF */	    for (i = 0; i < st_size; i++) {	    	/* find corner coordinate of excited RF in quantized space */ 	    	if (qstate[i] >= base[i])        		index = qstate[i] - ((qstate[i] - base[i]) % gen_size);        	else    			index = (qstate[i] + 1) + ((base[i] - (qstate[i] + 1)) % gen_size) - gen_size;			 	            /* update physical distance to closest side of RF,                and index into RF function table */            test_d = (long)state[i] - (index * (long)qnt[i]);            if (test_d < min_d) {            	min_d = test_d;            	rf_index = (RF_TABLE_SIZE * min_d) / ((gen_size * qnt[i]) >> 1);            }            test_d = (((long)gen_size + index) * (long)qnt[i]) - state[i] - 1;            if (test_d < min_d) {            	min_d = test_d;            	rf_index = (RF_TABLE_SIZE * min_d) / ((gen_size * qnt[i]) >> 1);            }	                    	/* add table offset for this dimension and wrap around */		 	index += (449 * i);            index %= 2048;            while (index < 0) index += 2048;				/* add selected random number to sum */		 	sum += (long)rndseq[(int)index];			if (!collide) hash_tag += (int)rndseq2[(int)index];		 	/* compute displacement of next RF layer in quantized space */		 	base[i] += disp[i];		}		if (hash_tag == 0) hash_tag = 1;		    /* compute psuedo-random index */	    indexes[j] = (int)(sum % mem_size);	    while (indexes[j] < 0) indexes[j] += mem_size;	    /* now search for hash tag match or open slot */		if (!collide) {			--indexes[j];			k = 0;		    i = (indexes[j] * (rsp_size + 1)) + rsp_size;		    do {		    	if (++indexes[j] >= mem_size) {		    		indexes[j] = 0;		    		i = rsp_size;		    	} else {		    		i += rsp_size + 1;		    	}			} while ((ap[i] != hash_tag) && (ap[i] != 0) && (++k < mem_size));		} else {		    i = (indexes[j] * (rsp_size + 1)) + rsp_size;		}		/* mark used memory location */	    		if ((in_learn) && (ap[i] == 0)) ap[i] = hash_tag;  	    /* update RF function parameters */	    if (rf_index >= RF_TABLE_SIZE) rf_index = RF_TABLE_SIZE - 1;	    rfield[j] = rfieldt[rf_index];	    sum_rfield += rfieldt[rf_index];	    sum2_rfield += ((long)rfieldt[rf_index] * (long)rfieldt[rf_index]);	}}/*************************************************************************** *      setup()                                                            * *                                                                         * *      Input:          cmac_id                                            * *      Output:         ap, st_size, rsp_size, gen_size, mem_size, qnt     * *                                                                         * *      Purpose:        Get parameters for current CMAC                    * *                                                                         * ***************************************************************************/static void setup(int cmac_id){	ap = ap_a[cmac_id];     /* pointer to cmac storage         */	st_size = *ap++;        /* size of input state vector      */	rsp_size = *ap++;       /* dimension of response vector    */	gen_size = *ap++;       /* generalization parameter        */	mem_size = *ap++;       /* # of response vectors in memory */	rf_shape = *ap++;		/* receptive field code 		   */	collide = *ap++;		/* allow hashing collisions	???    */	qnt = ap;               /* pointer to quantization vector  */	ap += st_size;	disp = ap;				/* pointer to displacement vector  */	ap += st_size;	rfieldt = ap;			/* pointer to rf function table    */	ap += RF_TABLE_SIZE;	/* pointer to CMAC weights         */  		}/*************************************************************************** *      genmap()                                                           * *                                                                         * *      Input:          none                                               * *      Output:         rndseq[]                                           * *                                                                         * *      Purpose:        Initialize random lookup table array               * *                                                                         * ***************************************************************************/static void genmap(void){	int i, k;	/* make sure that same random hashing always used */	/* (necessary if CMAC weights saved from day-to-day) */ 	srand(1);	/* random table for address hashing */	for(k = 0; k < 2048; k++) {		rndseq[k] = 0;		for (i=0; i<sizeof(int); ++i)	    	rndseq[k] = (rndseq[k] << 8) | (rand() & 0xff);    	}	/* random table for collision avoidance tags */	for(k = 0; k < 2048; k++) {		rndseq2[k] = 0;		for (i=0; i<sizeof(int); ++i)	    	rndseq2[k] = (rndseq2[k] << 8) | (rand() & 0xff);    	}}/*************************************************************************** *      init_disp_vector()                                                 * *                                                                         * *      Input:          num_state, field_shape                             * *      Output:         dv[]                                               * *                                                                         * *      Purpose:        Initialize lattice displacement vector             * *                                                                         * ***************************************************************************/static void init_disp_vector(int dv[], int num_state, int field_shape){