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