//-----------------------------------------------------------------------------
// Remote Control and Sync Board for Four GoPro Hero 2 Cameras
// Copyright 2012 by Glen Akins.
// All rights reserved.
//
// To program EEPROM jumper SDA and SCL lines from CPU to EEPROM as shown in 
// the table below and short the corresponding write enable pins together to 
// allow EEPROM writes.
//
//           CPU     CAM1    CAM2    CAM3     CAM4    
//           ------  ------  ------  -------  -------
// SCL:      J18-1   J1-12   J5-12   J10-12   J14-12
// SDA:      J18-2   J1-11   J5-11   J10-11   J14-11
// WR ENAB:  N/A     J4-1,2  J9-1,2  J13-1,2  J17-1,2
//
// GoPro 3D Slave Mode is enabled by erasing the entire EEPROM to 0xFF then
// writing a 0x05 to address zero.
//


//-----------------------------------------------------------------------------
// includes
//

#include <p24HJ128GP204.h>
#include <math.h>
#include "console.h"
#include "i2c.h"
#include "compass.h"


//-----------------------------------------------------------------------------
// config bits
//

_FOSCSEL (FNOSC_PRI);
_FOSC    (FCKSM_CSECMD & OSCIOFNC_OFF & POSCMD_EC);
_FWDT    (FWDTEN_OFF & WINDIS_OFF)


//-----------------------------------------------------------------------------
// defines
//

// baud rates
#define FCY 40000000
#define U1BAUDRATE 115200
#define U1BRGVAL (((FCY/U1BAUDRATE)/16) - 1)

// command processor
#undef  ECHO			// define to echo characters on com port
#undef  PROMPT			// define to send prompt at beginning of lines
#define CMD_MAXLEN 72

// I/O
#define LED1			LATCbits.LATC3			// RC3  output
#define BUTTON			PORTBbits.RB11			// RB11 input
#define REMOTE			PORTBbits.RB4			// RB4  input

// CAM 1
#define CAM1_ID1_IN		PORTCbits.RC5			// RC5  input
#define CAM1_ID2_OUT	LATBbits.LATB6			// RB6  output
#define CAM1_ID3_OUT	LATBbits.LATB5			// RB5  output
#define CAM1_TRIG_OUT	LATBbits.LATB7			// RB7  output
#define CAM1_POWER_IN	PORTCbits.RC6			// RC6  input
#define CAM1_MODE_OUT	LATCbits.LATC4			// RC4  output

// CAM 2
#define CAM2_ID1_IN		PORTCbits.RC8			// RC8  input
#define CAM2_ID2_OUT	LATBbits.LATB9			// RB9  output
#define CAM2_ID3_OUT	LATBbits.LATB8			// RB8  output
#define CAM2_TRIG_OUT	LATCbits.LATC9			// RC9  output
#define CAM2_POWER_IN	PORTCbits.RC7			// RC7  input
#define CAM2_MODE_OUT	LATBbits.LATB10			// RB10 output

// CAM 3
#define CAM3_ID1_IN		PORTCbits.RC0			// RC0  input
#define CAM3_ID2_OUT	LATCbits.LATC1			// RC1  output
#define CAM3_ID3_OUT	LATCbits.LATC2			// RC2  output
#define CAM3_TRIG_OUT	LATBbits.LATB2			// RB2  output
#define CAM3_POWER_IN	PORTBbits.RB3			// RB3  input
#define CAM3_MODE_OUT	LATAbits.LATA8			// RA8  output

// CAM 4
#define CAM4_ID1_IN		PORTBbits.RB14			// RB14 input
#define CAM4_ID2_OUT	LATBbits.LATB15			// RB15 output
#define CAM4_ID3_OUT	LATAbits.LATA1			// RA1  output
#define CAM4_TRIG_OUT	LATAbits.LATA0			// RA0  output
#define CAM4_POWER_IN	PORTAbits.RA7			// RA7  input
#define CAM4_MODE_OUT	LATAbits.LATA10			// RA10 output

// UART
// RXD = RB13/RB13								// RB13 input
// TXD = RP12/RB12								// RB12 output

// I2C (software implementation)
// SCL = RA4									// RA4  output
// SDA = RA9									// RA9	i/o (init to output)

#define CAMERA_ON_DELAY				50
#define CAMERA_OFF_DELAY			200
#define CAMERA_MODE_DELAY			25


//-----------------------------------------------------------------------------
// prototypes
//

// timer tick initiailization
void TickInit (void);

// command handling functions
void GetCommand (void);
void ProcessCommand (void);
unsigned char IsCommand (char *s1, unsigned char l1);
unsigned char fromhex (unsigned char ch);
void Get32BitsFromCommand (unsigned long *data);
void Get8BitsFromCommand (unsigned char *data);
void Get16BitsFromCommand (unsigned short *data);

