/*
 * Copyright (C) 2015 Jared Boone, ShareBrained Technology, Inc.
 * Copyright (C) 2016 Furrtek
 *
 * This file is part of PortaPack.
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2, or (at your option)
 * any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

#include "proc_afskrx.hpp"
#include "portapack_shared_memory.hpp"

#include "event_m4.hpp"

void AFSKRxProcessor::execute(const buffer_c8_t& buffer) {
    // This is called at 3072000 / 2048 = 1500Hz

    if (!configured) return;

    // FM demodulation
    const auto decim_0_out = decim_0.execute(buffer, dst_buffer);              // 2048 / 8 = 256 (512 I/Q samples)
    const auto decim_1_out = decim_1.execute(decim_0_out, dst_buffer);         // 256 / 8 = 32 (64 I/Q samples)
    const auto channel_out = channel_filter.execute(decim_1_out, dst_buffer);  // 32 / 2 = 16 (32 I/Q samples)

    feed_channel_stats(channel_out);

    auto audio = demod.execute(channel_out, audio_buffer);

    audio_output.write(audio);

    // Audio signal processing
    for (size_t c = 0; c < audio.count; c++) {
        const int32_t sample_int = audio.p[c] * 32768.0f;
        int32_t current_sample = __SSAT(sample_int, 16);

        current_sample /= 128;

        // Delay line put
        delay_line[delay_line_index & 0x3F] = current_sample;

        // Delay line get, and LPF
        sample_mixed = (delay_line[(delay_line_index - (samples_per_bit / 2)) & 0x3F] * current_sample) / 4;
        sample_filtered = prev_mixed + sample_mixed + (prev_filtered / 2);

        delay_line_index++;

        prev_filtered = sample_filtered;
        prev_mixed = sample_mixed;

        // Slice
        sample_bits <<= 1;
        sample_bits |= (sample_filtered < -20) ? 1 : 0;

        // Check for "clean" transition: either 0011 or 1100
        if ((((sample_bits >> 2) ^ sample_bits) & 3) == 3) {
            // Adjust phase
            if (phase < 0x8000)
                phase += 0x800;  // Is this a proper value ?
            else
                phase -= 0x800;
        }

        phase += phase_inc;

        if (phase >= 0x10000) {
            phase &= 0xFFFF;

            if (trigger_word) {
                // Continuous-stream value-triggered mode (AX.25) - UNTESTED
                word_bits <<= 1;
                word_bits |= (sample_bits & 1);

                bit_counter++;

                if (triggered) {
                    if (bit_counter == word_length) {
                        bit_counter = 0;

                        data_message.is_data = true;
                        data_message.value = word_bits & word_mask;
                        shared_memory.application_queue.push(data_message);
                    }
                } else {
                    if ((word_bits & word_mask) == trigger_value) {
                        triggered = !triggered;
                        bit_counter = 0;

                        data_message.is_data = true;
                        data_message.value = trigger_value;
                        shared_memory.application_queue.push(data_message);
                    }
                }

            } else {
                // RS232-like modem mode
                if (state == WAIT_START) {
                    if (!(sample_bits & 1)) {
                        // Got start bit
                        state = RECEIVE;
                        bit_counter = 0;
                    }
                } else if (state == WAIT_STOP) {
                    if (sample_bits & 1) {
                        // Got stop bit
                        state = WAIT_START;
                    }
                } else {
                    word_bits <<= 1;
                    word_bits |= (sample_bits & 1);

                    bit_counter++;
                }

                if (bit_counter == word_length) {
                    bit_counter = 0;
                    state = WAIT_STOP;

                    data_message.is_data = true;
                    data_message.value = word_bits;
                    shared_memory.application_queue.push(data_message);
                }
            }
        }
    }
}

void AFSKRxProcessor::on_message(const Message* const message) {
    if (message->id == Message::ID::AFSKRxConfigure)
        configure(*reinterpret_cast<const AFSKRxConfigureMessage*>(message));
}

void AFSKRxProcessor::configure(const AFSKRxConfigureMessage& message) {
    /*constexpr size_t decim_0_input_fs = baseband_fs;
        constexpr size_t decim_0_output_fs = decim_0_input_fs / decim_0.decimation_factor;

        constexpr size_t decim_1_input_fs = decim_0_output_fs;
        constexpr size_t decim_1_output_fs = decim_1_input_fs / decim_1.decimation_factor;

        constexpr size_t channel_filter_input_fs = decim_1_output_fs;
        const size_t channel_filter_output_fs = channel_filter_input_fs / 2;

        const size_t demod_input_fs = channel_filter_output_fs;*/

    decim_0.configure(taps_11k0_decim_0.taps);
    decim_1.configure(taps_11k0_decim_1.taps);
    channel_filter.configure(taps_11k0_channel.taps, 2);
    demod.configure(audio_fs, 5000);

    audio_output.configure(audio_24k_hpf_300hz_config, audio_24k_deemph_300_6_config, 0);

    samples_per_bit = audio_fs / message.baudrate;

    phase_inc = (0x10000 * message.baudrate) / audio_fs;
    phase = 0;

    trigger_word = message.trigger_word;
    word_length = message.word_length;
    trigger_value = message.trigger_value;
    word_mask = (1 << word_length) - 1;

    // Delay line
    delay_line_index = 0;

    triggered = false;
    state = WAIT_START;

    configured = true;
}

int main() {
    EventDispatcher event_dispatcher{std::make_unique<AFSKRxProcessor>()};
    event_dispatcher.run();
    return 0;
}