#include"precomp.h"
#include"rsbus.h"
#include"sysctl.h"

int cnt=0;
int cnt2=0;
int cnt3=0;

void port_init(void)
{
    P1DIR |= BIT0+BIT6;      // P1.0 P1.6 output
    P1OUT = 0;
}

void received(unsigned char* str,unsigned int len)
{
	unsigned char ch;
	switch (str[0])
	{
	case 1:// start
		if (!MSTATE)
		{
			SWITCH_STATE(ST_RUN);
			cnt2=0;
			cnt3=0;
			STATE(0)=0;
			SYSCALL(TEST);
		}
		RETSTATE;
		break;
	case 2:// Get Res
		if (MSTATE&ST_RUN)
		{
			ch=(unsigned char)(cnt2-STATE(0));
			rsbus_w(0,&ch,1);
		}
		else
		if (MSTATE&ST_RES)
		{
			SWITCH_STATE(ST_REST);
			ch=(unsigned char)(cnt2);
			rsbus_w(0,&ch,1);
		}
		else
			RETSTATE;
		break;
	case 3:// Stop
		if (MSTATE&ST_RUN)
		{
			SWITCH_STATE(ST_REST);
			SYSSTOP(TEST);
		}
		RETSTATE;
		break;
	case 4:// Block
		if (!MSTATE)
		{
			cnt2=0;
			SWITCH_STATE(ST_BUSY);
		}
		RETSTATE;
	}
	//rsbus_w(0,str,len);
}

// Timer A0 interrupt service routine
#pragma vector=TIMER0_A0_VECTOR
__interrupt void Timer_A0 (void)
{
	++cnt;
	if (cnt==25)
	{
		STATE(0)=(STATE(0)+1)&0xFF;
		if ((MSTATE&ST_BUSY) && !(MSTATE&ST_RES))
		{
			++cnt2;
			if (STATE(0)>200)
				SWITCH_STATE(ST_BUSY|ST_RES);
		}
		cnt=0;
		P1OUT^=BIT6;
	}
}

//TIMER A0 initialize -
// desired value: 40ms
void TimerA0_Init(void)
{
	// Configure TimerA0
	TA0CTL = TASSEL_2 + MC_1 +ID_3 ;      // Source: SMCLK=4MHz, UP mode, DIV by 8 -> 0.5M
	TA0CCR0 = 20000;					// 0.5MHz / 20000 -> 25Hz -> 40ms
	TA0CCTL0 = CCIE;                  	// CCR0 interrupt enabled
}

void init_devices(void)
{
	WDTCTL = WDTPW + WDTHOLD;                 // Stop watchdog timer

	if (CALBC1_8MHZ ==0xFF || CALDCO_8MHZ == 0xFF)
		while(1)_BIS_SR(CPUOFF);            	// If calibration constants erased, trap CPU!!

	BCSCTL1 = CALBC1_16MHZ; 		    // Set range
	DCOCTL = CALDCO_16MHZ;  		    // Set DCO step + modulation£¬DCO=8MHz
	BCSCTL3 |= LFXT1S_2;                      // LFXT1 = VLO
	IFG1 &= ~OFIFG;                           // Clear OSCFault flag
	BCSCTL2 |= DIVS_2;      //  SMCLK = DCO/4 = 4MHz

	sysctl_init();
	port_init();
	TimerA0_Init();
	rsbus_init(&received);

	_BIS_SR(GIE);
}

// Clock tick using cycle estimation
void basic_running_ex()
{
	delay_ms(2);
	++cnt2;
	if (cnt2==250)
	{
		++cnt3;
		cnt2=0;
	}
	SYSCALL(TEST);
}

void main(void)
{
	init_devices();
	sysctl_reghdlr(TEST,basic_running_ex);
	while(1)
		sysroutine();
}