From: ian <ian>
Date: Tue, 20 May 2008 21:00:32 +0000 (+0000)
Subject: spot end of plan by counting segments instead of inferring from state
X-Git-Url: http://www.chiark.greenend.org.uk/ucgi/~ijackson/git?a=commitdiff_plain;h=105d89ff5afe34032bd7cbdc1abe841838845b6d;p=trains.git

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

diff --git a/hostside/safety.c b/hostside/safety.c
index 67181f7..8e901a0 100644
--- 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 907e4d2..64abf9e 100644
--- 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 */