//----- 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 ------------------------------------------------------------

volatile uint32_t numofinterrupts;  // total number of time interrupts since reset
volatile uint64_t time;  // total number of time interrupts since reset
volatile int pong = 0;
uint64_t start_time = 0;
volatile uint64_t end_time = 0;
//u08 FixedPtBits;

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


SIGNAL(SIG_INTERRUPT2)
{
  end_time = time;
  pong = 1;
  cbi(PORTB, 4);    // Turn INIT off
  sbi(PORTB, 5);    // Power OFF the sonar module. Yes 1 = off, its a PN!P
}

/*
// fixedptInit() initializes fixed-point math function library
void fixedptInit(u08 fixedPtBits)
{
	// set the number of bits to use behind the point
	FixedPtBits = fixedPtBits;
}


s32 fixedptMultiply(s32 a, s32 b)
{
	// multiply a and b (a*b) with fixed-point math
	return (a*b)>>FixedPtBits;
}

s32 fixedptDivide(s32 numer, s32 denom)
{
	// divide numer by denom (numer/denom) with fixed-point math
	return (numer<<FixedPtBits)/denom;
}
*/

int pingSonar(void)
{
    rprintf("Ping!\n");
    // check the current time
    start_time = time;
    
    // initiate ping
    cbi(PORTB, 5);    // Power ON the sonar module. Yes 0 = on, its a PNP!
    timerPause(20);
    sbi(PORTB, 4);    // Ping! INIT on

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

    return 0;
}

int pongSonar(void) {
  unsigned long elapsed = 0;
  if (pong) {

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

    elapsed = end_time - start_time;
    elapsed = elapsed / 2;
    //elapsed = fixedptDivide(elapsed, 2);
    //distance = fixedptMultiply(elapsed, 0.00034);

    pong = 0;
    end_time = 0;
    start_time = 0;
  }
  return elapsed;
}

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!

    // fixed point math
    //fixedptInit(5); 

    return 0;
}

int main(void)
{
 
    // Setup
    setup();
    rprintf("Time: %u\n", time);
    pingSonar();

    while(1) {
      if(pong) {
	rprintf("Pong: %d\n", pongSonar());
      }
   } 

}