train changed wip going well

diff --git a/hostside/safety.c b/hostside/safety.c
index cc8ffac1b4305ffa946a6c9a55dda618c42ad07d..85198d4949066045ccf173fc89c1053642b28162 100644 (file)
--- a/hostside/safety.c
+++ b/hostside/safety.c
@@ -4,68 +4,173 @@
 
 #include "safety.h"
 
-State *s, safety_buf[2];
+State safety_actual;
 
 typedef struct TrackLocation TrackLocation;
 struct TrackLocation {
-  SegmentIx segi; /* current segment */
-  Distance into; /* distance from start of segment */
+  SegmentNum segn; /* current segment */
+  long into; /* distance from start of segment */
   unsigned backwards:1; /* if 1, into is positive and measured from end */
 };
 
-void trackfollow_further(TrackLocation *tloc, Distance *remain_io);
+void trackloc_further(TrackLocation *tloc, long *remain_io);
   /* Advances tloc, decrementing *remain_io, until either
-   * *remain_io becomes zero, or tloc->segi changes. */
+   * *remain_io becomes zero, or tloc->segn changes. */
 
-void safety_train_changed(TrainIx trai) {
-  TrainState *tra = s->tras[trai];
-  State *try = &safety_buf[!(s-safety_buf)];
+long trackloc_remaininseg(const TrackLocation *tloc);
+  /* Amount that tloc can advance before it goes into next segment. */
+
+static voi seg_clear_stale(SegmentState *seg) {
+  if (!seg->tr_updated) {
+    seg->tr_here_now= seg->tr_here_future=
+      seg->tr_detect_now= seg->tr_detect_future= 0;
+  }
+}
+
+static void lay_train(ErrorCode *ec, TrainNum tran,
+                     TrackLocation tloc, long distance,
+                     unsigned detect_now, unsigned detect_future,
+                     unsigned here_now, unsigned here_future,
+                     unsigned clash_if_updated) {
+  SegmentState *seg;
+
+  if (*ec) return;
+
+  for (;;) {
+    seg= s->segs[tloc.segn];
+    if (clash_if_updated && seg->tr_updated) {
+      *ec= safety_problem(tloc.segn, tran, tran, "collision with itself");
+      return;
+    }
+    if (seg->here_future) {
+      if (seg->owner != tran) {
+       *ec= safety_problem(tloc.segn, tran, seg->owner, "collision");
+       return;
+      }
+      seg_clear_stale(seg);
+    }
+    seg->tran= tran;
+    seg->tr_updated= 1;
+    seg->tr_here_now |= here_now;
+    seg->tr_here_future |= here_future;
+    seg->tr_detect_now |= detect_now;
+    seg->tr_detect_future |= detect_future;
+
+    if (!distance) break;
+    
+    trackloc_further(&tloc, &distance);
+  }
+}
+
+static void lay_train_done() {
+  for (segn=0, seg=s->segs;
+       segn <= NUM_SEGMENTS;
+       segn++, seg++) {
+    if (seg->tr_here_future && seg->tran == tran) {
+      seg_clear_stale(seg);
+      seg->tr_updated= 0;
+    }
+    assert(!seg->tr_updated);
+    assert(seg->tr_here_future >= seg->tr_here_now);
+    assert(seg->tr_detect_future >= seg->tr_detect_now);
+    assert(seg->tr_detect_future >= seg->tr_here_future);
+    assert(seg->tr_detect_now >= seg->tr_here_now);
+  }
+}
+
+void safety_train_changed(TrainNum tran) {
+  State *s= &safety_state;
+  TrainState *tra= &s->tras[tran];
+  TrainInfo *trai= info_train[tran];
+  SegmentState *seg;
   TrackLocation tloc;
+  long head, future;
+
+  segn= tra->foredetect;
+  seg= &s->seg[segn];
+
+  tloc.segn= segn;
+  tloc.into= 0;
+  tloc.backwards= seg->tr_backwards;
+
+  tloc.into= trackloc_remaininseg(&tloc);
+  if (tra->justarrived && tloc.into > JUSTARRIVED_DIST)
+    tloc.into= JUSTARRIVED_DIST;
+
+  head= tra->backwards ? trai->tail : trai->head;
+  future= tra->speed * SPEED_CLEAR_MULT;
+  lay_train(tran, &ec, tloc, head + future, 0,0,0,1);
+  lay_train(tran, &ec, tloc, head,          0,0,1,1);
+  lay_train(tran, &ec, tloc,        future, 0,1,0,1);
+  lay_train(tran, &ec, tloc, 0,             1,1,1,1);
+  lay_train_done(tran);
+
+  if (ec) return ec;
+  
+    
+           (
+           
+           );
+  lay_train(tloc,
+           tra->backwards ? trai->tail : trai->head,
+           
+    
+
+  remain= 
+    mark segment as being owned by train but not detectable;
+    
+
+  for (
+       remain
+  while (remain) {
+    trackloc_further
+
+
+  segi= &safety_segis[segn];
+  lnki1= (seg->reverse ? &segi->forwards :
+         seg->sideways ?  &segi->sideways :
+         &segi->backwards);
+  lnki2= (seg->reverse ? &segi->backwards :
+         seg->sideways ?  &segi->sideways :
+         &segi->forwares);
+
+  tloc.segn= segn;
+  tloc.into= 
+    
+    
 
-  try->tras= s->tras;
-  for (lsegi=0, dlseg=try->segs, slseg=s->segs;
-       lsegi < NUM_SEGMENTS;
-       lsegi++, dlseg++, slseg++) {
-    *dlseg = *slseg;
-    if (dlseg->owner == trai) {
-      dlseg->owner = NO_TRAIN;
-      dlseg->present_now = dlseg->present_future =
-       dlseg->detectable_now = dlseg->detectable_future =
-       dlseg->autostop = 0;
-      
 
-  tloc.segi= tra->foredetect;
-  tloc.into= tra->justarrived ? JUSTARRIVED_DIST : 
 
 
-  for (segi = 
-s->trais[trai]
-  SegmentState *seg = s->seg[segi];
+  for (segn = 
+s->tras[tran]
+  SegmentState *seg = s->seg[segn];
 
-void safety_notify_detection(SegmentIx segi) {
-  SegmentState *seg = s->seg[segi];
-  TrainIx trai = segs->owner;
-  TrainState *tra = s->tras[trai];
+void safety_notify_detection(SegmentNum segn) {
+  State *s = &safety_state;
+  SegmentState *seg = s->seg[segn];
+  TrainNum tran = segs->owner;
+  TrainState *tra = s->tras[tran];
 
   if (seg->detectable_now) return;
   if (!seg->detectable_future)
-    safety_panic(NONE, segi, "unexpected detection");
+    safety_panic(segn, NONE, "unexpected detection");
 
-  tra->foredetect= segi;
+  tra->foredetect= segn;
   tra->justarrived= 0;
 
   if (seg->autostop) {
-    actual_setspeed(trai, 0);
+    actual_setspeed(tran, 0);
     tra->justarrived= 1;
   }
 
-  ec= safety_train_changed(trai);
+  ec= safety_train_changed(tran);
   if (ec) {
-    logmsg(trai, segi, "emergency stop");
-    actual_emergencystop(trai);
+    logmsg(tran, segn, "emergency stop");
+    actual_emergencystop(tran);
     tra->justarrived= 0;
-    ec= safety_train_changed(s, trai);
-    if (ec) panic(trai, segi, "emergency stop insufficient!");
+    ec= safety_train_changed(s, tran);
+    if (ec) panic(segn, tran, "emergency stop insufficient!");
   }
 }
 

diff --git a/hostside/safety.h b/hostside/safety.h
index bf6d27663c0553d4499526050169b12d92779b5d..454a4fec36fd6f48696d069ed77d245711d5cf1c 100644 (file)
--- a/hostside/safety.h
+++ b/hostside/safety.h
@@ -3,37 +3,38 @@
 #ifndef SAFETY_H
 #define SAFETY_H
 
-typedef unsigned short TrainIx;
-typedef unsigned short SegmentIx;
-typedef unsigned short LocationIx;
+typedef unsigned short TrainNum;
+typedef unsigned short SegmentNum;
+typedef unsigned short LocationNum;
 typedef short Distance;
 typedef char Speed; /* non-negative, units of 4mm/s */
 
 typedef struct {
-  SegmentIx foredetect;
+  SegmentNum foredetect;
   unsigned
     justarrived:1, /* is stopped just after triggering foredetect segment */
-    reverse:1; /* train is moving backwards wrt its own front and back */
+    backwards:1; /* train is moving backwards wrt its own front and back */
   Speed speed;
 } TrainState;
 
 typedef struct {
   unsigned
-    present_now:1, present_future:1, /* owning train is or will use space */
-    detectable_now:1, detectable_future:1, /* owning train draws current */
-    reverse:1, /* owning train's motion is (would be) backwards wrt track */
-    sideways:1, /* for segment with points, points are set to `alternative' */
-    autostop:1, /* owning train is slow and wants to stop on detection */
-    updated:1; /* for use by safety_train_changed etc.; otherwise 0 */
+    tr_detect_now:1, tr_detect_future:1, /* owning train draws current */
+    tr_here_now:1, tr_here_future:1, /* owning train is or will use space */
+    tr_backwards:1, /* train's motion is (would be) backwards wrt track */
+    pt_sideways:1, /* points are set to `alternative'. (no points?: 0) */
+    tr_autostop:1, /* owning train is slow and wants to stop on detection */
+    tr_updated:1; /* for use by safety_train_changed etc.; otherwise 0 */
   /*polarity?*/
+  TrainNum tran; /* always valid but only meaningful iff tr_here_future */
 } SegmentState;
 
 typedef struct {
   unsigned next_rev:1;
-  SegmentIx next;
-  SegmentIx clashing;
+  SegmentNum next;
+  SegmentNum clashing;
   Distance dist;
-} SegmentLink;
+} SegmentLinkInfo;
 
 typedef struct {
   SegmentLink backwards, forwards, sideways;
@@ -44,9 +45,11 @@ typedef struct {
   Distance tail, detectable, head;
 } TrainInfo;
 
+extern const TrainInfo info_train[NUM_TRAINS];
+extern const SegmentInfo info_segment[NUM_SEGMENTS];
 
 typedef struct {
-  TrainState trasNUM_TRAINS];
+  TrainState tras[NUM_TRAINS];
   SegmentState segs[NUM_SEGMENTS];
 } State;