haptic_skin

18class Direction(IntEnum):
19    """8 cardinal positions, in the wearer's frame. Index 0 = front (12 o'clock),
20    going clockwise. These ARE the motor indices."""
21
22    N = 0   # front
23    NE = 1
24    E = 2   # right
25    SE = 3
26    S = 4   # back
27    SW = 5
28    W = 6   # left
29    NW = 7

 66class Necklace:
 67    """Owns a transport and the current motor state."""
 68
 69    def __init__(self, transport: Optional[Transport] = None,
 70                 port: Optional[str] = None,
 71                 motor_map: Optional[Sequence[int]] = None):
 72        self.transport = transport or open_transport(port)
 73        self.values: List[int] = [0] * protocol.NUM_MOTORS
 74        # motor_map[logical] = physical wire. Defaults to identity; connect()
 75        # passes BUILD_MOTOR_MAP for the mirrored prototype wiring.
 76        if motor_map is None:
 77            self.motor_map: List[int] = list(range(protocol.NUM_MOTORS))
 78        else:
 79            mm = list(motor_map)
 80            if sorted(mm) != list(range(protocol.NUM_MOTORS)):
 81                raise ValueError(
 82                    "motor_map must be a permutation of "
 83                    f"0..{protocol.NUM_MOTORS - 1}, got {motor_map!r}")
 84            self.motor_map = mm
 85
 86    # -- low level ----------------------------------------------------------
 87    # Set commands are fire-and-forget: the transport returns the (usually
 88    # empty) reply frame, which callers may ignore.
 89    def set_motor(self, index: int, value: int) -> bytes:
 90        if not 0 <= index < protocol.NUM_MOTORS:
 91            raise ValueError(
 92                f"motor index {index} out of range (valid 0..{protocol.NUM_MOTORS - 1})")
 93        self.values[index] = protocol.clamp_pwm(value)
 94        return self.transport.send(protocol.set_motor(self.motor_map[index], value))
 95
 96    def set_all(self, value: int) -> bytes:
 97        v = protocol.clamp_pwm(value)
 98        self.values = [v] * protocol.NUM_MOTORS
 99        return self.transport.send(protocol.set_all(value))
100
101    def set_vector(self, values: Sequence[int]) -> bytes:
102        if len(values) != protocol.NUM_MOTORS:
103            raise ValueError(
104                f"expected {protocol.NUM_MOTORS} values, got {len(values)}")
105        self.values = [protocol.clamp_pwm(v) for v in values]
106        phys = [0] * protocol.NUM_MOTORS
107        for logical, v in enumerate(values):
108            phys[self.motor_map[logical]] = v
109        return self.transport.send(protocol.set_vector(phys))
110
111    def stop(self) -> bytes:
112        self.values = [0] * protocol.NUM_MOTORS
113        return self.transport.send(protocol.stop())
114
115    def ping(self) -> bool:
116        reply = self.transport.send(protocol.ping())
117        frame = protocol.decode_one(reply, start_byte=protocol.START_P2H)
118        return frame is not None and frame[0] == protocol.EVT_PONG
119
120    def version(self) -> str:
121        return f"{protocol.FW_ID} N={protocol.NUM_MOTORS}"
122
123    # -- high level ---------------------------------------------------------
124    def haptic(self, direction, intensity: float = 1.0) -> str:
125        """Vibrate a single direction at 0.0..1.0 intensity (others off)."""
126        d = resolve_direction(direction)
127        vec = [0] * protocol.NUM_MOTORS
128        vec[int(d)] = protocol.intensity_to_pwm(intensity)
129        return self.set_vector(vec)
130
131    def close(self) -> None:
132        try:
133            self.stop()
134        finally:
135            self.transport.close()
136
137    def __enter__(self) -> "Necklace":
138        return self
139
140    def __exit__(self, *exc) -> None:
141        self.close()

32class Transport(ABC):
33    """Sends one protocol frame and returns the controller's reply frame."""
34
35    @abstractmethod
36    def send(self, frame: bytes) -> bytes:  # pragma: no cover - interface
37        ...
38
39    def close(self) -> None:  # pragma: no cover - default no-op
40        pass
41
42    def __enter__(self) -> "Transport":
43        return self
44
45    def __exit__(self, *exc) -> None:
46        self.close()

