//----------------------------------------------------------------------------
// C main line
//----------------------------------------------------------------------------

#include <m8c.h>        // part specific constants and macros
#include <string.h>
//#include <I2CHW_1Common.h>
//#include <I2CHW_1mstr.h>

#include "PSoCAPI.h"    // PSoC API definitions for all User Modules
#include "ed_host_int.h"
#include "ed_pad_int.h"
#include "ed_slave_int.h"
#include "util.h"
#include "pad.h"
#include "main.h"
#include "error.h"
#include "config.h"
#include "slave.h"

#define I2CTIMEOUT 5000

unsigned char hostbuf[HOSTBUFSIZE];
unsigned char hostbufpos=0;
unsigned char hostbufchk=0;

unsigned char padbuf[PADBUFSIZE];
unsigned char padbufpos=0;
unsigned char padbufchk=0;

unsigned char slavebuf[SLAVEBUFSIZE];
unsigned char slavebufpos=0;
unsigned char slavebufchk=0;

unsigned char slavestate[SLAVESTATEBUFSIZE];

unsigned char prt0andmask=0xff, prt0ormask=0x00;
unsigned char prt2andmask=0xff, prt2ormask=0x00;

unsigned char pad_joyx, pad_joyy, pad_buttons;
unsigned char pad_move_up, pad_move_down, pad_move_left, pad_move_right;
unsigned short pad_battery_voltage;

unsigned char pinmode[16];
unsigned char pad_ack_flag=0;

unsigned int usrange[2];
unsigned char uslastport=0;
unsigned char uspingcount=0;

unsigned char slave_ack=0;

unsigned short diag_button_count=0;

unsigned char indiag=0;			// are we in diagnostics mode?

unsigned char commerrs=0;		// comm error counter

// if 1, then each packet will be followed by an
// event character
unsigned char eventenable=1; 		

long gyrocount=0;
unsigned int lastgyrotime=0;

unsigned int lastPadReceivedTime=0;

//********************************************************
// GLOBALS
//******************************************************** 
unsigned int  analog[10];		// we store all of our analog values here, with 2_1 and 2_2 at 8 and 9.
unsigned char analogFilter[10];
unsigned char analogNext;	    // which port are we sampling?
unsigned char analogDelay;      // how many samples have we skipped since updating our main bank of A/Ds?

unsigned char dac[1];

unsigned char clockgenstate[3];

unsigned char allstopenabled=1;
unsigned char allowhost=1;

extern void diag(void);
extern unsigned char selftest(void);

unsigned char badhostchars=0;
unsigned char badslavechars=0;
unsigned char badpadchars=0;

unsigned int lastcounter;

//********************************************************
// FUNCTION PROTOTYPES
//******************************************************** 
void resetState(void);
unsigned char setpinmode(unsigned char pin, unsigned char mode);
void doSleepCounter(void);

unsigned char host_chksend(unsigned char chk, unsigned char c);
void sendAck(unsigned char flags, unsigned char error);
void sendPacket(unsigned char flags);
void doGyro(void);
void idle(void);
void commerror(unsigned char type);

//********************************************************
// Reset internal state.
//******************************************************** 
void resetState()
{
	int i;

	for (i=0;i<4;i++)
		servowidth[i]=0;
			
	for (i=0;i<10;i++)
	{
		analog[i]=0;
		analogFilter[i]=0;
	}
	analogFilter[8]=4;
	analogFilter[9]=4;
	
	analogNext=0;
	analogDelay=0;
	
	for (i=0;i<8;i++)
		setpinmode(i, PINMODE_DIG_IN_PULLUP);
	for (i=8;i<16;i++)
		setpinmode(i, PINMODE_ANALOG_IN);
		
	usrange[0]=0xffff; usrange[1]=0xffff;
	
	dac[0]=0x80;
	clockgenstate[0]=0x4;
	clockgenstate[1]=52;
	clockgenstate[2]=26;
	
	gyrocount=0;
	
	for (i=0;i<SLAVESTATEBUFSIZE;i++)
		slavestate[i]=0;
}

