/*==================================
 * Positracker code.
 *=================================*/

//-----------------------------------------------------------------------------
// Includes
//-----------------------------------------------------------------------------
#include "c8051f320.h"                    // SFR declarations
#include "USB_MAIN.h"
#include "USB_DESCRIPTOR.h"
#include "USB_REGISTER.h"
#include "sample.h" 

#include "intrins.h"

//-----------------------------------------------------------------------------
// Global CONSTANTS
//-----------------------------------------------------------------------------

sbit SW1  = P2^0;                          // SW2='0' means switch pressed
sbit SW2  = P2^1;                          // SW2='0' means switch pressed
sbit LED1 = P2^2;                          // LED='1' means ON
sbit LED2 = P2^3;                          // LED='1' means ON
sbit IN1  = P1^0;
sbit IN2  = P1^1;
sbit IN3  = P1^2;
sbit IN4  = P1^3;

//-----------------------------------------------------------------------------
// Function PROTOTYPES
//-----------------------------------------------------------------------------
void init(void);
xdata unsigned char smpl[1024] _at_ 0;

BYTE Out_Packet[8];   // Last packet received from host
BYTE In_Packet[8];   // Next packet to sent to host

//-----------------------------------------------------------------------------
// MAIN Routine
//-----------------------------------------------------------------------------
unsigned char adj( char tempc, char vel )
{
	if( tempc>(30+vel) ) tempc-=60;
	if( tempc<(vel-30) ) tempc+=60;

	return tempc;			
}

void pause( unsigned char val )
{
	unsigned char cnt;

	for( ; val!=0; val-- )
	for( cnt=255; cnt!=0; cnt-- );
}

void main (void)
{
	unsigned short position;
	unsigned short recordOffset;
	unsigned short position1;
	unsigned short position2;
	unsigned short position3;

	char os1, os2, os3, pos1, pos2, pos3;
	char vel1,vel2,vel3;

	// disable watchdog timer
	PCA0MD &= ~0x40; // WDTE = 0 (clear watchdog timer enable)

	init();

	for(;;)
	{
		// Retrieve starting position.
		// Doesn't matter that this is semi-inaccurate
		// need to do some sort of variance testing to make sure
		// 1 invalid value doesn't ruin everything
		pause(255);

		// could prob do some sort of subdivision thing here to speed things up
		for( recordOffset=0; recordOffset<32768; recordOffset+=256 )
		{
			// sample at recordOffset
			EIE1 &= ~0x02;
			sample(-recordOffset,3);
			EIE1 |= 0x02; // re-enable usb

			// pause before next wave
			pause(255);


			// make sure the first 256(128) positions have something in them
			if( sum(0,128)>16 ) break;
		}

		// find individual position offsets
		position1 = exceeds(8,1); // both *2 as 2 samples per memory pos
		position2 = exceeds(8,2);
		position3 = exceeds(8,3);

		// analyse phase and adjust all positions to coincide
		phase(); vel1 = 0;vel2 = 0;vel3 = 0;
		os1 = smpl[1021]; 
		os2 = smpl[1022]; 
		os3 = smpl[1023]; 

		while (1)
		{	
			// find minimum positionValue
			position = position1;
			if( position2<position) position=position2;
			if( position3<position ) position=position3;

			// timer runs at sysclock / 4 -> 6Mhz
			// sample clock runs at sysclock / 10 -> 2400Khz ~ 1200Khz per 2 samples
			// ratio is 5 timer clocks per 2 samples clocks
			// NOT: best fit is 5 timer clocks per 2 sample clocks
			// so if 128 sample clocks under shift by 32 sample clocks
			// which is 80 timer clocks

			// check if within lower range bounds
			if( position < 64 )
			{
				// dec value
				position1 += 60;
				position2 += 60;
				position3 += 60;
				recordOffset -= 150;
			}

			// check if within upper range bounds
			if( position > 200 )
			{
				// inc value
				position1 -= 120;
				position2 -= 120;
				position3 -= 120;
				recordOffset += 300;
			}

			// sample
			EIE1 &= ~0x02; // have to disable usb interrupt here to prevent it interrupting sampling
			/*recordOffset = */sample( -recordOffset, 3 );
			EIE1 |= 0x02; // re-enable usb

			// missing signal, ping for new position
			if( position1>1800 || position2>1800 || position3>1800 || sum(0,640)<32) break;

			// TODO: check quality of signal
			// if failed break outta this loop and re-ping for locations
			phase();
			
			// adjust positions based on phase results
			// old phases are os# new phases are pos#
			// so assuming for now a stationary target
			// move closer is abs(pos-os) ... yadda yadda
		
			// find min difference between phases
			pos1 = smpl[1021];
			vel1 = adj(pos1-os1,vel1); os1 = pos1;
			position1 += vel1;

			pos2 = smpl[1022];
			vel2 = adj(pos2-os2,vel2); os2 = pos2;
			position2 += vel2;
			
			pos3 = smpl[1023];
			vel3 = adj(pos3-os3,vel3); os3 = pos3;
			position3 += vel3;

			// write back to usb
			EIE1 &= ~0x02;
			*((unsigned short*)In_Packet) = recordOffset;
			*((unsigned short*)(In_Packet+2)) = position1;
			*((unsigned short*)(In_Packet+4)) = position2;
			*((unsigned short*)(In_Packet+6)) = position3;
			EIE1 |= 0x02; // re-enable usb
		}
	}
}

