/* +---------------------------------------------------------------------------+
   |          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 <lib_hwdrivers/CHokuyoURG.h>
#include <lib_hwdrivers/CSerialPort.h>

using namespace UTILS;
using namespace MRML;
using namespace HWDRIVERS;


/*-------------------------------------------------------------
						Constructor
-------------------------------------------------------------*/
CHokuyoURG::CHokuyoURG() :
    m_sensorPose(0,0,0),
    m_rx_buffer(0),
    m_rx_idx(0),
    m_wr_idx(0)
{
    m_rx_buffer.reserve( 15000 );
}

/*-------------------------------------------------------------
						doProcess
-------------------------------------------------------------*/
void  CHokuyoURG::doProcess(
	bool							&outThereIsObservation,
	MRML::CObservation2DRangeScan	&outObservation,
	bool							&hardwareError )
{
	outThereIsObservation	= false;
	hardwareError			= false;

	// Bound?
	ASSERT_( m_stream != NULL);

	// Wait for a message:
	char			rcv_status0,rcv_status1;
	char			rcv_data[10000];
	int				rcv_dataLength;
	int				nRanges = m_lastRange-m_firstRange+1;
	int				expectedSize = nRanges*3 + 4;

	if (!receiveResponse( m_lastSentMeasCmd.c_str(), rcv_status0,rcv_status1, rcv_data, rcv_dataLength ) )
	{
		// No new data
		return ;
	}

	// DECODE:
	if (rcv_status0!='0' && rcv_status0!='9')
	{
		hardwareError = true;
		return;
	}

	// -----------------------------------------------
	//   Extract the observation:
	// -----------------------------------------------
	outObservation.timestamp = UTILS::SystemUtils::getCurrentTime();

	if ( expectedSize!=rcv_dataLength )
	{
		debugOut->printf("[CHokuyoURG::doProcess] ERROR: Expecting %u data bytes, received %u instead!\n",expectedSize,rcv_dataLength);
		hardwareError = true;
		return;
	}

	// Extract the timestamp of the sensor:
/*
	unsigned long sensorTimestamp =
		((rcv_data[0]-0x30) << 18) +
		((rcv_data[1]-0x30) << 12) +
		((rcv_data[2]-0x30) << 6) +
		(rcv_data[3]-0x30);
	printf("sensorTimestamp=%u\n",sensorTimestamp);
*/
	// And the scan ranges:
	outObservation.rightToLeft = true;
	outObservation.aperture = nRanges * (float)M_2PI / 1024;
	outObservation.maxRange	= 5.6f;
	outObservation.stdError = 0.003f;
	outObservation.sensorPose = m_sensorPose;

	outObservation.scan.resize(nRanges);
	outObservation.validRange.resize(nRanges);
	char		*ptr = rcv_data+4;
	for (int i=0;i<nRanges;i++)
	{
		int		b1 = (*ptr++)-0x30;
		int		b2 = (*ptr++)-0x30;
		int		b3 = (*ptr++)-0x30;

		int		range_mm = ( (b1 << 12) | (b2 << 6) | b3);

		outObservation.scan[i]			= range_mm * 0.001f;
		outObservation.validRange[i]	= range_mm>=20;
	}

	outThereIsObservation = true;
}

/*-------------------------------------------------------------
						loadConfig
-------------------------------------------------------------*/
void  CHokuyoURG::loadConfig(
	const CConfigFileBase *configSource,
	const std::string	  &iniSection )
{
	m_firstRange		= configSource->read_int(iniSection,"HOKUYO_firstRange",44);
	m_lastRange			= configSource->read_int(iniSection,"HOKUYO_lastRange",725);
	m_motorSpeed_rpm	= configSource->read_int(iniSection,"HOKUYO_motorSpeed_rpm",600);
	m_sensorPose = CPose3D(
		configSource->read_float(iniSection,"pose_x",0),
		configSource->read_float(iniSection,"pose_y",0),
		configSource->read_float(iniSection,"pose_z",0),
		DEG2RAD( configSource->read_float(iniSection,"pose_yaw",0) ),
		DEG2RAD( configSource->read_float(iniSection,"pose_pitch",0) ),
		DEG2RAD( configSource->read_float(iniSection,"pose_roll",0) )
		);
}

