Improve the interface and algorithms of BlockAllocator

[r2c2.git] / source / libr2c2 / train.cpp

diff --git a/source/libr2c2/train.cpp b/source/libr2c2/train.cpp
index 72714d730032094f5ae5419c24ffdef38a13ff1e..b7dc2470c4c1a98e98e5aae789c39e4130e1f31f 100644 (file)
--- a/source/libr2c2/train.cpp
+++ b/source/libr2c2/train.cpp
@@ -40,7 +40,6 @@ Train::Train(Layout &l, const VehicleType &t, unsigned a, const string &p):
        speed_changing(false),
        reverse(false),
        functions(0),
-       travel_dist(0),
        pure_speed(false),
        speed_quantizer(0),
        accurate_position(false),
@@ -370,20 +369,9 @@ void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt)
                        set_active(true);
 
                Vehicle &vehicle = *(reverse ? vehicles.back() : vehicles.front());
-               Track *track = vehicle.get_track();
-
-               BlockIter first = allocator.first();
-               BlockIter last_current = allocator.last_current();
-               bool ok = false;
-               for(BlockIter i=first; !ok; i=i.next())
-               {
-                       ok = i->has_track(*track);
-                       if(i==last_current)
-                               break;
-               }
 
                float d = speed*(dt/Time::sec);
-               if(ok)
+               if(allocator.is_block_current(vehicle.get_track()->get_block()))
                {
                        SetFlag setf(advancing);
                        vehicle.advance(reverse ? -d : d);
@@ -482,52 +470,47 @@ void Train::loco_func_event(unsigned addr, unsigned func, bool state)
 
 void Train::block_state_changed(Block &block, Block::State state)
 {
-       if(state==Block::MAYBE_ACTIVE)
+       if(block.get_train()==this && state==Block::MAYBE_ACTIVE)
        {
-               const BlockIter &first = allocator.first();
-               const BlockIter &last_cur = allocator.last_current();
-               bool valid = false;
-               for(BlockIter i=first; !valid; i=i.next())
+               if(last_entry_block)
                {
-                       if(&*i==&block)
-                               valid = true;
-                       if(&*i==&*last_cur)
-                               break;
-               }
+                       float travel_distance = -1;
+                       if(pure_speed && speed_quantizer && current_speed_step>0)
+                               travel_distance = 0;
 
-               if(valid)
-               {
-                       // Compute speed and update related state
-                       float travel_time_secs = (Time::now()-last_entry_time)/Time::sec;
+                       for(BlockIter i=last_entry_block; &*i!=█ i=i.next())
+                       {
+                               if(i->get_sensor_id())
+                                       return;
+                               if(travel_distance>=0)
+                                       travel_distance += i->get_path_length(i.entry());
+                       }
 
-                       if(pure_speed && speed_quantizer && current_speed_step>0 && travel_time_secs>=2)
-                               speed_quantizer->learn(current_speed_step, travel_dist/travel_time_secs, travel_time_secs);
+                       if(travel_distance>0)
+                       {
+                               float travel_time_secs = (Time::now()-last_entry_time)/Time::sec;
+
+                               if(travel_time_secs>=2)
+                                       speed_quantizer->learn(current_speed_step, travel_distance/travel_time_secs, travel_time_secs);
+                       }
+               }
 
-                       travel_dist = 0;
-                       unsigned entry = allocator.get_entry_to_block(block);
-                       for(BlockIter i(&block, entry);; i=i.next())
+               last_entry_block = allocator.iter_for(block);
+               last_entry_time = Time::now();
+               pure_speed = true;
+               accurate_position = true;
+               overshoot_dist = 0;
+
+               if(!advancing && vehicles.front()->get_track())
+               {
+                       TrackIter track = last_entry_block.track_iter();
+                       if(reverse)
                        {
-                               travel_dist += i->get_path_length(i.entry());
-
-                               if(&*i==&block && !advancing && vehicles.front()->get_track())
-                               {
-                                       TrackIter track = i.track_iter();
-                                       if(reverse)
-                                       {
-                                               track = track.flip();
-                                               vehicles.back()->place(track, 0, Vehicle::BACK_AXLE);
-                                       }
-                                       else
-                                               vehicles.front()->place(track, 0, Vehicle::FRONT_AXLE);
-                               }
-
-                               if(i==last_cur)
-                                       break;
+                               track = track.flip();
+                               vehicles.back()->place(track, 0, Vehicle::BACK_AXLE);
                        }
-                       last_entry_time = Time::now();
-                       pure_speed = true;
-                       accurate_position = true;
-                       overshoot_dist = 0;
+                       else
+                               vehicles.front()->place(track, 0, Vehicle::FRONT_AXLE);
                }
        }
 }