/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2015, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */

// This file contains portions of code from sicklms200.cc from the Player/Stage project.

#include "hwdrivers-precomp.h"   // Precompiled headers

#include <mrpt/utils/crc.h>
#include <mrpt/utils/CTicTac.h>
#include <mrpt/system/os.h>
#include <cstdio> // printf

#include <mrpt/hwdrivers/CCANBusReader.h>

IMPLEMENTS_GENERIC_SENSOR(CCANBusReader,mrpt::hwdrivers)

#define RET_ERROR(msg) { cout << "[" << __CURRENT_FUNCTION_NAME__ <<"] " << msg << endl; return false; }

using namespace std;
using namespace mrpt;
using namespace mrpt::utils;
using namespace mrpt::obs;
using namespace mrpt::hwdrivers;

char hexCharToInt(char n)
{
    if (n >= '0' && n <= '9')
        return (n-'0');
    else

    if (n >= 'A' && n <= 'F')
        return (n-'A'+10);
    else
        return 0;
}

/*-------------------------------------------------------------
						CCANBusReader
-------------------------------------------------------------*/
CCANBusReader::CCANBusReader() :
	m_com_port(),
	m_mySerialPort( NULL ),
	m_com_baudRate(57600),
	m_nTries_connect(1),
	m_nTries_current(0),
	m_canbus_speed(250000),
	m_canreader_timestamp(false),
	m_CANBusChannel_isOpen(false)
{
	m_sensorLabel = "CANBusReader";
	memset(m_received_frame_buffer,0,sizeof(m_received_frame_buffer));
}

/*-------------------------------------------------------------
						~CCANBusReader
-------------------------------------------------------------*/
CCANBusReader::~CCANBusReader()
{
    if( m_CANBusChannel_isOpen )
	{
		try
		{
			CANBusCloseChannel();
		}
		catch(...) {}
	}

	if (m_mySerialPort)
	{
		delete m_mySerialPort;
		m_mySerialPort = NULL;
	}
}

void  CCANBusReader::doProcess()
{
    mrpt::obs::CObservationCANBusJ1939Ptr	obs = mrpt::obs::CObservationCANBusJ1939::Create();
    bool thereIsObservation;
    bool hardwareError;

    doProcessSimple( thereIsObservation, *obs, hardwareError );
    if( thereIsObservation )
        appendObservation(obs);
    else
        cout << "No frame received" << endl;
}

/*-------------------------------------------------------------
						doProcess
-------------------------------------------------------------*/
void  CCANBusReader::doProcessSimple(
	bool							    &outThereIsObservation,
	mrpt::obs::CObservationCANBusJ1939	&outObservation,
	bool							    &hardwareError )
{
	outThereIsObservation	= false;
	hardwareError			= false;

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

    m_state = ssWorking;

	// Wait for a scan:
    uint8_t out_prio,out_pdu_format,out_pdu_spec,out_src_address,out_data_length;
    uint16_t out_pgn;
    vector<uint8_t> out_data;
    vector<char> out_raw_frame;
	if (!waitContinuousSampleFrame( out_prio,
                                    out_pdu_format,
                                    out_pdu_spec,
                                    out_src_address,
                                    out_data_length,
                                    out_pgn,
                                    out_data,
                                    out_raw_frame ))
    return;

	// Yes, we have a new scan:
//    cout << "we've got a frame" << endl;
	// -----------------------------------------------
	//   Extract the observation:
	// -----------------------------------------------
	outObservation.timestamp        = mrpt::system::now();
	outObservation.sensorLabel      = m_sensorLabel;	// Set label

	// And the scan ranges:
	outObservation.m_priority       = out_prio;
	outObservation.m_pdu_spec       = out_pdu_spec;
	outObservation.m_pdu_format     = out_pdu_format;
	outObservation.m_src_address    = out_src_address;
	outObservation.m_pgn 	        = out_pgn;
	outObservation.m_data_length    = out_data_length;
	outObservation.m_data.resize( out_data.size() );
	for(uint8_t k = 0; k < out_data.size(); ++k)
        outObservation.m_data[k] = out_data[k];
    outObservation.m_raw_frame.resize( out_raw_frame.size() );
    for(uint8_t k = 0; k < out_raw_frame.size(); ++k)
        outObservation.m_raw_frame[k] = out_raw_frame[k];

	// we've got a new observation
	outThereIsObservation = true;
}

