#include <math.h>
#include "gpk_std.h"
#include "gpklib/assert.h"
#include "gpklib/box.h"
#include "gpklib/line_int.h"

#define D	if(0)

box::box(C vector2& c_00, C vector2& c_11, BOOL sort)
 : xl(c_00(0)), xh(c_11(0)), yl(c_00(1)), yh(c_11(1)),
 			sp(vector_space::vseq(c_00.space(), c_11.space()))
{
 if(sort && xl>xh)	{
	C double tmp = xl;
	xl = xh;
	xh = tmp;
 	}
 if(sort && yl>yh)	{
	C double tmp = yl;
	yl = yh;
	yh = tmp;
 	}
}

box::box(double x1, double y1, double x2, double y2, BOOL sort)
  : xl(x1), xh(x2), yl(y1), yh(y2), sp(0)
{
 if(sort && xl>xh)	{
	C double tmp = xl;
	xl = xh;
	xh = tmp;
 	}
 if(sort && yl>yh)	{
	C double tmp = yl;
	yl = yh;
	yh = tmp;
 	}
}


box::box(double x1, double y1, double x2, double y2, C vector_space *spc,
				BOOL sort)
  : xl(x1), xh(x2), yl(y1), yh(y2), sp(spc)
{
 if(sort && xl>xh)	{
	C double tmp = xl;
	xl = xh;
	xh = tmp;
 	}
 if(sort && yl>yh)	{
	C double tmp = yl;
	yl = yh;
	yh = tmp;
 	}
}

parallelogram::parallelogram(C box& x)
  : a(matrix22(x.size()(0), 0.0, 0.0, x.size()(1)), x.c00())
{assert(x.size()(0)>0); assert(x.size()(1)>0);}

parallelogram::parallelogram(C ltransform& b, C box& x)
 : a(b*ltransform(box::unitbox, x))
{assert(x.size()(0)>0); assert(x.size()(1)>0);}

parallelogram operator*(C ltransform& a, C parallelogram& b)
{
 return parallelogram(a*b.a, box::unitbox);
}

parallelogram operator*(C ltransform& a, C box& b)
{
 return parallelogram(a, b);
}

box parallelogram::outsidebox() C
{
 int i;
 double xmax = c(0)(0), ymax = c(0)(1), xmin = c(0)(0), ymin = c(0)(1);
 for(i=1;i<4;i++)	{
	C vector2 tmp = c(i);
	if(tmp(0) > xmax)	xmax = tmp(0);
	if(tmp(0) < xmin)	xmin = tmp(0);
	if(tmp(1) > ymax)	ymax = tmp(1);
	if(tmp(1) < ymin)	ymin = tmp(1);
	}
 return box(xmin, ymin, xmax, ymax);
}

box parallelogram::insidebox() C
{
 double ang[4];

 assert(encloses(center()));
 ang[0] = s0().angle();
 ang[1] = s1().angle();
 ang[2] = line(center(), c00()).angle();
 ang[3] = line(center(), c01()).angle();

 D fprintf(stderr, "angles=%g %g %g %g\n", ang[0], ang[1], ang[2], ang[3]);
 D fprintf(stderr, "p:ib center=(%g %g)\n", center()(0), center()(1));

 int i;
 vector2 best_spot(0.0, 0.0);
 for(i=0;i<4;i++)	{	/* find the angle that gives the maximum area. */
	C line lA(vector2(0.0, 0.0), ang[i]);
	C line lB(vector2(0.0, 0.0), -ang[i]);
	line_intersection spots[4];
	int j, ns=0;

	spots[ns++] = line_intersection(lA, s0()-center());
	spots[ns++] = line_intersection(lA, s1()-center());
	spots[ns++] = line_intersection(lB, s0()-center());
	spots[ns++] = line_intersection(lB, s1()-center());
	vector2 spot;
	assert(!spot.ok());

	for(j=0;j<ns;j++)	{
 		D fprintf(stderr, "p:ib spots[%d]=(%g %g) %s\n", j, spots[j][0](0),
							spots[j][0](1), spots[j].one_int()?"OK" : "NG");
		if(spots[j].one_int()
			&&  (!spot.ok() || dist(spots[j][0])<dist(spot)))
			spot = spots[j][0];
		}

	assert(spot.ok());

 	D fprintf(stderr, "p:ib spot=(%g %g)\n", spot(0), spot(1));
	if(fabs(spot(0)*spot(1)) > fabs(best_spot(0)*best_spot(1)))
		best_spot = spot;
	}
 D fprintf(stderr, "p:ib best_spot=(%g %g)\n", best_spot(0), best_spot(1));

 return center() + box(-fabs(best_spot(0)), -fabs(best_spot(1)),
 						fabs(best_spot(0)), fabs(best_spot(1)));
}


