// Scanlib.c

//Includes
#include "scan.h"
#include "scanfile.h"

// COM Port Definitions
#define RECBUFFSIZE (2048)		//Make this bigger if you think you need it
#define XMITBUFFSIZE (2048)
ushort ReadData = 0x0000;
ushort ReadConfig = 0x4000;
ushort COM2_Config = 0xC429;				// 38400/O/8/1  (Odd parity generated in putchar())
ushort COM3_Config = 0xC40B;				// 9600/N/8/1

unsigned char *TopOfRecBuffer0, *HeadRecBuffer0, *TailRecBuffer0;
unsigned char *TopOfRecBuffer1, *HeadRecBuffer1, *TailRecBuffer1;


// QPB Definitions
QPBDev MAX146DevTemplate = 
	{
	MAX146_SLOT,		// qslot			our qspi slot
	"MAX146",			// devName			C string with device name (15 max)
	2000000,			// maxBaud			maximum baud rate in Hz for device
	16,					// bits				Bits Per Transfer
	iaLowSCK,			// clockPolar		SPI Clock Polarity
	captLead,			// clockPhase		SPI Clock Phase
	true,				// contCSMulti		Continue CS assert between mult xfrs
	true,				// autoTiming		Auto adjust timing to clock flag
	0,					// psDelaySCK		Min. Delay Before SCK (picoSecs)
	0,					// psDelayTXFR		Min. Delay After Transfer
	0,					// *rcvData			pointer to received data buffer
	0					// xfrCount			words transferred
	};

QPBDev U7DevTemplate = 
	{
	U7_SLOT,			// qslot			our qspi slot
	"U7",				// devName			C string with device name (15 max)
	2000000,			// maxBaud			maximum baud rate in Hz for device
	8,					// bits				Bits Per Transfer
	iaLowSCK,			// clockPolar		SPI Clock Polarity
	captLead,			// clockPhase		SPI Clock Phase
	false,				// contCSMulti		Continue CS assert between mult xfrs
	true,				// autoTiming		Auto adjust timing to clock flag
	0,					// psDelaySCK		Min. Delay Before SCK (picoSecs)
	0,					// psDelayTXFR		Min. Delay After Transfer
	0,					// *rcvData			pointer to received data buffer
	0					// xfrCount			words transferred
	};

QPBDev COM2DevStruct = 
	{
	COM2_SLOT,				// qslot				our qspi slot (non-multiplexed connection to PCS0 -- tps)
	"Max3100_0\0",			// devName			C string with device name (15 max)
	42500000,				// maxBaud			maximum baud rate in Hz for device
	16,						// bits				Bits Per Transfer
	iaLowSCK,				// clockPolar		SPI Clock Polarity (iaHighSCK,iaLowSCK)
	captLead,				// clockPhase		SPI Clock Phase (captLead/captFall)
	true,						// contCSMulti		Continue CS assert between mult xfrs
	true,						// autoTiming		Auto adjust timing to clock flag
	0,							// psDelaySCK		Min. Delay Before SCK (picoSecs)
	0,							// psDelayTXFR		Min. Delay After Transfer
	0,							// *rcvData			pointer to received data buffer
	0							// xfrCount			words transferred
	};

QPBDev COM3DevStruct = 
	{
	COM3_SLOT,				// qslot				our qspi slot (non-multiplexed connection to PCS0 -- tps)
	"Max3100_1\0",			// devName			C string with device name (15 max)
	42500000,				// maxBaud			maximum baud rate in Hz for device
	16,						// bits				Bits Per Transfer
	iaLowSCK,				// clockPolar		SPI Clock Polarity (iaHighSCK,iaLowSCK)
	captLead,				// clockPhase		SPI Clock Phase (captLead/captFall)
	true,						// contCSMulti		Continue CS assert between mult xfrs
	true,						// autoTiming		Auto adjust timing to clock flag
	0,							// psDelaySCK		Min. Delay Before SCK (picoSecs)
	0,							// psDelayTXFR		Min. Delay After Transfer
	0,							// *rcvData			pointer to received data buffer
	0							// xfrCount			words transferred
	};

QPBDev U1DevTemplate = 
	{
	U1_SLOT,			// qslot			our qspi slot
	"U1",				// devName			C string with device name (15 max)
	2000000,			// maxBaud			maximum baud rate in Hz for device
	8,					// bits				Bits Per Transfer
	iaLowSCK,			// clockPolar		SPI Clock Polarity
	captLead,			// clockPhase		SPI Clock Phase
	false,				// contCSMulti		Continue CS assert between mult xfrs
	true,				// autoTiming		Auto adjust timing to clock flag
	0,					// psDelaySCK		Min. Delay Before SCK (picoSecs)
	0,					// psDelayTXFR		Min. Delay After Transfer
	0,					// *rcvData			pointer to received data buffer
	0					// xfrCount			words transferred
	};

QPBDev U14DevTemplate = 
	{
	U14_SLOT,			// qslot			our qspi slot
	"U14",				// devName			C string with device name (15 max)
	2000000,			// maxBaud			maximum baud rate in Hz for device
	8,					// bits				Bits Per Transfer
	iaLowSCK,			// clockPolar		SPI Clock Polarity
	captLead,			// clockPhase		SPI Clock Phase
	false,				// contCSMulti		Continue CS assert between mult xfrs
	true,				// autoTiming		Auto adjust timing to clock flag
	0,					// psDelaySCK		Min. Delay Before SCK (picoSecs)
	0,					// psDelayTXFR		Min. Delay After Transfer
	0,					// *rcvData			pointer to received data buffer
	0					// xfrCount			words transferred
	};