/*-------------------------------------------------------------
						loadConfig_sensorSpecific
-------------------------------------------------------------*/
void  CCANBusReader::loadConfig_sensorSpecific(
	const mrpt::utils::CConfigFileBase &configSource,
	const std::string	  &iniSection )
{
//	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) )
//		);  // irrelevant

	m_canbus_speed          = configSource.read_int(iniSection,"CANBusSpeed",m_canbus_speed);
	m_canreader_timestamp   = configSource.read_bool(iniSection,"useCANReaderTimestamp",m_canreader_timestamp);

#ifdef MRPT_OS_WINDOWS
	m_com_port = configSource.read_string(iniSection, "COM_port_WIN", m_com_port, true );
#else
	m_com_port = configSource.read_string(iniSection, "COM_port_LIN", m_com_port, true );
#endif

    m_com_baudRate = configSource.read_int(iniSection, "COM_baudRate", m_com_baudRate );
    m_nTries_connect = configSource.read_int(iniSection, "nTries_connect", m_nTries_connect );
}

/*-------------------------------------------------------------
 Tries to open the com port and setup
 all the LMS protocol. Returns true if OK or already open.
-------------------------------------------------------------*/
bool CCANBusReader::tryToOpenComms(std::string *err_msg)
{
	if (err_msg) *err_msg="";
	try
	{
		if (!m_mySerialPort)
		{
			// There is no COMMS port open yet...
			if (!m_com_port.empty())
			{
//			    cout << "Creating port" << endl;
				m_mySerialPort = new CSerialPort(); 				// Create the port myself:
			}
			else
			throw std::logic_error("ERROR: No serial port attached with bindIO, neither it set with 'setSerialPort'");
		}

		// We assure now we have a stream... try to open it, if it's not done yet.
		bool just_open = false;
//		CSerialPort* COM = dynamic_cast<CSerialPort*>(m_stream);
		if (m_mySerialPort!=NULL)
		{
			if (!m_mySerialPort->isOpen())
			{
				// Try to open it now:
				m_mySerialPort->setSerialPortName(m_com_port);
				m_mySerialPort->open(); // will raise an exception on error.

				// Set basic params:
				m_mySerialPort->setConfig(9600);
				m_mySerialPort->setTimeouts(100,0,10,0,50);

				just_open = true;
			}
		}

		// It seems the port was open and working so we are done here.
		if (!just_open)
			return true;

		// ==================================================================
		// Otherwise, it was just opened now, we must send the
		//  ** initialization commands **
		// and put the CAN Converter in recording mode:
		// ==================================================================
		cout << "Setting up serial comms in port " << m_com_port;
		if (!setupSerialComms())  RET_ERROR("error");
        cout << " ... done" << endl;

		// initialize
        // set CAN Bus speed
        /**/
        bool res;
        cout << "Setting up CAN BUS Speed at: " << m_canbus_speed << endl;
        for (int nTry=0;nTry<250000/*4*/;nTry++)
            if (true == (res=sendCANBusReaderSpeed()))
                break;
        if(!res) return false;
        cout << " ... done" << endl;

        // open the CAN channel. If true, at this point, frames should be poping out the CAN Bus
        cout << "Opening CAN BUS and starting to receive." << endl;
        for (int nTry=0;nTry<250000/*4*/;nTry++)
            if (true==(res=CANBusOpenChannel()))
                break;
        if(!res) return false;
        cout << " ... done" << endl;

//        cout << "Autopoll" << endl;
//        for (int nTry=0;nTry<250000/*4*/;nTry++)
//            if (true==(res=CANBusAutoPoll()))
//                break;
//        if(!res) return false;
//        cout << " ... done" << endl;

		return res;
		/**/
	}
	catch(std::exception &e)
	{
		std::string s = "[CCANBusReader] Error trying to open CANBusReader at port ";
		s+= e.what();
		if (err_msg) *err_msg=s;
		printf_debug(s.c_str());
		return false;
	}
}

/*-------------------------------------------------------------
 Tries to send the command to set up the speed of the
 CAN Bus reader -> tractor link
-------------------------------------------------------------*/
bool CCANBusReader::sendCANBusReaderSpeed()
{
    // command: S0 --> S8 according to the selected speed
    unsigned char cmd[2];

    cmd[0] = 'S';
    switch( m_canbus_speed )
    {
        case 10000:     cmd[1] = '0'; break;
        case 20000:     cmd[1] = '1'; break;
        case 50000:     cmd[1] = '2'; break;
        case 100000:    cmd[1] = '3'; break;
        case 125000:    cmd[1] = '4'; break;
        case 250000:    cmd[1] = '5'; break;
        case 500000:    cmd[1] = '6'; break;
        case 800000:    cmd[1] = '7'; break;
        case 1000000:   cmd[1] = '8'; break;
        default: RET_ERROR("Incorrect CAN Bus speed"); break;
    }
    sendCommandToCANReader(cmd,2);
    return waitACK(50);
}

