#include "UGSheader.h"
#include "Timer.h"
#include "Interrupts.h"
#include "usart.h"
#include "ADC.h"

#pragma code high_vector=0x8

void high_vector (void)
{
_asm goto Interrupts _endasm
}

#pragma code 
#pragma interrupt Interrupts

void Interrupts (void){
	
	unsigned int receive1, receive2, ad, dummy, dummyAD, n, i;
	unsigned char msg1, high1, low1, msg2, high2, low2, ADh[16];
	
if (PIR1bits.RC1IF == 1){
	
	//PORTBbits.RB0 =!PORTBbits.RB0;

	receive1=RCREG1;
	msg1=(receive1>>7);
	
		if (msg1==0 & rec_n1==16)
		{
			high1=((receive1 & 0x78)>>3);
			low1=(receive1 & 0x07);
	
			if ((high1==ESTADO) & (low1 <16))
			{
				rec_n1=ESTADO;
				estado=low1;
			}
			
			if ((high1 ==MOTORES) & (low1 <16))
			{
				rec_n1=MOTORES;
				motores=low1;
			}
		}
		else if (msg1==1 & rec_n1==ESTADO)
		{
			switch(estado){
				case ACTUALIZAR:
					//actualiza();
					break;	
				case LEDS:
					leds=(receive1 & 1);
					break;
				case ILUMINACAO:
					PORTCbits.RC3=(receive1 & 1);
					break;
				case PONTEIRO:
					PORTCbits.RC2=(receive1 & 1);
					break;
				case VALVO:
					data_act2[0]=(0 << 7 | ESTADO << 3 | VALVO);
					data_act2[1]=receive1;
					
					for(n=0;n<=1;n++)
					{
					while(!TXSTA2bits.TRMT);
					TXREG2 = data_act2[n];
					}
					
					break;
				case VALVC:
					data_act2[0]=(0 << 7 | ESTADO << 3 | VALVC);
					data_act2[1]=receive1;
					
					for(n=0;n<=1;n++)
					{
					while(!TXSTA2bits.TRMT);
					TXREG2 = data_act2[n];
					}
					
					break;															
			}			
		rec_n1=16;	
		}
		else if (msg1==1 & rec_n1==MOTORES)
		{			
			switch(motores){
				case SERVO:
				duty=(receive1 & 0x7F)/10000.0;		
				SetPWMt (20000000, 50, 2);
				
				data_act1[0]=(0 << 7 | MOTORES << 3 | SERVO);
				data_act1[1]=(1 << 7 | (int)(duty*10000));
				
				for(n=0;n<=1;n++)
				{
				while(!TXSTA1bits.TRMT);
				TXREG1 = data_act1[n];
				}
	
				break;
				case VENTOINHA:
				PORTCbits.RC1=(receive1 & 1);
				break;
			}		
		rec_n1=16;	
		}	
}	
if (PIR3bits.RC2IF == 1){
	
	receive2=RCREG2;
	msg2=(receive2>>7);
	
	while(!TXSTA1bits.TRMT);
	TXREG1 = receive2;
	
	if (msg2==0 & rec_n2==16){
		
		high2=((receive2 & 0x78)>>3);
		low2=(receive2 & 0x07);
		
		if ((high2==DISTANCIA) & (low2 <16))
		{
			rec_n2=DISTANCIA;
			distancia=low2;
		}
		if ((high2==GARRA) & (low2 <16)){
			rec_n2=GARRA;
			garra=low2;
		}	
	}
	else if (msg2==1 & rec_n2==DISTANCIA){
		dist_sen[distancia]=(receive2 & 0x7F);
		rec_n2=16;
	}			
	else if (msg2==1 & rec_n2==GARRA){
		estado_garra=(receive2 & 0x7F);	
		rec_n2=16;
	}
}
	
if (PIR1bits.ADIF == 1){
		
		ADh[nch]=ADRESH;
		dummyAD=ADRESL;
		
		nch++;
		
		PIR1bits.ADIF = 0;
	
	if(nch<=4){
		
		SetADChannel (nch);
		for(i==0;i<65000;i++){}
		ADCON0bits.GO_DONE=1;
	}
	
	if(nch==5){	
		
		RCSTA2bits.CREN = 0; 
		data_act1[0]=(0 << 7 | ULTRASONS << 3 | 0);
		data_act1[1]=(1 << 7 | ((ADh[0]*128)/256));
		data_act1[2]=(0 << 7 | ULTRASONS << 3 | 1);
		data_act1[3]=(1 << 7 | ((ADh[1]*128)/256));
		data_act1[4]=(0 << 7 | ULTRASONS << 3 | 2);
		data_act1[5]=(1 << 7 | ((ADh[2]*128)/256));
		data_act1[6]=(0 << 7 | ULTRASONS << 3 | 3);
		data_act1[7]=(1 << 7 | ((ADh[3]*128)/256));
		data_act1[8]=(0 << 7 | TEMPERATURA << 3 | 3);
		data_act1[9]=(1 << 7 | ((ADh[4]*128)/256));
		data_act1[10]=(0 << 7 | DISTANCIA << 3 | 0);
		data_act1[11]=(1 << 7 | (dist_sen[0]));
		data_act1[12]=(0 << 7 | DISTANCIA << 3 | 1);
		data_act1[13]=(1 << 7 | (dist_sen[1]));
		data_act1[14]=(0 << 7 | DISTANCIA << 3 | 2);
		data_act1[15]=(1 << 7 | (dist_sen[2]));
		data_act1[16]=(0 << 7 | DISTANCIA << 3 |3);
		data_act1[17]=(1 << 7 | (dist_sen[3]));		
		data_act1[18]=(0 << 7 | GARRA << 3 |garra);
		data_act1[19]=(1 << 7 | estado_garra);
			
		for(n=0;n<=19;n++)
		{
		while(!TXSTA1bits.TRMT);
		TXREG1 = data_act1[n];
		}
		
		RCSTA2bits.CREN = 1; 
	}	
	}
//interrupt do timer0
if (INTCONbits.TMR0IF == 1){
	
	T0CONbits.TMR0ON = 0; //timer0 off
			
	if (PORTCbits.RC0 == 0)
	{
	PORTCbits.RC0=1;
	TMR0H = (int)onH;
	TMR0L = (int)onL;
	}
	else
	{
	PORTCbits.RC0=0;
	TMR0H = (int)offH;
	TMR0L = (int)offL;
	}
	
	T0CONbits.TMR0ON = 1; //timer0 on
	INTCONbits.TMR0IF = 0;
}	

//interrupt do timer2
if (PIR1bits.TMR2IF == 1){
	
	tmr2_c++;
	
	if(tmr2_c==512){
		
		tmr2_c=0;
		
		nch=0;	
		SetADChannel (nch);

		ADCON0bits.GO_DONE=1;
	}
	
	if(leds==0){
		PORTB=0b00000000;
	}	
	else if(leds==1){
		
		if(estado_UGI==1){
		PORTBbits.RB0 =1;	
		PORTBbits.RB1 =0;		
		}
		
		if(garra==0 & estado_garra==0){
		PORTBbits.RB2 =1;	
		PORTBbits.RB3 =0;	
		}
		else if(garra==0 & estado_garra==1){
		PORTBbits.RB2 =0;	
		PORTBbits.RB3 =1;	
		}
		else{
		PORTBbits.RB2 =1;	
		PORTBbits.RB3 =1;	
		}
	}	
}	
}