#include <iostream.h>
#include <assert.h>

#include "GreedyInsert.hpp"
#include "Mask.hpp"

extern ImportMask *MASK;

GreedySubdivision *mesh;

void TrackedTriangle::update(Subdivision& s)
{
    GreedySubdivision& gs = (GreedySubdivision&)s;
    gs.scanTriangle(*this);
}

real
	TrackedTriangle::redo(void *ptr)
{
	GreedySubdivision *gs = (GreedySubdivision *)ptr;


	return gs->GetDensity(sx, sy);
}


GreedySubdivision::~GreedySubdivision()
{
	delete heap;
}

GreedySubdivision::GreedySubdivision(Map *map, real percentage)
{
	radius = 12;

	radius_method = false;
	square_method = true;
	bigger_square_method = false;
	interest_method = false;

	int wish = (int)(4*radius*radius*percentage);

	lowPointCount = 2*wish;
	highPointCount = 3*wish;

	oneOverH_P_Count = 1.0f/(highPointCount-lowPointCount);

	wish = (int)(256*256*percentage);

	lowPointCount_256 = 2*wish;
	highPointCount_256 = 3*wish;

	oneOverH_P_Count_256 = 1.0f/(highPointCount_256-lowPointCount_256);

 	wish = (int)(32*32*percentage);

	lowPointCount_32 = 2*wish;
	highPointCount_32 = 3*wish;

	oneOverH_P_Count_32 = 1.0f/(highPointCount_32-lowPointCount_32);

	H = map;
    heap = new Heap(128);

    int w = H->width;
    int h = H->height;
	
    is_used.init(w, h, (unsigned char)DATA_POINT_UNUSED);
	are_used_256.init((w>>8)+1, (h>>8)+1);
	are_used_32.init((w>>5)+1, (h>>5)+1);

	initMesh(Vec2(0,0),
		Vec2(0, h-1),
		Vec2(w-1, h-1),
		Vec2(w-1, 0));
	
	is_used(0, 0)     = DATA_POINT_USED;
	is_used(0, h-1)   = DATA_POINT_USED;
	is_used(w-1, h-1) = DATA_POINT_USED;
	is_used(w-1, 0)   = DATA_POINT_USED;
	
	count = 4;
}





Triangle *GreedySubdivision::allocFace(Edge *e)
{
    Triangle *t = new TrackedTriangle(e);
	
    heap->insert(t, -1.0, 1.0f);
	
    return t;
}




void GreedySubdivision::compute_plane(
									  PlaneX& plane,
									  Triangle& T,
									  Map& map
									  )
{
    const Vec2& p1 = T.point1();
    const Vec2& p2 = T.point2();
    const Vec2& p3 = T.point3();
	
    Vec3 v1(p1, map(p1[X], p1[Y]));
    Vec3 v2(p2, map(p2[X], p2[Y]));
    Vec3 v3(p3, map(p3[X], p3[Y]));
	
    plane.init(v1, v2, v3);
}

///////////////////////////
//
// This is indeed an ugly hack.
// It should be replaced
//
static int __cdecl vec2_y_compar(const void *a,const void *b)
{
    Vec2 &p1=*(Vec2 *)a,
		&p2=*(Vec2 *)b;
	
    return (p1[Y]==p2[Y]) ? 0 : (p1[Y] < p2[Y] ? -1 : 1);
}

static void order_triangle_points(Vec2 *by_y,
								  const Vec2& p1,
								  const Vec2& p2,
								  const Vec2& p3)
{
    by_y[0] = p1;
    by_y[1] = p2;
    by_y[2] = p3;
	
    qsort(by_y,3,sizeof(Vec2),vec2_y_compar);
}



