]> git.tdb.fi Git - r2c2.git/blob - source/libmarklin/train.cpp
f745313233d57ce7e2b513ee2628c815c69dade2
[r2c2.git] / source / libmarklin / train.cpp
1 /* $Id$
2
3 This file is part of the MSP Märklin suite
4 Copyright © 2006-2010  Mikkosoft Productions, Mikko Rasa
5 Distributed under the GPL
6 */
7
8 #include <cmath>
9 #include <msp/strings/formatter.h>
10 #include <msp/time/units.h>
11 #include <msp/time/utils.h>
12 #include "driver.h"
13 #include "layout.h"
14 #include "locotype.h"
15 #include "route.h"
16 #include "tracktype.h"
17 #include "train.h"
18
19 using namespace std;
20 using namespace Msp;
21
22 namespace Marklin {
23
24 Train::Train(Layout &l, const LocoType &t, unsigned a):
25         layout(l),
26         loco_type(t),
27         address(a),
28         pending_block(0),
29         target_speed(0),
30         current_speed(0),
31         reverse(false),
32         functions(0),
33         route(0),
34         next_route(0),
35         end_of_route(false),
36         status("Unplaced"),
37         travel_dist(0),
38         travel_speed(0),
39         pure_speed(false),
40         real_speed(15),
41         cur_track(0)
42 {
43         layout.add_train(*this);
44
45         layout.get_driver().add_loco(address);
46         layout.get_driver().signal_loco_speed.connect(sigc::mem_fun(this, &Train::loco_speed_event));
47         layout.get_driver().signal_loco_function.connect(sigc::mem_fun(this, &Train::loco_func_event));
48
49         layout.signal_block_reserved.connect(sigc::mem_fun(this, &Train::block_reserved));
50         layout.get_driver().signal_sensor.connect(sigc::mem_fun(this, &Train::sensor_event));
51         layout.get_driver().signal_turnout.connect(sigc::mem_fun(this, &Train::turnout_event));
52 }
53
54 Train::~Train()
55 {
56         layout.remove_train(*this);
57 }
58
59 void Train::set_name(const string &n)
60 {
61         name = n;
62
63         signal_name_changed.emit(name);
64 }
65
66 void Train::set_speed(unsigned speed)
67 {
68         if(speed==target_speed)
69                 return;
70         travel_speed = static_cast<int>(round(get_real_speed(speed)*87*3.6/5))*5;
71
72         target_speed = speed;
73         if(!target_speed)
74         {
75                 pending_block = 0;
76                 stop_timeout = Time::now()+(800+current_speed*150)*Time::msec;
77         }
78         else
79                 reserve_more();
80
81         signal_target_speed_changed.emit(target_speed);
82
83         update_speed();
84         pure_speed = false;
85 }
86
87 void Train::set_reverse(bool rev)
88 {
89         if(rev==reverse)
90                 return;
91
92         if(target_speed)
93         {
94                 set_speed(0);
95                 return;
96         }
97         else if(stop_timeout)
98                 return;
99
100         layout.get_driver().set_loco_reverse(address, rev);
101
102         release_blocks(rsv_blocks);
103         reverse_blocks(cur_blocks);
104
105         if(cur_track)
106         {
107                 unsigned path = cur_track->get_active_path();
108                 cur_track_ep = cur_track->traverse(cur_track_ep, path);
109                 offset = cur_track->get_type().get_path_length(path)-offset;
110         }
111 }
112
113 void Train::set_function(unsigned func, bool state)
114 {
115         if(!loco_type.get_functions().count(func))
116                 throw InvalidParameterValue("Invalid function");
117         if(func<5)
118                 layout.get_driver().set_loco_function(address, func, state);
119         else
120                 layout.get_driver().set_loco_function(address+1, func-4, state);
121 }
122
123 bool Train::get_function(unsigned func) const
124 {
125         return (functions>>func)&1;
126 }
127
128 void Train::set_route(const Route *r)
129 {
130         if(!rsv_blocks.empty())
131         {
132                 for(list<BlockRef>::iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
133                         if(i->block->get_sensor_id())
134                         {
135                                 release_blocks(rsv_blocks, ++i, rsv_blocks.end());
136                                 break;
137                         }
138         }
139
140         route = r;
141         next_route = 0;
142         end_of_route = false;
143
144         if(route)
145         {
146                 BlockRef &last = (rsv_blocks.empty() ? cur_blocks.back() : rsv_blocks.back());
147                 BlockRef next = last.next();
148                 const Block::Endpoint &ep = next.block->get_endpoints()[next.entry];
149                 if(!route->get_tracks().count(ep.track))
150                 {
151                         next_route = route;
152                         route = Route::find(*ep.track, ep.track_ep, *next_route);
153                 }
154         }
155
156         if(target_speed && reserve_more()<2)
157                 update_speed();
158
159         signal_route_changed.emit(route);
160 }
161
162 void Train::go_to(const Track &to)
163 {
164         for(list<BlockRef>::const_iterator i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
165                 if(i->block->get_tracks().count(const_cast<Track *>(&to)))
166                 {
167                         set_speed(0);
168                         set_route(0);
169                         return;
170                 }
171
172         BlockRef *last = 0;
173         if(rsv_blocks.empty())
174                 last = &cur_blocks.back();
175         else
176         {
177                 for(list<BlockRef>::iterator i=rsv_blocks.begin(); (i!=rsv_blocks.end() && !last); ++i)
178                         if(i->block->get_sensor_id())
179                                 last = &*i;
180         }
181
182         BlockRef next = last->next();
183         const Block::Endpoint &ep = next.block->get_endpoints()[next.entry];
184
185         set_route(Route::find(*ep.track, ep.track_ep, to));
186 }
187
188 void Train::place(Block &block, unsigned entry)
189 {
190         if(target_speed)
191                 set_speed(0);
192
193         release_blocks(rsv_blocks);
194         release_blocks(cur_blocks);
195
196         if(!block.reserve(this))
197         {
198                 set_status("Unplaced");
199                 return;
200         }
201
202         cur_blocks.push_back(BlockRef(&block, entry));
203         set_position(block.get_endpoints()[entry]);
204
205         set_status("Stopped");
206 }
207
208 bool Train::free_block(Block &block)
209 {
210         unsigned nsens = 0;
211         for(list<BlockRef>::iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
212         {
213                 if(i->block==&block)
214                 {
215                         if(nsens<1)
216                                 return false;
217                         release_blocks(rsv_blocks, i, rsv_blocks.end());
218                         update_speed();
219                         return true;
220                 }
221                 else if(i->block->get_sensor_id())
222                         ++nsens;
223         }
224
225         return false;
226 }
227
228 int Train::get_entry_to_block(Block &block) const
229 {
230         for(list<BlockRef>::const_iterator i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
231                 if(i->block==&block)
232                         return i->entry;
233         for(list<BlockRef>::const_iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
234                 if(i->block==&block)
235                         return i->entry;
236         return -1;
237 }
238
239 void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt)
240 {
241         if(stop_timeout && t>=stop_timeout)
242         {
243                 release_blocks(rsv_blocks);
244                 end_of_route = false;
245                 stop_timeout = Time::TimeStamp();
246         }
247
248         if(cur_track)
249         {
250                 unsigned path = cur_track->get_active_path();
251
252                 offset += get_real_speed(current_speed)*(dt/Time::sec);
253                 float path_len = cur_track->get_type().get_path_length(path);
254                 if(offset>path_len)
255                 {
256                         unsigned out = cur_track->traverse(cur_track_ep, path);
257                         Track *next = cur_track->get_link(out);
258
259                         bool ok = false;
260                         for(list<BlockRef>::const_iterator i=cur_blocks.begin(); (!ok && i!=cur_blocks.end()); ++i)
261                                 ok = i->block->get_tracks().count(next);
262
263                         if(ok)
264                         {
265                                 if(next)
266                                         cur_track_ep = next->get_endpoint_by_link(*cur_track);
267                                 cur_track = next;
268                                 offset = 0;
269                         }
270                         else
271                                 offset = path_len-0.001;
272                 }
273
274                 if(cur_track)
275                         pos = cur_track->get_point(cur_track_ep, path, offset).pos;
276         }
277 }
278
279 void Train::save(list<DataFile::Statement> &st) const
280 {
281         st.push_back((DataFile::Statement("name"), name));
282         for(unsigned i=0; i<=14; ++i)
283                 if(real_speed[i].weight)
284                         st.push_back((DataFile::Statement("real_speed"), i, real_speed[i].speed, real_speed[i].weight));
285
286         if(!cur_blocks.empty())
287         {
288                 list<BlockRef> blocks = cur_blocks;
289                 if(reverse)
290                         reverse_blocks(blocks);
291
292                 Block *prev = blocks.front().block->get_endpoints()[blocks.front().entry].link;
293                 st.push_back((DataFile::Statement("block_hint"), prev->get_id()));
294
295                 for(list<BlockRef>::const_iterator i=blocks.begin(); i!=blocks.end(); ++i)
296                         st.push_back((DataFile::Statement("block"), i->block->get_id()));
297         }
298
299         if(route)
300                 st.push_back((DataFile::Statement("route"), route->get_name()));
301 }
302
303 void Train::loco_speed_event(unsigned addr, unsigned speed, bool rev)
304 {
305         if(addr==address)
306         {
307                 current_speed = speed;
308                 reverse = rev;
309
310                 signal_speed_changed.emit(current_speed);
311                 signal_reverse_changed.emit(reverse);
312         }
313 }
314
315 void Train::loco_func_event(unsigned addr, unsigned func, bool state)
316 {
317         if(addr==address || (addr==address+1 && loco_type.get_max_function()>4))
318         {
319                 if(addr==address+1)
320                         func += 4;
321                 if(state)
322                         functions |= 1<<func;
323                 else
324                         functions &= ~(1<<func);
325
326                 signal_function_changed.emit(func, state);
327         }
328 }
329
330 void Train::sensor_event(unsigned addr, bool state)
331 {
332         if(state)
333         {
334                 // Find the first sensor block from our reserved blocks that isn't this sensor
335                 list<BlockRef>::iterator i;
336                 for(i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
337                         if(i->block->get_sensor_id() && i->block->get_sensor_id()!=addr)
338                                 break;
339
340                 if(i!=rsv_blocks.begin())
341                 {
342                         // Compute speed and update related state
343                         float travel_time_secs = (Time::now()-last_entry_time)/Time::sec;
344                         travel_speed = static_cast<int>(round(travel_dist/travel_time_secs*87*3.6/5))*5;
345
346                         if(pure_speed)
347                         {
348                                 RealSpeed &rs = real_speed[current_speed];
349                                 rs.add(travel_dist/travel_time_secs, travel_time_secs);
350                         }
351
352                         travel_dist = 0;
353                         float block_len;
354                         for(list<BlockRef>::iterator j=rsv_blocks.begin(); j!=i; ++j)
355                         {
356                                 j->block->traverse(j->entry, &block_len);
357                                 travel_dist += block_len;
358
359                                 if(j->block->get_sensor_id()==addr)
360                                         set_position(j->block->get_endpoints()[j->entry]);
361                         }
362                         last_entry_time = Time::now();
363                         pure_speed = true;
364
365                         // Check if we've reached the next route
366                         if(next_route)
367                         {
368                                 const set<const Track *> &rtracks = next_route->get_tracks();
369                                 for(list<BlockRef>::iterator j=rsv_blocks.begin(); j!=i; ++j)
370                                         if(rtracks.count(j->block->get_endpoints()[j->entry].track))
371                                         {
372                                                 route = next_route;
373                                                 next_route = 0;
374                                                 // XXX Exceptions?
375                                                 signal_route_changed.emit(route);
376                                                 break;
377                                         }
378                         }
379
380                         // Move blocks up to the next sensor to our current blocks
381                         cur_blocks.splice(cur_blocks.end(), rsv_blocks, rsv_blocks.begin(), i);
382
383                         // Try to get more blocks if we're moving
384                         if(target_speed)
385                         {
386                                 unsigned nsens = reserve_more();
387                                 if(!nsens && end_of_route)
388                                 {
389                                         set_speed(0);
390                                         set_route(0);
391                                 }
392                                 else if(nsens<2)
393                                         update_speed();
394                         }
395                 }
396         }
397         else
398         {
399                 // Find the first sensor in our current blocks that's still active
400                 list<BlockRef>::iterator end = cur_blocks.begin();
401                 for(list<BlockRef>::iterator i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
402                         if(i->block->get_sensor_id())
403                         {
404                                 if(layout.get_driver().get_sensor(i->block->get_sensor_id()))
405                                         break;
406                                 else
407                                         end = i;
408                         }
409                 
410                 if(end!=cur_blocks.begin())
411                 {
412                         // Free blocks up to the last inactive sensor
413                         ++end;
414                         release_blocks(cur_blocks, cur_blocks.begin(), end);
415                 }
416         }
417 }
418
419 void Train::turnout_event(unsigned addr, bool)
420 {
421         if(pending_block)
422         {
423                 unsigned pending_addr = pending_block->get_turnout_id();
424                 bool double_addr = (*pending_block->get_tracks().begin())->get_type().is_double_address();
425                 if(addr==pending_addr || (double_addr && addr==pending_addr+1))
426                         reserve_more();
427         }
428 }
429
430 void Train::block_reserved(const Block &block, const Train *train)
431 {
432         if(&block==pending_block && !train)
433                 reserve_more();
434 }
435
436 unsigned Train::reserve_more()
437 {
438         BlockRef *last = 0;
439         if(!rsv_blocks.empty())
440                 last = &rsv_blocks.back();
441         else if(!cur_blocks.empty())
442                 last = &cur_blocks.back();
443         if(!last)
444                 return 0;
445
446         pending_block = 0;
447
448         // See how many sensor blocks we already have
449         unsigned nsens = 0;
450         for(list<BlockRef>::const_iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
451                 if(i->block->get_sensor_id())
452                         ++nsens;
453         
454         const Route *cur_route = 0;
455         if(route)
456         {
457                 unsigned exit = last->block->traverse(last->entry);
458                 Track *track = last->block->get_endpoints()[exit].track;
459                 if(route->get_tracks().count(track))
460                         cur_route = route;
461                 else if(next_route && next_route->get_tracks().count(track))
462                         cur_route = next_route;
463         }
464
465         bool got_more = false;
466         BlockRef *good = last;
467         unsigned good_sens = nsens;
468         while(good_sens<3)
469         {
470                 // Traverse to the next block
471                 unsigned exit = last->block->traverse(last->entry);
472                 Block *link = last->block->get_link(exit);
473                 if(!link)
474                         break;
475
476                 int entry = link->get_endpoint_by_link(*last->block);
477                 if(entry<0)
478                         throw LogicError("Block links are inconsistent!");
479
480                 const Block::Endpoint &entry_ep = link->get_endpoints()[entry];
481
482                 if(cur_route)
483                 {
484                         if(cur_route!=next_route && next_route && next_route->get_tracks().count(entry_ep.track))
485                                 cur_route = next_route;
486                         else if(!cur_route->get_tracks().count(entry_ep.track))
487                         {
488                                 // Keep the blocks if we arrived at the end of the route
489                                 good = last;
490                                 good_sens = nsens;
491                                 end_of_route = true;
492                                 break;
493                         }
494                 }
495                 else if(route && route->get_tracks().count(entry_ep.track))
496                         cur_route = route;
497
498                 if(!link->reserve(this))
499                 {
500                         // If we found another train and it's not headed straight for us, we can keep the blocks we got
501                         int other_entry = link->get_train()->get_entry_to_block(*link);
502                         if(other_entry<0)
503                                 throw LogicError("Block reservation inconsistency");
504                         if(static_cast<unsigned>(entry)!=link->traverse(other_entry))
505                         {
506                                 good = last;
507                                 good_sens = nsens;
508                         }
509                         pending_block = link;
510                         break;
511                 }
512
513                 if(link->get_turnout_id())
514                 {
515                         const Endpoint &track_ep = entry_ep.track->get_type().get_endpoints()[entry_ep.track_ep];
516
517                         // Keep the blocks reserved so far, as either us or the other train can diverge
518                         good = last;
519                         good_sens = nsens;
520
521                         // Figure out what path we'd like to take on the turnout
522                         int path = -1;
523                         if(cur_route)
524                                 path = cur_route->get_turnout(link->get_turnout_id());
525                         if(path<0)
526                                 path = entry_ep.track->get_active_path();
527                         if(!((track_ep.paths>>path)&1))
528                         {
529                                 for(unsigned i=0; track_ep.paths>>i; ++i)
530                                         if((track_ep.paths>>i)&1)
531                                                 path = i;
532                         }
533
534                         if(path!=static_cast<int>(entry_ep.track->get_active_path()))
535                         {
536                                 // The turnout is set to wrong path - switch and wait for it
537                                 link->reserve(0);
538                                 pending_block = link;
539                                 entry_ep.track->set_active_path(path);
540                                 break;
541                         }
542                 }
543
544                 rsv_blocks.push_back(BlockRef(link, entry));
545                 last = &rsv_blocks.back();
546                 if(last->block->get_sensor_id())
547                 {
548                         ++nsens;
549                         got_more = true;
550                 }
551         }
552
553         // Unreserve blocks that were not good
554         while(!rsv_blocks.empty() && last!=good)
555         {
556                 last->block->reserve(0);
557                 rsv_blocks.erase(--rsv_blocks.end());
558                 if(!rsv_blocks.empty())
559                         last = &rsv_blocks.back();
560         }
561
562         if(got_more)
563                 update_speed();
564
565         return good_sens;
566 }
567
568 void Train::update_speed()
569 {
570         Driver &driver = layout.get_driver();
571
572         unsigned speed;
573         if(!target_speed)
574         {
575                 speed = 0;
576                 set_status("Stopped");
577         }
578         else
579         {
580                 unsigned nsens = 0;
581                 for(list<BlockRef>::const_iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
582                         if(i->block->get_sensor_id())
583                                 ++nsens;
584
585                 unsigned slow_speed = find_speed(0.1);  // 31.3 km/h
586                 if(nsens==0)
587                 {
588                         speed = 0;
589                         pure_speed = false;
590                         set_status("Blocked");
591                 }
592                 else if(nsens==1 && target_speed>slow_speed)
593                 {
594                         speed = slow_speed;
595                         pure_speed = false;
596                         set_status("Slow");
597                 }
598                 else
599                 {
600                         speed = target_speed;
601                         set_status(format("Traveling %d kmh", travel_speed));
602                 }
603         }
604
605         driver.set_loco_speed(address, speed);
606 }
607
608 float Train::get_real_speed(unsigned i) const
609 {
610         if(real_speed[i].weight)
611                 return real_speed[i].speed;
612
613         unsigned low;
614         unsigned high;
615         for(low=i; low>0; --low)
616                 if(real_speed[low].weight)
617                         break;
618         for(high=i; high<14; ++high)
619                 if(real_speed[high].weight)
620                         break;
621
622         if(real_speed[high].weight)
623         {
624                 if(real_speed[low].weight)
625                 {
626                         float f = float(i-low)/(high-low);
627                         return real_speed[low].speed*(1-f)+real_speed[high].speed*f;
628                 }
629                 else
630                         return real_speed[high].speed*float(i)/high;
631         }
632         else if(real_speed[low].weight)
633                 return real_speed[low].speed*float(i)/low;
634         else
635                 return 0;
636 }
637
638 unsigned Train::find_speed(float real) const
639 {
640         if(real<=real_speed[0].speed)
641                 return 0;
642
643         unsigned low = 0;
644         unsigned high = 0;
645         for(unsigned i=0; (!high && i<=14); ++i)
646                 if(real_speed[i].weight)
647                 {
648                         if(real_speed[i].speed<real)
649                                 low = i;
650                         else
651                                 high = i;
652                 }
653         if(!high)
654         {
655                 if(!low)
656                         return 0;
657                 return min(static_cast<unsigned>(low*real/real_speed[low].speed), 14U);
658         }
659
660         float f = (real-real_speed[low].speed)/(real_speed[high].speed-real_speed[low].speed);
661         return static_cast<unsigned>(low*(1-f)+high*f+0.5);
662 }
663
664 void Train::set_status(const string &s)
665 {
666         status = s;
667         signal_status_changed.emit(s);
668 }
669
670 void Train::set_position(const Block::Endpoint &bep)
671 {
672         cur_track = bep.track;
673         cur_track_ep = bep.track_ep;
674         offset = 0;
675         pos = cur_track->get_endpoint_position(cur_track_ep);
676 }
677
678 void Train::release_blocks(list<BlockRef> &blocks)
679 {
680         release_blocks(blocks, blocks.begin(), blocks.end());
681 }
682
683 void Train::release_blocks(list<BlockRef> &blocks, list<BlockRef>::iterator begin, list<BlockRef>::iterator end)
684 {
685         while(begin!=end)
686         {
687                 Block *block = begin->block;
688                 blocks.erase(begin++);
689                 block->reserve(0);
690         }
691 }
692
693 void Train::reverse_blocks(list<BlockRef> &blocks) const
694 {
695         blocks.reverse();
696         for(list<BlockRef>::iterator i=blocks.begin(); i!=blocks.end(); ++i)
697                 i->entry = i->block->traverse(i->entry);
698 }
699
700
701 Train::BlockRef::BlockRef(Block *b, unsigned e):
702         block(b),
703         entry(e)
704 { }
705
706 Train::BlockRef Train::BlockRef::next() const
707 {
708         Block *blk = block->get_endpoints()[block->traverse(entry)].link;
709         if(!blk)
710                 throw InvalidState("At end of line");
711
712         int ep = blk->get_endpoint_by_link(*block);
713         if(ep<0)
714                 throw LogicError("Block links are inconsistent");
715
716         return BlockRef(blk, ep);
717 }
718
719
720 Train::RealSpeed::RealSpeed():
721         speed(0),
722         weight(0)
723 { }
724
725 void Train::RealSpeed::add(float s, float w)
726 {
727         speed = (speed*weight+s*w)/(weight+w);
728         weight = min(weight+w, 300.0f);
729 }
730
731
732 Train::Loader::Loader(Train &t):
733         DataFile::BasicLoader<Train>(t),
734         prev_block(0)
735 {
736         add("block",       &Loader::block);
737         add("block_hint",  &Loader::block_hint);
738         add("name",        &Loader::name);
739         add("real_speed",  &Loader::real_speed);
740         add("route",       &Loader::route);
741 }
742
743 void Train::Loader::block(unsigned id)
744 {
745         Block &blk = obj.layout.get_block(id);
746         int entry = -1;
747         if(prev_block)
748                 entry = blk.get_endpoint_by_link(*prev_block);
749         if(entry<0)
750                 entry = 0;
751
752         blk.reserve(&obj);
753         obj.cur_blocks.push_back(BlockRef(&blk, entry));
754         obj.set_status("Stopped");
755         obj.set_position(blk.get_endpoints()[entry]);
756
757         prev_block = &blk;
758 }
759
760 void Train::Loader::block_hint(unsigned id)
761 {
762         prev_block = &obj.layout.get_block(id);
763 }
764
765 void Train::Loader::name(const string &n)
766 {
767         obj.set_name(n);
768 }
769
770 void Train::Loader::real_speed(unsigned i, float speed, float weight)
771 {
772         obj.real_speed[i].speed = speed;
773         obj.real_speed[i].weight = weight;
774 }
775
776 void Train::Loader::route(const string &n)
777 {
778         obj.set_route(&obj.layout.get_route(n));
779 }
780
781 } // namespace Marklin