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