/*
 * Copyright (c) 2005-2010 KOM – Multimedia Communications Lab
 *
 * This file is part of PeerfactSim.KOM.
 *
 * PeerfactSim.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.tud.kom.p2psim.impl.vehicular.decision;

import java.util.ArrayList;
import java.util.Collections;
import java.util.HashMap;
import java.util.List;
import java.util.Map;

import de.tudarmstadt.maki.simonstrator.api.Host;
import de.tudarmstadt.maki.simonstrator.api.Time;
import de.tudarmstadt.maki.simonstrator.api.component.sensor.environment.data.properties.RoadProperty;
import de.tudarmstadt.maki.simonstrator.api.component.vehicular.information.AvailableInformationAttributes;
import de.tudarmstadt.maki.simonstrator.api.component.vehicular.information.RoadInformation;
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;
import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.paths.VehiclePathTracker;
import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.paths.VehiclePathTrackerFactory;
import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.routing.DijkstraAlgorithm;

public class CombinedInformationVehicleDecisionComponent extends AbstractVehicleDecisionComponent {
    private static final boolean ENABLE_CACHING = true;

    private static Map<RoadNetworkEdge, CostGraph> _unknownCostGraphs = new HashMap<>();
    private static Map<RoadProperty, Map<RoadNetworkEdge, CostGraph>> _knownCostGraphs = new HashMap<>();

    public CombinedInformationVehicleDecisionComponent(Host pHost) {
        super(pHost);
    }

    @Override
    public BenefitBasedRoute getOptimalRoute(RoadNetworkRoute pInvestigatedRoute, List<RoadInformation> knownInformation) {
        if (!ENABLE_CACHING) {
            _unknownCostGraphs.clear();
            _knownCostGraphs.clear();
        }

        if (getVehicleInformation().isValid() && pInvestigatedRoute.getRoute().size() > 1) {
            if (knownInformation != null && knownInformation.size() > 1) {
                for (RoadInformation roadInformation : knownInformation) {
                    System.out.println("Known information for road " + roadInformation.getEdge());
                }
                throw new AssertionError("Only one information currently supported!");
            }

            DijkstraAlgorithm dijkstraAlgorithm = new DijkstraAlgorithm();

            List<RoadNetworkRoute> known = new ArrayList<>();
            known.add(pInvestigatedRoute);

            RoadNetworkRoute optimalRoute = dijkstraAlgorithm.findRoute(RoadNetwork.CURRENT_ROAD_NETWORK, pInvestigatedRoute.getStart(), pInvestigatedRoute.getDestination(), known, Collections.emptyList(), knownInformation);
            if (optimalRoute != null) {
                if (pInvestigatedRoute.equals(optimalRoute)) {
                    return new BenefitBasedRoute(0, pInvestigatedRoute);
                }

                double oldCostsActive = getRouteCosts(pInvestigatedRoute, knownInformation);
                double oldCostsInactive = getRouteCosts(pInvestigatedRoute, Collections.emptyList());

                double newCostsActive = getRouteCosts(optimalRoute, knownInformation);

                double activeProbability = 0;
                if (knownInformation != null && knownInformation.size() > 0) {
                    if (!knownInformation.get(0).getValue()
                            .equals(knownInformation.get(0).getValue().getDefaultProperty())) {
                        double traveltime = getRouteDuration(pInvestigatedRoute, knownInformation.get(0).getEdge());
                        long expected = (long) knownInformation.get(0)
                                .getAttribute(AvailableInformationAttributes.EXPECTED_DURATION) / Time.SECOND;
                        if (!Double.isNaN(traveltime)) {
                            activeProbability = Math.pow(1 - 1 / (double) (expected), traveltime);
                        }
                    } else {
                        activeProbability = 0;
                    }
                }

                // Check if detour can immediately be triggered due to high probability of success.
                double oldCosts = oldCostsActive * activeProbability + oldCostsInactive * (1 - activeProbability);

                if (oldCosts > newCostsActive) {
                    known.clear();
                    known.add(pInvestigatedRoute);
                    known.add(optimalRoute);
                    RoadNetworkRoute secondOptimalRoute = dijkstraAlgorithm.findRoute(RoadNetwork.CURRENT_ROAD_NETWORK, pInvestigatedRoute.getStart(), pInvestigatedRoute.getDestination(), known, Collections.emptyList(), knownInformation);
                    double secondOptimalCosts = 0;
                    if (secondOptimalRoute != null) {
                        secondOptimalCosts = getRouteCosts(secondOptimalRoute, knownInformation);
                    }
                    if (secondOptimalRoute == null || secondOptimalCosts * activeProbability + oldCostsInactive * (1 - activeProbability) > newCostsActive) {
                        return new BenefitBasedRoute(oldCosts - newCostsActive, optimalRoute);
                    }
                }

                // Check if detour can immediately be discarded due to high probability of failure.
                if (oldCosts < newCostsActive && optimalRoute.getRoute().size() > 1) {
                    known.clear();
                    known.add(pInvestigatedRoute);
                    known.add(optimalRoute);
                    RoadNetworkRoute secondOptimalRoute = dijkstraAlgorithm.findRoute(RoadNetwork.CURRENT_ROAD_NETWORK, optimalRoute.getRoute().get(1), pInvestigatedRoute.getDestination(), known, Collections.emptyList(), Collections.emptyList());
                    secondOptimalRoute.getRoute().add(0, optimalRoute.getStart());
                    if (secondOptimalRoute != null) {
                        double secondOptimalCosts = getRouteCosts(secondOptimalRoute, Collections.emptyList());
                        if (newCostsActive * activeProbability + secondOptimalCosts * (1 - activeProbability) > oldCosts || activeProbability == 0) {
                            return new BenefitBasedRoute(0, pInvestigatedRoute);
                        }
                    }
                }
            }

            CostGraph usedCostGraph = null;

            if (!_unknownCostGraphs.containsKey(pInvestigatedRoute.getDestination())) {
                CostGraph costGraph = new CostGraph(pInvestigatedRoute.getDestination());
                costGraph.createCostGraph();
                _unknownCostGraphs.put(pInvestigatedRoute.getDestination(), costGraph);
            }

            if (knownInformation != null && knownInformation.size() > 0) {
                if (!_knownCostGraphs.containsKey(knownInformation.get(0).getValue())) {
                    _knownCostGraphs.put(knownInformation.get(0).getValue(), new HashMap<>());
                }
                Map<RoadNetworkEdge, CostGraph> costGraphs = _knownCostGraphs.get(knownInformation.get(0).getValue());
                if (!costGraphs.containsKey(pInvestigatedRoute.getDestination())) {
                    CostGraph costGraph = new CostGraph(pInvestigatedRoute.getDestination(), knownInformation.get(0), _unknownCostGraphs.get(pInvestigatedRoute.getDestination()));
                    costGraph.setCostLimit(getRouteCosts(pInvestigatedRoute, knownInformation));
                    costGraph.createCostGraph();
                    costGraphs.put(pInvestigatedRoute.getDestination(), costGraph);
                }
            }

            if (knownInformation != null && knownInformation.size() > 0 && !knownInformation.get(0).getValue().equals(knownInformation.get(0).getValue().getDefaultProperty())) {
                Map<RoadNetworkEdge, CostGraph> costGraphs = _knownCostGraphs.get(knownInformation.get(0).getValue());
                usedCostGraph = costGraphs.get(pInvestigatedRoute.getDestination());
            } else {
                usedCostGraph = _unknownCostGraphs.get(pInvestigatedRoute.getDestination());
            }

            double minCosts = Double.MAX_VALUE;
            optimalRoute = null;
            for (RoadNetworkEdge edge : pInvestigatedRoute.getStart().getAccessibleEdges()) {
                if (usedCostGraph.getCosts(edge) < minCosts) {
                    RoadNetworkRoute roadNetworkRoute = usedCostGraph.getOptimalRoute(edge);
                    minCosts = usedCostGraph.getCosts(edge);
                    optimalRoute = roadNetworkRoute;
                }

            }

            if (optimalRoute != null) {
                optimalRoute.getRoute().add(0, pInvestigatedRoute.getStart());

                double oldCosts = getRouteCosts(pInvestigatedRoute, knownInformation);
                double newCosts = getRouteCosts(optimalRoute, knownInformation);

                if (oldCosts > newCosts) {
                    return new BenefitBasedRoute(oldCosts - newCosts, optimalRoute);
                }
            }
            return new BenefitBasedRoute(0, pInvestigatedRoute);
        }
        return new BenefitBasedRoute(0, pInvestigatedRoute);
    }

    private double getRouteCosts(RoadNetworkRoute pInvestigatedRoute, List<RoadInformation> knownInformation) {
        double oldCosts = 0;
        for (RoadNetworkEdge edge : pInvestigatedRoute.getRoute()) {
            oldCosts += edge.calculateStandardEdgeCosts();
            if (knownInformation != null) {
                for (RoadInformation roadInformation : knownInformation) {
                    double cost = edge.calculateAdditionalCostIfKnown(roadInformation.getValue());
                    oldCosts += cost;
                }
            }
        }
        return oldCosts;
    }

    private double getRouteDuration(RoadNetworkRoute pInvestigatedRoute, RoadNetworkEdge pEvent) {
        double duration = 0;

        if (!pInvestigatedRoute.containsEdge(pEvent)) {
            return Double.NaN;
        }

        for (int i = 1; i < pInvestigatedRoute.getRoute().size(); i++) {
            RoadNetworkEdge edge = pInvestigatedRoute.getRoute().get(i);
            if (edge.equals(pEvent)) {
                break;
            }
            duration += VehiclePathTrackerFactory.getVehiclePathTracker().getTravelTime(edge);
        }
        return duration;
    }

    private class CostGraph {
        private Map<RoadNetworkEdge, Double> _costGraph = new HashMap<>();
        private Map<RoadNetworkEdge, RoadNetworkEdge> _nextEdgeGraph = new HashMap<>();

        // For event active
        private RoadInformation _information;
        private CostGraph _costGraphWithoutEvent;

        private RoadNetworkEdge _destination;
        private double _maxCosts = Double.POSITIVE_INFINITY;

        // If event inactive
        public CostGraph(RoadNetworkEdge pDestination) {
            _costGraph.put(pDestination, 0d);
            _destination = pDestination;
        }

        public void setCostLimit(double pRouteCosts) {
            _maxCosts = pRouteCosts;
        }

        // If event active
        public CostGraph(RoadNetworkEdge pDestination, RoadInformation pInformation, CostGraph pCostGraphWithoutEvent) {
            this(pDestination);
            _information = pInformation;
            _costGraphWithoutEvent = pCostGraphWithoutEvent;
        }

        public RoadNetworkRoute getOptimalRoute(RoadNetworkEdge pEdge) {
            List<RoadNetworkEdge> edges = new ArrayList<>();
            RoadNetworkEdge current = pEdge;
            edges.add(current);
            while (_nextEdgeGraph.containsKey(current)) {
                RoadNetworkEdge next = _nextEdgeGraph.get(current);
                edges.add(next);
                current = next;

                if (next.equals(_destination) || !_costGraph.containsKey(current) || !_costGraph.containsKey(next)
                        || _costGraph.get(current) < _costGraph.get(next)) {
                    break;
                }
            }
            if (edges.get(edges.size() - 1).equals(_destination)) {
                return new RoadNetworkRoute(edges);
            }
            return null;
        }

        public void createCostGraph() {
            createCostGraph(25);
        }

        public void createCostGraph(int pMaxDepth) {
            RoadProperty property = null;
            if (_information != null) {
                property = _information.getValue();
            }

            if (_information != null) {
                VehiclePathTracker tracker = VehiclePathTrackerFactory.getVehiclePathTracker();

                long duration = _information.getAttribute(AvailableInformationAttributes.EXPECTED_DURATION);
                double existProbability = 1 - 1 / (double) (duration / Time.SECOND);

                for (int i = 0; i < pMaxDepth; i++) {
                    List<RoadNetworkEdge> labeledEdges = new ArrayList<>(_costGraph.keySet());

                    int updated = 0;
                    for (RoadNetworkEdge roadNetworkEdge : labeledEdges) {
                        List<RoadNetworkEdge> incomingEdges = roadNetworkEdge.getIncomingEdges();

                        for (RoadNetworkEdge incomingEdge : incomingEdges) {
                            if (isCyclic(incomingEdge, roadNetworkEdge)) {
                                continue;
                            }

                            double edgeCost = incomingEdge.calculateEdgeCostIfKnown(property);
                            if (edgeCost > duration / Time.SECOND) {
                                edgeCost = duration / (double) Time.SECOND;
                            }
                            double eventActiveCost = _costGraph.get(roadNetworkEdge);
                            double eventInactiveCost = _costGraphWithoutEvent.getCosts(roadNetworkEdge);

                            double stillExistProbability = Math.pow(existProbability,
                                    tracker.getTravelTime(incomingEdge, roadNetworkEdge));

                            double totalCosts = edgeCost + eventInactiveCost * (1 - stillExistProbability)
                                    + eventActiveCost * stillExistProbability;
                            if ((!_costGraph.containsKey(incomingEdge) || totalCosts < _costGraph.get(incomingEdge)) && totalCosts <= _maxCosts) {
                                _costGraph.put(incomingEdge, totalCosts);
                                _nextEdgeGraph.put(incomingEdge, roadNetworkEdge);
                                updated++;
                            }
                        }
                    }

                    if (updated == 0) {
                        break;
                    }
                }
            } else {
                for (int i = 0; i < pMaxDepth; i++) {
                    List<RoadNetworkEdge> labeledEdges = new ArrayList<>(_costGraph.keySet());

                    int updated = 0;
                    for (RoadNetworkEdge roadNetworkEdge : labeledEdges) {
                        List<RoadNetworkEdge> incomingEdges = roadNetworkEdge.getIncomingEdges();
                        for (RoadNetworkEdge incomingEdge : incomingEdges) {
                            double edgeCost = incomingEdge.calculateStandardEdgeCosts();
                            double totalCosts = _costGraph.get(roadNetworkEdge) + edgeCost;
                            if ((!_costGraph.containsKey(incomingEdge) || totalCosts < _costGraph.get(incomingEdge)) && totalCosts <= _maxCosts) {
                                _costGraph.put(incomingEdge, totalCosts);
                                _nextEdgeGraph.put(incomingEdge, roadNetworkEdge);
                                updated++;
                            }
                        }
                    }

                    if (updated == 0) {
                        break;
                    }
                }
            }
        }

        private boolean isCyclic(RoadNetworkEdge pIncomingEdge, RoadNetworkEdge pRoadNetworkEdge) {
            RoadNetworkEdge current = pRoadNetworkEdge;
            while (_nextEdgeGraph.containsKey(current)) {
                if (pIncomingEdge.equals(current)) {
                    return true;
                }
                if (current == _destination) {
                    break;
                }
                current = _nextEdgeGraph.get(current);
            }
            return false;
        }

        public double getCosts(RoadNetworkEdge pEdge) {
            if (_costGraph.containsKey(pEdge)) {
                return _costGraph.get(pEdge);
            } else {
                return Double.MAX_VALUE;
            }
        }
    }
}