134class SerialTransport(Transport):
135    """Talks to a real Pico over USB CDC-ACM — resilient to unplug/replug.
136
137    Never raises if the board is missing: it keeps trying to (re)open in the
138    background so the bridge stays up across a disconnect, and reconnects on its
139    own when the Pico comes back. Only ``Ping`` frames wait for a reply; every
140    other frame is fire-and-forget (so streaming motor updates never block).
141    """
142
143    def __init__(self, port: str, baud: int = 115200, timeout: float = 0.5):
144        try:
145            import serial  # type: ignore
146        except ImportError as exc:  # pragma: no cover
147            raise RuntimeError(
148                "pyserial is required for SerialTransport — `pip install pyserial`"
149            ) from exc
150        self._serial_mod = serial
151        self.port, self.baud, self.timeout = port, baud, timeout
152        self._serial = None
153        self._last_try = 0.0
154        self._open(initial=True)
155
156    def _open(self, initial: bool = False) -> None:
157        import time
158        try:
159            self._serial = self._serial_mod.Serial(self.port, self.baud, timeout=self.timeout)
160            if initial:
161                time.sleep(2.0)            # let the board enumerate / settle
162            self._serial.reset_input_buffer()
163            print(f"[serial] connected to {self.port}")
164        except Exception:
165            self._serial = None
166            if initial:
167                print(f"[serial] {self.port} not available yet — will retry")
168
169    def _ensure(self) -> bool:
170        if self._serial is not None:
171            return True
172        import time
173        now = time.monotonic()
174        if now - self._last_try < 2.0:     # throttle reconnect attempts
175            return False
176        self._last_try = now
177        self._open()
178        return self._serial is not None
179
180    def send(self, frame: bytes) -> bytes:
181        if not self._ensure():
182            return b""                      # board absent: no-op, keep running
183        try:
184            self._serial.write(frame)
185            self._serial.flush()
186            if _is_ping(frame):
187                return self._serial.read(64)  # pong frame (or b"" on timeout)
188            return b""                       # set commands are fire-and-forget
189        except Exception:                    # unplugged mid-session
190            try:
191                self._serial.close()
192            except Exception:
193                pass
194            self._serial = None
195            print(f"[serial] lost {self.port} — will reconnect")
196            return b""
197
198    def close(self) -> None:
199        try:
200            if self._serial is not None:
201                self._serial.close()
202        except Exception:  # pragma: no cover
203            pass

118class MockTransport(Transport):
119    """In-process transport backed by :class:`FakeMega` (no hardware)."""
120
121    def __init__(self) -> None:
122        self.device = FakeMega()
123        self.sent: List[bytes] = []
124
125    def send(self, frame: bytes) -> bytes:
126        self.sent.append(frame)
127        return self.device.handle(frame)

 49class FakeMega:
 50    """Pure-Python re-implementation of the firmware command handler.
 51
 52    Decodes binary frames, mirrors the power guard, and tracks motor state so
 53    tests and offline development see the same behaviour as the hardware.
 54    (Name kept for continuity; it now emulates the Pico/Rust firmware.)
 55    """
 56
 57    def __init__(self) -> None:
 58        self.motors: List[int] = [0] * protocol.NUM_MOTORS
 59        # history of every committed vector — handy for assertions in tests
 60        self.history: List[List[int]] = []
 61
 62    @staticmethod
 63    def _clamp(value: int) -> int:
 64        """Mirror ``Motors::set`` — every motor write is bounded to MAX_DUTY."""
 65        return max(0, min(MAX_DUTY, value))
 66
 67    @staticmethod
 68    def _power_guard(values: List[int]) -> List[int]:
 69        """Mirror ``tasks::power_guard::apply`` (the SetAll path only).
 70
 71        1) cap each motor to ``DUTY_CAP``; 2) keep the ``MAX_ACTIVE_MOTORS``
 72        strongest non-zero motors, cut the rest to 0.
 73        """
 74        v = [min(DUTY_CAP, max(0, x)) for x in values]
 75        ranked = sorted((i for i, x in enumerate(v) if x > 0),
 76                        key=lambda i: v[i], reverse=True)
 77        for i in ranked[MAX_ACTIVE_MOTORS:]:
 78            v[i] = 0
 79        return v
 80
 81    def _commit(self, values: List[int]) -> None:
 82        self.motors = [self._clamp(v) for v in values]
 83        self.history.append(list(self.motors))
 84
 85    def handle(self, frame: bytes) -> bytes:
 86        """Process one host->device frame, return the reply frame (or ``b""``)."""
 87        decoded = protocol.decode_one(frame, start_byte=protocol.START_H2P)
 88        if decoded is None:
 89            return protocol.error()
 90        cmd, payload = decoded
 91
 92        if cmd == protocol.CMD_PING:
 93            return protocol.pong()
 94
 95        if cmd == protocol.CMD_STOP_ALL:
 96            self._commit([0] * protocol.NUM_MOTORS)
 97            return b""
 98
 99        if cmd == protocol.CMD_SET_MOTOR:
100            # firmware SetMotor path: direct write, clamped, NO concurrency guard
101            if len(payload) != 2 or not 0 <= payload[0] < protocol.NUM_MOTORS:
102                return protocol.error()
103            req = list(self.motors)
104            req[payload[0]] = payload[1]
105            self._commit(req)
106            return b""
107
108        if cmd == protocol.CMD_SET_ALL:
109            # firmware SetAll path: power guard (cap + concurrency) then clamp
110            if len(payload) != protocol.NUM_MOTORS:
111                return protocol.error()
112            self._commit(self._power_guard(list(payload)))
113            return b""
114
115        return protocol.error()

206def open_transport(port: Optional[str] = None) -> Transport:
207    """Open a real serial transport if ``port`` is given, else a mock."""
208    if port:
209        return SerialTransport(port)
210    return MockTransport()

