// #define __dsPIC33FJ64MC202__
#include <p33Fxxxx.h>
#include <uart.h>
#include <pwm12.h>
//#include "slnode11dspic.c"
#include "slnode.h"
#include "adc.h"

// Three phase driver test

// option bits, -> .h 
_FBS(RBS_NO_RAM & BSS_NO_BOOT_CODE & BWRP_WRPROTECT_OFF); // no change
_FSS(RSS_NO_RAM & SSS_NO_FLASH & SWRP_WRPROTECT_OFF);     // no change
_FGS(GSS_OFF & GCP_OFF & GWRP_OFF); // probably no change
_FOSCSEL(FNOSC_PRI & IESO_ON);      // clock option 1
_FOSC(FCKSM_CSECMD & IOL1WAY_OFF & OSCIOFNC_ON & POSCMD_HS); // clk opt main
_FWDT(FWDTEN_OFF & WINDIS_OFF & WDTPRE_PR128 & WDTPOST_PS32768); // WDT
_FPOR(PWMPIN_ON & HPOL_OFF & LPOL_OFF   // PWM init state, should be condidered 
      & ALTI2C_OFF & FPWRT_PWR64); // not significant
_FICD(JTAGEN_OFF & ICS_PGD3);  // ICS should be set to using PG port

_FUID0(0x19); _FUID1(0x74); _FUID2(0x03); _FUID3(0x31);  // user id



void OnPWMCycle(void);
// include sub soruce
#include "MouseHardware.c"

// void ClockUp(void) : staring up PLL clock
// void IORemapping(void) : remapping for UART to RP8 & 9
// void SetupMCPWM(void) : init MCPWM
// void SetDutyCycle(unsigned int ph1,unsigned int ph2,unsigned int ph3) : set PWM, 0- raw
// void SetOutput(int ph1,int ph2,int ph3) : set output +-500
// void DisablePWM(void)
// void EnablePWM(void)
// int SetupADC(void) : setup AD with DMA
// int Cosine(int x) : 16bit input 16bit output quasi-cosine

volatile long PWMsysclock=0;
volatile long AD[4];

// called on each PWM cycle
void OnPWMCycle(void)
{
	// y=(1-r)y+ru
	// y=y+r(u-y)
	//AD+=(ADC<<N-AD)>>N 
	//AD+=(ADC-AD>>N)
#define ADS 1
#define ADFLT(d,s) d=d-((d+0x1ff)>>7)+(signed int)(s)

	//AD[0]=ADCbufferA[0];
	//AD[1]=ADCbufferA[1];
	//AD[2]=ADCbufferA[2];
	//AD[3]=ADCbufferA[3];
	ADFLT(AD[0],ADCbufferA[0]);
	ADFLT(AD[1],ADCbufferA[1]);
	ADFLT(AD[2],ADCbufferA[2]);
	ADFLT(AD[3],ADCbufferA[3]);
	PWMsysclock++;
}

#include "ADNS6010srom.c"

#define MNCS0  LATBbits.LATB3                                                    
#define MNCS1  LATBbits.LATB2                                                    
#define MNCS2  LATBbits.LATB1                                                    
#define MNCS3  LATBbits.LATB0
#define MNCSM  0x000f                                                    
#define SETMNCS LATB=LATB|MNCSM
#define CLRMNCS LATB=LATB&(~MNCSM)
#define MMISO0 PORTBbits.RB12
#define MMISO1 PORTBbits.RB13
#define MMISO2 PORTBbits.RB14
#define MMISO3 PORTBbits.RB15

#define MSCLK LATBbits.LATB11
#define MMOSI LATBbits.LATB10
#define MRESET LATBbits.LATB4
#define MNPD  LATBbits.LATB7
#define MTRIS 0xf360  // IS NPD,RES,NCS,SCLK,MOSI=out(0) 1111 0011 01100000
#define DEBUGPIN LATAbits.LATA4
#define PINDON DEBUGPIN=1;
#define PINDOFF DEBUGPIN=0;

unsigned char AR0,AR1,AR2,AR3;

void ReadValues(unsigned char addr)
{
	int i;
	long ns;
	PINDON;
	TRISB=MTRIS; // IS NCS,SCLK,MOSI=out 10 0010 11
	addr=addr&0x7f;
	for(i=0;i<10;i++) SETMNCS;
	for(i=0;i<10;i++) CLRMNCS;

	for(i=0;i<8;i++)
	{
		MSCLK=0; MSCLK=0;
		MMOSI=(addr&0x80)?1:0;
		addr=addr<<1;
		MSCLK=0; MSCLK=1; MSCLK=1; MSCLK=1;
	}
	ns=PWMsysclock;
	while(PWMsysclock<ns+2)
		;
	AR0=AR1=AR2=AR3=0;
	for(i=0;i<8;i++)
	{
		MSCLK=0; // MSCLK=0; MSCLK=0;
		AR0=AR0<<1;  AR1=AR1<<1;  AR2=AR2<<1;  AR3=AR3<<1;
		AR0=AR0|(MMISO0?1:0);
		AR1=AR1|(MMISO1?1:0);
		AR2=AR2|(MMISO2?1:0);
		AR3=AR3|(MMISO3?1:0);
		MSCLK=1; MSCLK=1; MSCLK=1;
	}
	SETMNCS;
//	TRISB=0xbfff;
	PINDOFF;
	return ;
}

