/**
 *
 *
 */
#include <linux/prctl.h>  /* Definition of PR_* constants */
#include <sys/prctl.h>
#include <unistd.h> /* for usleep */

#include "app_common.h"
#include <VX/vxu.h>
#include <utils/perf_stats/include/app_perf_stats.h>
#include <TI/j7_kernels_imaging_aewb.h>
#include <TI/tivx_mutex.h>
#include "app_sensor_module.h"
#include "app_capture_module.h"


#define NUM_CHANNELS        		(1U)
#define CSITX_INST_ID       		(0U)
#define CSITX_INST_ID_1       		(1U)
#define CSITX_LANE_BAND_SPEED       (TIVX_CSITX_LANE_BAND_SPEED_390_TO_450_MBPS)//(TIVX_CSITX_LANE_BAND_SPEED_770_TO_870_MBPS)
#define CSIRX_LANE_BAND_SPEED       (TIVX_CAPTURE_LANE_BAND_SPEED_720_TO_800_MBPS)
#define CSITX_LANE_SPEED_MBPS       (477U)//(392U)//(600)
#define VIDEO_PIPE_NUM              (4u)
#define _PIPE_MODE                  (1u)
#define _MAX_CSITX_NODE             (2u)
#define _ENABLIE_MULTI_CSITX_NODE   (1u)

#define TEST_IMG_W          (1920u)
#define TEST_IMG_H          (1280u)
#define TEST_FILE_PATH      (256u)

typedef struct
{
	vx_context context;
    vx_graph graph;
	vx_graph graph_csitx;

	SensorObj     sensorObj;
	CaptureObj    captureObj;

	vx_object_array input_image_arr[_MAX_CSITX_NODE];

	vx_node csitx_node;
	vx_node csitx_node_2;
	vx_user_data_object csitx_config;
	vx_user_data_object csitx_config2;
		;
	int32_t lPipeNum;
	uint32_t ulEnqueueCnt;
	//
	tivx_task task;
    vx_uint32 stop_task;
    vx_uint32 stop_task_done;
	//
	tivx_task task_csitx;
    vx_uint32 stop_task_csitx;
    vx_uint32 stop_task_done_csitx;
	//
	app_perf_point_t stTimeLog_Put;
	app_perf_point_t stTimeLog_Get;
	app_perf_point_t stTimeLog_CsiTx;

	tivx_mutex mtxHndlState;
	uint8_t ucNewFrameIn;
} appTestCsiTxInfo_t;

extern uint64_t appLogGetTimeInUsec();


static appTestCsiTxInfo_t g_stTestCsiTxInfo = {0};

static void app_parameter_by_node_index(vx_graph graph, vx_node node, vx_uint32 node_parameter_index)
{
    vx_parameter parameter = vxGetParameterByIndex(node, node_parameter_index);

    vxAddParameterToGraph(graph, parameter);
    vxReleaseParameter(&parameter);
}

static int32_t app_put_csitx_data(vx_reference frame_set[])
{
	appTestCsiTxInfo_t *obj = &g_stTestCsiTxInfo;
//	uint32_t i;
	
	tivxMutexLock(obj->mtxHndlState);
	appPerfPointBegin(&obj->stTimeLog_Put);
	if (frame_set[0])
	{
		vx_image dst_img = (vx_image)vxGetObjectArrayItem(obj->input_image_arr[0], 1);

		//
		vxuSwapImageHandle_oToBrite((vx_image)frame_set[0], dst_img);
		//
		vxReleaseImage(&dst_img);
		//
		obj->ucNewFrameIn = 1;
	}

	if (_ENABLIE_MULTI_CSITX_NODE && (frame_set[1]))
	{
		vx_image dst_img = (vx_image)vxGetObjectArrayItem(obj->input_image_arr[1], 1);

		//
		vxuSwapImageHandle_oToBrite((vx_image)frame_set[1], dst_img);
		//
		vxReleaseImage(&dst_img);
		//
		obj->ucNewFrameIn = 1;
	}
	appPerfPointEnd(&obj->stTimeLog_Put);
	tivxMutexUnlock(obj->mtxHndlState);
	return 0;
}

