/** HCSR04uST2.cpp -- Arduino library to use HCSR04 ultraSonic
 **	distance sensor using timer2 and D2 or D3 interrupts.
 ** munged up by: schip Oct 2018
 **
 **  As implemented this uses:
 **		timer 2 at a prescale of /1024 for a 64uS resolution
 **		any digital pin as the trigger output
 **		either D2 or D3 for the ping input
 **		note: tested with D2 as trigger and D3 as ping....
 **	  Resulting in a count range of about
 **		12 at minimum and 2300 at maximum, with no ping return
 **		 seems to be about 2cm to 200cm (12-180cnt) good returns
 **		 multiply counts by 1.1 to get approx cm
 **		if something goes wrong a negative value is returned
 */

#include <Arduino.h>
#include "HCSR04.h"

//#define DEBUG	// set to do a bit of debug tracing -- in .h file

/**
 ** The HCSR04 Ultrasonic distance sensor needs a start high
 ** going pulse on the TRIGPIN for a min of 10uS. The falling
 ** edge of this triggers a 40kHz burst of 8 cycles (200uS) and
 ** then the ECHOPIN goes positive for pulse width relative to the 
 ** target distance. It may 'over-timeout' if it doesn't get a signal.
 ** 
 ** The output pulse "Ew" is ~110->23200 uSec with a target,
 ** and the no target signal is about 38000 uSec.
 ** At 110uS the distance is ~2cm and at 23200 about 400cm (Ew/58).
 **	 Note: the max of 38mS does not seem to be accurate,
 **			more like 1.5sec...
 ** 
 ** See the HCSR04_USdistance.pdf doc for these details...
 ** 
 ** The scheme then is to:
 **  Setup the PINGPIN as a falling edge Hardware interrupt;
 **  Setup Timer2 for normal count-up behavior,
 **    with compare and overflow interrupts.
 **   enable interrupt on the ping input to trigger on low edge;
 **   enable counter compare interrupt to a 2 counts;
 **   enable counter overflow TOV2 interrupt;
 **   clear counter and TOV2 overflow bit;
 **   set the TRIGPIN high and start the counter;
 **   on the counter compare interrupt,
 **   	set TRIGPIN low and disable compare interrupt;
 **   if a ping input interrupt occurs,
 **   	grab the counter and the TOV2 overflow bit values to calc time,
 ** 	disable all relevant interrupts and stop counter;
 **   if a TOV int occurs before a ping input int,
 **   	if the TOV2 overflow bit is NOT set
 ** 		go ahead and set the TOV2 overflow bit (1x wrap on counter),
 ** 	else if the TOV2 overflow bit is already set (>1x wrap),
 ** 		deal with a failed target value,
 ** 	disable all relevant interrupts and stop counter;
 **/ 	


/**************** Hardware Interrupt on PINGPIN ********************/
// NOTE: see impl of attachInterrupt() for a full
//  set of possible chip registers and nonsense
//  for setting and unsettling External Pin interrupts
//  This is just for the good'ole'Arduino Atmega 328p

// these need to be accessed by interrupt routines so... local variables
int16_t distance; // the last good, or failed, sensor distance count
uint8_t t2state; // internal device state
uint8_t intnum;	// translation of ping pin to interrupt vec number
uint8_t trigbit;	// mask with trigger bit to twiddle
volatile uint8_t *trigout;	// trigger output register address
#ifdef DEBUG
uint8_t _uSflags;
#endif

extern void pingISR();	// fwd ref for ping interrupt routine

/** Enable PING interrupt on either D2 or D3
 **   note: only tested on D3, might also work with others, so YMMV
 **			and only works for ATmega 328p on the usual Arduino
 */
void pingIntEnable( uint8_t intNum )
{
	// clear appropriate int flag and then enable int
	if( intNum == 0 )
	{
		EIFR  |= (1 << INTF0 );
		EIMSK |= (1 << INT0);
	}
	else
	{
		EIFR  |= (1 << INTF1 );
		EIMSK |= (1 << INT1);
	}
}	  

/** Disable PING interrupt on either D2 or D3
 **   note: only tested on D3, might also work with others, so YMMV
 **			and only works for ATmega 328p on the usual Arduino
 */
void pingIntDisable( uint8_t intNum )
{
	if( intNum == 0 )
		EIMSK &= ~(1<<INT0);
	else
        EIMSK &= ~(1<<INT1);

}	  

