3 #include <msp/core/maputils.h>
4 #include <msp/strings/format.h>
5 #include <msp/time/units.h>
6 #include <msp/time/utils.h>
12 #include "simplecontroller.h"
13 #include "speedquantizer.h"
14 #include "timetable.h"
15 #include "trackiter.h"
16 #include "tracktype.h"
18 #include "trainrouter.h"
20 #include "vehicletype.h"
32 SetFlag(bool &f): flag(f) { flag = true; }
33 ~SetFlag() { flag = false; }
41 Train::Train(Layout &l, const VehicleType &t, unsigned a, const string &p):
47 cur_blocks_end(blocks.end()),
48 clear_blocks_end(blocks.end()),
52 controller(new SimpleController),
54 current_speed_step(0),
55 speed_changing(false),
61 accurate_position(false),
64 if(!loco_type.is_locomotive())
65 throw invalid_argument("Train::Train");
67 unsigned speed_steps = layout.get_driver().get_protocol_speed_steps(protocol);
69 speed_quantizer = new SpeedQuantizer(speed_steps);
71 vehicles.push_back(new Vehicle(layout, loco_type));
73 layout.add_train(*this);
75 layout.get_driver().add_loco(address, protocol, loco_type);
76 layout.get_driver().signal_loco_speed.connect(sigc::mem_fun(this, &Train::loco_speed_event));
77 layout.get_driver().signal_loco_function.connect(sigc::mem_fun(this, &Train::loco_func_event));
79 layout.signal_block_reserved.connect(sigc::mem_fun(this, &Train::block_reserved));
80 layout.signal_block_state_changed.connect(sigc::mem_fun(this, &Train::block_state_changed));
82 layout.get_driver().signal_halt.connect(sigc::mem_fun(this, &Train::halt_event));
84 const set<Track *> &tracks = layout.get_tracks();
85 for(set<Track *>::const_iterator i=tracks.begin(); i!=tracks.end(); ++i)
86 if((*i)->get_turnout_id())
87 (*i)->signal_path_changed.connect(sigc::hide(sigc::bind(sigc::mem_fun(this, &Train::turnout_path_changed), sigc::ref(**i))));
89 controller->signal_control_changed.connect(sigc::mem_fun(this, &Train::control_changed));
95 for(vector<Vehicle *>::iterator i=vehicles.begin(); i!=vehicles.end(); ++i)
97 layout.remove_train(*this);
100 void Train::set_name(const string &n)
104 signal_name_changed.emit(name);
107 void Train::add_vehicle(const VehicleType &vt)
109 Vehicle *veh = new Vehicle(layout, vt);
110 vehicles.back()->attach_back(*veh);
111 vehicles.push_back(veh);
114 void Train::remove_vehicle(unsigned i)
116 if(i>=vehicles.size())
117 throw out_of_range("Train::remove_vehicle");
119 throw logic_error("can't remove locomotive");
121 vehicles.erase(vehicles.begin()+i);
122 if(i<vehicles.size())
123 vehicles[i-1]->attach_back(*vehicles[i]);
126 unsigned Train::get_n_vehicles() const
128 return vehicles.size();
131 Vehicle &Train::get_vehicle(unsigned i)
133 if(i>=vehicles.size())
134 throw out_of_range("Train::get_vehicle");
138 const Vehicle &Train::get_vehicle(unsigned i) const
140 if(i>=vehicles.size())
141 throw out_of_range("Train::get_vehicle");
145 void Train::set_control(const string &n, float v)
147 controller->set_control(n, v);
150 void Train::set_active(bool a)
154 if(!a && controller->get_speed())
155 throw logic_error("moving");
160 stop_timeout = Time::TimeStamp();
164 stop_timeout = Time::now()+2*Time::sec;
167 void Train::set_function(unsigned func, bool state)
169 if(!loco_type.get_functions().count(func))
170 throw invalid_argument("Train::set_function");
171 layout.get_driver().set_loco_function(address, func, state);
174 float Train::get_control(const string &ctrl) const
176 return controller->get_control(ctrl).value;
179 float Train::get_speed() const
181 return controller->get_speed();
184 float Train::get_quantized_speed() const
187 return speed_quantizer->quantize_speed(controller->get_speed());
189 return controller->get_speed();
192 bool Train::get_function(unsigned func) const
194 return (functions>>func)&1;
197 void Train::add_ai(TrainAI &ai)
200 ai.signal_event.connect(sigc::bind<0>(signal_ai_event, sigc::ref(ai)));
203 void Train::remove_ai(TrainAI &ai)
205 list<TrainAI *>::iterator i = find(ais.begin(), ais.end(), &ai);
210 TrainAI *Train::get_tagged_ai(const string &tag) const
212 for(list<TrainAI *>::const_iterator i=ais.begin(); i!=ais.end(); ++i)
213 if((*i)->get_tag()==tag)
219 void Train::ai_message(const TrainAI::Message &msg)
221 for(list<TrainAI *>::iterator i=ais.begin(); i!=ais.end(); ++i)
225 void Train::place(Block &block, unsigned entry)
227 if(controller->get_speed())
228 throw logic_error("moving");
233 accurate_position = false;
235 blocks.push_back(BlockIter(&block, entry));
236 if(!block.reserve(this))
244 TrackIter track = BlockIter(&block, entry).reverse().track_iter();
245 vehicles.front()->place(*track, track.entry(), 0, Vehicle::FRONT_BUFFER);
249 const Block::Endpoint &bep = block.get_endpoint(entry);
250 vehicles.back()->place(*bep.track, bep.track_ep, 0, Vehicle::BACK_BUFFER);
254 void Train::unplace()
256 if(controller->get_speed())
257 throw logic_error("moving");
262 accurate_position = false;
264 for(vector<Vehicle *>::iterator i=vehicles.begin(); i!=vehicles.end(); ++i)
268 void Train::stop_at(Block *block)
270 stop_at_block = block;
273 bool Train::free_block(Block &block)
275 if(get_reserved_distance_until(&block, false)<controller->get_braking_distance()*1.3)
279 for(BlockList::iterator i=cur_blocks_end; i!=blocks.end(); ++i)
281 if(i->block()==&block)
285 release_blocks(i, blocks.end());
288 else if((*i)->get_sensor_id())
295 void Train::free_noncritical_blocks()
300 if(controller->get_speed()==0)
302 release_blocks(cur_blocks_end, blocks.end());
306 float margin = 10*layout.get_catalogue().get_scale();
307 float min_dist = controller->get_braking_distance()*1.3+margin;
309 Vehicle &veh = *(reverse ? vehicles.back() : vehicles.front());
311 TrackIter track(veh.get_track(), veh.get_entry());
312 BlockList::iterator block = blocks.begin();
314 while(block!=blocks.end() && !(*block)->has_track(*track))
317 if(block==cur_blocks_end)
321 float dist = veh.get_offset();
325 dist = track->get_type().get_path_length(track->get_active_path())-dist;
326 dist -= veh.get_type().get_length()/2;
331 track = track.next();
333 if(!(*block)->has_track(*track))
336 if(block==cur_blocks_end)
338 if(block==blocks.end())
341 if(dist>min_dist && nsens>0)
343 release_blocks(block, blocks.end());
347 if(in_rsv && (*block)->get_sensor_id())
351 dist += track->get_type().get_path_length(track->get_active_path());
355 const BlockIter &Train::get_head_block() const
358 throw logic_error("no blocks");
359 return blocks.back();
362 const BlockIter &Train::get_tail_block() const
365 throw logic_error("no blocks");
366 return blocks.front();
369 int Train::get_entry_to_block(const Block &block) const
371 for(BlockList::const_iterator i=blocks.begin(); i!=blocks.end(); ++i)
372 if(i->block()==&block)
377 float Train::get_reserved_distance() const
383 TrackIter next = blocks.back().next().track_iter();
384 if(next && next->get_type().is_turnout())
385 margin = 15*layout.get_catalogue().get_scale();
387 return max(get_reserved_distance_until(0, false)-margin, 0.0f);
390 void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt)
392 if(!active && stop_timeout && t>=stop_timeout)
394 release_blocks(cur_blocks_end, blocks.end());
395 stop_timeout = Time::TimeStamp();
398 Driver &driver = layout.get_driver();
400 for(list<TrainAI *>::iterator i=ais.begin(); i!=ais.end(); ++i)
402 controller->tick(dt);
403 float speed = controller->get_speed();
404 bool moving = speed>0;
406 if(controller->get_reverse()!=reverse)
408 reverse = controller->get_reverse();
410 if(loco_type.get_swap_direction())
412 driver.set_loco_reverse(address, r);
414 release_blocks(cur_blocks_end, blocks.end());
415 reverse_blocks(blocks);
422 unsigned speed_step = speed_quantizer->find_speed_step(speed);
423 if(speed_step!=current_speed_step && !speed_changing && !driver.is_halted() && driver.get_power())
425 speed_changing = true;
426 driver.set_loco_speed(address, speed_step);
431 speed = speed_quantizer->get_speed(current_speed_step);
439 Vehicle &vehicle = *(reverse ? vehicles.back() : vehicles.front());
440 Track *track = vehicle.get_track();
443 for(BlockList::const_iterator i=blocks.begin(); (!ok && i!=cur_blocks_end); ++i)
444 ok = (*i)->has_track(*track);
446 float d = speed*(dt/Time::sec);
449 SetFlag setf(advancing);
450 vehicle.advance(reverse ? -d : d);
452 else if(accurate_position)
455 if(overshoot_dist>40*layout.get_catalogue().get_scale())
457 layout.emergency(name+" has not arrived at sensor");
458 accurate_position = false;
463 if(!blocks.empty() && !blocks.front()->get_sensor_id())
465 float dist = get_reserved_distance_until(&*blocks.front(), true);
467 if(dist>10*layout.get_catalogue().get_scale())
469 Block &block = *blocks.front();
476 void Train::save(list<DataFile::Statement> &st) const
478 st.push_back((DataFile::Statement("name"), name));
480 for(vector<Vehicle *>::const_iterator i=vehicles.begin(); i!=vehicles.end(); ++i)
481 if(i!=vehicles.begin())
482 st.push_back((DataFile::Statement("vehicle"), (*i)->get_type().get_article_number()));
486 DataFile::Statement ss("quantized_speed");
487 speed_quantizer->save(ss.sub);
491 if(!blocks.empty() && cur_blocks_end!=blocks.begin())
493 BlockList blks(blocks.begin(), BlockList::const_iterator(cur_blocks_end));
495 reverse_blocks(blks);
497 BlockIter prev = blks.front().flip();
498 st.push_back((DataFile::Statement("block_hint"), prev->get_id()));
500 for(BlockList::const_iterator i=blks.begin(); i!=blks.end(); ++i)
501 st.push_back((DataFile::Statement("block"), (*i)->get_id()));
504 // XXX Need more generic way of saving AI state
505 for(list<TrainAI *>::const_iterator i=ais.begin(); i!=ais.end(); ++i)
507 if(TrainRouter *router = dynamic_cast<TrainRouter *>(*i))
509 DataFile::Statement ss("router");
510 router->save(ss.sub);
513 else if(Timetable *timetable = dynamic_cast<Timetable *>(*i))
515 DataFile::Statement ss("timetable");
516 timetable->save(ss.sub);
522 void Train::control_changed(const Controller::Control &ctrl)
524 signal_control_changed.emit(ctrl.name, ctrl.value);
527 void Train::loco_speed_event(unsigned addr, unsigned speed, bool rev)
531 current_speed_step = speed;
533 if(loco_type.get_swap_direction())
536 layout.get_driver().set_loco_reverse(address, r);
537 speed_changing = false;
542 void Train::loco_func_event(unsigned addr, unsigned func, bool state)
547 functions |= 1<<func;
549 functions &= ~(1<<func);
551 signal_function_changed.emit(func, state);
555 void Train::block_state_changed(Block &block, Block::State state)
557 if(state==Block::MAYBE_ACTIVE)
559 // Find the first sensor block from our reserved blocks that isn't this sensor
560 BlockList::iterator end;
562 for(end=cur_blocks_end; end!=blocks.end(); ++end)
563 if((*end)->get_sensor_id())
580 // Compute speed and update related state
581 float travel_time_secs = (Time::now()-last_entry_time)/Time::sec;
583 if(pure_speed && speed_quantizer && current_speed_step>0 && travel_time_secs>=2)
584 speed_quantizer->learn(current_speed_step, travel_dist/travel_time_secs, travel_time_secs);
587 for(BlockList::iterator j=cur_blocks_end; j!=end; ++j)
589 travel_dist += (*j)->get_path_length(j->entry());
591 if(&**j==&block && !advancing)
593 TrackIter track = j->track_iter();
596 track = track.flip();
597 vehicles.back()->place(*track, track.entry(), 0, Vehicle::BACK_AXLE);
600 vehicles.front()->place(*track, track.entry(), 0, Vehicle::FRONT_AXLE);
603 last_entry_time = Time::now();
605 accurate_position = true;
608 // Move blocks up to the next sensor to our current blocks
609 for(BlockList::iterator j=cur_blocks_end; j!=end; ++j)
610 signal_advanced.emit(**j);
611 cur_blocks_end = end;
613 // Try to get more blocks if we're moving
618 layout.emergency("Sensor for "+name+" triggered out of order");
620 else if(state==Block::INACTIVE)
622 const Vehicle &veh = *(reverse ? vehicles.front() : vehicles.back());
624 // Find the first sensor in our current blocks that's still active
625 BlockList::iterator end = blocks.begin();
626 for(BlockList::iterator i=blocks.begin(); i!=cur_blocks_end; ++i)
628 if((*i)->has_track(*veh.get_track()))
630 if((*i)->get_sensor_id())
632 if(layout.get_driver().get_sensor((*i)->get_sensor_id()))
642 if(end!=blocks.begin() && end!=cur_blocks_end)
643 // Free blocks up to the last inactive sensor
644 release_blocks(blocks.begin(), end);
648 void Train::turnout_path_changed(Track &track)
650 for(list<BlockIter>::iterator i=blocks.begin(); i!=blocks.end(); ++i)
651 if((*i)->get_turnout_id()==track.get_turnout_id() && !reserving)
653 if(&**i==pending_block)
656 check_turnout_paths(false);
660 void Train::halt_event(bool h)
663 accurate_position = false;
666 void Train::block_reserved(const Block &block, const Train *train)
668 if(&block==pending_block && !train && !reserving)
672 void Train::reserve_more()
674 if(!active || blocks.empty())
677 BlockIter start = blocks.back();
678 if(&*start==stop_at_block)
684 // See how many sensor blocks and how much track we already have
687 for(BlockList::const_iterator i=cur_blocks_end; i!=blocks.end(); ++i)
689 if((*i)->get_sensor_id())
692 dist += (*i)->get_path_length(i->entry());
695 float approach_margin = 50*layout.get_catalogue().get_scale();
696 float min_dist = controller->get_braking_distance()*1.3+approach_margin*2;
698 BlockIter block = start;
700 SetFlag setf(reserving);
704 BlockIter last = block;
705 block = block.next();
706 if(!block || block->get_endpoints().size()<2)
707 // The track ends here
710 if(block->get_turnout_id() && !last->get_turnout_id())
712 /* We are arriving at a turnout. See if we have enough blocks and
713 distance reserved. */
714 if(nsens>=3 && dist>=min_dist)
718 blocks.push_back(block);
719 if(!block->reserve(this))
722 pending_block = &*block;
726 if(cur_blocks_end==blocks.end())
728 if(clear_blocks_end==blocks.end())
731 TrackIter track = block.track_iter();
732 if(track->is_path_changing())
734 pending_block = &*block;
738 if(&*block==stop_at_block)
741 if(block->get_sensor_id())
744 dist += block->get_path_length(block.entry());
747 check_turnout_paths(true);
749 // Make any sensorless blocks at the beginning immediately current
750 while(cur_blocks_end!=clear_blocks_end && !(*cur_blocks_end)->get_sensor_id())
754 void Train::check_turnout_paths(bool set)
756 if(clear_blocks_end==blocks.end())
759 for(list<BlockIter>::iterator i=clear_blocks_end; i!=blocks.end(); ++i)
761 if((*i)->get_turnout_id())
763 TrackIter track = i->track_iter();
764 const TrackType::Endpoint &track_ep = track.endpoint();
767 list<BlockIter>::iterator j = i;
768 if(++j!=blocks.end())
770 TrackIter rev = j->track_iter().flip();
771 unsigned mask = rev.endpoint().paths&track_ep.paths;
772 for(path=0; mask>1; mask>>=1, ++path) ;
777 if(path!=track->get_active_path())
779 if(set && !track->is_path_changing())
781 track->set_active_path(path);
782 if(track->is_path_changing())
790 if(i==clear_blocks_end)
792 if(i==cur_blocks_end && !(*i)->get_sensor_id())
797 float Train::get_reserved_distance_until(const Block *until_block, bool back) const
802 Vehicle &veh = *(reverse!=back ? vehicles.back() : vehicles.front());
803 const VehicleType &vtype = veh.get_type();
805 TrackIter track(veh.get_track(), veh.get_entry());
806 if(!track) // XXX Probably unnecessary
809 BlockList::const_iterator block = blocks.begin();
810 while(block!=clear_blocks_end && !(*block)->has_track(*track))
812 if(block==clear_blocks_end || &**block==until_block)
815 float result = veh.get_offset();
817 track = track.reverse();
819 result = track->get_type().get_path_length(track->get_active_path())-result;
820 result -= vtype.get_length()/2;
824 track = track.next();
828 if(!(*block)->has_track(*track))
832 if(block==blocks.begin())
839 if(block==clear_blocks_end)
843 if(&**block==until_block)
847 result += track->get_type().get_path_length(track->get_active_path());
853 void Train::release_blocks()
855 release_blocks(blocks.begin(), blocks.end());
858 void Train::release_blocks(BlockList::iterator begin, BlockList::iterator end)
862 if(begin==cur_blocks_end)
863 cur_blocks_end = end;
864 if(begin==clear_blocks_end)
865 clear_blocks_end = end;
867 Block &block = **begin;
868 blocks.erase(begin++);
873 void Train::reverse_blocks(BlockList &blks) const
876 for(BlockList::iterator i=blks.begin(); i!=blks.end(); ++i)
881 Train::Loader::Loader(Train &t):
882 DataFile::ObjectLoader<Train>(t),
886 add("block", &Loader::block);
887 add("block_hint", &Loader::block_hint);
888 add("name", &Loader::name);
889 add("quantized_speed", &Loader::quantized_speed);
890 add("router", &Loader::router);
891 add("timetable", &Loader::timetable);
892 add("vehicle", &Loader::vehicle);
895 void Train::Loader::finish()
897 if(!obj.blocks.empty())
899 TrackIter track = obj.blocks.front().track_iter();
900 float offset = 2*obj.layout.get_catalogue().get_scale();
901 obj.vehicles.back()->place(*track, track.entry(), offset, Vehicle::BACK_BUFFER);
905 void Train::Loader::block(unsigned id)
913 blk = &obj.layout.get_block(id);
915 catch(const key_error &)
917 blocks_valid = false;
923 entry = blk->get_endpoint_by_link(*prev_block);
927 obj.blocks.push_back(BlockIter(blk, entry));
930 if(blk->get_sensor_id())
931 obj.layout.get_driver().set_sensor(blk->get_sensor_id(), true);
936 void Train::Loader::block_hint(unsigned id)
940 prev_block = &obj.layout.get_block(id);
942 catch(const key_error &)
944 blocks_valid = false;
948 void Train::Loader::name(const string &n)
953 void Train::Loader::quantized_speed()
955 if(obj.speed_quantizer)
956 load_sub(*obj.speed_quantizer);
959 void Train::Loader::router()
961 TrainRouter *rtr = new TrainRouter(obj);
965 void Train::Loader::timetable()
967 Timetable *ttbl = new Timetable(obj);
971 void Train::Loader::vehicle(ArticleNumber art_nr)
973 const VehicleType &vtype = obj.layout.get_catalogue().get_vehicle(art_nr);
974 Vehicle *veh = new Vehicle(obj.layout, vtype);
975 obj.vehicles.back()->attach_back(*veh);
976 obj.vehicles.push_back(veh);