/*************************************************************************\
* "$RCSfile: CXml.cpp,v $"                                                *
***************************************************************************
* Information by CVS or VSS:                                              *
*     Last check in on: $Date: 2011/10/20 07:04:10 $                      *
*     by                $Author: schluckebier $
*     Revision        : $Revision: 1.11 $                                 *
\*************************************************************************/
/*! \file CXml.cpp
*	\brief Implementation CXml Class
**************************************************************************/

#include <string.h>
#include "VIC_PS.h"
#include "GBit.h"
#include "CRegion.h"
#include "CLog.hpp"
#include "CPSApp.hpp"
#include "CDlg.hpp"
#include "CFeature.hpp"
#include "CCamFeatures.hpp"
#include "CDlgCamExpert.hpp"
#include "CXml.hpp"
#include <clk.h>

extern "C"
{
	int WriteCom(int Port, unsigned char *pBuff, unsigned long Size);
	int ReadComWait(int Port, unsigned char *pBuff, unsigned long Size, unsigned long TimeMax);
}

CXml *CXml::m_pThis = NULL;	//!< The one and only CXml instance

/*************************************************************************/
CXml::CXml()
/*************************************************************************/
/*! @brief Constructor

	Opens a XML and a data socket then starts an own task which listens to
	XML messages. Uses CPSApp::GetXmlPort() for XML port.
**************************************************************************/
{
	m_pA = CPSApp::GetInstance();
	const int Port = m_pA->GetXmlPort();
	int XSockopt = 0;
	int DSockopt = 0;
	TSK_Attrs ta;

	m_bXConnected = m_bDConnected = false;
	m_XSocket = m_DSocket = INVALID_SOCKET;
	m_XListenSocket = m_DListenSocket = INVALID_SOCKET;
	m_XMLPos = 0;
	m_Counter = 0;
	m_Timeout = 0;
	m_XMLString = new char[XMLBUFFER];

	if (m_pA->GetEthError())	// No Ethernet device?
		return;

	//=== Open sockets ===
	m_XListenSocket = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
	if (m_XListenSocket == INVALID_SOCKET)
	{
		printf("Error: Open XLSocket (%d)\n", socketerror());
		return;
	}
	m_DListenSocket = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
	if (m_DListenSocket == INVALID_SOCKET)
	{
		printf("Error: Open DLSocket (%d)\n", socketerror());
		return;
	}

	memset(&m_XSoIn, 0, sizeof(sockaddr));
	m_XSoIn.sin_family      = AF_INET;
	m_XSoIn.sin_len         = sizeof(m_XSoIn);
	m_XSoIn.sin_addr.s_addr = INADDR_ANY;
	m_XSoIn.sin_port        = htons(Port);
	m_DSoIn = m_XSoIn;
	m_DSoIn.sin_port        = htons(Port + 1); // Port: Data = XML+1

	//=== Bind the sockets ===
	if (bind(m_XListenSocket, &m_XSoIn, (int)sizeof(m_XSoIn)) < 0)
	{
		printf("Socket error: bind() (%d)\n", (int)socketerror());
		return;
	}
	if (bind(m_DListenSocket, &m_DSoIn, (int)sizeof(m_DSoIn)) < 0)
	{
		printf("Socket error: bind() (%d)\n", socketerror());
		return;
	}

	//=== Start listening ===
	if (listen(m_XListenSocket, 1) < 0)
	{
		printf("Socket error: listen() (%d)\n", socketerror());
		return;
	}
	if (listen(m_DListenSocket, 1) < 0)
	{
		printf("Socket error: listen() (%d)\n", socketerror());
		return;
	}

	// non-blocking accept()
	setsockopt(m_XListenSocket, SOL_SOCKET, SO_BLOCKING, &XSockopt, (int)sizeof(XSockopt));
	setsockopt(m_DListenSocket, SOL_SOCKET, SO_BLOCKING, &DSockopt, (int)sizeof(DSockopt));

	//=== Start Task Dispatcher ===
	ta.priority  = TSK_getpri(TSK_self());
	ta.stack     = 0;
	ta.stacksize = 1024 * 16;
	ta.stackseg  = ISRAM;
	ta.name      = "XMLDispatcher";
	ta.exitflag  = 0;
	ta.initstackflag = 1;
	TSK_create((Fxn)TaskDispatcher, &ta);

	//=== Start Task UDPBroadcast ===
	ta.priority  = TSK_getpri(TSK_self());
	ta.stack     = 0;
	ta.stacksize = 1024 * 8;
	ta.stackseg  = ISRAM;
	ta.name      = "UDPBroadcast";
	ta.exitflag  = 0;
	ta.initstackflag = 1;
	TSK_create((Fxn)TaskUDPBroadcast, &ta);

	//=== Create task for firmware update ===
	ta.priority	 = TSK_getpri(TSK_self());
	ta.stacksize = 4096;
	ta.name		 = "TaskFirmwareUpdate";
	ta.initstackflag= true;
	TSK_create((Fxn)TaskFirmwareUpdate, &ta);
}

/*************************************************************************/
CXml::~CXml()
/*************************************************************************/
/*! @brief Destructor.
**************************************************************************/
{
	if (m_XSocket != INVALID_SOCKET)
	{
		socketclose(m_XSocket);
		m_XSocket = INVALID_SOCKET;
		m_bXConnected = false;
	}
	if (m_DSocket != INVALID_SOCKET)
	{
		socketclose(m_DSocket);
		m_DSocket = INVALID_SOCKET;
		m_bDConnected = false;
	}
	if (!m_pA->GetEthError())
		fdCloseSession(TSK_self());
	delete [] m_XMLString;
}

