#pragma once
#ifndef STOPWATCH_H
#define STOPWATCH_H

class StopWatch
{
protected:

    const uint64_t UNAVAILABLE = 0xFFFFFFFFFFFFFFFF;
    const uint64_t UINT32_NUMERIC_LIMIT_MAX = 0x00000000FFFFFFFF;

public:

    virtual ~StopWatch() {}

    inline void start()
    {
        running = true;
        prev_us = micros();
        origin = now = prev_us;
        overflow = 0;
    }
    inline void stop()
    {
        running = false;
        prev_us = 0;
        origin = now = UNAVAILABLE;
        overflow = 0;
    }

    inline bool isRunning() const { return running; }
    inline bool isPausing() const { return (!running && (origin != UNAVAILABLE)); }

    inline uint64_t us()
    {
        Serial.println("inside us()");
        Serial.println((uint32_t)origin, HEX);
        Serial.println(micros(), HEX);

        if      ( isPausing()) ;
        else if (!isRunning()) return 0;
        else
        {
            uint32_t curr_us = micros();
            // Serial.print("uint32_t micros() = ");
            // Serial.print(prev_us);
            // Serial.print(", ");
            // Serial.println(curr_us);
            if (curr_us < prev_us)
            {
                overflow += UINT32_NUMERIC_LIMIT_MAX + (uint64_t)1;
                Serial.println("micros() overflow!!!!!!!!!");
                Serial.print("uint32_t micros() = ");
                Serial.print(prev_us);
                Serial.print(", ");
                Serial.println(curr_us);
                // delay(100000);
            }
            prev_us = curr_us;
            now = (uint64_t)curr_us + overflow;
        }
        // Serial.print("int64_t us_curr, start_ = ");
        // Serial.print((uint32_t)now);
        // Serial.print(" - ");
        // Serial.println((uint32_t)origin);
        return now - origin;
    }
    inline double ms() { return (double)us() * 0.001; }
    inline double sec() { return (double)us() * 0.000001; }

private:

    bool running {false};
    uint32_t prev_us {0};
    uint64_t origin {UNAVAILABLE};
    uint64_t now {UNAVAILABLE};
    uint64_t overflow {0};
};

class IntervalCounter : public StopWatch
{
public:

    IntervalCounter (double sec)
    : available(false)
    , interval((uint32_t)(sec * 1000000.))
    , next(interval)
    , cnt(0)
    {}

    virtual ~IntervalCounter() {}

    inline void start() { this->StopWatch::start(); next = interval; cnt = 0; }
    inline void stop() { this->StopWatch::stop(); next = UNAVAILABLE; cnt = 0; }
    inline void restart() { stop(); start(); }

    inline bool update()
    {
        uint64_t uss = us();

        Serial.print("curr, next = ");
        Serial.print((uint32_t)(uss & 0xFFFFFFFF00000000 >> 32));
        Serial.print(" ");
        Serial.print((uint32_t)(uss & 0x00000000FFFFFFFF));
        Serial.print(", ");
        Serial.print((uint32_t)(next & 0xFFFFFFFF00000000 >> 32));
        Serial.print(" ");
        Serial.print((uint32_t)(next & 0x00000000FFFFFFFF));
        Serial.print(" : ");
        Serial.print((uint32_t)(interval & 0xFFFFFFFF00000000 >> 32));
        Serial.print(" ");
        Serial.print((uint32_t)(interval& 0x00000000FFFFFFFF));
        Serial.print(", ");
        Serial.print((uint32_t)(cnt & 0xFFFFFFFF00000000 >> 32));
        Serial.print(" ");
        Serial.println((uint32_t)(cnt & 0x00000000FFFFFFFF));

        if (uss > next)
        {
            Serial.println("Next");
            available = true;
            next = interval * (cnt++ + 1);
        }
        else
        {
            available = false;
        }
        return available;
    }

    inline bool isNext() { return available; }

    inline double count() { return cnt; }

    inline void setInterval(double i) { interval = (uint64_t)(i * 1000000.); }

private:

    bool available {false};
    uint64_t interval {0};
    uint64_t next {0};
    uint64_t cnt {0};
};

class FrameRateCounter : public IntervalCounter
{
public:

    FrameRateCounter(double fps) // second
    : IntervalCounter(1.0 / fps)
    { }
    virtual ~FrameRateCounter() {}

    inline double frame() { return count(); }

    inline void setFrameRate(double fps) { setInterval(1. / fps); }
};



#endif // STOPWATCH_H
