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

#define D	if(0)

nparallelogram::nparallelogram(C box& x)
  : a(tensor222(), matrix22(x.size()(0), 0.0, 0.0, x.size()(1)), x.center(),
  		box::unitbox.center())
{assert(x.size()(0)>0); assert(x.size()(1)>0);}

nparallelogram::nparallelogram(C nltransform& b, C box& x)
 : a(b*nltransform(tensor222(), matrix22(x.size()(0), 0.0, 0.0, x.size()(1)),
 				x.center(), box::unitbox.center()))
{assert(x.size()(0)>0); assert(x.size()(1)>0);}

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

nparallelogram operator*(C nltransform& a, C box& b)
{
 return nparallelogram(a, b);
}

box nparallelogram::insidebox() C
{
 C vector2 ctr = (center() + 0.25*(c00() + c01() + c10() + c11())
 				     + 0.25*(s0()(0.5) + s1()(0.5) + s2()(0.5) + s3()(0.5)))
				*(1./3.);
 return insidebox(ctr);
}

#define NA	(4*6)
box nparallelogram::insidebox(C vector2& ctr) C
{
 double ang[NA];
 D fprintf(stderr, "entering insidebox\n");
 int i;
 int N=0;
 D fprintf(stderr, "entering angle loop\n");
 for(i=0;i<4;i++)	{
	D fprintf(stderr, "angle loop\n");
 	ang[N++] = chord(i).angle();
	D fprintf(stderr, "angle[%d]=%g (chord)\n", N-1, ang[N-1]);
	ang[N++] = line(ctr, c(i)).angle();
	D fprintf(stderr, "angle[%d]=%g (center)\n", N-1, ang[N-1]);
	if(s(i).check_hasangle(0))
		ang[N++] = line(ctr, s(i).hasangle(0)).angle();
	if(s(i).check_hasangle(M_PI/2))
		ang[N++] = line(ctr, s(i).hasangle(M_PI/2)).angle();
	if(s(i).check_hasangle(M_PI))
		ang[N++] = line(ctr, s(i).hasangle(M_PI)).angle();
	if(s(i).check_hasangle(-M_PI/2))
		ang[N++] = line(ctr, s(i).hasangle(-M_PI/2)).angle();
	}
 assert(N <= NA);

 D fprintf(stderr, "p:ib center=(%g %g)\n", ctr(0), ctr(1));

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

	for(j=0;j<4;j++)
		lA[j] = line(vector2(0.0, 0.0), (j%2==0?M_PI:0)+(j/2==0?1:-1)*ang[i]);

	int ns=0;
	for(j=0;j<4;j++)	{
		int k;
		for(k=0;k<4;k++)	{
			spots[ns] = line_intersection(lA[k], s(j)-ctr);
			D fflush(stdout);
			assert(spots[ns].invariant());
			ns++;
			}
		}
	vector2 spot;

	for(j=0;j<ns;j++)	{
		/*
 		D fprintf(stderr, "p:ib spots[%d]=(%g %g) %s%d%c%c%c\n",
					j, spots[j](0), spots[j](1),
					spots[j].num_ints()>0?"OK":"NG", spots[j].num_ints(),
					spots[j].at_inf()?'i':' ', spots[j].no_int()?'n':' ',
					!spots[j]?'N':' ');
		*/
		assert(spots[j].invariant());
		int q;
		for(q=0;q<spots[j].num_ints();q++)	{
			if(spots[j].sb(q)>=0.0 && spots[j].sb(q)<=1.0
						&& (!spot.ok() || dist(spots[j][q])<dist(spot)))	{
					spot = spots[j][q];
					D fprintf(stderr, "%d ints(%d)\n", spots[j].num_ints(), q);
					}
			}
		}

 	D if(spot.ok()) fprintf(stderr, "p:ib spot[%d][%d]=(%g %g)\n", i, j, spot(0), spot(1));
	if(spot.ok() &&  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));

 C box rv = ctr + box(-fabs(best_spot(0)), -fabs(best_spot(1)),
 						fabs(best_spot(0)), fabs(best_spot(1)));
 /*
 C parallelogram srv(ltransform(rv.center(), rv.center(),
 									matrix22(0.99999, 0, 0, 0.99999)), rv);
 assert(encloses(srv.c00()));
 assert(encloses(srv.c01()));
 assert(encloses(srv.c10()));
 assert(encloses(srv.c11()));
 */
 return rv;
}

line nparallelogram::chord(int which_side) C
{
 return line(c(which_side), c(which_side+1));
}

vector2 nparallelogram::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();
}

nline nparallelogram::s(int which_side) C
{
 return a(box::unitbox.s(which_side));
}

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

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


box nparallelogram::outsidebox() C
{
 double lx=0, ly=0, hx=0, hy=0;
 int i;
 for(i=0;i<4;i++)	{
	C vector2 tmp = c(i);
	if(i==0 || tmp(0)>hx)	hx = tmp(0);
	if(i==0 || tmp(0)<lx)	lx = tmp(0);
	if(i==0 || tmp(1)<ly)	ly = tmp(1);
	if(i==0 || tmp(1)>hy)	hy = tmp(1);
 	}
 for(i=0;i<4;i++)	{
	C nline side = s(i);
	int dir;
	for(dir=0;dir<4;dir++)
		if(side.check_hasangle(dir*M_PI/2.0))	{
			C double ss = side.p_hasangle(dir*M_PI/2.0);
			if(ss>=0.0 && ss<=1.0)	{
				C vector2 pos = side(ss);
				if(pos(0) > hx)	hx = pos(0);
				else if(pos(0) < lx)	lx = pos(0);
				if(pos(1) > hy)	hy = pos(1);
				else if(pos(1) < ly)	ly = pos(1);
				}
			}
 		}
 
 return box(lx, ly, hx, hy);
}

LOCAL vector2 vec_abs(C vector2& a)
{ return vector2(fabs(a(0)), fabs(a(1)), a.space()); }

/* returns a  box with the same area as 'a', a reasonable aspect ratio,
and the same center. */
LOCAL box boxify(C parallelogram& a)
{
 C vector2 ctr = a.center();
 vector2 hsz(0,0);
 int i;
 for(i=0;i<4;i++)	{
	hsz += vec_abs(a.c(i)-ctr);
	}
 hsz *= 0.25;
 hsz *= 0.25*a.area()/(hsz(0)*hsz(1));
 return box(ctr-hsz, ctr+hsz);
}

#define NLOOPS 30

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

 int k;
 for(k=0;k<NLOOPS;k++)	{
 /* This is how much of db1 is occupied by lc in the space of db1: */
 	C box testbox = nparallelogram(fwd, lc).outsidebox();	
	/* Improve the match: */
	C ltransform fix_in_db1_space(testbox, db1);
	C ltransform fix = il_fwd*fix_in_db1_space*l_fwd;
	lc = boxify(parallelogram(fix, lc));
	if((fix.Jacobian()-matrix22::identity()).norm()<1e-7
			&& dist(lc.center()-fix(lc.center()))<1e-7)
		break;
	}
 assert(k<NLOOPS);	/* Require convergence. */
/* lc is now the biggest box in output space that can be transformed and
still fit inside db1. */

 return lc;
}

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

double nparallelogram::area() C
{
 assert(0);
 return 0.0;
}

nparallelogram::~nparallelogram() {}