/*************************************************************************/
CXml *CXml::CreateInstance()
/*************************************************************************/
/*! @brief Creates CXml instance.
	@return Pointer to CXml instance
**************************************************************************/
{
	if (m_pThis == NULL)
		m_pThis = new CXml();
	return m_pThis;
}

/*************************************************************************/
void CXml::DisposeInstance()
/*************************************************************************/
/*! @brief Disposes CXml instance.
**************************************************************************/
{
	if (m_pThis)
		delete m_pThis;
	m_pThis = NULL;
}

/*************************************************************************/
int CXml::Dispatcher()
/*************************************************************************/
/*! @brief Receives Ethernet data and scans XML commands.
	@return 0: No new command\n
	1: Parse command
**************************************************************************/
{
	static int nRest = 0;
	static int nPos = 0;
	static int nResult = 0;
	int Size, End;
	int err;
	int DSockopt, XSockopt;
	static char Buffer[256];

	if (!m_pA->IsXmlAllowed())
		return 0;

	//=== Still data from last receive? ===
	if (nRest)
	{
		do
		{
			m_XMLString[m_XMLPos] = Buffer[nPos];
			End = strncmp(&m_XMLString[m_XMLPos - 12], "</VisionCam>", 12);
			m_XMLPos++;
			nPos++;
		} while (nPos<nResult && (End != 0));
		nRest = 0;
	}

	//=== Wait for connection ===
	if (m_DSocket == INVALID_SOCKET)
	{
		Size = sizeof(m_DSoIn);
		m_DSocket = accept(m_DListenSocket, (PSA)&m_DSoIn, &Size);
		if (m_DSocket != INVALID_SOCKET)
		{
			m_bDConnected = true;
			DSockopt = 1;
			setsockopt(m_DSocket, SOL_SOCKET, SO_BLOCKING, &DSockopt, sizeof(DSockopt));
		}
	}
	if (m_XSocket == INVALID_SOCKET)
	{
		Size = sizeof(m_XSoIn);
		m_XSocket = accept(m_XListenSocket, (PSA)&m_XSoIn, &Size);
		if (m_XSocket != INVALID_SOCKET)
		{
//			printf("Connection established.\n");
			CLog::Print("Socket connection established (%s)\n", inet_ntoa(m_XSoIn.sin_addr));
			m_bXConnected = true;
			XSockopt = 0;//1;
			m_Counter = m_Timeout;
			setsockopt(m_XSocket, SOL_SOCKET, SO_BLOCKING, &XSockopt, sizeof(XSockopt));
		}
	}
	else //=== Receive command ===
	{
		nResult = recv(m_XSocket, Buffer, 128, 0);
		if (nResult <= 0)
		{
			if (m_Timeout)
				m_Counter--;
			err = socketerror();
			if (err != EWOULDBLOCK || m_Counter < 0 || nResult == 0)
			{
				CLog::Print("Connection closed (%d).\n", err);
				socketclose(m_XSocket);
				m_bXConnected = false;
				m_XSocket = INVALID_SOCKET;
				if (m_DSocket != INVALID_SOCKET)
				{
					socketclose(m_DSocket);
					m_bDConnected = false;
					m_DSocket = INVALID_SOCKET;
				}
			}
			return 0;
		}
		m_Counter = m_Timeout; // Reset counter

		//=== Assemble command ===
		End = 1;
		if (strncmp(Buffer, "<?xml", 5) == 0) // New command
			m_XMLPos = nPos = 0;
		else // Concatenate
			nPos = 0;

		do
		{
			m_XMLString[m_XMLPos] = Buffer[nPos];
			End = strncmp(&m_XMLString[m_XMLPos - 12], "</VisionCam>", 12);
			m_XMLPos++;
			nPos++;
		} while (nPos<nResult && (End != 0));

		//=== End mark found ===
		if (End == 0)
		{
			m_XMLString[m_XMLPos] = '\0';
			m_XMLPos = 0;
			if (nPos < nResult)
				nRest = 1;
			else
				nPos = nRest = 0;
			return 1;
		}
		else if (nPos < nResult)
		{
			nRest = 1;
		}
	}
	return 0;
}

/*************************************************************************/
int CXml::SendMessage(TiXmlDocument *pXML)
/*************************************************************************/
/*! @brief Sends a XML file via socket.
	@param pXML Pointer to XML document structure.
	@return Error code
**************************************************************************/
{
	TiXmlPrinter Printer;
	const char *pString;

	if (m_XSocket == INVALID_SOCKET)
		return -1;

	Printer.SetIndent("  "); // "\t"
	Printer.SetLineBreak("\r\n");
	pXML->Accept(&Printer);

	pString = Printer.CStr();

	send(m_XSocket, pString, strlen(pString), 0);
	m_Counter = m_Timeout;
	return 0;
}

/*************************************************************************/
void CXml::TaskDispatcher()
/*************************************************************************/
/*! @brief Task: Ethernet Dispatcher.

	Calls CXml::Dispatcher(). Analyzes new commands with CXml::Parser().
**************************************************************************/
{
	CXml *pCXml = CXml::GetInstance();
	int Command;
	bool bActive = true;

	fdOpenSession(TSK_self());

	while (bActive)
	{
		if (pCXml->Dispatcher() == 1)
		{
			Command = pCXml->Parser();
			switch (Command)
			{
			case C_UNKOWN:
				CLog::Print("Unknown Command\n");
				break;
			}
		}
		TSK_sleep(100);
	}
	fdCloseSession(TSK_self());
}

