#include "stereo_encoder.h"

// Multiplier is the multiplier to get to 19 khz
void init_stereo_encoder(StereoEncoder* st, uint8_t stereo_ssb, uint8_t multiplier, Oscillator* osc, float audio_volume, float pilot_volume) {
    st->multiplier = multiplier;
    st->osc = osc;
    st->pilot_volume = pilot_volume;
    st->audio_volume = audio_volume * 0.5f;
    if(stereo_ssb) {
        init_delay_line(&st->delay_pilot, stereo_ssb*2);
        init_delay_line(&st->delay, stereo_ssb*2);
        st->stereo_hilbert = firhilbf_create(stereo_ssb, 80);
    } else st->stereo_hilbert = NULL;
}

float stereo_encode(StereoEncoder* st, uint8_t enabled, float left, float right, float *audio) {
    float mid = (left+right) * 0.5f;
    if(!enabled) {
        *audio = mid * st->audio_volume * 2.0f;
        return 0.0f;
    }

    float side = (left-right) * 0.5f;
    float signalx1 = get_oscillator_sin_multiplier_ni(st->osc, st->multiplier);
    float signalx2 = get_oscillator_sin_multiplier_ni(st->osc, st->multiplier * 2.0f);
    if(st->stereo_hilbert) {
        float complex stereo_hilbert = 0+0*I;
        float signalx2cos = get_oscillator_cos_multiplier_ni(st->osc, st->multiplier * 2.0f);

        mid = delay_line(&st->delay, mid);
        *audio = mid * st->audio_volume;
        signalx1 = delay_line(&st->delay_pilot, signalx1);

        firhilbf_r2c_execute(st->stereo_hilbert, side, &stereo_hilbert);

        *audio += ((crealf(stereo_hilbert) * signalx2) - (cimagf(stereo_hilbert) * signalx2cos)) * st->audio_volume;
        return signalx1 * st->pilot_volume;
    }

    *audio = mid * st->audio_volume + (side*signalx2) * st->audio_volume;
    return signalx1 * st->pilot_volume;
}

void exit_stereo_encoder(StereoEncoder* st) {
    if(st->stereo_hilbert) {
        exit_delay_line(&st->delay);
        exit_delay_line(&st->delay_pilot);
	    firhilbf_destroy(st->stereo_hilbert);
        st->stereo_hilbert = NULL;
    }
}