#include <math.h>
#include <emmintrin.h>
#include <assert.h>
#include <string.h>
#include <stdio.h>

#include "List.h"
#include "ESSAPrimitives.h"
/*
//////////////////// IMPORTANT NOTE ////////////////////

 Most of these functions assume that the inputs were *floats* that were casted to *doubles* prior to the call.
 These functions compute the exact result of single precision float inputs by using double precision multiplications
 with some splitting operations. 
 
 They will not work with full double precision inputs, for this you would need quad precision in these routines (i think). 

 Read the paper, it explains things better:

 "Exact computation of Voronoi diagram and Delaunay triangulation"
 M Gavrilova, H Ratschek, J Rokne - Reliable Computing, 2000 - cpsc.ucalgary.ca

////////////////////////// - ///////////////////////////
*/
static inline Ordering essaR(Sequence*,Sequence*);
static inline Sequence * place(Sequence * s, double x1, double x2, Node *n1, Node *n2);
static int cmp_d(const void *, const void *);
static inline LineIntersection coincidenceTest(double a1x, double a1y, double a2x, double a2y, double b1x, double b1y, double b2x, double b2y);
static inline int interval_test(double * topS, Ordering bottom, double * bottomS, IntersectionPoint * ip);

//////////////////////


void split_double(double a, double *x, double *y)
{
  __m128d factor = _mm_set_sd(pow(2.0,27)+1);
  __m128d a_     = _mm_set_sd(a);
  __m128d b_     = _mm_mul_sd(factor,a_);
  __m128d c_     = _mm_sub_sd(b_, a_);
  __m128d d_     = _mm_sub_sd(b_, c_);
  __m128d e_     = _mm_sub_sd(a_, d_);
  _mm_store_sd(x,d_);
  _mm_store_sd(y,e_);
}


// This is 20x slower than a straight floating point sum over the values

Ordering
essa_double(double* xs, size_t n)
{
  int i;
  double add[n]; // overestimating the space required - how bad could it be
  double sub[n];
  size_t an = 0;
  size_t sn = 0;

  for (i = 0; i < n; ++i)
  {
	if      (xs[i] > 0) { add[an] =  xs[i]; ++an; }
    else if (xs[i] < 0) { sub[sn] = -xs[i]; ++sn; }
  }

  qsort(add, an, sizeof (double), &cmp_d);
  qsort(sub, sn, sizeof (double), &cmp_d);

  Node as_storage[an];
  Node ss_storage[sn];
  Sequence as  = toSequence(add,an,as_storage);
  Sequence ss  = toSequence(sub,sn,ss_storage);
  return essaR(&as, &ss);
} 

/////////////////////////

static int
cmp_d(const void *ap, const void *bp)
{
  double a = *(double*)ap;
  double b = *(double*)bp;

  //  return (b - a);
  
  if (a > b)	  return -1;
  else if (a < b) return  1;
  else            return  0;

}

static inline
double exponent(double x)
{
  int e;

  assert (isfinite(x));

  frexp(x, &e);
  return (double) e;
}

static inline
Ordering
essaR(Sequence * s1, Sequence * s2)
{
  double a1 = 0, a2 = 0, b1 = 0, b2 = 0;
	
  {
	size_t m = length(s1);
	size_t n = length(s2);

	if (m == 0 && n == 0) return EQ;
	if (m > n  && n == 0) return GT;
	if (n > m  && m == 0) return LT;
	
	double a = head(s1);
	double b = head(s2);
	double e = exponent(a);
	double f = exponent(b);

	if (a >= (double) (n*pow(2,f))) return GT;
	if (b >= (double) (m*pow(2,e))) return LT;

	if (e == f)
    {
	  if (a >= b) a1 = a - b;
	  else        b1 = b - a;
	}
    else if (e > f)
    {
	  double p = pow(2,f-1);
	  double u = (b == p) ? p : pow(2,f);
	  a1 = a - u;
	  a2 = u - b;
	}
	else if (f > e)
	{
	  double p = pow(2,e-1);
	  double u = (a == p) ? p : pow(2,e);	
	  b1 = b - u;
	  b2 = u - a;
	}  
  }

  
  Node ns[4];
  return essaR(place(tail(s1),a1,a2, &ns[0],&ns[1]), 
			   place(tail(s2),b1,b2, &ns[2],&ns[3]));
}


static inline
Sequence *
place(Sequence * s, double x1, double x2, Node *n1, Node *n2)
{
  if (x1 != 0 && x2 != 0)
  {
	assert(x1>0);
	assert(x2>0);

	return insert(insert(s,x1,n1),x2,n2);
  }
  else if (x1 != 0)
  {
	assert(x1>0);
	return insert(s,x1,n1);
  }
  else if (x2 != 0)
  {
	assert(x2>0);
	return insert(s,x2,n2);
  }
  else
	return s;
}

/*
void
insertion_sort(double *xs, size_t n)
{
  int i;

  for (i=1; i < n; ++i)
  {
	double value = xs[i];
	int j;

	for (j = i - 1; j >= 0 && xs[j] < value; --j)
		xs[j+1] = xs[j];

	xs[j+1] = value;
  }
}
*/

