#include "system_task.h"


SystemTask::SystemTask(ServiceManager& manager)
:
serviceManager(manager),
taskHandle(nullptr)
{

}



// xTaskCreatePinnedToCore(
//     function,
//     name,
//     stack size,
//     parameter,
//     priority,
//     handle,
//     core
// );
void SystemTask::start()
{
    xTaskCreatePinnedToCore(
        taskEntry,
        "SystemTask",
        4096,
        this,
        2,
        &taskHandle,
        1
    );
}


void SystemTask::taskEntry(void* parameter)
{
    SystemTask* task =
        static_cast<SystemTask*>(parameter);

    task->run();
}


void SystemTask::run()
{
    serviceManager.begin();

    TickType_t lastWakeTime =
        xTaskGetTickCount();


    while(true)
    {
        // Time the length of the task:
        uint32_t start = micros();
        serviceManager.tick();
        uint32_t elapsed = micros() - start;


        vTaskDelayUntil(
            &lastWakeTime,
            pdMS_TO_TICKS(10)
        );
    }
}