Fix NmeaPositionInfoSource not combining sentences - simulation mode

This patch is the first half of a fix to prevent NmeaPositionInfoSource
to push an update per sentence instead of combining all sentences with
the same timestamp into one QGeoPositionInfo.
This patch fixes only the source in simulation mode.
Additional work is needed for standard mode.

Task-number: QTBUG-64699
Change-Id: I39d9905331c5a1f646b7465f4d7d155834bec815
Reviewed-by: Alex Blasche <alexander.blasche@qt.io>

6 files changed, 285 insertions, 60 deletions

diff --git a/src/positioning/positioning.pro b/src/positioning/positioning.pro
index 32f3c05c..c33f2c75 100644
--- a/src/positioning/positioning.pro
+++ b/src/positioning/positioning.pro
@@ -59,6 +59,7 @@ PRIVATE_HEADERS += \
                     qlocationdata_simulator_p.h \
                     qdoublematrix4x4_p.h \
                     qgeopath_p.h \
+                    qgeopositioninfo_p.h \
                     qclipperutils_p.h
 
 SOURCES += \
diff --git a/src/positioning/qgeopositioninfo.cpp b/src/positioning/qgeopositioninfo.cpp
index 84b7fa16..71e363d1 100644
--- a/src/positioning/qgeopositioninfo.cpp
+++ b/src/positioning/qgeopositioninfo.cpp
@@ -37,7 +37,7 @@
 **
 ****************************************************************************/
 #include "qgeopositioninfo.h"
-
+#include "qgeopositioninfo_p.h"
 #include <QHash>
 #include <QDebug>
 #include <QDataStream>
@@ -47,14 +47,6 @@
 
 QT_BEGIN_NAMESPACE
 
-class QGeoPositionInfoPrivate
-{
-public:
-    QDateTime timestamp;
-    QGeoCoordinate coord;
-    QHash<QGeoPositionInfo::Attribute, qreal> doubleAttribs;
-};
-
 /*!
     \class QGeoPositionInfo
     \inmodule QtPositioning
@@ -106,9 +98,12 @@ QGeoPositionInfo::QGeoPositionInfo(const QGeoCoordinate &coordinate, const QDate
     Creates a QGeoPositionInfo with the values of \a other.
 */
 QGeoPositionInfo::QGeoPositionInfo(const QGeoPositionInfo &other)
