#include <Arduino.h>

#define HCSR04_TRIG_PIN 22
#define HCSR04_ECHO_PIN 23

class HCSR04
{
    private:

    int m_trig; // trigger pin
    int m_echo; // echo pin
    double m_lastread; // cached reading
    uint64_t m_lastreadtime; // last reading time
    uint16_t m_cachetime; // cache invalidation time

    // Private, gets the distance in mm
    float getReading()
    {
        digitalWrite(m_trig, LOW); // Set trigger to low
        delayMicroseconds(2); // Wait 2us
        noInterrupts(); // Disable interrupts
        digitalWrite(m_trig, HIGH); // Set trigger to high
        delayMicroseconds(10); // Wait 10us
        digitalWrite(m_trig, LOW); // Set trigger to low
        unsigned long t = pulseIn(m_echo, HIGH, 23529.4); // Get time (3rd arg is timeout +/- 4 meters)
        interrupts(); // Enable interrupts
        return (float)t / (float)5.88235; // Number in HCSR04's datasheet
    }

    public:

    /* @brief Creates an HCSR04 class with the given pins
     * @param trig Trigger pin, will be set as out
     * @param echo Trigger pin, will be set as in
     * @param cachetimems Min time in ms to get a new reading */
    HCSR04(int trig, int echo, uint16_t cachetimems = 10)
    {
        m_trig = trig;
        pinMode(m_trig, OUTPUT);
        m_echo = echo;
        pinMode(m_echo, INPUT);
        m_lastread = 0;
        m_lastreadtime = 0;
        m_cachetime = cachetimems;
    }
 
    /* @brief Get's the distance in millimeters
     * @warning By default, value is cached for 10ms */
    float distmm()
    {
        uint64_t time = millis(); // get current time
        // check if cache is old enough
        if ((time - m_lastreadtime) > m_cachetime)
        {
            m_lastread = getReading(); // cache new reading
            m_lastreadtime = time; // save curr time
        }
        return m_lastread; // return cached value
    }

    /* @brief Get's the distance in centimeters
     * @warning By default, value is cached for 10ms */
    float distcm() { return distmm() / 10.0; }

    /* @brief Sets a new cache time
     * @warning Values smaller than 10ms can result in readings returning 0 */
    void setCacheTime(uint16_t newValue) { m_cachetime = newValue; }
};

HCSR04 sensor(HCSR04_TRIG_PIN,  HCSR04_ECHO_PIN);

void setup() 
{
    Serial.begin(115200);

    pinMode(HCSR04_TRIG_PIN, OUTPUT);
    pinMode(HCSR04_ECHO_PIN, INPUT);

    delay(2000);
}

void loop() 
{
    double dist = sensor.distmm();

    Serial.print(dist);
    Serial.println("mm");

    dist = sensor.distcm();

    Serial.print(dist);
    Serial.println("cm");

    delay(2000);
}