Merge pull request #18 from saa938/main

add wait function plus timer

Author: BaronClaps

src/App.svelte

@@ -225,6 +225,47 @@
     let robotInchesXY = getCurvePoint(linePercent, [_startPoint, ...currentLine.controlPoints, currentLine.endPoint]);
     robotXY = { x: x(robotInchesXY.x), y: y(robotInchesXY.y) };
 
+    // If this line is a wait, compute heading from the previous non-wait line's end heading
+    if ((currentLine as any).waitMs !== undefined) {
+      let prevIdx = currentLineIdx - 1;
+      while (prevIdx >= 0 && (lines[prevIdx] as any).waitMs !== undefined) {
+        prevIdx -= 1;
+      }
+
+      if (prevIdx >= 0) {
+        const prevLine = lines[prevIdx];
+        const prevStart = prevIdx === 0 ? startPoint : lines[prevIdx - 1].endPoint;
+        // determine heading at end of prevLine (t = 1)
+        switch (prevLine.endPoint.heading) {
+          case "linear":
+            robotHeading = -shortestRotation(
+              prevLine.endPoint.startDeg,
+              prevLine.endPoint.endDeg,
+              1
+            );
+            break;
+          case "constant":
+            robotHeading = -prevLine.endPoint.degrees;
+            break;
+          case "tangential": {
+            const pBefore = getCurvePoint(0.99, [prevStart, ...prevLine.controlPoints, prevLine.endPoint]);
+            const pEnd = getCurvePoint(1, [prevStart, ...prevLine.controlPoints, prevLine.endPoint]);
+            const pBeforePx = { x: x(pBefore.x), y: y(pBefore.y) };
+            const pEndPx = { x: x(pEnd.x), y: y(pEnd.y) };
+            const dx = pEndPx.x - pBeforePx.x;
+            const dy = pEndPx.y - pBeforePx.y;
+            if (dx !== 0 || dy !== 0) {
+              robotHeading = radiansToDegrees(Math.atan2(dy, dx));
+            }
+            break;
+          }
+        }
+      } else {
+        // no previous line, fall back to startPoint heading if available
+        if (startPoint.heading === "constant") robotHeading = -((startPoint as any).degrees ?? 0);
+        else if (startPoint.heading === "linear") robotHeading = -shortestRotation(startPoint.startDeg, startPoint.endDeg, 1);
+      }
+    } else {
     switch (currentLine.endPoint.heading) {
       case "linear":
         robotHeading = -shortestRotation(
@@ -254,6 +295,7 @@
 
         break;
     }
+    }
   }
 
   $: (() => {
@@ -276,47 +318,154 @@
     two.update();
   })();
 
-  let playing = false;
+let playing = false;
 
-  let animationFrame: number;
-  let startTime: number | null = null;
-  let previousTime: number | null = null;
+let animationFrame: number;
+let startTime: number | null = null;
+let previousTime: number | null = null;
 
