/**
 * @file Power.h
 * @author Phillip Kühne
 * @brief This component provides utilities for keeping track of power usage
 * consumption.
 * @version 0.1
 * @date 2024-11-23
 */

#include "Power.h"
static portMUX_TYPE mux;

Power::Power()
{
    this->freePowerBudget = this->totalPowerBudget;
}

void Power::begin()
{
    if (powerInstance == nullptr)
    {
        // Double check locking https://www.aristeia.com/Papers/DDJ%5FJul%5FAug%5F2004%5Frevised.pdf
        taskENTER_CRITICAL(&mux);
        if (powerInstance == nullptr)
        {
            powerInstance = new Power();
        }
        taskEXIT_CRITICAL(&mux);
    }
    else
    {
        // Power.begin() was called twice!
        Serial.println("Power.begin() was called twice! No harm done, but this is a bug.");
    }
}

Power *Power::getPowerManager()
{
    return powerInstance;
}

bool Power::tryAccquirePowerAllowance(uint16_t neededPower)
{
    if (this->freePowerBudget >= neededPower)
    {
        this->freePowerBudget -= neededPower;
        return true;
    }
    else
    {
        return false;
    }
}

void Power::releasePower(uint16_t power)
{
    if (this->freePowerBudget + power <= this->totalPowerBudget)
    {
        this->freePowerBudget += power;
    }
    else // TODO: Maybe we should actually throw an error here, since obviouslsy someone used us wrong.
    {
        this->freePowerBudget = this->totalPowerBudget;
    }
    // Check if there are tasks waiting for power
    checkWaitingTasks();
}

bool Power::waitForPowerAllowance(uint16_t neededPower, TickType_t ticksToWait)
{
    if (tryAccquirePowerAllowance(neededPower))
    {
        return true;
    }
    else
    {
        // Suspend the task while waiting for power to be available
        TaskHandle_t currentTask = xTaskGetCurrentTaskHandle();
        TickType_t initialTickCount = xTaskGetTickCount();
        waitingTasks.push(currentTask);
        uint32_t notificationValue;
        BaseType_t notificationStatus = xTaskNotifyWait(0, 0, &notificationValue, ticksToWait);
        // Code below will be executed after the task is woken up
        while (notificationStatus == pdPASS)
        {
            if (notificationValue == POWER_AVAILABLE)
            {
                // We were woken up because new power is available, check if it is enough
                if (tryAccquirePowerAllowance(neededPower))
                {
                    return true;
                }
                else
                {
                    // Still not enough power available for us. Wait the remaining ticks.
                    xTaskNotifyWait(0, 0, &notificationValue, ticksToWait - (xTaskGetTickCount() - initialTickCount));
                }
            }
        }
        if (notificationStatus == pdFALSE)
        {
            // We waited long enough...
            return false;
        }
        else
        {
            // Should be impossible to reach
            throw "Reached impossible state";
        }
    }
}

void Power::checkWaitingTasks(void)
{
    // Check if there are tasks waiting for power
    if (!waitingTasks.empty())
    {
        TaskHandle_t task = waitingTasks.front();
        waitingTasks.pop();
        xTaskNotify(task, POWER_AVAILABLE, eSetValueWithOverwrite);
    }
}