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