#include <vector>
#include <string>
#include <boost/log/trivial.hpp>
#include <fstream>
#include <iostream>
#include <cfloat>
#include <cstdio>
#include <cstdlib>
#include <sys/types.h>

#include "gdindex.h"
#include "../local_descriptor/sift_reader.h"
#include "../../../common/feature_set/feature_set.h"
#include "../../../common/utils/io_utils.h"
#include "definition.h"

extern "C" {
#include <yael/gmm.h>
#include <yael/matrix.h>
}

using namespace std;

namespace vrs
{
namespace components
{

void copy_floats(const uint n, float* in, float* out)
{
	for (uint d = 0; d < n; d++) {
		out[d] = in[d];
	}
}

void gdindex::write(const string index_path)
{
	// Open file for writing
	FILE* index_file = fopen(index_path.c_str(), "wb");
	if (index_file == nullptr) {
		fprintf(stderr, "gdindex::write : Cannot open: %s\n", index_path.c_str());
		exit(EXIT_FAILURE);
	}

	// Write number of global descriptors
	int number_gd_to_write = static_cast<int>(index_.number_global_descriptors);
	fwrite(&number_gd_to_write, sizeof(int), 1, index_file);

	// Assert that we have data in index_ and that it makes sense,
	// and allocate helper variables
	if (index_parameters_.gd_unbinarized) {
		assert(index_.fv.size() != 0);
		assert(index_.fv.size() == index_.number_global_descriptors);
		assert(index_.fv.size() == index_.word_l1_norms.size());
		assert(index_.fv.size() == index_.word_total_soft_assignment.size());
	} else {
		assert(index_.word_descriptor.size() != 0);
		assert(index_.word_descriptor.size() == index_.number_global_descriptors);
		assert(index_.word_descriptor.size() == index_.word_l1_norms.size());
		assert(index_.word_descriptor.size() == index_.word_total_soft_assignment.size());
	}
	uint* word_descriptor_to_write =
	    new uint[index_parameters_.gd_number_gaussians];
	float* fv_to_write =
	    new float[index_parameters_.gd_number_gaussians
	              *index_parameters_.ld_pca_dim];
	float* word_l1_norms_to_write =
	    new float[index_parameters_.gd_number_gaussians];
	float* word_total_soft_assignment_to_write =
	    new float[index_parameters_.gd_number_gaussians];

	// Loop over items in index and write them out
	for (int count_item = 0; count_item < number_gd_to_write; count_item++) {
		// Collect data that will be written
		for (uint count_gaussian = 0;
		     count_gaussian < index_parameters_.gd_number_gaussians;
		     count_gaussian++) {
			word_l1_norms_to_write[count_gaussian] =
			    index_.word_l1_norms.at(count_item).at(count_gaussian);
			word_total_soft_assignment_to_write[count_gaussian] =
			    index_.word_total_soft_assignment.at(count_item).at(count_gaussian);
			if (index_parameters_.gd_unbinarized) {
				// Get ld_pca_dim-sized vector for this Gaussian
				for (uint d = 0; d < index_parameters_.ld_pca_dim; d++) {
					fv_to_write[count_gaussian*index_parameters_.ld_pca_dim
					            + d]
					    = index_.fv.at(count_item).at(count_gaussian
					                                  *index_parameters_.ld_pca_dim
					                                  + d);
				}
			} else {
				word_descriptor_to_write[count_gaussian] =
				    index_.word_descriptor.at(count_item).at(count_gaussian);
			}
		}
		// Write to file
		fwrite(word_l1_norms_to_write, sizeof(float),
		       index_parameters_.gd_number_gaussians, index_file);
		fwrite(word_total_soft_assignment_to_write, sizeof(float),
		       index_parameters_.gd_number_gaussians, index_file);
		if (index_parameters_.gd_unbinarized) {
			fwrite(fv_to_write, sizeof(float),
			       index_parameters_.gd_number_gaussians
			       *index_parameters_.ld_pca_dim, index_file);
		} else {
			fwrite(word_descriptor_to_write, sizeof(uint),
			       index_parameters_.gd_number_gaussians, index_file);
		}
	}

	// Clean up
	if (word_l1_norms_to_write != nullptr) {
		delete [] word_l1_norms_to_write;
		word_l1_norms_to_write = nullptr;
	}
	if (word_total_soft_assignment_to_write != nullptr) {
		delete [] word_total_soft_assignment_to_write;
		word_total_soft_assignment_to_write = nullptr;
	}
	if (word_descriptor_to_write != nullptr) {
		delete [] word_descriptor_to_write;
		word_descriptor_to_write = nullptr;
	}
	if (fv_to_write != nullptr) {
		delete [] fv_to_write;
		fv_to_write = nullptr;
	}

	// Close file
	fclose(index_file);
}

void gdindex::read(const string index_path, const bool both_sel_modes)
{
	// Check that word selection mode is valid
	if (query_parameters_.word_selection_mode != WORD_L1_NORM &&
	    query_parameters_.word_selection_mode != WORD_SOFT_ASSGN) {
		cout << "Error! Mode " << query_parameters_.word_selection_mode
		     << " is not allowed. Quitting..."
		     << endl;
		exit(EXIT_FAILURE);
	}

	// Open file for reading
	FILE* index_file = fopen(index_path.c_str(), "rb");
	if (index_file == nullptr) {
		fprintf(stderr, "gdindex::read : Cannot open: %s\n", index_path.c_str());
		exit(EXIT_FAILURE);
	}

	// Read number of global descriptors
	int number_gd_to_read = 0;
	fread(&number_gd_to_read, sizeof(int), 1, index_file);

	// Size of current index
	uint current_db_size = index_.number_global_descriptors;

	// Allocate helper variables
	uint* word_descriptor_to_read =
	    new uint[index_parameters_.gd_number_gaussians];
	float* fv_to_read =
	    new float[index_parameters_.gd_number_gaussians
	              *index_parameters_.ld_pca_dim];
	float* word_l1_norms_to_read =
	    new float[index_parameters_.gd_number_gaussians];
	float* word_total_soft_assignment_to_read =
	    new float[index_parameters_.gd_number_gaussians];

	if (index_parameters_.gd_unbinarized) {
		index_.fv.resize(current_db_size + number_gd_to_read);
	} else {
		index_.word_descriptor.resize(current_db_size + number_gd_to_read);
	}
	// Depending on both_sel_modes and query_parameters_.word_selection_mode,
	// we will load either BOTH l1 norms and total soft assignment information,
	// or only one of them.
	if (both_sel_modes || query_parameters_.word_selection_mode == WORD_L1_NORM) {
		index_.word_l1_norms.resize(current_db_size + number_gd_to_read);
	}
	if (both_sel_modes || query_parameters_.word_selection_mode == WORD_SOFT_ASSGN) {
		index_.word_total_soft_assignment.resize(current_db_size + number_gd_to_read);
	}

	// Loop over items, read and insert them into index_
	for (uint count_item = current_db_size;
	     count_item < current_db_size + number_gd_to_read;
	     count_item++) {
		// Read data
		fread(word_l1_norms_to_read, sizeof(float),
		      index_parameters_.gd_number_gaussians, index_file);
		fread(word_total_soft_assignment_to_read, sizeof(float),
		      index_parameters_.gd_number_gaussians, index_file);
		if (index_parameters_.gd_unbinarized) {
			fread(fv_to_read, sizeof(float),
			      index_parameters_.gd_number_gaussians
			      *index_parameters_.ld_pca_dim, index_file);
		} else {
			fread(word_descriptor_to_read, sizeof(uint),
			      index_parameters_.gd_number_gaussians, index_file);
		}

		// Insert data into index_
		if (both_sel_modes ||
		    query_parameters_.word_selection_mode == WORD_L1_NORM) {
			index_.word_l1_norms.at(count_item)
			.resize(index_parameters_.gd_number_gaussians);
			for (uint count_gaussian = 0;
			     count_gaussian < index_parameters_.gd_number_gaussians;
			     count_gaussian++) {
				index_.word_l1_norms.at(count_item).at(count_gaussian)
				    = word_l1_norms_to_read[count_gaussian];
			}
		}
		if (both_sel_modes ||
		    query_parameters_.word_selection_mode == WORD_SOFT_ASSGN) {
			index_.word_total_soft_assignment.at(count_item)
			.resize(index_parameters_.gd_number_gaussians);
			for (uint count_gaussian = 0;
			     count_gaussian < index_parameters_.gd_number_gaussians;
			     count_gaussian++) {
				index_.word_total_soft_assignment.at(count_item).at(count_gaussian)
				    = word_total_soft_assignment_to_read[count_gaussian];
			}
		}
		if (index_parameters_.gd_unbinarized) {
			index_.fv.at(count_item)
			.resize(index_parameters_.gd_number_gaussians
			        *index_parameters_.ld_pca_dim);
			for (uint count_gaussian = 0;
			     count_gaussian < index_parameters_.gd_number_gaussians;
			     count_gaussian++) {
				// Get ld_pca_dim-sized vector for this Gaussian
				for (uint d = 0; d < index_parameters_.ld_pca_dim; d++) {
					index_.fv.at(count_item).at(count_gaussian
					                            *index_parameters_.ld_pca_dim
					                            + d) =
					                                fv_to_read[count_gaussian*index_parameters_.ld_pca_dim
					                                        + d];
				}
			}
		} else {
			index_.word_descriptor.at(count_item)
			.resize(index_parameters_.gd_number_gaussians);
			for (uint count_gaussian = 0;
			     count_gaussian < index_parameters_.gd_number_gaussians;
			     count_gaussian++) {
				index_.word_descriptor.at(count_item).at(count_gaussian)
				    = word_descriptor_to_read[count_gaussian];
			}
		}
	}

	// Clean up
	if (word_l1_norms_to_read != nullptr) {
		delete [] word_l1_norms_to_read;
		word_l1_norms_to_read = nullptr;
	}
	if (word_total_soft_assignment_to_read != nullptr) {
		delete [] word_total_soft_assignment_to_read;
		word_total_soft_assignment_to_read = nullptr;
	}
	if (word_descriptor_to_read != nullptr) {
		delete [] word_descriptor_to_read;
		word_descriptor_to_read = nullptr;
	}
	if (fv_to_read != nullptr) {
		delete [] fv_to_read;
		fv_to_read = nullptr;
	}

	// Close file
	fclose(index_file);

	// Update other index_ variables, since now index has changed
	update_index();
}

void gdindex::write_frame_list(const string file_path)
{
	ofstream out_file;
	out_file.open(file_path.c_str());
	uint number_frames_in_db = index_.frame_numbers_in_db.size();
	for (uint count_line = 0; count_line < number_frames_in_db; count_line++) {
		out_file << index_.frame_numbers_in_db.at(count_line) << endl;
	}
	out_file.close();
}

void gdindex::clean_index()
{
	index_.word_descriptor.clear();
	index_.fv.clear();
	index_.word_l1_norms.clear();
	index_.word_total_soft_assignment.clear();
	index_.frame_numbers_in_db.clear();

	update_index();
}

uint gdindex::get_number_global_descriptors()
{
	return index_.number_global_descriptors;
}

void gdindex::generate_index(const vector<string>& feature_files,
                             const int verbose_level)
{
	uint number_files_to_process = feature_files.size();

	// Allocate space in index_
	index_.word_l1_norms.resize(number_files_to_process);
	index_.word_total_soft_assignment.resize(number_files_to_process);
	if (index_parameters_.gd_unbinarized) {
		index_.fv.resize(number_files_to_process);
	} else {
		index_.word_descriptor.resize(number_files_to_process);
	}

	uint number_files_processed = 0;
	#pragma omp parallel for
	for (uint count_file = 0; count_file < number_files_to_process; count_file++) {
		// Load feature set
		if (!common::io_utils::is_file_exist(feature_files.at(count_file))) {
			fprintf(stderr, "Missing feature file: %s\n",
			        feature_files.at(count_file).c_str());
			exit(EXIT_FAILURE);
		}
		common::feature_set feat_set;
		const char *feat_file_path = feature_files[count_file].c_str();
		if (index_parameters_.ld_name == SIFT_NAME) {
			components::sift_reader::read_sift_file(feat_file_path,
			                                        index_parameters_.ld_frame_length,
			                                        index_parameters_.ld_length,feat_set);
		} else if (index_parameters_.ld_name == SIFTGEO_NAME) {
			components::sift_reader::read_sift_geo_file(feat_file_path,
			        index_parameters_.ld_frame_length,
			        index_parameters_.ld_length,feat_set);
		} else {
			cout << "Local feature " << index_parameters_.ld_name
			     << " is not supported" << endl;
		}

		// Generate global signature, put in index_
		index_.word_l1_norms.at(count_file).resize(index_parameters_.gd_number_gaussians);
		index_.word_total_soft_assignment.at(count_file).resize(index_parameters_.gd_number_gaussians);
		// -- auxiliary vectors
		vector<uint> aux_gd_word_descriptor(index_parameters_.gd_number_gaussians);
		vector<float> aux_gd_fv(index_parameters_.gd_number_gaussians
		                        *index_parameters_.ld_pca_dim);
		generate_global_descriptor(&feat_set,
		                           aux_gd_word_descriptor,
		                           aux_gd_fv,
		                           index_.word_l1_norms.at(count_file),
		                           index_.word_total_soft_assignment.at(count_file));
		if (index_parameters_.gd_unbinarized) {
			index_.fv.at(count_file) = aux_gd_fv;
		} else {
			index_.word_descriptor.at(count_file) = aux_gd_word_descriptor;
		}

		// Report status
		#pragma omp critical
		{
			number_files_processed++;
		}
		if (verbose_level >= 2) {
			printf("[%06d, %06d] %04d features \n",
			       count_file, number_files_processed, feat_set.get_num_features());
		}
	}

	update_index();
}

void gdindex::generate_index_shot_based(const vector<string>& feature_files,
                                        const vector<uint>& shot_beg_frames,
                                        const int shot_mode, const int shot_keyf,
                                        const int verbose_level)
{
	if (verbose_level >= 4) cout << "Starting generate_index_shot_based..."
		                             << endl;
	uint number_shots = shot_beg_frames.size();
	if (verbose_level >= 3) cout << "Using " << number_shots << " shots" << endl;

	uint number_frames_in_db = feature_files.size();
	if (verbose_level >= 3) cout << "There are " << number_frames_in_db
		                             << " frames in the database." << endl;

	// The number of entries in the generated index will vary depending
	// on mode and shot_keyf
	uint number_entries_in_index = 0;

	// This vector is used only if frames in shot are stored independently.
	// It contains, for each shot number (first entry), a vector with indices
	// which are the places in the index where each frame will be placed.
	vector < vector < uint > > inds_indep_mode;

	if (shot_mode == SHOT_MODE_INDEP_KEYF) {
		// The number of global signatures will vary for this mode,
		// it depends on the number of frames in each shot, so
		// we need to calculate
		for (uint count_shot = 0; count_shot < number_shots; count_shot++) {
			uint number_frames_this_shot;
			// Note: the number calculated in the following lines is a simple
			// subtraction, since the next number is not part of the shot, so
			// there's no +1 at the end of the calculation
			if (count_shot != number_shots - 1) {
				number_frames_this_shot = shot_beg_frames.at(count_shot + 1)
				                          - shot_beg_frames.at(count_shot);
			} else {
				number_frames_this_shot = number_frames_in_db
				                          - shot_beg_frames.at(count_shot);
			}

			uint n;
			if (shot_keyf != -1) {
				n = min(number_frames_this_shot, static_cast<uint>(shot_keyf));
			} else {
				n = number_frames_this_shot;
			}

			vector < uint > shot_inds;
			for (uint count_f = 0; count_f < n; count_f++) {
				shot_inds.push_back(number_entries_in_index + count_f);
			}

			number_entries_in_index += n;
			inds_indep_mode.push_back(shot_inds);
		}
	} else if (shot_mode == SHOT_MODE_SHOT_AGG) {
		// This will generate only a global signature per shot
		number_entries_in_index = number_shots;
	} else {
		cout << "Indexing for shot_mode " << shot_mode
		     << " is not currently implemented. Quitting..." << endl;
		exit(EXIT_FAILURE);
	}

	if (verbose_level >= 2) cout << "GD index will contain "
		                             << number_entries_in_index << " entries" << endl;

	// Allocate space in index_
	index_.word_l1_norms.resize(number_entries_in_index);
	index_.word_total_soft_assignment.resize(number_entries_in_index);
	if (index_parameters_.gd_unbinarized) {
		index_.fv.resize(number_entries_in_index);
	} else {
		index_.word_descriptor.resize(number_entries_in_index);
	}

	// We will loop over shots and aggregate features depending on the mode
	uint count_index = 0;
	#pragma omp parallel for
	for (uint count_shot = 0; count_shot < number_shots; count_shot++) {
		uint number_frames_this_shot;
		// Note: the number calculated in the following lines is a simple
		// subtraction, since the next number is not part of the shot, so
		// there's no +1 at the end of the calculation
		if (count_shot != number_shots - 1) {
			number_frames_this_shot = shot_beg_frames.at(count_shot + 1)
			                          - shot_beg_frames.at(count_shot);
		} else {
			number_frames_this_shot = number_frames_in_db
			                          - shot_beg_frames.at(count_shot);
		}

		if (verbose_level >= 3) cout << "Doing shot " << count_shot
			                             << " out of " << number_shots << endl;
		uint number_frames_to_use;
		if (shot_keyf != -1) {
			number_frames_to_use = min(number_frames_this_shot,
			                           static_cast<uint>(shot_keyf));
		} else {
			number_frames_to_use = number_frames_this_shot;
		}

		if (verbose_level >= 3) cout << "This shot will use "
			                             << number_frames_to_use << " frames" << endl;

		vector<uint> frames_to_use;
		sample_frames_from_shot(number_frames_to_use,
		                        shot_beg_frames.at(count_shot),
		                        number_frames_this_shot,
		                        frames_to_use);

		if (shot_mode == SHOT_MODE_INDEP_KEYF) {
			for (uint count_ind = 0; count_ind < number_frames_to_use;
			     count_ind++) {
				uint frame_this_ind = frames_to_use.at(count_ind);
				uint ind_in_index = inds_indep_mode.at(count_shot).at(count_ind);
				// Load feature set
				if (!common::io_utils::is_file_exist(feature_files.at(frame_this_ind))) {
					fprintf(stderr, "Missing feature file: %s\n",
					        feature_files.at(frame_this_ind).c_str());
					exit(EXIT_FAILURE);
				}
				const char *feature_file_path = feature_files[frame_this_ind].c_str();
				common::feature_set feat_set;
				if (index_parameters_.ld_name == SIFT_NAME) {
					components::sift_reader::read_sift_file(feature_file_path,
					                                        index_parameters_.ld_frame_length,
					                                        index_parameters_.ld_length,feat_set);
				} else if (index_parameters_.ld_name == SIFTGEO_NAME) {
					components::sift_reader::read_sift_geo_file(feature_file_path,
					        index_parameters_.ld_frame_length,
					        index_parameters_.ld_length,feat_set);
				} else {
					cout << "Local feature " << index_parameters_.ld_name
					     << " is not supported" << endl;
				}
				// Generate global signature, put in index_
				index_.word_l1_norms.at(ind_in_index).resize(index_parameters_.gd_number_gaussians);
				index_.word_total_soft_assignment.at(ind_in_index).resize(index_parameters_.gd_number_gaussians);
				// -- auxiliary vectors
				vector<uint> aux_gd_word_descriptor(index_parameters_.gd_number_gaussians);
				vector<float> aux_gd_fv(index_parameters_.gd_number_gaussians
				                        *index_parameters_.ld_pca_dim);
				generate_global_descriptor(&feat_set,
				                           aux_gd_word_descriptor,
				                           aux_gd_fv,
				                           index_.word_l1_norms.at(ind_in_index),
				                           index_.word_total_soft_assignment.at(ind_in_index));
				if (index_parameters_.gd_unbinarized) {
					index_.fv.at(ind_in_index) = aux_gd_fv;
				} else {
					index_.word_descriptor.at(ind_in_index) = aux_gd_word_descriptor;
				}

				#pragma omp critical
				{
					count_index++;
					index_.frame_numbers_in_db.push_back(frame_this_ind);
				}
			}
		} else if (shot_mode == SHOT_MODE_SHOT_AGG) {
			// Collect all features
			common::feature_set feat_set(index_parameters_.ld_length,
			                             index_parameters_.ld_frame_length);
			for (uint count_ind = 0; count_ind < number_frames_to_use; count_ind++) {
				uint frame_this_ind = frames_to_use.at(count_ind);
				if (verbose_level >= 4) cout << "Doing frame " << frame_this_ind << endl;

				// Load feature set for this frame
				common::feature_set feat_set_this_frame;
				const char *feat_path = feature_files[frame_this_ind].c_str();
				if (index_parameters_.ld_name == SIFT_NAME) {
					components::sift_reader::read_sift_file(feat_path,
					                                        index_parameters_.ld_frame_length,
					                                        index_parameters_.ld_length,feat_set_this_frame);
				} else if (index_parameters_.ld_name == SIFTGEO_NAME) {
					components::sift_reader::read_sift_geo_file(feat_path,
					        index_parameters_.ld_frame_length,
					        index_parameters_.ld_length,feat_set_this_frame);
				} else {
					cout << "Local feature " << index_parameters_.ld_name
					     << " is not supported" << endl;
				}

				if (verbose_level >= 4) cout << "Loaded features from this frame"
					                             << endl;

				// Add this frame's features to collection of all shot's features
				uint number_features_this_frame =
				    feat_set_this_frame.get_num_features();
				for (uint count_f = 0; count_f < number_features_this_frame;
				     count_f++) {
					float* aux_ld = new float[index_parameters_.ld_length];
					copy_floats(index_parameters_.ld_length,
					            feat_set_this_frame.get_descriptor_at(count_f), aux_ld);
					float* aux_f = new float[index_parameters_.ld_frame_length];
					copy_floats(index_parameters_.ld_frame_length,
					            feat_set_this_frame.get_frame_at(count_f), aux_f);
					feat_set.add_feature(aux_ld, aux_f);
				}
				if (verbose_level >= 4) cout << "Added these features to shot's features" << endl;
			}

			if (verbose_level >= 4) cout << "All features were collected, now shot contains " << feat_set.get_num_features() << " features" << endl;

			// Generate global signature, put in index_
			index_.word_l1_norms.at(count_shot).resize(index_parameters_.gd_number_gaussians);
			index_.word_total_soft_assignment.at(count_shot).resize(index_parameters_.gd_number_gaussians);
			// -- auxiliary vectors
			vector<uint> aux_gd_word_descriptor(index_parameters_.gd_number_gaussians);
			vector<float> aux_gd_fv(index_parameters_.gd_number_gaussians
			                        *index_parameters_.ld_pca_dim);
			generate_global_descriptor(&feat_set,
			                           aux_gd_word_descriptor,
			                           aux_gd_fv,
			                           index_.word_l1_norms.at(count_shot),
			                           index_.word_total_soft_assignment.at(count_shot));
			if (index_parameters_.gd_unbinarized) {
				index_.fv.at(count_shot) = aux_gd_fv;
			} else {
				index_.word_descriptor.at(count_shot) = aux_gd_word_descriptor;
			}

			#pragma omp critical
			{
				count_index++;
			}
		}
	}

	// We need to sort frame_numbers_in_db, because it might not be in order
	// due to parallelization.
	// Note: this variable is used only if the mode is SHOT_MODE_INDEP_KEYF,
	// so if the mode is different it will just sort nothing, it doesnt matter
	stable_sort(index_.frame_numbers_in_db.begin(), index_.frame_numbers_in_db.end());

	// Make sure we processed the correct number of REVV signatures
	assert(count_index == number_entries_in_index);
	if (shot_mode == SHOT_MODE_INDEP_KEYF) {
		assert(count_index == index_.frame_numbers_in_db.size());
	}

	update_index();
}

void gdindex::generate_global_descriptor(const common::feature_set* feature_set,
        vector<uint>& gd_word_descriptor,
        vector<float>& gd_fv,
        vector<float>& gd_word_l1_norm,
        vector<float>& gd_word_total_soft_assignment)
{
	// Resize the vectors that will be returned
	gd_word_descriptor.resize(index_parameters_.gd_number_gaussians);
	gd_word_l1_norm.resize(index_parameters_.gd_number_gaussians);
	gd_word_total_soft_assignment.resize(index_parameters_.gd_number_gaussians);

	uint unbinarized_signature_length = index_parameters_.gd_number_gaussians
	                                    *index_parameters_.ld_pca_dim;
	float* all_pca_desc = new float[feature_set->get_num_features()
	                                * index_parameters_.ld_pca_dim];
	// Project SIFT using PCA
	for (uint count_feat = 0; count_feat < feature_set->get_num_features(); count_feat++) {
		project_local_descriptor_pca(feature_set->get_descriptor_at(count_feat),
		                             all_pca_desc + count_feat*index_parameters_.ld_pca_dim);
	}

	// Compute Fisher vector
	int gmm_flags = GMM_FLAGS_MU;
	uint fisher_output_length = gmm_fisher_sizeof(index_parameters_.gd_gmm,
	                            gmm_flags);
	gd_fv.clear();
	gd_fv.resize(fisher_output_length, 0);
	// This will extract the FV with only the mean component and using FV normalization
	// but NOT the L2 normalization after the end nor the power normalization
	gmm_fisher_save_soft_assgn(feature_set->get_num_features(), all_pca_desc,
	                           index_parameters_.gd_gmm, gmm_flags, gd_fv.data(),
	                           gd_word_total_soft_assignment.data());

	// Compute L1Norm info
	for (uint count_gaussian = 0;
	     count_gaussian < index_parameters_.gd_number_gaussians;
	     count_gaussian++) {
		double sum_abs = 0;
		for (uint count_dim = count_gaussian*index_parameters_.ld_pca_dim;
		     count_dim < (count_gaussian + 1)*index_parameters_.ld_pca_dim;
		     count_dim++) {
			sum_abs += fabs(gd_fv.at(count_dim));
		}
		gd_word_l1_norm.at(count_gaussian) = static_cast<float>(sum_abs);
	}

	// IN normalization (if selected)
	if (index_parameters_.gd_intra_normalization) {
		for (uint count_gaussian = 0;
		     count_gaussian < index_parameters_.gd_number_gaussians;
		     count_gaussian++) {
			// Compute L2 norm for this Gaussian
			float l2_norm_sq_gaussian = 0;
			for (uint count_dim = count_gaussian*index_parameters_.ld_pca_dim;
			     count_dim < (count_gaussian + 1)*index_parameters_.ld_pca_dim;
			     count_dim++) {
				l2_norm_sq_gaussian += gd_fv.at(count_dim) * gd_fv.at(count_dim);
			}
			// Normalize this Gaussian, if it has non-zero norm
			float l2_norm_gaussian = sqrt(l2_norm_sq_gaussian);
			if (l2_norm_gaussian > 0) {
				for (uint count_dim = count_gaussian*index_parameters_.ld_pca_dim;
				     count_dim < (count_gaussian + 1)*index_parameters_.ld_pca_dim;
				     count_dim++) {
					gd_fv.at(count_dim) /= l2_norm_gaussian;
				}
			}
		}
	}

	// Apply power law (if using SSR normalization), and compute L2 norm
	float l2_norm_sq = 0;
	for (uint count_dim = 0; count_dim < unbinarized_signature_length; count_dim++) {
		if (!index_parameters_.gd_intra_normalization) {
			POWER_LAW_SAME(gd_fv.at(count_dim), index_parameters_.gd_power);
		}
		l2_norm_sq += gd_fv.at(count_dim) * gd_fv.at(count_dim);
	}

	// L2 normalize
	float l2_norm = sqrt(l2_norm_sq);
	if (l2_norm > L2_NORM_SQ_THRESH) {
		for (uint count_dim = 0; count_dim < unbinarized_signature_length; count_dim++) {
			gd_fv.at(count_dim) /= l2_norm;
		}
	}

	// Sign binarize
	sign_binarize(gd_fv, gd_word_descriptor);
}

void gdindex::generate_point_index(const vector<string>& feature_files,
                                   const int verbose_level,
                                   vector < vector < uint > >& vec_feat_assgns,
                                   vector < vector < float > >& vec_feat_assgn_weights,
                                   vector < vector < vector < float > > >& vec_feat_residuals)
{
	uint number_files_to_process = feature_files.size();

	vec_feat_assgns.resize(number_files_to_process);
	vec_feat_assgn_weights.resize(number_files_to_process);
	vec_feat_residuals.resize(number_files_to_process);

	uint number_files_processed = 0;
	#pragma omp parallel for
	for (uint count_file = 0; count_file < number_files_to_process; count_file++) {
		// Load feature set
		if (!common::io_utils::is_file_exist(feature_files.at(count_file))) {
			fprintf(stderr, "Missing feature file: %s\n",
			        feature_files.at(count_file).c_str());
			exit(EXIT_FAILURE);
		}
		common::feature_set feat_set;
		const char *feat_file_path = feature_files[count_file].c_str();
		if (index_parameters_.ld_name == SIFT_NAME) {
			components::sift_reader::read_sift_file(feat_file_path,
			                                        index_parameters_.ld_frame_length,
			                                        index_parameters_.ld_length,feat_set);
		} else if (index_parameters_.ld_name == SIFTGEO_NAME) {
			components::sift_reader::read_sift_geo_file(feat_file_path,
			        index_parameters_.ld_frame_length,
			        index_parameters_.ld_length,feat_set);
		} else {
			cout << "Local feature " << index_parameters_.ld_name
			     << " is not supported" << endl;
		}

		generate_point_indexed_descriptor(&feat_set,
		                                  verbose_level,
		                                  vec_feat_assgns.at(count_file),
		                                  vec_feat_assgn_weights.at(count_file),
		                                  vec_feat_residuals.at(count_file));

		// Report status
		#pragma omp critical
		{
			number_files_processed++;
		}
		if (verbose_level >= 2) {
			printf("[%06d, %06d] %04d features \n",
			       count_file, number_files_processed, feat_set.get_num_features());
		}
	}
}

void gdindex::generate_point_indexed_descriptor(const common::feature_set* feature_set,
        const int verbose_level,
        vector<uint>& feat_assgns,
        vector<float>& feat_assgn_weights,
        vector < vector < float > >& feat_residuals)
{
	unsigned long num_features = feature_set->get_num_features();
	float* all_pca_desc = new float[num_features
	                                * index_parameters_.ld_pca_dim];
	// Project SIFT using PCA
	for (uint count_feat = 0; count_feat < num_features; count_feat++) {
		project_local_descriptor_pca(feature_set->get_descriptor_at(count_feat),
		                             all_pca_desc + count_feat*index_parameters_.ld_pca_dim);
	}

	// Compute point-indexed Fisher vector
	int gmm_flags = GMM_FLAGS_MU;
	uint* p_feat_assgns = new uint[num_features];
	float* p_feat_assgn_weights = new float[num_features];
	float* p_feat_residuals = new float[num_features * LD_PCA_DIM];
	if (verbose_level >= 4) cout << "generate_point_indexed_descriptor: Starting gmm_fisher_point_indexed" << endl;
	gmm_fisher_point_indexed(num_features, all_pca_desc,
	                         index_parameters_.gd_gmm,
	                         gmm_flags, p_feat_assgns,
	                         p_feat_assgn_weights,
	                         p_feat_residuals);
	if (verbose_level >= 4) cout << "done!" << endl;

	// --> Transfer values to output vectors
	feat_assgns.resize(num_features);
	feat_assgn_weights.resize(num_features);
	feat_residuals.resize(num_features);
	for (uint i = 0; i < num_features; i++) {
		feat_assgns.at(i) = p_feat_assgns[i];
		feat_assgn_weights.at(i) = p_feat_assgn_weights[i];
		feat_residuals.at(i).resize(LD_PCA_DIM);
		for (uint j = 0; j < LD_PCA_DIM; j++) {
			feat_residuals.at(i).at(j) = p_feat_residuals[i*LD_PCA_DIM + j];
		}
	}

	// Clean up
	if (all_pca_desc != nullptr) {
		delete [] all_pca_desc;
		all_pca_desc = nullptr;
	}
	if (p_feat_assgns != nullptr) {
		delete [] p_feat_assgns;
		p_feat_assgns = nullptr;
	}
	if (p_feat_assgn_weights != nullptr) {
		delete [] p_feat_assgn_weights;
		p_feat_assgn_weights = nullptr;
	}
	if (p_feat_residuals != nullptr) {
		delete [] p_feat_residuals;
		p_feat_residuals = nullptr;
	}
}

void gdindex::perform_query(const string local_descriptors_path,
                            const gdindex* query_index_ptr,
                            const uint query_number,
                            const vector<uint>& indices,
                            vector< pair<float,uint> >& results,
                            const uint number_2nd_stage_rerank,
                            gdindex* gdindex_ptr_rerank,
                            const vector < vector < uint > >& group_lists_rerank,
                            const int verbose_level)
{
	common::feature_set feat_set;
	vector<uint> gd_word_descriptor;
	vector<float> gd_fv;
	vector<float> gd_word_l1_norm, gd_word_total_soft_assignment;
	if (query_index_ptr != nullptr) {
		// Using pre-computed global descriptor from query_index_ptr
		if (index_parameters_.gd_unbinarized) {
			gd_fv = query_index_ptr->index_.fv.at(query_number);
		} else {
			gd_word_descriptor =
			    query_index_ptr->index_.word_descriptor.at(query_number);
		}
		if (query_parameters_.word_selection_mode == WORD_L1_NORM) {
			gd_word_l1_norm =
			    query_index_ptr->index_.word_l1_norms.at(query_number);
		} else {
			gd_word_total_soft_assignment =
			    query_index_ptr->index_.word_total_soft_assignment.at(query_number);
		}
	} else {
		// Computing query global descriptor

		// --> Load local descriptors
		if (index_parameters_.ld_name == SIFT_NAME) {
			components::sift_reader::read_sift_file(local_descriptors_path.c_str(),
			                                        index_parameters_.ld_frame_length,
			                                        index_parameters_.ld_length,feat_set);
		} else if (index_parameters_.ld_name == SIFTGEO_NAME) {
			components::sift_reader::read_sift_geo_file(local_descriptors_path.c_str(),
			        index_parameters_.ld_frame_length,
			        index_parameters_.ld_length,feat_set);
		} else {
			cout << "Local feature " << index_parameters_.ld_name
			     << " is not supported" << endl;
		}

		// --> Generate query global descriptor
		generate_global_descriptor(&feat_set,
		                           gd_word_descriptor,
		                           gd_fv,
		                           gd_word_l1_norm,
		                           gd_word_total_soft_assignment);
	}

	// If number_2nd_stage_rerank is 0, we're not using two-stage scoring
	if (!number_2nd_stage_rerank) {
		query(gd_word_descriptor, gd_fv, gd_word_l1_norm,
		      gd_word_total_soft_assignment, indices, results);
	} else {
		// Using two-stage scoring
		// -- First stage
		vector<uint> indices_2_stages(index_.number_global_descriptors);
		for (uint count = 0; count < index_.number_global_descriptors; count++) {
			// Include indices corresponding to shot numbers
			indices_2_stages.at(count) = count;
		}
		vector< pair<float,uint> > results_1st_stage;
		query(gd_word_descriptor, gd_fv, gd_word_l1_norm,
		      gd_word_total_soft_assignment,
		      indices_2_stages, results_1st_stage);

		// -- Second stage
		vector<uint> gd_word_descriptor_rerank;
		vector<float> gd_fv_rerank;
		vector<float> gd_word_l1_norm_rerank, gd_word_total_soft_assignment_rerank;
		gdindex_ptr_rerank->generate_global_descriptor(&feat_set,
		        gd_word_descriptor_rerank,
		        gd_fv_rerank,
		        gd_word_l1_norm_rerank,
		        gd_word_total_soft_assignment_rerank);
		gdindex_ptr_rerank->query_2nd_stage(gd_word_descriptor_rerank,
		                                    gd_fv_rerank,
		                                    gd_word_l1_norm_rerank,
		                                    gd_word_total_soft_assignment_rerank,
		                                    number_2nd_stage_rerank,
		                                    group_lists_rerank,
		                                    results_1st_stage, results);
	}
}

void gdindex::set_index_parameters(const uint ld_length, const uint ld_frame_length,
                                   const string ld_extension, const string ld_name,
                                   const uint ld_pca_dim, const float ld_pre_pca_power,
                                   const uint gd_number_gaussians, const float gd_power,
                                   const bool gd_intra_normalization,
                                   const bool gd_unbinarized,
                                   const string trained_parameters_path,
                                   const int verbose_level)
{
	// Local descriptor information
	index_parameters_.ld_length = ld_length;
	index_parameters_.ld_frame_length = ld_frame_length;
	index_parameters_.ld_extension = ld_extension;
	index_parameters_.ld_name = ld_name;

	// Parameters for PCA-ing local descriptors
	index_parameters_.ld_pca_dim = ld_pca_dim;
	index_parameters_.ld_pre_pca_power = ld_pre_pca_power;
	// -- LD mean vector is stored in descriptor covariance file
	char aux_mean_vector_path[1024];
	sprintf(aux_mean_vector_path, "%s/%s.pre_alpha.%.2f.desc_covariance",
	        trained_parameters_path.c_str(),
	        index_parameters_.ld_name.c_str(),
	        index_parameters_.ld_pre_pca_power);
	string ld_mean_vector_path = aux_mean_vector_path;
	load_ld_mean_vector(ld_mean_vector_path);

	char aux_ld_pca_eigenvectors[1024];
	sprintf(aux_ld_pca_eigenvectors, "%s/%s.pre_alpha.%.2f.desc_eigenvectors",
	        trained_parameters_path.c_str(),
	        index_parameters_.ld_name.c_str(),
	        index_parameters_.ld_pre_pca_power);
	string ld_pca_eigenvectors_path = aux_ld_pca_eigenvectors;
	load_ld_pca_eigenvectors(ld_pca_eigenvectors_path);

	// Parameters used for global descriptor computation
	index_parameters_.gd_number_gaussians = gd_number_gaussians;
	index_parameters_.gd_power = gd_power;
	index_parameters_.gd_intra_normalization = gd_intra_normalization;
	index_parameters_.gd_unbinarized = gd_unbinarized;

	char aux_gd_gmm[1024];
	sprintf(aux_gd_gmm, "%s/%s.pre_alpha.%.2f.pca.%d.gmm.%d",
	        trained_parameters_path.c_str(),
	        index_parameters_.ld_name.c_str(),
	        index_parameters_.ld_pre_pca_power,
	        index_parameters_.ld_pca_dim,
	        index_parameters_.gd_number_gaussians);
	string gd_gmm_path = aux_gd_gmm;
	load_gd_gmm(gd_gmm_path);
}

void gdindex::set_query_parameters(const uint min_number_words_selected,
                                   const int asym_scoring_mode,
                                   const int word_selection_mode,
                                   const float word_selection_thresh,
                                   const float score_den_power_norm,
                                   const string trained_parameters_path,
                                   const int verbose_level)
{
	query_parameters_.min_number_words_selected = min_number_words_selected;
	query_parameters_.asym_scoring_mode = asym_scoring_mode;
	query_parameters_.word_selection_mode = word_selection_mode;
	query_parameters_.word_selection_thresh = word_selection_thresh;
	query_parameters_.score_den_power_norm = score_den_power_norm;

	char aux_corr_weights[1024];
	sprintf(aux_corr_weights, "%s/%s.pre_alpha.%.2f.pca.%d.gmm.%d.pre_alpha.%.2f.corr_weights",
	        trained_parameters_path.c_str(),
	        index_parameters_.ld_name.c_str(),
	        index_parameters_.ld_pre_pca_power,
	        index_parameters_.ld_pca_dim,
	        index_parameters_.gd_number_gaussians,
	        index_parameters_.gd_power);
	string corr_weights_path = aux_corr_weights;
	load_corr_weights(corr_weights_path);

	// Precompute pop-counts for fast executions
	query_parameters_.pop_count[0] = 0;
	for (int i = 1; i < 65536; i++) {
		query_parameters_.pop_count[i] = query_parameters_.pop_count[i>>1] + (i&1);
	}
}

/********************************
PRIVATE FUNCTIONS
********************************/

void gdindex::update_index()
{
	// Update number of global descriptors stored
	if (index_parameters_.gd_unbinarized) {
		index_.number_global_descriptors = index_.fv.size();
	} else {
		index_.number_global_descriptors = index_.word_descriptor.size();
	}

	// Update variables that are useful during retrieval; they will be ready
	// to serve a query, if perform_query() is called
	index_.number_words_selected.resize(index_.number_global_descriptors, 0);
	if (index_parameters_.gd_unbinarized) {
		index_.word_l2_norms_sq.resize(index_.number_global_descriptors,
		                               vector<float>(index_parameters_.gd_number_gaussians,
		                                       0));
	}
	const vector < vector < float > > *all_db_strengths;
	if (query_parameters_.word_selection_mode == WORD_L1_NORM) {
		all_db_strengths = &index_.word_l1_norms;
	} else {
		all_db_strengths = &index_.word_total_soft_assignment;
	}
	for (uint count_elem = 0; count_elem < index_.number_global_descriptors;
	     count_elem++) {
		// -- Find number of words (Gaussians) selected for this database element
		for (uint count_gaussian = 0;
		     count_gaussian < index_parameters_.gd_number_gaussians;
		     count_gaussian++) {
			if (query_parameters_.asym_scoring_mode == ASYM_OFF
			    || query_parameters_.asym_scoring_mode == ASYM_QAGS) {
				index_.number_words_selected.at(count_elem)++;
			} else if (query_parameters_.asym_scoring_mode == ASYM_DAGS
			           || query_parameters_.asym_scoring_mode == ASYM_SGS) {
				if (all_db_strengths->at(count_elem).at(count_gaussian) >
				    query_parameters_.word_selection_thresh) {
					index_.number_words_selected.at(count_elem)++;
				}
			}
			if (index_parameters_.gd_unbinarized) {
				for (uint d = 0; d < index_parameters_.ld_pca_dim; d++) {
					index_.word_l2_norms_sq.at(count_elem).at(count_gaussian) +=
					    index_.fv.at(count_elem).at(count_gaussian*
					                                index_parameters_.ld_pca_dim
					                                + d)
					    *index_.fv.at(count_elem).at(count_gaussian*
					                                 index_parameters_.ld_pca_dim
					                                 + d);
				}
			}
		}
	}
}

void gdindex::sign_binarize(const vector<float>& gd_word_residuals,
                            vector<uint>& gd_word_descriptor)
{
	gd_word_descriptor.resize(index_parameters_.gd_number_gaussians);
	for (uint count_gaussian = 0;
	     count_gaussian < index_parameters_.gd_number_gaussians;
	     count_gaussian++) {
		uint packed_block = 0;
		uint start = count_gaussian*index_parameters_.ld_pca_dim;
		uint end = (count_gaussian + 1)*index_parameters_.ld_pca_dim;
		// Sign binarize
		for (uint count_dim = start; count_dim < end; count_dim++) {
			uint bit = (gd_word_residuals.at(count_dim) > 0) ? 1 : 0;
			PUSH_BIT(packed_block, bit);
		}
		gd_word_descriptor.at(count_gaussian) = packed_block;
	}
}

void gdindex::project_local_descriptor_pca(const float* desc, float* pca_desc)
{
	vector < float > desc_pow(index_parameters_.ld_length, 0);
	// Pre power law and normalization
	float l2_norm_sq = 0;
	for (uint count_in_dim = 0; count_in_dim < index_parameters_.ld_length; count_in_dim++) {
		POWER_LAW(desc[count_in_dim], index_parameters_.ld_pre_pca_power,
		          desc_pow.at(count_in_dim));
		l2_norm_sq += desc_pow.at(count_in_dim) * desc_pow.at(count_in_dim);
	}
	float l2_norm = sqrt(l2_norm_sq);
	if (l2_norm > 0) {
		for (uint count_in_dim = 0; count_in_dim < index_parameters_.ld_length; count_in_dim++) {
			desc_pow.at(count_in_dim) /= l2_norm;
		}
	}

	// Projection onto eigenvectors
	for (uint count_out_dim = 0; count_out_dim < index_parameters_.ld_pca_dim;
	     count_out_dim++) {
		pca_desc[count_out_dim] = 0;
		for (uint count_in_dim = 0; count_in_dim < index_parameters_.ld_length;
		     count_in_dim++) {
			pca_desc[count_out_dim] +=
			    (desc_pow.at(count_in_dim) - index_parameters_.ld_mean_vector[count_in_dim])
			    * index_parameters_.ld_pca_eigenvectors.at(count_out_dim)[count_in_dim];
		}
	}
}

void gdindex::sample_frames_from_shot(const uint number_frames_out,
                                      const uint first_frame,
                                      const uint number_frames_this_shot,
                                      vector<uint>& out_frames)
{
	// Note: number_frames_this_shot must be bigger than number_frames_out
	out_frames.clear();
	if (number_frames_out == 1) {
		uint middle_frame = first_frame
		                    + static_cast<uint>(floor(static_cast<float>(number_frames_this_shot)/2));
		out_frames.push_back(middle_frame);
	} else {
		double rate_float = static_cast<double>(number_frames_this_shot)
		                    /static_cast<double>(number_frames_out);
		uint rate;
		if ((number_frames_this_shot % number_frames_out) == 0) {
			rate = static_cast<uint>(rate_float);
			for (uint count_sample = 0; count_sample < number_frames_this_shot;
			     count_sample += rate) {
				out_frames.push_back(first_frame + count_sample);
			}
		} else {
			if ((number_frames_out - 1)*static_cast<uint>(ceil(rate_float))
			    < number_frames_this_shot) {
				rate = static_cast<uint>(ceil(rate_float));
				for (uint count_sample = 0; count_sample < number_frames_this_shot;
				     count_sample += rate) {
					out_frames.push_back(first_frame + count_sample);
				}
			} else {
				rate = static_cast<uint>(floor(rate_float));
				vector<uint> out_frames_aux;
				for (uint count_sample = 0; count_sample < number_frames_this_shot;
				     count_sample += rate) {
					out_frames_aux.push_back(first_frame + count_sample);
				}
				uint start_ind, end_ind;
				uint extra_frames = (out_frames_aux.size() - number_frames_out);

				if (extra_frames) {
					// In this case, we'll keep the middle ones, in order not to unbalance
					// the selection too much
					if (extra_frames % 2) {
						// Odd: keep one more at the beginning
						start_ind = (extra_frames - 1)/2;
						end_ind = out_frames_aux.size() - 1
						          - static_cast<uint>(ceil((static_cast<double>(extra_frames)/2)));
					} else {
						// Even
						start_ind = extra_frames/2;
						end_ind = out_frames_aux.size() - 1 - extra_frames/2;
					}

					for (uint count_sample = start_ind; count_sample < end_ind + 1;
					     count_sample++) {
						out_frames.push_back(out_frames_aux.at(count_sample));
					}
				} else {
					out_frames = out_frames_aux;
				}
			}
		}
	}
	// Make sure we've collected the correct number
	assert(number_frames_out == out_frames.size());
}

void gdindex::score_database_item(const vector<uint>& query_word_descriptor,
                                  const vector<float>& query_fv,
                                  const vector<float>& query_word_l1_norm,
                                  const vector<float>& query_word_total_soft_assignment,
                                  const vector<float>& query_word_l2_norm_sq,
                                  const uint db_ind,
                                  float& score)
{
	// Check that the database item has at least the minimum number of
	// selected words that we require; if not, just set it to FLT_MAX
	if (index_.number_words_selected.at(db_ind)
	    <= query_parameters_.min_number_words_selected) {
		score = FLT_MAX;
		return;
	}

	// Figure out parameters for asymmetric scoring
	const vector<float> *q_strengths, *db_strengths;
	if (query_parameters_.word_selection_mode == WORD_L1_NORM) {
		q_strengths = &query_word_l1_norm;
		db_strengths = &index_.word_l1_norms.at(db_ind);
	} else {
		q_strengths = &query_word_total_soft_assignment;
		db_strengths = &index_.word_total_soft_assignment.at(db_ind);
	}

	float total_inner_product = 0;
	float query_norm_factor = 0;
	float db_norm_factor = 0;
	float qags_db_norm_factor = 0;
	for (uint count_gaussian = 0;
	     count_gaussian < index_parameters_.gd_number_gaussians;
	     count_gaussian++) {
		bool use_curr_gaussian = false;
		if (query_parameters_.asym_scoring_mode == ASYM_OFF) {
			if (index_parameters_.gd_unbinarized) {
				query_norm_factor += query_word_l2_norm_sq.at(count_gaussian);
				db_norm_factor += index_.word_l2_norms_sq.at(db_ind)
				                  .at(count_gaussian);
			} else {
				query_norm_factor += index_parameters_.ld_pca_dim;
				db_norm_factor += index_parameters_.ld_pca_dim;
			}
			use_curr_gaussian = true;
		} else if (query_parameters_.asym_scoring_mode == ASYM_QAGS) {
			if (q_strengths->at(count_gaussian) >
			    query_parameters_.word_selection_thresh) {
				if (index_parameters_.gd_unbinarized) {
					query_norm_factor += query_word_l2_norm_sq.at(count_gaussian);
					db_norm_factor += index_.word_l2_norms_sq.at(db_ind)
					                  .at(count_gaussian);
				} else {
					query_norm_factor += index_parameters_.ld_pca_dim;
					db_norm_factor += index_parameters_.ld_pca_dim;
				}
				use_curr_gaussian = true;
			}
			if (db_strengths->at(count_gaussian) >
			    query_parameters_.word_selection_thresh) {
				if (index_parameters_.gd_unbinarized) {
					qags_db_norm_factor += index_.word_l2_norms_sq.at(db_ind)
					                       .at(count_gaussian);
				} else {
					qags_db_norm_factor += index_parameters_.ld_pca_dim;
				}
			}
		} else if (query_parameters_.asym_scoring_mode == ASYM_DAGS) {
			if (db_strengths->at(count_gaussian) >
			    query_parameters_.word_selection_thresh) {
				if (index_parameters_.gd_unbinarized) {
					query_norm_factor += query_word_l2_norm_sq.at(count_gaussian);
					db_norm_factor += index_.word_l2_norms_sq.at(db_ind)
					                  .at(count_gaussian);
				} else {
					query_norm_factor += index_parameters_.ld_pca_dim;
					db_norm_factor += index_parameters_.ld_pca_dim;
				}
				use_curr_gaussian = true;
			}
		} else if (query_parameters_.asym_scoring_mode == ASYM_SGS) {
			if (q_strengths->at(count_gaussian) >
			    query_parameters_.word_selection_thresh) {
				if (index_parameters_.gd_unbinarized) {
					query_norm_factor += query_word_l2_norm_sq.at(count_gaussian);
				} else {
					query_norm_factor += index_parameters_.ld_pca_dim;
				}
				if (db_strengths->at(count_gaussian) >
				    query_parameters_.word_selection_thresh) {
					if (index_parameters_.gd_unbinarized) {
						db_norm_factor += index_.word_l2_norms_sq.at(db_ind)
						                  .at(count_gaussian);
					} else {
						db_norm_factor += index_parameters_.ld_pca_dim;
					}
					use_curr_gaussian = true;
				}
			} else if (db_strengths->at(count_gaussian) >
			           query_parameters_.word_selection_thresh) {
				if (index_parameters_.gd_unbinarized) {
					db_norm_factor += index_.word_l2_norms_sq.at(db_ind)
					                  .at(count_gaussian);
				} else {
					db_norm_factor += index_parameters_.ld_pca_dim;
				}
			}
		}

		if (use_curr_gaussian) {
			if (index_parameters_.gd_unbinarized) {
				for (uint d = count_gaussian*index_parameters_.ld_pca_dim;
				     d < (count_gaussian+1)*index_parameters_.ld_pca_dim;
				     d++) {
					total_inner_product += query_fv.at(d)
					                       *index_.fv.at(db_ind).at(d);
				}
			} else {
				// Compute Hamming distance
				uint aux = query_word_descriptor.at(count_gaussian)
				           ^ index_.word_descriptor.at(db_ind).at(count_gaussian);
				uint h = query_parameters_.pop_count[aux >> 16]
				         + query_parameters_.pop_count[aux & 65535];
				// Add to correlation
				total_inner_product += query_parameters_.fast_corr_weights[h];
			}
		}
	}

	// Compute final score, taking into account normalizations
	if (query_parameters_.asym_scoring_mode == ASYM_QAGS) {
		if (query_norm_factor != 0
		    && qags_db_norm_factor != 0
		    && db_norm_factor != 0) {
			float score_den = pow(query_norm_factor*db_norm_factor,
			                      0.5);
			total_inner_product /= score_den;
			total_inner_product *= pow(qags_db_norm_factor,
			                           0.5 - query_parameters_.score_den_power_norm);
		} else {
			total_inner_product = -FLT_MAX;
		}
	} else {
		if (query_norm_factor != 0
		    && db_norm_factor != 0) {
			float score_den = pow(query_norm_factor*db_norm_factor,
			                      query_parameters_.score_den_power_norm);
			total_inner_product /= score_den;
		} else {
			total_inner_product = -FLT_MAX;
		}
	}

	// Change sign such that smaller is better
	score = -total_inner_product;
}

void gdindex::query(const vector<uint>& query_word_descriptor,
                    const vector<float>& query_fv,
                    const vector<float>& query_word_l1_norm,
                    const vector<float>& query_word_total_soft_assignment,
                    const vector<uint>& database_indices,
                    vector< pair<float,uint> >& database_scores_indices)
{
	assert(index_.number_global_descriptors == database_indices.size());
	// Resize vector that will be passed back
	database_scores_indices.resize(index_.number_global_descriptors);

	// If using FV, compute per-Gaussian L2 norm
	vector<float> query_word_l2_norm_sq(index_parameters_.gd_number_gaussians, 0);
	if (index_parameters_.gd_unbinarized) {
		for (uint count_gaussian = 0;
		     count_gaussian < index_parameters_.gd_number_gaussians;
		     count_gaussian++) {
			for (uint d = count_gaussian*index_parameters_.ld_pca_dim;
			     d < (count_gaussian+1)*index_parameters_.ld_pca_dim;
			     d++) {
				query_word_l2_norm_sq.at(count_gaussian) +=
				    query_fv.at(d)*query_fv.at(d);
			}
		}
	}

	// Loop over database items and get their scores
	for (uint count_elem = 0; count_elem < index_.number_global_descriptors;
	     count_elem++) {
		uint number_item = database_indices.at(count_elem);
		database_scores_indices.at(count_elem).second = number_item;

		score_database_item(query_word_descriptor,
		                    query_fv,
		                    query_word_l1_norm,
		                    query_word_total_soft_assignment,
		                    query_word_l2_norm_sq,
		                    count_elem,
		                    database_scores_indices.at(count_elem).first);
	}
	// Sort scores
	sort(database_scores_indices.begin(), database_scores_indices.end(),
	     cmp_float_uint_ascend);
}

void gdindex::query_2nd_stage(const vector<uint>& query_word_descriptor,
                              const vector<float>& query_fv,
                              const vector<float>& query_word_l1_norm,
                              const vector<float>& query_word_total_soft_assignment,
                              const uint number_2nd_stage_rerank,
                              const vector < vector < uint > >& group_lists_rerank,
                              const vector< pair<float,uint> >& first_stage_scores_indices,
                              vector< pair<float,uint> >& database_scores_indices)
{
	// Resize vector that will be passed back
	database_scores_indices.clear();

	// If using FV, compute per-Gaussian L2 norm
	vector<float> query_word_l2_norm_sq(index_parameters_.gd_number_gaussians, 0);
	if (index_parameters_.gd_unbinarized) {
		for (uint count_gaussian = 0;
		     count_gaussian < index_parameters_.gd_number_gaussians;
		     count_gaussian++) {
			for (uint d = count_gaussian*index_parameters_.ld_pca_dim;
			     d < (count_gaussian+1)*index_parameters_.ld_pca_dim;
			     d++) {
				query_word_l2_norm_sq.at(count_gaussian) +=
				    query_fv.at(d)*query_fv.at(d);
			}
		}
	}

	// Figure out number of first stage items (groups) to re-rank
	uint number_rerank = min(number_2nd_stage_rerank,
	                         static_cast<uint>(first_stage_scores_indices.size()));

	// Loop over top number_rerank items from first stage and get score
	// of the constituent second stage items
	for (uint count_top = 0; count_top < number_rerank;
	     count_top++) {
		uint this_group_ind = first_stage_scores_indices.at(count_top).second;

		// Get number of 2nd stage items in this group
		uint number_items_this_group = group_lists_rerank.at(this_group_ind).size();

		// Loop over 2nd stage items in this group, get their signatures and score them
		for (uint count_item = 0; count_item < number_items_this_group;
		     count_item++) {
			uint this_item_number = group_lists_rerank.at(this_group_ind).at(count_item);
			pair < float, uint > score_this_item;
			score_this_item.second = this_item_number;

			score_database_item(query_word_descriptor,
			                    query_fv,
			                    query_word_l1_norm,
			                    query_word_total_soft_assignment,
			                    query_word_l2_norm_sq,
			                    this_item_number,
			                    score_this_item.first);
			database_scores_indices.push_back(score_this_item);
		}
	}
	// Sort scores
	sort(database_scores_indices.begin(), database_scores_indices.end(),
	     cmp_float_uint_ascend);
}

void gdindex::load_ld_mean_vector(string path)
{
	FILE* mv_file = fopen(path.c_str(), "rb");
	if (mv_file == nullptr) {
		printf("gdindex::load_ld_mean_vector: Error opening: \n%s \n", path.c_str());
		exit(EXIT_FAILURE);
	}
	index_parameters_.ld_mean_vector = new float[index_parameters_.ld_length];
	fread(index_parameters_.ld_mean_vector, sizeof(float),
	      index_parameters_.ld_length, mv_file);
	fclose(mv_file);
}

void gdindex::load_ld_pca_eigenvectors(string path)
{
	FILE* eig_file = fopen(path.c_str(), "rb");
	if (eig_file == nullptr) {
		printf("gdindex::load_ld_pca_eigenvectors: Error opening: \n%s \n", path.c_str());
		exit(EXIT_FAILURE);
	}
	index_parameters_.ld_pca_eigenvectors.resize(index_parameters_.ld_length, nullptr);
	for (uint n = 0; n < index_parameters_.ld_length; n++) {
		index_parameters_.ld_pca_eigenvectors.at(n)
		    = new float[index_parameters_.ld_length];
		fread(index_parameters_.ld_pca_eigenvectors.at(n), sizeof(float),
		      index_parameters_.ld_length, eig_file);
	}
	fclose(eig_file);
}

void gdindex::load_gd_gmm(string path)
{
	FILE* gmm_file = fopen(path.c_str(), "rb");
	if (gmm_file == nullptr) {
		printf("gdindex::load_gd_gmm: Error opening: \n%s \n", path.c_str());
		exit(EXIT_FAILURE);
	}
	index_parameters_.gd_gmm = gmm_read(gmm_file);
	fclose(gmm_file);
}

void gdindex::load_corr_weights(string path)
{
	FILE* cw_file = fopen(path.c_str(), "rb");
	if (cw_file == nullptr) {
		printf("gdindex::load_corr_weights: Error opening: \n%s \n", path.c_str());
		exit(EXIT_FAILURE);
	}
	float* weights = new float[index_parameters_.ld_pca_dim + 1];
	fread(weights, sizeof(float), index_parameters_.ld_pca_dim + 1,
	      cw_file);
	fclose(cw_file);

	// Build fast_corr_weights
	query_parameters_.fast_corr_weights = new float[index_parameters_.ld_pca_dim + 1];
	// -- initialize them to zero
	for (uint i = 0; i < index_parameters_.ld_pca_dim + 1; i++) {
		query_parameters_.fast_corr_weights[i] = 0;
	}

	for (uint count_bin = 0; count_bin < index_parameters_.ld_pca_dim + 1; count_bin++) {
		float prob = weights[index_parameters_.ld_pca_dim - count_bin]/2.0; // normalize to 1
		float corr = index_parameters_.ld_pca_dim - 2*count_bin;
		if (count_bin < CORR_WEIGHTS_CLIPPING) {
			query_parameters_.fast_corr_weights[count_bin] = prob * corr;
		}
	}

	// Clean up
	if (weights != nullptr) {
		delete [] weights;
		weights = nullptr;
	}
}

}
} /* namespace vrs */


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_X86_XOP_SIMD_FUNCTION_SHIFT_RIGHT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_XOP_SIMD_FUNCTION_SHIFT_RIGHT_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/detail/assert_utils.hpp>
#include <boost/simd/function/rshr.hpp>

