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 SegmentNum waggle_settle_seg;
 617 static int waggle_settle_feat;
 618
 619 static void waggle_settle_check(void) {
 620   for (;;) {
 621     if (waggle_settle_seg >= info_nsegments) return;
 622
 623     Segment *seg= &segments[waggle_settle_seg];
 624     if (waggle_settle_feat >= seg->i->n_movfeats) {
 625       waggle_settle_seg++; waggle_settle_feat=0; continue;
 626     }
 627
 628     const MovFeatInfo *feati= &seg->i->movfeats[waggle_settle_feat];
 629     if (feati->kind != mfk_relay) {
 630       waggle_settle_feat++; continue;
 631     }
 632
 633     waggle.f.ready= 0;
 634     waggle_do(&waggle, feati, (seg->movposcomb / feati->weight) & 1);
 635     waggle_settle_feat++;
 636   }
 637 }
 638
 639 void waggle_startup_manual(void) {
 640   waggle.f.ready= 1;
 641 }
 642
 643 void waggle_settle(void) {
 644   waggle_settle_seg= 0;
 645   waggle_settle_feat= 0;
 646   waggle.f.ready= 1;
 647   waggle_settle_check();
 648 }
 649
 650 void on_pic_waggled(const PicInsnInfo *pii, const PicInsn *pi, int objnum) {
 651   if (sta_state == Sta_Settling) {
 652     waggle.f.ready= 1;
 653     waggle_settle_check();
 654   } else if (sta_state >= Sta_Resolving || sta_state == Sta_Manual) {
 655     waggle.f.ready= 1;
 656     fsq_check_action(&waggle);
 657     return;
 658   }
 659 }
 660
 661 static FsqMethod waggle= {
 662   { "relay", mfk_relay,
 663     waggle_allocate, fsq_reserve, fsq_confirm,
 664     fsq_destroy, fsq_all_abandon },
 665   { .lag_ms= 5, .slot_ms= 50, .move= waggle_do }
 666 };
 667
 668 /*========== dummy `nomove' kind ==========*/
 669
 670 static Change *nomove_allocate(Method *m, int alloc_motions) {
 671   oprintf(DUPO("movfeatkind-momove") "allocate %d\n",alloc_motions);
 672   return mmalloc(sizeof(Change));
 673 }
 674 static ErrorCode nomove_reserve(Method *m, Change *c, Segment *move, int ms) {
 675   oprintf(DUPO("movfeatkind-nomove") "reserve\n");
 676   return 0;
 677 }
 678 static void nomove_destroy(Method *m, Change *c) { free(c); }
 679 static ErrorCode nomove_confirm(Method *m, Change *c, Segment *move, int n,
 680                        const Motion *motions, int ms) {
 681   oprintf(DUPO("movfeatkind-nomove") "confirm\n");
 682   nomove_destroy(m,c);
 683   return 0;
 684 }
 685
 686 static Method nomove_method= {
 687   "nomove", mfk_none, nomove_allocate, nomove_reserve, nomove_confirm,
 688   nomove_destroy, ignore_all_abandon
 689 };
 690
 691 /*========== method-independent machinery ==========*/
 692
 693 static Method *methods[]= {
 694   &nomove_method,
 695   (Method*)&points_method,
 696   (Method*)&waggle,
 697   0
 698 };
 699
 700 static Change *mp_allocate(Method *meth, Segment *move,
 701                            int alloc_motions, MovPosComb target) {
 702   assert(sta_state >= Sta_Resolving || sta_state == Sta_Manual);
 703   Change *chg= meth->allocate(meth, alloc_motions);
 704   chg->meth=      meth;
 705   chg->move=      move;
 706   chg->intent=    target;
 707   return chg;
 708 }
 709
 710 static int change_needed(const MovFeatInfo *feati,
 711                          MovPosComb startpoint, MovPosComb target) {
 712   int r;
 713   r= startpoint<0 ||
 714     (target - startpoint) / feati->weight % feati->posns;
 715   oprintf(DUPO("movpos/change-needed") "%s:%s(%d*%d) %d..%d => %d\n",
 716           methods[feati->kind]->pname, feati->pname,
 717           feati->posns, feati->weight,
 718           startpoint, target, r);
 719   return r;
 720 }
 721
 722 static int evaluate_target(Segment *move, MovPosComb target,
 723                            MovPosComb startpoint, MovFeatKind *kind_r) {
 724   /* returns number of features which have to change to reach target,
 725    * or -1 for mixed kinds.  kind_r may be 0. */
 726   const SegmentInfo *movei= move->i;
 727   int feat, tchanges;
 728   const MovFeatInfo *feati;
 729   MovFeatKind kind;
 730
 731   oprintf(DUPO("movpos/eval") "%s/%s <-%s\n",
 732           move->i->pname, posnpname(move,target), posnpname(move,startpoint));
 733
 734   if (startpoint<0) {
 735     startpoint= movpos_poscomb_actual(move);
 736     oprintf(DUPO("movpos/eval") "  actual <-%s\n",
 737             posnpname(move,startpoint));
 738   }
 739
 740   for (feat=0, feati=movei->movfeats, tchanges=0, kind= mfk_none;
 741        feat<movei->n_movfeats;
 742        feat++, feati++) {
 743     if (!change_needed(feati,startpoint,target)) continue;
 744     tchanges++;
 745     if (kind && feati->kind != kind) return -1;
 746     kind= feati->kind;
 747   }
 748
 749   if (kind_r) *kind_r= kind;
 750   oprintf(DUPO("movpos/eval") "changes=%d kind=%s\n",
 751           tchanges, methods[kind]->pname);
 752   return tchanges;
 753 }
 754
 755 ErrorCode movpos_findcomb_bysegs(Segment *back, Segment *move, Segment *fwd,
 756                                  MovPosComb startpoint, MovPosComb *chosen_r) {
 757   const SegmentInfo *movei= move->i;
 758   MovPosComb tcomb, bestcomb=-1;
 759   int tchanges, bestchanges=INT_MAX;
 760   const SegPosCombInfo *pci;
 761
 762   for (tcomb=0, pci=movei->poscombs;
 763        tcomb<movei->n_poscombs;
 764        tcomb++, pci++) {
 765     Segment *tback= &segments[pci->link[1].next];
 766     Segment *tfwd=  &segments[pci->link[0].next];
 767     if (back && !(back==tback || back==tfwd)) continue;
 768     if (fwd  && !(fwd ==tback || fwd ==tfwd)) continue;
 769
 770     /* we have to search for the one which is least effort */
 771     tchanges= evaluate_target(move,tcomb,startpoint,0);
 772
 773     if (tchanges==-1)
 774       /* mixed kinds */
 775       tchanges= INT_MAX-1;
 776
 777     if (tchanges >= bestchanges) /* prefer low-numbered movposcombs */
 778       continue;
 779
 780     bestcomb= tcomb;
 781     bestchanges= tchanges;
 782   }
 783   if (chosen_r) *chosen_r= bestcomb;
 784   return
 785     bestchanges==INT_MAX ? EC_MovFeatRouteNotFound :
 786     bestchanges==INT_MAX-1 ? EC_MovFeatKindsCombination :
 787     0;
 788 }
 789
 790 ErrorCode movpos_change(Segment *move, MovPosComb target,
 791                         int maxdelay_ms, MovPosChange *chg) {
 792   const SegmentInfo *movei= move->i;
 793   const MovFeatInfo *feati;
 794   int feat;
 795   MovPosComb actual;
 796   ErrorCode ec;
 797   MovFeatKind kind= mfk_none;
 798
 799   if (!move->moving) {
 800     actual= move->movposcomb;
 801     assert(!move->motion);
 802   } else {
 803     kind= move->motion->meth->kind;
 804     actual= move->motion->actual;
 805   }
 806
 807   oprintf(DUPO("movpos/change") "%s/%s maxdelay=%dms actual=%s\n",
 808           move->i->pname, posnpname(move,target),
 809           maxdelay_ms, posnpname(move, actual));
 810   if (chg) oprintf(DUPO("movpos/change") " chg=%s:%s/%s\n",
 811                    chg->meth->pname, chg->move->i->pname,
 812                    posnpname(chg->move, chg->intent));
 813
 814   { /* provide horizon for visibility of motions[] */
 815     int n_motions=0;
 816     Motion motions[movei->n_movfeats];
 817
 818     for (feat=0, feati=movei->movfeats;
 819          feat<movei->n_movfeats;
 820          feat++, feati++) {
 821       if (!change_needed(feati,actual,target))
 822         continue;
 823       MovPosComb posn= target / feati->weight % feati->posns;
 824       if (kind) {
 825         if (feati->kind != kind) { ec= EC_MovFeatKindsCombination; goto x; }
 826       } else {
 827         kind= feati->kind;
 828       }
 829       motions[n_motions].i= feati;
 830       motions[n_motions].posn= posn;
 831       n_motions++;
 832     }
 833
 834     Method *meth= methods[kind];
 835
 836     if (chg) {
 837       if (chg->meth != meth ||
 838           chg->move != move ||
 839           chg->intent != target)
 840         return EC_MovFeatReservationInapplicable;
 841     } else {
 842       chg= mp_allocate(meth,move,n_motions,target);
 843     }
 844     chg->actual= actual;
 845
 846     oprintf(DUPO("movpos/change") "confirm %s:%d...\n",
 847             meth->pname, n_motions);
 848     ec= meth->confirm(meth, chg, move, n_motions, motions, maxdelay_ms);
 849     oprintf(DUPO("movpos/change") "confirm => %s\n",errorcodelist[ec]);
 850     if (ec) goto x;
 851   }
 852   return 0;
 853
 854  x:
 855   movpos_unreserve(chg);
 856   return ec;
 857 }
 858
 859 ErrorCode
 860 movpos_reserve(Segment *move, int maxdelay_ms, MovPosChange **res_r,
 861                MovPosComb target, MovPosComb startpoint /*as for findcomb*/) {
 862   MovFeatKind kind= mfk_none;
 863   ErrorCode ec;
 864   int nchanges;
 865
 866   oprintf(DUPO("movpos/reserve") "%s/%s maxdelay=%dms startpoint=%s\n",
 867           move->i->pname, posnpname(move,target),
 868           maxdelay_ms, posnpname(move,startpoint));
 869
 870   nchanges= evaluate_target(move,target,startpoint,&kind);
 871   if (nchanges==-1) return EC_MovFeatKindsCombination;
 872
 873   Method *meth= methods[kind];
 874   oprintf(DUPO("movpos/reserve") "allocate %s:%d...\n",
 875           meth->pname, nchanges);
 876   Change *chg= mp_allocate(meth, move, nchanges, target);
 877   ec= meth->reserve(meth, chg, move, maxdelay_ms);
 878   oprintf(DUPO("movpos/reserve") "reserve => %s\n",errorcodelist[ec]);
 879   if (ec) goto x;
 880
 881   *res_r= chg;
 882   return 0;
 883
 884  x:
 885   movpos_unreserve(chg);
 886   return ec;
 887 }
 888
 889 void movpos_unreserve(MovPosChange *res) {
 890   if (!res) return;
 891   oprintf(DUPO("movpos/unreserve") "%s:%s/%s\n",
 892           res->meth->pname, res->move->i->pname,
 893           posnpname(res->move, res->intent));
 894   res->meth->destroy(res->meth, res);
 895 }
 896
 897 MovPosComb movpos_poscomb_actual(Segment *seg) {
 898  return seg->moving ? seg->motion->actual : seg->movposcomb;
 899 }
 900
 901 MovPosComb movpos_change_intent(MovPosChange *chg) {
 902   return chg->intent;
 903 }
 904
 905 void motions_all_abandon(void) {
 906   Method **meth;
 907   for (meth=methods; *meth; meth++)
 908     (*meth)->all_abandon(*meth);
 909 }