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

IOL_PTU	ptu;	// Pan/Tilt Unit

main(int num_args, char **arg){

	char	usr_cmd;

	// Interpret Command Line
	if (num_args == 2)
		strcpy(i_fn, arg[1]);
	else
		strcpy(i_fn, DSC_FILE);

	// Open description file
	if ((i_fp = fopen(i_fn, "ra")) == NULL) {
		printf("Error: Could not open description file (%s)\n", i_fn);
		return 1;
	}

	// Reboot MDC40 network
	if (Global_Network_Reboot() != RC_NO_ERROR) {
		printf("Error: Could not perform Global_Network_Reboot\n");
		return 1;
	}

	// Select C40 card using definition
	fscanf(i_fp, "%s\n", i_fn);
	fscanf(i_fp, "%x %x %x\n", &brd_base, &lia_base, &mod_site);
	if (Select_Board(brd_base, lia_base) != RC_NO_ERROR) {
		printf("Error: Could not select C40 according to definition\n");
		return (1);
	}

	// Load and execute name.OUT code onto module_site
	if (Load_And_Run_File(mod_site, i_fn, 0x32740010L, 0x3df22040L) != RC_NO_ERROR) {
		printf("Error: Could not load and execute file (%s)\n", i_fn);
		return (1);
	}

	// Setup user screen
	clrscr();
	putch('Ú');
	PUTCH('Ä', 78);
	putch('¿');
	putch('³');
	PUTCH(' ', 29);
	printf("AVISION - Version 2.0");
	PUTCH(' ', 28);
	putch('³');
	putch('³');
	PUTCH(' ', 20);
	printf("University of California, Santa Barbara");
	PUTCH(' ', 19);
	putch('³');
	putch('À');
	PUTCH('Ä', 78);
	putch('Ù');

	// Load C40 description & parameters
	while (fgets(i_fn, 256, i_fp))
		if (strncmp("PARAM", i_fn, 5) == 0) {
			sscanf(i_fn + 5, "%s %ld %ld %ld", p[p_num].txt,
			&p[p_num].min, &p[p_num].def, &p[p_num].max);
			p_num++;
		}
		else if (i_fn[0] != '\n') {
			printf("%s", i_fn);
			p_loc++;
		}
	fclose(i_fp);
	gotoxy(1, 21);
	PUTCH('Ä', 80);

	// Initialize C40 parameters
	upd_p_pntr(0);
	for (x = 0; x < p_num; x++) {
		upd_p_pntr(+1);
		upd_p_val(0);
	}
	upd_path();
	run_mode();

	// Initialize PTU

	// Initialize Terminal

	// Process user commands
	do {
		usr_cmd = 'X';
		if (kbhit()) usr_cmd = toupper(getch());
		switch (usr_cmd) {
			case '8':
				upd_p_pntr(-1);
				break;
			case '2':
				upd_p_pntr(+1);
				break;
			case '4':
				upd_p_val(-1);
				break;
			case '5':
				upd_p_val(0);
				break;
			case '6':
				upd_p_val(+1);
				break;
			case 'I':
				inp_mode();
				break;
			case 'O':
				out_mode();
				break;
			case 'R':
				run_mode();
				break;
			case 'P':
				upd_path();
				break;
			case 'X':
				execute();
				break;
		}
	} while (usr_cmd != 'Q');

	// Shutdown C40
	tx[0] = CMD_QUIT;
	WT_C40(tx, 1);
	clrscr();
	return 0;
}

void upd_p_pntr(int offset){

	// Erase old pointer
	gotoxy(1 + P_X, P_Y);
	printf(" ");

	// Update
	if (offset != 0) {
		p_pntr += offset;
		if (p_pntr < 0) p_pntr = p_num - 1;
		if (p_pntr >= p_num) p_pntr = 0;
	}
	else p_pntr = 0;

	// Display
	gotoxy(1 + P_X, P_Y);
	printf("¯");
}

