/*******************************************************************************
 * (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 "drivers/fpga_ip/CoreGPIO/core_gpio.h"
#include "drivers/fpga_ip/CoreUARTapb/core_uart_apb.h"
#include "drivers/fpga_ip/CoreI2C/core_i2c.h"
#include "hal/hal.h"
#include "slvs_ec_corei2c/slvs_ec_corei2c.h"

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

#define MIPI_TRNG_RST GPIO_4

#define ALPHA_ADDR			0x70009000

#define R_GAIN_ADDR  		0x70009020
#define G_GAIN_ADDR			0x70009024
#define B_GAIN_ADDR			0x70009028
#define CONTRAST_ADDR		0x70009030
#define BRIGHTNESS_ADDR		0x70009034
#define ALPHA_ADDR			0x70009000

#define RGB_SUM_ADDR        0x70009038

#define R_CONST_ADDR  		0x70009004
#define G_CONST_ADDR		0x70009008
#define B_CONST_ADDR		0x7000900C
#define SECOND_CONST_ADDR 	0x70009010
volatile uint32_t g_10ms_count;

volatile uint32_t timerdone = 0;
volatile uint32_t g_10ms_count1;
volatile uint32_t g_ms_count;
static void gui_handler(void);
static void auto_brightness( uint32_t div);

i2c_instance_t g_i2c_instance_cam1;

void msdelay(uint32_t tms);

/*-----------------------------------------------------------------------------
 * 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;

uint16_t contrast;
uint16_t brightness;
uint16_t r_gain, b_gain, g_gain;
uint16_t r_const, b_const, g_const;
uint32_t second_const,contrast_scl;
static uint16_t in_gain = 2000;
const uint32_t hdim = 1920;
const uint32_t vdim = 1080;
volatile char sem = 0;
//volatile static uint8_t test_done = 0;
//volatile static uint8_t first_lock_done = 0;
//volatile uint8_t interrupt_count = 0;

/*-----------------------------------------------------------------------------
 * UART handler specific.
 */
uint32_t i = 0;
uint32_t process_data = 0;



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

	g_state = (~g_state) & 0x01;

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

	if(rx_tmr_done == 1)
	{
		rx_ms_count1 += 1;
		if(rx_ms_count1 >= rx_ms_count){
			rx_tmr_done = 0;
			process_data = 1;
		}
	}
}

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

uint8_t MSYS_EI0_IRQHandler(void)
{
   sem=1;
   //slvs_ec_cam_standby_en_dis(1u);

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


    MRV_systick_config(SYS_CLK_FREQ / 1000);

	MRV_enable_local_irq(MRV32_MSYS_EIE2_IRQn);

    MRV_enable_local_irq(MRV32_MSYS_EIE1_IRQn);

    MRV_enable_local_irq(MRV32_MSYS_EIE0_IRQn);

    HAL_enable_interrupts();

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

#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);
	slvs_ec_cam_init();
    slvs_ec_cam_reginit(1u);
#endif
    GPIO_set_output(&g_gpio_out, LED3, 1);
	msdelay(1000);
	GPIO_set_output(&g_gpio_out, MIPI_TRNG_RST, 1u);

    uint32_t div = (hdim*vdim*2);

    GPIO_set_output(&g_gpio_out, LED4, 1);
	do {
		gui_handler();
		auto_brightness(div);
		if(100000u == counter)
		{
			counter = 0;
			state = !state;
		    GPIO_set_output(&g_gpio_out, LED1, state);
		}
		else
		{
			counter = counter + 1;
		}

		if(sem==1)
		{
		    sem=0;
		    slvs_ec_cam_standby_en_dis(1u);
		    GPIO_set_output(&g_gpio_out, LED4, 1);
		    GPIO_set_output(&g_gpio_out, LED3, 0);
		    GPIO_set_output(&g_gpio_out, LED2, 1);
		    GPIO_set_output(&g_gpio_out, LED1, 0);

		    //msdelay(1000);
		}

//		if(sem==1)
//		    {
//		        interrupt_count++;
//		        // Use simple debug without printf if stdio.h not available
//		        GPIO_set_output(&g_gpio_out, LED4, interrupt_count & 0x01);  // Toggle LED
//
//		        sem=0;
//
//		        if (interrupt_count == 1) {  // Only first time
//		            slvs_ec_cam_standby_en_dis(1u);
//		        }
//		        // Ignore subsequent interrupts
//
//		        GPIO_set_output(&g_gpio_out, LED3, 0);
//		        GPIO_set_output(&g_gpio_out, LED2, 1);
//		        GPIO_set_output(&g_gpio_out, LED1, 0);
//		    }
//		else{
//		    GPIO_set_output(&g_gpio_out, LED3, 0);
//		    GPIO_set_output(&g_gpio_out, LED3, 0);
//		    GPIO_set_output(&g_gpio_out, LED2, 0);
//		    GPIO_set_output(&g_gpio_out, LED1, 0);
//		}


//		if (test_done == 0) {
//		        //msdelay(5000);
//		        msdelay(100);
//		        slvs_ec_cam_standby_en_dis(1u); // enable when ERM usage
//		        test_done = 1;  // Never runs again
//		    }

	} while (1);

    return 0;
}

/**********************************************************/
/**********************FUNCTION CALLS**********************/
/**********************************************************/
void auto_brightness(uint32_t div)
{

    //const int16_t good_average=100;
    //const int16_t hysteresis=10;
    const int16_t good_average=120;
    const int16_t hysteresis=20;
    const int16_t min_gain=20;
    const int16_t max_gain=120;

    uint32_t total_sum =  (uint32_t)(*(volatile int*) RGB_SUM_ADDR);
    uint32_t total_average = total_sum/div;
    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 < min_gain)
            in_gain = min_gain;
        else
            if(in_gain >= max_gain)
                in_gain = max_gain;
    ///////////////////////////////////////////////////////////
    gain_setting(1u,in_gain);
    //msdelay(100);
        //slvs_ec_cam_standby_en_dis(1u);
//msdelay(100);
}

void gui_handler()
{
	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 = *(volatile int*) CONTRAST_ADDR;//8;//Range 3 - 30 (divided by 10 in later steps)
	brightness = *(volatile int*) BRIGHTNESS_ADDR;

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

	*(volatile int*) R_CONST_ADDR = r_const;
	*(volatile int*) G_CONST_ADDR = g_const;
	*(volatile int*) B_CONST_ADDR = b_const;
	*(volatile int*) SECOND_CONST_ADDR = second_const;
}

