"""Reference encoder/decoder for SailEver Track Binary (SLTB) v1. Conformance oracle for the Ruby decoder and the Monkey C encoder. Spec: docs/specs/shared/track-sync.md. """ import math, struct M_PER_SC = 6378137.0 * math.pi / 2**31 # metres per semicircle (lat) # END record payload (spec §4.4): reason, then device totals. END_FIELDS = ('reason', 'timer_s', 'distance_m', 'sailed_m', 'motored_m', 'motorsailed_m', 'sailed_s', 'motored_s', 'motorsailed_s', 'stationary_s', 'max_sog_ckn') END_FMT = '>BIIIIIIIIIH' REASON_SAVED, REASON_DISCARDED, REASON_RECOVERED = 0, 1, 2 def rnd(x): """Round half up, identical on every platform (spec §4.5).""" return math.floor(x + 0.5) def crc16(data): crc = 0xFFFF for b in data: crc ^= b << 8 for _ in range(8): crc = ((crc << 1) ^ 0x1021) if crc & 0x8000 else (crc << 1) crc &= 0xFFFF return crc def wrap32(v): return (v + 2**31) % 2**32 - 2**31 class Encoder: def __init__(self, track_id, interval=5): self.track_id, self.interval, self.seq = track_id, interval, 0 self.segments = [] self.stats = {} self._new_body() def _new_body(self): self.body = bytearray(); self.kf = None; self.zero_run = 0; self.first_in_seg = True def _count(self, k, n=1): self.stats[k] = self.stats.get(k, 0) + n def _flush_run(self): n = self.zero_run; self.zero_run = 0 while n > 0: if n == 1: self.body.append(60); self._count('tiny') elif n <= 8: self.body.append(119 + n); self._count('run_short') else: m = min(n, 255); self.body += bytes([0xE0, m]); self._count('run'); n -= m; continue break def keyframe(self, t, lat_sc, lon_sc, pres, state): self._flush_run() self.body += struct.pack('>BIiiHB', 0xF0, t, lat_sc, lon_sc, pres, state) self._count('keyframe') self.kf = (lat_sc, lon_sc, math.cos(lat_sc * math.pi / 2**31)) self.P = (0, 0); self.V = (0, 0); self.t = t self.pres, self.state = pres, state def project(self, lat_sc, lon_sc): lat0, lon0, c = self.kf return (rnd(wrap32(lat_sc - lat0) * M_PER_SC), rnd(wrap32(lon_sc - lon0) * M_PER_SC * c)) def point(self, t, lat_sc, lon_sc, pres=0, state=2): if self.kf is None: return self.keyframe(t, lat_sc, lon_sc, pres, state) expected = self.t + self.interval if t != expected: k = (t - expected) // self.interval if (t - expected) % self.interval or k > 255 or k < 0: return self.keyframe(t, lat_sc, lon_sc, pres, state) self._flush_run(); self.body += bytes([0xE3, k]); self._count('skip') self.V = (0, 0); self.t += k * self.interval if state != self.state: self._flush_run(); self.body += bytes([0xE2, state]); self._count('state'); self.state = state if pres and self.pres and abs(pres - self.pres) >= 2: d = max(-127, min(127, pres - self.pres)) self._flush_run(); self.body += struct.pack('>Bb', 0xE1, d); self._count('pressure'); self.pres += d T = self.project(lat_sc, lon_sc) pred = (self.P[0] + self.V[0], self.P[1] + self.V[1]) ry, rx = T[0] - pred[0], T[1] - pred[1] if not (-512 <= ry <= 511 and -512 <= rx <= 511): return self.keyframe(t, lat_sc, lon_sc, pres, state) if ry == 0 and rx == 0: self.zero_run += 1 else: self._flush_run() if -5 <= ry <= 5 and -5 <= rx <= 5: self.body.append((ry + 5) * 11 + (rx + 5)); self._count('tiny') elif -64 <= ry <= 63 and -64 <= rx <= 63: v = ((ry & 0x7F) << 7) | (rx & 0x7F) self.body += bytes([0x80 | (v >> 8), v & 0xFF]); self._count('small') else: v = ((ry & 0x3FF) << 10) | (rx & 0x3FF) self.body += bytes([0xC0 | (v >> 16), (v >> 8) & 0xFF, v & 0xFF]); self._count('medium') self.V = (T[0] - self.P[0], T[1] - self.P[1]); self.P = T; self.t = t def close_segment(self, final=False, end=None): self._flush_run() if final and end is not None: self.body += bytes([0xF1]) + struct.pack(END_FMT, *(end.get(k, 0) for k in END_FIELDS)) flags = (1 if final else 0) | (2 if self.seq == 0 else 0) hdr = struct.pack('>2sBBIHBBHH', b'SV', 1, flags, self.track_id, self.seq, self.interval, 1, len(self.body), crc16(self.body)) self.segments.append(bytes(hdr + self.body)); self.seq += 1 self._new_body() def sext(v, bits): return v - (1 << bits) if v & (1 << (bits - 1)) else v def decode(seg): magic, ver, flags, track_id, seq, interval, pred, blen, crc = struct.unpack('>2sBBIHBBHH', seg[:16]) assert magic == b'SV' and ver == 1 and pred == 1 body = seg[16:16 + blen]; assert len(body) == blen and crc16(body) == crc out = []; i = 0; P = V = (0, 0); t = None; kf = None; pres = state = None; end = None def emit(): if kf is None: raise ValueError('point before KEYFRAME') lat0, lon0, c = kf lat = (lat0 + P[0] / M_PER_SC) * 180 / 2**31 lon = (lon0 + P[1] / (M_PER_SC * c)) * 180 / 2**31 lon = (lon + 180) % 360 - 180 out.append({'t': t, 'lat': lat, 'lon': lon, 'pres': pres / 10 if pres else None, 'state': state}) def step(r): nonlocal P, V, t T = (P[0] + V[0] + r[0], P[1] + V[1] + r[1]); V = (T[0] - P[0], T[1] - P[1]); P = T; t += interval; emit() while i < len(body): b = body[i] if b <= 120: step(((b // 11) - 5, (b % 11) - 5)); i += 1 elif b <= 127: for _ in range(b - 119): step((0, 0)) i += 1 elif b <= 0xBF: v = ((b & 0x3F) << 8) | body[i + 1]; step((sext(v >> 7, 7), sext(v & 0x7F, 7))); i += 2 elif b <= 0xCF: v = ((b & 0x0F) << 16) | (body[i + 1] << 8) | body[i + 2]; step((sext(v >> 10, 10), sext(v & 0x3FF, 10))); i += 3 elif b == 0xE0: for _ in range(body[i + 1]): step((0, 0)) i += 2 elif b == 0xE1: pres += struct.unpack('>b', body[i + 1:i + 2])[0]; i += 2 elif b == 0xE2: state = body[i + 1]; i += 2 elif b == 0xE3: t += body[i + 1] * interval; V = (0, 0); i += 2 elif b == 0xEF: i += 3 + body[i + 2] elif b == 0xF0: t, la, lo, pres, state = struct.unpack('>Iii HB'.replace(' ', ''), body[i + 1:i + 16]) kf = (la, lo, math.cos(la * math.pi / 2**31)); P = V = (0, 0); emit(); i += 16 elif b == 0xF1: n = struct.calcsize(END_FMT) end = dict(zip(END_FIELDS, struct.unpack(END_FMT, body[i + 1:i + 1 + n]))); i += 1 + n else: raise ValueError('reserved record 0x%02X at %d' % (b, i)) return {'track_id': track_id, 'seq': seq, 'final': bool(flags & 1), 'points': out, 'end': end}