#include "sys/alt_stdio.h"
#include "io.h"
#include "../controller_bsp/system.h"
#include "unistd.h"
#include "btc_dptx_syslib/btc_dptx_syslib.h"
#include "displayport_tx.h"

alt_u32 dp_tx_rgb_parameters;
alt_u32 previous_dp_tx_rgb_parameters = 0;
alt_u32 dp_tx_ycbcr_parameters;
alt_u32 previous_dp_tx_ycbcr_parameters = 0;

void init_displayport(alt_u8 use_ycbcr, alt_u32 dp_tx_rgb_parameters, alt_u32 dp_tx_ycbcr_parameters)
{
	btc_dptx_syslib_init();

	BTC_DPTX_DISABLE_HPD_IRQ(0);
	BTC_DPTX_DISABLE_AUX_IRQ(0);

	unsigned int sr;
	sr = IORD(DISPLAYPORT_TX_0_BASE, DPTX_REG_TX_STATUS); // Reading SR clears IRQ

	if (sr & 0x04)
	{
		btc_dptx_hpd_change(0, 1);
		if (use_ycbcr == 0) btc_dptx_set_color_space(0, (dp_tx_rgb_parameters >> 0) & 0xF, (dp_tx_rgb_parameters >> 4) & 0xF, (dp_tx_rgb_parameters >> 8) & 0xF, (dp_tx_rgb_parameters >> 12) & 0xF, (dp_tx_rgb_parameters >> 16) & 0xF); else btc_dptx_set_color_space(0, (dp_tx_ycbcr_parameters >> 0) & 0xF, (dp_tx_ycbcr_parameters >> 4) & 0xF, (dp_tx_ycbcr_parameters >> 8) & 0xF, (dp_tx_ycbcr_parameters >> 12) & 0xF, (dp_tx_ycbcr_parameters >> 16) & 0xF);
		btc_dptx_link_training(0, 20, 4);
    }
}

