/*************************************************************************\
* "$RCSfile: Task_PSGrab.cpp,v $"                                         *
***************************************************************************
* Information by CVS or VSS:                                              *
*     Last check in on: $Date: 2011/06/08 13:50:37 $                      *
*     by                $Author: halfmann $
*     Revision        : $Revision: 1.30 $                                 *
\*************************************************************************/
/*! \file Task_PSGrab.cpp
*	\brief Grab task function
**************************************************************************/

#include <string.h>
#include <ctype.h>
#include "VIC_PS.h"
#include "socket.h"
#include "CRegion.h"
#include "CLog.hpp"
#include "CPSApp.hpp"
#include "CDlg.hpp"
#include "CDlgAppV.hpp"
#include "CCpuLoad.hpp"

/******************************************************************************/
void CPSApp::TaskPSGrab()
/******************************************************************************/
/*! @brief <b>Task:</b> Grab function.

	Trampoline for CPSApp::PSGrab().
*******************************************************************************/
{
	m_pThis->PSGrab();
}

/******************************************************************************/
void CPSApp::PSGrab()
/******************************************************************************/
/*! @brief Grab function.
*******************************************************************************/
{
	const int GrabImageCount = MAILBOXSIZE + 2;
	const int CalcImageCount = MAILBOXSIZE + 1;
	const int CamWidth = GetCamWidth();
	const int CamHeight = GetCamHeight();
	const int LineSize = CamWidth;
	const int GrabLineSize = CamWidth * GetBytesPerPixel();
	const int GrabBufferSize = GrabLineSize * CamHeight;
	const int LineBufferSize = 2 * CamWidth;
	unsigned char *pGrabBuffer[GrabImageCount];
	unsigned char *LineBuffer[LINEBUFFER_COUNT];
	unsigned char *restrict pLine0;
	unsigned char *restrict pLine1;
	YIMAGE *pTemp;
	CIMAGE *pCTemp;
	YIMAGE *pGrabImage[GrabImageCount];
	YIMAGE *pYCalcImage[CalcImageCount];
	CIMAGE *pCCalcImage[CalcImageCount];
	YIMAGE GrabImage[GrabImageCount];
	YIMAGE YCalcImage[CalcImageCount];
	CIMAGE CCalcImage[CalcImageCount];
	CAM_MSG Msg;
	int i, j;
	int TimeoutFlag = 0;
	bool bRotateGrabBuffers;
	bool bXmlSendImage = false;

	OpenCameraLink(1);
	fdOpenSession(TSK_self());

	// *** Init buffers ***
	for (i = 0; i < LINEBUFFER_COUNT; i++)
	{
		LineBuffer[i] = (unsigned char*)MEM_alloc(ISRAM, LineBufferSize, 128);
		if (LineBuffer[i] == MEM_ILLEGAL)
		{
			printf("LineBuffer[%d] == MEM_ILLEGAL\n", i);
			exit(1);
		}
	}
	pLine0 = LineBuffer[0];
	pLine1 = LineBuffer[1];

	for (i = 0; i < GrabImageCount; i++)
	{
		pGrabBuffer[i] = (unsigned char*)VIB_MemAlloc(GrabBufferSize);
		GrabImage[i] = GUI_YImage(pGrabBuffer[i], CamWidth, CamHeight);
		if (GrabImage[i].pY == NULL)
		{
			printf("pGrabImage[%d]->pY == NULL\n", i);
			exit(1);
		}
		memset(pGrabBuffer[i], 0, GrabBufferSize);
		pGrabImage[i] = &GrabImage[i];
	}
	for (i = 0; i < CalcImageCount; i++)
	{
		CCalcImage[i] = GUI_CImage(NULL, NULL, NULL, CamWidth, CamHeight);
		if (CCalcImage[i].Width != CamWidth)
		{
			printf("CCalcImage[%d].Width != CamWidth\n", i);
			exit(1);
		}
		pCCalcImage[i] = &CCalcImage[i];
		YCalcImage[i] = GUI_YImage(pCCalcImage[i]->pR, CamWidth, CamHeight);
		pYCalcImage[i] = &YCalcImage[i];
	}

	SEM_post(m_GrabTaskSem);

	// *** Grab first image ***
	VIB_ClInStartChannel(&m_ClInChannel, (unsigned int *)(pGrabImage[1]->pY), 0);

	if (VIB_ClInWaitChannel(&m_ClInChannel, m_CamTimeout) < 0)
		CLog::Print("DMA timeout A\n");

	// *** Main loop ******************************************************
	while (IsCameraActive())
	{
		VIB_ClInStartChannel(&m_ClInChannel, (unsigned int *)(pGrabImage[0]->pY), 0);

		// *** Send image to XML return mailbox ***
		if (bXmlSendImage)
		{
			bXmlSendImage = false;
			switch (m_CamPixelFormat)
			{
			case CamMode::_2TAP_12BIT:
			case CamMode::_1TAP_12BIT:
				Msg.Type = ImageType::Y12;
				break;
			case CamMode::_2TAP_10BIT:
			case CamMode::_1TAP_10BIT:
				Msg.Type = ImageType::Y10;
				break;
			default:
				Msg.Type = ImageType::Y8;
				break;
			}
			Msg.Image.pyI = pGrabImage[1];
//			Msg.Image.pyI->p = pGrabImage[1]->pY;
//			Msg.Image.pyI->Width = CamWidth;
//			Msg.Image.pyI->Height = CamHeight;
			MBX_post(m_MbxXmlRet, &Msg, 2000);
		}

		// *** Call User Class function ***
		i = pDlgApp->OnImage(pGrabImage[1], pCCalcImage[0]);
		//i = pDlgApp->OnImage(pGrabImage[1], pYCalcImage[0]);

		// ***
		if (m_View == ViewType::APP)
		{
			switch (i)
			{
			case AppImageType::COLOR:
				PostCalc(pCCalcImage[0]);
				break;
			case AppImageType::GRAY:
				PostCalc(pYCalcImage[0]);
				break;
			case AppImageType::NO_IMAGE:
				break;
			}
		}
//		else
//			PostCalc(pCCalcImage[0]);

		// *** Request from XML command mailbox ***
		if (MBX_pend(m_MbxXmlCmd, &Msg, 0))
			bXmlSendImage = true;

		// *** Format conversion ***
		switch (m_CamPixelFormat)
		{
		case CamMode::_2TAP_12BIT:
		case CamMode::_1TAP_12BIT:
			for (i = 0; i < CamHeight; i++)
			{
				const int Offset1 = i * GrabLineSize;
				const int Offset2 = i * LineSize;
				int pixel0;

				VIB_DMACopy(1, 1, pGrabImage[1]->pY + Offset1, pLine0, GrabLineSize);
				for (j = 0; j < LineSize; j++)
				{
					pixel0 = ((unsigned short *)(pLine0))[j];
					pLine1[j] = pixel0 >> 4;
				}
				VIB_DMACopy(1, 2, pLine1, pGrabImage[2]->pY + Offset2, LineSize);
			}
			break;
		case CamMode::_2TAP_10BIT:
		case CamMode::_1TAP_10BIT:
			for (i = 0; i < CamHeight; i++)
			{
				const int Offset1 = i * GrabLineSize;
				const int Offset2 = i * LineSize;
				int pixel0;

				VIB_DMACopy(1, 1, pGrabImage[1]->pY + Offset1, pLine0, GrabLineSize);
				for (j = 0; j < LineSize; j++)
				{
					pixel0 = ((unsigned short *)(pLine0))[j];
					pLine1[j] = pixel0 >> 2;
				}
				VIB_DMACopy(1, 2, pLine1, pGrabImage[2]->pY + Offset2, LineSize);
			}
			break;
		}

		if (VIB_ClInWaitChannel(&m_ClInChannel, m_CamTimeout) < 0)
		{
			bRotateGrabBuffers = false;
			if (TimeoutFlag == 0)
				CLog::GetInstance()->PrintD("DMA timeout B\n");
			TimeoutFlag++;
		}
		else
		{
			TimeoutFlag = 0;
			bRotateGrabBuffers = PostCam(pGrabImage[2]);
			CCpuLoad::IncrementFrameCounter();
		}
		
		// *** Rotate image buffers ***
		if (bRotateGrabBuffers)
		{
			pTemp = pGrabImage[GrabImageCount - 1];
			for (i = GrabImageCount - 1; i > 0; i--)
				pGrabImage[i] = pGrabImage[i - 1];
			pGrabImage[0] = pTemp;
		}
		else
		{
			pTemp = pGrabImage[1];
			pGrabImage[1] = pGrabImage[0];
			pGrabImage[0] = pTemp;
		}
		pTemp = pYCalcImage[CalcImageCount - 1];
		for (i = CalcImageCount - 1; i > 0; i--)
			pYCalcImage[i] = pYCalcImage[i - 1];
		pYCalcImage[0] = pTemp;
		pCTemp = pCCalcImage[CalcImageCount - 1];
		for (i = CalcImageCount - 1; i > 0; i--)
			pCCalcImage[i] = pCCalcImage[i - 1];
		pCCalcImage[0] = pCTemp;
	}

	// *** Clean up *******************************************************
	for (i = 0; i < MAILBOXSIZE; i++)
	{
		PostCam();
		PostCalc();
	}
	for (i = 0; i < GrabImageCount; i++)
		VIB_MemFree(pGrabBuffer[i]);
	for (i = 0; i < CalcImageCount; i++)
		GUI_FreeCImage(pCCalcImage[i]);

	CloseCameraLink();
	fdCloseSession(TSK_self());

	for (i = 0; i < LINEBUFFER_COUNT; i++)
		MEM_free(ISRAM, LineBuffer[i], LineBufferSize);
	SEM_post(m_GrabTaskSem);
}

