/*******************************************************************************
 * (c) Copyright 2016-2017 Microsemi SoC Products Group. All rights reserved.
 *
 * This SoftConsole Video project for MIPI sensor configuration and interfacing with GUI
 *
 * Please refer README.TXT in the root folder of this project for more details.
 */
#include "miv_rv32_hal/miv_rv32_hal.h"
#include "../platform/drivers/fpga_ip/CoreGPIO/core_gpio.h"
#include "../platform/drivers/fpga_ip/CoreAXI4-Lite/AXI4-Lite.h"
#include "../platform/drivers/fpga_ip/CoreUARTapb/core_uart_apb.h"
#include "../platform/drivers/fpga_ip/CoreI2c/core_i2c.h"
#include "../platform/hal/hal.h"
#include "../application/imx334_corei2c/imx334_corei2c.h"
#include "../application/hdmi_config/hdmi_tx.h"
#include <stdio.h>

/* Camera configuration (Data rate, resolution) and pattern generator
 * can be selected in imx334_corei2c.h file.
 * */

#define LED1 GPIO_0
#define LED2 GPIO_1
#define LED3 GPIO_2
#define LED4 GPIO_3

//#define F_End GPIO_31

#define MIPI_TRNG_RST GPIO_4


#define ALPHA_ADDR_IN       0x70009040
#define RGB_SUM_ADDR        0x70009038
#define RGB_SUM_ADDR1       0x7000903C
#define R_GAIN_ADDR         0x70009020
#define G_GAIN_ADDR         0x70009024
#define B_GAIN_ADDR         0x70009028
#define CONTRAST_ADDR       0x70009030
#define BRIGHTNESS_ADDR     0x70009034


#define DET_SEL_ADDR          0x7000910C
#define DET_BAYER_ADDR        0x70009110
#define DET_THR1_ADDR         0x70009114
#define DET_THR2_ADDR         0x70009118

#define BAYER_ADDR_0        0x60010000
#define BAYER_ADDR_1        0x60020000
#define BAYER_ADDR_4k       0x60030000

#define ALPHA_ADDR          0x60040014

#define IE1_IP_VER          0x60050004
#define R_CONST_ADDR        0x60050008
#define G_CONST_ADDR        0x6005000C
#define B_CONST_ADDR        0x60050010
#define SECOND_CONST_ADDR   0x60050014
#define INTENSITY_AVARAGE   0x60050018

#define R_CONST_ADDR_0       0x60060008
#define G_CONST_ADDR_0       0x6006000C
#define B_CONST_ADDR_0       0x60060010
#define SECOND_CONST_ADDR_0  0x60060014
#define INTENSITY_AVARAGE_0  0x60050018

#define R_CONST_ADDR_1       0x60070008
#define G_CONST_ADDR_1       0x6007000C
#define B_CONST_ADDR_1       0x60070010
#define SECOND_CONST_ADDR_1  0x60070014
#define INTENSITY_AVARAGE_1  0x60050018

#define DPC_enable_CAM1              0x6008000C
#define DPC_bayer_CAM1               0x60080000
#define DET_THR1_ADDR_CAM1           0x60080004
#define DET_THR2_ADDR_CAM1           0x60080008

#define DPC_enable_CAM2_SCALE        0x6009000C
#define DPC_bayer_CAM2_SCALE         0x60090000
#define DET_THR1_ADDR_CAM2_SCALE     0x60090004
#define DET_THR2_ADDR_CAM2_SCALE     0x60090008

#define DPC_enable_CAM1_4K           0x600A000C
#define DPC_bayer_CAM1_4K            0x600A0000
#define DET_THR1_ADDR_4K             0x600A0004
#define DET_THR2_ADDR_4K             0x600A0008



volatile uint32_t g_10ms_count;

volatile uint32_t timerdone = 0;
volatile uint32_t g_100ms_count1;
volatile uint32_t g_ms_count;
volatile void set_bgr_gain();
static void auto_brightness( uint32_t div);
static void gain_cal(uint32_t total_average);
static void gain_cal1(uint32_t total_average1);


i2c_instance_t g_i2c_instance_hdmi;
i2c_instance_t g_i2c_instance_cam1;
i2c_instance_t g_i2c_instance_cam2;
/*-----------------------------------------------------------------------------
 * GPIO instance data.
 */

