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 allocate()     */
  47   MovPosChange *indep;             /*  PQ         indep after allocate()     */
  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,           /* U->P; err: U->U; 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);
  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 static ErrorCode evaluate_target(Segment *move, MovPosComb target,
 693                                  MovPosComb startpoint,
 694                                  int *n_changes_r,
 695                                  Change *changes_r[N_METHODS],
 696                                  int *cost_r) {
 697   const SegmentInfo *movei= move->i;
 698   int feat;
 699   const MovFeatInfo *feati;
 700   MovFeatKind kind;
 701   int n_changes;
 702   Method *change_methods[N_METHODS];
 703   Motion *change_motions[N_METHODS];
 704   int i;
 705
 706   DPRINTF(movpos,eval, "%s/%s <-%s\n", move->i->pname,
 707           movpos_pname(move,target), movpos_pname(move,startpoint));
 708
 709   if (!SOMEP(startpoint)) {
 710     startpoint= movpos_poscomb_actual(move);
 711     DPRINTF(movpos,eval, "  actual <-%s\n",
 712             movpos_pname(move,startpoint));
 713   }
 714
 715   n_changes= 0;
 716
 717   for (feat=0, feati=movei->movfeats, tchanges=0, kind= mfk_none;
 718        feat<movei->n_movfeats;
 719        feat++, feati++) {
 720     if (!change_needed(feati,startpoint,target)) continue;
 721     for (i=0; i<n_changes; i++)
 722
 723
 724     tchanges++;
 725     if (kind && feati->kind != kind) return -1;
 726     kind= feati->kind;
 727   }
 728
 729   if (kind_r) *kind_r= kind;
 730   DPRINTF(movpos,eval, "changes=%d kind=%s\n",
 731           tchanges, methods[kind]->pname);
 732   return tchanges;
 733 }
 734
 735
 736
 737 ErrorCode
 738 movpos_reserve(Segment *move, int maxdelay_ms, MovPosChange **res_r,
 739                MovPosComb target, MovPosComb startpoint /*as for findcomb*/) {
 740   MovFeatKind kind= mfk_none;
 741   ErrorCode ec;
 742   int nchanges;
 743
 744   int n_changes;
 745   Change *changes[N_METHODS];
 746
 747   DPRINTF(movpos,reserve, "%s/%s maxdelay=%dms startpoint=%s\n",
 748           move->i->pname, movpos_pname(move,target),
 749           maxdelay_ms, movpos_pname(move,startpoint));
 750
 751   evaluate_target(move,target,startpoint, &n_changes,&changes);
 752
 753
 754   if (nchanges==-1) return EC_MovFeatKindsCombination;
 755
 756   Method *meth= methods[kind];
 757   DPRINTF(movpos,reserve, "allocate %s:%d...\n",
 758           meth->pname, nchanges);
 759   Change *chg= mp_allocate(meth, move, nchanges, target);
 760   ec= meth->reserve(meth, chg, move, maxdelay_ms);
 761   DPRINTF(movpos,reserve, "reserve => %s\n",errorcodelist[ec]);
 762   if (ec) goto x;
 763
 764   *res_r= chg;
 765   return 0;
 766
 767  x:
 768   movpos_unreserve(chg);
 769   return ec;
 770 }
 771
 772
 773
 774
 775
 776
 777
 778 static Change *mp_allocate(Method *meth, Segment *move,
 779                            int alloc_motions, MovPosComb target) {
 780   assert(sta_state >= Sta_Resolving || sta_state == Sta_Manual);
 781   Change *chg= meth->allocate(meth, alloc_motions);
 782   chg->meth=      meth;
 783   chg->move=      move;
 784   chg->intent=    target;
 785   return chg;
 786 }
 787
 788 static int change_needed(const MovFeatInfo *feati,
 789                          MovPosComb startpoint, MovPosComb target) {
 790   int r;
 791   r= startpoint<0 ||
 792     (target - startpoint) / feati->weight % feati->posns;
 793   DPRINTF(movpos,changeneeded, "%s:%s(%d*%d) %d..%d => %d\n",
 794           methods[feati->kind]->pname, feati->pname,
 795           feati->posns, feati->weight,
 796           startpoint, target, r);
 797   return r;
 798 }
 799
 800 ErrorCode movpos_findcomb_bysegs(Segment *back, Segment *move, Segment *fwd,
 801                                  MovPosComb startpoint, MovPosComb *chosen_r) {
 802   const SegmentInfo *movei= move->i;
 803   MovPosComb tcomb, bestcomb=-1;
 804   int tchanges, bestchanges=INT_MAX;
 805   const SegPosCombInfo *pci;
 806
 807   for (tcomb=0, pci=movei->poscombs;
 808        tcomb<movei->n_poscombs;
 809        tcomb++, pci++) {
 810     Segment *tback= &segments[pci->link[1].next];
 811     Segment *tfwd=  &segments[pci->link[0].next];
 812     if (back && !(back==tback || back==tfwd)) continue;
 813     if (fwd  && !(fwd ==tback || fwd ==tfwd)) continue;
 814
 815     /* we have to search for the one which is least effort */
 816     tchanges= evaluate_target(move,tcomb,startpoint,0);
 817
 818     if (tchanges==-1)
 819       /* mixed kinds */
 820       tchanges= INT_MAX-1;
 821
 822     if (tchanges >= bestchanges) /* prefer low-numbered movposcombs */
 823       continue;
 824
 825     bestcomb= tcomb;
 826     bestchanges= tchanges;
 827   }
 828   if (chosen_r) *chosen_r= bestcomb;
 829   return
 830     bestchanges==INT_MAX ? EC_MovFeatRouteNotFound :
 831     bestchanges==INT_MAX-1 ? EC_MovFeatKindsCombination :
 832     0;
 833 }
 834
 835 ErrorCode movpos_change(Segment *move, MovPosComb target,
 836                         int maxdelay_ms, MovPosChange *chg) {
 837   const SegmentInfo *movei= move->i;
 838   const MovFeatInfo *feati;
 839   int feat;
 840   MovPosComb actual;
 841   ErrorCode ec;
 842   MovFeatKind kind= mfk_none;
 843
 844   if (!move->moving) {
 845     actual= move->movposcomb;
 846     assert(!move->motion);
 847   } else {
 848     kind= move->motion->meth->kind;
 849     actual= move->motion->actual;
 850   }
 851
 852   DPRINTF(movpos,change, "%s/%s maxdelay=%dms actual=%s\n",
 853           move->i->pname, movpos_pname(move,target),
 854           maxdelay_ms, movpos_pname(move, actual));
 855   if (chg) DPRINTF(movpos,change, " chg=%s:%s/%s\n",
 856                    chg->meth->pname, chg->move->i->pname,
 857                    movpos_pname(chg->move, chg->intent));
 858
 859   { /* provide horizon for visibility of motions[] */
 860     int n_motions=0;
 861     Motion motions[movei->n_movfeats];
 862
 863     for (feat=0, feati=movei->movfeats;
 864          feat<movei->n_movfeats;
 865          feat++, feati++) {
 866       if (!change_needed(feati,actual,target))
 867         continue;
 868       MovPosComb posn= target / feati->weight % feati->posns;
 869       if (kind) {
 870         if (feati->kind != kind) { ec= EC_MovFeatKindsCombination; goto x; }
 871       } else {
 872         kind= feati->kind;
 873       }
 874       motions[n_motions].i= feati;
 875       motions[n_motions].posn= posn;
 876       n_motions++;
 877     }
 878
 879     Method *meth= methods[kind];
 880
 881     if (chg) {
 882       if (chg->meth != meth ||
 883           chg->move != move ||
 884           chg->intent != target)
 885         return EC_MovFeatReservationInapplicable;
 886     } else {
 887       chg= mp_allocate(meth,move,n_motions,target);
 888     }
 889     chg->actual= actual;
 890
 891     DPRINTF(movpos,change, "confirm %s:%d...\n",
 892             meth->pname, n_motions);
 893     ec= meth->confirm(meth, chg, move, n_motions, motions, maxdelay_ms);
 894     DPRINTF(movpos,change, "confirm => %s\n",errorcodelist[ec]);
 895     if (ec) goto x;
 896   }
 897   return 0;
 898
 899  x:
 900   movpos_unreserve(chg);
 901   return ec;
 902 }
 903
 904 void movpos_unreserve(MovPosChange *res) {
 905   if (!res) return;
 906   DPRINTF(movpos,unreserve, "%s:%s/%s\n",
 907           res->meth->pname, res->move->i->pname,
 908           movpos_pname(res->move, res->intent));
 909   res->meth->destroy(res->meth, res);
 910 }
 911
 912 MovPosComb movpos_poscomb_actual(Segment *seg) {
 913  return seg->moving ? seg->motion->actual : seg->movposcomb;
 914 }
 915
 916 MovPosComb movpos_change_intent(MovPosChange *chg) {
 917   return chg->intent;
 918 }
 919
 920 void motions_all_abandon(void) {
 921   Method **meth;
 922   for (meth=methods; *meth; meth++)
 923     (*meth)->all_abandon(*meth);
 924 }
 925
 926
 927
 928
 929
 930
 931
 932  STUFF BEYOND HERE IS FUNCTIONALITY WHICH NEEDS MOVING TO INDEP
 933    AND/OR CHECKING THAT WE STILL HAVE IT
 934
 935
 936 static ErrorCode fsq_confirm(Method *mm, Change *chg, Segment *move,
 937                              int n_motions, const Motion *motions,
 938                              int maxdelay_ms) {
 939   FsqMethod *m= (void*)mm;
 940   FsqReq *r= (FsqReq*)chg;
 941   FsqSlotSigned reldeadline;
 942   int allow_failure, DP;
 943   ErrorCode ec;
 944
 945   DPRINTF1(movpos,fsq, "%s confirm %s n=%d maxdelay=%dms",
 946            m->m.pname, move->i->pname, n_motions, maxdelay_ms);
 947
 948   assert(!r->motions[0].i); /* no confirming things already confirmed */
 949   if (r->deadline==FSQDN)
 950     DPRINTF2(" (alloc'd: %d)\n", r->n_motions);
 951   else
 952     DPRINTF2(" (res: %s/%s[%d@t+%d])\n",
 953              r->h.move->i->pname, movpos_pname(r->h.move, r->h.intent),
 954              r->n_motions, r->deadline);
 955
 956   /* If the segment is moving, these motions are already based on the
 957    * actual physical position which is stored in the existing request.
 958    * So we try removing the existing request from the queue and put
 959    * it back if it doesn't work.
 960    */
 961
 962   if (n_motions > r->n_motions)
 963     return EC_MovFeatReservationInapplicable;
 964   assert(n_motions <= r->n_motions);
 965   if (maxdelay_ms == -1) {
 966     reldeadline= r->deadline;
 967     if (reldeadline==FSQDN)
 968       reldeadline= fsq_maxdelay_reldeadline(m, -1, n_motions);
 969   } else {
 970     reldeadline= fsq_maxdelay_reldeadline(m, maxdelay_ms, n_motions);
 971   }
 972   allow_failure= reldeadline < (FsqSlotSigned)r->deadline;
 973   DPRINTF(movpos,fsq, "%s  reldeadline=[%d@t+%d] allow_failure=%d\n",
 974           m->m.pname, n_motions, reldeadline, allow_failure);
 975
 976   /* state A or R */
 977   fsq_dequeue(m, r);
 978                                            /* states of existing: */
 979   FsqReq *existing=
 980     move->moving ? (FsqReq*)move->motion : 0;   /* U or C */
 981   if (existing) {
 982     DPRINTF(movpos,fsq,
 983             "%s  existing %s n=%d deadline=t+%d\n",
 984             m->m.pname,
 985             existing->h.move->i->pname,
 986             existing->n_motions,
 987             existing->deadline - m->f.cslot);
 988     fsq_dequeue(m, existing);                   /* U or CA */
 989   }
 990
 991   /* state A or RA */
 992   memcpy(r->motions, motions, sizeof(r->motions[0])*n_motions);
 993   if (!n_motions) r->motions[0].i= move->i->movfeats;
 994   assert(r->motions[0].i);
 995   r->n_motions= n_motions;
 996   r->deadline= reldeadline + m->f.cslot;
 997
 998   if (n_motions == move->i->n_movfeats)
 999     r->actual= 0;
1000   else
1001     r->actual= chg->actual;
1002   assert(r->actual >= 0);
1003
1004   /* state CA */
1005   ec= fsq_enqueue(m, &m->f.confirmed, r);
1006   DPRINTF(movpos,fsq, "%s  fsq_enqueue=%s\n", m->m.pname, ec2str(ec));
1007   assert(allow_failure || !ec);
1008
1009   if (existing) {                                  /* CA */
1010     if (ec) { /* state C but bad */
1011       fsq_dequeue(m,r); /* state CA */
1012       fsq_mark_as_allocated(r); /* state A */
1013       ErrorCode ec_putback= fsq_enqueue(m,&m->f.confirmed, existing);
1014       assert(!ec_putback);                         /* C */
1015     } else { /* state C and good */
1016       free(existing);                              /* U */
1017     }
1018   }
1019   /* either  ec=0   state C                            U
1020    *     or  ec!=0  state A                            C
1021    *     or  ec!=0  state C but bad                    C
1022    */
1023
1024   if (ec) return ec;
1025
1026   move->moving= 1;
1027   move->motion= chg;
1028   move->movposcomb= -1;
1029   ouposn_moving(chg);
1030   fsq_check_action(m);
1031   return 0;
1032 }
1033
1034
1035
1036     r->actual= movposcomb_update_feature(r->actual,mo->i,mo->posn);
1037     if (r->h.actual >= 0 || !r->n_motions)
1038       r->h.actual= r->actual;
1039     ouposn_moving(&r->h);
1040
1041
1042
1043
1044     /* look for something to report
1045      * we can get things here other than from the above
1046      * eg if we are asked to move the
1047      */
1048     Segment *move= r->h.move;
1049     assert(move->moving && move->motion == (Change*)r);
1050     fsq_mark_as_allocated(r); /* now state A aka Done */
1051     motion_done(move,r->h.actual);
1052     free(r);
1053
1054 /*---------- entrypoints from rest of program ----------*/
1055
1056 static void fsq_all_abandon(Method *mm) {
1057   FsqMethod *m= (void*)mm;
1058   int i;
1059
1060   assert(!m->f.reserved.n);
1061
1062   for (i=0; i<m->f.confirmed.n; i++) {
1063     indep_
1064     FsqReq *r= m->f.confirmed.l[i];
1065     Segment *move= r->h.move;
1066     assert(move->motion == (Change*)r);
1067     motion_done(move,r->h.actual);
1068     free(r);
1069   }
1070   m->f.confirmed.n= 0;
1071 }
1072
1073 in points_all_abandon
1074   fsq_all_abandon(mm);
1075
1076
1077  *
1078
1079  * seg->moving and ->motion is in one of the states UC