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

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

/* サンプル固有定義 */
#define CAP_HISTOGRAM_WIDTH		256			/* ヒストグラム幅   [px] */
#define CAP_HISTOGRAM_HEIGHT	256			/* ヒストグラム高さ [px] */
#define CAP_COLOR_TRANCE		0xFFFF00FF	/* 透過色 */
#define CAP_COLOR_SATURATION	0xFF00FF00	/* S成分の色 */
#define CAP_COLOR_VALUE			0xFFDDDDDD	/* V成分の色 */
#define CAP_WINDOW_NUM			4			/* CAP + H,S,V */

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

static void *gHistogram = NULL;
static CAP_Framebuffer gFramebuffer[CAP_WINDOW_NUM];
static AG903_PgpMgrPipelineHandle *pgp;

/* ヒストグラム完了割り込みハンドラ */
static void pipeline_done_handler(int32_t id, void *handle);
static void histogram_done_handler(int32_t id, void *handle);

int32_t CAP_Histogram_Demo(uint32_t mode)
{
	int32_t 	pipe_num = -1;
	int32_t 	idx;
	uint32_t	in_width;
	uint32_t	in_height;
	uint32_t	status;
	uint32_t	loop;

	AG903_ViaMgrInputHandle  *via = NULL;
	AG903_VidMgrOutputHandle *vid = NULL;
	AG903_DSPMgrHandle *dsp;
	AG903_DSPMgrWinAttribute *attr[CAP_WINDOW_NUM];
	AG903_DSPMgrWindowParam win_param = {0};

	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);
		in_width	= CAP_ANALOG_IN_WIDTH;
		in_height	= CAP_ANALOG_IN_HEIGHT;
	}
	else {
		vid = CAP_GetVIDOutHandle();
		in_width	= CAP_DIGITAL_IN_WIDTH;
		in_height	= CAP_DIGITAL_IN_HEIGHT;
	}

	/* ヒストグラムサイズ制限 */
	if(CAP_HISTOGRAM_MAX_WIDTH < in_width) {
		in_width	= CAP_HISTOGRAM_MAX_WIDTH;
	}
	if(CAP_HISTOGRAM_MAX_HEIGHT < in_height) {
		in_height	= CAP_HISTOGRAM_MAX_HEIGHT;
	}

	/* ヒストグラム結果用メモリ確保 (8バイト境界) */
	gHistogram = sys_memalign(256*3, 8);
	if (gHistogram == NULL) {
		CAP_ErrPrintf("ERROR: Framebuffer allocate failed.\r\n");
		return -AG903_ENOMEM;
	}

	/* フレームバッファ確保 */
	sys_memset(gFramebuffer, 0, sizeof(gFramebuffer));
	for(idx=0; idx<(int32_t)(sizeof(gFramebuffer)/sizeof(gFramebuffer[0])); idx++) {
		if(idx == 0)
			gFramebuffer[idx].size = in_width*in_height*4;
		else
			gFramebuffer[idx].size = CAP_HISTOGRAM_WIDTH*CAP_HISTOGRAM_HEIGHT*4;
	}
	for(idx=0; idx<(int32_t)(sizeof(gFramebuffer)/sizeof(gFramebuffer[0])); idx++) {
		gFramebuffer[idx].buf = sys_memalign(gFramebuffer[idx].size, 0x1000);
		if (gFramebuffer[idx].buf == NULL) {
			CAP_ErrPrintf("ERROR: Framebuffer allocate failed.\r\n");
			return -AG903_ENOMEM;
		}
		sys_memset(gFramebuffer[idx].buf, 0, gFramebuffer[idx].size);
	}

	/* 表示共通設定 */
	NOERRCHK(AG903_ENONE, CAP_SetupDisplay(mode));
	if (CAP_MODE_VIA_LVDS == mode || CAP_MODE_VID == mode) {
		dsp = CAP_GetDSPHandle(1);
	} else {
		dsp = CAP_GetDSPHandle(0);
	}

	/* ウィンドウ0 */
	idx = 0;
	AG903_DSPMgrGetAttribute(dsp, idx, &attr[idx]);
	sys_memset(attr[idx], 0, 0x20);
	attr[idx]->destination_width		= CAP_WINDOW_WIDTH;
	attr[idx]->position_x				= ((CAP_DISPLAY_WIDTH - CAP_WINDOW_WIDTH)>>1) - (attr[idx]->destination_width/2);
	attr[idx]->destination_height		= CAP_WINDOW_HEIGHT;
	attr[idx]->position_y				= (CAP_DISPLAY_HEIGHT - CAP_WINDOW_HEIGHT)>>1;
	attr[idx]->framebuffer_base			= (uint32_t)gFramebuffer[0].buf;
	attr[idx]->conf.default_alpha		= 0xFF;
	attr[idx]->conf.valid				= true;
	attr[idx]->source_width				= in_width;
	attr[idx]->source_height			= in_height;
	attr[idx]->hrz_size					= attr[idx]->source_width<<2;
	attr[idx]->conf.framebuffer_format	= AG903_DSP_FFMT_X8R8G8B8;
	NOERRCHK(AG903_ENONE, AG903_DSPMgrSetAttribute(dsp, idx));

	/* ウィンドウ1-3 */
	for(idx=1; idx<CAP_WINDOW_NUM; idx++) {
		AG903_DSPMgrGetAttribute(dsp, idx, &attr[idx]);
		sys_memset(attr[idx], 0, 0x20);
		attr[idx]->source_height	= CAP_HISTOGRAM_HEIGHT;
		attr[idx]->source_width		= CAP_HISTOGRAM_WIDTH;
		switch(idx) {
			case 1: /* Hue */
				attr[idx]->position_x	 = attr[0]->position_x + attr[0]->destination_width;
				attr[idx]->position_y	 = attr[0]->position_y;
				break;
			case 2: /* Saturation */
				attr[idx]->position_x	 = attr[0]->position_x + attr[0]->destination_width;
				attr[idx]->position_y	 = attr[0]->position_y + attr[idx-1]->destination_height;
				break;
			case 3: /* Value */
				attr[idx]->position_x	 = attr[0]->position_x + attr[0]->destination_width + attr[idx-1]->destination_width;
				attr[idx]->position_y	 = attr[0]->position_y + attr[idx-1]->destination_height;
				break;
			default:
				break;
		}
		attr[idx]->hrz_size					= CAP_HISTOGRAM_WIDTH<<2;
		attr[idx]->destination_height		= attr[0]->destination_height>>1;
		attr[idx]->destination_width		= attr[0]->destination_width>>1;
		attr[idx]->framebuffer_base			= (uint32_t)gFramebuffer[idx].buf;
		attr[idx]->transparent_color_a		= CAP_COLOR_TRANCE >> 24 & 0xFF;
		attr[idx]->transparent_color_r		= CAP_COLOR_TRANCE >> 16 & 0xFF;
		attr[idx]->transparent_color_g		= CAP_COLOR_TRANCE >>  8 & 0xFF;
		attr[idx]->transparent_color_b		= CAP_COLOR_TRANCE >>  0 & 0xFF;

		attr[idx]->conf.framebuffer_format	= AG903_DSP_FFMT_X8R8G8B8;
		attr[idx]->conf.biliner				= false;
		attr[idx]->conf.default_alpha		= 0xFF;
		attr[idx]->conf.transparent_en  	= true;
		attr[idx]->conf.transparent_en_a	= true;
		attr[idx]->conf.transparent_en_r	= true;
		attr[idx]->conf.transparent_en_g	= true;
		attr[idx]->conf.transparent_en_b	= true;
		attr[idx]->conf.valid				= true;

		NOERRCHK(AG903_ENONE, AG903_DSPMgrSetAttribute(dsp, idx));
	}

	/* ウィンドウ設定 */
	win_param.background			= 0x00000000;
	win_param.num_attr				= CAP_WINDOW_NUM;
	win_param.num_config			= CAP_WINDOW_NUM;
	win_param.update_timing			= AG903_DSP_ATTR_START_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設定 */
	NOERRCHK(AG903_ENONE, AG903_PgpMgrGetPipelineHandle(1, &pgp));

	/* 割り込みハンドラの設定 */
	NOERRCHK(AG903_ENONE, AG903_PgpMgrSetIntrCallbackFunc(pgp, AG903_PGP_MGR_INTR_HGM_DONE, histogram_done_handler));
	NOERRCHK(AG903_ENONE, AG903_PgpMgrSetIntrCallbackFunc(pgp, AG903_PGP_MGR_INTR_DONE, pipeline_done_handler));

	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, in_height>>1, 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));

		/* HSV変換1 */
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetConfigHSVHLSConversion(pgp, AG903_PGP_MGR_CONVERT_HSV));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrEnableParamHSV(pgp, true));

		/* ヒストグラム */
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetConfigHistogramParameter(pgp, 0, AG903_PGP_MGR_HGM_EXC_NONE, 0, 6, 0, 0));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetConfigHistogramSize(pgp, in_height, in_width));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetConfigHistogramPosition(pgp, 0, 0));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetConfigHistogramBaseAddr(pgp, gHistogram));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrEnableParamHistogram(pgp, true));

		/* HSV変換2 */
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetConfigThresholdHSVHLSConversion(pgp, AG903_PGP_MGR_CONVERT_HSV));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetConfigThreshold(pgp, AG903_PGP_MGR_LIMIT_NONE, false, false, false, true, false, 0, 0));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrEnableParamThresholdOp(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, 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_PgpMgrSetConfigOutputBaseAddr(pgp, gFramebuffer[0].buf));
	}
	else if (mode == CAP_MODE_VID) { /* VID入力 */
		/*--- パイプラインの設定:入力段 ---*/
		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, in_height, in_width));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetParamInputPosition(pgp, CAP_VID_VPOS_OFFSET, CAP_VID_HPOS_OFFSET));

		/*--- パイプラインの設定:フィルタ段 ---*/
		/* HSV変換1 */
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetConfigHSVHLSConversion(pgp, AG903_PGP_MGR_CONVERT_HSV));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrEnableParamHSV(pgp, true));

		/* ヒストグラム */
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetConfigHistogramParameter(pgp, 0, AG903_PGP_MGR_HGM_EXC_NONE, 0, 6, 0, 0));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetConfigHistogramSize(pgp, in_height, in_width));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetConfigHistogramPosition(pgp, 0, 0));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetConfigHistogramBaseAddr(pgp, gHistogram));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrEnableParamHistogram(pgp, true));

		/* HSV変換2 */
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetConfigThresholdHSVHLSConversion(pgp, AG903_PGP_MGR_CONVERT_HSV));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetConfigThreshold(pgp, AG903_PGP_MGR_LIMIT_NONE, false, false, false, true, false, 0, 0));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrEnableParamThresholdOp(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, in_width << 2));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetConfigOutputLimit(pgp, AG903_PGP_MGR_LIMIT_0_255));
		NOERRCHK(AG903_ENONE, AG903_PgpMgrSetConfigOutputBaseAddr(pgp, gFramebuffer[0].buf));
	} else {
		CAP_ErrPrintf("ERROR: INVALID MODE. ABORT.\r\n");
		return -AG903_EINVAL;
	}
	/* パイプライン割り当て */
	pipe_num = AG903_PgpMgrAssignPipeline(pgp);
	CAP_Printf("[PGP] Pipeline #%u assigned.\r\n", pipe_num);

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

	{
		char input[2];
		CAP_Printf("Press enter key to exit.");
		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_PgpMgrExecHistogram(pgp, AG903_PGP_MGR_HGM_CMD_DELETE));

	/* 終了処理 */

	NOERRCHK(AG903_ENONE, AG903_PgpMgrReleaseAssignment(pgp));
	NOERRCHK(AG903_ENONE, AG903_PgpMgrReleasePipelineHandle(pgp));
	CAP_Printf("[PGP] Pipeline #%u released.\r\n", pipe_num);

	NOERRCHK(AG903_ENONE, CAP_ReleaseDisplay());

	if(gHistogram != NULL) sys_free(gHistogram);
	for(idx=0; idx<(int32_t)(sizeof(gFramebuffer)/sizeof(gFramebuffer[0])); idx++)
		if(gFramebuffer[idx].buf != NULL) sys_free(gFramebuffer[idx].buf);

	return gCAPErrCode;
}