gpio_instance_t g_gpio_out;


/*-----------------------------------------------------------------------------
 * Global state counter.
 */
uint32_t g_state = 1;

volatile uint32_t rx_tmr_done = 0;
volatile uint32_t rx_ms_count1;
volatile uint32_t rx_ms_count;
uint32_t t_ms_count = 0;

volatile uint16_t contrast;
volatile uint16_t brightness;
volatile uint16_t r_gain, b_gain, g_gain;
volatile uint16_t r_const, b_const, g_const;
volatile uint32_t contrast_scl,total_sum;
volatile uint32_t second_const;
uint16_t bayer0;
uint16_t bayer1;
uint16_t bayer4k;
uint16_t alpha;
uint16_t dpc_bayer_cam1, det_thr1_cam1, det_thr2_cam1;

volatile uint16_t dpc_sel, dpc_bayer, det_thr1, det_thr2;

uint32_t cam_IE_IA_Constant;
uint32_t scale_IE_IA_Constant;
uint32_t cam_4K_IE_IA_Constant;
uint32_t ie_ver;


const uint32_t hdim = 1920;
const uint32_t vdim = 1080;

static uint16_t in_gain = 80,in_gain1 = 80;
/*-----------------------------------------------------------------------------
 * UART handler specific.
 */
uint32_t i = 0;
uint32_t process_data = 0;

//static uint8_t counter = 0;

/*-----------------------------------------------------------------------------
 * System Tick interrupt handler
 */
void SysTick_Handler(void) {

//  g_state = (~g_state) & 0x01;

    if(timerdone == 1)
    {
        g_100ms_count1 += 1;
        if(g_ms_count <= g_100ms_count1)
            timerdone = 0;
    }

}

//void MSYS_EI0_IRQHandler(void) {

//    I2C_isr(&g_i2c_instance_cam2);
//}

//void MSYS_EI2_IRQHandler(void) {

//    I2C_isr(&g_i2c_instance_cam1);
//}

//void MSYS_EI1_IRQHandler(void) {

//    I2C_isr(&g_i2c_instance_hdmi);
//}

uint8_t MSYS_EI0_IRQHandler(void)
{
    I2C_isr(&g_i2c_instance_cam2);
    return (EXT_IRQ_KEEP_ENABLED);
}

uint8_t MSYS_EI1_IRQHandler(void)
{
    I2C_isr(&g_i2c_instance_hdmi);
    return (EXT_IRQ_KEEP_ENABLED);
}

uint8_t MSYS_EI2_IRQHandler(void)
{
    I2C_isr(&g_i2c_instance_cam1);
    return (EXT_IRQ_KEEP_ENABLED);
}


/*-----------------------------------------------------------------------------
 * main
 */
uint32_t a;

int main(int argc, char **argv) {
    volatile  uint32_t counter;
    uint8_t state;

    counter = 0;
    state = 0;

    GPIO_init(&g_gpio_out, COREGPIO_OUT_BASE_ADDR, GPIO_APB_32_BITS_BUS);

    GPIO_set_output(&g_gpio_out, LED1, 1);

//    PLIC_init();

    MRV_systick_config(SYS_CLK_FREQ / 1000);///100msec delay

//    PLIC_SetPriority(I2C_CAM1_IRQn, 1);
    MRV_enable_local_irq(MRV32_MSYS_EIE0_IRQn);

//    PLIC_EnableIRQ(I2C_CAM1_IRQn);

//    PLIC_SetPriority(HDMI_I2C_IRQn, 1);
    MRV_enable_local_irq(MRV32_MSYS_EIE1_IRQn);

//    PLIC_EnableIRQ(HDMI_I2C_IRQn);

//    PLIC_SetPriority(I2C_CAM2_IRQn, 1);
    MRV_enable_local_irq(MRV32_MSYS_EIE2_IRQn);

//    PLIC_EnableIRQ(I2C_CAM2_IRQn);
    HAL_enable_interrupts();

    GPIO_set_output(&g_gpio_out, MIPI_TRNG_RST, 0u);
    GPIO_set_output(&g_gpio_out, LED2, 1);
    HDMI_tx_init();

#if 1
    GPIO_set_output(&g_gpio_out, CAM1_RST, 1u);
    GPIO_set_output(&g_gpio_out, CAM2_RST, 1u);
    GPIO_set_output(&g_gpio_out, CAM_CLK_EN, 0u);
    imx334_cam_init();
    imx334_cam_reginit(1u);
#endif
    GPIO_set_output(&g_gpio_out, LED3, 1);
    msdelay(1000);
    GPIO_set_output(&g_gpio_out, MIPI_TRNG_RST, 1u);
    GPIO_set_output(&g_gpio_out, LED4, 1);

    uint32_t div = (hdim*vdim*2);
    uint32_t div1 = (hdim*vdim*2)/9;

    do {
        msdelay(30);
        auto_brightness(div);


        counter = counter +1;
        if (counter <16){
            GPIO_set_output(&g_gpio_out, LED1, 0);
        }
        else{
            GPIO_set_output(&g_gpio_out, LED1, 1);
        }
        if (counter ==32){
            counter =0;
        }

    } while (1);


    return 0;
}

