#include "CanDataHandler.h"
#include "string.h"

#ifndef MAX_DATA_STORAGE
#define MAX_DATA_STORAGE 100
#endif

#ifndef MAX_EVENT_STORAGE
#define MAX_EVENT_STORAGE 100
#endif

#ifndef MAX_FILTER_SLOTS
#define MAX_FILTER_SLOTS 100
#endif

// storage for upto MAX_DATA_STORAGE diffrent messages
static CanDataMessageSlot CanDataStore[MAX_DATA_STORAGE] = {0};

// storage for upto MAX_EVENT_STORAGE diffrent event handlers
static CanDataEventSlot CanEventStore[MAX_DATA_STORAGE] = {0};

// storage for upto MAX_FILTER_SLOTS diffrent can filters
static CanDataFilterSlot CanFilterSlots[MAX_FILTER_SLOTS] = {0};

// this will setup the canfilter accoridng to the settings
void setup_StmCanFilter(const CanDataFilterSlot* filterSetting ) {
    size_t f_index =  filterSetting - CanFilterSlots;// index of the filter in CanFilterSlots

    CLS_BSP_CAN_UniversalFilter clsFilterConfig = {
        .filterIndex = f_index,
        .filterDestination = filterSetting->fifo,
        .filterMode = CLS_BSP_CAN_FILTER_LIST,
        .id0 = filterSetting->id[0],
        .id1 = filterSetting->id[1],
    };

    CLS_BSP_CAN_SetUniversalFilter(&clsFilterConfig);
}

// this will setup the canfilter using a custom defined filter
void setup_StmCanFilterManual(const CanDataFilterSlot* filterSetting, CLS_BSP_CAN_UniversalFilter * filter ) {
    size_t f_index =  filterSetting - CanFilterSlots;// index of the filter in CanFilterSlots
    filter->filterIndex = f_index;
    CLS_BSP_CAN_SetUniversalFilter(filter);
}


void CanDataHandler_init(void) {
    // Reset all data message slots
    for (int i = 0; i < MAX_DATA_STORAGE; i++) {
        CanDataStore[i].msg.canid = 0;
        CanDataStore[i].msg.data_length = 0;
        memset(CanDataStore[i].msg.data, 0, MAX_DATA_LENGTH);
        CanDataStore[i].filter.ref = NULL;
        CanDataStore[i].filter.index = 0;
    }

    // Reset all event message slots
    for (int i = 0; i < MAX_EVENT_STORAGE; i++) {
        CanEventStore[i].canid = 0;
        CanEventStore[i].eventCall = NULL;
        CanEventStore[i].filter.ref = NULL;
        CanEventStore[i].filter.index = 0;
    }

    // Reset all filter slots
    for (int i = 0; i < MAX_FILTER_SLOTS; i++) {
        CanFilterSlots[i].free = CANDATA_ALLFREE;
        CanFilterSlots[i].fifo = CANDATA_FIFOX_UNDEFIEND;
        CanFilterSlots[i].id[0] = 0;
        CanFilterSlots[i].id[1] = 0;
    }
}


/**
 * Finds and returns an unused filter slot. If no unused slot is found, returns NULL.
 */
CanDataFilterRef CanData_unusedFilterSlot(CanDataSlotFifo reqest_fifo) {
    CanDataFilterRef filterRef = {NULL, 0};

    for (size_t i = 0; i < MAX_FILTER_SLOTS; i++) {
        // find a slot with free some id 
        if (CanFilterSlots[i].free < CANDATA_FULL) {

            // either the fifo settig matches or is undefined
            if(CanFilterSlots[i].fifo == reqest_fifo  || CanFilterSlots[i].fifo == CANDATA_FIFOX_UNDEFIEND) {
                filterRef.ref = &CanFilterSlots[i];
                if (CanFilterSlots[i].fifo == CANDATA_FIFOX_UNDEFIEND) {
                    CanFilterSlots[i].fifo = reqest_fifo;
                }

                // Determine the index based on the 'free' field
                switch (CanFilterSlots[i].free) {
                    case CANDATA_ALLFREE:
                        filterRef.index = 0;
                        CanFilterSlots[i].free = CANDATA_FREE1;
                        break;
                    case CANDATA_FREE1:
                        filterRef.index = 1;
                        CanFilterSlots[i].free = CANDATA_FULL;
                        break;
                    case CANDATA_FREE0:
                        filterRef.index = 0;
                        CanFilterSlots[i].free = CANDATA_FULL;
                        break;
                    default:
                        break;
                }

            break;
            }
            
        } 
    }
    return filterRef;
}

