#include <iostream>
#include <iomanip>

#include "log.h"

#include "perspective.h"


namespace {
	const int border_width   = 2;
	const int max_width      = 16384 - 2*border_width;
	const int max_height     = 16384 - 2*border_width;

	const int max_area_width = 4096 - 2*border_width;
	const int max_area       = max_area_width * max_area_width;
	
	const Real max_overscale = 2.0;
	Real max_overscale_sqr = max_overscale*max_overscale;
}


Matrix3
Perspective::make_matrix(
	const Vector2 &p0,
	const Vector2 &px,
	const Vector2 &py,
	const Vector2 &p1 )
{
	Vector2 A = px - p1;
	Vector2 B = py - p1;
	Vector2 C = p0 + p1 - px - py;
	
	Real cw = A.y*B.x - A.x*B.y;
	Real aw = B.x*C.y - B.y*C.x;
	Real bw = A.y*C.x - A.x*C.y;
	
	// normalize and force cw to be positive
	Real k = aw*aw + bw*bw + cw*cw;
	k = k > real_precision ? 1/sqrt(k) : 1;
	if (cw < 0) k = -k;
	aw *= k;
	bw *= k;
	cw *= k;
	
	Vector2 c = p0*cw;
	Vector2 a = px*(cw + aw) - c;
	Vector2 b = py*(cw + bw) - c;

	return Matrix3( Vector3(a, aw),
					Vector3(b, bw),
					Vector3(c, cw) );
}


Matrix3
Perspective::normalize_matrix(
	const Matrix3 &matrix )
{
	Real k = matrix.get_col(2).length();
	if (k <= real_precision)
		return matrix;
	k = 1/k;
	Matrix3 m = matrix;
	for(int i = 0; i < 9; ++i)
		m.a[i] *= k;
	return m;
}


int
Perspective::truncate_line(
	Vector2 *out_points,
	const Pair2 &bounds,
	Real a,
	Real b,
	Real c )
{
	// equatation of line is a*x + b*y + c = 0
	
	int count = 0;
	
	if (fabs(a) > real_precision) {
		Real x0 = -(c + bounds.p0.y*b)/a;
		if ( x0 + real_precision >= bounds.p0.x
		  && x0 - real_precision <= bounds.p1.x )
		{
			if (out_points) out_points[count] = Vector2(x0, bounds.p0.y);
			if (++count >= 2) return count;
		}
		
		Real x1 = -(c + bounds.p1.y*b)/a;
		if ( x1 + real_precision >= bounds.p0.x
		  && x1 - real_precision <= bounds.p1.x )
		{
			if (out_points) out_points[count] = Vector2(x1, bounds.p1.y);
			if (++count >= 2) return count;
		}
	}

	if (fabs(b) > real_precision) {
		Real y0 = -(c + bounds.p0.x*a)/b;
		if ( y0 + real_precision >= bounds.p0.y
		  && y0 - real_precision <= bounds.p1.y )
		{
			if (out_points) out_points[count] = Vector2(bounds.p0.x, y0);
			if (++count >= 2) return count;
		}
		
		Real y1 = -(c + bounds.p1.x*a)/b;
		if ( y1 + real_precision >= bounds.p0.y
		  && y1 - real_precision <= bounds.p1.y )
		{
			if (out_points) out_points[count] = Vector2(bounds.p1.x, y1);
			if (++count >= 2) return count;
		}
	}

	return count;
}


Vector2
Perspective::calc_optimal_resolution(
	const Vector2 &ox,
	const Vector2 &oy )
{
	const Real a = ox.x * ox.x;
	const Real b = ox.y * ox.y;
	const Real c = oy.x * oy.x;
	const Real d = oy.y * oy.y;
	Real e = fabs(ox.x*oy.y - ox.y*oy.x);
	if (e < real_precision_sqr)
		return Vector2(); // paranoid check, e must be non-zero when matrix is invertible
	e = 1.0/e;
	
	const Real sum = a*d + b*c;
	Vector2 scale;
	if (2*a*b + real_precision_sqr >= sum) {
		scale.x = sqrt(2*b)*e;
		scale.y = sqrt(2*a)*e;
	} else
	if (2*c*d + real_precision_sqr >= sum) {
		scale.x = sqrt(2*d)*e;
		scale.y = sqrt(2*c)*e;
	} else {
		const Real dif = a*d - b*c;
		scale.x = sqrt(dif/(a - c))*e;
		scale.y = sqrt(dif/(d - b))*e;
	}
	
	return scale.x <= real_precision || scale.y <= real_precision
	     ? Vector2() : scale;
}


