/**
 * @file main.cpp
 * @brief Program entry point.
 *
 * Copyright (C) 2020 Clyne Sullivan
 *
 * Distributed under the GNU GPL v3 or later. You should have received a copy of
 * the GNU General Public License along with this program.
 * If not, see <https://www.gnu.org/licenses/>.
 */

#include "ch.h"
#include "hal.h"

static_assert(sizeof(adcsample_t) == sizeof(uint16_t));
static_assert(sizeof(dacsample_t) == sizeof(uint16_t));

#include "adc.hpp"
#include "cordic.hpp"
#include "dac.hpp"
#include "elf_load.hpp"
#include "error.hpp"
#include "samplebuffer.hpp"
#include "sclock.hpp"
#include "usbserial.hpp"

#include <array>

constexpr unsigned int MAX_ELF_FILE_SIZE = 8 * 1024;

enum class RunStatus : char
{
    Idle = '1',
    Running,
    Recovering
};
static RunStatus run_status = RunStatus::Idle;

#define MSG_CONVFIRST          (1)
#define MSG_CONVSECOND         (2)
#define MSG_CONVFIRST_MEASURE  (3)
#define MSG_CONVSECOND_MEASURE (4)

#define MSG_FOR_FIRST(m)   (m & 1)
#define MSG_FOR_MEASURE(m) (m > 2)

static ErrorManager EM;

static msg_t conversionMBBuffer[2];
static MAILBOX_DECL(conversionMB, conversionMBBuffer, 2);

// Thread for LED status and wakeup hold
__attribute__((section(".stacks")))
static THD_WORKING_AREA(monitorThreadWA, 4096);
/*extern "C"*/static THD_FUNCTION(monitorThread, arg);

// Thread for managing the conversion task
__attribute__((section(".stacks")))
static THD_WORKING_AREA(conversionThreadMonitorWA, 4096);
static THD_FUNCTION(conversionThreadMonitor, arg);
static thread_t *conversionThreadHandle = nullptr;

// Thread for unprivileged algorithm execution
__attribute__((section(".stacks")))
static THD_WORKING_AREA(conversionThreadWA, 128); // All we do is enter unprivileged mode.
static THD_FUNCTION(conversionThread, arg);
__attribute__((section(".convdata")))
static THD_WORKING_AREA(conversionThreadUPWA, 62 * 1024);
__attribute__((section(".convdata")))
static thread_t *conversionThreadMonitorHandle = nullptr;

// Thread for USB monitoring
__attribute__((section(".stacks")))
static THD_WORKING_AREA(communicationThreadWA, 4096);
static THD_FUNCTION(communicationThread, arg);

static time_measurement_t conversion_time_measurement;
__attribute__((section(".convdata")))
static SampleBuffer samplesIn  (reinterpret_cast<Sample *>(0x38000000)); // 16k
__attribute__((section(".convdata")))
static SampleBuffer samplesOut (reinterpret_cast<Sample *>(0x30004000)); // 16k

static SampleBuffer samplesSigGen (reinterpret_cast<Sample *>(0x30000000)); // 16k

static unsigned char elf_file_store[MAX_ELF_FILE_SIZE];
__attribute__((section(".convdata")))
static ELF::Entry elf_entry = nullptr;

__attribute__((section(".convcode")))
static void conversion_unprivileged_main();

static void mpu_setup();
static void conversion_abort();
static void signal_operate(adcsample_t *buffer, size_t count);
static void signal_operate_measure(adcsample_t *buffer, size_t count);

int main()
{
    // Initialize the RTOS
    halInit();
    chSysInit();

    SCB->CPACR |= 0xF << 20; // Enable FPU
    mpu_setup();

    palSetLineMode(LINE_BUTTON, PAL_MODE_INPUT);
    ADC::begin();
    DAC::begin();
    SClock::begin();
    USBSerial::begin();
    math::init();

    SClock::setRate(SClock::Rate::R32K);
    ADC::setRate(SClock::Rate::R32K);

    chTMObjectInit(&conversion_time_measurement);
    chThdCreateStatic(
        monitorThreadWA, sizeof(monitorThreadWA),
        LOWPRIO,
        monitorThread, nullptr);
    conversionThreadMonitorHandle = chThdCreateStatic(
        conversionThreadMonitorWA, sizeof(conversionThreadMonitorWA),
        NORMALPRIO + 1,
        conversionThreadMonitor, nullptr);
    conversionThreadHandle = chThdCreateStatic(
        conversionThreadWA, sizeof(conversionThreadWA),
        HIGHPRIO,
        conversionThread,
        reinterpret_cast<void *>(reinterpret_cast<uint32_t>(conversionThreadUPWA) + 62 * 1024));
    chThdCreateStatic(
        communicationThreadWA, sizeof(communicationThreadWA),
        NORMALPRIO,
        communicationThread, nullptr);

    chThdExit(0);
    return 0;
}

