From 7b5ffc31cd86161ea95b06e25c5d3d54a6e952ee Mon Sep 17 00:00:00 2001
From: Don Gagne <don@thegagnes.com>
Date: Sat, 28 Dec 2013 15:05:20 -0800
Subject: [PATCH] Cleanup of TCPLink from unit test findings

---
 qgroundcontrol.pro                |   8 +-
 src/comm/SerialLink.cc            |   5 +-
 src/comm/SerialLink.h             |   5 +-
 src/comm/TCPLink.cc               | 167 ++++++++++++++++++++------------------
 src/comm/TCPLink.h                |  99 +++++++++++-----------
 src/ui/QGCTCPLinkConfiguration.cc |   2 +-
 6 files changed, 147 insertions(+), 139 deletions(-)

diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro
index 84c0d30..eb2af7f 100644
--- a/qgroundcontrol.pro
+++ b/qgroundcontrol.pro
@@ -751,13 +751,17 @@ CONFIG(debug, debug|release) {
         src/qgcunittest/UASUnitTest.h \
         src/qgcunittest/MockUASManager.h \
         src/qgcunittest/MockUAS.h \
-        src/qgcunittest/MockQGCUASParamManager.h
+        src/qgcunittest/MockQGCUASParamManager.h \
+        src/qgcunittest/MultiSignalSpy.h \
+        src/qgcunittest/TCPLinkTest.h
 
     SOURCES += \
         src/qgcunittest/UASUnitTest.cc \
         src/qgcunittest/MockUASManager.cc \
         src/qgcunittest/MockUAS.cc \
-        src/qgcunittest/MockQGCUASParamManager.cc 
+        src/qgcunittest/MockQGCUASParamManager.cc \
+        src/qgcunittest/MultiSignalSpy.cc \
+        src/qgcunittest/TCPLinkTest.cc
 }
 
 # Enable Google Earth only on Mac OS and Windows with Visual Studio compiler
diff --git a/src/comm/SerialLink.cc b/src/comm/SerialLink.cc
index 8e1bfed..e8bf610 100644
--- a/src/comm/SerialLink.cc
+++ b/src/comm/SerialLink.cc
@@ -413,8 +413,7 @@ bool SerialLink::hardwareConnect()
     }
 
     QObject::connect(m_port,SIGNAL(aboutToClose()),this,SIGNAL(disconnected()));
-    QObject::connect(m_port, SIGNAL(error(SerialLinkPortError_t)),
-                     this, SLOT(linkError(SerialLinkPortError_t)));
+    QObject::connect(m_port, SIGNAL(error(QSerialPort::SerialPortError)), this, SLOT(linkError(QSerialPort::SerialPortError)));
 
 //    port->setCommTimeouts(QSerialPort::CtScheme_NonBlockingRead);
 
@@ -444,7 +443,7 @@ bool SerialLink::hardwareConnect()
     return true; // successful connection
 }
 
-void SerialLink::linkError(SerialLinkPortError_t error)
+void SerialLink::linkError(QSerialPort::SerialPortError error)
 {
     qDebug() << error;
 }
diff --git a/src/comm/SerialLink.h b/src/comm/SerialLink.h
index 2245a0e..df38779 100644
--- a/src/comm/SerialLink.h
+++ b/src/comm/SerialLink.h
@@ -40,9 +40,6 @@ This file is part of the QGROUNDCONTROL project
 #include <configuration.h>
 #include "SerialLinkInterface.h"
 
-// convenience type for passing errors
-typedef  QSerialPort::SerialPortError SerialLinkPortError_t;
-
 /**
  * @brief The SerialLink class provides cross-platform access to serial links.
  * It takes care of the link management and provides a common API to higher
@@ -137,7 +134,7 @@ public slots:
     bool connect();
     bool disconnect();
 
-    void linkError(SerialLinkPortError_t error);
+    void linkError(QSerialPort::SerialPortError error);
 
 protected:
     quint64 m_bytesRead;
diff --git a/src/comm/TCPLink.cc b/src/comm/TCPLink.cc
index d2044a8..50a47f5 100644
--- a/src/comm/TCPLink.cc
+++ b/src/comm/TCPLink.cc
@@ -21,13 +21,6 @@
  
  ======================================================================*/
 
-/**
- * @file
- *   @brief Definition of TCP connection (server) for unmanned vehicles
- *   @author Lorenz Meier <mavteam@student.ethz.ch>
- *
- */
-
 #include <QTimer>
 #include <QList>
 #include <QDebug>
@@ -37,26 +30,29 @@
 #include "LinkManager.h"
 #include "QGC.h"
 #include <QHostInfo>