// delay functions
void delay_us (unsigned char x);
void DelayTicks (unsigned char ticks);

// external eeprom access functions
void WriteCommand (void);
void ReadCommand (void);
void ProgramCommand (void);
void DumpCommand (void);

// basic camera control / debug Commands
void PowerCommand (void);
void OffCommand (void);
void DetectCommand (void);
void ModeCommand (void);
void SnapCommand (void);

// mid-level camera control functions
unsigned char cam_Detect (unsigned char which);
unsigned char cam_GetPower (unsigned char which);
void cam_PowerOn (unsigned char which);
void cam_PowerOff (unsigned char which);

// low-level camera control functions
void cam1_PowerOn (void);
void cam2_PowerOn (void);
void cam3_PowerOn (void);
void cam4_PowerOn (void);
void cam1_PowerOff (void);
void cam2_PowerOff (void);
void cam3_PowerOff (void);
void cam4_PowerOff (void);


//-----------------------------------------------------------------------------
// globals
//

// flags, timers, counters
unsigned char flag50Hz = 0;
unsigned char timer1 = 0;
unsigned char ledTimer = 0;
unsigned char compassTimer = 0;

// command processing variables
unsigned char cmd_buffer[CMD_MAXLEN];
unsigned char cmd_length = 0;
unsigned char cmd_state = 0;
unsigned char word_begin = 0;
unsigned char word_end = 0;
unsigned char word_length = 0;

// camera states
unsigned char camera_count;
unsigned char camera_detects;


//-----------------------------------------------------------------------------
// main
//

int main (void)
{
	// set PLL to multiply external clock by 8
	// Fosc will be 80MHz and Fcyc will be 40MHz.
	PLLFBD = 30;
	CLKDIVbits.PLLPOST = 0;
	CLKDIVbits.PLLPRE  = 0;

	// switch to EC + PLL	
	__builtin_write_OSCCONH (0x03);
	__builtin_write_OSCCONL (0x01);

	// wait for PLL to lock
	while (OSCCONbits.LOCK != 0b1) {
	}

	// wait for oscillator to switch to EC + PLL
	while (OSCCONbits.COSC != 0b011) {
	}

	// unlock peripheral pin select and leave it unlocked
	__builtin_write_OSCCONL (OSCCON & ~(1<<6));

	// initialize timer 1 to generate 50Hz timebase
	TickInit ();

	// set AD converter inputs to digial I/O mode
	AD1PCFGL = 0b0001111111111111;
	
	// init data direction registers
	TRISCbits.TRISC9  = 0; // CAM2_TRIG_OUT  -- output
	TRISCbits.TRISC8  = 1; // CAM2_ID1_IN    -- input
	TRISCbits.TRISC7  = 1; // CAM2_POWER_IN  -- input
	TRISCbits.TRISC6  = 1; // CAM1_POWER_IN  -- input
	TRISCbits.TRISC5  = 1; // CAM1_ID1_IN    -- input
	TRISCbits.TRISC4  = 0; // CAM1_MODE_OUT  -- output
	TRISCbits.TRISC3  = 0; // LED1           -- output
	TRISCbits.TRISC2  = 0; // CAM3_ID3_OUT   -- output
	TRISCbits.TRISC1  = 0; // CAM3_ID2_OUT   -- output
	TRISCbits.TRISC0  = 1; // CAM3_ID1_IN    -- input
	TRISBbits.TRISB15 = 0; // CAM4_ID2_OUT   -- output
	TRISBbits.TRISB14 = 1; // CAM4_ID1_IN    -- input
	TRISBbits.TRISB13 = 1; // RXD            -- input
	TRISBbits.TRISB12 = 0; // TXD            -- output
	TRISBbits.TRISB11 = 1; // BUTTON         -- input
	TRISBbits.TRISB10 = 0; // CAM2_MODE_OUT  -- output
	TRISBbits.TRISB9  = 0; // CAM2_ID2_OUT   -- output
	TRISBbits.TRISB8  = 0; // CAM2_ID3_OUT   -- output
	TRISBbits.TRISB7  = 0; // CAM1_TRIG_OUT  -- output
	TRISBbits.TRISB6  = 0; // CAM1_ID2_OUT   -- output
	TRISBbits.TRISB5  = 0; // CAM1_ID3_OUT   -- output
	TRISBbits.TRISB4  = 1; // REMOTE         -- input
	TRISBbits.TRISB3  = 1; // CAM3_POWER_IN  -- input
	TRISBbits.TRISB2  = 0; // CAM3_TRIG_OUT  -- output
	TRISAbits.TRISA10 = 0; // CAM4_MODE_OUT  -- output
	TRISAbits.TRISA9  = 0; // SDA            -- output
	TRISAbits.TRISA8  = 0; // CAM3_MODE_OUT  -- output
	TRISAbits.TRISA7  = 1; // CAM4_POWER_IN  -- input
	TRISAbits.TRISA4  = 0; // SCL            -- output
	TRISAbits.TRISA1  = 0; // CAM4_ID3_OUT   -- output
	TRISAbits.TRISA0  = 0; // CAM4_TRIG_OUT  -- output

	// init uart1: 9600 bps at Fosc=80MHz, 8N1, no flow control
	RPINR18bits.U1RXR = 13;		// connect RP13 to U1RXD
	RPOR6bits.RP12R = 3;		// connect U1TXD to RP12
	U1MODE = 0b1000000000000000;
	U1STA = 0b0000010000000000;
	U1BRG = U1BRGVAL;

	// initialize camera outputs
	CAM1_ID2_OUT	= 1; // high CAM 1
	CAM1_ID3_OUT	= 0; // low
	CAM1_TRIG_OUT	= 1; // high
	CAM1_MODE_OUT	= 0; // low
	CAM2_ID2_OUT	= 1; // high CAM 2
	CAM2_ID3_OUT	= 0; // low
	CAM2_TRIG_OUT	= 1; // high
	CAM2_MODE_OUT	= 0; // low
	CAM3_ID2_OUT	= 1; // high CAM 3
	CAM3_ID3_OUT	= 0; // low
	CAM3_TRIG_OUT	= 1; // high
	CAM3_MODE_OUT	= 0; // low
	CAM4_ID2_OUT	= 1; // high CAM 4
	CAM4_ID3_OUT	= 0; // low
	CAM4_TRIG_OUT	= 1; // high
	CAM4_MODE_OUT	= 0; // low

	// initiailze i2c i/o
	i2c_init ();
	
	// intialize console I/O
	ConsoleInit ();

	// initialize compass
	compass_Init ();

	// initialize camera control variables
	camera_count = 0;
	camera_detects = 0;

	// main loop
	while (1) {

		// transmit dmx, update dmx state, and process counters at 50Hz
		if (flag50Hz) {
			// clear flag
			flag50Hz = 0;

			// 
			// periodic tasks go here
			// 

			// blink the LED at 1Hz
			ledTimer++;
			if (ledTimer == 25) {
				LED1 = ~LED1;
				ledTimer = 0;
			}

			// read accelerometer and magnetometer at 5Hz
			compassTimer++;
			if (compassTimer == 10) {
				compass_TakeMeasurement ();
				compassTimer = 0;
			}
		}

		// check uart1 command buffer for new characters
		// and append any to current command buffer
		GetCommand ();
		if (cmd_state == 2) {
			ProcessCommand ();
		}

		// clear any receiver overrun conditions
		if (U1STAbits.OERR) {
			U1STAbits.OERR = 0;
		}
	}

	return 0;
}