/*-------------------------------------------------------------
						turnOn
-------------------------------------------------------------*/
bool  CHokuyoURG::turnOn()
{
	MRPT_TRY_START;

	ASSERT_( m_stream != NULL);

	// If we are over a serial link, set it up:
	CSerialPort* COM = dynamic_cast<CSerialPort*>(m_stream);
	if (COM!=NULL)
	{
		// It is a COM:
		COM->setConfig( 19200 );
		COM->setTimeouts(10,0,150,0,0);

		// Assure the laser is off and quiet:
		switchLaserOff();
		MRPT_OS::sleep(10);

		COM->purgeBuffers();
		MRPT_OS::sleep(10);

		COM->setConfig( 115200 );
		switchLaserOff();
		MRPT_OS::sleep(10);
		COM->purgeBuffers();
		MRPT_OS::sleep(10);
		COM->setConfig( 19200 );
	}

	// Enable SCIP 2.0
	enableSCIP20();

	if (COM!=NULL)
	{
		// Set 115200 baud rate:
		setHighBaudrate();
		enableSCIP20();
		COM->setConfig( 115200 );
	}

	if (!enableSCIP20()) return false;

	// Turn on the laser:
	if (!switchLaserOn()) return false;

	// Set the motor speed:
	if (!setMotorSpeed( m_motorSpeed_rpm )) return false;

	// Display sensor information:
	if (!displaySensorInfo()) return false;
	if (!displayVersionInfo()) return false;

	// Start!
	if (!startScanningMode()) return false;

	return true;

	MRPT_TRY_END;
}

/*-------------------------------------------------------------
						turnOff
-------------------------------------------------------------*/
bool  CHokuyoURG::turnOff()
{
	// Turn off the laser:
	if (!switchLaserOff()) return false;

	return true;
}

/*-------------------------------------------------------------
						setHighBaudrate
-------------------------------------------------------------*/
bool  CHokuyoURG::setHighBaudrate()
{
	char			cmd[20];
	char			rcv_status0,rcv_status1;
	char			rcv_data[100];
	size_t			toWrite;
	int				rcv_dataLength;

	ASSERT_( m_stream != NULL);

	debugOut->printf("[CHokuyoURG::setHighBaudrate] Changing baudrate to 115200...");

	// Send command:
	MRPT_OS::strcpy(cmd,20, "SS115200\x0A");
	toWrite = 9;

	m_stream->WriteBuffer(cmd,toWrite);

	// Receive response:
	if (!receiveResponse( cmd, rcv_status0,rcv_status1, rcv_data, rcv_dataLength ) )
	{
		printf("ERROR!\n");
		return false;
	}


	// DECODE:
	if (rcv_status0!='0')
	{
		printf("ERROR!\n");
		return false;
	}

	debugOut->printf("OK\n");
	return true;
}


/*-------------------------------------------------------------
						assureBufferHasBytes
-------------------------------------------------------------*/
bool CHokuyoURG::assureBufferHasBytes(const int &nDesiredBytes)
{
    // Reset pointers?
    ASSERT_(m_wr_idx>=m_rx_idx);

    if (m_wr_idx==m_rx_idx)
    {
        m_wr_idx =
        m_rx_idx = 0;
    }

    if ( m_wr_idx-m_rx_idx < nDesiredBytes )  // Is it enough with bytes in the current buffer?
    {
        // NO: read more bytes:
        size_t nToRead = 1000;

        if ( m_rx_buffer.size()>40000 )
        {
            // Something bad is going on...
            std::cerr << "[CHokuyoURG] Clearing input buffer since it's growing too much..." << std::endl;
            m_wr_idx =
            m_rx_idx = 0;
        }

        // Assure buffer has room:
        m_rx_buffer.resize(m_wr_idx+nToRead);


        size_t nRead = m_stream->ReadBuffer(&m_rx_buffer[m_wr_idx],nToRead);
        m_wr_idx+=nRead;
        return (m_wr_idx-m_rx_idx)>=nDesiredBytes;
    }
    else return true;
}

