#include "sample_common.h"
#include "com.h"
#include "cap_common.h"

#include "via/viamgr.h"
#include "pgp/pgpmgr.h"
#include "dsp/dspmgr.h"

extern int32_t gCAPErrCode;		/* エラーチェック用 */

	int32_t CAP_FrameCalc_Demo(uint32_t mode)
{
	int32_t idx;
	int32_t rc = AG903_ENONE;
	static volatile int32_t pipe_num = -1;
	char input[2];
	uint32_t	status;
	uint32_t	loop;

	AG903_PgpMgrPipelineHandle	*pgp;
	AG903_ViaMgrInputHandle		*via	= NULL;
	AG903_VidMgrOutputHandle	*vid	= NULL;
	AG903_PgpMgrVRAMInputHandle	*vram	= NULL;
	AG903_PgpMgrFrameCalcHandle	*fcal	= NULL;

	AG903_DSPMgrHandle			*dsp;
	AG903_DSPMgrWinAttribute	*attr[2];
	AG903_DSPMgrWindowParam		win_param = {0};
	CAP_Framebuffer				framebuffer[2];

	if (mode >= CAP_MODE_MAX) {
		CAP_ErrPrintf("ERROR: INVALID MODE. ABORT.\r\n");
		return -AG903_EINVAL;
	}

	if(CAP_MODE_VIA_LVDS == mode || CAP_MODE_VIA_CMOS == mode) {
		via = CAP_GetVIAHandle(0);
		framebuffer[0].size = CAP_ANALOG_IN_WIDTH*CAP_ANALOG_IN_HEIGHT*4;
		framebuffer[1].size = CAP_ANALOG_IN_WIDTH*CAP_ANALOG_IN_HEIGHT*4;
	}
	else {
		vid = CAP_GetVIDOutHandle();
		framebuffer[0].size = CAP_DIGITAL_IN_WIDTH*CAP_DIGITAL_IN_HEIGHT*4;
		framebuffer[1].size = CAP_DIGITAL_IN_WIDTH*CAP_DIGITAL_IN_HEIGHT*4;
	}

	framebuffer[0].buf = sys_memalign(framebuffer[0].size, 0x1000);
	framebuffer[1].buf = sys_memalign(framebuffer[1].size, 0x1000);
	if (framebuffer[0].buf == NULL || framebuffer[1].buf == NULL) {
		CAP_ErrPrintf("ERROR: Framebuffer allocate failed.\r\n");
		rc = -AG903_ENOMEM;
		return rc;
	}
	sys_memset(framebuffer[0].buf, 0, framebuffer[0].size);
	sys_memset(framebuffer[1].buf, 0, framebuffer[1].size);
	rc = CAP_SetupDisplay(mode);
	if (CAP_MODE_VIA_LVDS == mode || CAP_MODE_VID == mode) {
		dsp = CAP_GetDSPHandle(1);
	} else {
		dsp = CAP_GetDSPHandle(0);
	}

	/* ウィンドウ0 */
	AG903_DSPMgrGetAttribute(dsp, 0, &attr[0]);
	sys_memset(attr[0], 0, 0x20);
	attr[0]->position_x					= (CAP_DISPLAY_WIDTH>>1) - CAP_WINDOW_WIDTH;
	attr[0]->position_y					= (CAP_DISPLAY_HEIGHT - CAP_WINDOW_HEIGHT)>>1; /* センタリング */
	attr[0]->destination_width			= CAP_WINDOW_WIDTH;
	attr[0]->destination_height			= CAP_WINDOW_HEIGHT;
	attr[0]->framebuffer_base			= (uint32_t)framebuffer[0].buf;
	attr[0]->conf.framebuffer_format	= AG903_DSP_FFMT_X8R8G8B8;
	attr[0]->conf.default_alpha			= 0xFF;
	attr[0]->conf.valid					= true;
	if(CAP_MODE_VIA_LVDS == mode || CAP_MODE_VIA_CMOS == mode) {
	    attr[0]->source_height	= CAP_ANALOG_IN_HEIGHT;
	    attr[0]->source_width	= CAP_ANALOG_IN_WIDTH;
	    attr[0]->hrz_size		= attr[0]->source_width<<2;
	}
	else {
	    attr[0]->source_height	= CAP_DIGITAL_IN_HEIGHT;
	    attr[0]->source_width	= CAP_DIGITAL_IN_WIDTH;
	    attr[0]->hrz_size		= attr[0]->source_width<<2;
	}

	/* ウィンドウ1 */
	AG903_DSPMgrGetAttribute(dsp, 1, &attr[1]);
	*attr[1] = *attr[0];
	attr[1]->position_x = attr[0]->position_x + attr[0]->destination_width;
	attr[1]->framebuffer_base = (uint32_t)framebuffer[1].buf;

	NOERRCHK(AG903_ENONE, AG903_DSPMgrSetAttribute(dsp, 0));
	NOERRCHK(AG903_ENONE, AG903_DSPMgrSetAttribute(dsp, 1));

	/* ウィンドウ設定 */
	win_param.background			= 0xFFFFFFFF;
	win_param.num_attr				= 2;
	win_param.num_config			= 2;
	win_param.update_timing			= AG903_DSP_ATTR_END_OF_VSYNC;
	win_param.window_attr_base		= (uint32_t)attr[0];
	win_param.window_attr_update	= true;
	NOERRCHK(AG903_ENONE, AG903_DSPMgrSetWindowParam(dsp, &win_param));

	AG903_DSPMgrEnable(dsp);
	sys_dlytsk(1000); /* ディスプレイの安定待ち */

	/* PGP設定 */
	rc = AG903_PgpMgrGetPipelineHandle(1, &pgp);

	if(CAP_MODE_VIA_LVDS == mode || CAP_MODE_VIA_CMOS == mode) { /* VIA入力 */
		/*--- パイプラインの設定:入力段 ---*/
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetInputPort(pgp, via));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamInputMode(pgp, AG903_PGP_MGR_INPUT_MD_VSYNC));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamInputHSyncDelay(pgp, CAP_HSYNC_DELAY));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamInputFormat(pgp, AG903_PGP_MGR_INPUT_FMT_YCBCR422_W_IP));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamInputSize(pgp, CAP_ANALOG_IN_HEIGHT>>1, CAP_ANALOG_IN_WIDTH));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamInputPosition(pgp, CAP_VIA_VPOS_OFFSET, CAP_VIA_HPOS_OFFSET));
		/*--- パイプラインの設定:フィルタ段 ---*/
		/* 色空間変換: YUV->RGB */
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetConfigPresetColorSpace(pgp, AG903_PGP_MGR_CSC_YUV_BT_601_LIMIT_TO_RGB));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrEnableParamColorSpace(pgp, true));
		/*--- パイプラインの設定:出力段 ---*/
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamOutputFormat(pgp, AG903_PGP_MGR_OUTPUT_FMT_RGB888_TO_RGB888_32BIT));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetConfigOutputBaseAddr(pgp, framebuffer[0].buf));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamOutputAddrDim(pgp, AG903_PGP_MGR_ADDR_DIMENTION_2));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamOutputStride(pgp, CAP_ANALOG_IN_WIDTH << 2));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetConfigOutputInterlaceFrame(pgp, AG903_PGP_MGR_OUTPUT_FRAME_SAVE_FIELD));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetConfigOutputLimit(pgp, AG903_PGP_MGR_LIMIT_0_255));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetConfigOutputFramePadding(pgp, 0xFF));
	}
	else {
		/*--- パイプラインの設定:入力段 ---*/
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetInputPort(pgp, vid));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamInputMode(pgp, AG903_PGP_MGR_INPUT_MD_VSYNC));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamInputHSyncDelay(pgp, CAP_HSYNC_DELAY));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamInputFormat(pgp, AG903_PGP_MGR_INPUT_FMT_RGB888));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamInputSize(pgp, CAP_DIGITAL_IN_HEIGHT, CAP_DIGITAL_IN_WIDTH));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamInputPosition(pgp, CAP_VID_VPOS_OFFSET, CAP_VID_HPOS_OFFSET));
		/* パイプラインの設定:出力段 */
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamOutputFormat(pgp, AG903_PGP_MGR_OUTPUT_FMT_RGB888_TO_RGB888_32BIT));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetConfigOutputBaseAddr(pgp, framebuffer[0].buf));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamOutputAddrDim(pgp, AG903_PGP_MGR_ADDR_DIMENTION_2));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamOutputStride(pgp, CAP_DIGITAL_IN_WIDTH<<2));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetConfigOutputLimit(pgp, AG903_PGP_MGR_LIMIT_0_255));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetConfigOutputFramePadding(pgp, 0xFF));
	}

	/* パイプライン割り当て */
	pipe_num = AG903_PgpMgrAssignPipeline(pgp);
	CAP_Printf("[PGP] Pipeline #%u assigned.\r\n", pipe_num);

	/* キャプチャ開始 */
	NOERRCHK(AG903_ENONE, AG903_PgpMgrExecPipeline(pgp, AG903_PGP_MGR_CMD_EXEC_CONTINUE));

	CAP_Printf("Press enter key to start calculation.");
	COM_GetNum_Wait(input, sizeof(input));

	/* キャプチャ停止 */
	NOERRCHK(AG903_ENONE, AG903_PgpMgrExecPipeline(pgp, AG903_PGP_MGR_CMD_IDLE));

	/* パイプライン停止待ち */
	for(loop=0; loop<CAP_PIPESTOP_TIMEOUT; loop++) {
		NOERRCHK(AG903_ENONE, AG903_PgpMgrGetPipelineStatus(pgp, NULL, NULL, NULL, NULL, &status));
		if(AG903_PGP_MGR_STATE_IDLE == status) {
			break;
		}
		sys_dlytsk(10);
	}
	if(CAP_PIPESTOP_TIMEOUT<=loop) {
		CAP_ErrPrintf("ERROR: Pipeline Stop Failure.\r\n");
	}

	/* パイプライン割り当て解除 */
	NOERRCHK(AG903_ENONE, AG903_PgpMgrReleaseAssignment(pgp));
	CAP_Printf("[PGP] Pipeline #%u released.\r\n", pipe_num);

	if(CAP_MODE_VIA_LVDS == mode || CAP_MODE_VIA_CMOS == mode) { /* VIA入力 */
		/*--- パイプラインの設定:入力段 ---*/
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetInputPort(pgp, via));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamInputMode(pgp, AG903_PGP_MGR_INPUT_MD_VSYNC));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamInputHSyncDelay(pgp, CAP_HSYNC_DELAY));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamInputFormat(pgp, AG903_PGP_MGR_INPUT_FMT_YCBCR422_W_IP));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamInputSize(pgp, CAP_ANALOG_IN_HEIGHT>>1, CAP_ANALOG_IN_WIDTH));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamInputPosition(pgp, CAP_VIA_VPOS_OFFSET, CAP_VIA_HPOS_OFFSET));
		/*--- パイプラインの設定:フィルタ段 ---*/
		/* 色空間変換: YUV->RGB */
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetConfigPresetColorSpace(pgp, AG903_PGP_MGR_CSC_YUV_BT_601_LIMIT_TO_RGB));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrEnableParamColorSpace(pgp, true));
		/*--- パイプラインの設定:出力段 ---*/
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamOutputFormat(pgp, AG903_PGP_MGR_OUTPUT_FMT_RGB888_TO_RGB888_32BIT));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamOutputAddrDim(pgp, AG903_PGP_MGR_ADDR_DIMENTION_2));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamOutputStride(pgp, CAP_ANALOG_IN_WIDTH << 2));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetConfigOutputBaseAddr(pgp, framebuffer[1].buf));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetConfigOutputInterlaceFrame(pgp, AG903_PGP_MGR_OUTPUT_FRAME_SAVE_FIELD));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetConfigOutputLimit(pgp, AG903_PGP_MGR_LIMIT_0_255));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetConfigOutputFramePadding(pgp, 0xFF));

		/*--- VRAM入力の設定 ---*/
		NOERRCHK(AG903_ENONE, AG903_PgpMgrGetVRAMInputHandle(1, &vram));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamVRAMInputScan(vram, AG903_PGP_MGR_SCAN_PROGRESSIVE));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamVRAMInputAddrDim(vram, AG903_PGP_MGR_ADDR_DIMENTION_2));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamVRAMSyncSignal(vram, 62, 16, 60, 6, 9, 30-1));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamVRAMFormat(vram, AG903_PGP_MGR_VI_FMT_RGB888_32));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamVRAMStride(vram, CAP_ANALOG_IN_WIDTH << 2));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamVRAMSize(vram, CAP_ANALOG_IN_WIDTH, CAP_ANALOG_IN_HEIGHT));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetConfigVRAMBaseAddr(vram, framebuffer[0].buf));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetConfigVRAMValidTime(vram, 1, AG903_PGP_MGR_MAX_VI_VALIDATE, 0));
	}
	else {
		/*--- パイプラインの設定:入力段 ---*/
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetInputPort(pgp, vid));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamInputMode(pgp, AG903_PGP_MGR_INPUT_MD_VSYNC));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamInputHSyncDelay(pgp, CAP_HSYNC_DELAY));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamInputFormat(pgp, AG903_PGP_MGR_INPUT_FMT_RGB888));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamInputSize(pgp, CAP_DIGITAL_IN_HEIGHT, CAP_DIGITAL_IN_WIDTH));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamInputPosition(pgp, CAP_VID_VPOS_OFFSET, CAP_VID_HPOS_OFFSET));
		/*--- パイプラインの設定:出力段 ---*/
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamOutputFormat(pgp, AG903_PGP_MGR_OUTPUT_FMT_RGB888_TO_RGB888_32BIT));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetConfigOutputBaseAddr(pgp, framebuffer[1].buf));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamOutputAddrDim(pgp, AG903_PGP_MGR_ADDR_DIMENTION_2));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamOutputStride(pgp, CAP_DIGITAL_IN_WIDTH<<2));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetConfigOutputFramePadding(pgp, 0xFF));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetConfigOutputLimit(pgp, AG903_PGP_MGR_LIMIT_0_255));

		/*--- VRAM入力の設定 ---*/
		NOERRCHK(AG903_ENONE, AG903_PgpMgrGetVRAMInputHandle(1, &vram));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamVRAMInputScan(vram, AG903_PGP_MGR_SCAN_PROGRESSIVE));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamVRAMInputAddrDim(vram, AG903_PGP_MGR_ADDR_DIMENTION_2));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetConfigVRAMValidTime(vram, 1, AG903_PGP_MGR_MAX_VI_VALIDATE, 0));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamVRAMFormat(vram, AG903_PGP_MGR_VI_FMT_RGB888_32));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetConfigVRAMBaseAddr(vram, framebuffer[0].buf));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamVRAMStride(vram, CAP_DIGITAL_IN_WIDTH<<2));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamVRAMSyncSignal(vram, 62, 16, 60, 6, 9, 30-1));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamVRAMSize(vram, CAP_DIGITAL_IN_WIDTH, CAP_DIGITAL_IN_HEIGHT));
	}

	/*--- フレーム間演算の設定 ---*/
	/* ハンドル取得：[master]キャプチャパイプライン,  [slave]VRAM入力 */
	NOERRCHK(AG903_ENONE, AG903_PgpMgrGetFrameCalcHandle(&fcal, pgp, vram));
	/* 処理順序：演算処理→フレーム間演算 */
	NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamFrameCalcOrder(pgp, AG903_PGP_MGR_IFCODR_OTHER_TO_FRAME));
	/* パラメータ設定：ADD */
	NOERRCHK(AG903_ENONE, AG903_PgpMgrSetConfigFrameCalcParameter(fcal,
																  AG903_PGP_MGR_SCAN_PROGRESSIVE,
																  AG903_PGP_MGR_IFC_LIMIT_0_255,
																  AG903_PGP_MGR_IFC_SLAVE_VRAM_UI,
																  AG903_PGP_MGR_IFC_OP_ABS));
	/* ゲイン設定：[master] 1, [slave] 1 */
	NOERRCHK(AG903_ENONE, AG903_PgpMgrSetConfigFrameCalcGain(fcal, 1, 1, 0, 0));

	/* パイプライン割り当て */
	pipe_num = AG903_PgpMgrAssignPipeline(fcal);
	CAP_Printf("[PGP] Pipeline #%u assigned.\r\n", pipe_num);

	/* フレーム間演算開始 */
	//NOERRCHK(AG903_ENONE, AG903_PgpMgrExecVRAMInput(vram, AG903_PGP_MGR_CMD_NONE));
	NOERRCHK(AG903_ENONE, AG903_PgpMgrExecPipeline(fcal, AG903_PGP_MGR_CMD_EXEC_CONTINUE));

	CAP_Printf("Press enter key to exit.");
	COM_GetNum_Wait(input, sizeof(input));

	/* フレーム間演算停止 */
	NOERRCHK(AG903_ENONE, AG903_PgpMgrExecPipeline(fcal, AG903_PGP_MGR_CMD_IDLE));

	/* パイプライン停止待ち */
	for(loop=0; loop<CAP_PIPESTOP_TIMEOUT; loop++) {
		NOERRCHK(AG903_ENONE, AG903_PgpMgrGetPipelineStatus(pgp, NULL, NULL, NULL, NULL, &status));
		if(AG903_PGP_MGR_STATE_IDLE == status) {
			break;
		}
		sys_dlytsk(10);
	}
	if(CAP_PIPESTOP_TIMEOUT<=loop) {
		CAP_ErrPrintf("ERROR: Pipeline Stop Failure.\r\n");
	}

	/* 各ハンドル解放 */
	NOERRCHK(AG903_ENONE, AG903_PgpMgrReleaseAssignment(fcal));
	NOERRCHK(AG903_ENONE, AG903_PgpMgrReleaseFrameCalcHandle(fcal));
	NOERRCHK(AG903_ENONE, AG903_PgpMgrReleasePipelineHandle(pgp));
	NOERRCHK(AG903_ENONE, AG903_PgpMgrReleaseVRAMInputHandle(vram));
	CAP_Printf("[PGP] Pipeline #%u released.\r\n", pipe_num);

	/* 終了処理 */
	for(idx=0; idx<(int32_t)(sizeof(framebuffer)/sizeof(framebuffer[0])); idx++)
		if(framebuffer[idx].buf != NULL) sys_free(framebuffer[idx].buf);

	NOERRCHK(AG903_ENONE, CAP_ReleaseDisplay());

	return AG903_ENONE;
}
