Improve the handling of sensor events in Train

diff --git a/source/libmarklin/train.cpp b/source/libmarklin/train.cpp
index e64ef00a1b78cee8c67edc65667836915d5c5f77..e992f418ed4cc324a4ff84d900ded8ac37700a0d 100644 (file)
--- a/source/libmarklin/train.cpp
+++ b/source/libmarklin/train.cpp
@@ -228,6 +228,7 @@ void Train::sensor_event(bool state, Sensor *sensor)
 
        if(state)
        {
+               // Find the first sensor block from our reserved blocks that isn't this sensor
                list<BlockRef>::iterator i;
                for(i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
                        if(i->block->get_sensor_id() && i->block->get_sensor_id()!=addr)
@@ -235,6 +236,7 @@ void Train::sensor_event(bool state, Sensor *sensor)
 
                if(i!=rsv_blocks.begin())
                {
+                       // Compute speed and update related state
                        float travel_time_secs = (Time::now()-last_entry_time)/Time::sec;
                        travel_speed = static_cast<int>(round(travel_dist/travel_time_secs*87*3.6/5))*5;
 
@@ -250,35 +252,44 @@ void Train::sensor_event(bool state, Sensor *sensor)
                        {
                                j->block->traverse(j->entry, &block_len);
                                travel_dist += block_len;
+
+                               if(j->block->get_sensor_id()==addr)
+                                       set_position(j->block->get_endpoints()[j->entry]);
                        }
                        last_entry_time = Time::now();
                        pure_speed = true;
 
+                       // Move blocks up to the next sensor to our current blocks
                        cur_blocks.splice(cur_blocks.end(), rsv_blocks, rsv_blocks.begin(), i);
-               }
-
-               for(i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
-                       if(i->block->get_sensor_id()==addr)
-                               set_position(i->block->get_endpoints()[i->entry]);
 
-               if(target_speed && reserve_more()<2)
-                       update_speed();
+                       // Try to get more blocks if we're moving
+                       if(target_speed && reserve_more()<2)
+                               update_speed();
+               }
        }
        else
        {
+               // Find the first sensor in our current blocks that's still active
+               list<BlockRef>::iterator end = cur_blocks.begin();
                for(list<BlockRef>::iterator i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
-                       if(unsigned b_addr = i->block->get_sensor_id())
+                       if(i->block->get_sensor_id())
                        {
-                               if(b_addr==addr)
-                               {
-                                       ++i;
-                                       for(list<BlockRef>::iterator j=cur_blocks.begin(); j!=i; ++j)
-                                               j->block->reserve(0);
-                                       cur_blocks.erase(cur_blocks.begin(), i);
-                               }
-                               break;
+                               if(trfc_mgr.get_control().get_sensor(i->block->get_sensor_id()).get_state())
+                                       break;
+                               else
+                                       end = i;
                        }
+               
+               if(end!=cur_blocks.begin())
+               {
+                       // Free blocks up to the last inactive sensor
+                       ++end;
+                       for(list<BlockRef>::iterator i=cur_blocks.begin(); i!=end; ++i)
+                               i->block->reserve(0);
+                       cur_blocks.erase(cur_blocks.begin(), end);
+               }
 
+               // XXX Should watch for trfc_mgr.signal_block_reserved rather than sensors
                if(target_speed && pending_block && addr==pending_block->get_sensor_id())
                        reserve_more();
        }