From 5791179aece0bd3e3f4e0590160c745e2f0713e1 Mon Sep 17 00:00:00 2001
From: Youwen Wu <youwenw@gmail.com>
Date: Thu, 29 Feb 2024 20:11:38 -0800
Subject: [PATCH] feat: new visualization system working

---
 client/src/app.css                            |   2 +
 client/src/lib/Apps/DevTools/DevTools.svelte  |  21 +++
 .../src/lib/Apps/DevTools/simulateMotion.ts   |  15 +-
 .../lib/Apps/DevTools/telemetrySimulators.ts  |  46 +++++-
 client/src/lib/Dashboard/SpeedLimit.svelte    |   8 +-
 .../CameraControls/CameraControls.svelte      |   9 +-
 .../Visualization/CameraControls/Scene.svelte | 140 +++++++-----------
 .../CameraControls/utils/cameraStore.ts       |  40 ++++-
 client/src/lib/stores/telemetryStore.ts       |  12 +-
 client/src/lib/utils/initializeTelemetry.ts   |   6 +-
 10 files changed, 191 insertions(+), 108 deletions(-)

diff --git a/client/src/app.css b/client/src/app.css
index f7782b4..2898b55 100644
--- a/client/src/app.css
+++ b/client/src/app.css
@@ -21,6 +21,8 @@
     overflow: auto; /* or 'scroll' if you always want scrollability */
     scrollbar-width: none; /* Hide scrollbar for Firefox */
     -ms-overflow-style: none; /* Hide scrollbar for IE 10+ and Edge */
+
+    overscroll-behavior: none;
   }
   body::-webkit-scrollbar {
     display: none;
diff --git a/client/src/lib/Apps/DevTools/DevTools.svelte b/client/src/lib/Apps/DevTools/DevTools.svelte
index f75c868..916b6df 100644
--- a/client/src/lib/Apps/DevTools/DevTools.svelte
+++ b/client/src/lib/Apps/DevTools/DevTools.svelte
@@ -1,4 +1,8 @@
 <script lang="ts">
+  import {
+    cameraControls,
+    cameraState,
+  } from '../../Dashboard/Visualization/CameraControls/utils/cameraStore'
   import AppContainer from '../AppContainer.svelte'
   import { simulateMotion } from './simulateMotion'
   import {
@@ -6,6 +10,22 @@
     increaseSpeedTo,
     setStationaryTelemetry,
   } from './telemetrySimulators'
+
+  let cameraMode = 'orbit'
+
+  const changeCamera = () => {
+    if (cameraMode === 'follow-direction') {
+      cameraMode = 'orbit'
+      cameraState.set('mode', 'orbit')
+      cameraState.set('userControlled', false)
+      console.log($cameraState.mode)
+    } else {
+      cameraMode = 'follow-direction'
+      cameraState.set('mode', 'follow-direction')
+      cameraState.set('userControlled', true)
+      console.log($cameraState.mode)
+    }
+  }
 </script>
 
 <AppContainer
@@ -49,6 +69,7 @@
   <button class="button" on:click={simulateMotion}>
     Simulate random motion
   </button>
+  <button class="button" on:click={changeCamera}> Change camera mode </button>
 </AppContainer>
 
 <style lang="postcss">
diff --git a/client/src/lib/Apps/DevTools/simulateMotion.ts b/client/src/lib/Apps/DevTools/simulateMotion.ts
index bbbeb79..d9f6dae 100644
--- a/client/src/lib/Apps/DevTools/simulateMotion.ts
+++ b/client/src/lib/Apps/DevTools/simulateMotion.ts
@@ -1,15 +1,18 @@
 import { get } from 'svelte/store'
 import { telemetryStore } from '../../stores/telemetryStore'
+import { increaseRotationTo, increaseSpeedTo } from './telemetrySimulators'
 
 // simulate some turning for testing
 export const simulateMotion = () => {
   let delay = Math.random() * 4500 + 500
   let randOffset = Math.random() * 360
-  telemetryStore.update({
-    ...get(telemetryStore),
-    'orientation': randOffset,
-    'chassis-x-speed': Math.random() * 4 * (Math.random() < 0.5 ? -1 : 1),
-    'chassis-y-speed': Math.random() * 4 * (Math.random() < 0.5 ? -1 : 1),
-  })
+
+  increaseSpeedTo(
+    Math.random() * 4 * (Math.random() < 0.5 ? -1 : 1),
+    Math.random() * 4 * (Math.random() < 0.5 ? -1 : 1)
+  )
+
+  increaseRotationTo(randOffset)
+
   setTimeout(simulateMotion, delay)
 }
diff --git a/client/src/lib/Apps/DevTools/telemetrySimulators.ts b/client/src/lib/Apps/DevTools/telemetrySimulators.ts
index a7d4135..d5906f9 100644
--- a/client/src/lib/Apps/DevTools/telemetrySimulators.ts
+++ b/client/src/lib/Apps/DevTools/telemetrySimulators.ts
@@ -38,7 +38,7 @@ export const increaseSpeedTo = async (targetX: number, targetY: number) => {
     }
   }
 