//-----------------------------------------------------------------------------
// TickInit
//

void TickInit(void)
{
	// 1:256 prescale
	T1CONbits.TCKPS = 3;
	// Base
	PR1 = 3125;
	// Clear counter
	TMR1 = 0;

	// Enable timer interrupt
	IPC0bits.T1IP = 2;	// Interrupt priority 2 (low)
	IFS0bits.T1IF = 0;
	IEC0bits.T1IE = 1;

	// Start timer
	T1CONbits.TON = 1;
}


//-----------------------------------------------------------------------------
// T1 ISR
//

void _ISR __attribute__((__no_auto_psv__)) _T1Interrupt(void)
{
	// Set 50Hz flag
	flag50Hz = 1;

	// decrement timers
	if (timer1 != 0) timer1--;

	// Reset interrupt flag
	IFS0bits.T1IF = 0;
}


//-----------------------------------------------------------------------------
// command processing
//

void GetCommand (void)
{
	short ch;

	if (cmd_state == 0) {
		cmd_length = 0;
		cmd_buffer[cmd_length] = 0;
#ifdef PROMPT
		putstring ("GPSlave> ");
#endif
		cmd_state++;
	} else if (cmd_state == 1) {
		// get character
		ch = getchar ();
		
		// process character
		if (ch >= 0) {
			if (ch == 0x0d) {							// return
				// carriage return and linefeed
#ifdef ECHO
				putchar (0x0d);
				putchar (0x0a);
#endif
				cmd_state++;
			} else if (ch == 0x08) {					// backspace
				if (cmd_length > 0) {
#ifdef ECHO
					putchar (0x08);
					putchar (' ');
					putchar (0x08);
#endif
					cmd_buffer[--cmd_length] = 0;
				}
			} else if (ch == 0x15) {					// ctrl-u is rub out
				while (cmd_length > 0) {
#ifdef ECHO
					putchar (0x08);
					putchar (' ');
					putchar (0x08);
#endif
					cmd_buffer[--cmd_length] = 0;
				}
			} else if (ch >= 0x20 && ch <= 0x7e) {		// printable characters
				if (cmd_length < (CMD_MAXLEN - 1)) {
#ifdef ECHO
					putchar (ch);
#endif
					cmd_buffer[cmd_length++] = ch;
					cmd_buffer[cmd_length] = 0;
				}
			}
		}
	}
}