QPBDev U5DevTemplate = 
	{
	U5_SLOT,			// qslot			our qspi slot
	"U5",				// devName			C string with device name (15 max)
	2000000,			// maxBaud			maximum baud rate in Hz for device
	8,					// bits				Bits Per Transfer
	iaLowSCK,			// clockPolar		SPI Clock Polarity
	captLead,			// clockPhase		SPI Clock Phase
	false,				// contCSMulti		Continue CS assert between mult xfrs
	true,				// autoTiming		Auto adjust timing to clock flag
	0,					// psDelaySCK		Min. Delay Before SCK (picoSecs)
	0,					// psDelayTXFR		Min. Delay After Transfer
	0,					// *rcvData			pointer to received data buffer
	0					// xfrCount			words transferred
	};

static QPB		*QPB_MAX146_Slot = NULL;
static QPB		*QPB_U7_Slot = NULL;
static QPB		*QPB_COM2_Slot = NULL;
static QPB		*QPB_COM3_Slot = NULL;
static QPB		*QPB_U1_Slot = NULL;
static QPB		*QPB_U14_Slot = NULL;
static QPB		*QPB_U5_Slot = NULL;


// MAX146 Definitions
enum { 	Max146CMD		= 0x80	};
enum { 	Max146SGL		= 0x04, 		Max146DIF		= 0x00	};
enum { 	Max146UNI		= 0x08, 		Max146BIP		= 0x00	};
enum { 	Max146PDFull	= 0x00,			Max146PDFast	= 0x01,
		Max146INT		= 0x02, 		Max146EXT		= 0x03	};


const uchar Max146SglChSel[8] = 	// convert chan to single-ended selector
		{ 0x00, 0x40, 0x10, 0x50, 0x20, 0x60, 0x30, 0x70 };
const uchar Max146DifChSel[8] = 	// convert chan to differential selector
		{ 0x00, 0x10, 0x20, 0x30, 0x40, 0x50, 0x60, 0x70 };


// Video Camera Definitions
unsigned char video_record[] = 	{3,0x20,0x02,0x22}; 
unsigned char video_stop[] = 	{3,0x20,0x00,0x20}; 
		

// Global Variables
extern struct def defaults;

int		g_grab_turns;
ushort 	g_U1_byte = 0;
ushort	g_U5_byte = 0x3F;
ushort	g_U7_byte = 0;
ushort	g_U14_byte = 0;

/*************************************************/

void init(void)
{

	PinWrite(33, 0);			// 5V BUS
	PinWrite(34, 0);			// LED1-ON
	PinWrite(35, 0);			// LED2-ON
	PinWrite(37, 0);			// STEP
	PinWrite(22, 0);			// CLEAR-FIFO
	PinWrite(23, 0);			// READ-FIFO
	PinWrite(25, 0);			// PING
	PinRead(24);				// PM_COUNTS
	PinRead(26);				// FILTER-INDEX
	PinRead(27);				// FOOTPAD
	PinRead(29);				// GRAB_LIMIT
	PinRead(32);				// GRAB_TURNS
	PinRead(31);				// Z_LIMIT
	PinRead(28);				// Y_LIMIT
	PinRead(30);				// X_LIMIT
	
	
	QPBInit(true);	
	QPB_U1_Slot = QPBInitSlot(&U1DevTemplate);		// Init U1 slot
	QPBTransact(QPB_U1_Slot, 0, 1, &g_U1_byte);		// All off

	QPB_U5_Slot = QPBInitSlot(&U5DevTemplate);		// Init U5 slot
	QPBTransact(QPB_U5_Slot, 0, 1, &g_U5_byte);		// Burnwires off, Stir power off

	QPB_U7_Slot = QPBInitSlot(&U7DevTemplate);		// Init U7 slot
	QPBTransact(QPB_U7_Slot, 0, 1, &g_U7_byte);		// All off

	QPB_U14_Slot = QPBInitSlot(&U14DevTemplate);	// Init U14 slot
	QPBTransact(QPB_U14_Slot, 0, 1, &g_U14_byte);		// All off

	QPB_MAX146_Slot = QPBInitSlot(&MAX146DevTemplate);	// Init MAX146 Slot
	
	
//	COM Port Initialization
	QPB_COM2_Slot = QPBInitSlot(&COM2DevStruct);	
	QPB_COM3_Slot = QPBInitSlot(&COM3DevStruct);	
	COM2Init();
	COM3Init();
	
//  Transceiver data port setup
	CS8Setup(0x00100000, 0x00000001, true);	// BaseAddr/size/16-bit
	CS8Options(true, false, false, 0);		// Read/Write/dsSync/wait

//  Grab turns interrupt setup
	CTSAEdgeSelect (SASM18B, false);		// Grab turns, negative edge
	CTSAInterruptFunction (SASM18B, &grab_turns_interrupt);
	
	PinWrite(33, 1);	// Turn on 5V bus
}

