Improve the block reservation algorithm

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

diff --git a/source/libmarklin/train.cpp b/source/libmarklin/train.cpp
index ef38416e98454ab4f0d3f3aaba00a6da07d05850..8f8ee490fe2fa67cf5f33113d2efd0efea03c7c5 100644 (file)
--- a/source/libmarklin/train.cpp
+++ b/source/libmarklin/train.cpp
@@ -26,6 +26,7 @@ Train::Train(TrafficManager &tm, Locomotive &l):
        loco(l),
        pending_block(0),
        target_speed(0),
+       route(0),
        status("Unplaced"),
        travel_dist(0),
        travel_speed(0),
@@ -138,6 +139,17 @@ bool Train::free_block(Block *block)
        return false;
 }
 
+int Train::get_entry_to_block(Block *block) const
+{
+       for(list<BlockRef>::const_iterator i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
+               if(i->block==block)
+                       return i->entry;
+       for(list<BlockRef>::const_iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
+               if(i->block==block)
+                       return i->entry;
+       return -1;
+}
+
 void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt)
 {
        if(try_reserve && t>try_reserve)
@@ -312,31 +324,67 @@ unsigned Train::reserve_more()
 
        pending_block = 0;
 
+       // See how many blocks we already have
        unsigned nsens = 0;
        for(list<BlockRef>::const_iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
                if(i->block->get_sensor_id())
                        ++nsens;
 
        bool got_more = false;
-       while(nsens<3)
+       BlockRef *good = last;
+       unsigned good_sens = nsens;
+       while(good_sens<3)
        {
-               int exit = last->block->traverse(last->entry);
-               if(exit<0)
+               // Traverse to the next block
+               unsigned exit = last->block->traverse(last->entry);
+               Block *link = last->block->get_link(exit);
+               if(!link)
                        break;
 
-               Block *link = last->block->get_link(exit);
-               if(!link || !link->reserve(this))
+               int entry = link->get_endpoint_by_link(*last->block);
+               if(!link->reserve(this))
                {
+                       // If we found another train going in the same direction as us, we can keep the blocks we got
+                       int other_entry = link->get_train()->get_entry_to_block(link);
+                       if(other_entry==entry || link->traverse(entry)==link->traverse(other_entry))
+                       {
+                               good = last;
+                               good_sens = nsens;
+                       }
                        pending_block = link;
                        break;
                }
 
-               if(route && link->get_turnout_id())
+               if(link->get_turnout_id())
                {
-                       int path = route->get_turnout(link->get_turnout_id());
+                       const Block::Endpoint &ep = link->get_endpoints()[entry];
+                       const Endpoint &track_ep = ep.track->get_type().get_endpoints()[ep.track_ep];
+
+                       if(track_ep.paths&(track_ep.paths-1))
+                       {
+                               // We're facing the points - keep the blocks reserved so far
+                               good = last;
+                               good_sens = nsens;
+                       }
+
                        Turnout &turnout = trfc_mgr.get_control().get_turnout(link->get_turnout_id());
-                       if(path>=0 && path!=turnout.get_path())
+
+                       // Figure out what path we'd like to take on the turnout
+                       int path = -1;
+                       if(route)
+                               path = route->get_turnout(link->get_turnout_id());
+                       if(path<0)
+                               path = turnout.get_path();
+                       if(!((track_ep.paths>>path)&1))
                        {
+                               for(unsigned i=0; track_ep.paths>>i; ++i)
+                                       if((track_ep.paths>>i)&1)
+                                               path = i;
+                       }
+
+                       if(path!=turnout.get_path())
+                       {
+                               // The turnout is set to wrong path - switch and wait for it
                                link->reserve(0);
                                pending_block = link;
                                turnout.set_path(path);
@@ -344,7 +392,7 @@ unsigned Train::reserve_more()
                        }
                }
 
-               rsv_blocks.push_back(BlockRef(link, link->get_endpoint_by_link(*last->block)));
+               rsv_blocks.push_back(BlockRef(link, entry));
                last = &rsv_blocks.back();
                if(last->block->get_sensor_id())
                {
@@ -353,14 +401,13 @@ unsigned Train::reserve_more()
                }
        }
 
-       while(!rsv_blocks.empty() && !last->block->get_sensor_id())
+       // Unreserve blocks that were not good
+       while(!rsv_blocks.empty() && last!=good)
        {
                last->block->reserve(0);
                rsv_blocks.erase(--rsv_blocks.end());
                if(!rsv_blocks.empty())
                        last = &rsv_blocks.back();
-               else
-                       last = 0;
        }
 
        if(got_more)