//********************************************************
// Main.
//******************************************************** 
void main()
{
	M8C_EnableGInt;

	resetState();

	//********************************************************
 	// turn everything on
	//******************************************************** 
	ED_HOST_init();
    UART_HOST_IntCntl(UART_HOST_ENABLE_RX_INT | UART_HOST_ENABLE_TX_INT);
    UART_HOST_Start(UART_HOST_PARITY_NONE);
   
    ED_PAD_init();
    UART_PAD_IntCntl(UART_PAD_ENABLE_RX_INT  | UART_PAD_ENABLE_TX_INT);
    UART_PAD_Start(UART_PAD_PARITY_NONE);
 
 	ED_SLAVE_init();
    UART_SLAVE_IntCntl(UART_SLAVE_ENABLE_RX_INT |  UART_SLAVE_ENABLE_TX_INT);
    UART_SLAVE_Start(UART_SLAVE_PARITY_NONE);

	AMUX4_1_Start();
 	AMUX4_2_Start();

	RefMux_1_Start(RefMux_1_HIGHPOWER);
	DelSig_1_Start(DelSig_1_HIGHPOWER);
	DelSig_1_StartAD();
	
	SAR6_1_Start(SAR6_1_FULLPOWER);
	SAR6_2_Start(SAR6_2_FULLPOWER);
	
	DAC8_1_Start(DAC8_1_HIGHPOWER);
	
	PWM8_BAUD_Start();
	PWM8_CLOCK_Start();
	
	//DAC8_1_Start(DAC8_1_FULLPOWER);
	//DAC8_1_WriteStall(0x80);
	
	PWM8_SERVOCLK_Start();
	
	PWM16_SERVO_Start();
	PWM16_SERVO_EnableInt();
	
	I2Cm_1_Start();
	
	sleepcounter=0;
 	INT_MSK0|=0x40; // enable sleep interrupts. 512Hz clock.
		
	loadPrefs();
	
	while (sleepcounter<64); // wait 0.125sec for LCD to get ready
	
	if (selftest())
		pad_LcdPrintC(0x00, 0x30, 'B', "OrcBoard Ready!");
	else
		pad_LcdPrintC(0x00, 0x30, 'b', "Self tests FAILED!");
	
	M8C_DisableGInt;
	lastPadReceivedTime=sleepcounter;
	M8C_EnableGInt;
	
	//********************************************************
 	// Run!
	//********************************************************  
 	while (1)
 	{
 		idle();
   	}   
}

void idle(void)
{
	unsigned int padElapsedTime;
	
	updateAnalog();
		 			
	while (allowhost && ED_HOST_isData())
 		doHost();
 			
 	while (ED_PAD_isData())
 		doPad();
 			
 	while (ED_SLAVE_isData())
 		doSlave();
 
	M8C_DisableGInt;
 	padElapsedTime=sleepcounter-lastPadReceivedTime;
	M8C_EnableGInt;
 	
 	if ((pad_currentButtons()&PAD_STOP) && allstopenabled)
 	{
 		allstopenabled=0;
 		allstop(ALLSTOP_STOPBUTTON);
 		allstopenabled=1;
 	}
 	
 	if (padElapsedTime>800L && allstopenabled)
 	{
 		allstopenabled=0;
 		allstop(ALLSTOP_PADTIMEOUT);
 		allstopenabled=1;
 	}
 		
 	if (lastcounter!=sleepcounter)
 	{
  		doSleepCounter();
		M8C_DisableGInt;
  		lastcounter=sleepcounter;
		M8C_EnableGInt;
  	}
 		
	usrange[uslastport]=0xffff-Timer16_SONAR_wReadCompareValue();
}

void doSleepCounter()
{
	unsigned char but=pad_currentButtons();
	long v;
	
	if (but&PAD_MENU)
		diag_button_count++;
	else
		diag_button_count=0;
	
	if (diag_button_count>DIAG_BUTTON_THRESH && !indiag)
		{
		indiag=1;
		diag();
		diag_button_count=0;
		indiag=0;
		}
}

void updateAnalog(void)
{
	unsigned int v;
	unsigned int dtime;
	long  sv;
	
	v=(SAR6_2_cGetSample()<<10)+0x8000;
	analog[8]=analog[8]-(analog[8]>>analogFilter[8])+(v>>analogFilter[8]);

	v=(SAR6_1_cGetSample()<<10)+0x8000;	
	analog[9]=analog[9]-(analog[9]>>analogFilter[9])+(v>>analogFilter[9]);

	if (!DelSig_1_fIsDataAvailable())
		return;

	v=DelSig_1_iGetDataClearFlag();

	analogDelay++;			// since the A/D is pipelined, we must throw out 2 samples after changing the mux.
	if (analogDelay>=3)
	{
		v=v<<2; // left align
		
		analogDelay=0;	
		analog[analogNext]=analog[analogNext]-(analog[analogNext]>>analogFilter[analogNext])+(v>>analogFilter[analogNext]);
	
		if (analogNext==7)
		{
			M8C_DisableGInt;
	 		dtime=sleepcounter-lastgyrotime;
			lastgyrotime=sleepcounter;
			M8C_EnableGInt;
			
			sv=(signed int) (v-32768);
			sv*=dtime;
			gyrocount+=sv;
		}
		
		// rotate the port
		analogNext=(analogNext+1)&7;
		AMUX4_1_InputSelect(analogNext>>1);
		AMUX4_2_InputSelect(analogNext>>1);
		
		if (analogNext&1)
			ABF_CR0=ABF_CR0|0x80;
		else
			ABF_CR0=ABF_CR0&0x7f;
	}	
}



