#include "chip.h"
#include "board.h"
#include "FreeRTOS.h"
#include "ff_stdio.h"

//wrap register
#define WRAP_CTL_INRSTATUS_REG				0x00	//00
#define WRAP_CTL_WORK_EN_REG				0x04	//01
#define WRAP_CTL_READ_ID_REG				0x08	//02
#define WRAP_CTL_INTER_CLR_REG				0x0C	//03
#define WRAP_CTL_INTER_MASK_REG				0x10	//04
#define WRAP_CTL_READ_WIDTH_S0_S1_REG0		0x14	//05
#define WRAP_CTL_READ_WIDTH_S2_S3_REG1		0x18	//06
#define WRAP_CTL_READ_WIDTH_S4_S5_REG2		0x1C	//07

#define WRAP_CTL_FISHEYE_H_W_INIT_S0		0x2C	//0b
#define WRAP_CTL_FISHEYE_H_W_INIT_S1		0x30	//0c
#define WRAP_CTL_FISHEYE_H_W_INIT_S2		0x34	//0d
#define WRAP_CTL_FISHEYE_H_W_INIT_S3		0x38	//0d
#define WRAP_CTL_FISHEYE_H_W_INIT_S4		0x3C	//0e
#define WRAP_CTL_FISHEYE_H_W_INIT_S5		0x40	//10
#define WRAP_CTL_RD_FADDR_Y_S0				0x44	//11
#define WRAP_CTL_RD_FADDR_Y_S1				0x48	//12
#define WRAP_CTL_RD_FADDR_Y_S2				0x4C	//13
#define WRAP_CTL_RD_FADDR_Y_S3				0x50	//14
#define WRAP_CTL_RD_FADDR_Y_S4				0x54	//15
#define WRAP_CTL_RD_FADDR_Y_S5				0x58	//16
#define WRAP_CTL_RD_HEIGHT_WIDETH_YUV_S0	0x5C	//17
#define WRAP_CTL_RD_HEIGHT_WIDETH_YUV_S1	0x60	//18
#define WRAP_CTL_RD_HEIGHT_WIDETH_YUV_S2	0x64	//19
#define WRAP_CTL_RD_HEIGHT_WIDETH_YUV_S3	0x68	//1a
#define WRAP_CTL_RD_HEIGHT_WIDETH_YUV_S4	0x6C	//1b
#define WRAP_CTL_RD_HEIGHT_WIDETH_YUV_S5	0x70	//1c
#define WRAP_CTL_RD_LINE_WIDE_INIT_S0		0x74	//1d
#define WRAP_CTL_RD_LINE_WIDE_INIT_S1		0x78	//1e
#define WRAP_CTL_RD_LINE_WIDE_INIT_S2		0x7c	//1f
#define WRAP_CTL_RD_LINE_WIDE_INIT_S3		0x80	//20
#define WRAP_CTL_RD_LINE_WIDE_INIT_S4		0x84	//21
#define WRAP_CTL_RD_LINE_WIDE_INIT_S5		0x88	//22

#define WRAP_CTL_RD_FADDR_ML_S0				0xA0	//28
#define WRAP_CTL_RD_FADDR_ML_S1				0xA4	//29
#define WRAP_CTL_RD_FADDR_ML_S2				0xA8	//2a
#define WRAP_CTL_RD_FADDR_ML_S3				0xAC	//2b
#define WRAP_CTL_RD_FADDR_ML_S4				0xB0	//2c
#define WRAP_CTL_RD_FADDR_ML_S5				0xB4	//2d
#define WRAP_CTL_ML_HEIGHT_WIDETH_S0		0xB8	//2e
#define WRAP_CTL_ML_HEIGHT_WIDETH_S1		0xBC	//2f
#define WRAP_CTL_ML_HEIGHT_WIDETH_S2		0xC0	//30
#define WRAP_CTL_ML_HEIGHT_WIDETH_S3		0xC4	//31
#define WRAP_CTL_ML_HEIGHT_WIDETH_S4		0xC8	//32
#define WRAP_CTL_ML_HEIGHT_WIDETH_S5		0xCC	//33

