/* +---------------------------------------------------------------------------+
   |          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>

#ifdef MRPT_OS_LINUX


#include <stdio.h>   /* Standard input/output definitions */
#include <string.h>  /* String function definitions */
#include <unistd.h>  /* UNIX standard function definitions */
#include <fcntl.h>   /* File control definitions */
#include <errno.h>   /* Error number definitions */

#include <termios.h> /* POSIX terminal control definitions */

#include <sys/ioctl.h>  // FIONREAD,...
#include <signal.h>
#include <map>


#include <MRPT/UTILS/CTicTac.h>
#include <lib_hwdrivers/CSerialPort.h>

using namespace UTILS;
using namespace HWDRIVERS;
using namespace std;

/* ===================================================
	  START OF AUXILIARY CLASSES USED BY CSerialPort

   Most of the code is adapted from the libserial 0.5.2
    library: http://libserial.sourceforge.net/

   =================================================== */

#include <stdexcept>

namespace HWDRIVERS
{

    /**

    @note The signal dispatcher will not interfere with any signals for which
    there are no signal handlers attached. Similarly, if a signal handler
    function is already attached to a signal that the dispatcher is asked to
    administer, that signal handler will be called after all the attached
    CSerialPorts.

    @todo Make this a singleton class.

    */
    class PosixSignalDispatcher
    {
    public:
        /**
         * This is a singleton class and there is only one instance of this
         * class per process. This instance can be obtained using the
         * GetInstance() method.
         */
        static PosixSignalDispatcher& Instance() ;


        /**
         * Attach a signal handler to the signal dispatcher. The signal
         * handler's HandlePosixSignal() method will be called every time
         * the specified signal is received. The signal handler should
         * not be destroyed while it attached to the signal dispatcher.
         * Otherwise, weird things are bound to happen (i.e. "undefined
         * behavior" shall ensue). Make sure you call DetachHandler() from
         * the destructor of the signal handler.
         *
         * If a CSerialPort is attached to the same signal number
         * multiple times, it will be called multiple times. Furthermore,
         * it should also be detached as many times as it was attached
         * before it is destroyed. Otherwise, undefined behavior may result.
         *
         * @param posixSignalNumber The signal number that will result in
         * call to the HandlePosixSignal() method of the signal handler.
         *
         * @param signalHandler The signal handler to be invoked on receiving
         * a posixSignalNumber signal.
         */
        void AttachHandler( const int           posixSignalNumber,
                            HWDRIVERS::CSerialPort& signalHandler );

        /**
         * Detach the specified signal handler from the signal dispatcher.
         * The signal handler will stop being called on receiving the
         * corresponding POSIX signal. If the signal handler is not
         * attached to the signal dispatcher when this method is called
         * then this method has no effect.
         *
         * @param posixSignalNumber The signal number corresponding to
         * the signal handler.
         *
         * @param signalHandler The signal handler to be detached.
         */
        void DetachHandler( const int                 posixSignalNumber,
                            const HWDRIVERS::CSerialPort& signalHandler );
    private:
        /**
         * This is a singleton class and the only instances of this class
         * can only be accessed using the Instance() method. This is
         * enforced by making the default constructor a private member
         * disalloweing construction of new instances of this class
         */
        PosixSignalDispatcher() ;

        /**
         * This class cannot be subclassed. We enforce this by making
         * the destructor a private member.
         */
        ~PosixSignalDispatcher() ;

        /**
         * Copying of an instance of this class is not allowed. We
         * enforce this by making the copy constructor and the
         * assignment operator private members.
         */
        PosixSignalDispatcher( const PosixSignalDispatcher& otherInstance ) ;

        /**
         * Copying of an instance of this class is not allowed. We
         * enforce this by making the copy constructor and the
         * assignment operator private members.
         */
        const PosixSignalDispatcher&
        operator=( const PosixSignalDispatcher& otherInstance ) ;
    };


