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

#include <m8c.h>        // part specific constants and macros
#include "PSoCAPI.h"    // PSoC API definitions for all User Modules
#include "util.h"
#include "ed_slave_int.h"
#include "error.h"
#include "qpasm.h"

#define PINMODE_DIG_IN 			0
#define PINMODE_DIG_IN_PULLUP   1
#define PINMODE_DIG_IN_PULLDN   2

#define PINMODE_DIG_OUT_STRONG  3
#define PINMODE_DIG_OUT_SLOW    4

#define PINMODE_SERVO			5
#define PINMODE_SONAR_PING		6
#define PINMODE_SONAR_ECHO		7

#define PINMODE_ANALOG_IN       8

#define PINMODE_ANALOG_OUT      9
#define PINMODE_CLOCK_OUT       10

#define PINMODE_QUADPHASE       11
#define PINMODE_MONOPHASE       12
#define PINMODE_MAX             14

#define PINMODE_QUADPHASEFAST   14

#define MOTORSLEWPERIOD 4

//********************************************************
// GLOBALS
//******************************************************** 
unsigned int  analog[8];		// we store all of our analog values here, with 2_1 and 2_2 at 8 and 9.
unsigned int  analogOffsetError[6]; // track the minimum seen value so we can subtract the error offset.

unsigned char analogFilter[8];
short analogNext;	// which port are we sampling?
short analogDelay;  // how many samples have we skipped since updating our main bank of A/Ds?
short analogResolution; // how many bits of resolution?
unsigned char motorEnable=0x00;

unsigned char pwmGoal[4];
unsigned char pwmActual[4];
unsigned char pwmSlew[4];
unsigned char motorDir[4];

#define SLAVEBUFSIZE 32 // power of two
unsigned char slavebuf[SLAVEBUFSIZE];
unsigned char slavebufpos=0;
unsigned char slavebufchk=0;

extern long sleepcounter;
long lastcounter;

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

unsigned char pinmode[8];

unsigned char motorslewcounter=0;

unsigned char analogOffsetSampled=0;

unsigned char badchars=0;

//********************************************************
// FUNCTION PROTOTYPES
//******************************************************** 
void resetState(void);
void doCommand(void);
void doSlave(void);
void doSlaveMessage(void);
void sendAck(unsigned char flags, unsigned char error);
void sendPacket(unsigned char flags);
unsigned char chksend(unsigned char chk, unsigned char c);
unsigned char chksendi(unsigned char chk, unsigned int c);
void doSleepCounter(void);
unsigned char setpinmode(unsigned char pin, unsigned char mode);
void updateAnalog(void);
void updateMotor(unsigned char i);

void updateQp(void);

extern unsigned int  qpdata0, qpdata1;
extern unsigned char qperrs0, qperrs1;
extern unsigned char qpoffset;

//********************************************************
// Reset internal state.
//******************************************************** 
void resetState()
{
	int i;
	
	for (i=0;i<8;i++)
	{
		analog[i]=0;
		analogFilter[i]=0;
	}
	
	for (i=0;i<6;i++)
		analogOffsetError[i]=65535;
//	for (i=0;i<6;i++)		// this disables the filter
//		analogOffsetError[i]=0;

	for (i=0;i<4;i++)
		analogFilter[i]=2;
	
	analogFilter[6]=4;
	analogFilter[7]=4;
	
	analogNext=0;
	analogDelay=0;
	analogResolution=14;
	
	for (i=0;i<4;i++)
	{
		pwmGoal[i]=0;
		pwmActual[i]=0;
		motorDir[i]=0;
		pwmSlew[i]=51;
	}
	
	for (i=0;i<4;i++)
		setpinmode(i, PINMODE_DIG_IN_PULLUP);

	for (i=4;i<8;i++)
		setpinmode(i, PINMODE_DIG_OUT_STRONG);
}

