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

#include "usb_host_config.h"
#include "usb_host.h"
#include "fsl_device_registers.h"
#include "usb_host_video.h"
#include "host_video.h"
#include "fsl_common.h"
#include "board.h"
#if (defined(FSL_FEATURE_SOC_SYSMPU_COUNT) && (FSL_FEATURE_SOC_SYSMPU_COUNT > 0U))
#include "fsl_sysmpu.h"
#endif /* FSL_FEATURE_SOC_SYSMPU_COUNT */
#include "app.h"

#if ((!USB_HOST_CONFIG_KHCI) && (!USB_HOST_CONFIG_EHCI) && (!USB_HOST_CONFIG_OHCI) && (!USB_HOST_CONFIG_IP3516HS))
#error Please enable USB_HOST_CONFIG_KHCI, USB_HOST_CONFIG_EHCI, USB_HOST_CONFIG_OHCI, or USB_HOST_CONFIG_IP3516HS in file usb_host_config.
#endif

//////////////////////////////////////////////////////
#include "fsl_debug_console.h"
#include "pin_mux.h"
#include "clock_config.h"

#include "vglite_support.h"
#include "vglite_window.h"
#include "vg_lite.h"

#include "fsl_gpio.h"
#include "display_support.h"

#if defined(CPU_MIMXRT798SGFOA_cm33_core0)
#include "fsl_lcdif.h"
#endif

#include "eiq_app.h"
#include "timer.h"

#include <model_config.h>
#include <user_action_in.h>
#include <user_action_out.h>
/*******************************************************************************
 * Definitions
 ******************************************************************************/
#define APP_BUFFER_COUNT 2
#define DEFAULT_SIZE     256.0f;

/*******************************************************************************
 * Prototypes
 ******************************************************************************/
static void vglite_task(void *pvParameters);
extern int init_buffer(vg_lite_buffer_t * buffer, vg_lite_buffer_format_t fmt, int width,int height, int stride, const uint8_t *mem_source);
/*******************************************************************************
 * Variables
 ******************************************************************************/
// Variables for drawing boxes and determining in/out
static int initial_top = -1;
static int prev_top = -1;
static vg_lite_buffer_t inf_res_out_img_buffer;

static vg_lite_display_t display;
static vg_lite_window_t window;

static int zoomOut    = 0;
static int scaleCount = 0;
static vg_lite_matrix_t matrix;
volatile bool s_frameDone = false;

#if (CUSTOM_VGLITE_MEMORY_CONFIG != 1)
#error "Application must be compiled with CUSTOM_VGLITE_MEMORY_CONFIG=1"
#else
#define VGLITE_COMMAND_BUFFER_SZ (128 * 1024)
/* On RT595S */
#if defined(CPU_MIMXRT595SFFOC_cm33) || defined(CPU_MIMXRT798SGFOA_cm33_core0)
#define VGLITE_HEAP_SZ 0x100000 /* 1 MB */
/* On RT1170 */
#elif defined(CPU_MIMXRT1176DVMAA_cm7) || defined(CPU_MIMXRT1166DVM6A_cm7)
#define VGLITE_HEAP_SZ 8912896 /* 8.5 MB */
#else
#error "Unsupported CPU !"
#endif
#if (720 * 1280 == (DEMO_PANEL_WIDTH) * (DEMO_PANEL_HEIGHT))
#define TW 720
/* On RT595S */
#if defined(CPU_MIMXRT595SFFOC_cm33)
/* Tessellation window = 720 x 128 */
#define TH 128
/* On RT798S */
#elif defined(CPU_MIMXRT798SGFOA_cm33_core0)
/* Tessellation window = 720 x 640 */
#define TH 640
/* On RT1170 */
#elif defined(CPU_MIMXRT1176DVMAA_cm7) || defined(CPU_MIMXRT1166DVM6A_cm7)
/* Tessellation window = 720 x 1280 */
#define TH 1280
#else
#error "Unsupported CPU !"
#endif
/* Panel RM67162. Supported only by platform RT595S. */
#elif (400 * 400 == (DEMO_PANEL_WIDTH) * (DEMO_PANEL_HEIGHT))
/* Tessellation window = 400 x 400 */
#define TW 400
#define TH 256
#else
/* Tessellation window = 256 x 256 */
#define TW 256
#define TH 256
#endif
/* Allocate the heap and set the command buffer(s) size */
AT_NONCACHEABLE_SECTION_ALIGN(uint8_t vglite_heap[VGLITE_HEAP_SZ], 64);