void app_get_csitx_img(appTestCsiTxInfo_t *obj, uint32_t idx)
{
	tivxMutexLock(obj->mtxHndlState);

	if (1 == obj->ucNewFrameIn)
	{
		appPerfPointBegin(&obj->stTimeLog_Get);
		
		vx_image old_img = (vx_image)vxGetObjectArrayItem(obj->input_image_arr[0], 0);
		vx_image new_img = (vx_image)vxGetObjectArrayItem(obj->input_image_arr[0], 1);

		//
		vxuSwapImageHandle_oToBrite(old_img, new_img);
		vxReleaseImage(&old_img);
		vxReleaseImage(&new_img);
		

		if (_ENABLIE_MULTI_CSITX_NODE)
		{
			old_img = (vx_image)vxGetObjectArrayItem(obj->input_image_arr[1], 0);
			new_img = (vx_image)vxGetObjectArrayItem(obj->input_image_arr[1], 1);

			//
			vxuSwapImageHandle_oToBrite(old_img, new_img);
			vxReleaseImage(&old_img);
			vxReleaseImage(&new_img);
		}
		//reset
		obj->ucNewFrameIn = 0;

		appPerfPointEnd(&obj->stTimeLog_Get);
	}
	tivxMutexUnlock(obj->mtxHndlState);
}

static int g_randomSleep = 0;
#include <time.h>   /* for nanosleep */

static void app_run_csitx_task(void *app_var)
{
	appTestCsiTxInfo_t *obj = (appTestCsiTxInfo_t *)app_var;
	uint64_t ts_us, te_us;
	int64_t interval_us;

	prctl(PR_SET_NAME, "CSI_TX_Tsk");
	appLogPrintf("Run csi-tx graph start !!!!!!!!!!!!!!!!!!!\n");
	while (1 && !obj->stop_task_csitx)
	{
		//appPerfPointBegin(&obj->stTimeLog_CsiTx);
		ts_us = appLogGetTimeInUsec();
		//vxProcessGraph(obj->graph_csitx);
		//Get new frame
		app_get_csitx_img(obj, 0);
		te_us = appLogGetTimeInUsec();

		if (te_us > ts_us)
			interval_us = (te_us - ts_us); // us -> ms
		else
			interval_us = ((0xFFFFFFFFFFFFFFFFu - te_us) + ts_us) / 1000u; //us -> ms

		int64_t sleepTime = 33333;
		if(g_randomSleep == 1)
		{
			static int random = 0;
			random += 7;
			sleepTime += ((random%32)-16)*1000;
		}

		if (interval_us < sleepTime)
		{
			//usleep((sleepTime - interval_us));
			
			struct timespec delay_time, remain_time;
			int ret;
			
			delay_time.tv_sec  = 0;
			delay_time.tv_nsec = ((sleepTime - interval_us))*1000;
			
			do
			{
				ret = nanosleep(&delay_time, &remain_time);
				if(ret < 0 && remain_time.tv_sec > 0 && remain_time.tv_nsec > 0)
				{
					/* restart for remaining time */
					delay_time = remain_time;
				}
				else
				{
					break;
				}
			} while(1);

		}
			
		
		//appPerfPointEnd(&obj->stTimeLog_CsiTx);

		#if 0
		uint64_t tmp  = appLogGetTimeInUsec() - obj->stTimeLog_CsiTx.tmp;
		if(tmp > 35000)
		{
			appLogPrintf("@@@ tmp=%lu\n", tmp);
		}
		#endif
	}

	appLogPrintf("Run csi-tx graph end !!!!!!!!!!!!!!!!!!!\n");
	obj->stop_task_done_csitx = 1;
}

