--- /dev/null
+#include "blockiter.h"
+#include "driver.h"
+#include "layout.h"
+#include "signal.h"
+#include "signaltype.h"
+#include "trackiter.h"
+#include "tracktype.h"
+#include "train.h"
+
+using namespace std;
+using namespace Msp;
+
+#include <msp/io/print.h>
+
+namespace R2C2 {
+
+Signal::Signal(Layout &l, const SignalType &t):
+ layout(l),
+ type(t),
+ address(0),
+ track(0),
+ block(0),
+ entry(0),
+ train(0),
+ check_train_direction(false),
+ check_allocated_blocks(false),
+ passing(false)
+{
+ layout.add_signal(*this);
+
+ layout.signal_block_reserved.connect(sigc::mem_fun(this, &Signal::block_reserved));
+}
+
+Signal::~Signal()
+{
+ layout.remove_signal(*this);
+}
+
+void Signal::set_address(unsigned a)
+{
+ address = a;
+
+ if(layout.has_driver() && address)
+ layout.get_driver().add_signal(address, type);
+}
+
+void Signal::set_position(const Vector &p)
+{
+ const set<Track *> &tracks = layout.get_tracks();
+ float dist = -1;
+ for(set<Track *>::const_iterator i=tracks.begin(); i!=tracks.end(); ++i)
+ if(!(*i)->get_type().is_turnout())
+ {
+ TrackPoint n = (*i)->get_nearest_point(p);
+ float d = distance(p, n.pos);
+ if(d<dist || dist<0)
+ {
+ position = n.pos;
+ direction = n.dir;
+ track = *i;
+ dist = d;
+ }
+ }
+
+ normalize_location();
+}
+
+void Signal::normalize_location()
+{
+ block = &track->get_block();
+
+ unsigned n_endpoints = track->get_type().get_endpoints().size();
+ for(unsigned j=0; j<n_endpoints; ++j)
+ {
+ float a = track->get_endpoint_direction(j)-direction;
+ while(a<-M_PI/2)
+ a += M_PI*2;
+ while(a>M_PI*3/2)
+ a -= M_PI*2;
+ if(a>=M_PI/2)
+ {
+ BlockIter biter = TrackIter(track, j).block_iter();
+ entry = biter.entry();
+ }
+ }
+}
+
+void Signal::set_direction(float d)
+{
+ float a = direction-d;
+ while(a>M_PI*3/2)
+ a -= M_PI*2;
+ while(a<-M_PI/2)
+ a += M_PI*2;
+ if(a>=M_PI/2)
+ {
+ direction += M_PI;
+ if(direction>M_PI*2)
+ direction -= M_PI*2;
+ }
+
+ normalize_location();
+}
+
+void Signal::tick(const Time::TimeDelta &)
+{
+ if(check_train_direction)
+ {
+ int train_entry = train->get_entry_to_block(*block);
+ if(train_entry>=0 && static_cast<unsigned>(train_entry)==entry)
+ {
+ IO::print("Train passing in the proper direction\n");
+ if(train_conn)
+ train_conn.disconnect();
+ train_conn = train->signal_advanced.connect(sigc::mem_fun(this, &Signal::train_advanced));
+ }
+ else
+ IO::print("Train passing in the wrong direction (entry=%d, train_entry=%d)\n", entry, train_entry);
+ check_train_direction = false;
+ check_allocated_blocks = true;
+ }
+
+ if(check_allocated_blocks)
+ {
+ unsigned n_blocks = 0;
+ BlockIter iter(block, entry);
+ iter = iter.next();
+ while(iter && iter->get_train()==train)
+ {
+ if(iter->get_sensor_id())
+ ++n_blocks;
+ iter=iter.next();
+ }
+ IO::print("%d sensor blocks allocated\n", n_blocks);
+ check_allocated_blocks = false;
+
+ const list<SignalType::Indication> &indications = type.get_indications();
+ unsigned aspect = indications.back().aspect;
+ for(list<SignalType::Indication>::const_iterator i=indications.begin(); i!=indications.end(); ++i)
+ if(n_blocks>=i->free_blocks)
+ {
+ aspect = i->aspect;
+ break;
+ }
+
+ IO::print("Setting signal %d to aspect %d\n", address, aspect);
+ layout.get_driver().set_signal(address, aspect);
+ }
+}
+
+void Signal::block_reserved(const Block &b, Train *t)
+{
+ if(&b==block)
+ {
+ if(t)
+ {
+ train = t;
+ passing = false;
+ check_train_direction = true;
+ }
+ else
+ {
+ layout.get_driver().set_signal(address, type.get_indications().back().aspect);
+ reset();
+ }
+ }
+ else if(train && t==train)
+ check_allocated_blocks = true;
+}
+
+void Signal::train_advanced(Block &b)
+{
+ if(&b==block)
+ {
+ IO::print("Train is passing the signal now\n");
+ passing = true;
+ }
+ else if(passing && b.get_sensor_id())
+ {
+ IO::print("Train has passed the signal\n");
+ layout.get_driver().set_signal(address, type.get_indications().back().aspect);
+ reset();
+ }
+}
+
+void Signal::reset()
+{
+ train = 0;
+ if(train_conn)
+ train_conn.disconnect();
+ check_train_direction = false;
+ check_allocated_blocks = false;
+}
+
+void Signal::save(list<DataFile::Statement> &st) const
+{
+ st.push_back((DataFile::Statement("position"), position.x, position.y, position.z));
+ st.push_back((DataFile::Statement("direction"), direction));
+ if(address)
+ st.push_back((DataFile::Statement("address"), address));
+}
+
+
+Signal::Loader::Loader(Signal &s):
+ DataFile::ObjectLoader<Signal>(s)
+{
+ add("address", &Loader::address);
+ add("direction", &Loader::direction);
+ add("position", &Loader::position);
+}
+
+void Signal::Loader::address(unsigned a)
+{
+ obj.set_address(a);
+}
+
+void Signal::Loader::direction(float d)
+{
+ obj.set_direction(d);
+}
+
+void Signal::Loader::position(float x, float y, float z)
+{
+ obj.set_position(Vector(x, y, z));
+}
+
+} // namespace R2C2
projects / r2c2.git / blobdiff