unsigned char ReadValue(unsigned char addr)
{
	ReadValues(addr);
	return AR0;
}

void WriteValue(unsigned char addr,unsigned char val)
{
	int i;
	PINDON;
	TRISB=MTRIS; 
	addr=addr|0x80;
	for(i=0;i<10;i++) SETMNCS;
	for(i=0;i<10;i++) CLRMNCS;

	for(i=0;i<8;i++)
	{
		MSCLK=0; MSCLK=0;
		MMOSI=(addr&0x80)?1:0;
		addr=addr<<1;
		MSCLK=0; MSCLK=1; MSCLK=1; MSCLK=1;
	}
	for(i=0;i<8;i++)
	{
		MSCLK=0; MSCLK=0;
		MMOSI=(val&0x80)?1:0;
		val=val<<1;
		MSCLK=0; MSCLK=1; MSCLK=1; MSCLK=1;
	}
	SETMNCS;
//	TRISB=0xbfff;
	PINDOFF;
	return ;
}

#define SDELAY(ns,a) { ns=PWMsysclock;	while(PWMsysclock<ns+(a)); }

void DownloadROM(void)
{
	long ns;
	int i,j;
	unsigned char v;

	WriteValue(0x14,0x1d);
	SDELAY(ns,20);
	WriteValue(0x14,0x18);
	SDELAY(ns,2);
	for(i=0;i<10;i++) CLRMNCS;
	v=0x60|0x80;
	for(i=0;i<8;i++)
	{
		MSCLK=0; MSCLK=0;
		MMOSI=(v&0x80)?1:0;
		v=v<<1;
		MSCLK=0; MSCLK=1; MSCLK=1; MSCLK=1;
	}
	for(j=0;j<1986;j++)
	{
		for(i=0;i<100;i++) CLRMNCS;
		v=Srom[j];
		for(i=0;i<8;i++)
		{
			MSCLK=0; MSCLK=0;
			MMOSI=(v&0x80)?1:0;
			v=v<<1;
			MSCLK=0; MSCLK=1; MSCLK=1; MSCLK=1;
		}
	}		
	for(i=0;i<1000;i++) CLRMNCS;
	SETMNCS;
	SDELAY(ns,2);
	
	WriteValue(0x14,0xa1);
	SDELAY(ns,20*8);
	WriteValue(0x14,0x00);
}

void DeviceReset(void)
{
	TRISB=MTRIS; // IS NCS,SCLK,MOSI=out 10 0010 11
	long ns;
	MRESET=1;
	SDELAY(ns,2);
	MRESET=0;
	MNPD=1;
}



void InitParams(void)
{
	long ns;
	SDELAY(ns,1);
	WriteValue(0x19,0xe0);
	SDELAY(ns,1);
	WriteValue(0x1a,0x2e);
	SDELAY(ns,20);

	WriteValue(0x1b,0x3e);
	SDELAY(ns,1);
	WriteValue(0x1c,0x0d);
	SDELAY(ns,20);

	WriteValue(0x1d,0xa2);
	SDELAY(ns,1);
	WriteValue(0x1e,0x21);
	SDELAY(ns,20);

	WriteValue(0x2c,0x13);
	SDELAY(ns,1);
	WriteValue(0x2d,0xec);
	SDELAY(ns,20);

	WriteValue(0x0a,0x5d);// 5d, rate set
	SDELAY(ns,20);
}

#define NCH 4
unsigned char BMotion[4*NCH];
#define Motion (BMotion+0)
#define DeltaX ((char *)(BMotion+4))   // +NCH*1
#define DeltaY ((char *)(BMotion+8))   // +NCH*2
#define Squal  (BMotion+12)          // +NCH*3

void MotionRead(void)
{
	int i,j;
	unsigned char r0,r1,r2,r3,addr;
	unsigned char *bm;
	long ns;
	addr=0x50;
	for(i=0;i<10;i++) SETMNCS;
	for(i=0;i<10;i++) CLRMNCS;

	for(i=0;i<8;i++)
	{
		MSCLK=0; MSCLK=0;
		MMOSI=(addr&0x80)?1:0;
		addr=addr<<1;
		MSCLK=0; MSCLK=1; MSCLK=1; MSCLK=1;
	}
	SDELAY(ns,3);
	bm=BMotion;
	for(j=0;j<4;j++)
	{
		r0=0; r1=0; r2=0; r3=0;
		for(i=0;i<8;i++)
		{
			MSCLK=0;// MSCLK=0; MSCLK=0;
			r0=r0<<1; r1=r1<<1; r2=r2<<1; r3=r3<<1;
			r0=r0|(MMISO0?1:0);
			r1=r1|(MMISO1?1:0);
			r2=r2|(MMISO2?1:0);
			r3=r3|(MMISO3?1:0);
			MSCLK=1; MSCLK=1; MSCLK=1;
		}
		*bm=r0; bm++;
		*bm=r1; bm++;
		*bm=r2; bm++;
		*bm=r3; bm++;
	}
	SETMNCS;
	return ;
}