/*-------------------------------------------------------------
						receiveResponse
-------------------------------------------------------------*/
bool  CHokuyoURG::receiveResponse(
		const char	*sentCmd_forEchoVerification,
		char	&rcv_status0,
		char	&rcv_status1,
		char	*rcv_data,
		int		&rcv_dataLength)
{
    ASSERT_(sentCmd_forEchoVerification!=NULL);
    ASSERT_(m_stream != NULL);

	try
	{
		// Process response:
		// ---------------------------------

		// COMMAND ECHO ---------
		int i=0;
		int verifLen = strlen(sentCmd_forEchoVerification);


		do
		{
            if (!assureBufferHasBytes( verifLen-i ))
                return false;

            // If matches the echo, go on:
            if ( m_rx_buffer[m_rx_idx++]==sentCmd_forEchoVerification[i] )
                i++;
            else
                i=0;
		} while ( i<verifLen );

		//printf("VERIF.CMD OK!: %s", sentCmd_forEchoVerification);


		// Now, the status bytes:
        if (!assureBufferHasBytes( 2 ))
            return false;

		rcv_status0 = m_rx_buffer[m_rx_idx++];
		rcv_status1 = m_rx_buffer[m_rx_idx++];
        //printf("STATUS: %c%c\n", rcv_status0,rcv_status1);

        // In SCIP2.0, there is an additional sum char:
        if (rcv_status1!=0x0A)
		{
			// Yes, it is SCIP2.0
            if (!assureBufferHasBytes( 1 )) return false;
            m_rx_idx++;  // Ignore this byte
			//char sumStatus = m_rx_buffer[m_rx_idx++];
            //printf("STATUS SUM: %c\n",sumStatus);
		}
		else
		{
			// Continue, it seems a SCIP1.1 response...
		}

        // After the status bytes, there is a LF:
        if (!assureBufferHasBytes( 1 )) return false;
        char nextChar = m_rx_buffer[m_rx_idx++];
        if (nextChar!=0x0A)   return false;

        // -----------------------------------------------------------------------------
        // Now the data:
        // There's a problem here, we don't know in advance how many bytes to read,
        //  so rely on the serial port class implemented buffer and call many times
        //  the read method with only 1 byte each time:
        // -----------------------------------------------------------------------------
        bool lastWasLF=false;
        i=0;
        for (;;)
        {
            if (!assureBufferHasBytes(1)) return false;
            rcv_data[i] = m_rx_buffer[m_rx_idx++];

            i++;    // One more byte in the buffer

            // No data?
            if (i==1 && rcv_data[0]==0x0A)
            {
                rcv_dataLength = 0;
                return true;
            }

            // Is a LF?
            if (rcv_data[i-1]==0x0A)
            {
                if (!lastWasLF)
                {
                    // Discard SUM+LF
                    ASSERT_(i>=2);
                    i-=2;
                }
                else
                {
                    // Discard this last LF:
                    i--;

                    // Done!
                    rcv_data[i]=0;
                    //printf("RX %u:\n'%s'\n",i,rcv_data);

                    rcv_dataLength = i;
                    return true;
                }
                lastWasLF = true;
            }
            else lastWasLF = false;
        }

	}
	catch(...)
	{
		return false;	// Serial port timeout,...
	}
}

