source/libmarklin/train.cpp

   1 #include <cmath>
   2 #include <msp/strings/formatter.h>
   3 #include <msp/time/units.h>
   4 #include <msp/time/utils.h>
   5 #include "control.h"
   6 #include "trafficmanager.h"
   7 #include "train.h"
   8
   9 using namespace Msp;
  10
  11 #include <iostream>
  12 using namespace std;
  13
  14 namespace Marklin {
  15
  16 Train::Train(TrafficManager &tm, Locomotive &l):
  17         trfc_mgr(tm),
  18         loco(l),
  19         target_speed(0),
  20         status("Unplaced"),
  21         travel_dist(0),
  22         real_speed(0)
  23 {
  24         trfc_mgr.add_train(this);
  25
  26         const map<unsigned, Sensor *> &sensors=trfc_mgr.get_control().get_sensors();
  27         for(map<unsigned, Sensor *>::const_iterator i=sensors.begin(); i!=sensors.end(); ++i)
  28                 i->second->signal_state_changed.connect(sigc::bind(sigc::mem_fun(this, &Train::sensor_event), i->second));
  29 }
  30
  31 void Train::set_name(const string &n)
  32 {
  33         name=n;
  34
  35         signal_name_changed.emit(name);
  36 }
  37
  38 void Train::set_speed(unsigned speed)
  39 {
  40         unsigned old_speed=target_speed;
  41
  42         target_speed=speed;
  43         if(!target_speed)
  44         {
  45                 // XXX We might roll onto a new sensor and get confused - should delay freeing blocks a bit
  46                 for(list<BlockRef>::iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
  47                         i->block->reserve(0);
  48                 rsv_blocks.clear();
  49                 try_reserve=Time::TimeStamp();
  50         }
  51         else if(rsv_blocks.empty() && !reserve_more())
  52                 return;
  53
  54         loco.set_speed(speed);
  55         if(!old_speed && target_speed)
  56                 set_status("Traveling --- kmh");
  57         else if(old_speed && !target_speed)
  58                 set_status("Stopped");
  59 }
  60
  61 void Train::place(Block *block, unsigned entry)
  62 {
  63         for(list<BlockRef>::iterator i=rsv_blocks.begin(); i!=rsv_blocks.end();)
  64         {
  65                 i->block->reserve(0);
  66                 i=rsv_blocks.erase(i);
  67         }
  68
  69         for(list<BlockRef>::iterator i=cur_blocks.begin(); i!=cur_blocks.end();)
  70         {
  71                 i->block->reserve(0);
  72                 i=cur_blocks.erase(i);
  73         }
  74
  75         if(!block->reserve(this))
  76         {
  77                 set_status("Unplaced");
  78                 return;
  79         }
  80
  81         cur_blocks.push_back(BlockRef(block, entry));
  82
  83         set_status("Stopped");
  84 }
  85
  86 bool Train::free_block(Block *block)
  87 {
  88         for(list<BlockRef>::iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
  89                 if(i->block==block)
  90                 {
  91                         while(i!=rsv_blocks.end())
  92                         {
  93                                 i->block->reserve(0);
  94                                 i=rsv_blocks.erase(i);
  95                         }
  96                         return true;
  97                 }
  98
  99         return false;
 100 }
 101
 102 void Train::tick(const Time::TimeStamp &t)
 103 {
 104         if(try_reserve && t>try_reserve)
 105         {
 106                 if(reserve_more() || !rsv_blocks.empty())
 107                 {
 108                         loco.set_speed(target_speed);
 109                         set_status("Traveling --- kmh");
 110                         try_reserve=Time::TimeStamp();
 111                 }
 112                 else
 113                         try_reserve=t+2*Time::sec;
 114         }
 115 }
 116
 117 void Train::sensor_event(bool state, Sensor *sensor)
 118 {
 119         unsigned addr=sensor->get_address();
 120
 121         if(state)
 122         {
 123                 list<BlockRef>::iterator i;
 124                 for(i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
 125                         if(i->block->get_sensor_id() && i->block->get_sensor_id()!=addr)
 126                                 break;
 127
 128                 if(i!=rsv_blocks.begin())
 129                 {
 130                         real_speed=static_cast<int>(round(travel_dist/((Time::now()-last_entry_time)/Time::sec)*87*3.6/5))*5;
 131                         set_status(format("Traveling %3d kmh", real_speed));
 132
 133                         travel_dist=0;
 134                         float block_len;
 135                         for(list<BlockRef>::iterator j=rsv_blocks.begin(); j!=i; ++j)
 136                         {
 137                                 j->block->traverse(j->entry, &block_len);
 138                                 cout<<"Advancing: block "<<j->block<<" (sensor "<<j->block->get_sensor_id()<<") length "<<block_len<<'\n';
 139                                 travel_dist+=block_len;
 140                         }
 141                         last_entry_time=Time::now();
 142
 143                         cur_blocks.splice(cur_blocks.end(), rsv_blocks, rsv_blocks.begin(), i);
 144                         cout<<"Train "<<name<<" advanced, "<<cur_blocks.size()<<" cur_blocks, "<<rsv_blocks.size()<<" rsv_blocks\n";
 145                 }
 146
 147                 if(target_speed && !reserve_more() && rsv_blocks.empty())
 148                 {
 149                         loco.set_speed(0);
 150                         try_reserve=Time::now()+2*Time::sec;
 151                         set_status("Blocked");
 152                 }
 153         }
 154         else
 155         {
 156                 cout<<"Train "<<name<<" finding blocks to free\n";
 157                 list<BlockRef>::iterator i;
 158                 for(i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
 159                 {
 160                         if(i->block->get_sensor_id()==addr)
 161                         {
 162                                 ++i;
 163                                 for(list<BlockRef>::iterator j=cur_blocks.begin(); j!=i; ++j)
 164                                         j->block->reserve(0);
 165                                 cout<<"  "<<distance(cur_blocks.begin(), i)<<" blocks freed, ";
 166                                 cur_blocks.erase(cur_blocks.begin(), i);
 167                                 cout<<cur_blocks.size()<<" cur_blocks\n";
 168                         }
 169                 }
 170
 171                 if(target_speed)
 172                         reserve_more();
 173         }
 174 }
 175
 176 bool Train::reserve_more()
 177 {
 178         BlockRef *last=0;
 179         if(!rsv_blocks.empty())
 180                 last=&rsv_blocks.back();
 181         else if(!cur_blocks.empty())
 182                 last=&cur_blocks.back();
 183         if(!last)
 184                 return false;
 185
 186         cout<<"Train "<<name<<" reserving more blocks\n";
 187
 188         bool result=false;
 189         unsigned size=rsv_blocks.size();
 190         while(size<3)
 191         {
 192                 int exit=last->block->traverse(last->entry);
 193                 if(exit>=0)
 194                 {
 195                         Block *link=last->block->get_link(exit);
 196                         if(link && link->reserve(this))
 197                         {
 198                                 rsv_blocks.push_back(BlockRef(link, link->get_endpoint_by_link(*last->block)));
 199                                 last=&rsv_blocks.back();
 200                                 ++size;
 201                                 result=true;
 202                         }
 203                         else
 204                                 break;
 205                 }
 206                 else
 207                         break;
 208         }
 209
 210         cout<<"  "<<rsv_blocks.size()<<" rsv_blocks\n";
 211
 212         return result;
 213 }
 214
 215 void Train::set_status(const string &s)
 216 {
 217         status=s;
 218         signal_status_changed.emit(s);
 219 }
 220
 221 } // namespace Marklin