54def resolve_direction(d) -> Direction:
55    """Accept a Direction, an int 0..7, or a name like 'turn_left'."""
56    if isinstance(d, Direction):
57        return d
58    if isinstance(d, int):
59        return Direction(d % protocol.NUM_MOTORS)
60    key = str(d).strip().lower()
61    if key in _DIRECTION_ALIASES:
62        return _DIRECTION_ALIASES[key]
63    raise ValueError(f"unknown direction: {d!r}")

37@dataclass
38class Route:
39    waypoints: List[Waypoint]
40
41    def __len__(self) -> int:
42        return len(self.waypoints)
43
44    def to_json(self) -> str:
45        return json.dumps(
46            {"waypoints": [vars(w) for w in self.waypoints]}, indent=2
47        )

25@dataclass
26class Waypoint:
27    lat: float
28    lon: float
29    next_bearing: float          # compass bearing to follow after this point
30    instruction: str = ""
31
32    @property
33    def point(self) -> LatLon:
34        return (self.lat, self.lon)

105class Navigator:
106    def __init__(self, route: Route):
107        self.route = route
108        self.idx = 1                       # waypoint we are heading toward (0 = start)
109        self.state = NavState("guide", 0, math.inf, "", 1, len(route), False)
110
111    # -- input ---------------------------------------------------------------
112    def update(self, fix: dict) -> NavState:
113        pos: LatLon = (fix["lat"], fix["lon"])
114        heading = float(fix.get("heading", 0.0))
115        wps = self.route.waypoints
116
117        # skip every waypoint we've reached OR already walked past (corner overshoot)
118        while self.idx < len(wps):
119            tgt = wps[self.idx].point
120            prev_pt = wps[self.idx - 1].point
121            reached = haversine_m(pos, tgt) <= ARRIVE_STEP_M
122            passed = _segment_progress_t(pos, prev_pt, tgt) >= 1.0
123            if reached or passed:
124                self.idx += 1
125            else:
126                break
127
128        if self.idx >= len(wps):
129            self.state = NavState("arrived", 0, 0.0, "Arrivée",
130                                  len(wps), len(wps), True)
131            return self.state
132
133        target = wps[self.idx]
134        dist = haversine_m(pos, target.point)
135
136        # off-route alarm: strayed too far from the current leg -> back motor
137        prev = wps[self.idx - 1]
138        if _point_segment_distance_m(pos, prev.point, target.point) > OFF_ROUTE_M:
139            self.state = NavState("off_route", 4, dist, "Hors itinéraire",
140                                  self.idx, len(wps), False)
141            return self.state
142
143        # homing: steer toward the NEXT waypoint POSITION (where to physically walk
144        # next). With the overshoot-advance above, the active motor swings onto the
145        # new street exactly when you reach the corner — never toward an abstract
146        # future heading (which would aim through a wall before the turn).
147        rel = relative_bearing(bearing_deg(pos, target.point), heading)
148        bucket = bucket_from_relative(rel)
149        band = "approach" if dist <= APPROACH_M else "guide"
150
151        self.state = NavState(band, bucket, dist, target.instruction,
152                              self.idx, len(wps), False)
153        return self.state
154
155    # -- output --------------------------------------------------------------
156    def vector(self, now: float) -> List[int]:
157        """8 PWM values to apply at time ``now`` (seconds, monotonic)."""
158        s = self.state
159        if s.band == "arrived":
160            step = SWEEP_PERIOD_S / protocol.NUM_MOTORS
161            motor = int((now / step) % protocol.NUM_MOTORS)
162            return self._one(motor, 0.8)
163
164        if s.band not in _CADENCE:
165            return list(_ZERO)
166
167        intensity, on_ms, off_ms = _CADENCE[s.band]
168        period = (on_ms + off_ms) / 1000.0
169        phase = (now % period) * 1000.0
170        return self._one(s.bucket, intensity) if phase < on_ms else list(_ZERO)
171
172    @staticmethod
173    def _one(bucket: int, intensity: float) -> List[int]:
174        vec = [0] * protocol.NUM_MOTORS
175        vec[int(bucket)] = protocol.intensity_to_pwm(intensity)
176        return vec

 94@dataclass
 95class NavState:
 96    band: str                 # guide | approach | arrived
 97    bucket: int               # active motor 0..7 (front=0, clockwise)
 98    distance_m: float
 99    instruction: str
100    step_index: int
101    total_steps: int
102    finished: bool

59def connect(port: Optional[str] = None) -> Necklace:
60    """Return a process-wide :class:`Necklace`, creating it on first use.
61
62    ``port`` defaults to ``$HAPTIC_SKIN_PORT``; if unset, a simulator is used.
63    """
64    global _default
65    if _default is None:
66        _default = Necklace(port=port or os.environ.get("HAPTIC_SKIN_PORT"),
67                            motor_map=BUILD_MOTOR_MAP)
68    return _default

71def haptic(direction, intensity: float = 1.0) -> str:
72    """One-liner: vibrate ``direction`` at ``intensity`` on the default necklace."""
73    return connect().haptic(direction, intensity)