    /**
     * Implementation class for the PosixSignalDispatcher.
     */
    class PosixSignalDispatcherImpl
    {
    public:
        /*
         * As in the case of PosixSignalDispatcher, this is also
         * a singleton class. The only instance of this class can
         * be obtained using this method.
         */
        static
        PosixSignalDispatcherImpl&
        Instance() ;

        /*
         * Implementation of PosixSignalDispatcher::AttachHandler()
         */
        void
        AttachHandler( const int           posixSignalNumber,
                       HWDRIVERS::CSerialPort& signalHandler );

        /*
         * Implementation of PosixSignalDispatcher::DetachHandler()
         */
        void
        DetachHandler( const int                 posixSignalNumber,
                       const HWDRIVERS::CSerialPort& signalHandler );
    private:
        /*
         * List of signal handlers that are currently associated
         * with the dispatcher.
         */
        typedef std::multimap<int, HWDRIVERS::CSerialPort*> SignalHandlerList ;
        static SignalHandlerList mSignalHandlerList ;

        /*
         * List of signal handlers that were originally attached
         * to the corresponding signals.
         */
        typedef std::map<int, struct sigaction> OriginalSigactionList ;
        static OriginalSigactionList mOriginalSigactionList ;

        /*
         * Default constructor.
         */
        PosixSignalDispatcherImpl() ;

        /*
         * Destuctor.
         */
        ~PosixSignalDispatcherImpl() ;

        /*
         * Static function that is used to attach the signal
         * dispatcher to a signal using sigaction().
         */
        static
        void
        SigactionHandler( int signalNumber ) ;
    } ;

//
// Initialization of static members of class PosixSignalDispatcherImpl.
//
    PosixSignalDispatcherImpl::SignalHandlerList
    PosixSignalDispatcherImpl::mSignalHandlerList ;

    PosixSignalDispatcherImpl::OriginalSigactionList
    PosixSignalDispatcherImpl::mOriginalSigactionList ;


    PosixSignalDispatcher::PosixSignalDispatcher()
    {
        /* empty */
    }

    PosixSignalDispatcher::~PosixSignalDispatcher()
    {
        /* empty */
    }

    PosixSignalDispatcher&
    PosixSignalDispatcher::Instance()
    {
        static PosixSignalDispatcher single_instance ;
        return single_instance ;
    }

    void
    PosixSignalDispatcher::AttachHandler( const int           posixSignalNumber,
                                          HWDRIVERS::CSerialPort& signalHandler )
    {
        PosixSignalDispatcherImpl::Instance().AttachHandler( posixSignalNumber,
                signalHandler ) ;
        return ;
    }

    void
    PosixSignalDispatcher::DetachHandler( const int                 posixSignalNumber,
                                          const HWDRIVERS::CSerialPort& signalHandler )
    {
        PosixSignalDispatcherImpl::Instance().DetachHandler( posixSignalNumber,
                signalHandler ) ;
    }

    inline
    PosixSignalDispatcherImpl::PosixSignalDispatcherImpl()
    {
        /* empty */
    }

    inline
    PosixSignalDispatcherImpl::~PosixSignalDispatcherImpl()
    {
        /* empty */
    }

    inline
    PosixSignalDispatcherImpl&
    PosixSignalDispatcherImpl::Instance()
    {
        static PosixSignalDispatcherImpl single_instance ;
        return single_instance ;
    }