/****************************************************/

float battery(void)
{
	short sample;
	
	sample = Max146Sample(AD0, true, true, false);	// Chan, Uni, Sgl, Pd
	return ((float)sample) / BATTERY_CAL;
}

/***************************************************/

float backup_bat(void)
{
	short sample;
	
	sample = Max146Sample(AD1, true, true, false);	// Chan, Uni, Sgl, Pd
	return ((float)sample) / BACKUP_BAT_CAL;
}

/***************************************************
	COM Port Routines (MAX3111)
****************************************************/

// COM2 Interrupt Service Routine

IEV_C_FUNCT(COM2Interrupt)
{
	#pragma unused(ievstack)

	ushort RecDataPort0;
	
	//	Read data from Port0
	QPBTransact(QPB_COM2_Slot, 0, 1, &ReadData);	// Read data command
	RecDataPort0 = QPB_COM2_Slot->dev->rcvData[0];

	if(RecDataPort0 & 0x8000)		//	If R (bit 15) == 1
	{
		*HeadRecBuffer0 = (uchar)(RecDataPort0 & 0x00FF);	// Move lower 8 bits to the receive buffer
		HeadRecBuffer0++;
		if(HeadRecBuffer0 >= (char *)(TopOfRecBuffer0+RECBUFFSIZE))	// Loop around if end of ring buffer
		{
			HeadRecBuffer0 = TopOfRecBuffer0;
		}
	}

	
}


/*****************************************************/

void COM2Init(void)
	{

//	To handle our interupts from the MAX3100 we install this handler
	IEVInsertCFunct(&COM2Interrupt, (short) level2InterruptAutovector);


//	Buffers point nowhere to start
	TopOfRecBuffer0 = NULL;
	HeadRecBuffer0 = NULL;
	TailRecBuffer0 = NULL;

//	Allocate memory for the buffers
	TopOfRecBuffer0 = malloc(RECBUFFSIZE * sizeof(char));
	if(TopOfRecBuffer0 == NULL)
	{
		cprintf("Cannot allocate memory for receive buffer 0\n\n");
		BIOSResetToPicoDOS();
	}
	else
	{
		COM2Flush();	//Resets everthing
	}

//	Send the configuration command
	QPBTransact(QPB_COM2_Slot, 0, 1, &COM2_Config);	// Configure COM2

//	Enable -IRQ2 as a bus function (normally it is an I/O pin and by default it is an input)
	PIOBusFunct(41);

	}	// COM2Init()

/***************************************************/

int COM2ByteAvail(void)
{
	if(TailRecBuffer0 == HeadRecBuffer0) return 0;
	else return 1;
}


/********************************************************/

void COM2Flush(void)
{
	ushort SaveSR;
//	Disable interupts 
	SaveSR = IEVSaveSRThenDisable();

	HeadRecBuffer0 = TailRecBuffer0 = TopOfRecBuffer0;

//	Restore interupts to what they were
	IEVRestoreSavedSR(SaveSR);
}




/********************************************************/

int COM2GetChar(void)
{
	ushort SaveSR;
	unsigned char c;

//	Disable interupts 
	SaveSR = IEVSaveSRThenDisable();

	if(TailRecBuffer0 == HeadRecBuffer0)
	{
		c = -1;
	}
	else
	{
		c = *TailRecBuffer0;

		TailRecBuffer0++;

		if(TailRecBuffer0 >= (char *)(TopOfRecBuffer0+RECBUFFSIZE))
		{
			TailRecBuffer0 = TopOfRecBuffer0;
		}
	}

//	Restore interupts to what they were
	IEVRestoreSavedSR(SaveSR);

	return (int ) c;
}


/***************************************************/

bool COM2PutChar(ushort c)
{
	int odd = 0, bit;
	ushort txdata, parity_mask;
	
	txdata = c;
	for(bit=0;bit<8;bit++){				// Determine if output byte is odd or even
		if(txdata & 0x0001) odd = ~odd;
		txdata >>= 1;
	}
	if(odd) parity_mask = 0x8000;		// If byte is odd, clear parity bit for odd parity
	else parity_mask = 0x8100;
	
	// Loop here until tx buffer is empty
	do{
		QPBTransact(QPB_COM2_Slot, 0, 1, &ReadData);
		txdata = QPB_COM2_Slot->dev->rcvData[0]; 
	}while(!(txdata & 0x4000));	
		
	txdata = c & 0x00FF;
	txdata |= parity_mask;
	QPBTransact(QPB_COM2_Slot, 0, 1, &txdata);

	return true;	

}


/**********************************************************************/

// COM3 Interrupt Service Routine

IEV_C_PROTO(COM3Interrupt);
IEV_C_FUNCT(COM3Interrupt)
{
	#pragma unused(ievstack)

	ushort RecDataPort0;
	
	//	Read data from Port0
	QPBTransact(QPB_COM3_Slot, 0, 1, &ReadData);	// Read data command
	RecDataPort0 = QPB_COM3_Slot->dev->rcvData[0];

	if(RecDataPort0 & 0x8000)		//	If R (bit 15) == 1
	{
		*HeadRecBuffer1 = (uchar)(RecDataPort0 & 0x00FF);	// Move lower 8 bits to the receive buffer
		HeadRecBuffer1++;
		if(HeadRecBuffer1 >= (char *)(TopOfRecBuffer1+RECBUFFSIZE))	// Loop around if end of ring buffer
		{
			HeadRecBuffer1 = TopOfRecBuffer1;
		}
	}

	
}

