/* +---------------------------------------------------------------------------+
   |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
   |                                                                           |
   |                   http://mrpt.sourceforge.net/                            |
   |                                                                           |
   |   Copyright (C) 2005-2008  University of Malaga                           |
   |                                                                           |
   |    This software was written by the Perception and Robotics               |
   |      research group, University of Malaga (Spain).                        |
   |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
   |                                                                           |
   |  This file is part of the MRPT project.                                   |
   |                                                                           |
   |     MRPT is free software: you can redistribute it and/or modify          |
   |     it under the terms of the GNU General Public License as published by  |
   |     the Free Software Foundation, either version 3 of the License, or     |
   |     (at your option) any later version.                                   |
   |                                                                           |
   |   MRPT is distributed in the hope that it will be useful,                 |
   |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
   |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
   |     GNU General Public License for more details.                          |
   |                                                                           |
   |     You should have received a copy of the GNU General Public License     |
   |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
   |                                                                           |
   +---------------------------------------------------------------------------+ */


#include <MRPT/UTILS/MRPT_OS.h>
#include <lib_hwdrivers/CInterfaceFTDIMessages.h>

using namespace UTILS;
using namespace HWDRIVERS;

/*-------------------------------------------------------------
					CInterfaceFTDIMessages
-------------------------------------------------------------*/
CInterfaceFTDIMessages::CInterfaceFTDIMessages()
{
}

/*-------------------------------------------------------------
					~CInterfaceFTDIMessages
-------------------------------------------------------------*/
CInterfaceFTDIMessages::~CInterfaceFTDIMessages()
{
}

/*-------------------------------------------------------------
					sendMessage
-------------------------------------------------------------*/
void  CInterfaceFTDIMessages::sendMessage( const UTILS::CMRPTMessage &msg)
{
	MRPT_TRY_START;

	unsigned char	buf[1024];
	unsigned int	nBytesTx = 0;

	ASSERT_(msg.content.size()<256);

	// Build frame -------------------------------------
	buf[nBytesTx++] = 0x69;
	buf[nBytesTx++] = (unsigned char)(msg.type);
	buf[nBytesTx++] = (unsigned char)msg.content.size();
	if (msg.content.size())
		memcpy(	buf+nBytesTx, &msg.content[0], msg.content.size() );
	nBytesTx += (unsigned char)msg.content.size();
	buf[nBytesTx++] = 0x96;

	// Send buffer -------------------------------------
	WriteBuffer(buf,nBytesTx);  // Exceptions will be raised on errors here

	MRPT_TRY_END;
}


/*-------------------------------------------------------------
					receiveMessage
-------------------------------------------------------------*/
bool  CInterfaceFTDIMessages::receiveMessage( UTILS::CMRPTMessage &msg )
{
	MRPT_TRY_START;

	unsigned char		buf[512];
	unsigned int		nBytesInFrame=0;
	unsigned long		nBytesToRx, nBytesRx;
	unsigned char		tries = 2;

	for (;;)
	{
		if (nBytesInFrame<3)
			nBytesToRx = 1;
		else
			nBytesToRx = (buf[2]+4) - nBytesInFrame;

		ftdi_read( buf+nBytesInFrame, nBytesToRx, &nBytesRx );

		// No more data! (read timeout is already included in the call to "Read")
		if (!nBytesRx)
			return false;

		if (!nBytesInFrame && buf[0]!=0x69)
		{
			// Start flag is invalid:
			if (!tries--) return false;
		}
		else
		{
			// Is a new byte for the frame:
			nBytesInFrame += nBytesRx;

			if (nBytesInFrame == (unsigned int)(buf[2]+4))
			{
				// check for frame be ok:

				// copy out data:
				msg.type = buf[1];
				msg.content.resize( buf[2] );
				if (msg.content.size())
					memcpy( &msg.content[0], &buf[3], buf[2] );
				return true;
			}

		}
	}

	MRPT_TRY_END;
}
