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