    inline
    void
    PosixSignalDispatcherImpl::AttachHandler(
        const int           posixSignalNumber,
        HWDRIVERS::CSerialPort& signalHandler )
    {
        /*
         * Attach this instance of PosixSignalDispatcher to the specified
         * signal.
         */
        struct sigaction sigaction_info ;
        sigaction_info.sa_handler = PosixSignalDispatcherImpl::SigactionHandler ;
        sigemptyset( &sigaction_info.sa_mask ) ;
        sigaction_info.sa_flags = 0 ;
        /*
         * Install the handler and get a copy of the previous handler.
         */
        struct sigaction old_action ;
        if ( sigaction( posixSignalNumber,
                        &sigaction_info,
                        &old_action ) < 0 )
        {
            THROW_EXCEPTION_CUSTOM_MSG1("CannotAttachHandler: %s",strerror(errno));
        }
        /*
         * Save a copy of the old handler if it is not PosixSignalDispatcher::SignalHandler.
         */
        if ( PosixSignalDispatcherImpl::SigactionHandler != old_action.sa_handler )
        {
            mOriginalSigactionList.insert(
                OriginalSigactionList::value_type( posixSignalNumber,
                                                   old_action ) ) ;
        }
        /*
         * Add the specified handler to the list of handlers associated with the signal.
         */
        mSignalHandlerList.insert(
            SignalHandlerList::value_type( posixSignalNumber,
                                           &signalHandler ) ) ;
        return ;
    }

    inline
    void
    PosixSignalDispatcherImpl::DetachHandler(
        const int                 posixSignalNumber,
        const HWDRIVERS::CSerialPort& signalHandler )
    {
        /*
         * Get the range of values in the SignalHandlerList corresponding
         * to the specified signal number.
         */
        std::pair<SignalHandlerList::iterator, SignalHandlerList::iterator>
        iterator_range = mSignalHandlerList.equal_range( posixSignalNumber ) ;
        /*
         * Check if signalHandler is attached to the posixSignalNumber signal.
         */
        SignalHandlerList::iterator sig_handler_location = mSignalHandlerList.end() ;
        for ( SignalHandlerList::iterator i=iterator_range.first ;
                i != iterator_range.second ;
                ++i )
        {
            if ( i->second == &signalHandler )
            {
                sig_handler_location = i ;
                break ;
            }
        }
        /*
         * If the signal handler is found, we need to remove it from the list.
         */
        if ( mSignalHandlerList.end() != sig_handler_location )
        {
            mSignalHandlerList.erase( sig_handler_location ) ;
            /*
             * If this was the only signal handler associated with the specified
             * signal number, then we remove the signal dispatcher from handling
             * the signal and install the original signal.
             */
            if ( 0 == mSignalHandlerList.count( posixSignalNumber ) )
            {
                /*
                 * Retrieve the original sigaction corresponding to the signal.
                 */
                OriginalSigactionList::iterator original_sigaction =
                    mOriginalSigactionList.find( posixSignalNumber ) ;
                /*
                 * If the signal dispatcher implementation is correct,
                 * then we should always find the original sigaction.
                 * If we do not find one, we throw an exception.
                 */
                if ( mOriginalSigactionList.end() == original_sigaction )
                {
                    throw std::logic_error( "Signal dispatcher in invalid state." ) ;
                }
                /*
                 * Install the original handler. Throw an exception if we
                 * encounter any error.
                 */
                if ( sigaction( posixSignalNumber,
                                &original_sigaction->second,
                                NULL ) < 0 )
                {
                    THROW_EXCEPTION_CUSTOM_MSG1("CannotDettachHandler: %s!!",strerror(errno));
                }
            }
        }
        return ;
    }


    void
    PosixSignalDispatcherImpl::SigactionHandler( int signalNumber )
    {
        /*
         * Get a list of handlers associated with signalNumber.
         */
        std::pair<SignalHandlerList::iterator, SignalHandlerList::iterator>
        iterator_range = mSignalHandlerList.equal_range( signalNumber ) ;
        /*
         * Call each handler.
         */
        for ( SignalHandlerList::iterator i=iterator_range.first ;
                i != iterator_range.second ;
                ++i )
        {
            i->second->HandlePosixSignal( signalNumber ) ;
        }
#if 0
        /*
         * Get the original handler that was associated with the
         * signal and call it if possible.
         */
        OriginalSigactionList::iterator original_sigaction =
            mOriginalSigactionList.find( posixSignalNumber ) ;
        if ( ( mOriginalSigactionList.end() != original_sigaction ) &&
                ( SIG_DFL != original_sigaction->second.sa_handler ) &&
                ( SIG_IGN != original_sigaction->second.sa_handler ) &&
                ( 0 != original_sigaction->second.sa_handler ) )
        {
            original_sigaction->second.sa_handler( signalNumber ) ;
        }
#endif
        //
        return ;
    }

} // end namespace HWDRIVERS
/* ===================================================

           START OF CSerialPort CLASS ITSELF

   =================================================== */


