From: ian <ian>
Date: Sun, 20 Apr 2008 21:38:34 +0000 (+0000)
Subject: wip new safety before tackle speed
X-Git-Url: http://www.chiark.greenend.org.uk/ucgi/~ijackson/git?a=commitdiff_plain;h=3c5534ad535b0fb32d0b55b9349fa07ad2f73096;p=trains.git

wip new safety before tackle speed
---

diff --git a/hostside/errorcodes.h.gen b/hostside/errorcodes.h.gen
index d8570b5..5ccde34 100755
--- a/hostside/errorcodes.h.gen
+++ b/hostside/errorcodes.h.gen
@@ -8,8 +8,8 @@
        BufferFull
        BadCmd
        NotFound
-       ProblemPredicted
-       SignalStopHorizonReached
+       SignallingProblemPredicted
+       SignallingHorizonReached
        );
 
 
diff --git a/hostside/realtime.c b/hostside/realtime.c
index fbeb8c5..c16b4a7 100644
--- a/hostside/realtime.c
+++ b/hostside/realtime.c
@@ -64,9 +64,9 @@ static char *transegn2suffixstring(Train *tra, Segment *seg) {
   return s;
 }
 
-void vlogmsg(ErrorCode ec, Train *tra, const SegmentInfo *segi,
+void vlogmsg(const char *intro, Train *tra, const SegmentInfo *segi,
 	     const char *fmt, va_list al) {
-  oprintf(UPO, "message %s", ec ? errorcodelist[ec] : "info");
+  oprintf(UPO, "log %s", intro);
   if (segi) oprintf(UPO, " @%s", segi->pname);
   if (tra) oprintf(UPO, " %s",  tra->pname);
   if (segi || tra) oprintf(UPO, ":");
@@ -75,11 +75,11 @@ void vlogmsg(ErrorCode ec, Train *tra, const SegmentInfo *segi,
   oprintf(UPO, "\n");
 }
 
-void logmsg(ErrorCode ec, Train *tra, const SegmentInfo *segi,
+void logmsg(const char *intro, Train *tra, const SegmentInfo *segi,
 	    const char *fmt,...) {
   va_list al;
   va_start(al,fmt);
-  vlogmsg(ec,tra,segi,fmt,al);
+  vlogmsg(intro,tra,segi,fmt,al);
   va_end(al);
 }
 
@@ -103,14 +103,6 @@ void safety_panic(Train *tra, Segment *seg, const char *fmt, ...) {
   safety_vpanic(tra, seg, fmt, al);
 }
 
-ErrorCode safety_problem(Train *tra, Segment *seg, const char *fmt,...) {
-  va_list al;
-  va_start(al,fmt);
-  vlogmsg(EC_Safety, tra, seg?seg->i:0, fmt, al);
-  va_end(al);
-  return EC_Safety;
-}
-
 /*---------- printing nmra data ----------*/
 
 typedef struct {
diff --git a/hostside/safety.c b/hostside/safety.c
index 2d6a35a..bb38bf3 100644
--- a/hostside/safety.c
+++ b/hostside/safety.c
@@ -186,15 +186,15 @@ Segment *segments;
 #define pred_vacated mark2
 #define will_polarise mark3
 
-typedef void PredictionProblemCallback(void *pu, Train *tra, TrackSegment *seg,
-				       const char *fmt, va_list al);
+typedef void PredictionProblemCallback(Train *tra, TrackSegment *seg,
+				       void *pu, const char *message);
 
 typedef struct {
   unsigned
     accelerating:1,
     walk_compute_polarise:1, /* nose_nextseg still needs to worry */
     need_polarise:1, /* when we commit */
-    train_polarity_inverted:1, /* right now */
+    train_polarity_inverted:1, /* right now, or if know_best, the best */
     know_best_polarity; /* longest-lasting into the future */
   TrackLocation nose, fdet, tail;
   TrackAdvanceContext nosec, tailc, fdetc;
@@ -214,7 +214,17 @@ typedef struct {
 
 static ErrorCode predict_vproblem(PredicUserContext *u, TrackSegment *seg,
 				  const char *fmt, va_list al) {
-  u->problem_callback(u->train,seg,fmt,al);
+  int l;
+  char *message;
+
+  l= vasprintf(&message, fmt, al);  if (l <= 0) diem();
+
+  if (!u->problem_callback)
+    safety_panic(u->train, seg, "unexpected problem predicted",
+		 " (context: %s): %s", u->problem_callback_u, message);
+  else
+    u->problem_callback(u->train, seg, u->problem_callback_u, message);
+
   return EC_ProblemPredicted;
 }
 static ErrorCode predict_problem(PredicUserContext *u, TrackSegment *seg,
@@ -286,33 +296,6 @@ static int nose_nextseg(TrackLocation *t, TrackAdvanceContext *c,
 			     interferer->i->pname);
   }
 
-  /* Check polarity */
-
-  if (u->walk_compute_polarise) {
-    if (!u->need_polarise) {
-      /* this is the very next segment as we only do this once: */
-      u->walk_compute_polarise= u->need_polarise=
-	(t->seg->seg_inverted ^ t->seg->tr_backwards ^
-	 u->train_polarity_inverted);
-    }
-    if (t->seg->will_polarise)
-      /* this is our 2nd visit, stop now */
-      u->walk_compute_polarise= 0;
-    
-    if (u->walk_compute_polarise)
-      seg->will_polarise= 1;
-
-    if (!u->know_best_polarity && !t->seg->i->invertible) {
-      u->know_best_polarity= 1;
-      u->
-      
-      
-      if (
-
-    int train_inverted_here= 
-    
-  }
-
   /* What about the route ? */
 
   if (t->seg->i->n_poscombs==1)
@@ -427,15 +410,55 @@ static int fdet_nextseg(TrackLocation *t, TrackAdvanceContext *c,
    * actually use it ourselves and trackloc_advance will call
    * getmovpos. */
 
+  /* Check polarity */
+
   if (!t->seg->invertible) {
     u->noninv_tally[t->backwards]++;
     if (u->noninv_tally[0] && u->noninv_tally[1])
       return predict_problem(u, t->seg, "cannot set track polarity");
   }
 
+  if (u->was_distance == INT_MAX) {
+    t->seg->det_expected= 1;
+
+    u->walk_compute_polarise= u->need_polarise=
+      (t->seg->seg_inverted ^ t->seg->tr_backwards ^
+       u->train_polarity_inverted);
+  }
+
+  if (u->walk_compute_polarise) {
+    if (t->seg->will_polarise)
+      /* this is our 2nd visit, stop now */
+      u->walk_compute_polarise= 0;
+  }
+  if (u->walk_compute_polarise) {
+    if (!u->know_best_polarity) {
+      if (u->noninv_tally[0]) {
+	u->train_polarity_inverted= 0;
+	u->know_best_polarity= 1;
+      } else if (u->noninv_tally[1]) {
+	u->train_polarity_inverted= 1;
+	u->know_best_polarity= 1;
+      }
+    }
+    if (u->know_best_polarity && 
+	!t->seg->i->invertible
+	u->noninv_tally[ t->backwards ^ u->train_polarity_inverted ]) {
+      /* incompatible, stop now */
+      u->walk_compute_polarise= 0;
+    }
+  }
+  if (u->walk_compute_polarise) {
+    seg->will_polarise= 1;
+  }
+
+  /* Advance the tail */
+
   u->tailc.distance= advanced;
   r= trackloc_advance(&u->tail,&u->tailc);  assert(!r);
 
+  /* Final adjustments, prepare for next iteration */
+
   if (u->was_distance == INT_MAX)
     c->distance= speedmanager_stoppingdistance(u->train);
 
@@ -453,6 +476,7 @@ static ErrorCode predict_confirm(Train *tra, int accelerate,
   /* Caller must call this with different situations until it succeeds! */
   PredictUserContext u;
   Segment *foredet;
+  SEG_IV;
 
   FOR_SEG {
     seg->now_preset= seg->pred_present=
@@ -464,7 +488,6 @@ static ErrorCode predict_confirm(Train *tra, int accelerate,
   memset(&u,0,sizeof(u));
   u.accelerating= accelerate;
   u.walk_will_polarise= 1;
-  u.need_polarise= 0;
   u.train_polarity_inverted= foredet->seg_inverted ^ foredet->tr_backwards;
   u.problem_callback= ppc;
   u.problem_callback_u= ppcu;
@@ -496,7 +519,7 @@ static ErrorCode predict_confirm(Train *tra, int accelerate,
   u.count_time= 0;
 
   r= trackloc_advance(&u.nose,&u.nosec);
-  if (r) return r;
+  if (r) goto xproblem;
 
   /* predict the future */
 
@@ -509,7 +532,7 @@ static ErrorCode predict_confirm(Train *tra, int accelerate,
   u.tailc.nextseg= tail_nextseg;
 
   r= trackloc_advance(&fdet,&fdetc);
-  if (r) return r;
+  if (r) goto xproblem;
 
   /*----- commit to the plan -----*/
 
@@ -521,6 +544,9 @@ static ErrorCode predict_confirm(Train *tra, int accelerate,
 
   r= trackloc_advance(&fdet,&fdetc);
 
+  if (u.need_polarise)
+    actual_inversions_start();
+  
   FOR_SEG {
     if (seg->owner == tra) {
       seg->det_ignore= seg->det_expected= 0;
@@ -537,322 +563,75 @@ static ErrorCode predict_confirm(Train *tra, int accelerate,
       ec= movpos_change(seg, target, -1, reservation);
       assert(!ec);
     }
-    
-	movpos_unreserve(seg->motion);
-	seg->motion= 0;
-      } else if (seg->pred_vacated
-
-  }
-}
-
-predict the future
-  fdet.seg= foredet;
-
-  fdet.into= 0;
-
-  tail.seg= foredet;
-  tail.into= 0;
-  tail.backwards=
-
-void update_head(Train *tra, Segment *seg) {
-  tra->foredetect= seg;
-  if (tra->foredetect->movposcomb < 0)
-    safety_panic(tra,seg, "track route not set and train has arrived");
-}
-
-void safety_notify_detection(Segment *seg) {
-  if (seg->det_ignore) return;
-  if (!seg->det_expected)
-    safety_panic(0,seg, "unexpected detection");
-
-  Train *tra= seg->owner;
-
-  update_head(tra,seg);
-  predict_and_provide(tra);
-
-
-
-  /* ============ OLD CODE ========== */
-
-static void seg_clear_stale(Segment *seg) {
-  seg->owner= 0;
-  seg->until_here= seg->until_detect= 0;
-}
-
-typedef struct {
-  /* constant inputs */
-  Train *tra;
-  /* modified in place by lay_train_pass: */
-  ErrorCode ec;
-  int invert_count[2]; /* count of (switchable) segments,
-			* invert_count[0]: inverted from train's pov
-			*   iff train is backwards (ie, train not inverted)
-			* invert_count[1]: train is inverted
-			* set to -1 if any unswitchable is the other way */
-  Segment *invert_forcer; /* the unswitchable which forces */
-} LayTrainState;
-
-static void lay_train_check_clash(LayTrainState *l, Segment *check,
-				  Segment *report) {
-  const char *exi1="", *exi2="";
-  if (check!=report) { exi1= " at "; exi2= check->i->pname; }
-
-  if (check->tr_updated) {
-    l->ec= safety_problem(l->tra, report, "self-collision%s%s", exi1,exi2);
-    return;
-  }
-  if (check->owner) {
-    if (check->owner != l->tra) {
-      l->ec= safety_problem(l->tra, report, "collision with %s"
-			    "%s%s", check->owner->pname, exi1,exi2);
-      return;
+    if (u.need_polarise) {
+      if (seg->will_polarise)
+	seg->seg_inverted= seg->tr_backwards ^ u.train_polarity_inverted;
+      actual_inversions_segment(seg);
     }
+    seg->now_present= seg->pred_present=
+      seg->pred_vacated= seg->will_polarise= 0;
   }
-}
-
-static void lay_train_pass(LayTrainState *l,
-			   TrackLocation tloc, long advance,
-			   long speed, unsigned backwards,
-			   unsigned check_clash) {
-  Segment *seg, *inter;
-  long overall, remain, dist_until, time_until;
-  int *invert_likehere, train_inverted_here;
-  Train *tra= l->tra;
-  const SegPosCombInfo *pci;
-  const SegmentLinkInfo *link;
-
-  if (l->ec) return;
-
-  seg= tra->foredetect;
-
-  remain= overall= advance + speed * SPEED_CLEAR_MULT;
-
-fprintf(stderr,"lay_train_pass %s @%s+%ld check_clash=%d"
-	" advance=%ld speed=%ld remain=%ld backwards=%u\n",
-	tra->pname, seg->i->pname, tloc.into,
-	check_clash, advance,speed,remain,
-	backwards);
-
-  for (;;) {
-    seg= tloc.seg;
-
-fprintf(stderr,"lay_train_pass  loop @%s remain=%ld\n",
-	seg->i->pname, remain);
-
-    if (check_clash) {
-      lay_train_check_clash(l,seg,seg);
-      inter= interferes(seg);
-      if (inter) lay_train_check_clash(l,inter,seg);
-    }
-
-    seg->owner= l->tra;
-    seg->tr_backwards= tloc.backwards ^ backwards;
-    seg->tr_updated= 1;
-
-    if (backwards)
-      seg->until_detect= 0;
 
-    train_inverted_here= seg->seg_inverted ^ seg->tr_backwards;
-    invert_likehere= &l->invert_count[train_inverted_here];
+  if (u.need_polarise)
+    actual_inversions_done();
 
-    if (seg->i->invertible) {
-      if (*invert_likehere >= 0)
-	(*invert_likehere)++;
-    } else {
-      if (*invert_likehere < 0) {
-	l->ec= safety_problem(l->tra,0, "train requires"
-			      " noninvertible segments with opposite polarity:"
-			      " @%s, @%s", seg->i->pname,
-			      l->invert_forcer->i->pname);
-	return;
-      }
-      assert(!seg->seg_inverted);
-      (*invert_likehere)++;
-      l->invert_count[!train_inverted_here]= -1;
-      l->invert_forcer= seg;
-    }
-
-    dist_until= (overall - remain) - advance;
-    time_until= !speed ? 0 : SPEED_CALC_TIME(speed, dist_until, DOWN);
-    *(check_clash ? &seg->until_here : &seg->until_detect)= time_until;
-
-    if (!remain) break;
-    int r= trackloc_further(&tloc, &remain);
-
-fprintf(stderr,"lay_train_pass   further=%d tloc=@%s+%ld remain=%ld\n",
-	r, tloc.seg->i->pname, tloc.into, remain);
-
-    switch (r) {
-    case -2:
-      l->ec= safety_problem(l->tra, seg, "end of track");
-      return;
-    case -1:
-      l->ec= safety_problem(l->tra, tloc.err, "track route not set");
-      return;
-    case +1:
-      pci= trackloc_segposcomb(&tloc);
-      link= trackloc_segmentlink(&tloc,pci,0);
-      if (link->next+segments != seg) {
-	l->ec= safety_problem(l->tra, link->next+segments,
-			      "track route set against us");
-	return;
-      }
-      break;
-    case 0:
-      return;
-    default:
-      abort();
-    }
-  }
-}
-
-static void lay_train_inversions(LayTrainState *l) {
-  SEG_IV;
-  int train_be_inverted, seg_be_inverted;
-
-  if (l->ec) return;
-
-  train_be_inverted= l->invert_count[1] > l->invert_count[0];
-  assert(l->invert_count[train_be_inverted] >= 0);
-
-  actual_inversions_start();
+  return 0;
 
+ xproblem:
   FOR_SEG {
-    if (!seg->tr_updated) continue;
-    assert(seg->owner == l->tra);
-    seg_be_inverted= train_be_inverted ^ seg->tr_backwards;
-    assert(!(seg_be_inverted && !segi->invertible));
-    if (!segi->invertible)
-      continue;
-    seg->seg_inverted= seg_be_inverted;
-    actual_inversions_segment(seg);
-  }
-  actual_inversions_done();
-}
-
-static void lay_train_done(LayTrainState *l) {
-  SEG_IV;
+    seg->now_present= seg->pred_present=
+      seg->pred_vacated= seg->will_polarise= 0;
 
-  FOR_SEG {
-    if (seg->owner == l->tra) {
-      if (!seg->tr_updated) seg_clear_stale(seg);
-      seg->tr_updated= 0;
+    if (!seg->owner && !seg->moving && seg->motion) {
+      movpos_unreserve(seg->motion);
+      seg->motion= 0;
     }
-    assert(!seg->tr_updated);
-    assert(seg->until_detect >= seg->until_here);
   }
 }
 
-static ErrorCode lay_train(Train *tra, long added_slop) {
-  Segment *seg;
-  TrackLocation tloc;
-  long head, headslop, tail, taildet;
-  LayTrainState l;
-
-  seg= tra->foredetect;
-
-  tloc.seg= seg;
-  tloc.into= tra->maxinto;
-  tloc.backwards= seg->tr_backwards;
-
-fprintf(stderr,"lay_train %s @%s added_slop=%ld maxinto=%ld "
-	"tra->backwards=%d tloc.backwards=%d\n",
-	seg->i->pname, tra->pname, added_slop, (long)tra->maxinto,
-	tra->backwards, tloc.backwards);
-
-  l.tra= tra;
-  l.ec= 0;
-  l.invert_count[0]= l.invert_count[1]= 0;
-
-  head= tra->backwards ? tra->tail : tra->head;
-  headslop= head + added_slop;
-
-  /* 1st pass:
-   * update actual train location (train's head and future locations).
-   *
-   * 2nd pass:
-   * update detection (future locations) only.
-   *
-   * 3rd pass:
-   * train location (current location and tail)
-   *
-   * Clash checking is done in passes 1 and 3, including checking
-   * whether the train clashes with itself (ie, fail if we find
-   * segment with same train and tr_updated set).
-   */
-  lay_train_pass(&l, tloc,  headslop, tra->speed, 0, 1);
-  lay_train_pass(&l, tloc,  0,        tra->speed, 0, 0);
-
-  trackloc_reverse(&tloc);
-  seg->tr_updated= 0; /* we're about to do this one again */
-  tail= tra->backwards ? tra->head : tra->tail;
-  taildet= tra->detectable + tail + tra->maxinto;
-
-  lay_train_pass(&l, tloc, taildet,   0,          1, 1);
+/*========== entrypoints from rest of the program ==========*/
 
-  lay_train_inversions(&l);
-  lay_train_done(&l);
-
-  return l.ec;
+static void detection_report_problem(Train *tra, TrackSegment *seg,
+				     void *pu, const char *message) {
+  logmsg("SignallingProblemPredicted",tra,seg->i,
+	 "re detection @%s: %s", tra->foredetect->i->pname, message);
 }
 
 void safety_notify_detection(Segment *seg) {
-  Train *tra= seg->owner;
-  ErrorCode ec;
-  TrackLocation tloc;
-
-  if (!seg->until_detect)
-    return;
-
-  tloc.seg= seg;
-  tloc.into= 0;
-  tloc.backwards= seg->tr_backwards;
+  Train *tra;
+  
+  if (seg->det_ignore) return;
+  if (!seg->det_expected)
+    safety_panic(0,seg, "unexpected detection");
 
+  tra= seg->owner;
   tra->foredetect= seg;
-  tra->maxinto= trackloc_remaininseg(&tloc);
-
-  if (seg->cm_autostop) {
-    seg->cm_autostop= 0;
-    if (!tra->estopping) {
-      speedmanager_autostop(tra);
-      if (!tra->speed && tra->maxinto > AUTOSTOP_UNCERTAINTY)
-	/* At some point we may need to allow more slop when the
-	 * next segment is points than when this is the last segment
-	 * (ie, just buffers next). */
-	tra->maxinto= AUTOSTOP_UNCERTAINTY;
-    }
-  }
-  tra->uncertainty= tra->maxinto;
-
-  if (!tra->estopping) {
-    ec= lay_train(tra, 0);
-    if (ec) {
-      logmsg(ec,tra,seg->i, "emergency stop on reaching here");
-      safety_emergencystop(tra);
-    }
-  }
-}
-
-void safety_emergencystop(Train *tra) {
-  ErrorCode ec;
+  if (tra->foredetect->movposcomb < 0)
+    safety_panic(tra,seg, "track route not set and train has arrived");
 
-  if (tra->estopping) return;
+  ec= predict_confirm(tra, 0, detection_report_problem, 0);
+  if (!ec) return;
+  
+  assert(ec == EC_SignallingProblemPredicted);
 
-  ec= lay_train(tra, ESTOP_UNCERTAINTY);
-  if (ec) safety_panic(tra,0, "emergency stop forbidden!");
-  speedmanager_emergencystop(tra);
+  tra->sigstopping= 1;
+  predict_confirm(tra, 0, 0,"detection");
+  assert(!ec);
 }
 
 ErrorCode safety_checkmovposchange(Segment *seg) {
   ErrorCode ec;
   if (seg->owner) {
-    ec= safety_problem(seg->owner, seg, "must not unset route train is using");
-    logmsg(ec,seg->owner,seg->i,"countermanded point change");
-    return ec;
+    oprintf(UPO,"ack SignallingProblemPredicted @%s %s:"
+	    " route set for approaching train",
+	    seg->i->pname, seg->owner->pname);
+    return EC_BadCmd;
   }
   return 0;
 }
 
+  /* ============ OLD CODE ========== */
+
 ErrorCode safety_requestspeed(Train *tra, long newspeed) {
   long oldspeed;
   ErrorCode ec;
diff --git a/hostside/safety.h b/hostside/safety.h
index 777b356..7947b64 100644
--- a/hostside/safety.h
+++ b/hostside/safety.h
@@ -83,9 +83,9 @@ struct Segment {
  * handling, arg parsing, etc.
  */
 
-void vlogmsg(ErrorCode ec, Train *tra, const SegmentInfo *segi,
+void vlogmsg(const char *intro, Train *tra, const SegmentInfo *segi,
 	     const char *fmt, va_list al) __attribute__((format(printf,4,0)));
-void logmsg(ErrorCode ec, Train *tra, const SegmentInfo *segi,
+void logmsg(const char *intro, Train *tra, const SegmentInfo *segi,
 	    const char *fmt, ...) __attribute__((format(printf,4,5)));
 
 void safety_vpanic(Train *tra, Segment *seg, const char *fmt, va_list al)
@@ -93,10 +93,6 @@ void safety_vpanic(Train *tra, Segment *seg, const char *fmt, va_list al)
 void safety_panic(Train *tra, Segment *seg, const char *fmt,...)
      __attribute__((format(printf,3,4),noreturn));
 
-ErrorCode safety_problem(Train *tra, Segment *seg, const char *fmt, ...)
-     __attribute__((format(printf,3,4)));
-     /* simple wrapper around vlogmsg; implies and returns EC_Safety */
-
 /*========== safety.c ==========*/
 /*
  * safety.c is responsible for ensuring that things don't go