/*************************************************************************/
void CPSApp::StartGrabTask()
/*************************************************************************/
/*! @brief Starts grab task.

	#m_GrabTaskSem is used for synchronization with the grab task. If the
	task is still active, it is stopped and restarted.
**************************************************************************/
{	
	struct TSK_Attrs ta;

	if (m_bCameraActive)
	{
		if (m_CamTimeout == 0)
			return;
		SEM_reset(m_GrabTaskSem, 0);
		m_bCameraActive = false;
		SEM_pend(m_GrabTaskSem, SYS_FOREVER);
		SEM_reset(m_GrabTaskSem, 0);
	}

	ta.priority  = TSK_getpri(TSK_self());
	ta.stack     = 0;
	ta.stacksize = 8 * 1024;
	ta.stackseg  = ISRAM;
	ta.environ   = 0;
	ta.exitflag  = 0;
	ta.initstackflag = TRUE;
	ta.name      = "TaskLive";

	SEM_reset(m_GrabTaskSem, 0);
	m_bCameraActive = true;
	m_hGrabTask = TSK_create((Fxn)TaskPSGrab , &ta);
	if (m_hGrabTask == NULL)
		CLog::Print("Error: TSK_create() failed\n");
	SEM_pend(m_GrabTaskSem, SYS_FOREVER);
	SEM_reset(m_GrabTaskSem, 0);

	//=== Clear camera images ===
	GUI_FillY(&m_YPVImage, 128);
	GUI_Fill(&m_CPVImage, &rgbGray50);
}

/*************************************************************************/
void CPSApp::StopGrabTask()
/*************************************************************************/
/*! @brief Stops grab task.

	#m_GrabTaskSem is used for synchronization with the grab task.
**************************************************************************/
{
	if (m_bCameraActive || m_CamTimeout > 0)
	{
		if (!m_bCameraActive)	//TODO: Check if case is possible
			return;
		SEM_reset(m_GrabTaskSem, 0);
		m_bCameraActive = false;
		SEM_pend(m_GrabTaskSem, SYS_FOREVER);	// Wait until grab task has stopped
		SEM_reset(m_GrabTaskSem, 0);
		if (m_hGrabTask != NULL)
		{
			TSK_delete((TSK_Handle)m_hGrabTask);
			m_hGrabTask = NULL;
		}
	}
}

/*************************************************************************/