THD_FUNCTION(communicationThread, arg)
{
    (void)arg;
	while (1) {
        if (USBSerial::isActive()) {
            // Attempt to receive a command packet
            if (unsigned char cmd[3]; USBSerial::read(&cmd[0], 1) > 0) {
                // Packet received, first byte represents the desired command/action
                switch (cmd[0]) {

                // 'a' - Read contents of ADC buffer.
                // 'A' - Write contents of ADC buffer.
                // 'B' - Set ADC/DAC buffer size.
                // 'd' - Read contents of DAC buffer.
                // 'D' - Set siggen size and write to its buffer.
                // 'E' - Load algorithm binary.
                // 'e' - Unload algorithm.
                // 'i' - Read "stmdsp" identifier string.
                // 'I' - Read status information.
                // 'M' - Begin conversion, measure algorithm execution time.
                // 'm' - Read last algorithm execution time.
                // 'R' - Begin conversion.
                // 'r' - Read or write sample rate.
                // 'S' - Stop conversion.
                // 's' - Get latest block of conversion results.
                // 't' - Get latest block of conversion input.
                // 'W' - Start signal generator (siggen).
                // 'w' - Stop siggen.

                case 'a':
                    USBSerial::write(samplesIn.bytedata(), samplesIn.bytesize());
                    break;
                case 'A':
                    USBSerial::read(samplesIn.bytedata(), samplesIn.bytesize());
                    break;

                case 'B':
                    if (EM.assert(run_status == RunStatus::Idle, Error::NotIdle) &&
                        EM.assert(USBSerial::read(&cmd[1], 2) == 2, Error::BadParamSize))
                    {
                        // count is multiplied by two since this command receives size of buffer
                        // for each algorithm application.
                        unsigned int count = (cmd[1] | (cmd[2] << 8)) * 2;
                        if (EM.assert(count <= MAX_SAMPLE_BUFFER_SIZE, Error::BadParam)) {
                            samplesIn.setSize(count);
                            samplesOut.setSize(count);
                        }
                    }
                    break;

                case 'd':
                    USBSerial::write(samplesOut.bytedata(), samplesOut.bytesize());
                    break;
                case 'D':
                    if (EM.assert(USBSerial::read(&cmd[1], 2) == 2, Error::BadParamSize)) {
                        unsigned int count = cmd[1] | (cmd[2] << 8);
                        if (EM.assert(count <= MAX_SAMPLE_BUFFER_SIZE, Error::BadParam)) {
                            samplesSigGen.setSize(count);
                            USBSerial::read(samplesSigGen.bytedata(), samplesSigGen.bytesize());
                        }
                    }
                    break;

                // 'E' - Reads in and loads the compiled conversion code binary from USB.
                case 'E':
                    if (EM.assert(run_status == RunStatus::Idle, Error::NotIdle) &&
                        EM.assert(USBSerial::read(&cmd[1], 2) == 2, Error::BadParamSize))
                    {
                        // Only load the binary if it can fit in the memory reserved for it.
                        unsigned int size = cmd[1] | (cmd[2] << 8);
                        if (EM.assert(size < sizeof(elf_file_store), Error::BadUserCodeSize)) {
                            USBSerial::read(elf_file_store, size);
                            elf_entry = ELF::load(elf_file_store);

                            EM.assert(elf_entry != nullptr, Error::BadUserCodeLoad);
                        }
                    }
                    break;

                // 'e' - Unloads the currently loaded conversion code
                case 'e':
                    elf_entry = nullptr;
                    break;

                // 'i' - Sends an identifying string to confirm that this is the stmdsp device.
                case 'i':
                    USBSerial::write(reinterpret_cast<const uint8_t *>("stmdsp"), 6);
                    break;

                // 'I' - Sends the current run status.
                case 'I':
                    {
                        unsigned char buf[2] = {
                            static_cast<unsigned char>(run_status),
                            static_cast<unsigned char>(EM.pop())
                        };
                        USBSerial::write(buf, sizeof(buf));
                    }
                    break;

                // 'M' - Begins continuous sampling, but measures the execution time of the first
                //       sample processing. This duration can be later read through 'm'.
                case 'M':
                    if (EM.assert(run_status == RunStatus::Idle, Error::NotIdle)) {
                        run_status = RunStatus::Running;
                        samplesOut.clear();
                        ADC::start(samplesIn.data(), samplesIn.size(), signal_operate_measure);
                        DAC::start(0, samplesOut.data(), samplesOut.size());
                    }
                    break;

                // 'm' - Returns the last measured sample processing time, presumably in processor
                //       ticks.
                case 'm':
                    USBSerial::write(reinterpret_cast<uint8_t *>(&conversion_time_measurement.last),
                                     sizeof(rtcnt_t));
                    break;

                // 'R' - Begin continuous sampling/conversion of the ADC. Samples will go through
                //       the conversion code, and will be sent out over the DAC.
                case 'R':
                    if (EM.assert(run_status == RunStatus::Idle, Error::NotIdle)) {
                        run_status = RunStatus::Running;
                        samplesOut.clear();
                        ADC::start(samplesIn.data(), samplesIn.size(), signal_operate);
                        DAC::start(0, samplesOut.data(), samplesOut.size());
                    }
                    break;

                case 'r':
                    if (EM.assert(USBSerial::read(&cmd[1], 1) == 1, Error::BadParamSize)) {
                        if (cmd[1] == 0xFF) {
                            unsigned char r = SClock::getRate();
                            USBSerial::write(&r, 1);
                        } else {
                            auto r = static_cast<SClock::Rate>(cmd[1]);
                            SClock::setRate(r);
                            ADC::setRate(r);
                        }
                    }
                    break;

                // 'S' - Stops the continuous sampling/conversion.
                case 'S':
                    if (run_status == RunStatus::Running) {
                        DAC::stop(0);
                        ADC::stop();
                        run_status = RunStatus::Idle;
                    }
                    break;

                case 's':
                    if (auto samps = samplesOut.modified(); samps != nullptr) {
                        unsigned char buf[2] = {
                            static_cast<unsigned char>(samplesOut.size() / 2 & 0xFF),
                            static_cast<unsigned char>(((samplesOut.size() / 2) >> 8) & 0xFF)
                        };
                        USBSerial::write(buf, 2);
                        unsigned int total = samplesOut.bytesize() / 2;
                        unsigned int offset = 0;
                        unsigned char unused;
                        while (total > 512) {
                            USBSerial::write(reinterpret_cast<uint8_t *>(samps) + offset, 512);
                            while (USBSerial::read(&unused, 1) == 0);
                            offset += 512;
                            total -= 512;
                        }
                        USBSerial::write(reinterpret_cast<uint8_t *>(samps) + offset, total);
                        while (USBSerial::read(&unused, 1) == 0);
                    } else {
                        USBSerial::write(reinterpret_cast<const uint8_t *>("\0\0"), 2);
                    }
                    break;
                case 't':
                    if (auto samps = samplesIn.modified(); samps != nullptr) {
                        unsigned char buf[2] = {
                            static_cast<unsigned char>(samplesIn.size() / 2 & 0xFF),
                            static_cast<unsigned char>(((samplesIn.size() / 2) >> 8) & 0xFF)
                        };
                        USBSerial::write(buf, 2);
                        unsigned int total = samplesIn.bytesize() / 2;
                        unsigned int offset = 0;
                        unsigned char unused;
                        while (total > 512) {
                            USBSerial::write(reinterpret_cast<uint8_t *>(samps) + offset, 512);
                            while (USBSerial::read(&unused, 1) == 0);
                            offset += 512;
                            total -= 512;
                        }
                        USBSerial::write(reinterpret_cast<uint8_t *>(samps) + offset, total);
                        while (USBSerial::read(&unused, 1) == 0);
                    } else {
                        USBSerial::write(reinterpret_cast<const uint8_t *>("\0\0"), 2);
                    }
                    break;

                case 'W':
                    DAC::start(1, samplesSigGen.data(), samplesSigGen.size());
                    break;
                case 'w':
                    DAC::stop(1);
                    break;

                default:
                    break;
                }
            }
        }

		chThdSleepMicroseconds(100);
	}
}