void ProcessCommand (void)
{
	unsigned char address;
	unsigned char data;

	// find beginning of first word
	word_begin = 0;
	while ((cmd_buffer[word_begin] == ' ') && (word_begin < cmd_length)) {
		word_begin++;
	}

	// find end of first word
	word_end = word_begin;
	while ((cmd_buffer[word_end] != ' ') && (word_end < cmd_length)) {
		word_end++;
	}

	// find length of first word
	word_length = word_end - word_begin;

	// IsCommand accesses the global variables word_begin and word_length


	// EEPROM Programming Commands
	if (IsCommand ("wr", 2)) {				// write one byte in eeprom
		WriteCommand ();
	} else if (IsCommand ("rd", 2)) {		// read one byte from eeprom
		ReadCommand ();
	} else if (IsCommand ("prog", 4)) {		// program entire eeprom
		ProgramCommand ();
	} else if (IsCommand ("dump", 4)) {		// dump entire eeprom
		DumpCommand ();
	} 


	// Primary Camera Control Commands
	else if (IsCommand ("power", 5)) {	// get power state of all cameras
		PowerCommand ();
	} else if (IsCommand ("off", 3)) {		// turn off all cameras
		OffCommand ();
	} else if (IsCommand ("detect", 6)) {	// detect all cameras
		DetectCommand ();
	} else if (IsCommand ("mode", 4)) {		// press mode button on all cameras
		ModeCommand ();
	} else if (IsCommand ("snap", 4)) {		// take a snapshot with all cameras
		SnapCommand ();
	}

	// Secondary Camera Control Commands
	else if (IsCommand ("id3", 3)) {
		Get8BitsFromCommand (&data);
		if (data == 0) {
			CAM1_ID3_OUT = 0;
			CAM2_ID3_OUT = 0;
			CAM3_ID3_OUT = 0;
			CAM4_ID3_OUT = 0;
			putstringcrlf ("ID3 <= 0");
		} else {
			CAM1_ID3_OUT = 1;
			CAM2_ID3_OUT = 1;
			CAM3_ID3_OUT = 1;
			CAM4_ID3_OUT = 1;
			putstringcrlf ("ID3 <= 1");
		}
	} else if (IsCommand ("id2", 3)) {
		Get8BitsFromCommand (&data);
		if (data == 0) {
			CAM1_ID2_OUT = 0;
			CAM2_ID2_OUT = 0;
			CAM3_ID2_OUT = 0;
			CAM4_ID2_OUT = 0;
			putstringcrlf ("ID2 <= 0");
		} else {
			CAM1_ID2_OUT = 1;
			CAM2_ID2_OUT = 1;
			CAM3_ID2_OUT = 1;
			CAM4_ID2_OUT = 1;
			putstringcrlf ("ID2 <= 1");
		}
	} else if (IsCommand ("trig", 4)) {
		Get8BitsFromCommand (&data);
		if (data == 0) {
			CAM1_TRIG_OUT = 0;
			CAM2_TRIG_OUT = 0;
			CAM3_TRIG_OUT = 0;
			CAM4_TRIG_OUT = 0;
			putstringcrlf ("TRIG <= 0");
		} else {
			CAM1_TRIG_OUT = 1;
			CAM2_TRIG_OUT = 1;
			CAM3_TRIG_OUT = 1;
			CAM4_TRIG_OUT = 1;
			putstringcrlf ("TRIG <= 1");
		}
	} 

	// accel, magne, gyro register access commands
	else if (IsCommand ("wg", 2)) {
		Get8BitsFromCommand (&address);
		Get8BitsFromCommand (&data);
		WriteGyro (address, data);
		putstringcrlf ("ok");
	} else if (IsCommand ("rg", 2)) {
		Get8BitsFromCommand (&address);
		data = ReadGyro (address);
		puthex8 (data);
		putstringcrlf ("");
	} else if (IsCommand ("wa", 2)) {
		Get8BitsFromCommand (&address);
		Get8BitsFromCommand (&data);
		WriteAccel (address, data);
		putstringcrlf ("ok");
	} else if (IsCommand ("ra", 2)) {
		Get8BitsFromCommand (&address);
		data = ReadAccel (address);
		puthex8 (data);
		putstringcrlf ("");
	} else if (IsCommand ("wm", 2)) {
		Get8BitsFromCommand (&address);
		Get8BitsFromCommand (&data);
		WriteMagne (address, data);
		putstringcrlf ("ok");
	} else if (IsCommand ("rm", 2)) {
		Get8BitsFromCommand (&address);
		data = ReadMagne (address);
		puthex8 (data);
		putstringcrlf ("");
	} 

	// compass commands
	else if (IsCommand ("calibrate", 9)) {
		compass_Calibrate ();
	} else if (IsCommand ("bearing", 7)) {
		compass_ReportBearing ();
	}

	// miscellaneous commands
	else if (IsCommand ("?", 1)) {
		putstringcrlf ("Remote Control and Sync Board for Four GoPro Hero 2 Cameras");
		putstringcrlf ("Copyright 2012 by Glen Akins. All rights reserved.");
	}

	// misc calibration commands
	else if (IsCommand ("defcal", 6)) {
		compass_SetDefaultCalibration ();
	}
	else if (IsCommand ("rdcal", 5)) {
		compass_ReadCalibration ();
	}
	else if (IsCommand ("wrcal", 5)) {
		compass_WriteCalibration ();
	}
	else if (IsCommand ("ldcal", 5)) {
		short a,b,c,d,e,f;
		Get16BitsFromCommand (&a);
		Get16BitsFromCommand (&b);
		Get16BitsFromCommand (&c);
		Get16BitsFromCommand (&d);
		Get16BitsFromCommand (&e);
		Get16BitsFromCommand (&f);
		compass_LoadCalibration (a,b,c,d,e,f);
	}
	else if (IsCommand ("prcal", 5)) {
		compass_PrintCalibration ();
	}

	cmd_state = 0;
}