+#include <QSignalSpy>
 
-TCPLink::TCPLink(QHostAddress hostAddress, quint16 socketPort) :
-    host(hostAddress),
-    port(socketPort),
-    socket(NULL),
-    socketIsConnected(false)
+/// @file
+///     @brief TCP link type for SITL support
+///
+///     @author Don Gagne <don@thegagnes.com>
 
+TCPLink::TCPLink(QHostAddress hostAddress, quint16 socketPort) :
+    _hostAddress(hostAddress),
+    _port(socketPort),
+    _socket(NULL),
+    _socketIsConnected(false)
 {
-    // Set unique ID and add link to the list of links
-    this->id = getNextLinkId();
-	this->name = tr("TCP Link (port:%1)").arg(this->port);
-	emit nameChanged(this->name);
+    _linkId = getNextLinkId();
+    _resetName();
     
-    qDebug() << "TCP Created " << this->name;
+    qDebug() << "TCP Created " << _name;
 }
 
 TCPLink::~TCPLink()
 {
     disconnect();
-	this->deleteLater();
+	deleteLater();
 }
 
 void TCPLink::run()
@@ -64,45 +60,47 @@ void TCPLink::run()
 	exec();
 }
 
-void TCPLink::setAddress(const QString &text)
-{
-    setAddress(QHostAddress(text));
-}
-
-void TCPLink::setAddress(QHostAddress host)
+void TCPLink::setHostAddress(QHostAddress hostAddress)
 {
-    bool reconnect(false);
-	if (this->isConnected())
-	{
+    bool reconnect = false;
+    
+	if (this->isConnected()) {
 		disconnect();
 		reconnect = true;
 	}
-	this->host = host;
-	if (reconnect)
-	{
+    
+	_hostAddress = hostAddress;
+    _resetName();
+    
+	if (reconnect) {
 		connect();
 	}
 }
 
+void TCPLink::setHostAddress(const QString& hostAddress)
+{
+    setHostAddress(QHostAddress(hostAddress));
+}
+
 void TCPLink::setPort(int port)
 {
-	bool reconnect(false);
-	if(this->isConnected())
-	{
+    bool reconnect = false;
+    
+	if (this->isConnected()) {
 		disconnect();
 		reconnect = true;
 	}
-    this->port = port;
-	this->name = tr("TCP Link (port:%1)").arg(this->port);
-	emit nameChanged(this->name);
-	if(reconnect)
-	{
+    
+	_port = port;
+    _resetName();
+    
+	if (reconnect) {
 		connect();
 	}
 }
 
 #ifdef TCPLINK_READWRITE_DEBUG
-void TCPLink::writeDebugBytes(const char *data, qint16 size)
+void TCPLink::_writeDebugBytes(const char *data, qint16 size)
 {
     QString bytes;
     QString ascii;
@@ -119,7 +117,7 @@ void TCPLink::writeDebugBytes(const char *data, qint16 size)
             ascii.append(219);
         }
     }
-    qDebug() << "Sent" << size << "bytes to" << host.toString() << ":" << port << "data:";
+    qDebug() << "Sent" << size << "bytes to" << _hostAddress.toString() << ":" << _port << "data:";
     qDebug() << bytes;
     qDebug() << "ASCII:" << ascii;
 }
@@ -128,9 +126,9 @@ void TCPLink::writeDebugBytes(const char *data, qint16 size)
 void TCPLink::writeBytes(const char* data, qint64 size)
 {
 #ifdef TCPLINK_READWRITE_DEBUG
-    writeDebugBytes(data, size);
+    _writeDebugBytes(data, size);
 #endif
-    socket->write(data, size);
+    _socket->write(data, size);
 
     // Log the amount and time written out for future data rate calculations.
     QMutexLocker dataRateLocker(&dataRateMutex);
@@ -145,14 +143,14 @@ void TCPLink::writeBytes(const char* data, qint64 size)
  **/
 void TCPLink::readBytes()
 {
-    qint64 byteCount = socket->bytesAvailable();
+    qint64 byteCount = _socket->bytesAvailable();
     
     if (byteCount)
     {
         QByteArray buffer;
         buffer.resize(byteCount);
         
-        socket->read(buffer.data(), buffer.size());
+        _socket->read(buffer.data(), buffer.size());
         
         emit bytesReceived(this, buffer);
 
@@ -166,7 +164,6 @@ void TCPLink::readBytes()
     }
 }
 
-
 /**
  * @brief Get the number of bytes to read.
  *
@@ -174,7 +171,7 @@ void TCPLink::readBytes()
  **/
 qint64 TCPLink::bytesAvailable()
 {
-    return socket->bytesAvailable();
+    return _socket->bytesAvailable();
 }
 
 /**
@@ -184,15 +181,18 @@ qint64 TCPLink::bytesAvailable()
  **/
 bool TCPLink::disconnect()
 {
-	this->quit();
-	this->wait();
+	quit();
+	wait();
     
-    if (socket)
+    if (_socket)
 	{
-        socket->disconnect();
-        socketIsConnected = false;
-		delete socket;
-		socket = NULL;
+        _socket->disconnectFromHost();
+        _socketIsConnected = false;
+		delete _socket;
+		_socket = NULL;
+
+        emit disconnected();
+        emit connected(false);
 	}
     
     return true;
@@ -205,42 +205,54 @@ bool TCPLink::disconnect()
  **/
 bool TCPLink::connect()
 {
-	if(this->isRunning())
+	if (isRunning())
 	{
-		this->quit();
-		this->wait();
+		quit();
+		wait();
 	}
-    bool connected = this->hardwareConnect();
-    start(HighPriority);
+    bool connected = _hardwareConnect();
+    if (connected) {
+        start(HighPriority);
+    }
     return connected;
 }
 
-bool TCPLink::hardwareConnect(void)
+bool TCPLink::_hardwareConnect(void)
 {
-	socket = new QTcpSocket();
+    Q_ASSERT(_socket == NULL);
+	_socket = new QTcpSocket();
     
-    socket->connectToHost(host, port);
+    QSignalSpy errorSpy(_socket, SIGNAL(error(QAbstractSocket::SocketError)));
     
-    QObject::connect(socket, SIGNAL(readyRead()), this, SLOT(readBytes()));
-    QObject::connect(socket, SIGNAL(error(QAbstractSocket::SocketError)), this, SLOT(socketError(QAbstractSocket::SocketError)));
+    _socket->connectToHost(_hostAddress, _port);
+    
+    QObject::connect(_socket, SIGNAL(readyRead()), this, SLOT(readBytes()));
+    QObject::connect(_socket, SIGNAL(error(QAbstractSocket::SocketError)), this, SLOT(_socketError(QAbstractSocket::SocketError)));
     
     // Give the socket a second to connect to the other side otherwise error out
-    if (!socket->waitForConnected(1000))
+    if (!_socket->waitForConnected(1000))
     {
-        emit communicationError(getName(), "connection failed");
+        // Whether a failed connection emits an error signal or not is platform specific.
+        // So in cases where it is not emitted, we emit one ourselves.
+        if (errorSpy.count() == 0) {
+            emit communicationError(getName(), "Connection failed");
+        }
+        delete _socket;
+        _socket = NULL;
         return false;
     }
     
-    socketIsConnected = true;
+    _socketIsConnected = true;
     emit connected(true);
+    emit connected();
 
     return true;
 }
 
-void TCPLink::socketError(QAbstractSocket::SocketError socketError)
+void TCPLink::_socketError(QAbstractSocket::SocketError socketError)
 {
     Q_UNUSED(socketError);
-    emit communicationError(getName(), "Error on socket: " + socket->errorString());
+    emit communicationError(getName(), "Error on socket: " + _socket->errorString());
 }
 
 /**
@@ -250,26 +262,19 @@ void TCPLink::socketError(QAbstractSocket::SocketError socketError)
  **/
 bool TCPLink::isConnected() const
 {
-    return socketIsConnected;
+    return _socketIsConnected;
 }
 
 int TCPLink::getId() const
 {
-    return id;
+    return _linkId;
 }
 
 QString TCPLink::getName() const
 {
-    return name;
-}
-
-void TCPLink::setName(QString name)
-{
-    this->name = name;
-    emit nameChanged(this->name);
+    return _name;
 }
 
-
 qint64 TCPLink::getConnectionSpeed() const
 {
     return 54000000; // 54 Mbit
@@ -284,3 +289,9 @@ qint64 TCPLink::getCurrentOutDataRate() const
 {
     return 0;
 }
+
+void TCPLink::_resetName(void)
+{
+    _name = QString("TCP Link (host:%1 port:%2)").arg(_hostAddress.toString()).arg(_port);
+    emit nameChanged(_name);
+}
\ No newline at end of file
diff --git a/src/comm/TCPLink.h b/src/comm/TCPLink.h
index 6834fe3..43d160f 100644
--- a/src/comm/TCPLink.h
+++ b/src/comm/TCPLink.h
@@ -21,12 +21,10 @@
  
  ======================================================================*/
 
-/**
- * @file
- *   @brief TCP connection (server) for unmanned vehicles
- *   @author Lorenz Meier <mavteam@student.ethz.ch>
- *
- */
+/// @file
+///     @brief TCP link type for SITL support
+///
+///     @author Don Gagne <don@thegagnes.com>
 
 #ifndef TCPLINK_H
 #define TCPLINK_H
@@ -50,66 +48,65 @@ public:
     TCPLink(QHostAddress hostAddress = QHostAddress::LocalHost, quint16 socketPort = 5760);
     ~TCPLink();
     
-    void requestReset() { }
+    void setHostAddress(QHostAddress hostAddress);
     
-    bool isConnected() const;
-    qint64 bytesAvailable();
-    int getPort() const {
-        return port;
-    }
-    QHostAddress getHostAddress() const {
-        return host;
-    }
+    QHostAddress getHostAddress(void) const { return _hostAddress; }
+    quint16 getPort(void) const { return _port; }
+    QTcpSocket* getSocket(void) { return _socket; }
     
-    QString getName() const;
-    int getBaudRate() const;
-    int getBaudRateType() const;
-    int getFlowType() const;
-    int getParityType() const;
-    int getDataBitsType() const;
-    int getStopBitsType() const;
+    // LinkInterface methods
+    virtual int     getId(void) const;
+    virtual QString getName(void) const;
+    virtual bool    isConnected(void) const;
+    virtual bool    connect(void);
+    virtual bool    disconnect(void);
+    virtual qint64  bytesAvailable(void);
+    virtual void    requestReset(void) {};
 
     // Extensive statistics for scientific purposes
     qint64 getConnectionSpeed() const;
     qint64 getCurrentInDataRate() const;
     qint64 getCurrentOutDataRate() const;
     
-    void run();
-    
-    int getId() const;
-    
 public slots:
-    void setAddress(QHostAddress host);
-    void setPort(int port);    
-    void readBytes();
-    void writeBytes(const char* data, qint64 length);
-    bool connect();
-    bool disconnect();
-    void socketError(QAbstractSocket::SocketError socketError);
-    void setAddress(const QString &text);
-
+    void setHostAddress(const QString& hostAddress);
+    void setPort(int port);
     
+    // From LinkInterface
+    virtual void writeBytes(const char* data, qint64 length);
+
+protected slots:
+    void _socketError(QAbstractSocket::SocketError socketError);
+
+    // From LinkInterface
+    virtual void readBytes(void);
+
 protected:
-    QString name;
-    QHostAddress host;
-    quint16 port;
-    int id;
-    QTcpSocket* socket;
-    bool socketIsConnected;
-    
-    QMutex dataMutex;
-    
-    void setName(QString name);
-    
+    // From LinkInterface->QThread
+    virtual void run(void);
+
 private:
-	bool hardwareConnect(void);
+    void _resetName(void);
+	bool _hardwareConnect(void);
 #ifdef TCPLINK_READWRITE_DEBUG
-    void writeDebugBytes(const char *data, qint16 size);
+    void _writeDebugBytes(const char *data, qint16 size);
 #endif
+
+    QString         _name;
+    QHostAddress    _hostAddress;
+    quint16         _port;
+    int             _linkId;
+    QTcpSocket*     _socket;
+    bool            _socketIsConnected;
     
-signals:
-    //Signals are defined by LinkInterface
-    
+    quint64 _bitsSentTotal;
+    quint64 _bitsSentCurrent;
+    quint64 _bitsSentMax;
+    quint64 _bitsReceivedTotal;
+    quint64 _bitsReceivedCurrent;
+    quint64 _bitsReceivedMax;
+    quint64 _connectionStartTime;
+    QMutex  _statisticsMutex;
 };
 
 #endif // TCPLINK_H
diff --git a/src/ui/QGCTCPLinkConfiguration.cc b/src/ui/QGCTCPLinkConfiguration.cc
index 7eb0f66..e816ec7 100644
--- a/src/ui/QGCTCPLinkConfiguration.cc
+++ b/src/ui/QGCTCPLinkConfiguration.cc
@@ -14,7 +14,7 @@ QGCTCPLinkConfiguration::QGCTCPLinkConfiguration(TCPLink* link, QWidget *parent)
     QString addr = link->getHostAddress().toString();
     ui->hostAddressLineEdit->setText(addr);
     connect(ui->portSpinBox, SIGNAL(valueChanged(int)), link, SLOT(setPort(int)));
-    connect(ui->hostAddressLineEdit, SIGNAL(textChanged (const QString &)), link, SLOT(setAddress(const QString &)));
+    connect(ui->hostAddressLineEdit, SIGNAL(textChanged (const QString &)), link, SLOT(setHostAddress(const QString &)));
 }
 
 QGCTCPLinkConfiguration::~QGCTCPLinkConfiguration()