From 0fb2387e301c115e7c9a018422958bba84e4f934 Mon Sep 17 00:00:00 2001
From: Rune Harlyk <rune@harlyk.dk>
Date: Sat, 30 Mar 2024 01:29:08 +0100
Subject: [PATCH] Adds simple data sync

---
 .../src/lib/components/UpdateIndicator.svelte |  4 ++
 app2/src/lib/components/Visualization.svelte  | 36 +++++++---------
 app2/src/lib/stores/socket-store.ts           | 10 +----
 app2/src/routes/+layout.svelte                | 43 ++++++++++++++++++-
 app2/src/routes/+page.svelte                  |  2 +-
 app2/src/routes/controller/+page.svelte       |  2 +-
 app2/src/routes/login.svelte                  |  2 +-
 esp32/src/ActuatorStateService.h              | 14 +++---
 8 files changed, 73 insertions(+), 40 deletions(-)

diff --git a/app2/src/lib/components/UpdateIndicator.svelte b/app2/src/lib/components/UpdateIndicator.svelte
index 55ea360..898e8cc 100644
--- a/app2/src/lib/components/UpdateIndicator.svelte
+++ b/app2/src/lib/components/UpdateIndicator.svelte
@@ -29,6 +29,10 @@
 				}
 			);
 			const results = await response.json();
+            if (results.message == "Not Found") {
+                console.error('Error: Could not find releases in the repository');
+                return;
+            }
 
 			update = false;
 			firmwareVersion = '';
diff --git a/app2/src/lib/components/Visualization.svelte b/app2/src/lib/components/Visualization.svelte
index 884cb2c..2852eb2 100644
--- a/app2/src/lib/components/Visualization.svelte
+++ b/app2/src/lib/components/Visualization.svelte
@@ -1,7 +1,6 @@
 <script lang="ts">
 	import { onDestroy, onMount } from 'svelte';
 	import { BufferGeometry, Line, LineBasicMaterial, Vector3, type NormalBufferAttributes } from 'three';
-	import socketService from '$lib/services/socket-service';
 	import uzip from 'uzip';
 	import { model } from '$lib/stores';
 	import { footColor, isEmbeddedApp, location, toeWorldPositions } from '$lib/utilities';
@@ -19,8 +18,9 @@
 	let sceneManager = new SceneBuilder();
 	let canvas: HTMLCanvasElement
 
-	let modelAngles: number[] | Int16Array = [0, 45, 45, 0, 45, 45, 0, 45, 45, 0, 45, 45]// new Array(12).fill(0);
-	let modelTargetAngles: number[] | Int16Array = [0, 45, 45, 0, 45, 45, 0, 45, 45, 0, 45, 45] //new Array(12).fill(0);
+	let currentModelAngles: number[] = new Array(12).fill(0);
+	let modelTargetAngles: number[] = new Array(12).fill(0)
+    let gui_panel: GUI
 
     let feet_trace = new Array(4).fill([]);
     let trace_lines: BufferGeometry<NormalBufferAttributes>[] = []
@@ -35,18 +35,22 @@
         await cacheModelFiles()
         await createScene();
         if (!isEmbeddedApp && panel) createPanel();
+        servoAngles.subscribe(angles => {
+            modelTargetAngles = angles
+        })
 	});
 
 	onDestroy(() => {
-		canvas.remove();
-	});
+        canvas.remove()
+        gui_panel?.destroy()
+    });
 
     const createPanel = () => {
-        const panel = new GUI({width: 310});
-        panel.close();
-        panel.domElement.id = 'three-gui-panel';
+        gui_panel = new GUI({width: 310});
+        gui_panel.close();
+        gui_panel.domElement.id = 'three-gui-panel';
  
-        const visibility = panel.addFolder('Visualization');
+        const visibility = gui_panel.addFolder('Visualization');
         visibility.add(settings, 'Trace feet')
         visibility.add(settings, 'Trace points', 1, 1000, 1)
     }
