2 * Handling of points and other moveable features.
7 /*========== declarations ==========*/
14 typedef struct Method Method;
15 typedef struct Change Change;
17 typedef struct MovPosChange {
19 /* everything beyond here is private for indep */
20 MovPosComb actual, target;
25 static void method_update_feature(Method*, MovPosChange*, const Motion *mo);
26 /* Called from methods' execution logic when an individual feature
27 * has been moved. This is used by the method-independent code to
28 * compute the correct delta set of movements from the current
29 * actual position, when thinking about new plans. It is also sent
30 * to clients and ultimately used by the UI.
33 static void method_change_done(Method *m, Change *chg);
34 /* Called from methods' execution logic when a whole change
35 * has been done. The method-independent code will take care of
36 * updating move->movposcomb. etc.
38 * REENTRANCY: must NOT be called from within a call to the method's
39 * execute() (and of course cannot legally be called from within
40 * prepare, consider, check or dispose).
44 /* Kind-independent code is responsible for determining
45 * the method, doing a bit of cleanup, and adjusting the flow
46 * slightly. Per-kind code does the actual work and is mostly in
47 * charge - it is also responsible for updating seg->moving and ->motion.
50 * The following states exist for each Method
51 * S Starting corresponds to global states other than Sta_Run
52 * T Tentative changes have been made but may yet be undone
53 * Y Yes proposed changes have been checked and are OK
54 * E Executing the method is executing
56 * The following states exist for each Change
57 * at points when control flow passes between kind and indep:
58 * U Unallocated no memory allocated (Change does not exist)
59 * P Prepared memory allocation done, basics filled in
60 * I Installed reservation or confirmation successful
61 * D Done motion is complete and callback is being entered
62 * G Garbage motion is complete and callback is being exited
68 * No Changes remain Prepared while the Method is Executing.
71 struct Change { /* valid in: filled in by and when: */
72 Method *meth; /* PIDG indep after prepare() */
73 MovPosChange *indep; /* PID indep after prepare() */
74 unsigned installed:1; /* private for indep */
75 /* kind-specific data follows */ /* varies kind-specific code, varies */
81 ErrorCode (*prepare)(Method *m, /* TYE->T */
83 int n_motions, const Motion *motions,
84 int ms, int confirmation,
85 Change **chg_r, /* 0->P; err: 0->0; may be 0 */
86 int *cost_r /* may be 0 */);
87 void (*dispose)(Method *m, /* TY->TY */
90 ErrorCode (*install)(Method *m, /* TYE->T; err: TYE->TYY */
91 Change *inst); /* P->I; err: P->P */
92 /* error: ET->T, no change to Changes */
93 void (*remove)(Method *m, /* TYE->TYY */
94 Change *remove); /* I->P */
95 /* error: ET->T, no change to Changes */
97 ErrorCode (*check)(Method *m); /* TYE->Y; err: TYE->TYE */
98 void (*execute)(Method *m); /* EY->E */
99 void (*all_abandon)(Method *m); /* TYE->S */
101 unsigned needcheck, needexec; /* used by indep to track T/Y/E */
104 /*========== general utility functions ==========*/
106 const char *movpos_pname(const Segment *move, MovPosComb poscomb) {
107 return !SOMEP(poscomb) ? "?" : move->i->poscombs[poscomb].pname;
110 static void ouposn_moving(const MovPosChange *indep) {
111 Segment *move= indep->move;
112 ouprintf("movpos %s position %s moving\n",
113 move->i->pname, movpos_pname(move, indep->actual));
116 MovPosComb movposcomb_update_feature(MovPosComb startpoint,
117 const MovFeatInfo *mfi,
119 MovPosComb above_weight= mfi->weight * mfi->posns;
120 MovPosComb above= startpoint / above_weight;
121 MovPosComb below= startpoint % mfi->weight;
122 return above*above_weight + featpos*mfi->weight + below;
125 MovPosComb movpos_poscomb_actual(const Segment *seg) {
126 return seg->moving ? seg->motion->actual : seg->movposcomb;
129 static void ignore_all_abandon(Method *m) { }
131 /*========== points and other fixed timeslot movfeats ==========*/
134 * We maintain two queues, one for reserved one for actually confirmed
135 * requests where we know what we're doing.
137 * We divide time into discrete slots, numbered with clock arithmetic.
139 * cslot cslot+1 cslot+2
141 * currently next in after
144 * We increment cslot when we consider the movfeat to move;
145 * for points and relays this is when we issue the command to the PIC.
146 * In a request, the deadline represents the latest allowable value
147 * of cslot just before that increment.
150 typedef unsigned FsqSlot;
151 typedef int FsqSlotSigned;
153 #define FSQDN (~(FsqSlot)0)
155 typedef struct { /* state Prep Resv Inst Resv Prep Conf Inst Conf */
156 Change h; /* in queue? absent reserved absent confirmed */
157 FsqSlot deadline; /* relative relative absolute absolute */
158 int n_motions; /* 1 1 num undone num undone */
159 Motion motions[]; /* [0].i: 0 0 non-0 non-0 */
160 /* .posn: undef undef defined defined */
163 #define FSQ_MAX_QUEUE 15
167 FsqReq *l[FSQ_MAX_QUEUE];
170 typedef struct FsqMethod FsqMethod;
173 /* set by fsq's client before fsq_init */
175 void (*move)(FsqMethod *m, const MovFeatInfo *mfi, int movfeatposn);
177 int ready; /* >0 means ready; set to 0 by fsq just before move */
178 /* fsq's client must arrange that these start out at all-bits-zero */
180 FsqQueue confirmed, reserved;
186 /* client may put things here */
189 static void fsq_check_action(FsqMethod *m);
190 static ErrorCode fsq_check_plan(FsqMethod *m);
192 static FsqSlotSigned fsq_maxdelay_reldeadline(FsqMethod *m,
193 int maxdelay_ms, int n_motions) {
194 if (maxdelay_ms==-1) return FSQ_MAX_QUEUE*16;
195 return (maxdelay_ms - m->f.lag_ms) / m->f.slot_ms - n_motions;
198 static void fsq_queue_remove_index(FsqQueue *q, int index) {
200 memmove(&q->l[index], &q->l[index+1], sizeof(q->l[0]) * (q->n - index));
203 static int fsq_req_compar_approx(const void *av, const void *bv) {
204 FsqReq *const *a= av;
205 FsqReq *const *b= av;
206 FsqSlotSigned dldiff= (*b)->deadline - (*a)->deadline;
210 static void fsq_queue_remove_item(FsqQueue *q, FsqReq *r) {
213 entry= bsearch(r, q->l, q->n, sizeof(q->l[0]), fsq_req_compar_approx);
216 #define SEARCH_CHECK do{ \
217 if (q->l[i] == r) goto found; \
218 if (q->l[i]->deadline != r->deadline) break; \
220 for(i= entry - q->l; i >= 0; i--) SEARCH_CHECK;
221 for(i= entry - q->l + 1; i < q->n; i++) SEARCH_CHECK;
225 fsq_queue_remove_index(q, i);
228 static FsqQueue *fsq_item_queue(FsqMethod *m, FsqReq *r)
229 { return r->motions[0].i ? &m->f.confirmed : &m->f.reserved; }
233 whichwhen= wh##when, \
236 static ErrorCode fsq_check_plan(FsqMethod *m) {
237 /* Checks whether we can meet the currently queued commitments */
238 /* if this fails, indep machinery calls fsq_prepare to dequeue */
239 int future, conf, resv, whichwhen, DP;
245 DPRINTF1(movpos,fsq, "%s plan", m->m.pname);
247 /* If CDU isn't charged we can't do one right away */
254 FsqReq *confr= conf < m->f.confirmed.n ? m->f.confirmed.l[conf] : 0;
255 FsqReq *resvr= resv < m->f.reserved .n ? m->f.reserved .l[resv] : 0;
256 if (!confr && !resvr) break;
257 DPRINTF2(" %d:",future);
258 int confwhen= confr ? confr->deadline - m->f.cslot : INT_MAX;
259 int resvwhen= resvr ? resvr->deadline : INT_MAX;
260 if (future && resvwhen < confwhen) {
261 WHICH(resv); DPRINTF2("~");
265 future++; DPRINTF2("-");
268 DPRINTF2("%s/%s[%d@t+%d]", whichr->h.indep->move->i->pname,
269 movpos_pname(whichr->h.indep->move, whichr->h.indep->target),
270 whichr->n_motions, whichwhen);
271 if (future > whichwhen) {
272 DPRINTF2("!...bad\n");
273 return EC_MovFeatTooLate;
275 future += whichr->n_motions;
283 static ErrorCode fsq_queue_insert_item(FsqMethod *m, FsqQueue *q, FsqReq *r) {
286 if (q->n == FSQ_MAX_QUEUE)
287 return EC_BufferFull;
290 insat>0 && (FsqSlotSigned)(r->deadline - q->l[insat-1]->deadline) < 0;
292 q->l[insat]= q->l[insat-1];
299 static void fsq_item_debug(FsqMethod *m, FsqReq *r, const char *opwhat) {
301 Segment *move= r->h.indep->move;
302 DPRINTF1(movpos,fsq, "%s %s %s", m->m.pname, opwhat, move->i->pname);
303 if (r->motions[0].i) {
306 for (i=0, mo=r->motions; i<r->n_motions; i++, mo++)
307 DPRINTF2("/%s%d", mo->i->pname, (int)mo->posn);
310 DPRINTF2("(%d)\n", r->n_motions);
314 static ErrorCode fsq_enqueue(FsqMethod *m, FsqReq *r) { /* P->I; err: P->P */
316 fsq_item_debug(m,r,"enqueue");
317 return fsq_queue_insert_item(m, fsq_item_queue(m,r), r);
320 static void fsq_dequeue(FsqMethod *m, FsqReq *r) { /* I->P */
322 fsq_item_debug(m,r,"dequeue");
323 fsq_queue_remove_item(fsq_item_queue(m,r), r);
326 /*---------- method entrypoints ----------*/
328 static ErrorCode fsq_prepare(Method *mm, const Segment *move,
329 int n_motions, const Motion *motions,
330 int ms, int confirmation,
331 Change **chg_r, int *cost_r) {
332 FsqMethod *m= (void*)mm;
334 assert(n_motions > 0);
336 FsqSlotSigned reldeadline= fsq_maxdelay_reldeadline(m, ms, n_motions);
337 if (reldeadline <= 0) return EC_MovFeatTooLate;
340 int alloc_motions= confirmation ? n_motions : 1;
341 /* we need at least one motion in the table so we can tell
342 * the difference between the states by looking at motions[0].i */
343 FsqReq *r= mmalloc(sizeof(*r) + alloc_motions * sizeof(r->motions[0]));
344 r->n_motions= n_motions;
346 r->deadline= reldeadline + m->f.cslot;
347 memcpy(r->motions, motions, sizeof(*r->motions)*n_motions);
349 r->deadline= reldeadline;
355 *cost_r= n_motions * m->f.slot_ms;
361 static void fsq_dispose(Method *mm, Change *chg) {
362 FsqReq *r= (FsqReq*)chg;
366 static void fsq_remove(Method *mm, Change *remvchg) {
367 FsqMethod *m= (void*)mm;
368 FsqReq *remv= (FsqReq*)remvchg;
370 fsq_dequeue(m, remv);
373 static ErrorCode fsq_install(Method *mm, Change *instchg) {
374 FsqMethod *m= (void*)mm;
375 FsqReq *inst= (FsqReq*)instchg;
377 return fsq_enqueue(m, inst);
380 static ErrorCode fsq_check(Method *mm) {
381 FsqMethod *m= (void*)mm;
382 ErrorCode ec= fsq_check_plan(m);
386 static void fsq_execute(Method *mm) {
387 FsqMethod *m= (void*)mm;
391 /*---------- something to do ? ----------*/
393 static void fsq_check_action(FsqMethod *m) {
394 /* client should call this after it sets ready */
397 if (!m->f.confirmed.n) {
398 if (sta_state == Sta_Finalising) resolve_motioncheck();
402 FsqReq *r= m->f.confirmed.l[0];
404 if (r->n_motions && m->f.ready>0) {
405 /* look for something to move */
406 Motion *mo= &r->motions[--r->n_motions];
407 assert(mo->posn < mo->i->posns);
409 m->f.move(m, mo->i, mo->posn);
412 method_update_feature(&m->m, r->h.indep, mo);
416 fsq_queue_remove_index(&m->f.confirmed, 0);
417 method_change_done(&m->m, &r->h);
418 m->m.dispose(&m->m, &r->h);
420 ec= fsq_check_plan(m); assert(!ec);
425 /*========== points ==========*/
428 * CDU and point queue states:
431 * ____________ conf'd
432 * / points_ \ ready .n
436 * |from INACTIVE -1 0
437 * |any <=Sta_Settling
440 * ___________ |turning
451 * | |fsq_check_action
452 * | | calls point_move which fires a point
457 #define CDU_RECHARGE 350 /*ms*/
458 #define POINT_MOVEMENT 50 /*ms*/
460 static ErrorCode point_prepare(Method *mm, const Segment *move,
461 int n_motions, const Motion *motions,
462 int ms, int confirmation,
463 Change **chg_r, int *cost_r) {
464 FsqMethod *m= (void*)mm;
465 assert(m->f.ready>=0);
466 return fsq_prepare(mm,move, n_motions,motions,
467 ms,confirmation, chg_r,cost_r);
470 static void point_move(FsqMethod *m, const MovFeatInfo *mfi, int posn) {
471 /* actually firing points, yay! */
473 enco_pic_point(&piob, mfi->boob[posn]);
474 serial_transmit(&piob);
477 static void points_all_abandon(Method *mm) {
478 FsqMethod *m= (void*)mm;
482 static FsqMethod points_method;
484 void points_turning_on(void) {
485 FsqMethod *m= &points_method;
488 void on_pic_charged(const PicInsnInfo *pii, const PicInsn *pi, int objnum) {
489 FsqMethod *m= &points_method;
490 if (m->f.ready<0) return;
495 static FsqMethod points_method= {
497 point_prepare, fsq_dispose,
498 fsq_install, fsq_remove,
499 fsq_check, fsq_execute, points_all_abandon },
500 { .lag_ms= POINT_MOVEMENT, .slot_ms= CDU_RECHARGE, .move= point_move }
503 /*========== relays ==========*/
508 * ____________ conf'd
509 * / wagglers_ \ ready .n
514 * |any <=Sta_Settling
517 * ___________ |turning
528 * | |fsq_check_action
529 * | | calls waggle_do which switches a relay
534 static FsqMethod waggle;
536 static void waggle_do(FsqMethod *m, const MovFeatInfo *mfi, int posn) {
537 /* actually setting relays */
539 enco_pic_waggle(&piob, mfi->boob[0], posn);
540 serial_transmit(&piob);
543 static SegmentNum waggle_settle_seg;
544 static int waggle_settle_feat;
546 static void waggle_settle_check(void) {
548 if (waggle_settle_seg >= info_nsegments) return;
550 Segment *seg= &segments[waggle_settle_seg];
551 if (waggle_settle_feat >= seg->i->n_movfeats) {
552 waggle_settle_seg++; waggle_settle_feat=0; continue;
555 const MovFeatInfo *feati= &seg->i->movfeats[waggle_settle_feat];
556 if (feati->kind != mfk_relay) {
557 waggle_settle_feat++; continue;
561 waggle_do(&waggle, feati, (seg->movposcomb / feati->weight) & 1);
562 waggle_settle_feat++;
566 void waggle_startup_manual(void) {
570 void waggle_settle(void) {
571 waggle_settle_seg= 0;
572 waggle_settle_feat= 0;
574 waggle_settle_check();
577 void on_pic_waggled(const PicInsnInfo *pii, const PicInsn *pi, int objnum) {
578 if (sta_state == Sta_Settling) {
580 waggle_settle_check();
581 } else if (sta_state >= Sta_Resolving || sta_state == Sta_Manual) {
583 fsq_check_action(&waggle);
588 static FsqMethod waggle= {
590 fsq_prepare, fsq_dispose,
591 fsq_install, fsq_remove,
592 fsq_check, fsq_execute, ignore_all_abandon },
593 { .lag_ms= 5, .slot_ms= 50, .move= waggle_do }
596 /*========== dummy `nomove' kind ==========*/
598 typedef struct NomoveChange {
600 DLIST_NODE(struct NomoveChange) inqueue;
601 int n_motions; /* 0 for reservations */
607 unsigned eventqueued:1;
608 NomoveChange *queuehead; /* contains confirmations only */
611 static ErrorCode nomove_prepare(Method *meth_in, const Segment *move,
612 int n_motions, const Motion *motions,
613 int ms, int confirming,
614 Change **chg_r, int *cost_r) {
618 if (!confirming) n_motions= 0;
619 chg= mmalloc(sizeof(*chg) + sizeof(Motion)*n_motions);
620 chg->n_motions= n_motions;
621 memcpy(chg->motions, motions, sizeof(Motion)*n_motions);
628 static void nomove_dispose(Method *mm, Change *remvchg) {
629 NomoveChange *remv= (void*)remvchg;
633 static ErrorCode nomove_install(Method *mm, Change *instchg) {
634 NomoveMethod *meth= (void*)mm;
635 NomoveChange *inst= (void*)instchg;
637 DLIST1_PREPEND(meth->queuehead, inst, inqueue);
640 static void nomove_remove(Method *mm, Change *remvchg) {
641 NomoveMethod *meth= (void*)mm;
642 NomoveChange *remv= (void*)remvchg;
644 DLIST1_REMOVE(meth->queuehead, remv, inqueue);
647 static ErrorCode nomove_check(Method *mm) { return 0; }
649 static void *nomove_execute_now(oop_source *source, struct timeval tv,
651 NomoveMethod *meth= meth_v;
652 meth->eventqueued= 0;
655 while ((done= meth->queuehead)) {
656 assert(done->n_motions);
658 for (i=0; i<done->n_motions; i++)
659 method_update_feature(&meth->m, done->h.indep, &done->motions[i]);
660 method_change_done(&meth->m, &done->h);
661 DLIST1_REMOVE(meth->queuehead, done, inqueue);
662 nomove_dispose(&meth->m, &done->h);
667 static void nomove_execute(Method *mm) {
668 NomoveMethod *meth= (void*)mm;
669 if (!meth->eventqueued) {
670 meth->eventqueued= 1;
671 events->on_time(events, OOP_TIME_NOW, nomove_execute_now, meth);
675 static Method nomove_method= {
677 nomove_prepare, nomove_dispose,
678 nomove_install, nomove_remove,
679 nomove_check, nomove_execute, ignore_all_abandon
682 /*========== method-independent machinery ==========*/
684 #define INDEP_DBG_FMT "<%p:%s/%s[%d]>"
685 #define INDEP_DBG_ARGS(in) (in), \
686 ((in)->move->i->pname), (movpos_pname((in)->move, (in)->target)), \
689 #define INDEP_DPFX_FMT "movpos " INDEP_DBG_FMT " "
690 #define INDEP_DPFX_ARGS(in) INDEP_DBG_ARGS((in))
692 #define METH_DPFX_FMT "%s " INDEP_DBG_FMT " "
693 #define METH_DPFX_ARGS(indep, meth) ((meth).pname), INDEP_DBG_ARGS((indep))
695 static Method *methods[]= {
696 [mfk_none] = (Method*)&nomove_method,
697 [mfk_point] = (Method*)&points_method,
698 [mfk_relay] = (Method*)&waggle,
701 /*---------- entrypoints from methods ----------*/
703 static void method_update_feature(Method *m, MovPosChange *indep,
705 ouprintf("movpos %s feat %s %d %s\n", indep->move->i->pname,
706 mo->i->pname, mo->posn, m->pname);
707 if (SOMEP(indep->actual))
709 movposcomb_update_feature(indep->actual, mo->i, mo->posn);
710 ouposn_moving(indep);
713 static void method_change_done(Method *m, Change *chg) {
714 Indep *indep= chg->indep;
717 DPRINTF(movpos,meth, METH_DPFX_FMT "method_change_done...\n",
718 METH_DPFX_ARGS(indep,*m));
720 for (search=indep->changes; *search; search++)
721 if ((*search) == chg) goto found;
722 assert(!"change in indep");
726 if (indep->n_changes) {
727 *search= indep->changes[indep->n_changes];
732 Segment *move= indep->move;
735 move->movposcomb= indep->target;
736 ouprintf("movpos %s position %s stable\n",
737 move->i->pname, movpos_pname(move, move->movposcomb));
741 /*---------- internal core machinery ----------*/
743 static Method *feature_method(const MovFeatInfo *feati) {
744 assert(feati->kind >= 0);
745 assert(feati->kind < ARRAY_SIZE(methods));
746 Method *meth= methods[feati->kind];
751 static int change_needed(const MovFeatInfo *feati,
752 MovPosComb startpoint, MovPosComb target) {
754 r= !SOMEP(startpoint) ||
755 (target - startpoint) / feati->weight % feati->posns;
756 if (DEBUGP(movpos,eval))
757 DPRINTFA(" { %s:%s(%d*%d) %d..%d => %d }",
758 feature_method(feati)->pname, feati->pname,
759 feati->posns, feati->weight,
760 startpoint, target, r);
764 static void indep_dispose(MovPosChange *indep) {
767 DPRINTF(movpos,intern, INDEP_DPFX_FMT "dispose...\n",
768 INDEP_DPFX_ARGS(indep));
771 for (changei=0; changei<indep->n_changes; changei++) {
772 Change *chg= indep->changes[changei];
773 Method *meth= chg->meth;
775 DPRINTF(movpos,meth, METH_DPFX_FMT "dispose...\n",
776 METH_DPFX_ARGS(indep,*meth));
777 meth->dispose(meth, chg);
783 #define EVAL_MAX_METHODS 2
784 #define EVAL_MAX_MOTIONS 2
786 static ErrorCode indep_prepare(Segment *move, MovPosComb target,
787 MovPosComb startpoint,
788 int ms, int confirming,
789 MovPosChange **indep_r /* 0 ok */,
790 int *cost_r /* 0 ok */) {
792 static Method *meths[EVAL_MAX_METHODS];
793 static int n_motions[EVAL_MAX_METHODS];
794 static Motion motions[EVAL_MAX_METHODS][EVAL_MAX_MOTIONS];
796 const SegmentInfo *movei= move->i;
799 MovPosChange *indep=0;
801 DPRINTF1(movpos,eval, "movpos prepare %s/%s <-%s", move->i->pname,
802 movpos_pname(move,target), movpos_pname(move,startpoint));
804 if (!SOMEP(startpoint)) {
805 startpoint= movpos_poscomb_actual(move);
806 DPRINTF2(" actual <-%s", movpos_pname(move,startpoint));
811 for (feat=0; feat<movei->n_movfeats; feat++) {
812 const MovFeatInfo *feati= &movei->movfeats[feat];
813 if (!change_needed(feati,startpoint,target)) continue;
814 MovPosComb posn= target / feati->weight % feati->posns;
815 Method *meth= feature_method(feati);
818 for (methi=0; methi<n_meths; methi++)
819 if (meths[methi] == meth) goto found_method;
820 /* need a new method */
822 if (methi >= EVAL_MAX_METHODS) {
823 DPRINTF2(" MovFeatTooManyMethods methi=%d\n",methi);
824 return EC_MovFeatTooManyMethods;
828 DPRINTF2(" meths[%d]=%s", methi,meth->pname);
831 int motioni= n_motions[methi]++;
832 if (motioni >= EVAL_MAX_MOTIONS) {
833 DPRINTF2(" MovFeatTooManyMotions motioni=%d\n",motioni);
834 return EC_MovFeatTooManyMotions;
836 DPRINTF2(" motion[%d][%d]=%s%d", methi, motioni, feati->pname,posn);
837 motions[methi][motioni].i= feati;
838 motions[methi][motioni].posn= posn;
843 indep= mmalloc(sizeof(*indep) + sizeof(Change*) * n_meths);
845 indep->actual= startpoint;
846 indep->target= target;
847 indep->n_changes= n_meths;
848 memset(indep->changes, 0, sizeof(Change*) * n_meths);
856 for (changei=0; changei<n_meths; changei++) {
857 Method *meth= meths[changei];
863 DPRINTF(movpos,meth, METH_DPFX_FMT "prepare n_motions=%d...\n",
864 METH_DPFX_ARGS(indep,*meth), n_motions[changei]);
866 DPRINTF(movpos,meth, "%s prepare (costing) n_motions=%d...\n",
867 meth->pname, n_motions[changei]);
869 ec= meth->prepare(meth,move,
870 n_motions[changei],motions[changei],
872 indep ? &indep->changes[changei] : 0,
876 Change *chg= indep->changes[changei];
881 totalcost += thiscost;
884 if (indep_r) *indep_r= indep;
885 if (cost_r) *cost_r= totalcost;
888 DPRINTF(movpos,entry, INDEP_DPFX_FMT "prepare cost=%d ok\n",
889 INDEP_DPFX_ARGS(indep), totalcost);
891 DPRINTF(movpos,entry, "movpos prepare %s/%s cost=%d ok\n",
892 move->i->pname, movpos_pname(move,target), totalcost);
896 indep_dispose(indep);
897 DPRINTF(movpos,entry, "movpos prepare %s/%s err=%s\n", move->i->pname,
898 movpos_pname(move,target), ec2str(ec));
902 static void indep_remove(MovPosChange *remv) {
905 DPRINTF(movpos,intern, INDEP_DPFX_FMT "remove...\n",
906 INDEP_DPFX_ARGS(remv));
909 for (i=0; i<remv->n_changes; i++) {
910 Change *chg= remv->changes[i];
911 if (!chg->installed) continue;
912 Method *meth= chg->meth;
915 DPRINTF(movpos,meth, METH_DPFX_FMT "remove...\n",
916 METH_DPFX_ARGS(remv,*meth));
917 meth->remove(meth, chg);
923 indep_install(MovPosChange *inst, int checknow) {
924 /* if this fails, inst may be left partially installed */
927 DPRINTF(movpos,intern, INDEP_DPFX_FMT "install checknow=%d...\n",
928 INDEP_DPFX_ARGS(inst), checknow);
932 for (i=0; i<inst->n_changes; i++) {
933 Change *chg= inst->changes[i];
934 assert(!chg->installed);
935 Method *meth= chg->meth;
938 ec= meth->install(meth, chg);
939 DPRINTF(movpos,meth, METH_DPFX_FMT "install=%s\n",
940 METH_DPFX_ARGS(inst,*meth), ec2str(ec));
946 ec= meth->check(meth);
947 DPRINTF(movpos,meth, METH_DPFX_FMT "check=%s\n",
948 METH_DPFX_ARGS(inst,*meth), ec2str(ec));
959 static void indep_check_execute(void) {
960 DPRINTF(movpos,intern, "movpos indep_check_execute\n");
962 Method **methwalk, *meth;
963 for (methwalk= methods; (meth= *methwalk); methwalk++) {
964 if (meth->needcheck) {
965 ErrorCode ec= meth->check(meth);
966 DPRINTF(movpos,meth, "%s check=%s\n", meth->pname, ec2str(ec));
970 if (meth->needexec) {
972 DPRINTF(movpos,meth, "%s execute...\n", meth->pname);
978 /*---------- entrypoints from rest of program ----------*/
981 movpos_reserve(Segment *move, int maxdelay_ms, MovPosChange **res_r,
982 MovPosComb target, MovPosComb startpoint /*as for findcomb*/) {
984 MovPosChange *indep= 0;
986 DPRINTF(movpos,entry, "movpos reserve %s/%s maxdelay=%dms startpoint=%s\n",
987 move->i->pname, movpos_pname(move,target),
988 maxdelay_ms, movpos_pname(move,startpoint));
990 ec= indep_prepare(move,target, startpoint,
995 ec= indep_install(indep, 1);
998 indep_check_execute();
1000 DPRINTF(movpos,entry, "movpos reserve %s/%s ok\n",
1001 move->i->pname, movpos_pname(move,target));
1006 indep_remove(indep);
1007 indep_dispose(indep);
1008 indep_check_execute();
1010 DPRINTF(movpos,entry, "movpos reserve %s/%s err=%s\n",
1011 move->i->pname, movpos_pname(move,target), ec2str(ec));
1015 ErrorCode movpos_findcomb_bysegs(Segment *back, Segment *move, Segment *fwd,
1016 MovPosComb startpoint, MovPosComb *chosen_r) {
1017 const SegmentInfo *movei= move->i;
1018 MovPosComb tcomb, bestcomb=-1;
1019 int tcost, bestcost=INT_MAX;
1020 const SegPosCombInfo *pci;
1022 for (tcomb=0, pci=movei->poscombs;
1023 tcomb<movei->n_poscombs;
1025 Segment *tback= &segments[pci->link[1].next];
1026 Segment *tfwd= &segments[pci->link[0].next];
1027 if (back && !(back==tback || back==tfwd)) continue;
1028 if (fwd && !(fwd ==tback || fwd ==tfwd)) continue;
1030 /* we have to search for the one which is least effort */
1031 ErrorCode ec= indep_prepare(move,tcomb,startpoint, -1,0, 0,&tcost);
1034 if (tcost >= bestcost) /* prefer low-numbered movposcombs */
1040 if (chosen_r) *chosen_r= bestcomb;
1042 bestcost==INT_MAX ? EC_MovFeatRouteNotFound :
1046 ErrorCode movpos_change(Segment *move, MovPosComb target,
1047 int maxdelay_ms, MovPosChange *resv) {
1052 if (!move->moving) {
1053 actual= move->movposcomb;
1054 assert(!move->motion);
1056 actual= move->motion->actual;
1059 DPRINTF1(movpos,entry, "movpos change %s/%s maxdelay=%dms actual=%s",
1060 move->i->pname, movpos_pname(move, target),
1061 maxdelay_ms, movpos_pname(move, actual));
1062 if (resv) DPRINTF2(" resv=%s/%s",
1063 resv->move->i->pname,
1064 movpos_pname(resv->move, resv->target));
1065 if (move->motion) DPRINTF2(" oldmotion=/%s",
1066 movpos_pname(move, move->motion->target));
1069 MovPosChange *inst= 0;
1071 ec= indep_prepare(move,target, actual,
1077 indep_remove(move->motion);;
1079 ec= indep_install(inst, 1);
1082 indep_dispose(resv);
1083 indep_dispose(move->motion);
1087 ouposn_moving(inst);
1088 indep_check_execute();
1090 DPRINTF(movpos,entry, "movpos change %s/%s ok\n",
1091 move->i->pname, movpos_pname(move, target));
1096 indep_dispose(inst);
1097 ec= indep_install(move->motion, 0); assert(!ec);
1098 ec= indep_install(resv, 0); assert(!ec);
1099 indep_check_execute();
1101 DPRINTF(movpos,entry, "movpos change %s/%s err=%s\n",
1102 move->i->pname, movpos_pname(move, target), ec2str(ec));
1106 void movpos_unreserve(MovPosChange *resv) {
1108 DPRINTF(movpos,entry, "movpos unreserve %s/%s...\n",
1109 resv->move->i->pname, movpos_pname(resv->move, resv->target));
1111 indep_dispose(resv);
1112 indep_check_execute();
1115 MovPosComb movpos_change_intent(MovPosChange *indep) {
1116 return indep->target;
1119 void motions_all_abandon(void) {
1122 DPRINTF(movpos,entry, "movpos motions_all_abandon...\n");
1124 if (!seg->moving) continue;
1126 MovPosChange *abandon= seg->motion;
1127 indep_remove(abandon);
1128 indep_dispose(abandon);
1129 seg->movposcomb= abandon->actual;
1132 for (meth=methods; *meth; meth++)
1133 (*meth)->all_abandon(*meth);