source/libr2c2/signal.cpp

   1 #include "blockiter.h"
   2 #include "driver.h"
   3 #include "layout.h"
   4 #include "signal.h"
   5 #include "signaltype.h"
   6 #include "trackiter.h"
   7 #include "tracktype.h"
   8 #include "train.h"
   9
  10 using namespace std;
  11 using namespace Msp;
  12
  13 namespace R2C2 {
  14
  15 Signal::Signal(Layout &l, const SignalType &t):
  16         layout(l),
  17         type(t),
  18         address(0),
  19         track(0),
  20         block(0),
  21         entry(0),
  22         train(0),
  23         check_allocated_blocks(false),
  24         passing(false)
  25 {
  26         layout.add_signal(*this);
  27
  28         layout.signal_block_reserved.connect(sigc::mem_fun(this, &Signal::block_reserved));
  29 }
  30
  31 Signal::~Signal()
  32 {
  33         layout.remove_signal(*this);
  34 }
  35
  36 void Signal::set_address(unsigned a)
  37 {
  38         address = a;
  39
  40         if(layout.has_driver() && address)
  41                 layout.get_driver().add_signal(address, type);
  42 }
  43
  44 void Signal::set_position(const Vector &p)
  45 {
  46         const set<Track *> &tracks = layout.get_tracks();
  47         float dist = -1;
  48         for(set<Track *>::const_iterator i=tracks.begin(); i!=tracks.end(); ++i)
  49                 if(!(*i)->get_type().is_turnout())
  50                 {
  51                         TrackPoint n = (*i)->get_nearest_point(p);
  52                         float d = distance(p, n.pos);
  53                         if(d<dist || dist<0)
  54                         {
  55                                 position = n.pos;
  56                                 direction = n.dir;
  57                                 track = *i;
  58                                 dist = d;
  59                         }
  60                 }
  61
  62         normalize_location();
  63 }
  64
  65 void Signal::normalize_location()
  66 {
  67         block = &track->get_block();
  68
  69         unsigned n_endpoints = track->get_type().get_endpoints().size();
  70         for(unsigned j=0; j<n_endpoints; ++j)
  71         {
  72                 float a = track->get_endpoint_direction(j)-direction;
  73                 while(a<-M_PI/2)
  74                         a += M_PI*2;
  75                 while(a>M_PI*3/2)
  76                         a -= M_PI*2;
  77                 if(a>=M_PI/2)
  78                 {
  79                         BlockIter biter = TrackIter(track, j).block_iter();
  80                         entry = biter.entry();
  81                 }
  82         }
  83 }
  84
  85 void Signal::set_direction(float d)
  86 {
  87         float a = direction-d;
  88         while(a>M_PI*3/2)
  89                 a -= M_PI*2;
  90         while(a<-M_PI/2)
  91                 a += M_PI*2;
  92         if(a>=M_PI/2)
  93         {
  94                 direction += M_PI;
  95                 if(direction>M_PI*2)
  96                         direction -= M_PI*2;
  97         }
  98
  99         normalize_location();
 100 }
 101
 102 void Signal::tick(const Time::TimeDelta &)
 103 {
 104         if(check_allocated_blocks)
 105         {
 106                 unsigned n_blocks = 0;
 107                 BlockIter iter(block, entry);
 108                 iter = iter.next();
 109                 while(iter && iter->get_train()==train)
 110                 {
 111                         if(iter->get_sensor_id())
 112                                 ++n_blocks;
 113                         iter=iter.next();
 114                 }
 115                 check_allocated_blocks = false;
 116
 117                 const list<SignalType::Indication> &indications = type.get_indications();
 118                 unsigned aspect = indications.back().aspect;
 119                 for(list<SignalType::Indication>::const_iterator i=indications.begin(); i!=indications.end(); ++i)
 120                         if(n_blocks>=i->free_blocks)
 121                         {
 122                                 aspect = i->aspect;
 123                                 break;
 124                         }
 125
 126                 layout.get_driver().set_signal(address, aspect);
 127         }
 128 }
 129
 130 void Signal::block_reserved(const Block &b, Train *t)
 131 {
 132         if(&b==block)
 133         {
 134                 if(t)
 135                 {
 136                         int train_entry = t->get_entry_to_block(*block);
 137                         if(train_entry>=0 && static_cast<unsigned>(train_entry)==entry)
 138                         {
 139                                 if(train_conn)
 140                                         train_conn.disconnect();
 141                                 train = t;
 142                                 passing = false;
 143                                 train_conn = train->signal_advanced.connect(sigc::mem_fun(this, &Signal::train_advanced));
 144                                 check_allocated_blocks = true;
 145                         }
 146                 }
 147                 else
 148                 {
 149                         layout.get_driver().set_signal(address, type.get_indications().back().aspect);
 150                         reset();
 151                 }
 152         }
 153         else if(train && t==train)
 154                 check_allocated_blocks = true;
 155 }
 156
 157 void Signal::train_advanced(Block &b)
 158 {
 159         if(&b==block)
 160                 passing = true;
 161         else if(passing && b.get_sensor_id())
 162         {
 163                 layout.get_driver().set_signal(address, type.get_indications().back().aspect);
 164                 reset();
 165         }
 166 }
 167
 168 void Signal::reset()
 169 {
 170         train = 0;
 171         if(train_conn)
 172                 train_conn.disconnect();
 173         check_allocated_blocks = false;
 174 }
 175
 176 void Signal::save(list<DataFile::Statement> &st) const
 177 {
 178         st.push_back((DataFile::Statement("position"), position.x, position.y, position.z));
 179         st.push_back((DataFile::Statement("direction"), direction));
 180         if(address)
 181                 st.push_back((DataFile::Statement("address"), address));
 182 }
 183
 184
 185 Signal::Loader::Loader(Signal &s):
 186         DataFile::ObjectLoader<Signal>(s)
 187 {
 188         add("address",   &Loader::address);
 189         add("direction", &Loader::direction);
 190         add("position",  &Loader::position);
 191 }
 192
 193 void Signal::Loader::address(unsigned a)
 194 {
 195         obj.set_address(a);
 196 }
 197
 198 void Signal::Loader::direction(float d)
 199 {
 200         obj.set_direction(d);
 201 }
 202
 203 void Signal::Loader::position(float x, float y, float z)
 204 {
 205         obj.set_position(Vector(x, y, z));
 206 }
 207
 208 } // namespace R2C2