Add a pathfinder function to Route

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

diff --git a/source/libmarklin/train.cpp b/source/libmarklin/train.cpp
index bb36bcfc378518ab48c7bea2960dcd57892b7677..69e7212a8919937d327f027c7a314296b057b361 100644 (file)
--- a/source/libmarklin/train.cpp
+++ b/source/libmarklin/train.cpp
@@ -29,7 +29,9 @@ Train::Train(Layout &l, const LocoType &t, unsigned a):
        target_speed(0),
        current_speed(0),
        reverse(false),
+       functions(0),
        route(0),
+       end_of_route(false),
        status("Unplaced"),
        travel_dist(0),
        travel_speed(0),
@@ -124,10 +126,50 @@ bool Train::get_function(unsigned func) const
 
 void Train::set_route(const Route *r)
 {
+       if(!rsv_blocks.empty())
+       {
+               for(list<BlockRef>::iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
+                       if(i->block->get_sensor_id())
+                       {
+                               release_blocks(rsv_blocks, ++i, rsv_blocks.end());
+                               break;
+                       }
+       }
+
        route = r;
+       end_of_route = false;
+
+       if(target_speed && reserve_more()<2)
+               update_speed();
+
        signal_route_changed.emit(route);
 }
 
+void Train::go_to(const Track &to)
+{
+       BlockRef *last = 0;
+       if(rsv_blocks.empty())
+               last = &cur_blocks.back();
+       else
+       {
+               for(list<BlockRef>::iterator i=rsv_blocks.begin(); (i!=rsv_blocks.end() && !last); ++i)
+                       if(i->block->get_sensor_id())
+                               last = &*i;
+       }
+
+       Block *next = last->block->get_endpoints()[last->block->traverse(last->entry)].link;
+       if(!next)
+               throw InvalidState("At end of line");
+
+       int entry = next->get_endpoint_by_link(*last->block);
+       if(entry<0)
+               throw LogicError("Block links are inconsistent");
+
+       const Block::Endpoint &ep = next->get_endpoints()[entry];
+
+       set_route(Route::find(*ep.track, ep.track_ep, to));
+}
+
 void Train::place(Block &block, unsigned entry)
 {
        if(target_speed)
@@ -184,6 +226,7 @@ void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt)
        if(stop_timeout && t>=stop_timeout)
        {
                release_blocks(rsv_blocks);
+               end_of_route = false;
                stop_timeout = Time::TimeStamp();
        }
 
@@ -307,8 +350,17 @@ void Train::sensor_event(unsigned addr, bool state)
                        cur_blocks.splice(cur_blocks.end(), rsv_blocks, rsv_blocks.begin(), i);
 
                        // Try to get more blocks if we're moving
-                       if(target_speed && reserve_more()<2)
-                               update_speed();
+                       if(target_speed)
+                       {
+                               unsigned nsens = reserve_more();
+                               if(!nsens && end_of_route)
+                               {
+                                       set_speed(0);
+                                       set_route(0);
+                               }
+                               else if(nsens<2)
+                                       update_speed();
+                       }
                }
        }
        else
@@ -335,8 +387,13 @@ void Train::sensor_event(unsigned addr, bool state)
 
 void Train::turnout_event(unsigned addr, bool)
 {
-       if(pending_block && addr==pending_block->get_turnout_id())
-               reserve_more();
+       if(pending_block)
+       {
+               unsigned pending_addr = pending_block->get_turnout_id();
+               bool double_addr = (*pending_block->get_tracks().begin())->get_type().is_double_address();
+               if(addr==pending_addr || (double_addr && addr==pending_addr+1))
+                       reserve_more();
+       }
 }
 
 void Train::block_reserved(const Block &block, const Train *train)
@@ -357,12 +414,13 @@ unsigned Train::reserve_more()
 
        pending_block = 0;
 
-       // See how many blocks we already have
+       // See how many sensor 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 on_route = (route && route->get_tracks().count(last->block->get_endpoints()[last->entry].track));
        bool got_more = false;
        BlockRef *good = last;
        unsigned good_sens = nsens;
@@ -377,6 +435,16 @@ unsigned Train::reserve_more()
                int entry = link->get_endpoint_by_link(*last->block);
                if(entry<0)
                        throw LogicError("Block links are inconsistent!");
+
+               if(on_route && !route->get_tracks().count(link->get_endpoints()[entry].track))
+               {
+                       // Keep the blocks if we arrived at the end of the route
+                       good = last;
+                       good_sens = nsens;
+                       end_of_route = true;
+                       break;
+               }
+
                if(!link->reserve(this))
                {
                        // If we found another train and it's not headed straight for us, we can keep the blocks we got
@@ -431,6 +499,9 @@ unsigned Train::reserve_more()
                        ++nsens;
                        got_more = true;
                }
+
+               if(route && !on_route)
+                       on_route = route->get_tracks().count(link->get_endpoints()[entry].track);
        }
 
        // Unreserve blocks that were not good
@@ -445,7 +516,7 @@ unsigned Train::reserve_more()
        if(got_more)
                update_speed();
 
-       return nsens;
+       return good_sens;
 }
 
 void Train::update_speed()