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

typedef struct sAdjElement* AdjList;
struct sAdjElement {
    unsigned int nodeto;
    float weight;
    AdjList next;
};

struct Graph {
    AdjList* edges;
    unsigned int node_count;
};

struct Graph* graph_create(unsigned int node_count) {
    struct Graph* graph = (struct Graph*)malloc(sizeof(struct Graph));
    graph->edges = (AdjList*) malloc(node_count * sizeof(AdjList));
    graph->node_count = node_count;
    for(unsigned int i = 0; i < node_count; i++) graph->edges[i] = NULL;
    return graph;
}

void graph_add_edge(struct Graph* graph, unsigned int nodefrom, unsigned int nodeto, float weight) {
    if(graph->edges[nodefrom] == NULL) {
        AdjList element = malloc(sizeof(struct sAdjElement));
        element->nodeto = nodeto;
        element->weight = weight;
        element->next = NULL;
        graph->edges[nodefrom] = element;
    } else if(graph->edges[nodefrom]->weight < weight) {
        AdjList element = malloc(sizeof(struct sAdjElement));
        element->nodeto = nodeto;
        element->weight = weight;
        element->next = graph->edges[nodefrom];
        graph->edges[nodefrom] = element;
    } else {
        AdjList element = graph->edges[nodefrom];
        while(element->nodeto != nodeto && element->next != NULL && element->weight > weight) element = element->next;
        if(element->nodeto != nodeto) {
            AdjList newelement = malloc(sizeof(struct sAdjElement));
            newelement->nodeto = nodeto;
            newelement->weight = weight;
            newelement->next = element->next;
            element->next = newelement;
        }
    }
}

AdjList graph_get_edges(struct Graph* graph, unsigned int nodefrom) {
    return graph->edges[nodefrom];
}

struct Stack {
    unsigned int* elements;
    unsigned int top;
};

struct Stack* stack_create(unsigned int maxsize) {
    struct Stack* newstack = malloc(sizeof(struct Stack));
    newstack->elements = malloc(maxsize * sizeof(unsigned int));
    return newstack;
}

void stack_destroy(struct Stack* stack) {
    free(stack->elements);
    free(stack);
}

void stack_push(struct Stack* stack, unsigned int element) {
    stack->elements[stack->top++] = element;
}

short stack_contains(struct Stack* stack, unsigned int element) {
    for(unsigned int i = 0; i < stack->top; i++) {
        if(stack->elements[i] == element) return 1;
    }
    return 0;
}

unsigned int stack_pop(struct Stack* stack) {
    return stack->elements[--stack->top];
}

short stack_empty(struct Stack* stack) {
    return stack->top == 0;
}

float* dijkstra(struct Graph* graph, unsigned int startnode) {
    struct Stack* stack = stack_create(graph->node_count);
    struct Stack* visited = stack_create(graph->node_count);
    stack_push(stack,startnode);
    float* distances = malloc(graph->node_count * sizeof(float));
    for(unsigned int node = 0; node < graph->node_count; node++) {
        distances[node] = INFINITY;
    }
    distances[startnode] = 0;

    while(!stack_empty(stack)) {
        unsigned int current_node = stack_pop(stack);
        printf("Selecting node %d\n", current_node);
        stack_push(visited,current_node);
        AdjList edge = graph_get_edges(graph, current_node);
        while(edge != NULL) {
            printf("Edge to node %d\n", edge->nodeto);
            if(!stack_contains(visited, edge->nodeto)) {
                if(distances[edge->nodeto] > distances[current_node] + edge->weight) {
                    distances[edge->nodeto] = distances[current_node] + edge->weight;
                }
                if(!stack_contains(stack,edge->nodeto)) stack_push(stack,edge->nodeto);
            }
            edge = edge->next;
        }
    }

    stack_destroy(stack);

    return distances;
}

int main( int argc, char *argv[] ) {
    struct Graph* graph = graph_create(5);

    graph_add_edge(graph, 0, 2, 5.0f);
    graph_add_edge(graph, 0, 1, 2.0f);
    graph_add_edge(graph, 1, 2, 2.0f);
    graph_add_edge(graph, 2, 3, 1.0f);
    graph_add_edge(graph, 2, 4, 5.0f);

    float* distances = dijkstra(graph,0);

    for(unsigned int i = 0; i < 5; i++) {
        printf("%d: %.2f\n", i, distances[i]);
    }


	return 0;
}