/* +---------------------------------------------------------------------------+
   |          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/utils_defs.h>
#include <MRPT/UTILS/MRPT_OS.h>

#include <lib_hwdrivers/CSickLaserUSB.h>

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


int CSickLaserUSB::CRC16_GEN_POL = 0x8005;

/*-------------------------------------------------------------
						CSickLaserUSB
-------------------------------------------------------------*/
CSickLaserUSB::CSickLaserUSB()
{
	MRPT_TRY_START;
	m_serialNumber = "LASER001";
	m_usbConnection = new CInterfaceFTDI();
	MRPT_TRY_END;
}

/*-------------------------------------------------------------
						~CSickLaserUSB
-------------------------------------------------------------*/
CSickLaserUSB::~CSickLaserUSB()
{
	MRPT_TRY_START;
	delete m_usbConnection;
	MRPT_TRY_END;
}

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

	if ( !checkControllerIsConnected() )
	{
		hardwareError = true;
		return;
	}

	short			ranges[361];
	unsigned char	LMS_stat;

	// Wait for a scan:
	if (!waitContinuousSampleFrame( ranges,LMS_stat ))
		return;

	// Yes, we have a new scan:

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

	// Extract the timestamp of the sensor:

	// And the scan ranges:
	outObservation.rightToLeft = true;
	outObservation.aperture = M_PIf;
	outObservation.maxRange	= 81.0f;
	outObservation.stdError = 0.002f;
	outObservation.sensorPose = m_sensorPose;

	outObservation.scan.resize(361);
	outObservation.validRange.resize(361);

	for (size_t i=0;i<361;i++)
	{
		outObservation.scan[i]			= 0.01f*ranges[i];
		outObservation.validRange[i]	= (outObservation.scan[i]<=80.0f);
	}

	outThereIsObservation = true;
}

/*-------------------------------------------------------------
						loadConfig
-------------------------------------------------------------*/
void  CSickLaserUSB::loadConfig(
	const CConfigFileBase *configSource,
	const std::string	  &iniSection )
{
	m_serialNumber = configSource->read_string(iniSection.c_str(),"SICKUSB_serialNumber",m_serialNumber);
	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  CSickLaserUSB::turnOn()
{
	return true;
}

/*-------------------------------------------------------------
						turnOff
-------------------------------------------------------------*/
bool  CSickLaserUSB::turnOff()
{
	return true;
}

/*-------------------------------------------------------------
					checkControllerIsConnected
-------------------------------------------------------------*/
bool  CSickLaserUSB::checkControllerIsConnected()
{
	// If device is already open, thats ok:
	if (m_usbConnection->isOpen())
		return true;

	// If it isn't, try to open it now:
	try
	{
		m_usbConnection->OpenBySerialNumber( m_serialNumber );
		m_usbConnection->ResetDevice( );
		MRPT_OS::sleep(10);
		m_usbConnection->SetTimeouts( 20, 20 );	// read, write, in milliseconds
		MRPT_OS::sleep(10);
		m_usbConnection->SetLatencyTimer(6);		// 1ms, the minimum
		MRPT_OS::sleep(10);

		debugOut->printf("[CSickLaserUSB] USB DEVICE S/N:'%s' OPEN SUCCESSFULLY!!!\n",m_serialNumber.c_str() );
		return true;
	}
	catch(std::exception &e)
	{
		debugOut->printf(e.what());
		debugOut->printf("[CSickLaserUSB] ERROR TRYING TO OPEN USB DEVICE S/N:'%s'\n",m_serialNumber.c_str() );
		return false;
	}
}

/*-------------------------------------------------------------
					waitContinuousSampleFrame
-------------------------------------------------------------*/
bool  CSickLaserUSB::waitContinuousSampleFrame( short ranges[361], unsigned char &LMS_status )
{
	unsigned long	nRead,nBytesToRead;
	int				nFrameBytes = 0;
	int				lenghtField;
	unsigned char	buf[1000];
	buf[2]=buf[3]=0;

	while ( nFrameBytes < (lenghtField=( 6 + (buf[2] | (buf[3] << 8))) ))
	{
		if (lenghtField>800)
		{
			printf("#");
			nFrameBytes = 0;	// No es cabecera de trama correcta
			buf[2]=buf[3]=0;
		}

		if (nFrameBytes<4) nBytesToRead = 1;
			else nBytesToRead = lenghtField  - nFrameBytes;

		try
		{
			nRead = m_usbConnection->ReadBuffer( buf+nFrameBytes,nBytesToRead );
		}
		catch (std::exception &)
		{
			// Disconnected?
			m_usbConnection->Close();
			return false;
		}

		if ( nRead != nBytesToRead )
			return false;
		else
			if (nRead>0)
			{
				// Lectura OK:
				// Era la primera?
				if (nFrameBytes>1 || (!nFrameBytes && buf[0]==0x02) || (nFrameBytes==1 && buf[1]==0x80))
						nFrameBytes+=nRead;
				else
				{
					printf(".");
					nFrameBytes = 0;	// No es cabecera de trama correcta
					buf[2]=buf[3]=0;
				}
			}
	}

	// Trama completa:
	//  Checkear que el byte de comando es 0xB0:
	if ( buf[4]!=0xB0 )		return false;

	// Copiar rangos:
	for (int i=0;i<361;i++)
		ranges[i] = (buf[7+i*2] | (buf[8+i*2] << 8)) & 0x1FFF;

	// Status
	LMS_status = buf[729];

	// CRC:
	short CRC = computeCRC(buf,lenghtField-2);
	short CRC_packet = buf[lenghtField-2] | ( buf[lenghtField-1] << 8);

	return (CRC_packet==CRC);
}


/*-------------------------------------------------------------
					ComputeCRC
-------------------------------------------------------------*/
short  CSickLaserUSB::computeCRC(unsigned char *data, unsigned long len)
{
  short			uCrc16;
  unsigned char abData[2];

  uCrc16 = 0;
  abData[0] = 0;

  while(len-- )
  {
    abData[1] = abData[0];
    abData[0] = *data++;

    if( uCrc16 & 0x8000 )
    {
      uCrc16 = (uCrc16 & 0x7fff) << 1;
      uCrc16 ^= CRC16_GEN_POL;
    }
    else
    {
      uCrc16 <<= 1;
    }
    uCrc16 ^= (abData[0] | (abData[1]<<8));
  }
  return (uCrc16);
}
