/*
 * Copyright (c) 2018-2020, NVIDIA CORPORATION.  All rights reserved. All
 * information contained herein is proprietary and confidential to NVIDIA
 * Corporation.  Any use, reproduction, or disclosure without the written
 * permission of NVIDIA Corporation is prohibited.
 */

#include <stdlib.h>
#include <string.h>
#include <stdbool.h>

#include "log_utils.h"
#include "devblk_cdi.h"
#include "cdi_constellation.h"
#include "cdi_constellation_regmap.h"

#define EMBEDDED_LINE_START_TAG_8BIT         0x0A
#define EMBEDDED_LINE_MSB_ADDR_TAG_8BIT      0xAA
#define EMBEDDED_LINE_LSB_ADDR_TAG_8BIT      0xA5
#define EMBEDDED_LINE_DATA_TAG_8BIT          0x5A
#define EMBEDDED_LINE_END_TAG_8BIT           0x07

#define EMBEDDED_LINE_START_TAG_16BIT        0x0A0A
#define EMBEDDED_LINE_ADDR_TAG_16BIT         0xAAAA
#define EMBEDDED_LINE_DATA_TAG_16BIT         0x5A5A
#define EMBEDDED_LINE_END_TAG_16BIT          0x0707

#define EMBEDDED_LINE_NULL                   0x00

#define CONSTELLATION_DEFAULT_CFG_SET_IDX    (0u)
#define CONSTELLATION_HDR_NUM_EXPOSURES      (3u)

#define LONG_EXPOSURE_TIME                   (1.0f / 30.0f)
#define SHORT_EXPOSURE_TIME                  ((LONG_EXPOSURE_TIME) / 16.0f)
#define VERY_SHORT_EXPOSURE_TIME             ((SHORT_EXPOSURE_TIME) / 16.0f)

#define MIN(a, b) ((a) < (b) ? (a) : (b))
#define GET_SIZE(a) (sizeof(a) / sizeof(a[0u]))
#define CONSTELLATION_CONFIG(inputWidth, inputHeight, inputFrameRate, inputFormat)        \
    {                                                                           \
        .width = inputWidth,                                                    \
        .height = inputHeight,                                                  \
        .pixelFormat = inputFormat,                                             \
        .resolution = #inputWidth "x" #inputHeight,                             \
        .framerate = inputFrameRate                                             \
    }

typedef enum {
    EMB_STATE_START,
    EMB_STATE_TAG,
    EMB_STATE_ADDR_MSB,
    EMB_STATE_ADDR_LSB,
    EMB_STATE_DATA_8BIT,
} EmbeddedDataParsingState;

typedef struct {
    DevBlkCDIRegSet regsel_table[CONSTELLATION_EMB_DATA_NUM_REGISTERS];
    unsigned int configSetIdx;
    DevBlkCDIModuleConfig moduleCfg;
    uint32_t numActiveExposures;
} _DriverHandle;

typedef struct {
    const char *resolution;
    NvMediaICPInputFormatType pixelFormat;
    uint32_t framerate;
    uint16_t width;
    uint16_t height;
} ConstellationConfig;

static const ConstellationConfig ConstellationConfigs[] = {
    CONSTELLATION_CONFIG(1920, 1208, 30, NVMEDIA_IMAGE_CAPTURE_INPUT_FORMAT_TYPE_RAW12),
    CONSTELLATION_CONFIG(1920, 1236, 30, NVMEDIA_IMAGE_CAPTURE_INPUT_FORMAT_TYPE_RAW12),
    CONSTELLATION_CONFIG(3848, 2168, 30, NVMEDIA_IMAGE_CAPTURE_INPUT_FORMAT_TYPE_RAW12),
};