void *vglite_heap_base        = &vglite_heap;
uint32_t vglite_heap_size     = VGLITE_HEAP_SZ;
#endif

usb_host_handle g_HostHandle;
extern usb_host_video_camera_instance_t g_Video;
/*******************************************************************************
 * Prototypes
 ******************************************************************************/
/*!
 * @brief host callback function.
 *
 * device attach/detach callback function.
 *
 * @param deviceHandle          device handle.
 * @param configurationHandle   attached device's configuration descriptor information.
 * @param eventCode             callback event code, please reference to enumeration host_event_t.
 *
 * @retval kStatus_USB_Success              The host is initialized successfully.
 * @retval kStatus_USB_NotSupported         The application don't support the configuration.
 */
static usb_status_t USB_HostEvent(usb_device_handle deviceHandle,
                                  usb_host_configuration_handle configurationHandle,
                                  uint32_t eventCode);

/*!
 * @brief application initialization.
 */
static void USB_HostApplicationInit(void);

/*!
 * @brief host freertos task function.
 *
 * @param g_HostHandle   host handle
 */
static void USB_HostTask(void *param);

/*!
 * @brief host video freertos task function.
 *
 * @param param   the host video instance pointer.
 */
static void USB_HostApplicationTask(void *param);

extern void USB_HostClockInit(void);
extern void USB_HostIsrEnable(void);
extern void USB_HostTaskFn(void *param);
void BOARD_InitHardware(void);
/*******************************************************************************
 * Code
 ******************************************************************************/

/*!
 * @brief host callback function.
 *
 * device attach/detach callback function.
 *
 * @param deviceHandle           device handle.
 * @param configurationHandle attached device's configuration descriptor information.
 * @param eventCode           callback event code, please reference to enumeration host_event_t.
 *
 * @retval kStatus_USB_Success              The host is initialized successfully.
 * @retval kStatus_USB_NotSupported         The application don't support the configuration.
 */
static usb_status_t USB_HostEvent(usb_device_handle deviceHandle,
                                  usb_host_configuration_handle configurationHandle,
                                  uint32_t eventCode)
{
    usb_status_t status = kStatus_USB_Success;

    switch (eventCode & 0x0000FFFFU)
    {
        case kUSB_HostEventAttach:
            status = USB_HostVideoEvent(deviceHandle, configurationHandle, eventCode);
            break;

        case kUSB_HostEventNotSupported:
            usb_echo("device not supported.\r\n");
            break;

        case kUSB_HostEventEnumerationDone:
            status = USB_HostVideoEvent(deviceHandle, configurationHandle, eventCode);
            break;

        case kUSB_HostEventDetach:
            status = USB_HostVideoEvent(deviceHandle, configurationHandle, eventCode);
            break;

        case kUSB_HostEventEnumerationFail:
            usb_echo("enumeration failed\r\n");
            break;

        default:
            break;
    }
    return status;
}

static void USB_HostApplicationInit(void)
{
    usb_status_t status = kStatus_USB_Success;

    USB_HostClockInit();

#if ((defined FSL_FEATURE_SOC_SYSMPU_COUNT) && (FSL_FEATURE_SOC_SYSMPU_COUNT))
    SYSMPU_Enable(SYSMPU, 0);
#endif /* FSL_FEATURE_SOC_SYSMPU_COUNT */

    status = USB_HostInit(CONTROLLER_ID, &g_HostHandle, USB_HostEvent);
    if (status != kStatus_USB_Success)
    {
        usb_echo("host init error\r\n");
        return;
    }
    USB_HostIsrEnable();

    usb_echo("host init done\r\n");
}

static void USB_HostTask(void *param)
{
    while (1)
    {
        USB_HostTaskFn(param);
    }
}

static void USB_HostApplicationTask(void *param)
{
    while (1)
    {
        USB_HostVideoTask(param);
    }
}