@@ -64,13 +68,6 @@
 
 	const updateAngles = (name: string, angle: number) => {
 		modelTargetAngles[$jointNames.indexOf(name)] = angle * (180 / Math.PI);
-		socketService.send(
-			JSON.stringify({
-				type: 'kinematic/angle',
-				angle: angle * (180 / Math.PI),
-				id: $jointNames.indexOf(name)
-			})
-		);
 	};
 
 	const createScene = async () => {
@@ -85,12 +82,12 @@
 			// .addArrowHelper({ origin: { x: 0, y: 0, z: 0 }, direction: { x: 0, y: -2, z: 0 } })
 			.addFogExp2(0xcccccc, 0.015)
 			.addModel($model)
-			.addDragControl(updateAngles)
 			.fillParent()
 			.addRenderCb(render)
 			.startRenderLoop();
         
         if (sky) sceneManager.addSky()
+        if (debug) sceneManager.addDragControl(updateAngles)
 
         for (let i = 0; i < 4; i++) {
 			const geometry = new BufferGeometry();
@@ -130,15 +127,14 @@
 
 		robot.position.y = robot.position.y - Math.min(...toes.map(toe => toe.y));
 		robot.rotation.z = lerp(robot.rotation.z, degToRad($mpu.heading + 90), 0.1);
-		modelTargetAngles = $servoAngles;
 
 		for (let i = 0; i < $jointNames.length; i++) {
-			modelAngles[i] = lerp(
+			currentModelAngles[i] = lerp(
 				(robot.joints[$jointNames[i]].angle as number) * (180 / Math.PI),
 				modelTargetAngles[i],
 				0.1
 			);
-			robot.joints[$jointNames[i]].setJointValue(degToRad(modelAngles[i]));
+			robot.joints[$jointNames[i]].setJointValue(degToRad(currentModelAngles[i]));
 		}
 	};
 
diff --git a/app2/src/lib/stores/socket-store.ts b/app2/src/lib/stores/socket-store.ts
index 80f6905..e296147 100644
--- a/app2/src/lib/stores/socket-store.ts
+++ b/app2/src/lib/stores/socket-store.ts
@@ -2,15 +2,13 @@ import { writable, type Writable } from 'svelte/store';
 import { type angles } from '$lib/models';
 
 export const isConnected = writable(false);
-export const servoAngles: Writable<angles> = writable([
+export const servoAngles: Writable<number[]> = writable([
 	0, 45, -90, 0, 45, -90, 0, 45, -90, 0, 45, -90
 ]);
 export const logs = writable([] as string[]);
 export const battery = writable({});
 export const mpu = writable({ heading: 0 });
 export const distances = writable({});
-export const settings = writable({});
-export const systemInfo = writable({} as number);
 
 export interface socketDataCollection {
 	angles: Writable<angles>;
@@ -18,8 +16,6 @@ export interface socketDataCollection {
 	battery: Writable<unknown>;
 	mpu: Writable<unknown>;
 	distances: Writable<unknown>;
-	settings: Writable<unknown>;
-	systemInfo: Writable<unknown>;
 }
 
 export const socketData = {
@@ -27,7 +23,5 @@ export const socketData = {
 	logs,
 	battery,
 	mpu,
-	distances,
-	settings,
-	systemInfo
+	distances
 };
diff --git a/app2/src/routes/+layout.svelte b/app2/src/routes/+layout.svelte
index 7dc08ef..5634294 100644
--- a/app2/src/routes/+layout.svelte
+++ b/app2/src/routes/+layout.svelte
@@ -14,18 +14,57 @@
 	import Menu from './menu.svelte';
 	import Statusbar from './statusbar.svelte';
 	import Login from './login.svelte';
+	import { mode, outControllerData, servoAngles, socketData } from '$lib/stores';
+	import { throttler } from '$lib/utilities';
 
 	export let data: LayoutData;
 
+    type WebsocketOutData = string | ArrayBufferLike | Blob | ArrayBufferView;
+
 	onMount(async () => {
 		if ($user.bearer_token !== '') {
 			validateUser($user);
 		}
 		connectToEventSource();
+        connectToSocket()
+        addPublisher(outControllerData)
+        addPublisher(mode)
 	});
 
+    const connectToSocket = () => {
+        const ws_token = $page.data.features.security ? '?access_token=' + $user.bearer_token : '';
+
+	    socket = new WebSocket('ws://' + $page.url.host + '/ws' + ws_token);
+        socket.onmessage = ((event: MessageEvent) => {
+            const message = JSON.parse(event.data);
+            if (message.type === 'log') {
+                socketData.logs.update((entries) => {
+                    entries.push(message.data);
+                    return entries;
+                });
+            } else if (message.data && message.type in socketData) {
+                const store = socketData[message.type as keyof typeof socketData];
+                 if (JSON.stringify(get(store)) !== JSON.stringify(message.data))
+                    store.set(message.data);
+            }
+        });
+    }
+
+    const addPublisher = (store: Writable<WebsocketOutData>, type?: string) => {
+        const publish = (data: WebsocketOutData) => {
+            console.log('Got updated', data);
+            
+            if (socket.readyState === WebSocket.OPEN)
+            throttle.throttle(
+                () => socket.send(type ? JSON.stringify({ type, data }) : data), 
+                100);
+        }
+		store.subscribe(publish);
+	}
+
 	onDestroy(() => {
 		NotificationSource?.close();
+        socket?.close();
 	});
 
 	async function validateUser(userdata: userProfile) {
@@ -47,8 +86,10 @@
 	}
 
 	let menuOpen = false;
+    let throttle = new throttler();
 
 	let NotificationSource: EventSource;
+    let socket: WebSocket
 	let reconnectIntervalId: number = 0;
 	let connectionLost = false;
 	let unresponsiveTimeout: number;
@@ -155,7 +196,7 @@
 
 	function reconnectEventSource() {
 		if (connectionLost === false) {
-			NotificationSource.close;
+			NotificationSource.close();
 			notifications.error('Connection to device lost', 5000);
 			if (reconnectIntervalId === 0) {
 				reconnectIntervalId = setInterval(connectToEventSource, 2000) as unknown as number;
diff --git a/app2/src/routes/+page.svelte b/app2/src/routes/+page.svelte
index 53cb622..038f9a7 100644
--- a/app2/src/routes/+page.svelte
+++ b/app2/src/routes/+page.svelte
@@ -9,7 +9,7 @@
 <div class="hero bg-base-100 h-screen">
 	<div class="card md:card-side bg-base-200 shadow-2xl flex justify-center items-center">
         <div class="w-64 h-64">
-            <Visualization sky={false} orbit={true} panel={false}/>
+            <Visualization sky={false} orbit panel={false}/>
         </div>
 		<div class="card-body w-80">
 			<h2 class="card-title text-center text-2xl">Welcome to {data.app_name}</h2>
diff --git a/app2/src/routes/controller/+page.svelte b/app2/src/routes/controller/+page.svelte
index e3d3fd5..a30d935 100644
--- a/app2/src/routes/controller/+page.svelte
+++ b/app2/src/routes/controller/+page.svelte
@@ -4,6 +4,6 @@
 
 <div class="grow flex">
     <div class="absolute h-screen w-full top-0">
-        <Visualization />
+        <Visualization debug />
     </div>
 </div> 
\ No newline at end of file
diff --git a/app2/src/routes/login.svelte b/app2/src/routes/login.svelte
index c8d1481..a9d646d 100644
--- a/app2/src/routes/login.svelte
+++ b/app2/src/routes/login.svelte
@@ -78,7 +78,7 @@
 					<button
 						class="btn btn-primary inline-flex items-center"
 						on:click={() => {
-							signInUser({ username: username, password: password });
+							signInUser({ username, password });
 						}}><Login class="mr-2 h-5 w-5" /><span>Login</span></button
 					>
 				</div>
diff --git a/esp32/src/ActuatorStateService.h b/esp32/src/ActuatorStateService.h
index a9a4343..d0358a7 100644
--- a/esp32/src/ActuatorStateService.h
+++ b/esp32/src/ActuatorStateService.h
@@ -28,7 +28,8 @@ public:
 
     static void read(ActuatorState &settings, JsonObject &root)
     {
-        JsonArray array = root.createNestedArray("state");
+        root["type"] = "angles";
+        JsonArray array = root.createNestedArray("data");
         for(int i = 0; i < 12; i++)
         {
             array.add(settings.state[i]);
@@ -37,26 +38,23 @@ public:
 
     static StateUpdateResult update(JsonObject &root, ActuatorState &actuatorState)
     {
-        Serial.print("New state array: [");
-        JsonArray array = root["state"];
+        if (root["type"] != "angles") return StateUpdateResult::UNCHANGED;
+        JsonArray array = root["data"];
         bool changed = false;
         for(int i = 0; i < 12; i++)
         {
-            Serial.print(array[i].as<int16_t>());
-            Serial.print(", ");
             if (actuatorState.state[i] != array[i].as<int16_t>())
             {
                 actuatorState.state[i] = array[i];
                 changed = true;
             }
         }
-        Serial.println("]");
         return changed ? StateUpdateResult::CHANGED : StateUpdateResult::UNCHANGED;
     }
 
     static void homeAssistRead(ActuatorState &settings, JsonObject &root)
     {
-        JsonArray array = root.createNestedArray("state");
+        JsonArray array = root.createNestedArray("angles");
         for(int i = 0; i < 12; i++)
         {
             array.add(settings.state[i]);
@@ -65,7 +63,7 @@ public:
 
     static StateUpdateResult homeAssistUpdate(JsonObject &root, ActuatorState &actuatorState)
     {
-        JsonArray array = root["state"];
+        JsonArray array = root["angles"];
         if(array.size() != 12) return StateUpdateResult::ERROR;
         
         bool changed = false;