#include "ultrasonic.hpp"

struct usData_t usData[2];

void us_0_isr() {
  if(digitalRead(US_RX_0_PIN) && usData[0].state == rxPending) {
    usData[0].pulseStart = TCNT1;
    usData[0].state = counting;
  } else if(usData[0].state == counting) {
    usData[0].duration  = TCNT1 - usData[0].pulseStart;
    usData[0].state = finished;
  }
}

void us_1_isr() {
  if(digitalRead(US_RX_1_PIN) && usData[1].state == rxPending) {
    usData[1].pulseStart = TCNT1;
    usData[1].state = counting;
  } else if(usData[1].state == counting) {
    usData[1].duration  = TCNT1 - usData[1].pulseStart;
    usData[1].state = finished;
  }
}

void us_init() {
  attachInterrupt(digitalPinToInterrupt(US_RX_0_PIN), us_0_isr, CHANGE);
  attachInterrupt(digitalPinToInterrupt(US_RX_1_PIN), us_1_isr, CHANGE);

  //timer 1 is used as an accurate timer for measuring the TOF pulses
  
  //1:1 prescaler for timer 1
  TCCR1A = 0;
  TCCR1B = _BV(CS10);
  //timer 1 now runs with 16MHz, therefore overflows every (2^16 / 16MHz) = 4096 us

}

void us_transmit() {
  //disable pin interrupts
  EIMSK = 0;

  //set pin 2, 3 and 4 to output
  DDRD  |= 0b00011100;
  PORTD &= 0b11100011;

  delayMicroseconds(2);

  //pull pin 2, 3 and 4 to 5V
  //doing this seems to lead to power supply issues and the serial interface stopps working
  //further testing needed!
  PORTD = (PORTD & 0b11100011) | 0b00000100;
  delayMicroseconds(5);
  PORTD = (PORTD & 0b11100011) | 0b00001000;
  delayMicroseconds(5);

  //pull pin 2 and 3 to GND
  PORTD &= 0b11110011;
  //set pin 2 and 3 back to input
  DDRD  &= 0b11110011;

  delayMicroseconds(5);

  //enable pin interrupts
  EIMSK = _BV(INT1) | _BV(INT0);

  usData[0].state = rxPending;
  usData[1].state = rxPending;

  PORTD = (PORTD & 0b11100011) | 0b00010000;
  delayMicroseconds(5);

  //pull pin 4 to GND
  PORTD &= 0b11101111;

  //set pin 4 back to input
  DDRD  &= 0b11101111;

} 

long us_get_duration(byte sensor) {
  // check if there is new sensor data
  if(usData[sensor].state == finished) {
    usData[sensor].state = idle;
    return usData[sensor].duration;
  } else {
    return -(F_CPU / 1000000);
  }
}