/**********************************************************/
/**********************FUNCTION CALLS**********************/
/**********************************************************/



void set_bgr_gain()
{
    contrast_scl = (325*(contrast+128) / (387 - contrast))>>5u;
    r_const = (r_gain * contrast_scl)/10;
    g_const = (g_gain * contrast_scl/10);
    b_const = (b_gain * contrast_scl)/10;
    second_const = 128 * (brightness - ((128*contrast_scl)/10));

    bayer0 = 0x00;      // BI1
    bayer1 = 0x00;      // BI2
    bayer4k = 0x00;     // 4k BI
    ie_ver = 0x00;

    cam_IE_IA_Constant  = 0x00;

    scale_IE_IA_Constant  = 0x00;

    cam_4K_IE_IA_Constant  = 0x00;

    dpc_bayer_cam1 = 0x01;
    det_thr1_cam1 = 0x1E;
    det_thr2_cam1 = 0x0A;

    dpc_bayer_cam2_scale = 0x01;
    det_thr1_cam2_scale = 0x1E;
    det_thr2_cam2_scale = 0x0A;

    dpc_bayer_cam1_4k = 0x01;
    det_thr1_cam1_4k = 0x1E;
    det_thr2_cam1_4k = 0x0A; 

    axi4litewrite(BAYER_ADDR_0,bayer0);
            bayer0 = (uint16_t)axi4literead(BAYER_ADDR_0);
    axi4litewrite(BAYER_ADDR_1,bayer1);
            bayer1 = (uint16_t)axi4literead(BAYER_ADDR_1);
    axi4litewrite(BAYER_ADDR_4k,bayer4k);
            bayer4k = (uint16_t)axi4literead(BAYER_ADDR_4k);

            //IE1 AXI4-Lite write operation
                    axi4litewrite(R_CONST_ADDR,r_const);
                    axi4litewrite(G_CONST_ADDR,g_const);
                    axi4litewrite(B_CONST_ADDR,b_const);
                    axi4litewrite(SECOND_CONST_ADDR,second_const);
            //axi4litewrite(R_CONST_ADDR,0XFF);
            //axi4litewrite(G_CONST_ADDR,0XFF);
            //axi4litewrite(B_CONST_ADDR,0XFF);
           // axi4litewrite(SECOND_CONST_ADDR,0XFF);


            ie_ver = (uint32_t)axi4literead(R_CONST_ADDR);

                    cam_IE_IA_Constant = (uint32_t)axi4literead(RGB_SUM_ADDR);

                //IE2 AXI4-Lite write operation
                    axi4litewrite(R_CONST_ADDR_0,r_const);
                    axi4litewrite(G_CONST_ADDR_0,g_const);
                    axi4litewrite(B_CONST_ADDR_0,b_const);
                    axi4litewrite(SECOND_CONST_ADDR_0,second_const);

                    scale_IE_IA_Constant = (uint32_t)axi4literead(INTENSITY_AVARAGE_0);

                //4k IE3 AXI4-Lite write operation
                    axi4litewrite(R_CONST_ADDR_1,r_const);
                    axi4litewrite(G_CONST_ADDR_1,g_const);
                    axi4litewrite(B_CONST_ADDR_1,b_const);
                    axi4litewrite(SECOND_CONST_ADDR_1,second_const);

                    cam_4K_IE_IA_Constant = (uint32_t)axi4literead(INTENSITY_AVARAGE_1);

                    axi4litewrite(ALPHA_ADDR,alpha);

                    //---- DPC CAM1
                    axi4litewrite(DPC_enable_CAM1,dpc_sel);
                    axi4litewrite(DPC_bayer_CAM1,dpc_bayer);
                    axi4litewrite(DET_THR1_ADDR_CAM1,det_thr1);
                    axi4litewrite(DET_THR2_ADDR_CAM1,det_thr2);

                    //---- DPC CAM2 for Scale
                    axi4litewrite(DPC_enable_CAM2_SCALE,dpc_sel);
                    axi4litewrite(DPC_bayer_CAM2_SCALE,dpc_bayer);
                    axi4litewrite(DET_THR1_ADDR_CAM2_SCALE,det_thr1);
                    axi4litewrite(DET_THR2_ADDR_CAM2_SCALE,det_thr2);

                    //---- DPC CAM1 for 4K
                    axi4litewrite(DPC_enable_CAM1_4K,dpc_sel);
                    axi4litewrite(DPC_bayer_CAM1_4K,dpc_bayer);
                    axi4litewrite(DET_THR1_ADDR_4K,det_thr1);
                    axi4litewrite(DET_THR2_ADDR_4K,det_thr2);

}