/*-------------------------------------------------------------
						enableSCIP20
-------------------------------------------------------------*/
bool  CHokuyoURG::enableSCIP20()
{
	char			cmd[20];
	char			rcv_status0,rcv_status1;
	char			rcv_data[100];
	size_t			toWrite;
	int				rcv_dataLength;

	ASSERT_( m_stream != NULL);

	debugOut->printf("[CHokuyoURG::enableSCIP20] Changing protocol to SCIP2.0...");

	// Send command:
	MRPT_OS::strcpy(cmd,20, "SCIP2.0\x0A");
	toWrite = 8;

	m_stream->WriteBuffer(cmd,toWrite);

	// Receive response:
	if (!receiveResponse( cmd, rcv_status0,rcv_status1, rcv_data, rcv_dataLength ) )
	{
		printf("ERROR!\n");
		return false;
	}


	// DECODE:
	if (rcv_status0!='0')
	{
		printf("ERROR!\n");
		return false;
	}


	debugOut->printf("OK\n");
	return true;
}

/*-------------------------------------------------------------
						switchLaserOn
-------------------------------------------------------------*/
bool  CHokuyoURG::switchLaserOn()
{
	char			cmd[20];
	char			rcv_status0,rcv_status1;
	char			rcv_data[100];
	size_t			toWrite;
	int				rcv_dataLength;

	ASSERT_( m_stream != NULL);

	debugOut->printf("[CHokuyoURG::switchLaserOn] Switching laser ON...");

	// Send command:
	MRPT_OS::strcpy(cmd,20, "BM\x0A");
	toWrite = 3;

	m_stream->WriteBuffer(cmd,toWrite);

	// Receive response:
	if (!receiveResponse( cmd, rcv_status0,rcv_status1, rcv_data, rcv_dataLength ) )
	{
		printf("ERROR!\n");
		return false;
	}

	// DECODE:
	if (rcv_status0!='0')
	{
		printf("ERROR!\n");
		return false;
	}

	debugOut->printf("OK\n");

	return true;
}

/*-------------------------------------------------------------
						switchLaserOff
-------------------------------------------------------------*/
bool  CHokuyoURG::switchLaserOff()
{
	char			cmd[20];
	char			rcv_status0,rcv_status1;
	char			rcv_data[100];
	size_t			toWrite;
	int				rcv_dataLength;

	ASSERT_( m_stream != NULL);

	debugOut->printf("[CHokuyoURG::switchLaserOff] Switching laser OFF...");

	// Send command:
	MRPT_OS::strcpy(cmd,20, "QT\x0A");
	toWrite = 3;

	m_stream->WriteBuffer(cmd,toWrite);

	// Receive response:
	if (!receiveResponse( cmd, rcv_status0,rcv_status1, rcv_data, rcv_dataLength ) )
	{
		printf("ERROR!\n");
		return false;
	}

	// DECODE:
	if (rcv_status0!='0')
	{
		printf("ERROR!\n");
		return false;
	}

	debugOut->printf("OK\n");
	return true;
}

/*-------------------------------------------------------------
						setMotorSpeed
-------------------------------------------------------------*/
bool  CHokuyoURG::setMotorSpeed(int motoSpeed_rpm)
{
	char			cmd[20];
	char			rcv_status0,rcv_status1;
	char			rcv_data[100];
	size_t			toWrite;
	int				rcv_dataLength;

	ASSERT_( m_stream != NULL);

	debugOut->printf("[CHokuyoURG::setMotorSpeed] Setting to %i rpm...",motoSpeed_rpm);

	// Send command:
	int		motorSpeedCode = (600 - motoSpeed_rpm) / 6;
	if (motorSpeedCode<0 || motorSpeedCode>10)
	{
		printf("ERROR! Motorspeed must be in the range 540-600 rpm\n");
		return false;
	}

	MRPT_OS::sprintf(cmd,20, "CR%02i\x0A",motorSpeedCode);
	toWrite = 5;

	m_stream->WriteBuffer(cmd,toWrite);

	// Receive response:
	if (!receiveResponse( cmd, rcv_status0,rcv_status1, rcv_data, rcv_dataLength ) )
	{
		printf("ERROR!\n");
		return false;
	}

	// DECODE:
	if (rcv_status0!='0')
	{
		printf("ERROR!\n");
		return false;
	}

	debugOut->printf("OK\n");
	return true;
}

