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

DTCEntry_s DTCStorage[MAX_DTC_STORAGE];

void MaintainDTC(DTCNums_t DTC_no, DTCSeverity_t DTC_severity, 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].severity = DTC_severity;
                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;
                DTCStorage[i].severity = DTC_severity;
                return;
            }
        }
    }
}

void ClearDTC(DTCNums_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_NONE;
            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_NONE;
        DTCStorage[i].timestamp = 0;
    }
}

DTCNums_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;
}

DTCNums_t getlastDTC_Severity(boolean only_active, DTCSeverity_t severity)
{
    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) && DTCStorage[i].severity == severity)
            {
                pointer = i;
                lasttimestamp = DTCStorage[i].timestamp;
            }
        }
    }

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