If a train is not on the specified route, find a route to it

diff --git a/source/libmarklin/route.cpp b/source/libmarklin/route.cpp
index 13f4e3efff012e57622d2e387ac25e8613ffc6bc..2ff6962fe67551a17be8bde9052595b6933bf171 100644 (file)
--- a/source/libmarklin/route.cpp
+++ b/source/libmarklin/route.cpp
@@ -48,11 +48,20 @@ struct TrackMatch
 
        TrackMatch(const Track &t): track(t) { }
 
-       bool operator()(const Track &t) { return &t==&track; }
+       bool operator()(const Track &t) const { return &t==&track; }
+};
+
+struct TrackInRoute
+{
+       const Route &route;
+
+       TrackInRoute(const Route &r): route(r) { }
+
+       bool operator()(const Track &t) const { return route.get_tracks().count(&t); }
 };
 
 template<typename Pred>
-list<const Track *> dijkstra(const Track &from, unsigned ep, Pred goal)
+list<const Track *> dijkstra(const Track &from, unsigned ep, const Pred &goal)
 {
        // XXX Fails to find some routes - should use track+ep as key
        map<const Track *, Node> track_nodes;
@@ -104,6 +113,20 @@ list<const Track *> dijkstra(const Track &from, unsigned ep, Pred goal)
        return result;
 }
 
+unsigned count = 0;
+
+template<typename Pred>
+Route *create_route(const Track &from, unsigned ep, const Pred &goal)
+{
+       list<const Track *> tracks = dijkstra(from, ep, goal);
+
+       Route *route = new Route(from.get_layout(), format("-%d-", ++count));
+       for(list<const Track *>::iterator i=tracks.begin(); i!=tracks.end(); ++i)
+               route->add_track(**i);
+
+       return route;
+}
+
 }
 
 
@@ -295,15 +318,12 @@ void Route::track_removed(Track &t)
 
 Route *Route::find(const Track &from, unsigned ep, const Track &to)
 {
-       TrackMatch goal(to);
-       list<const Track *> tracks = dijkstra(from, ep, goal);
-
-       static unsigned n = 0;
-       Route *route = new Route(from.get_layout(), format("Route::find %d", ++n));
-       for(list<const Track *>::iterator i=tracks.begin(); i!=tracks.end(); ++i)
-               route->add_track(**i);
+       return create_route(from, ep, TrackMatch(to));
+}
 
-       return route;
+Route *Route::find(const Track &from, unsigned ep, const Route &to)
+{
+       return create_route(from, ep, TrackInRoute(to));
 }
 
 

diff --git a/source/libmarklin/route.h b/source/libmarklin/route.h
index 14ac94ce9073f0dcba51d788ea140e64c8a9d811..89788e3b62b477fc945229a96f4e1ede0ecbfc7f 100644 (file)
--- a/source/libmarklin/route.h
+++ b/source/libmarklin/route.h
@@ -54,6 +54,7 @@ private:
 
 public:
        static Route *find(const Track &, unsigned, const Track &);
+       static Route *find(const Track &, unsigned, const Route &);
 };
 
 } // namespace Marklin

diff --git a/source/libmarklin/train.cpp b/source/libmarklin/train.cpp
index 69e7212a8919937d327f027c7a314296b057b361..f745313233d57ce7e2b513ee2628c815c69dade2 100644 (file)
--- a/source/libmarklin/train.cpp
+++ b/source/libmarklin/train.cpp
@@ -31,6 +31,7 @@ Train::Train(Layout &l, const LocoType &t, unsigned a):
        reverse(false),
        functions(0),
        route(0),
+       next_route(0),
        end_of_route(false),
        status("Unplaced"),
        travel_dist(0),
@@ -137,8 +138,21 @@ void Train::set_route(const Route *r)
        }
 
        route = r;
+       next_route = 0;
        end_of_route = false;
 
+       if(route)
+       {
+               BlockRef &last = (rsv_blocks.empty() ? cur_blocks.back() : rsv_blocks.back());
+               BlockRef next = last.next();
+               const Block::Endpoint &ep = next.block->get_endpoints()[next.entry];
+               if(!route->get_tracks().count(ep.track))
+               {
+                       next_route = route;
+                       route = Route::find(*ep.track, ep.track_ep, *next_route);
+               }
+       }
+
        if(target_speed && reserve_more()<2)
                update_speed();
 
@@ -147,6 +161,14 @@ void Train::set_route(const Route *r)
 
 void Train::go_to(const Track &to)
 {
+       for(list<BlockRef>::const_iterator i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
+               if(i->block->get_tracks().count(const_cast<Track *>(&to)))
+               {
+                       set_speed(0);
+                       set_route(0);
+                       return;
+               }
+
        BlockRef *last = 0;
        if(rsv_blocks.empty())
                last = &cur_blocks.back();
@@ -157,15 +179,8 @@ void Train::go_to(const Track &to)
                                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];
+       BlockRef next = last->next();
+       const Block::Endpoint &ep = next.block->get_endpoints()[next.entry];
 
        set_route(Route::find(*ep.track, ep.track_ep, to));
 }
@@ -267,8 +282,6 @@ void Train::save(list<DataFile::Statement> &st) const
        for(unsigned i=0; i<=14; ++i)
                if(real_speed[i].weight)
                        st.push_back((DataFile::Statement("real_speed"), i, real_speed[i].speed, real_speed[i].weight));
-       if(route)
-               st.push_back((DataFile::Statement("route"), route->get_name()));
 
        if(!cur_blocks.empty())
        {
@@ -282,6 +295,9 @@ void Train::save(list<DataFile::Statement> &st) const
                for(list<BlockRef>::const_iterator i=blocks.begin(); i!=blocks.end(); ++i)
                        st.push_back((DataFile::Statement("block"), i->block->get_id()));
        }
