#include <stdlib.h>
#include <stdio.h>
#include "disk.h"


DiskQueue *createDiskQueue() {
	DiskQueue *queue = malloc(sizeof(DiskQueue));
	if (queue == NULL) {
		printf("Error allocating memory for queue\n");
		exit(EXIT_FAILURE);
	}
	queue->head = NULL;
	queue->size = 0;
	return queue;
}

void enqueue(DiskQueue *queue, int time, int position) {
	DiskRequest *request = malloc(sizeof(DiskRequest));
	if (request == NULL) {
		printf("Error allocating memory for request\n");
		exit(EXIT_FAILURE);
	}
	request->time = time;
	request->position = position;
	request->next = NULL;

	if (queue->head == NULL) {
		queue->head = request;
	} else {
		DiskRequest *current = queue->head;
		while (current->next != NULL) {
			current = current->next;
		}
		current->next = request;
	}
	queue->size++;
}

void delete(DiskQueue *queue, DiskRequest request) {
	DiskRequest *current = queue->head;
	DiskRequest *previous = NULL;
	while (current != NULL) {
		if (current->position == request.position && current->time == request.time) {
			if (previous == NULL) {
				queue->head = current->next;
			} else {
				previous->next = current->next;
			}
			queue->size--;
			free(current);
			return;
		}
		previous = current;
		current = current->next;
	}
	printf("Error: Request not found\n");
	exit(EXIT_FAILURE);
}

DiskRequest *dequeue(DiskQueue *queue) {
	DiskRequest *head = queue->head;
	queue->head = head->next;
	queue->size--;
	return head;
}

void destroyDiskQueue(DiskQueue *queue) {
	DiskRequest *current = queue->head;
	DiskRequest *next = NULL;
	while (current != NULL) {
		next = current->next;
		free(current);
		current = next;
	}
	free(queue);
}

DiskRequest* findClosest(DiskQueue *queue, int position, double time) {
	DiskRequest *current = queue->head;
	DiskRequest *next = NULL;
	DiskRequest *first = NULL;
	while (current != NULL) {
		if (current->time <= time) { // Filter out requests that haven't come in yet
			if (first == NULL) {
				first = current;
			} else {
				if (current->position < first->position) {
					first = current;
				}
			}
			int currentDistance = current->position - position; // Distance from the current to the current position, negative means down (bad)
			if (currentDistance > 0) { // If the current is up
				if (next == NULL) { // If there is no next yet, set it to the current
					next = current;
				} else {
					int nextDistance = next->position - position; // Distance from the next to the current position, negative means down (bad)
					if (currentDistance < nextDistance) { // If the current is closer than the next
						next = current;
					}
				}
			}
		}
		current = current->next;
	}
	// If we didn't find a request that is above the current position, return the first as a fallback
	if (next == NULL) {
		return first;
	}
	return next;
}