static NvMediaStatus
DriverCreate(
    DevBlkCDIDevice *handle,
    void *clientContext)
{
    _DriverHandle *drvHandle = NULL;
    NvMediaStatus status = NVMEDIA_STATUS_ERROR;
    const ContextConstellation *ctx = NULL;
    uint32_t lenCfgName = 0u;

    if (NULL == handle) {
        status =  NVMEDIA_STATUS_BAD_PARAMETER;
        goto failed;
    }

    drvHandle = calloc(1, sizeof(_DriverHandle));
    if (NULL == drvHandle) {
        status = NVMEDIA_STATUS_OUT_OF_MEMORY;
        goto failed;
    }

    handle->deviceDriverHandle = (void *)drvHandle;

    /* Initialize register table */
    memcpy(drvHandle->regsel_table, regsel_constellation, sizeof(regsel_constellation));

    /* Initialize with client context */
    if (clientContext != NULL) {
        ctx = clientContext;
        if (ctx->configSetIdx >= GET_SIZE(ConstellationConfigs)) {
            status = NVMEDIA_STATUS_BAD_PARAMETER;
            goto failed;
        }

        drvHandle->configSetIdx = ctx->configSetIdx;

        lenCfgName = strlen(ctx->moduleConfig.cameraModuleCfgName);
        if (sizeof(drvHandle->moduleCfg.cameraModuleCfgName) > lenCfgName) {
            strncpy(drvHandle->moduleCfg.cameraModuleCfgName,
                    ctx->moduleConfig.cameraModuleCfgName,
                    lenCfgName);
        } else {
            status = NVMEDIA_STATUS_OUT_OF_MEMORY;
            goto failed;
        }

        drvHandle->moduleCfg.cameraModuleConfigPass1 =
            ctx->moduleConfig.cameraModuleConfigPass1;
        drvHandle->moduleCfg.cameraModuleConfigPass2 =
            ctx->moduleConfig.cameraModuleConfigPass2;

    /* If client context is NULL, use the defaults */
    } else {
        drvHandle->configSetIdx = CONSTELLATION_DEFAULT_CFG_SET_IDX;
    }

    drvHandle->numActiveExposures = CONSTELLATION_HDR_NUM_EXPOSURES;

    return NVMEDIA_STATUS_OK;

failed:
    if (drvHandle != NULL) {
        free(drvHandle);
    }
    return status;
}

static NvMediaStatus
DriverDestroy(
    DevBlkCDIDevice *handle)
{
    if (NULL == handle) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    if (handle->deviceDriverHandle != NULL) {
        free(handle->deviceDriverHandle);
        handle->deviceDriverHandle = NULL;
    }

    return NVMEDIA_STATUS_OK;
}

NvMediaStatus
ConstellationCheckPresence(
    DevBlkCDIDevice *handle)
{
    if (NULL == handle) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    return NVMEDIA_STATUS_OK;
}

static bool
SetEmbeddedDataRegister(
    DevBlkCDIRegSet *regset,
    uint32_t regsetSize,
    uint16_t addr,
    uint16_t value)
{
    uint16_t i;
    for (i = 0u; i < regsetSize; i++) {
        if (regset[i].addr == addr) {
            regset[i].data = value;
            regset[i].isSet = true;
            return true;
        }
    }
    return false;
}

static uint32_t
GetNextEmbeddedDataWord(
    const uint8_t *byteStream,
    uint32_t *offset,
    NvMediaICPInputFormatType inputFormat)
{
    uint32_t word = 0u;
    switch (inputFormat) {
        /* For 12 bpp, 2 bytes are used to store each pixel.  If its embedded
           data, then a single byte is stored in the MSB */
        case NVMEDIA_IMAGE_CAPTURE_INPUT_FORMAT_TYPE_RAW12:
            word = byteStream[*offset + 1u];
            *offset += 2u;
            break;
        /* For 16 bpp, 2 bytes of embedded data are stored in each pixel, in
           little endian */
        case NVMEDIA_IMAGE_CAPTURE_INPUT_FORMAT_TYPE_RAW16:
            word |= (byteStream[*offset + 1u]) << 8u;
            word |= (byteStream[*offset + 0u]) << 0u;
            *offset += 2u;
            break;
        /* For 20 bpp, 2 bytes of embedded data are stored in each pixel, in
           little endian */
        default:
            word |= (byteStream[*offset + 3u]) << 8u;
            word |= (byteStream[*offset + 2u]) << 0u;
            *offset += 4u;
            break;
    }
    return word;
}

static NvMediaStatus
ParseWBGain(
    _DriverHandle *drvHandle,
    DevBlkCDIWhiteBalance *parsedWbInfo)
{
    uint32_t i, channel;

    memset(parsedWbInfo, 0, sizeof(DevBlkCDIWhiteBalance));

    parsedWbInfo->wbValid = NVMEDIA_TRUE;
    for (i = 0u; i < drvHandle->numActiveExposures; i++) {
        for (channel = 0u; channel < DEVBLK_CDI_MAX_COLOR_COMPONENT; channel++) {
            parsedWbInfo->wbGain[i].value[channel] = 1.0;
        }
    }

    return NVMEDIA_STATUS_OK;
}