// initialize the trigger out and ping in pins and interrupts
//  leaves the interrupt disabled
void HCSR04::pingInit( uint8_t trigpin,  uint8_t pingpin)
{
	pinMode( pingpin, INPUT_PULLUP );
	pinMode( trigpin, OUTPUT );
	// save TRIGPIN register addr and bitmask, for convenience
	//  note: this is stolen from digitalWrite() in wiring_digital.c
	uint8_t port = digitalPinToPort( trigpin );
	trigout = portOutputRegister( port );
	trigbit = digitalPinToBitMask( trigpin );
	
	// this will attach pingISR() to INT.0 or INT.1 depending...
	// save pingpin interrupt number
	intnum = digitalPinToInterrupt( pingpin );
	attachInterrupt( intnum, pingISR, FALLING ); 
	pingIntDisable( intnum );

	return;
}


/**************** Hardware Interrupts for Timer2 ********************/
// internal states
enum
{
	T2S_NONE	= 0x00,	// init
	T2S_OCNT	= 0x0F, // the low 4 bits are used as an overflow counter
	T2S_OVERF	= 0x10,	// overflow happened
	T2S_RUN		= 0x40,	// timer running -- not really used...
	T2S_AVAIL	= 0x80,	// count data available
} T2STATES;


/** init Timer2 to "normal" mode and set compare match value
 **  leaves timer NOT running and interrupts disabled
 **/
void HCSR04::initTimer2( uint8_t compare )
{
	t2state = T2S_NONE;
	TCCR2A = 0;		// clear control registers
	TCCR2B = 0;		//   no counting until set correctly later

	OCR2A = compare;  // compare match register, to stop ping trigger
}

/** enable timer2 run upcounting from value of 0
 **   with 1024 prescaler
 **   compare and overflow interrupts are enabled
 **/
void startTimer2()
{
	t2state = T2S_RUN;
	TCNT2  = 0;		// start count at 0
	TIFR2 |= ((1 << OCF2B) | (1 << OCF2A) | (1 << TOV2)); // clear int bits
	// set 1024 prescaler -- 15,625 Hz clock 64uS, max timing 16.38 mS
	TCCR2B |= ((1 << CS22) | (1 << CS21) | (1 << CS20));
	TIMSK2 |= (1 << OCIE2A);  // enable timer compare interrupt
	TIMSK2 |= (1 << TOIE2);   // enable timer overflow interrupt
}

/** stop Timer2 counting and
 **  disable its compare and overflow interrupts
 **/
void stopTimer2()
{
#ifdef DEBUG
	_uSflags |= 0x80;
#endif

	t2state &= ~T2S_RUN;

	//  note: to stop counter, clear these, i.e., set all CS bits to 0
	TCCR2B &= ~((1 << CS22) | (1 << CS21) | (1 << CS20));
	TIMSK2 &= ~(1 << OCIE2A);  // disable timer compare interrupt
	TIMSK2 &= ~(1 << TOIE2);   // disable timer overflow interrupt
}

/** return the current Timer2 count and factor in
 **  (a minimum of one) overflow flag, for a 0-511 range
 **  if it's more than one overflow return NODATA (0)
 **/
uint16_t getTimer2Cnt()
{
	uint16_t cnt = TCNT2;		// get timer count

	// use t2state to get overflow value
	//  we will assume that 16 overflows are way more than needed
	//   and that more than 1 overflow is too much
	if( t2state & T2S_OVERF	)	// overflow happened
	{
		if( (t2state & T2S_OCNT) > 1 )
			cnt = HCSR04NODATA;
		else
			cnt += 256;
	}

	return cnt;
}


/** Disable the Timer2 compare interrupt
 **/
#define t2CompIntDisable() (TIMSK2 &= ~(1 << OCIE2A));

// timer2 compare interrupt service routine
//  used to shut off ping pin, which starts ping count cycle
ISR(TIMER2_COMPA_vect)
{
#ifdef DEBUG
	_uSflags |= 0x10;
	//digitalWrite(LEDPIN, digitalRead(LEDPIN) ^ 1);   // toggle LED pin
#endif

	// just shut off the TRIGPIN and disable ourself
	//digitalWrite( TRIGPIN, 0 );
	*trigout &= ~trigbit;
	t2CompIntDisable();
}