void upd_p_val(int offset){

	// Update parameter on PC
	if (offset != 0) {
		p[p_pntr].val += offset;
		if (p[p_pntr].val > p[p_pntr].max) p[p_pntr].val = p[p_pntr].min;
		if (p[p_pntr].val < p[p_pntr].min) p[p_pntr].val = p[p_pntr].max;
	}
	else p[p_pntr].val = p[p_pntr].def;

	// Transmit C40 CMD_UPD Command
	tx[0] = CMD_UPD | p_pntr;
	tx[1] = p[p_pntr].val;
	WT_C40(tx, 2);

	// Display
	gotoxy(2 + P_X, P_Y);
	printf("%-10s = %+5ld", p[p_pntr].txt, p[p_pntr].val);
}

void execute(void){
long	speed;

	// Transmit C40 CMD_INP/OUT Command
	if (r_steps != steps && r_mode == 'S' || r_mode == 'C') {
		tx[0] = 0;
		gotoxy(74, 23);
		printf("%06ld", ++steps);
		if (i_mode != 'D')
			tx[0] = tx[0] | CMD_INP;
		if (o_mode != 'D')
			tx[0] = tx[0] | CMD_OUT;

		// Get User Interface Actions
		tx[0] |= 0;	//get_button();

		// Write PAI Command to C40
		WT_C40(tx, 1);

		// Write PAI Data to C40
		if (i_mode != 'D') pai_inp();

		// Read Tracking Data
		RD_C40(tx, 3);

		// Control Camera
		gotoxy(30, 24);
		printf("Control (%+4ld,%+4ld,%1ld) ",
			(long)tx[0],(long)tx[1],(long)tx[2]);
		if(tx[2]==1){
				speed=8*(abs(tx[0])+abs(tx[1]));
				ptu.Parameter(IOL_PTU::PAN_SPEED,speed);
				ptu.Parameter(IOL_PTU::TILT_SPEED,speed);
				ptu.Parameter(IOL_PTU::PAN_OFFSET,tx[0]);
				ptu.Parameter(IOL_PTU::TILT_OFFSET,-tx[1]);
		}
		if(tx[2]==2){
				ptu.Parameter(IOL_PTU::PAN_SPEED,2800);
				ptu.Parameter(IOL_PTU::TILT_SPEED,2800);
				ptu.Parameter(IOL_PTU::PAN_POSITION,0);
				ptu.Parameter(IOL_PTU::TILT_POSITION,0);
		}


		// Read PAI Data from C40
		if (o_mode != 'D')
			pai_out();
	}
}

void inp_mode(void)
{
	char	usr_cmd, cmd[255];

	// Prompt User for Mode
	gotoxy(1, 22);
	printf("INP MODE: K/F/D");

	// Handle Response
	i_fn[0] = cmd[0] = 0;
	fclose(i_fp);
	while (!kbhit());
	usr_cmd = toupper(getch());
	switch (usr_cmd) {
		case 'K':
			i_mode = 'K';
			break;
		case 'F':
			gotoxy(1, 23);
			printf("FILE: ");
			gets(cmd);
			sprintf(i_fn, "%s%s", PATH, cmd);
			i_fp = fopen(i_fn, "rb");
			fread(tx, 4, 1, i_fp);
			fseek(i_fp, 0, SEEK_SET);
			if (i_fp != NULL && tx[0] == 0x00494150) {
				i_mode = 'F';
				break;
			}
			else
				cmd[0] = 0;
		default:
			i_mode = 'D';
	}

	// Display Mode
	gotoxy(1, 22);
	printf("Inp Mode: %c      ", i_mode);
	gotoxy(1, 23);
	printf("File: %-20s", cmd);
}

void out_mode(void){

	char	usr_cmd;
	char	cmd[255];

	// Prompt User for Mode
	gotoxy(30, 22);
	printf("OUT MODE: S/F/D");

	// Handle Response
	o_fn[0] = cmd[0] = 0;
	fclose(o_fp);
	while (!kbhit()){}
	usr_cmd = toupper(getch());
	switch (usr_cmd) {
		case 'S':
			o_mode = 'S';
			break;
		case 'F':
			gotoxy(30, 23);
			printf("FILE: ");
			gets(cmd);
			sprintf(o_fn, "%s%s", PATH, cmd);
			if ((o_fp = fopen(o_fn, "wb")) != NULL) {
				o_mode = 'F';
				break;
			}
			else
				cmd[0] = 0;
		default:
			o_mode = 'D';
	}

	// Display Mode
	gotoxy(30, 22);
	printf("Out Mode: %c     ", o_mode);
	gotoxy(30, 23);
	printf("File: %-20s", cmd);
}