/* -----------------------------------------------------
                Constructor
   ----------------------------------------------------- */
CSerialPort::CSerialPort( const string &portName, bool openNow )
{
    hCOM = -1;	// Not connected

    m_totalTimeout_ms      = 0;
    m_interBytesTimeout_ms = 0;

    m_serialName = portName;
    if (openNow) open();
}

/* -----------------------------------------------------
                Destructor
   ----------------------------------------------------- */
CSerialPort::~CSerialPort()
{
    if ( isOpen() )
        close();
}

/* -----------------------------------------------------
                Open
   ----------------------------------------------------- */
void  CSerialPort::open( )
{
    MRPT_TRY_START

    // Check name:
    if (!m_serialName.size()) THROW_EXCEPTION("Serial port name is empty!!")
        if (m_serialName[0]!='/') m_serialName = string("/dev/") + m_serialName;

    // Open the serial port:
    if ( -1==( hCOM= ::open( m_serialName.c_str(),  O_RDWR | O_NOCTTY | O_NDELAY ) ) )
        THROW_EXCEPTION_CUSTOM_MSG1("Error trying to open the serial port %s!!",m_serialName.c_str());

    // Clear flags:
    fcntl( hCOM, F_SETFL, 0 );

    PosixSignalDispatcher& signal_dispatcher = PosixSignalDispatcher::Instance() ;

    signal_dispatcher.AttachHandler( SIGIO, *this ) ;

    /* Direct all SIGIO and SIGURG signals for the port to the current
     * process.
     */
    if ( fcntl( hCOM,
                F_SETOWN,
                getpid() ) < 0 )
        THROW_EXCEPTION_CUSTOM_MSG1("Cannot change owner of posix dispatcher: %s",strerror(errno));

    /* Enable asynchronous I/O with the serial port.
     */
    if ( fcntl( hCOM,
                F_SETFL,
                FASYNC ) < 0 )
        THROW_EXCEPTION_CUSTOM_MSG1("Cannot set asynchronous flag: %s",strerror(errno));

    //
    // Start assembling the new port settings.
    //
    termios port_settings;
    bzero( &port_settings,sizeof( port_settings ) ) ;

    //
    // Enable the receiver (CREAD) and ignore modem control lines
    // (CLOCAL).
    //
    port_settings.c_cflag |= CREAD | CLOCAL ;

    //
    // Set the VMIN and VTIME parameters to zero by default. VMIN is
    // the minimum number of characters for non-canonical read and
    // VTIME is the timeout in deciseconds for non-canonical
    // read. Setting both of these parameters to zero implies that a
    // read will return immediately only giving the currently
    // available characters.
    //
    port_settings.c_cc[ VMIN  ] = 0 ;
    port_settings.c_cc[ VTIME ] = 0 ;
    /*
     * Flush the input buffer associated with the port.
     */
    if ( tcflush( hCOM,TCIFLUSH ) < 0 )
        THROW_EXCEPTION_CUSTOM_MSG1("Cannot flush serial port: %s",strerror(errno) ) ;

    /*
     * Write the new settings to the port.
     */
    if ( tcsetattr( hCOM,TCSANOW,&port_settings ) < 0 )
        THROW_EXCEPTION_CUSTOM_MSG1("Cannot set the new config to the serial port: %s",strerror(errno) ) ;

    // Success!
    MRPT_TRY_END
}