Vector3
Perspective::make_alpha_matrix_col(
	Real w0,
	Real w1,
	const Vector3 &w_col )
{
	Real k = w1 - w0;
	if (fabs(k) <= real_precision)
		return w_col;

	k = w1/k;
	return Vector3(
		k*w_col.x,
		k*w_col.y,
		k*(w_col.z - w0) );
}


Matrix3
Perspective::make_alpha_matrix(
	Real aw0, Real aw1,
	Real bw0, Real bw1,
	const Vector3 &w_col )
{
	const Vector3 a_col = make_alpha_matrix_col(aw0, aw1, w_col);
	const Vector3 b_col = make_alpha_matrix_col(bw0, bw1, w_col);
	return Matrix3(
		Vector3( a_col.x, b_col.x, w_col.x ),
		Vector3( a_col.y, b_col.y, w_col.y ),
		Vector3( a_col.z, b_col.z, w_col.z ) );
}


void
Perspective::calc_raster_size(
	Pair2 &out_bounds,
	IntVector2 &out_raster_size,
	Matrix3 &out_raster_matrix,
	const Vector2 &resolution,
	const Pair2 &bounds,
	const Vector2 &dst_size )
{
	const Vector2 offset = bounds.p0;
	const Vector2 raster_size_orig(
		(bounds.p1.x - bounds.p0.x)*resolution.x,
		(bounds.p1.y - bounds.p0.y)*resolution.y );
	
	Vector2 raster_size_float = raster_size_orig;
	Real sqr = raster_size_float.square();
	Real sqr_max = dst_size.square() * max_overscale_sqr;
	if (sqr_max > real_precision && sqr > sqr_max)
		raster_size_float *= sqrt(sqr_max/sqr);
	if (raster_size_float.x > max_width)
		raster_size_float.x = max_width;
	if (raster_size_float.y > max_height)
		raster_size_float.y = max_height;
	
	IntVector2 raster_size(
		std::max(1, (int)ceil( raster_size_float.x - real_precision )),
		std::max(1, (int)ceil( raster_size_float.y - real_precision )) );
	if (raster_size.x * raster_size.y > max_area) {
		const Real k = sqrt(Real(max_area)/(raster_size.x * raster_size.y));
		raster_size.x = std::max(1, (int)floor(raster_size.x*k + real_precision));
		raster_size.y = std::max(1, (int)floor(raster_size.y*k + real_precision));
	}
	
	const Vector2 new_resolution(
		raster_size_orig.x > real_precision ? resolution.x * raster_size.x / raster_size_orig.x : resolution.x,
		raster_size_orig.y > real_precision ? resolution.y * raster_size.y / raster_size_orig.y : resolution.y );
	const Vector2 new_border(
		new_resolution.x > real_precision ? border_width/new_resolution.x : Real(0),
		new_resolution.y > real_precision ? border_width/new_resolution.y : Real(0) );
	
	out_bounds = bounds.inflated( new_border );
	out_raster_size = raster_size + IntVector2(border_width, border_width)*2;
	out_raster_matrix = Matrix3::translation(Vector2(border_width, border_width))
					  * Matrix3::scaling(new_resolution)
					  * Matrix3::translation(-offset);
}


