+#include "layout.h"
+#include "route.h"
+#include "train.h"
+#include "trainrouteplanner.h"
+#include "trainrouter.h"
+#include "vehicle.h"
+
+using namespace std;
+using namespace Msp;
+
+namespace R2C2 {
+
+TrainRoutePlanner::TrainRoutePlanner(Layout &layout)
+{
+ const map<unsigned, Train *> &trains = layout.get_trains();
+ for(map<unsigned, Train *>::const_iterator i=trains.begin(); i!=trains.end(); ++i)
+ {
+ TrainRoutingInfo info(*i->second);
+ if(info.router && info.router->has_destination())
+ routed_trains.push_back(info);
+ }
+
+ steps.push_back(RoutingStep());
+ RoutingStep &start = steps.back();
+ for(vector<TrainRoutingInfo>::iterator i=routed_trains.begin(); i!=routed_trains.end(); ++i)
+ start.trains.push_back(TrainRoutingState(*i));
+}
+
+void TrainRoutePlanner::plan()
+{
+ RoutingStep *goal = 0;
+ for(list<RoutingStep>::iterator i=steps.begin(); i!=steps.end(); ++i)
+ {
+ if(i->is_goal())
+ {
+ goal = &*i;
+ break;
+ }
+
+ if(update_states(*i))
+ {
+ int next_train = find_next_train(*i);
+ if(next_train>=0)
+ add_steps(*i, next_train);
+ }
+ }
+
+ if(goal)
+ create_routes(*goal);
+}
+
+bool TrainRoutePlanner::update_states(RoutingStep &step)
+{
+ RoutingStep next(&step);
+ bool changes = false;
+ for(vector<TrainRoutingState>::iterator i=next.trains.begin(); i!=next.trains.end(); ++i)
+ {
+ TrainState old_state = i->state;
+ if(i->state==BLOCKED)
+ i->state = MOVING;
+
+ TrackIter next_track = i->track.next(i->path);
+ if(!next_track)
+ return false;
+
+ for(vector<TrainRoutingState>::iterator j=next.trains.begin(); j!=next.trains.end(); ++j)
+ if(j!=i)
+ {
+ if(j->track.track()==next_track.track())
+ {
+ unsigned other_exit = j->track.reverse(j->path).entry();
+ if(next_track.entry()==other_exit)
+ return false;
+ }
+ else if(!j->is_occupied(*next_track))
+ continue;
+
+ i->state = BLOCKED;
+ }
+
+ if(i->state!=old_state)
+ changes = true;
+ }
+
+ if(changes)
+ {
+ list<RoutingStep>::iterator i;
+ for(i=steps.begin(); (i!=steps.end() && !(next<*i)); ++i) ;
+ steps.insert(i, next);
+ }
+
+ return !changes;
+}
+
+int TrainRoutePlanner::find_next_train(RoutingStep &step)
+{
+ Time::TimeDelta min_dt;
+ int next_train = -1;
+ for(unsigned i=0; i<step.trains.size(); ++i)
+ if(step.trains[i].state==MOVING)
+ {
+ Time::TimeDelta dt = step.trains[i].get_time_to_next_track();
+ if(dt<min_dt || next_train<0)
+ {
+ min_dt = dt;
+ next_train = i;
+ }
+ }
+
+ return next_train;
+}
+
+void TrainRoutePlanner::add_steps(RoutingStep &step, unsigned train_index)
+{
+ TrainRoutingState &train = step.trains[train_index];
+ Time::TimeDelta dt = train.get_time_to_next_track();
+ TrackIter next_track = train.track.next(train.path);
+
+ list<RoutingStep> new_steps;
+
+ RoutingStep next(&step);
+ next.advance(dt);
+ TrainRouter &router = *train.info->router;
+ if(router.is_destination(*train.track) && !router.is_destination(*next_track))
+ {
+ next.trains[train_index].state = ARRIVED;
+ new_steps.push_back(next);
+ }
+ else
+ {
+ next.trains[train_index].advance_track(0);
+
+ const TrackType::Endpoint &next_entry_ep = next_track.endpoint();
+ for(unsigned i=0; next_entry_ep.paths>>i; ++i)
+ if(next_entry_ep.has_path(i))
+ {
+ next.trains[train_index].path = i;
+ new_steps.push_back(next);
+ }
+
+ if(next_entry_ep.paths!=next_track->get_type().get_paths())
+ {
+ RoutingStep wait(&step);
+ wait.advance(dt);
+ wait.trains[train_index].state = WAITING;
+ new_steps.push_back(wait);
+ }
+ }
+
+ new_steps.sort();
+ steps.merge(new_steps);
+}
+
+void TrainRoutePlanner::create_routes(RoutingStep &goal)
+{
+ for(vector<TrainRoutingInfo>::iterator i=routed_trains.begin(); i!=routed_trains.end(); ++i)
+ {
+ i->route = new Route(i->train->get_layout());
+ i->route->set_name("Router");
+ }
+
+ for(RoutingStep *i=&goal; i; i=i->prev)
+ {
+ for(vector<TrainRoutingState>::iterator j=i->trains.begin(); j!=i->trains.end(); ++j)
+ {
+ if(j->state==WAITING || j->state==BLOCKED)
+ j->info->waits.push_front(&*j);
+ j->info->route->add_track(*j->track);
+ }
+ }
+
+ for(vector<TrainRoutingInfo>::iterator i=routed_trains.begin(); i!=routed_trains.end(); ++i)
+ {
+ i->router->set_route(i->route);
+ TrainRoutingState *current_wait = 0;
+ for(list<TrainRoutingState *>::iterator j=i->waits.begin(); j!=i->waits.end(); ++j)
+ if(!current_wait || (*j)->track.track()!=current_wait->track.track())
+ {
+ Block &block = (*j)->track.next()->get_block();
+ i->router->add_wait(block, 0);
+ current_wait = *j;
+ }
+ }
+}
+
+
+TrainRoutePlanner::TrainRoutingInfo::TrainRoutingInfo(Train &t):
+ train(&t),
+ router(train->get_ai_of_type<TrainRouter>()),
+ route(0)
+{ }
+
+
+TrainRoutePlanner::OccupiedTrack::OccupiedTrack(Track &t, unsigned p, OccupiedTrack *n):
+ track(&t),
+ path_length(track->get_type().get_path_length(p)),
+ next(n),
+ n_tracks(next ? next->n_tracks+1 : 1),
+ refcount(1)
+{
+ if(next)
+ ++next->refcount;
+}
+
+TrainRoutePlanner::OccupiedTrack::OccupiedTrack(const OccupiedTrack &other):
+ track(other.track),
+ path_length(other.path_length),
+ next(other.next),
+ n_tracks(other.n_tracks),
+ refcount(1)
+{
+ if(next)
+ ++next->refcount;
+}
+
+TrainRoutePlanner::OccupiedTrack::~OccupiedTrack()
+{
+ if(next && !--next->refcount)
+ delete next;
+}
+
+
+TrainRoutePlanner::TrainRoutingState::TrainRoutingState(TrainRoutingInfo &inf):
+ info(&inf),
+ occupied_tracks(0),
+ state(MOVING)
+{
+ const Vehicle *veh = &info->train->get_vehicle(0);
+ track = TrackIter(veh->get_track(), veh->get_entry());
+ // TODO margins
+ offset = veh->get_offset()+veh->get_type().get_length()/2;
+ path = track->get_active_path();
+
+ float path_length = track->get_type().get_path_length(path);
+ while(offset>path_length)
+ {
+ offset -= path_length;
+ track = track.next();
+ path = track->get_active_path();
+ path_length = track->get_type().get_path_length(path);
+ }
+
+ while(Vehicle *next = veh->get_link(1))
+ veh = next;
+ back_offset = veh->get_offset()-veh->get_type().get_length()/2;
+
+ TrackIter iter(veh->get_track(), veh->get_entry());
+ while(back_offset<0)
+ {
+ TrackIter prev = iter.flip().reverse();
+ if(!prev)
+ break;
+ iter = prev;
+ back_offset += iter->get_type().get_path_length(iter->get_active_path());
+ }
+
+ while(1)
+ {
+ occupied_tracks = new OccupiedTrack(*iter, iter->get_active_path(), occupied_tracks);
+ if(iter.track()==track.track())
+ break;
+ iter = iter.next();
+ }
+}
+
+TrainRoutePlanner::TrainRoutingState::TrainRoutingState(const TrainRoutingState &other):
+ info(other.info),
+ track(other.track),
+ path(other.path),
+ occupied_tracks(other.occupied_tracks),
+ offset(other.offset),
+ back_offset(other.back_offset),
+ state(other.state)
+{
+ ++occupied_tracks->refcount;
+}
+
+TrainRoutePlanner::TrainRoutingState::~TrainRoutingState()
+{
+ if(!--occupied_tracks->refcount)
+ delete occupied_tracks;
+}
+
+Time::TimeDelta TrainRoutePlanner::TrainRoutingState::get_time_to_next_track() const
+{
+ // TODO Consider the speed of the train
+ return (track->get_type().get_path_length(path)-offset)*Time::sec;
+}
+
+bool TrainRoutePlanner::TrainRoutingState::is_occupied(Track &trk) const
+{
+ OccupiedTrack *occ = occupied_tracks;
+ for(unsigned n=occ->n_tracks; n>0; --n, occ=occ->next)
+ if(occ->track==&trk)
+ return true;
+ return false;
+}
+
+void TrainRoutePlanner::TrainRoutingState::advance(float distance)
+{
+ offset += distance;
+ back_offset += distance;
+
+ OccupiedTrack *last_occ = occupied_tracks;
+ for(unsigned n=occupied_tracks->n_tracks; n>1; --n)
+ last_occ = last_occ->next;
+
+ // XXX What if there's multiple tracks to remove?
+ if(back_offset>last_occ->path_length)
+ {
+ back_offset -= last_occ->path_length;
+ if(occupied_tracks->refcount>1)
+ {
+ --occupied_tracks->refcount;
+ occupied_tracks = new OccupiedTrack(*occupied_tracks);
+ }
+ --occupied_tracks->n_tracks;
+ }
+}
+
+void TrainRoutePlanner::TrainRoutingState::advance_track(unsigned next_path)
+{
+ float distance = occupied_tracks->path_length-offset;
+ track = track.next(path);
+ path = next_path;
+ occupied_tracks = new OccupiedTrack(*track, path, occupied_tracks);
+ advance(distance);
+ offset = 0;
+}
+
+
+TrainRoutePlanner::RoutingStep::RoutingStep():
+ prev(0)
+{ }
+
+TrainRoutePlanner::RoutingStep::RoutingStep(RoutingStep *p):
+ time(p->time),
+ trains(p->trains),
+ prev(p)
+{ }
+
+void TrainRoutePlanner::RoutingStep::advance(const Time::TimeDelta &dt)
+{
+ time += dt;
+ for(vector<TrainRoutingState>::iterator i=trains.begin(); i!=trains.end(); ++i)
+ if(i->state==MOVING)
+ {
+ float distance = dt/Time::sec;
+ i->advance(distance);
+ }
+}
+
+bool TrainRoutePlanner::RoutingStep::is_goal() const
+{
+ for(vector<TrainRoutingState>::const_iterator i=trains.begin(); i!=trains.end(); ++i)
+ if(i->state!=ARRIVED)
+ return false;
+ return true;
+}
+
+bool TrainRoutePlanner::RoutingStep::operator<(const RoutingStep &other) const
+{
+ return time<other.time;
+}
+
+} // namespace R2C2