ESP32-WijiBoard/web/js/motion-effects.js

import IK from './kinematics.js';
import Settings from './settings.js';
import BLE from './ble.js';

export const MotionEffects = {
  // Returns true if the mode should randomize
  resolveMode(mode) {
    if (mode === 'random') {
      const modes = ['direct', 'snaky', 'erratic', 'jumpy', 'hesitant', 'overshoot'];
      return modes[Math.floor(Math.random() * modes.length)];
    }
    return mode || 'direct';
  },

  // Called before a movement sequence starts
  async onSequenceStart(mode) {
    if (!BLE.isConnected()) return;
    if (mode === 'jumpy' || mode === 'overshoot') {
      const fastSpeed = Math.min(2000, Settings.speed * 1.5);
      try { await BLE.write(`SPD:${Math.floor(fastSpeed)}`); } catch(e){}
    }
  },

  // Called after a movement sequence finishes
  async onSequenceEnd(mode) {
    if (!BLE.isConnected()) return;
    if (mode !== 'direct') {
      try { await BLE.write(`SPD:${Settings.speed}`); } catch(e){}
    }
  },

  // Called right before sending a waypoint to BLE
  async onWaypointSend(target, hasHesitated) {
    let delay = 0;
    let newHasHesitated = hasHesitated;

    if (target.mode === 'hesitant') {
      if (target.fraction >= 0.35 && hasHesitated === false) {
        // Slow down massively instead of stopping
        if (BLE.isConnected()) {
          try { await BLE.write(`SPD:${Math.floor(Math.max(30, Settings.speed * 0.1))}`); } catch(e){}
        }
        newHasHesitated = true; // Started hesitation
      } else if (target.fraction >= 0.55 && hasHesitated === true) {
        // Speed back up aggressively
        if (BLE.isConnected()) {
          try { await BLE.write(`SPD:${Math.floor(Settings.speed * 1.2)}`); } catch(e){}
        }
        newHasHesitated = 2; // Finished hesitation
      }
    } else if (target.mode === 'erratic') {
      // Aggressive speed changes randomly during the move
      if (Math.random() > 0.4) {
        if (BLE.isConnected()) {
          const spd = Math.floor(Math.max(30, Settings.speed * (0.1 + Math.random() * 1.5)));
          try { await BLE.write(`SPD:${spd}`); } catch(e){}
        }
      }
    } else if (target.mode === 'overshoot_settle') {
      delay = 200; // Brief pause before returning
      if (BLE.isConnected()) {
        try { await BLE.write(`SPD:${Math.floor(Math.max(100, Settings.speed * 0.4))}`); } catch(e){}
      }
    }
    return { delay, hasHesitated: newHasHesitated };
  },

  // Called to determine blending behaviour during queue checks
  getQueueAction(targetMode, nextTargetMode, maxDist, blendThreshold) {
    if (nextTargetMode === 'overshoot_settle') {
      // Must physically reach the overshoot point before popping the settle command
      if (maxDist <= 10) return { action: 'blend' };
      return { action: 'continue' };
    } else if (targetMode === 'jumpy') {
      if (maxDist <= 5) return { action: 'wait', delay: 100 };
    } else if (targetMode === 'hesitant') {
      // Lower blend threshold prevents blasting all speed-change waypoints instantly
      if (maxDist <= 30) return { action: 'blend' };
    } else {
      if (maxDist <= blendThreshold) return { action: 'blend' };
    }
    return { action: 'continue' };
  },

  // Called to modify the final destination before slicing (e.g. overshoot)
  modifyDestination(startX, startY, endX, endY, mode) {
    if (mode === 'overshoot') {
      const dx = endX - startX;
      const dy = endY - startY;
      const dist = Math.sqrt(dx * dx + dy * dy);
      if (dist > 0) {
        let nx = endX + (dx / dist) * 20; // Overshoot 20mm
        let ny = endY + (dy / dist) * 20;
        if (IK.checkWorkspace(nx, ny).ok) {
          return { endX: nx, endY: ny };
        }
      }
    }
    return { endX, endY };
  },

  // Called to determine segment count
  getSegmentCount(distance, baseSegmentSize, mode) {
    let count = Math.ceil(distance / baseSegmentSize);
    if (mode === 'snaky') {
      count = Math.max(count, Math.ceil(distance / 10)); // fine resolution for sine
    } else if (mode === 'erratic') {
      count = Math.max(count, Math.ceil(distance / 20)); // fewer, larger jumps
    } else if (mode === 'hesitant') {
      count = Math.max(count, Math.ceil(distance / 15)); // ensure enough waypoints for speed changes
    }
    return Math.max(1, count);
  },

  // Called to apply per-waypoint visual perturbations
  applyPerturbation(px, py, nx, ny, fraction, distance, mode) {
    if (fraction === 1) return { px, py }; // Always hit exact target at the end

    if (mode === 'snaky') {
      const cycles = Math.max(1, Math.floor(distance / 50)); 
      const offset = Math.sin(fraction * Math.PI * 2 * cycles) * 30; // 30mm amplitude
      px += nx * offset;
      py += ny * offset;
    } else if (mode === 'erratic') {
      // Meaningful, aggressive jumps
      px += (Math.random() - 0.5) * 40;
      py += (Math.random() - 0.5) * 40;
    }
    return { px, py };
  },

  // Generate extra waypoints at the end (e.g. settle from overshoot)
  getSettlingWaypoints(endX, endY, mode) {
    const segs = [];
    if (mode === 'overshoot') {
      const res = IK.solve(endX, endY);
      if (res.reachable && !IK.armsCrossed(res.theta1, res.theta2)) {
        segs.push({
          s1: IK.radToStepsAbsolute(res.theta1, 1),
          s2: IK.radToStepsAbsolute(res.theta2, 2),
          mode: 'overshoot_settle',
          fraction: 1.1
        });
      }
    }
    return segs;
  }
};