Reimplement navigation API with VIS signals

Reimplement the navigation interface with use of VIS signals.
There is some impedance mismatching done between VIS signal
notifications and the API Qt signals to avoid changing the
clients signficantly for now.  If the Qt demo lifetime is
extended, this may be revisited to either switch to directly
using the VehicleSignals interface or doing a more granular
mapping that works better with the VSS schema / VIS.

Bug-AGL: SPEC-4408

Signed-off-by: Scott Murray <scott.murray@konsulko.com>
Change-Id: Ieab42daddadca60de634f414171a133af53938f5
(cherry picked from commit 4ce04ff30e753556cf9cb371780f2a873f6fedaa)

3 files changed, 163 insertions, 35 deletions

diff --git a/navigation/CMakeLists.txt b/navigation/CMakeLists.txt
index 6264623..9e0ee84 100644
--- a/navigation/CMakeLists.txt
+++ b/navigation/CMakeLists.txt
@@ -8,7 +8,7 @@ add_library(qtappfw-navigation SHARED navigation.cpp)
 target_include_directories(qtappfw-navigation PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}")
 target_include_directories(qtappfw-navigation PUBLIC "${CMAKE_INSTALL_INCLUDEDIR}")
 
-target_link_libraries(qtappfw-navigation Qt5::Qml)
+target_link_libraries(qtappfw-navigation Qt5::Qml qtappfw-vehicle-signals)
 set_target_properties(qtappfw-navigation PROPERTIES
 	VERSION ${PROJECT_VERSION}
 	SOVERSION 1
diff --git a/navigation/navigation.cpp b/navigation/navigation.cpp
index 6116637..e33772a 100644
--- a/navigation/navigation.cpp
+++ b/navigation/navigation.cpp
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2019-2021 Konsulko Group
+ * Copyright (C) 2019-2022 Konsulko Group
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
@@ -17,37 +17,159 @@
 #include <QDebug>
 
 #include "navigation.h"
+#include "vehiclesignals.h"
 
+Navigation::Navigation(VehicleSignals *vs, QObject * parent) :
+	QObject(parent),
+	m_vs(vs),
+	m_connected(false)
 
-Navigation::Navigation (QObject * parent) :
-    QObject(parent)
 {
+	QObject::connect(m_vs, &VehicleSignals::connected, this, &Navigation::onConnected);
+	QObject::connect(m_vs, &VehicleSignals::authorized, this, &Navigation::onAuthorized);
+	QObject::connect(m_vs, &VehicleSignals::disconnected, this, &Navigation::onDisconnected);
+
+	if (m_vs)
+		m_vs->connect();
 }
 
 Navigation::~Navigation()
 {
+	delete m_vs;
 }
 
 void Navigation::sendWaypoint(double lat, double lon)
 {
+	if (!(m_vs && m_connected))
+		return;
+
+	// The original implementation resulted in at least 9 decimal places
+	// making it through to the clients, so explicitly format for 9.  In
+	// practice going from the QString default 6 to 9 does make a difference
+	// with respect to smoothness in the position-based map rotations done
+	// in tbtnavi.
+	m_vs->set("Vehicle.Cabin.Infotainment.Navigation.DestinationSet.Latitude", QString::number(lat, 'f', 9));
+	m_vs->set("Vehicle.Cabin.Infotainment.Navigation.DestinationSet.Longitude", QString::number(lon, 'f', 9));
 }
 
 void Navigation::broadcastPosition(double lat, double lon, double drc, double dst)
 {
+	if (!(m_vs && m_connected))
+		return;
+
+	m_vs->set("Vehicle.CurrentLocation.Latitude", QString::number(lat, 'f', 9));
+	m_vs->set("Vehicle.CurrentLocation.Longitude", QString::number(lon, 'f', 9));
+	m_vs->set("Vehicle.CurrentLocation.Heading", QString::number(drc, 'f', 9));
+
+	// NOTES:
+	// - This signal is an AGL addition, it may make sense to engage with the
+	//   VSS specification upstream to discuss an addition along these lines.
+	// - The signal makes more sense in kilometers wrt VSS expectations, so
+	//   conversion from meters happens here for now to avoid changing the
+	//   existing clients.  This may be worth revisiting down the road.
+	m_vs->set("Vehicle.Cabin.Infotainment.Navigation.ElapsedDistance", QString::number(dst / 1000, 'f', 9));
 }
 
 void Navigation::broadcastRouteInfo(double lat, double lon, double route_lat, double route_lon)
 {
+	if (!(m_vs && m_connected))
+		return;
+
+	m_vs->set("Vehicle.CurrentLocation.Latitude", QString::number(lat, 'f', 9));
+	m_vs->set("Vehicle.CurrentLocation.Longitude", QString::number(lon, 'f', 9));
+
+	m_vs->set("Vehicle.Cabin.Infotainment.Navigation.DestinationSet.Latitude", QString::number(route_lat, 'f', 9));
+	m_vs->set("Vehicle.Cabin.Infotainment.Navigation.DestinationSet.Longitude", QString::number(route_lon, 'f', 9));
 }
 
 void Navigation::broadcastStatus(QString state)
 {
+	if (!(m_vs && m_connected))
+		return;
+
+	m_vs->set("Vehicle.Cabin.Infotainment.Navigation.State", state);
 }
 
 void Navigation::onConnected()
 {
+	if (!m_vs)
+		return;
+
+	m_vs->authorize();
+}
+
+void Navigation::onAuthorized()
+{
+	if (!m_vs)
+		return;
+
+	m_connected = true;
+
+	QObject::connect(m_vs, &VehicleSignals::signalNotification, this, &Navigation::onSignalNotification);
+
+	// NOTE: This signal is another AGL addition where it is possible
+	//       upstream may be open to adding it to VSS.
+	m_vs->subscribe("Vehicle.Cabin.Infotainment.Navigation.State");
+	m_vs->subscribe("Vehicle.CurrentLocation.Latitude");
+	m_vs->subscribe("Vehicle.CurrentLocation.Longitude");
+	m_vs->subscribe("Vehicle.CurrentLocation.Heading");
+	m_vs->subscribe("Vehicle.Cabin.Infotainment.Navigation.DestinationSet.Latitude");
+	m_vs->subscribe("Vehicle.Cabin.Infotainment.Navigation.DestinationSet.Longitude");
+	m_vs->subscribe("Vehicle.Cabin.Infotainment.Navigation.ElapsedDistance");
 }
 
 void Navigation::onDisconnected()
 {
+	QObject::disconnect(m_vs, &VehicleSignals::signalNotification, this, &Navigation::onSignalNotification);
+
+	m_connected = false;
+}
+
+void Navigation::onSignalNotification(QString path, QString value, QString timestamp)
+{
+	// NOTE: Since all the known AGL users of the VSS signals are users of
+	//       this API, we know that updates occur in certain sequences and
+	//       can leverage this to roll up for emitting the existing events.
+	//       This is the path of least effort with respect to changing
+	//       the existing clients, but it may make sense down the road to
+	//       either switch them to using VehicleSignals directly or having
+	//       a more granular signal scheme that maps more directly onto
+	//       VSS.
+	if (path == "Vehicle.Cabin.Infotainment.Navigation.State") {
+		QVariantMap event;
+		event["state"] = value;
+		emit statusEvent(event);
+	} else if (path == "Vehicle.CurrentLocation.Latitude") {
+		m_latitude = value.toDouble();
+	} else if (path == "Vehicle.CurrentLocation.Longitude") {
+		m_longitude = value.toDouble();
+	} else if (path == "Vehicle.CurrentLocation.Heading") {
+		m_heading = value.toDouble();
+	} else if (path == "Vehicle.Cabin.Infotainment.Navigation.ElapsedDistance") {
+		m_distance = value.toDouble();
+		QVariantMap event;
+		event["position"] = "car";
+		event["latitude"] = m_latitude;
+		event["longitude"] = m_longitude;
+		event["direction"] = m_heading;
+		event["distance"] = m_distance * 1000;
+		emit positionEvent(event);
+	} else if (path == "Vehicle.Cabin.Infotainment.Navigation.DestinationSet.Latitude") {
+		m_dest_latitude = value.toDouble();
+	} else if (path == "Vehicle.Cabin.Infotainment.Navigation.DestinationSet.Longitude") {
+		m_dest_longitude = value.toDouble();
+		QVariantMap event;
+		event["position"] = "route";
+		event["latitude"] = m_latitude;
+		event["longitude"] = m_longitude;
+		event["route_latitude"] = m_dest_latitude;
+		event["route_longitude"] = m_dest_longitude;
+		emit positionEvent(event);
+
+		// NOTE: Potentially could emit a waypointsEvent here, but
+		//       nothing in the demo currently requires it, so do
+		//       not bother for now.  If something like Alexa is
+		//       added it or some other replacement / rework will
+		//       be required.
+	}
 }
diff --git a/navigation/navigation.h b/navigation/navigation.h
index 0606d1e..2c8a8ff 100644
--- a/navigation/navigation.h
+++ b/navigation/navigation.h
@@ -1,5 +1,5 @@
  /*
- * Copyright (C) 2019-2021 Konsulko Group
+ * Copyright (C) 2019-2022 Konsulko Group
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
@@ -17,40 +17,46 @@
 #ifndef NAVIGATION_H
 #define NAVIGATION_H
 
-#include <memory>
 #include <QObject>
-#include <QtQml/QQmlListProperty>
+#include <QVariant>
+
+class VehicleSignals;
 
 class Navigation : public QObject
 {
-    Q_OBJECT
-
-    public:
-        explicit Navigation(QObject * parent = Q_NULLPTR);
-        virtual ~Navigation();
-
-        Q_INVOKABLE void broadcastPosition(double lat, double lon, double drc, double dst);
-        Q_INVOKABLE void broadcastRouteInfo(double lat, double lon, double route_lat, double route_lon);
-        Q_INVOKABLE void broadcastStatus(QString state);
-
-        // only support one waypoint for now
-        Q_INVOKABLE void sendWaypoint(double lat, double lon);
-
-    signals:
-        void statusEvent(QVariantMap data);
-        void positionEvent(QVariantMap data);
-        void waypointsEvent(QVariantMap data);
-
-    private:
-        // slots
-        void onConnected();
-        void onDisconnected();
-
-        const QStringList events {
-            "status",
-            "position",
-            "waypoints",
-        };
+	Q_OBJECT
+
+public:
+	explicit Navigation(VehicleSignals *vs, QObject *parent = Q_NULLPTR);
+	virtual ~Navigation();
+
+	Q_INVOKABLE void broadcastPosition(double lat, double lon, double drc, double dst);
+	Q_INVOKABLE void broadcastRouteInfo(double lat, double lon, double route_lat, double route_lon);
+	Q_INVOKABLE void broadcastStatus(QString state);
+
+	// only support one waypoint for now
+	Q_INVOKABLE void sendWaypoint(double lat, double lon);
+
+signals:
+	void statusEvent(QVariantMap data);
+	void positionEvent(QVariantMap data);
+	void waypointsEvent(QVariantMap data);
+
+private slots:
+	void onConnected();
+	void onAuthorized();
+	void onDisconnected();
+	void onSignalNotification(QString path, QString value, QString timestamp);
+
+private:
+	VehicleSignals *m_vs;
+	bool m_connected;
+	double m_latitude;
+	double m_longitude;
+	double m_heading;
+	double m_distance;
+	double m_dest_latitude;
+	double m_dest_longitude;
 };
 
 #endif // NAVIGATION_H