-  const delay = () => new Promise(resolve => setTimeout(resolve, 1000)) // Assuming a 100ms tick for demonstration
+  const delay = () => new Promise(resolve => setTimeout(resolve, 500)) // Assuming a 100ms tick for demonstration
   const lerp = (start: number, end: number, alpha: number) =>
     start + (end - start) * alpha
 
@@ -69,3 +69,47 @@ export const changeGear = (gear: Gear) => {
     gear: gear,
   })
 }
+
+let cancelPreviousCall = () => {} // Function to cancel the previous interpolation
+
+const getAngle = () => {
+  return get(telemetryStore)['orientation']
+}
+
+const setAngle = (angle: number) => {
+  telemetryStore.update({
+    ...get(telemetryStore),
+    orientation: angle,
+  })
+}
+
+export const increaseRotationTo = async (targetAngle: number) => {
+  let isCancelled = false
+  cancelPreviousCall() // Cancel any ongoing interpolation
+  cancelPreviousCall = () => {
+    isCancelled = true
+  } // Setup cancellation for the current call
+
+  const lerp = (start: number, end: number, alpha: number) =>
+    start + (end - start) * alpha
+  const tick = () => new Promise(resolve => setTimeout(resolve, 1000)) // Assuming a 100ms tick for demonstration
+
+  let currentAngle = getAngle() // Assume getAngle() retrieves the current angle
+
+  const steps = 10 // Number of steps for the interpolation
+  for (let i = 1; i <= steps; i++) {
+    if (isCancelled) return // Exit if a new target angle is set
+
+    const alpha = i / steps // Calculate interpolation fraction
+
+    // Interpolate angle
+    const nextAngle = lerp(currentAngle, targetAngle, alpha)
+
+    setAngle(nextAngle) // Update angle
+
+    await tick() // Wait for state update synchronization
+
+    // Update current angle for the next iteration
+    currentAngle = nextAngle
+  }
+}
diff --git a/client/src/lib/Dashboard/SpeedLimit.svelte b/client/src/lib/Dashboard/SpeedLimit.svelte
index f9c22b7..21bbb61 100644
--- a/client/src/lib/Dashboard/SpeedLimit.svelte
+++ b/client/src/lib/Dashboard/SpeedLimit.svelte
@@ -16,12 +16,10 @@
 >
   <div
     class="px-3 py-1 border-black rounded-xl border-2 flex flex-col text-center gap-1 transition-all"
+    class:speed-limit-placeholder={placeholder}
   >
     <div class="text-lg font-medium">SPEED<br />LIMIT</div>
-    <div
-      class="text-2xl font-bold transition"
-      class:speed-limit-placeholder={placeholder}
-    >
+    <div class="text-2xl font-bold transition">
       {speedLimit}
     </div>
   </div>
