/******************************************************************************
**
** Copyright (C) 2021 The Qt Company Ltd.
** Contact: https://www.qt.io/licensing/
**
** This file is part of the Qt Quick Ultralite module.
**
** $QT_BEGIN_LICENSE:COMM$
**
** Commercial License Usage
** Licensees holding valid commercial Qt licenses may use this file in
** accordance with the commercial license agreement provided with the
** Software or, alternatively, in accordance with the terms contained in
** a written agreement between you and The Qt Company. For licensing terms
** and conditions see http://www.qt.io/terms-conditions. For further
** information use the contact form at http://www.qt.io/contact-us.
**
** $QT_END_LICENSE$
**
******************************************************************************/

#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_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});
}

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

    BOARD_InitCameraResource();

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

    err = CAMERA_DEVICE_Init(&cameraDevice, &cameraConfig);
    FAIL_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]));
        FAIL_IF(err != kStatus_Success, "Failed to submit camera buffer!");
    }
}

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