static NvMediaStatus
ParseExposure(
    _DriverHandle *drvHandle,
    DevBlkCDIExposure *sensorExpInfo)
{

    uint32_t i;

    memset(sensorExpInfo, 0, sizeof(DevBlkCDIExposure));

    sensorExpInfo->expTimeValid = NVMEDIA_TRUE;
    sensorExpInfo->gainValid = NVMEDIA_TRUE;

    sensorExpInfo->exposureTime[0] = LONG_EXPOSURE_TIME;
    sensorExpInfo->exposureTime[1] = SHORT_EXPOSURE_TIME;
    sensorExpInfo->exposureTime[2] = VERY_SHORT_EXPOSURE_TIME;

    for (i = 0u; i < drvHandle->numActiveExposures; i++) {
        sensorExpInfo->sensorGain[i] = 1.0;
    }

    return NVMEDIA_STATUS_OK;
}

static NvMediaStatus
DepackEmbeddedLine(
    DevBlkCDIDevice *handle,
    uint32_t lineLength,
    const uint8_t *lineData,
    uint16_t selLength)
{
    DevBlkCDIRegSet *regset;
    _DriverHandle *drvHandle;
    uint32_t pixelValue;
    uint32_t currentByte = 0u;
    uint32_t currentLine = 0u;
    EmbeddedDataParsingState state;
    uint16_t regData = 0u;
    uint16_t regAddr = 0u;
    uint32_t width;
    uint32_t bytesPerPixel;
    bool dataHasMSB;
    const ConstellationConfig *constellationConfig;

    drvHandle = (_DriverHandle *)(handle->deviceDriverHandle);
    regset = drvHandle->regsel_table;
    constellationConfig = &ConstellationConfigs[drvHandle->configSetIdx];
    width = constellationConfig->width;

    /* Set variables based on format */
    switch (constellationConfig->pixelFormat) {
        case NVMEDIA_IMAGE_CAPTURE_INPUT_FORMAT_TYPE_RAW12:
            bytesPerPixel = 2u;
            break;
        default:
            LOG_ERR("Embedded data parsing does not support pixel format\n");
            return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    /* Loop through all lines */
    while (currentByte < lineLength) {
        state = EMB_STATE_START;
        dataHasMSB = false;

        /* Loop through all pixels in this line */
        while (currentByte < MIN((currentLine + 1u)*bytesPerPixel*width, lineLength)) {
            /* Read the next available pixel from this line */
            pixelValue = GetNextEmbeddedDataWord(lineData,
                                                 &currentByte,
                                                 constellationConfig->pixelFormat);

            /* Use the state to determine how this pixel should be interpreted */
            switch(state) {
                /* Make sure the embedded data identifier is present at the front of the line */
                case EMB_STATE_START:
                    switch(pixelValue) {
                        case EMBEDDED_LINE_START_TAG_8BIT:
                            state = EMB_STATE_TAG;
                            break;
                        case EMBEDDED_LINE_NULL:
                            goto line_end;
                        default:
                            /* If the identifier is missing from the beginning of the first line,
                               the embedded data is probably not present at all */
                            if (currentLine == 0) {
                                LOG_WARN("No embedded data detected!\n");
                            /* If the identifier is missing from a different line, the embedded
                               data is present but is corrupted/malformed */
                            } else {
                                LOG_ERR("Unrecognized identifier at start of embedded line!\n");
                            }
                            return NVMEDIA_STATUS_ERROR;
                    }
                    break;

                /* Set the state based off of which tag was read */
                case EMB_STATE_TAG:
                    switch (pixelValue) {
                        case EMBEDDED_LINE_MSB_ADDR_TAG_8BIT:
                            state = EMB_STATE_ADDR_MSB;
                            break;
                        case EMBEDDED_LINE_LSB_ADDR_TAG_8BIT:
                            state = EMB_STATE_ADDR_LSB;
                            break;
                        case EMBEDDED_LINE_DATA_TAG_8BIT:
                            state = EMB_STATE_DATA_8BIT;
                            break;
                        case EMBEDDED_LINE_END_TAG_8BIT:
                            goto line_end;
                            break;
                        default:
                           LOG_ERR("Unrecognized tag (%02x) in embedded data!\n", pixelValue);
                           return NVMEDIA_STATUS_ERROR;
                    }
                    break;

                /* Record the MSB of the register address */
                case EMB_STATE_ADDR_MSB:
                    regAddr &= 0x00FF;
                    regAddr |= pixelValue << 8u;
                    state = EMB_STATE_TAG;
                    break;

                /* Record the LSB of the register address */
                case EMB_STATE_ADDR_LSB:
                    regAddr &= 0xFF00;
                    regAddr |= pixelValue;
                    state = EMB_STATE_TAG;
                    break;

                case EMB_STATE_DATA_8BIT:
                    /* Record the MSB of the register data */
                    if (!dataHasMSB) {
                        regData &= 0x00FF;
                        regData |= pixelValue << 8u;
                    /* If we are recording the LSB, save the data and increment
                       the address counter by 2 */
                    } else {
                        regData &= 0xFF00;
                        regData |= pixelValue;
                        SetEmbeddedDataRegister(regset,
                                                CONSTELLATION_EMB_DATA_NUM_REGISTERS,
                                                regAddr,
                                                regData);
                        regAddr += 2u;
                    }
                    dataHasMSB = !dataHasMSB;
                    state = EMB_STATE_TAG;
                    break;
            }
        }
        line_end:
            currentLine++;
            currentByte = currentLine * bytesPerPixel * width;
    }
    return NVMEDIA_STATUS_OK;
}

static NvMediaStatus
SetSensorControls(
    DevBlkCDIDevice *handle,
    const struct DevBlkCDISensorControl *sensorControl,
    const size_t sensrCtrlStructSize)
{
    if ((NULL == handle) || (NULL == sensorControl) || (0 == sensrCtrlStructSize)) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    return NVMEDIA_STATUS_OK;
}

static NvMediaStatus
GetModuleConfig(
    DevBlkCDIDevice *handle,
    DevBlkCDIModuleConfig *cameraModuleConfig)
{
    _DriverHandle *drvHandle = (_DriverHandle *)(handle->deviceDriverHandle);

    if ((handle == NULL) || (cameraModuleConfig == NULL)) {
        return NVMEDIA_STATUS_BAD_PARAMETER;
    }

    strncpy(cameraModuleConfig->cameraModuleCfgName,
            drvHandle->moduleCfg.cameraModuleCfgName,
            strlen(drvHandle->moduleCfg.cameraModuleCfgName));

    cameraModuleConfig->cameraModuleConfigPass1 = drvHandle->moduleCfg.cameraModuleConfigPass1;
    cameraModuleConfig->cameraModuleConfigPass2 = drvHandle->moduleCfg.cameraModuleConfigPass2;

    return NVMEDIA_STATUS_OK;
}

static NvMediaStatus
ParseEmbedDataInfo(
    DevBlkCDIDevice *handle,
    const struct DevBlkCDIEmbeddedDataChunk *embeddedTopDataChunk,
    const struct DevBlkCDIEmbeddedDataChunk *embeddedBotDataChunk,
    const size_t dataChunkStructSize,
    struct DevBlkCDIEmbeddedDataInfo *parsedInfo,
    const size_t parsedInfoStructSize)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    _DriverHandle *drvHandle = NULL;
    if ((handle == NULL) || ((drvHandle = (_DriverHandle *)handle->deviceDriverHandle) == NULL) ||
        (embeddedTopDataChunk == NULL) || (dataChunkStructSize == 0) || (parsedInfo == NULL) ||
        (parsedInfoStructSize == 0)) {
        LOG_ERR("%s : Bad parameter \n", __func__);
        status = NVMEDIA_STATUS_BAD_PARAMETER;
        goto DONE;
    }

    if ((dataChunkStructSize != sizeof(DevBlkCDIEmbeddedDataChunk)) ||
        (parsedInfoStructSize != sizeof(DevBlkCDIEmbeddedDataInfo))) {
        LOG_ERR("Version mismatch. Please re-compile the client application. \n");
        status = NVMEDIA_STATUS_ERROR;
        goto DONE;
    }

    memset(parsedInfo, 0, sizeof(DevBlkCDIEmbeddedDataInfo));

    parsedInfo->numExposures = drvHandle->numActiveExposures;
    parsedInfo->sensorTempInfo.tempValid = NVMEDIA_FALSE;

    /* decoding embedded line & filling register set */
    status = DepackEmbeddedLine (
                handle,
                embeddedTopDataChunk->lineLength,
                embeddedTopDataChunk->lineData,
                CONSTELLATION_EMB_DATA_NUM_REGISTERS);
    if (NVMEDIA_STATUS_OK != status) {
        return NVMEDIA_STATUS_OK;
    }

    status = ParseExposure(drvHandle, &parsedInfo->sensorExpInfo);
    if (NVMEDIA_STATUS_OK != status) {
        goto DONE;
    }

    status = ParseWBGain(drvHandle, &parsedInfo->sensorWBInfo);
    if (NVMEDIA_STATUS_OK != status) {
        goto DONE;
    }

    parsedInfo->frameSeqNumInfo.frameSeqNumValid = NVMEDIA_TRUE;
    parsedInfo->frameSeqNumInfo.frameSequenceNumber =
        (drvHandle->regsel_table[REG_FRAME_COUNT_HIGH_CONSTELLATION].data << 16u) |
        (drvHandle->regsel_table[REG_FRAME_COUNT_LOW_CONSTELLATION].data << 0u) ;

DONE:
    return status;
}

