#include <16f628a.h>
//#include <16f648a.h>

// hardware config:
#fuses NOBROWNOUT, NOPROTECT, NOLVP, NOWDT, INTRC, NOMCLR
#use delay(clock=4000000)
#use rs232(baud=2400, xmit=PIN_A3, rcv=PIN_A4)
#use fast_io(b) 
#byte portb = 6

// pic16F628A/16F648A pin assigns:
#define X1 PIN_B0 // tilt switch X1 = Y1
#define X2 PIN_B1 // tilt switch X2 
#define X3 PIN_B2 // tilt switch X3 = Z1
#define X4 PIN_B3 // tilt switch X4 
#define Y2 PIN_B4 // tilt switch Y2
#define Y3 PIN_B5 // tilt switch Y3 = Z3
#define Y4 PIN_B6 // tilt switch Y4
#define Z2 PIN_B7 // tilt switch Z2
#define AX PIN_A0 // X axis acceleration
#define AY PIN_A1 // Y axis acceleration
#define XL PIN_A2 // turns on/off the adxl311
#define TX PIN_A3 // serial send
#define RC PIN_A4 // serial receive
#define MC PIN_A5 // MCLR / extra analog input
#define NA PIN_A6 // not available
#define Z4 PIN_A7 // tilt switch Z4

#define SWITCH_INTERVAL 10
#define N 5
#define W 25

// global variables (remember: unsigned by default!):
int8 switch17;   // the first 7 bits are read straight away as 7 bits
int8 switch89;   // and then there's 2 remaining bits (in another byte)
int16 switches, switches_first, hamming;   // switch memory
int16 pwmcount;                            // pulse width mod. counter
int16 bad_overflow;                        // when is the acc. dead?
int8 accx[W], accy[W];                     // acceleration x and y
int8 iter;                                 // iterators for readings
int8 badx, bady;                           // bad counters
int8 minx, miny;                           // minima for acceleration
int8 maxx, maxy;                           // maxima for acceleration
int16 avgx_sum, avgy_sum;                  // sum for averages
int8 avgx, avgy;                           // average containers
signed int8 px_l, py_l;                    // peak length (+sign place)
int16 px_s_sum, py_s_sum;                  // area under peak
int8 px_h, py_h, px_s, py_s;               // peak height & size
signed int8 signx, signy;                  // signs keeping

void init_acc(void);              // initialise the adxl202
void exit_acc(void);              // turn the adxl202 off  
void prepare_switches(void);      // prepare the tilt switches' variables
void read_switches(int8 num);     // read the status of the switches
void prepare_switch_out(void);    // prepare the switches' output 
void prepare_accs(void);          // prepare the accelerometer's vars.
void read_accX(void);             // read X acceleration
void read_accY(void);             // read Y acceleration

void main()
{
	int8 i;
  	badx = bady = 0;
	bad_overflow = 500;

   // booting:
	 delay_ms(120);     // to make sure the host is the first to boot
	 set_tris_b(0xFF);
	 prepare_accs();    // preparation of variables
     init_acc();
   
   // main loop:
	iter=0;
	while (TRUE)
	{
      prepare_switches();  // preparation of switches' variables 
      read_switches(SWITCH_INTERVAL);      
		for (i=0; i<N; i++) {
         	read_accX();
			read_switches(SWITCH_INTERVAL);
			read_accY();
			read_switches(SWITCH_INTERVAL);
			iter++;
			if (iter==W) iter=0;
		}
		// output to serial:
		 prepare_switch_out();
		 printf("%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c", 
		                       0xFF, switch17, switch89, avgx,avgy, 
							   minx,miny, maxx,maxy, px_h,py_h, 
                               (signx*px_l),(signy*py_l), px_s,py_s );
		 ////////////////////////////////////////////////////////////////
		 // the structure of the package is: 120bits or 15 bytes       //
		 // a 8bit preamble, followed by 16 bits for the switches,     //
		 // 48 bits for accelerometer averages, minimums and maximums, //
		 // and 48 bits for accelerometer peak height, length, sizes:  //
		 //  11111111 0sssssss 0ss0hamm                                //
		 //  averagex averagey minimumx minimumy maximumx maximumy     //
		 //  heightpx heightpy lengthpx lengthpy sizepxxx sizepyyy     //
		 ////////////////////////////////////////////////////////////////
 	}

	exit_acc();
}




// power & initialise the adxl202/adxl311 accelerometer
// change operating speed to 4Mhz (and wait a little)
void init_acc(void)
{
	output_high(XL); // power up the accelerometer
}

// power down the adxl311 accelerometer, revert to slower mode
// (and wait a little)
void exit_acc(void)
{
	output_low(XL); // power down the accelerometer
}

void prepare_switches(void)
{
	hamming = 0;
	switches_first = portb;
	switches_first |= input(Z4)*0x100;
}

void read_switches(int8 num) 
{
	int8 i;
	for (i=0; i<num; i++) {
		switches = portb;
		switches |= input(Z4)*0x100;
		hamming |= (switches ^ switches_first);
	}
}