void main()
{
	unsigned char v;
	
	PRT2DR=motorEnable; //disable motors (note: HIGH nibble)

	resetState();
	
	M8C_EnableGInt;

	PWM8_1_Start();
	PWM8_2_Start();
	PWM8_3_Start();
	PWM8_4_Start();
 
	PWM8_1_WritePulseWidth(0x0);
	PWM8_2_WritePulseWidth(0x0);
	PWM8_3_WritePulseWidth(0x0);
	PWM8_4_WritePulseWidth(0x0);
 	
 	PWM8_SLAVECLK_Start();
 	
	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);
	
  	ED_SLAVE_init();
    UART_SLAVE_IntCntl(UART_SLAVE_ENABLE_RX_INT |  UART_SLAVE_ENABLE_TX_INT);
    UART_SLAVE_Start(UART_SLAVE_PARITY_NONE);

	QP0A_Start();
	QP0B_Start();
	QP0INV_Start();
	QP1A_Start();
	QP1B_Start();
	QP1INV_Start();
	
	sleepcounter=0;
 	INT_MSK0|=0x40; // enable sleep interrupts. 512Hz clock.
		
	while (1)
 	{
		if (pinmode[0]==PINMODE_QUADPHASEFAST)
			updateQp();

 		updateAnalog();
 			
  		while (ED_SLAVE_isData())
 			doSlave();
 	
 		M8C_DisableGInt;
 		v = (lastcounter!=sleepcounter);
 		M8C_EnableGInt;	
 		if (v)
 		{
 			doSleepCounter();
 
 			M8C_DisableGInt;
 			lastcounter=sleepcounter;
 			M8C_EnableGInt;

 		}
	}
}

void doSleepCounter()
{
	unsigned char i;
	
	motorslewcounter++;
	if (motorslewcounter<MOTORSLEWPERIOD)
		return;
	motorslewcounter=0;
	
	for (i=0;i<4;i++)
	{
		updateMotor(i);
	}
}

void updateMotor(unsigned char i)
{
	unsigned char actualdir, goaldir, mode;
	int v;
	
	goaldir=motorDir[i]&3;
	actualdir=(motorDir[i]>>2)&3;
		
	if (goaldir==0) // do nothing if we're disabled.
		return;
			
	// now, compute our new PWM value
	v=pwmActual[i];
		
	if (goaldir==actualdir)
	{
		// we're already going in the right direction; make pwmactual follow pwmgoal
		if (v<pwmGoal[i])
			{
			v=v+pwmSlew[i];
			if (v>pwmGoal[i])
				v=pwmGoal[i];
			}
			
		if (v>pwmGoal[i])
			{
			v=v-pwmSlew[i];
			if (v<pwmGoal[i])
				v=pwmGoal[i];
			}
	}
	else
	{
		// we're going the wrong way. Slow down!
		v=v-pwmSlew[i];
		if (v<0)
			v=0;
			
		if (v==0) // if we've slowed down all the way, we can change direction.
		{
			// we've reached zero speed. set the new direction.
			
			if (goaldir==1)
				mode=0x05;
			else
				mode=0x30;
				
			// change the direction...
			switch (i)
				{
				case 0:
					RDI1LT0=mode;
					break;
				case 1:
					RDI1LT1=mode;
					break;
				case 2:
					RDI2LT0=mode;
					break;
				case 3:
					RDI2LT1=mode;
					break;
				}

			motorDir[i]=(goaldir<<2)|goaldir;
		}
	}
		
	// write the new pwm value
	pwmActual[i]=v;
		
	switch (i)
		{
		case 0:
			PWM8_1_WritePulseWidth(v);
			break;
		case 1:
			PWM8_2_WritePulseWidth(v);
			break;
		case 2:
			PWM8_3_WritePulseWidth(v);
			break;
		case 3:
			PWM8_4_WritePulseWidth(v);
			break;						
		}	
}

void doSlave(void)
{
	char c=ED_SLAVE_getData();
	
	// wait for header
	if (slavebufpos==0 && c!=237)
	{
		badchars++;
		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;
	
	if (slavebufchk==slavebuf[slavebufpos-1])
		doSlaveMessage();
	else
		badchars+=slavebuf[1];
		
	// handle the packet
	slavebufpos=0;
	slavebufchk=0;
}

void updateAnalog(void)
{
	unsigned int v;

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

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

	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 (v<analogOffsetError[analogNext])
			analogOffsetError[analogNext]=v;
			
		// rotate the port
		analogNext=(analogNext+1)&7;
		if (analogNext>=6)
			analogNext=0;
			
		AMUX4_1_InputSelect(analogNext>>1);
		AMUX4_2_InputSelect(analogNext>>1);
		
		if (analogNext&1)
			ABF_CR0=ABF_CR0|0x80;
		else
			ABF_CR0=ABF_CR0&0x7f;
	}
	
}


