/******************************************************************************
 * $Id$
 *
 * Project:  MapServer
 * Purpose:  Functions for basic undirected, weighted graph support.
 * Author:   Steve Lime and the MapServer team.
 *
 ******************************************************************************
 * Copyright (c) 1996-2005 Regents of the University of Minnesota.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a
 * copy of this software and associated documentation files (the "Software"),
 * to deal in the Software without restriction, including without limitation
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
 * and/or sell copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies of this Software or works derived from this Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
 * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
 * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
 * DEALINGS IN THE SOFTWARE.
 ****************************************************************************/

#include "mapserver.h"
#include "mapgraph.h"
#include  // for std::swap

graphObj *msCreateGraph(signed int numnodes)
{
  graphObj *graph=nullptr;

  if(numnodes head = (graphNodeObj **) calloc(numnodes, sizeof(graphNodeObj*));
  if(!graph->head) {
    free(graph);
    return nullptr;
  }
  graph->numnodes = numnodes;

  return graph;
}

void msFreeGraph(graphObj *graph)
{
  if(!graph) return;

  graphNodeObj *tmp=nullptr;
  
  for(int i=0; inumnodes; i++) {
    while(graph->head[i] != nullptr) {
      tmp = graph->head[i];
      graph->head[i] = graph->head[i]->next;
      free(tmp);
    }
  }

  free(graph->head);
  free(graph);
}

int msGraphAddEdge(graphObj *graph, int src, int dest, double weight)
{
  graphNodeObj *node=nullptr;

  if(!graph) return MS_FAILURE;

  // src -> dest
  node = (graphNodeObj *) malloc(sizeof(graphNodeObj));
  if(!node) return MS_FAILURE;

  node->dest = dest;
  node->weight = weight;
  node->next = graph->head[src];
  graph->head[src] = node;

  // dest -> src
  node = (graphNodeObj *) malloc(sizeof(graphNodeObj));
  if(!node) return MS_FAILURE;

  node->dest = src;
  node->weight = weight;
  node->next = graph->head[dest];
  graph->head[dest] = node;

  return MS_SUCCESS;
}

void msPrintGraph(graphObj *graph)
{
  int i;

  if(!graph) return;

  for(i=0; inumnodes; i++) {
    graphNodeObj *node = graph->head[i];
    if(node != nullptr) {
      do {
        msDebug("%d -> %d (%.6f)\t", i, node->dest, node->weight);
        node = node->next;
      } while (node != nullptr);
      msDebug("\n");
    }
  }
}

/*
** Derived from an number web resources including:
**
**   https://www.geeksforgeeks.org/dijkstras-algorithm-for-adjacency-list-representation-greedy-algo-8/
**   https://youtube.com/watch?v=pSqmAO-m7Lk
**   https://en.wikipedia.org/wiki/Dijkstra%27s_algorithm
**
** Much of the bulk here is for min-heap (key,value) management.
*/

typedef struct {
  int idx;
  double dist;
} minHeapNodeObj;

typedef struct {
  int size;
  int capacity;
  int *pos;
  minHeapNodeObj **nodes;
} minHeapObj;

static minHeapNodeObj *newMinHeapNode(int idx, double dist)
{
  minHeapNodeObj *node = (minHeapNodeObj *) malloc(sizeof(minHeapNodeObj));
  if(!node) return nullptr;
  node->idx = idx;
  node->dist = dist;
  return node;
}

static void freeMinHeap(minHeapObj *minHeap)
{
  if(!minHeap) return;

  free(minHeap->pos);
  for(int i=0; isize; i++) {
    free(minHeap->nodes[i]);
  }
  free(minHeap->nodes);
  free(minHeap);
}

static minHeapObj *createMinHeap(signed int capacity)
{
  minHeapObj *minHeap = (minHeapObj *) malloc(sizeof(minHeapObj));
  if(!minHeap) return nullptr;

  minHeap->pos = (int *) malloc(capacity * sizeof(int));
  if(!minHeap->pos) {
    free(minHeap);
    return nullptr;
  }
  minHeap->size = 0;
  minHeap->capacity = capacity;
  minHeap->nodes = (minHeapNodeObj **) malloc(capacity * sizeof(minHeapNodeObj *));
  if(!minHeap->nodes) {
    free(minHeap->pos);
    free(minHeap);
    return nullptr;
  }
  return minHeap;
}

static void minHeapify(minHeapObj *minHeap, int idx)
{
  int smallest = idx;
  const int left = 2*idx + 1;
  const int right = 2*idx + 2;
 
  if (left < minHeap->size && minHeap->nodes[left]->dist < minHeap->nodes[smallest]->dist)
    smallest = left;
 
  if (right < minHeap->size && minHeap->nodes[right]->dist < minHeap->nodes[smallest]->dist)
    smallest = right;
 
  if (smallest != idx) {
    minHeapNodeObj *smallestNode = minHeap->nodes[smallest];
    minHeapNodeObj *idxNode = minHeap->nodes[idx];
 
    minHeap->pos[smallestNode->idx] = idx; // swap positions
    minHeap->pos[idxNode->idx] = smallest;
 
    std::swap(minHeap->nodes[smallest], minHeap->nodes[idx]); // swap nodes
    minHeapify(minHeap, smallest);
  }
}

static bool isEmpty(const minHeapObj *minHeap)
{
  return minHeap->size == 0;
}

static minHeapNodeObj *extractMin(minHeapObj *minHeap)
{
  if (isEmpty(minHeap)) return nullptr;
 
  // store root node
  minHeapNodeObj *root = minHeap->nodes[0];
 
  // replace root node with last node
  minHeapNodeObj *lastNode = minHeap->nodes[minHeap->size - 1];
  minHeap->nodes[0] = lastNode;
 
  // update position of last node
  minHeap->pos[root->idx] = minHeap->size - 1;
  minHeap->pos[lastNode->idx] = 0;
 
  // Reduce heap size and heapify root
  --minHeap->size;
  minHeapify(minHeap, 0);
 
  return root;
}

static void decreaseKey(minHeapObj *minHeap, int idx, int dist)
{
  // get the index of idx in min heap nodes
  int i = minHeap->pos[idx];
 
  // get the node and update its dist value
  minHeap->nodes[i]->dist = dist;
 
  // travel up while the complete tree is not hepified (this is a O(Logn) loop)
  while (i && minHeap->nodes[i]->dist < minHeap->nodes[(i - 1) / 2]->dist) {
    // swap this node with its parent
    minHeap->pos[minHeap->nodes[i]->idx] = (i-1)/2;
    minHeap->pos[minHeap->nodes[(i-1)/2]->idx] = i;
    std::swap(minHeap->nodes[i], minHeap->nodes[(i - 1) / 2]);
 
    // move to parent index
    i = (i - 1) / 2;
  }
}

static bool isInMinHeap(const minHeapObj *minHeap, int idx)
{
  return minHeap->pos[idx] < minHeap->size;
}

typedef struct {
  double *dist;
  int *prev;
} dijkstraOutputObj;

static dijkstraOutputObj *dijkstra(graphObj *graph, int src)
{
  int n = graph->numnodes;

  minHeapObj *minHeap = createMinHeap(n); // priority queue implemented as a min heap structure
  if(!minHeap) return nullptr;

  dijkstraOutputObj *output = nullptr;
  output = (dijkstraOutputObj *) malloc(sizeof(dijkstraOutputObj));
  output->dist = (double *) malloc(n * sizeof(double));
  output->prev = (int *) malloc(n * sizeof(int));
  if(!output->dist || !output->prev) {
    msFree(output->dist);
    msFree(output->prev);
    free(output);
    freeMinHeap(minHeap);
    return nullptr;
  }

  // initialize
  for (int i=0; idist[i] = HUGE_VAL;
    output->prev[i] = -1;
    minHeap->nodes[i] = newMinHeapNode(i, output->dist[i]); // allocate a min heap node for each graph node
    minHeap->pos[i] = i;
  }
 
  // make dist value of src vertex as 0 so that it is extracted first
  minHeap->pos[src] = src;
  output->dist[src] = 0;
  decreaseKey(minHeap, src, output->dist[src]);
 
  // initially size of min heap is equal to graph->numnodes (n)
  minHeap->size = n;
 
  // In the following loop, minHeap contains all nodes
  // whose shortest distance is not yet finalized.
  while (!isEmpty(minHeap)) {

    // extract the vertex with minimum distance value and store the node index
    minHeapNodeObj *minHeapNode = extractMin(minHeap);
    int u = minHeapNode->idx;
    free(minHeapNode); // done with this node

    // traverse through all adjacent nodes of u and update their distance values
    graphNodeObj *node = graph->head[u];
    while (node != nullptr) {
      int v = node->dest;
 
      // if shortest distance to v is not finalized yet, and distance to v
      // through u is less than its previously calculated distance
      if (isInMinHeap(minHeap, v) && output->dist[u] != HUGE_VAL && node->weight + output->dist[u] < output->dist[v]) {
        output->dist[v] = output->dist[u] + node->weight;
        output->prev[v] = u;
        decreaseKey(minHeap, v, output->dist[v]);
      }
      node = node->next;
    }
  }
 
  freeMinHeap(minHeap);

  return output;
}

int *msGraphGetLongestShortestPath(graphObj *graph, int src, int *path_size, double *path_dist)
{
  if(!graph || src < 0 || src > graph->numnodes) return nullptr;

  int* path = (int *) malloc((graph->numnodes)*sizeof(int)); // worst case is path traverses all nodes
  if(!path) return nullptr;

  dijkstraOutputObj *output = dijkstra(graph, src);
  if(!output) {
    free(path);
    return nullptr; // algorithm failed for some reason
  }

  // get longest shortest distance from src to another node (our dest)
  *path_dist = -1;
  int dest = -1;
  for(int i=0; inumnodes; i++) {
    if(output->dist[i] != HUGE_VAL && *path_dist < output->dist[i]) {
      *path_dist = output->dist[i];
      dest = i;
    }
  }

  if(dest == -1) { // unable to determine destination node
    free(path);
    free(output->dist);
    free(output->prev);
    free(output);
    return nullptr;
  }

  // construct the path from src to dest
  int j = 0;
  for(int i=dest; i!=-1; i=output->prev[i],j++) path[j] = i;
  std::reverse(path, path + j);
  *path_size = j;

  // clean up dijkstra output
  free(output->dist);
  free(output->prev);
  free(output);

  return path;
}