#include "restaurant.h"

/*
	Sets *ptr to the i'th restaurant. If this restaurant is already in the cache,
	it just copies it directly from the cache to *ptr. Otherwise, it fetches
	the block containing the i'th restaurant and stores it in the cache before
	setting *ptr to it.
*/
void getRestaurant(restaurant* ptr, int i, Sd2Card* card, RestCache* cache) {
	// calculate the block with the i'th restaurant
	uint32_t block = REST_START_BLOCK + i/8;

	// if this is not the cached block, read the block from the card
	if (block != cache->cachedBlock) {
		while (!card->readBlock(block, (uint8_t*) cache->block)) {
			Serial.print("readblock failed, try again");
		}
		cache->cachedBlock = block;
	}

	// either way, we have the correct block so just get the restaurant
	*ptr = cache->block[i%8];
}

// Swaps the two restaurants (which is why they are pass by reference).
void swap(RestDist& r1, RestDist& r2) {
	RestDist tmp = r1;
	r1 = r2;
	r2 = tmp;
}

// Insertion sort to sort the restaurants.
void insertionSort(RestDist restaurants[], int restaurantsAdded) {
	// Invariant: at the start of iteration i, the
	// array restaurants[0 .. i-1] is sorted.
	for (int i = 1; i < restaurantsAdded; ++i) {
		// Swap restaurant[i] back through the sorted list restaurants[0 .. i-1]
		// until it finds its place.
		for (int j = i; j > 0 && restaurants[j].dist < restaurants[j-1].dist; --j) {
			swap(restaurants[j-1], restaurants[j]);
		}
	}
}

// Computes the manhattan distance between two points (x1, y1) and (x2, y2).
int16_t manhattan(int16_t x1, int16_t y1, int16_t x2, int16_t y2) {
	return abs(x1-x2) + abs(y1-y2);
}


int pivotSort(RestDist restaurants[], int start, int end, int pi) {
	int n = end - start;

	swap(restaurants[start + pi], restaurants[end]);
	int lo = 0;
	int hi = n - 1;

	while (lo <= hi) {
		if (restaurants[start + hi].dist > restaurants[end].dist) {
			hi--;
		} else if (restaurants[start + lo].dist <= restaurants[end].dist) {
			lo++;
		} else {
			swap(restaurants[start + lo], restaurants[start + hi]);
		}
	}

	swap(restaurants[start + lo], restaurants[end]);
	return start + lo;
}

void quickSort(RestDist restaurants[], int start, int end) {
	int length = end - start;
	if (start >= end)
		return;

	int pivot = (length) / 2;
	int newPivot = pivotSort(restaurants, start, end, pivot);
	quickSort(restaurants, start, newPivot - 1);
	quickSort(restaurants, newPivot + 1, end);
}
















/*
	Fetches all restaurants from the card, saves their RestDist information
	in restaurants[], and then sorts them based on their distance to the
	point on the map represented by the MapView.

	Returns the number of restaurants which satisfy the rating level
*/
int getAndSortRestaurants(const MapView& mv, RestDist restaurants[], Sd2Card* card, RestCache* cache, int sortingType, int rating) {
	restaurant r;

	// First get all the restaurants and store their corresponding RestDist information.
	int restaurantsAdded = 0;
	for (int i = 0; i < NUM_RESTAURANTS; ++i) {
		getRestaurant(&r, i, card, cache);
		int16_t restaurantRating = max((r.rating+1)/2, 1);

		if (restaurantRating >= rating) {
			restaurants[restaurantsAdded].index = i;
			restaurants[restaurantsAdded].dist = manhattan(lat_to_y(r.lat), lon_to_x(r.lon),
																			mv.mapY + mv.cursorY, mv.mapX + mv.cursorX);
			restaurantsAdded++;
		}
	}


	int time = millis();

	 // quicksort or both
	if (sortingType == 0 || sortingType == 2)
		quickSort(restaurants, 0, restaurantsAdded);

	// insertion sort or both
	if (sortingType == 1 || sortingType == 2)
		insertionSort(restaurants, restaurantsAdded);

	int elapsedTime = millis() - time;
	Serial.println("Sorting time: ");
	Serial.print(elapsedTime);
	Serial.println(" ms");

	return restaurantsAdded;
}