static void pipeline_done_handler(int32_t id, void *handle)
{
	((void)id);
	((void)handle);

	uint32_t queue;

	/* ヒストグラムのコマンドキューが空なら実行コマンド追加 */
	NOERRCHK(AG903_ENONE, AG903_PgpMgrGetHistogramStatus(pgp, NULL, NULL, NULL, &queue, NULL));
	if (queue == 0)
		NOERRCHK(AG903_ENONE, AG903_PgpMgrExecHistogram(pgp, AG903_PGP_MGR_HGM_CMD_EXEC));
}

static void histogram_done_handler(int32_t id, void *handle)
{
	/* ヒストグラム解析結果描画処理 */
	((void)id);
	((void)handle);

	uint8_t  *histo_v	= ((uint8_t *)gHistogram) + 0x000;
	uint8_t  *histo_s	= ((uint8_t *)gHistogram) + 0x100;
	uint8_t  *histo_h	= ((uint8_t *)gHistogram) + 0x200;
	uint32_t *glagh_v	= (uint32_t *)gFramebuffer[3].buf;
	uint32_t *glagh_s	= (uint32_t *)gFramebuffer[2].buf;
	uint32_t *glagh_h	= (uint32_t *)gFramebuffer[1].buf;

	int32_t hr, vr;
	for(hr=CAP_HISTOGRAM_WIDTH-1; hr>=0; hr--) { /* 横 */
		for(vr=CAP_HISTOGRAM_HEIGHT-1; vr>=0; vr--) { /* 縦 */
			int32_t vofs = 256*vr;

			/* 輝度 */
			if(*(histo_v + hr) <= (255-vr)) {
				*(glagh_v+(vofs + hr)) = CAP_COLOR_TRANCE;
			} else {
				uint8_t value = hr*3/4+0x40;
				*(glagh_v+(vofs + hr)) = (0xFF<<24|value<<16|value<<8|value);
			}

			/* 彩度 */
			if(*(histo_s + hr) <= (255-vr)) {
				*(glagh_s+(vofs + hr)) = CAP_COLOR_TRANCE;
			} else {
				*(glagh_s+(vofs + hr)) = (0xFFFFFFFF-(hr<<16|hr));
			}

			/* 色相 */
			if(hr < 240) {
				if(*(histo_h + hr) <= (255-vr)) {
					*(glagh_h+(vofs + hr)) = CAP_COLOR_TRANCE;
				} else {
					*(glagh_h+(vofs + hr)) = getColorByHue(hr);
				}
			} else {
				*(glagh_h+(vofs + hr)) = CAP_COLOR_TRANCE;
			}
		}
	}
}

