#include "srf05.h"

typedef enum
{
  SRF_IDLE = 0,
  SRF_WAITING_FOR_ECHO,
  SRF_ECHO_ACTIVE,
  SRF_RESULT_READY,
  SRF_TIMEOUT
} e_srfstate;

volatile uint32_t SRF_LastResult;
volatile uint8_t  SRF_Overflows = 0;
volatile uint16_t SRF_TCNT = 0;
volatile uint32_t SRF_TotalTimeTicks = 0;

volatile e_srfstate SRF_State = SRF_IDLE;

void SRF05_Init(void)
{
  // Setup result pin
  SRF_RESULT_PORT  &=~ (1 << SRF_RESULT_PIN);
  SRF_RESULT_DDR   &=~ (1 << SRF_RESULT_PIN);

  // setup trigger pin
  SRF_TRIGGER_PORT &=~ (1 << SRF_TRIGGER_PIN);
  SRF_TRIGGER_DDR  |=  (1 << SRF_TRIGGER_PIN);

  // Setup result pin interrupt:
  SRF_RESULT_INT_ISC_REG |= (1 << SRF_RESULT_INT_ISC_BIT);
}

uint32_t SRF05_GetResponseTime(void)
{
  SRF_LastResult = 0xFFFFFFFF;
  // Response signal 100us - 25ms
  // FCPU = 16MHz
  // Prescaler 8 => 2 MHz => 0.5 us tick duration
  
  // Prepare Timer
  TCCR1A = 0; // Standard timer; just count here
  TCCR1B = 0; // Stop timer
  TCNT1 = 0;
  SRF_Overflows = 0;
  SRF_TCNT = 0;
  SRF_TotalTimeTicks = 0;

  // clear a possible pending interrupt request
  SRF_RESULT_INT_FLAG_REG |= (1 << SRF_RESULT_INT_FLAG_BIT);

  // enable interrupt (both edges)
  SRF_RESULT_INT_MSK_REG |= (1 << SRF_RESULT_INT_MSK_BIT);
  
  // enable timer overflow interrupt
  TIMSK |= (1 << TOIE1);
  
  SRF_State = SRF_WAITING_FOR_ECHO;

  // Send trigger pulse (20us)
  SRF_TRIGGER_HIGH();
  _delay_us(20);
  SRF_TRIGGER_LOW();
  
  // wait until measurement is complete
  while((SRF_State == SRF_ECHO_ACTIVE) || (SRF_State == SRF_WAITING_FOR_ECHO))
  {
    // nothing to do; just wait...
  }

  // Calculate result  
  SRF_TotalTimeTicks  = SRF_Overflows;
  SRF_TotalTimeTicks <<= 16;
  SRF_TotalTimeTicks += SRF_TCNT;
  SRF_LastResult = SRF_TotalTimeTicks;
  return SRF_TotalTimeTicks;
}

ISR(SRF_RESULT_ISR)
{
  uint8_t pinlevel = SRF_RESULT_INPUT & (1 << SRF_RESULT_PIN);
  
  if (pinlevel)
  {
    // rising edge: result pulse started
    // start timer for counting
    TCCR1B = (1 << CS11);
    SRF_State = SRF_ECHO_ACTIVE;
  }
  else
  {
    // falling edge: result pulse end
    // stop timer
    SRF_TCNT = TCNT1;
    TCCR1B = 0;
    
    // disable trigger interrupt
    SRF_RESULT_INT_MSK_REG &=~ (1 << SRF_RESULT_INT_MSK_BIT);

    // disable timer overflow interrupt
    TIMSK &=~ (1 << TOIE1);
    SRF_State = SRF_RESULT_READY;

    // clear possible pending interrupt requests
    SRF_RESULT_INT_FLAG_REG |= (1 << SRF_RESULT_INT_FLAG_BIT);
    TIFR |= (1 << TOV1);
  }
}

ISR (SIG_OVERFLOW1) // Interrupt Timer 1
{
  SRF_Overflows++;
}