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