//----- 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 "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
uint64_t time;             // total number of microseconds since reset
volatile int processing_flag = 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++;
  processing_flag = 1;
}



int pingSonar(void)
{
    int starttime;
    int elapsed;
    float distance;

    // check the current time
    starttime = time;

    rprintf("%d",starttime);
    rprintf("\n");

    // initiate ping
    cbi(PORTB, 5);    // Power ON the sonar module. Yes 0 = on, its a PNP!
    sbi(PORTB, 4);    // Ping!

    for (;(PINB & (1<<3)) != 0;);
   
    elapsed = time - starttime;

    rprintf("%d",elapsed);
    rprintf("\n");

    elapsed = elapsed / 2;

    // Speed of sound at 19 degrees C = 342.785m/s
    distance = elapsed / 343;



    rprintf("%d",distance);
    rprintf("\n");


    return distance;
}


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);

    timerInit();

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

    // 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, 3);		// 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();


    while (1) {
      if (processing_flag = 1) {
	time = (uint64_t)numofinterrupts * 250 + TCNT2;
	processgin_flag = 0;
      }
    }



}