spot end of plan by counting segments instead of inferring from state

diff --git a/hostside/safety.c b/hostside/safety.c
index 67181f743245591f64a587a6464500b4bace8f3e..8e901a0700b53d8e02d131013f7601ef189c4342 100644 (file)
--- a/hostside/safety.c
+++ b/hostside/safety.c
@@ -280,6 +280,7 @@ typedef struct {
   double maxspeed;
   Segment *hindmost, *furthest, *desire_move;
   int desire_movposcomb;
+  int lookahead;
 
   PredictionProblemCallback *problem_callback;
   void *problem_callback_u;
@@ -347,7 +348,7 @@ static void pred_callback_debug(const char *what, TrackLocation *t,
   
   oprintf(DUPO("safety") " predict   %s"
          " %c%s dist=%-4d until=%-4ld %c%c%c.%c (was %s%s..%d dist=%-4d)"
-         "  %c%c%c.%c%c%c%c elapsed=%ld"
+         "  %c%c%c.%c%c%c%c elapsed=%ld la#%d"
          " %s/%s %c%s >%s"
          " nit=%d,%d\n",
          what,
@@ -378,10 +379,11 @@ static void pred_callback_debug(const char *what, TrackLocation *t,
          "-i"[ u->train_polarity_inverted ],
          "-b"[ u->know_best_polarity ],
          (long)u->elapsed,
+         u->lookahead,
 
          t->seg->i->pname,
          movpos_pname(t->seg, t->seg->movposcomb),
-         "!~"[ t->seg->moving ],
+         "~!"[ t->seg->moving ],
          motion_pname(t->seg, t->seg->motion),
          motion_pname(t->seg, t->seg->motion_newplan),
 
@@ -446,7 +448,7 @@ static int nose_nextseg(TrackLocation *t, TrackAdvanceContext *c,
 
   /* Is it empty ? */
 
-  if (u->stopping && t->seg->owner != u->train)
+  if (u->stopping && u->lookahead == u->train->plan_lookahead_nsegs)
     return EC_SignallingHorizonReached;
 
   if (t->seg->owner) {
@@ -543,6 +545,7 @@ static int nose_nextseg(TrackLocation *t, TrackAdvanceContext *c,
     t->seg->tr_backwards= t->backwards;
   t->seg->pred_present= 1;
   t->seg->until= u->elapsed < 0 ? 0 : u->elapsed;
+  u->lookahead++;
   return 0; /* yay! */
 }
 
@@ -676,6 +679,7 @@ static int optunwind_nextseg(TrackLocation *t, TrackAdvanceContext *c,
 
   t->seg->pred_present= 0;
   t->seg->until= 0;
+  u->lookahead--;
   return 0;
 }
 
@@ -832,7 +836,10 @@ ErrorCode predict(Train *tra, struct timeval tnow, unsigned flags,
 
   /*----- commit to the plan -----*/
 
-  oprintf(DUPO("safety") " predict  committing\n");
+  oprintf(DUPO("safety") " predict  committing la#%d\n",
+         u.lookahead);
+
+  tra->plan_lookahead_nsegs= u.lookahead;
 
   if (u.need_polarise) {
     oprintf(DUPO("safety") " predict  polarising train_polarity=%d\n",
@@ -843,14 +850,19 @@ ErrorCode predict(Train *tra, struct timeval tnow, unsigned flags,
   oprintf(DUPO("safety") " predict  ");
   FOR_SEG {
     oprintf(UPO,
-           " %s%s%s%c%c%c%c",
+           " %s%s%s%c%c%c%c/%s%s%s%s%s",
            seg->tr_backwards?"-":"", seg->i->pname,
-           seg->owner == tra ? "=" :
-           seg->owner ? "#" : "-" ,
+           seg->owner == tra ? "=" : seg->owner ? "#" : "-",
            "-N"[ seg->now_present ],
            "-P"[ seg->pred_present ],
            "-V"[ seg->pred_vacated ],
-           "-p"[ seg->will_polarise ]);
+           "-p"[ seg->will_polarise ],
+           movpos_pname(seg,seg->movposcomb),
+           seg->motion ? (seg->moving ? "!" : "~") : "",
+           seg->motion ? motion_pname(seg,seg->motion) : "",
+           seg->motion_newplan ? ">" : "",
+           seg->motion && seg->motion_newplan==seg->motion ? "=" :
+             motion_pname(seg,seg->motion_newplan));
 
     /* set ownership and det_ignore */
     if (seg->pred_present || seg->pred_vacated) {
@@ -859,40 +871,42 @@ ErrorCode predict(Train *tra, struct timeval tnow, unsigned flags,
     } else if (seg->owner == tra) {
       seg->det_ignore= 0;
       seg->owner= 0;
-    }
-
-    /* garbage collect our previous plan */
-    if (!seg->owner) {
+      /* garbage collect our old plan for now-irrelevant segments */
       assert(!seg->motion_newplan);
       if (!seg->moving) {
        movpos_unreserve(seg->motion);
        seg->motion= 0;
       }
-    }
-    if (seg->owner != tra)
       continue;
+    } else {
+      /* segment not relevant to us at all */
+      continue;
+    }
 
     /* now it's our segment: */
 
-    if (seg->motion_newplan == seg->motion) {
-      seg->motion_newplan= 0;
-    } else {
-      if (seg->motion && !seg->moving) {
+    /* clear any irrelevant old plan */
+    if (seg->motion && !seg->moving) {
+      if (seg->motion != seg->motion_newplan)
        movpos_unreserve(seg->motion);
-       seg->motion= 0;
-      }
-      if (seg->motion_newplan && !seg->now_present) {
+      seg->motion= 0;
+    }
+    /* now motion!=0 only if seg->moving */
+
+    /* install the new plan, if any */
+    if (seg->motion_newplan) {
+      if (!seg->now_present) {
        MovPosComb target= movpos_change_intent(seg->motion_newplan);
        ec= movpos_change(seg, target, -1, seg->motion_newplan);
        assert(!ec);
-       seg->motion_newplan= 0;
-      }
-      if (seg->motion_newplan) {
-       assert(!seg->motion);
+       /* motion is updated by movpos_change */
+      } else {
        seg->motion= seg->motion_newplan;
-       seg->motion_newplan= 0;
       }
+      seg->motion_newplan= 0;
     }
+    /* now the new plan is in motion and moving */
+
     if (u.need_polarise && seg->will_polarise) {
       seg->seg_inverted= seg->tr_backwards ^ u.train_polarity_inverted;
       if (seg->i->invertible) {
@@ -962,14 +976,9 @@ static int report_nextseg(TrackLocation *t, struct TrackAdvanceContext *c,
   char flags[6];
   int r;
 
-  if (t->seg->owner != u->train) return -1;
-  if (before &&
-      t->seg == u->hindmost &&
-      t->seg->tr_backwards == t->backwards) {
-    /* We've looped the loop. */
-    oprintf(UPO, " ...");
-    return -1;
-  }
+  if (u->alwaysusemotions) /* we've had foredetect */
+    if (++u->lookahead > u->train->plan_lookahead_nsegs)
+      return -1;
 
   flags[0]= 0;
 
@@ -1039,6 +1048,7 @@ void safety_notify_detection(Segment *seg) {
   struct timeval tnow;
 
   if (seg->det_ignore) return;
+
   if (!seg->det_expected) {
     interferer= segment_interferes(0,seg);
     if (!interferer) safety_panic(0,seg, "unexpected detection");
@@ -1063,6 +1073,7 @@ void safety_notify_detection(Segment *seg) {
 
   tra->foredetect= seg;
   tra->uncertainty= tra->maxinto= maxinto;
+  tra->plan_lookahead_nsegs--;
   report_train_position(tra);
 
   ec= predict(tra,tnow, PREDF_JUSTDET,0,0, detection_report_problem,0);

diff --git a/hostside/safety.h b/hostside/safety.h
index 907e4d236081b53b13a8917bb63338506b76c395..64abf9e99dc56b15aeddb448a34c9f413d0511df 100644 (file)
--- a/hostside/safety.h
+++ b/hostside/safety.h
@@ -48,6 +48,8 @@ struct Train {
   /* Startup resolution (resolve.c): */
     resolution:2; /* for use by speedmanager */
 
+  int plan_lookahead_nsegs; /* counts from foredetect exclusive */
+
   struct {
     int step; /* commanded */
     TimeoutEvent decel; /* running iff decelerating */