/*
 * Copyright 2025 NXP
 * All rights reserved.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */

#include <fastestdet_postprocess.h>
#include <model_config.h>
#include "eiq_app.h"
#include "fsl_debug_console.h"
#include "model.h"
#include "multicore_coms.h"
#include "profile_pins.h"

/*******************************************************************************
 * Variables
 ******************************************************************************/
tensor_dims_t inputDims;
tensor_type_t inputType;
uint8_t* inputData;
tensor_dims_t outputDims;
tensor_type_t outputType;
uint8_t* outputData;
box_data obj_boxes[MAX_BOXES];

/*******************************************************************************
 * Functions
 ******************************************************************************/
void eIQApp_Init()
{
    PRINTF("****************************************************************\r\n");
    // Real Core System Clock as reported by the system
    PRINTF("CoreSysClkFreq: %d MHz, %d MHz\r\n", CLOCK_GetCoreSysClkFreq()/1000000, CLOCK_GetFreq(kCLOCK_CoreSysClk)/1000000);
    // This is the indented SystemCoreClock used for initialization
    PRINTF("SystemCoreClock (default init):   %d MHz\r\n", SystemCoreClock/1000000);
    SystemCoreClockUpdate();
    // This is the actual SystemCoreClock after initialization
    PRINTF("SystemCoreClock (system updated): %d MHz\r\n", SystemCoreClock/1000000);

    if (MODEL_Init() != kStatus_Success)
    {
        PRINTF("Failed initializing model\r\n");
        for (;;) {}
    }

    inputData = MODEL_GetInputTensorData(&inputDims, &inputType);
    outputData = MODEL_GetOutputTensorData(&outputDims, &outputType);
}

void eIQApp_Inference_Loop()
{
	multicore_coms_msg frame_data = {0};
    MCC_Send(&frame_data, 1);
    INIT_CORE0_PROFILE_PIN();

    while (1)
    {
        SET_CORE0_PROFILE_PIN();

    	MCC_Rcv(&frame_data, 1);
		memcpy(inputData, (uint8_t *)frame_data.aImgRGB, MODEL_WIDTH*MODEL_HEIGHT*3);
        MODEL_RunInference();
        Fastestdet_Postprocess((int8_t *)outputData, obj_boxes);
        MCC_Send(&frame_data, 1);

        CLR_CORE0_PROFILE_PIN();

        for (uint32_t i = 0 ; i < MAX_BOXES; i++)
        {
            if (obj_boxes[i].area > 0)  // Confidence threshold
            {
                PRINTF("Detected: Box[%d] - Score: %f, t: %d, b: %d, l: %d, r: %d\r\n",
                       i, obj_boxes[i].score, obj_boxes[i].top, obj_boxes[i].bottom, obj_boxes[i].left, obj_boxes[i].right);
                obj_boxes[i].area = 0;
            }
        }
    }
}