/***************************************************/


void COM3Init(void)
	{

//	To handle our interupts from the MAX3111 we install this handler
	IEVInsertCFunct(&COM3Interrupt, (short) level5InterruptAutovector);


//	Buffers point nowhere to start
	TopOfRecBuffer1 = NULL;
	HeadRecBuffer1 = NULL;
	TailRecBuffer1 = NULL;

//	Allocate memory for the buffers
	TopOfRecBuffer1 = malloc(RECBUFFSIZE * sizeof(char));
	if(TopOfRecBuffer1 == NULL)
	{
		cprintf("Cannot allocate memory for receive buffer 1\n\n");
		BIOSResetToPicoDOS();
	}
	else
	{
		COM3Flush();	//Resets everthing
	}

	QPBTransact(QPB_COM3_Slot, 0, 1, &COM3_Config);	// Configure COM3

//	Enable -IRQ5 as a bus function
	PIOBusFunct(39);

	}	// COM3Init()

/*****************************************************/


int COM3ByteAvail(void)
{
	if(TailRecBuffer1 == HeadRecBuffer1) return 0;
	else return 1;
}


/********************************************************/

void COM3Flush(void)
{
	ushort SaveSR;
	SaveSR = IEVSaveSRThenDisable();

	HeadRecBuffer1 = TailRecBuffer1 = TopOfRecBuffer1;

	IEVRestoreSavedSR(SaveSR);
}


/********************************************************/

int COM3GetChar(void)
{
	ushort SaveSR;
	unsigned char c;

//	Disable interupts 
	SaveSR = IEVSaveSRThenDisable();

	if(TailRecBuffer1 == HeadRecBuffer1)
	{
		c = -1;
	}
	else
	{
		c = *TailRecBuffer1;

		TailRecBuffer1++;

		if(TailRecBuffer1 >= (char *)(TopOfRecBuffer1+RECBUFFSIZE))
		{
			TailRecBuffer1 = TopOfRecBuffer1;
		}
	}

//	Restore interupts to what they were
	IEVRestoreSavedSR(SaveSR);

	return (int ) c;
}


/***************************************************/

bool COM3PutChar(ushort c)
{
	ushort txdata;
	
	// Loop here until tx buffer is empty
	do{
		QPBTransact(QPB_COM3_Slot, 0, 1, &ReadData);
		txdata = QPB_COM3_Slot->dev->rcvData[0]; 
	}while(!(txdata & 0x4000));	
		
	txdata = (ushort)c & 0x00FF;
	txdata |= 0x8000;
	QPBTransact(QPB_COM3_Slot, 0, 1, &txdata);

	return true;	

}


/***************************************************/

void delay_secs(int secs)
{
	int x;
	
	for(x=0;x<secs;x++)	RTCDelayMicroSeconds(1000000);

}


/***************************************************/

int filter_motor(int position)
{
	int step;
	
	step = 0;
	while(PinRead(26))					// Move to index
	{
		PinWrite(37, 1);				// Set STEP pin
		RTCDelayMicroSeconds(10000);	// Set for maximum step rate
		PinWrite(37, 0);				// Clear STEP pin
		step++;
		if(step > 200)
		{
			cprintf("\nFilter motor timeout...\n");
			return 1;
		}
	}
	
	for(step=0;step<position;step++)		// Move to filter position
	{
		PinWrite(37, 1);				// Set STEP pin
		RTCDelayMicroSeconds(10000);	// Set for maximum step rate
		PinWrite(37, 0);				// Clear STEP pin
	}

	return 0;
}


/***************************************************/

float depth(void)
{

	short sample;
//	press_pwr_on();
	RTCDelayMicroSeconds(10000);	
	sample = Max146Sample(AD2, true, true, false);	// Chan, Uni, Sgl, Pd
//	press_pwr_off();
	return ((float)sample - defaults.depth_zero) / defaults.depth_cal;
}


/***************************************************/

void grab_motor(int dir)
{

	switch(dir)
	{
		case UP:
			U1_control(GRAB_UP, ON);		
			U1_control(GRAB_DOWN, OFF);		
			U1_control(GRAB_MOT_ON, ON);		
			break;
			
		case DOWN:
			U1_control(GRAB_UP, OFF);		
			U1_control(GRAB_DOWN, ON);		
			U1_control(GRAB_MOT_ON, ON);
			break;
			
		case STOP:
			U1_control(GRAB_MOT_ON, OFF);
			U1_control(GRAB_UP, OFF);		
			U1_control(GRAB_DOWN, OFF);		
			break;
			
	}
					

}

/***************************************************/


IEV_C_FUNCT(grab_turns_interrupt)
{
	#pragma unused(ievstack)

	g_grab_turns++;
}