int main(void)
{
    BOARD_InitHardware();
	eIQApp_Init();
    USB_HostApplicationInit();

    if (xTaskCreate(vglite_task, "vglite_task", configMINIMAL_STACK_SIZE + 2000, NULL, configMAX_PRIORITIES - 1, NULL) != pdPASS)
    {
        usb_echo("create vglite task error\r\n");
        while (1)
            ;
    }

    if (xTaskCreate(USB_HostTask, "usb host task", 2500L / sizeof(portSTACK_TYPE), g_HostHandle, 4, NULL) != pdPASS)
    {
        usb_echo("create host task error\r\n");
    }
    if (xTaskCreate(USB_HostApplicationTask, "app task", 2500L / sizeof(portSTACK_TYPE), &g_Video, 3, NULL) != pdPASS)
    {
        usb_echo("create video task error\r\n");
    }
    if (xTaskCreate(CPU_LoadMonitor_Task, "cpu load task", 6000L / sizeof(portSTACK_TYPE), NULL, 1, NULL) != pdPASS)
    {
        usb_echo("CPU_LoadMonitor_Task error\r\n");
    }

    vTaskStartScheduler();
    while (1)
    {
        ;
    }
}

static vg_lite_error_t init_vg_lite(void)
{
    vg_lite_error_t error = VG_LITE_SUCCESS;
    int fb_width, fb_height;

    error = VGLITE_CreateDisplay(&display);
    if (error)
    {
        PRINTF("VGLITE_CreateDisplay failed: VGLITE_CreateDisplay() returned error %d\r\n", error);
        return error;
    }
    // Initialize the window.
    error = VGLITE_CreateWindow(&display, &window);
    if (error)
    {
        PRINTF("VGLITE_CreateWindow failed: VGLITE_CreateWindow() returned error %d\r\n", error);
        return error;
    }
    // Set GPU command buffer size for this drawing task.
    error = vg_lite_set_command_buffer_size(VGLITE_COMMAND_BUFFER_SZ);
    if (error)
    {
        PRINTF("vg_lite_set_command_buffer_size() returned error %d\n", error);
        return error;
    }
    // Initialize the draw.
    error = vg_lite_init(TW, TH);
    if (error)
    {
        PRINTF("vg_lite engine init failed: vg_lite_init() returned error %d\r\n", error);
        return error;
    }

    // Setup a scale at center of buffer.
    fb_width  = window.width;
    fb_height = window.height;
    vg_lite_identity(&matrix);
    vg_lite_translate(fb_width / 2 - 20 * fb_width / 640.0f, fb_height / 2 - 100 * fb_height / 480.0f, &matrix);
    vg_lite_scale(2, 2, &matrix);
    vg_lite_scale(fb_width / 640.0f, fb_height / 480.0f, &matrix);

    return error;
}

void gpu_blit(vg_lite_buffer_t *rt, vg_lite_buffer_t * source, bool clear, vg_lite_blend_t blend, uint32_t dest_off_x, uint32_t dest_off_y)
{
    vg_lite_buffer_t *blitBuffer = source;
    int8_t error = 0;

    if (rt == NULL)
    {
        PRINTF("vg_lite_get_renderTarget error\r\n");
        while (1)
            ;
    }

#if CLEAR_COLOR_STRIPE
    vg_lite_rectangle_t rectangle;
    rectangle.width = DEMO_BUFFER_WIDTH / 3;
    rectangle.height = DEMO_BUFFER_HEIGHT / 3;

    rectangle.x = 0;
    rectangle.y = 0;
    vglite_error = vg_lite_clear(target, &rectangle, 0x000000FF);
#endif
    if (clear)
    {
       vg_lite_clear(rt, NULL, 0x005F2F2F);
       vg_lite_finish();
    }
    vg_lite_identity(&matrix);

#if ROTATE_LANDSCAPE_TO_PORTRAIT
    //vg_lite_translate(DEMO_PANEL_WIDTH / 2 , DEMO_PANEL_HEIGHT / 2 , &matrix);
    vg_lite_translate(DEMO_PANEL_WIDTH , 0 , &matrix);
    vg_lite_rotate(90, &matrix);
    //vg_lite_scale(0.5, 0.5, &matrix);
#endif

    vg_lite_translate(dest_off_x , dest_off_y , &matrix);
    error = vg_lite_blit(rt, blitBuffer, &matrix, blend, 0, VG_LITE_FILTER_POINT);
    if (error)
    {
      PRINTF("Error****%d:%s\n", __LINE__, __func__);
      return;
    }

    vg_lite_finish();

    return;
}