// 0.6 seconds -- about 15% faster than insert_sort and 300% faster than qsort.
/*
	s.s_head = malloc(sizeof (Node));
	s.s_head->value = v;
	s.s_head->next  = NULL;
*/
/////////////////
 /*
int
main()
{
  int i = 0;
  double xs[] = { -4, 1, 2147483649 };
  size_t   n  = (sizeof xs) / (sizeof (double));
  Ordering x;
  double y = 0;

  //  for (i = 0; i < 99999; ++i)
	x = essa_double(xs, n);


	//insertion_sort(xs,n);
	//printSequence((Sequence){xs,n});

  printf("x=%d,y=%f,n=%ld\n",x,y,n);
  return 0;
}
*/



/* 
   Ignore this note:

   _MM_SET_ROUNDING_MODE
   int mode = _MM_GET_ROUNDING_MODE();

   _MM_ROUND_NEAREST
   _MM_ROUND_DOWN
   _MM_ROUND_UP 
   _MM_ROUND_TOWARD_ZERO

   _MM_SET_FLUSH_ZERO_MODE(_MM_FLUSH_ZERO_OFF);

*/


typedef struct {
  int p_sign;

  struct {
	int i;
	int j;
	int k;
  } p_idx;

} LeviCivita;

#define PERMUTATION_N 24
const LeviCivita permutations[PERMUTATION_N] = {{1,{0,1,2}},{-1,{0,1,3}},{-1,{0,2,1}},{1,{0,2,3}},
												{1,{0,3,1}},{-1,{0,3,2}},{-1,{1,0,2}},{1,{1,0,3}},
												{1,{1,2,0}},{-1,{1,2,3}},{-1,{1,3,0}},{1,{1,3,2}},
												{1,{2,0,1}},{-1,{2,0,3}},{-1,{2,1,0}},{1,{2,1,3}},
												{1,{2,3,0}},{-1,{2,3,1}},{-1,{3,0,1}},{1,{3,0,2}},
												{1,{3,1,0}},{-1,{3,1,2}},{-1,{3,2,0}},{1,{3,2,1}}};
Ordering
incircle_essa(float fxi, float fyi, float fxj, float fyj, float fxk, float fyk, float fxl, float fyl)
{
  double xi=(double)fxi, yi=(double)fyi, 
	     xj=(double)fxj, yj=(double)fyj,
	     xk=(double)fxk, yk=(double)fyk,
	     xl=(double)fxl, yl=(double)fyl;

  #define SUMS_N (PERMUTATION_N*4)

  int index, cursor;
  double xs[] = { xi, xj, xk, xl };
  double ys[] = { yi, yj, yk, yl };
  double sums[SUMS_N];

  for (index = 0, cursor = 0; index < PERMUTATION_N; ++index)
  {
	const LeviCivita p = permutations[index];

	double x = xs[p.p_idx.i];
	double y = ys[p.p_idx.j];
	double a = xs[p.p_idx.k];
	double b = ys[p.p_idx.k];

	// Would require quadrouple precision ESSA if we diddnt split 
	// these into lower precision combinations
	double xyL, xyR; split_double (x*y, &xyL, &xyR);
    double a2L, a2R; split_double (a*a, &a2L, &a2R);
	double b2L, b2R; split_double (b*b, &b2L, &b2R);

	if (p.p_sign == -1)
	{
	  sums[cursor++] = -(xyL*a2L);
	  sums[cursor++] = -(xyR*a2R);
      sums[cursor++] = -(xyL*b2L);
	  sums[cursor++] = -(xyR*b2R);
	}
	else
	{
	  sums[cursor++] = xyL*a2L;
	  sums[cursor++] = xyR*a2R;
      sums[cursor++] = xyL*b2L;
	  sums[cursor++] = xyR*b2R;
	}  
  }

  return essa_double(sums, SUMS_N);
  #undef SUMS_N
}



Ordering
ccw_essa(float fx1, float fy1, float fx2, float fy2, float fx3, float fy3)
{
  double x1=(double)fx1, y1=(double)fy1, 
	     x2=(double)fx2, y2=(double)fy2,
	     x3=(double)fx3, y3=(double)fy3;

  double sums[6] = { x1*y2, x2*y3, x3*y1, -(x1*y3), -(x2*y1), -(x3*y2) };
  return essa_double(sums, 6);
}


// Exactly test if a point is within a closed interval
int
cintt_essa(float fx1, float fx2, float fp)
{
  double x1=(double)fx1, x2=(double)fx2, p=(double)fp;

  double s_top[2]  = { x1, -p };
  double s_bot[2]  = { -x2, x1 }; 
  double s_sign[4] = { x1, -p, x2, -x1 }; 
  Ordering top     = essa_double(s_top, 2);
  Ordering bottom  = essa_double(s_bot, 2);
  Ordering sign    = essa_double(s_sign, 4);

  if (top == LT && sign != EQ) // need to flip the sign if were subtracting negative values
	sign = (sign == LT) ? GT : LT; 

  if      (top == EQ)     return 1;  // = 0
  else if (top != bottom) return 0;  // < 0
  else if (sign == EQ)    return 1;  // = 1
  else if (sign == LT)    return 1;  // < 1
  else if (sign == GT)    return 0;  // > 1  

  else return 0; // This shouldn't be reachable

}