void run_mode(void){

	char	usr_cmd;
	char	cmd[255];

	// Prompt User for Mode
	gotoxy(60, 22);
	printf("RUN MODE: C/S");

	// Handle Response
	while (!kbhit());
	usr_cmd = toupper(getch());
	if (usr_cmd != 'S') r_mode = 'C';
	else {
		r_mode = 'S';
		gotoxy(60, 23);
		printf("STEPS: ");
		gets(cmd);
		cmd[6] = 0;
		r_steps = atol(cmd);
	}
	steps = 0;

	// Display Mode
	gotoxy(60, 22);
	printf("Run Mode: %c   ", r_mode);
	gotoxy(60, 23);
	printf("Steps: %06ld/", r_steps);
}

void upd_path(void){

	// Prompt User for PATH
	gotoxy(1, 24);
	printf("PATH: ");

	// Handle Response
	PATH[0] = 0;
	gets(PATH);
	if (PATH[0] == 0)
		strcpy(PATH, "c:\\");

	// Display PATH
	gotoxy(1, 24);
	printf("Path: %-40s", PATH);
	inp_mode();
	out_mode();
}


void pai_inp(void){

	// Check Header
	tx_app = tx_dim = tx_blk = 0;
	fread(tx, 4, 2, i_fp);
	if (tx[0] != 0x00494150) {
		fseek(i_fp, 0, SEEK_SET);
		fread(tx, 4, 2, i_fp);
	}

	tx_dim =(tx[1] & 0xffff0000) >> 16;
	tx_app =(tx[1] & 0x0000ffff);
	WT_C40(&tx[1], 1);

	// Transmit Dimensionality Structure
	for (x = 0; x < tx_dim; x++) {
		fread(tx, 4, 3, i_fp);
		WT_C40(tx, 3);
		if (x == 0)
			tx_blk =(tx[1] - tx[0] + tx[2]) / tx[2];
		else
			tx_blk *=(tx[1] - tx[0] + tx[2]) / tx[2];
	}

	gotoxy(1, 25);
	printf("A=%04lx D=%04lx B=%06ld", tx_app, tx_dim, tx_blk);

	// Transmit Data
	while (tx_blk > tx_size) {
		fread(tx, 4, tx_size, i_fp);
		WT_C40(tx, tx_size);
		tx_blk -= tx_size;
		gotoxy(15, 25);
		printf("B=%06ld", tx_blk);
	}
	if (tx_blk) {
		fread(tx, 4, tx_blk, i_fp);
		WT_C40(tx, tx_blk);
		tx_blk = 0;
	}
	gotoxy(15, 25);
	printf("B=%06ld", tx_blk);
}

void pai_out(void){

	// Check Header
	RD_C40(tx, 1);
	tx_app = tx_dim = tx_blk = 0;
	if (tx[0] != 0) {
		tx_dim =(tx[0] & 0xffff0000) >> 16;
		tx_app =(tx[0] & 0x0000ffff);
		tx[1] = 0x00494150;
		fwrite(&tx[1], 4, 1, o_fp);
		fwrite(&tx[0], 4, 1, o_fp);
	}
	else
		out_mode();

	// Receive Dimensionality Structure
	for (x = 0; x < tx_dim; x++) {
		RD_C40(tx, 3);
		fwrite(tx, 4, 3, o_fp);
		if (x == 0)
			tx_blk =(tx[1] - tx[0] + tx[2]) / tx[2];
		else
			tx_blk *=(tx[1] - tx[0] + tx[2]) / tx[2];
	}

	gotoxy(30, 25);
	printf("A=%04lx D=%04lx B=%06ld", tx_app, tx_dim, tx_blk);

	// Receive Data
	while (tx_blk > tx_size) {
		RD_C40(tx, tx_size);
		if (fwrite(tx, 4, tx_size, o_fp) < tx_size)
			exit(1);
		tx_blk -= tx_size;
		gotoxy(44, 25);
		printf("B=%06ld", tx_blk);
	}
	if (tx_blk) {
		RD_C40(tx, tx_blk);
		fwrite(tx, 4, tx_blk, o_fp);
		tx_blk = 0;
	}
	gotoxy(44, 25);
	printf("B=%06ld", tx_blk);
}