Simulate commands if no serial device is set to make turnouts work

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

diff --git a/source/libmarklin/train.cpp b/source/libmarklin/train.cpp
index e904ebfc5f94b8152890c47c68a9b77cc4ebf189..462ff1dff730318b18f35d049a540ccc16438d19 100644 (file)
--- a/source/libmarklin/train.cpp
+++ b/source/libmarklin/train.cpp
@@ -11,6 +11,7 @@ Distributed under the GPL
 #include <msp/time/utils.h>
 #include "control.h"
 #include "except.h"
+#include "route.h"
 #include "tracktype.h"
 #include "trafficmanager.h"
 #include "train.h"
@@ -23,7 +24,9 @@ namespace Marklin {
 Train::Train(TrafficManager &tm, Locomotive &l):
        trfc_mgr(tm),
        loco(l),
+       pending_block(0),
        target_speed(0),
+       route(0),
        status("Unplaced"),
        travel_dist(0),
        travel_speed(0),
@@ -80,6 +83,12 @@ void Train::set_reverse(bool rev)
        loco.set_reverse(rev);
 }
 
+void Train::set_route(const Route *r)
+{
+       route = r;
+       signal_route_changed.emit(route);
+}
+
 void Train::place(Block *block, unsigned entry)
 {
        for(list<BlockRef>::iterator i=rsv_blocks.begin(); i!=rsv_blocks.end();)
@@ -133,10 +142,7 @@ bool Train::free_block(Block *block)
 void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt)
 {
        if(try_reserve && t>try_reserve)
-       {
                reserve_more();
-               update_speed();
-       }
 
        if(cur_track)
        {
@@ -182,7 +188,6 @@ void Train::locomotive_reverse_changed(bool)
        for(list<BlockRef>::iterator i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
                i->entry = i->block->traverse(i->entry);
        reserve_more();
-       update_speed();
 
        if(cur_track)
        {
@@ -233,11 +238,8 @@ void Train::sensor_event(bool state, Sensor *sensor)
                        if(i->block->get_sensor_id()==addr)
                                set_position(i->block->get_endpoints()[i->entry]);
 
-               if(target_speed)
-               {
-                       reserve_more();
+               if(target_speed && reserve_more()<2)
                        update_speed();
-               }
        }
        else
        {
@@ -254,7 +256,7 @@ void Train::sensor_event(bool state, Sensor *sensor)
                                break;
                        }
 
-               if(target_speed)
+               if(target_speed && pending_block && addr==pending_block->get_sensor_id())
                        reserve_more();
        }
 }
@@ -292,9 +294,11 @@ void Train::turnout_path_changed(unsigned, Turnout *turnout)
                                i = rsv_blocks.erase(i);
                        }
                        reserve_more();
-                       update_speed();
                        return;
                }
+
+       if(pending_block && tid==pending_block->get_turnout_id())
+               reserve_more();
 }
 
 unsigned Train::reserve_more()
@@ -307,36 +311,50 @@ unsigned Train::reserve_more()
        if(!last)
                return 0;
 
+       pending_block = 0;
+
        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 result = false;
-       while(nsens<2)
+       bool got_more = false;
+       while(nsens<3)
        {
                int exit = last->block->traverse(last->entry);
-               if(exit>=0) 
+               if(exit<0)
+                       break;
+
+               Block *link = last->block->get_link(exit);
+               if(!link || !link->reserve(this))
                {
-                       Block *link = last->block->get_link(exit);
-                       if(link && link->reserve(this))
+                       pending_block = link;
+                       break;
+               }
+
+               if(route && link->get_turnout_id())
+               {
+                       int path = route->get_turnout(link->get_turnout_id());
+                       Turnout &turnout = trfc_mgr.get_control().get_turnout(link->get_turnout_id());
+                       if(path>=0 && path!=turnout.get_path())
                        {
-                               rsv_blocks.push_back(BlockRef(link, link->get_endpoint_by_link(*last->block)));
-                               last = &rsv_blocks.back();
-                               if(last->block->get_sensor_id())
-                               {
-                                       ++nsens;
-                                       result = true;
-                               }
-                       }
-                       else
+                               link->reserve(0);
+                               pending_block = link;
+                               turnout.set_path(path);
                                break;
+                       }
+               }
+
+               rsv_blocks.push_back(BlockRef(link, link->get_endpoint_by_link(*last->block)));
+               last = &rsv_blocks.back();
+               if(last->block->get_sensor_id())
+               {
+                       ++nsens;
+                       got_more = true;
                }
-               else
-                       break;
        }
 
-       while(last && !last->block->get_sensor_id())
+       while(!rsv_blocks.empty() && !last->block->get_sensor_id())
        {
                last->block->reserve(0);
                rsv_blocks.erase(--rsv_blocks.end());
@@ -346,6 +364,9 @@ unsigned Train::reserve_more()
                        last = 0;
        }
 
+       if(got_more)
+               update_speed();
+
        return nsens;
 }
 
@@ -422,14 +443,23 @@ unsigned Train::find_speed(float real) const
 {
        if(real<=real_speed[0].speed)
                return 0;
-       if(real>=real_speed[14].speed)
-               return 14;
 
        unsigned low = 0;
        unsigned high = 0;
-       for(high=0; real_speed[high].speed<real; ++high)
-               if(real_speed[high].weight)
-                       low = high;
+       for(unsigned i=0; (!high && i<=14); ++i)
+               if(real_speed[i].weight)
+               {
+                       if(real_speed[i].speed<real)
+                               low = i;
+                       else
+                               high = i;
+               }
+       if(!high)
+       {
+               if(!low)
+                       return 0;
+               return min(static_cast<unsigned>(low*real/real_speed[low].speed), 14U);
+       }
 
        float f = (real-real_speed[low].speed)/(real_speed[high].speed-real_speed[low].speed);
        return static_cast<unsigned>(low*(1-f)+high*f+0.5);