-        : d(new QGeoPositionInfoPrivate)
+        : d(other.d->clone())
+{
+}
+
+QGeoPositionInfo::QGeoPositionInfo(QGeoPositionInfoPrivate &dd) : d(&dd)
 {
-    operator=(other);
 }
 
 /*!
@@ -127,9 +122,12 @@ QGeoPositionInfo &QGeoPositionInfo::operator=(const QGeoPositionInfo & other)
     if (this == &other)
         return *this;
 
-    d->timestamp = other.d->timestamp;
-    d->coord = other.d->coord;
-    d->doubleAttribs = other.d->doubleAttribs;
+    delete d;
+    d = other.d->clone();
+
+//    d->timestamp = other.d->timestamp;
+//    d->coord = other.d->coord;
+//    d->doubleAttribs = other.d->doubleAttribs;
 
     return *this;
 }
@@ -140,9 +138,7 @@ QGeoPositionInfo &QGeoPositionInfo::operator=(const QGeoPositionInfo & other)
 */
 bool QGeoPositionInfo::operator==(const QGeoPositionInfo &other) const
 {
-    return d->timestamp == other.d->timestamp
-           && d->coord == other.d->coord
-           && d->doubleAttribs == other.d->doubleAttribs;
+    return *d == *other.d;
 }
 
 /*!
@@ -356,4 +352,27 @@ QDataStream &operator>>(QDataStream &stream, QGeoPositionInfo &info)
 }
 #endif
 
+QGeoPositionInfoPrivate::~QGeoPositionInfoPrivate()
+{
+
+}
+
+QGeoPositionInfoPrivate *QGeoPositionInfoPrivate::clone() const
+{
+    return new QGeoPositionInfoPrivate(*this);
+}
+
+bool QGeoPositionInfoPrivate::operator==(const QGeoPositionInfoPrivate &other) const
+{
+    return timestamp == other.timestamp
+           && coord == other.coord
+            && doubleAttribs == other.doubleAttribs;
+}
+
+QGeoPositionInfoPrivate *QGeoPositionInfoPrivate::getPimpl(const QGeoPositionInfo &info)
+{
+    return info.d;
+}
+
 QT_END_NAMESPACE
+
diff --git a/src/positioning/qgeopositioninfo.h b/src/positioning/qgeopositioninfo.h
index 9f43fe8e..77d44970 100644
--- a/src/positioning/qgeopositioninfo.h
+++ b/src/positioning/qgeopositioninfo.h
@@ -64,6 +64,7 @@ public:
     QGeoPositionInfo();
     QGeoPositionInfo(const QGeoCoordinate &coordinate, const QDateTime &updateTime);
     QGeoPositionInfo(const QGeoPositionInfo &other);
+    QGeoPositionInfo(QGeoPositionInfoPrivate &dd);
     ~QGeoPositionInfo();
 
     QGeoPositionInfo &operator=(const QGeoPositionInfo &other);
@@ -95,6 +96,7 @@ private:
     friend Q_POSITIONING_EXPORT QDataStream &operator>>(QDataStream &stream, QGeoPositionInfo &info);
 #endif
     QGeoPositionInfoPrivate *d;
+    friend class QGeoPositionInfoPrivate;
 };
 
 #ifndef QT_NO_DEBUG_STREAM
diff --git a/src/positioning/qgeopositioninfo_p.h b/src/positioning/qgeopositioninfo_p.h
new file mode 100644
index 00000000..cc4a9f3d
--- /dev/null
+++ b/src/positioning/qgeopositioninfo_p.h
@@ -0,0 +1,67 @@
+/****************************************************************************
+**
+** Copyright (C) 2017 The Qt Company Ltd.
+** Contact: https://www.qt.io/licensing/
+**
+** This file is part of the QtPositioning module of the Qt Toolkit.
+**
+** $QT_BEGIN_LICENSE:LGPL$
+** Commercial License Usage
+** Licensees holding valid commercial Qt licenses may use this file in
+** accordance with the commercial license agreement provided with the
+** Software or, alternatively, in accordance with the terms contained in
+** a written agreement between you and The Qt Company. For licensing terms
+** and conditions see https://www.qt.io/terms-conditions. For further
+** information use the contact form at https://www.qt.io/contact-us.
+**
+** GNU Lesser General Public License Usage
+** Alternatively, this file may be used under the terms of the GNU Lesser
+** General Public License version 3 as published by the Free Software
+** Foundation and appearing in the file LICENSE.LGPL3 included in the
+** packaging of this file. Please review the following information to
+** ensure the GNU Lesser General Public License version 3 requirements
+** will be met: https://www.gnu.org/licenses/lgpl-3.0.html.
+**
+** GNU General Public License Usage
+** Alternatively, this file may be used under the terms of the GNU
+** General Public License version 2.0 or (at your option) the GNU General
+** Public license version 3 or any later version approved by the KDE Free
+** Qt Foundation. The licenses are as published by the Free Software
+** Foundation and appearing in the file LICENSE.GPL2 and LICENSE.GPL3
+** included in the packaging of this file. Please review the following
+** information to ensure the GNU General Public License requirements will
+** be met: https://www.gnu.org/licenses/gpl-2.0.html and
+** https://www.gnu.org/licenses/gpl-3.0.html.
+**
+** $QT_END_LICENSE$
+**
+****************************************************************************/
+#ifndef QGEOPOSITIONINFO_P_H
+#define QGEOPOSITIONINFO_P_H
+
+#include <QtPositioning/private/qpositioningglobal_p.h>
+#include "qgeopositioninfo.h"
+#include <QHash>
+#include <QDateTime>
+#include <QtPositioning/qgeocoordinate.h>
+
+QT_BEGIN_NAMESPACE
+
+class Q_POSITIONING_PRIVATE_EXPORT QGeoPositionInfoPrivate
+{
+public:
+    virtual ~QGeoPositionInfoPrivate();
+    virtual QGeoPositionInfoPrivate *clone() const;
+
+    virtual bool operator==(const QGeoPositionInfoPrivate &other) const;
+
+    QDateTime timestamp;
+    QGeoCoordinate coord;
+    QHash<QGeoPositionInfo::Attribute, qreal> doubleAttribs;
+
+    static QGeoPositionInfoPrivate *getPimpl(const QGeoPositionInfo &info);
+};
+
+QT_END_NAMESPACE
+
+#endif // QGEOPOSITIONINFO_P_H
diff --git a/src/positioning/qnmeapositioninfosource.cpp b/src/positioning/qnmeapositioninfosource.cpp
index 75fa9645..282f30b4 100644
--- a/src/positioning/qnmeapositioninfosource.cpp
+++ b/src/positioning/qnmeapositioninfosource.cpp
@@ -39,17 +39,93 @@
 **
 ****************************************************************************/
 #include "qnmeapositioninfosource_p.h"
