#include "C:\Program Files\Microchip\mplabc30\v3.25\support\dsPIC30F\h\p30f4011.h"
//#include "p30f4011.h"
#include <libpic30.h>
#include <math.h>
//#include "can_functions.c"
#include "circbuf.c"
#define BAUD2    19200

#define SERVO

#ifdef SERVO
	#define BAUD1 19200
#else 
	#define BAUD1 19200
#endif

#define FCY		5000000
#define LED_CHARACTERIZE 0b100000000; //8
#define LED_RPM 0b10000000;//7

_FOSC(CSW_FSCM_OFF & XT); //_FOSC(CSW_FSCM_OFF & XT_PLL4); 
_FWDT(WDT_OFF);                 /* Turn off the Watch-Dog Timer.  */
_FBORPOR(PBOR_OFF & MCLR_EN);
_FGS(CODE_PROT_OFF);

  


void InitTMRs(void);
void init_uart(void);
//
char serial_data;
/*******************************************************************/



/*********************************************************************
MAIN
*********************************************************************/
struct circbuff rxbuffer;
struct circbuff txbuffer;
//rxbuffer.numel = 0; //zero elements in buffer
int a = 1;
int txstat = 0;
int main(void){

   //Initialize The circular buffer
   rxbuffer.numel =0;
   rxbuffer.start = rxbuffer.ch;
   rxbuffer.end = rxbuffer.ch;

   txbuffer.numel =0;
   txbuffer.start = txbuffer.ch;
   txbuffer.end = txbuffer.ch;
   
	char c = 0; 
    //***************set pins to output and initialize********************//
	TRISB = 0b1111111000110111;
	TRISD = 0xFFFF;
	TRISF = 0xFFFF;
	TRISE = 0xFFFF;

	//InitTMRs();
	init_uart();
	
    
    LATB = 0;
    while(1){
        if(U2STAbits.OERR)
            U2STAbits.OERR = 0;
        a++;
        c++;
        /* Tx text
        U1STAbits.UTXEN = 1;  
        txstat = 1;
        U1TXREG = c;
        while(txstat){
         a++;
		}*/
        while(rxbuffer.start!=rxbuffer.end)
		{
		  txstat = 1;
   	  U1TXREG = next(&rxbuffer);
		  //while(txstat){
           while(U1STAbits.UTXBF){
         	a++;
			}
		}
        /*while(txbuffer.start!=txbuffer.end)
		{
		  txstat2 = 1;
   	      U2TXREG = next(&txbuffer);
		  while(txstat2){
         	c++;
			}
		}  */        
        
	    if(a==25000){

		  LATB ^= 1<<7;
          //LATB ^= 1<<5;
		}
		 	
     
    } 	// End While 
 
}	// End of main

void __attribute__((interrupt)) _U2RXInterrupt(void)
{
    int status = 1;
    if(U2STAbits.URXDA)//if serial data is available
    {
        serial_data = U2RXREG;
		status = addlastbuff(&rxbuffer,serial_data);
     }
   IFS1bits.U2RXIF = 0; // clear flag
}  

void __attribute__((interrupt)) _U1TXInterrupt(void)
{
   txstat = 0; 
   IFS0bits.U1TXIF = 0; // clear flag
}


void init_uart()
{
     U2BRG = ((FCY/16)/BAUD2) - 1;    // set baud rate of uart2
	 U1BRG = ((FCY/16)/BAUD1) - 1;    // set baud rate of uart1

      IEC1bits.U2RXIE = 1;  // rx int enabled for uart2
      IEC0bits.U1TXIE = 1; //enable tx interrupts
      U2MODEbits.PDSEL = 0; //8bit, no parity
     U2MODEbits.STSEL = 0; //1 stop bit
	U1MODEbits.PDSEL = 0; //8bit, no parity

    #ifdef SERVO
     U1MODEbits.STSEL = 1; //2 stop bits
	#else
	 U1MODEbits.STSEL = 0; //1 stop bit
	#endif

U1MODEbits.ALTIO =0; //use U1ARX & U1ATX
     U2STAbits.URXISEL = 0; //Interrupt on shift register transfer
     U2MODEbits.UARTEN=1; //enable UART2
	
     U1MODEbits.UARTEN=1; //enable UART1
     //U2STAbits.URXISEL = 0b00;
     U1STAbits.UTXISEL =0; // set UTXIF flag when there is one empty word in tx bbuffer
     U1STAbits.UTXEN = 1;           // Initiate transmission on UART 1

}

//********************************************end of main***************************************


// Initialize timers	
void InitTMRs(void)
{
  T4CON = 0x8000; // internal Tcy/1		
  T3CON = 0x8030; // internal Tcy/256 //19531 = 1 second
  T2CON = 0x8000; // internal Tcy/1
  T1CON = 0x8000; // internal Tcy/1
	
  TMR3 = 0; // Init timers to 0;
  TMR2 = 0; 
  TMR4 = 0;
  TMR1 = 0;
}