static NvMediaStatus
GetSensorAttributes(
    DevBlkCDIDevice *handle,
    struct DevBlkCDISensorAttributes *sensorAttr,
    const size_t sensorAttrStructSize)
{
    NvMediaStatus status = NVMEDIA_STATUS_OK;
    _DriverHandle *drvHandle = NULL;
    uint8_t i = 0u;

    if ((handle == NULL) || ((drvHandle = (_DriverHandle *)handle->deviceDriverHandle) == NULL) ||
        (sensorAttr == NULL) || (sensorAttrStructSize == 0u)) {
        LOG_ERR("[%s:%d] Bad input parameter\n", __func__, __LINE__);
        status = NVMEDIA_STATUS_BAD_PARAMETER;
        goto done;
    }
    sensorAttr->sensorCFA = NVM_SURF_ATTR_COMPONENT_ORDER_CRBC;
    sensorAttr->numActiveExposures = drvHandle->numActiveExposures;

    for (i = 0u; i < drvHandle->numActiveExposures; i++) {
        sensorAttr->sensorExpRange[i].min = 0.0f;
        sensorAttr->sensorExpRange[i].max = 1.0f;
        sensorAttr->sensorGainRange[i].min = 0.0f;
        sensorAttr->sensorGainRange[i].max = 100.0f;
        sensorAttr->sensorWhiteBalanceRange[i].min = 1.0f;
        sensorAttr->sensorWhiteBalanceRange[i].max = 10.0f;
    }

done:
    return status;
}

