#include "fingerprint.hpp"
#include "wavelet.hpp"
#include "utilities.hpp"

#include <ostream>
#include <cmath>

rgb_wavelet_coefficients rgb_wavelet_coefficients::calculate(const raw_rgb_image& image){
	rgb_wavelet_coefficients ret;

	std::vector<double> vector(make_u(image.width() * image.height()));

	// for every color
	for(unsigned int color = 0; color < 3; ++color){
		auto& coefficient_array = color == 0 ? ret.reds : (color == 1 ? ret.greens : ret.blues);
		unsigned int array_index = 0;

		for(unsigned int n = 0; n < make_u(image.width() * image.height()); ++n){
			vector[n] = 2.0 * image.data[3*n + color] / double(255) - 1.0;
		}

		wvlt::wavelet_2D(vector.data(), make_u(image.width()), make_u(image.height()));

		auto copy = vector;
		for(auto & x : copy) x = std::abs(x);

		auto n_coefficients = coefficient_array.size();
		std::nth_element(copy.begin(), copy.begin() + n_coefficients, copy.end(), std::greater<double>());
		double threshold = copy[n_coefficients-1];

		for(unsigned int n = 0; n < vector.size(); ++n){
			auto x = vector[n];
			if(std::abs(x) >= threshold) {
				coefficient_array[array_index++] = std::make_pair(n, x);
			}
			if(array_index >= coefficient_array.size()) {
				break;
			}
		}
	}

	return ret;
}

static double square(double x){
	return x*x;
}

double rgb_wavelet_coefficients::distance_to(const rgb_wavelet_coefficients& y) const {
	double distance = 0;

	for(unsigned int color = 0; color < 3; ++color){
		unsigned int i = 0, j = 0;
		auto& x_array = color == 0 ? reds : (color == 1 ? greens : blues);
		auto& y_array = color == 0 ? y.reds : (color == 1 ? y.greens : y.blues);

		// "merge"
		while(i < x_array.size() && j < y_array.size()){
			auto x_pair = x_array[i];
			auto y_pair = y_array[j];

			if(x_pair.first == y_pair.first) {
				distance += square(y_pair.second - x_pair.second);
				++i;
				++j;
			} else if(x_pair.first < y_pair.first) {
				distance += square(x_pair.second);
				++i;
			} else {
				distance += square(y_pair.second);
				++j;
			}
		}

		// remaining part, either x or y
		for(; i < x_array.size(); ++i){
			distance += square(x_array[i].second);
		}
		for(; j < y_array.size(); ++j){
			distance += square(y_array[j].second);
		}
	}

	return distance;
}

namespace std {
	template <typename U, typename V>
	ostream& operator<<(ostream& out, pair<U, V> const & p){
		return out << '(' << p.first << ", " << p.second << ')';
	}
}

std::ostream& operator<<(std::ostream& out, rgb_wavelet_coefficients const & x){
	out << "rgb_wavelet_coefficients" << std::endl;

	for(int color = 0; color < 3; ++color){
		auto& coefficient_array = color == 0 ? x.reds : (color == 1 ? x.greens : x.blues);

		out << '[' << coefficient_array[0];
		for(unsigned int i = 1; i < coefficient_array.size(); ++i) out << ", " << coefficient_array[i];
		out << ']' << std::endl;
	}
	return out;
}