#include <gpk_std.h>
#include "../line.h"
#include "../nline.h"
#include "../line_int.h"
#include <stdio.h>
#include <assert.h>
#include <math.h>

main()
{
 C vector2 spot(5.4, 2.221);
 C line one(spot, 0.5121);
 C nline two(spot, vector2(cos(0.12223), sin(0.12223)), vector2(0.1, 0.0023));
 C line_intersection ot(one, two);
	int i;

 assert(two.angle(0)>.12222 && two.angle(0)<.12224);
 assert(one.angle()>.5120 && one.angle()<.5122);

 BOOL match = FALSE;
 assert(ot.num_ints() > 0);
 printf("intersect at (%g %g), should be (%g %g)\n", ot[0](0), ot[0](1),
 				spot(0), spot(1));
 if(ot.num_ints() > 0)	{
 	printf("other intersects:\n");
	for(i=0;i<ot.num_ints();i++)	{
		printf("\tat (%g %g), should be (%g %g)\n", ot[i](0), ot[i](1),
 					spot(0), spot(1));
		printf("\twith s=%g, and s=%g\n", ot.sa(i), ot.sb(i));
		if(dist(ot[i]-spot)<1e-8 && fabs(ot.sb(i))<1e-6 &&
				fabs(ot.sa(i)<1e-6))	match = TRUE;
		}
 	}
 assert(match);

 printf("\n");
 C line_intersection ab(line(vector2(1,0), vector2(1,5)),
 						nline(vector2(0,2), vector2(1,0), vector2(0,0)));
 assert(ab.one_int());
 for(i=0;i<ab.num_ints();i++)
 	printf("intersect at (%g %g), should be (%g %g)\n", ab[i](0), ab[i](1),
 									1.0, 2.0);

 assert(dist(ab[0]-vector2(1,2)) < 1e-8);
 assert(fabs(ab.sa(0)-0.4)<1e-6);
 assert(fabs(ab.sb(0)-1.0)<1e-6);
 printf("\n");
 C line_intersection at(line(vector2(1,0), M_PI/2.0),
 						nline(vector2(0,2), vector2(0,1), vector2(1,-1)));
 assert(at.num_ints() > 0);
 for(match=FALSE,i=0;i<at.num_ints();i++)	{
 	printf("intersect at (%g %g), should be (%g %g)\n", at[i](0), at[i](1),
 				1.0, 2.0);
	printf("\twith s=%.8g, and s=%.8g\n", at.sa(i), at.sb(i));
	if(dist(at[i]-vector2(1,2))<1e-8 && fabs(at.sb(i)-1.0)<1e-6
				&& fabs(at.sa(i)-2.0)<1e-6)	match = TRUE;
	}
 assert(match);

 C line_intersection oos(line(vector2(0,0),0.0),
 							nline(vector2(0,1), vector2(1,0),
							vector2(0,1)));
 assert(oos.no_int());
 printf("OK\n");
}
