/* +---------------------------------------------------------------------------+
   |          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/CGPSInterface.h>

using namespace HWDRIVERS;

/* -----------------------------------------------------
                Constructor
   ----------------------------------------------------- */
CGPSInterface::CGPSInterface(
    const std::string serialPortName,
    int			baudios,
    int			BUFFER_LENGTH)
{
	m_COMname				= serialPortName;
	m_COM					= NULL;
	m_COMbauds				= baudios;
	m_BUFFER_LENGTH			= BUFFER_LENGTH;

	m_GPS_comsWork			= false;
	m_GPS_signalAcquired	= false;
	m_bufferLength			= 0;
	m_bufferWritePos		= 0;

	m_latestGPS_data.has_GGA_datum =
	    m_latestGPS_data.has_RMC_datum = false;

	m_buffer				= new char[m_BUFFER_LENGTH];
}

/* -----------------------------------------------------
                Destructor
   ----------------------------------------------------- */
CGPSInterface::~CGPSInterface()
{
	delete[] 	m_buffer;
}

/* -----------------------------------------------------
				tryToOpenTheCOM
----------------------------------------------------- */
bool  CGPSInterface::tryToOpenTheCOM()
{
	if (m_COM)
		return true;	// Already open

	try
	{
		m_COM = new CSerialPort( m_COMname , true );

		ASSERT_(m_COM);

		// Config:
		m_COM->setConfig( m_COMbauds, 0,8,0 );

		m_COM->setTimeouts( 1,0,1,1,1 );

		// Start NMEA commands (For the JAVAD's DGPS !!!):
		// -------------------------------------------------
		const char	*INIT_STR = "%%em,,/msg/nmea/GGA:0.2\r\n";
		size_t		written;

		written = m_COM->Write(INIT_STR,strlen(INIT_STR));
		ASSERT_( written == strlen(INIT_STR) );

		INIT_STR = "%%em,,/msg/nmea/RMC:0.2\r\n";

		m_COM->Write(INIT_STR,strlen(INIT_STR));
		ASSERT_( written == strlen(INIT_STR) );

		m_latestGPS_data.has_GGA_datum =
		    m_latestGPS_data.has_RMC_datum = false;

		return true;
	}
	catch (...)
	{
		if (m_COM)	delete m_COM;
		m_COM = NULL;
		return false;
	}
}

/* -----------------------------------------------------
				isGPS_connected
----------------------------------------------------- */
bool  CGPSInterface::isGPS_connected()
{
	return m_GPS_comsWork;
}

/* -----------------------------------------------------
				isGPS_signalAcquired
----------------------------------------------------- */
bool  CGPSInterface::isGPS_signalAcquired()
{
	return m_GPS_signalAcquired;
}

/* -----------------------------------------------------
				doProcess
----------------------------------------------------- */
void  CGPSInterface::doProcess()
{
	size_t	bytesToRead, bytesRead;

	// Is the COM open?
	if (!tryToOpenTheCOM())
		return;	// Nothing we can do!

	// Read as many bytes as available:
	bytesRead = 1;
	while (bytesRead)
	{
		bytesToRead = (m_BUFFER_LENGTH-10) - m_bufferWritePos;

		try
		{
			bytesRead = m_COM->Read(m_buffer + m_bufferWritePos, bytesToRead);
		}
		catch (...)
		{
			// ERROR:
			debugOut->printf("[CGPSInterface::doProcess] Error reading COM port: Closing communications\n");
			delete m_COM;
			m_COM					= NULL;
			m_GPS_comsWork			= false;
			m_GPS_signalAcquired	= false;
			return;
		}


		// Process the data:
		if (bytesRead)
		{
			m_bufferLength += bytesRead;
			processBuffer( );
		}
	};

}

/* -----------------------------------------------------
					processBuffer
----------------------------------------------------- */
void  CGPSInterface::processBuffer()
{
	unsigned int	i=0, lineStart = 0;

	while (i<m_bufferLength)
	{
		// Loof for the first complete line of text:
		while (i<m_bufferLength && m_buffer[i]!='\r' && m_buffer[i]!='\n')
			i++;

		// End of buffer or end of line?
		if (i<m_bufferLength)
		{
			// It is the end of a line: Build the string:
			std::string			str;
			int				lenOfLine = i-1 - lineStart;
			if (lenOfLine>0)
			{
				str.resize( lenOfLine );
				memcpy( (void*)str.c_str(), m_buffer + lineStart, lenOfLine );

				// Process it!:
				processGPSstring(str);
			}

			// And this means the comms works!
			m_GPS_comsWork			= true;

			// Pass over this end of line:
			while (i<m_bufferLength && (m_buffer[i]=='\r' || m_buffer[i]=='\n'))
				i++;

			// And start a new line:
			lineStart = i;
		}

	};

	// Dejar en el buffer desde "lineStart" hasta "i-1", inclusive.
	size_t	newBufLen = m_bufferLength - lineStart;

	memcpy( m_buffer, m_buffer + lineStart, newBufLen);

	m_bufferLength = newBufLen;

	// Seguir escribiendo por aqui:
	m_bufferWritePos = i - lineStart;

}

