typedef struct Method Method;
+struct MovPosChange {
+ Segment *move;
+ /* everything beyond here is private for indep */
+ MovPosComb actual, target;
+ int n_changes;
+ Change *changes[];
+};
+
/* 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
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;
/*========== general utility functions ==========*/
const char *movpos_pname(Segment *move, MovPosComb poscomb) {
- return poscomb<0 ? "?" : move->i->poscombs[poscomb].pname;
+ return !SOMEP(poscomb) ? "?" : move->i->poscombs[poscomb].pname;
}
static void ouposn_moving(Change *chg) {
}
static void fsq_item_debug(FsqMethod *m, FsqReq *r,
- const char *pfx, Segment *move) {
- DPRINTF1(movpos,fsq, "%s%s", pfx, move.pname);
+ const char *opwhat, Segment *move) {
+ int DP;
+ DPRINTF1("%s %s %s", m->m.pname, opwhat, move.pname);
if (r->motions[0].i) {
for (int i=0, Motion *mo=r->motions; i<r->n_motions; i++, mo++)
- DPRINTF1(movpos,fsq, "/%s%d", mo->i->pname, (int)mo->posn);
+ DPRINTF2("/%s%d", mo->i->pname, (int)mo->posn);
+ DPRINTF2("\n");
} else {
- DPRINTF1(movpos,fsq, "(%d)", r->n_motions);
+ 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:",r->h.indep.move);
+ fsq_item_debug(m,r,"enqueue",r->h.indep.move);
return fsq_queue_insert_item(fsq_item_queue(m,r), r);
}
static void fsq_dequeue(FsqMethod *m, FsqReq *r) { /* I->P */
if (!r) return 0;
- fsq_item_debug(m,r,"dequeue:",r->h.indep.move);
+ fsq_item_debug(m,r,"dequeue",r->h.indep.move);
fsq_remove_item(fsq_item_queue(m,r), r);
}
static ErrorCode fsq_check(Method *mm) {
FsqMethod *m= (void*)mm;
ErrorCode ec= fsq_check_plan(m);
- DPRINTF(movpos,fsq, "%s check=%s\n", m->m.pname, ec2str(ec));
return ec;
}
static void fsq_execute(Method *mm) {
FsqMethod *m= (void*)mm;
- DPRINTF(movpos,fsq, "%s execute\n", m->m.pname);
fsq_check_action(m);
}
/*========== method-independent machinery ==========*/
-struct MovPosChange {
- Segment *move;
- MovPosComb actual, target;
- unsigned installed:1;
- int n_changes;
- Change *changes[];
-};
+#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 Method *methods[]= {
[mfk_none] = (Method*)&nomove_method,
MovPosComb *indep= chg->indep;
Change **search;
+ DPRINTF(movpos,meth, METH_DPFX_FMT "method_change_done...\n",
+ METH_DPFX_ARGS(indep, m->m));
+
for (search=indep->changes; *search; search++)
if ((*search) == chg) goto found;
assert(!"change in indep");
int r;
r= !SOMEP(startpoint) ||
(target - startpoint) / feati->weight % feati->posns;
- DPRINTF(movpos,changeneeded, "%s:%s(%d*%d) %d..%d => %d\n",
- feature_method(feati)->pname, feati->pname,
- feati->posns, feati->weight,
- startpoint, target, r);
+ 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 Motion meths[EVAL_MAX_METHODS][EVAL_MAX_MOTIONS];
const SegmentInfo *movei= move->i;
- int feat;
+ int feat, DP;
const MovFeatInfo *feati;
MovPosChange *indep=0;
- DPRINTF(movpos,eval, "%s/%s <-%s", move->i->pname,
- movpos_pname(move,target), movpos_pname(move,startpoint));
+ DPRINTF1(movpos,eval, "movpos prepare %s/%s <-%s", move->i->pname,
+ movpos_pname(move,target), movpos_pname(move,startpoint));
if (!SOMEP(startpoint)) {
startpoint= movpos_poscomb_actual(move);
- DPRINTF(movpos,eval, " actual <-%s\n",
- movpos_pname(move,startpoint));
+ DPRINTF2(" actual <-%s", movpos_pname(move,startpoint));
}
n_meths= 0;
if (methi >= EVAL_MAX_METHODS) return EC_MovFeatTooManyMethods;
meths[methi]= meth;
n_motions[methi]= 0;
- DPRINTF(movpos,eval, " meths[%d]=%s", methi,meth->pname);
+ DPRINTF2(" meths[%d]=%s", methi,meth->pname);
found_method:
int motioni= ++n_motions[methi];
if (motioni >= EVAL_MAX_MOTIONS) return EC_MovFeatTooManyMotions;
- DPRINTF(movpos,eval, " motion[%d][%d]=%s%d",
- methi,motioni,feati->pname,posn);
+ DPRINTF2(" motion[%d][%d]=%s%d", methi, motioni, feati->pname,posn);
motions[methi][motioni].i= feati;
motions[methi][motioni].posn= posn;
}
if (indep_r) {
- DPRINTF(movpos,eval, " alloc");
+ DPRINTF2(" alloc");
indep= mmalloc(sizeof(*indep) + sizeof(Change*) * n_meths);
memset(indep->changes, 0, sizeof(Change*) * n_meths);
indep->move= move;
indep->target= target;
indep->considering= 0;
}
- DPRINTF(movpos,eval, "\n");
+ DPRINTF2("\n");
int totalcost= 0;
int thiscost= 0;
meth->needcheck= 1;
meth->needexec= 1;
+
+ if (indep_r)
+ DPRINTF(movpos,meth, METH_DPFX_FMT "prepare n_motions=%d...\n",
+ INDEP_DPFX_ARGS(indep, m->m), n_motions[changei]);
+ else
+ DPRINTF(movpos,meth, "%s prepare (costing) n_motions=%d...\n",
+ m->m.pname, n_motions[changei]);
+
ec= meth->prepare(meth,move,
n_motions[changei],motions[changei],
ms, confirming,
if (indep_r) *indep_r= indep;
if (cost_r) *cost_r= totalcost;
- DPRINTF(movpos,eval, "%s/%s ok cost=%d\n", move->i->pname,
- movpos_pname(move,target), totalcost);
+
+ if (indep_r)
+ DPRINTF(movpos,entry, INDEP_DPFX_FMT "prepare cost=%d ok\n",
+ INDEP_DPFX_ARGS(indep), totalcost);
+ else
+ DPRINTF(movpos,entry, "movpos prepare %s/%s cost=%d ok\n",
+ move->i->pname, movpos_pname(move,target), totalcost);
return 0;
x:
- DPRINTF(movpos,eval, "%s/%s abandon %s\n", move->i->pname,
- movpos_pname(move,target), totalcost, ec2str(ec));
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(indep));
+
for (i=0; i<remv->n_changes; i++) {
Change *chg= inst->changes[i];
if (!chg->installed) continue;
Method *meth= chg->meth;
meth->needexec= 1;
+
+ DPRINTF(movpos,meth, METH_DPFX_FMT "remove...\n",
+ METH_DPFX_ARGS(indep,m->m));
meth->remove(meth, chg);
+ chg->installed= 0;
}
}
static ErrorCode
-indep_install(MovPosChange *inst, int needcheck) {
+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(indep), checknow);
+
for (i=0; i<inst->n_changes; i++) {
Change *chg= inst->changes[n_installed];
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,m->m), ec2str(ec));
if (ec) goto x;
chg->installed= 1;
+ meth->needcheck= 1;
- if (needcheck) {
- meth->needcheck= 1;
+ if (checknow) {
ec= meth->check(meth);
+ DPRINTF(movpos,meth, METH_DPFX_FMT "check=%s\n",
+ METH_DPFX_ARGS(inst,m->m), ec2str(ec));
if (ec) goto x;
meth->needcheck= 0;
}
}
- inst->installed= 1;
x:
return ec;
}
static void indep_check_execute(void) {
+ DPRINTF(movpos,intern, "movpos indep_check_execute\n");
+
for (Method **methwalk= methods;
(meth= *methwalk);
methwalk++) {
if (meth->needcheck) {
ec= meth->check(meth);
+ DPRINTF(movpos,meth, "%s check=%s\n", m->m.pname, ec2str(ec));
assert(!ec);
meth->needcheck= 0;
}
if (meth->needexec) {
meth->needexec= 0;
+ DPRINTF(movpos,meth, "%s execute...\n", m->m.pname);
meth->execute(meth);
}
}
static void indep_dispose(MovPosChange *indep) {
if (!indep) return;
+ DPRINTF(movpos,intern, INDEP_DPFX_FMT "dispose...\n",
+ INDEP_DPFX_ARGS(indep));
+
for (int changei=0; changei<indep->n_changes; changei++) {
Change *chg= indep->changes[changei];
Method *meth= chg->meth;
- if (chg)
+ if (chg) {
+ DPRINTF(movpos,meth, METH_DPFX_FMT "dispose...\n",
+ INDEP_DPFX_ARGS(indep,m->m));
meth->dispose(meth, chg);
+ }
}
free(indep);
}
ErrorCode ec;
MovPosChange *indep= 0;
- DPRINTF(movpos,reserve, "%s/%s maxdelay=%dms startpoint=%s\n",
+ 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));
if (ec) goto x;
indep_check_execute();
+
+ DPRINTF(movpos,reserve, "movpos reserve %s/%s ok\n",
+ move->i->pname, movpos_pname(move,target));
return 0;
x:
indep_remove(indep);
indep_dispose(indep);
indep_check_execute();
+
+ DPRINTF(movpos,reserve, "movpos reserve %s/%s err=%s\n",
+ move->i->pname, movpos_pname(move,target), ec2str(ec));
return ec;
}
int maxdelay_ms, MovPosChange *resv) {
const SegmentInfo *movei= move->i;
const MovFeatInfo *feati;
- int feat;
+ int feat, DP;
MovPosComb actual;
ErrorCode ec;
actual= move->motion->actual;
}
- DPRINTF(movpos,change, "%s/%s maxdelay=%dms actual=%s\n",
- move->i->pname, movpos_pname(move, target),
- maxdelay_ms, movpos_pname(move, actual));
- if (resv) DPRINTF(movpos,change, " resv=%s:%s/%s\n",
- resv->meth->pname, resv->move->i->pname,
- movpos_pname(resv->move, resv->target));
+ 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(movpos,entry, " resv=%s:%s/%s",
+ resv->meth->pname, resv->move->i->pname,
+ movpos_pname(resv->move, resv->target));
+ if (move->motion) DPRINTF2(movpos,entry, " oldmotion=%s:/%s",
+ move->motion->meth->pname,
+ movpos_pname(move, move->motion->target));
+ DPRINTF2("\n");
MovPosChange *inst= 0;
indep_check_execute();
- DPRINTF(movpos,change, "%s/%s maxdelay=%dms ok\n",
- move->i->pname, movpos_pname(move, target),
- maxdelay_ms);
+ DPRINTF(movpos,change, "movpos change %s/%s ok\n",
+ move->i->pname, movpos_pname(move, target));
return 0;
x:
ec= indep_install(resv, 0); assert(!ec);
indep_check_execute();
- DPRINTF(movpos,change, "%s/%s maxdelay=%dms err=%s\n",
- move->i->pname, movpos_pname(move, target),
- maxdelay_ms, ec2str(ec));
+ 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 *resv) {
if (!res) return;
- DPRINTF(movpos,unreserve, "%s:%s/%s\n",
- res->meth->pname, res->move->i->pname,
- movpos_pname(res->move, res->intent));
+ DPRINTF(movpos,entry, "movpos unreserve %s/%s...\n",
+ res->move->i->pname, movpos_pname(res->move, res->intent));
indep_remove(resv);
indep_dispose(resv);
indep_check_execute();
void motions_all_abandon(void) {
Method **meth;
- SEG_IV
+ SEG_IV;
+ DPRINTF(movpos,entry, "movpos motions_all_abandon...\n");
FOR_SEG {
if (!seg->moving) continue;
for (meth=methods; *meth; meth++)
(*meth)->all_abandon(*meth);
}
-
-
-
-
-
-
-
- STUFF BEYOND HERE IS JUST KEPT FOR GETTING DPRINTFS OUT OF
-
-
-static ErrorCode fsq_confirm(Method *mm, Change *chg, Segment *move,
- int n_motions, const Motion *motions,
- int maxdelay_ms) {
- FsqMethod *m= (void*)mm;
- FsqReq *r= (FsqReq*)chg;
- FsqSlotSigned reldeadline;
- int allow_failure, DP;
- ErrorCode ec;
-
- DPRINTF1(movpos,fsq, "%s confirm %s n=%d maxdelay=%dms",
- m->m.pname, move->i->pname, n_motions, maxdelay_ms);
-
- assert(!r->motions[0].i); /* no confirming things already confirmed */
- if (r->deadline==FSQDN)
- DPRINTF2(" (alloc'd: %d)\n", r->n_motions);
- else
- DPRINTF2(" (res: %s/%s[%d@t+%d])\n",
- r->h.move->i->pname, movpos_pname(r->h.move, r->h.intent),
- r->n_motions, r->deadline);
-
- /* 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.
- */
-
- if (n_motions > r->n_motions)
- return EC_MovFeatReservationInapplicable;
- assert(n_motions <= r->n_motions);
- if (maxdelay_ms == -1) {
- reldeadline= r->deadline;
- if (reldeadline==FSQDN)
- reldeadline= fsq_maxdelay_reldeadline(m, -1, n_motions);
- } else {
- reldeadline= fsq_maxdelay_reldeadline(m, maxdelay_ms, n_motions);
- }
- allow_failure= reldeadline < (FsqSlotSigned)r->deadline;
- DPRINTF(movpos,fsq, "%s reldeadline=[%d@t+%d] allow_failure=%d\n",
- m->m.pname, n_motions, reldeadline, allow_failure);
-
- /* state A or R */
- fsq_dequeue(m, r);
- /* states of existing: */
- FsqReq *existing=
- move->moving ? (FsqReq*)move->motion : 0; /* U or C */
- if (existing) {
- DPRINTF(movpos,fsq,
- "%s existing %s n=%d deadline=t+%d\n",
- m->m.pname,
- existing->h.move->i->pname,
- existing->n_motions,
- existing->deadline - m->f.cslot);
- fsq_dequeue(m, 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 + m->f.cslot;
-
- if (n_motions == move->i->n_movfeats)
- r->actual= 0;
- else
- r->actual= chg->actual;
- assert(r->actual >= 0);
-
- /* state CA */
- ec= fsq_enqueue(m, &m->f.confirmed, r);
- DPRINTF(movpos,fsq, "%s fsq_enqueue=%s\n", m->m.pname, ec2str(ec));
- assert(allow_failure || !ec);
-
- if (existing) { /* CA */
- if (ec) { /* state C but bad */
- fsq_dequeue(m,r); /* state CA */
- fsq_mark_as_allocated(r); /* state A */
- ErrorCode ec_putback= fsq_enqueue(m,&m->f.confirmed, existing);
- assert(!ec_putback); /* C */
- } else { /* state C and good */
- free(existing); /* U */
- }
- }
- /* 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);
- fsq_check_action(m);
- return 0;
-}
-
-
-
-/*---------- entrypoints from rest of program ----------*/
-
-static void fsq_all_abandon(Method *mm) {
- FsqMethod *m= (void*)mm;
- int i;
-
- assert(!m->f.reserved.n);
-
- for (i=0; i<m->f.confirmed.n; i++) {
- indep_
- FsqReq *r= m->f.confirmed.l[i];
- Segment *move= r->h.move;
- assert(move->motion == (Change*)r);
- motion_done(move,r->h.actual);
- free(r);
- }
- m->f.confirmed.n= 0;
-}
-
-in points_all_abandon
- fsq_all_abandon(mm);
-
-
- *
-
- * seg->moving and ->motion is in one of the states UC
-
-static Change *mp_allocate(Method *meth, Segment *move,
- int alloc_motions, MovPosComb target) {
- assert(sta_state >= Sta_Resolving || sta_state == Sta_Manual);
- Change *chg= meth->allocate(meth, alloc_motions);
- chg->meth= meth;
- chg->move= move;
- chg->intent= target;
- return chg;
-}
-