+
+       if(route)
+               st.push_back((DataFile::Statement("route"), route->get_name()));
 }
 
 void Train::loco_speed_event(unsigned addr, unsigned speed, bool rev)
@@ -346,6 +362,21 @@ void Train::sensor_event(unsigned addr, bool state)
                        last_entry_time = Time::now();
                        pure_speed = true;
 
+                       // Check if we've reached the next route
+                       if(next_route)
+                       {
+                               const set<const Track *> &rtracks = next_route->get_tracks();
+                               for(list<BlockRef>::iterator j=rsv_blocks.begin(); j!=i; ++j)
+                                       if(rtracks.count(j->block->get_endpoints()[j->entry].track))
+                                       {
+                                               route = next_route;
+                                               next_route = 0;
+                                               // XXX Exceptions?
+                                               signal_route_changed.emit(route);
+                                               break;
+                                       }
+                       }
+
                        // Move blocks up to the next sensor to our current blocks
                        cur_blocks.splice(cur_blocks.end(), rsv_blocks, rsv_blocks.begin(), i);
 
@@ -419,8 +450,18 @@ unsigned Train::reserve_more()
        for(list<BlockRef>::const_iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
                if(i->block->get_sensor_id())
                        ++nsens;
+       
+       const Route *cur_route = 0;
+       if(route)
+       {
+               unsigned exit = last->block->traverse(last->entry);
+               Track *track = last->block->get_endpoints()[exit].track;
+               if(route->get_tracks().count(track))
+                       cur_route = route;
+               else if(next_route && next_route->get_tracks().count(track))
+                       cur_route = next_route;
+       }
 
-       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;
@@ -436,14 +477,23 @@ unsigned Train::reserve_more()
                if(entry<0)
                        throw LogicError("Block links are inconsistent!");
 
-               if(on_route && !route->get_tracks().count(link->get_endpoints()[entry].track))
+               const Block::Endpoint &entry_ep = link->get_endpoints()[entry];
+
+               if(cur_route)
                {
-                       // Keep the blocks if we arrived at the end of the route
-                       good = last;
-                       good_sens = nsens;
-                       end_of_route = true;
-                       break;
+                       if(cur_route!=next_route && next_route && next_route->get_tracks().count(entry_ep.track))
+                               cur_route = next_route;
+                       else if(!cur_route->get_tracks().count(entry_ep.track))
+                       {
+                               // Keep the blocks if we arrived at the end of the route
+                               good = last;
+                               good_sens = nsens;
+                               end_of_route = true;
+                               break;
+                       }
                }
+               else if(route && route->get_tracks().count(entry_ep.track))
+                       cur_route = route;
 
                if(!link->reserve(this))
                {
@@ -462,8 +512,7 @@ unsigned Train::reserve_more()
 
                if(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];
+                       const Endpoint &track_ep = entry_ep.track->get_type().get_endpoints()[entry_ep.track_ep];
 
                        // Keep the blocks reserved so far, as either us or the other train can diverge
                        good = last;
@@ -471,10 +520,10 @@ unsigned Train::reserve_more()
 
                        // 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(cur_route)
+                               path = cur_route->get_turnout(link->get_turnout_id());
                        if(path<0)
-                               path = ep.track->get_active_path();
+                               path = entry_ep.track->get_active_path();
                        if(!((track_ep.paths>>path)&1))
                        {
                                for(unsigned i=0; track_ep.paths>>i; ++i)
@@ -482,12 +531,12 @@ unsigned Train::reserve_more()
                                                path = i;
                        }
 
-                       if(path!=static_cast<int>(ep.track->get_active_path()))
+                       if(path!=static_cast<int>(entry_ep.track->get_active_path()))
                        {
                                // The turnout is set to wrong path - switch and wait for it
                                link->reserve(0);
                                pending_block = link;
-                               ep.track->set_active_path(path);
+                               entry_ep.track->set_active_path(path);
                                break;
                        }
                }
@@ -499,9 +548,6 @@ 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
@@ -652,6 +698,25 @@ void Train::reverse_blocks(list<BlockRef> &blocks) const
 }
 
 
+Train::BlockRef::BlockRef(Block *b, unsigned e):
+       block(b),
+       entry(e)
+{ }
+
+Train::BlockRef Train::BlockRef::next() const
+{
+       Block *blk = block->get_endpoints()[block->traverse(entry)].link;
+       if(!blk)
+               throw InvalidState("At end of line");
+
+       int ep = blk->get_endpoint_by_link(*block);
+       if(ep<0)
+               throw LogicError("Block links are inconsistent");
+
+       return BlockRef(blk, ep);
+}
+
+
 Train::RealSpeed::RealSpeed():
        speed(0),
        weight(0)

diff --git a/source/libmarklin/train.h b/source/libmarklin/train.h
index a89dc93c6addca56ff07ab23b0bd3b10cca6e404..b579d7f37deba40ba47d607baabf0e42817e7de7 100644 (file)
--- a/source/libmarklin/train.h
+++ b/source/libmarklin/train.h
@@ -50,7 +50,8 @@ private:
                Block *block;
                unsigned entry;
 
-               BlockRef(Block *s, unsigned e): block(s), entry(e) { }
+               BlockRef(Block *, unsigned);
+               BlockRef next() const;
        };
 
        struct RealSpeed
@@ -75,6 +76,7 @@ private:
        Msp::Time::TimeStamp stop_timeout;
        unsigned functions;
        const Route *route;
+       const Route *next_route;
        bool end_of_route;
        std::string status;