]> git.tdb.fi Git - r2c2.git/blob - source/libr2c2/signal.cpp
Add basic support for signals
[r2c2.git] / 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