#pragma once

#include <cstdint>
#include <complex>
#include <chrono>
#include <thread>

typedef uint32_t phase_t;
#define PHASE_MAX UINT32_MAX

static phase_t frequency_to_ftw(double f) {
    return f*(double)PHASE_MAX;
}

class DDS {
    private:
        phase_t phase = 0;
    public:
        phase_t ftw = 0;
        double get() {
            phase += ftw;  // wraps on overflow
            return sin(phase*2.0*M_PI/(double)PHASE_MAX);
        }
        phase_t get_phase() {
            return phase;
        }
};

template<typename T, unsigned int order>
class Lowpass {
    private:
        double k = 0.0;
        T s[order] = {};
    public:
        void set_bandwidth(double f) {
            k = 2.0*M_PI*f;
        }
        T update(T x) {
            T a = x;
            for(int i=0;i<order;i++) {
                s[i] += (a - s[i])*k;
                a = s[i];
            }
            return a;
        }
};

template<unsigned int order>
class Lockin {
    private:
        double scale = 1.0;
        phase_t multiplier = 1;
        phase_t phase = 0;
        Lowpass<std::complex<double>, order> lpf;
    public:
        phase_t ftw = 0;
        void set_scale(double s) {
            scale = s;
        }
        void set_multiplier(phase_t m) {
            multiplier = m;
        }
        void set_bandwidth(double f) {
            lpf.set_bandwidth(f);
        }
        std::complex<double> update(double x) {
            std::complex<double> rotated;
            rotated = x*std::polar(scale, multiplier*phase*2.0*M_PI/(double)PHASE_MAX);
            phase -= ftw;  // wraps on underflow
            return lpf.update(rotated);
        }
        phase_t get_phase() {
            return phase;
        }
};

class Clocker {
    private:
        std::chrono::milliseconds period;
        std::chrono::time_point<std::chrono::steady_clock> next_tick = std::chrono::time_point<std::chrono::steady_clock>::min();
    public:
        Clocker(std::chrono::milliseconds period): period(period) {};
        void tick() {
            if(next_tick == std::chrono::time_point<std::chrono::steady_clock>::min()) {
                next_tick = std::chrono::steady_clock::now() + period;
            } else {
                auto duration = next_tick - std::chrono::steady_clock::now();
                if(duration >= duration.zero())
                    std::this_thread::sleep_for(duration);
                else
                    std::cerr << "missed tick" << std::endl;
                next_tick += period;
            }
        }
};