/*********************************************************************
 *											   						 *
 *    DESCRIPTION: main routine for Timer - Pulse Period/Width 		 *
 *                 Measurement										 *
 *											  						 *
 *											   						 *
 *    PURPOSE: 
 The srf section sets up two timers TB4 and TB1 for use with the SRF04.
 The TB4 timer is used to count how the echo delay, and the ISR calculates
 the distance in millimetres and puts it into the global srf_echo_dist.

 The TB1 timer is used as a timer to count out the 10us high output to 
 trigger the SRF04 (via port P10_2), then to wait out 10ms after the echo is received before
 the next transmission is allowed.
 
 
 ********************************************************************/
/*===========================================================================
*   Revision history: 


*==========================================================================*/

/* Include the required header files */
#include "skp_bsp.h"	// include SKP board support package
#include <math.h>
#include "srf.h"
#include "skp_lcd.h"



int srf_echo_dist;
int SRF_state;

/*****************************************************************************
Name:    get_srf_state()       
Parameters: none                     
Returns:   the current state of the SRF     
Description:  getter function for SRF_state
*****************************************************************************/
int get_srf_state(void) {
	return SRF_state;
}

/*****************************************************************************
Name:    is_srf_reading_ready()       
Parameters: none                     
Returns:   1 if reading is ready, otherwise 0      
Description:  getter function for srf_reading_ready
*****************************************************************************/
int is_srf_reading_ready(void) {

	if ( (SRF_state == SRF_READY) || (SRF_state == SRF_MEASURE) ) {
		return 1;
	} else {
		return 0;
	}
	
}
 
/*****************************************************************************
Name:    is_srf_ready()       
Parameters: none                     
Returns:   1 if srf is ready, otherwise 0      
Description:  getter function for srf__ready
*****************************************************************************/
int is_srf_ready(void) {

	if (SRF_state == SRF_READY) {
		return 1;
	} else {
		return 0;
	}
}


/*****************************************************************************
Name:     get_srf_reading_mm()       
Parameters: none                     
Returns:  distance to object in millimetres      
Description:	returns the last reading from the srf in mm. 
**************************************************************************** */
int get_srf_reading_mm(void) {
#ifdef DEBUG_SRF	
	DisplayString(2, "c\0");
#endif
	return srf_echo_dist;
	
}

/*****************************************************************************
Name:     srf_trigger()       
Parameters: none                     
Returns:  nothing      
Description:	Starts the srf ranger for a transmission.  Will block until the 
SRF is ready.  Therefore better for caller to check first.
**************************************************************************** */
void srf_trigger(void) {
#ifdef DEBUG_SRF
	DisplayString(3, "d\0");
#endif
	while  (SRF_state != SRF_READY) {
	};
	
	TRIGGER_OUTPUT = 1;					// set the trigger output line high
#ifdef DEBUG_SRF
	DisplayString(4, "e\0");
#endif
	tb1mr = B1TIME_CONFIG;
	tb1 = TB1_10US;
	tb1s = 1;					// start the 10us delay timer
	SRF_state = SRF_TRIGGER;


}

/*****************************************************************************
Name:     tb1_irq()       
Parameters: none                     
Returns:  nothing      
Description:	Timer B1 Interrupt Service Routine. This interrupt can be called 
from three states moving to other states.
**************************************************************************** */
void tb1_irq(void){
	tb1s = 0; 			// stop the timer

	switch (SRF_state) {

	case SRF_INIT:		// then we have just made the 10ms initial wait			
#ifdef DEBUG_SRF	
						DisplayString(5, "f");
#endif
						SRF_state = SRF_READY;
						break;

	case SRF_TRIGGER:	// then we have completed the 10us tx burst
#ifdef DEBUG_SRF
						DisplayString(6, "g\0");
#endif
						TRIGGER_OUTPUT = 0;		// set it to low, since we're done
						tb4mr = B4TIME_CONFIG;	// reload and start the timer
						tb4s = 1;
						SRF_state = SRF_TX;
						break;
	
	case SRF_MEASURE:	// then we have just made the 10ms retx wait
#ifdef DEBUG_SRF
						DisplayString(7, "h\0");
#endif
						SRF_state = SRF_READY;	// isr means that the 10ms is up, hence ready.
	
	default:			break;
	}
}


/*****************************************************************************
Name:     tb4_irq()       
Parameters: none                     
Returns:  nothing      
Description:	Timer B4 Interrupt Service Routine. The overflow flag is check 
			to determine if the TB4 register contains valid data. If so, the 
			input is tested to determine if the value in the TB0 register
			is the high pulse width or low width which is then stored in the 
			appropriate	variable.  
**************************************************************************** */
void tb4_irq(void){ 

	double distance;				//
	int echo_ticks;

#ifdef DEBUG_SRF
						DisplayString(16, "z\0");
#endif

	if (SRF_state == SRF_TX) {
	
		tb4s = 0;

  		if (mr3_tb4mr == 1){		// check for timer overflow
#ifdef DEBUG_SRF
						DisplayString(17, "y\0");
#endif
  	 		tb4mr = B4TIME_CONFIG;  // if so clear flag and
			srf_echo_dist = -1;	// this is the beyond the limit number
    
		// check if high or low pulse width was measured and then determine distance	
  		} else if (ECHO_INPUT == 0)	{				// port 9_4 (ECHO_INPUT) is the TB4in port
  			// if input is now low, high width measured; don't care about the low width
#ifdef DEBUG_SRF
						DisplayString(18, "x\0");
#endif			
			echo_ticks = tb4;       	

			/* Theory - maybe need to divide by 2 for 2-way trip?
			 distance = pulse_width / tick_rate * speed_of_sound
			 dist (mm) = echo_ticks / (24000000/8) * 340000 = echoticks / (300) * 34 = 34/300 * echoticks
			 dist (mm) = 0.113 * echo_ticks
			*/

			distance = floor(echo_ticks * 0.113);

			// set the new value of the reading
			srf_echo_dist = (int)distance;
		} else {
			return;
		};
		
		// set up the timer for the 10ms wait
			
		tb1s = 0;
		tb1mr = B1TIME_CONFIG;
		tb1 = TB1_10MS;
		tb1s = 1;

		SRF_state = SRF_MEASURE;

	}

}

/*****************************************************************************
Name:  srf_init()   
Parameters: none                   
Returns:  nothing      
Description: Setups timer B4 for pulse width measurement    
**************************************************************************** */
 void srf_init(){

#ifdef DEBUG_SRF
	DisplayString(0, "a\0");
#endif
   
	SRF_state = SRF_INIT;   	

   	tb4mr = B4TIME_CONFIG;

	tb1mr = B1TIME_CONFIG;
	tb1 = TB1_10MS;


    DISABLE_IRQ				//  macro defined in skp_bsp.h
   	tb4ic = TB4_IPL; 
	tb1ic = TB1_IPL;    	
    ENABLE_IRQ				

   	
	// nothing is ready
	srf_echo_dist = -1;				// invalid dist
	
	ECHO_INPUT_PD = 0;				// input

	TRIGGER_OUTPUT_PD = 1;			// output
	TRIGGER_OUTPUT = 0;				// make sure the enable line is low

#ifdef DEBUG_SRF
	DisplayString(1, "b\0");
#endif
	tb1s = 1;				// count to 10ms			
   	//tb4s = 1;               //start counting

}