#ifndef FSK_MODULATOR_H
#define FSK_MODULATOR_H

#include <cmath>
#include <cstdint>
#include <functional>
#include <memory>
#include <vector>

#include "BitStreamReader.h"

class FSKModulatorConfig {
    public:
    int freq_lo;
    int freq_hi;
    int sample_rate;
    int baud_rate;
    std::shared_ptr<BitStreamReader> bitstreamreader;
};

namespace milstd {
    class FSKModulator {
        public:
        FSKModulator(const FSKModulatorConfig& s) : freq_lo(s.freq_lo), freq_hi(s.freq_hi), sample_rate(s.sample_rate), baud_rate(s.baud_rate), bit_reader(s.bitstreamreader) {
            omega[0] = (2.0 * M_PI * freq_lo) / sample_rate;
            omega[1] = (2.0 * M_PI * freq_hi) / sample_rate;
            baud_incr = static_cast<double>(baud_rate) / sample_rate;
            phase = 0.0;
            baud_frac = 0.0;
            current_bit = 0;
        }

        std::vector<int16_t> modulate(unsigned int num_samples) {
            std::vector<int16_t> samples;
            samples.reserve(num_samples);

            int bit = current_bit;

            for (unsigned int i = 0; i < num_samples; i++) {
                baud_frac += baud_incr;
                if (baud_frac >= 1.0) {
                    baud_frac -= 1.0;
                    try
                    {
                        bit = bit_reader->getNextBit();
                    }
                    catch(const std::out_of_range&)
                    {
                        bit = 0;
                    }
                    if (bit != 0 && bit != 1)
                        bit = 0;
                }

                double sample = std::cos(phase);
                int16_t sample_int = static_cast<int16_t>(sample * 32767);
                samples.push_back(sample_int);

                phase += omega[bit];
                if (phase >= 2.0 * M_PI) {
                    phase -= 2.0 * M_PI;
                }
            }

            current_bit = bit;

            return samples;
        }

        private:
        // parameters
        int freq_lo, freq_hi;
        int sample_rate;
        int baud_rate;
        std::shared_ptr<BitStreamReader> bit_reader;

        // state variables
        double phase;
        double baud_frac;
        double baud_incr;
        std::array<double, 2> omega;
        int current_bit;
    };

} // namespace milstd

#endif /* FSK_MODULATOR_H */