static void app_run_graph_task(void *app_var)
{
	appTestCsiTxInfo_t *obj = (appTestCsiTxInfo_t *)app_var;
	uint32_t i;

	//read test image;
	//app_read_test_img(obj);

	prctl(PR_SET_NAME, "Capture_Tsk");
	appLogPrintf("Run video graph start !!!!!!!!!!!!!!!!!!!\n");
	appStartImageSensor(obj->sensorObj.sensor_name, obj->sensorObj.ch_mask);
	while (!obj->stop_task)
	{
		if (_PIPE_MODE)
		{
			if (obj->lPipeNum < 0)
			{
				//read test image;
				//app_read_test_img(obj, obj->ulEnqueueCnt);
				vxGraphParameterEnqueueReadyRef(obj->graph, 0, (vx_reference*)&obj->captureObj.raw_image_arr[obj->ulEnqueueCnt], 1);

				++obj->ulEnqueueCnt;
				++obj->lPipeNum;
				//if (0 == obj->lPipeNum)
				//	tivxTaskWaitMsecs(33*VIDEO_PIPE_NUM);
			}
			else
			{
				vx_object_array csitx_input_arr;
				uint32_t num_refs;
				vx_reference frame_set[8] = {NULL};

				//dequeu
				vxGraphParameterDequeueDoneRef(obj->graph, 0, (vx_reference*)&csitx_input_arr, 1, &num_refs);
				//Put data
				frame_set[0] = vxGetObjectArrayItem(csitx_input_arr, 0);
				frame_set[1] = vxGetObjectArrayItem(csitx_input_arr, 1);
				app_put_csitx_data(frame_set);
				for (i = 0; i < 8; ++i)
				{
					if (frame_set[i])
						vxReleaseImage((vx_image*)&frame_set[i]);
				}
				//equeue
				vxGraphParameterEnqueueReadyRef(obj->graph, 0, (vx_reference*)&csitx_input_arr, 1);

				++obj->ulEnqueueCnt;
				++obj->lPipeNum;

				obj->ulEnqueueCnt %= VIDEO_PIPE_NUM;
				//tivxTaskWaitMsecs(33);
			}
		}
		else
		{
			vxProcessGraph(obj->graph);
			tivxTaskWaitMsecs(33);
		}

	}
	if (_PIPE_MODE)
	{
		vxWaitGraph(obj->graph);
	}
	appStopImageSensor(obj->sensorObj.sensor_name, obj->sensorObj.ch_mask);
	appLogPrintf("Run video graph end !!!!!!!!!!!!!!!!!!!\n");
	obj->stop_task_done = 1;
}

static void app_test_csitx_run(appTestCsiTxInfo_t *obj)
{
	char c;

	do
	{
		c = getchar();

		if ('x' == c)
			break;
		else if('s' == c)
		{
			app_get_csitx_img(obj, 0);
			vxProcessGraph(obj->graph_csitx);
		}
		else if('r' == c)
		{
			g_randomSleep = (g_randomSleep + 1) % 2;
			printf("g_randomSleep=%d\n", g_randomSleep);
		}
		else if ('p' == c)
		{
			#if 0
			appPerfStatsPrintAll();
			#endif
			tivx_utils_graph_perf_print(obj->graph);
			printf("\n");
			tivx_utils_graph_perf_print(obj->graph_csitx);
			printf("\n");
			appPerfPointPrint(&obj->stTimeLog_Get);
			appPerfPointPrint(&obj->stTimeLog_Put);
			appPerfPointPrint(&obj->stTimeLog_CsiTx);
			printf("\n");
			#if 0
			vx_reference refs[1];
			refs[0] = (vx_reference)obj->captureObj.raw_image_arr[0];
			tivxNodeSendCommand(obj->captureObj.node, 0u,
							TIVX_CAPTURE_PRINT_STATISTICS,
							refs, 1u);
			#endif

			//tivxNodeSendCommand(obj->csitx_node, 0u,
			//				TIVX_CAPTURE_PRINT_STATISTICS,
			//				refs, 1u);
		}

	} while (1);
	//
	obj->stop_task = 1;
	obj->stop_task_csitx = 1;
	while (!obj->stop_task_done)
		tivxTaskWaitMsecs(33);
	while (!obj->stop_task_done_csitx)
		tivxTaskWaitMsecs(33);
}

#if 1
#include <signal.h> 
#include <time.h> 

