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