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   ouprintf("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   ouprintf("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, DP;
 254   FsqReq *whichr;
 255
 256   conf=resv=0;
 257   future=0;
 258
 259   DPRINTF1(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     DPRINTF2(" +");
 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     DPRINTF2(" %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);  DPRINTF2("~");
 276     } else if (confr) {
 277       WHICH(conf);
 278     } else {
 279       future++;     DPRINTF2("-");
 280       continue;
 281     }
 282     DPRINTF2("%s/%s[%d@t+%d]", whichr->h.move->i->pname,
 283             movpos_pname(whichr->h.move, whichr->h.intent),
 284             whichr->n_motions, whichwhen);
 285     if (future > whichwhen) {
 286       DPRINTF2("!...bad\n");
 287       return EC_MovFeatTooLate;
 288     }
 289     future += whichr->n_motions;
 290   }
 291   DPRINTF2(" ok\n");
 292   return 0;
 293 }
 294
 295 #undef WHICH
 296
 297 static ErrorCode fsq_enqueue(FsqMethod *m, FsqQueue *q,         /* XA -> X */
 298                              FsqReq *r) {        /* or on error,   XA -> A */
 299   int insat;                 /* ... where X is R or C and corresponds to q */
 300
 301   if (q->n == FSQ_MAX_QUEUE)
 302     return EC_BufferFull;
 303
 304   for (insat= q->n;
 305        insat>0 && (FsqSlotSigned)(r->deadline - q->l[insat-1]->deadline) < 0;
 306        insat--)
 307     q->l[insat]= q->l[insat-1];
 308   q->l[insat]= r;
 309   q->n++;
 310
 311   return fsq_check_plan(m);
 312   /* if this fails, indep machinery calls fsq_destroy which dequeues */
 313 }
 314
 315 /*---------- method entrypoints ----------*/
 316
 317 static Change *fsq_allocate(Method *mm, int alloc_motions) {
 318   FsqReq *r;
 319
 320   if (!alloc_motions)
 321     /* we need at least one motion in the table so we can tell
 322      *  the difference between the states by looking at motions[0].i */
 323     alloc_motions= 1;
 324
 325   r= mmalloc(sizeof(*r) + alloc_motions * sizeof(r->motions[0]));
 326   r->deadline= FSQDN;
 327   r->n_motions= alloc_motions;
 328   r->motions[0].i= 0;
 329   return (Change*)r;
 330 }
 331
 332 static ErrorCode fsq_reserve(Method *mm, Change *chg,
 333                              Segment *move, int maxdelay_ms) {
 334   FsqMethod *m= (void*)mm;
 335   FsqReq *r= (FsqReq*)chg;
 336   FsqSlotSigned reldeadline;
 337
 338   reldeadline= fsq_maxdelay_reldeadline(m, maxdelay_ms, r->n_motions);
 339   if (reldeadline <= 0) { fsq_mark_as_allocated(r); return EC_MovFeatTooLate; }
 340   r->deadline= reldeadline;
 341   return fsq_enqueue(m, &m->f.reserved, r);
 342 }
 343
 344 static ErrorCode fsq_confirm(Method *mm, Change *chg, Segment *move,
 345                              int n_motions, const Motion *motions,
 346                              int maxdelay_ms) {
 347   FsqMethod *m= (void*)mm;
 348   FsqReq *r= (FsqReq*)chg;
 349   FsqSlotSigned reldeadline;
 350   int allow_failure, DP;
 351   ErrorCode ec;
 352
 353   DPRINTF1(movpos,fsq, "%s confirm %s n=%d maxdelay=%dms",
 354            m->m.pname, move->i->pname, n_motions, maxdelay_ms);
 355
 356   assert(!r->motions[0].i); /* no confirming things already confirmed */
 357   if (r->deadline==FSQDN)
 358     DPRINTF2(" (alloc'd: %d)\n", r->n_motions);
 359   else
 360     DPRINTF2(" (res: %s/%s[%d@t+%d])\n",
 361              r->h.move->i->pname, movpos_pname(r->h.move, r->h.intent),
 362              r->n_motions, r->deadline);
 363
 364   /* If the segment is moving, these motions are already based on the
 365    * actual physical position which is stored in the existing request.
 366    * So we try removing the existing request from the queue and put
 367    * it back if it doesn't work.
 368    */
 369
 370   if (n_motions > r->n_motions)
 371     return EC_MovFeatReservationInapplicable;
 372   assert(n_motions <= r->n_motions);
 373   if (maxdelay_ms == -1) {
 374     reldeadline= r->deadline;
 375     if (reldeadline==FSQDN)
 376       reldeadline= fsq_maxdelay_reldeadline(m, -1, n_motions);
 377   } else {
 378     reldeadline= fsq_maxdelay_reldeadline(m, maxdelay_ms, n_motions);
 379   }
 380   allow_failure= reldeadline < (FsqSlotSigned)r->deadline;
 381   DPRINTF(movpos,fsq, "%s  reldeadline=[%d@t+%d] allow_failure=%d\n",
 382           m->m.pname, n_motions, reldeadline, allow_failure);
 383
 384   /* state A or R */
 385   fsq_dequeue(m, r);
 386                                            /* states of existing: */
 387   FsqReq *existing=
 388     move->moving ? (FsqReq*)move->motion : 0;   /* U or C */
 389   if (existing) {
 390     DPRINTF(movpos,fsq,
 391             "%s  existing %s n=%d deadline=t+%d\n",
 392             m->m.pname,
 393             existing->h.move->i->pname,
 394             existing->n_motions,
 395             existing->deadline - m->f.cslot);
 396     fsq_dequeue(m, existing);                   /* U or CA */
 397   }
 398
 399   /* state A or RA */
 400   memcpy(r->motions, motions, sizeof(r->motions[0])*n_motions);
 401   if (!n_motions) r->motions[0].i= move->i->movfeats;
 402   assert(r->motions[0].i);
 403   r->n_motions= n_motions;
 404   r->deadline= reldeadline + m->f.cslot;
 405
 406   if (n_motions == move->i->n_movfeats)
 407     r->actual= 0;
 408   else
 409     r->actual= chg->actual;
 410   assert(r->actual >= 0);
 411
 412   /* state CA */
 413   ec= fsq_enqueue(m, &m->f.confirmed, r);
 414   DPRINTF(movpos,fsq, "%s  fsq_enqueue=%s\n", m->m.pname, ec2str(ec));
 415   assert(allow_failure || !ec);
 416
 417   if (existing) {                                  /* CA */
 418     if (ec) { /* state C but bad */
 419       fsq_dequeue(m,r); /* state CA */
 420       fsq_mark_as_allocated(r); /* state A */
 421       ErrorCode ec_putback= fsq_enqueue(m,&m->f.confirmed, existing);
 422       assert(!ec_putback);                         /* C */
 423     } else { /* state C and good */
 424       free(existing);                              /* U */
 425     }
 426   }
 427   /* either  ec=0   state C                            U
 428    *     or  ec!=0  state A                            C
 429    *     or  ec!=0  state C but bad                    C
 430    */
 431
 432   if (ec) return ec;
 433
 434   move->moving= 1;
 435   move->motion= chg;
 436   move->movposcomb= -1;
 437   ouposn_moving(chg);
 438   fsq_check_action(m);
 439   return 0;
 440 }
 441
 442 static void fsq_destroy(Method *mm, Change *chg) { /* X->XA and then freed */
 443   FsqMethod *m= (void*)mm;
 444   FsqReq *r= (FsqReq*)chg;
 445   fsq_dequeue(m,r);
 446   free(r);
 447 }
 448
 449 /*---------- something to do ? ----------*/
 450
 451 static void fsq_check_action(FsqMethod *m) {
 452   /* client should call this after it sets ready */
 453   ErrorCode ec;
 454
 455   if (!m->f.confirmed.n) {
 456     if (sta_state == Sta_Finalising) resolve_motioncheck();
 457     return;
 458   }
 459
 460   FsqReq *r= m->f.confirmed.l[0];
 461
 462   if (r->n_motions && m->f.ready>0) {
 463     /* look for something to move */
 464     Motion *mo= &r->motions[--r->n_motions];
 465     assert(mo->posn < mo->i->posns);
 466     m->f.ready= 0;
 467     m->f.move(m, mo->i, mo->posn);
 468     ouprintf("movpos %s feat %s %d %s\n", r->h.move->i->pname,
 469              mo->i->pname, mo->posn, m->m.pname);
 470     m->f.cslot++;
 471
 472     r->actual= movposcomb_update_feature(r->actual,mo->i,mo->posn);
 473     if (r->h.actual >= 0 || !r->n_motions)
 474       r->h.actual= r->actual;
 475     ouposn_moving(&r->h);
 476   }
 477
 478   if (!r->n_motions) {
 479     /* look for something to report
 480      * we can get things here other than from the above
 481      * eg if we are asked to move the
 482      */
 483     Segment *move= r->h.move;
 484     assert(move->moving && move->motion == (Change*)r);
 485     fsq_queue_remove_index(&m->f.confirmed,0);
 486     fsq_mark_as_allocated(r); /* now state A aka Done */
 487     motion_done(move,r->h.actual);
 488     free(r);
 489     ec= fsq_check_plan(m);  assert(!ec);
 490     fsq_check_action(m);
 491   }
 492 }
 493
 494 /*---------- entrypoints from rest of program ----------*/
 495
 496 static void fsq_all_abandon(Method *mm) {
 497   FsqMethod *m= (void*)mm;
 498   int i;
 499
 500   assert(!m->f.reserved.n);
 501
 502   for (i=0; i<m->f.confirmed.n; i++) {
 503     FsqReq *r= m->f.confirmed.l[i];
 504     Segment *move= r->h.move;
 505     assert(move->motion == (Change*)r);
 506     motion_done(move,r->h.actual);
 507     free(r);
 508   }
 509   m->f.confirmed.n= 0;
 510 }
 511
 512 /*========== points ==========*/
 513
 514 /*
 515  * CDU and point queue states:
 516  *
 517  *
 518  *    ____________                              conf'd
 519  *   /   points_  \                 ready         .n
 520  *   |   all_      |
 521  *   |   abandon   |
 522  *   |             V
 523  *   |from       INACTIVE               -1      0
 524  *   |any       <=Sta_Settling
 525  *  ^^^^^^^^     (start)
 526  *                 |
 527  *     ___________ |turning
 528  *    /           \| _on
 529  *   |             V
 530  *   |           CHARGING               0       any
 531  *   |          >=Sta_Resolving
 532  *   |             |
 533  *   |             |on_pic
 534  *   |             |_charged
 535  *   |             V
 536  *   ^           READY                  1       any
 537  *   |             |
 538  *   |             |fsq_check_action
 539  *   |             | calls point_move which fires a point
 540  *    \___________/
 541  *
 542  */
 543
 544 #define CDU_RECHARGE   350 /*ms*/
 545 #define POINT_MOVEMENT  50 /*ms*/
 546
 547 static Change *point_allocate(Method *mm, int alloc_motions) {
 548   FsqMethod *m= (void*)mm;
 549   assert(m->f.ready>=0);
 550   return fsq_allocate(mm, alloc_motions);
 551 }
 552
 553 static void point_move(FsqMethod *m, const MovFeatInfo *mfi, int posn) {
 554   /* actually firing points, yay! */
 555   PicInsn piob;
 556   enco_pic_point(&piob, mfi->boob[posn]);
 557   serial_transmit(&piob);
 558 }
 559
 560 static void points_all_abandon(Method *mm) {
 561   FsqMethod *m= (void*)mm;
 562   m->f.ready= -1;
 563   fsq_all_abandon(mm);
 564 }
 565
 566 static FsqMethod points_method;
 567
 568 void points_turning_on(void) {
 569   FsqMethod *m= &points_method;
 570   m->f.ready= 0;
 571 }
 572 void on_pic_charged(const PicInsnInfo *pii, const PicInsn *pi, int objnum) {
 573   FsqMethod *m= &points_method;
 574   if (m->f.ready<0) return;
 575   m->f.ready= 1;
 576   fsq_check_action(m);
 577 }
 578
 579 static FsqMethod points_method= {
 580   { "point", mfk_point,
 581     point_allocate, fsq_reserve, fsq_confirm,
 582     fsq_destroy, points_all_abandon },
 583   { .lag_ms= POINT_MOVEMENT, .slot_ms= CDU_RECHARGE, .move= point_move }
 584 };
 585
 586 /*========== relays ==========*/
 587
 588 /*
 589  * Waggler states:
 590  *
 591  *    ____________                              conf'd
 592  *   /  wagglers_ \                 ready         .n
 593  *   |   all_      |
 594  *   |   abandon   |
 595  *   |             V
 596  *   |from       UNKNOWN                -1      0
 597  *   |any       <=Sta_Settling
 598  *  ^^^^^^^^     (start)
 599  *                 |
 600  *     ___________ |turning
 601  *    /           \| _on
 602  *   |             V
 603  *   |           CHARGING               0       any
 604  *   |          >=Sta_Resolving
 605  *   |             |
 606  *   |             |on_pic
 607  *   |             |_charged
 608  *   |             V
 609  *   ^           READY                  1       any
 610  *   |             |
 611  *   |             |fsq_check_action
 612  *   |             | calls point_move which fires a point
 613  *    \___________/
 614  *
 615  */
 616
 617 static FsqMethod waggle;
 618
 619 static Change *waggle_allocate(Method *mm, int alloc_motions) {
 620   FsqMethod *m= (void*)mm;
 621   assert(m->f.ready>=0);
 622   return fsq_allocate(mm, alloc_motions);
 623 }
 624
 625 static void waggle_do(FsqMethod *m, const MovFeatInfo *mfi, int posn) {
 626   /* actually firing points, yay! */
 627   PicInsn piob;
 628   enco_pic_waggle(&piob, mfi->boob[0], posn);
 629   serial_transmit(&piob);
 630 }
 631
 632 static SegmentNum waggle_settle_seg;
 633 static int waggle_settle_feat;
 634
 635 static void waggle_settle_check(void) {
 636   for (;;) {
 637     if (waggle_settle_seg >= info_nsegments) return;
 638
 639     Segment *seg= &segments[waggle_settle_seg];
 640     if (waggle_settle_feat >= seg->i->n_movfeats) {
 641       waggle_settle_seg++; waggle_settle_feat=0; continue;
 642     }
 643
 644     const MovFeatInfo *feati= &seg->i->movfeats[waggle_settle_feat];
 645     if (feati->kind != mfk_relay) {
 646       waggle_settle_feat++; continue;
 647     }
 648
 649     waggle.f.ready= 0;
 650     waggle_do(&waggle, feati, (seg->movposcomb / feati->weight) & 1);
 651     waggle_settle_feat++;
 652   }
 653 }
 654
 655 void waggle_startup_manual(void) {
 656   waggle.f.ready= 1;
 657 }
 658
 659 void waggle_settle(void) {
 660   waggle_settle_seg= 0;
 661   waggle_settle_feat= 0;
 662   waggle.f.ready= 1;
 663   waggle_settle_check();
 664 }
 665
 666 void on_pic_waggled(const PicInsnInfo *pii, const PicInsn *pi, int objnum) {
 667   if (sta_state == Sta_Settling) {
 668     waggle.f.ready= 1;
 669     waggle_settle_check();
 670   } else if (sta_state >= Sta_Resolving || sta_state == Sta_Manual) {
 671     waggle.f.ready= 1;
 672     fsq_check_action(&waggle);
 673     return;
 674   }
 675 }
 676
 677 static FsqMethod waggle= {
 678   { "relay", mfk_relay,
 679     waggle_allocate, fsq_reserve, fsq_confirm,
 680     fsq_destroy, fsq_all_abandon },
 681   { .lag_ms= 5, .slot_ms= 50, .move= waggle_do }
 682 };
 683
 684 /*========== dummy `nomove' kind ==========*/
 685
 686 static Change *nomove_allocate(Method *m, int alloc_motions) {
 687   DPRINTF(movpos,nomove, "allocate %d\n",alloc_motions);
 688   return mmalloc(sizeof(Change));
 689 }
 690 static ErrorCode nomove_reserve(Method *m, Change *c, Segment *move, int ms) {
 691   DPRINTF(movpos,nomove, "reserve\n");
 692   return 0;
 693 }
 694 static void nomove_destroy(Method *m, Change *c) { free(c); }
 695 static ErrorCode nomove_confirm(Method *m, Change *c, Segment *move, int n,
 696                        const Motion *motions, int ms) {
 697   DPRINTF(movpos,nomove, "confirm\n");
 698   nomove_destroy(m,c);
 699   return 0;
 700 }
 701
 702 static Method nomove_method= {
 703   "nomove", mfk_none, nomove_allocate, nomove_reserve, nomove_confirm,
 704   nomove_destroy, ignore_all_abandon
 705 };
 706
 707 /*========== method-independent machinery ==========*/
 708
 709 static Method *methods[]= {
 710   &nomove_method,
 711   (Method*)&points_method,
 712   (Method*)&waggle,
 713   0
 714 };
 715
 716 static Change *mp_allocate(Method *meth, Segment *move,
 717                            int alloc_motions, MovPosComb target) {
 718   assert(sta_state >= Sta_Resolving || sta_state == Sta_Manual);
 719   Change *chg= meth->allocate(meth, alloc_motions);
 720   chg->meth=      meth;
 721   chg->move=      move;
 722   chg->intent=    target;
 723   return chg;
 724 }
 725
 726 static int change_needed(const MovFeatInfo *feati,
 727                          MovPosComb startpoint, MovPosComb target) {
 728   int r;
 729   r= startpoint<0 ||
 730     (target - startpoint) / feati->weight % feati->posns;
 731   DPRINTF(movpos,changeneeded, "%s:%s(%d*%d) %d..%d => %d\n",
 732           methods[feati->kind]->pname, feati->pname,
 733           feati->posns, feati->weight,
 734           startpoint, target, r);
 735   return r;
 736 }
 737
 738 static int evaluate_target(Segment *move, MovPosComb target,
 739                            MovPosComb startpoint, MovFeatKind *kind_r) {
 740   /* returns number of features which have to change to reach target,
 741    * or -1 for mixed kinds.  kind_r may be 0. */
 742   const SegmentInfo *movei= move->i;
 743   int feat, tchanges;
 744   const MovFeatInfo *feati;
 745   MovFeatKind kind;
 746
 747   DPRINTF(movpos,eval, "%s/%s <-%s\n", move->i->pname,
 748           movpos_pname(move,target), movpos_pname(move,startpoint));
 749
 750   if (startpoint<0) {
 751     startpoint= movpos_poscomb_actual(move);
 752     DPRINTF(movpos,eval, "  actual <-%s\n",
 753             movpos_pname(move,startpoint));
 754   }
 755
 756   for (feat=0, feati=movei->movfeats, tchanges=0, kind= mfk_none;
 757        feat<movei->n_movfeats;
 758        feat++, feati++) {
 759     if (!change_needed(feati,startpoint,target)) continue;
 760     tchanges++;
 761     if (kind && feati->kind != kind) return -1;
 762     kind= feati->kind;
 763   }
 764
 765   if (kind_r) *kind_r= kind;
 766   DPRINTF(movpos,eval, "changes=%d kind=%s\n",
 767           tchanges, methods[kind]->pname);
 768   return tchanges;
 769 }
 770
 771 ErrorCode movpos_findcomb_bysegs(Segment *back, Segment *move, Segment *fwd,
 772                                  MovPosComb startpoint, MovPosComb *chosen_r) {
 773   const SegmentInfo *movei= move->i;
 774   MovPosComb tcomb, bestcomb=-1;
 775   int tchanges, bestchanges=INT_MAX;
 776   const SegPosCombInfo *pci;
 777
 778   for (tcomb=0, pci=movei->poscombs;
 779        tcomb<movei->n_poscombs;
 780        tcomb++, pci++) {
 781     Segment *tback= &segments[pci->link[1].next];
 782     Segment *tfwd=  &segments[pci->link[0].next];
 783     if (back && !(back==tback || back==tfwd)) continue;
 784     if (fwd  && !(fwd ==tback || fwd ==tfwd)) continue;
 785
 786     /* we have to search for the one which is least effort */
 787     tchanges= evaluate_target(move,tcomb,startpoint,0);
 788
 789     if (tchanges==-1)
 790       /* mixed kinds */
 791       tchanges= INT_MAX-1;
 792
 793     if (tchanges >= bestchanges) /* prefer low-numbered movposcombs */
 794       continue;
 795
 796     bestcomb= tcomb;
 797     bestchanges= tchanges;
 798   }
 799   if (chosen_r) *chosen_r= bestcomb;
 800   return
 801     bestchanges==INT_MAX ? EC_MovFeatRouteNotFound :
 802     bestchanges==INT_MAX-1 ? EC_MovFeatKindsCombination :
 803     0;
 804 }
 805
 806 ErrorCode movpos_change(Segment *move, MovPosComb target,
 807                         int maxdelay_ms, MovPosChange *chg) {
 808   const SegmentInfo *movei= move->i;
 809   const MovFeatInfo *feati;
 810   int feat;
 811   MovPosComb actual;
 812   ErrorCode ec;
 813   MovFeatKind kind= mfk_none;
 814
 815   if (!move->moving) {
 816     actual= move->movposcomb;
 817     assert(!move->motion);
 818   } else {
 819     kind= move->motion->meth->kind;
 820     actual= move->motion->actual;
 821   }
 822
 823   DPRINTF(movpos,change, "%s/%s maxdelay=%dms actual=%s\n",
 824           move->i->pname, movpos_pname(move,target),
 825           maxdelay_ms, movpos_pname(move, actual));
 826   if (chg) DPRINTF(movpos,change, " chg=%s:%s/%s\n",
 827                    chg->meth->pname, chg->move->i->pname,
 828                    movpos_pname(chg->move, chg->intent));
 829
 830   { /* provide horizon for visibility of motions[] */
 831     int n_motions=0;
 832     Motion motions[movei->n_movfeats];
 833
 834     for (feat=0, feati=movei->movfeats;
 835          feat<movei->n_movfeats;
 836          feat++, feati++) {
 837       if (!change_needed(feati,actual,target))
 838         continue;
 839       MovPosComb posn= target / feati->weight % feati->posns;
 840       if (kind) {
 841         if (feati->kind != kind) { ec= EC_MovFeatKindsCombination; goto x; }
 842       } else {
 843         kind= feati->kind;
 844       }
 845       motions[n_motions].i= feati;
 846       motions[n_motions].posn= posn;
 847       n_motions++;
 848     }
 849
 850     Method *meth= methods[kind];
 851
 852     if (chg) {
 853       if (chg->meth != meth ||
 854           chg->move != move ||
 855           chg->intent != target)
 856         return EC_MovFeatReservationInapplicable;
 857     } else {
 858       chg= mp_allocate(meth,move,n_motions,target);
 859     }
 860     chg->actual= actual;
 861
 862     DPRINTF(movpos,change, "confirm %s:%d...\n",
 863             meth->pname, n_motions);
 864     ec= meth->confirm(meth, chg, move, n_motions, motions, maxdelay_ms);
 865     DPRINTF(movpos,change, "confirm => %s\n",errorcodelist[ec]);
 866     if (ec) goto x;
 867   }
 868   return 0;
 869
 870  x:
 871   movpos_unreserve(chg);
 872   return ec;
 873 }
 874
 875 ErrorCode
 876 movpos_reserve(Segment *move, int maxdelay_ms, MovPosChange **res_r,
 877                MovPosComb target, MovPosComb startpoint /*as for findcomb*/) {
 878   MovFeatKind kind= mfk_none;
 879   ErrorCode ec;
 880   int nchanges;
 881
 882   DPRINTF(movpos,reserve, "%s/%s maxdelay=%dms startpoint=%s\n",
 883           move->i->pname, movpos_pname(move,target),
 884           maxdelay_ms, movpos_pname(move,startpoint));
 885
 886   nchanges= evaluate_target(move,target,startpoint,&kind);
 887   if (nchanges==-1) return EC_MovFeatKindsCombination;
 888
 889   Method *meth= methods[kind];
 890   DPRINTF(movpos,reserve, "allocate %s:%d...\n",
 891           meth->pname, nchanges);
 892   Change *chg= mp_allocate(meth, move, nchanges, target);
 893   ec= meth->reserve(meth, chg, move, maxdelay_ms);
 894   DPRINTF(movpos,reserve, "reserve => %s\n",errorcodelist[ec]);
 895   if (ec) goto x;
 896
 897   *res_r= chg;
 898   return 0;
 899
 900  x:
 901   movpos_unreserve(chg);
 902   return ec;
 903 }
 904
 905 void movpos_unreserve(MovPosChange *res) {
 906   if (!res) return;
 907   DPRINTF(movpos,unreserve, "%s:%s/%s\n",
 908           res->meth->pname, res->move->i->pname,
 909           movpos_pname(res->move, res->intent));
 910   res->meth->destroy(res->meth, res);
 911 }
 912
 913 MovPosComb movpos_poscomb_actual(Segment *seg) {
 914  return seg->moving ? seg->motion->actual : seg->movposcomb;
 915 }
 916
 917 MovPosComb movpos_change_intent(MovPosChange *chg) {
 918   return chg->intent;
 919 }
 920
 921 void motions_all_abandon(void) {
 922   Method **meth;
 923   for (meth=methods; *meth; meth++)
 924     (*meth)->all_abandon(*meth);
 925 }
 926
 927 MovPosComb movposcomb_update_feature(MovPosComb startpoint,
 928                                      const MovFeatInfo *mfi,
 929                                      int featpos) {
 930   MovPosComb above_weight= mfi->weight * mfi->posns;
 931   MovPosComb above= startpoint / above_weight;
 932   MovPosComb below= startpoint % mfi->weight;
 933   return above*above_weight + featpos*mfi->weight + below;
 934 }