#if BOOST_HW_SIMD_X86_AMD_XOP
namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD_IF( shift_right_
                            , (typename A0, typename A1)
                            , (brigand::bool_<A0::static_size == A1::static_size>)
                            , bs::avx_
                            , bs::pack_< bd::integer_<A0>, bs::sse_>
                            , bs::pack_< bd::integer_<A1>, bs::sse_>
                            )
  {
    BOOST_FORCEINLINE A0 operator()(A0 const& a0,A1 const& a1) const
    {
      BOOST_ASSERT_MSG( assert_good_shift<A0>(a1)
                      , "boost::simd::shift_right: shift value is out of range"
                      );

      return rshr(a0,a1);
    }
  };
} } }
#endif

#endif


#include "extractor/guidance/turn_handler.hpp"
#include "extractor/guidance/constants.hpp"
#include "extractor/guidance/intersection_scenario_three_way.hpp"
#include "extractor/guidance/toolkit.hpp"

#include "util/guidance/toolkit.hpp"

#include <limits>
#include <utility>

#include <boost/assert.hpp>

using EdgeData = osrm::util::NodeBasedDynamicGraph::EdgeData;
using osrm::util::guidance::getTurnDirection;
using osrm::util::guidance::angularDeviation;

namespace osrm
{
namespace extractor
{
namespace guidance
{

TurnHandler::TurnHandler(const util::NodeBasedDynamicGraph &node_based_graph,
                         const std::vector<QueryNode> &node_info_list,
                         const util::NameTable &name_table,
                         const SuffixTable &street_name_suffix_table)
    : IntersectionHandler(node_based_graph, node_info_list, name_table, street_name_suffix_table)
{
}

bool TurnHandler::canProcess(const NodeID, const EdgeID, const Intersection &) const
{
    return true;
}

Intersection TurnHandler::
operator()(const NodeID, const EdgeID via_edge, Intersection intersection) const
{
    if (intersection.size() == 1)
        return handleOneWayTurn(std::move(intersection));

    if (intersection[0].entry_allowed)
    {
        intersection[0].turn.instruction = {findBasicTurnType(via_edge, intersection[0]),
                                            DirectionModifier::UTurn};
    }

    if (intersection.size() == 2)
        return handleTwoWayTurn(via_edge, std::move(intersection));

    if (intersection.size() == 3)
        return handleThreeWayTurn(via_edge, std::move(intersection));

    return handleComplexTurn(via_edge, std::move(intersection));
}

Intersection TurnHandler::handleOneWayTurn(Intersection intersection) const
{
    BOOST_ASSERT(intersection[0].turn.angle < 0.001);
    return intersection;
}

Intersection TurnHandler::handleTwoWayTurn(const EdgeID via_edge, Intersection intersection) const
{
    BOOST_ASSERT(intersection[0].turn.angle < 0.001);
    intersection[1].turn.instruction =
        getInstructionForObvious(intersection.size(), via_edge, false, intersection[1]);

    return intersection;
}

bool TurnHandler::isObviousOfTwo(const EdgeID via_edge,
                                 const ConnectedRoad &road,
                                 const ConnectedRoad &other) const
{
    const auto &in_data = node_based_graph.GetEdgeData(via_edge);

    const auto &first_data = node_based_graph.GetEdgeData(road.turn.eid);
    const auto &second_data = node_based_graph.GetEdgeData(other.turn.eid);
    const auto &first_classification = first_data.road_classification;
    const auto &second_classification = second_data.road_classification;
    const bool is_ramp = first_classification.IsRampClass();
    const bool is_obvious_by_road_class =
        (!is_ramp &&
         (2 * first_classification.GetPriority() < second_classification.GetPriority()) &&
         in_data.road_classification == first_classification) ||
        (!first_classification.IsLowPriorityRoadClass() &&
         second_classification.IsLowPriorityRoadClass());

    if (is_obvious_by_road_class)
        return true;

    const bool other_is_obvious_by_road_class =
        (!second_classification.IsRampClass() &&
         (2 * second_classification.GetPriority() < first_classification.GetPriority()) &&
         in_data.road_classification == second_classification) ||
        (!second_classification.IsLowPriorityRoadClass() &&
         first_classification.IsLowPriorityRoadClass());

    if (other_is_obvious_by_road_class)
        return false;

    const bool turn_is_perfectly_straight =
        angularDeviation(road.turn.angle, STRAIGHT_ANGLE) < std::numeric_limits<double>::epsilon();

    if (turn_is_perfectly_straight && in_data.name_id != EMPTY_NAMEID &&
        in_data.name_id == node_based_graph.GetEdgeData(road.turn.eid).name_id)
        return true;

    const bool is_much_narrower_than_other =
        angularDeviation(other.turn.angle, STRAIGHT_ANGLE) /
                angularDeviation(road.turn.angle, STRAIGHT_ANGLE) >
            INCREASES_BY_FOURTY_PERCENT &&
        angularDeviation(angularDeviation(other.turn.angle, STRAIGHT_ANGLE),
                         angularDeviation(road.turn.angle, STRAIGHT_ANGLE)) >
            FUZZY_ANGLE_DIFFERENCE;

    return is_much_narrower_than_other;
}

Intersection TurnHandler::handleThreeWayTurn(const EdgeID via_edge, Intersection intersection) const
{
    const auto &in_data = node_based_graph.GetEdgeData(via_edge);
    const auto &first_data = node_based_graph.GetEdgeData(intersection[1].turn.eid);
    const auto &second_data = node_based_graph.GetEdgeData(intersection[2].turn.eid);
    BOOST_ASSERT(intersection[0].turn.angle < 0.001);
    /* Two nearly straight turns -> FORK
               OOOOOOO
             /
      IIIIII
             \
               OOOOOOO
     */
    const auto fork_range = findFork(via_edge, intersection);
    if (fork_range.first == 1 && fork_range.second == 2)
        assignFork(via_edge, intersection[2], intersection[1]);

    /*  T Intersection

        OOOOOOO T OOOOOOOO
                I
                I
                I
     */
    else if (isEndOfRoad(intersection[0], intersection[1], intersection[2]) &&
             !isObviousOfTwo(via_edge, intersection[1], intersection[2]) &&
             !isObviousOfTwo(via_edge, intersection[2], intersection[1]))
    {
        if (intersection[1].entry_allowed)
        {
            if (TurnType::OnRamp != findBasicTurnType(via_edge, intersection[1]))
                intersection[1].turn.instruction = {TurnType::EndOfRoad, DirectionModifier::Right};
            else
                intersection[1].turn.instruction = {TurnType::OnRamp, DirectionModifier::Right};
        }
        if (intersection[2].entry_allowed)
        {
            if (TurnType::OnRamp != findBasicTurnType(via_edge, intersection[2]))

                intersection[2].turn.instruction = {TurnType::EndOfRoad, DirectionModifier::Left};
            else
                intersection[2].turn.instruction = {TurnType::OnRamp, DirectionModifier::Left};
        }
    }
    else
    {
        if (isObviousOfTwo(via_edge, intersection[1], intersection[2]) &&
            (in_data.name_id != second_data.name_id || first_data.name_id == second_data.name_id))
        {
            intersection[1].turn.instruction = getInstructionForObvious(
                3, via_edge, isThroughStreet(1, intersection), intersection[1]);
        }
        else
        {
            intersection[1].turn.instruction = {findBasicTurnType(via_edge, intersection[1]),
                                                getTurnDirection(intersection[1].turn.angle)};
        }

        if (isObviousOfTwo(via_edge, intersection[2], intersection[1]) &&
            (in_data.name_id != first_data.name_id || first_data.name_id == second_data.name_id))
        {
            intersection[2].turn.instruction = getInstructionForObvious(
                3, via_edge, isThroughStreet(2, intersection), intersection[2]);
        }
        else
        {
            intersection[2].turn.instruction = {findBasicTurnType(via_edge, intersection[2]),
                                                getTurnDirection(intersection[2].turn.angle)};
        }
    }
    return intersection;
}

Intersection TurnHandler::handleComplexTurn(const EdgeID via_edge, Intersection intersection) const
{
    const std::size_t obvious_index = findObviousTurn(via_edge, intersection);
    const auto fork_range = findFork(via_edge, intersection);
    std::size_t straightmost_turn = 0;
    double straightmost_deviation = 180;
    for (std::size_t i = 0; i < intersection.size(); ++i)
    {
        const double deviation = angularDeviation(intersection[i].turn.angle, STRAIGHT_ANGLE);
        if (deviation < straightmost_deviation)
        {
            straightmost_deviation = deviation;
            straightmost_turn = i;
        }
    }

    // check whether there is a turn of the same name
    const auto &in_data = node_based_graph.GetEdgeData(via_edge);

    const bool has_same_name_turn = [&]() {
        for (std::size_t i = 1; i < intersection.size(); ++i)
        {
            if (node_based_graph.GetEdgeData(intersection[i].turn.eid).name_id == in_data.name_id)
                return true;
        }
        return false;
    }();

    // check whether the obvious choice is actually a through street
    if (obvious_index != 0)
    {
        intersection[obvious_index].turn.instruction =
            getInstructionForObvious(intersection.size(),
                                     via_edge,
                                     isThroughStreet(obvious_index, intersection),
                                     intersection[obvious_index]);
        if (has_same_name_turn &&
            node_based_graph.GetEdgeData(intersection[obvious_index].turn.eid).name_id !=
                in_data.name_id &&
            intersection[obvious_index].turn.instruction.type == TurnType::NewName)
        {
            // this is a special case that is necessary to correctly handle obvious turns on
            // continuing streets. Right now osrm does not know about right of way. If a street
            // turns to the left just like:
            //
            //       a
            //       a
            // aaaaaaa b b
            //
            // And another road exits here, we don't want to call it a new name, even though the
            // turn is obvious and does not require steering. To correctly handle these situations
            // in turn collapsing, we use the turn + straight combination here
            intersection[obvious_index].turn.instruction.type = TurnType::Turn;
            intersection[obvious_index].turn.instruction.direction_modifier =
                DirectionModifier::Straight;
        }

        // assign left/right turns
        intersection = assignLeftTurns(via_edge, std::move(intersection), obvious_index + 1);
        intersection = assignRightTurns(via_edge, std::move(intersection), obvious_index);
    }
    else if (fork_range.first != 0 && fork_range.second - fork_range.first <= 2) // found fork
    {
        if (fork_range.second - fork_range.first == 1)
        {
            auto &left = intersection[fork_range.second];
            auto &right = intersection[fork_range.first];
            const auto left_classification =
                node_based_graph.GetEdgeData(left.turn.eid).road_classification;
            const auto right_classification =
                node_based_graph.GetEdgeData(right.turn.eid).road_classification;
            if (canBeSeenAsFork(left_classification, right_classification))
                assignFork(via_edge, left, right);
            else if (left_classification.GetPriority() > right_classification.GetPriority())
            {
                right.turn.instruction =
                    getInstructionForObvious(intersection.size(), via_edge, false, right);
                left.turn.instruction = {findBasicTurnType(via_edge, left),
                                         DirectionModifier::SlightLeft};
            }
            else
            {
                left.turn.instruction =
                    getInstructionForObvious(intersection.size(), via_edge, false, left);
                right.turn.instruction = {findBasicTurnType(via_edge, right),
                                          DirectionModifier::SlightRight};
            }
        }
        else if (fork_range.second - fork_range.first == 2)
        {
            assignFork(via_edge,
                       intersection[fork_range.second],
                       intersection[fork_range.first + 1],
                       intersection[fork_range.first]);
        }
        // assign left/right turns
        intersection = assignLeftTurns(via_edge, std::move(intersection), fork_range.second + 1);
        intersection = assignRightTurns(via_edge, std::move(intersection), fork_range.first);
    }
    else if (straightmost_deviation < FUZZY_ANGLE_DIFFERENCE &&
             !intersection[straightmost_turn].entry_allowed)
    {
        // invalid straight turn
        intersection = assignLeftTurns(via_edge, std::move(intersection), straightmost_turn + 1);
        intersection = assignRightTurns(via_edge, std::move(intersection), straightmost_turn);
    }
    // no straight turn
    else if (intersection[straightmost_turn].turn.angle > 180)
    {
        // at most three turns on either side
        intersection = assignLeftTurns(via_edge, std::move(intersection), straightmost_turn);
        intersection = assignRightTurns(via_edge, std::move(intersection), straightmost_turn);
    }
    else if (intersection[straightmost_turn].turn.angle < 180)
    {
        intersection = assignLeftTurns(via_edge, std::move(intersection), straightmost_turn + 1);
        intersection = assignRightTurns(via_edge, std::move(intersection), straightmost_turn + 1);
    }
    else
    {
        assignTrivialTurns(via_edge, intersection, 1, intersection.size());
    }
    return intersection;
}

// Assignment of left turns hands of to right turns.
// To do so, we mirror every road segment and reverse the order.
// After the mirror and reversal / we assign right turns and
// mirror again and restore the original order.
Intersection TurnHandler::assignLeftTurns(const EdgeID via_edge,
                                          Intersection intersection,
                                          const std::size_t starting_at) const
{
    BOOST_ASSERT(starting_at <= intersection.size());
    const auto switch_left_and_right = [](Intersection &intersection) {
        BOOST_ASSERT(!intersection.empty());

        for (auto &road : intersection)
            road = mirror(std::move(road));

        std::reverse(intersection.begin() + 1, intersection.end());
    };

    switch_left_and_right(intersection);
    // account for the u-turn in the beginning
    const auto count = intersection.size() - starting_at + 1;
    intersection = assignRightTurns(via_edge, std::move(intersection), count);
    switch_left_and_right(intersection);

    return intersection;
}

// can only assign three turns
Intersection TurnHandler::assignRightTurns(const EdgeID via_edge,
                                           Intersection intersection,
                                           const std::size_t up_to) const
{
    BOOST_ASSERT(up_to <= intersection.size());
    const auto count_valid = [&intersection, up_to]() {
        std::size_t count = 0;
        for (std::size_t i = 1; i < up_to; ++i)
            if (intersection[i].entry_allowed)
                ++count;
        return count;
    };
    if (up_to <= 1 || count_valid() == 0)
        return intersection;
    // handle single turn
    if (up_to == 2)
    {
        assignTrivialTurns(via_edge, intersection, 1, up_to);
    }
    // Handle Turns 1-3
    else if (up_to == 3)
    {
        const auto first_direction = getTurnDirection(intersection[1].turn.angle);
        const auto second_direction = getTurnDirection(intersection[2].turn.angle);
        if (first_direction == second_direction)
        {
            // conflict
            handleDistinctConflict(via_edge, intersection[2], intersection[1]);
        }
        else
        {
            assignTrivialTurns(via_edge, intersection, 1, up_to);
        }
    }
    // Handle Turns 1-4
    else if (up_to == 4)
    {
        const auto first_direction = getTurnDirection(intersection[1].turn.angle);
        const auto second_direction = getTurnDirection(intersection[2].turn.angle);
        const auto third_direction = getTurnDirection(intersection[3].turn.angle);
        if (first_direction != second_direction && second_direction != third_direction)
        {
            // due to the circular order, the turn directions are unique
            // first_direction != third_direction is implied
            BOOST_ASSERT(first_direction != third_direction);
            assignTrivialTurns(via_edge, intersection, 1, up_to);
        }
        else if (2 >= (intersection[1].entry_allowed + intersection[2].entry_allowed +
                       intersection[3].entry_allowed))
        {
            // at least a single invalid
            if (!intersection[3].entry_allowed)
            {
                handleDistinctConflict(via_edge, intersection[2], intersection[1]);
            }
            else if (!intersection[1].entry_allowed)
            {
                handleDistinctConflict(via_edge, intersection[3], intersection[2]);
            }
            else // handles one-valid as well as two valid (1,3)
            {
                handleDistinctConflict(via_edge, intersection[3], intersection[1]);
            }
        }
        // From here on out, intersection[1-3].entry_allowed has to be true (Otherwise we would have
        // triggered 2>= ...)
        //
        // Conflicting Turns, but at least farther than what we call a narrow turn
        else if (angularDeviation(intersection[1].turn.angle, intersection[2].turn.angle) >=
                     NARROW_TURN_ANGLE &&
                 angularDeviation(intersection[2].turn.angle, intersection[3].turn.angle) >=
                     NARROW_TURN_ANGLE)
        {
            BOOST_ASSERT(intersection[1].entry_allowed && intersection[2].entry_allowed &&
                         intersection[3].entry_allowed);

            intersection[1].turn.instruction = {findBasicTurnType(via_edge, intersection[1]),
                                                DirectionModifier::SharpRight};
            intersection[2].turn.instruction = {findBasicTurnType(via_edge, intersection[2]),
                                                DirectionModifier::Right};
            intersection[3].turn.instruction = {findBasicTurnType(via_edge, intersection[3]),
                                                DirectionModifier::SlightRight};
        }
        else if (((first_direction == second_direction && second_direction == third_direction) ||
                  (first_direction == second_direction &&
                   angularDeviation(intersection[2].turn.angle, intersection[3].turn.angle) <
                       GROUP_ANGLE) ||
                  (second_direction == third_direction &&
                   angularDeviation(intersection[1].turn.angle, intersection[2].turn.angle) <
                       GROUP_ANGLE)))
        {
            BOOST_ASSERT(intersection[1].entry_allowed && intersection[2].entry_allowed &&
                         intersection[3].entry_allowed);
            // count backwards from the slightest turn
            assignTrivialTurns(via_edge, intersection, 1, up_to);
        }
        else if (((first_direction == second_direction &&
                   angularDeviation(intersection[2].turn.angle, intersection[3].turn.angle) >=
                       GROUP_ANGLE) ||
                  (second_direction == third_direction &&
                   angularDeviation(intersection[1].turn.angle, intersection[2].turn.angle) >=
                       GROUP_ANGLE)))
        {
            BOOST_ASSERT(intersection[1].entry_allowed && intersection[2].entry_allowed &&
                         intersection[3].entry_allowed);

            if (angularDeviation(intersection[2].turn.angle, intersection[3].turn.angle) >=
                GROUP_ANGLE)
            {
                handleDistinctConflict(via_edge, intersection[2], intersection[1]);
                intersection[3].turn.instruction = {findBasicTurnType(via_edge, intersection[3]),
                                                    third_direction};
            }
            else
            {
                intersection[1].turn.instruction = {findBasicTurnType(via_edge, intersection[1]),
                                                    first_direction};
                handleDistinctConflict(via_edge, intersection[3], intersection[2]);
            }
        }
        else
        {
            assignTrivialTurns(via_edge, intersection, 1, up_to);
        }
    }
    else
    {
        assignTrivialTurns(via_edge, intersection, 1, up_to);
    }
    return intersection;
}

std::pair<std::size_t, std::size_t> TurnHandler::findFork(const EdgeID via_edge,
                                                          const Intersection &intersection) const
{

    std::size_t best = 0;
    double best_deviation = 180;

    // TODO handle road classes
    for (std::size_t i = 1; i < intersection.size(); ++i)
    {
        const double deviation = angularDeviation(intersection[i].turn.angle, STRAIGHT_ANGLE);
        if (intersection[i].entry_allowed && deviation < best_deviation)
        {
            best_deviation = deviation;
            best = i;
        }
    }
    if (best_deviation <= NARROW_TURN_ANGLE)
    {
        std::size_t left = best, right = best;
        while (left + 1 < intersection.size() &&
               (angularDeviation(intersection[left + 1].turn.angle, STRAIGHT_ANGLE) <=
                    NARROW_TURN_ANGLE ||
                (angularDeviation(intersection[left].turn.angle,
                                  intersection[left + 1].turn.angle) <= NARROW_TURN_ANGLE &&
                 angularDeviation(intersection[left].turn.angle, STRAIGHT_ANGLE) <= GROUP_ANGLE)))
            ++left;
        while (
            right > 1 &&
            (angularDeviation(intersection[right - 1].turn.angle, STRAIGHT_ANGLE) <=
                 NARROW_TURN_ANGLE ||
             (angularDeviation(intersection[right].turn.angle, intersection[right - 1].turn.angle) <
                  NARROW_TURN_ANGLE &&
              angularDeviation(intersection[right - 1].turn.angle, STRAIGHT_ANGLE) <= GROUP_ANGLE)))
            --right;

        if (left == right)
            return std::make_pair(std::size_t{0}, std::size_t{0});

        const bool valid_indices = 0 < right && right < left;
        const bool separated_at_left_side =
            angularDeviation(intersection[left].turn.angle,
                             intersection[(left + 1) % intersection.size()].turn.angle) >=
            GROUP_ANGLE;
        const bool separated_at_right_side =
            right > 0 &&
            angularDeviation(intersection[right].turn.angle, intersection[right - 1].turn.angle) >=
                GROUP_ANGLE;

        const bool not_more_than_three = (left - right) <= 2;
        const bool has_obvious = [&]() {
            if (left - right == 1)
            {
                return isObviousOfTwo(via_edge, intersection[left], intersection[right]) ||
                       isObviousOfTwo(via_edge, intersection[right], intersection[left]);
            }
            else if (left - right == 2)
            {
                return isObviousOfTwo(via_edge, intersection[right + 1], intersection[right]) ||
                       isObviousOfTwo(via_edge, intersection[right], intersection[right + 1]) ||
                       isObviousOfTwo(via_edge, intersection[left], intersection[right + 1]) ||
                       isObviousOfTwo(via_edge, intersection[right + 1], intersection[left]);
            }
            return false;
        }();

        const bool has_compatible_classes = [&]() {
            const bool ramp_class = node_based_graph.GetEdgeData(intersection[right].turn.eid)
                                        .road_classification.IsLinkClass();
            for (std::size_t index = right + 1; index <= left; ++index)
                if (ramp_class !=
                    node_based_graph.GetEdgeData(intersection[index].turn.eid)
                        .road_classification.IsLinkClass())
                    return false;
            return true;
        }();

        // TODO check whether 2*NARROW_TURN is too large
        if (valid_indices && separated_at_left_side && separated_at_right_side &&
            not_more_than_three && !has_obvious && has_compatible_classes)
            return std::make_pair(right, left);
    }
    return std::make_pair(std::size_t{0}, std::size_t{0});
}

void TurnHandler::handleDistinctConflict(const EdgeID via_edge,
                                         ConnectedRoad &left,
                                         ConnectedRoad &right) const
{
    // single turn of both is valid (don't change the valid one)
    // or multiple identical angles -> bad OSM intersection
    if ((!left.entry_allowed || !right.entry_allowed) || (left.turn.angle == right.turn.angle))
    {
        if (left.entry_allowed)
            left.turn.instruction = {findBasicTurnType(via_edge, left),
                                     getTurnDirection(left.turn.angle)};
        if (right.entry_allowed)
            right.turn.instruction = {findBasicTurnType(via_edge, right),
                                      getTurnDirection(right.turn.angle)};
        return;
    }

    if (getTurnDirection(left.turn.angle) == DirectionModifier::Straight ||
        getTurnDirection(left.turn.angle) == DirectionModifier::SlightLeft ||
        getTurnDirection(right.turn.angle) == DirectionModifier::SlightRight)
    {
        const auto left_classification =
            node_based_graph.GetEdgeData(left.turn.eid).road_classification;
        const auto right_classification =
            node_based_graph.GetEdgeData(right.turn.eid).road_classification;
        if (canBeSeenAsFork(left_classification, right_classification))
            assignFork(via_edge, left, right);
        else if (left_classification.GetPriority() > right_classification.GetPriority())
        {
            // FIXME this should possibly know about the actual roads?
            // here we don't know about the intersection size. To be on the save side,
            // we declare it
            // as complex (at least size 4)
            right.turn.instruction = getInstructionForObvious(4, via_edge, false, right);
            left.turn.instruction = {findBasicTurnType(via_edge, left),
                                     DirectionModifier::SlightLeft};
        }
        else
        {
            // FIXME this should possibly know about the actual roads?
            // here we don't know about the intersection size. To be on the save side,
            // we declare it
            // as complex (at least size 4)
            left.turn.instruction = getInstructionForObvious(4, via_edge, false, left);
            right.turn.instruction = {findBasicTurnType(via_edge, right),
                                      DirectionModifier::SlightRight};
        }
    }
    const auto left_type = findBasicTurnType(via_edge, left);
    const auto right_type = findBasicTurnType(via_edge, right);
    // Two Right Turns
    if (angularDeviation(left.turn.angle, 90) < MAXIMAL_ALLOWED_NO_TURN_DEVIATION)
    {
        // Keep left perfect, shift right
        left.turn.instruction = {left_type, DirectionModifier::Right};
        right.turn.instruction = {right_type, DirectionModifier::SharpRight};
        return;
    }
    if (angularDeviation(right.turn.angle, 90) < MAXIMAL_ALLOWED_NO_TURN_DEVIATION)
    {
        // Keep Right perfect, shift left
        left.turn.instruction = {left_type, DirectionModifier::SlightRight};
        right.turn.instruction = {right_type, DirectionModifier::Right};
        return;
    }
    // Two Right Turns
    if (angularDeviation(left.turn.angle, 270) < MAXIMAL_ALLOWED_NO_TURN_DEVIATION)
    {
        // Keep left perfect, shift right
        left.turn.instruction = {left_type, DirectionModifier::Left};
        right.turn.instruction = {right_type, DirectionModifier::SlightLeft};
        return;
    }
    if (angularDeviation(right.turn.angle, 270) < MAXIMAL_ALLOWED_NO_TURN_DEVIATION)
    {
        // Keep Right perfect, shift left
        left.turn.instruction = {left_type, DirectionModifier::SharpLeft};
        right.turn.instruction = {right_type, DirectionModifier::Left};
        return;
    }
    // Shift the lesser penalty
    if (getTurnDirection(left.turn.angle) == DirectionModifier::SharpLeft)
    {
        left.turn.instruction = {left_type, DirectionModifier::SharpLeft};
        right.turn.instruction = {right_type, DirectionModifier::Left};
        return;
    }
    if (getTurnDirection(right.turn.angle) == DirectionModifier::SharpRight)
    {
        left.turn.instruction = {left_type, DirectionModifier::Right};
        right.turn.instruction = {right_type, DirectionModifier::SharpRight};
        return;
    }

    if (getTurnDirection(left.turn.angle) == DirectionModifier::Right)
    {
        if (angularDeviation(left.turn.angle, 90) > angularDeviation(right.turn.angle, 90))
        {
            left.turn.instruction = {left_type, DirectionModifier::SlightRight};
            right.turn.instruction = {right_type, DirectionModifier::Right};
        }
        else
        {
            left.turn.instruction = {left_type, DirectionModifier::Right};
            right.turn.instruction = {right_type, DirectionModifier::SharpRight};
        }
    }
    else
    {
        if (angularDeviation(left.turn.angle, 270) > angularDeviation(right.turn.angle, 270))
        {
            left.turn.instruction = {left_type, DirectionModifier::SharpLeft};
            right.turn.instruction = {right_type, DirectionModifier::Left};
        }
        else
        {
            left.turn.instruction = {left_type, DirectionModifier::Left};
            right.turn.instruction = {right_type, DirectionModifier::SlightLeft};
        }
    }
}

} // namespace guidance
} // namespace extractor
} // namespace osrm