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)

1 files changed, 125 insertions, 3 deletions

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.
+	}
 }