/*************************************************************************/
int CXml::Parser()
/*************************************************************************/
/*! @brief Analyzes XML file.
	@return Error code
**************************************************************************/
{
	TiXmlDocument XMLDoc("VisionCam.xml");
	TiXmlHandle hDoc(&XMLDoc);
	TiXmlHandle hRoot(0);
	TiXmlElement *pElem, *pProperty, *pProp, *pImage;
	int Flag;
	const char *pText, *pValStr;
	char pString[256];

	XMLDoc.Parse(m_XMLString);
	pElem = hDoc.FirstChildElement().Element();
	if (!pElem)
		return -1; // XML error

	hRoot = TiXmlHandle(pElem);

	// *** Get ************************************************************
	pElem = hRoot.FirstChild("Get").Element();
	if (pElem)
	{
		pProperty = pElem->FirstChildElement("CFeature");
		if (pProperty)
		{
			pProp = pProperty->FirstChildElement();
			while (pProp)
			{
				pText = pProp->Value();
				if (GetCValue(pText, pString) < 0)
				{
					pProp->SetAttribute("State", "Fail");
				}
				else
				{
					pProp->SetAttribute("State", "OK");
					pProp->LinkEndChild(new TiXmlText(pString));
				}
				pProp = pProp->NextSiblingElement();
			}
		}

		pProperty = pElem->FirstChildElement("Property");
		if (pProperty)
		{
			pProp = pProperty->FirstChildElement();
			while (pProp)
			{
				pText = pProp->Value();
				if (GetPValue(pText, pString) < 0)
				{
					pProp->SetAttribute("State", "Fail");
				}
				else
				{
					TOKEN Tok = pfProperty_ParseName(m_pA->GetCamSerialPort(), pText);
					PropertyType Type = pfProperty_GetType(m_pA->GetCamSerialPort(), Tok);

					pProp->SetAttribute("State", "OK");
					switch (Type)
					{
					case PF_INT:
						pProp->SetAttribute("PropertyType", "PF_INT");
						break;
					case PF_FLOAT:
						pProp->SetAttribute("PropertyType", "PF_FLOAT");
						break;
					case PF_BOOL:
						pProp->SetAttribute("PropertyType", "PF_BOOL");
						break;
					case PF_MODE:
						pProp->SetAttribute("PropertyType", "PF_MODE");
						break;
					case PF_REGISTER:
						pProp->SetAttribute("PropertyType", "PF_REGISTER");
						break;
					case PF_STRING:
						pProp->SetAttribute("PropertyType", "PF_STRING");
						break;
					case PF_BUFFER:
						pProp->SetAttribute("PropertyType", "PF_BUFFER");
						break;
					case PF_COMMAND:
						pProp->SetAttribute("PropertyType", "PF_COMMAND");
						break;
					}
					pProp->LinkEndChild(new TiXmlText(pString));
				}
				pProp = pProp->NextSiblingElement();
			}
		}

		pProperty = pElem->FirstChildElement("CamReg");
		if (pProperty)
		{
			pProp = pProperty->FirstChildElement("Size");
			if (pProp)
			{
				pText = pProp->GetText();

				if (GetCamRegValue(pText, pString) >= 0)
				{
					TiXmlElement *pValue = new TiXmlElement("Value");
					pValue->LinkEndChild(new TiXmlText(pString));
					pProperty->LinkEndChild(pValue);
				}
			}
		}

		pImage = pElem->FirstChildElement("Image");
		if (pImage)
		{
			pProp = pImage->FirstChildElement();
			if (pProp)
			{
				int IType = ImageFormat::NO;

				pText = pProp->Value();
				if (strcmp(pText, "BMP") == 0)
					IType = ImageFormat::BMP;
				else if (strcmp(pText, "PNG") == 0)
					IType = ImageFormat::PNG;
				else if (strcmp(pText, "RAW") == 0)
					IType = ImageFormat::RAW;
				SendImage(&XMLDoc, pProp, IType);
			}
		}
		SendMessage(&XMLDoc);
		return 0;
	}

	// *** Set ************************************************************
	pElem = hRoot.FirstChild("Set").Element();
	if (pElem)
	{
		pProperty = pElem->FirstChildElement("CFeature");
		if (pProperty)
		{
			pProp = pProperty->FirstChildElement();
			Flag = 0;
			while (pProp)
			{
				pText = pProp->Value();
				pValStr = pProp->GetText();
				if (SetCValue(pText, pValStr) < 0)
				{
					pProp->SetAttribute("State", "Fail");
				}
				else
				{
					pProp->SetAttribute("State", "OK");
					Flag = 1;
				}
				pProp = pProp->NextSiblingElement();
			}
			if (Flag)
				CCamFeatures::GetInstance()->UpdateAllFeatures();
			SendMessage(&XMLDoc);
			return 0;
		}
		pProperty = pElem->FirstChildElement("Property");
		if (pProperty)
		{
			pProp = pProperty->FirstChildElement();
			Flag = 0;
			while (pProp)
			{
				pText = pProp->Value();
				pValStr = pProp->GetText();
				if (SetPValue(pText, pValStr) < 0)
				{
					pProp->SetAttribute("State", "Fail");
				}
				else
				{
					pProp->SetAttribute("State", "OK");
					Flag = 1;
				}
				pProp = pProp->NextSiblingElement();
			}
//			if (Flag)
//				CCamFeatures::GetInstance()->UpdateAllFeatures();
			SendMessage(&XMLDoc);
			return 0;
		}

		pProperty = pElem->FirstChildElement("CamReg");
		if (pProperty)
		{
			bool myOK = false;

			pProp = pProperty->FirstChildElement("Size");
			if (pProp)
			{
				pText = pProp->GetText();
				myOK = true;
			}

			pProp = pProperty->FirstChildElement("Value");
			if (pProp && myOK)
			{
				pValStr = pProp->GetText();
			}
			else
			{
				myOK = false;
			}

			if (myOK)
			{
				SetCamRegValue(pText, pValStr);
			}
			return 0;
		}

		return C_UNKOWN;
	}
	return C_UNKOWN;
}