//畸变校正图坐标读取,整个坐标图在memory的一行宽度
#define WRAP_CTL_IMG_R_S1_S0				0xD0	//34
#define WRAP_CTL_IMG_R_S3_S2				0xD4	//35
#define WRAP_CTL_IMG_R_S5_S4				0xD8	//36

//畸变校正图回写memory时，memory中一行的宽度
#define WRAP_CTL_IMG_W_MRM_S1_S0			0xDC	//37
#define WRAP_CTL_IMG_W_MRM_S3_S2			0xE0	//38
#define WRAP_CTL_IMG_W_MRM_S5_S4			0xE4	//39

//畸变校正图，分段数据的存储起始地址
#define WRAP_CTL_WR_FADDR_RGB_S0			0xE8	//3a
#define WRAP_CTL_WR_FADDR_RGB_S1			0xEC	//3b
#define WRAP_CTL_WR_FADDR_RGB_S2			0xF0	//3c
#define WRAP_CTL_WR_FADDR_RGB_S3			0xF4	//3d
#define WRAP_CTL_WR_FADDR_RGB_S4			0xF8	//3e
#define WRAP_CTL_WR_FADDR_RGB_S5			0xFC	//3f

#define WRAP_MAX_WIDTH						800		//VIN_WIDTH
#define WRAP_MAX_HEIGHT						480		//VIN_HEIGHTs
#define WRAP_SEC_MAX_WHDTH					1024

//enum
enum wrap_sections {
	SECT_1 = 0x1,
	SECT_2 = 0x3,
};

//structure
struct wrap_obj {
	enum wrap_sections sect;	//分段数配置 当分段数位1时，srcwide不得超过1024
	unsigned int srcwide;
	unsigned int srcheight;
	unsigned int *srcwrap_buf;
	unsigned int *coordinatewrap_buf;
	unsigned int *deswrap_buf;
};

//global variable
static TaskHandle_t wrap_task = NULL;

//external
extern int usb_wait_stor_dev_pluged(uint32_t timeout);

static void wrap_interupt_handler(void *param)
{
	unsigned int val;

	val = readl(REGS_WRAP_BASE + WRAP_CTL_INRSTATUS_REG);
	writel(1, REGS_WRAP_BASE + WRAP_CTL_INTER_CLR_REG);

	if ((val >> 5) & 0x1) {
		printf("%s write back error!!\n", __func__);
	}

	if ((val >> 3) & 0x1) {
		if (wrap_task)
			xTaskNotifyFromISR(wrap_task, 0, eSetValueWithOverwrite, 0);
		printf("%s Fish eye work down!!\n", __func__);
	}
}