/******************************************************************************\
**	Irq4RxISR			Interrupt handler for IRQ4 (tied to CMOS RxD)
**	
**	This single interrrupt service routine handles both the IRQ4 interrupt
**	and the very likely spurious interrupts that my be generated by the
**	repeated asynchronous and non-acknowledged pulses of RS-232 input.
**	
**	The handler simply reverts IRQ4 back to input (to prevent further level
**	sensative interrupts) and returns. It's assumed the routine that set this
**	up is just looking for the side effect of breaking the CPU out of STOP
**	or LPSTOP mode.
**	
**	Note that this very simple handler is defined as a normal C function
**	rather than an IEV_C_PROTO/IEV_C_FUNCT. We can do this because we know
**	(and can verify by checking the disassembly) that is generates only
**	direct addressing instructions and will not modify any registers.
\******************************************************************************/
void Irq4RxISR(void)
	{

	PinIO(IRQ4RXD);		// 31 // /IRQ4 (tied to Rx)
	RTE();

	}	//____ Irq4RxISR() ____//


/******************************************************************************/

short Max146Sample(ushort chan, bool uni, bool sgl, bool pd)
	{
	ushort		adcmd[3];
	short		count = 2;

//	Setup the command word we send to the Max146 which is a combination of
//	selector bits that pick the channel, mode, and clocking.
	adcmd[0] = adcmd[1] =
			 		(sgl	? (Max146SglChSel[chan] | Max146SGL)
							: (Max146DifChSel[chan] | Max146DIF))
				|	(uni	? Max146UNI : Max146BIP)
				|	Max146CMD;

	adcmd[0] |= Max146EXT;
	if (pd)
		{
		adcmd[1] |= Max146PDFast;
		adcmd[count++] = 0;
		}
	else
		adcmd[1] |= Max146EXT;
		
//	Perform a QSPI transaction with two transfers, the first sets up the next
//	conversion, and the second value clocks out the previous channel results.
//	We don't set the QPBASync bit in the command count so the function will
//	wait for the conversion to complete before returning.

	QPBTransact(QPB_MAX146_Slot, 0, count, adcmd);
//	The rcvData array contains the real result in its second word since word
//	one is the result of some previous conversion.

	return QPB_MAX146_Slot->dev->rcvData[1];

	}	//____ Max146Sample() ____//



/***************************************************/

void ping_cycle(int type)
{
	int sample;
	ulong delay_time;
	
	switch(type)
	{
		case ZERO_CAL:
		delay_time = 1000;		// 1 mS lets echos die down
		break;
		
		case NORMAL:
		delay_time = MAX_SAMPLE_TIME;	// Roughly 15 cm sample length 
		break;
		
		default:
		cprintf("\nUnrecognized ping_cycle command");
		break;
	}
	
	clear_fifo_on();
	clear_fifo_off();
	ping_on();
	RTCDelayMicroSeconds(delay_time);
	ping_off();

	for(sample=0; sample < MAX_SAMPLES; sample++)
	{
		read_fifo_on();
		read_fifo_off();		
		g_buffer[sample] = *((vushort *)0x00100000);
	}
	
}


/***************************************************/

ushort scan_ad_zero(void)
{
	int		sample;
	ushort	ad_zero;
	long	temp;	

	// Find the a/d converter zero point
	ping_cycle(ZERO_CAL);
	temp = 0;
	for(sample=MAX_SAMPLES-500; sample<MAX_SAMPLES; sample++)
	{
		temp += g_buffer[sample];
	}
	ad_zero = temp/500;
	cprintf("\nA/D zero = %d", ad_zero);
	return ad_zero;
}

/***************************************************/

