#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x27, 16, 2);
#include "US.h"

USMeter usMeter;
// su pin 3 ci sono 490Hz
// su pin 5 ci sono 976HZ
#define TRIGGER_PIN 3
#define ECHO_PIN 2

#define DEBUG_PIN   7
volatile uint8_t *portDebug;
uint8_t portDebugBitMask;

void setup() {
    pinMode(DEBUG_PIN, OUTPUT);
    portDebug = portOutputRegister(digitalPinToPort(DEBUG_PIN));
    portDebugBitMask = digitalPinToBitMask(DEBUG_PIN);
    usMeter.begin(ECHO_PIN, TRIGGER_PIN);
    usMeter.setTriggerInterval(1);
  
    Serial.begin(115200);
    attachInterrupt(digitalPinToInterrupt(ECHO_PIN), rxEcho, CHANGE);
    analogWrite(5, 3);
    lcd.init();                      // initialize the lcd 
    // Print a message to the LCD.
    lcd.backlight();
    lcd.setCursor(0,0);
    lcd.print("Cm = ");
}

void rxEcho() {
    usMeter.rstTrigger();
    usMeter.rxEcho();
}


//Velocità del suono m/s = 331,4 + (0,606 * Temp) + (0,0124 * Umidità)
// https://www.michelediluca.it/sensore-ultrasuoni-hc-sr04-arduino/

void loop(void) {
    *portDebug |= portDebugBitMask;
    
    usMeter.trigger();

    uint32_t echoTime = usMeter.getEchoTime();
    if (echoTime) {
        // echoTime = 60ms outrange
        //Serial.print("echoTime = ");
        //Serial.print(echoTime);
        //Serial.print('\t');
        
        //Serial.println(usMeter.usToCm(echoTime));
        char printBuff[10] = {0};
        sprintf(printBuff, "%#3u", usMeter.usToCm(echoTime));
        
        lcd.setCursor(5,0);
        lcd.print(printBuff);
        dtostrf(usMeter.toUS_100cm(echoTime), 6, 2, printBuff);
        //Serial.println(usMeter.toUS_100cm(echoTime));
        lcd.setCursor(5,1);
        lcd.print(printBuff);
    } 
    *portDebug &= ~portDebugBitMask;
    
}