THD_FUNCTION(conversionThreadMonitor, arg)
{
    (void)arg;
    while (1) {
        // Recover from algorithm fault if necessary
        //if (run_status == RunStatus::Recovering)
        //    conversion_abort();

        msg_t message;
        if (chMBFetchTimeout(&conversionMB, &message, TIME_INFINITE) == MSG_OK)
            chMsgSend(conversionThreadHandle, message);
    }
}

THD_FUNCTION(conversionThread, stack)
{
    elf_entry = nullptr;
    port_unprivileged_jump(reinterpret_cast<uint32_t>(conversion_unprivileged_main),
                           reinterpret_cast<uint32_t>(stack));
}

THD_FUNCTION(monitorThread, arg)
{
    (void)arg;

    bool erroron = false;
    while (1) {
        bool isidle = run_status == RunStatus::Idle;
        auto led = isidle ? LINE_LED_GREEN : LINE_LED_YELLOW;
        auto delay = isidle ? 500 : 250;

        palSetLine(led);
        chThdSleepMilliseconds(delay);
        palClearLine(led);
        chThdSleepMilliseconds(delay);

        if (run_status == RunStatus::Idle && palReadLine(LINE_BUTTON)) {
            palSetLine(LINE_LED_RED);
            palSetLine(LINE_LED_YELLOW);
            chSysLock();
            while (palReadLine(LINE_BUTTON))
                asm("nop");
            while (!palReadLine(LINE_BUTTON))
                asm("nop");
            chSysUnlock();
            palClearLine(LINE_LED_RED);
            palClearLine(LINE_LED_YELLOW);
            chThdSleepMilliseconds(500);
        }

        if (auto err = EM.hasError(); err ^ erroron) {
            erroron = err;
            if (err)
                palSetLine(LINE_LED_RED);
            else
                palClearLine(LINE_LED_RED);
        }
    }
}