/* -----------------------------------------------------
					processGPSstring
----------------------------------------------------- */
void  CGPSInterface::processGPSstring(const std::string &s)
{
//	debugOut->printf("[GPS raw string:] %s\n",s.c_str());

	// Firstly! If the string does not start with "$GP" it is not valid:
	if ( s[0]!='$' ||
	        s[1]!='G' ) return;

	// Try to determine the kind of command:
	if ( s[3]=='G' &&
	        s[4]=='G' &&
	        s[5]=='A' &&
	        s[6]==',')
	{

		// ---------------------------------------------
		//					GGA
		// ---------------------------------------------
		unsigned int	parserPos = 7;
		std::string			token;

		// Fill out the output structure:

		// Time:

		getNextToken(s,token,parserPos); //printf("TOKEN: %s\n",token.c_str());
		if (token.size()>=6)
		{
			m_latestGPS_data.GGA_datum.UTCTime.hour		= 10 * (token[0]-'0') + token[1]-'0';
			m_latestGPS_data.GGA_datum.UTCTime.minute	= 10 * (token[2]-'0') + token[3]-'0';
			m_latestGPS_data.GGA_datum.UTCTime.sec		= atof( & (token.c_str()[4]) );
		}

		// Latitude:
		getNextToken(s,token,parserPos); //printf("TOKEN: %s\n",token.c_str());
		if (token.size()>=4)
		{
			double	lat = 10 * (token[0]-'0') + token[1]-'0';
			lat += atof( & (token.c_str()[2]) ) / 60.0;
			m_latestGPS_data.GGA_datum.latitude_degrees = lat;
		}

		// N/S:
		getNextToken(s,token,parserPos); //printf("TOKEN: %s\n",token.c_str());
		if (token[0]=='S')
			m_latestGPS_data.GGA_datum.latitude_degrees = -m_latestGPS_data.GGA_datum.latitude_degrees;

		// Longitude:
		getNextToken(s,token,parserPos); //printf("TOKEN: %s\n",token.c_str());
		if (token.size()>=5)
		{
			double	lat = 100 * (token[0]-'0') + 10 * (token[1]-'0')+ token[2]-'0';
			lat += atof( & (token.c_str()[3]) ) / 60.0;
			m_latestGPS_data.GGA_datum.longitude_degrees = lat;
		}

		// E_W:
		getNextToken(s,token,parserPos); //printf("TOKEN: %s\n",token.c_str());
		if (token[0]=='W')
			m_latestGPS_data.GGA_datum.longitude_degrees = -m_latestGPS_data.GGA_datum.longitude_degrees;

		// fix quality:
		getNextToken(s,token,parserPos); //printf("TOKEN: %s\n",token.c_str());
		m_latestGPS_data.GGA_datum.fix_quality = (unsigned char)atoi(token.c_str());

		// sats:
		getNextToken(s,token,parserPos); //printf("TOKEN: %s\n",token.c_str());
		m_latestGPS_data.GGA_datum.satellitesUsed = (unsigned char)atoi( token.c_str() );

		// HDOP:
		getNextToken(s,token,parserPos); //printf("TOKEN: %s\n",token.c_str());
		m_latestGPS_data.GGA_datum.HDOP = (float)atof( token.c_str() );

		// Altitude:
		getNextToken(s,token,parserPos); //printf("TOKEN: %s\n",token.c_str());
		m_latestGPS_data.GGA_datum.altitude_meters = atof( token.c_str() );

		m_latestGPS_data.has_GGA_datum = true;
		m_latestGPS_data.timestamp = UTILS::SystemUtils::getCurrentTime();

		//debugOut->printf("[GPS decoded GGA string] %s\n",s.c_str());
	}
	else
	{
		// Try to determine the kind of command:
		if ( s[3]=='R' &&
		        s[4]=='M' &&
		        s[5]=='C' &&
		        s[6]==',')
		{

			// ---------------------------------------------
			//					GPRMC
			//
			// ---------------------------------------------
			unsigned int	parserPos = 7;
			std::string			token;

			// Rellenar la estructura de "ultimo dato RMC recibido"

			// Time:
			getNextToken(s,token,parserPos); //printf("TOKEN: %s\n",token.c_str());
			if (token.size()>=6)
			{
				m_latestGPS_data.RMC_datum.UTCTime.hour		= 10 * (token[0]-'0') + token[1]-'0';
				m_latestGPS_data.RMC_datum.UTCTime.minute	= 10 * (token[2]-'0') + token[3]-'0';
				m_latestGPS_data.RMC_datum.UTCTime.sec		= atof( & (token.c_str()[4]) );
			}

			// Valid?
			getNextToken(s,token,parserPos); //printf("TOKEN: %s\n",token.c_str());
			m_latestGPS_data.RMC_datum.validity_char = token.c_str()[0];

			// Latitude:
			getNextToken(s,token,parserPos); //printf("TOKEN: %s\n",token.c_str());
			if (token.size()>=4)
			{
				double	lat = 10 * (token[0]-'0') + token[1]-'0';
				lat += atof( & (token.c_str()[2]) ) / 60.0;
				m_latestGPS_data.RMC_datum.latitude_degrees = lat;
			}

			// N/S:
			getNextToken(s,token,parserPos); //printf("TOKEN: %s\n",token.c_str());
			if (token[0]=='S')
				m_latestGPS_data.RMC_datum.latitude_degrees = -m_latestGPS_data.RMC_datum.latitude_degrees;

			// Longitude:
			getNextToken(s,token,parserPos); //printf("TOKEN: %s\n",token.c_str());
			if (token.size()>=5)
			{
				double	lat = 100 * (token[0]-'0') + 10 * (token[1]-'0')+ token[2]-'0';
				lat += atof( & (token.c_str()[3]) ) / 60.0;
				m_latestGPS_data.RMC_datum.longitude_degrees = lat;
			}

			// E_W:
			getNextToken(s,token,parserPos); //printf("TOKEN: %s\n",token.c_str());
			if (token[0]=='W')
				m_latestGPS_data.RMC_datum.longitude_degrees = -m_latestGPS_data.RMC_datum.longitude_degrees;

			// Speed:
			getNextToken(s,token,parserPos); //printf("TOKEN: %s\n",token.c_str());
			m_latestGPS_data.RMC_datum.speed_knots = atof( token.c_str() );

			// Directorion:
			getNextToken(s,token,parserPos); //printf("TOKEN: %s\n",token.c_str());
			m_latestGPS_data.RMC_datum.direction_degrees= atof( token.c_str() );

			m_latestGPS_data.has_RMC_datum = true;
			m_latestGPS_data.timestamp = UTILS::SystemUtils::getCurrentTime();


			//debugOut->printf("[GPS decoded RMC string] %s\n",s.c_str());

		}
		else
		{
			// ... parse other commands
		}
	}




}

