//////////////////////////////////////////////////////////////////////
// UCSB - Electrical and Computer Engineering Dept.
//
// IOL_PTU (IOL Pan Tilt Unit Class)
//
// Version  When      Who                    Why
// -------  ----      ---                    ---
// 1.0      02/01/00  Daryl Fortney          Initial Design
//
// This class supports IOL_PTU access under UNIX & DOS platforms.
// It accomplishes this through the UART.
//
// Copyright 2000 UCSB.  All rights reserved.
//////////////////////////////////////////////////////////////////////
// Definitions
//////////////////////////////////////////////////////////////////////
#ifndef IOL_PTU_CPP
#define IOL_PTU_CPP

#include <iol\ptu\ptu.hpp>

void IOL_PTU::IOL_PTU(char *id,long mode){

	L1.Open(id,mode);
	L1.Timeout(0);
	Flush();
	Config();
printf("PTU Configured\n");
}

void IOL_PTU::~IOL_PTU(void){
}

//////////////////////////////////////////////////////////////////////
// Maintainance Functions
//////////////////////////////////////////////////////////////////////

//
// Print(): Simply prints the IOL_PTU Attributes to CERR.
//
void	IOL_PTU::Print(void){
#ifdef IOL_PTU_PRINT_FLOWS
cerr<<"IOL_PTU("<<this<<")::Print()"<<endl;
#endif

	#ifdef IOL_PTU_PRINT_PRINT
	cerr<<"IOL_PTU: ";

	// Check For Open UART
	if(L1.Status()==IOL_OPEN){
		cerr<<"Id = "<<Id()<<" Mode = "<<hex<<Mode()<<dec<<endl
			 <<"========================================"<<endl
			 <<"Pan Speed       RW = "<<Speed(PAN)<<endl
			 <<"Pan Position    RW = "<<Position(PAN)<<endl
			 <<"Tilt Speed      RW = "<<Speed(TILT)<<endl
			 <<"Tilt Position   RW = "<<Position(TILT)<<endl;
	}else
		cerr<<"CLOSED"<<endl;
	#endif
}

//////////////////////////////////////////////////////////////////////
// Attribute Functions
//////////////////////////////////////////////////////////////////////

//
// Config(): Configures IOL_PTU for communication.
//
void	IOL_PTU::Config(void){
#ifdef IOL_PTU_PRINT_FLOWS
cerr<<"IOL_PTU("<<this<<")::Config()"<<endl;
#endif

	Tx("ED ",128,' '); Rx();
	Tx("FT ",128,' '); Rx();
	Tx("I ",128,' '); Rx();
	Tx("PU2800 ",128,' '); Rx();
	Tx("TU2800 ",128,' '); Rx();
}

//
// Parameter(num): Returns parameter value.
//
long	IOL_PTU::Parameter(long num){
#ifdef IOL_PTU_PRINT_FLOWS
cerr<<"IOL_PTU("<<this<<")::Parameter("<<num<<")"<<endl;
#endif

	// Tx Command (Possibly Throw Error)
	switch(num){
		case PAN_SPEED:		Tx("PS ",128,' '); break;
		case PAN_POSITION:	Tx("PP ",128,' '); break;
		case TILT_SPEED:	Tx("TS ",128,' '); break;
		case TILT_POSITION:	Tx("TP ",128,' '); break;
		default:
			#ifdef IOL_PTU_CHECKS
			#ifdef IOL_PTU_PRINT_ERRORS
			cerr<<"IOL ERROR: IOL_PTU("<<this<<")::Parameter("<<num<<")"<<endl;
			#endif
			throw(IOL_ERROR);
			#endif
			break;
	}

	// Rx Parameter Echo
	char	rx[128];
	Rx(rx);

	// Extract Parameter Value (LF SPACE value CR)
	long	value;
	sscanf(rx+2," %ld",&value);

	return value;
}

//
// Parameter(num,value): Sets parameter value.
//
long	IOL_PTU::Parameter(long num,long value){
#ifdef IOL_PTU_PRINT_FLOWS
cerr<<"IOL_PTU("<<this<<")::Parameter("<<num<<","<<value<<")"<<endl;
#endif

	// Tx Command (Possibly Throw Error)
	// ALL COMMANDS TERMINATE WITH SPACE!
	char	tx[128];
	switch(num){
		case PAN_SPEED:		sprintf(tx,"PS%ld ",value); break;
		case PAN_POSITION:	sprintf(tx,"PP%ld ",value); break;
		case PAN_OFFSET:	sprintf(tx,"PO%ld ",value); break;
		case TILT_SPEED:	sprintf(tx,"TS%ld ",value); break;
		case TILT_POSITION:	sprintf(tx,"TP%ld ",value); break;
		case TILT_OFFSET:	sprintf(tx,"TO%ld ",value); break;
		default:
			#ifdef IOL_PTU_CHECKS
			#ifdef IOL_PTU_PRINT_ERRORS
			cerr<<"IOL ERROR: IOL_PTU("<<this<<")::Parameter("<<num<<","<<value<<")"<<endl;
			#endif
			throw(IOL_ERROR);
			#endif
			break;
	}
	Tx(tx,128,' ');

	// Trash Command Echo
	return Rx();
}

//////////////////////////////////////////////////////////////////////
// Tx & Rx Functions
//////////////////////////////////////////////////////////////////////

//
// Tx(): Transmits IOL_PTU Command over UART.
//
long	IOL_PTU::Tx(void *block,long num,long until){
#ifdef IOL_PTU_PRINT_FLOWS
cerr<<"IOL_PTU("<<this<<")::Tx(blk,"<<num<<","<<until<<")"<<endl;
#endif

	// Tx Block Through UART Channel
	L1.Tx((char*)block,num,until);

	return IOL_NOERR;
}

//
// Rx(): Receives IOL_PTU Response over UART.
//
long	IOL_PTU::Rx(void *block,long num,long until){
#ifdef IOL_PTU_PRINT_FLOWS
cerr<<"IOL_PTU("<<this<<")::Rx(blk,"<<num<<","<<until<<")"<<endl;
#endif

	// Trash Response
	if(block==NULL){
		char	blk[128];
		long	n=L1.Rx(blk,128,until);

		// Possibly Return Command Error
		for(long i=0;i<n;i++)
			if(blk[i]=='!'){
			#ifdef IOL_PTU_PRINT_ERRORS
				cerr<<"COMMAND ERROR: IOL_PTU("<<this<<")"<<endl;
			#endif
				return IOL_ERROR;
			}

		return IOL_NOERR;
	}else{

		// Keep Response
		char	*blk=(char *)block;
		long	n=L1.Rx(blk,num,until);

		// Possibly Return Command Error
		for(long i=0;i<n;i++) if(blk[i]=='!'){
			#ifdef IOL_PTU_PRINT_ERRORS
			cerr<<"COMMAND ERROR: IOL_PTU("<<this<<")"<<endl;
			#endif
			return IOL_ERROR;
		}

		return IOL_NOERR;
	}
}

#endif