/* -----------------------------------------------------
                isOpen
   ----------------------------------------------------- */
bool  CSerialPort::isOpen()
{
    return hCOM != -1;
}

/* -----------------------------------------------------
                setConfig
   ----------------------------------------------------- */
void  CSerialPort::setConfig(
    int		baudRate,
    int		parity,
    int		bits,
    int		nStopBits,
    bool    enableFlowControl )
{
    MRPT_TRY_START

    // Port must be open!
    if (!isOpen()) THROW_EXCEPTION("The serial port is not open!");

    termios port_settings;
    if ( tcgetattr( hCOM, & port_settings ) < 0 )
        THROW_EXCEPTION_CUSTOM_MSG1("Cannot get the current settings: %s",strerror(errno) ) ;

    //
    // Apply baud rate
    //
    int BR;
    switch (baudRate)
    {
    	case 50: BR = B50; break;
    	case 75: BR = B75; break;
    	case 110: BR = B110; break;
    	case 134: BR = B134; break;
    	case 200: BR = B200; break;
    	case 300: BR = B300; break;
    	case 600: BR = B600; break;
    	case 1200: BR = B1200; break;
    	case 2400: BR = B2400; break;
    	case 4800: BR = B4800; break;
    	case 9600: BR = B9600; break;
    	case 19200: BR = B19200; break;
    	case 38400: BR = B38400; break;
    	case 57600: BR = B57600; break;
    	case 115200: BR = B115200; break;
    	case 230400: BR = B230400; break;
    default:
        THROW_EXCEPTION_CUSTOM_MSG1("Invalid desired baud rate value: %i",baudRate ) ;
        break;
    }

    if ( ( cfsetispeed( &port_settings,BR ) < 0 ) ||
            ( cfsetospeed( &port_settings,BR) < 0 ) )
        THROW_EXCEPTION_CUSTOM_MSG1("Cannot change baudRate in setting structure: %s",strerror(errno) ) ;

    //
    // Set the character size.
    //
    port_settings.c_cflag &= ~CSIZE ;
    switch (bits)
    {
    case 5:
        port_settings.c_cflag |= CS5;
        break;
    case 6:
        port_settings.c_cflag |= CS6;
        break;
    case 7:
        port_settings.c_cflag |= CS7;
        break;
    case 8:
        port_settings.c_cflag |= CS8;
        break;
    default:
        THROW_EXCEPTION_CUSTOM_MSG1("Invalid character size: %i",bits ) ;
        break;
    }

    // parity  0:No parity, 1:Odd, 2:Even
    switch ( parity )
    {
    case 2:
        port_settings.c_cflag |= PARENB ;
        port_settings.c_cflag &= ~PARODD ;
        port_settings.c_iflag |= INPCK ;
        break ;
    case 1:
        port_settings.c_cflag |= ( PARENB | PARODD );
        port_settings.c_iflag |= INPCK;
        break ;
    case 0:
        port_settings.c_cflag &= ~(PARENB);
        port_settings.c_iflag |= IGNPAR;
        break ;
    default:
        THROW_EXCEPTION_CUSTOM_MSG1("Invalid parity selection: %i",parity) ;
        break;
    }

    // stop bits:
    switch ( nStopBits )
    {
    case 1:
        port_settings.c_cflag &= ~(CSTOPB) ;
        break ;
    case 2:
        port_settings.c_cflag |= CSTOPB ;
        break ;
    default:
        THROW_EXCEPTION_CUSTOM_MSG1("Invalid number of stop bits: %i",nStopBits) ;
        break;
    }

    //
    // Set the flow control.
    //
    if (enableFlowControl)
    {
        // RTS/CTS ON:
        port_settings.c_cflag |= CRTSCTS ;
    }
    else
    {
        // none
        port_settings.c_cflag &= ~(CRTSCTS) ;
    }

    /* Write the new settings to the port.
     */
    if ( tcsetattr( hCOM,TCSANOW,&port_settings ) < 0 )
        THROW_EXCEPTION_CUSTOM_MSG1("Cannot set the new settings: %s",strerror(errno) ) ;

    m_baudRate = baudRate;

    MRPT_TRY_END
}

