#include <htc.h>
#include "sonarcontrol.h"
#include "main.h"

unsigned short ccptemp[2];

void SetSonar(unsigned char n)
{
	PORTC |= (0x80 >> n);
}

void ResetSonar(unsigned char n)
{
	PORTC &= ~(0x80 >> n);
}

void SetMux(unsigned char n)
{
	PORTA &= 0xfc; // null DB0 and DB1
	PORTA |= n; // set address
}

void FallingEdge(unsigned char ccp)
{
	unsigned char i;
	unsigned char l,h;
	if(ccp == 0)
	{
		l = CCPR1L;
		h = CCPR1H;
		CCP1IE = 0;
		i = CurrentMode*3 + 1;
	}
	else
	{
		l = CCPR2L;
		h = CCPR2H;
		CCP2IE = 0;
		i = CurrentMode*3 + NextEcho;
		unsigned short test = h;
		test = test << 8;
		test += l;
		if(test > 1087) 
		{
			PORTE &= ~(0x01 << 0);
		}
	}
	
	Time[i][0] = l;
	Time[i][1] = h;
}

void Measure(unsigned char mode, unsigned char secondit)
{
		di();
		// init pulse
		if(secondit == 0)
		{
			SetSonar(reciever1[mode]);
			SetSonar(reciever2[mode]);
  			//	__delay_us(INIT_PULSE_WIDTH); // max je 600 us 
			ResetSonar(reciever1[mode]);
			ResetSonar(reciever2[mode]);
		}
		else
		{
			SetSonar(reciever3[mode]);
  			//	__delay_us(INIT_PULSE_WIDTH); // max je 600 us 
			ResetSonar(reciever3[mode]);
		}
		
		__delay_us(AFTER_INIT_PULSE);


		if(secondit == 0)
		{
			SetMux(mux[mode]);
			NextEcho = 0;
		}
		else
		{
			SetMux(mux_after[mode]);
			NextEcho = 2;
		}


		// measuring pulse
		SetSonar(transiever[mode]);

		// Capture init	
		CCP1M0 = 0;
		CCP2M0 = 0;
		CCP2IE = 1;
		if(secondit == 0)
		{
			CCP1IE = 1;
		}
		else
		{
			CCP1IE = 0;
		}

		CCP1IF = 0;
		CCP2IF = 0;
		TMR1H = 0;
		TMR1L = 0;
		TMR1ON = 1;


  		ei();

	
	// measure pulse ended
		__delay_us(MEAS_PULSE_WIDTH); // max je 600 us 
		ResetSonar(transiever[mode]); // inout = 0


		__delay_ms(5);
		CCP1IE = 0;
		CCP2IE = 0;
		TMR1ON = 0;
		__delay_ms(AFTER_MEAS_PULSE-5);
		if(secondit == 0)
		{
			Measure(mode,1);
		}
}