#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;
        }
};

// Demodulates at the fundamental frequency and at its first harmonics, n_freq altogether.
template<unsigned int n_freq, unsigned int lpf_order>
class Lockin {
    private:
        double scale = 1.0;
        phase_t phase = 0;
        Lowpass<std::complex<double>, lpf_order> lpf[n_freq];
    public:
        phase_t ftw = 0;
        void set_scale(double s) {
            scale = s;
        }
        void set_bandwidth(double f) {
            for(int i=0;i<n_freq;i++)
                lpf[i].set_bandwidth(f);
        }
        void update(double x, std::complex<double> *output) {
            for(int i=0;i<n_freq;i++) {
                std::complex<double> rotated;
                rotated = x*std::polar(scale, (i+1)*phase*2.0*M_PI/(double)PHASE_MAX);
                output[i] = lpf[i].update(rotated);
            }
            phase -= ftw;  // wraps on underflow
        }
        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();
            } else {
                next_tick += period;
                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;
            }
        }
};