// #define __dsPIC33FJ64MC202__
#include <p33Fxxxx.h>
#include <uart.h>
#include <pwm12.h>
#include "slnode.h"
#include "adc.h"
#include "i2c.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);
volatile long PWMsysclock=0;
// include sub soruce
#include "Hardware.c"
#include "MPU6050.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 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++;
}

#ifndef SDR2
#define SDR2(a)  (((a)+   0x1)>> 2)
#define SDR3(a)  (((a)+   0x3)>> 3)
#define SDR4(a)  (((a)+   0x7)>> 4)
#define SDR5(a)  (((a)+   0xf)>> 5)
#define SDR6(a)  (((a)+  0x1f)>> 6)
#define SDR7(a)  (((a)+  0x3f)>> 7)
#define SDR8(a)  (((a)+  0x7f)>> 8)
#define SDR9(a)  (((a)+  0xff)>> 9)
#define SDR10(a) (((a)+ 0x1ff)>>10)
#define SDR11(a) (((a)+ 0x3ff)>>11)
#define SDR12(a) (((a)+ 0x7ff)>>12)
#define SDR13(a) (((a)+ 0xfff)>>13)
#define SDR14(a) (((a)+0x1fff)>>14)
#define SDR15(a) (((a)+0x3fff)>>15)
#define SDR16(a) (((a)+0x7fff)>>16)
#endif

#define USE_LEDSW 1

#ifdef USE_LEDSW
void SetLED(char sw)
{
	if(sw&1) LATBbits.LATB7=1;  else LATBbits.LATB7=0;
	if(sw&2) LATBbits.LATB10=1; else LATBbits.LATB10=0;
	if(sw&4) LATBbits.LATB12=1; else LATBbits.LATB12=0;
}

char GetSwitch(void)
{
	char r=0;
	if(PORTAbits.RA4) r|=1;
	if(PORTBbits.RB4) r|=2;
	if(PORTBbits.RB3) r|=4;
	return r;
}
#endif

int main(void)
{
	int i,c,l,sw,osw;
	long ns;
	long lastclock;
	long cyclerest;
	int I2CR;

	// InitializeHardware -> TPHardware.c
	LATB=0x0;
	LATA=0x0;
	//TRISA=0xffff; // 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();
	InitializeMPU6050();

	//Switch&LED
#ifdef USE_LEDSW
	TRISBbits.TRISB7 =0;
	TRISBbits.TRISB10=0;
	TRISBbits.TRISB12=0;
	SetLED(0x5);
	AD1PCFGLbits.PCFG5=1;
#endif

	InitializeSerialLoop(1,SLB115200,6,"MPU6050-I2C-Sensor");
	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;
	RegFileS[2]=0x8020;
	RegFileS[3]=0x8020;

	Delay(20);

	for(i=0;i<20;i++) SLNop();	
	SLReply32(63,1,0x12345678);

	ns=PWMsysclock+20;
	
	while(1)
	{
		// lock to 200Hz, 20kHz 100cycle.
		cyclerest=ns-PWMsysclock;   // it should be >=0 anytime, but single -1 should be fine
		while(ns>PWMsysclock)
			;
		ns+=100;
		RegFileL[0]++;
		c++; l++;
#ifdef USE_LEDSW
		sw=GetSwitch();
		if(sw^osw) { SLReply16(63,4,(~sw)&0x7); }
		osw=sw;
		if(!(sw&2)) RegFileS[1]=1;
		SetLED(((l&0x40)?0x1:0)|(RegFileS[1]?0x4:0)|(RegFileS[4]?0x2:0));
#endif
		if(RegFileS[1]==0x10) // sensor reset
		{
			ResetMPU6050();
			RegFileS[1]=0;
		}
		if(RegFileS[1]==1) // zero caribration
		{
			RegFileS[1]=ReadMPU6050(RegFileS[1]);
		}
		else  // or normal operation
		{
			ReadMPU6050(0);
		}

		// report to serial port
		if(c>=(RegFileS[2]&0x1ff))
		{
			//ReadMPU6050Register();  // non-FIFO direct register read
			if(RegFileS[2]&0x8000) SLReply16(63, 8,imu.GX);	
			if(RegFileS[2]&0x4000) SLReply16(63, 9,imu.GY);	
			if(RegFileS[2]&0x2000) SLReply16(63,10,imu.GZ);	
			if(RegFileS[2]&0x1000) SLReply16(63,11,imu.AX);	
			if(RegFileS[2]&0x0800) SLReply16(63,12,imu.AY);	
			if(RegFileS[2]&0x0400) SLReply16(63,13,imu.AZ);
			//if(RegFileS[2]&0x0200) SLReply16(63,12,TMP);
			if(RegFileS[2]&0x0200) SLReply16(63,13,imu.FIFOc);

			// test code
			//SLReply16(63,14,I2CReceiveDataWord(0x72));  // fifo count
			/*I2CR=I2CReceiveDataByte(0x75); // Who am I
			SLReply16(63,8,I2CR);*/
			c=0;
		}
		if(c>=(RegFileS[3]&0xff))
		{
			if(RegFileS[3]&0x8000) SLReply16(63, 6,SDR7(imu.GyroAVX));	
			if(RegFileS[3]&0x4000) SLReply16(63, 7,SDR7(imu.GyroAVY));	
			if(RegFileS[3]&0x2000) SLReply16(63, 8,SDR12(imu.AccAngleX));
			if(RegFileS[3]&0x1000) SLReply16(63, 9,SDR12(imu.AccAngleY));
			if(RegFileS[3]&0x0800) SLReply16(63,10,SDR12(imu.GyroAngleX));	
			if(RegFileS[3]&0x0400) SLReply16(63,11,SDR12(imu.GyroAngleY));
			if(RegFileS[3]&0x0200) SLReply16(63,12,SDR12(imu.CGAngleX));	
			if(RegFileS[3]&0x0100) SLReply16(63,13,SDR12(imu.CGAngleY));
			c=0;
		}
		
		if((RegFileS[2]&0x200)&&((l&0xfff)==(0xfff)))
		{
			{
				SLReply32(63,15,cyclerest);
				SLReply32(63,14,PWMsysclock-lastclock); 
			}
			lastclock=PWMsysclock;
		}
	}
}

// carib check: 
// set16 6 2 0x00ff;set16 6 3 0xaa10;set16 6 1 1;
// set16 6 2 0x00ff;set16 6 3 0x5510;set16 6 1 1;