#ifndef PSK_MODULATOR_H
#define PSK_MODULATOR_H

#include <array>
#include <cmath>
#include <cstdint>
#include <vector>


class PSKModulatorConfig {
public:
    int carrier_frequency;
    int sample_rate;
    int baud_rate;
    int bits_per_symbol;

    PSKModulatorConfig(int cf, int sr, int br) : carrier_frequency(cf), sample_rate(sr), baud_rate(br) {}
};

namespace milstd {
    class PSKModulator {
    public:
        PSKModulator(const PSKModulatorConfig& config) : carrier_frequency(config.carrier_frequency), sample_rate(config.sample_rate), baud_rate(config.baud_rate) {
            omega = 2.0 * M_PI * carrier_frequency / sample_rate;
            baud_incr = static_cast<double>(baud_rate) / sample_rate;
            phase = 0.0;
            baud_frac = 0.0;
            initializeSymbolMap();
        }

        std::vector<int16_t> modulate(const std::vector<uint8_t>& symbol_stream) {
            std::vector<int16_t> modulated_signal;
            modulated_signal.reserve(static_cast<size_t>(symbol_stream.size() / baud_incr));

            for (uint8_t symbol : symbol_stream) {
                double symbol_phase = symbolMap[symbol];

                while (baud_frac < 1.0) {
                    double sample = std::sin(omega * phase + symbol_phase);
                    int16_t output_sample = static_cast<int16_t>(sample * 32767);
                    modulated_signal.push_back(output_sample);

                    phase = std::fmod(phase + omega, 2.0 * M_PI);
                    baud_frac += baud_incr;
                }

                baud_frac -= 1.0;
            }

            return modulated_signal;
        }

    private:
        int carrier_frequency;
        int sample_rate;
        int baud_rate;

        double phase;
        double baud_frac;
        double baud_incr;
        double omega;

        std::array<double, 8> symbolMap;

        void initializeSymbolMap() {
            double phase_increment = 2.0 * M_PI / 8.0;
            for (int symbol = 0; symbol < 8; ++symbol) {
                symbolMap[symbol] = symbol * phase_increment;
            }
        }
    };
}

#endif /* PSK_MODULATOR_H */