#include "cmsis_os.h"
#include "ImC/imc_kernel.h"
#include "ImC/imc_extension.h"
#include "stm32l5xx_hal.h"

#include <stdio.h>

void vBenchmarkDriver(void *_benchmark)
{
    void (*benchmark)(void) = _benchmark;
    int i = 0;
    #if (imcBENCH_INFINITE_LOOP)
    while(1) {
    #endif
    portDISABLE_INTERRUPTS();
    printf("Start benchmark (%d)\r\n", i);
    portENABLE_INTERRUPTS();
    // HAL_GPIO_WritePin(GPIOF, GPIO_PIN_11, GPIO_PIN_SET);

    benchmark();

    #if(imcPRINT_STATS)
        portDISABLE_INTERRUPTS();
        printf("(STAT) checkpoint triggered: %d\r\n", imc_checkpoint_triggered);
        printf("(STAT) checkpoint executed: %d\r\n", imc_checkpoint_executed);
        printf("(STAT) no forward progress: %d\r\n", no_forward_progress);
        portENABLE_INTERRUPTS();
        imc_checkpoint_triggered = 0;
        imc_checkpoint_executed = 0;
        no_forward_progress = 0;
    #endif

    #if(imcENABLE_ADAPTIVE_LOOP_PASS_COUNT && imcPRINT_LATENCY_OVERHEAD)
        unsigned int tick = DWT->CYCCNT;
        imc_ticks_total += tick - imc_last_tick;
        imc_last_tick = tick;
        portDISABLE_INTERRUPTS();
        printf("(STAT) cycles in init(): %d\r\n", imc_ticks_init);
        printf("(STAT) cycles in getCSC(): %d\r\n", imc_ticks_getCSC);
        printf("(STAT) cycles in adjust(): %d\r\n", imc_ticks_adjust);
        printf("(STAT) total cycles: %d\r\n", imc_ticks_total);
        portENABLE_INTERRUPTS();
        imc_ticks_init = 0;
        imc_ticks_getCSC = 0;
        imc_ticks_adjust = 0;
        imc_ticks_total = 0;
    #endif

    portDISABLE_INTERRUPTS();
    printf("End benchmark (%d)\r\n", i++);
    portENABLE_INTERRUPTS();

    #if (imcUSE_IMC_KERNEL == 1)
        vImcInvalidateRecovery();
    #endif

    // imcREQUEST_CHECKPOINT();


    #if (imcBENCH_INFINITE_LOOP)
    }
    #endif


}