+#include "qgeopositioninfo_p.h"
 #include "qlocationutils_p.h"
 
 #include <QIODevice>
 #include <QBasicTimer>
 #include <QTimerEvent>
 #include <QTimer>
+#include <array>
+#include <QDebug>
 #include <QtCore/QtNumeric>
 
 
 QT_BEGIN_NAMESPACE
 
+#define USE_NMEA_PIMPL 0
+
+#if USE_NMEA_PIMPL
+class QGeoPositionInfoPrivateNmea : public QGeoPositionInfoPrivate
+{
+public:
+    virtual ~QGeoPositionInfoPrivateNmea();
+    virtual QGeoPositionInfoPrivate *clone() const;
+
+    QList<QByteArray> nmeaSentences;
+};
+
+
+QGeoPositionInfoPrivateNmea::~QGeoPositionInfoPrivateNmea()
+{
+
+}
+
+QGeoPositionInfoPrivate *QGeoPositionInfoPrivateNmea::clone() const
+{
+    return new QGeoPositionInfoPrivateNmea(*this);
+}
+#else
+typedef QGeoPositionInfoPrivate QGeoPositionInfoPrivateNmea;
+#endif
+
+static void mergePositions(QGeoPositionInfo &dst, const QGeoPositionInfo &src, QByteArray nmeaSentence)
+{
+#if USE_NMEA_PIMPL
+    QGeoPositionInfoPrivateNmea *dstPimpl = static_cast<QGeoPositionInfoPrivateNmea *>(QGeoPositionInfoPrivate::getPimpl(dst));
+    dstPimpl->nmeaSentences.append(nmeaSentence);
+#else
+    Q_UNUSED(nmeaSentence)
+#endif
+
+    QGeoCoordinate c = dst.coordinate();
+    if (!qIsNaN(src.coordinate().latitude()))
+        c.setLatitude(src.coordinate().latitude());
+    if (!qIsNaN(src.coordinate().longitude()))
+        c.setLongitude(src.coordinate().longitude());
+    if (!qIsNaN(src.coordinate().altitude()))
+        c.setAltitude(src.coordinate().altitude());
+    dst.setCoordinate(c);
+
+    if (!dst.timestamp().date().isValid() && src.timestamp().isValid()) // time was supposed to be set/the same already. Date can be overwritten.
+        dst.setTimestamp(src.timestamp());
+
+    static Q_DECL_CONSTEXPR std::array<QGeoPositionInfo::Attribute, 6> attrs {
+                                                { QGeoPositionInfo::GroundSpeed
+                                                 ,QGeoPositionInfo::HorizontalAccuracy
+                                                 ,QGeoPositionInfo::VerticalAccuracy
+                                                 ,QGeoPositionInfo::Direction
+                                                 ,QGeoPositionInfo::VerticalSpeed
+                                                 ,QGeoPositionInfo::MagneticVariation} };
+    for (const auto a: attrs) {
+        if (src.hasAttribute(a))
+            dst.setAttribute(a, src.attribute(a));
+    }
+
+
+}
+
+static qint64 msecsTo(const QDateTime &from, const QDateTime &to)
+{
+    if (!from.time().isValid() || !to.time().isValid())
+        return 0;
+
+    if (!from.date().isValid() || !to.date().isValid()) // use only time
+        return from.time().msecsTo(to.time());
+
+    return from.msecsTo(to);
+}
+
 QNmeaRealTimeReader::QNmeaRealTimeReader(QNmeaPositionInfoSourcePrivate *sourcePrivate)
         : QNmeaReader(sourcePrivate)
 {
@@ -107,25 +183,111 @@ void QNmeaSimulatedReader::readAvailableData()
     }
 }
 