/////////////////////////////// PAD ///////////////////////////////////////////

void doPad(void)
{
	char c=ED_PAD_getData();
	
	// wait for header
	if (padbufpos==0 && c!=237)
	{
		badpadchars++;
		return;
	}
		
	padbuf[padbufpos++]=c;
	
	if (padbufpos<2 || padbufpos<padbuf[1])
		padbufchk=(padbufchk<<1) + c + (padbufchk&0x80 ? 1 : 0);
		
	// if we're too long, quit!
	if (padbufpos>=PADBUFSIZE)
		{
		padbufpos=0;
		padbufchk=0;
		return;
		}
	
	// wait for rest of packet. 
	if (padbuf[1]!=padbufpos)
		return;
	
	// we have the whole packet now.
	if (padbufchk==padbuf[padbufpos-1])
	{
		M8C_DisableGInt;
		lastPadReceivedTime=sleepcounter;
		M8C_EnableGInt;
		doPadMessage();
	}
	else
	{
		badpadchars+=padbuf[1];
		commerror(1);
	}
		
	// handle the packet
	padbufpos=0;
	padbufchk=0;
}

void doPadMessage()
{
	unsigned char i;
	
	if (padbuf[3]=='*') // if this is a state packet, spy on it.
	{
		pad_joyx=padbuf[4];
		pad_joyy=padbuf[5];
		pad_buttons=padbuf[6];
		pad_move_up=padbuf[7]>>4;
		pad_move_down=padbuf[7]&0x0f;
		pad_move_left=padbuf[8]>>4;
		pad_move_right=padbuf[8]&0x0f;
		pad_battery_voltage=(padbuf[9]<<8)+padbuf[10];
	}
	
	if (((padbuf[2]&0xc0)!=0xc0 || padbuf[3]=='*') && !indiag)
	{
		// forward the packet to the host
		for (i=0;i<padbufpos;i++)
			ED_HOST_putData(padbuf[i]);
		if (eventenable)
			ED_HOST_putData(EVENTCHAR);
	}

	if (padbuf[2]==0xc0 && padbuf[3]==0) // an ack?
		pad_ack_flag=1;
}

/////////////////////////////// SLAVE ///////////////////////////////////////////

void doSlave(void)
{
	char c=ED_SLAVE_getData();
	
	// wait for header
	if (slavebufpos==0 && c!=237)
	{
		badslavechars++;
		return;
	}
		
	slavebuf[slavebufpos++]=c;
	
	if (slavebufpos<2 || slavebufpos<slavebuf[1])
		slavebufchk=(slavebufchk<<1) + c + (slavebufchk&0x80 ? 1 : 0);
	
	// if we're too long, quit!
	if (slavebufpos>=SLAVEBUFSIZE)
		{
		slavebufpos=0;
		slavebufchk=0;
		return;
		}
	
	// wait for rest of packet. 
	if (slavebuf[1]!=slavebufpos)
		return;
	
	// check checksum	
	if (slavebufchk==slavebuf[slavebufpos-1])
		doSlaveMessage();
	else
	{
		badslavechars+=slavebuf[1];
		commerror(2);
	}

	// handle the packet
	slavebufpos=0;
	slavebufchk=0;
}

void doSlaveMessage()
{
	unsigned char i;
	
	if (slavebuf[3]=='*') // if this is a state packet, spy on it.
	{
		for (i=0;i<slavebufpos;i++)
			{
			if (i<SLAVESTATEBUFSIZE)
				slavestate[i]=slavebuf[i];
			}		
	}
	
	if ((slavebuf[2]&0xc0)!=0xc0)
	{
		// forward the packet to the host
		for (i=0;i<slavebufpos;i++)
			{
			ED_HOST_putData(slavebuf[i]);
			}
			
		if (eventenable)
			ED_HOST_putData(EVENTCHAR);
	}

	if ((slavebuf[2]&0xc0)==0xc0)
	{
		slave_ack=1;
	}
}