/*************************************************************************/
int CXml::GetCValue(const char *pFeatureName, char *pValStr)
/*************************************************************************/
/*! @brief Gets value of camera feature.
	@param[in] pFeatureName Name of camera feature
	@param[out] pValStr Value of property
	@return Error code
**************************************************************************/
{
	CCamFeatures *pCF = CCamFeatures::GetInstance();
	CFeature **ppFeatureList = pCF->GetFeatureList();
	int i;
	int Ret = -1;
	const char *pString;

	if (pFeatureName == NULL || pValStr == NULL)
		return -1;

	i = 0;
	while (ppFeatureList[i])
	{
		pString = ppFeatureList[i]->GetName();
		if (strcmp(pString, pFeatureName) == 0)
		{
			sprintf(pValStr, "%s", ppFeatureList[i]->GetValueStr());
			Ret = 0;
			break;
		}
		i++;
	}
	return Ret;
}

/*************************************************************************/
int CXml::GetPValue(const char *pPropertyName, char *pValStr)
/*************************************************************************/
/*! @brief Gets value of camera property.
	@param[in] pPropertyName Name of camera property
	@param[out] pValStr Value of property
	@return Error code
**************************************************************************/
{
	const int Port = m_pA->GetCamSerialPort();
	TOKEN Tok;

	if (pPropertyName == NULL || pValStr == NULL)
		return -1;

	Tok = pfProperty_ParseName(Port, pPropertyName);

	if (pfDevice_GetProperty_String(Port, Tok, pValStr, 64) < 0)
		return -1;
	else
		return 0;
}

/*************************************************************************/
int CXml::GetCamRegValue(const char *pSizeStr, char *pValStr)
/*************************************************************************/
/*! @brief Gets value of camera register.
	@param[in] pSizeStr Size
	@param[out] pValStr Value of property
	@return Error code
**************************************************************************/
{
	const int Port = m_pA->GetCamSerialPort();
	int error, i;
	unsigned long mySize = (unsigned long)atoi(pSizeStr);
	unsigned char myBuff[64];

	error = ReadComWait(Port, myBuff, mySize, 10);

	if (error != mySize)
		return -1;

	pValStr[0] = '\0';
	for (i = 0; i < mySize; i++)
	{
		sprintf(pValStr, "%s%.2x", pValStr, myBuff[i]);
	}
	sprintf(pValStr, "%s\0", pValStr);

	return 0;
}

/*************************************************************************/
int CXml::SetCValue(const char *pFeatureName, const char *pValStr)
/*************************************************************************/
/*! @brief Sets value of camera feature.
	@param[in] pFeatureName Name of camera feature
	@param[in] pValStr Value of property
	@return Error code
**************************************************************************/
{
	CCamFeatures *pCF = CCamFeatures::GetInstance();
	CFeature **ppFeatureList = pCF->GetFeatureList();
	int i;
	int Ret = -1;
	const char *pString;

	if (pFeatureName == NULL || pValStr == NULL)
		return -1;

	i = 0;
	while (ppFeatureList[i])
	{
		pString = ppFeatureList[i]->GetName();
		if (strcmp(pString, pFeatureName) == 0)
		{
			ppFeatureList[i]->SetValue(atoi(pValStr));
			ppFeatureList[i]->UpdateValue();
			Ret = 0;
			break;
		}
		i++;
	}
	return Ret;
}

/*************************************************************************/
int CXml::SetPValue(const char *pPropertyName, const char *pValStr)
/*************************************************************************/
/*! @brief Sets value of camera property.
	@param[in] pPropertyName Name of camera property
	@param[in] pValStr Value of property
	@return Error code
**************************************************************************/
{
	const int Port = m_pA->GetCamSerialPort();
	TOKEN Tok;

	if (pPropertyName == NULL || pValStr == NULL)
		return -1;

	Tok = pfProperty_ParseName(Port, pPropertyName);

	if (pPropertyName == NULL || pValStr == NULL)
		return -1;
	if (pfDevice_SetProperty_String(Port, Tok, pValStr) < 0)
		return -1;
	else
		return 0;
}

