From dc7689793d8627fa035cc40db2e01566b42dd3db Mon Sep 17 00:00:00 2001
From: Rune Harlyk <rune@harlyk.dk>
Date: Sat, 30 Mar 2024 02:21:18 +0100
Subject: [PATCH] More sync

---
 app2/src/lib/components/Visualization.svelte | 12 ++++++++----
 app2/src/lib/sceneBuilder.ts                 | 11 +++++++++--
 app2/src/routes/+layout.svelte               |  9 ++++++++-
 app2/src/routes/controller/+layout.svelte    | 12 +++++++++---
 esp32/src/ActuatorStateService.h             |  2 +-
 5 files changed, 35 insertions(+), 11 deletions(-)

diff --git a/app2/src/lib/components/Visualization.svelte b/app2/src/lib/components/Visualization.svelte
index 2852eb2..c741359 100644
--- a/app2/src/lib/components/Visualization.svelte
+++ b/app2/src/lib/components/Visualization.svelte
@@ -3,7 +3,7 @@
 	import { BufferGeometry, Line, LineBasicMaterial, Vector3, type NormalBufferAttributes } from 'three';
 	import uzip from 'uzip';
 	import { model } from '$lib/stores';
-	import { footColor, isEmbeddedApp, location, toeWorldPositions } from '$lib/utilities';
+	import { footColor, isEmbeddedApp, toeWorldPositions } from '$lib/utilities';
 	import { fileService } from '$lib/services';
 	import { servoAngles, mpu, jointNames } from '$lib/stores';
 	import SceneBuilder from '$lib/sceneBuilder';
@@ -35,10 +35,13 @@
         await cacheModelFiles()
         await createScene();
         if (!isEmbeddedApp && panel) createPanel();
-        servoAngles.subscribe(angles => {
-            modelTargetAngles = angles
-        })
+        servoAngles.subscribe(updateAnglesFromStore)
 	});
+    
+    const updateAnglesFromStore = (angles: number[]) => {
+        if (sceneManager.isDragging) return
+        modelTargetAngles = angles;
+    }
 
 	onDestroy(() => {
         canvas.remove()
@@ -68,6 +71,7 @@
 
 	const updateAngles = (name: string, angle: number) => {
 		modelTargetAngles[$jointNames.indexOf(name)] = angle * (180 / Math.PI);
+        servoAngles.set(modelTargetAngles)
 	};
 
 	const createScene = async () => {
diff --git a/app2/src/lib/sceneBuilder.ts b/app2/src/lib/sceneBuilder.ts
index 1440921..76abcae 100644
--- a/app2/src/lib/sceneBuilder.ts
+++ b/app2/src/lib/sceneBuilder.ts
@@ -73,6 +73,7 @@ export default class SceneBuilder {
 	public liveStreamTexture: CanvasTexture;
 	private fog: FogExp2;
 	private isLoaded: boolean = false;
+	public isDragging: boolean = false;
 	highlightMaterial: any;
 
 	constructor() {
@@ -292,8 +293,14 @@ export default class SceneBuilder {
 			this.setJointValue(joint.name, angle);
 			updateAngle(joint.name, angle);
 		};
-		dragControls.onDragStart = () => (this.controls.enabled = false);
-		dragControls.onDragEnd = () => (this.controls.enabled = true);
+		dragControls.onDragStart = () => {
+			this.controls.enabled = false;
+			this.isDragging = true;
+		};
+		dragControls.onDragEnd = () => {
+			this.controls.enabled = true;
+			this.isDragging = false;
+		};
 		dragControls.onHover = (joint: URDFMimicJoint) =>
 			this.highlightLinkGeometry(joint, false, highlightMaterial);
 		dragControls.onUnhover = (joint: URDFMimicJoint) =>
diff --git a/app2/src/routes/+layout.svelte b/app2/src/routes/+layout.svelte
index 5634294..af4a0f7 100644
--- a/app2/src/routes/+layout.svelte
+++ b/app2/src/routes/+layout.svelte
@@ -14,7 +14,8 @@
 	import Menu from './menu.svelte';
 	import Statusbar from './statusbar.svelte';
 	import Login from './login.svelte';
-	import { mode, outControllerData, servoAngles, socketData } from '$lib/stores';
+	import { get, type Writable } from 'svelte/store';
+	import { isConnected, mode, outControllerData, servoAngles, socketData } from '$lib/stores';
 	import { throttler } from '$lib/utilities';
 
 	export let data: LayoutData;
@@ -29,12 +30,18 @@
         connectToSocket()
         addPublisher(outControllerData)
         addPublisher(mode)
+        addPublisher(servoAngles as unknown as Writable<WebsocketOutData>, "angles")
 	});
 
     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.onopen = (event) => isConnected.set(true);
+        socket.onclose = (event) => {
+            isConnected.set(false)
+            notifications.error('Websocket disconnected', 5000);
+        };
         socket.onmessage = ((event: MessageEvent) => {
             const message = JSON.parse(event.data);
             if (message.type === 'log') {
diff --git a/app2/src/routes/controller/+layout.svelte b/app2/src/routes/controller/+layout.svelte
index 6f787c1..c6433de 100644
--- a/app2/src/routes/controller/+layout.svelte
+++ b/app2/src/routes/controller/+layout.svelte
@@ -1,8 +1,14 @@
 <script lang="ts">
 	import Controls from '$lib/components/Controls.svelte';
+	import { isConnected } from '$lib/stores';
 </script>
-
 <div>
-	<Controls />
-    <slot/>
+    {#if $isConnected}
+        <Controls />
+        <slot/>
+    {:else}
+        <div class="flex justify-center items-center">
+            <h2>Waiting for connection</h2>
+        </div>
+    {/if}
 </div>
diff --git a/esp32/src/ActuatorStateService.h b/esp32/src/ActuatorStateService.h
index d0358a7..8f0aecb 100644
--- a/esp32/src/ActuatorStateService.h
+++ b/esp32/src/ActuatorStateService.h
@@ -46,7 +46,7 @@ public:
             if (actuatorState.state[i] != array[i].as<int16_t>())
             {
                 actuatorState.state[i] = array[i];
-                changed = true;
+                //changed = true;
             }
         }
         return changed ? StateUpdateResult::CHANGED : StateUpdateResult::UNCHANGED;