#pragma once

#include <cstdint>
#include <complex>

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

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;
        phase_t phase = 0;
        Lowpass<std::complex<double>, order> lpf;
    public:
        phase_t ftw = 0;
        void set_scale(double s) {
            scale = s;
        }
        void set_bandwidth(double f) {
            lpf.set_bandwidth(f);
        }
        std::complex<double> update(double x) {
            std::complex<double> rotated;
            rotated = x*std::polar(scale, phase*2.0*M_PI/(double)PHASE_MAX);
            phase -= ftw;  // wraps on underflow
            return lpf.update(rotated);
        }
};