/*************************************************************************/
int CXml::SetCamRegValue(const char *pSizeStr, const char *pValStr)
/*************************************************************************/
/*! @brief Sets value of camera property.
	@param[in] pSizeStr Size
	@param[in] pValStr Value of property
	@return Error code
**************************************************************************/
{
	const int Port = m_pA->GetCamSerialPort();
	int error, i;
	unsigned long mySize = (unsigned long)atoi(pSizeStr);
	unsigned char myBuff[128];
	char tmpString[3];

	tmpString[2] = '\0';
	for (i = 0; i < mySize; i++)
	{
		tmpString[0] = pValStr[2 * i];
		tmpString[1] = pValStr[2 * i + 1];
		myBuff[i] = strtol(tmpString, NULL, 16);
	}

	error = WriteCom(Port, myBuff, mySize);

	if (error != mySize)
		return -1;
	else
		return 0;
}

/*************************************************************************/
int CXml::SendImage(TiXmlDocument *pXML, TiXmlElement *pElem, int Type)
/*************************************************************************/
/*! @brief Sends camera image via TCP/IP port.
	@param pXML Pointer to XML document structure
	@param pElem Pointer to XML element
	@param Type #ImageFormat::eIMAGEFORMAT
	@return Error code
**************************************************************************/
{
	const int Width = m_pA->GetCamWidth();
	const int Height = m_pA->GetCamHeight();
	MBX_Handle MbxXmlCmd = m_pA->GetMbxXmlCmd();
	MBX_Handle MbxXmlRet = m_pA->GetMbxXmlRet();
	TiXmlElement *pPicture;
	int FSize;
	int ImgSize;
	int Ret;
	bool bSend;
	CAM_MSG Msg;
	YIMAGE yImage;
	YIMAGE *pyImage;
	char Text[32];
	char Header[4];
	unsigned char *Buffer = NULL;
	unsigned char *ImgBuffer = NULL;

	if (!m_pA->IsCameraActive() || Type == ImageFormat::NO)
	{
		pElem->SetAttribute("State", "Fail");
		return -1;
	}

	pPicture = new TiXmlElement("Picture");
	sprintf(Text, "%d", Width);
	pPicture->SetAttribute("Width", Text);
	sprintf(Text, "%d", Height);
	pPicture->SetAttribute("Height", Text);
	pElem->LinkEndChild(pPicture);

	if (!m_bDConnected) // No data connection
	{
		pElem->SetAttribute("State", "Fail");
		return -1;
	}

	MBX_post(MbxXmlCmd, &Msg, SYS_FOREVER); // Image request to CPSApp::TaskPSGrab()

	if (MBX_pend(MbxXmlRet, &Msg, 5000)) // Wait for image
	{
		pyImage = Msg.Image.pyI;
		if (pyImage->Width == Width && pyImage->Height == Height)
		{
			bSend = true;
			ImgSize = Width * Height;
			if (Msg.Type != ImageType::Y8)
				ImgSize *= 2;
			else
			{
				Buffer = (unsigned char*)VIB_MemAlloc(ImgSize + 2048); // For BMP / PNG
				if (Buffer == NULL)
					bSend = false;
			}
			ImgBuffer = (unsigned char*)VIB_MemAlloc(ImgSize);	// Copy of buffer
			if (ImgBuffer == NULL)
				bSend = false;
			if (Msg.Type != ImageType::Y8)
//				VIB_MemCopy2D(Msg.Image.p, ImgBuffer, Width * 2, Height, Width * 2, Width * 2);
				memcpy(ImgBuffer, Msg.Image.pyI->pY, ImgSize);
			else
//				VIB_MemCopy2D(Msg.Image.pyI, ImgBuffer, Width, Height, Width, Width);
				memcpy(ImgBuffer, Msg.Image.pyI->pY, ImgSize);

			if (bSend)
			{
				switch (Type)
				{
				case ImageFormat::BMP:
					yImage = GUI_YImage(ImgBuffer, Width, Height);
					FSize = GUI_WriteYImageMem(Buffer, &yImage);
					Header[0] = 'B';
					Header[1] = 'M';
					Header[2] = 'P';
					Header[3] = '\0';
					break;
				case ImageFormat::PNG:
					yImage = GUI_YImage(ImgBuffer, Width, Height);
					FSize = PNG_WriteYImageMem(Buffer, &yImage);
					Header[0] = 'P';
					Header[1] = 'N';
					Header[2] = 'G';
					Header[3] = '\0';
					break;
				case ImageFormat::RAW:
					FSize = ImgSize;
					Header[0] = 'R';
					Header[1] = 'A';
					Header[2] = 'W';
					Header[3] = '\0';
					break;
				}

				Ret = send(m_DSocket, &FSize, 4, 0);		// Size of image file
				Ret = send(m_DSocket, Header, 4, 0);		// File type
				if (Type == ImageFormat::RAW)
					Ret = send(m_DSocket, ImgBuffer, FSize, 0);	// RAW Data
				else
					Ret = send(m_DSocket, Buffer, FSize, 0);	// BMP/PNG Data

				if (Buffer)
					VIB_MemFree(Buffer);
				if (ImgBuffer)
					VIB_MemFree(ImgBuffer);
				pElem->SetAttribute("State", "OK");
				sprintf(Text, "%d", FSize);
				pPicture->SetAttribute("Size", Text);
				return Ret;
			}
		}
	}

	// Error
	if (ImgBuffer)
		VIB_MemFree(ImgBuffer);
	pElem->SetAttribute("State", "Fail");
	return -1;
}

