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