/*-------------------------------------------------------------
						displayVersionInfo
-------------------------------------------------------------*/
bool  CHokuyoURG::displayVersionInfo()
{
	char			cmd[20];
	char			rcv_status0,rcv_status1;
	char			rcv_data[1000];
	size_t			toWrite;
	int				rcv_dataLength;

	ASSERT_( m_stream != NULL);

	debugOut->printf("[CHokuyoURG::displayVersionInfo] Asking info...");

	// Send command:
	MRPT_OS::sprintf(cmd,20, "VV\x0A");
	toWrite = 3;

	m_stream->WriteBuffer(cmd,toWrite);

	// Receive response:
	if (!receiveResponse( cmd, rcv_status0,rcv_status1, rcv_data, rcv_dataLength ) )
	{
		printf("ERROR!\n");
		return false;
	}

	// DECODE:
	if (rcv_status0!='0')
	{
		printf("ERROR!\n");
		return false;
	}

	debugOut->printf("OK\n");

	// PRINT:
	for (int i=0;i<rcv_dataLength;i++)
	{
		if (rcv_data[i]==';')
			rcv_data[i]='\n';
	}
	rcv_data[rcv_dataLength]=0;

	debugOut->printf("\n------------- HOKUYO URG04: Version Information ------\n");
	debugOut->printf(rcv_data);
	debugOut->printf("-------------------------------------------------------\n\n");
	return true;
}

/*-------------------------------------------------------------
						displaySensorInfo
-------------------------------------------------------------*/
bool  CHokuyoURG::displaySensorInfo()
{
	char			cmd[20];
	char			rcv_status0,rcv_status1;
	char			rcv_data[1000];
	size_t			toWrite;
	int				rcv_dataLength;

	ASSERT_( m_stream != NULL);

	debugOut->printf("[CHokuyoURG::displaySensorInfo] Asking info...");

	// Send command:
	MRPT_OS::sprintf(cmd,20, "PP\x0A");
	toWrite = 3;

	m_stream->WriteBuffer(cmd,toWrite);

	// Receive response:
	if (!receiveResponse( cmd, rcv_status0,rcv_status1, rcv_data, rcv_dataLength ) )
	{
		printf("ERROR!\n");
		return false;
	}

	// DECODE:
	if (rcv_status0!='0')
	{
		printf("ERROR!\n");
		return false;
	}

	debugOut->printf("OK\n");

	// PRINT:
	for (int i=0;i<rcv_dataLength;i++)
	{
		if (rcv_data[i]==';')
			rcv_data[i]='\n';
	}
	rcv_data[rcv_dataLength]=0;

	debugOut->printf("\n------------- HOKUYO URG04: Product Information ------\n");
	debugOut->printf(rcv_data);
	debugOut->printf("-------------------------------------------------------\n\n");
	return true;
}

/*-------------------------------------------------------------
						startScanningMode
-------------------------------------------------------------*/
bool  CHokuyoURG::startScanningMode()
{
	char			cmd[51];
	char			rcv_status0,rcv_status1;
	char			rcv_data[3000];
	size_t			toWrite;
	int				rcv_dataLength;

	ASSERT_( m_stream != NULL);

	debugOut->printf("[CHokuyoURG::startScanningMode] Starting scanning mode...");

	// Send command:
	MRPT_OS::sprintf(cmd,50, "MD%04u%04u01000\x0A", m_firstRange,m_lastRange);
	toWrite = 16;

	m_lastSentMeasCmd = cmd;

	m_stream->WriteBuffer(cmd,toWrite);

	// Receive response:
	if (!receiveResponse( cmd, rcv_status0,rcv_status1, rcv_data, rcv_dataLength ) )
	{
		printf("ERROR!\n");
		return false;
	}

	// DECODE:
	if (rcv_status0!='0')
	{
		printf("ERROR!\n");
		return false;
	}

	debugOut->printf("OK\n");
	return true;
}
