/* License: AGPLv3 or later. https://www.gnu.org/licenses/licenses.html
 * 
 * IMPORTANT: THIS PROGRAM REQUIRES THE -lm FLAG TO COMPILE.
 * Compile it as:
 * gcc -Wall -std=c11 ass3.c -o ass3 -lm
 * 
 * Assignment 3 - Optimum Routing
 * Manish
 * Student Login: *****
 * 
 * NOTE: I have not implemented memorisation for heuristics (euclidean
 * distance) even though we find shortest path several times (as we also find
 * second shortest path). Because, it would have marginal speed improvement
 * (and has complexity of O(1)) but can potentially require (no. of vertices)^2
 * space which is not very scalable.
 */

#include <float.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>


typedef struct
{
    double x;
    double y;
} vertex;

vertex * V = NULL; // Vertices
int nV; // Number of Vertices
double * E = NULL; // Edge Weights
int nE; // Number of Edges
int src; // source/from
int dst; // destination/to

 // reused in multiple aStar calls
double* D = NULL; // Best path distance/weight
int* P = NULL; // Parent of current vertices following best path
int* C = NULL; // Candidate vertices. It's min heap
int nC; // Number of Candidate vertices/ C size.

/* NOTE: Same memory as for V is allocated as dynamic
 * memory allocation later is not allowed and in worst case
 * would have to go through all vertices (or path does not exist).
 */
// Used for removing edges while finding second_shortest path
int* shortestPath = NULL;
int shortestPathVertices = 0;

// Used for storing second best candidate yet while search remaining options
int* secondShortestPath = NULL;
int secondShortestPathVertices = 0;


void aStar();

double euclidianDistance(int i, int j); // Heuristics
double absolute(double n);

// Work on C (Candidate List)
void heapify();
void shiftdown(int i);
void shiftup(int i);
void swap(int i, int j);


int main(void)
{
    printf("Enter file name containing graph details: ");
    char filename[257];
    // Assuming filename/file path won't be longer than 256 characters
    scanf("%256s", filename);

    FILE* file = fopen(filename, "r");
    if (!file)
    {
        perror(filename);
        exit(EXIT_FAILURE);
    }
    
    // Read nV & nE and dynamically allocate memory
    if (fscanf(file, " %d %d ", &nV, &nE) != EOF)
    {
        // +1 as keeping 1 indexed for code readability
        V = malloc(sizeof(vertex)*(nV+1));
        E = malloc(sizeof(double)*(nV+1)*(nV+1));
        D = malloc(sizeof(double)*(nV+1));
        P = malloc(sizeof(int)*(nV+1));
        // Can be nV-1 since one would always be source node
        C = malloc(sizeof(int)*nV);
        shortestPath = malloc(sizeof(vertex)*nV);
        secondShortestPath = malloc(sizeof(vertex)*nV);
        if (
            V == NULL
            || E == NULL
            || D == NULL
            || P == NULL
            || C == NULL
            || shortestPath == NULL
            || secondShortestPath == NULL
        )
        {
            fprintf(stderr, "Failed to allocate memory\n");
            return 1;
        }
    }
    else
    {
        fprintf(stderr, "File read error\n");
        return 1;
    }
    
    // Initializing all edge weights to double max;
    for (int i=0; i <= nV; i++)
    {
        for (int j=0; j <= nV; j++)
        {
            E[i*nV+j] = DBL_MAX;
            if (i == j)
                E[i*nV+j] = 0; // Distance/weight to self is 0
        }
    }
    
    // Read vertices
    int v;
    double x, y;
    for (int i=1; i <= nV; i++)
    {
        fscanf(file, " %d %lf %lf ", &v, &x, &y);
        if (v <= nV)
        {
            V[v].x = x;
            V[v].y = y;
        }
        else
        {
            fprintf(stderr,  "Vertex i (%d) > nV (%d)\n", v, nV);
            return 1;
        }
    }
    
    // Read Edges/Weights
    int v1, v2;
    double w;
    for (int i=0; i < nE; i++)
    {
        fscanf(file, " %d %d %lf", &v1, &v2, &w);
        if (v1 <= nV && v2 <= nV)
        {
            if (E[v1*nV+v2] > w)
            {
                E[v1*nV+v2] = w;
                E[v1+v2*nV] = w;
            }
        }
        else
        {
            fprintf(stderr, "v1 or v2 > nE in Edges\n");
            return 1;
        }
    }
    
    // Read source and destination vertices for path finding
    fscanf(file, " %d %d ", &src, &dst);
    
    fclose(file);
    
    aStar(); // Find shortest path
    if (D[dst] == DBL_MAX)
        printf(
            "Path from vertex %d to vertex %d does not exist\n", src, dst);
    else
    {
        // Recreate path from P (parents array)
        int parent = dst;
        while (P[parent] != parent)
        {
            shortestPath[shortestPathVertices++] = parent;
            parent = P[parent];
        }
        shortestPath[shortestPathVertices++] = parent;
        
        printf(
            "Shortest path from vertex %d to vertex %d is:\n%d",
            src,
            dst,
            src);
        for (int i=shortestPathVertices-2; i >= 0; i--)
            printf(" -> %d", shortestPath[i]);
        printf(
            "\nDistance/weight from vertex %d to vertex %d following the "
            "shortest path is %.2lf\n\n",
            src,
            dst,
            D[dst]);
        
        // Find second shortest path
        double secondShortestDistance = DBL_MAX;
        for (int i=0; i < shortestPathVertices-1; i++)
        {
            double w = E[shortestPath[i]*nV+shortestPath[i+1]];
            // Remove ith edge from shortest path
            E[shortestPath[i]*nV+shortestPath[i+1]] = DBL_MAX;
            E[shortestPath[i+1]*nV+shortestPath[i]] = DBL_MAX;
            aStar(); // Find new path
            // Add back ith edge from shortest path
            E[shortestPath[i]*nV+shortestPath[i+1]] = w;
            E[shortestPath[i+1]*nV+shortestPath[i]] = w;
            // if current distance is shorter than previously found 2nd best
            if (D[dst] < secondShortestDistance)
            { // Update 2nd shortest distance and path
                secondShortestDistance = D[dst];
                secondShortestPathVertices = 0;
                 int parent = dst;
                while (P[parent] != parent)
                {
                    secondShortestPath[
                        secondShortestPathVertices++] = parent;
                    parent = P[parent];
                }
                secondShortestPath[secondShortestPathVertices++] = parent;
            }
        }
        // Print second shortest path found (or not found)
        if (secondShortestDistance == DBL_MAX)
            printf(
                "Another/second Path from vertex %d to vertex %d does not "
                "exist\n",
                src,
                dst);
        else
        {
            printf("Second shortest path from vertex %d to vertex %d is:\n%d", src, dst, src);
            for (int i=secondShortestPathVertices-2; i >= 0; i--)
                printf(" -> %d", secondShortestPath[i]);
            printf(
                "\nDistance/weight from vertex %d to vertex %d following the "
                "second shortest path is %.2lf\n",
                src,
                dst,
                secondShortestDistance
            );
        }
    }
    
    // Free dynamically allocated memory
    free(V);
    free(E);
    free(D);
    free(P);
    free(C);
    free(shortestPath);
    free(secondShortestPath);
    
    return 0;
}


