#include "conversion.hpp"

#include "periph/adc.hpp"
#include "periph/dac.hpp"
#include "elfload.hpp"
#include "error.hpp"
#include "runstatus.hpp"
#include "samples.hpp"

constexpr msg_t MSG_CONVFIRST          = 1;
constexpr msg_t MSG_CONVSECOND         = 2;
constexpr msg_t MSG_CONVFIRST_MEASURE  = 3;
constexpr msg_t MSG_CONVSECOND_MEASURE = 4;

constexpr auto MSG_FOR_FIRST   = [](msg_t m) { return m & 1; };
constexpr auto MSG_FOR_MEASURE = [](msg_t m) { return m > 2; };

time_measurement_t conversion_time_measurement;

__attribute__((section(".convdata")))
thread_t *ConversionManager::m_thread_monitor = nullptr;
thread_t *ConversionManager::m_thread_runner = nullptr;

__attribute__((section(".stacks")))
std::array<char, 1024> ConversionManager::m_thread_monitor_stack = {};
__attribute__((section(".stacks")))
std::array<char, 128> ConversionManager::m_thread_runner_entry_stack = {};
__attribute__((section(".convdata")))
std::array<char, CONVERSION_THREAD_STACK_SIZE> ConversionManager::m_thread_runner_stack = {};

std::array<msg_t, 2> ConversionManager::m_mailbox_buffer;
mailbox_t ConversionManager::m_mailbox = _MAILBOX_DATA(m_mailbox, m_mailbox_buffer.data(), m_mailbox_buffer.size());

void ConversionManager::begin()
{
    m_thread_monitor = chThdCreateStatic(m_thread_monitor_stack.data(),
                                         m_thread_monitor_stack.size(),
                                         NORMALPRIO + 1,
                                         threadMonitor,
                                         nullptr);
    auto runner_stack_end = &m_thread_runner_stack[CONVERSION_THREAD_STACK_SIZE];
    m_thread_runner = chThdCreateStatic(m_thread_runner_entry_stack.data(),
                                        m_thread_runner_entry_stack.size(),
                                        HIGHPRIO,
                                        threadRunnerEntry,
                                        runner_stack_end);
}

void ConversionManager::start()
{
    Samples::Out.clear();
    ADC::start(Samples::In.data(), Samples::In.size(), adcReadHandler);
    DAC::start(0, Samples::Out.data(), Samples::Out.size());
}

void ConversionManager::startMeasured()
{
    Samples::Out.clear();
    ADC::start(Samples::In.data(), Samples::In.size(), adcReadHandlerMeasure);
    DAC::start(0, Samples::Out.data(), Samples::Out.size());
}

void ConversionManager::stop()
{
    DAC::stop(0);
    ADC::stop();
}

thread_t *ConversionManager::getMonitorHandle()
{
    return m_thread_monitor;
}

void ConversionManager::abort()
{
    ELFManager::unload();
    EM.add(Error::ConversionAborted);
    run_status = RunStatus::Recovering;

    // Confirm that the exception return thread is the algorithm...
    uint32_t *psp;
	asm("mrs %0, psp" : "=r" (psp));

    bool isRunnerStack = 
           (uint32_t)psp >= reinterpret_cast<uint32_t>(m_thread_runner_stack.data()) &&
           (uint32_t)psp <= reinterpret_cast<uint32_t>(m_thread_runner_stack.data() +
                                                       m_thread_runner_stack.size());

    if (isRunnerStack)
    {
        // If it is, we can force the algorithm to exit by "resetting" its thread.
        // We do this by rebuilding the thread's stacked exception return.
        auto newpsp = reinterpret_cast<uint32_t *>(m_thread_runner_stack.data() + 
                                                   m_thread_runner_stack.size() -
                                                   8 * sizeof(uint32_t));
        // Set the LR register to the thread's entry point.
        newpsp[5] = reinterpret_cast<uint32_t>(threadRunner);
        // Overwrite the instruction we'll return to with "bx lr" (jump to address in LR).
        newpsp[6] = psp[6];
        *reinterpret_cast<uint16_t *>(newpsp[6]) = 0x4770; // "bx lr"
        // Keep PSR contents (bit set forces Thumb mode, just in case).
        newpsp[7] = psp[7] | (1 << 24);
        // Set the new stack pointer.
	    asm("msr psp, %0" :: "r" (newpsp));
    }
}

void ConversionManager::threadMonitor(void *)
{
    while (1) {
        msg_t message;
        msg_t fetch = chMBFetchTimeout(&m_mailbox, &message, TIME_INFINITE);
        if (fetch == MSG_OK)
            chMsgSend(m_thread_runner, message);
    }
}

void ConversionManager::threadRunnerEntry(void *stack)
{
    ELFManager::unload();
    port_unprivileged_jump(reinterpret_cast<uint32_t>(threadRunner),
                           reinterpret_cast<uint32_t>(stack));
}

__attribute__((section(".convcode")))
void ConversionManager::threadRunner(void *)
{
    while (1) {
        // Sleep until we receive a mailbox message.
        msg_t message;
        asm("svc 0; mov %0, r0" : "=r" (message));

        if (message != 0) {
            auto samples = MSG_FOR_FIRST(message) ? Samples::In.data()
                                                  : Samples::In.middata();
            auto size = Samples::In.size() / 2;

            auto entry = ELFManager::loadedElf();
            if (entry) {
                // Below, we remember the stack pointer just in case the
                // loaded algorithm messes things up.
                uint32_t sp;

                if (!MSG_FOR_MEASURE(message)) {
                    asm("mov %0, sp" : "=r" (sp));
                    samples = entry(samples, size);
                    asm("mov sp, %0" :: "r" (sp));
                } else {
                    // Start execution timer:
                    asm("mov %0, sp; eor r0, r0; svc 2" : "=r" (sp));
                    samples = entry(samples, size);
                    // Stop execution timer:
                    asm("mov r0, #1; svc 2; mov sp, %0" :: "r" (sp));
                } 
            }

            // Update the sample out buffer with the transformed samples.
            if (samples != nullptr) {
                if (MSG_FOR_FIRST(message))
                    Samples::Out.modify(samples, size);
                else
                    Samples::Out.midmodify(samples, size);
            }
        }
    }
}

void ConversionManager::adcReadHandler(adcsample_t *buffer, size_t)
{
    chSysLockFromISR();

    // If previous request hasn't been handled, then we're going too slow.
    // We'll need to abort.
    if (chMBGetUsedCountI(&m_mailbox) > 1) {
        chMBResetI(&m_mailbox);
        chMBResumeX(&m_mailbox);
        chSysUnlockFromISR();
        abort();
    } else {
        // Mark the modified samples as 'fresh' or ready for manipulation.
        if (buffer == Samples::In.data()) {
            Samples::In.setModified();
            chMBPostI(&m_mailbox, MSG_CONVFIRST);
        } else {
            Samples::In.setMidmodified();
            chMBPostI(&m_mailbox, MSG_CONVSECOND);
        }
        chSysUnlockFromISR();
    }
}

void ConversionManager::adcReadHandlerMeasure(adcsample_t *buffer, size_t)
{
    chSysLockFromISR();
    if (buffer == Samples::In.data()) {
        Samples::In.setModified();
        chMBPostI(&m_mailbox, MSG_CONVFIRST_MEASURE);
    } else {
        Samples::In.setMidmodified();
        chMBPostI(&m_mailbox, MSG_CONVSECOND_MEASURE);
    }
    chSysUnlockFromISR();

    ADC::setOperation(adcReadHandler);
}