/////////////////////////////// HOST ///////////////////////////////////////////

void doHost(void)
{
	unsigned char i;
	
	char c=ED_HOST_getData();
	
	// wait for header
	if (hostbufpos==0 && c!=237)
	{
		badhostchars++;
		return;
	}
		
	hostbuf[hostbufpos++]=c;

	if (hostbufpos<2 || hostbufpos<hostbuf[1])
		hostbufchk=(hostbufchk<<1) + c + (hostbufchk&0x80 ? 1 : 0);
		
	// if we're too long, quit!
	if (hostbufpos>=HOSTBUFSIZE)
		{
		hostbufpos=0;
		hostbufchk=0;
		return;
		}
	
	// wait for rest of packet. 
	if (hostbuf[1]!=hostbufpos)
		return;
	
	// check the checksum...
	// we have the whole packet now.
	if (hostbufchk==hostbuf[hostbufpos-1])
		{
		c=hostbuf[2]&0xc0;
		if (c==0x00) // host->master
			{
			doHostMessage();
			}
		else if (c==0x40) // host->slave
			{
			for (i=0;i<hostbufpos;i++)
				ED_SLAVE_putData(hostbuf[i]);
			}
		else if (c==0x80) // host->pad
			{
			for (i=0;i<hostbufpos;i++)
				ED_PAD_putData(hostbuf[i]);
			}
		}
	else
	{
		badhostchars+=hostbuf[1];
		commerror(3);
	}
	
	// reset
	hostbufpos=0;
	hostbufchk=0;
}

#define OFFSET 3

