} Motion;
typedef struct Method Method;
+typedef struct Change Change;
-struct MovPosChange {
+typedef struct MovPosChange {
Segment *move;
/* everything beyond here is private for indep */
MovPosComb actual, target;
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: must NOT be called from within a call to the method's
+ * execute() (and 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
* No Changes remain Prepared while the Method is Executing.
*/
-typedef struct Change Change;
-
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;
+};
struct Method {
const char *pname;
- unsigned needcheck, needexec; /* used by indep to track T/Y/E */
ErrorCode (*prepare)(Method *m, /* TYE->T */
- Segment *move,
+ const Segment *move,
int n_motions, const Motion *motions,
- int ms, int confirming,
- Change *chg_r, /* 0->P; err: 0->0; may be 0 */
+ 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 (*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 */
};
/*========== general utility functions ==========*/
-const char *movpos_pname(Segment *move, MovPosComb poscomb) {
+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;
+static void ouposn_moving(const MovPosChange *indep) {
+ Segment *move= indep->move;
ouprintf("movpos %s position %s moving\n",
- move->i->pname, movpos_pname(move, chg->actual));
+ move->i->pname, movpos_pname(move, indep->actual));
}
MovPosComb movposcomb_update_feature(MovPosComb startpoint,
return above*above_weight + featpos*mfi->weight + below;
}
-MovPosComb movpos_poscomb_actual(Segment *seg) {
+MovPosComb movpos_poscomb_actual(const Segment *seg) {
return seg->moving ? seg->motion->actual : seg->movposcomb;
}
future++; DPRINTF2("-");
continue;
}
- DPRINTF2("%s/%s[%d@t+%d]", whichr->h.move->i->pname,
- movpos_pname(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) {
DPRINTF2("!...bad\n");
q->l[insat]= q->l[insat-1];
q->l[insat]= r;
q->n++;
+
+ return 0;
}
-static void fsq_item_debug(FsqMethod *m, FsqReq *r,
- const char *opwhat, Segment *move) {
+static void fsq_item_debug(FsqMethod *m, FsqReq *r, const char *opwhat) {
int DP;
- DPRINTF1("%s %s %s", m->m.pname, opwhat, move.pname);
+ Segment *move= r->h.indep->move;
+ DPRINTF1(movpos,fsq, "%s %s %s", m->m.pname, opwhat, move->i->pname);
if (r->motions[0].i) {
- for (int i=0, Motion *mo=r->motions; i<r->n_motions; i++, mo++)
+ 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 {
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);
- return fsq_queue_insert_item(fsq_item_queue(m,r), r);
+ fsq_item_debug(m,r,"enqueue");
+ return fsq_queue_insert_item(m, 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_remove_item(fsq_item_queue(m,r), r);
+ if (!r) return;
+ fsq_item_debug(m,r,"dequeue");
+ fsq_queue_remove_item(fsq_item_queue(m,r), r);
}
/*---------- method entrypoints ----------*/
-static ErrorCode fsq_prepare(Method *mm, Segment *move,
+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) {
+ Change **chg_r, int *cost_r) {
FsqMethod *m= (void*)mm;
assert(n_motions > 0);
- FsqSlotSigned reldeadline= fsq_maxdelay_reldeadline(m, ms, r->n_motions);
+ FsqSlotSigned reldeadline= fsq_maxdelay_reldeadline(m, ms, n_motions);
if (reldeadline <= 0) return EC_MovFeatTooLate;
if (chg_r) {
r->n_motions= n_motions;
if (confirmation) {
r->deadline= reldeadline + m->f.cslot;
- memcpy(r->motions, motions, sizeof(*r->motions)*motions);
+ 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->slot_ms;
+ *cost_r= n_motions * m->f.slot_ms;
}
return 0;
}
static void fsq_dispose(Method *mm, Change *chg) {
- FsqMethod *m= (void*)mm;
FsqReq *r= (FsqReq*)chg;
free(r);
}
-static void fsq_remove(Method *mm, Change *instchg) {
+static void fsq_remove(Method *mm, Change *remvchg) {
FsqMethod *m= (void*)mm;
FsqReq *remv= (FsqReq*)remvchg;
if (!r->n_motions) {
fsq_queue_remove_index(&m->f.confirmed, 0);
method_change_done(&m->m, &r->h);
- m->h.dispose(&m->m, &r->h);
+ m->m.dispose(&m->m, &r->h);
ec= fsq_check_plan(m); assert(!ec);
fsq_check_action(m);
#define CDU_RECHARGE 350 /*ms*/
#define POINT_MOVEMENT 50 /*ms*/
-static ErrorCode point_prepare(Method *mm, Segment *move,
+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) {
+ Change **chg_r, int *cost_r) {
FsqMethod *m= (void*)mm;
assert(m->f.ready>=0);
return fsq_prepare(mm,move, n_motions,motions,
}
static FsqMethod points_method= {
- { "point", mfk_point,
+ { "point",
point_prepare, fsq_dispose,
fsq_install, fsq_remove,
fsq_check, fsq_execute, points_all_abandon },
}
static FsqMethod waggle= {
- { "relay", mfk_relay,
+ { "relay",
fsq_prepare, fsq_dispose,
fsq_install, fsq_remove,
fsq_check, fsq_execute, ignore_all_abandon },
typedef struct NomoveChange {
Change h;
- DLIST_NODE inqueue;
- int n_motions;
+ DLIST_NODE(struct NomoveChange) inqueue;
+ int n_motions; /* 0 for reservations */
Motion motions[];
-};
+} NomoveChange;
typedef struct {
Method m;
unsigned eventqueued:1;
- NomoveChange *queuehead;
+ NomoveChange *queuehead; /* contains confirmations only */
} NomoveMethod;
-static ErrorCode nomove_prepare(Method *meth_in, Segment *move,
+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) {
+ 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);
static ErrorCode nomove_install(Method *mm, Change *instchg) {
NomoveMethod *meth= (void*)mm;
NomoveChange *inst= (void*)instchg;
- DLIST1_PREPEND(meth->queuehead, inst, inqueue);
+ 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;
- DLIST1_REMOVE(meth->queuehead, remv, inqueue);
+ if (remv->n_motions)
+ DLIST1_REMOVE(meth->queuehead, remv, inqueue);
}
static ErrorCode nomove_check(Method *mm) { return 0; }
-static void (*nomove_execute_now)(oop_source *source, struct timeval tv,
- void *meth_v) {
+static void *nomove_execute_now(oop_source *source, struct timeval tv,
+ void *meth_v) {
NomoveMethod *meth= meth_v;
meth->eventqueued= 0;
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->indep, &done->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);
}
+ return OOP_CONTINUE;
}
+
static void nomove_execute(Method *mm) {
NomoveMethod *meth= (void*)mm;
if (!meth->eventqueued) {
}
static Method nomove_method= {
- "nomove", mfk_none,
+ "nomove",
nomove_prepare, nomove_dispose,
nomove_install, nomove_remove,
nomove_check, nomove_execute, ignore_all_abandon
#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,
+ ((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))
static void method_update_feature(Method *m, MovPosChange *indep,
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.
- */
ouprintf("movpos %s feat %s %d %s\n", indep->move->i->pname,
- mo->i->pname, mo->posn, m.pname);
+ mo->i->pname, mo->posn, m->pname);
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) {
- /* 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: must NOT be called from within a call to the method's
- * execute() (and of course cannot legally be called from within
- * prepare, consider, check or dispose).
- */
- MovPosComb *indep= chg->indep;
+ Indep *indep= chg->indep;
Change **search;
DPRINTF(movpos,meth, METH_DPFX_FMT "method_change_done...\n",
- METH_DPFX_ARGS(indep, m->m));
+ METH_DPFX_ARGS(indep,*m));
for (search=indep->changes; *search; search++)
if ((*search) == chg) goto found;
found:
indep->n_changes--;
if (indep->n_changes) {
- *search= indep[n_changes];
+ *search= indep->changes[indep->n_changes];
return;
}
/* all done */
+ Segment *move= indep->move;
move->moving= 0;
move->motion= 0;
move->movposcomb= indep->target;
/*---------- internal core machinery ----------*/
-static Method *feature_method(MovFeatInfo *feati) {
+static Method *feature_method(const MovFeatInfo *feati) {
assert(feati->kind >= 0);
assert(feati->kind < ARRAY_SIZE(methods));
Method *meth= methods[feati->kind];
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];
+ Method *meth= chg->meth;
+ if (chg) {
+ 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 */,
+ 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 meths[EVAL_MAX_METHODS][EVAL_MAX_MOTIONS];
+ static Motion motions[EVAL_MAX_METHODS][EVAL_MAX_MOTIONS];
const SegmentInfo *movei= move->i;
int feat, DP;
- const MovFeatInfo *feati;
MovPosChange *indep=0;
n_meths= 0;
- for (feat=0, feati=movei->movfeats, tchanges=0, kind= mfk_none;
- feat<movei->n_movfeats;
- feat++, feati++) {
+ for (feat=0; feat<movei->n_movfeats; feat++) {
+ const MovFeatInfo *feati= &movei->movfeats[feat];
if (!change_needed(feati,startpoint,target)) continue;
MovPosComb posn= target / feati->weight % feati->posns;
Method *meth= feature_method(feati);
n_motions[methi]= 0;
DPRINTF2(" meths[%d]=%s", methi,meth->pname);
- found_method:
+ found_method:;
int motioni= ++n_motions[methi];
if (motioni >= EVAL_MAX_MOTIONS) return EC_MovFeatTooManyMotions;
DPRINTF2(" motion[%d][%d]=%s%d", methi, motioni, feati->pname,posn);
indep->move= move;
indep->actual= startpoint;
indep->target= target;
- indep->considering= 0;
}
DPRINTF2("\n");
int totalcost= 0;
+ int changei;
+ ErrorCode ec;
- for (int changei=0; changei<n_meths; changei++) {
+ for (changei=0; changei<n_meths; changei++) {
Method *meth= meths[changei];
int thiscost= 0;
meth->needcheck= 1;
if (indep_r)
DPRINTF(movpos,meth, METH_DPFX_FMT "prepare n_motions=%d...\n",
- INDEP_DPFX_ARGS(indep, m->m), n_motions[changei]);
+ METH_DPFX_ARGS(indep,*meth), n_motions[changei]);
else
DPRINTF(movpos,meth, "%s prepare (costing) n_motions=%d...\n",
- m->m.pname, n_motions[changei]);
+ meth->pname, n_motions[changei]);
ec= meth->prepare(meth,move,
n_motions[changei],motions[changei],
ms, confirming,
indep ? &indep->changes[changei] : 0,
- thiscost);
+ &thiscost);
if (ec) goto x;
if (indep) {
Change *chg= indep->changes[changei];
if (!remv) return;
DPRINTF(movpos,intern, INDEP_DPFX_FMT "remove...\n",
- INDEP_DPFX_ARGS(indep));
+ INDEP_DPFX_ARGS(remv));
+ int i;
for (i=0; i<remv->n_changes; i++) {
- Change *chg= inst->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(indep,m->m));
+ METH_DPFX_ARGS(remv,*meth));
meth->remove(meth, chg);
chg->installed= 0;
}
if (!inst) return 0;
DPRINTF(movpos,intern, INDEP_DPFX_FMT "install checknow=%d...\n",
- INDEP_DPFX_ARGS(indep), checknow);
+ INDEP_DPFX_ARGS(inst), checknow);
+ int i;
+ ErrorCode ec;
for (i=0; i<inst->n_changes; i++) {
- Change *chg= inst->changes[n_installed];
+ 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,m->m), ec2str(ec));
+ 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,m->m), ec2str(ec));
+ METH_DPFX_ARGS(inst,*meth), ec2str(ec));
if (ec) goto x;
meth->needcheck= 0;
}
}
+ return 0;
x:
return ec;
static void indep_check_execute(void) {
DPRINTF(movpos,intern, "movpos indep_check_execute\n");
- for (Method **methwalk= methods;
- (meth= *methwalk);
- methwalk++) {
+ Method **methwalk, *meth;
+ for (methwalk= methods; (meth= *methwalk); methwalk++) {
if (meth->needcheck) {
- ec= meth->check(meth);
- DPRINTF(movpos,meth, "%s check=%s\n", m->m.pname, ec2str(ec));
+ 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", m->m.pname);
+ DPRINTF(movpos,meth, "%s execute...\n", meth->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) {
- DPRINTF(movpos,meth, METH_DPFX_FMT "dispose...\n",
- INDEP_DPFX_ARGS(indep,m->m));
- meth->dispose(meth, chg);
- }
- }
- free(indep);
-}
-
/*---------- entrypoints from rest of program ----------*/
ErrorCode
maxdelay_ms, movpos_pname(move,startpoint));
ec= indep_prepare(move,target, startpoint,
- ms,0,
+ maxdelay_ms,0,
&indep, 0);
if (ec) return ec;
indep_check_execute();
- DPRINTF(movpos,reserve, "movpos reserve %s/%s ok\n",
+ DPRINTF(movpos,entry, "movpos reserve %s/%s ok\n",
move->i->pname, movpos_pname(move,target));
return 0;
indep_dispose(indep);
indep_check_execute();
- DPRINTF(movpos,reserve, "movpos reserve %s/%s err=%s\n",
+ DPRINTF(movpos,entry, "movpos reserve %s/%s err=%s\n",
move->i->pname, movpos_pname(move,target), ec2str(ec));
return ec;
}
if (fwd && !(fwd ==tback || fwd ==tfwd)) continue;
/* we have to search for the one which is least effort */
- ec= indep_prepare(move,tcomb,startpoint, -1,0, 0,&tcost);
+ ErrorCode ec= indep_prepare(move,tcomb,startpoint, -1,0, 0,&tcost);
if (ec) return ec;
if (tcost >= bestcost) /* prefer low-numbered movposcombs */
ErrorCode movpos_change(Segment *move, MovPosComb target,
int maxdelay_ms, MovPosChange *resv) {
- const SegmentInfo *movei= move->i;
- const MovFeatInfo *feati;
- int feat, DP;
+ int DP;
MovPosComb actual;
ErrorCode ec;
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,
+ if (resv) DPRINTF2(" resv=%s/%s",
+ resv->move->i->pname,
movpos_pname(resv->move, resv->target));
- if (move->motion) DPRINTF2(movpos,entry, " oldmotion=%s:/%s",
- move->motion->meth->pname,
+ if (move->motion) DPRINTF2(" oldmotion=/%s",
movpos_pname(move, move->motion->target));
DPRINTF2("\n");
move->motion= inst;
+ ouposn_moving(inst);
indep_check_execute();
- DPRINTF(movpos,change, "movpos change %s/%s ok\n",
+ DPRINTF(movpos,entry, "movpos change %s/%s ok\n",
move->i->pname, movpos_pname(move, target));
return 0;
}
void movpos_unreserve(MovPosChange *resv) {
- if (!res) return;
+ if (!resv) return;
DPRINTF(movpos,entry, "movpos unreserve %s/%s...\n",
- res->move->i->pname, movpos_pname(res->move, res->intent));
+ resv->move->i->pname, movpos_pname(resv->move, resv->target));
indep_remove(resv);
indep_dispose(resv);
indep_check_execute();
}
MovPosComb movpos_change_intent(MovPosChange *indep) {
- return indep->intent;
+ return indep->target;
}
void motions_all_abandon(void) {