#include "dtc.h"
#include "debugger.h"

DTCEntry_t DTCStorage[MAX_DTC_STORAGE];

void MaintainDTC(DTCNum_t DTC_no, boolean active, uint32_t DebugValue)
{
    for (int i = 0; i < MAX_DTC_STORAGE; i++)
    {
        if (DTCStorage[i].Number == DTC_no)
        {
            if (active && DTCStorage[i].active != DTC_ACTIVE)
            {
                Debug_pushMessage("DTC gone active: %d, DebugVal: %d\n", DTC_no, DebugValue);
                DTCStorage[i].timestamp = millis();
                DTCStorage[i].active = DTC_ACTIVE;
                DTCStorage[i].debugVal = DebugValue;
            }
            if (!active && DTCStorage[i].active == DTC_ACTIVE)
            {
                Debug_pushMessage("DTC gone previous: %d\n", DTC_no);
                DTCStorage[i].active = DTC_PREVIOUS;
            }
            return;
        }
    }

    // DTC was not found with upper iteration, but is active
    // so we need to look for free space to store DTC
    if (active == true)
    {
        for (int i = 0; i < MAX_DTC_STORAGE; i++)
        {
            if (DTCStorage[i].Number == DTC_LAST_DTC)
            {
                Debug_pushMessage("new DTC registered: %d, DebugVal: %d\n", DTC_no, DebugValue);
                DTCStorage[i].Number = DTC_no;
                DTCStorage[i].timestamp = millis();
                DTCStorage[i].active = DTC_ACTIVE;
                DTCStorage[i].debugVal = DebugValue;
                return;
            }
        }
    }
}

void ClearDTC(DTCNum_t DTC_no)
{
    for (int i = 0; i < MAX_DTC_STORAGE; i++)
    {
        if (DTCStorage[i].Number == DTC_no)
        {
            DTCStorage[i].Number = DTC_LAST_DTC;
            DTCStorage[i].active = DTC_INACTIVE;
            DTCStorage[i].timestamp = 0;
        }
    }
}

void ClearAllDTC()
{
    for (int i = 0; i < MAX_DTC_STORAGE; i++)
    {
        DTCStorage[i].Number = DTC_LAST_DTC;
        DTCStorage[i].active = DTC_INACTIVE;
        DTCStorage[i].timestamp = 0;
    }
}

DTCNum_t getlastDTC(boolean only_active)
{
    int8_t pointer = -1;
    uint32_t lasttimestamp = 0;

    for (int i = 0; i < MAX_DTC_STORAGE; i++)
    {
        if (DTCStorage[i].Number > 0 && DTCStorage[i].timestamp > lasttimestamp)
        {
            if (only_active == false || DTCStorage[i].active == DTC_ACTIVE)
            {
                pointer = i;
                lasttimestamp = DTCStorage[i].timestamp;
            }
        }
    }

    return pointer >= 0 ? DTCStorage[pointer].Number : DTC_LAST_DTC;
}

DTCSeverity_t getSeverityForDTC(DTCNum_t targetCode)
{
    for (int i = 0; i < DTC_LAST_DTC; i++)
    {
        if (dtc_definitions[i].code == targetCode)
        {
            return dtc_definitions[i].severity;
        }
    }
    return DTC_NONE;
}

void DTC_Process()
{
    static tSystem_Status preserverSysStatusError;
    DTCNum_t lastDTC = getlastDTC(true);

    if (lastDTC < DTC_LAST_DTC)
    {
        globals.hasDTC = true;

        if (getSeverityForDTC(lastDTC) == DTC_CRITICAL && globals.systemStatus != sysStat_Shutdown)
        {
            if (globals.systemStatus != sysStat_Error)
            {
                preserverSysStatusError = globals.systemStatus;
            }
            globals.systemStatus = sysStat_Error;
        }
    }
    else
    {
        globals.hasDTC = false;
        
        if (globals.systemStatus == sysStat_Error)
        {
            globals.systemStatus = preserverSysStatusError;
        }
    }
}