#ifndef PSK_MODULATOR_H
#define PSK_MODULATOR_H

#include <vector>
#include <cmath>
#include <cstdint>
#include <stdexcept>

class PSKModulator {
public:
    PSKModulator(double sample_rate) : sample_rate(sample_rate), carrier_freq(1800), phase(0.0) {
        initializeSymbolMap();
    }

    std::vector<int16_t> modulate(const std::vector<uint8_t>& symbols) {
        std::vector<int16_t> modulated_signal;

        const double phase_increment = 2 * M_PI * carrier_freq / sample_rate;
        for (auto symbol : symbols) {
            if (symbol >= symbolMap.size()) {
                throw std::out_of_range("Invalid symbol value for 8-PSK modulation");
            }
            double target_phase = symbolMap[symbol];

            for (size_t i = 0; i < samples_per_symbol; ++i) {
                double value = std::sin(phase + target_phase);
                modulated_signal.push_back(static_cast<int16_t>(value * std::numeric_limits<int16_t>::max()));
                phase += phase_increment;
                if (phase >= 2 * M_PI) {
                    phase -= 2 * M_PI;
                }
            }
        }
        return modulated_signal;
    }

private:
    double sample_rate;        ///< The sample rate of the system.
    double carrier_freq;       ///< The frequency of the carrier, set to 1800 Hz as per standard.
    double phase;              ///< Current phase of the carrier waveform.
    size_t samples_per_symbol = 40; ///< Number of samples per symbol, calculated to match symbol duration with cycle.
    std::vector<double> symbolMap;  ///< The mapping of tribit symbols to phase shifts.

    void initializeSymbolMap() {
        symbolMap = {
            0.0,          // 0 (000) corresponds to 0 degrees
            M_PI / 4,     // 1 (001) corresponds to 45 degrees
            M_PI / 2,     // 2 (010) corresponds to 90 degrees
            3 * M_PI / 4, // 3 (011) corresponds to 135 degrees
            M_PI,         // 4 (100) corresponds to 180 degrees
            5 * M_PI / 4, // 5 (101) corresponds to 225 degrees
            3 * M_PI / 2, // 6 (110) corresponds to 270 degrees
            7 * M_PI / 4  // 7 (111) corresponds to 315 degrees
        };
    }
};

#endif