#define OFFSET 3

void doSlaveMessage()
{
	unsigned char port, v, dir, i, mask;
	unsigned char len=slavebuf[1]-4;
	unsigned char err;

	if (slavebuf[OFFSET]=='*' && len==1)
	{
		sendPacket(slavebuf[2]);
		return;
	}

	// pwm slew: Wps
	if (slavebuf[OFFSET]=='W' && len==3)
	{
		port=slavebuf[OFFSET+1];
		v=slavebuf[OFFSET+2];

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

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

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

		if (err)
		{
			sendAck(slavebuf[2], err);
			return;		
		}
		
		if (v==PINMODE_QUADPHASE)
		{
			for (i=0;i<4;i++)
				setpinmode(i, PINMODE_QUADPHASE);	
			
			qpoffset=0x00;  // quadrature phase
			M8C_EnableIntMask(INT_MSK0, INT_MSK0_GPIO);		
		}
		else if (v==PINMODE_MONOPHASE)
		{
			setpinmode(0, PINMODE_MONOPHASE);
			setpinmode(1, PINMODE_DIG_IN_PULLUP);
			setpinmode(2, PINMODE_MONOPHASE);
			setpinmode(3, PINMODE_DIG_IN_PULLUP);		
			qpoffset=0x10;  // mono phase
			M8C_EnableIntMask(INT_MSK0, INT_MSK0_GPIO);		
		}
		else if (v==PINMODE_QUADPHASEFAST)
		{
			M8C_DisableIntMask(INT_MSK0, INT_MSK0_GPIO);		

			for (i=0;i<4;i++)
				setpinmode(i, PINMODE_QUADPHASEFAST);	
		}
		else
		{
			for (i=0;i<4;i++)
			{
				if (pinmode[i]==PINMODE_QUADPHASE || pinmode[i]==PINMODE_MONOPHASE || pinmode[i]==PINMODE_QUADPHASEFAST)
				{
					setpinmode(i, PINMODE_DIG_IN_PULLUP);	
				}
			}						
		}
								
		sendAck(slavebuf[2], err);
		return;		
	}

	// Motor: Mpdv  [port, direction, pwm value]
	// direction, [0,1,2] (0=disabled, 1=forw, 2=back. Braking occurs with either forw/back 
	// when pwm is small. When disabled, pwm is ignored. (this disables slewing))
	if (slavebuf[OFFSET]=='M' && len==4)
	{
		port=slavebuf[OFFSET+1];
		dir=slavebuf[OFFSET+2];
		v=slavebuf[OFFSET+3];

		if (port>3)
		{
			sendAck(slavebuf[2], EBADPORT);
			return;
		}
		
		if (dir>2)
		{
			sendAck(slavebuf[2], EBADPARAM);
			return;
		}

		// due to routing issues, motor ports 1 and 3 are wired up
		// backwards. So we reverse the direction of odd motors below.
		if (dir>0 && (dir&1))
		{
			dir=2-dir;
		}
		
		motorDir[port]=(motorDir[port]&0x0c)|dir;
		pwmGoal[port]=v;
		
		// if they're disabling the motor, *really* shut it down
		// and set actual to zero so they don't accidentally start
		// it back up too fast.
		mask=(1<<port)<<4; // convert the port to a mask.
		if (dir==0)
		{
			pwmActual[port]=0;
			pwmGoal[port]=0;

			motorEnable&=(~mask);
		}
		else
		{
			motorEnable|=mask;	
		}

		PRT2DR=(PRT2DR&0x0f)|motorEnable;
		
		sendAck(slavebuf[2], ESUCCESS);
		return;
	}
	
	// all stop! Set PWM goal to zero.
	if (slavebuf[OFFSET]=='X' && len==1)
	{
		for (i=0;i<4;i++)
		{
			pwmGoal[i]=0;
		}
		
		sendAck(slavebuf[2], ESUCCESS);
		return;
	}
	
	sendAck(slavebuf[2], EUNKNOWNCMD);
	return;

}

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

