hostside/movpos.c

   1 /*
   2  * Handling of points and other moveable features.
   3  */
   4
   5 #include "realtime.h"
   6
   7 /*========== declarations ==========*/
   8
   9 typedef struct {
  10   const MovFeatInfo *i;
  11   Small posn;
  12 } Motion;
  13
  14 typedef struct Method Method;
  15
  16 /* Kind-independent code is responsible for determining
  17  * the method, doing a bit of cleanup, and adjusting the flow
  18  * slightly.  Per-kind code does the actual work and is mostly in
  19  * charge - it is also responsible for updating seg->moving and ->motion.
  20  */
  21 /* The following states exist for each MovPosChange
  22  * at points when control flow  passes between kind and indep:
  23  *   U  Unallocated  no memory allocated (MovPosChange does not exist)
  24  *   A  Allocated    memory allocation done
  25  *   R  Reserved     reservation was successful
  26  *   C  Confirmed    motion queued and will occur
  27  *   D  Done         motion is complete and callback just needs to be made
  28  *   E  Erroneous    indep must call destroy straight away
  29  * seg->moving and ->motion is in one of the states UC
  30  */
  31
  32 typedef struct MovPosChange {      /* valid in:   filled in by and when:     */
  33   Method *meth;                    /*  ARCDE       indep after allocate()    */
  34   Segment *move;                   /*  ARCDE       indep after allocate()    */
  35   MovPosComb actual;               /*  CD          see below                 */
  36   MovPosComb intent;               /*  RCD         indep after allocate()    */
  37   /* kind-specific data follows */ /*  varies     kind-specific code, varies */
  38 } Change;
  39   /* `actual' contains the kind's public opinion about the physical
  40    * state.  It is initialised by indep (just before confirm) from
  41    * move->motion->actual or move->movposcomb as the case may be.  It
  42    * should be updated by the kind, since it is used by indep for
  43    * calculating the number and identities of the features which may
  44    * need to change when a new move request is intended to replace an
  45    * existing one - ie, the contents of the motions[] provided to a
  46    * subsequent confirm().  So while a change is Confirmed, the
  47    * physical state is recorded only in the relevant change, and not
  48    * in the segment's movposcomb.  Once a change goes to Confirmed,
  49    * the indep code never untangles it so the kind can manage the
  50    * proper transition.  */
  51
  52 struct Method {
  53   const char *pname;
  54   MovFeatKind kind;
  55   Change *(*allocate)(Method *m, int alloc_motions);     /* U->A (never err) */
  56   ErrorCode (*reserve)(Method *m, Change*, Segment*,                 /* A->R */
  57                        int ms);                               /* error: A->E */
  58   ErrorCode (*confirm)(Method *m, Change*, Segment*,          /*     [AR]->C */
  59                        int n_motions, const Motion*, int ms); /* err:[AR]->E */
  60   void (*destroy)(Method *m, Change*);                          /* [ARCE]->U */
  61   void (*all_abandon)(Method *m);
  62   /* indep guarantees that
  63    *   alloc_motions >= move->i->n_motions    on reserve
  64    *   alloc_motions >= n_motions             on confirm
  65    * and that if on entry to reserve move->motion is non-0,
  66    *  it move->motion is non-0 and of the same kind
  67    */
  68 };
  69
  70 const char *movpos_pname(Segment *move, MovPosComb poscomb) {
  71   return poscomb<0 ? "?" : move->i->poscombs[poscomb].pname;
  72 }
  73
  74 static void ouposn_moving(Change *chg) {
  75   Segment *move= chg->move;
  76   oprintf(UPO, "movpos %s position %s moving\n",
  77           move->i->pname, movpos_pname(move, chg->actual));
  78 }
  79
  80 static void motion_done(Segment *move, MovPosComb actual) {
  81   move->moving= 0;
  82   move->motion= 0;
  83   move->movposcomb= actual;
  84   oprintf(UPO, "movpos %s position %s stable\n",
  85           move->i->pname, movpos_pname(move, move->movposcomb));
  86 }
  87
  88 static void ignore_all_abandon(Method *m) { }
  89
  90 /*========== points and other fixed timeslot movfeats ==========*/
  91
  92 /*
  93  * We maintain two queues, one for reserved one for actually confirmed
  94  *  requests where we know what we're doing.
  95  *
  96  * We divide time into discrete slots, numbered with clock arithmetic.
  97  *
  98  *     cslot         cslot+1      cslot+2
  99  *
 100  *     currently     next in      after
 101  *     changing      line         that
 102  *
 103  * We increment cslot when we consider the movfeat to move;
 104  * for points and relays this is when we issue the command to the PIC.
 105  * In a request, the deadline represents the latest allowable value
 106  * of cslot just before that increment.
 107  */
 108
 109 typedef unsigned FsqSlot;
 110 typedef int FsqSlotSigned;
 111
 112 /* We think there are three states: Allocated, Reserved and Confirmed.
 113  * (plus of course Unallocated where we don't have a request at all).
 114  * These correspond to the indep code as follows:
 115  *
 116  *  indep state   pt state    queues checked and plan viable
 117  *   Unallocated   n/a         yes
 118  *   Allocated     Allocated   yes
 119  *   Reserved      Reserved    yes
 120  *   Confirmed     Confirmed   yes
 121  *   Erroneous     A/R/C       no
 122  *
 123  * Erroneous exists only after a failed reserve() or confirm() so it's
 124  * not that confusing to have this slightly malleable terminology.
 125  */
 126
 127 #define FSQDN (~(FsqSlot)0)
 128
 129 typedef struct {                 /* Allocated  Reserved   Confirmed       */
 130                      /* in queue?    absent     reserved   confirmed      */
 131   Change h;
 132   FsqSlot deadline;  /*              ~0==FSQDN  relative   absolute    <- */
 133   MovPosComb actual; /*              undef      undef      see below      */
 134   int n_motions;     /*              alloc'd    alloc'd    undone         */
 135   Motion motions[];  /*   [0].i:     0          0          non-0       <- */
 136                      /*  [..].i:     undef      undef      non-0          */
 137                      /*   .posn:     undef      undef      defined        */
 138 } FsqReq;
 139   /* We can determine the the state by looking at the two
 140    * `statedet' fields, marked <- above.
 141    * There are also intermediate states where the req's
 142    *  statedet fields do not agree with the queue it's on.
 143    *  We write these as, for example,
 144    *      AR   to mean  statedet says Allocated, but queued on fsq_reserved
 145    *      A?   to mean  statedet says Allocated, but may be queued
 146    *  etc.  They are only allowed while we are in a fsq_... method function.
 147    */
 148   /* FsqReq.actual is subtly differnet to MovPosChange.actual,
 149    * as follows:
 150    *                              in MovPosChange     in FsqReq
 151    *  Position unknown              -1                  0
 152    *  Position partly known         -1                  unknown feats are 0
 153    *  Position completely known     exact               exact
 154    *
 155    * The partial knowledge positions can only occur in requests that
 156    * are confirmed with as many motions as features, so we know that
 157    * if we complete a request we know that we can copy actual out
 158    * to MovPosChange.
 159    *
 160    * If we abandon a half-done change to a multi-feat segment
 161    * we lose the partial knowledge.
 162    */
 163
 164 #define FSQ_MAX_QUEUE  15
 165
 166 typedef struct {
 167   int n;
 168   FsqReq *l[FSQ_MAX_QUEUE];
 169 } FsqQueue;
 170
 171 typedef struct FsqMethod FsqMethod;
 172
 173 typedef struct {
 174   /* set by fsq's client before fsq_init */
 175   int slot_ms, lag_ms;
 176   void (*move)(FsqMethod *m, const MovFeatInfo *mfi, int movfeatposn);
 177   /* shared */
 178   int ready; /* >0 means ready; set to 0 by fsq just before move */
 179   /* fsq's client must arrange that these start out at all-bits-zero */
 180   FsqSlot cslot;
 181   FsqQueue confirmed, reserved;
 182 } FsqState;
 183
 184 struct FsqMethod {
 185   Method m;
 186   FsqState f;
 187   /* client may put things here */
 188 };
 189
 190 static void fsq_check_action(FsqMethod *m);
 191 static ErrorCode fsq_check_plan(FsqMethod *m);
 192
 193 static FsqSlotSigned fsq_maxdelay_reldeadline(FsqMethod *m,
 194                                               int maxdelay_ms, int n_motions) {
 195   if (maxdelay_ms==-1) return FSQ_MAX_QUEUE*16;
 196   return (maxdelay_ms - m->f.lag_ms) / m->f.slot_ms - n_motions;
 197 }
 198
 199 static void fsq_queue_remove_index(FsqQueue *q, int index) {
 200   q->n--;
 201   memmove(&q->l[index], &q->l[index+1], sizeof(q->l[0]) * (q->n - index));
 202 }
 203
 204 static int fsq_req_compar_approx(const void *av, const void *bv) {
 205   FsqReq *const *a= av;
 206   FsqReq *const *b= av;
 207   FsqSlotSigned dldiff= (*b)->deadline - (*a)->deadline;
 208   return dldiff;
 209 }
 210
 211 static void fsq_queue_remove_item(FsqQueue *q, FsqReq *r) {
 212   FsqReq **entry;
 213   int i;
 214   entry= bsearch(r, q->l, q->n, sizeof(q->l[0]), fsq_req_compar_approx);
 215   assert(entry);
 216
 217 #define SEARCH_CHECK do{                                \
 218     if (q->l[i] == r) goto found;                       \
 219     if (q->l[i]->deadline != r->deadline) break;        \
 220 }while(0)
 221   for(i= entry - q->l;     i >= 0;   i--)  SEARCH_CHECK;
 222   for(i= entry - q->l + 1; i < q->n; i++)  SEARCH_CHECK;
 223   abort();
 224
 225  found:
 226   fsq_queue_remove_index(q, i);
 227 }
 228
 229 static void fsq_dequeue(FsqMethod *m, FsqReq *r) { /* X->XA */
 230   if (r->motions[0].i) {
 231     fsq_queue_remove_item(&m->f.confirmed, r);
 232   } else if (r->deadline!=FSQDN) {
 233     fsq_queue_remove_item(&m->f.reserved, r);
 234   } else {
 235     return;
 236   }
 237   fsq_check_plan(m);
 238 }
 239
 240 static void fsq_mark_as_allocated(FsqReq *r) { /* AX->X */
 241   /* Sets statedet fields for Allocated */
 242   r->deadline= FSQDN;
 243   r->motions[0].i=0;
 244 }
 245
 246 #define WHICH(wh)                               \
 247   (whichr= wh##r,                               \
 248    whichwhen= wh##when,                         \
 249    wh++)
 250
 251 static ErrorCode fsq_check_plan(FsqMethod *m) {
 252   /* Checks whether we can meet the currently queued commitments */
 253   int future, conf, resv, whichwhen;
 254   FsqReq *whichr;
 255
 256   conf=resv=0;
 257   future=0;
 258
 259   oprintf(DUPO("movpos/fsq") "%s   plan", m->m.pname);
 260
 261   /* If CDU is charged we can't do one right away */
 262   if (m->f.ready<0) {
 263     oprintf(UPO, " +");
 264     future++;
 265   }
 266
 267   for (;;) {
 268     FsqReq *confr= conf < m->f.confirmed.n ? m->f.confirmed.l[conf] : 0;
 269     FsqReq *resvr= resv < m->f.reserved .n ? m->f.reserved .l[resv] : 0;
 270     if (!confr && !resvr) break;
 271     oprintf(UPO," %d:",future);
 272     int confwhen= confr ? confr->deadline - m->f.cslot : INT_MAX;
 273     int resvwhen= resvr ? resvr->deadline              : INT_MAX;
 274     if (future && resvwhen < confwhen) {
 275       WHICH(resv);
 276       oprintf(UPO,"~");
 277     } else if (confr) {
 278       WHICH(conf);
 279     } else {
 280       oprintf(UPO,"-");
 281       future++;
 282       continue;
 283     }
 284     oprintf(UPO, "%s/%s[%d@t+%d]", whichr->h.move->i->pname,
 285             movpos_pname(whichr->h.move, whichr->h.intent),
 286             whichr->n_motions, whichwhen);
 287     if (future > whichwhen) {
 288       oprintf(UPO,"!...bad\n");
 289       return EC_MovFeatTooLate;
 290     }
 291     future += whichr->n_motions;
 292   }
 293   oprintf(UPO," ok\n");
 294   return 0;
 295 }
 296
 297 #undef WHICH
 298
 299 static ErrorCode fsq_enqueue(FsqMethod *m, FsqQueue *q,         /* XA -> X */
 300                              FsqReq *r) {        /* or on error,   XA -> A */
 301   int insat;                 /* ... where X is R or C and corresponds to q */
 302
 303   if (q->n == FSQ_MAX_QUEUE)
 304     return EC_BufferFull;
 305
 306   for (insat= q->n;
 307        insat>0 && (FsqSlotSigned)(r->deadline - q->l[insat-1]->deadline) < 0;
 308        insat--)
 309     q->l[insat]= q->l[insat-1];
 310   q->l[insat]= r;
 311   q->n++;
 312
 313   return fsq_check_plan(m);
 314   /* if this fails, indep machinery calls fsq_destroy which dequeues */
 315 }
 316
 317 /*---------- method entrypoints ----------*/
 318
 319 static Change *fsq_allocate(Method *mm, int alloc_motions) {
 320   FsqReq *r;
 321
 322   if (!alloc_motions)
 323     /* we need at least one motion in the table so we can tell
 324      *  the difference between the states by looking at motions[0].i */
 325     alloc_motions= 1;
 326
 327   r= mmalloc(sizeof(*r) + alloc_motions * sizeof(r->motions[0]));
 328   r->deadline= FSQDN;
 329   r->n_motions= alloc_motions;
 330   r->motions[0].i= 0;
 331   return (Change*)r;
 332 }
 333
 334 static ErrorCode fsq_reserve(Method *mm, Change *chg,
 335                              Segment *move, int maxdelay_ms) {
 336   FsqMethod *m= (void*)mm;
 337   FsqReq *r= (FsqReq*)chg;
 338   FsqSlotSigned reldeadline;
 339
 340   reldeadline= fsq_maxdelay_reldeadline(m, maxdelay_ms, r->n_motions);
 341   if (reldeadline <= 0) { fsq_mark_as_allocated(r); return EC_MovFeatTooLate; }
 342   r->deadline= reldeadline;
 343   return fsq_enqueue(m, &m->f.reserved, r);
 344 }
 345
 346 static ErrorCode fsq_confirm(Method *mm, Change *chg, Segment *move,
 347                              int n_motions, const Motion *motions,
 348                              int maxdelay_ms) {
 349   FsqMethod *m= (void*)mm;
 350   FsqReq *r= (FsqReq*)chg;
 351   FsqSlotSigned reldeadline;
 352   int allow_failure;
 353   ErrorCode ec;
 354
 355   oprintf(DUPO("movpos/fsq") "%s confirm %s n=%d maxdelay=%dms",
 356           m->m.pname, move->i->pname, n_motions, maxdelay_ms);
 357
 358   assert(!r->motions[0].i); /* no confirming things already confirmed */
 359   if (r->deadline==FSQDN)
 360     oprintf(UPO, " (alloc'd: %d)\n", r->n_motions);
 361   else
 362     oprintf(UPO, " (res: %s/%s[%d@t+%d])\n",
 363             r->h.move->i->pname, movpos_pname(r->h.move, r->h.intent),
 364             r->n_motions, r->deadline);
 365
 366   /* If the segment is moving, these motions are already based on the
 367    * actual physical position which is stored in the existing request.
 368    * So we try removing the existing request from the queue and put
 369    * it back if it doesn't work.
 370    */
 371
 372   if (n_motions > r->n_motions)
 373     return EC_MovFeatReservationInapplicable;
 374   assert(n_motions <= r->n_motions);
 375   if (maxdelay_ms == -1) {
 376     reldeadline= r->deadline;
 377     if (reldeadline==FSQDN)
 378       reldeadline= fsq_maxdelay_reldeadline(m, -1, n_motions);
 379   } else {
 380     reldeadline= fsq_maxdelay_reldeadline(m, maxdelay_ms, n_motions);
 381   }
 382   allow_failure= reldeadline < (FsqSlotSigned)r->deadline;
 383   oprintf(DUPO("movpos/fsq") "%s  reldeadline=[%d@t+%d] allow_failure=%d\n",
 384           m->m.pname, n_motions, reldeadline, allow_failure);
 385
 386   /* state A or R */
 387   fsq_dequeue(m, r);
 388                                            /* states of existing: */
 389   FsqReq *existing=
 390     move->moving ? (FsqReq*)move->motion : 0;   /* U or C */
 391   if (existing) {
 392     oprintf(DUPO("movpos/fsq")
 393             "%s  existing %s n=%d deadline=t+%d\n",
 394             m->m.pname,
 395             existing->h.move->i->pname,
 396             existing->n_motions,
 397             existing->deadline - m->f.cslot);
 398     fsq_dequeue(m, existing);                   /* U or CA */
 399   }
 400
 401   /* state A or RA */
 402   memcpy(r->motions, motions, sizeof(r->motions[0])*n_motions);
 403   if (!n_motions) r->motions[0].i= move->i->movfeats;
 404   assert(r->motions[0].i);
 405   r->n_motions= n_motions;
 406   r->deadline= reldeadline + m->f.cslot;
 407
 408   if (n_motions == move->i->n_movfeats)
 409     r->actual= 0;
 410   else
 411     r->actual= chg->actual;
 412   assert(r->actual >= 0);
 413
 414   /* state CA */
 415   ec= fsq_enqueue(m, &m->f.confirmed, r);
 416   oprintf(DUPO("movpos/fsq") "%s  fsq_enqueue=%s\n", m->m.pname, ec2str(ec));
 417   assert(allow_failure || !ec);
 418
 419   if (existing) {                                  /* CA */
 420     if (ec) { /* state C but bad */
 421       fsq_dequeue(m,r); /* state CA */
 422       fsq_mark_as_allocated(r); /* state A */
 423       ErrorCode ec_putback= fsq_enqueue(m,&m->f.confirmed, existing);
 424       assert(!ec_putback);                         /* C */
 425     } else { /* state C and good */
 426       free(existing);                              /* U */
 427     }
 428   }
 429   /* either  ec=0   state C                            U
 430    *     or  ec!=0  state A                            C
 431    *     or  ec!=0  state C but bad                    C
 432    */
 433
 434   if (ec) return ec;
 435
 436   move->moving= 1;
 437   move->motion= chg;
 438   move->movposcomb= -1;
 439   ouposn_moving(chg);
 440   fsq_check_action(m);
 441   return 0;
 442 }
 443
 444 static void fsq_destroy(Method *mm, Change *chg) { /* X->XA and then freed */
 445   FsqMethod *m= (void*)mm;
 446   FsqReq *r= (FsqReq*)chg;
 447   fsq_dequeue(m,r);
 448   free(r);
 449 }
 450
 451 /*---------- something to do ? ----------*/
 452
 453 static void fsq_check_action(FsqMethod *m) {
 454   /* client should call this after it sets ready */
 455   ErrorCode ec;
 456
 457   if (!m->f.confirmed.n) {
 458     if (sta_state == Sta_Finalising) resolve_motioncheck();
 459     return;
 460   }
 461
 462   FsqReq *r= m->f.confirmed.l[0];
 463
 464   if (r->n_motions && m->f.ready>0) {
 465     /* look for something to move */
 466     Motion *mo= &r->motions[--r->n_motions];
 467     assert(mo->posn < mo->i->posns);
 468     m->f.ready= 0;
 469     m->f.move(m, mo->i, mo->posn);
 470     oprintf(UPO, "movpos %s feat %s %d %s\n", r->h.move->i->pname,
 471             mo->i->pname, mo->posn, m->m.pname);
 472     m->f.cslot++;
 473
 474     MovPosComb above_weight= mo->i->weight * mo->i->posns;
 475     MovPosComb above= r->actual / above_weight;
 476     MovPosComb below= r->actual % mo->i->weight;
 477     r->actual= above*above_weight + mo->posn*mo->i->weight + below;
 478     if (r->h.actual >= 0 || !r->n_motions)
 479       r->h.actual= r->actual;
 480     ouposn_moving(&r->h);
 481   }
 482
 483   if (!r->n_motions) {
 484     /* look for something to report
 485      * we can get things here other than from the above
 486      * eg if we are asked to move the
 487      */
 488     Segment *move= r->h.move;
 489     assert(move->moving && move->motion == (Change*)r);
 490     fsq_queue_remove_index(&m->f.confirmed,0);
 491     fsq_mark_as_allocated(r); /* now state A aka Done */
 492     motion_done(move,r->h.actual);
 493     free(r);
 494     ec= fsq_check_plan(m);  assert(!ec);
 495     fsq_check_action(m);
 496   }
 497 }
 498
 499 /*---------- entrypoints from rest of program ----------*/
 500
 501 static void fsq_all_abandon(Method *mm) {
 502   FsqMethod *m= (void*)mm;
 503   int i;
 504
 505   assert(!m->f.reserved.n);
 506
 507   for (i=0; i<m->f.confirmed.n; i++) {
 508     FsqReq *r= m->f.confirmed.l[i];
 509     Segment *move= r->h.move;
 510     assert(move->motion == (Change*)r);
 511     motion_done(move,r->h.actual);
 512     free(r);
 513   }
 514   m->f.confirmed.n= 0;
 515 }
 516
 517 /*========== points ==========*/
 518
 519 /*
 520  * CDU and point queue states:
 521  *
 522  *
 523  *    ____________                              conf'd
 524  *   /   points_  \                 ready         .n
 525  *   |   all_      |
 526  *   |   abandon   |
 527  *   |             V
 528  *   |from       INACTIVE               -1      0
 529  *   |any       <=Sta_Settling
 530  *  ^^^^^^^^     (start)
 531  *                 |
 532  *     ___________ |turning
 533  *    /           \| _on
 534  *   |             V
 535  *   |           CHARGING               0       any
 536  *   |          >=Sta_Resolving
 537  *   |             |
 538  *   |             |on_pic
 539  *   |             |_charged
 540  *   |             V
 541  *   ^           READY                  1       any
 542  *   |             |
 543  *   |             |fsq_check_action
 544  *   |             | calls point_move which fires a point
 545  *    \___________/
 546  *
 547  */
 548
 549 #define CDU_RECHARGE   250 /*ms*/
 550 #define POINT_MOVEMENT  50 /*ms*/
 551
 552 static Change *point_allocate(Method *mm, int alloc_motions) {
 553   FsqMethod *m= (void*)mm;
 554   assert(m->f.ready>=0);
 555   return fsq_allocate(mm, alloc_motions);
 556 }
 557
 558 static void point_move(FsqMethod *m, const MovFeatInfo *mfi, int posn) {
 559   /* actually firing points, yay! */
 560   PicInsn piob;
 561   enco_pic_point(&piob, mfi->boob[posn]);
 562   serial_transmit(&piob);
 563 }
 564
 565 static void points_all_abandon(Method *mm) {
 566   FsqMethod *m= (void*)mm;
 567   m->f.ready= -1;
 568   fsq_all_abandon(mm);
 569 }
 570
 571 static FsqMethod points_method;
 572
 573 void points_turning_on(void) {
 574   FsqMethod *m= &points_method;
 575   m->f.ready= 0;
 576 }
 577 void on_pic_charged(const PicInsnInfo *pii, const PicInsn *pi, int objnum) {
 578   FsqMethod *m= &points_method;
 579   if (m->f.ready<0) return;
 580   m->f.ready= 1;
 581   fsq_check_action(m);
 582 }
 583
 584 static FsqMethod points_method= {
 585   { "point", mfk_point,
 586     point_allocate, fsq_reserve, fsq_confirm,
 587     fsq_destroy, points_all_abandon },
 588   { .lag_ms= POINT_MOVEMENT, .slot_ms= CDU_RECHARGE, .move= point_move }
 589 };
 590
 591 /*========== relays ==========*/
 592
 593 /*
 594  * Waggler states:
 595  *
 596  *    ____________                              conf'd
 597  *   /  wagglers_ \                 ready         .n
 598  *   |   all_      |
 599  *   |   abandon   |
 600  *   |             V
 601  *   |from       UNKNOWN                -1      0
 602  *   |any       <=Sta_Settling
 603  *  ^^^^^^^^     (start)
 604  *                 |
 605  *     ___________ |turning
 606  *    /           \| _on
 607  *   |             V
 608  *   |           CHARGING               0       any
 609  *   |          >=Sta_Resolving
 610  *   |             |
 611  *   |             |on_pic
 612  *   |             |_charged
 613  *   |             V
 614  *   ^           READY                  1       any
 615  *   |             |
 616  *   |             |fsq_check_action
 617  *   |             | calls point_move which fires a point
 618  *    \___________/
 619  *
 620  */
 621
 622 static FsqMethod waggle;
 623
 624 static Change *waggle_allocate(Method *mm, int alloc_motions) {
 625   FsqMethod *m= (void*)mm;
 626   assert(m->f.ready>=0);
 627   return fsq_allocate(mm, alloc_motions);
 628 }
 629
 630 static void waggle_do(FsqMethod *m, const MovFeatInfo *mfi, int posn) {
 631   /* actually firing points, yay! */
 632   PicInsn piob;
 633   enco_pic_waggle(&piob, mfi->boob[0], posn);
 634   serial_transmit(&piob);
 635 }
 636
 637 static SegmentNum waggle_settle_seg;
 638 static int waggle_settle_feat;
 639
 640 static void waggle_settle_check(void) {
 641   for (;;) {
 642     if (waggle_settle_seg >= info_nsegments) return;
 643
 644     Segment *seg= &segments[waggle_settle_seg];
 645     if (waggle_settle_feat >= seg->i->n_movfeats) {
 646       waggle_settle_seg++; waggle_settle_feat=0; continue;
 647     }
 648
 649     const MovFeatInfo *feati= &seg->i->movfeats[waggle_settle_feat];
 650     if (feati->kind != mfk_relay) {
 651       waggle_settle_feat++; continue;
 652     }
 653
 654     waggle.f.ready= 0;
 655     waggle_do(&waggle, feati, (seg->movposcomb / feati->weight) & 1);
 656     waggle_settle_feat++;
 657   }
 658 }
 659
 660 void waggle_startup_manual(void) {
 661   waggle.f.ready= 1;
 662 }
 663
 664 void waggle_settle(void) {
 665   waggle_settle_seg= 0;
 666   waggle_settle_feat= 0;
 667   waggle.f.ready= 1;
 668   waggle_settle_check();
 669 }
 670
 671 void on_pic_waggled(const PicInsnInfo *pii, const PicInsn *pi, int objnum) {
 672   if (sta_state == Sta_Settling) {
 673     waggle.f.ready= 1;
 674     waggle_settle_check();
 675   } else if (sta_state >= Sta_Resolving || sta_state == Sta_Manual) {
 676     waggle.f.ready= 1;
 677     fsq_check_action(&waggle);
 678     return;
 679   }
 680 }
 681
 682 static FsqMethod waggle= {
 683   { "relay", mfk_relay,
 684     waggle_allocate, fsq_reserve, fsq_confirm,
 685     fsq_destroy, fsq_all_abandon },
 686   { .lag_ms= 5, .slot_ms= 50, .move= waggle_do }
 687 };
 688
 689 /*========== dummy `nomove' kind ==========*/
 690
 691 static Change *nomove_allocate(Method *m, int alloc_motions) {
 692   oprintf(DUPO("movfeatkind-momove") "allocate %d\n",alloc_motions);
 693   return mmalloc(sizeof(Change));
 694 }
 695 static ErrorCode nomove_reserve(Method *m, Change *c, Segment *move, int ms) {
 696   oprintf(DUPO("movfeatkind-nomove") "reserve\n");
 697   return 0;
 698 }
 699 static void nomove_destroy(Method *m, Change *c) { free(c); }
 700 static ErrorCode nomove_confirm(Method *m, Change *c, Segment *move, int n,
 701                        const Motion *motions, int ms) {
 702   oprintf(DUPO("movfeatkind-nomove") "confirm\n");
 703   nomove_destroy(m,c);
 704   return 0;
 705 }
 706
 707 static Method nomove_method= {
 708   "nomove", mfk_none, nomove_allocate, nomove_reserve, nomove_confirm,
 709   nomove_destroy, ignore_all_abandon
 710 };
 711
 712 /*========== method-independent machinery ==========*/
 713
 714 static Method *methods[]= {
 715   &nomove_method,
 716   (Method*)&points_method,
 717   (Method*)&waggle,
 718   0
 719 };
 720
 721 static Change *mp_allocate(Method *meth, Segment *move,
 722                            int alloc_motions, MovPosComb target) {
 723   assert(sta_state >= Sta_Resolving || sta_state == Sta_Manual);
 724   Change *chg= meth->allocate(meth, alloc_motions);
 725   chg->meth=      meth;
 726   chg->move=      move;
 727   chg->intent=    target;
 728   return chg;
 729 }
 730
 731 static int change_needed(const MovFeatInfo *feati,
 732                          MovPosComb startpoint, MovPosComb target) {
 733   int r;
 734   r= startpoint<0 ||
 735     (target - startpoint) / feati->weight % feati->posns;
 736   oprintf(DUPO("movpos/change-needed") "%s:%s(%d*%d) %d..%d => %d\n",
 737           methods[feati->kind]->pname, feati->pname,
 738           feati->posns, feati->weight,
 739           startpoint, target, r);
 740   return r;
 741 }
 742
 743 static int evaluate_target(Segment *move, MovPosComb target,
 744                            MovPosComb startpoint, MovFeatKind *kind_r) {
 745   /* returns number of features which have to change to reach target,
 746    * or -1 for mixed kinds.  kind_r may be 0. */
 747   const SegmentInfo *movei= move->i;
 748   int feat, tchanges;
 749   const MovFeatInfo *feati;
 750   MovFeatKind kind;
 751
 752   oprintf(DUPO("movpos/eval") "%s/%s <-%s\n", move->i->pname,
 753           movpos_pname(move,target), movpos_pname(move,startpoint));
 754
 755   if (startpoint<0) {
 756     startpoint= movpos_poscomb_actual(move);
 757     oprintf(DUPO("movpos/eval") "  actual <-%s\n",
 758             movpos_pname(move,startpoint));
 759   }
 760
 761   for (feat=0, feati=movei->movfeats, tchanges=0, kind= mfk_none;
 762        feat<movei->n_movfeats;
 763        feat++, feati++) {
 764     if (!change_needed(feati,startpoint,target)) continue;
 765     tchanges++;
 766     if (kind && feati->kind != kind) return -1;
 767     kind= feati->kind;
 768   }
 769
 770   if (kind_r) *kind_r= kind;
 771   oprintf(DUPO("movpos/eval") "changes=%d kind=%s\n",
 772           tchanges, methods[kind]->pname);
 773   return tchanges;
 774 }
 775
 776 ErrorCode movpos_findcomb_bysegs(Segment *back, Segment *move, Segment *fwd,
 777                                  MovPosComb startpoint, MovPosComb *chosen_r) {
 778   const SegmentInfo *movei= move->i;
 779   MovPosComb tcomb, bestcomb=-1;
 780   int tchanges, bestchanges=INT_MAX;
 781   const SegPosCombInfo *pci;
 782
 783   for (tcomb=0, pci=movei->poscombs;
 784        tcomb<movei->n_poscombs;
 785        tcomb++, pci++) {
 786     Segment *tback= &segments[pci->link[1].next];
 787     Segment *tfwd=  &segments[pci->link[0].next];
 788     if (back && !(back==tback || back==tfwd)) continue;
 789     if (fwd  && !(fwd ==tback || fwd ==tfwd)) continue;
 790
 791     /* we have to search for the one which is least effort */
 792     tchanges= evaluate_target(move,tcomb,startpoint,0);
 793
 794     if (tchanges==-1)
 795       /* mixed kinds */
 796       tchanges= INT_MAX-1;
 797
 798     if (tchanges >= bestchanges) /* prefer low-numbered movposcombs */
 799       continue;
 800
 801     bestcomb= tcomb;
 802     bestchanges= tchanges;
 803   }
 804   if (chosen_r) *chosen_r= bestcomb;
 805   return
 806     bestchanges==INT_MAX ? EC_MovFeatRouteNotFound :
 807     bestchanges==INT_MAX-1 ? EC_MovFeatKindsCombination :
 808     0;
 809 }
 810
 811 ErrorCode movpos_change(Segment *move, MovPosComb target,
 812                         int maxdelay_ms, MovPosChange *chg) {
 813   const SegmentInfo *movei= move->i;
 814   const MovFeatInfo *feati;
 815   int feat;
 816   MovPosComb actual;
 817   ErrorCode ec;
 818   MovFeatKind kind= mfk_none;
 819
 820   if (!move->moving) {
 821     actual= move->movposcomb;
 822     assert(!move->motion);
 823   } else {
 824     kind= move->motion->meth->kind;
 825     actual= move->motion->actual;
 826   }
 827
 828   oprintf(DUPO("movpos/change") "%s/%s maxdelay=%dms actual=%s\n",
 829           move->i->pname, movpos_pname(move,target),
 830           maxdelay_ms, movpos_pname(move, actual));
 831   if (chg) oprintf(DUPO("movpos/change") " chg=%s:%s/%s\n",
 832                    chg->meth->pname, chg->move->i->pname,
 833                    movpos_pname(chg->move, chg->intent));
 834
 835   { /* provide horizon for visibility of motions[] */
 836     int n_motions=0;
 837     Motion motions[movei->n_movfeats];
 838
 839     for (feat=0, feati=movei->movfeats;
 840          feat<movei->n_movfeats;
 841          feat++, feati++) {
 842       if (!change_needed(feati,actual,target))
 843         continue;
 844       MovPosComb posn= target / feati->weight % feati->posns;
 845       if (kind) {
 846         if (feati->kind != kind) { ec= EC_MovFeatKindsCombination; goto x; }
 847       } else {
 848         kind= feati->kind;
 849       }
 850       motions[n_motions].i= feati;
 851       motions[n_motions].posn= posn;
 852       n_motions++;
 853     }
 854
 855     Method *meth= methods[kind];
 856
 857     if (chg) {
 858       if (chg->meth != meth ||
 859           chg->move != move ||
 860           chg->intent != target)
 861         return EC_MovFeatReservationInapplicable;
 862     } else {
 863       chg= mp_allocate(meth,move,n_motions,target);
 864     }
 865     chg->actual= actual;
 866
 867     oprintf(DUPO("movpos/change") "confirm %s:%d...\n",
 868             meth->pname, n_motions);
 869     ec= meth->confirm(meth, chg, move, n_motions, motions, maxdelay_ms);
 870     oprintf(DUPO("movpos/change") "confirm => %s\n",errorcodelist[ec]);
 871     if (ec) goto x;
 872   }
 873   return 0;
 874
 875  x:
 876   movpos_unreserve(chg);
 877   return ec;
 878 }
 879
 880 ErrorCode
 881 movpos_reserve(Segment *move, int maxdelay_ms, MovPosChange **res_r,
 882                MovPosComb target, MovPosComb startpoint /*as for findcomb*/) {
 883   MovFeatKind kind= mfk_none;
 884   ErrorCode ec;
 885   int nchanges;
 886
 887   oprintf(DUPO("movpos/reserve") "%s/%s maxdelay=%dms startpoint=%s\n",
 888           move->i->pname, movpos_pname(move,target),
 889           maxdelay_ms, movpos_pname(move,startpoint));
 890
 891   nchanges= evaluate_target(move,target,startpoint,&kind);
 892   if (nchanges==-1) return EC_MovFeatKindsCombination;
 893
 894   Method *meth= methods[kind];
 895   oprintf(DUPO("movpos/reserve") "allocate %s:%d...\n",
 896           meth->pname, nchanges);
 897   Change *chg= mp_allocate(meth, move, nchanges, target);
 898   ec= meth->reserve(meth, chg, move, maxdelay_ms);
 899   oprintf(DUPO("movpos/reserve") "reserve => %s\n",errorcodelist[ec]);
 900   if (ec) goto x;
 901
 902   *res_r= chg;
 903   return 0;
 904
 905  x:
 906   movpos_unreserve(chg);
 907   return ec;
 908 }
 909
 910 void movpos_unreserve(MovPosChange *res) {
 911   if (!res) return;
 912   oprintf(DUPO("movpos/unreserve") "%s:%s/%s\n",
 913           res->meth->pname, res->move->i->pname,
 914           movpos_pname(res->move, res->intent));
 915   res->meth->destroy(res->meth, res);
 916 }
 917
 918 MovPosComb movpos_poscomb_actual(Segment *seg) {
 919  return seg->moving ? seg->motion->actual : seg->movposcomb;
 920 }
 921
 922 MovPosComb movpos_change_intent(MovPosChange *chg) {
 923   return chg->intent;
 924 }
 925
 926 void motions_all_abandon(void) {
 927   Method **meth;
 928   for (meth=methods; *meth; meth++)
 929     (*meth)->all_abandon(*meth);
 930 }