bool CCANBusReader::CANBusOpenChannel()
{
    unsigned char cmd[1];
    cmd[0] = 'O';
    sendCommandToCANReader(cmd,1);
    m_CANBusChannel_isOpen = waitACK(50);
    return m_CANBusChannel_isOpen;
}

bool CCANBusReader::CANBusCloseChannel()
{
    unsigned char cmd[1];
    cmd[0] = 'C';
    sendCommandToCANReader(cmd,1);
//    m_CANBusChannel_isOpen = !waitACK(50);
    m_CANBusChannel_isOpen = false;
//    return !m_CANBusChannel_isOpen;
    return true;
}

bool CCANBusReader::CANBusAutoPoll()
{
    unsigned char cmd[1];
    cmd[0] = 'A';
    sendCommandToCANReader(cmd,1);
    return waitACK(50);
}

bool CCANBusReader::CANBusX1()
{
    unsigned char cmd[2];
    cmd[0] = 'X';
    cmd[1] = '1';
    sendCommandToCANReader(cmd,2);
    return waitACK(50);
}

bool CCANBusReader::CANBusPoll()
{
    unsigned char cmd[1];
    cmd[0] = 'P';
    sendCommandToCANReader(cmd,1);
    return waitACK(50);
}

/*-------------------------------------------------------------
					waitContinuousSampleFrame
-------------------------------------------------------------*/
bool CCANBusReader::waitContinuousSampleFrame(
    uint8_t &out_prio,
    uint8_t &out_pdu_format,
    uint8_t &out_pdu_spec,
    uint8_t &out_src_address,
    uint8_t &out_data_length,
    uint16_t &out_pgn,
    vector<uint8_t> &out_data,
    vector<char> &out_raw_frame)
{
//	CSerialPort* COM = dynamic_cast<CSerialPort*>(m_stream);
//	ASSERTMSG_(COM!=NULL,"No I/O channel bound to this object");

	size_t 	nRead,nBytesToRead;
	size_t	nFrameBytes = 0;
	size_t	lengthField;
	unsigned char	buf[40];

    // clear buffer
	for(uint8_t k = 0; k < 40; ++k) buf[k]=0;

	uint8_t dlc = 0;
    while( nFrameBytes < (lengthField=(10U+dlc+1U) )  )
	{
//	    cout << "trying to receive" << endl;
		if(lengthField > 30)
		{
			cout << "#" << int(dlc) << " ";
			nFrameBytes = 0;	// No es cabecera de trama correcta
            for(uint8_t k = 0; k < 40; ++k) buf[k]=0;
            dlc = 0;
		}

		if (nFrameBytes < 10)
			nBytesToRead = 1;
		else
		{
            dlc = 2*uint8_t(hexCharToInt(buf[9]));
//		    cout << "dlc: " << int(dlc) << endl;
		    nBytesToRead = (lengthField) - nFrameBytes;
		}

		try
		{
			nRead = m_mySerialPort->Read( buf+nFrameBytes,nBytesToRead );
//			cout << "to read: " << nBytesToRead << " received: " << nRead << " -> ";
//			for( uint8_t k = 0; k < nRead; ++k )
//                cout << int(buf[k+nFrameBytes]);
//            cout << endl;
		}
		catch (std::exception &e)
		{
			// Disconnected?
			printf_debug("[CCANBusReader::waitContinuousSampleFrame] Disconnecting due to comms error: %s\n", e.what());
			return false;
		}

		if ( !nRead )
			return false;

		if (nRead<nBytesToRead)
			mrpt::system::sleep(30);

		// Reading OK:
		// Was it the first one?
		if (nFrameBytes>0 || (nFrameBytes == 0 && buf[0]==0x54 /*T*/))
            nFrameBytes+=nRead;
		else
		{
			nFrameBytes = 0;	// No es cabecera de trama correcta
			for(uint8_t k = 0; k < 40; ++k) buf[k]=0;
		}
	} // end while

    // Process frame
    // convert ASCII text into integer
    vector<uint8_t> aux;
    out_raw_frame.resize(nFrameBytes);
    for(uint8_t k = 0; k < nFrameBytes; ++k)
    {
        aux.push_back( hexCharToInt(buf[k]) );
        out_raw_frame[k] = buf[k];
    }

    out_prio        = (aux[1] << 2) | (aux[2] >> 2);
    out_pdu_format  = (aux[3] << 4) | aux[4];
    out_pdu_spec    = (aux[5] << 4) | aux[6];
    out_src_address = (aux[7] << 4) | aux[8];
    out_data_length = aux[9];
    out_pgn         = uint16_t(out_pdu_format) << 8 | uint16_t(out_pdu_spec);
    out_data.resize(out_data_length);
    for(uint8_t k = 0, k2 = 0; k < 2*out_data_length; k+=2, k2++)
        out_data[k2] = (aux[10+k] << 4) | aux[11+k];

    if( buf[nFrameBytes-1] != 0x0D )
    {
        cout << format("[CCANBusReader::waitContinuousSampleFrame] expected 0x0D ending flag, 0x%X found instead", buf[nFrameBytes]) << endl;
		return false; // Bad ending flag
    }

	// All OK
	return true;
}

