using System;
using System.Collections.Generic;
using HeuristicLab.Algorithms.GraphRouting.Interfaces;
using HeuristicLab.Problems.RoutePlanning.Interfaces;
namespace HeuristicLab.Algorithms.GraphRouting {
  public class BidrectionalDijkstraAlgorithmV2 : IRouter {
    private IGraph graph;

    private HashSet<long> visitedNodesForward;
    private HashSet<long> visitedNodesBackward;
    private HashSet<long> unvisitedNodesForward;
    private HashSet<long> unvisitedNodesBackward;
    private Dictionary<long, float> distancesForward;
    private Dictionary<long, long> predecessorsForward;
    private Dictionary<long, float> distancesBackward;
    private Dictionary<long, long> predecessorsBackward;

    public BidrectionalDijkstraAlgorithmV2(IGraph graph) {
      this.graph = graph;
    }

    #region IRouter Members

    public long[] Calculate(long sourceNodeId, long targetNodeId) {
      visitedNodesForward = new HashSet<long>();
      visitedNodesBackward = new HashSet<long>();

      unvisitedNodesForward = new HashSet<long>();
      unvisitedNodesBackward = new HashSet<long>();

      distancesForward = new Dictionary<long, float>();
      predecessorsForward = new Dictionary<long, long>();

      distancesBackward = new Dictionary<long, float>();
      predecessorsBackward = new Dictionary<long, long>();

      List<long> resultRouteForward = new List<long>();
      List<long> resultRouteBackward = new List<long>();

      if (sourceNodeId == targetNodeId) {
        resultRouteForward.Add(sourceNodeId);
        return resultRouteForward.ToArray();
      }

      long currentNodeForward = sourceNodeId;
      distancesForward.Add(currentNodeForward, 0);
      unvisitedNodesForward.Add(currentNodeForward);

      long currentNodeBackward = targetNodeId;
      distancesBackward.Add(currentNodeBackward, 0);
      unvisitedNodesBackward.Add(currentNodeBackward);

      while (unvisitedNodesForward.Count > 0 && unvisitedNodesBackward.Count > 0) {
        currentNodeForward = GetMinimumDistance(distancesForward, unvisitedNodesForward);
        visitedNodesForward.Add(currentNodeForward);
        unvisitedNodesForward.Remove(currentNodeForward);

        currentNodeBackward = GetMinimumDistance(distancesBackward, unvisitedNodesBackward);
        visitedNodesBackward.Add(currentNodeBackward);
        unvisitedNodesBackward.Remove(currentNodeBackward);

        if (visitedNodesForward.Contains(currentNodeBackward)) {
          return GetRoute(currentNodeBackward).ToArray();
        }

        if (visitedNodesBackward.Contains(currentNodeForward)) {
          return GetRoute(currentNodeForward).ToArray();
        }

        FindMinimalWeightForward(currentNodeForward);
        FindMinimalWeightBackward(currentNodeBackward);
      }

      throw new Exception(string.Format("No route found from {0} to {1}", sourceNodeId, targetNodeId));
    }

    #endregion

    private long GetMinimumDistance(Dictionary<long, float> distances, HashSet<long> nodeIds) {
      long minimum = -1;
      foreach (long id in nodeIds) {
        if (minimum == -1) minimum = id;
        else {
          if (GetDistance(distances, id) < GetDistance(distances, minimum)) {
            minimum = id;
          }
        }
      }
      return minimum;
    }

    private float GetDistance(Dictionary<long, float> distances, long id) {
      float d;
      if (distances.TryGetValue(id, out d)) {
        return d;
      } else { return float.MaxValue; }
    }

    private void FindMinimalWeightForward(long nodeId) {
      Dictionary<long, float> currentNeighbors = graph.GetNeighborsWithWeight(nodeId);
      foreach (KeyValuePair<long, float> neighbor in currentNeighbors) {
        // update relax
        float totalWeight = GetDistance(distancesForward, nodeId) + neighbor.Value;
        if (GetDistance(distancesForward, neighbor.Key) > totalWeight) {
          distancesForward[neighbor.Key] = totalWeight;
          predecessorsForward[neighbor.Key] = nodeId;
          unvisitedNodesForward.Add(neighbor.Key);
        }
      }
    }

    private void FindMinimalWeightBackward(long nodeId) {
      Dictionary<long, float> currentNeighbors = graph.GetNeighborsWithWeightReversed(nodeId);
      foreach (KeyValuePair<long, float> neighbor in currentNeighbors) {
        // update relax
        float totalWeight = GetDistance(distancesBackward, nodeId) + neighbor.Value;
        if (GetDistance(distancesBackward, neighbor.Key) > totalWeight) {
          distancesBackward[neighbor.Key] = totalWeight;
          predecessorsBackward[neighbor.Key] = nodeId;
          unvisitedNodesBackward.Add(neighbor.Key);
        }
      }
    }

    private List<long> GetRoute(long nodeId) {
      List<long> route = new List<long>();
      long current = nodeId;
      long next;

      if (!predecessorsForward.TryGetValue(current, out next)) {
        return null;
      }
      route.Add(current);
      while (predecessorsForward.TryGetValue(current, out next)) {
        current = predecessorsForward[current];
        route.Add(current);
      }
      route.Reverse();

      current = nodeId;
      while (predecessorsBackward.TryGetValue(current, out next)) {
        current = predecessorsBackward[current];
        route.Add(current);
      }

      return route;
    }
  }
}