int scan_cycle(int type)
{
	int x_cell, y_cell, max_x_cells, max_y_cells, x_cell_steps, y_cell_steps;
	int x_fluor_cell, y_fluor_cell, max_x_fluor_cells, max_y_fluor_cells;
	int	bin, bin_sample, sample, samples_per_bin;
	int elev_error, check_z_elev, z_elev, dir, z_elev_buff[MAX_Y_FLUOR_CELLS];
	char fname[15];
	FILE	*fp;
	ushort	ad_zero, ping_buff[END_BIN + 1];
	long temp;
	
	type = 0;

	max_x_fluor_cells = defaults.x_scan_len;	// Scan length in cm
	max_y_fluor_cells = defaults.y_scan_len;

	max_x_cells = 10/CELL_SIZE;	// Cell size in mm
	max_y_cells = 10/CELL_SIZE;

	x_cell_steps = CELL_SIZE * X_STEPS_PER_MM;
	y_cell_steps = CELL_SIZE * Y_STEPS_PER_MM;
	samples_per_bin = SAMPLES_PER_MM * BIN_LENGTH;
	
	kbflush();
  	
	// Return all scanner axes to limits
/*	cprintf("\nZ-axis to limit...\n");
	steps = 0;
	while(!z_limit())
	{
		scan_motor(Z, Z_UP, 1);
		steps++;
		if(steps > MAX_Z_STEPS)
		{
			cprintf("\nZ-axis motor error\n");
			return 1;
		}
	}
	
	cprintf("\nY-axis to limit...\n");
	steps = 0;
	while(!y_limit())
	{
		scan_motor(Y, BKWD, 1);
		steps++;
		if(steps > MAX_Y_STEPS)
		{
			cprintf("\nY-axis motor error\n");
			return 1;
		}
	}
	
	cprintf("\nX-axis to limit...\n");
	steps = 0;
	while(!x_limit())
	{
		scan_motor(X, BKWD, 1);
		steps++;
		if(steps > MAX_X_STEPS)
		{
			cprintf("\nX-axis motor error\n");
			return 1;
		}
	}
*/	

	
	ad_zero = scan_ad_zero();	// Find the a/d converter zero point
	z_elev = 0;

	// Scan an area
	for(x_fluor_cell=0; x_fluor_cell<max_x_fluor_cells; x_fluor_cell++)
	{	
		check_z_elev = 1;		// Run scan_z_cycle() to check elevation
		
		// Scan a 1 cm fluorometer swath
		for(x_cell=0; x_cell<max_x_cells; x_cell++)
		{
/*
			// Return Y-axis to limit
			cprintf("\nY-axis to limit...\n");
			steps = 0;
			while(!y_limit())
			{
				scan_motor(Y, BKWD, 1);
				steps++;
				if(steps > MAX_Y_STEPS) return 1;
			}
*/

			// Open a data file for the forward scan slice
			sprintf(fname, "%d.bin", (x_fluor_cell*10)+x_cell);
			if ((fp = fopen(fname, "wb")) == 0)
			{
				cprintf("\nCouldn't open data file!\n");
				return 1;
			}
			else cprintf("\nOpened %s", fname);
			
			
			// Scan the slice forward checking the Z elevation on each new X fluor cell
			for(y_fluor_cell=0; y_fluor_cell<max_y_fluor_cells; y_fluor_cell++)
			{
				cprintf("\nFWD y_fluor_cell = %d", y_fluor_cell);
				
				if(check_z_elev)
				{
					// Move the transducer down to 2 cm above the sediment 
					if(scan_z_cycle(ad_zero, &z_elev)) return 1;		// Bail out if there's a problem
					z_elev_buff[y_fluor_cell] = z_elev; 				// Remember the z-elevation
					cprintf("\nScan_z_cycle = %d", z_elev);
				}

				else
				{
					// Figure out how many steps to move the Z axis from stored data
					elev_error = z_elev_buff[y_fluor_cell] - z_elev;
					if(elev_error >= 0) dir = Z_DOWN;
					else if(elev_error < 0) dir = Z_UP;
					scan_motor(Z, dir, abs(elev_error * Z_STEPS_PER_MM));
					z_elev += elev_error;
					cprintf("\nz_elev = %d", z_elev);
				}

				// Store z elevation in mm with ping data
				ping_buff[0] = z_elev_buff[y_fluor_cell];		
				
				// Scan within a 1cm fluorometer cell
				for(y_cell=0; y_cell<max_y_cells; y_cell++)
				{
					if(kbhit()) return 0;
					cprintf("\ny_cell = %d", y_cell);
					
					// Ping a column
					ping_cycle(NORMAL);
				
					// Rectify and sum the samples
					sample = START_BIN * SAMPLES_PER_MM * BIN_LENGTH;
					for(bin=START_BIN; bin<=END_BIN; bin++)
					{
						temp = 0;
						for(bin_sample=0; bin_sample<samples_per_bin; bin_sample++)
						{
							temp += abs(g_buffer[sample] - ad_zero);
							sample++;
						}
						ping_buff[bin] = (ushort)(temp/samples_per_bin); 
					}
					fwrite(ping_buff, sizeof(ushort), END_BIN+1, fp);
			
					// Move Y axis one cell forward
					scan_motor(Y, FWD, y_cell_steps);
				}										// End of forward y_cell
			}											// End of forward y_fluor_cell
			fclose(fp);									// Done with the forward scan slice file
		
			// Move x-axis one cell forward
			scan_motor(X, FWD, x_cell_steps);
			x_cell++;
			check_z_elev = 0;	// Don't need to check elevation after first slice
			
			// Open a data file for the backward scan slice
			sprintf(fname, "%d.bin", (x_fluor_cell*10)+x_cell);
			if ((fp = fopen(fname, "wb")) == 0)
			{
				cprintf("\nCouldn't open data file!\n");
				return 1;
			}
			else cprintf("\nOpened %s", fname);
			
			y_fluor_cell--;		// Decrement once to line things up right
			
			// Scan the slice backward changing the Z elevation from stored data
			for(y_fluor_cell; y_fluor_cell>=0; y_fluor_cell--)
			{
				cprintf("\nBKWD y_fluor_cell = %d", y_fluor_cell);
	
				// Figure out how many steps to move the Z axis using stored data
				elev_error = z_elev_buff[y_fluor_cell] - z_elev;
				if(elev_error >= 0) dir = Z_DOWN;
				else if(elev_error < 0) dir = Z_UP;
				scan_motor(Z, dir, abs(elev_error * Z_STEPS_PER_MM));
				z_elev += elev_error;
				cprintf("\nz_elev = %d", z_elev);
					
				ping_buff[0] = z_elev_buff[y_fluor_cell];		// Store z elevation with ping data

				// Scan the slice backward
				for(y_cell=max_y_cells-1; y_cell>=0; y_cell--)
				{
					if(kbhit()) return 0;
					cprintf("\ny_cell = %d", y_cell);
		
					// Ping a column
					ping_cycle(NORMAL);
		
					// Rectify and sum the samples
					sample = START_BIN * SAMPLES_PER_MM * BIN_LENGTH;
					for(bin=START_BIN; bin<=END_BIN; bin++)
					{
						temp = 0;
						for(bin_sample=0; bin_sample<samples_per_bin; bin_sample++)
						{
							temp += abs(g_buffer[sample] - ad_zero);
							sample++;
						}
						ping_buff[bin] = (ushort)(temp/samples_per_bin); 
					}
					fwrite(ping_buff, sizeof(ushort), END_BIN+1, fp);
				
					// Move Y axis one cell forward
					scan_motor(Y, BKWD, y_cell_steps);
				}											// End of backward y_cell
			}												// End of backward y_fluor_cell
			fclose(fp);	// Done with the backward scan slice file

			// Move x-axis one cell forward
			scan_motor(X, FWD, x_cell_steps);
		
		}	// End of x_cell
	}	// End of x_fluor_cell


	return 0;	
}


