#include "calibration.h"
#include "environment.h"
#include "image_control.h"

//---------------------------------------------------------------------------
void Calib_Capture_Stop_All(void);

int trig_test_start = 0;
int trig_test_end = 0;
int trig_test_prd = 0;
int ThreeDCameraCalibrationMode;

TCalibCaptureInfo CalibCaptureInfo[SYSTEM_SPB_CAMERA_MAX_COUNT];
//---------------------------------------------------------------------------
void Calib_Reset(void)
{
	int spbCamIndex;
	for (spbCamIndex = 0; spbCamIndex < SYSTEM_SPB_CAMERA_MAX_COUNT; spbCamIndex++)
	{
		CalibCaptureInfo[spbCamIndex].Enabled = FALSE;
		CalibCaptureInfo[spbCamIndex].CaptureQueueStart = CalibCaptureInfo[spbCamIndex].CaptureQueueEnd = 0;
		CalibCaptureInfo[spbCamIndex].CaptureMode = CAPTURE_MODE_NONE;
	}

	ThreeDCameraCalibrationMode = 0;
}
//---------------------------------------------------------------------------
void Calib_Capture_Start(unsigned char captureKind, unsigned char cameraMask, unsigned int captureMode, 
	int frameBufferCount, unsigned short *shutterSpeed, unsigned char *SubsamplingMode)
{
	int spbCamIndex;
	unsigned char *imageBuffer;
	TCalibCaptureInfo *calibInfo;

	ThreeDCameraCalibrationMode = 1;
	CameraSetupData.SensorLength = 1000;

	for (spbCamIndex = 0; spbCamIndex < SYSTEM_SPB_CAMERA_MAX_COUNT; spbCamIndex++)
	{	
		if (cameraMask & (0x01 << spbCamIndex))
		{
			if(spbCamIndex == 0)
				imageBuffer = ImageBuffer1;
			else if(spbCamIndex == 1)
				imageBuffer = ImageBuffer2;
			else if(spbCamIndex == 2)
				imageBuffer = ImageBuffer3;
			else if(spbCamIndex == 3)
				imageBuffer = ImageBuffer4;		

			SelectedImageControl_ClearBuffer(imageBuffer);
		}
	}

	for (spbCamIndex = 0; spbCamIndex < SYSTEM_SPB_CAMERA_MAX_COUNT; spbCamIndex++)
	{	
		if (cameraMask & (0x01 << spbCamIndex))
		{
			calibInfo = &(CalibCaptureInfo[spbCamIndex]);
			// clear capture queue
			calibInfo->CaptureQueueStart = calibInfo->CaptureQueueEnd = 0;
			calibInfo->CaptureMode = captureMode;
			calibInfo->FrameBufferCount = frameBufferCount;			
			CameraSetupData.CameraEnable[spbCamIndex]  = 1;
			SetImageControlMode(spbCamIndex, IMAGE_CONTROL_MODE_FIFO);

			ImageControl_CalibMode_Clear();
			if (calibInfo->FrameBufferCount - ImageControl_CalibMode_GetFilledFrameCount(spbCamIndex) < 2)
			{
				goto fail;
			}
			calibInfo->CaptureImageBufferList[calibInfo->CaptureQueueEnd] = ImageControl_CalibMode_GetLastFrameBuffer(spbCamIndex);
			calibInfo->FrameIndexList[calibInfo->CaptureQueueEnd] = ImageControl_CalibMode_GetLastFrameIndex(spbCamIndex);			
			calibInfo->CaptureStatusList[calibInfo->CaptureQueueEnd] = CAPTURE_STATUS_NONE;

			if (CaptureStart(captureKind, spbCamIndex, shutterSpeed, SubsamplingMode,
				calibInfo->CaptureImageBufferList[calibInfo->CaptureQueueEnd],
				&(calibInfo->CaptureStatusList[calibInfo->CaptureQueueEnd])) != 0)
			{
				goto fail;
			}

			ImageControl_CalibMode_SetLastBufferState(spbCamIndex, IC_CALIB_STATE_ON_CAPTURE);
			ImageControl_CalibMode_IncreaseFIFO(spbCamIndex);

			calibInfo->CaptureQueueEnd = (calibInfo->CaptureQueueEnd + 1) & MAX_CALIB_QUEUE_MASK;
			calibInfo->Enabled = TRUE;
		}
	}
	return;
fail:
	return;
}
//---------------------------------------------------------------------------
void Calib_Capture_Stop_All(void)
{
	int spbCamIndex;

	for (spbCamIndex = 0; spbCamIndex < SYSTEM_SPB_CAMERA_MAX_COUNT; spbCamIndex++)
	{
		CaptureStop(spbCamIndex);
		CalibCaptureInfo[spbCamIndex].Enabled = FALSE;
	}
	
	ThreeDCameraCalibrationMode = 0;
}
//---------------------------------------------------------------------------
void Calib_Capture_Stop(int spbCamIndex)
{
	CaptureStop(spbCamIndex);
	CalibCaptureInfo[spbCamIndex].Enabled = FALSE;
	
	ThreeDCameraCalibrationMode = 0;
}
//---------------------------------------------------------------------------
void Calib_Capture_Process(void)		// per 1ms
{
	int spbCamIndex;
	
	TCalibCaptureInfo *calibInfo;
	for (spbCamIndex = 0; spbCamIndex < SYSTEM_SPB_CAMERA_MAX_COUNT; spbCamIndex++)
	{
		calibInfo = &(CalibCaptureInfo[spbCamIndex]);
		if (calibInfo->Enabled)
		{
			if (calibInfo->CaptureQueueStart != calibInfo->CaptureQueueEnd)
			{
				// check status
				if (calibInfo->CaptureStatusList[calibInfo->CaptureQueueStart] == CAPTURE_STATUS_TIMEOUT)
				{
					ImageControl_CalibMode_SetBufferState(spbCamIndex, calibInfo->FrameIndexList[calibInfo->CaptureQueueStart], IC_CALIB_STATE_ERROR);
					calibInfo->CaptureQueueStart = (calibInfo->CaptureQueueStart + 1) & MAX_CALIB_QUEUE_MASK;
//					calibInfo->CaptureQueueEnd = (calibInfo->CaptureQueueEnd + 1) & MAX_CALIB_QUEUE_MASK;
				}
				else if (calibInfo->CaptureStatusList[calibInfo->CaptureQueueStart] == CAPTURE_STATUS_COMPLETE)
				{
					ImageControl_CalibMode_SetBufferState(spbCamIndex, calibInfo->FrameIndexList[calibInfo->CaptureQueueStart], IC_CALIB_STATE_CAPTURE_COMPLETE);
					calibInfo->CaptureQueueStart = (calibInfo->CaptureQueueStart + 1) & MAX_CALIB_QUEUE_MASK;

					if (calibInfo->CaptureMode & CAPTURE_MODE_CONTINUOUS)	// frame 일 때는 polling, line 일 때는 pending으로 처리
					{
						if (calibInfo->FrameBufferCount - ImageControl_CalibMode_GetFilledFrameCount(spbCamIndex) > 1)
						{
							calibInfo->CaptureImageBufferList[calibInfo->CaptureQueueEnd] = ImageControl_CalibMode_GetLastFrameBuffer(spbCamIndex);
							calibInfo->FrameIndexList[calibInfo->CaptureQueueEnd] = ImageControl_CalibMode_GetLastFrameIndex(spbCamIndex);										
							calibInfo->CaptureStatusList[calibInfo->CaptureQueueEnd] = CAPTURE_STATUS_NONE;
							if (NextCaptureTrigger(spbCamIndex, calibInfo->CaptureImageBufferList[calibInfo->CaptureQueueEnd], 
								&(calibInfo->CaptureStatusList[calibInfo->CaptureQueueEnd])) == 0)
							{	
								ImageControl_CalibMode_SetLastBufferState(spbCamIndex, IC_CALIB_STATE_ON_CAPTURE);
								ImageControl_CalibMode_IncreaseFIFO(spbCamIndex);
								calibInfo->CaptureQueueEnd = (calibInfo->CaptureQueueEnd + 1) & MAX_CALIB_QUEUE_MASK;
							}
						}
					}
					else
					{
						calibInfo->Enabled = 0;
					}
				}
			}
			else
			{
				if (calibInfo->CaptureMode & CAPTURE_MODE_CONTINUOUS)	// frame 일 때는 polling, line 일 때는 pending으로 처리
				{
					if (calibInfo->FrameBufferCount - ImageControl_CalibMode_GetFilledFrameCount(spbCamIndex) >= 1)
					{
						calibInfo->CaptureImageBufferList[calibInfo->CaptureQueueEnd] = ImageControl_CalibMode_GetLastFrameBuffer(spbCamIndex);
						calibInfo->FrameIndexList[calibInfo->CaptureQueueEnd] = ImageControl_CalibMode_GetLastFrameIndex(spbCamIndex);										
						calibInfo->CaptureStatusList[calibInfo->CaptureQueueEnd] = CAPTURE_STATUS_NONE;
						if (NextCaptureTrigger(spbCamIndex, calibInfo->CaptureImageBufferList[calibInfo->CaptureQueueEnd], 
							&(calibInfo->CaptureStatusList[calibInfo->CaptureQueueEnd])) == 0)
						//if (NextCaptureTrigger(spbCamIndex, ImageBuffer1, 
						//	&(calibInfo->CaptureStatusList[calibInfo->CaptureQueueEnd])) == 0)
						{
							ImageControl_CalibMode_SetLastBufferState(spbCamIndex, IC_CALIB_STATE_ON_CAPTURE);
							ImageControl_CalibMode_IncreaseFIFO(spbCamIndex);
							calibInfo->CaptureQueueEnd = (calibInfo->CaptureQueueEnd + 1) & MAX_CALIB_QUEUE_MASK;
						}
					}
				}
			}
		}
	}
}
//---------------------------------------------------------------------------