void
Perspective::create_layers(
	LayerList &out_layers,
	const Matrix3 &matrix,
	const IntPair2 &dst_bounds,
	const Real step )
{
	bool is_invertible = false;
	Matrix3 back_matrix = normalize_matrix(matrix).inverted(&is_invertible);
	if (!is_invertible)
		return; // matrix is collapsed
	
	// calc src resolution
	const Vector2 resolution = calc_optimal_resolution(
		back_matrix.row_x().vec2(),
		back_matrix.row_y().vec2() );
	if (resolution.x <= real_precision || resolution.y <= real_precision)
		return; // cannot calc resolution, this can happen if matrix is (almost) not invertible

	// corners
	Vector3 dst_corners[4] = {
		Vector3(dst_bounds.p0.x, dst_bounds.p0.y, 1),
		Vector3(dst_bounds.p1.x, dst_bounds.p0.y, 1),
		Vector3(dst_bounds.p1.x, dst_bounds.p1.y, 1),
		Vector3(dst_bounds.p0.x, dst_bounds.p1.y, 1) };
	
	// calc coefficient for equatation of "horizontal" line: A*x + B*y + C = 1/w (aka A*x + B*y = 1/w - C)
	//                     equatation of line of horizon is: A*x + B*y + C = 0   (aka A*x + B*y = -C)
	const Real A = back_matrix.m02;
	const Real B = back_matrix.m12;
	const Real C = back_matrix.m22;
	
	Real hd = sqrt(A*A + B*B);
	if (hd <= real_precision) {
		// orthogonal projection - no perspective - no subdiviosions
		
		if (fabs(C) < real_precision)
			return; // only when matrix was not invertible (additional check)
		
		// calc bounds
		Pair2 layer_src_bounds(Vector2(INFINITY, INFINITY), Vector2(-INFINITY, -INFINITY));
		for(int i = 0; i < 4; ++i)
			layer_src_bounds.expand( (back_matrix*dst_corners[i]).vec2() );
		if (layer_src_bounds.empty())
			return;
		
		std::cout << "debug: layer_src_bounds: " << Log::to_string(layer_src_bounds) << std::endl;
		std::cout << "debug: back_matrix:"<< std::endl << Log::to_string(back_matrix, 10);
		
		// make layer
		Layer layer;
		layer.dst_bounds = dst_bounds;
		calc_raster_size(
			layer.src_bounds,
			layer.src_size,
			layer.back_matrix,
			resolution,
			layer_src_bounds,
			Vector2(layer.dst_bounds.size()) );
		layer.back_matrix *= back_matrix;
		layer.back_alpha_matrix = Matrix3(
			Vector3(0, 0, 0),
			Vector3(0, 0, 0),
			Vector3(1, 1, 1) );
		out_layers.push_back(layer);

		return; 
	}

	// find visible w range
	hd = 1/hd;
	const Real horizonw1 = hd;
	const Real horizonw2 = hd/std::min(Real(2), step);
	const Real horizonw3 = hd/step;
	Real maxw = -INFINITY, minw = INFINITY;
	Vector3 src_corners[4];
	for(int i = 0; i < 4; ++i) {
		Vector3 src = back_matrix * dst_corners[i];
		if (fabs(src.z) > real_precision) {
			Real w = 1/src.z;
			if (w > 0 && w < horizonw1)
				src_corners[i] = Vector3(src.x*w, src.y*w, w);
			else
				w = horizonw1;
			if (minw > w) minw = w;
			if (maxw < w) maxw = w;
		}
	}
	if (minw >= maxw - real_precision)
		return; // all bounds too thin
	const Real maxw3 = std::min(maxw, horizonw3);

	// steps
	const Real stepLog = log(step);
	int minlog = (int)floor(log(minw)/stepLog + real_precision);
	int maxlog = (int)ceil(log(maxw3)/stepLog - real_precision);
	if (maxlog < minlog) maxlog = minlog;
	
	Real w = pow(step, Real(minlog));
	for(int i = minlog; i <= maxlog; ++i, w *= step) {
		// w range
		const Real w0  = w/step;
		const Real w1  = std::min(w*step, horizonw1);
		
		// alpha ranges
		const Real aw0 = w0;
		const Real aw1 = w;
		const Real bw0 = i == maxlog ? horizonw1 : w1;
		const Real bw1 = i == maxlog ? horizonw2 : w;
		
		// calc bounds
		Pair2 layer_src_bounds(Vector2(INFINITY, INFINITY), Vector2(-INFINITY, -INFINITY));
		Pair2 layer_dst_bounds(Vector2(INFINITY, INFINITY), Vector2(-INFINITY, -INFINITY));
		for(int j = 0; j < 4; ++j) {
			if (src_corners[j].z && src_corners[j].z > w0 && src_corners[j].z < w1) {
				layer_src_bounds.expand(src_corners[j].vec2());
				layer_dst_bounds.expand(dst_corners[j].vec2());
			}
		}
		Vector2 line[2];
		int line_count = truncate_line(line, Pair2(dst_bounds), A, B, C - 1/w0);
		for(int j = 0; j < line_count; ++j) {
			layer_src_bounds.expand( (back_matrix * Vector3(line[j], 1)).vec2() * w0 );
			layer_dst_bounds.expand( line[j] );
		}
		
		line_count = truncate_line(line, Pair2(dst_bounds), A, B, C - 1/w1);
		for(int j = 0; j < line_count; ++j) {
			layer_src_bounds.expand( (back_matrix * Vector3(line[j], 1)).vec2() * w1 );
			layer_dst_bounds.expand( line[j] );
		}
		
		if (layer_src_bounds.empty() || layer_dst_bounds.empty())
			continue;
		
		// make layer
		Layer layer;
		layer.dst_bounds = IntPair2(
			IntVector2( std::max(dst_bounds.p0.x, (int)floor(layer_dst_bounds.p0.x + real_precision)),
						std::max(dst_bounds.p0.y, (int)floor(layer_dst_bounds.p0.y + real_precision)) ),
			IntVector2( std::min(dst_bounds.p1.x, (int)ceil (layer_dst_bounds.p1.x - real_precision)),
						std::min(dst_bounds.p1.y, (int)ceil (layer_dst_bounds.p1.y - real_precision)) ));
		calc_raster_size(
			layer.src_bounds,
			layer.src_size,
			layer.back_matrix,
			resolution / w,
			layer_src_bounds,
			Vector2(layer.dst_bounds.size()) );
		layer.back_matrix *= back_matrix;
		layer.back_alpha_matrix = make_alpha_matrix(
			1/aw0, 1/aw1, 1/bw0, 1/bw1,
			layer.back_matrix.get_col(2) );
		out_layers.push_back(layer);
	}
}


