/*
 * Copyright (c) 2005-2010 KOM – Multimedia Communications Lab
 *
 * This file is part of Simonstrator.KOM.
 * 
 * Simonstrator.KOM is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * any later version.
 * 
 * PeerfactSim.KOM is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with PeerfactSim.KOM.  If not, see <http://www.gnu.org/licenses/>.
 *
 */


package de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.paths;

import java.util.ArrayList;
import java.util.Comparator;
import java.util.List;
import java.util.PriorityQueue;
import java.util.Queue;

import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.RoadNetwork;
import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.RoadNetworkEdge;
import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.RoadNetworkRoute;

/**
 * @author Tobias Meuser (tobias.meuser@kom.tu-darmstadt.de)
 * @version 1.0 at 28.11.2017
 *
 */
public class SimplePathDetectionAlgorithm implements PathDetectionAlgorithm {
	private static final double ABORT_THRESHOLD = 0.0001d;

	@Override
	public List<ProbabilisticRoadNetworkRoute> findAllRoutes(RoadNetwork pNetwork, RoadNetworkEdge pCurrentPosition,
			RoadNetworkEdge pDestination) {
	    List<ProbabilisticRoadNetworkRoute> routes = new ArrayList<>();
	    
		Queue<RoadNetworkPath> currentPaths = new PriorityQueue<>(new RoadNetworkPathComperator());

		currentPaths.add(new RoadNetworkPath(null, pCurrentPosition, 1, 0));

		while (!currentPaths.isEmpty()) {
			RoadNetworkPath roadNetworkPath = currentPaths.poll();

			if (roadNetworkPath.getProbability() < ABORT_THRESHOLD) {
				break;
			}

			List<RoadNetworkEdge> accessibleEdges = roadNetworkPath.getEdge().getAccessibleEdges();

			for (RoadNetworkEdge accessibleEdge : accessibleEdges) {
				if (accessibleEdge.isUsable()) {
					double travelTime = VehiclePathTrackerFactory.getVehiclePathTracker()
							.getTravelTime(roadNetworkPath.getEdge(), accessibleEdge);
					double probability = VehiclePathTrackerFactory.getVehiclePathTracker()
							.getTransitionProbability(roadNetworkPath.getEdge(), accessibleEdge);

					RoadNetworkPath newPath = roadNetworkPath.createPath(accessibleEdge, probability, travelTime);

					if (newPath.getEdge() == pDestination) {
						double totalTravelTime = newPath.getTravelTime();
						double totalProbability = newPath.getProbability();
						RoadNetworkRoute route = newPath.getRoute();

						double temporalRelevance = 1; // TODO: To be done!

						routes.add(new ProbabilisticRoadNetworkRoute(totalProbability, route));
					} else {
						currentPaths.add(newPath);
					}
				}
			}
		}

		double sum = 0;
		for (ProbabilisticRoadNetworkRoute probabilisticRoadNetworkRoute : routes) {
			sum += probabilisticRoadNetworkRoute.getProbability();
		}

		return routes;
	}

	private class RoadNetworkPathComperator implements Comparator<RoadNetworkPath> {

		public RoadNetworkPathComperator() {
		}

		@Override
		public int compare(RoadNetworkPath pPath0, RoadNetworkPath pPath1) {
			return -Double.compare(pPath0.getProbability(), pPath1.getProbability());
		}

	}

	private class RoadNetworkPath {
		private RoadNetworkPath previous;
		private double probability;
		private double travelTime;
		private RoadNetworkEdge edge;

		public RoadNetworkPath(RoadNetworkPath pPrevious, RoadNetworkEdge pEdge, double pProbability,
				double pTravelTime) {
			edge = pEdge;
			probability = pProbability;
			travelTime = pTravelTime;
			previous = pPrevious;
		}

		public RoadNetworkEdge getEdge() {
			return edge;
		}

		public double getProbability() {
			return probability;
		}

		public double getTravelTime() {
			return travelTime;
		}

		public RoadNetworkPath createPath(RoadNetworkEdge pCurrentEdge, double pProbability, double pTravelTime) {
			return new RoadNetworkPath(this, pCurrentEdge, getProbability() * pProbability,
					getTravelTime() + pTravelTime);
		}

		public RoadNetworkRoute getRoute() {
			List<RoadNetworkEdge> edges = new ArrayList<>();
			getRoute(edges);
			return new RoadNetworkRoute(edges);
		}

		private void getRoute(List<RoadNetworkEdge> pEdges) {
			if (previous != null) {
				previous.getRoute(pEdges);
			}
			pEdges.add(edge);
		}
	}
}