/***************************************************/

void scan_motor(int motor, int dir, int steps)
{
	int step;
	
	switch(motor){
		case X:
			x_motor_on();
			break;
		
		case Y:
			y_motor_on();
			break;
			
		case Z:
			z_motor_on();
			break;
				
		default:
			cprintf("\nUnrecognized motor command...\n");
			
	}  

	if(dir == FWD) dir_on();		// Set motor direction
	else dir_off();
	
	for(step=0;step<steps;step++)
	{
		PinWrite(37, 1);						// Set STEP pin
		RTCDelayMicroSeconds(SCAN_STEP_PER/2);	// Set for maximum step rate
		PinWrite(37, 0);						// Clear STEP pin
		RTCDelayMicroSeconds(SCAN_STEP_PER/2);	// Set for maximum step rate
	}

	switch(motor){
		case X:
			x_motor_off();
			break;
		
		case Y:
			y_motor_off();
			break;
			
		case Z:
			z_motor_off();
			break;
				
		default:
			cprintf("\nUnrecognized motor command...\n");
			
	}  

}

/***************************************************/

int scan_z_cycle(ushort ad_zero, int *z_elev) 
{
	int sample, error=0, bin, elev_error, try, retry, steps;  
	ulong ad_sum;

	// z_elev and elev_error are in mm
	
	kbflush();

	
	for(retry=0; retry < 5; retry++)
	{
		for(try=0; try<10; try++)
		{
			ping_cycle(NORMAL);
		
			// Look through the samples for the closest echo
			for(bin=10; bin<=50; bin++)
			{
				ad_sum = 0;
				
				// Sum the samples for a 1mm bin
				for(sample = SAMPLES_PER_MM*bin; sample<SAMPLES_PER_MM*(bin+1); sample++)
				{
					ad_sum += abs(g_buffer[sample] - ad_zero);
				}
				cprintf("\nad_sum = %lu", ad_sum);
				
				
				// Look for an echo and adjust the z elevation 
				if(ad_sum > defaults.z_threshold)
				{
					elev_error = bin - 30;
					cprintf("\nelev_error = %d", elev_error);
					if(elev_error < 0) scan_motor(Z, Z_UP, Z_STEPS_PER_MM * abs(elev_error));
					else scan_motor(Z, Z_DOWN, Z_STEPS_PER_MM * elev_error);
					*z_elev += elev_error;
					return 0;		// Success!
				}
			}
			
			cprintf("\nNo echo...moving down 1 cm");
			scan_motor(Z, Z_DOWN, Z_STEPS_PER_MM*10);	// Move down 1cm and try again
			*z_elev += 10;
		}

		// No echo found...retry the Z scan cycle
		cprintf("\nRetrying Z scan cycle...Z-axis to limit");
		steps = 0;
		while(!z_limit())
		{
			scan_motor(Z, Z_UP, 1);
			steps++;
			if(steps > MAX_Z_STEPS)
			{
				cprintf("\nZ-axis motor error");
				return 1;
			}
			if(kbhit()) break;
		}
		*z_elev = 0;
	}

	RTCDelayMicroSeconds(10000);	// Let the motor settle

	return error;
}

/***************************************************/

void sleep_with_break(void)
{
	// Shut down all power

	QPBTransact(QPB_U1_Slot, 0, 1, &g_U1_byte);		// All off
	QPBTransact(QPB_U5_Slot, 0, 1, &g_U5_byte);		// Burnwires off, Stir power off
	QPBTransact(QPB_U7_Slot, 0, 1, &g_U7_byte);		// All off
	QPBTransact(QPB_U14_Slot, 0, 1, &g_U14_byte);	// All off
	PinWrite(33, 0);								// Turn off 5V bus

	// Install the interrupt handlers that will break us out from keystroke.
	IEVInsertAsmFunct(Irq4RxISR, level4InterruptAutovector);
	IEVInsertAsmFunct(Irq4RxISR, spuriousInterrupt);

    cprintf("\n\nPress any key to wake from sleep...");

	PITSet51msPeriod(PITOff);	// disable timer (drops power)
	CTMRun(false);				// turn off CTM6 module
	SCITxWaitCompletion();		// let any pending UART data finish
	EIAForceOff(true);			// turn off the RS232 driver
	QSMStop();					// shut down the QSM
	CFEnable(false);			// turn off the CompactFlash card
	PinBus(IRQ4RXD);			// make it an interrupt pin

	LPStopCSE(LPMODE);			// we're here until interrupted

	EIAForceOff(false);			// turn on the RS232 driver
	QSMRun();					// bring back the QSM
	CFEnable(true);				// turn on the CompactFlash card
	ciflush();					// discard any garbage characters

	PinWrite(33, 1);			// Turn on 5V bus
	cprintf(" Awake\n");			// tell 'em we're back

}