int main()
{
	printf("Begining \n");

	usleep(100);

	IOWR(PIO_0_BASE, 0, 0x01); // DP TX connect 
	IOWR(PIO_1_BASE, 0, 0b101);// default hotplug inversion and lane mapping inversion

	btc_dptx_syslib_add_tx(0, DISPLAYPORT_TX_0_BASE, DISPLAYPORT_TX_0_IRQ_INTERRUPT_CONTROLLER_ID, DISPLAYPORT_TX_0_IRQ);
	init_displayport(1, dp_tx_rgb_parameters, dp_tx_ycbcr_parameters);

	usleep(50000);

	printf("Loop...\n");

//IOWR(DISPLAYPORT_TX_0_BASE, DPTX0_REG_MSA_MVID    ,);
//IOWR(DISPLAYPORT_TX_0_BASE, DPTX0_REG_MSA_NVID    ,);
IOWR(DISPLAYPORT_TX_0_BASE, DPTX0_REG_MSA_HTOTAL  ,2200);
IOWR(DISPLAYPORT_TX_0_BASE, DPTX0_REG_MSA_VTOTAL  ,1125);
IOWR(DISPLAYPORT_TX_0_BASE, DPTX0_REG_MSA_HSP     ,0);// );
IOWR(DISPLAYPORT_TX_0_BASE, DPTX0_REG_MSA_VSP     ,0);// );
IOWR(DISPLAYPORT_TX_0_BASE, DPTX0_REG_MSA_HSW     ,44);//);
IOWR(DISPLAYPORT_TX_0_BASE, DPTX0_REG_MSA_VSW     ,5);
IOWR(DISPLAYPORT_TX_0_BASE, DPTX0_REG_MSA_HSTART  ,88 + 44);//
IOWR(DISPLAYPORT_TX_0_BASE, DPTX0_REG_MSA_VSTART  ,5 + 4);//
/*IOWR(DISPLAYPORT_TX_0_BASE, DPTX0_REG_MSA_HWIDTH  ,);
IOWR(DISPLAYPORT_TX_0_BASE, DPTX0_REG_MSA_VHEIGHT ,);
IOWR(DISPLAYPORT_TX_0_BASE, DPTX0_REG_MSA_MISC0   ,);
IOWR(DISPLAYPORT_TX_0_BASE, DPTX0_REG_MSA_MISC1   ,);*/
IOWR(DISPLAYPORT_TX_0_BASE, DPTX0_REG_MSA_COLOUR  ,0b0000010000001);
/*IOWR(DISPLAYPORT_TX_0_BASE, DPTX0_REG_VBID        ,);
IOWR(DISPLAYPORT_TX_0_BASE, DPTX0_REG_CRC_R       ,);
IOWR(DISPLAYPORT_TX_0_BASE, DPTX0_REG_CRC_G       ,);
IOWR(DISPLAYPORT_TX_0_BASE, DPTX0_REG_CRC_B       ,);
IOWR(DISPLAYPORT_TX_0_BASE, DPTX0_REG_AUD_CONTROL ,);*/

printf("MSA HTOTAL  %x\n", IORD(DISPLAYPORT_TX_0_BASE,DPTX0_REG_MSA_HTOTAL )); 
printf("MSA VTOTAL  %x\n", IORD(DISPLAYPORT_TX_0_BASE,DPTX0_REG_MSA_VTOTAL )); 
printf("MSA HSP     %x\n", IORD(DISPLAYPORT_TX_0_BASE,DPTX0_REG_MSA_HSP    )); 
printf("MSA VSP     %x\n", IORD(DISPLAYPORT_TX_0_BASE,DPTX0_REG_MSA_VSP    )); 
printf("MSA HSW     %x\n", IORD(DISPLAYPORT_TX_0_BASE,DPTX0_REG_MSA_HSW    )); 
printf("MSA VSW     %x\n", IORD(DISPLAYPORT_TX_0_BASE,DPTX0_REG_MSA_VSW    )); 
printf("MSA HSTART  %x\n", IORD(DISPLAYPORT_TX_0_BASE,DPTX0_REG_MSA_HSTART )); 
printf("MSA VSTART  %x\n", IORD(DISPLAYPORT_TX_0_BASE,DPTX0_REG_MSA_VSTART )); 
printf("MSA HWIDTH  %x\n", IORD(DISPLAYPORT_TX_0_BASE,DPTX0_REG_MSA_HWIDTH )); 
printf("MSA VHEIGHT %x\n", IORD(DISPLAYPORT_TX_0_BASE,DPTX0_REG_MSA_VHEIGHT)); 
printf("MSA MISC0   %x\n", IORD(DISPLAYPORT_TX_0_BASE,DPTX0_REG_MSA_MISC0  )); 
printf("MSA MISC1   %x\n", IORD(DISPLAYPORT_TX_0_BASE,DPTX0_REG_MSA_MISC1  )); 
printf("MSA COLOUR  %x\n", IORD(DISPLAYPORT_TX_0_BASE,DPTX0_REG_MSA_COLOUR )); 
printf("VBID        %x\n", IORD(DISPLAYPORT_TX_0_BASE,DPTX0_REG_VBID       )); 
printf("CRC_R       %x\n", IORD(DISPLAYPORT_TX_0_BASE,DPTX0_REG_CRC_R      )); 
printf("CRC_G       %x\n", IORD(DISPLAYPORT_TX_0_BASE,DPTX0_REG_CRC_G      )); 
printf("CRC_B       %x\n", IORD(DISPLAYPORT_TX_0_BASE,DPTX0_REG_CRC_B      )); 
printf("AUD_CTR     %x\n", IORD(DISPLAYPORT_TX_0_BASE,DPTX0_REG_AUD_CONTROL)); 

printf("CONTROLL    %x\n", IORD(DISPLAYPORT_TX_0_BASE,DPTX_REG_TX_CONTROL));
printf("STATUS      %x\n", IORD(DISPLAYPORT_TX_0_BASE,DPTX_REG_TX_STATUS));
printf("VERSION     %x\n", IORD(DISPLAYPORT_TX_0_BASE,DPTX_REG_TX_VERSION));

printf("AUX CMD     %x\n", IORD(DISPLAYPORT_TX_0_BASE,DPTX_REG_AUX_COMMAND));
printf("AUX CTRL    %x\n", IORD(DISPLAYPORT_TX_0_BASE,DPTX_REG_AUX_CONTROL));


printf("VALID VID   %x\n", 1 & (IORD(DISPLAYPORT_TX_0_BASE,DPTX0_REG_VBID)>>7));

	int link_bw = 0;
	while (1)
	{
		check_displayport_status();
		IOWR(DISPLAYPORT_TX_0_BASE, DPTX0_REG_MSA_HTOTAL, 2200);// this is needed others are obligatory 
	    IOWR(DISPLAYPORT_TX_0_BASE, DPTX0_REG_MSA_HTOTAL, 2200);//
	    IOWR(DISPLAYPORT_TX_0_BASE, DPTX0_REG_MSA_VTOTAL, 1125);//
	    IOWR(DISPLAYPORT_TX_0_BASE, DPTX0_REG_MSA_HSP, 0);//
	    IOWR(DISPLAYPORT_TX_0_BASE, DPTX0_REG_MSA_VSP, 0);//
	    IOWR(DISPLAYPORT_TX_0_BASE, DPTX0_REG_MSA_HSW, 44);//
	    IOWR(DISPLAYPORT_TX_0_BASE, DPTX0_REG_MSA_VSW, 5);//
	    IOWR(DISPLAYPORT_TX_0_BASE, DPTX0_REG_MSA_HSTART, 88 + 44);//
	    IOWR(DISPLAYPORT_TX_0_BASE, DPTX0_REG_MSA_VSTART, 5 + 4);//
		if(link_bw != btc_dptx_link_bw(0)) printf("DP BW %x\n", btc_dptx_link_bw(0)); //Print on change
		link_bw = btc_dptx_link_bw(0);
	}

	return 0;
}