* 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
+ * I Initialising 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
+ * Q Queued 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: */
- Method *meth; /* ARCDE indep after allocate() */
- Segment *move; /* ARCDE indep after allocate() */
- MovPosComb actual; /* CD see below */
- MovPosComb intent; /* RCD indep after allocate() */
+typedef struct Change Change;
+
+struct Change { /* valid in: filled in by and when: */
+ Method *meth; /* PQ indep after allocate() */
+ MovPosChange *indep; /* PQ indep after allocate() */
/* kind-specific data follows */ /* varies kind-specific code, varies */
} Change;
/* `actual' contains the kind's public opinion about the physical
* 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
+ * physical state is recorded only in the relevant MovPosChange, 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 Method {
const char *pname;
MovFeatKind kind;
- Change *(*allocate)(Method *m, int alloc_motions); /* U->A (never err) */
- ErrorCode (*reserve)(Method *m, Change*, Segment*, /* A->R */
- int ms); /* error: A->E */
- ErrorCode (*confirm)(Method *m, Change*, Segment*, /* [AR]->C */
- int n_motions, const Motion*, int ms); /* err:[AR]->E */
- void (*destroy)(Method *m, Change*); /* [ARCE]->U */
+ ErrorCode (*prepare)(Method *m, /* TYE->T */
+ Segment *move,
+ int n_motions, const Motion *motions,
+ int ms, int confirming,
+ Change *chg_r, /* U->P; err: U->U; may be 0 */
+ int *cost_r /* may be 0 */);
+ ErrorCode (*consider)(Method *m, /* TYE->T */
+ Change *remove, /* Q->P; err: Q->Q */
+ Change *install); /* P->Q; err: P->P */
+ /* error: ET->T, no change to Changes */
+ 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);
- /* 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
- */
};
const char *movpos_pname(Segment *move, MovPosComb poscomb) {
move->i->pname, movpos_pname(move, chg->actual));
}
-static void motion_done(Segment *move, MovPosComb actual) {
- move->moving= 0;
- move->motion= 0;
- move->movposcomb= actual;
- ouprintf("movpos %s position %s stable\n",
- move->i->pname, movpos_pname(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;
}
static void ignore_all_abandon(Method *m) { }
#define FSQDN (~(FsqSlot)0)
-typedef struct { /* Allocated Reserved Confirmed */
- /* in queue? absent reserved confirmed */
- Change h;
- FsqSlot deadline; /* ~0==FSQDN 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 */
+typedef struct { /* PR QR PC QC */
+ /* in queue? absent reserved absent confirmed */
+ Change h;
+ FsqSlot deadline; /* relative relative absolute absolute */
+ MovPosComb actual; /* undef undef see below */
+ int n_motions; /* 1 1 alloc'd num undone */
+ Motion motions[];
+ /* [0].i: 0 0 non-0 non-0 */
+ /* .posn: undef undef defined defined */
} FsqReq;
/* We can determine the the state by looking at the two
* `statedet' fields, marked <- above.
fsq_queue_remove_index(q, i);
}
-static void fsq_dequeue(FsqMethod *m, FsqReq *r) { /* X->XA */
- if (r->motions[0].i) {
- fsq_queue_remove_item(&m->f.confirmed, r);
- } else if (r->deadline!=FSQDN) {
- fsq_queue_remove_item(&m->f.reserved, r);
- } else {
- return;
- }
- fsq_check_plan(m);
-}
-
-static void fsq_mark_as_allocated(FsqReq *r) { /* AX->X */
- /* Sets statedet fields for Allocated */
- r->deadline= FSQDN;
- r->motions[0].i=0;
-}
+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, \
static ErrorCode fsq_check_plan(FsqMethod *m) {
/* Checks whether we can meet the currently queued commitments */
+ /* if this fails, indep machinery calls fsq_prepare to dequeue */
int future, conf, resv, whichwhen, DP;
FsqReq *whichr;
DPRINTF1(movpos,fsq, "%s plan", m->m.pname);
- /* If CDU is charged we can't do one right away */
+ /* If CDU isn't charged we can't do one right away */
if (m->f.ready<0) {
DPRINTF2(" +");
future++;
#undef WHICH
-static ErrorCode fsq_enqueue(FsqMethod *m, FsqQueue *q, /* XA -> X */
- FsqReq *r) { /* or on error, XA -> A */
- int insat; /* ... where X is R or C and corresponds to q */
+static ErrorCode fsq_queue_insert_item(FsqMethod *m, FsqQueue *q, FsqReq *r) {
+ int insat;
if (q->n == FSQ_MAX_QUEUE)
return EC_BufferFull;
q->l[insat]= q->l[insat-1];
q->l[insat]= r;
q->n++;
-
- return fsq_check_plan(m);
- /* if this fails, indep machinery calls fsq_destroy which dequeues */
}
-/*---------- method entrypoints ----------*/
-
-static Change *fsq_allocate(Method *mm, int alloc_motions) {
- FsqReq *r;
-
- 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= FSQDN;
- r->n_motions= alloc_motions;
- r->motions[0].i= 0;
- return (Change*)r;
+static void fsq_item_debug(FsqMethod *m, FsqReq *r,
+ const char *pfx, Segment *move) {
+ DPRINTF1(movpos,fsq, "%s%s", pfx, 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);
+ } else {
+ DPRINTF1(movpos,fsq, "(%d)", r->n_motions);
+ }
+}
+
+static ErrorCode fsq_enqueue(FsqMethod *m, FsqReq *r) { /* P->Q; 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);
}
-static ErrorCode fsq_reserve(Method *mm, Change *chg,
- Segment *move, int maxdelay_ms) {
- FsqMethod *m= (void*)mm;
- FsqReq *r= (FsqReq*)chg;
- FsqSlotSigned reldeadline;
-
- reldeadline= fsq_maxdelay_reldeadline(m, maxdelay_ms, r->n_motions);
- if (reldeadline <= 0) { fsq_mark_as_allocated(r); return EC_MovFeatTooLate; }
- r->deadline= reldeadline;
- return fsq_enqueue(m, &m->f.reserved, r);
+static void fsq_dequeue(FsqMethod *m, FsqReq *r) { /* Q->P */
+ if (!r) return 0;
+ fsq_item_debug(m,r,"dequeue:",r->h.indep.move);
+ fsq_remove_item(fsq_item_queue(m,r), r);
}
-static ErrorCode fsq_confirm(Method *mm, Change *chg, Segment *move,
+/*---------- method entrypoints ----------*/
+
+static ErrorCode fsq_prepare(Method *mm, Segment *move,
int n_motions, const Motion *motions,
- int maxdelay_ms) {
+ int ms, int confirmation,
+ Change *chg_r, int *cost_r) {
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(n_motions > 0);
- 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);
+ FsqSlotSigned reldeadline= fsq_maxdelay_reldeadline(m, ms, r->n_motions);
+ if (reldeadline <= 0) return EC_MovFeatTooLate;
- /* 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);
+ 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)*motions);
+ } else {
+ r->deadline= reldeadline;
+ r->motions[0].i= 0;
+ }
+ *chg_r= &r->h;
}
- 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 */
+ if (cost_r) {
+ *cost_r= n_motions * m->slot_ms;
}
- /* 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;
+ return 0;
+}
- if (n_motions == move->i->n_movfeats)
- r->actual= 0;
- else
- r->actual= chg->actual;
- assert(r->actual >= 0);
+static ErrorCode fsq_consider(Method *mm, Change *remove, Change *install) {
+ FsqMethod *m= (void*)mm;
+ FsqReq *remv= (FsqReq*)remove;
+ FsqReq *inst= (FsqReq*)install;
- /* 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);
+ DPRINTF1(movpos,fsq, "%s consider ", m->m.pname);
- 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 */
- }
+ fsq_dequeue(m, remv);
+ ErrorCode ec= fsq_enqueue(m, inst);
+ if (ec) {
+ ErrorCode ec2= fsq_enqueue(m, remv);
+ assert(!ec2);
}
- /* either ec=0 state C U
- * or ec!=0 state A C
- * or ec!=0 state C but bad C
- */
+ DPRINTF1(movpos,fsq, " %s\n", ec2str(ec));
+ return ec;
+}
- if (ec) return ec;
+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;
+}
- move->moving= 1;
- move->motion= chg;
- move->movposcomb= -1;
- ouposn_moving(chg);
+static void fsq_execute(Method *mm) {
+ FsqMethod *m= (void*)mm;
+ DPRINTF(movpos,fsq, "%s execute\n", m->m.pname);
fsq_check_action(m);
- return 0;
}
-static void fsq_destroy(Method *mm, Change *chg) { /* X->XA and then freed */
+static void fsq_dispose(Method *mm, Change *chg) {
FsqMethod *m= (void*)mm;
FsqReq *r= (FsqReq*)chg;
- fsq_dequeue(m,r);
free(r);
}
assert(mo->posn < mo->i->posns);
m->f.ready= 0;
m->f.move(m, mo->i, mo->posn);
- ouprintf("movpos %s feat %s %d %s\n", r->h.move->i->pname,
- mo->i->pname, mo->posn, m->m.pname);
m->f.cslot++;
- r->actual= movposcomb_update_feature(r->actual,mo->i,mo->posn);
- if (r->h.actual >= 0 || !r->n_motions)
- r->h.actual= r->actual;
- ouposn_moving(&r->h);
+ indep_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);
- fsq_queue_remove_index(&m->f.confirmed,0);
- fsq_mark_as_allocated(r); /* now state A aka Done */
- motion_done(move,r->h.actual);
- free(r);
+ fsq_queue_remove_index(&m->f.confirmed, 0);
+ indep_change_done(&m->m, &r->h);
+ m->h.dispose(&m->m, &r->h);
+
ec= fsq_check_plan(m); assert(!ec);
fsq_check_action(m);
}
}
-/*---------- 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++) {
- 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;
-}
-
/*========== points ==========*/
/*
#define CDU_RECHARGE 350 /*ms*/
#define POINT_MOVEMENT 50 /*ms*/
-static Change *point_allocate(Method *mm, int alloc_motions) {
+static ErrorCode point_prepare(Method *mm, 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_allocate(mm, alloc_motions);
-}
+ 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! */
static void points_all_abandon(Method *mm) {
FsqMethod *m= (void*)mm;
m->f.ready= -1;
- fsq_all_abandon(mm);
}
static FsqMethod points_method;
static FsqMethod points_method= {
{ "point", mfk_point,
- point_allocate, fsq_reserve, fsq_confirm,
- fsq_destroy, points_all_abandon },
+ point_prepare, fsq_consider, fsq_dispose,
+ fsq_check, fsq_execute, points_all_abandon },
{ .lag_ms= POINT_MOVEMENT, .slot_ms= CDU_RECHARGE, .move= point_move }
};
static FsqMethod waggle;
-static Change *waggle_allocate(Method *mm, int alloc_motions) {
- FsqMethod *m= (void*)mm;
- assert(m->f.ready>=0);
- return fsq_allocate(mm, alloc_motions);
-}
-
static void waggle_do(FsqMethod *m, const MovFeatInfo *mfi, int posn) {
/* actually setting relays */
PicInsn piob;
static FsqMethod waggle= {
{ "relay", mfk_relay,
- waggle_allocate, fsq_reserve, fsq_confirm,
- fsq_destroy, fsq_all_abandon },
+ fsq_prepare, fsq_consider, fsq_dispose,
+ fsq_check, fsq_execute, ignore_all_abandon },
{ .lag_ms= 5, .slot_ms= 50, .move= waggle_do }
};
/*========== dummy `nomove' kind ==========*/
-static Change *nomove_allocate(Method *m, int alloc_motions) {
- DPRINTF(movpos,nomove, "allocate %d\n",alloc_motions);
- return mmalloc(sizeof(Change));
-}
-static ErrorCode nomove_reserve(Method *m, Change *c, Segment *move, int ms) {
- DPRINTF(movpos,nomove, "reserve\n");
- return 0;
-}
-static void nomove_destroy(Method *m, Change *c) { free(c); }
-static ErrorCode nomove_confirm(Method *m, Change *c, Segment *move, int n,
- const Motion *motions, int ms) {
- DPRINTF(movpos,nomove, "confirm\n");
- nomove_destroy(m,c);
+static ErrorCode nomove_prepare(Method *m, Segment *move,
+ int n_motions, const Motion *motions,
+ int ms, int confirming,
+ Change *chg_r, int *cost_r) {
+ if (chg_r) {
+ *chg= mmalloc(sizeof(Change));
+ fixme
+ }
+ if (cost_r) {
+ *cost_r= 0;
+ }
return 0;
}
+static ErrorCode nomove_consider(Method *m, Change *remove, Change *install)
+ { return 0; }
+
+static void nomove_dispose(Method *m, Change *chg) { }
+static ErrorCode nomove_check(Method *m) { return 0; }
+static ErrorCode nomove_execute(Method *m) {
+ fixme
+}
+
static Method nomove_method= {
- "nomove", mfk_none, nomove_allocate, nomove_reserve, nomove_confirm,
- nomove_destroy, ignore_all_abandon
+ "nomove", mfk_none,
+ nomove_prepare, nomove_consider, nomove_dispose,
+ nomove_check, nomove_execute, ignore_all_abandon
};
/*========== method-independent machinery ==========*/
+struct MovPosChange {
+ Segment *move;
+ MovPosComb start;
+ MovPosComb actual;
+ int n_changes;
+ Change *changes[];
+};
+
+
static Method *methods[]= {
&nomove_method,
(Method*)&points_method,
0
};
-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;
+static void indep_update_feature(Method *m, MovPosChange *indep,
+ const Motion *mo) {
+ if (SOMEP(indep->start))
+ ouprintf("movpos %s feat %s %d %s\n", indep->move->i->pname,
+ mo->i->pname, mo->posn, m.pname);
+ indep->actual=
+ movposcomb_update_feature(indep->actual, mo->i, mo->posn);
}
-static int change_needed(const MovFeatInfo *feati,
- MovPosComb startpoint, MovPosComb target) {
- int r;
- r= startpoint<0 ||
- (target - startpoint) / feati->weight % feati->posns;
- DPRINTF(movpos,changeneeded, "%s:%s(%d*%d) %d..%d => %d\n",
- methods[feati->kind]->pname, feati->pname,
- feati->posns, feati->weight,
- startpoint, target, r);
- return r;
-}
+static void indep_change_done(Method *m, Change *chg) {
+ MovPosComb *indep= chg->indep;
+ Change **search;
+
+ for (search=indep->changes; *search; search++)
+ if ((*search) == chg) goto found;
+ assert(!"change in indep");
-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. */
+ found:
+ indep->n_changes--;
+ if (indep->n_changes) {
+ *search= indep[n_changes];
+ return;
+ }
+
+ /* all done */
+ move->moving= 0;
+ move->motion= 0;
+ move->movposcomb= indep->actual;
+ ouprintf("movpos %s position %s stable\n",
+ move->i->pname, movpos_pname(move, move->movposcomb));
+ free(indep);
+}
+
+static ErrorCode evaluate_target(Segment *move, MovPosComb target,
+ MovPosComb startpoint,
+ int *n_changes_r,
+ Change *changes_r[N_METHODS],
+ int *cost_r) {
const SegmentInfo *movei= move->i;
- int feat, tchanges;
+ int feat;
const MovFeatInfo *feati;
MovFeatKind kind;
+ int n_changes;
+ Method *change_methods[N_METHODS];
+ Motion *change_motions[N_METHODS];
+ int i;
DPRINTF(movpos,eval, "%s/%s <-%s\n", move->i->pname,
movpos_pname(move,target), movpos_pname(move,startpoint));
- if (startpoint<0) {
+ if (!SOMEP(startpoint)) {
startpoint= movpos_poscomb_actual(move);
DPRINTF(movpos,eval, " actual <-%s\n",
movpos_pname(move,startpoint));
}
+ n_changes= 0;
+
for (feat=0, feati=movei->movfeats, tchanges=0, kind= mfk_none;
feat<movei->n_movfeats;
feat++, feati++) {
if (!change_needed(feati,startpoint,target)) continue;
+ for (i=0; i<n_changes; i++)
+
+
tchanges++;
if (kind && feati->kind != kind) return -1;
kind= feati->kind;
return tchanges;
}
+
+
+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;
+
+ int n_changes;
+ Change *changes[N_METHODS];
+
+ DPRINTF(movpos,reserve, "%s/%s maxdelay=%dms startpoint=%s\n",
+ move->i->pname, movpos_pname(move,target),
+ maxdelay_ms, movpos_pname(move,startpoint));
+
+ evaluate_target(move,target,startpoint, &n_changes,&changes);
+
+
+ if (nchanges==-1) return EC_MovFeatKindsCombination;
+
+ Method *meth= methods[kind];
+ DPRINTF(movpos,reserve, "allocate %s:%d...\n",
+ meth->pname, nchanges);
+ Change *chg= mp_allocate(meth, move, nchanges, target);
+ ec= meth->reserve(meth, chg, move, maxdelay_ms);
+ DPRINTF(movpos,reserve, "reserve => %s\n",errorcodelist[ec]);
+ if (ec) goto x;
+
+ *res_r= chg;
+ return 0;
+
+ x:
+ movpos_unreserve(chg);
+ return ec;
+}
+
+
+
+
+
+
+
+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;
+}
+
+static int change_needed(const MovFeatInfo *feati,
+ MovPosComb startpoint, MovPosComb target) {
+ int r;
+ r= startpoint<0 ||
+ (target - startpoint) / feati->weight % feati->posns;
+ DPRINTF(movpos,changeneeded, "%s:%s(%d*%d) %d..%d => %d\n",
+ methods[feati->kind]->pname, feati->pname,
+ feati->posns, feati->weight,
+ startpoint, target, r);
+ return r;
+}
+
ErrorCode movpos_findcomb_bysegs(Segment *back, Segment *move, Segment *fwd,
MovPosComb startpoint, MovPosComb *chosen_r) {
const SegmentInfo *movei= move->i;
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;
-
- DPRINTF(movpos,reserve, "%s/%s maxdelay=%dms startpoint=%s\n",
- move->i->pname, movpos_pname(move,target),
- maxdelay_ms, movpos_pname(move,startpoint));
-
- nchanges= evaluate_target(move,target,startpoint,&kind);
- if (nchanges==-1) return EC_MovFeatKindsCombination;
-
- Method *meth= methods[kind];
- DPRINTF(movpos,reserve, "allocate %s:%d...\n",
- meth->pname, nchanges);
- Change *chg= mp_allocate(meth, move, nchanges, target);
- ec= meth->reserve(meth, chg, move, maxdelay_ms);
- DPRINTF(movpos,reserve, "reserve => %s\n",errorcodelist[ec]);
- if (ec) goto x;
-
- *res_r= chg;
- return 0;
-
- x:
- movpos_unreserve(chg);
- return ec;
-}
-
void movpos_unreserve(MovPosChange *res) {
if (!res) return;
DPRINTF(movpos,unreserve, "%s:%s/%s\n",
(*meth)->all_abandon(*meth);
}
-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;
+
+
+
+
+
+
+ STUFF BEYOND HERE IS FUNCTIONALITY WHICH NEEDS MOVING TO INDEP
+ AND/OR CHECKING THAT WE STILL HAVE IT
+
+
+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;
}
+
+
+
+ r->actual= movposcomb_update_feature(r->actual,mo->i,mo->posn);
+ if (r->h.actual >= 0 || !r->n_motions)
+ r->h.actual= r->actual;
+ ouposn_moving(&r->h);
+
+
+
+
+ /* 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);
+ fsq_mark_as_allocated(r); /* now state A aka Done */
+ motion_done(move,r->h.actual);
+ free(r);
+
+/*---------- 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
~ijackson / trains.git / commitdiff