unsigned char chksendi(unsigned char chk, unsigned int c)
{
	chk = chksend(chk, c>>8);
	chk = 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 = chksend(chk, 237);
	chk = chksend(chk, 5);
	chk = chksend(chk, flags);
	chk = chksend(chk, error);
	chk = chksend(chk, chk);
}

void sendPacket(unsigned char flags)
{
	unsigned char chk=0;
	unsigned char i;
	unsigned int v1, v2;
		
	chk = chksend(chk, 237);
	chk = chksend(chk, 49);
	chk = chksend(chk, flags);
	chk = chksend(chk, '*'); // i'm a state packet

	M8C_DisableGInt; // atomically sample quad phase data.
	v1=qpdata0;
	v2=qpdata1;
	M8C_EnableGInt;
	
	// PIN MODES
	for (i=0;i<4;i+=2)
		chk = chksend(chk, pinmode[i] | (pinmode[i+1]<<4));

	chk = chksend(chk, PRT2DR);
	
	chk = chksendi(chk, v1);
	chk = chksend(chk, qperrs0);
	chk = chksendi(chk, v2);
	chk = chksend(chk, qperrs1);

	for (i=0;i<4;i++)
	{
		chk = chksend(chk, pwmGoal[i]);
		chk = chksend(chk, pwmActual[i]);
		chk = chksend(chk, pwmSlew[i]);
	}
	
	for (i=0;i<4;i+=2)
	{
		chk = chksend(chk, (motorDir[i+1]<<4) | motorDir[i]);
	}
	
	// ANALOG
	for (i=0;i<8;i++)
		chk = chksendi(chk, analog[i]-analogOffsetError[i]); 

	for (i=0;i<8;i+=2)
		chk = chksend(chk, (analogFilter[i+1]<<4) | analogFilter[i]);
	
	chk = chksend(chk, badchars);
	
	chk = chksend(chk, chk);
}

// returns non-zero on error
unsigned char setpinmode(unsigned char pin, unsigned char mode)
{
unsigned char mask;
unsigned char dm2, dm1, dm0, gs=0;
unsigned char ormask=0, andmask=0xff;

// 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>3)   // illegal pin number
	return EBADPORT;

if (pin>PINMODE_MAX)
	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_DIG_OUT_STRONG:
		dm2=0x00; dm1=0x00; dm0=0xff;
		break;
		
	case PINMODE_DIG_OUT_SLOW:
		dm2=0xff; dm1=0x00; dm0=0xff;
		break;
		
	case PINMODE_MONOPHASE:
	case PINMODE_QUADPHASE:
		dm2=0x00; dm1=0xff; dm0=0x00; gs=0x00;
		ormask=0xff;
		break;	
	
	case PINMODE_ANALOG_OUT:
	case PINMODE_ANALOG_IN:
		dm2=0xff; dm1=0xff; dm0=0x00;
		break;	

	case PINMODE_QUADPHASEFAST:
		dm2=0x00; dm1=0xff; dm0=0x00; gs=0xff;
		break;	

	default:
		return EBADPARAM; // unknown pin type
	}

	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;

	pinmode[pin]=mode;

	return ESUCCESS;
}

unsigned char qp0LastUp, qp0LastDown;
unsigned char qp1LastUp, qp1LastDown;

void updateQp(void)
{
	int newUp, newDown;
	int deltaUp, deltaDown;
	
	newUp=255-QP0A_readDirect();
	newDown=255-QP0B_readDirect();
	
	deltaUp=newUp-qp0LastUp;
	if (deltaUp<0)
		deltaUp+=256;
	
	deltaDown=newDown-qp0LastDown;
	if (deltaDown<0)
		deltaDown+=256;
	
	qpdata0+=(deltaUp-deltaDown);
	qp0LastUp=newUp;
	qp0LastDown=newDown;
	
	////// same thing for channel 1

	newUp=255-QP1A_readDirect();
	newDown=255-QP1B_readDirect();
	
	deltaUp=newUp-qp1LastUp;
	if (deltaUp<0)
		deltaUp+=256;
	
	deltaDown=newDown-qp1LastDown;
	if (deltaDown<0)
		deltaDown+=256;
	
	qpdata1+=(deltaUp-deltaDown);
	qp1LastUp=newUp;
	qp1LastDown=newDown;

}