/***************************************************/

void stir_motor(ushort speed, int on_off)
{
#pragma unused(on_off)	

	*CTMCModulusReg(MCSM30) = speed;
	CTDASetupOPWM(DASM26, false, false, 14, 0, 8192);	// Pin,busB,posB,bits,le,te
	cprintf("\nStir motor running...press any key to stop\n");
	TMGSetSpeed(320);				// Slow down clock
	CTMPrescalerRun(true);	// Start the prescaler
	stir_5V_on();
	kbflush();
	while(!kbhit())	delay_secs(1);	// Wait here till key hit
	stir_5V_off();
	TMGSetSpeed(16000);
	PinWrite(37, 0);			// Stop PWM on STEP pin


	
}

/***************************************************/

void store_data(int type, FILE *file_ptr)
{
	char time_buf[80];
	time_t *temp_time = NULL;
	
	switch(type)
	{
		case DEPLOY_DATA: 
		time(temp_time);
		strftime(time_buf, 80, "%y/%m/%d %H:%M:%S", localtime(temp_time));
		fprintf(file_ptr, "\n%s%t%f%t%f", time_buf, depth(), battery());		// Time, Depth, Batt
		break;
	}
}

/***************************************************/

void test_scan_motor(int motor)
{
	char in_key;
	ushort distance = 1, steps;
	
	do
	{
  		cprintf("\n\n\nF = Forward (Z up)");
  		cprintf("\nB = Backward (Z down)");
  		cprintf("\nE = Exit to Test Menu");
  		cprintf("\n\nEnter Selection: ");

  		kbflush();
		in_key = (char)tolower( getch() );       // Get command in lower case
		switch(in_key)
		{
			case 'f':
				QRushort("\n\nEnter distance (mm)",	"%u", true, &distance, 1, 200);
  				switch(motor)
  				{
  					case X:
  						steps = distance * X_STEPS_PER_MM;
  						break;
  					case Y:
  						steps = distance * Y_STEPS_PER_MM;
  						break;
  					case Z:
  						steps = distance * Z_STEPS_PER_MM;
  						break;
  				}
  				scan_motor(motor, FWD, steps);
  				break;

    		case 'b':
				QRushort("\n\nEnter distance (mm)",	"%u", true, &distance, 1, 200);
  				switch(motor)
  				{
  					case X:
  						steps = distance * X_STEPS_PER_MM;
  						break;
  					case Y:
  						steps = distance * Y_STEPS_PER_MM;
  						break;
  					case Z:
  						steps = distance * Z_STEPS_PER_MM;
  						break;
  				}
      			scan_motor(motor, BKWD, steps);
      			break;
    		
    		case 'e':
      			break;
		}

  		printf("\n\nBattery Voltage: %.2f volts\n", battery());
		if(	x_limit() ) cprintf("\nX-axis limit!");
		if(	y_limit() ) cprintf("\nY-axis limit!");
		if(	z_limit() ) cprintf("\nZ-axis limit!");
  
	} while(in_key != 'e');

}



/***************************************************/

void U1_control(ushort device, char on_off)
{

	if(on_off) g_U1_byte |= device;
	else g_U1_byte &= ~device;
	
	QPBTransact(QPB_U1_Slot, 0, 1, &g_U1_byte);
	
}
	

/***************************************************/

void U5_control(ushort device, char on_off)
{
	
	if(on_off) g_U5_byte |= device;
	else g_U5_byte &= ~device;
	
	QPBTransact(QPB_U5_Slot, 0, 1, &g_U5_byte);
	
}
	

/***************************************************/

void U7_control(ushort device, char on_off)
{
	
	if(on_off) g_U7_byte |= device;
	else g_U7_byte &= ~device;
	
	QPBTransact(QPB_U7_Slot, 0, 1, &g_U7_byte);
	
}
	

/***************************************************/

void U14_control(ushort device, char on_off)
{
	
	if(on_off) g_U14_byte |= device;
	else g_U14_byte &= ~device;
	
	QPBTransact(QPB_U14_Slot, 0, 1, &g_U14_byte);
	
}
	

/***********************************************************/

void video(short function)
{
	com2_on();	
	delay_secs(1);

  	switch(function){

    	case VIDEO_START:
    		video_pwr_on();
	   		delay_secs(10);
    		video_send_packet(video_record);
    		cprintf("\nvideo started\n");
    		break;

    	case VIDEO_STOP:
    		video_send_packet(video_stop);
	   		delay_secs(10);
    		video_pwr_off();
    		cprintf("\nvideo stopped\n");
    		break;

    	default:
      		cprintf("\nUnrecognized video command\n");
  	}
  
	com2_off();
}


/****************************************************/

void video_send_packet(unsigned char *packet)
{
  	int msglen; 		//This function sends a data packet to the video camera
 
  	msglen = *packet;
  	packet++;

  	while(msglen){
  		COM2PutChar(*packet);
   		RTCDelayMicroSeconds(500);
   		packet++;
  		msglen--;
  }
}