#include "bfsk.hpp"
#include <cmath>
#include <cstdlib>
#include <iostream>

std::vector<float> bfskModulate(const std::vector<unsigned char> &data) {
    int samplesPerBit = (int)((double)SAMPLE_RATE / BFSK_BAUD);
    size_t totalBits = data.size() * 8;
    size_t totalSamples = totalBits * samplesPerBit;
    std::vector<float> out(totalSamples * 2, 0.0f); 

    double phase0 = 0.0;
    double phase1 = 0.0;
    double inc0   = 2.0 * M_PI * BFSK_FREQ0 / (double)SAMPLE_RATE;
    double inc1   = 2.0 * M_PI * BFSK_FREQ1 / (double)SAMPLE_RATE;

    size_t sampleIndex = 0;

    for (auto byteVal : data) {
        for (int b = 0; b < 8; b++) {
            int bit = (byteVal >> b) & 1;
            for (int s = 0; s < samplesPerBit; s++) {
                float val;
                if (bit == 0) {
                    val = sinf((float)phase0);
                    phase0 += inc0;
                } else {
                    val = sinf((float)phase1);
                    phase1 += inc1;
                }
                out[sampleIndex*2 + 0] = val * 0.3f; 
                out[sampleIndex*2 + 1] = val * 0.3f; 
                sampleIndex++;
            }
        }
    }
    return out;
}

std::vector<unsigned char> bfskDemodulate(const std::vector<float> &monoData) {
    int samplesPerBit = (int)((double)SAMPLE_RATE / BFSK_BAUD);
    size_t totalBits = monoData.size() / samplesPerBit;
    size_t totalBytes = totalBits / 8;

    double inc0 = 2.0 * M_PI * BFSK_FREQ0 / (double)SAMPLE_RATE;
    double inc1 = 2.0 * M_PI * BFSK_FREQ1 / (double)SAMPLE_RATE;

    std::vector<unsigned char> result(totalBytes, 0);

    size_t bitIndex = 0;
    for (size_t b = 0; b < totalBits; b++) {
        double sum0 = 0.0;
        double sum1 = 0.0;
        double phase0 = 0.0;
        double phase1 = 0.0;

        size_t startSample = b * samplesPerBit;
        for (int s = 0; s < samplesPerBit; s++) {
            float sample = monoData[startSample + s];
            float ref0   = sinf((float)phase0);
            float ref1   = sinf((float)phase1);
            sum0 += sample * ref0;
            sum1 += sample * ref1;
            phase0 += inc0;
            phase1 += inc1;
        }

        int bit = (std::fabs(sum1) > std::fabs(sum0)) ? 1 : 0;
        size_t bytePos = bitIndex / 8;
        int bitPos     = bitIndex % 8;
        if (bytePos < result.size()) {
            result[bytePos] |= (bit << bitPos);
        }
        bitIndex++;
    }

    return result;
}