LOCAL double max(double a, double b) { return a>b ? a : b;}
LOCAL double min(double a, double b) { return a<b ? a : b;}

box intersect(C box& a, C box& b)
{
 return box(max(a.c00()(0), b.c00()(0)),
 			max(a.c00()(1), b.c00()(1)),
			min(a.c11()(0), b.c11()(0)),
			min(a.c11()(1), b.c11()(1)),
			vector_space::vseq(a.c00().space(), b.c00().space()), FALSE);
}

vector2 box::c(int which_corner) C
{

 if(which_corner < 0)
 	which_corner = 4-(-which_corner)%4;
 else which_corner %= 4;

 if(which_corner == 0)
 	return c00();
 else if(which_corner == 1)
 	return c01();
 else if(which_corner == 2)
 	return c11();
 else
 	return c10();
}

line box::s(int which_side) C
{
 return line(c(which_side), c(which_side+1));
}

line parallelogram::s(int which_side) C
{
 return line(c(which_side), c(which_side+1));
}

vector2 parallelogram::c(int which_corner) C
{
 if(which_corner < 0)
 	which_corner = 4-(-which_corner)%4;
 else which_corner %= 4;

 if(which_corner == 0)
 	return c00();
 else if(which_corner == 1)
 	return c01();
 else if(which_corner == 2)
 	return c11();
 else
 	return c10();
}

box operator+(C box& a, C vector2& shift)
{
 return box(a.c00()+shift, a.c11()+shift);
}

parallelogram operator+(C parallelogram& a, C vector2& shift)
{
 C ltransform a_shift(matrix22::identity(), shift);
 return parallelogram(a_shift(a.a), box::unitbox);
}

BOOL box::encloses(C vector2& pos) C
{
 assert(vector_space::vseQ(sp, pos.space()));
 return pos(0)>=xl && pos(0)<=xh && pos(1)>=yl && pos(1)<=yh;
}

BOOL parallelogram::encloses(C vector2& pos) C
{
 return box::unitbox.encloses(inverse(a)(pos));
}

BOOL box::encloses(C box& a) C
{
 return encloses(a.c00()) && encloses(a.c11());
}

BOOL parallelogram::encloses(C box& b) C
{
 C parallelogram ib = inverse(a)*b;
 C box ub(box::unitbox);
 return ub.encloses(ib.c00()) && ub.encloses(ib.c01())
 		 && ub.encloses(ib.c10()) && ub.encloses(ib.c11());
}

ostream& operator<<(ostream& s, C box& b)
{
 if(b.ok())
 	s << "#< " << b.c00() << ", " << b.c11() << ">#";
 else	s << "#< BAD BOX >#";
 return s;
}

C box box::unitbox(0.0, 0.0, 1.0, 1.0);

#define NLOOPS 30

	/* Returns the largest box whose image (under transform fwd) fits inside
   	box db1. */
box inverse_image(C box& db1, C ltransform& fwd)
{
 /* The linear approximation to the inverse image of db1: */
 box lc = parallelogram(inverse(fwd.linearize()), db1).insidebox();
 /* lc is in the Output Space. */
 assert(lc.size()(0)>0 && lc.size()(1)>0);

 return lc;
}

parallelogram::~parallelogram() {}