void auto_brightness(uint32_t div){

    r_gain = (uint16_t)(*(volatile int*) R_GAIN_ADDR);
    g_gain = (uint16_t)(*(volatile int*) G_GAIN_ADDR);
    b_gain = (uint16_t)(*(volatile int*) B_GAIN_ADDR);
    contrast = (uint16_t)(*(volatile int*) CONTRAST_ADDR);//8;//Range 3 - 30 (divided by 10 in later steps)
    brightness = (uint16_t)(*(volatile int*) BRIGHTNESS_ADDR);
    alpha = (uint16_t)(*(volatile int*) ALPHA_ADDR_IN);

    dpc_sel    = (uint16_t)(*(volatile int*) DET_SEL_ADDR  );
    dpc_bayer  = (uint16_t)(*(volatile int*) DET_BAYER_ADDR);
    det_thr1   = (uint16_t)(*(volatile int*) DET_THR1_ADDR );
    det_thr2   = (uint16_t)(*(volatile int*) DET_THR2_ADDR );

    set_bgr_gain();

    uint32_t total_sum =  (uint32_t)(*(volatile int*) RGB_SUM_ADDR);
    uint32_t total_average = total_sum/div;
    uint32_t total_sum1 =  (uint32_t)(*(volatile int*) RGB_SUM_ADDR1);
    uint32_t total_average1 = total_sum1/(div/9);

    gain_cal(total_average);
    gain_cal1(total_average1);

}

void gain_cal(uint32_t total_average){
    //////////////////////////////////////////////////////
    const int16_t good_average=100;
    const int16_t hysteresis=4;
    int16_t step;
        if(total_average < (good_average - hysteresis))
            step = 1;
        else
            if(total_average > (good_average + hysteresis))
                step = -1;
            else
                step = 0;

        in_gain = in_gain + step;

        if(in_gain < 5)
            in_gain = 5;
        else
            if(in_gain >= 100)
                in_gain = 100;
    ///////////////////////////////////////////////////////////
    gain_setting(1u,in_gain);
}
//
void gain_cal1(uint32_t total_average1){

    int16_t step1;
    const int16_t good_average=100;
    const int16_t hysteresis=4;
        if(total_average1 < (good_average - hysteresis))
            step1 = 1;
        else
            if(total_average1 > (good_average + hysteresis))
                step1 = -1;
            else
                step1 = 0;

        in_gain1 = in_gain1 + step1;

        if(in_gain1 < 5)
            in_gain1 = 5;
        else
            if(in_gain1 >= 100)
                in_gain1 = 100;
    ///////////////////////////////////////////////////////////
     gain_setting(2u,in_gain1);
}
//