void CanData_clearFilterSlot(CanDataFilterRef filter) {

        // reset the filter can id
        filter.ref->id[filter.index] = 0;

        // set the slot as free
        if(filter.index == 0) {
            filter.ref->free = filter.ref->free & CANDATA_FREE0;
        } else if( filter.index == 1) {
            filter.ref->free = filter.ref->free & CANDATA_FREE1;
        }

        // if the filterslot is now fully free reset the fifo settings
        if(filter.ref->free == CANDATA_ALLFREE) {
            filter.ref->fifo = CANDATA_FIFOX_UNDEFIEND;
        }

        
        // finaly write new HW config
        setup_StmCanFilter(filter.ref);
}

/**
 * Finds and returns an unused data message slot. If no unused slot is found, returns NULL.
 */
CanDataMessageSlot * CanData_unusedDataSlot() {
    for (size_t i = 0; i < MAX_DATA_STORAGE; i++) {
        if (CanDataStore[i].msg.canid == 0) {
            return &CanDataStore[i];
        }
    }
    return NULL; // No unused slot found
}

/**
 * Finds and returns an unused event slot. If no unused slot is found, returns NULL.
 */
CanDataEventSlot * CanData_unusedEventSlot() {
    for (size_t i = 0; i < MAX_EVENT_STORAGE; i++) {
        if (CanEventStore[i].canid == 0) {
            return &CanEventStore[i];
        }
    }
    return NULL; // No unused slot found
}

/**
 * Registers a data message with given CanDataId. Returns true if registration is successful, false otherwise.
 */
bool CanData_regDataMsg(CanDataId canid) {
    
    // check if we already have this canid to avoid duplicates
    if(CanData_getDataMessage(canid) != NULL) {
        return true; // but return true beause there is already a vaild entry for this canid
    }

    // Find an unused slot in the data store
    CanDataMessageSlot *dataSlot = CanData_unusedDataSlot();
    if (!dataSlot) {
        return false; // No unused data slot available
    }

    // Find and configure an unused slot for the data message
    CanDataFilterRef filter = CanData_unusedFilterSlot(CANDATA_FIFO0_DATA);
    if (filter.ref == NULL) {
        return false; // No unused filter slot available
    }

    // Configure the filter slot
    filter.ref->id[filter.index] = canid;

    // Set up the filter with the new settings
    setup_StmCanFilter(filter.ref);


    // Register the data message with the data slot
    dataSlot->msg.canid = canid;
    dataSlot->filter = filter; // Store the pointer to the filter slot

    return true;
}


// Function to register a Set of ID for Data Messages using a Manual filter
size_t CanData_regDataManualMsg(const CanDataId* canid, size_t id_count, CLS_BSP_CAN_UniversalFilter * filter) {
    // first check all id to be new
    for (size_t i = 0; i < id_count; i++) {
        if(CanData_getDataMessage(canid[i]) != NULL) {
            return 0; // invalid operation one id is already used
        }
    }

    // Find and configure one unused slot for the manual filter
    CanDataFilterRef filterRef = CanData_unusedFilterSlot(CANDATA_FIFOX_MANUAL);
    if (filterRef.ref == NULL) {
        return 0; // No unused filter slot available
    }

    // setup the manual filters
    filterRef.ref->fifo = CANDATA_FIFOX_MANUAL;
    filterRef.ref->free = CANDATA_MANUAL;
    setup_StmCanFilterManual(filterRef.ref, filter);


    for (size_t i = 0; i < id_count; i++) {
        // Find an unused slot in the data store
        CanDataMessageSlot *dataSlot = CanData_unusedDataSlot();
        if (!dataSlot) {
            return i; // No unused data slot available
        }
        dataSlot->msg.canid = canid[i];
        dataSlot->filter = filterRef;
        dataSlot->filter.index = -1; // this idecates is a manual filter
    }

    return id_count;
}