unsigned char IsCommand (char *s1, unsigned char l1)
{
	unsigned char s2;
	unsigned char l2;

	s2 = word_begin;
	l2 = word_length;

	if (l1 != l2) {
		return 0;
	}

	while (l1--) {
		if (*s1++ != cmd_buffer[s2++]) {
			return 0;
		}
	}

	return 1;
}


unsigned char fromhex (unsigned char ch)
{
	if (ch >= '0' && ch <= '9')
		return ch - '0';
	if (ch >= 'a' && ch <= 'f')
		return ch - 'a' + 10;
	if (ch >= 'A' && ch <= 'F')
		return ch - 'A' + 10;
	return 0;
}


void Get32BitsFromCommand (unsigned long *data)
{
	unsigned char i;

	// find beginning of parameter
	word_begin = word_end;
	while ((cmd_buffer[word_begin] == ' ') && (word_begin < cmd_length)) {
		word_begin++;
	}

	// find end of parameter
	word_end = word_begin;
	while ((cmd_buffer[word_end] != ' ') && (word_end < cmd_length)) {
		word_end++;
	}

	// find length of parameter
	word_length = word_end - word_begin;

	*data = 0;

	for (i = word_begin; i < word_end; i++) {
		*data <<= 4;
		*data |= fromhex (cmd_buffer[i]);
	}
}


void Get16BitsFromCommand (unsigned short *data)
{
	unsigned char i;

	// find beginning of key
	word_begin = word_end;
	while ((cmd_buffer[word_begin] == ' ') && (word_begin < cmd_length)) {
		word_begin++;
	}

	// find end of key
	word_end = word_begin;
	while ((cmd_buffer[word_end] != ' ') && (word_end < cmd_length)) {
		word_end++;
	}

	// find length of key
	word_length = word_end - word_begin;

	*data = 0;

	for (i = word_begin; i < word_end; i++) {
		*data <<= 4;
		*data |= fromhex (cmd_buffer[i]);
	}
}


void Get8BitsFromCommand (unsigned char *data)
{
	unsigned char i;

	// find beginning of key
	word_begin = word_end;
	while ((cmd_buffer[word_begin] == ' ') && (word_begin < cmd_length)) {
		word_begin++;
	}

	// find end of key
	word_end = word_begin;
	while ((cmd_buffer[word_end] != ' ') && (word_end < cmd_length)) {
		word_end++;
	}

	// find length of key
	word_length = word_end - word_begin;

	*data = 0;

	for (i = word_begin; i < word_end; i++) {
		*data <<= 4;
		*data |= fromhex (cmd_buffer[i]);
	}
}


//-----------------------------------------------------------------------------
// External EEPROM Access Functions
//

void WriteCommand (void)
{
	unsigned char address;
	unsigned char data;

	Get8BitsFromCommand (&address);
	Get8BitsFromCommand (&data);

	i2c_start ();
	// control code = 1010, block address = XXX, R/W_n = 0
	i2c_send_byte (0b10100000);			
	i2c_send_byte (address);
	i2c_send_byte (data);
	i2c_stop ();

	putstring ("addr: ");
	puthex8 (address);
	putstring (", ");
	putstring ("data: ");
	puthex8 (data);
	putstringcrlf ("");
}


