/*this file just holds temp code, stuph I'm removing and/or adding in bulk, and it would be a pain to comment out, etc*/

/*from OBF/OBT*/
/*some are self explanatory*/
#define STILL 1
#define AVOID 2 /*ir- avoid impending collision with objects*/
#define ESCAPE 4 /*detect collision and escape*/
#define ROAM 8 /*roam room (hopefully with purpose to learn/map borders)*/
#define LINE_FOLLOW 16
#define LIGHT_FOLLOW 32
#define WALL_FOLLOW 64 /*this includes corners & edges*/
#define LISTEN_MIC 128
#define UNUSED 256
#define RESERVED 512
#define RANDOM 1024 /*and if we don't know what to do, Chaos Game often works*/



/*
*in .h file
I threw this prototype in here, should be elsewhere
Interrupt service routine for mtask scheduler
 don't need this
extern void system_interrupt(void);
#pragma interrupt_handler system_interrupt
#define TOC4_isr system_interrupt
*/

	printf("About to put define TOC4 handler.\n");
	/* *(void (**)())0x7e13=system_interrupt; */



/****************************** calibrate() ***********************************/
void calibrate(void)
{
/*
Right now I'm assuming that lucid starts out in an open environment
low sound, etc
*/
	unsigned int i,j;
	unsigned int ir_low[NUM_IR], ir_high[NUM_IR];
	unsigned int cds_low[NUM_CDS], cds_high[NUM_CDS];

	#ifdef DEBUG
		printf("Calibrating.\n");
	#endif

	/*init values*/
	for (i = 0; i < NUM_IR; i++)
	{
		ir_low[i]=255; ir_high[i]=0;
		ir_threshold[i] = 0;
	}
	for (i = 0; i < NUM_CDS; i++)
	{
		cds_low[i]=255; cds_high[i]=0;
	}

	/*get mic*/
	read_IR();
	microphone_threshold = MIC;
	DEBUG_OUT(D_MIC);

	/*turn off LEDS ; read their low values and also cds cells*/
	IRE_OUT = IRE_ALL_OFF;
	wait(WAIT_LED);
	OMF(CLOCKWISE);
	for (j = 0; j < 5; j++)
	{ 
		read_CDS();
		for (i = 0; i < NUM_CDS; i++)
		{
			if  ( CDS[i] < cds_low[i] )
			{
				cds_low[i] = CDS[i];
			}
			if  ( CDS[i] > cds_high[i] )
			{
				cds_high[i] = CDS[i];
			}
		}
		read_IR();
		for (i = 0; i < NUM_IR; i++)
		{
			if  ( IRDT[i+IR_OFFSET] < ir_low[i] )
			{
				ir_low[i] = IRDT[i+IR_OFFSET];
			}
			ir_threshold[i] += IRDT[i+IR_OFFSET];
		}
		wait(100);
	}
	DEBUG_OUT(D_CDS);

	/*now turn them on and read highs ; this is not close dist. measurment*/
	IRE_OUT = IRE_ALL_ON;
	wait(WAIT_LED);
	for (j = 0; j < 5; j++)
	{ 
		read_IR();
		for (i = 0; i < NUM_IR; i++)
		{
			if  ( IRDT[i+IR_OFFSET] > ir_high[i] )
			{
				ir_high[i] = IRDT[i+IR_OFFSET];
			}
		}
	}

	OMF(STOP);

	for (i = 0; i < NUM_IR; i++)
	{
		ir_threshold[i] /= 5;
		//printf("low %d thresh %d high %d\n", ir_low[i], ir_threshold[i], ir_high[i] );
	}
	DEBUG_OUT(D_IR);

	fb = front_bumper(); /*read front bumper*/
	rb = read_bumper(); /*read rear bumper*/
	DEBUG_OUT(D_BUMP);

} /*end of calibrate()*/