// construct two tranmission bytes, including cumulative hamming:
void prepare_switch_out(void)
{
	int8 i;
	switch89 = 0;
	for (i=0; i<16; i++) {  // count the ones:
		hamming >>= 1;
	 	if ( (hamming&0x01) != 0)   switch89++; 
	}
	switch17= portb;
	switch17 >>= 1; // shift right, we'll add Z3 to switch89 
	switch89 |= ((input(Z2)*0x40) + (input(Z4)*0x20));
}

// initialise accelerometer variables, peak features
void prepare_accs(void) 
{
    int8 i;
	for (i=0; i<W; i++)   accx[i] = accy[i] = 50;
	minx = miny = 0xFF;
	maxx = maxy = avgx = avgy = 0;
    avgx_sum = avgy_sum = W*50;
    px_s_sum = py_s_sum = 0;
	px_l = py_l = px_h = py_h = px_s = py_s = 0;
    signx = signy = 1; // positive, -1 is negative
}

void read_accX(void)
{
   int8 i, absdiff;
   signed int8 diff;
	pwmcount = 0;
	if (badx<128){ 
		while ( input(AX) && pwmcount<bad_overflow )	pwmcount++;
		if ( pwmcount>bad_overflow )	badx++;
		else {
			pwmcount = 0;
			while ( !input(AX) );
			while ( input(AX) )	pwmcount++;
			if ( pwmcount>bad_overflow )	{
				badx++;
				pwmcount = 0;
			}
		}
	}
   // check whether the value that gets kicked out is min or max:
    if (accx[iter]==minx) {
      minx = maxx; 
      for (i=0; i<W; i++)   
		if ( (accx[i] < minx) && (i!=iter) )   minx = accx[i];
    }
    if (accx[iter]==maxx) {
      maxx = 0;
      for (i=0; i<W; i++)   
		if ( (accx[i] > maxx) && (i!=iter) )   maxx = accx[i];
    }
   // update with the latest reading:
	avgx_sum -= accx[iter];
    accx[iter] = pwmcount;
    if (accx[iter]<minx) minx=accx[iter];
    if (accx[iter]>maxx) maxx=accx[iter];
	avgx_sum += accx[iter];
    avgx = avgx_sum/W;
   // calulate peaks:
    diff = accx[iter]-avgx;
    absdiff = abs(diff);
	if (diff==0) {   // on the baseline
		px_l = 0;
		px_h = 0;
		px_s_sum = 0;
	} else
    if ( signx != ((diff<0)?-1:1) ) {  // new sign? => new peak
      px_l = 1;
      px_h = absdiff;
      px_s_sum = px_h;
      signx = (diff<0)?-1:1;
    }
    else {                          // update peak info
      px_l ++;
	  if (px_l>126) px_l = 126;
      if (px_h < absdiff)  px_h = absdiff;
      px_s_sum += absdiff;
    }
    px_s = (px_s_sum*255) / (px_h*px_l);  // percentage of max area
}

void read_accY(void)
{
   int8 i, absdiff;
   signed int8 diff;
	pwmcount = 0;
	if (bady<128){ 
		while ( input(AY) && pwmcount<bad_overflow )	pwmcount++;
		if ( pwmcount>bad_overflow )	bady++;
		else {
			pwmcount = 0;
			while ( !input(AY) );
			while ( input(AY) )	pwmcount++;
			if ( pwmcount>bad_overflow )	{
				bady++;
				pwmcount = 0;
			}
		}
	}
   // check whether the value that gets kicked out is min or max:
    if (accy[iter]==miny) {
      miny = maxy; 
      for (i=0; i<W; i++)   
		if ( (accy[i] < miny) && (i!=iter) )   miny = accy[i];
    }
    if (accy[iter]==maxy) {
      maxy = 0;
      for (i=0; i<W; i++)   
		if ( (accy[i] > maxy) && (i!=iter) )   maxy = accy[i];
    }
   // update with the latest reading:
	avgy_sum -= accy[iter];
	accy[iter] = pwmcount;
    if (accy[iter]<miny) miny=accy[iter];
    if (accy[iter]>maxy) maxy=accy[iter];
	avgy_sum += accy[iter];
    avgy = avgy_sum/W;
   // calulate peaks:
    diff = accy[iter]-avgy;
    absdiff = abs(diff);
	if (diff==0) {   // on the baseline
		py_l = 0;
		py_h = 0;
		py_s_sum = 0;
	} else
    if ( signy != ((diff<0)?-1:1) ) {  // new sign? => new peak
      py_l = 1;
      py_h = absdiff;
      py_s_sum = py_h;
      signy = (diff<0)?-1:1;
    }
    else {                          // update peak info
      py_l ++;
      if (py_l>126) py_l = 126;
      if (py_h < absdiff)  py_h = absdiff;
      py_s_sum += absdiff;
    }
    py_s = (py_s_sum*255) / (py_h*py_l);  // percentage of max area
}