-bool QNmeaSimulatedReader::setFirstDateTime()
+static int processSentence(QGeoPositionInfo &info,
+                           QByteArray &m_nextLine,
+                           QNmeaPositionInfoSourcePrivate *m_proxy,
+                           QQueue<QPendingGeoPositionInfo> &m_pendingUpdates,
+                           bool &hasFix)
 {
-    // find the first update with valid date and time
-    QGeoPositionInfo update;
-    bool hasFix = false;
-    while (m_proxy->m_device->bytesAvailable() > 0) {
-        char buf[1024];
-        qint64 size = m_proxy->m_device->readLine(buf, sizeof(buf));
+    int timeToNextUpdate = -1;
+    QDateTime prevTs;
+    if (m_pendingUpdates.size() > 0)
+        prevTs = m_pendingUpdates.head().info.timestamp();
+
+    // find the next update with a valid time (as long as the time is valid,
+    // we can calculate when the update should be emitted)
+    while (m_nextLine.size() || (m_proxy->m_device && m_proxy->m_device->bytesAvailable() > 0)) {
+        char static_buf[1024];
+        char *buf = static_buf;
+        QByteArray nextLine;
+        qint64 size = 0;
+        if (m_nextLine.size()) {
+            // Read something in the previous call, but TS was later.
+            size = m_nextLine.size();
+            nextLine = m_nextLine;
+            m_nextLine.clear();
+            buf = nextLine.data();
+        } else {
+            size = m_proxy->m_device->readLine(buf, sizeof(static_buf));
+        }
+
         if (size <= 0)
             continue;
-        bool ok = m_proxy->parsePosInfoFromNmeaData(buf, size, &update, &hasFix);
-        if (ok && update.timestamp().isValid()) {
-            QPendingGeoPositionInfo pending;
-            pending.info = update;
-            pending.hasFix = hasFix;
-            m_pendingUpdates.enqueue(pending);
-            return true;
+
+        const QTime infoTime = info.timestamp().time(); // if info has been set, time must be valid.
+        const QDate infoDate = info.timestamp().date(); // this one might not be valid, as some sentences do not contain it
+
+        /*
+             Packets containing time information are GGA, RMC, ZDA, GLL:
+
+             GGA : GPS fix data                           - only time
+             GLL : geographic latitude and longitude      - only time
+             RMC : recommended minimum FPOS/transit data  - date/time
+             ZDA : only timestamp                         - date/time
+
+             QLocationUtils is currently also capable of parsing VTG and GSA sentences:
+
+             VTG: containing Track made good and ground speed
+             GSA: overall satellite data
+
+             Since these sentences contain no timestamp, their content will be merged with the content
+             from any prior sentence that had timestamp info, if any is available.
+         */
+
+        QGeoPositionInfoPrivateNmea *pimpl = new QGeoPositionInfoPrivateNmea;
+        QGeoPositionInfo pos(*pimpl);
+        if (m_proxy->parsePosInfoFromNmeaData(buf, size, &pos, &hasFix)) {
+            // Date may or may not be valid, as some packets do not have date.
+            // If date isn't valid, match is performed on time only.
+            // Hence, make sure that packet blocks are generated with
+            // the sentences containing the full timestamp (e.g., GPRMC) *first* !
+            if (infoTime.isValid()) {
+                if (pos.timestamp().time().isValid()) {
+                    if (infoTime != pos.timestamp().time() || infoDate != pos.timestamp().date()) {
+                        // Effectively read data for different update, so copy buf into m_nextLine
+                        m_nextLine = QByteArray(buf, size);
+                        break;
+                    } else {
+                        // timestamps match -- merge into info
+                        mergePositions(info, pos, QByteArray(buf, size));
+                    }
+                } else {
+                    // no timestamp available -- merge into info
+                    mergePositions(info, pos, QByteArray(buf, size));
+                }
+            } else {
+                // there was no info with valid TS. Overwrite with whatever is parsed.
+#if USE_NMEA_PIMPL
+                pimpl->nmeaSentences.append(QByteArray(buf, size));
+#endif
+                info = pos;
+            }
+
+            if (prevTs.time().isValid()) {
+                timeToNextUpdate = msecsTo(prevTs, info.timestamp());
+                if (timeToNextUpdate < 0) // Somehow parsing expired packets, reset info
+                    info = QGeoPositionInfo(*new QGeoPositionInfoPrivateNmea);
+            }
         }
     }
+
+    return timeToNextUpdate;
+}
+
+bool QNmeaSimulatedReader::setFirstDateTime()
+{
+    // find the first update with valid date and time
+    QGeoPositionInfo info(*new QGeoPositionInfoPrivateNmea);
+    bool hasFix = false;
+    processSentence(info, m_nextLine, m_proxy, m_pendingUpdates, hasFix);
+
+    if (info.timestamp().time().isValid()) { // NMEA may have sentences with only time and no date. These would generate invalid positions
+        QPendingGeoPositionInfo pending;
+        pending.info = info;
+        pending.hasFix = hasFix;
+        m_pendingUpdates.enqueue(pending);
+        return true;
+    }
     return false;
 }
 
@@ -149,37 +311,10 @@ void QNmeaSimulatedReader::timerEvent(QTimerEvent *event)
 
 void QNmeaSimulatedReader::processNextSentence()
 {
-    QGeoPositionInfo info;
+    QGeoPositionInfo info(*new QGeoPositionInfoPrivateNmea);
     bool hasFix = false;
-    int timeToNextUpdate = -1;
-    QTime prevTime;
-    if (m_pendingUpdates.size() > 0)
-        prevTime = m_pendingUpdates.head().info.timestamp().time();
-
-    // find the next update with a valid time (as long as the time is valid,
-    // we can calculate when the update should be emitted)
-    while (m_proxy->m_device && m_proxy->m_device->bytesAvailable() > 0) {
-        char buf[1024];
-        qint64 size = m_proxy->m_device->readLine(buf, sizeof(buf));
-        if (size <= 0)
-            continue;
-        if (m_proxy->parsePosInfoFromNmeaData(buf, size, &info, &hasFix)) {
-            QTime time = info.timestamp().time();
-            if (time.isValid()) {
-                if (!prevTime.isValid()) {
-                    timeToNextUpdate = 0;
-                    break;
-                }
-                timeToNextUpdate = prevTime.msecsTo(time);
-                if (timeToNextUpdate >= 0)
-                    break;
-            } else {
-                timeToNextUpdate = 0;
-                break;
-            }
-        }
-    }
 
+    int timeToNextUpdate = processSentence(info, m_nextLine, m_proxy, m_pendingUpdates, hasFix);
     if (timeToNextUpdate < 0)
         return;
 
diff --git a/src/positioning/qnmeapositioninfosource_p.h b/src/positioning/qnmeapositioninfosource_p.h
index 056de90d..6efb5648 100644
--- a/src/positioning/qnmeapositioninfosource_p.h
+++ b/src/positioning/qnmeapositioninfosource_p.h
@@ -168,6 +168,7 @@ private:
     void processNextSentence();
 
     QQueue<QPendingGeoPositionInfo> m_pendingUpdates;
+    QByteArray m_nextLine;
     int m_currTimerId;
     bool m_hasValidDateTime;
 };