-  function animate(timestamp: number) {
-    if (!startTime) {
-      startTime = timestamp;
-    }
+let waiting = false;
+let waitTimerRemaining = 0;
+let lastHandledLineIdx = -1;
+let waitEndTimestamp: number | null = null;
+let prevPercent = 0;
+let cycleTimerRunning = false;
+// Playback elapsed timer (ms) — resets when `play()` starts
+let playElapsedMs = 0;
 
-    if (previousTime !== null) {
-      const deltaTime = timestamp - previousTime;
+function normalizeWaitMs(value: any): number {
+  const n = Number(value) || 0;
+  return n <= 0 ? 0 : n; // treat value as milliseconds directly
+}
 
-      if (percent >= 100) {
-        percent = 0;
+function animate(timestamp: number) {
+  // First frame init
+  if (previousTime === null) {
+    previousTime = timestamp;
+    animationFrame = requestAnimationFrame(animate);
+    return;
+  }
+
+  // Calculate elapsed ms since last frame
+  const deltaTime = timestamp - previousTime;
+  // Move previousTime forward now (keeps delta strictly the time spent since last frame)
+  previousTime = timestamp;
+
+  // compute playback elapsed deterministically from `percent` and waits
+  function computeElapsedFromPercent(p: number) {
+    if (!lines || lines.length === 0) return 0;
+    const clamped = Math.max(0, Math.min(100, p));
+    const totalLineProgress = (lines.length * clamped) / 100;
+    let idx = Math.min(Math.trunc(totalLineProgress), Math.max(0, lines.length - 1));
+    const frac = totalLineProgress - Math.floor(totalLineProgress);
+
+    // motion time per non-wait line (derived from existing speed formula)
+    const motionMsPerLine = 100 / 0.065; // ~=1538.461538ms
+
+    let sum = 0;
+    for (let j = 0; j < idx; j++) {
+      const l = lines[j] as any;
+      if (l && l.waitMs !== undefined) {
+        sum += Number(l.waitMs) || 0;
       } else {
-        percent += (0.65 / lines.length) * (deltaTime * 0.1);
+        sum += motionMsPerLine;
       }
     }
 
-    previousTime = timestamp;
+    // current line partial
+    const cur = lines[idx] as any;
+    if (cur) {
+      if (cur.waitMs !== undefined) {
+        sum += (Number(cur.waitMs) || 0) * frac;
+      } else {
+        sum += motionMsPerLine * frac;
+      }
+    }
+
+    return sum;
+  }
+
+  playElapsedMs = computeElapsedFromPercent(percent);
+
+  // Detect current line based on percent
+  const totalLineProgress = (lines.length * Math.min(percent, 99.999999)) / 100;
+  const currentLineIdx = Math.min(Math.trunc(totalLineProgress), Math.max(0, lines.length - 1));
+  const currentLine = lines[currentLineIdx];
+
+  // If we just entered a line that has a wait, start the wait timer (only once per line)
+  if (currentLine && (currentLine as any).waitMs !== undefined && lastHandledLineIdx !== currentLineIdx) {
+    waiting = true;
+    const ms = normalizeWaitMs((currentLine as any).waitMs);
+    // set an absolute end timestamp for the wait based on current frame timestamp
+    waitEndTimestamp = timestamp + ms;
+    waitTimerRemaining = ms;
+    lastHandledLineIdx = currentLineIdx;
+    console.log(`Entering wait on line ${currentLineIdx}: raw waitMs=${(currentLine as any).waitMs} normalized=${ms}ms, will end at ${waitEndTimestamp}`);
+  }
 
-    if (playing) {
-      requestAnimationFrame(animate);
+  // HANDLE WAIT: subtract elapsed time (ms). Do not advance percent while waiting.
+  if (waiting) {
+    // compute remaining based on absolute end timestamp if set
+    if (waitEndTimestamp !== null) {
+      const remaining = Math.max(0, waitEndTimestamp - timestamp);
+      waitTimerRemaining = remaining;
+      if (timestamp >= waitEndTimestamp) {
+        // wait finished
+        waiting = false;
+        waitTimerRemaining = 0;
+        waitEndTimestamp = null;
+        // reset previousTime so the next frame's delta doesn't include the time that passed during the wait
+        previousTime = timestamp;
+        console.log(`Wait finished on line ${currentLineIdx}`);
+      }
     }
+
+    // keep animating loop but do not advance percent while waiting
+    animationFrame = requestAnimationFrame(animate);
+    return;
   }
 
-  function play() {
-    if (!playing) {
-      playing = true;
-      startTime = null;
-      previousTime = null;
-      animationFrame = requestAnimationFrame(animate);
+  // NORMAL PROGRESS
+  if (percent >= 100) {
+    percent = 0;
+    lastHandledLineIdx = -1; // allow waits to re-trigger on next loop
+    // cycle finished — stop cycle timer so it will restart on the next run
+    cycleTimerRunning = false;
+  } else {
+    // Your original motion formula — kept similar, but uses real ms deltaTime
+    const speed = 0.65 / Math.max(1, lines.length);
+    percent += speed * deltaTime * 0.1; // you can tune this multiplier if you want motion faster/slower
+    // If we transitioned from 0 -> >0, this is the very first path beginning — reset cycle timer
+    if (!cycleTimerRunning && prevPercent === 0 && percent > 0) {
+      playElapsedMs = 0;
+      cycleTimerRunning = true;
+      console.log('Cycle timer started (first path began)');
     }
   }
 
-  function pause() {
-    playing = false;
-    cancelAnimationFrame(animationFrame);
+  animationFrame = requestAnimationFrame(animate);
+  // remember percent for next frame to detect 0->>0 transitions
+  prevPercent = percent;
+}
+
+function play() {
+  if (!playing) {
+    playing = true;
+    // If we're starting from the very beginning (never played), reset elapsed timer.
+    // If we're resuming (percent > 0 or waits already handled), don't reset timers or progress.
+    startTime = null;
+    previousTime = null; // force animate to initialize timing on next frame (prevents big delta)
+    if (percent === 0 && lastHandledLineIdx === -1) {
+      playElapsedMs = 0;
+    }
+    animationFrame = requestAnimationFrame(animate);
   }
+}
+
+function pause() {
+  playing = false;
+  cancelAnimationFrame(animationFrame);
+}
 
   async function fpa(l: FPALine, s: FPASettings): Promise<Line> {
     let status = 'Starting optimization...';
@@ -749,6 +898,6 @@ hotkeys('s', function(event, handler){
     bind:robotHeading
     {x}
     {y}
-    {fpa}
+    {playElapsedMs}
   />
 </div>

src/lib/ControlTab.svelte

@@ -12,15 +12,20 @@
   export let robotHeight: number = 16;
   export let robotXY: BasePoint;
   export let robotHeading: number;
-  export let fpa: (l: FPALine, s: FPASettings) => Promise<Line>;
   export let x: d3.ScaleLinear<number, number, number>;
   export let y: d3.ScaleLinear<number, number, number>;
   export let settings: FPASettings;
+  export let playElapsedMs: number = 0;
+  function formatMs(ms: number) {
+    const s = Math.floor(ms / 1000);
+    const rem = Math.floor(ms % 1000);
+    return `${s}.${String(rem).padStart(3, '0')}s`;
+  }
 </script>
 
 <div class="flex-1 flex flex-shrink-0 flex-col justify-start items-center gap-2 h-full overflow-y-auto">
   <div
-    class="flex flex-col justify-start items-start w-full rounded-lg bg-neutral-50 dark:bg-neutral-900 shadow-md p-4 overflow-y-scroll overflow-x-hidden h-full gap-6"
+    class="flex flex-col justify-start items-start w-full rounded-lg bg-neutral-50 dark:bg-neutral-900 shadow-md p-4 overflow-y-scroll overflow-x-hidden h-full gap-3"
   >
     <div class="flex flex-col w-full justify-start items-start gap-0.5 text-sm">
       <div class="font-semibold">Canvas Options</div>
@@ -162,6 +167,7 @@
           </div>
         </div>
         <div class={`h-[0.75px] w-full`} style={`background: ${line.color}`} />
+        {#if line.waitMs === undefined}
         <div class="flex flex-col justify-start items-start">
           <div class="font-light">End Point:</div>
           <div class="flex flex-row justify-start items-center gap-2">
@@ -230,40 +236,25 @@ With tangential heading, the heading follows the direction of the line."
               <p class="text-sm font-extralight">Reverse:</p>
               <input type="checkbox" bind:checked={line.endPoint.reverse} title="Reverse the direction the robot faces along the tangential path" />
             {/if}
-            <!--
-                  <button
-                    class="px-2 rounded-md bg-neutral-100 dark:bg-neutral-950 dark:border-neutral-700 border-[0.5px] focus:outline-none text-sm"
-                    title="Optimize"
-                    name="Optimize"
-                    on:click={async () => {
-                    try {
-                      const optimizedLine = await fpa(
-                      {
-                        startPoint: idx === 0 ? startPoint : lines[idx - 1].endPoint,
-                        endPoint: line.endPoint,
-                        controlPoints: line.controlPoints,
-                        interpolation: line.endPoint.heading,
-                        color: line.color,
-                      },
-                      settings
-                      );
-                      lines = lines.map((l, i) => i === idx ? optimizedLine : l);
-                    } catch (error) {
-                      console.error('Optimization failed:', error);
-
-                      // Check if it's an offline error
-                      if (error.message && error.message.startsWith('OFFLINE:')) {
-                      const offlineMessage = error.message.replace('OFFLINE: ', '');
-                      alert(`🌐 ${offlineMessage}\n\nThe optimization feature requires an internet connection.`);
-                      } else {
-                      alert(`❌ Optimization failed: ${error.message}`);
-                      }
-                    }
-                    }}
-                  >Optimize</button>
-            -->
           </div>
         </div>
+        {:else}
+        <div class="flex flex-col justify-start items-start">
+          <div class="font-light">Wait</div>
+          <div class="flex flex-row justify-start items-center gap-2">
+            <div class="font-extralight">Duration (ms):</div>
+            <input
+              class="pl-1.5 rounded-md bg-neutral-100 dark:bg-neutral-950 dark:border-neutral-700 border-[0.5px] focus:outline-none w-28"
+              step="10"
+              type="number"
+              min="0"
+              bind:value={line.waitMs}
+              on:change={() => { line.waitMs = Number(line.waitMs) || 0 }}
+            />
+            <div class="font-extralight">(Robot pauses at previous point)</div>
+          </div>
+        </div>
+        {/if}
         {#each line.controlPoints as point, idx1}
           <div class="flex flex-col justify-start items-start">
             <div class="font-light">Control Point {idx1 + 1}:</div>
@@ -313,6 +304,7 @@ With tangential heading, the heading follows the direction of the line."
         {/each}
       </div>
     {/each}
+    <div class="flex flex-row gap-2">
     <button
       on:click={() => {
         lines = [
@@ -330,7 +322,7 @@ With tangential heading, the heading follows the direction of the line."
           },
         ];
       }}
-      class="font-semibold text-green-500 text-sm flex flex-row justify-start items-center gap-1"
+      class="font-semibold text-green-500 text-sm flex flex-row justify-center items-center gap-1 h-8 px-3"
     >
       <svg
         xmlns="http://www.w3.org/2000/svg"
@@ -348,6 +340,29 @@ With tangential heading, the heading follows the direction of the line."
       </svg>
       <p>Add Line</p>
     </button>
+    <button
+      on:click={() => {
+        // Add a wait entry: keep endPoint equal to last end point (or startPoint)
+        const lastEnd = lines.length > 0 ? lines[lines.length - 1].endPoint : startPoint;
+        lines = [
+          ...lines,
+          {
+            name: `Wait ${lines.length + 1}`,
+            endPoint: { x: lastEnd.x, y: lastEnd.y, heading: "constant", degrees: 0 },
+            controlPoints: [],
+            color: '#888888',
+            waitMs: 1000,
+          },
+        ];
+      }}
+      class="font-semibold text-yellow-500 text-sm flex flex-row justify-center items-center gap-1 h-8 px-3"
+    >
+      <svg xmlns="http://www.w3.org/2000/svg" fill="none" viewBox="0 0 24 24" stroke-width={2} stroke="currentColor" class="size-5">
+        <path stroke-linecap="round" stroke-linejoin="round" d="M12 6v12M6 12h12" />
+      </svg>
+      <p>Add Wait</p>
+    </button>
+    </div>
   </div>
   <div
     class="w-full bg-neutral-50 dark:bg-neutral-900 rounded-lg p-3 flex flex-row justify-start items-center gap-3 shadow-lg"
@@ -402,5 +417,12 @@ With tangential heading, the heading follows the direction of the line."
       step="0.000001"
       class="w-full appearance-none slider focus:outline-none"
     />
+    <div class="text-sm font-extralight ml-2">
+      {#if playElapsedMs}
+        {formatMs(playElapsedMs)}
+      {:else}
+        0.000s
+      {/if}
+    </div>
   </div>
 </div>