void aStar()
{
    // Infantilise/Reset global variables for each iteration.
    nC = 0;
    for (int i=1; i <= nV; i++)
    {
        D[i] = DBL_MAX;
        P[i] = i;
        C[nC++] = i;
    }

    // Remove source vertex from candidate list
    swap(src-1, --nC);
    int v = src;
    D[src] = 0; // distance from src to src is 0
    for (int i=1; i <= nV; i++)
    {
        D[i] = E[src*nV+i];
        if (D[i] != DBL_MAX)
            P[i] = src;
    }
    
    heapify();
    
    while (nC)
    {
        v = C[0];
        swap(0, --nC);
        shiftdown(0);

        if (v == dst)
            break;
        
        for (int j=0; j < nC; j++)
        {
            if (D[C[j]] > D[v] + E[v*nV+C[j]])
            {
                D[C[j]] = D[v] + E[v*nV+C[j]];
                P[C[j]] = v;
                shiftup(j);
            }
        }
    }
}

double euclidianDistance(int i, int j)
{
    double xDiff = absolute(V[i].x - V[j].x);
    double yDiff = absolute(V[i].y - V[j].y);
    return sqrt((xDiff*xDiff)+(yDiff*yDiff));
}


double absolute(double n)
{
    return (n < 0) ? -n : n;
}


void heapify()
{
    int shiftdowns_required = (nC/2)-1;
    for (int i = shiftdowns_required; i >= 0; i--)
    {
        shiftdown(i);
    };
}

void shiftdown(int i)
{
    int child = (i * 2) + 1; // left child
    if (child < nC) // has at least one child
    {
        if (child < nC - 1) // has both children
        {
            // if right child smaller
            if (
                D[C[child]]+euclidianDistance(C[child], dst) >
                D[C[child + 1]]+euclidianDistance(C[child+1], dst))
                child++; // pick right child
        }
        if (
            D[C[i]]+euclidianDistance(C[i], dst)
            > D[C[child]]+euclidianDistance(C[child], dst))
        {
            swap(i, child);
            shiftdown(child);
        }
    }
}

void shiftup(int i)
{
    if (i == 0)
        return;
    int parent = (i - 1) / 2;
    if (D[C[parent]]+euclidianDistance(C[parent], dst) >
        D[C[i]]+euclidianDistance(C[i], dst))
    {
        swap(parent, i);
        shiftup(parent);
    }
}

void swap(int i, int j)
{
    int tmp = C[i];
    C[i] = C[j];
    C[j] = tmp;
}