void draw_rectangles(vg_lite_buffer_t *rt, box_data *final_boxes,  bool clear_boxes) {
	uint32_t box_counter = 0;

	/* other rectangles show detected objects */
	for (uint32_t i = 0; i < MAX_BOXES && box_counter < MAX_BOXES_TO_DRAW; i++) {
		if (final_boxes[i].area == 0)
			continue;

		/* input tensor preview is scaled and moved to fit on screen, and so its bounding boxes */
		int left = (int)((final_boxes[i].left * MATCH_RES_W)/ MODEL_WIDTH);
		int right = (int)((final_boxes[i].right * MATCH_RES_W)/ MODEL_WIDTH);
		int bottom = (int)((final_boxes[i].bottom * MATCH_RES_H)/MODEL_HEIGHT);
		int top = (int)((final_boxes[i].top * MATCH_RES_H)/MODEL_HEIGHT);
		box_counter++;

		if (initial_top == -1) {
			initial_top = top;
		}

		prev_top = top;

		if (clear_boxes)
			final_boxes[i].area = 0;

		// draw rectangles on buffer
	    vg_lite_rectangle_t rect_top = { left, top, right-left, 3 };       // top
	    vg_lite_clear(rt, &rect_top, 0xFF0000FF);
	    vg_lite_rectangle_t rect_right = { right, top, 3, bottom-top };    // right
	    vg_lite_clear(rt, &rect_right, 0xFF0000FF);
	    vg_lite_rectangle_t rect_bottom = { left, bottom, right-left, 3 }; // bottom
	    vg_lite_clear(rt, &rect_bottom, 0xFF0000FF);
	    vg_lite_rectangle_t rect_left = { left, top, 3, bottom-top };      // left
	    vg_lite_clear(rt, &rect_left, 0xFF0000FF);
	    break;
	}

	if (box_counter == 0) {
		if (initial_top != -1) {
			if (prev_top > (MODEL_HEIGHT/2)) {
				init_buffer(&inf_res_out_img_buffer, VG_LITE_RGB888, MODEL_WIDTH, MODEL_HEIGHT, MODEL_WIDTH*3, user_action_in);
			} else {
				init_buffer(&inf_res_out_img_buffer, VG_LITE_RGB888, MODEL_WIDTH, MODEL_HEIGHT, MODEL_WIDTH*3, user_action_out);
			}

		}

		initial_top = -1;
		prev_top = -1;
	}
}

void redraw_img(vg_lite_buffer_t * source_buffer1, vg_lite_buffer_t * source_buffer2, vg_lite_buffer_t * source_buffer3, vg_lite_buffer_t * source_buffer4, box_data *boxes, bool clear_boxes)
{
    vg_lite_error_t error = VG_LITE_SUCCESS;
    uint8_t count;
    vg_lite_buffer_t *rt = VGLITE_GetRenderTarget(&window);
    if (rt == NULL)
    {
        PRINTF("vg_lite_get_renderTarget error\r\n");
        while (1)
            ;
    }

    // Clear screen to white.
    vg_lite_clear(rt, NULL, 0xFFFFFFFF);

    gpu_blit(rt, source_buffer1, 0, VG_LITE_BLEND_NONE, 0, 0);

    if (boxes) {
    	draw_rectangles(rt, boxes, clear_boxes);
    }

    gpu_blit(rt, &inf_res_out_img_buffer, 0, VG_LITE_BLEND_NONE, 0, 500);
    VGLITE_SwapBuffers(&window);

    return;
}

static void vglite_task(void *pvParameters)
{
    status_t status;
    vg_lite_error_t error;
    uint32_t n = 0, fps_x_1000;

    status = BOARD_PrepareVGLiteController();
    if (status != kStatus_Success)
    {
        PRINTF("Prepare VGlite contolor error\r\n");
        while (1)
            ;
    }

    error = init_vg_lite();
    if (error)
    {
        PRINTF("init_vg_lite failed: init_vg_lite() returned error %d\r\n", error);
        while (1)
            ;
    }

    vTaskDelete(NULL);
}