void doHostMessage()
{
	unsigned char port;
	unsigned int v;
	long count;
	unsigned char len=hostbuf[1]-4;
	unsigned char err, res;
	unsigned char chk;
	
	if (hostbuf[OFFSET]=='*' && len==1)
	{
		sendPacket(hostbuf[2]);
		return;
	}

	if (hostbuf[OFFSET]=='X' && len==1)
	{
		sendAck(hostbuf[2], ESUCCESS);
		allstop(ALLSTOP_SOFTWAREREQ);
		return;
	}
	
	// set servo: Spvv
	if (hostbuf[OFFSET]=='S' && len==4)
	{
		port=hostbuf[OFFSET+1];
		v=(hostbuf[OFFSET+2]<<8)+hostbuf[OFFSET+3];
		
		if (port>=0 && port<=3)
		{
			// don't allow an interrupt while we're writing the new value
			// or we could (with low probability) read a bogus servo width.
			M8C_DisableGInt;
			servowidth[port]=v;
			M8C_EnableGInt;

			sendAck(hostbuf[2], ESUCCESS);
			return;
		}

		sendAck(hostbuf[2], EBADPORT);
		return;
	}
	
	// digital write: Dpv
	if (hostbuf[OFFSET]=='D' && len==3)
	{
		port=hostbuf[OFFSET+1];
		v=hostbuf[OFFSET+2];
		if (port>15 || v>1)
		{
			sendAck(hostbuf[2], EBADPORT);
			return;
		}
		
		if (v>1)
		{
			sendAck(hostbuf[2], EBADPARAM);
			return;
		}
		
		if (v)
			v=0xff;
			
		if (port<8)
		{
			port=1<<port;

			PRT2DR=(PRT2DR&prt2andmask&(~port))|prt2ormask|(v&port);
		}
		else
		{
			port=1<<(port-8);
			PRT0DR=(PRT0DR&prt0andmask&(~port))|prt0ormask|(v&port);
		}
		
		sendAck(hostbuf[2], ESUCCESS);
		return;
	}
	
	// set mode:   Cpm
	if (hostbuf[OFFSET]=='C' && len==3)
	{
		port=hostbuf[OFFSET+1];
		v=hostbuf[OFFSET+2];
		err=setpinmode(port, v);

		sendAck(hostbuf[2], err);
		return;		
	}

	// set clock:  Kspw  (source, period, width)
	if (hostbuf[OFFSET]=='K' && len==4)
	{
		if (hostbuf[OFFSET+1]>0x0f)
			{
			sendAck(hostbuf[2], EBADPARAM);
			return;			
			}
		
		clockgenstate[0]=hostbuf[OFFSET+1];
		clockgenstate[1]=hostbuf[OFFSET+2];
		clockgenstate[2]=hostbuf[OFFSET+3];
		
	//	PWM8_CLOCK_Stop();
		
	//	PWM8_CLOCK_INPUT_REG=(PWM8_CLOCK_INPUT_REG&0xf0) | hostbuf[OFFSET+1];
		PWM8_CLOCK_WritePeriod(hostbuf[OFFSET+2]);
		PWM8_CLOCK_WritePulseWidth(hostbuf[OFFSET+3]);

	//	PWM8_CLOCK_Start();

		sendAck(hostbuf[2], ESUCCESS);
		return;
	}
	
	if (hostbuf[OFFSET]=='F' && len==3)
	{
		port=hostbuf[OFFSET+1];
		v=hostbuf[OFFSET+2];

		if (port>9)
		{
			sendAck(hostbuf[2], EBADPORT);
			return;
		}
		if (v>15)
		{
			sendAck(hostbuf[2], EBADPARAM);
			return;
		}
			
		analogFilter[port]=v;
		sendAck(hostbuf[2], ESUCCESS);
		return;
	}

	// set DAC:  Vv
	if (hostbuf[OFFSET]=='V' && len==2)
	{
		dac[0]=hostbuf[OFFSET+1];
		if (dac[0]>254)	// enforce wonky maximum value.
			dac[0]=254;
		DAC8_1_WriteBlind(dac[0]);
		sendAck(hostbuf[2], ESUCCESS);
		return;
	}
	
	// we've set the hardware to do a timer capture on a falling edge.
	// we must generate a >10uSec pulse. At 24 MHz, this means 240 cycles.
	// Rp
	if (hostbuf[OFFSET]=='R' && len==2) // sonaR.
	{
		Timer16_SONAR_Stop();

		port=hostbuf[OFFSET+1];
		if (port!=4 && port!=6)
			{
			sendAck(hostbuf[2], EBADPORT);
			return;
			}
		
		if (port==4)
			{
			uslastport=0;
			uspingcount=(uspingcount&0xf0) | (((uspingcount&0x0f)+1)&0x0f);
			}
		else
			{
			uslastport=1;
			uspingcount=(uspingcount&0x0f) | (((uspingcount&0xf0)+0x10)&0xf0);
			}
			
		// hook up pulse input; use row 1 or row 3 depending on port=4 or 6
		// (echo will be on pin 5 or 7)
		Timer16_SONAR_INPUT_LSB_REG=(Timer16_SONAR_INPUT_LSB_REG&0x0f) | ((0x0c+(port-4+1))<<4);
		
		// convert the port into a bit mask.
		port=1<<port;
				
		// pulse goes high
		PRT2DR=(PRT2DR&prt2andmask&(~port))|prt2ormask|port;

		// about 240 cycle delay
		for (count=0;count<5;count++);
				
		// intialize period to 0xffff and start timer
		Timer16_SONAR_WritePeriod(0xffff);
		Timer16_SONAR_WriteCompareValue(0x0000);
		
		// don't allow an interrupt to introduce unpredictable noise in our range
		// measurement
		M8C_DisableGInt;

		Timer16_SONAR_Start();

		// pulse goes low		
		PRT2DR=(PRT2DR&prt2andmask&(~port))|prt2ormask;

		M8C_EnableGInt;
		
		// set reload-period to 0x0000 (so the timer stops at zero)
		// Timer16_SONAR_WritePeriod(0x0000);
		sendAck(hostbuf[2], ESUCCESS);

		return;
	}
	
	// combined write/read i2c transaction
	// J<addr><readlen><buf>
	if (hostbuf[OFFSET]=='J' && len>=3)
	{
		res = I2Cm_1_bWriteBytes(hostbuf[OFFSET+1], &hostbuf[OFFSET+3], len-3, I2Cm_1_NoStop);
		
		if (!res)
		{
			sendAck(hostbuf[2], EI2CERR);
			return;
		}
		
		len = hostbuf[OFFSET+2];
		res = I2Cm_1_fReadBytes(hostbuf[OFFSET+1], &hostbuf[OFFSET+3], len, I2Cm_1_RepStart);
		
		if (!res)
		{
			sendAck(hostbuf[2], EI2CERR);
			return;
		}

		chk=0;	
		chk = host_chksend(chk, 237);
		chk = host_chksend(chk, 5+len);
		chk = host_chksend(chk, hostbuf[2]);
		chk = host_chksend(chk, ESUCCESS);
		for (port=0;port<len;port++) // reusing port variable
			chk = host_chksend(chk, hostbuf[OFFSET+3+port]);
		chk = host_chksend(chk, chk);

		return;	
	}
	
	// I2C write: I<addr><buf>
	if (hostbuf[OFFSET]=='I' && len>=2)
	{
		res = I2Cm_1_bWriteBytes(hostbuf[OFFSET+1], &hostbuf[OFFSET+2], len-2, I2Cm_1_CompleteXfer);
		
		if (res)
		{
			sendAck(hostbuf[2], ESUCCESS);
		}
		else
		{
			sendAck(hostbuf[2], EI2CERR);
		}
			
		if (eventenable)
			ED_HOST_putData(EVENTCHAR);

		return;
	}
	
	// I2C read: i<addr><len>
	if (hostbuf[OFFSET]=='i' && len==3)
	{
		len=hostbuf[OFFSET+2];

		res = I2Cm_1_fReadBytes(hostbuf[OFFSET+1], &hostbuf[OFFSET+2], len, I2Cm_1_CompleteXfer);
		
		if (res)
		{
			chk=0;	
			chk = host_chksend(chk, 237);
			chk = host_chksend(chk, 5+len);
			chk = host_chksend(chk, hostbuf[2]);
			chk = host_chksend(chk, ESUCCESS);
			for (port=0;port<len;port++) // reusing port variable
				chk = host_chksend(chk, hostbuf[OFFSET+2+port]);
			chk = host_chksend(chk, chk);
		}
		else
		{
			sendAck(hostbuf[2], EI2CERR);
		}
	
		if (eventenable)
			ED_HOST_putData(EVENTCHAR);
	
		return;
	}
	
	sendAck(hostbuf[2], EUNKNOWNCMD);
}