/*-------------------------------------------------------------
						initialize
-------------------------------------------------------------*/
void CCANBusReader::initialize()
{
	string err_str;
	memset(m_received_frame_buffer,0,sizeof(m_received_frame_buffer));
	if (!tryToOpenComms(&err_str))
    {
        cerr << err_str << endl;
		throw std::logic_error(err_str);
    }
}

/*-----------------------------------------------------------------
	Assures laser is connected and operating at 38400, in
	 its case returns true.
  -----------------------------------------------------------------*/
bool CCANBusReader::setupSerialComms()
{
    ASSERT_(m_com_baudRate==9600 || m_com_baudRate==38400 || m_com_baudRate == 57600 || m_com_baudRate==500000);

//	CSerialPort* COM = dynamic_cast<CSerialPort*>(m_stream);
	if (m_mySerialPort==NULL) return true;

    int detected_rate = 0;
	for (size_t reps=0;!detected_rate && reps<m_nTries_connect;reps++)
	{
        m_nTries_current=reps;

        int rates[] = {0, 9600,38400,57600,500000};

        // Try first the desired rate to speed up the process, just in case
        //  the converter is already setup from a previous run:
        rates[0] = m_com_baudRate;

        detected_rate = 0;

        for (size_t i=0;!detected_rate && i<sizeof(rates)/sizeof(rates[0]);i++)
        {
            // Are we already receiving at 500k?
            // ------------------------------------------------
            m_mySerialPort->setConfig( rates[i] );
            mrpt::system::sleep(100);
            m_mySerialPort->purgeBuffers();

            // close the connection
            /**/
            cout << endl << "Closing CAN Channel " << endl;
            for (int nTry=0;nTry<250000/*4*/;nTry++)
                if( true==CANBusCloseChannel() )
                    break;
            cout << " ... done" << endl;
            /**/

            mrpt::system::sleep(100);
            m_mySerialPort->purgeBuffers();

            for (int nTry=0;nTry<250000/*4*/ && !detected_rate;nTry++)
            {
                m_mySerialPort->purgeBuffers();

                // Ask for the laser version at the current rate:
                if( queryVersion(true) )
                {
                    detected_rate = rates[i];
                    break;
                }
                mrpt::system::sleep(20);
            } // for tries
            // There is no link, or the baudrate is wrong...
        }

        // Try again in a while:
        if (!detected_rate && reps!=(m_nTries_connect-1))
            mrpt::system::sleep(5000);
	}

	// Switch "this" serial port to the detected baudrate
    setBaudRate( detected_rate );

	m_mySerialPort->setConfig(m_com_baudRate);
	m_mySerialPort->purgeBuffers();

	// Wait...
	mrpt::system::sleep(500);

	// And check comms at the new baud rate:
	return true;
}

/*-----------------------------------------------------------------
  Query to LMS a status query.
   Returns true if response is read ok.
  -----------------------------------------------------------------*/
bool CCANBusReader::queryVersion(bool printOutVersion)
{
	ASSERT_(m_mySerialPort);

	uint8_t cmd[1];
	cmd[0] = 'V';
    uint16_t cmd_len = 1;

    if (!sendCommandToCANReader(cmd,cmd_len,false)) return false;
	return waitForVersion(500, printOutVersion);
}

// Returns false if timeout
bool CCANBusReader::waitACK(uint16_t timeout_ms)
{
//	CSerialPort* COM = dynamic_cast<CSerialPort*>(m_stream);
//	ASSERT_(COM);

	uint8_t b = 0;
	CTicTac tictac;
	tictac.Tic();

	do
	{
		if ( m_mySerialPort->Read(&b,1) )
		{
		    // Byte rx:
			if(b==0x0D/*0x30*/)
			{
			    cout << int(b) << endl;
			    return true;       // [CR]
			}

		}
	} while(tictac.Tac()<timeout_ms*1e-3 );

	if (b==0x07)                            // [BELL]
        RET_ERROR(format("ERROR received."))
	else if (b!=0)
         RET_ERROR(format("Unexpected code received: 0x%02X",b))
    else
        return false; //RET_ERROR("Timeout")
}