static uint64_t PRE_ts_us = 0;

void timer_thread(union sigval v) 
{ 
	//printf("@@@ =%lu\n", appLogGetTimeInUsec() - XXX_ts_us);
	uint64_t tmp = appLogGetTimeInUsec() - PRE_ts_us;
	
	PRE_ts_us = appLogGetTimeInUsec();

	appTestCsiTxInfo_t *obj = &g_stTestCsiTxInfo;

	appPerfPointBegin(&obj->stTimeLog_CsiTx);
	
	//app_get_csitx_img(obj, 0);
	vxProcessGraph(obj->graph_csitx);

	appPerfPointEnd(&obj->stTimeLog_CsiTx);

#if 1
	if(tmp > 34000 || tmp < 32000)
	{
		appLogPrintf("@@@ tmp=%lu\n", tmp);
	}
	#endif
} 
#endif

int app_test_csitx_main(vx_int32 argc, vx_char* argv[])
{
#if 1
    timer_t timerid; 
    struct sigevent evp; 
    memset(&evp, 0, sizeof(struct sigevent));       //清零初始化 

    evp.sigev_value.sival_int = 111;                //也是标识定时器的，回调函数可以获得 
    evp.sigev_notify = SIGEV_THREAD;                //线程通知的方式，派驻新线程 
    evp.sigev_notify_function = timer_thread;       //线程函数地址 

    if (timer_create(CLOCK_REALTIME, &evp, &timerid) == -1) 
    {   
        perror("fail to timer_create"); 
        exit(-1); 
    }   

    /* 第一次间隔it.it_value这么长,以后每次都是it.it_interval这么长,就是说it.it_value变0的时候会>装载it.it_interval的值 */
    struct itimerspec it; 
    it.it_interval.tv_sec = 0;//1;  // 回调函数执行频率为1s运行1次
    it.it_interval.tv_nsec = 33 * 1000 * 1000; 
    it.it_value.tv_sec = 20;     // 倒计时3秒开始调用回调函数
    it.it_value.tv_nsec = 0; 

    if (timer_settime(timerid, 0, &it, NULL) == -1) 
    {   
        perror("fail to timer_settime"); 
        exit(-1); 
    }   
#endif

	vx_status status;
	appTestCsiTxInfo_t *obj = &g_stTestCsiTxInfo;
	uint32_t i;

	status = tivxMutexCreate(&obj->mtxHndlState);
	if (VX_SUCCESS != status)
	{
		appLogPrintf("Create mutext fail !!!!!!!!!!!!!!!!!!!!!!!\n");
		return 0;
	}
	appPerfPointSetName(&obj->stTimeLog_Put, "PutData");
	appPerfPointReset(&obj->stTimeLog_Put);
	appPerfPointSetName(&obj->stTimeLog_Get, "GetData");
	appPerfPointReset(&obj->stTimeLog_Get);
	appPerfPointSetName(&obj->stTimeLog_CsiTx, "CsiTxProc");
	appPerfPointReset(&obj->stTimeLog_CsiTx);

	obj->context = vxCreateContext();
	tivxHwaLoadKernels(obj->context);
	tivxImagingLoadKernels(obj->context);
	obj->graph = vxCreateGraph(obj->context);
	obj->graph_csitx = vxCreateGraph(obj->context);

	obj->sensorObj.is_interactive = 0;
	obj->sensorObj.sensor_index = 6; //TI-913
	obj->sensorObj.num_cameras_enabled = 2;
	obj->sensorObj.ch_mask = 0x0C;
	status = app_querry_sensor(&obj->sensorObj);
	if (VX_SUCCESS != status)
	{
		appLogPrintf("app_querry_sensor fail !!\n");
		goto __exit;
	}
	appLogPrintf("Sensor name : %s, %ux%u, ch_mask = 0x%X \n", obj->sensorObj.sensor_name, obj->sensorObj.image_width, obj->sensorObj.image_height, obj->sensorObj.ch_mask);
	obj->captureObj.capture_format = 1;
	obj->captureObj.enable_error_detection = 1;
	app_init_sensor(&obj->sensorObj, "sensor_obj");
	app_init_capture(obj->context, &obj->captureObj, &obj->sensorObj, "capture_obj", VIDEO_PIPE_NUM);

	//ceate resource for csitx graph
	vx_image exemplar = vxCreateImage(obj->context, obj->sensorObj.image_width, obj->sensorObj.image_height, VX_DF_IMAGE_UYVY);
	for (i = 0; i < _MAX_CSITX_NODE; i++)
	{
		obj->input_image_arr[i] = vxCreateObjectArray(obj->context, (vx_reference)exemplar, 2); //0 : for graph process, 1 : for backup image
	}
	vxReleaseImage(&exemplar);
	//reset parameter
	obj->lPipeNum = -VIDEO_PIPE_NUM;
	obj->ulEnqueueCnt = 0;

	///create video graph
	app_create_graph_capture(obj->graph, &obj->captureObj);
	//setup pipe mode
	if (_PIPE_MODE)
	{
		uint32_t graph_parameter_index = 0;
		vx_graph_parameter_queue_params_t graph_parameters_queue_params_list[6];

		app_parameter_by_node_index(obj->graph, obj->captureObj.node, 1);
		graph_parameters_queue_params_list[graph_parameter_index].graph_parameter_index = graph_parameter_index;
	    graph_parameters_queue_params_list[graph_parameter_index].refs_list_size = VIDEO_PIPE_NUM;
	    graph_parameters_queue_params_list[graph_parameter_index].refs_list = (vx_reference*)&obj->captureObj.raw_image_arr[0];
	    graph_parameter_index++;

		status = vxSetGraphScheduleConfig(obj->graph,
                VX_GRAPH_SCHEDULE_MODE_QUEUE_AUTO,
                graph_parameter_index,
                graph_parameters_queue_params_list);
		if (VX_SUCCESS == status)
		{
			 status = tivxSetGraphPipelineDepth(obj->graph, VIDEO_PIPE_NUM);
			 if (VX_SUCCESS != status)
			 	appLogPrintf("tivxSetGraphPipelineDepth fail !!\n");
		}
	}
	
	//ceate csitx#1
	
    tivx_csitx_params_t local_csitx_config;
	uint32_t loopCnt;
	tivx_csitx_params_init(&local_csitx_config);
	local_csitx_config.numInst							= 1U;
	local_csitx_config.numCh							= NUM_CHANNELS;
	local_csitx_config.instId[0U]						= CSITX_INST_ID;
	local_csitx_config.instCfg[0U].rxCompEnable 		= (uint32_t)vx_true_e;
	local_csitx_config.instCfg[0U].rxv1p3MapEnable		= (uint32_t)vx_true_e;
	local_csitx_config.instCfg[0U].laneBandSpeed		= CSITX_LANE_BAND_SPEED;
	local_csitx_config.instCfg[0U].laneSpeedMbps		= CSITX_LANE_SPEED_MBPS;
	local_csitx_config.instCfg[0U].numDataLanes 		= 2U;//4U;
	for (loopCnt = 0U ;
		loopCnt < local_csitx_config.instCfg[0U].numDataLanes ;
		loopCnt++)
	{
		local_csitx_config.instCfg[0U].lanePolarityCtrl[loopCnt] = 0u;
	}
	for (loopCnt = 0U; loopCnt < NUM_CHANNELS; loopCnt++)
	{
		local_csitx_config.chVcNum[loopCnt]   = loopCnt;
		local_csitx_config.chInstMap[loopCnt] = CSITX_INST_ID;
	}
	obj->csitx_config = vxCreateUserDataObject(obj->context, "tivx_csitx_params_t", sizeof(tivx_csitx_params_t), &local_csitx_config);
    obj->csitx_node = tivxCsitxNode(obj->graph_csitx, obj->csitx_config, obj->input_image_arr[0]);
	vxSetNodeTarget(obj->csitx_node, VX_TARGET_STRING, TIVX_TARGET_CSITX);
	vxSetReferenceName((vx_reference)obj->csitx_node, "csitx_node_1");
	//ceate csitx#2
	if (_ENABLIE_MULTI_CSITX_NODE)
	{
    	tivx_csitx_params_t local_csitx_config2;
		tivx_csitx_params_init(&local_csitx_config2);
		local_csitx_config2.numInst							= 1U;
		local_csitx_config2.numCh							= NUM_CHANNELS;
		local_csitx_config2.instId[0U]						= CSITX_INST_ID_1;
		local_csitx_config2.instCfg[0U].rxCompEnable 		= (uint32_t)vx_true_e;
		local_csitx_config2.instCfg[0U].rxv1p3MapEnable		= (uint32_t)vx_true_e;
		local_csitx_config2.instCfg[0U].laneBandSpeed		= CSITX_LANE_BAND_SPEED;
		local_csitx_config2.instCfg[0U].laneSpeedMbps		= CSITX_LANE_SPEED_MBPS;
		local_csitx_config2.instCfg[0U].numDataLanes 		= 2U;//4U;
	
		for (loopCnt = 0U ;
			loopCnt < local_csitx_config2.instCfg[0U].numDataLanes ;
			loopCnt++)
		{
			local_csitx_config2.instCfg[0U].lanePolarityCtrl[loopCnt] = 0u;
		}
		for (loopCnt = 0U; loopCnt < NUM_CHANNELS; loopCnt++)
		{
			local_csitx_config2.chVcNum[loopCnt]   = loopCnt;
			local_csitx_config2.chInstMap[loopCnt] = CSITX_INST_ID_1;
		}

		obj->csitx_config2 = vxCreateUserDataObject(obj->context, "tivx_csitx_params_t", sizeof(tivx_csitx_params_t), &local_csitx_config2);
	    obj->csitx_node_2 = tivxCsitxNode(obj->graph_csitx, obj->csitx_config2, obj->input_image_arr[1]);
	    vxSetNodeTarget(obj->csitx_node_2, VX_TARGET_STRING, TIVX_TARGET_CSITX2);
		vxSetReferenceName((vx_reference)obj->csitx_node_2, "csitx_node_2");
	}
	
	//verif all graph
	if (VX_SUCCESS == status)
	{
		status = vxVerifyGraph(obj->graph);
		status |= vxVerifyGraph(obj->graph_csitx);
	}
	if (VX_SUCCESS == status)
	{
		tivx_task_create_params_t params;

		app_send_error_frame(&obj->captureObj);

		//csitx task
		tivxTaskSetDefaultCreateParams(&params);
		params.task_main = app_run_csitx_task;
		params.app_var = (void *)obj;
		obj->stop_task_csitx = 0;
		status |= tivxTaskCreate(&obj->task_csitx, &params);

		//Video task
   	 	tivxTaskSetDefaultCreateParams(&params);
    	params.task_main = app_run_graph_task;
		params.app_var = (void *)obj;
		obj->stop_task = 0;
    	status |= tivxTaskCreate(&obj->task, &params);

		//
		if (VX_SUCCESS == status)
		{
			app_test_csitx_run(obj);
		}
	}
	else
	{
		appLogPrintf("vxVerifyGraph fail !!\n");
	}
	app_delete_capture(&obj->captureObj);
	app_deinit_sensor(&obj->sensorObj);
	app_deinit_capture(&obj->captureObj, VIDEO_PIPE_NUM);
__exit:	
	vxReleaseNode(&obj->csitx_node);
	vxReleaseNode(&obj->csitx_node_2);
	for (i = 0; i < _MAX_CSITX_NODE; i++)
		vxReleaseObjectArray(&obj->input_image_arr[i]);

	tivxImagingUnLoadKernels(obj->context);
	tivxHwaUnLoadKernels(obj->context);
	vxReleaseUserDataObject(&obj->csitx_config);
	vxReleaseUserDataObject(&obj->csitx_config2);
	vxReleaseGraph(&obj->graph);
	vxReleaseGraph(&obj->graph_csitx);
	vxReleaseContext(&obj->context);
	tivxMutexDelete(&obj->mtxHndlState);

	return 0;
}