void ReadCommand (void)
{
	unsigned char address;
	unsigned char data;
	Get8BitsFromCommand (&address);

	i2c_start ();
	// control code = 1010, block address = XXX, R/W_n = 0
	i2c_send_byte (0b10100000);			
	i2c_send_byte (address);
	i2c_start ();
	// control code = 1010, block address = XXX, R/W_n = 1
	i2c_send_byte (0b10100001);			
	data = i2c_read_byte ();
	i2c_stop ();

	putstring ("addr: ");
	puthex8 (address);
	putstring (", ");
	putstring ("data: ");
	puthex8 (data);
	putstringcrlf ("");
}


void ProgramCommand (void)
{
	unsigned char address;
	unsigned char i;

	for (address = 0; address < 128; address++) {
		i2c_start ();
		i2c_send_byte (0b10100000);			
		i2c_send_byte (address);
		if (address == 0) i2c_send_byte (0x05);
		else i2c_send_byte (0xFF);
		i2c_stop ();
		putchar ('.');
		for (i = 0; i < 100; i++) {
			delay_us (255);
		}
	}
	putstringcrlf ("");
}


void DumpCommand (void)
{
	unsigned char address;
	unsigned char data;

	for (address = 0; address < 128; address++) {
		i2c_start ();
		// control code = 1010, block address = XXX, R/W_n = 0
		i2c_send_byte (0b10100000);			
		i2c_send_byte (address);
		i2c_start ();
		// control code = 1010, block address = XXX, R/W_n = 1
		i2c_send_byte (0b10100001);			
		data = i2c_read_byte ();
		i2c_stop ();
		puthex8 (data);
		putchar (' ');
		if ((address & 0xf) == 0xf) {
			putstringcrlf ("");
		}
	}
}


void delay_us (unsigned char x)
{
	while (x--) {
		Nop ();
		Nop ();
		Nop ();
		Nop ();
		Nop ();
		Nop ();
		Nop ();
		Nop ();
		Nop ();
		Nop ();
		Nop ();
		Nop ();
		Nop ();
		Nop ();
		Nop ();
		Nop ();
	}
}


//-----------------------------------------------------------------------------
// Basic Camera Control / Debug Commands
//

void PowerCommand (void)
{
	unsigned char camera;
	unsigned char state;

	putchar ('$');
	putchar ('P');
	putchar (',');
	for (camera = 1; camera <= 4; camera++) {
		state = cam_GetPower (camera);
		if (state) {
			putchar ('1');
		} else {
			putchar ('0');
		}
		if (camera != 4) {
			putchar (',');
		}
	}
	putchar (0x0d);
	putchar (0x0a);
}


void OffCommand (void)
{
	unsigned char camera;

	putchar ('$');
	putchar ('F');

	Get8BitsFromCommand (&camera);

	if (camera == 0) {
		if (cam_GetPower (1)) cam_PowerOff (1);
		if (cam_GetPower (2)) cam_PowerOff (2);
		if (cam_GetPower (3)) cam_PowerOff (3);
		if (cam_GetPower (4)) cam_PowerOff (4);
	} else {
		cam_PowerOff (camera);
	}

	camera_count = 0;
	camera_detects = 0;

	putchar (0x0d);
	putchar (0x0a);
}


void DetectCommand (void)
{
	unsigned char camera;

	putchar ('$');
	putchar ('D');
	putchar (',');

	camera_count = 0;
	camera_detects = 0;

	for (camera = 1; camera <= 4; camera++) {
		if (cam_Detect (camera)) {
			camera_count++;
			camera_detects |= (1 << (camera - 1));
			putchar ('1');
		} else {
			putchar ('0');
		}
		if (camera != 4) {
			putchar (',');
		}
	}

	putchar (0x0d);
	putchar (0x0a);
}


void ModeCommand (void)
{
	putchar ('$');
	putchar ('M');

	CAM1_MODE_OUT = 1;
	CAM2_MODE_OUT = 1;
	CAM3_MODE_OUT = 1;
	CAM4_MODE_OUT = 1;
	timer1 = CAMERA_MODE_DELAY;
	while (timer1 != 0) {
	}
	CAM1_MODE_OUT = 0;
	CAM2_MODE_OUT = 0;
	CAM3_MODE_OUT = 0;
	CAM4_MODE_OUT = 0;

	putchar (0x0d);
	putchar (0x0a);
}