/*************************************************************************/
void CXml::TaskUDPBroadcast()
/*************************************************************************/
/*! @brief Receives broadcast UDP requests and sends IP and serial number.
**************************************************************************/
{
	CPSApp *pApp = CPSApp::GetInstance();
	SOCKET sockUdp;
	sockaddr sa, SenderAddr;
	int Broadcast = 1;
	int result;
	int SenderAddrSize = sizeof(sa);
	bool bActive = true;
	char Packet[64];

	fdOpenSession(TSK_self());
	srand(CLK_gethtime());

	sockUdp = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
	if (sockUdp == INVALID_SOCKET)
	{
		printf("failed socket create (%d)\n", socketerror());
		return;
	}

	setsockopt(sockUdp, SOL_SOCKET, SO_BROADCAST, (char*)&Broadcast, (int)sizeof(int));

	memset(&sa, 0, sizeof(sa));
	sa.sin_len         = sizeof(sa);
	sa.sin_addr.s_addr = INADDR_ANY;
	sa.sin_port        = htons(UDP_PORT_IN);
	sa.sin_family      = AF_INET;
	if (bind(sockUdp, &sa, (int)sizeof(sa)) < 0)
	{
		printf("failed bind (%d)\n", socketerror());
		return;
	}

	//=== Check for broadcast message ===
	while (bActive)
	{
		// === Receive UDP broadcast ===
		result = recvfrom(sockUdp, Packet, 64, 0, &SenderAddr, &SenderAddrSize);
		if (result < 0)
		{
			CLog::Print("TaskUDPBroadcast() => recvfrom failed\n");
		}
		else
		{
			//=== Send IP & serial number ===
			if (strncmp(Packet, "Who?", 4) == 0)
			{
				VIB_SStrCpy(Packet, pApp->GetSerialNumber(), 64);

				// Send packet
				memset(&sa, 0, sizeof(sa));
				sa.sin_len         = sizeof(sa);
				sa.sin_addr.s_addr = inet_addr("255.255.255.255");
				sa.sin_port        = htons(UDP_PORT_OUT);
				sa.sin_family      = AF_INET;
				VIB_WaitBios(rand() % 128);

				if (sendto(sockUdp, Packet, 64, 0, &sa, (int)sizeof(sa)) < 0)
					CLog::Print("sendto failed (%d)\n", socketerror());
				else
					CLog::Print("Broadcast received\n");
			}
			//=== Send feature list ===
			else if (strncmp(Packet, "Get?", 4) == 0)
			{
				int length = 16;
				const char *ApplicationName;

				Packet[0] = '#';
				Packet[1] = 1 | 2 | 4 | 8 | 0x10 | 0x20; // Reset | Change IP | TCP Display | Subnet mask | Web Server | Firmware update
				Packet[2] = 1;	// Setup clock
				Packet[3] = '\0';

				// Get subnet mask
				*(unsigned int*)(Packet + 4) = inet_addr(pApp->GetSubnetMask());

				// TCP port number for firmware update
				*(unsigned short*)(Packet + 8) = TCP_UPDATE_PORT;

				// MAC Address
				if (pApp->GetEthError() == 0)
					if (GBIT_GetPhyInfo((unsigned char*)Packet + 10, NULL) == 0)
						Packet[1] |= 0x40;

				// App name
				ApplicationName = pApp->GetApplicationName();
				if (ApplicationName[0] != '\0')
				{
					Packet[1] |= 0x80;
					length += 32;
					strcpy(Packet + 16, ApplicationName);
				}

				// Send packet
				memset(&sa, 0, sizeof(sa));
				sa.sin_len         = sizeof(sa);
				sa.sin_addr.s_addr = inet_addr("255.255.255.255");
				sa.sin_port        = htons(UDP_PORT_OUT);
				sa.sin_family      = AF_INET;
				VIB_WaitBios(rand() % 128);

				if (sendto(sockUdp, Packet, length, 0, &sa, (int)sizeof(sa)) < 0)
					CLog::Print("sendto failed (%d)\n", socketerror());
			}
			//=== Reset ===
			else if (strncmp(Packet, "Res!", 4) == 0)
			{
				if (strncmp(&Packet[4], pApp->GetSerialNumber(), 32) == 0)
				{
					if (!strncmp(getenv("OS"), "Windows", 7)) // Check for JTAG connection
						exit(0);
					VIB_Reboot();
				}
			}
			//=== Change IP-Address ===
			else if (strncmp(Packet, "CIP!", 4) == 0)
			{
				if (strncmp(&Packet[4], pApp->GetSerialNumber(), 32) == 0)
				{
					struct in_addr Address, Subnetmask;
					char *pIpStr;
					char SubStr[32];

					if (result < 64 && result >= 44)
					{	// Also change subnet mask
						Subnetmask.s_addr = *(unsigned int*)(Packet + 40);
						strcpy(SubStr, inet_ntoa(Subnetmask));
					}
					else
					{
						SubStr[0] = '\0';
					}

					Address.s_addr = *(unsigned int*)(Packet + 36);
					pIpStr = inet_ntoa(Address);
					if (pApp->ChangeIpAddr(pIpStr, SubStr))
						CLog::Print("Error changing IP-Address %s\n", pIpStr);
					else
						CLog::Print("IP-Address changed to %s\n", pIpStr);
				}
			}
			//=== Set Clock ===
			else if (strncmp(Packet, "SeC!", 4) == 0)
			{
				if (strncmp(&Packet[4], pApp->GetSerialNumber(), 32) == 0)
				{
					struct tm sTime;

					sTime.tm_year = (*(unsigned short*)(Packet + 36)) - 1900;
					sTime.tm_mon = Packet[38] - 1;
					sTime.tm_mday = Packet[39];
					sTime.tm_hour = Packet[40];
					sTime.tm_min = Packet[41];
					sTime.tm_sec = Packet[42];
					mktime(&sTime);
					VIB_SetTime((unsigned int)sTime.tm_hour, (unsigned int)sTime.tm_min,  (unsigned int)sTime.tm_sec);
					VIB_SetDate((unsigned int)sTime.tm_wday, (unsigned int)sTime.tm_mday, (unsigned int)sTime.tm_mon, (unsigned int)(sTime.tm_year - 100));
				}
			}
		}
	}
}