static DevBlkCDIDeviceDriver deviceDriver = {
    .deviceName = "Constellationulated Image Sensor",
    .regLength = 2u,
    .dataLength = 2u,
    .DriverCreate = DriverCreate,
    .DriverDestroy = DriverDestroy,
    .GetModuleConfig = GetModuleConfig,
    .GetSensorAttributes = GetSensorAttributes,
    .SetSensorControls = SetSensorControls,
    .ParseEmbedDataInfo = ParseEmbedDataInfo,
};

DevBlkCDIDeviceDriver *
GetConstellationDriver(void)
{
    return &deviceDriver;
}

NvMediaStatus
GetConstellationConfigSet(
    const char *resolution,
    NvMediaICPInputFormatType inputFormat,
    unsigned int framerate,
    int *configSet)
{
    uint32_t configIndex;
    const ConstellationConfig *config;

    for (configIndex = 0u; configIndex < GET_SIZE(ConstellationConfigs) ; configIndex++) {
        config = &ConstellationConfigs[configIndex];
        if ((strcasecmp(resolution, config->resolution) == 0) &&
            (inputFormat == config->pixelFormat) &&
            (framerate == config->framerate))
        {
            *configSet = configIndex;
            return NVMEDIA_STATUS_OK;
        }

    }
    LOG_ERR("Cannot find valid configuration with given parameters\n");
    return NVMEDIA_STATUS_ERROR;
}