// returns non-zero on error
unsigned char setpinmode(unsigned char pin, unsigned char mode)
{
unsigned char mask;						// the bit mask for this pin
unsigned char dm2, dm1, dm0, gs=0; 		// direction mode bits and global select
unsigned char ormask=0, andmask=0xff;	// whether this pin will be prevented from going hi/low
unsigned char clearbit=0;				// whether this pin will have a zero written to it

// Summary of Tech Ref Manual info:
// Each pin described by three bits, called DM2, DM1, and DM0.
// Each bit is stored in a different register.
//
// DM: 2 1 0	Pin Output High    Pin Output Low    Notes
// ----------   ----------------   ----------------  -------------------
//     0 0 0         Strong           Resistive		 "pull down", set out=1
//     0 0 1         Strong            Strong        "totem pole"
//     0 1 0          Hi-Z              Hi-Z         Digital Input Enabled
//     0 1 1        Resistive          Strong        "pull up", set out=0
//     1 0 0          Weak              Hi-Z         
//     1 0 1          Weak              Weak         "weak totem pole"
//     1 1 0          Hi-Z				Hi-Z         Analog Input
//     1 1 1          Hi-Z              Weak         I2C-Compat.
//
// PRTxGS port connects the pin to its corresponding global bus.
// If DM210=010, it connects to global input
// Otherwise, it connects to global output.
//
// Note: 
// If using pull-ups, you MUST write '0's to the corresponding bit whenever
// you write to PRTxDR. Likewise, when using pull-downs, you MUST write '1's to 
// the corresponding bit.
//
// Here's our general scheme: for a given pin, it can have any of a number of
// "standard" modes (dig in/out, analog in for some pins). The "special" modes, 
// such as servo, sonar_echo, are activated by setting the corresponding GS pin.
// We "preconfigure" all of the special modes so that by setting the GS pin, it 
// becomes appropriately connected. Note that this means that any given pin can only
// have a single "special" function.

// catch illegal arguments 
if (pin>15)   // illegal pin number
	return EBADPORT;

if (mode>PINMODE_MAX)
	return EBADPARAM;
		
if (mode==PINMODE_ANALOG_IN && !(pin>=8 || pin==0 || pin==3))
	return EBADPARAM; // can't do analog on that pin.

if (mode==PINMODE_SERVO && pin>3)
	return EBADPARAM; // can't do servo on that pin

if (mode==PINMODE_SONAR_PING && !(pin==4 || pin==6))
	return EBADPARAM;

if (mode==PINMODE_SONAR_ECHO && !(pin==5 || pin==7))
	return EBADPARAM;

if (mode==PINMODE_CLOCK_OUT && !(pin==14))
	return EBADPARAM;

if (mode==PINMODE_ANALOG_OUT && !(pin==12))
	return EBADPARAM;
	

// Look-up table for our mode pins.
switch (mode)
	{
	case PINMODE_DIG_IN:
		dm2=0x00; dm1=0xff; dm0=0x00;
	break;
	
	case PINMODE_DIG_IN_PULLUP:
		dm2=0x00; dm1=0xff; dm0=0xff;
		ormask=0xff;
	break;

	case PINMODE_DIG_IN_PULLDN:
		dm2=0x00; dm1=0x00; dm0=0x00;
		andmask=0x00;
		break;

	case PINMODE_SONAR_PING:
		clearbit=1;
		// fallthrough
	case PINMODE_DIG_OUT_STRONG:
		dm2=0x00; dm1=0x00; dm0=0xff;
		break;
		
	case PINMODE_DIG_OUT_SLOW:
		dm2=0xff; dm1=0x00; dm0=0xff;
		break;
	
	case PINMODE_CLOCK_OUT:
	case PINMODE_SERVO:
		dm2=0x00; dm1=0x00; dm0=0xff; gs=0xff;
		break;
	
	case PINMODE_SONAR_ECHO:
		dm2=0x00; dm1=0xff; dm0=0x00; gs=0xff;
		break;	

	case PINMODE_ANALOG_OUT:
	case PINMODE_ANALOG_IN:
		dm2=0xff; dm1=0xff; dm0=0x00;
		break;	
		
	default:
		return EBADPARAM; // unknown pin type
	}

if (pin==12)
{
	if (mode!=PINMODE_ANALOG_OUT)
		ABF_CR0&=~(0x10);
	else
		ABF_CR0|=(0x10);
}

if (pin<=7)
	{
	mask=1<<pin;
	PRT2DM2=(PRT2DM2&(~mask)) | (dm2&mask);
	PRT2DM1=(PRT2DM1&(~mask)) | (dm1&mask);
	PRT2DM0=(PRT2DM0&(~mask)) | (dm0&mask);
	PRT2GS =(PRT2GS&(~mask))  | (gs&mask);
	prt2andmask = (prt2andmask&(~mask)) | (andmask&mask);
	prt2ormask  = (prt2ormask&(~mask))  | (ormask&mask);
	
	PRT2DR=(PRT2DR&prt2andmask) | prt2ormask;
	
	if (clearbit)
		PRT2DR=PRT2DR&(~mask);
	}
else
	{
	mask=1<<(pin-8);
	PRT0DM2=(PRT0DM2&(~mask)) | (dm2&mask);
	PRT0DM1=(PRT0DM1&(~mask)) | (dm1&mask);
	PRT0DM0=(PRT0DM0&(~mask)) | (dm0&mask);
	PRT0GS =(PRT0GS&(~mask))  | (gs&mask);	
	prt0andmask = (prt0andmask&(~mask)) | (andmask&mask);
	prt0ormask  = (prt0ormask&(~mask))  | (ormask&mask);

	PRT0DR=(PRT0DR&prt0andmask) | prt0ormask;
	if (clearbit)
		PRT0DR=PRT0DR&(~mask);
	}

pinmode[pin]=mode;

return ESUCCESS;
}

