#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);
}

int calcDistance(int position, int destination) {
	if (position > destination) {
		return destination + (9999 + 1 - position);
	} else {
		return destination - position;
	}
}

DiskRequest* findClosest(DiskQueue *queue, int position, double time) {
	DiskRequest *current = queue->head;
	DiskRequest *closest = NULL;
	while (current != NULL) {
		if (current->time <= time) { // Filter out requests that haven't come in yet
			if (closest == NULL) { // If there is no closest yet, set it to the current
				closest = current;
			} else {
				int closestDistance = calcDistance(position, closest->position); // Distance from the closest to the current position, negative means down (bad)
				int currentDistance = calcDistance(position, current->position); // Distance from the current to the current position, negative means down (bad
				if (currentDistance < closestDistance) { // If the current is closer than the closest
					closest = current;
				}
			}
		}
		current = current->next;
	}
	return closest;
}