static void wrap_configure(struct wrap_obj *wo)
{
	unsigned int value = 0;
	unsigned int fml_whc_sel;
	unsigned int fml_bit_sel;
	unsigned int Pic_src_init_sel;
	unsigned int wb_bus_bvalid_dsen;
	unsigned int wb_wait_en;

	//wrap_ram_sel
	value = readl(REGS_SYSCTL_BASE + 0x1c8);
	value |= (1 << 0);
	writel(value, REGS_SYSCTL_BASE + 0x1c8);

	Pic_src_init_sel = 0;	//set srcpicture mode select bit22
	wb_bus_bvalid_dsen = 0;	//interrupt mode bit20
	wb_wait_en = 0;			//set if wait bresp
	fml_whc_sel = 1;
	fml_bit_sel = 0;

	//softreset
	value = readl(REGS_WRAP_BASE + WRAP_CTL_INRSTATUS_REG);
	value |= (1 << 1);
	writel(value, REGS_WRAP_BASE + WRAP_CTL_INRSTATUS_REG);

	value = readl(REGS_WRAP_BASE + WRAP_CTL_INRSTATUS_REG);
	value &= ~((0x3F << 8) | (0x1 << 16) | (0x1 << 17) | (0x1 << 18) | (0x1 << 20) | (0x1 << 19) | (0x1 << 22));
	value |= ((wo->sect << 8) |
		(fml_bit_sel << 16) |
		(fml_whc_sel << 17) |
		(wb_wait_en << 19) |
		(wb_bus_bvalid_dsen << 20) |
		(Pic_src_init_sel << 22));
	writel(value, REGS_WRAP_BASE + WRAP_CTL_INRSTATUS_REG);

	writel((wo->srcheight << 16) | (wo->srcwide << 0), REGS_WRAP_BASE + WRAP_CTL_FISHEYE_H_W_INIT_S0);

	//源图，分段，Y(rgb)通道的起始地址
	writel((unsigned int)wo->srcwrap_buf, REGS_WRAP_BASE + WRAP_CTL_RD_FADDR_Y_S0);

	//源图分段的高度和宽度
	writel((wo->srcwide << 16) | (wo->srcwide << 0), REGS_WRAP_BASE + WRAP_CTL_RD_HEIGHT_WIDETH_YUV_S0);

	//分段1源图的起始点在原图中的垂直位置和水平位置
	writel((0 << 16) | (0 << 0), REGS_WRAP_BASE + WRAP_CTL_RD_LINE_WIDE_INIT_S0);

	//畸变校正图坐标，分段，映射坐标的起始地址配置
	writel((unsigned int)wo->coordinatewrap_buf, REGS_WRAP_BASE + WRAP_CTL_RD_FADDR_ML_S0);

	//畸变校正图坐标，分段的高度和宽度
	writel((wo->srcheight << 16) | (wo->srcwide << 0), REGS_WRAP_BASE + WRAP_CTL_ML_HEIGHT_WIDETH_S0);

	//畸变校正图坐标读取,整个坐标图在memory的一行宽度
	writel((wo->srcwide << 16) | (wo->srcwide << 0), REGS_WRAP_BASE + WRAP_CTL_IMG_R_S1_S0);

	//畸变校正图回写memory时，memory中一行的宽度
	writel((wo->srcwide << 16) | (wo->srcwide << 0), REGS_WRAP_BASE + WRAP_CTL_IMG_W_MRM_S1_S0);

	//畸变校正图，分段，数据的存储起始地址
	writel((unsigned int)wo->deswrap_buf, REGS_WRAP_BASE + WRAP_CTL_WR_FADDR_RGB_S0);

	if (wo->sect == SECT_2) {
		writel((wo->srcheight << 16) | (wo->srcwide << 0), REGS_WRAP_BASE + WRAP_CTL_FISHEYE_H_W_INIT_S1);
		writel((unsigned int)wo->srcwrap_buf + (wo->srcwide / 2 - 4) * 4, REGS_WRAP_BASE + WRAP_CTL_RD_FADDR_Y_S1);
		writel((wo->srcheight << 16) | ((wo->srcwide / 2) << 0), REGS_WRAP_BASE + WRAP_CTL_RD_HEIGHT_WIDETH_YUV_S0);
		writel((wo->srcheight << 16) | ((wo->srcwide / 2) << 0), REGS_WRAP_BASE + WRAP_CTL_RD_HEIGHT_WIDETH_YUV_S1);
		writel((0 << 16) | ((wo->srcwide / 2 - 4) << 0), REGS_WRAP_BASE + WRAP_CTL_RD_LINE_WIDE_INIT_S1);
		writel((unsigned int)wo->coordinatewrap_buf + (wo->srcwide / 2) * 8, REGS_WRAP_BASE + WRAP_CTL_RD_FADDR_ML_S1);
		writel((wo->srcheight << 16) | ((wo->srcwide / 2) << 0), REGS_WRAP_BASE + WRAP_CTL_ML_HEIGHT_WIDETH_S0);
		writel((wo->srcheight << 16) | ((wo->srcwide / 2) << 0), REGS_WRAP_BASE + WRAP_CTL_ML_HEIGHT_WIDETH_S1);
		writel((wo->srcwide << 16) | (wo->srcwide << 0), REGS_WRAP_BASE + WRAP_CTL_IMG_R_S3_S2);
		writel((wo->srcwide << 16) | (wo->srcwide << 0), REGS_WRAP_BASE + WRAP_CTL_IMG_R_S5_S4);
		writel((wo->srcwide << 16) | (wo->srcwide << 0), REGS_WRAP_BASE + WRAP_CTL_IMG_W_MRM_S3_S2);
		writel((wo->srcwide << 16) | (wo->srcwide << 0), REGS_WRAP_BASE + WRAP_CTL_IMG_W_MRM_S5_S4);
		writel((unsigned int)wo->deswrap_buf + (wo->srcwide / 2) * 4, REGS_WRAP_BASE + WRAP_CTL_WR_FADDR_RGB_S1);
	}

	request_irq(WRAP_IRQn, 0, wrap_interupt_handler, NULL);

	writel(0, REGS_WRAP_BASE + WRAP_CTL_INTER_MASK_REG);
	value = readl(REGS_WRAP_BASE + WRAP_CTL_READ_ID_REG);
	value |= (0x55 << 12);
	writel(value, REGS_WRAP_BASE + WRAP_CTL_READ_ID_REG);
	writel(1, REGS_WRAP_BASE + WRAP_CTL_WORK_EN_REG);
}

