///////////////////////////////////////////////////////////////////////////
// Project:             AVISION
// Module:              avi_c40 - C40 Side Image Processor
// Version:             2.0
// Date:                02/01/00
// Company:             University of Santa Barbara
// Author:              Daryl Fortney
// References:  Spectrum Signal Processing - C40 Quad Board & LIA Interface
//                                      Texas Instruments - TMS320C40 Users Guide
///////////////////////////////////////////////////////////////////////////
#include "avi_c40.h"

void main(void) {
	long    x;

	asm(" or  800h, ST"); // Enable C40 Cache

	// Initialize MDC40IC VIDEO
	video_setup(&rs170_esync);
	video_clear_image();
	video_clear_overlay();
	video_set_text_back_colour(BACKGROUND);
	for (x = 0; x < 256; x++)
		video_set_dac_lut_entry(x, x, x, x);
	for (x = 0; x < COLORS; x++)
		video_set_dac_lut_overlay_entry(x, COLOR[x][0], COLOR[x][1], COLOR[x][2]);

	// Initialize PAI Params
	for (x = 0; x < TX_NUM; x++)
		P(x) = 0;

	// Initialize Tracking Object
	OBJECT1.STATE=TRK_STATE_OFF;

	// Read PAI Command
	PC_RX(tx_cmd);
	while (tx_cmd != CMD_QUIT) {

		// Update PAI Parameters
		if (tx_cmd & CMD_UPD)
			pai_upd();
		else {

			// Display Screen
			if (P(DSP_MODE)) {
				if (P(CLS_MODE) || P(TRK_MODE))
					video_clear_overlay();
				video_set_text_colour(WHITE);
				video_set_text_scale(2, 2);
				video_gotoxy(140, 10);
				video_printf("AVISION - V2.0");
				video_set_colour(BLUE);
				video_rect(P(IMG_X_S) - 1, P(IMG_Y_S) - 1, P(IMG_X_E) + 1, P(IMG_Y_E) + 1);
			}

			// Input Image Data
			if (tx_cmd & CMD_INP)
				pai_inp();
			else if (P(DSP_MODE) &&(P(CLS_MODE) || P(TRK_MODE))) {
				grab = TRUE;
				video_grab();
			}
			else
				video_capture_cont();

			// Classify
			if(P(CLS_MODE))
				classify(P(IMG_X_S),
					P(IMG_Y_S),
					P(IMG_X_E),
					P(IMG_Y_E),
					FALSE,
					P(CLS_MODE));

			// Track
			if(P(TRK_MODE))
				track(&OBJECT1);

			// Control Camera
			control(&OBJECT1);

			// Output Results
			if (tx_cmd & CMD_OUT)
				pai_out();

			grab = FALSE;
		}
		// Read PAI Command
		PC_RX(tx_cmd);
	}

	// QUIT !
	video_clear_overlay();
	video_pass_through();
	video_set_text_colour(WHITE);
	video_set_text_scale(2, 2);
	video_gotoxy(10, 450);
	video_printf("Pass");
}

void pai_upd(void) {

	long    x;        // Index Register
	long    data; // PAI Data Buffer

	// Receive PAI Data
	data = tx_cmd & CMD_DAT;
	if (data < TX_NUM) {
		PC_RX(P(data));

		// Update Display Parameter
		video_clear_overlay();

		OBJECT1.GAIN=P(CNT_GAIN);
		OBJECT1.SENS=P(CNT_SENS);
	}
	else {
		PC_RX(tx);
	}
}