@@ -29,6 +27,6 @@
 
 <style lang="postcss">
   .speed-limit-placeholder {
-    @apply text-neutral-200 bg-neutral-200 animate-pulse rounded-lg;
+    @apply text-neutral-300 bg-neutral-300 animate-pulse rounded-lg;
   }
 </style>
diff --git a/client/src/lib/Dashboard/Visualization/CameraControls/CameraControls.svelte b/client/src/lib/Dashboard/Visualization/CameraControls/CameraControls.svelte
index f6d3935..29b96c8 100644
--- a/client/src/lib/Dashboard/Visualization/CameraControls/CameraControls.svelte
+++ b/client/src/lib/Dashboard/Visualization/CameraControls/CameraControls.svelte
@@ -34,6 +34,7 @@
     type PerspectiveCamera,
   } from 'three'
   import { DEG2RAD } from 'three/src/math/MathUtils'
+  import { cameraState } from './utils/cameraStore'
 
   const subsetOfTHREE = {
     Vector2,
@@ -70,11 +71,9 @@
 
   const getControls = () => ref
 
-  let disableAutoRotate = false
-
   useTask(
     delta => {
-      if (autoRotate && !disableAutoRotate) {
+      if (autoRotate && !$cameraState.userControlled) {
         getControls().azimuthAngle += 4 * delta * DEG2RAD * autoRotateSpeed
       }
       const updated = getControls().update(delta)
@@ -91,13 +90,13 @@
 <T
   is={ref}
   on:controlstart={e => {
-    disableAutoRotate = true
+    cameraState.set('userControlled', true)
   }}
   on:zoom={e => {
     console.log('zoomstart', e)
   }}
   on:controlend={() => {
-    disableAutoRotate = false
+    cameraState.set('userControlled', false)
   }}
   {...$$restProps}
   bind:this={$forwardingComponent}
diff --git a/client/src/lib/Dashboard/Visualization/CameraControls/Scene.svelte b/client/src/lib/Dashboard/Visualization/CameraControls/Scene.svelte
index c39e00d..8e06fe2 100644
--- a/client/src/lib/Dashboard/Visualization/CameraControls/Scene.svelte
+++ b/client/src/lib/Dashboard/Visualization/CameraControls/Scene.svelte
@@ -1,122 +1,96 @@
 <script lang="ts">
-  import { T, useTask } from "@threlte/core";
-  import { Grid } from "@threlte/extras";
-  import CameraControls from "./CameraControls.svelte";
-  import { cameraControls, mesh } from "./utils/cameraStore";
-  import { Vector3 } from "three";
-  import { onMount } from "svelte";
-  import RobotDecimated from "../models/RobotDecimated.svelte";
+  import { T, useTask } from '@threlte/core'
+  import { Grid } from '@threlte/extras'
+  import CameraControls from './CameraControls.svelte'
+  import { cameraControls, mesh, cameraState } from './utils/cameraStore'
+  import { Vector3 } from 'three'
+  import { onMount } from 'svelte'
+  import RobotDecimated from '../models/RobotDecimated.svelte'
+  import { telemetryReadonlyStore } from '../../../stores/telemetryStore'
+  import { DEG2RAD } from 'three/src/math/MathUtils.js'
 
-  function vectorFromObject() {
-    let ideal: Vector3 = new Vector3();
-    $cameraControls.getPosition(ideal, true);
-
-    ideal.applyQuaternion($mesh.quaternion);
-    ideal.add(
-      new Vector3($mesh.position.x, $mesh.position.y, $mesh.position.z)
-    );
-    return ideal;
-  }
+  const SPEED_MULTIPLIER = 4
+  const axis = new Vector3(0, 1, 0)
 
   const follow = (delta: number) => {
     // the object's position is bound to the prop
-    if (!$mesh || !$cameraControls) return;
+    if (!$mesh || !$cameraControls) return
 
-    // typescript HACKS! never do this! How does this work? who knows!
-    const robotPosition = vectorFromObject();
+    const offsetPosition = new Vector3()
+    offsetPosition.copy($mesh.position)
+    const offsetVector = new Vector3(2.5, 0, -2)
+    offsetVector.applyAxisAngle(axis, $mesh.rotation.y)
+    offsetPosition.add(offsetVector)
 
-    const horizontalOffsetDistance = 12; // Distance behind the leading vector
-    const direction = new Vector3(0, 0, 1); // Default forward direction in Three.js is negative z-axis, so behind is positive z-axis
-    const verticalOffset = new Vector3(0, -2.8, 0);
-
-    // Calculate the offset vector
-    const offsetVector = direction
-      .normalize()
-      .multiplyScalar(horizontalOffsetDistance)
-      .add(verticalOffset);
-
-    // If the leading object is rotating, apply its rotation to the offset vector
-    const rotatedOffsetVector = offsetVector.applyQuaternion($mesh.quaternion);
-
-    // Calculate the trailing vector's position
-    const trailingVector = robotPosition.clone().sub(rotatedOffsetVector);
-
-    $cameraControls.setLookAt(
-      trailingVector.x,
-      trailingVector.y,
-      trailingVector.z,
-      $mesh.position.x,
-      $mesh.position.y,
-      $mesh.position.z,
-      true
-    );
-  };
-
-  useTask((delta) => {
-    // follow(delta)
-    // $cameraControls.moveTo(
-    //   $mesh.position.x,
-    //   $mesh.position.y,
-    //   $mesh.position.z,
-    //   true
-    // )
-  });
-
-  onMount(() => {
-    setTimeout(() => {
-      // $cameraControls.setLookAt(
-      //   $mesh.position.x,
-      //   $mesh.position.y,
-      //   $mesh.position.z,
-      //   $mesh.position.x,
-      //   $mesh.position.y,
-      //   $mesh.position.z,
-      //   true
-      // )
+    cameraState.set('mode', 'follow-direction')
+    if (($cameraState.mode = 'follow-facing')) {
       $cameraControls.setLookAt(
-        30,
-        20,
-        -40,
-        $mesh.position.x,
-        $mesh.position.y,
-        $mesh.position.z,
+        offsetPosition.x + 15 * Math.sin($mesh.rotation.y),
+        offsetPosition.y + 8,
+        offsetPosition.z + 15 * Math.cos($mesh.rotation.y),
+        offsetPosition.x,
+        offsetPosition.y,
+        offsetPosition.z,
         true
-      );
-    }, 8000);
-  });
+      )
+    }
+
+    if (($cameraState.mode = 'orbit')) {
+      $cameraControls.moveTo(
+        offsetPosition.x,
+        offsetPosition.y,
+        offsetPosition.z
+        // true
+      )
+    }
+  }
+
+  useTask(delta => {
+    $mesh.position.x +=
+      $telemetryReadonlyStore['chassis-y-speed'] * delta * SPEED_MULTIPLIER
+    $mesh.position.z +=
+      $telemetryReadonlyStore['chassis-x-speed'] * delta * SPEED_MULTIPLIER
+    $mesh.rotation.y = $telemetryReadonlyStore.orientation * DEG2RAD
+    follow(delta)
+  })
+
+  onMount(() => {})
 </script>
 
 <T.PerspectiveCamera
   makeDefault
-  position={[10, 10, 10]}
+  position={[12, 10, 12]}
   on:create={({ ref }) => {
-    ref.lookAt(0, 1, 0);
+    ref.lookAt(0, 1, 0)
   }}
 >
   <CameraControls
     on:create={({ ref }) => {
-      $cameraControls = ref;
+      $cameraControls = ref
     }}
+    autoRotate={$cameraState.mode === 'orbit'}
+    autoRotateSpeed={3}
   />
 </T.PerspectiveCamera>
 
 <T.DirectionalLight position={[3, 10, 7]} />
+<T.AmbientLight color={'#f0f0f0'} intensity={0.1} />
 
 <RobotDecimated
   scale={[10, 10, 10]}
   position.y={0}
   on:create={({ ref }) => {
     // @ts-expect-error
-    $mesh = ref;
+    $mesh = ref
   }}
 />
 
 <Grid
-  sectionColor={"#ff3e00"}
+  sectionColor={'#ff3e00'}
   sectionThickness={1}
   fadeDistance={100}
   cellSize={6}
   sectionSize={24}
-  cellColor={"#cccccc"}
+  cellColor={'#cccccc'}
   infiniteGrid
 />
diff --git a/client/src/lib/Dashboard/Visualization/CameraControls/utils/cameraStore.ts b/client/src/lib/Dashboard/Visualization/CameraControls/utils/cameraStore.ts
index 37a02c6..9a68df5 100644
--- a/client/src/lib/Dashboard/Visualization/CameraControls/utils/cameraStore.ts
+++ b/client/src/lib/Dashboard/Visualization/CameraControls/utils/cameraStore.ts
@@ -1,7 +1,35 @@
-import type CameraControls from "camera-controls";
-import { writable } from "svelte/store";
-import type { Mesh, Object3DEventMap } from "three";
-import type { Group } from "three/examples/jsm/libs/tween.module.js";
+import type CameraControls from 'camera-controls'
+import { writable } from 'svelte/store'
+import type { Mesh } from 'three'
 
-export const cameraControls = writable<CameraControls>();
-export const mesh = writable<Mesh>();
+export const cameraControls = writable<CameraControls>()
+export const mesh = writable<Mesh>()
+
+type CameraMode =
+  | 'orbit'
+  | 'follow-facing'
+  | 'follow-direction'
+  | 'follow-position'
+  | 'showcase'
+
+interface CameraState {
+  mode: CameraMode
+  userControlled: boolean
+}
+
+const { set, update, subscribe } = writable<CameraState>({
+  mode: 'orbit',
+  userControlled: false,
+})
+
+const createCameraState = () => {
+  return {
+    update,
+    subscribe,
+    set: (prop: keyof CameraState, val: any) =>
+      update(state => ({ ...state, [prop]: val })),
+    reset: () => set({ mode: 'orbit', userControlled: false }),
+  }
+}
+
+export const cameraState = createCameraState()
diff --git a/client/src/lib/stores/telemetryStore.ts b/client/src/lib/stores/telemetryStore.ts
index 4663c97..7521597 100644
--- a/client/src/lib/stores/telemetryStore.ts
+++ b/client/src/lib/stores/telemetryStore.ts
@@ -1,4 +1,4 @@
-import { writable, readonly } from 'svelte/store'
+import { writable, readonly, get } from 'svelte/store'
 
 let defaults: TelemetryData = {
   'orientation': 0,
@@ -28,6 +28,16 @@ const createTelemetryStore = () => {
         return store
       })
     },
+    set: (key: keyof TelemetryData, value: any) => {
+      let newObj = {
+        ...get(telemetryStore),
+      }
+      newObj = {
+        ...newObj,
+        [key]: value,
+      }
+      set(newObj)
+    },
     reset: () => set(defaults),
   }
 }
diff --git a/client/src/lib/utils/initializeTelemetry.ts b/client/src/lib/utils/initializeTelemetry.ts
index 3f2d847..68d4fb3 100644
--- a/client/src/lib/utils/initializeTelemetry.ts
+++ b/client/src/lib/utils/initializeTelemetry.ts
@@ -34,7 +34,11 @@ export const initializeTelemetry = async (
   })
 
   const unlistenTelemetry = await listen('telemetry_data', event => {
-    console.log(event.payload)
+    const data = JSON.parse(event.payload as string)
+    // console.log(JSON.parse)
+    telemetryStore.set('connected', true)
+
+    telemetryStore.set(data['topic_name'], data['data'])
   })
 
   const unlistenAll = () => {