//-------------------------
// Delay
//-------------------------
// Used for a small pause, approximately 80 us in Full Speed,
// and 1 ms when clock is configured for Low Speed
//
/*void delay( unsigned short time )
{
	time =- time;

	TCON = 0x00; // stop timer

	TL0 = time&0xFF; // set timer
	TH0 = time/256;
	
	TF0 = 0; // reset interrupt bit

	TCON = 0x10;

	for(;TF0==0;);

	TCON = 0x00;
}*/

//-----------------------------------------------------------------------------
// sysclockInit
//-----------------------------------------------------------------------------
//
// This routine initializes the system clock to use the internal 24.5MHz / 8 
// oscillator as its clock source.  Also enables missing clock detector reset.
//
void init(void)
{
// sysclockInit
#ifdef _USB_LOW_SPEED_

   OSCICN |= 0x03;                       // Configure internal oscillator for
                                         // its maximum frequency and enable
                                         // missing clock detector

   CLKSEL  = SYS_INT_OSC;                // Select System clock
   CLKSEL |= USB_INT_OSC_DIV_4;          // Select USB clock
#else
unsigned char n;

	// initialize clock sources
	OSCICN |= 0x03; // Configure internal oscillator for its maximum frequency
	CLKMUL = 0x00; // Select internal oscillator as input to clock multiplier
	CLKMUL |= 0x80; // Enable clock multiplier
	CLKMUL |= 0xC0; // Initialize the clock multiplier
	for(n=255;n!=0;n--);
	while(!CLKMUL & 0x20); // Wait for multiplier to lock
	CLKSEL = SYS_4X_DIV_2; // Select system clock
	CLKSEL |= USB_4X_CLOCK; // Select USB clock

#endif  /* _USB_LOW_SPEED_ */ 

// usb0Init
   POLL_WRITE_BYTE(POWER,  0x08);          // Force Asynchronous USB Reset
   POLL_WRITE_BYTE(IN1IE,  0x07);          // Enable Endpoint 0-2 in interrupts
   POLL_WRITE_BYTE(OUT1IE, 0x07);          // Enable Endpoint 0-2 out interrupts
   POLL_WRITE_BYTE(CMIE,   0x07);          // Enable Reset, Resume, and Suspend interrupts
#ifdef _USB_LOW_SPEED_
   USB0XCN = 0xC0;                         // Enable transceiver; select low speed
   POLL_WRITE_BYTE(CLKREC, 0xA0);          // Enable clock recovery; single-step mode
                                           // disabled; low speed mode enabled
#else                                      
   USB0XCN = 0xE0;                         // Enable transceiver; select full speed
   POLL_WRITE_BYTE(CLKREC, 0x80);          // Enable clock recovery, single-step mode
                                           // disabled
#endif /* _USB_LOW_SPEED_ */

   EIE1 |= 0x02;                           // Enable USB0 Interrupts
   EA = 1;                                 // Global Interrupt enable
                                           // Enable USB0 by clearing the USB Inhibit bit
   POLL_WRITE_BYTE(POWER,  0x01);          // and enable suspend detection

   // portInit
	XBR0 = 0x00; // no digital peripherals selected                      
	XBR1 = 0x40; // Enable crossbar and weak pull-ups                       

	P0MDOUT = 0x0f; // P0.0, P0.1 push/pull
	P0MDIN  = 0x0f; // P0.0, P0.1 digital

	// init leds / switchs
	P2MDIN  = 0x0F; // all digital input
	P2MDOUT = 0x0C; // switchs open drain / leds push pull
	P2SKIP  = 0x00; // skip nothing

	P1MDIN  = 0x07; // all digital input
	P1MDOUT = 0x00; // switchs open drain
	P1SKIP  = 0x00; // skip nothing

	// timerInit
	// config T0
	CKCON = 0x01; // start T0, T0 runs off sysclock/4, should give us ~6m range
	TL0 = 0x00; // Timer 0 Low Byte
	TH0 = 0x00; // Timer 0 High Byte 
	TMOD = 0x01; // Timer Mode Register
	TCON = 0x00; // Timer Control Register stop 
}

//#define ULTRA_GROUND P0 = P0&~3
//#define ULTRA_INIT P0 = (P0&~3)|1
//#define ULTRA_TOGGLE P0 = P0^3

/**
void testTone(void)
{
	char pulseLength;
	char pause;

	ULTRA_INIT;
	for( pulseLength=255; pulseLength!=0; pulseLength-- )
	{
		// toggle ultrasonic outputs
		ULTRA_TOGGLE;
		for( pause=48; pause!=0; pause-- );
	}
	ULTRA_GROUND;
}
*/