package student;

import com.google.common.collect.Lists;
import cz.cvut.atg.zui.astar.AbstractOpenList;
import cz.cvut.atg.zui.astar.PlannerInterface;
import cz.cvut.atg.zui.astar.RoadGraph;
import eu.superhub.wp5.planner.planningstructure.GraphEdge;
import eu.superhub.wp5.planner.planningstructure.GraphNode;

import java.util.*;

import static cz.cvut.atg.zui.astar.Utils.distanceInKM;

class OpenList extends AbstractOpenList<GraphNode> {
    private PriorityQueue<GraphNode> queue;

    OpenList(GraphNode finish, Map<Long, Double> distances) {
        queue = new PriorityQueue<>(1000, Comparator.comparingDouble(a -> (distanceInKM(a, finish) / 120.00) + distances.get(a.getId())));
    }

    @Override
    protected boolean addItem(GraphNode item) {
        queue.add(item);
        return true;
    }

    public GraphNode pop() {
        return queue.remove();
    }

    public boolean empty () {
        return queue.isEmpty();
    }

    public void removeNode(GraphNode node){
        queue.remove(node);
    }

    public void clear(){
        queue.clear();
    }
}


public class Planner implements PlannerInterface {
    private OpenList openList;
    public Planner() {}

    @Override
    public List<GraphEdge> plan(RoadGraph graph, GraphNode origin, GraphNode destination) {
        Map<Long, GraphEdge> inEdge = new HashMap<>();
        Map<Long, Double> distanceFromOrigin = new HashMap<>();
        Map<Long, Boolean> closed = new HashMap<>();
        ArrayList<GraphEdge> path = new ArrayList<>();
        openList = new OpenList(destination, distanceFromOrigin);

        distanceFromOrigin.put(origin.getId(), 0.0);
        openList.add(origin);
        while(!openList.empty()){
            final GraphNode curr = openList.pop();
            if(curr == destination){
                GraphNode tmp =  curr;
                while(tmp != origin){
                    path.add(inEdge.get(tmp.getId()));
                    tmp = graph.getNodeByNodeId(inEdge.get(tmp.getId()).getFromNodeId());
                }
                break;
            }
            if(graph.getNodeOutcomingEdges(curr.getId()) == null){
                continue;
            }
            for (GraphEdge edge : graph.getNodeOutcomingEdges(curr.getId())) {
                double node_distance = (edge.getLengthInMetres() / 1000) / edge.getAllowedMaxSpeedInKmph();
                double tentative_g = distanceFromOrigin.get(curr.getId()) + node_distance;
                GraphNode nextnode = graph.getNodeByNodeId(edge.getToNodeId());
                if (!distanceFromOrigin.containsKey(nextnode.getId()) && !closed.containsKey(nextnode.getId())) {
                    distanceFromOrigin.put(nextnode.getId(), tentative_g);
                    inEdge.put(edge.getToNodeId(), edge);
                    openList.add(nextnode);
                } else if (tentative_g < distanceFromOrigin.get(nextnode.getId()) && !closed.containsKey(nextnode.getId())) {
                    openList.removeNode(nextnode);
                    distanceFromOrigin.put(nextnode.getId(), tentative_g);
                    inEdge.put(nextnode.getId(), edge);
                    openList.add(nextnode);
                }
            }
            closed.put(curr.getId(), true);
        }
        if(path.isEmpty()){
            return null;
        }
        return Lists.reverse(path);
    }

    @Override
    public AbstractOpenList getOpenList() {
        return openList;
    }
}