Real
Perspective::find_optimal_step(
	const Matrix3 &matrix,
	const IntPair2 &dst_bounds )
{
	max_overscale_sqr = 1e10;
	
	ULongInt min_area = -1ull;
	Real optimal_step = 0;
	LayerList layers;
	for(Real step = 2; step < 16; step *= 1.01) {
		create_layers(layers, matrix, dst_bounds, step);
		ULongInt area = 0;
		for(LayerList::const_iterator i = layers.begin(); i != layers.end(); ++i)
			area += i->src_size.square();
		if (area < min_area) {
			min_area = area;
			optimal_step = step;
		}
		layers.clear();
	}
	
	max_overscale_sqr = max_overscale*max_overscale;
	
	return optimal_step;
}



void
Perspective::add_premulted(
	const Layer &layer,
	Surface &src_surface,
	Surface &dst_surface )
{
	if (src_surface.empty()) return;
	if (dst_surface.empty()) return;
	
	const int x0 = std::max(layer.dst_bounds.p0.x, 0);
	const int y0 = std::max(layer.dst_bounds.p0.y, 0);
	const int x1 = std::min(layer.dst_bounds.p1.x, dst_surface.width());
	const int y1 = std::min(layer.dst_bounds.p1.y, dst_surface.height());
	if (x0 >= x1 || y0 >= y1) return;
	
	const int w = x1 - x0;
	const int h = y1 - y0;
	if (w <= 0 || h <= 0) return;
	
	const Vector3 coord_dx = layer.back_matrix.row_x();
	const Vector3 coord_dy = layer.back_matrix.row_y() + coord_dx*(x0 - x1);
	Vector3 coord = layer.back_matrix * Vector3(x0, y0, 1);

	const Vector2 alpha_dx = layer.back_alpha_matrix.row_x().vec2();
	const Vector2 alpha_dy = layer.back_alpha_matrix.row_y().vec2() + alpha_dx*(x0 - x1);
	Vector2 alpha = (layer.back_alpha_matrix * Vector3(x0, y0, 1)).vec2();
	
	const int dr = dst_surface.pitch() - x1 + x0;
	Color *c = &dst_surface[y0][x0];
	for(int r = y0; r < y1; ++r, c += dr, coord += coord_dy, alpha += alpha_dy) {
		for(Color *end = c + w; c != end; ++c, coord += coord_dx, alpha += alpha_dx) {
			if (coord.z > real_precision) {
				const Real w = 1/coord.z;
				const Real a = clamp(alpha.x*w, 0, 1) * clamp(alpha.y*w, 0, 1);
				if (a > real_precision)
					*c += src_surface.get_pixel_premulted(coord.x*w, coord.y*w) * a;
			}
		}
	}
}


void
Perspective::print_layer(const Layer &layer, const std::string &prefix) {
	const int w = 10;
	std::cout << prefix << "dst_bounds: " << Log::to_string(layer.dst_bounds, w) << std::endl;
	std::cout << prefix << "src_bounds: " << Log::to_string(layer.dst_bounds, w) << std::endl;
	std::cout << prefix << "src_size:   " << Log::to_string(layer.src_size,   w) << std::endl;
	std::cout << prefix << "back_matrix:" << std::endl
			  << Log::to_string(layer.back_matrix, w, prefix + Log::tab()); 
	std::cout << prefix << "back_alpha_matrix:" << std::endl
			  << Log::to_string(layer.back_alpha_matrix, w, prefix + Log::tab()); 
}


void
Perspective::print_layers(const LayerList &layers, const std::string &prefix) {
	int index = 0;
	for(LayerList::const_iterator i = layers.begin(); i < layers.end(); ++i, ++index) {
		std::cout << prefix << "layer #" << index << std::endl;
		print_layer(*i, prefix + Log::tab());
	}
}