]> git.tdb.fi Git - r2c2.git/blob - source/libr2c2/blockallocator.cpp
215d746a25e3442bbe0cad476028545d53569789
[r2c2.git] / source / libr2c2 / blockallocator.cpp
1 #include <msp/core/maputils.h>
2 #include <msp/core/raii.h>
3 #include "blockallocator.h"
4 #include "block.h"
5 #include "catalogue.h"
6 #include "driver.h"
7 #include "layout.h"
8 #include "trackcircuit.h"
9 #include "trackiter.h"
10 #include "train.h"
11 #include "vehicle.h"
12
13 using namespace std;
14 using namespace Msp;
15
16 namespace R2C2 {
17
18 struct BlockAllocator::BlockMatch
19 {
20         const Block &block;
21
22         BlockMatch(const Block &b): block(b) { }
23
24         bool operator()(const BlockIter &bi) const { return &*bi==&block; }
25 };
26
27
28 BlockAllocator::BlockAllocator(Train &t):
29         train(t),
30         cur_blocks_end(blocks.end()),
31         next_sensor(0),
32         pending_block(0),
33         stop_at_block(0),
34         reserving(false),
35         advancing(false)
36 {
37         Layout &layout = train.get_layout();
38         layout.signal_block_reserved.connect(sigc::mem_fun(this, &BlockAllocator::block_reserved));
39         layout.signal_sensor_state_changed.connect(sigc::mem_fun(this, &BlockAllocator::sensor_state_changed));
40
41         const set<Track *> &tracks = layout.get_all<Track>();
42         for(set<Track *>::const_iterator i=tracks.begin(); i!=tracks.end(); ++i)
43                 if((*i)->get_turnout_id())
44                 {
45                         (*i)->signal_path_changing.connect(sigc::hide(sigc::bind(sigc::mem_fun(this, &BlockAllocator::turnout_path_changing), sigc::ref(**i))));
46                         (*i)->signal_path_changed.connect(sigc::hide(sigc::bind(sigc::mem_fun(this, &BlockAllocator::turnout_path_changed), sigc::ref(**i))));
47                 }
48 }
49
50 void BlockAllocator::set_active(bool a)
51 {
52         active = a;
53         if(active)
54                 reserve_more();
55         else
56         {
57                 release_blocks_end(cur_blocks_end);
58                 pending_block = 0;
59         }
60 }
61
62 void BlockAllocator::start_from(const BlockIter &block)
63 {
64         if(!block)
65                 throw invalid_argument("BlockAllocator::start_from");
66
67         clear();
68         reserve_block(block);
69 }
70
71 void BlockAllocator::rewind_to(const Block &block)
72 {
73         if(!active)
74                 return;
75
76         BlockList::iterator i = find_if(cur_blocks_end, blocks.end(), BlockMatch(block));
77         if(i!=blocks.end())
78         {
79                 release_blocks_end(i);
80                 reserve_more();
81         }
82 }
83
84 void BlockAllocator::clear()
85 {
86         release_blocks_begin(blocks.end());
87         active = false;
88         next_sensor = 0;
89         pending_block = 0;
90         stop_at_block = 0;
91 }
92
93 void BlockAllocator::stop_at(const Block *block)
94 {
95         stop_at_block = block;
96         if(active && !block)
97                 reserve_more();
98 }
99
100 const BlockIter &BlockAllocator::first() const
101 {
102         if(blocks.empty())
103                 throw logic_error("no blocks");
104         return blocks.front();
105 }
106
107 const BlockIter &BlockAllocator::last() const
108 {
109         if(blocks.empty())
110                 throw logic_error("no blocks");
111         BlockList::const_iterator i = --blocks.end();
112         if(i->block()==pending_block)
113                 --i;
114         return *i;
115 }
116
117 const BlockIter &BlockAllocator::last_current() const
118 {
119         if(blocks.empty())
120                 throw logic_error("no blocks");
121         if(cur_blocks_end==blocks.begin())
122                 throw logic_error("internal error (no current blocks)");
123         BlockList::const_iterator i = cur_blocks_end;
124         return *--i;
125 }
126
127 const BlockIter &BlockAllocator::iter_for(const Block &block) const
128 {
129         BlockList::const_iterator i = find_if(blocks.begin(), blocks.end(), BlockMatch(block));
130         if(i==blocks.end())
131                 throw key_error(&block);
132         return *i;
133 }
134
135 bool BlockAllocator::has_block(const Block &block) const
136 {
137         return find_if(blocks.begin(), blocks.end(), BlockMatch(block))!=blocks.end();
138 }
139
140 bool BlockAllocator::is_block_current(const Block &block) const
141 {
142         BlockList::const_iterator end = cur_blocks_end;
143         return find_if(blocks.begin(), end, BlockMatch(block))!=cur_blocks_end;
144 }
145
146 void BlockAllocator::reserve_more()
147 {
148         if(blocks.empty())
149                 throw logic_error("no blocks");
150
151         BlockIter start = blocks.back();
152         if(&*start==stop_at_block)
153                 return;
154         else if(&*start==pending_block)
155         {
156                 TrackIter track = start.track_iter();
157                 if(track->is_path_changing() || !track.endpoint().has_path(track->get_active_path()))
158                         return;
159         }
160
161         pending_block = 0;
162
163         // See how many sensor blocks and how much track we already have
164         unsigned nsens = 0;
165         float dist = 0;
166         for(BlockList::const_iterator i=cur_blocks_end; i!=blocks.end(); ++i)
167         {
168                 if((*i)->get_sensor_id())
169                         ++nsens;
170                 if(nsens>0)
171                         dist += (*i)->get_path_length(i->entry());
172         }
173
174         float approach_margin = 50*train.get_layout().get_catalogue().get_scale();
175         float min_dist = train.get_controller().get_braking_distance()*1.3+approach_margin*2;
176
177         BlockIter block = start;
178
179         SetFlag setf(reserving);
180
181         while(1)
182         {
183                 BlockIter prev = block;
184                 block = block.next();
185                 if(!block || block->get_endpoints().size()<2)
186                         // The track ends here
187                         break;
188
189                 if(block->get_turnout_id() && !prev->get_turnout_id())
190                 {
191                         /* We are arriving at a turnout.  See if we have enough blocks and
192                         distance reserved. */
193                         if(nsens>=3 && dist>=min_dist)
194                                 break;
195                 }
196
197                 if(!reserve_block(block))
198                 {
199                         pending_block = &*block;
200                         break;
201                 }
202
203                 if(cur_blocks_end==blocks.end())
204                         --cur_blocks_end;
205
206                 TrackIter track = block.track_iter();
207                 if(track->is_path_changing())
208                 {
209                         pending_block = &*block;
210                         break;
211                 }
212                 else
213                 {
214                         const TrackType::Endpoint &entry_ep = track.endpoint();
215                         unsigned path = track->get_active_path();
216                         if(!entry_ep.has_path(path))
217                         {
218                                 const TrackType::Endpoint &exit_ep = track.reverse().endpoint();
219                                 if(entry_ep.has_common_paths(exit_ep))
220                                 {
221                                         unsigned mask = entry_ep.paths&exit_ep.paths;
222                                         for(path=0; mask>1; ++path, mask>>=1) ;
223
224                                         track->set_active_path(path);
225                                         if(track->is_path_changing())
226                                         {
227                                                 pending_block = &*block;
228                                                 break;
229                                         }
230                                 }
231                                 else
232                                         // XXX Do something here
233                                         break;
234                         }
235                 }
236
237                 if(&*block==stop_at_block)
238                         break;
239
240                 if(block->get_sensor_id())
241                         ++nsens;
242                 if(nsens>0)
243                         dist += block->get_path_length(block.entry());
244         }
245
246         if(!next_sensor)
247         {
248                 update_next_sensor(0);
249                 // Immediately advance to just before the next sensor
250                 advance_to(next_sensor ? next_sensor->get_block() : 0);
251         }
252 }
253
254 bool BlockAllocator::reserve_block(const BlockIter &block)
255 {
256         /* Add it to the list first to present a consistent state in block_reserved
257         signal. */
258         blocks.push_back(block);
259         try
260         {
261                 if(!block->reserve(&train))
262                 {
263                         blocks.pop_back();
264                         return false;
265                 }
266
267                 return true;
268         }
269         catch(...)
270         {
271                 blocks.pop_back();
272                 throw;
273         }
274 }
275
276 void BlockAllocator::advance_to(const Block *block)
277 {
278         BlockList::iterator end;
279         if(block)
280                 end = find_if(cur_blocks_end, blocks.end(), BlockMatch(*block));
281         else
282                 end = blocks.end();
283
284         SetFlag setf(advancing);
285         BlockList::iterator i = cur_blocks_end;
286         // Update cur_blocks_end first to keep things consistent.
287         cur_blocks_end = end;
288         for(; i!=end; ++i)
289                 train.signal_advanced.emit(**i);
290 }
291
292 void BlockAllocator::release_blocks_begin(const BlockList::iterator &end)
293 {
294         for(BlockList::iterator i=blocks.begin(); i!=end; )
295                 release_block(i++);
296 }
297
298 void BlockAllocator::release_blocks_end(const BlockList::iterator &begin)
299 {
300         // Guard against decrementing blocks.begin()
301         if(begin==blocks.begin())
302                 return release_blocks_begin(blocks.end());
303
304         if(begin==blocks.end())
305                 return;
306
307         /* Release the blocks in reverse order so that a consistent state is
308         presented in block_reserved signal. */
309         bool done = false;
310         for(BlockList::iterator i=--blocks.end(); !done; )
311         {
312                 done = (i==begin);
313                 release_block(i--);
314         }
315 }
316
317 void BlockAllocator::release_block(const BlockList::iterator &i)
318 {
319         if(advancing)
320                 throw logic_error("cannot release while advancing");
321         if(i==cur_blocks_end)
322                 ++cur_blocks_end;
323         if(next_sensor && &**i==next_sensor->get_block())
324                 next_sensor = 0;
325         if(&**i==pending_block)
326                 pending_block = 0;
327
328         Block &block = **i;
329         blocks.erase(i);
330         block.reserve(0);
331 }
332
333 void BlockAllocator::reverse()
334 {
335         release_blocks_end(cur_blocks_end);
336         blocks.reverse();
337         for(BlockList::iterator i=blocks.begin(); i!=blocks.end(); ++i)
338                 *i = i->reverse();
339
340         if(active)
341                 reserve_more();
342 }
343
344 void BlockAllocator::turnout_path_changing(Track &track)
345 {
346         BlockList::iterator i = find_if(blocks.begin(), blocks.end(), BlockMatch(track.get_block()));
347         if(i!=blocks.end())
348         {
349                 ++i;
350                 release_blocks_end(i);
351                 pending_block = &track.get_block();
352         }
353 }
354
355 void BlockAllocator::turnout_path_changed(Track &track)
356 {
357         if(&track.get_block()==pending_block && !reserving)
358                 reserve_more();
359 }
360
361 void BlockAllocator::block_reserved(Block &block, const Train *tr)
362 {
363         if(&block==pending_block && !tr && !reserving)
364                 reserve_more();
365 }
366
367 void BlockAllocator::sensor_state_changed(Sensor &sensor, Sensor::State state)
368 {
369         Block *block = sensor.get_block();
370         if(!block || block->get_train()!=&train)
371                 return;
372
373         if(state==Sensor::MAYBE_ACTIVE)
374         {
375                 if(&sensor==next_sensor)
376                 {
377                         update_next_sensor(next_sensor);
378                         advance_to(next_sensor ? next_sensor->get_block() : 0);
379
380                         if(active)
381                                 reserve_more();
382                 }
383                 else if(!is_block_current(*block))
384                         train.get_layout().emergency("Sensor for "+train.get_name()+" triggered out of order");
385         }
386         else if(state==Sensor::INACTIVE)
387         {
388                 const Vehicle &veh = train.get_vehicle(train.get_controller().get_reverse() ? 0 : train.get_n_vehicles()-1);
389                 const Block &veh_block = veh.get_track()->get_block();
390
391                 /* Sensors aren't guaranteed to be detriggered in order.  Go through the
392                 block list and locate the first sensor that's still active. */
393                 BlockList::iterator end = blocks.end();
394                 for(BlockList::iterator i=blocks.begin(); i!=cur_blocks_end; ++i)
395                 {
396                         // Avoid freeing blocks that still hold the train's vehicles
397                         if(&**i==&veh_block)
398                                 break;
399
400                         if(Sensor *s = (*i)->get_sensor())
401                         {
402                                 if(s->get_state()>=Sensor::MAYBE_INACTIVE)
403                                         break;
404                                 else
405                                         end = i;
406                         }
407                 }
408                 
409                 if(end!=blocks.end())
410                         // Free blocks up to the last inactive sensor
411                         release_blocks_begin(++end);
412         }
413 }
414
415 void BlockAllocator::update_next_sensor(Sensor *after)
416 {
417         BlockList::iterator i = cur_blocks_end;
418         if(after)
419                 i = find_if(i, blocks.end(), BlockMatch(*after->get_block()));
420
421         for(; i!=blocks.end(); ++i)
422                 if(Sensor *sensor = (*i)->get_sensor())
423                         if(sensor!=next_sensor)
424                         {
425                                 next_sensor = sensor;
426                                 return;
427                         }
428
429         next_sensor = 0;
430 }
431
432 void BlockAllocator::save(list<DataFile::Statement> &st) const
433 {
434         if(!blocks.empty() && cur_blocks_end!=blocks.begin())
435         {
436                 BlockList cur_blocks(blocks.begin(), BlockList::const_iterator(cur_blocks_end));
437                 BlockIter prev;
438                 if(train.get_controller().get_reverse())
439                 {
440                         cur_blocks.reverse();
441                         prev = cur_blocks.front().next();
442                 }
443                 else
444                         prev = cur_blocks.front().flip();
445
446                 st.push_back((DataFile::Statement("hint"), prev->get_id()));
447
448                 for(BlockList::const_iterator i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
449                         st.push_back((DataFile::Statement("block"), (*i)->get_id()));
450         }
451 }
452
453
454 BlockAllocator::Loader::Loader(BlockAllocator &ba):
455         DataFile::ObjectLoader<BlockAllocator>(ba),
456         valid(true)
457 {
458         add("block", &Loader::block);
459         add("hint",  &Loader::hint);
460 }
461
462 void BlockAllocator::Loader::block(unsigned id)
463 {
464         if(!valid)
465                 return;
466
467         Block *blk;
468         try
469         {
470                 blk = &obj.train.get_layout().get_block(id);
471         }
472         catch(const key_error &)
473         {
474                 valid = false;
475                 return;
476         }
477
478         int entry = -1;
479         if(prev_block)
480                 entry = blk->get_endpoint_by_link(*prev_block);
481         if(entry<0)
482                 entry = 0;
483
484         obj.blocks.push_back(BlockIter(blk, entry));
485         blk->reserve(&obj.train);
486
487         if(blk->get_sensor_id())
488                 obj.train.get_layout().get_driver().set_sensor(blk->get_sensor_id(), true);
489
490         prev_block = blk;
491 }
492
493 void BlockAllocator::Loader::hint(unsigned id)
494 {
495         try
496         {
497                 prev_block = &obj.train.get_layout().get_block(id);
498         }
499         catch(const key_error &)
500         {
501                 valid = false;
502         }
503 }
504
505 } // namespace R2C2