#include "ultrasonic.hpp"

struct usData_t usData[2];

void us_0_isr() {
  if(digitalRead(US_RX_0_PIN) && usData[1].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;

  pinMode(US_RX_0_PIN, OUTPUT);
  pinMode(US_RX_1_PIN, OUTPUT);
  pinMode(US_TX_0_PIN, OUTPUT);
  digitalWrite(US_RX_0_PIN, LOW);
  digitalWrite(US_RX_1_PIN, LOW);
  digitalWrite(US_TX_0_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(US_RX_0_PIN, HIGH);
  digitalWrite(US_RX_1_PIN, HIGH);
  digitalWrite(US_TX_0_PIN, HIGH);
  delayMicroseconds(5);
  digitalWrite(US_RX_0_PIN, LOW);
  digitalWrite(US_RX_1_PIN, LOW);
  digitalWrite(US_TX_0_PIN, LOW);
  pinMode(US_RX_0_PIN, INPUT);
  pinMode(US_RX_1_PIN, INPUT);
  pinMode(US_TX_0_PIN, INPUT);

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

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

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