/* -----------------------------------------------------
                setConfig
   ----------------------------------------------------- */
void  CSerialPort::setTimeouts(
    int		ReadIntervalTimeout,
    int		ReadTotalTimeoutMultiplier,
    int		ReadTotalTimeoutConstant,
    int		WriteTotalTimeoutMultiplier,
    int		WriteTotalTimeoutConstant )
{
    MRPT_TRY_START

    // Port must be open!
    if (!isOpen()) THROW_EXCEPTION("The serial port is not open!");


    // Save variables which are used in other methods:
    m_totalTimeout_ms      = ReadTotalTimeoutConstant;
    m_interBytesTimeout_ms = ReadIntervalTimeout;


    // http://www.unixwiz.net/techtips/termios-vmin-vtime.html
    // VMIN & VTIME
    termios port_settings;
    if ( tcgetattr( hCOM, & port_settings ) < 0 )
        THROW_EXCEPTION_CUSTOM_MSG1("Cannot get the current settings: %s",strerror(errno) ) ;

    //
    // JLBC: We set VMIN=1 and VTIME=ReadIntervalTimeout (in thenth of seconds)
    //
    //
    port_settings.c_cc[ VMIN  ] = 0;
    port_settings.c_cc[ VTIME ] = max(1,ReadTotalTimeoutConstant / 100);

    /* Write the new settings to the port.
     */
    if ( tcsetattr( hCOM,TCSANOW,&port_settings ) < 0 )
        THROW_EXCEPTION_CUSTOM_MSG1("Cannot set the new settings: %s",strerror(errno) ) ;

    // Success
    MRPT_TRY_END
}

/* -----------------------------------------------------
                Close
   ----------------------------------------------------- */
void  CSerialPort::close(  )
{
    MRPT_TRY_START

    if (hCOM<0) return; // Already closed

    //
    PosixSignalDispatcher& signal_dispatcher = PosixSignalDispatcher::Instance() ;
    signal_dispatcher.DetachHandler( SIGIO, *this ) ;

    // Close the serial port file descriptor.
    ::close(hCOM);

    hCOM=-1;	// Means the port is closed

    MRPT_TRY_END
}

/* -----------------------------------------------------
                read
   ----------------------------------------------------- */
size_t  CSerialPort::Read(void *Buffer, size_t Count)
{
    MRPT_TRY_START

    // Port must be open!
    if (!isOpen()) THROW_EXCEPTION("The port is not open yet!");


    // Use the "m_totalTimeout_ms" global timeout
    //  and the "m_interBytesTimeout_ms" for inter-bytes:
    CTicTac tictac;

    tictac.Tic();

    size_t  alreadyRead = 0;
    int		leftTime = m_totalTimeout_ms - (int)(tictac.Tac()*1000);

    while ( alreadyRead<Count && leftTime>=0 )
    {
    	bool readOk = ReadByte( ((unsigned char*)Buffer)[alreadyRead], leftTime );

        if ( readOk )
            alreadyRead++; // 1 character copied from the internal buffer
		else MRPT_OS::sleep(1);

		// Reset interbytes timer:
        leftTime = m_totalTimeout_ms - (int)(tictac.Tac()*1000);
        if (readOk)
			leftTime = max(leftTime, m_interBytesTimeout_ms);
    }

    return alreadyRead;

    MRPT_TRY_END
}

/* -----------------------------------------------------
                write
   ----------------------------------------------------- */
