#include "game.h"
#include "maze.h"


bool maze::load_from_file(const char* filename){
	FILE *infile = fopen(filename, "r");
	if(!infile){
		printf(RED("Maze file %s could not be opened").c_str(), filename);
		return false;
	}
	char *line;
	size_t linesize = 0;
	ssize_t readlen;
	glm::vec3 place = start_location;
	while(0 < (readlen = getline(&line, &linesize, infile))){
		for(char *c = line; *c && (*c == '#' || *c == ' '); c++) {
			if(*c == '#')
				locations.push_back(place);
			place.x += 8;
		}
		place.x = start_location.x;
		place.z += 8;
	}
	fclose(infile);
	return true;	
}

void maze_solver::launch(float maze_height){
	glm::vec3 lap = get_look_at_point();
	data_mutex.lock();
	sphere::addsphere(lap, 3.5);
	float height_change = player_position.y - maze_height;
	lap -= player_position;
	lap *= height_change / lap.y;
	lap = player_position - lap;
	lap = glm::vec3(8.0f * roundf(lap.x / 8.0f), maze_height, 8.0f * roundf(lap.z / 8.0f));
	new_locations.push_back(lap);
	main_hud.lprintf(0, "Solver sent to:  %f, %f, %f", lap.x, lap.y, lap.z);
	speeds.push_back(0.1f);
	rotation_matrices.push_back(glm::mat4(1.0f));
	visiteds.push_back(std::set<intpoint>());
	stacks.push_back(std::vector<intpoint>());
	data_mutex.unlock();
}

void maze_solver::move(int elapsed_time) {
	for(size_t i = 0; i < locations.size(); i++){
		if(locations[i].z < 580 && locations[i].y <= -95){ // TODO:  If the maze ends somewhere else, this would be wrong
			ups[i].a -= 0.1 * solve_speed;
		}
		if(locations[i].z < 540 && locations[i].y <= -95){
			remove(i);
			continue;
		}
		bool found_next = false;
		if(speeds[i] < 0.001){
			for(auto &n : intpoint(locations[i]).neighbors()){
				if(!is_empty(n.vec3(), 0.0f))
					continue;
				if(visiteds[i].count(n))
					continue;
				visiteds[i].insert(n);
				stacks[i].push_back(n);
				found_next = true;
				roll_to(i, n.vec3(), solve_speed);
				main_hud.lprintf(0, "Going to %d, %d, %d", n.x, n.y, n.z);
				break; // We found somewhere new to go!
			}
			if(!found_next){ // Backtrack
				if(stacks[i].size()){
					roll_to(i, stacks[i].back().vec3(), solve_speed);
					main_hud.lprintf(0, "Backtrack to %f, %f, %f", new_locations[i].x, new_locations[i].y, new_locations[i].z);
					stacks[i].pop_back();
				} else { // We can't backtrack, because the stack is empty
					visiteds[i].clear(); // Maybe just start over?
				}
			}
		}
	}
	rollsphere::move(elapsed_time);
}

void maze_solver::remove(size_t idx){
	locations.erase(locations.begin() + idx);
	radii.erase(radii.begin() + idx);
	ups.erase(ups.begin() + idx);
	forwards.erase(forwards.begin() + idx);
	new_locations.erase(new_locations.begin() + idx);
	speeds.erase(speeds.begin() + idx);
	rotation_matrices.erase(rotation_matrices.begin() + idx);
	visiteds.erase(visiteds.begin() + idx);
	stacks.erase(stacks.begin() + idx);
}