void SnapCommand (void)
{
	unsigned char i;
	unsigned char init_id1_states;
	unsigned char cameras_ready;
	
	putchar ('$');
	putchar ('S');
	putchar (',');

	// wait up to five seconds for command to complete
	timer1 = 250;

	// save initial state of ID1 for each camera
	init_id1_states = 0;
	if (CAM1_ID1_IN) init_id1_states |= 1;
	if (CAM2_ID1_IN) init_id1_states |= 2;
	if (CAM3_ID1_IN) init_id1_states |= 4;
	if (CAM4_ID1_IN) init_id1_states |= 8;

	// set id3 high for all cameras
	CAM1_ID3_OUT = 1;
	CAM2_ID3_OUT = 1;
	CAM3_ID3_OUT = 1;
	CAM4_ID3_OUT = 1;

	// delay about 24ms
	for (i = 0; i < 180; i++) {
		delay_us (255);
	}

	// set id2 low for all cameras
	CAM1_ID2_OUT = 0;
	CAM2_ID2_OUT = 0;
	CAM3_ID2_OUT = 0;
	CAM4_ID2_OUT = 0;

	// wait for id1 to change to opposite of initial state on detected cameras
	do {
		// no cameras ready
		cameras_ready = 0;

		// see if camera 1 is attached
		if (camera_detects & 1) {
			// attached -- see if ID1 state is opposite from initial ID1 state
			if (init_id1_states & 1) {
				if (!CAM1_ID1_IN) cameras_ready++;
			} else {
				if (CAM1_ID1_IN) cameras_ready++;
			}
		}

		// see if camera 2 is attached
		if (camera_detects & 2) {
			// attached -- see if ID1 state is opposite from initial ID1 state
			if (init_id1_states & 2) {
				if (!CAM2_ID1_IN) cameras_ready++;
			} else {
				if (CAM2_ID1_IN) cameras_ready++;
			}
		}

		// see if camera 3 is attached
		if (camera_detects & 4) {
			// attached -- see if ID1 state is opposite from initial ID1 state
			if (init_id1_states & 4) {
				if (!CAM3_ID1_IN) cameras_ready++;
			} else {
				if (CAM3_ID1_IN) cameras_ready++;
			}
		}

		// see if camera 4 is attached
		if (camera_detects & 8) {
			// attached -- see if ID1 state is opposite from initial ID1 state
			if (init_id1_states & 8) {
				if (!CAM4_ID1_IN) cameras_ready++;
			} else {
				if (CAM4_ID1_IN) cameras_ready++;
			}
		}

		// check for timeout
		if (timer1 == 0) {
			putchar ('+');
			puthex8 (camera_detects);
			puthex8 (camera_count);
			puthex8 (init_id1_states);
			puthex8 (cameras_ready);
			putchar (' ');
			break;
		}

	} while (cameras_ready != camera_count);

	// set id2 high on all cameras
	CAM1_ID2_OUT = 1;
	CAM2_ID2_OUT = 1;
	CAM3_ID2_OUT = 1;
	CAM4_ID2_OUT = 1;

	// wait for id1 to return to its initial state on all attached cameras
	do {
		// no cameras ready
		cameras_ready = 0;

		// see if camera 1 is attached
		if (camera_detects & 1) {
			// attached -- see if ID1 state is same as initial ID1 state
			if (init_id1_states & 1) {
				if (CAM1_ID1_IN) cameras_ready++;
			} else {
				if (!CAM1_ID1_IN) cameras_ready++;
			}
		}

		// see if camera 2 is attached
		if (camera_detects & 2) {
			// attached -- see if ID1 state is same as initial ID1 state
			if (init_id1_states & 2) {
				if (CAM2_ID1_IN) cameras_ready++;
			} else {
				if (!CAM2_ID1_IN) cameras_ready++;
			}
		}

		// see if camera 3 is attached
		if (camera_detects & 4) {
			// attached -- see if ID1 state is same as initial ID1 state
			if (init_id1_states & 4) {
				if (CAM3_ID1_IN) cameras_ready++;
			} else {
				if (!CAM3_ID1_IN) cameras_ready++;
			}
		}

		// see if camera 4 is attached
		if (camera_detects & 8) {
			// attached -- see if ID1 state is same as initial ID1 state
			if (init_id1_states & 8) {
				if (CAM4_ID1_IN) cameras_ready++;
			} else {
				if (!CAM4_ID1_IN) cameras_ready++;
			}
		}

		// check for timeout
		if (timer1 == 0) {
			putchar ('-');
			puthex8 (camera_detects);
			puthex8 (init_id1_states);
			puthex8 (cameras_ready);
			putchar (' ');
			break;
		}

	} while (cameras_ready != camera_count);

	// delay about 24ms
	for (i = 0; i < 180; i++) {
		delay_us (255);
	}

	// pulse trigger low to take the picture
	CAM1_TRIG_OUT = 0;
	CAM2_TRIG_OUT = 0;
	CAM3_TRIG_OUT = 0;
	CAM4_TRIG_OUT = 0;

	// delay a bit to make a short pulse
	delay_us (20);

	// set trigger high to finish the pulse
	CAM1_TRIG_OUT = 1;
	CAM2_TRIG_OUT = 1;
	CAM3_TRIG_OUT = 1;
	CAM4_TRIG_OUT = 1;

/*
	// delay about 24ms
	for (i = 0; i < 180; i++) {
		delay_us (255);
	}

	// set id2 high on all cameras
	CAM1_ID2_OUT = 1;
	CAM2_ID2_OUT = 1;
	CAM3_ID2_OUT = 1;
	CAM4_ID2_OUT = 1;
*/

	// delay about 24ms
	for (i = 0; i < 180; i++) {
		delay_us (255);
	}

	// set id3 low
	CAM1_ID3_OUT = 0;
	CAM2_ID3_OUT = 0;
	CAM3_ID3_OUT = 0;
	CAM4_ID3_OUT = 0;

	if (timer1 == 0) {
		putstring ("ERR");
	} else {
		putstring ("OK");
	}

	putchar (0x0d);
	putchar (0x0a);
}