void GreedySubdivision::scan_triangle_line(
										   PlaneX& plane,
										   int y,
										   real x1, real x2,
										   Candidate& candidate
										   )
{
    int startx = (int)ceil(MIN(x1,x2));
    int endx   = (int)floor(MAX(x1,x2));
	
    if( startx > endx ) return;
	
    real z0 = plane(startx, y);
    real dz = plane.a;
    real z, diff;
	
    for(int x=startx;x<=endx;x++)
    {
		if( !is_used(x,y) )
		{
			z = H->eval(x,y);
			diff = fabs(z - z0);
			
			candidate.consider(x, y,  MASK->apply(x, y, diff), GetDensity(x, y));
		}
		
		z0 += dz;
    }
}


void GreedySubdivision::scanTriangle(TrackedTriangle& T)
{
	PlaneX z_plane;
	compute_plane(z_plane, T, *H);
	
	Vec2 by_y[3];
	order_triangle_points(by_y,T.point1(),T.point2(),T.point3());
	Vec2& v0 = by_y[0];
	Vec2& v1 = by_y[1];
	Vec2& v2 = by_y[2];
	
	int y;
	int starty, endy;
	Candidate candidate;
	
	starty = (int)v0[Y];
	endy   = (int)v1[Y];
	
	real dx1, dx2, x1, x2;
	
	if(starty != endy && (v2[Y] - v0[Y]) > EPS)
	{
		dx1 = (v1[X] - v0[X]) / (v1[Y] - v0[Y]);
		dx2 = (v2[X] - v0[X]) / (v2[Y] - v0[Y]);
		
		x1 = v0[X];
		x2 = v0[X];
		
		for(y=starty;y<endy;y++)
		{
			scan_triangle_line(z_plane, y, x1, x2, candidate);
			
			x1 += dx1;
			x2 += dx2;
		}
	}
	else
	{
		dx2 = (v2[X] - v0[X]) / (v2[Y] - v0[Y]);
		x2 = v0[X];
	}
	
	
	/////////////////////////////
	
	starty = (int)v1[Y];
	endy   = (int)v2[Y];
	
	if(starty != endy && (v2[Y] - v0[Y]) > EPS)
	{
		dx1 = (v2[X] - v1[X]) / (v2[Y] - v1[Y]);
		
		x1 = v1[X];
		
		for(y=starty;y<=endy;y++)
		{
			scan_triangle_line(z_plane, y, x1, x2, candidate);
			
			x1 += dx1;
			x2 += dx2;
		}
	}
	
	/////////////////////////////////
	//
	// We have now found the appropriate candidate point.
	//
	if( candidate.import < 1e-4 )
	{
		if( T.token != NOT_IN_HEAP )
			heap->kill(T.token);
		
#ifdef SAFETY
		T.setCandidate(-69, -69, 0.0);
#endif
    }
    else
    {
		assert( !is_used(candidate.x, candidate.y) );
		
		T.setCandidate(candidate.x, candidate.y, candidate.import);
		if( T.token == NOT_IN_HEAP )
			heap->insert(&T, candidate.import, candidate.factor);
		else
			heap->update(&T, candidate.import, candidate.factor);
	}
}

Edge *GreedySubdivision::select(int sx, int sy, Triangle *t)
{
    if( is_used(sx, sy) )
    {
		cerr << "   WARNING: Tried to reinsert point: " << sx<<" "<<sy<<endl;
		return NULL;
    }

    is_used(sx, sy) = DATA_POINT_USED;
	are_used_256(sx>>8, sy>>8)++;
	are_used_32(sx>>5, sy>>5)++;

    count++;
    return insert(Vec2(sx,sy), t);
}


int GreedySubdivision::greedyInsert()
{
    heap_node *node = heap->extract();
	
    if( !node ) return false;
	
    TrackedTriangle &T = *(TrackedTriangle *)node->obj;
    int sx, sy;
    T.getCandidate(&sx, &sy);
	
    select(sx, sy, &T);
	
	is_used(sx, sy) |= ((int)(heap->GetSize()*0.1f) << 2);

	heap->redo(this);

    return true;
}

real GreedySubdivision::maxError()
{
    heap_node *node = heap->top();
	
    if( !node )
		return 0.0;
	
    return node->import;
}

