Convert engineer to use mspgbase instead of SDL

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

diff --git a/source/libmarklin/train.cpp b/source/libmarklin/train.cpp
index c2e278cba2870c2543090b906fffdb505ff10f07..c42448176e814f0032e2810732aadf5a3f216bc5 100644 (file)
--- a/source/libmarklin/train.cpp
+++ b/source/libmarklin/train.cpp
@@ -1,7 +1,11 @@
+#include <msp/time/units.h>
+#include <msp/time/utils.h>
 #include "control.h"
 #include "trafficmanager.h"
 #include "train.h"
 
+using namespace Msp;
+
 #include <iostream>
 using namespace std;
 
@@ -29,7 +33,13 @@ void Train::set_name(const string &n)
 void Train::set_speed(unsigned speed)
 {
        target_speed=speed;
-       if(rsv_blocks.empty() && !reserve_more())
+       if(!target_speed)
+       {
+               for(list<BlockRef>::iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
+                       i->block->reserve(0);
+               rsv_blocks.clear();
+       }
+       else if(rsv_blocks.empty() && !reserve_more())
                return;
        loco.set_speed(speed);
 }
@@ -70,6 +80,20 @@ bool Train::free_block(Block *block)
        return false;
 }
 
+void Train::tick(const Time::TimeStamp &t)
+{
+       if(try_reserve && t>try_reserve)
+       {
+               if(reserve_more())
+               {
+                       loco.set_speed(target_speed);
+                       try_reserve=Time::TimeStamp();
+               }
+               else
+                       try_reserve=t+2*Time::sec;
+       }
+}
+
 void Train::sensor_event(bool state, Sensor *sensor)
 {
        if(!loco.get_speed())
@@ -85,27 +109,29 @@ void Train::sensor_event(bool state, Sensor *sensor)
                                break;
                cur_blocks.splice(cur_blocks.end(), rsv_blocks, rsv_blocks.begin(), i);
 
-               cout<<"Train advanced, "<<cur_blocks.size()<<" cur_blocks, "<<rsv_blocks.size()<<" rsv_blocks\n";
+               cout<<"Train "<<this<<" advanced, "<<cur_blocks.size()<<" cur_blocks, "<<rsv_blocks.size()<<" rsv_blocks\n";
 
-               reserve_more();
-               if(rsv_blocks.empty())
+               if(rsv_blocks.empty() && !reserve_more())
+               {
                        loco.set_speed(0);
+                       try_reserve=Time::now()+2*Time::sec;
+               }
        }
        else
        {
-               cout<<"Finding blocks to free\n";
+               cout<<"Train "<<this<<" finding blocks to free\n";
                list<BlockRef>::iterator i;
                for(i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
-                       if(i->block->get_sensor_id()==addr)
-                               break;
-               if(i!=cur_blocks.end())
                {
-                       cout<<"found\n";
-                       ++i;
-                       for(list<BlockRef>::iterator j=cur_blocks.begin(); j!=i; ++j)
-                               j->block->reserve(0);
-                       cur_blocks.erase(cur_blocks.begin(), i);
-                       cout<<cur_blocks.size()<<" cur_blocks\n";
+                       if(i->block->get_sensor_id()==addr)
+                       {
+                               ++i;
+                               for(list<BlockRef>::iterator j=cur_blocks.begin(); j!=i; ++j)
+                                       j->block->reserve(0);
+                               cout<<"  "<<distance(cur_blocks.begin(),i )<<" blocks freed, ";
+                               cur_blocks.erase(cur_blocks.begin(), i);
+                               cout<<cur_blocks.size()<<" cur_blocks\n";
+                       }
                }
 
                reserve_more();
@@ -122,26 +148,32 @@ bool Train::reserve_more()
        if(!last)
                return false;
 
+       cout<<"Train "<<this<<" reserving more blocks\n";
+
        bool result=false;
        unsigned size=rsv_blocks.size();
-       while(size<3)
+       while(size<2)
        {
                int exit=last->block->traverse(last->entry);
                if(exit>=0) 
                {
                        Block *link=last->block->get_link(exit);
-                       if(link)
+                       if(link && link->reserve(this))
                        {
                                rsv_blocks.push_back(BlockRef(link, link->get_endpoint_by_link(*last->block)));
                                last=&rsv_blocks.back();
                                ++size;
                                result=true;
                        }
+                       else
+                               break;
                }
                else
                        break;
        }
 
+       cout<<"  "<<rsv_blocks.size()<<" rsv_blocks\n";
+
        return result;
 }