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