void conversion_unprivileged_main()
{
    while (1) {
        msg_t message;
        asm("svc 0; mov %0, r0" : "=r" (message)); // sleep until next message
        if (message != 0) {
            auto samples = MSG_FOR_FIRST(message) ? samplesIn.data() : samplesIn.middata();
            auto size = samplesIn.size() / 2;

            if (elf_entry) {
                if (!MSG_FOR_MEASURE(message)) {
                    samples = elf_entry(samples, size);
                } else {
                    asm("eor r0, r0; svc 2"); // start measurement
                    samples = elf_entry(samples, size);
                    asm("mov r0, #1; svc 2"); // stop measurement
                } 
            }

            if (MSG_FOR_FIRST(message))
                samplesOut.modify(samples, size); 
            else
                samplesOut.midmodify(samples, size); 
        }
    }
}

void mpu_setup()
{
    // Set up MPU for user algorithm
    // Region 2: Data for algorithm manager thread
    mpuConfigureRegion(MPU_REGION_2,
                       0x20000000,
                       MPU_RASR_ATTR_AP_RW_RW | MPU_RASR_ATTR_NON_CACHEABLE |
                       MPU_RASR_SIZE_64K |
                       MPU_RASR_ENABLE);
    // Region 3: Code for algorithm manager thread
    mpuConfigureRegion(MPU_REGION_3,
                       0x0807F800,
                       MPU_RASR_ATTR_AP_RO_RO | MPU_RASR_ATTR_NON_CACHEABLE |
                       MPU_RASR_SIZE_2K |
                       MPU_RASR_ENABLE);
    // Region 4: Algorithm code
    mpuConfigureRegion(MPU_REGION_4,
                       0x00000000,
                       MPU_RASR_ATTR_AP_RW_RW | MPU_RASR_ATTR_NON_CACHEABLE |
                       MPU_RASR_SIZE_64K |
                       MPU_RASR_ENABLE);
}

