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

std::vector<float> bfskModulate(const std::vector<uint8_t> &data){
    int spb=(int)(SAMPLE_RATE/BFSK_BAUD);
    size_t tb=data.size()*8;
    size_t ts=tb*spb;
    std::vector<float> out(ts*2,0.0f);
    double phase0=0.0,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 idx=0;
    for(auto byteVal:data){
        for(int b=0;b<8;b++){
            int bit=(byteVal>>b)&1;
            for(int s=0;s<spb;s++){
                float val=bit? sinf((float)phase1): sinf((float)phase0);
                if(bit) phase1+=inc1; else phase0+=inc0;
                out[idx*2+0]=val*0.3f;
                out[idx*2+1]=val*0.3f;
                idx++;
            }
        }
    }
    return out;
}

std::vector<uint8_t> bfskDemodulate(const std::vector<float> &monoData){
    int spb=(int)(SAMPLE_RATE/BFSK_BAUD);
    size_t tb=monoData.size()/spb;
    size_t tbytes=tb/8;
    std::vector<uint8_t> res(tbytes,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 bitIdx=0;
    for(size_t b=0;b<tb;b++){
        double sum0=0.0,sum1=0.0;
        double ph0=0.0,ph1=0.0;
        size_t st=b*spb;
        for(int s=0;s<spb;s++){
            float sample=monoData[st+s];
            float r0=sinf((float)ph0), r1=sinf((float)ph1);
            sum0+=sample*r0; sum1+=sample*r1;
            ph0+=inc0; ph1+=inc1;
        }
        int bit=(std::fabs(sum1)>std::fabs(sum0))?1:0;
        size_t bytePos=bitIdx/8; int bitPos=bitIdx%8;
        if(bytePos<res.size()) res[bytePos]|=(bit<<bitPos);
        bitIdx++;
    }
    return res;
}