Don't reset router to free run mode until allocator has deactivated

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

diff --git a/source/libr2c2/blockallocator.cpp b/source/libr2c2/blockallocator.cpp
index b38b62d6d1f1cd37763fcb2c3979dff8d4e97439..215d746a25e3442bbe0cad476028545d53569789 100644 (file)
--- a/source/libr2c2/blockallocator.cpp
+++ b/source/libr2c2/blockallocator.cpp
@@ -28,9 +28,11 @@ struct BlockAllocator::BlockMatch
 BlockAllocator::BlockAllocator(Train &t):
        train(t),
        cur_blocks_end(blocks.end()),
+       next_sensor(0),
        pending_block(0),
        stop_at_block(0),
-       reserving(false)
+       reserving(false),
+       advancing(false)
 {
        Layout &layout = train.get_layout();
        layout.signal_block_reserved.connect(sigc::mem_fun(this, &BlockAllocator::block_reserved));
@@ -81,8 +83,9 @@ void BlockAllocator::rewind_to(const Block &block)
 
 void BlockAllocator::clear()
 {
-       active = false;
        release_blocks_begin(blocks.end());
+       active = false;
+       next_sensor = 0;
        pending_block = 0;
        stop_at_block = 0;
 }
@@ -151,7 +154,7 @@ void BlockAllocator::reserve_more()
        else if(&*start==pending_block)
        {
                TrackIter track = start.track_iter();
-               if(!track.endpoint().has_path(track->get_active_path()))
+               if(track->is_path_changing() || !track.endpoint().has_path(track->get_active_path()))
                        return;
        }
 
@@ -192,7 +195,10 @@ void BlockAllocator::reserve_more()
                }
 
                if(!reserve_block(block))
+               {
+                       pending_block = &*block;
                        break;
+               }
 
                if(cur_blocks_end==blocks.end())
                        --cur_blocks_end;
@@ -237,9 +243,12 @@ void BlockAllocator::reserve_more()
                        dist += block->get_path_length(block.entry());
        }
 
-       // Make any sensorless blocks at the beginning immediately current
-       while(cur_blocks_end!=blocks.end() && !(*cur_blocks_end)->get_sensor_id())
-               ++cur_blocks_end;
+       if(!next_sensor)
+       {
+               update_next_sensor(0);
+               // Immediately advance to just before the next sensor
+               advance_to(next_sensor ? next_sensor->get_block() : 0);
+       }
 }
 
 bool BlockAllocator::reserve_block(const BlockIter &block)
@@ -264,6 +273,22 @@ bool BlockAllocator::reserve_block(const BlockIter &block)
        }
 }
 
+void BlockAllocator::advance_to(const Block *block)
+{
+       BlockList::iterator end;
+       if(block)
+               end = find_if(cur_blocks_end, blocks.end(), BlockMatch(*block));
+       else
+               end = blocks.end();
+
+       SetFlag setf(advancing);
+       BlockList::iterator i = cur_blocks_end;
+       // Update cur_blocks_end first to keep things consistent.
+       cur_blocks_end = end;
+       for(; i!=end; ++i)
+               train.signal_advanced.emit(**i);
+}
+
 void BlockAllocator::release_blocks_begin(const BlockList::iterator &end)
 {
        for(BlockList::iterator i=blocks.begin(); i!=end; )
@@ -291,8 +316,12 @@ void BlockAllocator::release_blocks_end(const BlockList::iterator &begin)
 
 void BlockAllocator::release_block(const BlockList::iterator &i)
 {
+       if(advancing)
+               throw logic_error("cannot release while advancing");
        if(i==cur_blocks_end)
                ++cur_blocks_end;
+       if(next_sensor && &**i==next_sensor->get_block())
+               next_sensor = 0;
        if(&**i==pending_block)
                pending_block = 0;
 
@@ -337,54 +366,27 @@ void BlockAllocator::block_reserved(Block &block, const Train *tr)
 
 void BlockAllocator::sensor_state_changed(Sensor &sensor, Sensor::State state)
 {
-       Block *block = 0;
-       if(TrackCircuit *tc = dynamic_cast<TrackCircuit *>(&sensor))
-               block = &tc->get_block();
-       else
-               return;
-
-       if(block->get_train()!=&train)
+       Block *block = sensor.get_block();
+       if(!block || block->get_train()!=&train)
                return;
 
        if(state==Sensor::MAYBE_ACTIVE)
        {
-               // Find the first sensor block from our reserved blocks that isn't this sensor
-               BlockList::iterator end;
-               unsigned result = 0;
-               for(end=cur_blocks_end; end!=blocks.end(); ++end)
-                       if((*end)->get_sensor_id())
-                       {
-                               if(&**end!=block)
-                               {
-                                       if(result==0)
-                                               result = 2;
-                                       else if(result==1)
-                                               break;
-                               }
-                               else if(result==0)
-                                       result = 1;
-                               else if(result==2)
-                                       result = 3;
-                       }
-
-               if(result==1)
+               if(&sensor==next_sensor)
                {
-                       // Move blocks up to the next sensor to our current blocks
-                       for(BlockList::iterator j=cur_blocks_end; j!=end; ++j)
-                               train.signal_advanced.emit(**j);
-                       cur_blocks_end = end;
+                       update_next_sensor(next_sensor);
+                       advance_to(next_sensor ? next_sensor->get_block() : 0);
 
                        if(active)
                                reserve_more();
                }
-               else if(result==3)
+               else if(!is_block_current(*block))
                        train.get_layout().emergency("Sensor for "+train.get_name()+" triggered out of order");
        }
        else if(state==Sensor::INACTIVE)
        {
                const Vehicle &veh = train.get_vehicle(train.get_controller().get_reverse() ? 0 : train.get_n_vehicles()-1);
                const Block &veh_block = veh.get_track()->get_block();
-               const Driver &driver = train.get_layout().get_driver();
 
                /* Sensors aren't guaranteed to be detriggered in order.  Go through the
                block list and locate the first sensor that's still active. */
@@ -395,9 +397,9 @@ void BlockAllocator::sensor_state_changed(Sensor &sensor, Sensor::State state)
                        if(&**i==&veh_block)
                                break;
 
-                       if((*i)->get_sensor_id())
+                       if(Sensor *s = (*i)->get_sensor())
                        {
-                               if(driver.get_sensor((*i)->get_sensor_id()))
+                               if(s->get_state()>=Sensor::MAYBE_INACTIVE)
                                        break;
                                else
                                        end = i;
@@ -410,6 +412,23 @@ void BlockAllocator::sensor_state_changed(Sensor &sensor, Sensor::State state)
        }
 }
 
+void BlockAllocator::update_next_sensor(Sensor *after)
+{
+       BlockList::iterator i = cur_blocks_end;
+       if(after)
+               i = find_if(i, blocks.end(), BlockMatch(*after->get_block()));
+
+       for(; i!=blocks.end(); ++i)
+               if(Sensor *sensor = (*i)->get_sensor())
+                       if(sensor!=next_sensor)
+                       {
+                               next_sensor = sensor;
+                               return;
+                       }
+
+       next_sensor = 0;
+}
+
 void BlockAllocator::save(list<DataFile::Statement> &st) const
 {
        if(!blocks.empty() && cur_blocks_end!=blocks.begin())