Small posn;
} Motion;
-typedef struct KindInfo KindInfo;
+typedef struct Method Method;
+typedef struct Change Change;
+
+typedef struct MovPosChange {
+ Segment *move;
+ /* everything beyond here is private for indep */
+ MovPosComb actual, target;
+ int refcount;
+ int n_changes;
+ Change *changes[];
+} Indep;
+
+static void method_update_feature(Method*, MovPosChange*, const Motion *mo);
+ /* Called from methods' execution logic when an individual feature
+ * has been moved. This is used by the method-independent code to
+ * compute the correct delta set of movements from the current
+ * actual position, when thinking about new plans. It is also sent
+ * to clients and ultimately used by the UI.
+ */
+
+static void method_change_done(Method *m, Change *chg);
+ /* Called from methods' execution logic when a whole change
+ * has been done. The method-independent code will take care of
+ * updating move->movposcomb. etc.
+ *
+ * REENTRANCY: May be called from within a call to the method's
+ * execute(). Of course cannot legally be called from within
+ * prepare, consider, check or dispose.
+ */
+
/* Kind-independent code is responsible for determining
* the method, doing a bit of cleanup, and adjusting the flow
* slightly. Per-kind code does the actual work and is mostly in
* charge - it is also responsible for updating seg->moving and ->motion.
*/
-/* The following states exist for each MovPosChange
+/*
+ * The following states exist for each Method
+ * S Starting corresponds to global states other than Sta_Run
+ * T Tentative changes have been made but may yet be undone
+ * Y Yes proposed changes have been checked and are OK
+ * E Executing the method is executing
+ *
+ * The following states exist for each Change
* at points when control flow passes between kind and indep:
- * U Unallocated no memory allocated (MovPosChange does not exist)
- * A Allocated memory allocation done
- * R Reserved reservation was successful
- * C Confirmed motion queued and will occur
- * D Done motion is complete and callback just needs to be made
- * E Erroneous indep must call destroy straight away
- * seg->moving and ->motion is in one of the states UC
+ * U Unallocated no memory allocated (Change does not exist)
+ * P Prepared memory allocation done, basics filled in
+ * I Installed reservation or confirmation successful
+ * D Done motion is complete and callback is being entered
+ * G Garbage motion is complete and callback is being exited
+ *
+ * Changes may be for:
+ * R Reservation
+ * C Confirmation
+ *
+ * No Changes remain Prepared while the Method is Executing.
*/
-typedef struct MovPosChange { /* valid in: filled in by and when: */
- const KindInfo *ki; /* ARCDE indep after allocate() */
- Segment *move; /* ARCDE indep after allocate() */
- MovPosComb actual; /* CD see below */
- MovPosComb intent; /* RCD indep after allocate() */
+struct Change { /* valid in: filled in by and when: */
+ Method *meth; /* PIDG indep after prepare() */
+ MovPosChange *indep; /* PID indep after prepare() */
+ unsigned installed:1; /* private for indep */
/* kind-specific data follows */ /* varies kind-specific code, varies */
-} Change;
- /* `actual' contains the kind's public opinion about the physical
- * state. It is initialised by indep (just before confirm) from
- * move->motion->actual or move->movposcomb as the case may be. It
- * should be updated by the kind, since it is used by indep for
- * calculating the number and identities of the features which may
- * need to change when a new move request is intended to replace an
- * existing one - ie, the contents of the motions[] provided to a
- * subsequent confirm(). So while a change is Confirmed, the
- * physical state is recorded only in the relevant change, and not
- * in the segment's movposcomb. Once a change goes to Confirmed,
- * the indep code never untangles it so the kind can manage the
- * proper transition. */
-
-struct KindInfo {
+};
+
+struct Method {
const char *pname;
- Change *(*allocate)(int alloc_motions); /* U->A (always succeeds) */
- ErrorCode (*reserve)(Change*, Segment*, int ms); /* A->R; error: A->E */
- ErrorCode (*confirm)(Change*, Segment*, int n_motions,
- const Motion*, int ms); /* [AR]->C; error; [AR]->E */
- void (*destroy)(Change*); /* [ARCE]->U */
- /* indep guarantees that
- * alloc_motions >= move->i->n_motions on reserve
- * alloc_motions >= n_motions on confirm
- * and that if on entry to reserve move->motion is non-0,
- * it move->motion is non-0 and of the same kind
- */
+
+ ErrorCode (*prepare)(Method *m, /* TYE->T */
+ const Segment *move,
+ int n_motions, const Motion *motions,
+ int ms, int confirmation,
+ Change **chg_r, /* 0->P; err: 0->0; may be 0 */
+ int *cost_r /* may be 0 */);
+ void (*dispose)(Method *m, /* TY->TY */
+ Change*); /* P->U */
+
+ ErrorCode (*install)(Method *m, /* TYE->T; err: TYE->TYY */
+ Change *inst); /* P->I; err: P->P */
+ /* error: ET->T, no change to Changes */
+ void (*remove)(Method *m, /* TYE->TYY */
+ Change *remove); /* I->P */
+ /* error: ET->T, no change to Changes */
+
+ ErrorCode (*check)(Method *m); /* TYE->Y; err: TYE->TYE */
+ void (*execute)(Method *m); /* EY->E */
+ void (*all_abandon)(Method *m); /* TYE->S */
+
+ unsigned needcheck, needexec; /* used by indep to track T/Y/E */
};
-static const char *posnpname(Segment *move, MovPosComb poscomb) {
- return poscomb<0 ? "?" : move->i->poscombs[poscomb].pname;
+/*========== general utility functions ==========*/
+
+const char *movpos_pname(const Segment *move, MovPosComb poscomb) {
+ return !SOMEP(poscomb) ? "?" : move->i->poscombs[poscomb].pname;
}
-static void ouposn_moving(Change *chg) {
- Segment *move= chg->move;
- oprintf(UPO, "movpos %s position %s moving\n",
- move->i->pname, posnpname(move, chg->actual));
+static void ouposn_moving(const MovPosChange *indep) {
+ Segment *move= indep->move;
+ ouprintf("movpos %s position %s moving\n",
+ move->i->pname, movpos_pname(move, indep->actual));
+}
+static void ouposn_stable(const Segment *move) {
+ ouprintf("movpos %s position %s stable\n",
+ move->i->pname, movpos_pname(move, move->movposcomb));
+}
+static void ouposn_feat(const Segment *move, const MovFeatInfo *feati,
+ MovPosComb posn, const Method *m) {
+ ouprintf("movpos %s feat %s %d %s\n", move->i->pname,
+ feati->pname, posn, m->pname);
}
-static void motion_done(Segment *move, MovPosComb actual) {
- move->moving= 0;
- move->motion= 0;
- move->movposcomb= actual;
- oprintf(UPO, "movpos %s position %s stable\n",
- move->i->pname, posnpname(move, move->movposcomb));
+MovPosComb movposcomb_update_feature(MovPosComb startpoint,
+ const MovFeatInfo *mfi,
+ int featpos) {
+ MovPosComb above_weight= mfi->weight * mfi->posns;
+ MovPosComb above= startpoint / above_weight;
+ MovPosComb below= startpoint % mfi->weight;
+ return above*above_weight + featpos*mfi->weight + below;
}
-/*========== points ==========*/
+MovPosComb movpos_poscomb_actual(const Segment *seg) {
+ return seg->moving ? seg->motion->actual : seg->movposcomb;
+}
+
+static void ignore_all_abandon(Method *m) { }
+
+/*========== points and other fixed timeslot movfeats ==========*/
/*
* We maintain two queues, one for reserved one for actually confirmed
* currently next in after
* changing line that
*
- * We increment cslot when we issue a POINT command to the PIC.
+ * We increment cslot when we consider the movfeat to move;
+ * for points and relays this is when we issue the command to the PIC.
* In a request, the deadline represents the latest allowable value
* of cslot just before that increment.
*/
-typedef unsigned PtSlot;
-typedef int PtSlotSigned;
+typedef unsigned FsqSlot;
+typedef int FsqSlotSigned;
-/* We think there are three states: Allocated, Reserved and Confirmed.
- * (plus of course Unallocated where we don't have a request at all).
- * These correspond to the indep code as follows:
- *
- * indep state pt state queues checked and plan viable
- * Unallocated n/a yes
- * Allocated Allocated yes
- * Reserved Reserved yes
- * Confirmed Confirmed yes
- * Erroneous A/R/C no
- *
- * Erroneous exists only after a failed reserve() or confirm() so it's
- * not that confusing to have this slightly malleable terminology.
- */
+#define FSQDN (~(FsqSlot)0)
-typedef struct { /* Allocated Reserved Confirmed */
- /* in queue? absent reserved confirmed */
- Change h;
- PtSlot deadline; /* ~0 relative absolute <- */
- MovPosComb actual; /* undef undef see below */
- int n_motions; /* alloc'd alloc'd undone */
- Motion motions[]; /* [0].i: 0 0 non-0 <- */
- /* [..].i: undef undef non-0 */
- /* .posn: undef undef defined */
-} PointReq;
- /* We can determine the the state by looking at the two
- * `statedet' fields, marked <- above.
- * There are also intermediate states where the req's
- * statedet fields do not agree with the queue it's on.
- * We write these as, for example,
- * AR to mean statedet says Allocated, but queued on pt_reserved
- * A? to mean statedet says Allocated, but may be queued
- * etc. They are only allowed while we are in a pt_... method function.
- */
- /* PointReq.actual is subtly differnet to MovPosChange.actual,
- * as follows:
- * in MovPosChange in PointReq
- * Position unknown -1 0
- * Position partly known -1 unknown feats are 0
- * Position completely known exact exact
- *
- * The partial knowledge positions can only occur in requests that
- * are confirmed with as many motions as features, so we know that
- * if we complete a request we know that we can copy actual out
- * to MovPosChange.
- *
- * If we abandon a half-done change to a multi-feat segment
- * we lose the partial knowledge.
- */
+typedef struct { /* state Prep Resv Inst Resv Prep Conf Inst Conf */
+ Change h; /* in queue? absent reserved absent confirmed */
+ FsqSlot deadline; /* relative relative absolute absolute */
+ int n_motions; /* 1 1 num undone num undone */
+ Motion motions[]; /* [0].i: 0 0 non-0 non-0 */
+ /* .posn: undef undef defined defined */
+} FsqReq;
-#define CDU_RECHARGE 250 /*ms*/
-#define POINT_MOVEMENT 50 /*ms*/
-#define PT_MAX_QUEUE 15
+#define FSQ_MAX_QUEUE 15
typedef struct {
int n;
- PointReq *l[PT_MAX_QUEUE];
-} PointQueue;
+ FsqReq *l[FSQ_MAX_QUEUE];
+} FsqQueue;
-/*
- * CDU and point queue states:
- *
- *
- * ____________ pt_cdu_ conf'd
- * / points_ \ charged .n
- * | all_ |
- * | abaondon |
- * | V
- * |from INACTIVE -1 0
- * |any <=Sta_Settling
- * ^^^^^^^^ (start)
- * |
- * ___________ |turning
- * / \| _on
- * | V
- * | CHARGING 0 any
- * | >=Sta_Resolving
- * | |
- * | |on_pic
- * | |_charged
- * | V
- * ^ READY 1 any
- * | |
- * | |pt_check_action
- * | | fires a point
- * \___________/
- *
- */
+typedef struct FsqMethod FsqMethod;
-static PtSlot pt_cslot;
-static int pt_cdu_charged;
-static PointQueue pt_confirmed, pt_reserved;
+typedef struct {
+ /* set by fsq's client before fsq_init */
+ int slot_ms, lag_ms;
+ void (*move)(FsqMethod *m, const MovFeatInfo *mfi, int movfeatposn);
+ /* shared */
+ int ready; /* >0 means ready; set to 0 by fsq just before move */
+ /* fsq's client must arrange that these start out at all-bits-zero */
+ FsqSlot cslot;
+ FsqQueue confirmed, reserved;
+} FsqState;
+
+struct FsqMethod {
+ Method m;
+ FsqState f;
+ /* client may put things here */
+};
-static void pt_check_action(void);
-static ErrorCode pt_check_plan(void);
+static void fsq_check_action(FsqMethod *m);
+static ErrorCode fsq_check_plan(FsqMethod *m);
-static PtSlotSigned pt_maxdelay_reldeadline(int maxdelay_ms, int n_motions) {
- if (maxdelay_ms==-1) return PT_MAX_QUEUE*16;
- return (maxdelay_ms - POINT_MOVEMENT) / CDU_RECHARGE - n_motions;
+static FsqSlotSigned fsq_maxdelay_reldeadline(FsqMethod *m,
+ int maxdelay_ms, int n_motions) {
+ if (maxdelay_ms==-1) return FSQ_MAX_QUEUE*16;
+ return (maxdelay_ms - m->f.lag_ms) / m->f.slot_ms - n_motions;
}
-static void pt_queue_remove_index(PointQueue *q, int index) {
+static void fsq_queue_remove_index(FsqQueue *q, int index) {
q->n--;
memmove(&q->l[index], &q->l[index+1], sizeof(q->l[0]) * (q->n - index));
}
-static int pt_req_compar(const void *av, const void *bv) {
- PointReq *const *a= av;
- PointReq *const *b= av;
- return (PtSlotSigned)((*b)->deadline - (*a)->deadline);
+static int fsq_req_compar_approx(const void *av, const void *bv) {
+ FsqReq *const *a= av;
+ FsqReq *const *b= av;
+ FsqSlotSigned dldiff= (*b)->deadline - (*a)->deadline;
+ return dldiff;
}
-static void pt_queue_remove_item(PointQueue *q, PointReq *r) {
- PointReq **entry;
- entry= bsearch(r, q->l, q->n, sizeof(q->l[0]), pt_req_compar);
+static void fsq_queue_remove_item(FsqQueue *q, FsqReq *r) {
+ FsqReq **entry;
+ int i;
+ entry= bsearch(r, q->l, q->n, sizeof(q->l[0]), fsq_req_compar_approx);
assert(entry);
- pt_queue_remove_index(q, entry - q->l);
-}
-static void pt_dequeue(PointReq *r) { /* X->XA */
- if (r->motions[0].i) {
- pt_queue_remove_item(&pt_confirmed, r);
- } else if (~r->deadline) {
- pt_queue_remove_item(&pt_reserved, r);
- } else {
- return;
- }
- pt_check_plan();
-}
+#define SEARCH_CHECK do{ \
+ if (q->l[i] == r) goto found; \
+ if (q->l[i]->deadline != r->deadline) break; \
+}while(0)
+ for(i= entry - q->l; i >= 0; i--) SEARCH_CHECK;
+ for(i= entry - q->l + 1; i < q->n; i++) SEARCH_CHECK;
+ abort();
-static void pt_mark_as_allocated(PointReq *r) { /* AX->X */
- /* Sets statedet fields for Allocated */
- r->deadline= ~(PtSlot)0;
- r->motions[0].i=0;
+ found:
+ fsq_queue_remove_index(q, i);
}
+static FsqQueue *fsq_item_queue(FsqMethod *m, FsqReq *r)
+ { return r->motions[0].i ? &m->f.confirmed : &m->f.reserved; }
+
#define WHICH(wh) \
(whichr= wh##r, \
whichwhen= wh##when, \
wh++)
-static ErrorCode pt_check_plan(void) {
+static ErrorCode fsq_check_plan(FsqMethod *m) {
/* Checks whether we can meet the currently queued commitments */
- int future, conf, resv, whichwhen;
- PointReq *whichr;
+ /* if this fails, indep machinery calls fsq_prepare to dequeue */
+ int future, conf, resv, whichwhen, DP;
+ FsqReq *whichr;
conf=resv=0;
future=0;
- oprintf(DUPO("movpos/point") " plan");
+ DPRINTF1(movpos,fsq, "%s plan", m->m.pname);
- /* If CDU is charged we can't do one right away */
- if (!pt_cdu_charged) {
- oprintf(UPO, " +");
+ /* If CDU isn't charged we can't do one right away */
+ if (m->f.ready<0) {
+ DPRINTF2(" +");
future++;
}
for (;;) {
- PointReq *confr= conf < pt_confirmed.n ? pt_confirmed.l[conf] : 0;
- PointReq *resvr= resv < pt_reserved .n ? pt_reserved .l[resv] : 0;
+ FsqReq *confr= conf < m->f.confirmed.n ? m->f.confirmed.l[conf] : 0;
+ FsqReq *resvr= resv < m->f.reserved .n ? m->f.reserved .l[resv] : 0;
if (!confr && !resvr) break;
- oprintf(UPO," %d:",future);
- int confwhen= confr ? confr->deadline - pt_cslot : INT_MAX;
- int resvwhen= resvr ? resvr->deadline : INT_MAX;
+ DPRINTF2(" %d:",future);
+ int confwhen= confr ? confr->deadline - m->f.cslot : INT_MAX;
+ int resvwhen= resvr ? resvr->deadline : INT_MAX;
if (future && resvwhen < confwhen) {
- WHICH(resv);
- oprintf(UPO,"~");
+ WHICH(resv); DPRINTF2("~");
} else if (confr) {
WHICH(conf);
} else {
- oprintf(UPO,"-");
- future++;
+ future++; DPRINTF2("-");
continue;
}
- oprintf(UPO, "%s/%s[%d@t+%d]", whichr->h.move->i->pname,
- posnpname(whichr->h.move, whichr->h.intent),
+ DPRINTF2("%s/%s[%d@t+%d]", whichr->h.indep->move->i->pname,
+ movpos_pname(whichr->h.indep->move, whichr->h.indep->target),
whichr->n_motions, whichwhen);
if (future > whichwhen) {
- oprintf(UPO,"!...bad\n");
+ DPRINTF2("!...bad\n");
return EC_MovFeatTooLate;
}
future += whichr->n_motions;
}
- oprintf(UPO," ok\n");
+ DPRINTF2(" ok\n");
return 0;
}
#undef WHICH
-static ErrorCode pt_enqueue(PointQueue *q, PointReq *r) { /* XA -> X */
- int insat; /* ... where X is R or C and corresponds to q */
- /* or on error, XA -> A */
+static ErrorCode fsq_queue_insert_item(FsqMethod *m, FsqQueue *q, FsqReq *r) {
+ int insat;
- if (q->n == PT_MAX_QUEUE) {
+ if (q->n == FSQ_MAX_QUEUE)
return EC_BufferFull;
- }
for (insat= q->n;
- insat>0 && (PtSlotSigned)(r->deadline - q->l[insat-1]->deadline) < 0;
+ insat>0 && (FsqSlotSigned)(r->deadline - q->l[insat-1]->deadline) < 0;
insat--)
q->l[insat]= q->l[insat-1];
q->l[insat]= r;
q->n++;
- return pt_check_plan();
- /* if this fails, indep machinery calls pt_destroy which dequeues */
+ return 0;
}
-/*---------- kind method entrypoints ----------*/
-
-static Change *point_allocate(int alloc_motions) {
- PointReq *r;
-
- assert(pt_cdu_charged>=0);
- if (!alloc_motions)
- /* we need at least one motion in the table so we can tell
- * the difference between the states by looking at motions[0].i */
- alloc_motions= 1;
-
- r= mmalloc(sizeof(*r) + alloc_motions * sizeof(r->motions[0]));
- r->deadline= ~(PtSlot)0;
- r->n_motions= alloc_motions;
- r->motions[0].i= 0;
- return (Change*)r;
+static void fsq_item_debug(FsqMethod *m, FsqReq *r, const char *opwhat) {
+ int DP;
+ Segment *move= r->h.indep->move;
+ DPRINTF1(movpos,fsq, "%s %s %s", m->m.pname, opwhat, move->i->pname);
+ if (r->motions[0].i) {
+ int i;
+ Motion *mo;
+ for (i=0, mo=r->motions; i<r->n_motions; i++, mo++)
+ DPRINTF2("/%s%d", mo->i->pname, (int)mo->posn);
+ DPRINTF2("\n");
+ } else {
+ DPRINTF2("(%d)\n", r->n_motions);
+ }
+}
+
+static ErrorCode fsq_enqueue(FsqMethod *m, FsqReq *r) { /* P->I; err: P->P */
+ if (!r) return 0;
+ fsq_item_debug(m,r,"enqueue");
+ return fsq_queue_insert_item(m, fsq_item_queue(m,r), r);
}
-static ErrorCode point_reserve(Change *chg, Segment *move,
- int maxdelay_ms) {
- PointReq *r= (PointReq*)chg;
- PtSlotSigned reldeadline;
-
- reldeadline= pt_maxdelay_reldeadline(maxdelay_ms, r->n_motions);
- if (reldeadline <= 0) { pt_mark_as_allocated(r); return EC_MovFeatTooLate; }
- r->deadline= reldeadline;
- return pt_enqueue(&pt_reserved, r);
+static void fsq_dequeue(FsqMethod *m, FsqReq *r) { /* I->P */
+ if (!r) return;
+ fsq_item_debug(m,r,"dequeue");
+ fsq_queue_remove_item(fsq_item_queue(m,r), r);
}
-static ErrorCode point_confirm(Change *chg, Segment *move,
- int n_motions, const Motion *motions,
- int maxdelay_ms) {
- PointReq *r= (PointReq*)chg;
- PtSlotSigned reldeadline;
- int allow_failure;
- ErrorCode ec;
+/*---------- method entrypoints ----------*/
- oprintf(DUPO("movpos/point") "confirm %s n=%d maxdelay=%dms"
- " (res: [%d@t+%d])\n",
- move->i->pname, n_motions, maxdelay_ms,
- r->n_motions, r->deadline);
+static ErrorCode fsq_prepare(Method *mm, const Segment *move,
+ int n_motions, const Motion *motions,
+ int ms, int confirmation,
+ Change **chg_r, int *cost_r) {
+ FsqMethod *m= (void*)mm;
- /* If the segment is moving, these motions are already based on the
- * actual physical position which is stored in the existing request.
- * So we try removing the existing request from the queue and put
- * it back if it doesn't work.
- */
+ assert(n_motions > 0);
- if (n_motions > r->n_motions)
- return EC_MovFeatReservationInapplicable;
- assert(n_motions <= r->n_motions);
- if (maxdelay_ms == -1) {
- reldeadline= r->deadline;
- if (!~r->deadline) reldeadline= pt_maxdelay_reldeadline(-1, n_motions);
- } else {
- reldeadline= pt_maxdelay_reldeadline(maxdelay_ms, n_motions);
- }
- allow_failure= reldeadline < (PtSlotSigned)r->deadline;
- oprintf(DUPO("movpos/point") " reldeadline=[%d@t+%d] allow_failure=%d\n",
- n_motions, reldeadline, allow_failure);
-
- /* state A or R */
- pt_dequeue(r);
- /* states of existing: */
- PointReq *existing=
- move->moving ? (PointReq*)move->motion : 0; /* U or C */
- if (existing) {
- oprintf(DUPO("movpos/point")
- " existing %s n=%d deadline=t+%d\n",
- existing->h.move->i->pname,
- existing->n_motions,
- existing->deadline - pt_cslot);
- pt_dequeue(existing); /* U or CA */
- }
-
- /* state A or RA */
- memcpy(r->motions, motions, sizeof(r->motions[0])*n_motions);
- if (!n_motions) r->motions[0].i= move->i->movfeats;
- assert(r->motions[0].i);
- r->n_motions= n_motions;
- r->deadline= reldeadline + pt_cslot;
+ FsqSlotSigned reldeadline= fsq_maxdelay_reldeadline(m, ms, n_motions);
+ if (reldeadline <= 0) return EC_MovFeatTooLate;
- if (n_motions == move->i->n_movfeats)
- r->actual= 0;
- else
- r->actual= chg->actual;
- assert(r->actual >= 0);
-
- /* state CA */
- ec= pt_enqueue(&pt_confirmed, r);
- oprintf(DUPO("movpos/point") " pt_enqueue=%s\n", ec2str(ec));
- assert(allow_failure || !ec);
-
- if (existing) { /* CA */
- if (ec) { /* state C but bad */
- pt_dequeue(r); /* state CA */
- pt_mark_as_allocated(r); /* state A */
- ErrorCode ec_putback= pt_enqueue(&pt_confirmed, existing);
- assert(!ec_putback); /* C */
- } else { /* state C and good */
- free(existing); /* U */
+ if (chg_r) {
+ int alloc_motions= confirmation ? n_motions : 1;
+ /* we need at least one motion in the table so we can tell
+ * the difference between the states by looking at motions[0].i */
+ FsqReq *r= mmalloc(sizeof(*r) + alloc_motions * sizeof(r->motions[0]));
+ r->n_motions= n_motions;
+ if (confirmation) {
+ r->deadline= reldeadline + m->f.cslot;
+ memcpy(r->motions, motions, sizeof(*r->motions)*n_motions);
+ } else {
+ r->deadline= reldeadline;
+ r->motions[0].i= 0;
}
+ *chg_r= &r->h;
+ }
+ if (cost_r) {
+ *cost_r= n_motions * m->f.slot_ms;
}
- /* either ec=0 state C U
- * or ec!=0 state A C
- * or ec!=0 state C but bad C
- */
-
- if (ec) return ec;
- move->moving= 1;
- move->motion= chg;
- move->movposcomb= -1;
- ouposn_moving(chg);
- pt_check_action();
return 0;
}
-static void point_destroy(Change *chg) { /* X->XA and then free it */
- PointReq *r= (PointReq*)chg;
- pt_dequeue(r);
+static void fsq_dispose(Method *mm, Change *chg) {
+ FsqReq *r= (FsqReq*)chg;
free(r);
}
-/*---------- actually firing points, yay! ----------*/
+static void fsq_remove(Method *mm, Change *remvchg) {
+ FsqMethod *m= (void*)mm;
+ FsqReq *remv= (FsqReq*)remvchg;
-static void pt_check_action(void) {
- PicInsn piob;
+ fsq_dequeue(m, remv);
+}
+
+static ErrorCode fsq_install(Method *mm, Change *instchg) {
+ FsqMethod *m= (void*)mm;
+ FsqReq *inst= (FsqReq*)instchg;
+
+ return fsq_enqueue(m, inst);
+}
+
+static ErrorCode fsq_check(Method *mm) {
+ FsqMethod *m= (void*)mm;
+ ErrorCode ec= fsq_check_plan(m);
+ return ec;
+}
+
+static void fsq_execute(Method *mm) {
+ FsqMethod *m= (void*)mm;
+ fsq_check_action(m);
+}
+
+/*---------- something to do ? ----------*/
+
+static void fsq_check_action(FsqMethod *m) {
+ /* client should call this after it sets ready */
ErrorCode ec;
- if (!pt_confirmed.n) {
+ if (!m->f.confirmed.n) {
if (sta_state == Sta_Finalising) resolve_motioncheck();
return;
}
- PointReq *r= pt_confirmed.l[0];
-
- if (r->n_motions && pt_cdu_charged) {
- /* look for something to fire */
- Motion *m= &r->motions[--r->n_motions];
- assert(m->posn < m->i->posns);
- enco_pic_point(&piob, m->i->boob[m->posn]);
- serial_transmit(&piob);
- oprintf(UPO, "movpos %s point %s%d\n", r->h.move->i->pname,
- m->i->pname, m->posn);
- pt_cdu_charged= 0;
- pt_cslot++;
-
- MovPosComb above_weight= m->i->weight * m->i->posns;
- MovPosComb above= r->actual / above_weight;
- MovPosComb below= r->actual % m->i->weight;
- r->actual= above*above_weight + m->posn*m->i->weight + below;
- if (r->h.actual >= 0 || !r->n_motions)
- r->h.actual= r->actual;
- ouposn_moving(&r->h);
+ FsqReq *r= m->f.confirmed.l[0];
+
+ if (r->n_motions && m->f.ready>0) {
+ /* look for something to move */
+ Motion *mo= &r->motions[--r->n_motions];
+ assert(mo->posn < mo->i->posns);
+ m->f.ready= 0;
+ m->f.move(m, mo->i, mo->posn);
+ m->f.cslot++;
+
+ method_update_feature(&m->m, r->h.indep, mo);
}
if (!r->n_motions) {
- /* look for something to report
- * we can get things here other than from the above
- * eg if we are asked to move the
- */
- Segment *move= r->h.move;
- assert(move->moving && move->motion == (Change*)r);
- pt_queue_remove_index(&pt_confirmed,0);
- pt_mark_as_allocated(r); /* now state A aka Done */
- motion_done(move,r->h.actual);
- free(r);
- ec= pt_check_plan(); assert(!ec);
- pt_check_action();
+ fsq_queue_remove_index(&m->f.confirmed, 0);
+ method_change_done(&m->m, &r->h);
+ m->m.dispose(&m->m, &r->h);
+
+ ec= fsq_check_plan(m); assert(!ec);
+ fsq_check_action(m);
}
}
-/*---------- entrypoints from rest of program ----------*/
+/*========== points ==========*/
-void points_all_abandon(void) {
- int i;
+/*
+ * CDU and point queue states:
+ *
+ *
+ * ____________ conf'd
+ * / points_ \ ready .n
+ * | all_ |
+ * | abandon |
+ * | V
+ * |from INACTIVE -1 0
+ * |any <=Sta_Settling
+ * ^^^^^^^^ (start)
+ * |
+ * ___________ |turning
+ * / \| _on
+ * | V
+ * | CHARGING 0 any
+ * | >=Sta_Resolving
+ * | |
+ * | |on_pic
+ * | |_charged
+ * | V
+ * ^ READY 1 any
+ * | |
+ * | |fsq_check_action
+ * | | calls point_move which fires a point
+ * \___________/
+ *
+ */
- assert(!pt_reserved.n);
+#define CDU_RECHARGE 350 /*ms*/
+#define POINT_MOVEMENT 50 /*ms*/
- for (i=0; i<pt_confirmed.n; i++) {
- PointReq *r= pt_confirmed.l[i];
- Segment *move= r->h.move;
- assert(move->motion == (Change*)r);
- motion_done(move,r->h.actual);
- free(r);
- }
- pt_confirmed.n= 0;
- pt_cdu_charged= -1;
+static ErrorCode point_prepare(Method *mm, const Segment *move,
+ int n_motions, const Motion *motions,
+ int ms, int confirmation,
+ Change **chg_r, int *cost_r) {
+ FsqMethod *m= (void*)mm;
+ assert(m->f.ready>=0);
+ return fsq_prepare(mm,move, n_motions,motions,
+ ms,confirmation, chg_r,cost_r);
+ }
+
+static void point_move(FsqMethod *m, const MovFeatInfo *mfi, int posn) {
+ /* actually firing points, yay! */
+ PicInsn piob;
+ enco_pic_point(&piob, mfi->boob[posn]);
+ serial_transmit(&piob);
}
-void points_turning_on(void) {
- pt_cdu_charged= 0;
+static void points_all_abandon(Method *mm) {
+ FsqMethod *m= (void*)mm;
+ m->f.ready= -1;
}
+static FsqMethod points_method;
+
+void points_turning_on(void) {
+ FsqMethod *m= &points_method;
+ m->f.ready= 0;
+}
void on_pic_charged(const PicInsnInfo *pii, const PicInsn *pi, int objnum) {
- if (pt_cdu_charged<0) return;
- pt_cdu_charged= 1;
- pt_check_action();
+ FsqMethod *m= &points_method;
+ if (m->f.ready<0) return;
+ m->f.ready= 1;
+ fsq_check_action(m);
}
-/*========== dummy `nomove' kind ==========*/
+static FsqMethod points_method= {
+ { "point",
+ point_prepare, fsq_dispose,
+ fsq_install, fsq_remove,
+ fsq_check, fsq_execute, points_all_abandon },
+ { .lag_ms= POINT_MOVEMENT, .slot_ms= CDU_RECHARGE, .move= point_move }
+};
+
+/*========== relays ==========*/
-static Change *nomove_allocate(int alloc_motions) {
- oprintf(DUPO("movfeatkind-momove") "allocate %d\n",alloc_motions);
- return mmalloc(sizeof(Change));
+/*
+ * Waggler states:
+ *
+ * ____________ conf'd
+ * / wagglers_ \ ready .n
+ * | all_ |
+ * | abandon |
+ * | V
+ * |from UNKNOWN -1 0
+ * |any <=Sta_Settling
+ * ^^^^^^^^ (start)
+ * |
+ * ___________ |turning
+ * / \| _on
+ * | V
+ * | CHARGING 0 any
+ * | >=Sta_Resolving
+ * | |
+ * | |on_pic
+ * | |_charged
+ * | V
+ * ^ READY 1 any
+ * | |
+ * | |fsq_check_action
+ * | | calls waggle_do which switches a relay
+ * \___________/
+ *
+ */
+
+static FsqMethod waggle;
+
+static void waggle_do(FsqMethod *m, const MovFeatInfo *mfi, int posn) {
+ /* actually setting relays */
+ PicInsn piob;
+ enco_pic_waggle(&piob, mfi->boob[0], posn);
+ serial_transmit(&piob);
+}
+
+static SegmentNum waggle_settle_seg;
+static int waggle_settle_feat;
+
+static void waggle_settle_check(void) {
+ for (;;) {
+ if (waggle_settle_seg >= info_nsegments) return;
+
+ Segment *seg= &segments[waggle_settle_seg];
+ if (waggle_settle_feat >= seg->i->n_movfeats) {
+ waggle_settle_seg++; waggle_settle_feat=0; continue;
+ }
+
+ const MovFeatInfo *feati= &seg->i->movfeats[waggle_settle_feat];
+ if (feati->kind != mfk_relay) {
+ waggle_settle_feat++; continue;
+ }
+
+ waggle.f.ready= 0;
+ waggle_do(&waggle, feati, (seg->movposcomb / feati->weight) & 1);
+ waggle_settle_feat++;
+ }
+}
+
+void waggle_startup_manual(void) {
+ waggle.f.ready= 1;
+}
+
+void waggle_settle(void) {
+ waggle_settle_seg= 0;
+ waggle_settle_feat= 0;
+ waggle.f.ready= 1;
+ waggle_settle_check();
}
-static void nomove_destroy(Change *chg) {
- free(chg);
+
+void on_pic_waggled(const PicInsnInfo *pii, const PicInsn *pi, int objnum) {
+ if (sta_state == Sta_Settling) {
+ waggle.f.ready= 1;
+ waggle_settle_check();
+ } else if (sta_state >= Sta_Resolving || sta_state == Sta_Manual) {
+ waggle.f.ready= 1;
+ fsq_check_action(&waggle);
+ return;
+ }
}
-static ErrorCode nomove_reserve(Change *chg, Segment *move, int ms) {
- oprintf(DUPO("movfeatkind-nomove") "reserve\n");
+static FsqMethod waggle= {
+ { "relay",
+ fsq_prepare, fsq_dispose,
+ fsq_install, fsq_remove,
+ fsq_check, fsq_execute, ignore_all_abandon },
+ { .lag_ms= 5, .slot_ms= 50, .move= waggle_do }
+};
+
+/*========== dummy `nomove' kind ==========*/
+
+typedef struct NomoveChange {
+ Change h;
+ DLIST_NODE(struct NomoveChange) inqueue;
+ int n_motions; /* 0 for reservations */
+ Motion motions[];
+} NomoveChange;
+
+typedef struct {
+ Method m;
+ NomoveChange *queuehead; /* contains confirmations only */
+} NomoveMethod;
+
+static ErrorCode nomove_prepare(Method *meth_in, const Segment *move,
+ int n_motions, const Motion *motions,
+ int ms, int confirming,
+ Change **chg_r, int *cost_r) {
+ if (chg_r) {
+ NomoveChange *chg;
+ assert(n_motions>0);
+ if (!confirming) n_motions= 0;
+ chg= mmalloc(sizeof(*chg) + sizeof(Motion)*n_motions);
+ chg->n_motions= n_motions;
+ memcpy(chg->motions, motions, sizeof(Motion)*n_motions);
+ }
+ if (cost_r) {
+ *cost_r= 0;
+ }
return 0;
}
-static ErrorCode nomove_confirm(Change *chg, Segment *move, int n_motions,
- const Motion *motions, int ms) {
- oprintf(DUPO("movfeatkind-nomove") "confirm\n");
- nomove_destroy(chg);
+static void nomove_dispose(Method *mm, Change *remvchg) {
+ NomoveChange *remv= (void*)remvchg;
+ free(remv);
+}
+
+static ErrorCode nomove_install(Method *mm, Change *instchg) {
+ NomoveMethod *meth= (void*)mm;
+ NomoveChange *inst= (void*)instchg;
+ if (inst->n_motions)
+ DLIST1_PREPEND(meth->queuehead, inst, inqueue);
return 0;
}
+static void nomove_remove(Method *mm, Change *remvchg) {
+ NomoveMethod *meth= (void*)mm;
+ NomoveChange *remv= (void*)remvchg;
+ if (remv->n_motions)
+ DLIST1_REMOVE(meth->queuehead, remv, inqueue);
+}
+
+static ErrorCode nomove_check(Method *mm) { return 0; }
+
+static void nomove_execute(Method *mm) {
+ NomoveMethod *meth= (void*)mm;
+ NomoveChange *done;
+
+ while ((done= meth->queuehead)) {
+ assert(done->n_motions);
+ int i;
+ for (i=0; i<done->n_motions; i++)
+ method_update_feature(&meth->m, done->h.indep, &done->motions[i]);
+ method_change_done(&meth->m, &done->h);
+ DLIST1_REMOVE(meth->queuehead, done, inqueue);
+ nomove_dispose(&meth->m, &done->h);
+ }
+}
+
+static Method nomove_method= {
+ "nomove",
+ nomove_prepare, nomove_dispose,
+ nomove_install, nomove_remove,
+ nomove_check, nomove_execute, ignore_all_abandon
+};
/*========== method-independent machinery ==========*/
-static const KindInfo methodinfos[]= {
- { "nomove", nomove_allocate, nomove_reserve, nomove_confirm, nomove_destroy },
- { "point", point_allocate, point_reserve, point_confirm, point_destroy },
- { 0 }
+#define INDEP_DBG_FMT "<%p:%s/%s[%d]>"
+#define INDEP_DBG_ARGS(in) (in), \
+ ((in)->move->i->pname), (movpos_pname((in)->move, (in)->target)), \
+ ((in)->n_changes)
+
+#define INDEP_DPFX_FMT "movpos " INDEP_DBG_FMT " "
+#define INDEP_DPFX_ARGS(in) INDEP_DBG_ARGS((in))
+
+#define METH_DPFX_FMT "%s " INDEP_DBG_FMT " "
+#define METH_DPFX_ARGS(indep, meth) ((meth).pname), INDEP_DBG_ARGS((indep))
+
+static void indep_indep_done(Indep *indep);
+
+static Method *methods[]= {
+ [mfk_none] = (Method*)&nomove_method,
+ [mfk_point] = (Method*)&points_method,
+ [mfk_relay] = (Method*)&waggle,
+ 0
};
-static Change *mp_allocate(const KindInfo *ki, Segment *move,
- int alloc_motions, MovPosComb target) {
- assert(sta_state >= Sta_Resolving);
- Change *chg= ki->allocate(alloc_motions);
- chg->ki= ki;
- chg->move= move;
- chg->intent= target;
- return chg;
+/*---------- entrypoints from methods ----------*/
+
+static void method_update_feature(Method *m, MovPosChange *indep,
+ const Motion *mo) {
+ ouposn_feat(indep->move, mo->i, mo->posn, m);
+ if (SOMEP(indep->actual))
+ indep->actual=
+ movposcomb_update_feature(indep->actual, mo->i, mo->posn);
+ ouposn_moving(indep);
+}
+
+static void method_change_done(Method *m, Change *chg) {
+ Indep *indep= chg->indep;
+ Change **search;
+
+ DPRINTF(movpos,meth, METH_DPFX_FMT "method_change_done...\n",
+ METH_DPFX_ARGS(indep,*m));
+
+ for (search=indep->changes; *search; search++)
+ if ((*search) == chg) goto found;
+ assert(!"change in indep");
+
+ found:
+ indep->n_changes--;
+ if (indep->n_changes) {
+ *search= indep->changes[indep->n_changes];
+ return;
+ }
+
+ if (!indep->refcount)
+ indep_indep_done(indep);
+}
+
+static void indep_indep_done(Indep *indep) {
+ /* all done */
+ Segment *move= indep->move;
+ move->moving= 0;
+ move->motion= 0;
+ move->movposcomb= indep->target;
+ ouposn_stable(move);
+ free(indep);
+}
+
+/*---------- internal core machinery ----------*/
+
+static Method *feature_method(const MovFeatInfo *feati) {
+ assert(feati->kind >= 0);
+ assert(feati->kind < ARRAY_SIZE(methods));
+ Method *meth= methods[feati->kind];
+ assert(meth);
+ return meth;
}
static int change_needed(const MovFeatInfo *feati,
MovPosComb startpoint, MovPosComb target) {
int r;
- r= startpoint<0 ||
- (target - startpoint) / feati->weight % feati->posns;
- oprintf(DUPO("movpos/change-needed") "%s:%s(%d*%d) %d..%d => %d\n",
- methodinfos[feati->kind].pname, feati->pname,
- feati->posns, feati->weight,
- startpoint, target, r);
+ r= !SOMEP(startpoint) ||
+ (target / feati->weight) % feati->posns -
+ (startpoint / feati->weight) % feati->posns;
+ if (DEBUGP(movpos,eval))
+ DPRINTFA(" { %s:%s(%d*%d) %d..%d => %d }",
+ feature_method(feati)->pname, feati->pname,
+ feati->posns, feati->weight,
+ startpoint, target, r);
return r;
-}
+}
+
+static void indep_dispose(MovPosChange *indep) {
+ if (!indep) return;
+
+ DPRINTF(movpos,intern, INDEP_DPFX_FMT "dispose...\n",
+ INDEP_DPFX_ARGS(indep));
+
+ int changei;
+ for (changei=0; changei<indep->n_changes; changei++) {
+ Change *chg= indep->changes[changei];
+ if (!chg) continue;
+
+ Method *meth= chg->meth;
+ DPRINTF(movpos,meth, METH_DPFX_FMT "dispose...\n",
+ METH_DPFX_ARGS(indep,*meth));
+ meth->dispose(meth, chg);
+ }
+ free(indep);
+}
+
+#define EVAL_MAX_METHODS 2
+#define EVAL_MAX_MOTIONS 2
+
+static ErrorCode indep_prepare(Segment *move, MovPosComb target,
+ MovPosComb startpoint,
+ int ms, int confirming,
+ MovPosChange **indep_r /* 0 ok */,
+ int *cost_r /* 0 ok */) {
+ static int n_meths;
+ static Method *meths[EVAL_MAX_METHODS];
+ static int n_motions[EVAL_MAX_METHODS];
+ static Motion motions[EVAL_MAX_METHODS][EVAL_MAX_MOTIONS];
-static int evaluate_target(Segment *move, MovPosComb target,
- MovPosComb startpoint, MovFeatKind *kind_r) {
- /* returns number of features which have to change to reach target,
- * or -1 for mixed kinds. kind_r may be 0. */
const SegmentInfo *movei= move->i;
- int feat, tchanges;
- const MovFeatInfo *feati;
- MovFeatKind kind;
+ int feat, DP;
+
+ MovPosChange *indep=0;
- oprintf(DUPO("movpos/eval") "%s/%s <-%s\n",
- move->i->pname, posnpname(move,target), posnpname(move,startpoint));
+ DPRINTF1(movpos,eval, "movpos prepare %s/%s <-%s", move->i->pname,
+ movpos_pname(move,target), movpos_pname(move,startpoint));
- if (startpoint<0) {
+ if (!SOMEP(startpoint)) {
startpoint= movpos_poscomb_actual(move);
- oprintf(DUPO("movpos/eval") " actual <-%s\n",
- posnpname(move,startpoint));
+ DPRINTF2(" actual <-%s", movpos_pname(move,startpoint));
}
- for (feat=0, feati=movei->movfeats, tchanges=0, kind= mfk_none;
- feat<movei->n_movfeats;
- feat++, feati++) {
+ n_meths= 0;
+
+ for (feat=0; feat<movei->n_movfeats; feat++) {
+ const MovFeatInfo *feati= &movei->movfeats[feat];
if (!change_needed(feati,startpoint,target)) continue;
- tchanges++;
- if (kind && feati->kind != kind) return -1;
- kind= feati->kind;
+ MovPosComb posn= target / feati->weight % feati->posns;
+ Method *meth= feature_method(feati);
+
+ int methi;
+ for (methi=0; methi<n_meths; methi++)
+ if (meths[methi] == meth) goto found_method;
+ /* need a new method */
+ methi= n_meths++;
+ if (methi >= EVAL_MAX_METHODS) {
+ DPRINTF2(" MovFeatTooManyMethods methi=%d\n",methi);
+ return EC_MovFeatTooManyMethods;
+ }
+ meths[methi]= meth;
+ n_motions[methi]= 0;
+ DPRINTF2(" meths[%d]=%s", methi,meth->pname);
+
+ found_method:;
+ int motioni= n_motions[methi]++;
+ if (motioni >= EVAL_MAX_MOTIONS) {
+ DPRINTF2(" MovFeatTooManyMotions motioni=%d\n",motioni);
+ return EC_MovFeatTooManyMotions;
+ }
+ DPRINTF2(" motion[%d][%d]=%s%d", methi, motioni, feati->pname,posn);
+ motions[methi][motioni].i= feati;
+ motions[methi][motioni].posn= posn;
+ }
+
+ if (indep_r) {
+ DPRINTF2(" alloc");
+ indep= mmalloc(sizeof(*indep) + sizeof(Change*) * n_meths);
+ indep->move= move;
+ indep->actual= startpoint;
+ indep->target= target;
+ indep->n_changes= n_meths;
+ indep->refcount= 0;
+ memset(indep->changes, 0, sizeof(Change*) * n_meths);
+ }
+ DPRINTF2("\n");
+
+ int totalcost= 0;
+ int changei;
+ ErrorCode ec;
+
+ for (changei=0; changei<n_meths; changei++) {
+ Method *meth= meths[changei];
+ int thiscost= 0;
+ meth->needcheck= 1;
+ meth->needexec= 1;
+
+ if (indep_r)
+ DPRINTF(movpos,meth, METH_DPFX_FMT "prepare n_motions=%d...\n",
+ METH_DPFX_ARGS(indep,*meth), n_motions[changei]);
+ else
+ DPRINTF(movpos,meth, "%s prepare (costing) n_motions=%d...\n",
+ meth->pname, n_motions[changei]);
+
+ ec= meth->prepare(meth,move,
+ n_motions[changei],motions[changei],
+ ms, confirming,
+ indep ? &indep->changes[changei] : 0,
+ &thiscost);
+ if (ec) goto x;
+ if (indep) {
+ Change *chg= indep->changes[changei];
+ chg->meth= meth;
+ chg->indep= indep;
+ chg->installed= 0;
+ }
+ totalcost += thiscost;
+ }
+
+ if (indep_r) *indep_r= indep;
+ if (cost_r) *cost_r= totalcost;
+
+ if (indep_r)
+ DPRINTF(movpos,eval, INDEP_DPFX_FMT "prepare cost=%d ok\n",
+ INDEP_DPFX_ARGS(indep), totalcost);
+ else
+ DPRINTF(movpos,eval, "movpos prepare %s/%s cost=%d ok\n",
+ move->i->pname, movpos_pname(move,target), totalcost);
+ return 0;
+
+ x:
+ indep_dispose(indep);
+ DPRINTF(movpos,entry, "movpos prepare %s/%s err=%s\n", move->i->pname,
+ movpos_pname(move,target), ec2str(ec));
+ return ec;
+}
+
+static void indep_remove(MovPosChange *remv) {
+ if (!remv) return;
+
+ DPRINTF(movpos,intern, INDEP_DPFX_FMT "remove...\n",
+ INDEP_DPFX_ARGS(remv));
+
+ int i;
+ for (i=0; i<remv->n_changes; i++) {
+ Change *chg= remv->changes[i];
+ if (!chg->installed) continue;
+ Method *meth= chg->meth;
+ meth->needexec= 1;
+
+ DPRINTF(movpos,meth, METH_DPFX_FMT "remove...\n",
+ METH_DPFX_ARGS(remv,*meth));
+ meth->remove(meth, chg);
+ chg->installed= 0;
+ }
+}
+
+static ErrorCode
+indep_install(MovPosChange *inst, int checknow) {
+ /* if this fails, inst may be left partially installed */
+ if (!inst) return 0;
+
+ DPRINTF(movpos,intern, INDEP_DPFX_FMT "install checknow=%d...\n",
+ INDEP_DPFX_ARGS(inst), checknow);
+
+ int i;
+ ErrorCode ec;
+ for (i=0; i<inst->n_changes; i++) {
+ Change *chg= inst->changes[i];
+ assert(!chg->installed);
+ Method *meth= chg->meth;
+
+ meth->needexec= 1;
+ ec= meth->install(meth, chg);
+ DPRINTF(movpos,meth, METH_DPFX_FMT "install=%s\n",
+ METH_DPFX_ARGS(inst,*meth), ec2str(ec));
+ if (ec) goto x;
+ chg->installed= 1;
+ meth->needcheck= 1;
+
+ if (checknow) {
+ ec= meth->check(meth);
+ DPRINTF(movpos,meth, METH_DPFX_FMT "check=%s\n",
+ METH_DPFX_ARGS(inst,*meth), ec2str(ec));
+ if (ec) goto x;
+ meth->needcheck= 0;
+ }
}
+ return 0;
+
+ x:
+ return ec;
+}
- if (kind_r) *kind_r= kind;
- oprintf(DUPO("movpos/eval") "changes=%d kind=%s\n",
- tchanges, methodinfos[kind].pname);
- return tchanges;
+static void indep_check_execute(void) {
+ DPRINTF(movpos,intern, "movpos indep_check_execute\n");
+
+ Method **methwalk, *meth;
+ for (methwalk= methods; (meth= *methwalk); methwalk++) {
+ if (meth->needcheck) {
+ ErrorCode ec= meth->check(meth);
+ DPRINTF(movpos,meth, "%s check=%s\n", meth->pname, ec2str(ec));
+ assert(!ec);
+ meth->needcheck= 0;
+ }
+ if (meth->needexec) {
+ meth->needexec= 0;
+ DPRINTF(movpos,meth, "%s execute...\n", meth->pname);
+ meth->execute(meth);
+ }
+ }
+}
+
+/*---------- entrypoints from rest of program ----------*/
+
+ErrorCode
+movpos_reserve(Segment *move, int maxdelay_ms, MovPosChange **res_r,
+ MovPosComb target, MovPosComb startpoint /*as for findcomb*/) {
+ ErrorCode ec;
+ MovPosChange *indep= 0;
+
+ DPRINTF(movpos,entry, "movpos reserve %s/%s maxdelay=%dms startpoint=%s\n",
+ move->i->pname, movpos_pname(move,target),
+ maxdelay_ms, movpos_pname(move,startpoint));
+
+ ec= indep_prepare(move,target, startpoint,
+ maxdelay_ms,0,
+ &indep, 0);
+ if (ec) return ec;
+
+ ec= indep_install(indep, 1);
+ if (ec) goto x;
+
+ indep_check_execute();
+
+ DPRINTF(movpos,entry, "movpos reserve %s/%s ok\n",
+ move->i->pname, movpos_pname(move,target));
+ *res_r= indep;
+ return 0;
+
+ x:
+ indep_remove(indep);
+ indep_dispose(indep);
+ indep_check_execute();
+
+ DPRINTF(movpos,entry, "movpos reserve %s/%s err=%s\n",
+ move->i->pname, movpos_pname(move,target), ec2str(ec));
+ return ec;
}
ErrorCode movpos_findcomb_bysegs(Segment *back, Segment *move, Segment *fwd,
MovPosComb startpoint, MovPosComb *chosen_r) {
const SegmentInfo *movei= move->i;
MovPosComb tcomb, bestcomb=-1;
- int tchanges, bestchanges=INT_MAX;
+ int tcost, bestcost=INT_MAX;
const SegPosCombInfo *pci;
+ DPRINTF(movpos,eval, "movpos_findcomb_bysegs %s-%s-%s <-%s\n",
+ back ? back->i->pname : "*", move->i->pname,
+ fwd ? fwd ->i->pname : "*", movpos_pname(move, startpoint));
+
for (tcomb=0, pci=movei->poscombs;
tcomb<movei->n_poscombs;
tcomb++, pci++) {
+ /* these next assignments may generate segments[-1] but we don't
+ * care because that won't compare equal to back or fwd */
Segment *tback= &segments[pci->link[1].next];
Segment *tfwd= &segments[pci->link[0].next];
+
+ DPRINTF(movpos,intern, "movpos_findcomb_bysegs ... %s : %s-%s-%s\n",
+ movpos_pname(move, tcomb),
+ SOMEP(pci->link[1].next) ? tback->i->pname : "#",
+ move->i->pname,
+ SOMEP(pci->link[0].next) ? tfwd->i->pname : "#");
+
if (back && !(back==tback || back==tfwd)) continue;
if (fwd && !(fwd ==tback || fwd ==tfwd)) continue;
/* we have to search for the one which is least effort */
- tchanges= evaluate_target(move,tcomb,startpoint,0);
-
- if (tchanges==-1)
- /* mixed kinds */
- tchanges= INT_MAX-1;
+ ErrorCode ec= indep_prepare(move,tcomb,startpoint, -1,0, 0,&tcost);
+ if (ec) return ec;
- if (tchanges >= bestchanges) /* prefer low-numbered movposcombs */
+ if (tcost >= bestcost) /* prefer low-numbered movposcombs */
continue;
bestcomb= tcomb;
- bestchanges= tchanges;
+ bestcost= tcost;
}
+ DPRINTF(movpos,entry, "movpos_findcomb_bysegs %s..%s..%s <-%s => %s/%s\n",
+ back ? back->i->pname : "-", move->i->pname,
+ fwd ? fwd ->i->pname : "-", movpos_pname(move, startpoint),
+ move->i->pname, movpos_pname(move,bestcomb));
+
if (chosen_r) *chosen_r= bestcomb;
return
- bestchanges==INT_MAX ? EC_MovFeatRouteNotFound :
- bestchanges==INT_MAX-1 ? EC_MovFeatKindsCombination :
+ bestcost==INT_MAX ? EC_MovFeatRouteNotFound :
0;
}
ErrorCode movpos_change(Segment *move, MovPosComb target,
- int maxdelay_ms, MovPosChange *chg) {
- const SegmentInfo *movei= move->i;
- const MovFeatInfo *feati;
- int feat;
+ int maxdelay_ms, MovPosChange *resv) {
+ int DP;
MovPosComb actual;
ErrorCode ec;
- MovFeatKind kind= mfk_none;
if (!move->moving) {
actual= move->movposcomb;
assert(!move->motion);
} else {
- kind= move->motion->ki - methodinfos;
actual= move->motion->actual;
}
- oprintf(DUPO("movpos/change") "%s/%s maxdelay=%dms actual=%s\n",
- move->i->pname, posnpname(move,target),
- maxdelay_ms, posnpname(move, actual));
- if (chg) oprintf(DUPO("movpos/change") " chg=%s:%s/%s\n",
- chg->ki->pname, chg->move->i->pname,
- posnpname(chg->move, chg->intent));
-
- { /* provide horizon for visibility of motions[] */
- int n_motions=0;
- Motion motions[movei->n_movfeats];
-
- for (feat=0, feati=movei->movfeats;
- feat<movei->n_movfeats;
- feat++, feati++) {
- if (!change_needed(feati,actual,target))
- continue;
- MovPosComb posn= target / feati->weight % feati->posns;
- if (kind) {
- if (feati->kind != kind) { ec= EC_MovFeatKindsCombination; goto x; }
- } else {
- kind= feati->kind;
- }
- motions[n_motions].i= feati;
- motions[n_motions].posn= posn;
- n_motions++;
- }
+ DPRINTF1(movpos,entry, "movpos change %s/%s maxdelay=%dms actual=%s",
+ move->i->pname, movpos_pname(move, target),
+ maxdelay_ms, movpos_pname(move, actual));
+ if (resv) DPRINTF2(" resv=%s/%s",
+ resv->move->i->pname,
+ movpos_pname(resv->move, resv->target));
+ if (move->motion) DPRINTF2(" oldmotion=/%s",
+ movpos_pname(move, move->motion->target));
+ DPRINTF2("\n");
+
+ MovPosChange *inst= 0;
+
+ ec= indep_prepare(move,target, actual,
+ maxdelay_ms,1,
+ &inst, 0);
+ if (ec) goto x;
- const KindInfo *ki= &methodinfos[kind];
+ indep_remove(resv);
+ indep_remove(move->motion);;
- if (chg) {
- if (chg->ki != ki ||
- chg->move != move ||
- chg->intent != target)
- return EC_MovFeatReservationInapplicable;
- } else {
- chg= mp_allocate(ki,move,n_motions,target);
- }
- chg->actual= actual;
+ ec= indep_install(inst, 1);
+ if (ec) goto x;
- oprintf(DUPO("movpos/change") "confirm %s:%d...\n", ki->pname, n_motions);
- ec= ki->confirm(chg, move, n_motions, motions, maxdelay_ms);
- oprintf(DUPO("movpos/change") "confirm => %s\n",errorcodelist[ec]);
- if (ec) goto x;
- }
- return 0;
+ indep_dispose(resv);
+ indep_dispose(move->motion);
- x:
- movpos_unreserve(chg);
- return ec;
-}
-
-ErrorCode
-movpos_reserve(Segment *move, int maxdelay_ms, MovPosChange **res_r,
- MovPosComb target, MovPosComb startpoint /*as for findcomb*/) {
- MovFeatKind kind= mfk_none;
- ErrorCode ec;
- int nchanges;
+ move->motion= inst;
+ move->moving= 1;
- oprintf(DUPO("movpos/reserve") "%s/%s maxdelay=%dms startpoint=%s\n",
- move->i->pname, posnpname(move,target),
- maxdelay_ms, posnpname(move,startpoint));
+ inst->refcount++; /* prevents method_change_done from destroying it */
- nchanges= evaluate_target(move,target,startpoint,&kind);
- if (nchanges==-1) return EC_MovFeatKindsCombination;
+ ouposn_moving(inst);
+ indep_check_execute();
- const KindInfo *ki= &methodinfos[kind];
- oprintf(DUPO("movpos/reserve") "allocate %s:%d...\n", ki->pname, nchanges);
- Change *chg= mp_allocate(ki, move, nchanges, target);
- ec= ki->reserve(chg, move, maxdelay_ms);
- oprintf(DUPO("movpos/reserve") "reserve => %s\n",errorcodelist[ec]);
- if (ec) goto x;
+ inst->refcount--;
+ if (!inst->n_changes)
+ /* oh! */
+ indep_indep_done(inst);
- *res_r= chg;
+ DPRINTF(movpos,entry, "movpos change %s/%s ok\n",
+ move->i->pname, movpos_pname(move, target));
return 0;
x:
- movpos_unreserve(chg);
+ indep_remove(inst);
+ indep_dispose(inst);
+ ec= indep_install(move->motion, 0); assert(!ec);
+ ec= indep_install(resv, 0); assert(!ec);
+ indep_check_execute();
+
+ DPRINTF(movpos,entry, "movpos change %s/%s err=%s\n",
+ move->i->pname, movpos_pname(move, target), ec2str(ec));
return ec;
}
-void movpos_unreserve(MovPosChange *res) {
- if (!res) return;
- oprintf(DUPO("movpos/unreserve") "%s:%s/%s\n",
- res->ki->pname, res->move->i->pname,
- posnpname(res->move, res->intent));
- res->ki->destroy(res);
+void movpos_unreserve(MovPosChange *resv) {
+ if (!resv) return;
+ DPRINTF(movpos,entry, "movpos unreserve %s/%s...\n",
+ resv->move->i->pname, movpos_pname(resv->move, resv->target));
+ indep_remove(resv);
+ indep_dispose(resv);
+ indep_check_execute();
}
-MovPosComb movpos_poscomb_actual(Segment *seg) {
- return seg->moving ? seg->motion->actual : seg->movposcomb;
+MovPosComb movpos_change_intent(MovPosChange *indep) {
+ return indep->target;
}
-MovPosComb movpos_change_intent(MovPosChange *chg) {
- return chg->intent;
+void motions_all_abandon(void) {
+ Method **meth;
+ SEG_IV;
+ DPRINTF(movpos,entry, "movpos motions_all_abandon...\n");
+ FOR_SEG {
+ if (!seg->moving) continue;
+
+ MovPosChange *abandon= seg->motion;
+ indep_remove(abandon);
+ seg->movposcomb= abandon->actual;
+ seg->moving= 0;
+ seg->motion= 0;
+ indep_dispose(abandon);
+ }
+ for (meth=methods; *meth; meth++)
+ (*meth)->all_abandon(*meth);
}
~ijackson / trains.git / blobdiff