void pai_inp(void) {

	long    x, y;
	long    xo, yo;
	long    start, end, rate, size;

	// Receive Header
	PC_RX(tx);
	tx_dim =(tx & 0xffff0000) >> 16;
	tx_app =(tx & 0x0000ffff);
	tx_skip = tx_keep = 1;

	// Receive Dimensionality Structure
	for (x = 0; x < tx_dim; x++) {
		PC_RX(tx);
		start = tx;
		PC_RX(tx);
		end = tx;
		PC_RX(tx);
		rate = tx;
		size =(end - start + rate) / rate;
		tx_skip *= size;
		if (x < TX_DIM && tx_app == TX_APP) {
			tx_d[x].s = start;
			tx_d[x].e = end;
			tx_d[x].r = rate;
			tx_d[x].z = size;
			tx_keep *= size;
		}
	}

	// Receive Data
	video_display();
	if (tx_app == TX_APP)
		switch (P(IMG_MODE)) {
			case 1:
				for (y = tx_d[_Y].s; y <= tx_d[_Y].e; y += tx_d[_Y].r)
					for (x = tx_d[_X].s; x <= tx_d[_X].e; x += tx_d[_X].r) {
						PC_RX(tx);
						for (yo = 0; yo < tx_d[_Y].r; yo++)
							for (xo = 0; xo < tx_d[_X].r; xo++) {
								*(image + x + xo + 512 *(y + yo)) = tx;
								if (P(IMG_OVR))
									*(overlay + x + xo + 512 *(y + yo)) = tx >> 24;
							}
					}
				break;
			case 0:
				for (y = 0; y < tx_d[_Y].z; y++)
					for (x = 0; x < tx_d[_X].z; x++) {
						PC_RX(tx);
						*(image +(x + tx_d[_X].s) + 512 *(y + tx_d[_Y].s)) = tx;
						if (P(IMG_OVR))
							*(overlay +(x + tx_d[_X].s) + 512 *(y + tx_d[_Y].s)) = tx >> 24;
					}
				break;
		}

	for (x = 0; x < tx_skip - tx_keep; x++) {
		PC_RX(tx);
	}
}

void pai_out(void) {

	long    x, y;

	// Transmit Header
	tx =((TX_DIM & 0x0000ffff) << 16) |(TX_APP & 0x0000ffff);
	PC_TX(tx);

	// Transmit Dimensionality Structure
	PC_TX(P(IMG_X_S));
	PC_TX(P(IMG_X_E) - P(IMG_X_R));
	PC_TX(P(IMG_X_R));
	PC_TX(P(IMG_Y_S));
	PC_TX(P(IMG_Y_E) - P(IMG_Y_R));
	PC_TX(P(IMG_Y_R));

	// Transmit Data
	if (!grab) video_grab();
	for (y = P(IMG_Y_S); y <= P(IMG_Y_E) - P(IMG_Y_R); y += P(IMG_Y_R))
		for (x = P(IMG_X_S); x <= P(IMG_X_E) - P(IMG_X_R); x += P(IMG_X_R)) {
			tx =(0x00ffffff) &(*(image + x + 512 * y));
			if (P(IMG_OVR))
				tx |=((*(overlay + x + 512 * y)) << 24);
			PC_TX(tx);
		}
}

void classify(long x_s, long y_s, long x_e, long y_e, BOOL wsh,WORD mode){
	WORD    i;

	static WORD ohlut[H_SIZE];
	static WORD     oxm,oym;

	// Setup Classifier Parameters
	X_RES = P(CLS_X_R);
	Y_RES = P(CLS_Y_R);
	V_LO = P(CLS_V_LO);
	S_LO = P(CLS_S_LO);
	OVR = P(DSP_MODE);
	WSH = wsh;

	// Update Classifier Parameters
	switch(mode){
		case CLS_MODE_PRESET:

			// Setup Preset LUT
			for (i = 0; i < H_SIZE; i++)
				H_LUT[i] = 0;

			if (P(CLS_H_LO) < P(CLS_H_HI))
				for (i = P(CLS_H_LO); i <= P(CLS_H_HI); i++)
					H_LUT[i] = 1;
			else {
				for (i = P(CLS_H_LO); i < H_SIZE; i++)
					H_LUT[i] = 1;
				for (i = 0; i <= P(CLS_H_HI); i++)
					H_LUT[i] = 1;
			}
			break;

		case CLS_MODE_LEARN:

			// Use Previous LUT to Determine Next LUT
			for(i=0;i<H_SIZE;i++)
				if(H_LUT[i]>H_FILTER)
					H_LUT[i]=1;
				else
					H_LUT[i]=0;
			break;

		default:
			return;
	}

	// Limit Space
	if (x_s < P(IMG_X_S))
		x_s = P(IMG_X_S);
	if (y_s < P(IMG_Y_S))
		y_s = P(IMG_Y_S);
	if (x_e > P(IMG_X_E))
		x_e = P(IMG_X_E);
	if (y_e > P(IMG_Y_E))
		y_e = P(IMG_Y_E);

	// Call Classifier
	ClassifyHSV(x_s, y_s, x_e, y_e);

	// Display Results
	if(P(DSP_MODE)){

		// Update Hue Graph
		for(i=0;i<H_SIZE;i++){
			video_set_colour(BACKGROUND);
			video_plot_line(i+128,450-ohlut[i],i+128,450);
			if(H_LUT[i])
				video_set_colour(GREEN);
			else
				video_set_colour(WHITE);
			video_plot_line(i+128,450-H_LUT[i],i+128,450);
			ohlut[i]=H_LUT[i];
		}
		video_set_colour(WHITE);
		video_set_text_colour(WHITE);
		video_set_text_scale(1, 1);
		video_gotoxy(200, 460);
		video_printf("Hue Histogram");

	}

	// Compute New Statistics
	H_M=0;
	if(NUM){
		for(i=0;i<H_SIZE;i++)
			if(H_LUT[i])
				H_M+=i*(H_LUT[i]-1);
		H_M/=NUM;
	}
	S_M=0.0;
	V_M=0.0;

	// C Marks The Spot
	video_set_text_scale(1, 1);
	video_set_text_colour(BACKGROUND);
	video_gotoxy(oxm-5,oym-5);
	video_printf("C");
	video_set_text_colour(WHITE);
	video_gotoxy(X_M-5, Y_M-5);
	video_printf("C");
	oxm=X_M;
	oym=Y_M;

	if (P(DSP_MODE)) {
		video_set_colour(WHITE);
		video_set_text_colour(WHITE);
		video_set_text_scale(1, 1);
		video_rect(x_s, y_s, x_e, y_e);
		video_gotoxy(10, CLS_TXTY1);
		video_printf("CLS: NUM = %-5ld ",(long)NUM);
		video_gotoxy(10, CLS_TXTY2);
		video_printf("CLS: X = %5.1f Y = %5.1f ", X_M, Y_M);
		video_gotoxy(10, CLS_TXTY3);
		video_printf("CLS: H = %5.1f S = %5.1f V = %-5.1f ", H_M, S_M, V_M);
	}
}

