Hopefully-this-time-correct arrangements for dealing with uncertainty>maxinto. safet...

diff --git a/hostside/safety.c b/hostside/safety.c
index 21e1d9e324942b24fdeab22e27e449bd28db575c..f7bd78218e6fc0703d3e45c0a49145709d84f050 100644 (file)
--- a/hostside/safety.c
+++ b/hostside/safety.c
@@ -732,12 +732,15 @@ ErrorCode predict(Train *tra, struct timeval tnow, unsigned flags,
   u.desire_move= desire_move;
   u.desire_movposcomb= desire_movposcomb;
 
+  Distance detectable= ceil(tra->detectable * MARGIN_TRAINLENGTH);
+
   if (DEBUGP(safety,core)) {
     DPRINTF(safety,core," ***predicting*** %s%s ",
            tra->backwards?"-":"",tra->pname);
     qprintf_position(tra,DPRINTFA);
-    DPRINTFA(" %smaxspeed=%f%s stopdist=%d (speed %f, step %d%s)"
+    DPRINTFA(" det.able=%d %smaxspeed=%f%s stopdist=%d (speed %f, step %d%s)"
             " flags=%c%c%s desire=%s/%s\n",
+            detectable,
             u.speed_info_specified ? "specified " : "",
             u.maxspeed, u.stopping ? "(stopping)" : "",
             u.stopping_distance,
@@ -772,17 +775,22 @@ ErrorCode predict(Train *tra, struct timeval tnow, unsigned flags,
    * so we must instead extend our idea of the length of the train.
    */
 
-  int tail_walkback= MARGIN_NOSE + tail_length(tra) +
-    ceil(tra->detectable * MARGIN_TRAINLENGTH);
-  int tail_advance_delaydist= MARGIN_TAIL;
+  int effective_uncertainty= tra->uncertainty;
+  int effective_length= MARGIN_NOSE + tail_length(tra) + detectable;
 
-  int effective_into_fdet= tra->maxinto - tra->uncertainty;
-  if (effective_into_fdet < 0) {
-    tail_advance_delaydist += effective_into_fdet;
-    effective_into_fdet -= effective_into_fdet;
-    assert(!effective_into_fdet);
+  if (effective_uncertainty > tra->maxinto) {
+    effective_length += tra->uncertainty - tra->maxinto;
+    effective_uncertainty= tra->maxinto;
   }
-  
+
+  int effective_into_fdet= tra->maxinto - effective_uncertainty;
+  int tail_advance_delaydist= MARGIN_TAIL;
+
+  DPRINTF(safety,core,"  predict eff.uncert=%d eff.len=%d"
+         " eff.into_fdet=%d tail.adv.delay=%d\n",
+         effective_uncertainty, effective_length, 
+         effective_into_fdet, tail_advance_delaydist);
+
   u.fdetc.u= &u;
   u.fdetc.getmovpos= pred_getmovpos;
 
@@ -798,7 +806,7 @@ ErrorCode predict(Train *tra, struct timeval tnow, unsigned flags,
   u.tailc.getmovpos= pred_getmovpos;
   u.tailc.nextseg= initpresent_nextseg;
   u.tailc.trackend= pred_trackend_panic;
-  u.tailc.distance= tail_walkback;
+  u.tailc.distance= effective_length;
 
   u.tail= u.fdet;
 
@@ -837,7 +845,7 @@ ErrorCode predict(Train *tra, struct timeval tnow, unsigned flags,
     /* we actually know exactly where we are */
     u.fdetc.distance= 0;
   } else { /* stopping, but we're mid-segment */
-    u.fdetc.distance= tra->uncertainty;
+    u.fdetc.distance= effective_uncertainty;
   }
   u.elapsed= -u.fdetc.distance / u.maxspeed; /* imagine we're already there */
   u.fdetc.distance += u.stopping_distance + MARGIN_NOSE;