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