From f14b422a17b5293b933aa580db736745cce591d1 Mon Sep 17 00:00:00 2001
From: Simon Frei <simon-frei@hotmail.com>
Date: Fri, 9 Dec 2022 13:39:39 +0100
Subject: [PATCH] set Splitflap message according to robo mobile state

---
 ADIS_Csharp/RaspiControl/AppState.cs      | 17 ++++++
 ADIS_Csharp/RaspiControl/MqttConstants.cs |  7 +++
 ADIS_Csharp/RaspiControl/Program.cs       | 65 ++++++++++++++++++++---
 3 files changed, 83 insertions(+), 6 deletions(-)
 create mode 100644 ADIS_Csharp/RaspiControl/AppState.cs

diff --git a/ADIS_Csharp/RaspiControl/AppState.cs b/ADIS_Csharp/RaspiControl/AppState.cs
new file mode 100644
index 0000000..81b45b2
--- /dev/null
+++ b/ADIS_Csharp/RaspiControl/AppState.cs
@@ -0,0 +1,17 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+
+namespace RaspiControl {
+    public enum APP_STATE {
+        STARTUP = 0,
+        INIT = 1,
+        CALIBRATE = 2,
+        READY = 3,
+        FOLLOW_LINE = 4,
+        IDLE = 5,
+        FINAL = 6,
+    }
+}
diff --git a/ADIS_Csharp/RaspiControl/MqttConstants.cs b/ADIS_Csharp/RaspiControl/MqttConstants.cs
index c84cac2..a8cc79a 100644
--- a/ADIS_Csharp/RaspiControl/MqttConstants.cs
+++ b/ADIS_Csharp/RaspiControl/MqttConstants.cs
@@ -6,8 +6,15 @@ using System.Threading.Tasks;
 
 namespace RaspiControl {
     internal static class MqttConstants {
+        #region Publish
         public static string MOBILE_NAV_TURN_TOPIC = "mobile/cmd/nav/turn";
         public static string MOBILE_NAV_MOVE_TOPIC = "mobile/cmd/nav/move";
         public static string MOBILE_NAV_STOP_TOPIC = "mobile/cmd/nav/stop";
+        public static string SPLITFLAP_DISPLAY = "splitFlap/cmd/display";
+        #endregion
+
+        #region subscribe
+        public static string DEVICE_STATUS_APP_TOPIC = "device/status/+/app";
+        #endregion
     }
 }
diff --git a/ADIS_Csharp/RaspiControl/Program.cs b/ADIS_Csharp/RaspiControl/Program.cs
index cc1d4ba..d81f287 100644
--- a/ADIS_Csharp/RaspiControl/Program.cs
+++ b/ADIS_Csharp/RaspiControl/Program.cs
@@ -3,6 +3,8 @@ using System.Text;
 using M2Mqtt;
 using M2Mqtt.Messages;
 using System.Text.Json;
+using System.Linq;
+using System.Runtime.CompilerServices;
 
 namespace RaspiControl {
     class Programm {
@@ -14,7 +16,7 @@ namespace RaspiControl {
                 client.MqttMsgPublishReceived += Client_MqttMsgPublishReceived;
                 string clientId = Guid.NewGuid().ToString();
                 client.Connect(clientId);
-                client.Subscribe(new string[] { "APROG/REQUEST" }, new byte[] { MqttMsgBase.QOS_LEVEL_AT_MOST_ONCE });
+                client.Subscribe(new string[] { MqttConstants.DEVICE_STATUS_APP_TOPIC }, new byte[] { MqttMsgBase.QOS_LEVEL_AT_MOST_ONCE });
                 Joystick joystick = new Joystick();
                 joystick.JoystickChanged += Joystick_JoystickChanged;
 
@@ -45,13 +47,64 @@ namespace RaspiControl {
             }
         }
 
+        private static void PublishSplitFlapDisplay(string message) {
+            string payload = JsonSerializer.Serialize<Dictionary<string, string>>(new Dictionary<string, string>() { { "message", message } });
+            client.Publish(MqttConstants.SPLITFLAP_DISPLAY, Encoding.UTF8.GetBytes(payload),MqttMsgBase.QOS_LEVEL_AT_MOST_ONCE,false);
+        }
         private static void Client_MqttMsgPublishReceived(object sender, MqttMsgPublishEventArgs e) {
-            Console.Write(e.Topic + "\n");
-            Console.Write(Encoding.UTF8.GetString(e.Message) + "\n");
-            Dictionary<string,object> data = JsonSerializer.Deserialize<Dictionary<string, object>>(e.Message);
-            foreach(KeyValuePair<string,object> entry in data) {
-                Console.WriteLine(entry.Key+ ": " + entry.Value);
+            string deviceId = "";
+            Dictionary<string, object> data;
+            if (e.Topic.StartsWith(string.Join('/',MqttConstants.DEVICE_STATUS_APP_TOPIC.Split('/').Take(2)))) {
+                if(e.Topic.EndsWith(MqttConstants.DEVICE_STATUS_APP_TOPIC.Split('/').Last())) {
+                    deviceId = deviceIdFromStatusTopic(e.Topic);
+                    if(deviceId == "mobile") {
+                            try {
+                                data = getValueFromTopic(Encoding.UTF8.GetString(e.Message));
+                                object value;
+                                data.TryGetValue("state", out value);
+                                if (value != null) {
+                                    APP_STATE appState = (APP_STATE)Convert.ToUInt16(value.ToString());
+                                    switch (appState) {
+                                        case APP_STATE.STARTUP:
+                                            break;
+                                        case APP_STATE.INIT:
+                                            PublishSplitFlapDisplay("INIT");
+                                            break;
+                                        case APP_STATE.CALIBRATE:
+                                            break;
+                                        case APP_STATE.FOLLOW_LINE:
+                                            PublishSplitFlapDisplay("AUTO");
+                                            break;
+                                        case APP_STATE.IDLE:
+                                            break;
+                                        case APP_STATE.FINAL:
+                                            break;
+                                        case APP_STATE.READY:
+                                            PublishSplitFlapDisplay("REDY");
+                                            break;
+                                        default:
+                                            throw new ArgumentException();
+                                    }
+                                }
+                            } catch(Exception) {
+                                Console.WriteLine($"Invalid payload received: {Encoding.UTF8.GetString(e.Message)}");
+                            }
+                            
+                            
+                        } 
+                    else {
+                            Console.WriteLine($"on topic: {e.Topic} no key with state found! | {Encoding.UTF8.GetString(e.Message)}");
+                    }
+                }
             }
         }
+
+        private static string deviceIdFromStatusTopic(string statusTopic) {
+            string[] topic = statusTopic.Split('/');
+            return topic[topic.Length - 2];
+        }
+        private static Dictionary<string, object> getValueFromTopic(string topic) {
+            return JsonSerializer.Deserialize<Dictionary<string, object>>(topic);
+        }
     }
 }
\ No newline at end of file