real GreedySubdivision::rmsError()
{
    real err = 0.0;
    int width = H->width;
    int height = H->height;
	
	
	
    for(int i=0; i<width; i++)
		for(int j=0; j<height; j++)
		{
			real diff = eval(i, j) - H->eval(i, j);
			err += diff * diff;
		}
		
		return sqrt(err / (width * height));
}


real GreedySubdivision::eval(int x,int y) 
{
    Vec2 p(x,y);
    Triangle *T = locate(p)->Lface();
	
    PlaneX z_plane;
    compute_plane(z_plane, *T, *H);
	
    return z_plane(x,y);
}

int GreedySubdivision::areUsed(int sx, int sy)
{
	int i, j, ret = 0;
    int w = H->width;
    int h = H->height;
	
	int ie = sy+radius<=h?sy+radius:h;
	for(i=(sy-radius>=0?sy-radius:0);i<ie;i++)
	{
		int je = sx+radius<=w?sx+radius:w;
		for(j=(sx-radius>=0?sx-radius:0);j<je;j++)
		{
			const unsigned char *ptr = is_used.GetData(i, j);

			ret += (*ptr++) & DATA_POINT_USED;
		}
	}
	
	return ret;
}

real
	GreedySubdivision::GetDensity(int sx, int sy)
{
	if(radius_method == true)
	{
		int density = areUsed(sx, sy);
		return (density < lowPointCount ? 1.0f : density > highPointCount ? 0.0f : 1.0f - (density - lowPointCount)*oneOverH_P_Count);
	}

	real factor = 1.0f;
	if(	interest_method == true)
	{
		factor = 0.8f + interest_data(sx, sy)*(0.4f/255.0f);
	}

	int density_256, density_32;
	if(square_method == true)
	{
		density_256 = are_used_256(sx>>8, sy>>8);
		density_32 = are_used_32(sx>>5, sy>>5);

		real den_256, den_32;

		den_256 = density_256 < lowPointCount_256 ? 1.0f : density_256 > highPointCount_256 ? 0.0f : 1.0f - (density_256 - lowPointCount_256)*oneOverH_P_Count_256;
		den_32 = density_32 < lowPointCount_32 ? 1.0f : density_32 > highPointCount_32 ? 0.0f : 1.0f - (density_32 - lowPointCount_32)*oneOverH_P_Count_32;
		return factor*(den_32 < den_256 ? den_32 : den_256);
	}
	else
	{
		if(bigger_square_method == true)
		{
			int i, j, px, py, count;

			px = sx>>8;
			py = sy>>8;
			count = 0;
			density_256 = 0;
			for(j=py-1;j<py+1;j++)
			{
				for(i=px-1;i<px+1;i++)
				{
					if(j>=0 && j<are_used_256.height() && i>=0 && i<are_used_256.width())
					{
						density_256 += are_used_256(i, j);
						count++;
					}
				}
			}
			density_256 /= count;

			px = sx>>5;
			py = sy>>5;
			count = 0;
			density_32 = 0;
			for(j=py-1;j<py+1;j++)
			{
				for(i=px-1;i<px+1;i++)
				{
					if(j>=0 && j<are_used_32.height() && i>=0 && i<are_used_32.width())
					{
						density_32 += are_used_32(i, j);
						count++;
					}
				}
			}
			density_32 /= count;

			real den_256, den_32;

			den_256 = density_256 < lowPointCount_256 ? 1.0f : density_256 > highPointCount_256 ? 0.0f : 1.0f - (density_256 - lowPointCount_256)*oneOverH_P_Count_256;
			den_32 = density_32 < lowPointCount_32 ? 1.0f : density_32 > highPointCount_32 ? 0.0f : 1.0f - (density_32 - lowPointCount_32)*oneOverH_P_Count_32;
			return factor*(den_32 < den_256 ? den_32 : den_256);
		}
	}

	return factor;
}