#include <math.h>
#include <emmintrin.h>
#include <stdio.h>
#include "IntervalSSE.h"


static inline
void fromInterval(double output[2], __m128d interval) 
{
  __m128d signmask = _mm_set_pd(0.0, -1.0 * 0.0);
  _mm_storeu_pd(output, _mm_xor_pd(interval, signmask));
}

static inline
__m128d toInterval(double x)
{
  __m128d signmask = _mm_set_pd(0.0, -1.0 * 0.0);
  return _mm_xor_pd(_mm_set1_pd(x), signmask);
  //  return _mm_set_pd(x,-x);
}



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

static __m128d
ccw(__m128d x1, __m128d y1, __m128d x2, __m128d y2, __m128d x3, __m128d y3)
{
  __m128d s1 = interval_add(interval_add(interval_mul(x1,y2), 
										 interval_mul(x2,y3)), 
							             interval_mul(x3,y1));

  __m128d s2 = interval_add(interval_add(interval_mul(x1,y3), 
										 interval_mul(x2,y1)), 
							             interval_mul(x3,y2));

  return interval_sub(s1,s2);
}

static __m128d
incircle(__m128d x1, __m128d y1, __m128d x2, __m128d y2, __m128d x3, __m128d y3, __m128d x4, __m128d y4)
{
  #define DDD(x,y)  interval_add(interval_mul(x,x),interval_mul(y,y))
  __m128d a = interval_mul(DDD(x1,y1), ccw(x2,y2, x3,y3, x4,y4));
  __m128d b = interval_mul(DDD(x2,y2), ccw(x1,y1, x3,y3, x4,y4));
  __m128d c = interval_mul(DDD(x3,y3), ccw(x1,y1, x2,y2, x4,y4));
  __m128d d = interval_mul(DDD(x4,y4), ccw(x1,y1, x2,y2, x3,y3));

  return interval_add(interval_sub(a,b),interval_sub(c,d));
  #undef DDD
}

static __m128d
cintt(__m128d lo, __m128d hi, __m128d p, int * error)
{
  __m128d n = interval_sub(lo, p);
  __m128d v = interval_sub(hi,lo);

  return interval_div(n, interval_negate(v), error);
}

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

void
ccw_d(double ax, double ay, double bx, double by, double cx, double cy, double output[2])
{
  int mode = _MM_GET_ROUNDING_MODE();
  _MM_SET_ROUNDING_MODE(_MM_ROUND_DOWN); 

  __m128d x1 = toInterval(ax);
  __m128d y1 = toInterval(ay); 
  __m128d x2 = toInterval(bx);
  __m128d y2 = toInterval(by);
  __m128d x3 = toInterval(cx);
  __m128d y3 = toInterval(cy);

  fromInterval(output, ccw(x1,y1,x2,y2,x3,y3));
  _MM_SET_ROUNDING_MODE(mode);
}


void
incircle_d(double ax, double ay, double bx, double by, double cx, double cy, double dx, double dy, double output[2])
{
  int mode = _MM_GET_ROUNDING_MODE();
  _MM_SET_ROUNDING_MODE(_MM_ROUND_DOWN); 

  __m128d x1 = toInterval(ax);
  __m128d y1 = toInterval(ay); 
  __m128d x2 = toInterval(bx);
  __m128d y2 = toInterval(by);
  __m128d x3 = toInterval(cx);
  __m128d y3 = toInterval(cy);
  __m128d x4 = toInterval(dx);
  __m128d y4 = toInterval(dy);

  fromInterval(output, incircle(x1,y1,x2,y2,x3,y3,x4,y4));
  _MM_SET_ROUNDING_MODE(mode);
}

// Input interval must not be degenerate

int
cintt_d(double lo, double hi, double p, double output[2])
{
  if (lo == hi)
	return 0;

  int mode = _MM_GET_ROUNDING_MODE();
  int error = 0;

  _MM_SET_ROUNDING_MODE(_MM_ROUND_DOWN); 
  __m128d result = cintt(toInterval(lo),toInterval(hi),toInterval(p),&error);

  if (error == 1)
  {
	_MM_SET_ROUNDING_MODE(mode);
	return 0;

  } else
  {
	fromInterval(output, result);

	_MM_SET_ROUNDING_MODE(mode);
	return 1;
  }
}




/*
void
main()
{
  _MM_SET_ROUNDING_MODE(_MM_ROUND_DOWN); 

  double output[2];

  __m128d x1 = toInterval(3);
  __m128d y1 = toInterval(4);

  fromInterval(output, interval_sub(x1,y1));


  printf("x=%.16f,x2=%.16f\n", output[0],output[1]);
}
*/