#include "costas.h"
#include "common/dsp/block.h"

namespace satdump
{
    namespace ndsp
    {
        CostasBlock::CostasBlock() : BlockSimple("costas_cc", {{"in", DSP_SAMPLE_TYPE_CF32}}, {{"out", DSP_SAMPLE_TYPE_CF32}}) {}

        CostasBlock::~CostasBlock() {}

        uint32_t CostasBlock::process(complex_t *input, uint32_t nsamples, complex_t *output)
        {
            complex_t *ibuf = input;
            complex_t *obuf = output;

            for (uint32_t i = 0; i < nsamples; i++)
            {
                // Mix input & VCO
                tmp_val = ibuf[i] * complex_t(cosf(-phase), sinf(-phase));
                obuf[i] = tmp_val;

                // Calculate error
                switch (order)
                {
                case 2: // Order 2, BPSK
                    error = tmp_val.real * tmp_val.imag;
                    break;
                case 4: // Order 4, QPSK
                    error = (tmp_val.real > 0.0f ? 1.0f : -1.0f) * tmp_val.imag - (tmp_val.imag > 0.0f ? 1.0f : -1.0f) * tmp_val.real;
                    break;
                case 8: // Order 8, 8-PSK
                    const float K = (sqrtf(2.0) - 1);
                    if (fabsf(tmp_val.real) >= fabsf(tmp_val.imag))
                        error = ((tmp_val.real > 0.0f ? 1.0f : -1.0f) * tmp_val.imag - (tmp_val.imag > 0.0f ? 1.0f : -1.0f) * tmp_val.real * K);
                    else
                        error = ((tmp_val.real > 0.0f ? 1.0f : -1.0f) * tmp_val.imag * K - (tmp_val.imag > 0.0f ? 1.0f : -1.0f) * tmp_val.real);
                    break;
                }

                // Clip error
                error = dsp::branched_clip(error, 1.0);

                // Compute new freq and phase.
                freq += beta * error;
                phase += freq + alpha * error;

                // Wrap phase
                while (phase > (2 * M_PI))
                    phase -= 2 * M_PI;
                while (phase < (-2 * M_PI))
                    phase += 2 * M_PI;

                // Clamp freq
                if (freq > freq_limit_max)
                    freq = freq_limit_max;
                if (freq < freq_limit_min)
                    freq = freq_limit_min;
            }

            return nsamples;
        }
    } // namespace ndsp
} // namespace satdump