// timer2 overflow interrupt service routine
//  used to accumulate overflows to extend count range
//   and stop cycle if we don't get a ping response in time
ISR(TIMER2_OVF_vect)
{
	// count the number of overflows
#ifdef DEBUG
	_uSflags |= 0x40;
	++_uSflags;
	//digitalWrite(LEDPIN, digitalRead(LEDPIN) ^ 1);
#endif
	++t2state;

	// if 16 previous overflows are set, this is a not-found situation
	// otherwise set as first pass on counter
	if( (t2state & T2S_OCNT) == T2S_OCNT )	// overflow has overflowed
	{
#ifdef DEBUG
		_uSflags |= 0x80;
#endif
		// not found, shut off
		distance = HCSR04FAILED;
		pingIntDisable( intnum );
		stopTimer2();
		t2state = T2S_AVAIL;	 // (bad) count data available
	}
	else
	{
		t2state |= T2S_OVERF;	// any overflow has happened
	}
}

/** The PING pin interrupt routine.
 ** Executed when PINGPIN transitions low at end of timing period.
 ** We should have a reasonable count from the timer.
 **/
void pingISR()
{
#ifdef DEBUG
	_uSflags |= 0x20;
	//digitalWrite( LEDPIN,  0 );   // toggle LED pin
#endif

	// get the distance value, reset, and mark as done
	// note that 295-300 is about the max reading (around 3 meters)
	//  even though 38000/64 == 593 is the spec...
	//   and what seems to a bit more than 9 overflows (~160mS)
	//   is what the actual sensor does before it resets
	distance = getTimer2Cnt();
	pingIntDisable( intnum );
	stopTimer2();
	t2state = T2S_AVAIL;	 // (good) count data available
}
/***************************** PUBLIC INTERFACE ***************************/

/** default constructor
 ** just initialize stuff, calling init() is still needed
 **/
HCSR04::HCSR04()
{
	t2state = 0;
}

/** initialize the HCSR04 sensor pins and timer interrupts
 **  leaves all the interrupt disabled
 **   use SR04.startPing() to start a sample cycle
 **/
void HCSR04::init( uint8_t trigpin, uint8_t pingpin )
{
	noInterrupts();    // disable all interrupts, just in case
	pingInit( trigpin, pingpin );	// init HCSR04 pins and interrupt
	initTimer2( 0x02 ); // initialize timer2 with compare fire on 2nd count
						//  note: this will turn off the TRIGPIN after 128uS
						//  which then starts the ping sample cycle
	interrupts();		// enable global interrupts

	return;
}

/** Start a sensor ping cycle
 **  turns on TRIGPIN and enables interrupts
 ** Presumes that HCSR04.init() including initTimer2() have already been called.
 **/
void HCSR04::startPing()
{
#ifdef DEBUG
	_uSflags = 0;
	//digitalWrite( LEDPIN,  1 );   // toggle LED pin
#endif

	distance = HCSR04FAILED; // just to be sure we got something later

	// turn on trigger pin and enable ping pin interrupt
	//digitalWrite( TRIGPIN, 1 );
	*trigout |= trigbit;
	pingIntEnable( intnum );

	// start the timer running, with interrupts enabled
	startTimer2();

	// now we wait....

	return;
}

/** return true if there is a new distance value available
 **  will clear itself, so a second call will return false...
 **/
bool HCSR04::available()
{
	if( t2state & T2S_AVAIL )	 // count data available
	{
		// clear this state, so no subsequent calls are true
		t2state &= ~T2S_AVAIL;
		return true;
	}

	return false;
}


/** return the last distance value from the sensor
 **  if it's 0x0000 we didn't get anything...
 **  if it's 0x8000 we got some device failure
 **/
int16_t HCSR04::getDistance()
{
	return distance;
}

#if 0
// NOTE: different ping interrupts but would probably work on any pin
// to setup the specified Arduino pin as an interrupt source
void pciSetup(byte pin)
{
    *digitalPinToPCMSK(pin) |= bit (digitalPinToPCMSKbit(pin));  // enable pin
    PCIFR  |= bit (digitalPinToPCICRbit(pin)); // clear any outstanding interrupt
    PCICR  |= bit (digitalPinToPCICRbit(pin)); // enable interrupt for the group
}
 
// Use one Routine to handle each group
 
ISR (PCINT0_vect) // handle pin change interrupt for D8 to D13 here
 {    
     digitalWrite(13,digitalRead(8) and digitalRead(9));
 } 

/** there's these too...
ISR (PCINT1_vect) // handle pin change interrupt for A0 to A5 here
 {
     digitalWrite(13,digitalRead(A0));
 }  
 
ISR (PCINT2_vect) // handle pin change interrupt for D0 to D7 here
 {
     digitalWrite(13,digitalRead(7));
 }  
**/
#endif // 0