static void wrap_display_demo(void *param)
{
	struct wrap_obj *wo = (struct wrap_obj *)param;
	uint32_t ulNotifiedValue;
	unsigned int status;
	FF_FILE *fp;
	size_t filesize;
	int rlen;

	if (!wo) {
		printf("%s Invalid wo.\n", __func__);
		goto end;
	}

	status = usb_wait_stor_dev_pluged(portMAX_DELAY);
	if (status != 0) {
		printf("%s usb_wait_stor_dev_pluged fail\n", __func__);
		goto end;
	}

	fp = ff_fopen("/usb/Distort_480x800_a888.bin", "rb");
	if (!fp) {
		printf("%s open file fail.\n", __func__);
		goto end;
	}

	filesize = ff_filelength(fp);
	rlen = ff_fread(wo->srcwrap_buf, 1, filesize, fp);
	if (rlen <= 0) {
		printf("%s read data fail.\n", __func__);
		ff_fclose(fp);
		goto end;
	}

	if (fp)
		ff_fclose(fp);

	fp = ff_fopen("/usb/outij480_800.bin", "rb");
	if (!fp) {
		printf("%s open file fail.\n", __func__);
		goto end;
	}

	filesize = ff_filelength(fp);
	rlen = ff_fread(wo->coordinatewrap_buf, 1, filesize, fp);
	if (rlen <= 0) {
		printf("%s read data fail.\n", __func__);
		ff_fclose(fp);
		goto end;
	}

	if (fp)
		ff_fclose(fp);

	wrap_configure(wo);
	xTaskNotifyWait(0x00, /* Don't clear any notification bits on entry. */
			0xffffffff, /* Reset the notification value to 0 on exit. */
			&ulNotifiedValue, /* Notified value pass out in ulNotifiedValue. */
			portMAX_DELAY);

	ark_lcd_osd_enable(LCD_VIDEO_LAYER, 0);
	ark_lcd_osd_enable(LCD_UI_LAYER, 0);
	ark_lcd_set_osd_sync(LCD_VIDEO_LAYER);
	ark_lcd_set_osd_sync(LCD_UI_LAYER);
	ark_lcd_osd_coeff_enable(LCD_VIDEO_LAYER, 1);
	ark_lcd_osd_set_coeff(LCD_VIDEO_LAYER, 0xFF);
	ark_lcd_set_osd_possition(LCD_VIDEO_LAYER, 0, 0);
	ark_lcd_set_osd_size(LCD_VIDEO_LAYER, wo->srcwide, wo->srcheight);
	ark_lcd_set_osd_format(LCD_VIDEO_LAYER, LCD_OSD_FORAMT_ARGB888);
	ark_lcd_set_osd_yaddr(LCD_VIDEO_LAYER, (unsigned int)wo->deswrap_buf);
	ark_lcd_osd_enable(LCD_VIDEO_LAYER, 1);
	ark_lcd_set_osd_sync(LCD_VIDEO_LAYER);
	ark_lcd_wait_for_vsync();

end:
	while(1)
		vTaskDelay(portMAX_DELAY);
}