void conversion_abort()
{
    elf_entry = nullptr;
    DAC::stop(0);
    ADC::stop();
    EM.add(Error::ConversionAborted);

    chMBReset(&conversionMB);
    run_status = RunStatus::Idle;
}

void signal_operate(adcsample_t *buffer, size_t)
{
    chSysLockFromISR();

    if (chMBGetUsedCountI(&conversionMB) > 1) {
        chSysUnlockFromISR();
        conversion_abort();
    } else {
        if (buffer == samplesIn.data()) {
            samplesIn.setModified();
            chMBPostI(&conversionMB, MSG_CONVFIRST);
        } else {
            samplesIn.setMidmodified();
            chMBPostI(&conversionMB, MSG_CONVSECOND);
        }
        chSysUnlockFromISR();
    }
}

void signal_operate_measure(adcsample_t *buffer, [[maybe_unused]] size_t count)
{
    chSysLockFromISR();
    if (buffer == samplesIn.data()) {
        samplesIn.setModified();
        chMBPostI(&conversionMB, MSG_CONVFIRST_MEASURE);
    } else {
        samplesIn.setMidmodified();
        chMBPostI(&conversionMB, MSG_CONVSECOND_MEASURE);
    }
    chSysUnlockFromISR();

    ADC::setOperation(signal_operate);
}

extern "C" {

__attribute__((naked))
void port_syscall(struct port_extctx *ctxp, uint32_t n)
{
    switch (n) {
    case 0:
        {
            chSysLock();
            chMsgWaitS();
            auto msg = chMsgGet(conversionThreadMonitorHandle);
            chMsgReleaseS(conversionThreadMonitorHandle, MSG_OK);
            chSysUnlock();
            ctxp->r0 = msg;
        }
        break;
    case 1:
        {
            using mathcall = void (*)();
            static mathcall funcs[4] = {
                reinterpret_cast<mathcall>(math::sin),
                reinterpret_cast<mathcall>(math::cos),
                reinterpret_cast<mathcall>(math::tan),
                reinterpret_cast<mathcall>(math::sqrt),
            };
            asm("vmov.f64 d0, %0, %1" :: "r" (ctxp->r1), "r" (ctxp->r2));
            if (ctxp->r0 < 4) {
                funcs[ctxp->r0]();
                asm("vmov.f64 %0, %1, d0" : "=r" (ctxp->r1), "=r" (ctxp->r2));
            } else {
                asm("eor r0, r0; vmov.f64 d0, r0, r0");
            }
        }
        break;
    case 2:
        if (ctxp->r0 == 0) {
            chTMStartMeasurementX(&conversion_time_measurement);
        } else {
            chTMStopMeasurementX(&conversion_time_measurement);
            // Subtract measurement overhead from the result.
            // Running an empty algorithm ("bx lr") takes 196 cycles as of 2/4/21.
            // Only measures algorithm code time (loading args/storing result takes 9 cycles).
            constexpr rtcnt_t measurement_overhead = 196 - 1;
            if (conversion_time_measurement.last > measurement_overhead)
                conversion_time_measurement.last -= measurement_overhead;
        }
        break;
    default:
        while (1);
        break;
    }

    asm("svc 0");
    while (1);
}

__attribute__((naked))
void MemManage_Handler()
{
    while (1);
}

__attribute__((naked))
void HardFault_Handler()
{
    // Below not working (yet)
    while (1);

    // 1. Get the stack pointer
    uint32_t *stack;
    uint32_t lr;
	asm("\
		tst lr, #4; \
		ite eq; \
		mrseq %0, msp; \
		mrsne %0, psp; \
        mov %1, lr; \
	" : "=r" (stack), "=r" (lr));

    // 2. Only attempt to recover from failed algorithm code
    if ((lr & 4) == 0 || run_status != RunStatus::Running)
        while (1);

    // 3. Post the failure and unload algorithm
    elf_entry = nullptr;
    EM.add(Error::ConversionAborted);
    run_status = RunStatus::Recovering;

    // 4. Make this exception return to point after algorithm exec.
    stack[6] = stack[5];
    stack[7] |= (1 << 24); // Ensure Thumb mode stays enabled

    asm("mov lr, %0; bx lr" :: "r" (lr));
}

} // extern "C"