int main(void)
{
	int i,c,l;
	int x[NCH],y[NCH];
	long ns;
	long lastclock;
	long cyclerest;

	// InitializeHardware -> TPHardware.c
	LATB=0x0;
	LATA=0x0;
	TRISA=0xffef; // RA4 out
	//TRISB=0x037f;  // RB10-15 out(PWM), RB7=out(test point, debug)
	//TRISA=0x000f;  // RA4 out(Fault clear)
	//PORTAbits.RA4=1;
	ClockUp();
	IORemapping();
	SetupMCPWM();
	DisablePWM();
	SetupADC();
	InitializeSerialLoop(1,SLB115200,4,"ADNSSensorNode");
	for(i=0;i<16;i++) ADCbufferA[i]=i*0x11;
	for(i=0;i<16;i++) { RegFileS[i]=0; RegFileL[i]=0; }
	i=0; c=0; l=0;
	DeviceReset();
	DownloadROM();
	InitParams();
	for(i=0;i<NCH;i++)
	{	x[i]=0; y[i]=0; }
	RegFileS[2]=0xf1ff;
	
	ns=PWMsysclock+20;
	while(ns>PWMsysclock)
		;
	DownloadROM();

	SLReply32(63,1,0x12345678);
	
	while(1)
	{
		// lock to 1kHz, 20kHz 20cycle.
		cyclerest=ns-PWMsysclock;   // it should be >=0 anytime, but single -1 should be fine
		while(ns>PWMsysclock)
			;
		ns+=20;
		RegFileL[0]++;
		c++; l++;
		MotionRead();
		for(i=0;i<NCH;i++)		
		{ x[i]+=DeltaX[i];	y[i]+=DeltaY[i]; }
		if(RegFileS[1])
		{
			if(RegFileS[1]==1) DownloadROM();
			if(RegFileS[1]==2) InitParams();
			if(RegFileS[1]==3) DeviceReset();
			RegFileS[1]=0;
		}
		if(RegFileS[3]) // substitute AD for ch3
		{
			x[3]=AD[2]>>ADS; y[3]=AD[1]>>ADS;
		}
		// report cycle info (not necessary)
		if(c>=(RegFileS[2]&0x1ff))
		{
			switch(RegFileS[2]&0xc00)
			{
			case 0x000:
			case 0x400: // req x
				if(RegFileS[2]&0x1000) SLReply16(63,4,x[0]);
				if(RegFileS[2]&0x2000) SLReply16(63,5,x[1]);
				if(RegFileS[2]&0x4000) SLReply16(63,6,x[2]);
				if(RegFileS[2]&0x8000) SLReply16(63,7,x[3]);
				break;
			case 0x800: // req y
				if(RegFileS[2]&0x1000) SLReply16(63,4,y[0]);
				if(RegFileS[2]&0x2000) SLReply16(63,5,y[1]);
				if(RegFileS[2]&0x4000) SLReply16(63,6,y[2]);
				if(RegFileS[2]&0x8000) SLReply16(63,7,y[3]);
				break;
			case 0xc00:  // req x&y
				if(RegFileS[2]&0x1000) SLReply32(63,4,((unsigned long)(y[0])<<16)|((unsigned long)(x[0])&0xffff));
				if(RegFileS[2]&0x2000) SLReply32(63,5,((unsigned long)(y[1])<<16)|((unsigned long)(x[1])&0xffff));
				if(RegFileS[2]&0x4000) SLReply32(63,6,((unsigned long)(y[2])<<16)|((unsigned long)(x[2])&0xffff));
				if(RegFileS[2]&0x8000) SLReply32(63,7,((unsigned long)(y[3])<<16)|((unsigned long)(x[3])&0xffff));
				break;
			}
			c=0;
		}
		if((RegFileS[2]&0x200)&&((l&0xfff)==(0xfff)))
		{
			{
				SLReply32(63,15,cyclerest);
				SLReply32(63,14,PWMsysclock-lastclock); 
				SLReply16(63,13,(ReadValue(0x0d)<<8)|ReadValue(0x0c));
				SLReply16(63,12,(ReadValue(0x11)<<8)|ReadValue(0x10));
				SLReply16(63,11,(ReadValue(0x1a)<<8)|ReadValue(0x19));
				SLReply16(63,10,(ReadValue(0x1c)<<8)|ReadValue(0x1b));
				SLReply16(63, 9,(ReadValue(0x1e)<<8)|ReadValue(0x1d));
				SLReply16(63, 8,ReadValue(0x1f));
				SLReply16(63, 7,(ReadValue(0x2d)<<8)|ReadValue(0x2c));
				SLReply16(63, 6,(ReadValue(0x0b)<<8)|ReadValue(0x0a));
			}
			lastclock=PWMsysclock;
		}
	}
}