/* -----------------------------------------------------
					processGPSstring
----------------------------------------------------- */
void  CGPSInterface::getNextToken(
    const std::string	&str,
    std::string			&token,
    unsigned int		&parserPos)
{
	unsigned int	start = parserPos;
	char			c;

	if (parserPos>=str.size())
	{
		token.resize(0);
		return;
	}

	do
	{
		c = str[parserPos++];
	} while (parserPos<str.size() && c!=',');

	unsigned int	tokenSize = parserPos - 1 - start;
	if (tokenSize<1)
	{
		token.resize(0);
	}
	else
	{
		token.resize( tokenSize );
		memcpy( (void*)token.c_str(), str.c_str()+start, tokenSize );
	}

}


/* -----------------------------------------------------
					getLastData
----------------------------------------------------- */
void  CGPSInterface::getLastData(
    bool			&out_thereIsData,
    MRML::CObservationGPS &out_data
)
{
	if (m_latestGPS_data.has_GGA_datum ||
	        m_latestGPS_data.has_RMC_datum )
	{
		out_data		= m_latestGPS_data;
		out_thereIsData = true;

		m_latestGPS_data.has_GGA_datum = false;
		m_latestGPS_data.has_RMC_datum = false;
	}
	else
	{
		out_thereIsData = false;
	}
}