//-----------------------------------------------------------------------------
// mid-level camera control functions
//

unsigned char cam_Detect (unsigned char which)
{
	unsigned char state;

	// is camera on?
	state = cam_GetPower (which);

	// if camera is on, try to turn it off
	if (state) {
		cam_PowerOff (which);
		DelayTicks (100);
		state = cam_GetPower (which);
		// if camera is still on, detection failed
		if (state) {
			return 0;
		}
	}

	// camera should be off now, try to turn it on
	cam_PowerOn (which);
	DelayTicks (100);
	state = cam_GetPower (which);

	// if camera is not on, detection failed
	if (!state) {
		return 0;
	}

	// camera found
	return 1;
}


unsigned char cam_GetPower (unsigned char which)
{
	unsigned char state;

	switch (which) {
		case 1: state = CAM1_POWER_IN; break;
		case 2: state = CAM2_POWER_IN; break;
		case 3: state = CAM3_POWER_IN; break;
		case 4: state = CAM4_POWER_IN; break;
		default: putstringcrlf ("Bad camera number"); return 0;
	}

	return state;
}


void cam_PowerOn (unsigned char which)
{
	switch (which) {
		case 1: cam1_PowerOn (); break;
		case 2: cam2_PowerOn (); break;
		case 3: cam3_PowerOn (); break;
		case 4: cam4_PowerOn (); break;
		default: putstringcrlf ("Bad camera number"); return;
	}
}

void cam_PowerOff (unsigned char which)
{
	switch (which) {
		case 1: cam1_PowerOff (); break;
		case 2: cam2_PowerOff (); break;
		case 3: cam3_PowerOff (); break;
		case 4: cam4_PowerOff (); break;
		default: putstringcrlf ("Bad camera number"); return;
	}
}


//-----------------------------------------------------------------------------
// low-level camera control functions
//

void cam1_PowerOn (void)
{
	CAM1_MODE_OUT = 1;
	timer1 = CAMERA_ON_DELAY;
	while (timer1 != 0) {
	}
	CAM1_MODE_OUT = 0;
}

void cam1_PowerOff (void)
{
	CAM1_MODE_OUT = 1;
	timer1 = CAMERA_OFF_DELAY;
	while (timer1 != 0) {
	}
	CAM1_MODE_OUT = 0;
}

void cam2_PowerOn (void)
{
	CAM2_MODE_OUT = 1;
	timer1 = CAMERA_ON_DELAY;
	while (timer1 != 0) {
	}
	CAM2_MODE_OUT = 0;
}

void cam2_PowerOff (void)
{
	CAM2_MODE_OUT = 1;
	timer1 = CAMERA_OFF_DELAY;
	while (timer1 != 0) {
	}
	CAM2_MODE_OUT = 0;
}

void cam3_PowerOn (void)
{
	CAM3_MODE_OUT = 1;
	timer1 = CAMERA_ON_DELAY;
	while (timer1 != 0) {
	}
	CAM3_MODE_OUT = 0;
}

void cam3_PowerOff (void)
{
	CAM3_MODE_OUT = 1;
	timer1 = CAMERA_OFF_DELAY;
	while (timer1 != 0) {
	}
	CAM3_MODE_OUT = 0;
}

void cam4_PowerOn (void)
{
	CAM4_MODE_OUT = 1;
	timer1 = CAMERA_ON_DELAY;
	while (timer1 != 0) {
	}
	CAM4_MODE_OUT = 0;
}

void cam4_PowerOff (void)
{
	CAM4_MODE_OUT = 1;
	timer1 = CAMERA_OFF_DELAY;
	while (timer1 != 0) {
	}
	CAM4_MODE_OUT = 0;
}

void DelayTicks (unsigned char ticks)
{
	timer1 = ticks;
	while (timer1 != 0) {
	}
}