void wrap_demo(void)
{
	struct wrap_obj *wo;

	wo = pvPortMalloc(sizeof(struct wrap_obj));
	if (!wo) {
		printf("%s malloc wrap_obj fail!\n", __func__);
		return;
	}
	memset(wo, 0, sizeof(struct wrap_obj));

	wo->sect = SECT_1;
	wo->srcwide = WRAP_MAX_WIDTH;
	wo->srcheight = WRAP_MAX_HEIGHT;

	//源宽度小于1024的时候用分段1，源宽度大于1024用分段2
	if (!((wo->sect == SECT_1 && wo->srcwide <= WRAP_SEC_MAX_WHDTH) || 
		(wo->sect == SECT_2 && (WRAP_SEC_MAX_WHDTH < wo->srcwide) && (wo->srcwide <= LCD_WIDTH)))) {
		printf("%s wrap set fail.\n", __func__);
		goto err;
	}

	wo->srcwrap_buf = (void *)pvPortMalloc(WRAP_MAX_WIDTH * WRAP_MAX_HEIGHT * 4);
	if (!wo->srcwrap_buf) {
		printf("%s Src wrap buf malloc memory fail.\n", __func__);
		goto err;
	}

	wo->deswrap_buf = (void *)pvPortMalloc(WRAP_MAX_WIDTH * WRAP_MAX_HEIGHT * 4);
	if (!wo->deswrap_buf) {
		printf("%s Des wrap buf malloc memory fail.\n", __func__);
		goto err;
	}

	wo->coordinatewrap_buf = (void *)pvPortMalloc(WRAP_MAX_WIDTH * WRAP_MAX_HEIGHT * 8);
	if (!wo->coordinatewrap_buf) {
		printf("%s Des wrap buf malloc memory fail.\n", __func__);
		goto err;
	}

	memset(wo->srcwrap_buf, 0, WRAP_MAX_WIDTH * WRAP_MAX_HEIGHT * 4);
	memset(wo->deswrap_buf, 0, WRAP_MAX_WIDTH * WRAP_MAX_HEIGHT * 4);
	memset(wo->coordinatewrap_buf, 0, WRAP_MAX_WIDTH * WRAP_MAX_HEIGHT * 8);

	printf(">>>srcwrap_buf 0x%x, deswrap_buf 0x%x, coordinatewrap_buf 0x%x\n",
			wo->srcwrap_buf, wo->deswrap_buf, wo->coordinatewrap_buf);
	if (xTaskCreate(wrap_display_demo, "wrapdis", configMINIMAL_STACK_SIZE * 10, (void *)wo,
		configMAX_PRIORITIES - 3, &wrap_task) != pdPASS) {
		printf("%s create wrap display task fail.\n", __func__);
		goto err;
	}

	return;
err:
	if (wo->coordinatewrap_buf)
		vPortFree(wo->coordinatewrap_buf);

	if (wo->deswrap_buf)
		vPortFree(wo->deswrap_buf);

	if (wo->srcwrap_buf)
		vPortFree(wo->srcwrap_buf);

	if (wo)
		vPortFree(wo);
}