// Copyright (C) 2024 The Qt Company Ltd.
// SPDX-License-Identifier: LicenseRef-Qt-Commercial

#include "camerainterface.h"
#include "camera_support.h"
#include "board.h"
#include "pin_mux.h"
#include "fsl_debug_console.h"
#include <cstdarg>

#define CAMERA_BUFFER_COUNT 3
#define CAMERA_FRAME_WIDTH 480
#define CAMERA_FRAME_HEIGHT 272
#define CAMERA_BPP 2

#define FAIL_IF(cond, msg) \
    do { \
        if (cond) { \
            DbgConsole_Printf(msg); \
            DbgConsole_Printf("\r\n"); \
            while (true) \
                ; \
        } \
    } while (false)

#define RETURN_FALSE_IF(cond, msg) \
    do { \
        if (cond) { \
            DbgConsole_Printf(msg); \
            DbgConsole_Printf("\r\n"); \
            return false; \
        } \
    } while (false)

#define RETURN_IF(cond, msg) \
    do { \
        if (cond) { \
            DbgConsole_Printf(msg); \
            DbgConsole_Printf("\r\n"); \
            return; \
        } \
    } while (false)

AT_NONCACHEABLE_SECTION_ALIGN(
    static uint16_t s_cameraBuffers[CAMERA_BUFFER_COUNT][CAMERA_FRAME_WIDTH][CAMERA_FRAME_HEIGHT], 64);

static void newCameraFrame(camera_receiver_handle_t *handle, status_t status, void *userData)
{
    uint32_t cameraFrameAddr;
    CameraInterface *me = static_cast<CameraInterface *>(userData);

    RETURN_IF(status != kStatus_Success, "Camera driver reported new frame with fail status!");

    status = CAMERA_RECEIVER_GetFullBuffer(handle, &cameraFrameAddr);
    RETURN_IF(status != kStatus_Success, "Failed to get camera buffer!");

    me->postEventFromInterrupt(FrameEvent{(uint8_t *) cameraFrameAddr});
}

bool CameraInterface::initCamera()
{
    status_t err;

    /* Set CSI interrupt priority to match configLIBRARY_MAX_SYSCALL_INTERRUPT_PRIORITY from our FreeRTOSConfig.h*/
    NVIC_SetPriority(CSI_IRQn, 2);

    BOARD_InitCSIPins();

    const camera_config_t cameraConfig = {
        .pixelFormat = kVIDEO_PixelFormatRGB565,
        .bytesPerPixel = CAMERA_BPP,
        .resolution = FSL_VIDEO_RESOLUTION(CAMERA_FRAME_WIDTH, CAMERA_FRAME_HEIGHT),
        .frameBufferLinePitch_Bytes = CAMERA_FRAME_WIDTH * CAMERA_BPP,
        .interface = kCAMERA_InterfaceGatedClock,
        .controlFlags = (kCAMERA_HrefActiveHigh | kCAMERA_DataLatchOnRisingEdge),
        .framePerSec = 30,
        .mipiChannel = 0,
        .csiLanes = 0,
        .fwaddress = 0,
    };

    BOARD_InitCameraResource();

    err = CAMERA_RECEIVER_Init(&cameraReceiver, &cameraConfig, newCameraFrame, this);
    RETURN_FALSE_IF(err != kStatus_Success, "Failed to initialize camera receiver!");

    err = CAMERA_DEVICE_Init(&cameraDevice, &cameraConfig);
    RETURN_FALSE_IF(err != kStatus_Success, "Failed to initialize camera device!");

    for (uint32_t i = 0; i < CAMERA_BUFFER_COUNT; i++) {
        err = CAMERA_RECEIVER_SubmitEmptyBuffer(&cameraReceiver, (uint32_t) (s_cameraBuffers[i]));
        RETURN_FALSE_IF(err != kStatus_Success, "Failed to submit camera buffer!");
    }

    return true;
}

void CameraInterface::startCamera()
{
    status_t err = CAMERA_RECEIVER_Start(&cameraReceiver);
    FAIL_IF(err != kStatus_Success, "Failed to start camera receiver!");
}

void CameraInterface::stopCamera()
{
    status_t err = CAMERA_RECEIVER_Stop(&cameraReceiver);
    FAIL_IF(err != kStatus_Success, "Failed to stop camera receiver!");
}

static void cleanup(uint8_t *memory)
{
    status_t err = CAMERA_RECEIVER_SubmitEmptyBuffer(&cameraReceiver, (uint32_t) (memory));
    FAIL_IF(err != kStatus_Success, "Failed to submit camera buffer");
}

void CameraInterface::onEvent(const FrameEvent &frameEvent)
{
    Qul::Image newFrame(frameEvent.newFrame,
                        CAMERA_FRAME_WIDTH,
                        CAMERA_FRAME_HEIGHT,
                        Qul::PixelFormat_RGB16,
                        -1,
                        cleanup);
    image.setValue(Qul::SharedImage(newFrame));
}

/* We use the platform function instead of SDK VIDEO_DelayMs because the DelayLoop() function in
 * NXP SDK code (fsl_common_arm.c) uses a non-local label in inline assembly, which is
 * incompatible with GCC LTO when that function gets inlined */
extern "C" void qul_delayMs(uint32_t ms);
void VIDEO_DelayMs(uint32_t ms)
{
    qul_delayMs(ms);
}