void track(TRK* ptrk) {
	static WORD     ox,oy;

	// Turn Tracker On
	if (ptrk->STATE == TRK_STATE_OFF)
		ptrk->STATE = TRK_STATE_INI;

	// Assume No Activity
	ptrk->ACTIVE=0;

	// Perform Tracker Update
	switch (ptrk->STATE) {

		case TRK_STATE_INI:
			trk_ini(ptrk);
			break;

		case TRK_STATE_TRK:
			trk_trk(ptrk);
			break;

		case TRK_STATE_SRC:
			trk_src(ptrk);
			break;

		default:
			break;
	}

	// T Marks The Spot
	video_set_text_scale(2, 2);
	video_set_text_colour(BACKGROUND);
	video_gotoxy(ox-10,oy-10);
	video_printf("+");
	if(ptrk->ACTIVE==1)
		video_set_text_colour(GREEN);
	else
		video_set_text_colour(RED);
	video_gotoxy(ptrk->X[0]-10, ptrk->Y[0]-10);
	video_printf("+");
	ox=ptrk->X[0];
	oy=ptrk->Y[0];

	/*
	if(ptrk->COUNT==40 && ptrk->STATE==TRK_STATE_TRK){
		video_set_text_scale(2, 2);
		video_set_text_colour(RED);
		video_gotoxy(100,50);
		video_printf("Terminating Target !!!");
	}

	if(ptrk->STATE==TRK_STATE_SRC){
		video_set_text_scale(2, 2);
		video_set_text_colour(BACKGROUND);
		video_gotoxy(100,50);
		video_printf("Terminating Target !!!");
	}
	*/

	// Display Results
	if (P(DSP_MODE)) {
		video_set_text_colour(WHITE);
		video_set_text_scale(1, 1);
		video_gotoxy(10,TRK_TXTY);
		video_printf("TRK: X = %5.1f Y = %5.1f State = %lx Count = %-5ld ",
			ptrk->X[0], ptrk->Y[0], ptrk->STATE,ptrk->COUNT);
	}
}

void trk_ini(TRK* ptrk) {
	WORD    i;

	// Reset Tracking Attributes
	ptrk->STATE = TRK_STATE_INI;
	ptrk->COUNT = 0;
	ptrk->GAIN = 1.0;
	ptrk->SENS = 1.0;
	for (i = 0; i < TRK_PRED; i++) {
		ptrk->X[i] = XRES/2;
		ptrk->Y[i] = YRES/2;
	}
	ptrk->XSIZE = P(TRK_XSIZE);
	ptrk->YSIZE = P(TRK_YSIZE);

	// Classify Entire Screen
	classify(0,0,XRES,YRES,FALSE,CLS_MODE_PRESET);

	// Object Visible?
	if (NUM >= P(TRK_Zi)) {

		// Start Tracking
		ptrk->STATE = TRK_STATE_TRK;

		// Initialize Tracking History
		for(i=0;i<TRK_PRED;i++){
			ptrk->X[i] = X_M;
			ptrk->Y[i] = Y_M;
		}
	}
}

