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 typedef struct Change Change;
  16
  17 typedef struct MovPosChange {
  18   Segment *move;
  19   /* everything beyond here is private for indep */
  20   MovPosComb actual, target;
  21   int n_changes;
  22   Change *changes[];
  23 } Indep;
  24
  25 static void method_update_feature(Method*, MovPosChange*, const Motion *mo);
  26   /* Called from methods' execution logic when an individual feature
  27    * has been moved.  This is used by the method-independent code to
  28    * compute the correct delta set of movements from the current
  29    * actual position, when thinking about new plans.  It is also sent
  30    * to clients and ultimately used by the UI.
  31    */
  32
  33 static void method_change_done(Method *m, Change *chg);
  34   /* Called from methods' execution logic when a whole change
  35    * has been done.  The method-independent code will take care of
  36    * updating move->movposcomb. etc.
  37    *
  38    * REENTRANCY: must NOT be called from within a call to the method's
  39    * execute() (and of course cannot legally be called from within
  40    * prepare, consider, check or dispose).
  41    */
  42
  43
  44 /* Kind-independent code is responsible for determining
  45  * the method, doing a bit of cleanup, and adjusting the flow
  46  * slightly.  Per-kind code does the actual work and is mostly in
  47  * charge - it is also responsible for updating seg->moving and ->motion.
  48  */
  49 /*
  50  * The following states exist for each Method
  51  *   S  Starting     corresponds to global states other than Sta_Run
  52  *   T  Tentative    changes have been made but may yet be undone
  53  *   Y  Yes          proposed changes have been checked and are OK
  54  *   E  Executing    the method is executing
  55  *
  56  * The following states exist for each Change
  57  * at points when control flow  passes between kind and indep:
  58  *   U  Unallocated  no memory allocated (Change does not exist)
  59  *   P  Prepared     memory allocation done, basics filled in
  60  *   I  Installed    reservation or confirmation successful
  61  *   D  Done         motion is complete and callback is being entered
  62  *   G  Garbage      motion is complete and callback is being exited
  63  *
  64  * Changes may be for:
  65  *   R  Reservation
  66  *   C  Confirmation
  67  *
  68  * No Changes remain Prepared while the Method is Executing.
  69  */
  70
  71 struct Change {                    /* valid in:  filled in by and when:      */
  72   Method *meth;                    /*  PIDG       indep after prepare()      */
  73   MovPosChange *indep;             /*  PID        indep after prepare()      */
  74   unsigned installed:1; /* private for indep */
  75   /* kind-specific data follows */ /*  varies     kind-specific code, varies */
  76 };
  77
  78 struct Method {
  79   const char *pname;
  80
  81   ErrorCode (*prepare)(Method *m,                                 /* TYE->T */
  82                        const Segment *move,
  83                        int n_motions, const Motion *motions,
  84                        int ms, int confirmation,
  85                        Change **chg_r,          /* 0->P; err: 0->0; may be 0 */
  86                        int *cost_r /* may be 0 */);
  87   void (*dispose)(Method *m,                                       /* TY->TY */
  88                   Change*);                                          /* P->U */
  89
  90   ErrorCode (*install)(Method *m,                   /* TYE->T; err: TYE->TYY */
  91                        Change *inst);                     /* P->I; err: P->P */
  92       /* error: ET->T, no change to Changes */
  93   void (*remove)(Method *m,                                      /* TYE->TYY */
  94                  Change *remove);                                    /* I->P */
  95       /* error: ET->T, no change to Changes */
  96
  97   ErrorCode (*check)(Method *m);                    /* TYE->Y; err: TYE->TYE */
  98   void (*execute)(Method *m);                                       /* EY->E */
  99   void (*all_abandon)(Method *m);                                  /* TYE->S */
 100
 101   unsigned needcheck, needexec; /* used by indep to track T/Y/E */
 102 };
 103
 104 /*========== general utility functions ==========*/
 105
 106 const char *movpos_pname(const Segment *move, MovPosComb poscomb) {
 107   return !SOMEP(poscomb) ? "?" : move->i->poscombs[poscomb].pname;
 108 }
 109
 110 static void ouposn_moving(const MovPosChange *indep) {
 111   Segment *move= indep->move;
 112   ouprintf("movpos %s position %s moving\n",
 113            move->i->pname, movpos_pname(move, indep->actual));
 114 }
 115
 116 MovPosComb movposcomb_update_feature(MovPosComb startpoint,
 117                                      const MovFeatInfo *mfi,
 118                                      int featpos) {
 119   MovPosComb above_weight= mfi->weight * mfi->posns;
 120   MovPosComb above= startpoint / above_weight;
 121   MovPosComb below= startpoint % mfi->weight;
 122   return above*above_weight + featpos*mfi->weight + below;
 123 }
 124
 125 MovPosComb movpos_poscomb_actual(const Segment *seg) {
 126   return seg->moving ? seg->motion->actual : seg->movposcomb;
 127 }
 128
 129 static void ignore_all_abandon(Method *m) { }
 130
 131 /*========== points and other fixed timeslot movfeats ==========*/
 132
 133 /*
 134  * We maintain two queues, one for reserved one for actually confirmed
 135  *  requests where we know what we're doing.
 136  *
 137  * We divide time into discrete slots, numbered with clock arithmetic.
 138  *
 139  *     cslot         cslot+1      cslot+2
 140  *
 141  *     currently     next in      after
 142  *     changing      line         that
 143  *
 144  * We increment cslot when we consider the movfeat to move;
 145  * for points and relays this is when we issue the command to the PIC.
 146  * In a request, the deadline represents the latest allowable value
 147  * of cslot just before that increment.
 148  */
 149
 150 typedef unsigned FsqSlot;
 151 typedef int FsqSlotSigned;
 152
 153 #define FSQDN (~(FsqSlot)0)
 154
 155 typedef struct {   /* state      Prep Resv  Inst Resv  Prep Conf  Inst Conf   */
 156   Change h;        /* in queue?  absent     reserved   absent     confirmed   */
 157   FsqSlot deadline; /*           relative   relative   absolute   absolute    */
 158   int n_motions;    /*            1          1         num undone num undone  */
 159   Motion motions[]; /*  [0].i:    0          0         non-0      non-0       */
 160                     /*  .posn:    undef      undef     defined    defined     */
 161 } FsqReq;
 162
 163 #define FSQ_MAX_QUEUE  15
 164
 165 typedef struct {
 166   int n;
 167   FsqReq *l[FSQ_MAX_QUEUE];
 168 } FsqQueue;
 169
 170 typedef struct FsqMethod FsqMethod;
 171
 172 typedef struct {
 173   /* set by fsq's client before fsq_init */
 174   int slot_ms, lag_ms;
 175   void (*move)(FsqMethod *m, const MovFeatInfo *mfi, int movfeatposn);
 176   /* shared */
 177   int ready; /* >0 means ready; set to 0 by fsq just before move */
 178   /* fsq's client must arrange that these start out at all-bits-zero */
 179   FsqSlot cslot;
 180   FsqQueue confirmed, reserved;
 181 } FsqState;
 182
 183 struct FsqMethod {
 184   Method m;
 185   FsqState f;
 186   /* client may put things here */
 187 };
 188
 189 static void fsq_check_action(FsqMethod *m);
 190 static ErrorCode fsq_check_plan(FsqMethod *m);
 191
 192 static FsqSlotSigned fsq_maxdelay_reldeadline(FsqMethod *m,
 193                                               int maxdelay_ms, int n_motions) {
 194   if (maxdelay_ms==-1) return FSQ_MAX_QUEUE*16;
 195   return (maxdelay_ms - m->f.lag_ms) / m->f.slot_ms - n_motions;
 196 }
 197
 198 static void fsq_queue_remove_index(FsqQueue *q, int index) {
 199   q->n--;
 200   memmove(&q->l[index], &q->l[index+1], sizeof(q->l[0]) * (q->n - index));
 201 }
 202
 203 static int fsq_req_compar_approx(const void *av, const void *bv) {
 204   FsqReq *const *a= av;
 205   FsqReq *const *b= av;
 206   FsqSlotSigned dldiff= (*b)->deadline - (*a)->deadline;
 207   return dldiff;
 208 }
 209
 210 static void fsq_queue_remove_item(FsqQueue *q, FsqReq *r) {
 211   FsqReq **entry;
 212   int i;
 213   entry= bsearch(r, q->l, q->n, sizeof(q->l[0]), fsq_req_compar_approx);
 214   assert(entry);
 215
 216 #define SEARCH_CHECK do{                                \
 217     if (q->l[i] == r) goto found;                       \
 218     if (q->l[i]->deadline != r->deadline) break;        \
 219 }while(0)
 220   for(i= entry - q->l;     i >= 0;   i--)  SEARCH_CHECK;
 221   for(i= entry - q->l + 1; i < q->n; i++)  SEARCH_CHECK;
 222   abort();
 223
 224  found:
 225   fsq_queue_remove_index(q, i);
 226 }
 227
 228 static FsqQueue *fsq_item_queue(FsqMethod *m, FsqReq *r)
 229   { return r->motions[0].i ? &m->f.confirmed : &m->f.reserved; }
 230
 231 #define WHICH(wh)                               \
 232   (whichr= wh##r,                               \
 233    whichwhen= wh##when,                         \
 234    wh++)
 235
 236 static ErrorCode fsq_check_plan(FsqMethod *m) {
 237   /* Checks whether we can meet the currently queued commitments */
 238   /* if this fails, indep machinery calls fsq_prepare to dequeue */
 239   int future, conf, resv, whichwhen, DP;
 240   FsqReq *whichr;
 241
 242   conf=resv=0;
 243   future=0;
 244
 245   DPRINTF1(movpos,fsq, "%s   plan", m->m.pname);
 246
 247   /* If CDU isn't charged we can't do one right away */
 248   if (m->f.ready<0) {
 249     DPRINTF2(" +");
 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     DPRINTF2(" %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);  DPRINTF2("~");
 262     } else if (confr) {
 263       WHICH(conf);
 264     } else {
 265       future++;     DPRINTF2("-");
 266       continue;
 267     }
 268     DPRINTF2("%s/%s[%d@t+%d]", whichr->h.indep->move->i->pname,
 269             movpos_pname(whichr->h.indep->move, whichr->h.indep->target),
 270             whichr->n_motions, whichwhen);
 271     if (future > whichwhen) {
 272       DPRINTF2("!...bad\n");
 273       return EC_MovFeatTooLate;
 274     }
 275     future += whichr->n_motions;
 276   }
 277   DPRINTF2(" ok\n");
 278   return 0;
 279 }
 280
 281 #undef WHICH
 282
 283 static ErrorCode fsq_queue_insert_item(FsqMethod *m, FsqQueue *q, FsqReq *r) {
 284   int insat;
 285
 286   if (q->n == FSQ_MAX_QUEUE)
 287     return EC_BufferFull;
 288
 289   for (insat= q->n;
 290        insat>0 && (FsqSlotSigned)(r->deadline - q->l[insat-1]->deadline) < 0;
 291        insat--)
 292     q->l[insat]= q->l[insat-1];
 293   q->l[insat]= r;
 294   q->n++;
 295
 296   return 0;
 297 }
 298
 299 static void fsq_item_debug(FsqMethod *m, FsqReq *r, const char *opwhat) {
 300   int DP;
 301   Segment *move= r->h.indep->move;
 302   DPRINTF1(movpos,fsq, "%s %s %s", m->m.pname, opwhat, move->i->pname);
 303   if (r->motions[0].i) {
 304     int i;
 305     Motion *mo;
 306     for (i=0, mo=r->motions; i<r->n_motions; i++, mo++)
 307       DPRINTF2("/%s%d", mo->i->pname, (int)mo->posn);
 308     DPRINTF2("\n");
 309   } else {
 310     DPRINTF2("(%d)\n", r->n_motions);
 311   }
 312 }
 313
 314 static ErrorCode fsq_enqueue(FsqMethod *m, FsqReq *r) { /* P->I; err: P->P */
 315   if (!r) return 0;
 316   fsq_item_debug(m,r,"enqueue");
 317   return fsq_queue_insert_item(m, fsq_item_queue(m,r), r);
 318 }
 319
 320 static void fsq_dequeue(FsqMethod *m, FsqReq *r) { /* I->P */
 321   if (!r) return;
 322   fsq_item_debug(m,r,"dequeue");
 323   fsq_queue_remove_item(fsq_item_queue(m,r), r);
 324 }
 325
 326 /*---------- method entrypoints ----------*/
 327
 328 static ErrorCode fsq_prepare(Method *mm, const Segment *move,
 329                              int n_motions, const Motion *motions,
 330                              int ms, int confirmation,
 331                              Change **chg_r, int *cost_r) {
 332   FsqMethod *m= (void*)mm;
 333
 334   assert(n_motions > 0);
 335
 336   FsqSlotSigned reldeadline= fsq_maxdelay_reldeadline(m, ms, n_motions);
 337   if (reldeadline <= 0) return EC_MovFeatTooLate;
 338
 339   if (chg_r) {
 340     int alloc_motions= confirmation ? n_motions : 1;
 341       /* we need at least one motion in the table so we can tell
 342        *  the difference between the states by looking at motions[0].i */
 343     FsqReq *r= mmalloc(sizeof(*r) + alloc_motions * sizeof(r->motions[0]));
 344     r->n_motions= n_motions;
 345     if (confirmation) {
 346       r->deadline= reldeadline + m->f.cslot;
 347       memcpy(r->motions, motions, sizeof(*r->motions)*n_motions);
 348     } else {
 349       r->deadline= reldeadline;
 350       r->motions[0].i= 0;
 351     }
 352     *chg_r= &r->h;
 353   }
 354   if (cost_r) {
 355     *cost_r= n_motions * m->f.slot_ms;
 356   }
 357
 358   return 0;
 359 }
 360
 361 static void fsq_dispose(Method *mm, Change *chg) {
 362   FsqReq *r= (FsqReq*)chg;
 363   free(r);
 364 }
 365
 366 static void fsq_remove(Method *mm, Change *remvchg) {
 367   FsqMethod *m= (void*)mm;
 368   FsqReq *remv= (FsqReq*)remvchg;
 369
 370   fsq_dequeue(m, remv);
 371 }
 372
 373 static ErrorCode fsq_install(Method *mm, Change *instchg) {
 374   FsqMethod *m= (void*)mm;
 375   FsqReq *inst= (FsqReq*)instchg;
 376
 377   return fsq_enqueue(m, inst);
 378 }
 379
 380 static ErrorCode fsq_check(Method *mm) {
 381   FsqMethod *m= (void*)mm;
 382   ErrorCode ec= fsq_check_plan(m);
 383   return ec;
 384 }
 385
 386 static void fsq_execute(Method *mm) {
 387   FsqMethod *m= (void*)mm;
 388   fsq_check_action(m);
 389 }
 390
 391 /*---------- something to do ? ----------*/
 392
 393 static void fsq_check_action(FsqMethod *m) {
 394   /* client should call this after it sets ready */
 395   ErrorCode ec;
 396
 397   if (!m->f.confirmed.n) {
 398     if (sta_state == Sta_Finalising) resolve_motioncheck();
 399     return;
 400   }
 401
 402   FsqReq *r= m->f.confirmed.l[0];
 403
 404   if (r->n_motions && m->f.ready>0) {
 405     /* look for something to move */
 406     Motion *mo= &r->motions[--r->n_motions];
 407     assert(mo->posn < mo->i->posns);
 408     m->f.ready= 0;
 409     m->f.move(m, mo->i, mo->posn);
 410     m->f.cslot++;
 411
 412     method_update_feature(&m->m, r->h.indep, mo);
 413   }
 414
 415   if (!r->n_motions) {
 416     fsq_queue_remove_index(&m->f.confirmed, 0);
 417     method_change_done(&m->m, &r->h);
 418     m->m.dispose(&m->m, &r->h);
 419
 420     ec= fsq_check_plan(m);  assert(!ec);
 421     fsq_check_action(m);
 422   }
 423 }
 424
 425 /*========== points ==========*/
 426
 427 /*
 428  * CDU and point queue states:
 429  *
 430  *
 431  *    ____________                              conf'd
 432  *   /   points_  \                 ready         .n
 433  *   |   all_      |
 434  *   |   abandon   |
 435  *   |             V
 436  *   |from       INACTIVE               -1      0
 437  *   |any       <=Sta_Settling
 438  *  ^^^^^^^^     (start)
 439  *                 |
 440  *     ___________ |turning
 441  *    /           \| _on
 442  *   |             V
 443  *   |           CHARGING               0       any
 444  *   |          >=Sta_Resolving
 445  *   |             |
 446  *   |             |on_pic
 447  *   |             |_charged
 448  *   |             V
 449  *   ^           READY                  1       any
 450  *   |             |
 451  *   |             |fsq_check_action
 452  *   |             | calls point_move which fires a point
 453  *    \___________/
 454  *
 455  */
 456
 457 #define CDU_RECHARGE   350 /*ms*/
 458 #define POINT_MOVEMENT  50 /*ms*/
 459
 460 static ErrorCode point_prepare(Method *mm, const Segment *move,
 461                                int n_motions, const Motion *motions,
 462                                int ms, int confirmation,
 463                                Change **chg_r, int *cost_r) {
 464   FsqMethod *m= (void*)mm;
 465   assert(m->f.ready>=0);
 466   return fsq_prepare(mm,move, n_motions,motions,
 467                      ms,confirmation, chg_r,cost_r);
 468  }
 469
 470 static void point_move(FsqMethod *m, const MovFeatInfo *mfi, int posn) {
 471   /* actually firing points, yay! */
 472   PicInsn piob;
 473   enco_pic_point(&piob, mfi->boob[posn]);
 474   serial_transmit(&piob);
 475 }
 476
 477 static void points_all_abandon(Method *mm) {
 478   FsqMethod *m= (void*)mm;
 479   m->f.ready= -1;
 480 }
 481
 482 static FsqMethod points_method;
 483
 484 void points_turning_on(void) {
 485   FsqMethod *m= &points_method;
 486   m->f.ready= 0;
 487 }
 488 void on_pic_charged(const PicInsnInfo *pii, const PicInsn *pi, int objnum) {
 489   FsqMethod *m= &points_method;
 490   if (m->f.ready<0) return;
 491   m->f.ready= 1;
 492   fsq_check_action(m);
 493 }
 494
 495 static FsqMethod points_method= {
 496   { "point",
 497     point_prepare, fsq_dispose,
 498     fsq_install, fsq_remove,
 499     fsq_check, fsq_execute, points_all_abandon },
 500   { .lag_ms= POINT_MOVEMENT, .slot_ms= CDU_RECHARGE, .move= point_move }
 501 };
 502
 503 /*========== relays ==========*/
 504
 505 /*
 506  * Waggler states:
 507  *
 508  *    ____________                              conf'd
 509  *   /  wagglers_ \                 ready         .n
 510  *   |   all_      |
 511  *   |   abandon   |
 512  *   |             V
 513  *   |from       UNKNOWN                -1      0
 514  *   |any       <=Sta_Settling
 515  *  ^^^^^^^^     (start)
 516  *                 |
 517  *     ___________ |turning
 518  *    /           \| _on
 519  *   |             V
 520  *   |           CHARGING               0       any
 521  *   |          >=Sta_Resolving
 522  *   |             |
 523  *   |             |on_pic
 524  *   |             |_charged
 525  *   |             V
 526  *   ^           READY                  1       any
 527  *   |             |
 528  *   |             |fsq_check_action
 529  *   |             | calls waggle_do which switches a relay
 530  *    \___________/
 531  *
 532  */
 533
 534 static FsqMethod waggle;
 535
 536 static void waggle_do(FsqMethod *m, const MovFeatInfo *mfi, int posn) {
 537   /* actually setting relays */
 538   PicInsn piob;
 539   enco_pic_waggle(&piob, mfi->boob[0], posn);
 540   serial_transmit(&piob);
 541 }
 542
 543 static SegmentNum waggle_settle_seg;
 544 static int waggle_settle_feat;
 545
 546 static void waggle_settle_check(void) {
 547   for (;;) {
 548     if (waggle_settle_seg >= info_nsegments) return;
 549
 550     Segment *seg= &segments[waggle_settle_seg];
 551     if (waggle_settle_feat >= seg->i->n_movfeats) {
 552       waggle_settle_seg++; waggle_settle_feat=0; continue;
 553     }
 554
 555     const MovFeatInfo *feati= &seg->i->movfeats[waggle_settle_feat];
 556     if (feati->kind != mfk_relay) {
 557       waggle_settle_feat++; continue;
 558     }
 559
 560     waggle.f.ready= 0;
 561     waggle_do(&waggle, feati, (seg->movposcomb / feati->weight) & 1);
 562     waggle_settle_feat++;
 563   }
 564 }
 565
 566 void waggle_startup_manual(void) {
 567   waggle.f.ready= 1;
 568 }
 569
 570 void waggle_settle(void) {
 571   waggle_settle_seg= 0;
 572   waggle_settle_feat= 0;
 573   waggle.f.ready= 1;
 574   waggle_settle_check();
 575 }
 576
 577 void on_pic_waggled(const PicInsnInfo *pii, const PicInsn *pi, int objnum) {
 578   if (sta_state == Sta_Settling) {
 579     waggle.f.ready= 1;
 580     waggle_settle_check();
 581   } else if (sta_state >= Sta_Resolving || sta_state == Sta_Manual) {
 582     waggle.f.ready= 1;
 583     fsq_check_action(&waggle);
 584     return;
 585   }
 586 }
 587
 588 static FsqMethod waggle= {
 589   { "relay",
 590     fsq_prepare, fsq_dispose,
 591     fsq_install, fsq_remove,
 592     fsq_check, fsq_execute, ignore_all_abandon },
 593   { .lag_ms= 5, .slot_ms= 50, .move= waggle_do }
 594 };
 595
 596 /*========== dummy `nomove' kind ==========*/
 597
 598 typedef struct NomoveChange {
 599   Change h;
 600   DLIST_NODE(struct NomoveChange) inqueue;
 601   int n_motions; /* 0 for reservations */
 602   Motion motions[];
 603 } NomoveChange;
 604
 605 typedef struct {
 606   Method m;
 607   unsigned eventqueued:1;
 608   NomoveChange *queuehead; /* contains confirmations only */
 609 } NomoveMethod;
 610
 611 static ErrorCode nomove_prepare(Method *meth_in, const Segment *move,
 612                                 int n_motions, const Motion *motions,
 613                                 int ms, int confirming,
 614                                 Change **chg_r, int *cost_r) {
 615   if (chg_r) {
 616     NomoveChange *chg;
 617     assert(n_motions>0);
 618     if (!confirming) n_motions= 0;
 619     chg= mmalloc(sizeof(*chg) + sizeof(Motion)*n_motions);
 620     chg->n_motions= n_motions;
 621     memcpy(chg->motions, motions, sizeof(Motion)*n_motions);
 622   }
 623   if (cost_r) {
 624     *cost_r= 0;
 625   }
 626   return 0;
 627 }
 628 static void nomove_dispose(Method *mm, Change *remvchg) {
 629   NomoveChange *remv= (void*)remvchg;
 630   free(remv);
 631 }
 632
 633 static ErrorCode nomove_install(Method *mm, Change *instchg) {
 634   NomoveMethod *meth= (void*)mm;
 635   NomoveChange *inst= (void*)instchg;
 636   if (inst->n_motions)
 637     DLIST1_PREPEND(meth->queuehead, inst, inqueue);
 638   return 0;
 639 }
 640 static void nomove_remove(Method *mm, Change *remvchg) {
 641   NomoveMethod *meth= (void*)mm;
 642   NomoveChange *remv= (void*)remvchg;
 643   if (remv->n_motions)
 644     DLIST1_REMOVE(meth->queuehead, remv, inqueue);
 645 }
 646
 647 static ErrorCode nomove_check(Method *mm) { return 0; }
 648
 649 static void *nomove_execute_now(oop_source *source, struct timeval tv,
 650                                 void *meth_v) {
 651   NomoveMethod *meth= meth_v;
 652   meth->eventqueued= 0;
 653   NomoveChange *done;
 654
 655   while ((done= meth->queuehead)) {
 656     assert(done->n_motions);
 657     int i;
 658     for (i=0; i<done->n_motions; i++)
 659       method_update_feature(&meth->m, done->h.indep, &done->motions[i]);
 660     method_change_done(&meth->m, &done->h);
 661     DLIST1_REMOVE(meth->queuehead, done, inqueue);
 662     nomove_dispose(&meth->m, &done->h);
 663   }
 664   return OOP_CONTINUE;
 665 }
 666
 667 static void nomove_execute(Method *mm) {
 668   NomoveMethod *meth= (void*)mm;
 669   if (!meth->eventqueued) {
 670     meth->eventqueued= 1;
 671     events->on_time(events, OOP_TIME_NOW, nomove_execute_now, meth);
 672   }
 673 }
 674
 675 static Method nomove_method= {
 676   "nomove",
 677   nomove_prepare, nomove_dispose,
 678   nomove_install, nomove_remove,
 679   nomove_check, nomove_execute, ignore_all_abandon
 680 };
 681
 682 /*========== method-independent machinery ==========*/
 683
 684 #define INDEP_DBG_FMT "<%p:%s/%s[%d]>"
 685 #define INDEP_DBG_ARGS(in) (in),                                        \
 686     ((in)->move->i->pname), (movpos_pname((in)->move, (in)->target)),   \
 687     ((in)->n_changes)
 688
 689 #define INDEP_DPFX_FMT "movpos " INDEP_DBG_FMT " "
 690 #define INDEP_DPFX_ARGS(in) INDEP_DBG_ARGS((in))
 691
 692 #define METH_DPFX_FMT "%s " INDEP_DBG_FMT " "
 693 #define METH_DPFX_ARGS(indep, meth) ((meth).pname), INDEP_DBG_ARGS((indep))
 694
 695 static void indep_indep_done(Indep *indep);
 696
 697 static Method *methods[]= {
 698   [mfk_none] = (Method*)&nomove_method,
 699   [mfk_point] = (Method*)&points_method,
 700   [mfk_relay] = (Method*)&waggle,
 701   0
 702 };
 703
 704 /*---------- entrypoints from methods ----------*/
 705
 706 static void method_update_feature(Method *m, MovPosChange *indep,
 707                                   const Motion *mo) {
 708   ouprintf("movpos %s feat %s %d %s\n", indep->move->i->pname,
 709            mo->i->pname, mo->posn, m->pname);
 710   if (SOMEP(indep->actual))
 711     indep->actual=
 712       movposcomb_update_feature(indep->actual, mo->i, mo->posn);
 713   ouposn_moving(indep);
 714 }
 715
 716 static void method_change_done(Method *m, Change *chg) {
 717   Indep *indep= chg->indep;
 718   Change **search;
 719
 720   DPRINTF(movpos,meth, METH_DPFX_FMT "method_change_done...\n",
 721           METH_DPFX_ARGS(indep,*m));
 722
 723   for (search=indep->changes; *search; search++)
 724     if ((*search) == chg) goto found;
 725   assert(!"change in indep");
 726
 727  found:
 728   indep->n_changes--;
 729   if (indep->n_changes) {
 730     *search= indep->changes[indep->n_changes];
 731     return;
 732   }
 733
 734   indep_indep_done(indep);
 735 }
 736
 737 static void indep_indep_done(Indep *indep) {
 738   /* all done */
 739   Segment *move= indep->move;
 740   move->moving= 0;
 741   move->motion= 0;
 742   move->movposcomb= indep->target;
 743   ouprintf("movpos %s position %s stable\n",
 744            move->i->pname, movpos_pname(move, move->movposcomb));
 745   free(indep);
 746 }
 747
 748 /*---------- internal core machinery ----------*/
 749
 750 static Method *feature_method(const MovFeatInfo *feati) {
 751   assert(feati->kind >= 0);
 752   assert(feati->kind < ARRAY_SIZE(methods));
 753   Method *meth= methods[feati->kind];
 754   assert(meth);
 755   return meth;
 756 }
 757
 758 static int change_needed(const MovFeatInfo *feati,
 759                          MovPosComb startpoint, MovPosComb target) {
 760   int r;
 761   r= !SOMEP(startpoint) ||
 762     (target / feati->weight) % feati->posns -
 763     (startpoint / feati->weight) % feati->posns;
 764   if (DEBUGP(movpos,eval))
 765     DPRINTFA(" { %s:%s(%d*%d) %d..%d => %d }",
 766              feature_method(feati)->pname, feati->pname,
 767              feati->posns, feati->weight,
 768              startpoint, target, r);
 769   return r;
 770 }
 771
 772 static void indep_dispose(MovPosChange *indep) {
 773   if (!indep) return;
 774
 775   DPRINTF(movpos,intern, INDEP_DPFX_FMT "dispose...\n",
 776           INDEP_DPFX_ARGS(indep));
 777
 778   int changei;
 779   for (changei=0; changei<indep->n_changes; changei++) {
 780       Change *chg= indep->changes[changei];
 781       if (!chg) continue;
 782
 783       Method *meth= chg->meth;
 784       DPRINTF(movpos,meth, METH_DPFX_FMT "dispose...\n",
 785               METH_DPFX_ARGS(indep,*meth));
 786       meth->dispose(meth, chg);
 787   }
 788   free(indep);
 789 }
 790
 791 #define EVAL_MAX_METHODS 2
 792 #define EVAL_MAX_MOTIONS 2
 793
 794 static ErrorCode indep_prepare(Segment *move, MovPosComb target,
 795                                MovPosComb startpoint,
 796                                int ms, int confirming,
 797                                MovPosChange **indep_r /* 0 ok */,
 798                                int *cost_r /* 0 ok */) {
 799   static int n_meths;
 800   static Method *meths[EVAL_MAX_METHODS];
 801   static int n_motions[EVAL_MAX_METHODS];
 802   static Motion motions[EVAL_MAX_METHODS][EVAL_MAX_MOTIONS];
 803
 804   const SegmentInfo *movei= move->i;
 805   int feat, DP;
 806
 807   MovPosChange *indep=0;
 808
 809   DPRINTF1(movpos,eval, "movpos prepare %s/%s <-%s", move->i->pname,
 810            movpos_pname(move,target), movpos_pname(move,startpoint));
 811
 812   if (!SOMEP(startpoint)) {
 813     startpoint= movpos_poscomb_actual(move);
 814     DPRINTF2(" actual <-%s", movpos_pname(move,startpoint));
 815   }
 816
 817   n_meths= 0;
 818
 819   for (feat=0; feat<movei->n_movfeats; feat++) {
 820     const MovFeatInfo *feati= &movei->movfeats[feat];
 821     if (!change_needed(feati,startpoint,target)) continue;
 822     MovPosComb posn= target / feati->weight % feati->posns;
 823     Method *meth= feature_method(feati);
 824
 825     int methi;
 826     for (methi=0; methi<n_meths; methi++)
 827       if (meths[methi] == meth) goto found_method;
 828     /* need a new method */
 829     methi= n_meths++;
 830     if (methi >= EVAL_MAX_METHODS) {
 831       DPRINTF2(" MovFeatTooManyMethods methi=%d\n",methi);
 832       return EC_MovFeatTooManyMethods;
 833     }
 834     meths[methi]= meth;
 835     n_motions[methi]= 0;
 836     DPRINTF2(" meths[%d]=%s", methi,meth->pname);
 837
 838   found_method:;
 839     int motioni= n_motions[methi]++;
 840     if (motioni >= EVAL_MAX_MOTIONS) {
 841       DPRINTF2(" MovFeatTooManyMotions motioni=%d\n",motioni);
 842       return EC_MovFeatTooManyMotions;
 843     }
 844     DPRINTF2(" motion[%d][%d]=%s%d", methi, motioni, feati->pname,posn);
 845     motions[methi][motioni].i= feati;
 846     motions[methi][motioni].posn= posn;
 847   }
 848
 849   if (indep_r) {
 850     DPRINTF2(" alloc");
 851     indep= mmalloc(sizeof(*indep) + sizeof(Change*) * n_meths);
 852     indep->move= move;
 853     indep->actual= startpoint;
 854     indep->target= target;
 855     indep->n_changes= n_meths;
 856     memset(indep->changes, 0, sizeof(Change*) * n_meths);
 857   }
 858   DPRINTF2("\n");
 859
 860   int totalcost= 0;
 861   int changei;
 862   ErrorCode ec;
 863
 864   for (changei=0; changei<n_meths; changei++) {
 865     Method *meth= meths[changei];
 866     int thiscost= 0;
 867     meth->needcheck= 1;
 868     meth->needexec= 1;
 869
 870     if (indep_r)
 871       DPRINTF(movpos,meth, METH_DPFX_FMT "prepare n_motions=%d...\n",
 872               METH_DPFX_ARGS(indep,*meth), n_motions[changei]);
 873     else
 874       DPRINTF(movpos,meth, "%s prepare (costing) n_motions=%d...\n",
 875               meth->pname, n_motions[changei]);
 876
 877     ec= meth->prepare(meth,move,
 878                       n_motions[changei],motions[changei],
 879                       ms, confirming,
 880                       indep ? &indep->changes[changei] : 0,
 881                       &thiscost);
 882     if (ec) goto x;
 883     if (indep) {
 884       Change *chg= indep->changes[changei];
 885       chg->meth= meth;
 886       chg->indep= indep;
 887       chg->installed= 0;
 888     }
 889     totalcost += thiscost;
 890   }
 891
 892   if (indep_r) *indep_r= indep;
 893   if (cost_r) *cost_r= totalcost;
 894
 895   if (indep_r)
 896     DPRINTF(movpos,eval, INDEP_DPFX_FMT "prepare cost=%d ok\n",
 897             INDEP_DPFX_ARGS(indep), totalcost);
 898   else
 899     DPRINTF(movpos,eval, "movpos prepare %s/%s cost=%d ok\n",
 900             move->i->pname, movpos_pname(move,target), totalcost);
 901   return 0;
 902
 903  x:
 904   indep_dispose(indep);
 905   DPRINTF(movpos,entry, "movpos prepare %s/%s err=%s\n", move->i->pname,
 906           movpos_pname(move,target), ec2str(ec));
 907   return ec;
 908 }
 909
 910 static void indep_remove(MovPosChange *remv) {
 911   if (!remv) return;
 912
 913   DPRINTF(movpos,intern, INDEP_DPFX_FMT "remove...\n",
 914           INDEP_DPFX_ARGS(remv));
 915
 916   int i;
 917   for (i=0; i<remv->n_changes; i++) {
 918     Change *chg= remv->changes[i];
 919     if (!chg->installed) continue;
 920     Method *meth= chg->meth;
 921     meth->needexec= 1;
 922
 923     DPRINTF(movpos,meth, METH_DPFX_FMT "remove...\n",
 924             METH_DPFX_ARGS(remv,*meth));
 925     meth->remove(meth, chg);
 926     chg->installed= 0;
 927   }
 928 }
 929
 930 static ErrorCode
 931 indep_install(MovPosChange *inst, int checknow) {
 932   /* if this fails, inst may be left partially installed */
 933   if (!inst) return 0;
 934
 935   DPRINTF(movpos,intern, INDEP_DPFX_FMT "install checknow=%d...\n",
 936           INDEP_DPFX_ARGS(inst), checknow);
 937
 938   int i;
 939   ErrorCode ec;
 940   for (i=0; i<inst->n_changes; i++) {
 941     Change *chg= inst->changes[i];
 942     assert(!chg->installed);
 943     Method *meth= chg->meth;
 944
 945     meth->needexec= 1;
 946     ec= meth->install(meth, chg);
 947     DPRINTF(movpos,meth, METH_DPFX_FMT "install=%s\n",
 948             METH_DPFX_ARGS(inst,*meth), ec2str(ec));
 949     if (ec) goto x;
 950     chg->installed= 1;
 951     meth->needcheck= 1;
 952
 953     if (checknow) {
 954       ec= meth->check(meth);
 955       DPRINTF(movpos,meth, METH_DPFX_FMT "check=%s\n",
 956               METH_DPFX_ARGS(inst,*meth), ec2str(ec));
 957       if (ec) goto x;
 958       meth->needcheck= 0;
 959     }
 960   }
 961   return 0;
 962
 963  x:
 964   return ec;
 965 }
 966
 967 static void indep_check_execute(void) {
 968   DPRINTF(movpos,intern, "movpos indep_check_execute\n");
 969
 970   Method **methwalk, *meth;
 971   for (methwalk= methods; (meth= *methwalk); methwalk++) {
 972     if (meth->needcheck) {
 973       ErrorCode ec= meth->check(meth);
 974       DPRINTF(movpos,meth, "%s check=%s\n", meth->pname, ec2str(ec));
 975       assert(!ec);
 976       meth->needcheck= 0;
 977     }
 978     if (meth->needexec) {
 979       meth->needexec= 0;
 980       DPRINTF(movpos,meth, "%s execute...\n", meth->pname);
 981       meth->execute(meth);
 982     }
 983   }
 984 }
 985
 986 /*---------- entrypoints from rest of program ----------*/
 987
 988 ErrorCode
 989 movpos_reserve(Segment *move, int maxdelay_ms, MovPosChange **res_r,
 990                MovPosComb target, MovPosComb startpoint /*as for findcomb*/) {
 991   ErrorCode ec;
 992   MovPosChange *indep= 0;
 993
 994   DPRINTF(movpos,entry, "movpos reserve %s/%s maxdelay=%dms startpoint=%s\n",
 995           move->i->pname, movpos_pname(move,target),
 996           maxdelay_ms, movpos_pname(move,startpoint));
 997
 998   ec= indep_prepare(move,target, startpoint,
 999                     maxdelay_ms,0,
1000                     &indep, 0);
1001   if (ec) return ec;
1002
1003   ec= indep_install(indep, 1);
1004   if (ec) goto x;
1005
1006   indep_check_execute();
1007
1008   DPRINTF(movpos,entry, "movpos reserve %s/%s ok\n",
1009           move->i->pname, movpos_pname(move,target));
1010   *res_r= indep;
1011   return 0;
1012
1013  x:
1014   indep_remove(indep);
1015   indep_dispose(indep);
1016   indep_check_execute();
1017
1018   DPRINTF(movpos,entry, "movpos reserve %s/%s err=%s\n",
1019           move->i->pname, movpos_pname(move,target), ec2str(ec));
1020   return ec;
1021 }
1022
1023 ErrorCode movpos_findcomb_bysegs(Segment *back, Segment *move, Segment *fwd,
1024                                  MovPosComb startpoint, MovPosComb *chosen_r) {
1025   const SegmentInfo *movei= move->i;
1026   MovPosComb tcomb, bestcomb=-1;
1027   int tcost, bestcost=INT_MAX;
1028   const SegPosCombInfo *pci;
1029
1030   DPRINTF(movpos,eval, "movpos_findcomb_bysegs %s-%s-%s <-%s\n",
1031           back ? back->i->pname : "*", move->i->pname,
1032           fwd  ? fwd ->i->pname : "*", movpos_pname(move, startpoint));
1033
1034   for (tcomb=0, pci=movei->poscombs;
1035        tcomb<movei->n_poscombs;
1036        tcomb++, pci++) {
1037     /* these next assignments may generate segments[-1] but we don't
1038      * care because that won't compare equal to back or fwd */
1039     Segment *tback= &segments[pci->link[1].next];
1040     Segment *tfwd=  &segments[pci->link[0].next];
1041
1042     DPRINTF(movpos,intern, "movpos_findcomb_bysegs  ... %s : %s-%s-%s\n",
1043             movpos_pname(move, tcomb),
1044             SOMEP(pci->link[1].next) ? tback->i->pname : "#",
1045             move->i->pname,
1046             SOMEP(pci->link[0].next) ? tfwd->i->pname : "#");
1047
1048     if (back && !(back==tback || back==tfwd)) continue;
1049     if (fwd  && !(fwd ==tback || fwd ==tfwd)) continue;
1050
1051     /* we have to search for the one which is least effort */
1052     ErrorCode ec= indep_prepare(move,tcomb,startpoint, -1,0, 0,&tcost);
1053     if (ec) return ec;
1054
1055     if (tcost >= bestcost) /* prefer low-numbered movposcombs */
1056       continue;
1057
1058     bestcomb= tcomb;
1059     bestcost= tcost;
1060   }
1061   DPRINTF(movpos,entry, "movpos_findcomb_bysegs %s..%s..%s <-%s => %s/%s\n",
1062           back ? back->i->pname : "-", move->i->pname,
1063           fwd  ? fwd ->i->pname : "-", movpos_pname(move, startpoint),
1064           move->i->pname, movpos_pname(move,bestcomb));
1065
1066   if (chosen_r) *chosen_r= bestcomb;
1067   return
1068     bestcost==INT_MAX ? EC_MovFeatRouteNotFound :
1069     0;
1070 }
1071
1072 ErrorCode movpos_change(Segment *move, MovPosComb target,
1073                         int maxdelay_ms, MovPosChange *resv) {
1074   int DP;
1075   MovPosComb actual;
1076   ErrorCode ec;
1077
1078   if (!move->moving) {
1079     actual= move->movposcomb;
1080     assert(!move->motion);
1081   } else {
1082     actual= move->motion->actual;
1083   }
1084
1085   DPRINTF1(movpos,entry, "movpos change %s/%s maxdelay=%dms actual=%s",
1086            move->i->pname, movpos_pname(move, target),
1087            maxdelay_ms, movpos_pname(move, actual));
1088   if (resv) DPRINTF2(" resv=%s/%s",
1089                      resv->move->i->pname,
1090                      movpos_pname(resv->move, resv->target));
1091   if (move->motion) DPRINTF2(" oldmotion=/%s",
1092                              movpos_pname(move, move->motion->target));
1093   DPRINTF2("\n");
1094
1095   MovPosChange *inst= 0;
1096
1097   ec= indep_prepare(move,target, actual,
1098                     maxdelay_ms,1,
1099                     &inst, 0);
1100   if (ec) goto x;
1101
1102   indep_remove(resv);
1103   indep_remove(move->motion);;
1104
1105   ec= indep_install(inst, 1);
1106   if (ec) goto x;
1107
1108   indep_dispose(resv);
1109   indep_dispose(move->motion);
1110
1111   move->motion= inst;
1112   move->moving= 1;
1113
1114   ouposn_moving(inst);
1115   indep_check_execute();
1116
1117   if (!inst->n_changes)
1118     /* oh! */
1119     indep_indep_done(inst);
1120
1121   DPRINTF(movpos,entry, "movpos change %s/%s ok\n",
1122           move->i->pname, movpos_pname(move, target));
1123   return 0;
1124
1125  x:
1126   indep_remove(inst);
1127   indep_dispose(inst);
1128   ec= indep_install(move->motion, 0);  assert(!ec);
1129   ec= indep_install(resv, 0);          assert(!ec);
1130   indep_check_execute();
1131
1132   DPRINTF(movpos,entry, "movpos change %s/%s err=%s\n",
1133           move->i->pname, movpos_pname(move, target), ec2str(ec));
1134   return ec;
1135 }
1136
1137 void movpos_unreserve(MovPosChange *resv) {
1138   if (!resv) return;
1139   DPRINTF(movpos,entry, "movpos unreserve %s/%s...\n",
1140           resv->move->i->pname, movpos_pname(resv->move, resv->target));
1141   indep_remove(resv);
1142   indep_dispose(resv);
1143   indep_check_execute();
1144 }
1145
1146 MovPosComb movpos_change_intent(MovPosChange *indep) {
1147   return indep->target;
1148 }
1149
1150 void motions_all_abandon(void) {
1151   Method **meth;
1152   SEG_IV;
1153   DPRINTF(movpos,entry, "movpos motions_all_abandon...\n");
1154   FOR_SEG {
1155     if (!seg->moving) continue;
1156
1157     MovPosChange *abandon= seg->motion;
1158     indep_remove(abandon);
1159     indep_dispose(abandon);
1160     seg->movposcomb= abandon->actual;
1161     seg->moving= 0;
1162     seg->motion= 0;
1163   }
1164   for (meth=methods; *meth; meth++)
1165     (*meth)->all_abandon(*meth);
1166 }