void allstop(unsigned char type)
{
	slave_allstop();
	
	pad_LcdClear();
	allowhost=0;
	
	//                          1234567890123456
	
	pad_LcdPrintC(0x00, 0x08, 'd', "                ");
	pad_LcdPrintC(0x00, 0x10, 'd', "    All Stop    ");


	pad_LcdPrintC(0x00, 0x20, 'B', "Hit stick button");
	pad_LcdPrintC(0x00, 0x28, 'B', "to resume.");

	if (type==ALLSTOP_PADTIMEOUT)
		pad_LcdPrintC(0x00, 0x38, 'd', "  (Pad Timeout) ");
	else if (type==ALLSTOP_STOPBUTTON)
		pad_LcdPrintC(0x00, 0x38, 'd', "  (Stop Button) ");
	else
		pad_LcdPrintC(0x00, 0x38, 'd', "   (Soft Halt)  ");
	
	// wait for them to release the stop button
	while (pad_pollButtons()!=0)
		slave_allstop();
	
	// wait for them to press the menu/stick button
	while ((pad_pollButtons()&(PAD_MENU | PAD_STICK))==0);
	
	// wait for them to release the menu/stick button
	pad_waitButtonRelease();
	
	pad_LcdClear();

	// reset all our comms buffers
	ED_HOST_init();
	
	allowhost=1;

}