/*************************************************************************/
void CXml::TaskFirmwareUpdate()
/*************************************************************************/
/*! @brief This task allows firmware update over TCP/IP.
**************************************************************************/
{
	sockaddr TcpAddr, SenderAddr;
	int SenderAddrSize = sizeof(TcpAddr);
	int result;
	int i;
	SOCKET sockTcp, sockUpdate;
	unsigned int VbuSize = 0;
	unsigned char *pVbuBuffer = NULL;
	char *pReplyBuffer = NULL;
	char *Text = (String)VIB_MemAlloc(10 * 1024);

	if (Text == NULL)
		return;

	fdOpenSession(TSK_self());

	//=== Create TCP Socket for updates ===
	sockTcp = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
	if (sockTcp == INVALID_SOCKET)
	{
		printf("failed socket create (%d)\n", socketerror());
		return;
	}

	memset(&TcpAddr, 0, sizeof(TcpAddr));
	TcpAddr.sin_family      = AF_INET;
	TcpAddr.sin_len         = sizeof(TcpAddr);
	TcpAddr.sin_addr.s_addr = INADDR_ANY;
	TcpAddr.sin_port        = htons(TCP_UPDATE_PORT);
	if (bind(sockTcp, &TcpAddr, (int)sizeof(TcpAddr)) < 0)
	{
		printf("failed bind (%d)\n", socketerror());
		return;
	}
	if (listen(sockTcp, 1) < 0)
	{
		printf("failed listen (%d)\n", socketerror());
		return;
	}

	while (1)
	{
		// === Wait for connection ===
		sockUpdate = accept(sockTcp, &SenderAddr, &SenderAddrSize);
		while (sockUpdate != INVALID_SOCKET)
		{
			FILE *pLogFile;
			int LogSize;
			char *ReplyText = NULL;
			char LogFile[40];
			unsigned int msg[2];

			// Receive command
			result = recv(sockUpdate, msg, 4, 0);
			if (result == 0) // Connection closed
				break;

			do
			{
				if (result != 4)
				{
					ReplyText = "TaskFirmwareUpdate() => recv() failed\r\n";
					result = -1;
					break;
				}
				result = 0;
				if (msg[0] == 1) // Command: Receive Update Archive
				{
					char *pDescription;
					int Bootloader, FPGAVersion[2], AddOnVersion[4], GBitVersion;

					// Receive file size
					if (recv(sockUpdate, &VbuSize, 4, 0) != 4 || VbuSize == 0)
					{
						ReplyText = "TaskFirmwareUpdate() => recv() failed\r\n";
						result = -1;
						break;
					}
					if (pVbuBuffer)
						VIB_MemFree(pVbuBuffer);
					pVbuBuffer = (unsigned char*)VIB_MemAlloc(VbuSize);
					if (!pVbuBuffer)
					{
						ReplyText = "TaskFirmwareUpdate() => Out of memory\r\n";
						result = -1;
						break;
					}
					// Receive update archive
					if (recvblock(sockUpdate, pVbuBuffer, VbuSize, 0) != VbuSize)
					{
						ReplyText = "TaskFirmwareUpdate() => failed to receive update file\r\n";
						result = -1;
						break;
					}
					if (VIB_FirmwareUpdateVersion(pVbuBuffer, VbuSize, &pDescription,
						&Bootloader, FPGAVersion, AddOnVersion, &GBitVersion) < 0)
					{
						ReplyText = "TaskFirmwareUpdate() => unable to read version information\r\n";
						result = -1;
						break;
					}
					Text[0] = '\0';
					if (pDescription)
					{
						strcat(Text, pDescription);
						free(pDescription);
					}
					sprintf(Text, "%s\r\nVersion information:\r\n%20s | %20s | %20s\r\n", Text, "", "Currently running", "Update Archive");
					if (Bootloader == -1)
						sprintf(Text, "%s%20s | %17d.%02d | %20s\r\n", Text, "Bootloader",
							VIB_HWInfo.Bootloader / 100, VIB_HWInfo.Bootloader % 100, "no update");
					else if (Bootloader >= 0)
						sprintf(Text, "%s%20s | %17d.%02d | %17d.%02d\r\n", Text, "Bootloader",
							VIB_HWInfo.Bootloader / 100, VIB_HWInfo.Bootloader % 100, Bootloader / 100, Bootloader % 100);
					for (i = 0; i < 2; i++)
					{
						if (FPGAVersion[i] == -1)
							sprintf(Text, "%s%18s %d | %17d.%02d | %20s\r\n", Text, "Main FPGA",
								i, VIB_HWInfo.FPGA_Version[i] / 100, VIB_HWInfo.FPGA_Version[i] % 100, "no Update");
						else if (FPGAVersion[i] >= 0)
							sprintf(Text, "%s%18s %d | %17d.%02d | %17d.%02d\r\n", Text, "Main FPGA",
								i, VIB_HWInfo.FPGA_Version[i] / 100, VIB_HWInfo.FPGA_Version[i] % 100, FPGAVersion[i] / 100, FPGAVersion[i] % 100);
					}
					for (i = 0; i < 4; i++)
					{
						if (AddOnVersion[i] == -1)
							sprintf(Text, "%s%18s %d | %17d.%02d | %20s\r\n", Text, "Add-On FPGA",
								i, VIB_HWInfo.AddOnVersion[i] / 100, VIB_HWInfo.AddOnVersion[i] % 100, "no update");
						else if (AddOnVersion[i] >= 0)
							sprintf(Text, "%s%18s %d | %17d.%02d | %17d.%02d\r\n", Text, "Add-On FPGA",
								i, VIB_HWInfo.AddOnVersion[i] / 100, VIB_HWInfo.AddOnVersion[i] % 100, AddOnVersion[i] / 100, AddOnVersion[i] % 100);
					}
					if (GBitVersion == -1)
						sprintf(Text, "%s%20s | %20d | %20s\r\n", Text, "GBit Stack Software", GBIT_Version, "no Update");
					else if (GBitVersion >= 0)
						sprintf(Text, "%s%20s | %20d | %20d\r\n", Text, "GBit Stack Software", GBIT_Version, GBitVersion);
					ReplyText = Text;
				}
				else if (msg[0] == 2) // Command: Start Update
				{
					if (!pVbuBuffer)
					{
						ReplyText = "TaskFirmwareUpdate() => error occured\r\n";
						result = -1;
						break;
					}
					// Store update log file to memory card
					sprintf(LogFile, "DRV:%s:FwUpdate.txt", VIB_GetDeviceName(VIB_Drive[1]));
					if (VIB_FindFile2(VIB_Drive[1], "FwUpdate.txt") >= 0)
					{
						VIB_DeleteFile(VIB_Drive[1], "FwUpdate.bak");
						VIB_RenameFile(VIB_Drive[1], "FwUpdate.txt", "FwUpdate.bak");
					}
					pLogFile = fopen(LogFile, "w");
					if (!pLogFile)
					{
						ReplyText = "TaskFirmwareUpdate() => failed to create log file\r\n";
						result = -1;
						break;
					}
					// Start Update
					if (VIB_FirmwareUpdate(pVbuBuffer, VbuSize, pLogFile, 0) < 0)
						result = -1;

					// Send log file
					fclose(pLogFile);
					LogSize = VIB_FindFile2(VIB_Drive[1], "FwUpdate.txt");
					if (LogSize <= 0)
					{
						ReplyText = "TaskFirmwareUpdate() => Update finished, but failed to read log file!\r\n";
					}
					else
					{
						if (pReplyBuffer)
							VIB_MemFree(pReplyBuffer);
						pReplyBuffer = (char*)VIB_MemAlloc(LogSize + 1);
						if (!pReplyBuffer)
						{
							ReplyText = "TaskFirmwareUpdate() => Update finished, but failed to send log file!\r\n";
						}
						else
						{
							VIB_ReadFile(VIB_Drive[1], "FwUpdate.txt", pReplyBuffer);
							pReplyBuffer[LogSize] = '\0';
							ReplyText = pReplyBuffer;
						}
					}
				}
				else if (msg[0] == 3) // Command: Show Info
				{
					sprintf(Text, "Version information:\r\n%20s | %20s\r\n%20s | %17d.%02d\r\n",
						"", "Currently running", "Bootloader",
						VIB_HWInfo.Bootloader / 100, VIB_HWInfo.Bootloader % 100);
					for (i = 0; i < 2; i++)
					{
						sprintf(Text, "%s%18s %d | %17d.%02d\r\n", Text, "Main FPGA",
							i, VIB_HWInfo.FPGA_Version[i] / 100, VIB_HWInfo.FPGA_Version[i] % 100);
					}
					for (i = 0; i < 4; i++)
					{
						if (VIB_HWInfo.AddOnVersion[i] > 0)
							sprintf(Text, "%s%18s %d | %17d.%02d\r\n", Text, "Add-On FPGA",
								i, VIB_HWInfo.AddOnVersion[i] / 100, VIB_HWInfo.AddOnVersion[i] % 100);
					}
					sprintf(Text, "%s%20s | %20d\r\n", Text, "GBit Stack Software", GBIT_Version);
					ReplyText = Text;
				}
				else
				{
					ReplyText = "TaskFirmwareUpdate() => unknown command received\r\n";
					result = -1;
					break;
				}
			} while (0);

			// Send result code
			msg[0] = result;
			msg[1] = ReplyText ? (strlen(ReplyText) + 1) : 0;
			send(sockUpdate, msg, 2 * 4, 0);
			if (ReplyText)
			{	// Send reply text
				send(sockUpdate, ReplyText, (int)(strlen(ReplyText) + 1), 0);
				ReplyText = NULL;
			}
		}

		if (pVbuBuffer)
		{
			VIB_MemFree(pVbuBuffer);
			pVbuBuffer = NULL;
		}
		if (pReplyBuffer)
		{
			VIB_MemFree(pReplyBuffer);
			pReplyBuffer = NULL;
		}

		// Close connection
		socketclose(sockUpdate);
		sockUpdate = INVALID_SOCKET;
	}
}

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