/**
 * Registers a event message with given CanDataId. Returns true if registration is successful, false otherwise.
 */
bool CanData_regEventMsg(CanDataId canid, EventCallback event_callback) {

    // Check for vaild callback
    if(event_callback == NULL) {
        return false;
    }

    // Check if the canid is already registered
    for (int i = 0; i < MAX_EVENT_STORAGE; i++) {
        if (CanEventStore[i].canid == canid) {
            return false; // The canid is already registered
        }
    }

    // Find an unused slot in the event store
    CanDataEventSlot *eventSlot = CanData_unusedEventSlot();
    if (eventSlot == NULL) {
        return false; // No unused event slot available
    }

    // Find and configure an unused slot for the data message
    CanDataFilterRef filter = CanData_unusedFilterSlot(CANDATA_FIFO1_EVENT);
    if (filter.ref == NULL) {
        return false; // No unused filter slot available
    }

    // Configure the filter slot
    filter.ref->id[filter.index] = canid;

    // Set up the filter with the new settings
    setup_StmCanFilter(filter.ref);

    // Register the event message with the event slot
    eventSlot->canid = canid;
    eventSlot->eventCall = event_callback;
    eventSlot->filter = filter; // Store the pointer to the filter slot

    return true;
}

// Function to remove a CAN ID from Event Messages Register
bool CanData_removeEvent(CanDataId canid) {
    // Search for the event slot with the given CanDataId
    for (size_t i = 0; i < MAX_EVENT_STORAGE; i++) {
        if (CanEventStore[i].canid == canid) {
            CanDataEventSlot * eventSlot = &CanEventStore[i];
            CanDataFilterRef  filter = eventSlot->filter; 

            // reset the CanDataEventSlot
            eventSlot->canid =0;
            eventSlot->eventCall = NULL;
            eventSlot->filter.index =0;
            eventSlot->filter.ref = NULL;


            // reset the filter
            CanData_clearFilterSlot(filter);

            return true; 
        }
    }

    return false;
}




// Function to insert a Data Message into the array
void CanData_insertDataMessage(CanDataId canid, uint8_t* data, uint8_t data_length) {
    if(data == NULL || data_length == 0 ) {
        return;
    }

    // Search for the data slot with the given CanDataId
    for (size_t i = 0; i < MAX_DATA_STORAGE; i++) {
        if (CanDataStore[i].msg.canid == canid) {
            // If found, update the data message in the slot
            CanDataStore[i].msg.data_length = data_length;
            memcpy(CanDataStore[i].msg.data, data, data_length);
            return;
        }
    }
}

// Function to read stored Data Message based on CAN ID
// Returns a const pointer to the stored data or NULL if not found
const CanDataMessage* CanData_getDataMessage(CanDataId canid) {
    // Search for the data message with the given CanDataId
    for (size_t i = 0; i < MAX_DATA_STORAGE; i++) {
        if (CanDataStore[i].msg.canid == canid) {
            return &CanDataStore[i].msg; // Return a pointer to the found data message
        }
    }
    return NULL; // No data message with the given CanDataId found
}



// Function to handle reception of Data Messages from FIFO0
void CanData_canFifo0RxCallback(CanDataId canid, uint8_t* data, uint8_t len) {
    // Insert the data message into the data store
    CanData_insertDataMessage(canid, data, len);
}

// Function to handle reception of Event Messages from FIFO1
void CanData_canFifo1RxCallback(CanDataId canid, uint8_t* data, uint8_t len) {
    // Search for the event slot with the given CanDataId
    for (size_t i = 0; i < MAX_EVENT_STORAGE; i++) {
        if (CanEventStore[i].canid == canid) {
            // If found, call the event callback with the received data
            CanEventStore[i].eventCall(canid ,data, len);
            return;
        }
    }
}