hostside/movpos.c

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