//----- Include Files ---------------------------------------------------------
#include <avr/io.h>			// include I/O definitions (port names, pin names, etc)
#include <avr/iom32.h>			// include I/O definitions (port names, pin names, etc)
#include <avr/interrupt.h>	// include interrupt support
#include <stdint.h> 

#include "global.h"		// include our global settings
#include "uart.h"		// include uart function library
#include "rprintf.h"	// include printf function library
#include "timer.h"		// include timer function library (timing, PWM, etc)

void timerTest(void);

//----- Begin Code ------------------------------------------------------------

uint32_t numofinterrupts;  // total number of time interrupts since reset
volatile uint64_t time;             // total number of microseconds since reset
volatile uint8_t pong = 0;
volatile uint64_t start_time = 0;
volatile uint64_t end_time = 0;

// Real time interupt function
// time will equal the number of ticks that have happened
// each tick is 4e-9 seconds
SIGNAL(SIG_OUTPUT_COMPARE2)
{
  numofinterrupts++;
  time = (uint64_t)numofinterrupts * 1000 + TCNT2;
}

SIGNAL(SIG_INTERRUPT2)
{
  pong = 1;
  end_time = time;
}

int pingSonar(void)
{
    // check the current time
    start_time = time;

    sbi(PORTB, 5);    // Power OFF the sonar module. Yes 1 = off, its a PN!P
    timerPause(10);
    // initiate ping
    cbi(PORTB, 5);    // Power ON the sonar module. Yes 0 = on, its a PNP!
    sbi(PORTB, 4);    // Ping! INIT on
    timerPause(10);
    cbi(PORTB, 4);    // Turn INIT off

    // Speed of sound at 19 degrees C = 342.785m/s

    return 0;
}

int pongSonar() {
  if (pong) {
    uint64_t elapsed = 0;

    // Calc the distance from times
    // 340 (m / s) = 0.00034 m / microseconds

    elapsed = end_time - start_time;
    elapsed = elapsed / 2;
    //elapsed = elapsed * 
    //THis is the part where I am stuck


    rprintf("PONG\n");
    pong = 0;
    end_time = 0;
    start_time = 0;
  }

  return 0;
}

int setup(void)
{
    // initialize our libraries
    // initialize the UART (serial port)
    uartInit();
    // set the baud rate of the UART for our debug/reporting output
    uartSetBaudRate(2400);
    // initialize rprintf system
    rprintfInit(uartSendByte);

    // print a little intro message so we know things are working
    rprintf("\r\n\n\nWelcome to the sonar test program!\r\n");

    // set the OC1A and OC1B for PWM output
    sbi(DDRD, PD4);
    sbi(DDRD, PD5);


    // initialize the timer for RTC
    sbi(TIMSK, OCIE2);
    TCCR2 = _BV(CS22)        // 64 prescale
           | _BV(WGM21);      // CTC mode, TOP = OCR2
    OCR2 = 250;

    // setup interrupts
    sbi(MCUCSR, ISC2);
    sbi(GICR, INT2);


    timerInit();

    // setup the sonar pins 
    sbi(DDRB, 4);		// sets PB4 to be an output - INIT
    sbi(DDRB, 5);		// sets PB5 to be an output - PWR
    cbi(DDRB, 2);		// sets PB3 to be an input - ECHO
    cbi(PORTB, 4);              // Dont ping yet
    cbi(PORTB, 5);		// Power ON the sonar module. Yes 0 = on, its a PNP!

    return 0;
}

int main(void)
{
 
    // Setup
    setup();
    pingSonar();

    while (1) {
      timerPause(500);
      pingSonar();
      pongSonar();
    }



}