/////////// Exact Line intersection test /////////////


#define BOTTOM_N 8
#define TOP_N 6

LineIntersection
intersect2D_essa(float fa1x, float fa1y,
				 float fa2x, float fa2y,
				 float fb1x, float fb1y,
				 float fb2x, float fb2y,
				 int * ip1, int * ip2)
{
  double a1x=(double)fa1x, a1y=(double)fa1y, 
	     a2x=(double)fa2x, a2y=(double)fa2y,
	     b1x=(double)fb1x, b1y=(double)fb1y,
	     b2x=(double)fb2x, b2y=(double)fb2y;

  int i;
  double bottom_sm[BOTTOM_N] = { b1y*a1x, -(b2y*a1x), -(b1y*a2x), b2y*a2x, -(a1y*b1x), a1y*b2x, a2y*b1x, -(a2y*b2x) };
  Ordering bottom = essa_double(bottom_sm, BOTTOM_N);

  if (bottom == EQ)
	return (ccw_essa(a1x,a1y,a2x,a2y,b1x,b1y) != EQ) 
	  ? PARALLEL 
	  : coincidenceTest(a1x,a1y,a2x,a2y,b1x,b1y,b2x,b2y);

  double top1_sm[6] = { -(a2y*b1x), b1x*b2y, a2y*b2x, b1y*a2x, -(b2y*a2x), -(b1y*b2x) };
  double top2_sm[6] = { a1x*a2y, b2y*a2x, -(b2y*a1x), a1y*b2x, -(a1y*a2x), -(a2y*b2x) };

  // negate top1 and bottom
  for (i = 0; i < TOP_N; ++i)
	top1_sm[i] = -top1_sm[i];

  for (i = 0; i < BOTTOM_N; ++i)
	bottom_sm[i] = -bottom_sm[i];
  
  if (interval_test(top1_sm,bottom,bottom_sm, ip1) && 
	  interval_test(top2_sm,bottom,bottom_sm, ip2))

	return INTERSECTING;

  else 

	return NINP;
}


static 
int
interval_test(double * topS, Ordering bottom, double * bottomS, IntersectionPoint * ip)
{
  double sign_sm[TOP_N+BOTTOM_N];

  // TODO: make sure these two copies dont overflow

  memcpy(sign_sm, topS, TOP_N * sizeof (double));
  memcpy(sign_sm+TOP_N, bottomS, BOTTOM_N * sizeof (double));

  Ordering top  = essa_double(topS, TOP_N);
  Ordering sign = essa_double(sign_sm, TOP_N+BOTTOM_N);

  if (top == LT && sign != EQ) // need to flip the sign if were subtracting negative values
	sign = (sign == LT) ? GT : LT; 

  if      (top == EQ) { *ip = ENDPOINT_1; return 1; } // = 0
  else if (top != bottom)                 return 0;   // < 0
  else if (sign == EQ){ *ip = ENDPOINT_0; return 1; } // = 1
  else if (sign == LT){ *ip = BETWEEN;    return 1; } // < 1
  else if (sign == GT)                    return 0;   // > 1  

  else return 0; // This shouldn't be reachable
}


static
LineIntersection
coincidenceTest(double a1x, double a1y,
				 double a2x, double a2y,
				 double b1x, double b1y,
				 double b2x, double b2y)
{
  // Vertical line. compare Y values.
  if ((a1x == a2x && a2x == b1x && b1x == b2x) && 
 	 (
	  (a1y <= b1y && b1y <= a2y) || 
	  (a1y <= b2y && b2y <= a2y) || 
	  (a1y >= b1y && b1y >= a2y) || 
	  (a1y >= b2y && b2y >= a2y)
	  )) 
    return COINCIDENT;

  else if ((a1x <= b1x && b1x <= a2x) || 
	       (a1x <= b2x && b2x <= a2x) ||
           (a1x >= b1x && b1x >= a2x) || 
		   (a1x >= b2x && b2x >= a2x)) 

	return COINCIDENT;

  else
	return NINP;
}

#undef BOTTOM_N
#undef TOP_N

///////////////////////////////////////

/*

Slope
slope_essa(float fx1, float fy1, float fx2, float fy2)
{
  double x1=(double)fx1, y1=(double)fy1, 
	     x2=(double)fx2, y2=(double)fy2;

  Ordering top    = essa_double((double[]){y2, -y1}, 2);
  Ordering bottom = essa_double((double[]){x2, -x1}, 2);

  if      (top == EQ)     return ZERO_SLOPE;
  else if (bottom == EQ)  return UNDEFINED_SLOPE;
  else if (top != bottom) return NEGATIVE_SLOPE;
  else                    return POSITIVE_SLOPE;
}

*/