bool CCANBusReader::waitForVersion(uint16_t timeout, bool printOutVersion )
{
//    CSerialPort* COM = dynamic_cast<CSerialPort*>(m_stream);
//	ASSERT_(COM);

	uint8_t b;
	unsigned int nBytes=0;

	CTicTac tictac;
	tictac.Tic();
	const double maxTime = timeout*1e-3;

	while( nBytes<6 )
	{
		if ( m_mySerialPort->Read(&b,1) )
		{
//		    cout << "received " << nBytes << " bytes: " << char(b) << endl;
			// First byte must be STX:
			if ( nBytes > 0 || (nBytes == 0 && b == 'V') )
			{
				// Store in frame:
				m_received_frame_buffer[nBytes] = b;
				nBytes++;
			}
		}
		if (tictac.Tac()>=maxTime)
		{
		    cout << "Version timeout" << endl;
		    return false;	// Timeout
		}
	}

	// Check len:
	if (m_received_frame_buffer[nBytes-1] != 0x0D )
	{
        printf("[CCANBusReader::waitForVersion] Error: expected 0x0D final byte, received %x\n", m_received_frame_buffer[nBytes-1]);
        return false;
	}

	if( printOutVersion )
	{
	    cout << "Version: ";
        for(uint8_t k = 0; k < nBytes; ++k)
            cout << char(m_received_frame_buffer[k]);
        cout << endl;
	}
	return true;
}
// Returns false if timeout
bool CCANBusReader::waitIncomingFrame(uint16_t timeout)
{
//	CSerialPort* COM = dynamic_cast<CSerialPort*>(m_stream);
//	ASSERT_(COM);

	uint8_t b;
	unsigned int nBytes=0;

	CTicTac tictac;
	tictac.Tic();
	const double maxTime = timeout*1e-3;
    uint8_t dlc = 0;
	while( nBytes<10 || (nBytes<(10U+dlc+1U/*CR*/)) )
	{
		if(m_mySerialPort->Read(&b,1) )
		{
			// First byte must be STX:
			if( nBytes>1 || (!nBytes && b==0x54 /*'T'*/) )
			{
				// Store in frame:
				m_received_frame_buffer[nBytes] = b;
				nBytes++;
			}
			if( nBytes == 10 )
                dlc = 2*uint8_t(hexCharToInt(m_received_frame_buffer[9]));   // here is the number of BYTES of data -> 2 hex values for byte
		}
		if (tictac.Tac()>=maxTime)
			return false;	// Timeout
	}
    // Check final flag
	if (m_received_frame_buffer[10U+dlc] != 0x0D)
	{
        printf("[CCANBusReader::waitIncomingFrame] Error: expected 0x0D as final flag, received %x\n",m_received_frame_buffer[10U+dlc]);
        return false;
	}

#if 0
    printf("RX: ");
    for (unsigned int i=0;i<nBytes;i++)
        printf("%02X ",m_received_frame_buffer[i]);
    printf("\n");
#endif

	// OK
	return true;
}

bool CCANBusReader::sendCommandToCANReader(const uint8_t *cmd,const uint16_t cmd_len, bool wait)
{
	MRPT_UNUSED_PARAM(wait);
    uint8_t cmd_full[1024];
    ASSERT_(sizeof(cmd_full)>cmd_len);

//	CSerialPort* COM = dynamic_cast<CSerialPort*>(m_stream);
//	ASSERT_(COM);

	// command is just plain text so no frame header nor CRC is needed
	memcpy(cmd_full,cmd,cmd_len);
	cmd_full[cmd_len] = 0x0D;       // [CR] at the end

    const size_t toWrite = cmd_len+1;

#if 1
    printf("TX: ");
    for (unsigned int i=0;i<toWrite;i++)
        printf("%02X ",cmd_full[i]);
    printf("\n");
#endif

//    const int NTRIES = 3;

//    for (int k=0;k<NTRIES;k++)
//    {
        if (toWrite!=m_mySerialPort->Write( cmd_full, toWrite ))
        {
            cout << "[CCANBusReader::SendCommandToCANReader] Error writing data to serial port." << endl;
            return false;
        }
        return true;
//        mrpt::system::sleep(15);
//        if(wait)
//        {
//            if(waitACK(5000))
//                return true;
//            mrpt::system::sleep(10);
//        }
//        else
//            return true; // perform special wait outside this method
//    }

    return false;
}

