mayhem-firmware/firmware/baseband/proc_spectrum_painter.cpp

/*
 * Copyright (C) 2023 Bernd Herzog
 *
 * 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_spectrum_painter.hpp"
#include "event_m4.hpp"
#include "dsp_fft.hpp"
#include "random.hpp"

#include <cstdint>

// TODO move to class members SpectrumPainterProcessor
complex16_t* current_line_data = nullptr;
complex16_t* volatile next_line_data = nullptr;
uint32_t current_line_index = 0;
uint32_t current_line_width = 0;
int32_t current_bw = 0;

std::vector<uint8_t> fifo_data[1 << SpectrumPainterBufferConfigureResponseMessage::fifo_k]{};
SpectrumPainterFIFO fifo{fifo_data, SpectrumPainterBufferConfigureResponseMessage::fifo_k};

int max_val = 127;

// This is called at 3072000/2048 = 1500Hz
void SpectrumPainterProcessor::execute(const buffer_c8_t& buffer) {
    for (uint32_t i = 0; i < buffer.count; i++) {
        if (current_line_data != nullptr) {
            auto data = current_line_data[(current_line_index++ * current_bw / 3072) % current_line_width];
            buffer.p[i] = {(int8_t)data.real(), (int8_t)data.imag()};
        } else
            buffer.p[i] = {0, 0};
    }

    // collect new data
    if (next_line_data != nullptr) {
        if (current_line_data != nullptr)
            delete current_line_data;

        current_line_data = next_line_data;
        next_line_data = nullptr;
    }
}

WORKING_AREA(thread_wa, 4096);

void SpectrumPainterProcessor::run() {
    int ui = 0;
    init_genrand(22267);

    while (true) {
        if (fifo.is_empty() == false && next_line_data == nullptr) {
            std::vector<uint8_t> data;
            fifo.out(data);

            auto picture_width = data.size();

            auto fft_width = picture_width * 2;
            auto qu = fft_width / 4;

            complex16_t* v = new complex16_t[fft_width];
            complex16_t* tmp = new complex16_t[fft_width];

            for (uint32_t fft_index = 0; fft_index < fft_width; fft_index++) {
                if (fft_index < qu) {
                } else if (fft_index < qu * 3) {
                    // TODO: Improve index handling
                    auto image_index = fft_index - qu;

                    auto bin_power = data[image_index];  // 0 to 255
                    auto bin_phase = genrand_int31();    // 0 to 255

                    // rotate by random angle
                    auto phase_cos = (sine_table_i8[((int)(bin_phase + 0x40)) & 0xFF]);  // -127 to 127
                    auto phase_sin = (sine_table_i8[((int)(bin_phase)) & 0xFF]);

                    auto real = (int16_t)((int16_t)phase_cos * bin_power / 255);  // -127 to 127
                    auto imag = (int16_t)((int16_t)phase_sin * bin_power / 255);  // -127 to 127

                    auto fftshift_index = 0;
                    if (fft_index < qu * 2)                   // first half (fft_index = qu; fft_index < qu*2)
                        fftshift_index = fft_index + 2 * qu;  // goes to back
                    else                                      // 2nd half (fft_index =  qu*2; fft_index < qu*3)
                        fftshift_index = fft_index - 2 * qu;  // goes to front

                    v[fftshift_index] = {real, imag};
                }
            }

            ifft<complex16_t>(v, fft_width, tmp);

            // normalize
            volatile int32_t maximum = 1;
            for (uint32_t i = 0; i < fft_width; i++) {
                if (v[i].real() > maximum)
                    maximum = v[i].real();
                if (v[i].real() < -maximum)
                    maximum = -v[i].real();
                if (v[i].imag() > maximum)
                    maximum = v[i].imag();
                if (v[i].imag() < -maximum)
                    maximum = -v[i].imag();
            }

            if (maximum == 1) {  // a black line
                for (uint32_t i = 0; i < fft_width; i++)
                    v[i] = {0, 0};
            } else {
                for (uint32_t i = 0; i < fft_width; i++) {
                    v[i] = {(int8_t)((int32_t)v[i].real() * 120 / maximum), (int8_t)((int32_t)v[i].imag() * 120 / maximum)};
                }
            }

            delete tmp;
            next_line_data = v;
            ui++;
        } else {
            chThdSleepMilliseconds(1);
        }
    }
}

void SpectrumPainterProcessor::on_message(const Message* const msg) {
    switch (msg->id) {
        case Message::ID::SpectrumPainterBufferRequestConfigure: {
            const auto message = *reinterpret_cast<const SpectrumPainterBufferConfigureRequestMessage*>(msg);
            current_line_width = message.width;
            current_bw = message.bw / 500;

            if (message.update == false) {
                SpectrumPainterBufferConfigureResponseMessage response{&fifo};
                shared_memory.application_queue.push(response);

                if (configured == false) {
                    thread = chThdCreateStatic(thread_wa, sizeof(thread_wa),
                                               NORMALPRIO, SpectrumPainterProcessor::fn,
                                               this);

                    configured = true;
                }
            }
            break;
        }

        default:
            break;
    }
}

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