void trk_trk(TRK* ptrk) {
	WORD    i;
	WORD    mode;

	// Update Counter
	ptrk->COUNT++;

	if(P(TRK_MODE)==1)
		mode=CLS_MODE_PRESET;
	else
		mode=CLS_MODE_LEARN;

	// Classify Fixed Spot
	classify(ptrk->X[0] - ptrk->XSIZE / 2,
		ptrk->Y[0] - ptrk->YSIZE / 2,
		ptrk->X[0] + ptrk->XSIZE / 2,
		ptrk->Y[0] + ptrk->YSIZE / 2,
		FALSE,
		mode);

	// Lets Move
	ptrk->ACTIVE=1;

	// Object Lost?
	if (NUM <= P(TRK_Zo)) {

		// Search For It
		ptrk->STATE = TRK_STATE_SRC;
		ptrk->COUNT=0;
	}
	else {

		// Update Tracking History
		for(i=TRK_PRED;i>0;i--){
			ptrk->X[i] = ptrk->X[i-1];
			ptrk->Y[i] = ptrk->Y[i-1];
		}
		ptrk->X[0]=X_M;
		ptrk->Y[0]=Y_M;
	}
}

void trk_src(TRK* ptrk) {
	WORD    i;

	// Do not Move While Searching
	ptrk->ACTIVE=0;
	
	// Classify Fixed Spot
	classify(ptrk->X[1] - ptrk->XSIZE / 2,
		ptrk->Y[1] - ptrk->YSIZE / 2,
		ptrk->X[1] + ptrk->XSIZE / 2,
		ptrk->Y[1] + ptrk->YSIZE / 2,
		FALSE,
		CLS_MODE_PRESET);

	// Object Visible?
	if (NUM >= P(TRK_Zi)) {

		// Resume Tracking
		ptrk->STATE = TRK_STATE_TRK;
		ptrk->GAIN=0;
		ptrk->COUNT=0;
		ptrk->XSIZE = P(TRK_XSIZE);
		ptrk->YSIZE = P(TRK_YSIZE);
		
		// Initialize Tracking History
		for(i=0;i<TRK_PRED;i++){
			ptrk->X[i] = X_M;
			ptrk->Y[i] = Y_M;
		}
	}
	else {

		// Grow Search Box
		ptrk->XSIZE += 4;
		ptrk->YSIZE += 4;
		ptrk->COUNT++;

		// Searching Too Long?
		if (ptrk->COUNT > P(TRK_SRCt)){
			
			// Return To Center
			ptrk->ACTIVE= 2;   
			
			// Reinitialize Tracking
			ptrk->STATE = TRK_STATE_OFF;
		}
	}
}

void control(TRK* ptrk) {
	float   dx;
	float   dy;
	BOOL    x_n;
	BOOL    y_n;

	// Calculate Displacement Vector
	dx =(ptrk->X[0]-XRES/2);
	if (dx < 0)
		x_n = TRUE;
	else
		x_n = FALSE;

	dy =(ptrk->Y[0]-YRES/2);
	if (dy < 0)
		y_n = TRUE;
	else
		y_n = FALSE;

	// Normalize & Sensitize Displacement Vector
	if (x_n)
		dx = -pow(-dx / XRES, ptrk->SENS)*XRES;
	else
		dx = pow(dx / XRES, ptrk->SENS)*XRES;

	if (y_n)
		dy = -pow(-dy / YRES, ptrk->SENS)*YRES;
	else
		dy = pow(dy / YRES, ptrk->SENS)*YRES;

	// Apply Tracking Gain
//ptrk->GAIN=P(CNT_GAIN);
	dx *=ptrk->GAIN*.1;
	dy *=ptrk->GAIN*.1;

	// Increase Gain Slowly
	if (ptrk->GAIN < P(CNT_GAIN))
		ptrk->GAIN += .05;

	// Send Results to PC
	PC_TX((long)dx);
	PC_TX((long)dy);
	if(P(CNT_MODE))
		PC_TX(ptrk->ACTIVE);
	else
		PC_TX(2);

	// Display Results
	if (P(DSP_MODE)) {
		video_set_text_colour(WHITE);
		video_set_text_scale(1, 1);
		video_gotoxy(10,CNT_TXTY);
		video_printf("CNT: dx = %5.1f dy = %5.1f Gain = %5.1f Active = %lx ",
			dx, dy, ptrk->GAIN,ptrk->ACTIVE);
	}
}