unsigned char host_chksend(unsigned char chk, unsigned char c)
{
	ED_HOST_putData(c);
	
	return (chk<<1)+c+(chk&0x80 ? 1 : 0);
}

unsigned char host_chksendi(unsigned char chk, unsigned int c)
{
	chk = host_chksend(chk, c>>8);
	chk = host_chksend(chk, c&0xff);
	return chk;
}

// note: convention that error==0 means no error
void sendAck(unsigned char flags, unsigned char error)
{
	unsigned char chk=0;	
	chk = host_chksend(chk, 237);
	chk = host_chksend(chk, 5);
	chk = host_chksend(chk, flags);
	chk = host_chksend(chk, error);
	chk = host_chksend(chk, chk);
	
	if (eventenable)
		ED_HOST_putData(EVENTCHAR);

}

// These functions allow for partial sending of
// a packet. You set up skipchars and length appropriately,
// and it will only transmit the characters you want.
unsigned char host_chksendfilt_skipchars=0;
unsigned char host_chksendfilt_length=0;

unsigned char host_chksendfilt(unsigned char chk, unsigned char c)
{
	if (host_chksendfilt_skipchars==0)
	{
		if (host_chksendfilt_length>0)
			{
			host_chksendfilt_length--;
			return host_chksend(chk, c);	
			}
		return chk;
	}
	host_chksendfilt_skipchars--;
	return chk;
}

unsigned char host_chksendfilti(unsigned char chk, unsigned int c)
{
	chk = host_chksendfilt(chk, c>>8);
	chk = host_chksendfilt(chk, c&0xff);
	return chk;
}

//////////////////////////////////////////////////
void sendPacket(unsigned char flags)
{
	unsigned char chk=0;
	unsigned char i;
		
	chk = host_chksend(chk, 237);
	chk = host_chksend(chk, 69);
	chk = host_chksend(chk, flags);
	chk = host_chksend(chk, '*'); // i'm a state packet

	chk = host_chksendi(chk, sleepcounter); 

	chk = host_chksend(chk, PRT0DR);  // dig 0-7
	chk = host_chksend(chk, PRT2DR);  // dig 8-15
	
	// SERVOS
	for (i=0;i<4;i++)
		chk = host_chksendi(chk, servowidth[i]); 

	// ANALOG
	for (i=0;i<10;i++)
		chk = host_chksendi(chk, analog[i]); 
	for (i=0;i<10;i+=2)
		chk = host_chksend(chk, (analogFilter[i+1]<<4) | analogFilter[i]);
		
	// PIN MODES
	for (i=0;i<16;i+=2)
		chk = host_chksend(chk, pinmode[i] | (pinmode[i+1]<<4));
	
	// ULTRASOUND RANGE
	chk = host_chksendi(chk, usrange[0]); 
	chk = host_chksendi(chk, usrange[1]); 
	chk = host_chksend(chk, uspingcount);
	
	// CLOCK DATA
	chk = host_chksend(chk, clockgenstate[0]&0x0f);
	chk = host_chksend(chk, clockgenstate[1]);
	chk = host_chksend(chk, clockgenstate[2]);

	// DAC value
	chk = host_chksend(chk, dac[0]);
	
	// Gyro
	chk = host_chksendi(chk, gyrocount>>16); 
	chk = host_chksendi(chk, gyrocount&0x00ffff); 

	// Comm statistics
	chk = host_chksend(chk, badhostchars);
	chk = host_chksend(chk, badslavechars);
	chk = host_chksend(chk, badpadchars);
	
	chk = host_chksend(chk, ED_HOST_fStatus);
	chk = host_chksend(chk, ED_SLAVE_fStatus);
	chk = host_chksend(chk, ED_PAD_fStatus);
	
	ED_HOST_fStatus=0;
	ED_SLAVE_fStatus=0;
	ED_PAD_fStatus=0;

	// the checksum
	host_chksend(chk, chk);

	if (eventenable)
		ED_HOST_putData(EVENTCHAR);

}

void commerror(unsigned char type)
{
/*
	char buf[4];
	formatInt(type, buf, 2);
	
//	pad_LcdPrintC(0,56,'b',"err");
	pad_LcdPrint(112,56,'b',buf);
*/
}
