#define I2C_SENSOR_ADDRESS  0x27
#define REGISTER_MAP_SIZE 2
#define PING_FREQUENCY 150 // milliseconds between pings

#include <avr/interrupt.h>

#if defined( __AVR_ATtiny85__ )
  #include <TinyWireS.h>
  #include <avr/power.h>
  #define PING_PIN 4
#else
  #include <Wire.h>
  #define PING_PIN 12
#endif


// this won't work on ATTINY
#ifndef _DEBUG
#define _DEBUG false
#endif

byte register_map[REGISTER_MAP_SIZE];

volatile int32_t duration; // duration of the ping
int32_t startPulse = 0;
int32_t last_ping_time = 0;
int32_t ping_freq = PING_FREQUENCY;

// Interrupt vector for external interrupt on pin PCINT7..0
// This will be called when any of the pins D0 - D4 on the trinket change
// or pins D8 - D13 on an Arduino Uno change.
ISR(PCINT0_vect) {
  // we are interested in when the PING_PIN goes high
  if (digitalRead(PING_PIN) == HIGH) {
    duration = millis() - startPulse;

    // should never have a print statement or delay
    // in an ISR, but for debugging this may be okay
#if _DEBUG
    Serial.println(duration);
#endif
  }
}

void setup() {

#if defined( __AVR_ATtiny85__ )
  // Set prescaler so CPU runs at 16MHz
  if (F_CPU == 16000000) clock_prescale_set(clock_div_1);

  // Use TinyWire for ATTINY
  TinyWireS.begin(I2C_SENSOR_ADDRESS);
  TinyWireS.onRequest(requestData);
#else
  Wire.begin(I2C_SENSOR_ADDRESS);
  Wire.onRequest(requestData);
#endif

#if _DEBUG
  Serial.begin(9600);
  Serial.println("Ping Sensor I2C");
#endif
}


void loop() {

  get_distance();

#if defined( __AVR_ATtiny85__ )
  // USE this for ATTINY as you can't use delay
  TinyWireS_stop_check();
#else
  delay(20);
#endif
}

void disablePCInterrupt() {
  // disable all interrupts temporarily
  cli();

  // disable pin change interrupt
  PCMSK0 &= ~_BV(PCINT4);

  // clear pin change interrupt flag register
#if defined( __AVR_ATtiny85__ )
  GIFR &= ~_BV(PCIF);
#else
  PCIFR &= ~_BV(PCIF2);
#endif
  // re-enable all interrupts
  sei();
}

void enablePCInterrupt() {
  // disable all interrupts temporarily
  cli();

  // enable pin change interrupt on PB4 (D4 on Trinket, D12 on Uno)
  PCMSK0 |= _BV(PCINT4);

  // enable pin change interrupt 0
#if defined( __AVR_ATtiny85__ )
  GIMSK |= _BV(PCIE);
#else
  PCICR |= _BV(PCIE0);
#endif
  // re-enable all interrupts
  sei();
}

void get_distance() {

  if ((millis() - last_ping_time) > ping_freq) {

    // disable interrupt while pinMode is OUTPUT
    // not sure if this is actually necessary, but just
    // playing it safe to avoid false interrupt when
    // pin mode is OUTPUT
    disablePCInterrupt();

    pinMode(PING_PIN, OUTPUT);
    digitalWrite(PING_PIN, LOW);
    delayMicroseconds(2);
    digitalWrite(PING_PIN, HIGH);
    delayMicroseconds(5);
    digitalWrite(PING_PIN, LOW);

    pinMode(PING_PIN, INPUT);

    // we'll use a pin change interrupt to notify when the
    // ping pin goes high rather than being blocked by
    // Arduino's built-in pulseIn function
    enablePCInterrupt();

    startPulse = millis();

    last_ping_time = millis();
  }
}

void requestData() {

  register_map[0] = duration >> 8; // msb
  register_map[1] = duration & 0xFF; //LSB

#if defined( __AVR_ATtiny85__ )
  // ATTINY you need to do a send of each register individually.
  TinyWireS.send(register_map[0]);
  TinyWireS.send(register_map[1]);
#else
  // ATMEGA cat write out a buffer
  Wire.write(register_map, REGISTER_MAP_SIZE);
#endif

#if _DEBUG
  Serial.print("rm: ");
  Serial.print(register_map[0]);
  Serial.print(" ");
  Serial.print(register_map[1]);
  Serial.println();
#endif
}