size_t  CSerialPort::Write(const void *Buffer, size_t Count)
{
    MRPT_TRY_START
    // Port must be open!
    if (!isOpen()) THROW_EXCEPTION("The port is not open yet!");

    // Write the data to the serial port. Keep retrying if EAGAIN
    // error is received.

    /** \todo Add support for write timeout here
      */
    int num_of_bytes_written = -1 ;
    do
    {
        num_of_bytes_written = write( hCOM,Buffer, Count );
    }
    while ( ( num_of_bytes_written < 0 ) &&
            ( EAGAIN == errno ) ) ;
    //
    if ( num_of_bytes_written < 0 )
        THROW_EXCEPTION_CUSTOM_MSG1("Error writing data to the serial port: %s",strerror(errno) ) ;

    // OK:
    return num_of_bytes_written;

    MRPT_TRY_END
}

/* -----------------------------------------------------
                purgeBuffers
   ----------------------------------------------------- */
void  CSerialPort::purgeBuffers()
{
    MRPT_TRY_START

    // Port must be open!
    if (!isOpen()) THROW_EXCEPTION("The port is not open yet!");

    /*
     * Flush the input buffer associated with the port.
     */
    if ( tcflush( hCOM,TCIFLUSH ) < 0 )
        THROW_EXCEPTION_CUSTOM_MSG1("Cannot flush serial port: %s",strerror(errno) ) ;

    MRPT_TRY_END
}


/* -----------------------------------------------------
                HandlePosixSignal
   ----------------------------------------------------- */
void CSerialPort::HandlePosixSignal( int signalNumber )
{
    // We only want to deal with SIGIO signals here.
    if ( SIGIO != signalNumber ) return;

    // Check if any data is available at the specified file
    // descriptor.
    //
    int num_of_bytes_available = 0;
    if ( ioctl( hCOM, FIONREAD, &num_of_bytes_available ) < 0 )
    {
        /*
         * Ignore any errors and return immediately.
         */
        return ;
    }

    //
    // If data is available, read all available data and shove
    // it into the corresponding input buffer.
    //
    /*    for (int i=0; i<num_of_bytes_available; ++i)
        {
            unsigned char next_byte ;
            if ( read( mFileDescriptor,
                       &next_byte,
                       1 ) > 0 )
            {
                mInputBuffer.push( next_byte ) ;
            }
            else
            {
                break ;
            }
        }
    */
    // More optimal in this way, isn't it ??!?!
    unsigned char *newReadBlock= new unsigned char[num_of_bytes_available];
    int actuallyRead = read( hCOM,newReadBlock, num_of_bytes_available );

    // Add to the internal buffer (actuallyRead might be 0 or -1)
    for (int i=0;i<actuallyRead;i++)
        m_inputBuffer.push( newReadBlock[i] );

    delete[] newReadBlock;

    return ;
}


/* -----------------------------------------------------
                ReadByte
   ----------------------------------------------------- */
inline bool CSerialPort::ReadByte(unsigned char &readByte, const int msTimeout)
{
    MRPT_TRY_START

    // Port must be open!
    if (!isOpen()) THROW_EXCEPTION("The port is not open yet!");

    UTILS::CTicTac	tictac;

    // Start timer:
    tictac.Tic();

    //
    // Wait for data to be available.

    while ( !m_inputBuffer.size() )
    {
        //
        // Obtain the elapsed time.
        //
        int elapsed_ms = (int)(tictac.Tac()*1000);

        // Timeout??
        if ( ( msTimeout > 0 ) && ( elapsed_ms > msTimeout ) )
            return false; // False


		// Check if we are still connected or there is an error...
		int dumm;
		if ( ioctl( hCOM, FIONREAD, &dumm ) < 0 )
		{
			if (errno==EIO)
			{
				// The port has been disconnect (for USB ports)
				this->close();
				return false;
			}
		}

        //
        // Wait for 1ms for data to arrive.
        //
        MRPT_OS::sleep( 1 );
    }

    // Return the first byte and remove it from the queue.
    readByte = m_inputBuffer.front() ;
    m_inputBuffer.pop() ;

    return true; // OK

    MRPT_TRY_END
}


#endif  // linux

