#include "stddefx.h"
#include "geometry.h"

/********/
/* USES */
/********/

/* libs ext. <>, our ""  */

/* global header (opt.) and point's prototypes "" */
#include "mathx.h"
#include "misc.h"
#include <float.h>

#ifndef INCLUDED_BOOST_VERSION
#include <boost/version.hpp>
#define INCLUDED_BOOST_VERSION
#endif

#if BOOST_VERSION > 105800
#include <boost/test/tools/floating_point_comparison.hpp>
#else
#include <boost/test/floating_point_comparison.hpp>
#endif

/* headers of this app. modules called */

/***************/
/* EXTERNALS   */
/***************/

/* tell PointInPolygon() if point ON polygon is IN
 * If geomPointOnPolygon is non-zero (default) then
 * PointInPolygon regards a point On polygon as IN
 * the polygon
 */
int geomPointOnPolygonIsIn = 1;
/* used in SmallestFittingRectangleCentre()
 * The smallest fitting rectangle is found by rotating the polygon
 * in steps of pi/geomFittingRectangleStep and computing the area.
 * geomFittingRectangleStep has default value 16.
 */
double geomFittingRectangleStep = 16;

/**********************/
/* LOCAL DECLARATIONS */
/**********************/
#define LENGTH(p1,p2)		(fabs((p1)-(p2)))
#define IN_BETWEEN(p1, b, p2)	(LENGTH((p1),(b)) + LENGTH((p2),(b)) <= \
					LENGTH((p1),(p2)))
				/* NOTE the macro IN_BETWEEN compares
				   at <= instead of ( the mathematicall
				   justifiable) ==. This implementation will
				   hold better if the parameters are floats */

#define Double(x)	((double)x)
/* put long double here if neccessary */
#define LDouble(x)	((double)x)
#define LD(x)		LDouble(x)

#define ON_POLYGON (geomPointOnPolygonIsIn)

/*********************/
/* LOCAL DEFINITIONS */
/*********************/

/******************/
/* IMPLEMENTATION */
/******************/

/*
 * FLT_EPSILON is to precise for testcase in
 * void geom::PointTest::testPerpOnCord()
 * TODO
 *  make all this a C++ template class
 *   with types and a certains EPS approiate
 *     in certains conditions
 *  Below is the one DT2D uses
 */
#define EPS	0.000004

static BOOL CompEps(
	double a, /* r- value to compare against b */
	double b) /* r- value to compare against a */
{
  /* void geom::PointTest::testPerpOnCord()
   * really streches this one
   * found this code on the NET
   */
  double d=MAX(1,MAX(fabs(a),fabs(b)));
  return (fabs(a-b)/d) < EPS;
/*
  double eps;
  printf("%g  %g %g %g\n",a,b,d,fabs(a-b)/d);
  if (fabs(a) < EPS  || fabs(b) < EPS)
    return (fabs(a-b) < EPS);
 *
 * Let Eps be a percentage of value a
  eps=fabs(EPS*a);
return a-eps <= b && b <= a+eps;
*/
}

/* create a line from 2 distinct points on that line
 * returns argument l
 */
LINE  *CalcLine(
	LINE *l,    /* write-only, line created */
	const POINT2D *a, /* first point on the line */
	const POINT2D *b) /* second point on the line */
{
	if(PointsEq(a,b))
  	PRECOND(! PointsEq(a,b));
	if (a->x == b->x)
	{
		l->parY  = TRUE;
		l->xInt  = Double(a->x);
		l->slope = 0.0;
	}
	else
	{
		l->parY  = FALSE;
		l->slope = Double(a->y - b->y) / Double(a->x - b->x);
		l->yInt  = Double(a->y) - ( l->slope * Double(a->x) );
	}
	return(l);
}

/* compute line perpendicular to line l going through point k
 * returns argument perp
 */
LINE *PerpLine(
	LINE *perp,    /* write-only. line created */
	const POINT2D *k,/* point element of perp */
	const LINE *l) /* line perpendicular to perp  */
{
	if (l->parY)  /* vertical */
	{
		/* perpendicular is horizontal */
		perp->slope = 0;
		perp->parY  = FALSE;
		perp->yInt  = k->y;
	}
	else
	{
	if (l->slope == 0) /* horizontal */
	     {
		/* perpendicular is vertical */
		perp->slope = 0;  /* actually undefined */
		perp->parY  = TRUE;
		perp->xInt  = k->x;
	     }
	 else
	    { /* not vertical or horizontal */
		perp->slope = -1 / l->slope;
		perp->parY  = FALSE;
		perp->yInt  = -(perp->slope * k->x) + k->y;
	        POSTCOND( CompEps( YgivenX(perp, (PTYPE)0) , perp->yInt));
	    }
	 }
   /* we could relax the EPS for the CONDITION below
    * for now disable, this made EPS bigger and bigger!
    * POSTCOND( PointOnLine(perp, k));
    */
	 return(perp);
}

/* compute intersection of 2 lines
 * returns argument p if lines intersect or
 * NULL if lines are parallel
 */
POINT2D *IntersectLines(
	POINT2D *p, /* write-only, point of intersection. Previous
	           *  value is untouched if lines are parallel
	           */
	const LINE *l1, /* line 1 */
	const LINE *l2) /* line 2 */
{
	if (l1->parY && l2->parY)
		return(NULL);

	if (l1->parY) /* l2 not */
	{
		p->x = (PTYPE)l1->xInt;
		p->y = (PTYPE)YgivenX(l2, p->x);
		return(p);
	}
	if (l2->parY) /* l1 not */
	{
		p->x = (PTYPE)l2->xInt;
		p->y = (PTYPE)YgivenX(l1, p->x);
		return(p);
	}
	if (l1->slope == l2->slope)
		return(NULL);
	p->x =  (PTYPE)((l2->yInt - l1->yInt) / (l1->slope - l2->slope));
	p->y =  (PTYPE)YgivenX(l1, p->x);
	return(p);
}

/* compute intersection on 2 cords
 * returns argument i if cords intersect or
 * NULL if not or if parallel
 */
POINT2D *IntersectCords(
	POINT2D *i, /* write-only, point of intersection.
	           * Undefined if cords do not intersect
	           */
	const POINT2D *l1p1,  /* first end of cord 1 */
	const POINT2D *l1p2,  /* second end of cord 1 */
	const POINT2D *l2p1,  /* first end of cord 2 */
	const POINT2D *l2p2)  /* second end of cord 2 */
{
	LINE l1, l2;

	(void)CalcLine(&l1, l1p1, l1p2);
	(void)CalcLine(&l2, l2p1, l2p2);

	if (! IntersectLines(i, &l1, &l2))
		return(NULL);
	/* else there is an intersection,
	 *  So point i is on both lines,
	 *  now test if that intersection falls on both cords:
	 */
	if (
	     PointOnLineAlsoOnCord(i, l1p1, l1p2) &&
	     PointOnLineAlsoOnCord(i, l2p1, l2p2) )
		return(i);
	return(NULL);
}

/* compute intersection on a cord and a line
 * returns argument i if cord and line intersect or
 * NULL if not
 */
POINT2D *IntersectLineCord(
	POINT2D *i, /* write-only, point of intersection. Previous
	           *  value is undefined if cords do not intersect
	           */
	const LINE  *l,     /* line  */
	const POINT2D *c1,    /* first end of cord */
	const POINT2D *c2)    /* second end of cord */
{
	LINE cordLine;

	(void)CalcLine(&cordLine, c1, c2);

	if (! IntersectLines(i, l, &cordLine))
		return(NULL);
	/* else there is an intersection,
	 *  So point i is on both lines,
	 *  now test if that intersection falls on the cord:
	 */
	if ( PointOnLineAlsoOnCord(i, c1, c2))
		return(i);
	return(NULL);
}

/* test if a point of a line through a cord is on the cord
 * Point p is assumed to be on the line through the cord.
 * This precondition is tested in DEBUG mode.
 * Behaviour is undefined if this precondition fails
 * returns 0 if point p is not on cord c1-c2
 * non-zero if point p is on cord c1-c2
 */
int PointOnLineAlsoOnCord(
	const POINT2D *p,     /* point that is on the line through
	                     the cord  c2,c2 */
	const POINT2D *c1,  /* first end of cord */
	const POINT2D *c2)  /* second end of cord  */
{
	PTYPE maxX ,minX, maxY, minY;
	IFDEBUG(LINE lineThroughCord);

	PRECOND(PointOnLine(CalcLine(&lineThroughCord,c1,c2), p));

	/* maxX, minX, maxY, minY defines the
	rectangle of the 2 points c1 and c2 */
	maxX = MAX(c1->x, c2->x);
	minX = MIN(c1->x, c2->x);
	maxY = MAX(c1->y, c2->y);
	minY = MIN(c1->y, c2->y);

	/* see if it falls on the cord */
	return(
	     (minX <= p->x) && (p->x <= maxX) &&
	     (minY <= p->y) && (p->y <= maxY) );
}

/* compute perpendicular line going through point p and perpendicular to cord c1-c2
 * returns argument cut if the perpendicular line cuts the cord.
 * NULL otherwise.
 */
POINT2D *PerpOnCord(
	POINT2D *cut,   /* write-only. Cutting point of perp and *line through*
	               * cord c1-c2. Even if the function returns NULL!
	               */
	LINE  *perp,  /* write-only. perpendicular line on *line through*
	               * cord c1-c2 going through point p. Even if the function
	               * returns NULL!
	               */
	const POINT2D *p,   /* point going through the perpendicular */
	const POINT2D *c1,  /* first end of cord */
	const POINT2D *c2)  /* second end of cord  */
{
	LINE lineThroughCord;

	(void)CalcLine(&lineThroughCord, c1, c2);
	(void)PerpLine(perp, p, &lineThroughCord);
	(void)IntersectLines(cut, &lineThroughCord, perp);

	if (PointOnLineAlsoOnCord(cut, c1, c2))
		return(cut);
	return(NULL);
}

void Projected(
    POINT2D *projected,
    const POINT2D *p,
    const LINE    *line)
{
  LINE perp;
 (void)PerpLine(&perp, p, line);
 // if they do not intersect then it was already on the line
 if (!IntersectLines(projected, line, &perp))
   *projected = *p;
}

double DistPointLine(
    const POINT2D *p,
    const LINE    *line)
{
  POINT2D projected;
  Projected(&projected,p,line);
  return Dist(&projected,p);
}

/* compute the middle point of 3 points on a line
 * Points p1, p2 and p3 are assumed to be on the line.
 * This precondition is tested in DEBUG mode.
 * returns
 *
 * 1 if p1 is between p2 and p3 on the line,
 *
 * 2 if p2 is between p1 and p3 on the line,
 *
 * 3 if p3 is between p1 and p2 on the line.
 */
int MiddleOnLine(
	const LINE *l,    /* line where all three points are part of */
	const POINT2D *p1,  /* point 1 */
	const POINT2D *p2,  /* point 2 */
	const POINT2D *p3)  /* point 3 */
{
	double d1,d2,d3;

	PRECOND(PointOnLine(l, p1));
	PRECOND(PointOnLine(l, p2));
	PRECOND(PointOnLine(l, p3));

	if (l->parY)
	{
	   /* test on Y-value */
	   d1 = p1->y;
	   d2 = p2->y;
	   d3 = p3->y;
	}
	else
	{
	   /* test on X-value */
	   d1 = p1->x;
	   d2 = p2->x;
	   d3 = p3->x;
	}
	if((d1 <= d2 && d2 <= d3) ||
	   (d1 >= d2 && d2 >= d3))
		return(2);
	else
	{
		if((d1 <= d3 && d3 <= d2) ||
		   (d1 >= d3 && d3 >= d2))
			return 3;
		else
		{
			POSTCOND((d2 <= d1 && d1 <= d3) ||
		   		(d2 >= d1 && d1 >= d3));
			/* if COND fails, loss of pre.  */
			return 1;
		}
	}
}

/* compute X co-ordinate of point on line l given Y co-ordinate
 * Line l is assumed
 * .B not
 * to be of type 'x = constantValue'.
 * This precondition is tested in DEBUG mode.
 * returns X co-ordinate belonging to Y co-ordinate on line l
 */
double	XgivenY(
	const LINE *l, /* the line  */
	double y)      /* value of Y co-ordinate */
{
	PRECOND( l->parY || l->slope != 0); /* ! parX */

	if (l->parY)
		return(l->xInt);
	else
		return( (y - l->yInt) / l->slope );
}

/* compute Y co-ordinate of point on line l given X co-ordinate
 * Line l is assumed
 * .B not
 * to be of type 'y = constantValue'.
 * This precondition is tested in DEBUG mode.
 * returns Y co-ordinate belonging to X co-ordinate on line l
 */
double	YgivenX(
	const LINE *l, /* the line  */
	double x)      /* value of X co-ordinate */
{
	PRECOND(! l->parY);

	return( l->yInt + (x*l->slope) );
}

/* test if a point p in on line l
 * returns
 *
 * 0 if point is not on line,
 *
 * non-zero if point is on line.
 */
int PointOnLine(
	const LINE *l,  /* line */
	const POINT2D *p) /* point */
{
	if (l->parY)
		return(l->xInt == p->x);
	if (l->slope == 0)
		return(l->yInt == p->y);
	return(CompEps(
		YgivenX(l, p->x)
		, p->y));
}

/* test if a point p is in polygon pol
 * whether a point on
 * the cords of a polygon
 * is regarded as in the polygon is defined by
 * the global variable geomPointOnPolygonIsIn.
 * If it is 0 then a point on a cord is not in polygon,
 * otherwise point on a cord is in polygon (default)
 *
 * returns
 *
 * 0 if point is not in polygon,
 * non-zero if point is in polygon
 *
 * Warning
 *
 * Optimization of this function in Microsoft 6.0
 * generated wrong code used #pragma optimize("e",off/on)
 * to prevent this.
 */
int  PointInPolygon(
 const POINT2D 	*p,   /* point  */
 const POINT2D *pol,     /* polygon */
 int	nr)           /* number of points defining the polygon  */
{
        /* algorithm used is plumb-line-algorithm described
         * in Monmonier's Computer Assisted Cartography
         * Revision 1.2  and earlier was wrong for edges that were
         * parallel to the plumb-line
         */
	int	i;
	int	nrInter=0;
	double  atY;
	PTYPE	minX, maxX, minY, maxY;
	LINE    l;

	PRECOND(nr > 2);
	PRECOND(pol[0].x == pol[nr].x);
	PRECOND(pol[0].y == pol[nr].y);

	for(i = 0; i < nr; i++)
	{
	  maxX = MAX(pol[i].x, pol[i+1].x);
          minX = MIN(pol[i].x, pol[i+1].x);
	  maxY = MAX(pol[i].y, pol[i+1].y);
          minY = MIN(pol[i].y, pol[i+1].y);

	  if (p->x == pol[i].x)
	  { /* do not test i+1 -> double counting otherwise */
	  	if (p->y == pol[i].y)
		  return(ON_POLYGON); /* p == pol[i] */
	  	if (pol[i+1].x != pol[i].x)
		{
#	ifdef NEVER
		   /* rev 1.2 and earlier */
			if (p->y > pol[i].y)
				nrInter++;
#	  endif
		}
		else /* line : x = a */
		{
			if (minY < p->y && p->y < maxY)
			  return(ON_POLYGON); /* p on cord pol[i],pol[i+1] */
			else
			{
#	   ifdef NEVER
		          /* rev 1.2 and earlier */
			   /* DEAD WRONG:
			     edge is parallel to plumb-line: NO INTERSECTION */
				if (  (maxY < p->y) &&
				      /* i-1 -> (i ? nr:i)-1   */
				    IN_BETWEEN(pol[(i?nr:i)-1].x , p->x ,
				      pol[(i+2)%nr].x ))
					nrInter++;
#	   endif
		        }
		}
	}  /* eoif (p->x == pol[i].x )) */
	else
	  if ( minX < p->x && p->x < maxX )
	  {
	     /* intersection is possible */
	     /* line: y = ax + b or y = a */
	     if (p->y == minY && minY == maxY)
		return(ON_POLYGON);  /* y = a, p on cord pol[i],pol[i+1] */
	     if (maxY < p->y)
			nrInter++;
	     else
		{
		  if (minY < p->y)
		  {
			atY = YgivenX(CalcLine(&l,
				&(pol[i]), &(pol[i+1])), Double(p->x));
				if (atY < Double(p->y))
					nrInter++;
				if (atY == Double(p->y))
					return(ON_POLYGON);  /* p on cord pol[i],pol[i+1] */
                  }
		}
	  }
      } /* eofor */
      return((nrInter%2)==1);
}

/* compute the centroid of polygon pol
 * In addition to computing the centroid, this function
 * test if the centroid is in the polygon by calling
 * PointInPolygon
 * returns 0 if centroid is not in polygon
 * non-zero if centroid is in polygon
 *
 * Warning: Optimization of this function in Microsoft 6.0
 * generated wrong code used #pragma optimize("e",off/on)
 * to prevent this
 */
int CentroidOfPolygon(
       POINT2D 	*c,   /* write-only. the centroid  */
 const POINT2D *pol,    /* polygon */
 int	nr)           /* number of points defining the polygon  */
{
	int	i;
	long double up,low;

	PRECOND(nr > 2);
	PRECOND(pol[0].x == pol[nr].x);
	PRECOND(pol[0].y == pol[nr].y);

	up = 0;
	low = 0;
	for(i = 0; i < nr; i++)
	{
		up  += LDouble((pol[i+1].x + pol[i].x)/2) * LDouble(pol[i+1].x - pol[i].x)
			* LDouble((pol[i+1].y + pol[i].y)/2);
		low += LDouble(pol[i+1].x - pol[i].x) * LDouble((pol[i+1].y + pol[i].y)/2);
	}
	c->x = (PTYPE)(up/low);

	up = 0;
	low = 0;
	for(i = 0; i < nr; i++) /* this loop is wrong when /Oe is enabled */
	{
		up  += LDouble((pol[i+1].y + pol[i].y)/2) * LDouble(pol[i+1].y - pol[i].y)
			* LDouble((pol[i+1].x + pol[i].x)/2);
		low += LDouble(pol[i+1].y - pol[i].y) * LDouble((pol[i+1].x + pol[i].x)/2);
	}
	c->y = (PTYPE)(up/low);

	return(PointInPolygon(c, pol, nr));
}

/* compute the centre of the smallest fitting rectangle enclosing a polygon
 * the smallest fitting rectangle is found by rotating the polygon
 * in steps of pi/geomFittingRectangleStep and computing the area.
 * the centre of the smallest fitting rectangle that has a centre in
 * the polygon is returned. geomFittingRectangleStep
 * (double) is a global variable with default value 16.
 *
 * returns
 *
 * 0 if no rectangle is found that has its centre in the polygon,
 * non-zero if such a rectangle is found.
 */
int  SmallestFittingRectangleCentre(
	POINT2D *c,   /* write-only. the centre of the smallest rectangle that
	             * surrounds polygon pol, lying in polygon pol.
	             * undefined if return value is 0.
	             */
	const POINT2D *pol, /* the polygon */
	int nr)           /* number of points in the polygon */
{
	PTYPE minX, maxX, minY, maxY;
	double angle, bestAngle;
	double area, smallArea;
	double step = geomFittingRectangleStep;
	int i;
	POINT2D *p, nc; /* p is work-copy of polygon */

	PRECOND(pol[0].x == pol[nr].x);
	PRECOND(pol[0].y == pol[nr].y);

	p = (POINT2D*)MemcpyChkMalloc(pol, (nr+1)*sizeof(POINT2D));

	smallArea = 0.0;
	angle = 0.0;
	while (angle < M_PI/2.0)
	{
		minX = maxX = p[0].x;
		minY = maxY = p[0].y;
		for (i=1;i < nr ; i++)
		{
			minX = MIN(minX, p[i].x);
			maxX = MAX(maxX, p[i].x);
			minY = MIN(minY, p[i].y);
			maxY = MAX(maxY, p[i].y);
		}

		area =  AreaRectangle(maxX, minX, maxY, minY);
		if (smallArea == 0.0 || smallArea > area)
		{
			nc.x = minX + ((maxX - minX)/2);
	        	nc.y = minY + ((maxY - minY)/2);
			if (PointInPolygon(&nc, p, nr))
			{
				smallArea = area;
				(void)CopyPoint(c, &nc);
				bestAngle = angle;
			}
		}

		for (i=0;i < nr+1; i++)
			(void)RotPoint(&(p[i]), M_PI/step);
		angle += M_PI/step;
	}
	Free(p);

	if (smallArea == 0.0)
		return(FALSE);

	(void)RotPoint(c, -bestAngle);
	POSTCOND(PointInPolygon(c, pol, nr));
	return(TRUE);
}

/* compute area of rectangle. DOES NOT WORK!
 * Flagged with a PRECOND(FALSE)
 */
double AreaRectangle(
double	maxX,
double  minX,
double  maxY,
double  minY)
{
	PRECOND(FALSE);
	return( (maxX - minX) * (maxY - minY));
}

/* compute area of polygon
 * returns the area of the polygon
 */
double	AreaOfPolygon(
	const POINT2D *p, /* the polygon */
	int	nr)     /* number of points defining the polygon */
{
	int	i;
	long double	a=0;

	PRECOND(p[0].x == p[nr].x);
	PRECOND(p[0].y == p[nr].y);

	for(i = 0; i < nr; i++)
		a += LDouble(p[i].x)*LDouble(p[i+1].y) -
			LDouble(p[i].y)*LDouble(p[i+1].x);
	a /= -2;
	return((double)ABS(a));
}

/* clockwise angle of a point with the x-axis
 * returns the clockwise angle between 0 (included) and 2 *pi* (not included)
 *    or -1 if the point (0,0) is given.
 *
 * Example: if point is (1,1), CWAngle returns 1.75 pi.
 */
double CWAngle(
	const POINT2D *p) /* the point */
{
	double angle;

	/********************************************************/
	/* Documentation note	CWAngle				*/
	/* What we want is a clockwise scale starting at 0 and  */
	/* ending at 2 PI where 0 == 2 PI			*/
	/* 2 PI is not used, only 0                             */
	/* Range = [0, 2 PI>				        */
	/* We use the function atan2 for this purpose           */
	/* this function is not defined for the angles 0, PI/2, */
	/* PI and 3/2 PI 					*/
	/* after that we have to transform the result to our    */
	/* clockwise scale					*/
	/********************************************************/
	/*	atan2( 3.000000, 1.000000) = 1.249046		*/
	/*	atan2( -3.000000, 1.000000) = -1.249046		*/
	/*	atan2( -3.000000, -1.000000) = -1.892547	*/
	/*	atan2( 3.000000, -1.000000) = 1.892547		*/
	/*							*/
	/********************************************************/

	if (p->x == 0 && p->y == 0)
		return -1;

	if (p->x == 0)
	{
		if (p->y > 0)
			return(0);
		return(M_PI);
	}
	if (p->y == 0)
	{
		if (p->x > 0)
			return(M_PI/2);
		return(M_PI + M_PI/2);
	}

	angle = atan2(p->x, p->y);
	if (angle < 0) /* to clock wise scale */
		angle += M_2PI;

	POSTCOND(0.0 <= angle && angle < M_2PI);

	return(angle);
}

/* distance between two points
 * returns the distance between the two points
 */
double Dist(
	const POINT2D *p1,
	const POINT2D *p2)
{
	return((double)
#ifdef _MSC_VER
   _hypot
#else
   hypot
#endif
    ((p1->x)-(p2->x), (p1->y)-(p2->y)));
}

/* compare two points
 * returns 0 if they are not equal
 *         non-zero they are equal
 */
int PointsEq(
	const POINT2D *p1, /* point 1 */
	const POINT2D *p2) /* point 2 */
{
	return(p1->x == p2->x && p1->y == p2->y);
}

/* compare two points in qsort fashion
 * CmpPoints is for qsort-type comparison
 *
 * returns
 *
 * 0 if they are equal,
 *
 * < 0 if (p1.x < p2.x) || (p1.x==p2.x && p1.y < p2.y),
 *
 * < 0 if (p1.x > p2.x) || (p1.x==p2.x && p1.y > p2.y),
 */
int CmpPoints(
	const POINT2D *p1, /* point 1 */
	const POINT2D *p2) /* point 2 */
{
	int xc = CmpDouble(&(p1->x), &(p2->x));
	if (!xc)
		return CmpDouble(&(p1->y), &(p2->y));
	return xc;
}

/* multiply point by a constant
 * returns argument p
 */
POINT2D *MultiplyPoint(
	POINT2D *p, /* read-write, point that is multiplied */
	double f) /* multiplication factor */
{

	p->x *= f;
	p->y *= f;
	return p;
}

/* multiply point by a constant given an origin
 * The vector org,src is multiplied by d resulting
 * in vector org,p
 * returns argument p
 *
 * Example: MulPoint_0(p, {2,2}, 3, {1,1}),
 * p becomes {4,4}
 */
POINT2D *MulPoint_0(
 POINT2D 	*p,	   /* write-only, result */
 const POINT2D *src, /* point to multiply   */
 double	d,         /* multiplication factor */
 const POINT2D *org) /* origin of multiplication */
{
	(void)CopyPoint(p,src);

	return(
		AddPoint(
			MultiplyPoint(
				SubtrPoint(p, org),
				d),
			org)
	      );
}

/* add two points
 * returns argument d
 */
POINT2D *AddPoint(
 POINT2D *d,      /* read-write, point modified */
 const POINT2D *s)/* other point */
{
	d->x += s->x;
	d->y += s->y;
	return(d);
}

/* Subtract two points
 * returns argument d
 */
POINT2D *SubtrPoint(
 POINT2D *d,      /* read-write, point modified */
 const POINT2D *s)/* other point */
{
	d->x -= s->x;
	d->y -= s->y;
	return(d);
}

/* copy point
 * returns argument d
 */
POINT2D *CopyPoint(
	POINT2D *d, /* destination point */
 const	POINT2D *s) /* source point */
{
	d->x = s->x;
	d->y = s->y;
	return(d);
}

/* rotate a point
 * Rotate a point by a counter clockwise
 * angle. For example rotating (x=1,y=0) by
 * pi/4 (45 degrees) results in (x=0.7,y=0.7).
 * returns argument p
 */
POINT2D *RotPoint(
	POINT2D *p, /* read-write, point to rotated */
	double a) /* angle in radians, counter clock wise */
{
	double c, s, x;

	c = cos(a);
	s = sin(a);

	x =  (PTYPE)(
			((p->x) * c) - ((p->y) * s)
			);
	p->y =  (PTYPE)(
			((p->x) * s) + ((p->y) * c)
			);
	p->x = (PTYPE)x;

	return(p);
}

/* minimum X co-ordinate of polygon
 * returns minimum X co-ordinate of polygon
 */
double MinXPolygon(
	const POINT2D *p,/* the polygon */
	int   n)       /* number of points defining the polygon */
{
	int i;
	double m = p[0].x;
	PRECOND(n > 0);
	for(i=1; i < n; i++)
		m = MIN(p[i].x,m);
	return m;
}

/* minimum Y co-ordinate of polygon
 * returns minimum Y co-ordinate of polygon
 */
double MinYPolygon(
	const POINT2D *p,/* the polygon */
	int   n)       /* number of points defining the polygon */
{
	int i;
	double m = p[0].y;
	PRECOND(n > 0);
	for(i=1; i < n; i++)
		m = MIN(p[i].y,m);
	return m;
}

/* maximum X co-ordinate of polygon
 * returns maximum X co-ordinate of polygon
 */
double MaxXPolygon(
	const POINT2D *p,/* the polygon */
	int   n)       /* number of points defining the polygon */
{
	int i;
	double m = p[0].x;
	PRECOND(n > 0);
	for(i=1; i < n; i++)
		m = MAX(p[i].x,m);
	return m;
}

/* maximum Y co-ordinate of polygon
 * returns maximum Y co-ordinate of polygon
 */
double MaxYPolygon(
	const POINT2D *p,/* the polygon */
	int   n)       /* number of points defining the polygon */
{
	int i;
	double m = p[0].y;
	PRECOND(n > 0);
	for(i=1; i < n; i++)
		m = MAX(p[i].y,m);
	return m;
}

/* special case of intersection of 2 rectangles r1 and r2.
 * IntersectAllignedRectangles is a special case where r1 and
 * r2 must have sides parallel to x and y axis.
 * returns number of points in resulting polygon:
 *
 * 0 if rectangles do not intersect,
 *
 * 4 if rectangles do intersect
 */
int IntersectAllignedRectangles(
	POINT2D *pol,      /* intersecting rectangle stored in polygon format */
	const POINT2D *r1, /* rectangle 1 */
	const POINT2D *r2) /* rectangle 2 */
{
	double yMaxR1 = MaxYPolygon(r1,4);
	double yMinR1 = MinYPolygon(r1,4);
	double xMaxR1 = MaxXPolygon(r1,4);
	double xMinR1 = MinXPolygon(r1,4);

	double yMaxR2 = MaxYPolygon(r2,4);
	double yMinR2 = MinYPolygon(r2,4);
	double xMaxR2 = MaxXPolygon(r2,4);
	double xMinR2 = MinXPolygon(r2,4);

	double yMax= MIN(yMaxR1, yMaxR2);
	double yMin= MAX(yMinR1, yMinR2);
	double xMax= MIN(xMaxR1, xMaxR2);
	double xMin= MAX(xMinR1, xMinR2);

  // bug/sf463 and sf485
  // Test if rectangles R1 and R2 are adjacent.
  // If so, do not create an intersection ploygon
  // that represents the line between R1 and R2.
  // Such a line/polygon may incorrectly get a very
  // small area assigned due to floating point operations
#if BOOST_VERSION > 105800
  static boost::math::fpc::close_at_tolerance<double> tester(
         boost::math::fpc::fpc_detail::fraction_tolerance<double>(double(1e-8)),
         boost::math::fpc::FPC_STRONG);
#else
  boost::test_tools::close_at_tolerance<double>
         tester(boost::test_tools::fraction_tolerance_t<double>(double(1e-8)),
         boost::test_tools::FPC_STRONG);
#endif

  if(tester(static_cast<double>(yMax), static_cast<double>(yMin)) ||
         tester(static_cast<double>(xMax), static_cast<double>(xMin))){
    return 0;
  }

	/* centre of intersection must be in both rectangle
	 * test one to see if they intersect
	 */
	double  xCtr = (xMax+xMin)/2;
	double  yCtr = (yMax+yMin)/2;

	if((yMinR1 <= yCtr && yCtr <= yMaxR1) &&
	    (xMinR1 <= xCtr && xCtr <= xMaxR1) )
	{
		/* they intersect */

		/* bottom left */
		pol[0].x = xMin;
		pol[0].y = yMin;
		/* top left */
		pol[1].x = xMin;
		pol[1].y = yMax;
		/* top right */
		pol[2].x = xMax;
		pol[2].y = yMax;
		/* bottom right */
		pol[3].x = xMax;
		pol[3].y = yMin;
		/* copy last point */
		pol[4]   = pol[0];

		return 4;
	}
	return 0;
}


#include <chrono>
#include <cstdlib>
#include <iostream>
#include <string>
#include <boost/asio.hpp>
#include <asio-zmq.hpp>
#include "helper.hpp"

static std::string const ep = "inproc://lat_test";

int main(int argc, char* argv[])
{
    if (argc != 3) {
        std::cerr << "usage: inproc_lat <message-size> <roundtrip-count>\n";
        return 1;
    }

    int message_size = std::atoi(argv[1]);
    int roundtrip_count = std::atoi(argv[2]);

    std::cout << "message size: " << message_size << " [B]\n";
    std::cout << "roundtrip count: " << roundtrip_count << "\n";

    boost::asio::io_service ios;
    boost::asio::zmq::context ctx;

    auto watch = std::chrono::system_clock::now();

    boost::asio::zmq::test::perf::replier rep(ios, ctx, roundtrip_count, ep);
    boost::asio::zmq::test::perf::requester req(ios, ctx, roundtrip_count, message_size, ep);

    ios.run();

    auto elapsed = std::chrono::system_clock::now() - watch;
    double latency = static_cast<double>(
                         std::chrono::duration_cast<std::chrono::microseconds>(elapsed).count()) /
                     (roundtrip_count * 2);

    std::cout << "average latency: " << latency << " [us]\n";
}


#pragma once
#include <pybind11/eigen.h>
#include <pybind11/numpy.h>

#include <Eigen/Core>
#include <boost/geometry/algorithms/area.hpp>
#include <boost/geometry/algorithms/covered_by.hpp>
#include <boost/geometry/srs/spheroid.hpp>
#if BOOST_VERSION >= 107500
#include <boost/geometry/strategy/area.hpp>
#include <boost/geometry/strategy/geographic/area.hpp>
#else
#include <boost/geometry/strategies/area.hpp>
#include <boost/geometry/strategies/geographic/area.hpp>
#endif
#include <boost/geometry/strategies/geographic/distance_andoyer.hpp>
#include <boost/geometry/strategies/geographic/distance_thomas.hpp>
#include <boost/geometry/strategies/geographic/distance_vincenty.hpp>
#include <optional>

#include "pyinterp/detail/broadcast.hpp"
#include "pyinterp/detail/thread.hpp"
#include "pyinterp/geodetic/system.hpp"

namespace pyinterp::geodetic {

/// Distance calculation strategy.
enum DistanceStrategy { kAndoyer = 0x0, kThomas = 0x1, kVincenty = 0x2 };

using Andoyer = boost::geometry::strategy::distance::andoyer<
    boost::geometry::srs::spheroid<double>>;
using Thomas = boost::geometry::strategy::distance::thomas<
    boost::geometry::srs::spheroid<double>>;
using Vincenty = boost::geometry::strategy::distance::vincenty<
    boost::geometry::srs::spheroid<double>>;

/// Calculate the area
template <typename Geometry>
[[nodiscard]] inline auto area(const Geometry &geometry,
                               const std::optional<System> &wgs) -> double {
  auto spheroid = wgs.has_value()
                      ? boost::geometry::srs::spheroid(wgs->semi_major_axis(),
                                                       wgs->semi_minor_axis())
                      : boost::geometry::srs::spheroid<double>();
  auto strategy = boost::geometry::strategy::area::geographic<
      boost::geometry::strategy::vincenty, 5>(spheroid);
  return boost::geometry::area(geometry, strategy);
}

/// Checks if the first geometry is inside or on border the second geometry
/// using the specified strategy.
template <typename Geometry1, typename Geometry2>
[[nodiscard]] inline auto covered_by(
    const Geometry2 &geometry2, const Eigen::Ref<const Eigen::VectorXd> &lon,
    const Eigen::Ref<const Eigen::VectorXd> &lat, const size_t num_threads)
    -> pybind11::array_t<int8_t> {
  detail::check_eigen_shape("lon", lon, "lat", lat);
  auto size = lon.size();
  auto result =
      pybind11::array_t<int8_t>(pybind11::array::ShapeContainer{{size}});
  auto _result = result.template mutable_unchecked<1>();

  {
    pybind11::gil_scoped_release release;

    // Captures the detected exceptions in the calculation function
    // (only the last exception captured is kept)
    auto except = std::exception_ptr(nullptr);

    detail::dispatch(
        [&](size_t start, size_t end) {
          try {
            for (auto ix = static_cast<int64_t>(start);
                 ix < static_cast<int64_t>(end); ++ix) {
              _result(ix) = static_cast<int8_t>(boost::geometry::covered_by(
                  Geometry1(lon(ix), lat(ix)), geometry2));
            }
          } catch (...) {
            except = std::current_exception();
          }
        },
        size, num_threads);

    if (except != nullptr) {
      std::rethrow_exception(except);
    }
  }
  return result;
}

/// Calculate the distance between two geometries.
template <typename Geometry1, typename Geometry2>
[[nodiscard]] inline auto distance(const Geometry1 &geometry1,
                                   const Geometry2 &geometry2,
                                   const DistanceStrategy strategy,
                                   const std::optional<System> &wgs) -> double {
  auto spheroid = wgs.has_value()
                      ? boost::geometry::srs::spheroid(wgs->semi_major_axis(),
                                                       wgs->semi_minor_axis())
                      : boost::geometry::srs::spheroid<double>();
  switch (strategy) {
    case kAndoyer:
      return boost::geometry::distance(geometry1, geometry2, Andoyer(spheroid));
      break;
    case kThomas:
      return boost::geometry::distance(geometry1, geometry2, Thomas(spheroid));
      break;
    case kVincenty:
      return boost::geometry::distance(geometry1, geometry2,
                                       Vincenty(spheroid));
      break;
  }
  throw std::invalid_argument("unknown strategy: " +
                              std::to_string(static_cast<int>(strategy)));
}

/// Calculate the distance between two geometries.
template <typename Geometry1, typename Geometry2>
[[nodiscard]] inline auto distance(const Geometry1 &geometry1,
                                   const Geometry2 &geometry2) -> double {
  return boost::geometry::distance(geometry1, geometry2);
}

/// Calculate the distance between coordinates.
template <typename Geometry, typename Strategy>
[[nodiscard]] inline auto coordinate_distances(
    const Eigen::Ref<const Eigen::VectorXd> &lon1,
    const Eigen::Ref<const Eigen::VectorXd> &lat1,
    const Eigen::Ref<const Eigen::VectorXd> &lon2,
    const Eigen::Ref<const Eigen::VectorXd> &lat2, const Strategy &strategy,
    const size_t num_threads) -> pybind11::array_t<double> {
  auto size = lon1.size();
  auto result =
      pybind11::array_t<double>(pybind11::array::ShapeContainer{{size}});
  auto _result = result.template mutable_unchecked<1>();

  {
    pybind11::gil_scoped_release release;

    // Captures the detected exceptions in the calculation function
    // (only the last exception captured is kept)
    auto except = std::exception_ptr(nullptr);

    detail::dispatch(
        [&](size_t start, size_t end) {
          try {
            for (auto ix = static_cast<int64_t>(start);
                 ix < static_cast<int64_t>(end); ++ix) {
              _result(ix) = boost::geometry::distance(
                  Geometry(lon1(ix), lat1(ix)), Geometry(lon2(ix), lat2(ix)),
                  strategy);
            }
          } catch (...) {
            except = std::current_exception();
          }
        },
        size, num_threads);

    if (except != nullptr) {
      std::rethrow_exception(except);
    }
  }
  return result;
}

/// Calculate the distance between coordinates.
template <typename Geometry>
[[nodiscard]] inline auto coordinate_distances(
    const Eigen::Ref<const Eigen::VectorXd> &lon1,
    const Eigen::Ref<const Eigen::VectorXd> &lat1,
    const Eigen::Ref<const Eigen::VectorXd> &lon2,
    const Eigen::Ref<const Eigen::VectorXd> &lat2,
    const DistanceStrategy strategy, const std::optional<System> &wgs,
    const size_t num_threads) -> pybind11::array_t<double> {
  detail::check_eigen_shape("lon1", lon1, "lat1", lat1, "lon2", lon2, "lat2",
                            lat2);
  auto spheroid = wgs.has_value()
                      ? boost::geometry::srs::spheroid(wgs->semi_major_axis(),
                                                       wgs->semi_minor_axis())
                      : boost::geometry::srs::spheroid<double>();
  switch (strategy) {
    case kAndoyer:
      return coordinate_distances<Geometry, Andoyer>(
          lon1, lat1, lon2, lat2, Andoyer(spheroid), num_threads);
      break;
    case kThomas:
      return coordinate_distances<Geometry, Thomas>(
          lon1, lat1, lon2, lat2, Thomas(spheroid), num_threads);
      break;
    case kVincenty:
      return coordinate_distances<Geometry, Vincenty>(
          lon1, lat1, lon2, lat2, Vincenty(spheroid), num_threads);
      break;
  }
  throw std::invalid_argument("unknown strategy: " +
                              std::to_string(static_cast<int>(strategy)));
}

}  // namespace pyinterp::geodetic


#include <Eigen/LU>
#include <Eigen/SVD>

#include <PnC/WBC/WBLC/WBLC.hpp>
#include <Utils/IO/IOUtilities.hpp>

WBLC::WBLC(const std::vector<bool>& act_list, const Eigen::MatrixXd* Jci)
    : WBC(act_list, Jci) {
    myUtils::pretty_constructor(3, "WBLC");
    Sf_ = Eigen::MatrixXd::Zero(6, num_qdot_);
    Sf_.block(0, 0, 6, 6).setIdentity();

    act_list_.clear();
    for (int i(0); i < num_qdot_; ++i) {
        if (act_list[i]) act_list_.push_back(i);
    }
    qddot_ = Eigen::VectorXd::Zero(num_qdot_);
    // dynacore::pretty_print(Sv_, std::cout, "Sv");
}

void WBLC::updateSetting(const Eigen::MatrixXd& A, const Eigen::MatrixXd& Ainv,
                         const Eigen::VectorXd& cori,
                         const Eigen::VectorXd& grav, void* extra_setting) {
    A_ = A;
    Ainv_ = Ainv;
    cori_ = cori;
    grav_ = grav;
    b_updatesetting_ = true;

    // dynacore::pretty_print(grav_, std::cout, "grav");
    // dynacore::pretty_print(cori_, std::cout, "cori");
    // dynacore::pretty_print(A_, std::cout, "A");
}

void WBLC::makeWBLC_Torque(const Eigen::VectorXd& des_jacc_cmd,
                           const std::vector<ContactSpec*>& contact_list,
                           Eigen::VectorXd& cmd, void* extra_input) {
    if (!b_updatesetting_) {
        printf("[Wanning] WBLC setting is not done\n");
    }
    if (extra_input) data_ = static_cast<WBLC_ExtraData*>(extra_input);

    // Internal Constraint Check
    Nci_ = Eigen::MatrixXd::Identity(num_qdot_, num_qdot_);

    if (b_internal_constraint_) {
        Eigen::MatrixXd JciBar;
        _WeightedInverse(Jci_, Ainv_, JciBar);
        Nci_ -= JciBar * Jci_;
    }
    for (int i(0); i < num_act_joint_; ++i) {
        qddot_[act_list_[i]] = des_jacc_cmd[i];
    }

    // Contact Jacobian & Uf & Fr_ieq
    _BuildContactMtxVect(contact_list);

    // Dimension Setting
    dim_opt_ = num_qdot_ + 2 * dim_rf_;  // (delta_qddot, Fr, xddot_c)
    dim_eq_cstr_ = num_passive_ + dim_rf_;
    dim_ieq_cstr_ = 2 * num_act_joint_ + Uf_.rows();

    _Build_Equality_Constraint();
    _Build_Inequality_Constraint();
    _OptimizationPreparation(Aeq_, beq_, Cieq_, dieq_);

    double f = solve_quadprog(G, g0, CE, ce0, CI, ci0, z);

    _GetSolution(cmd);
    // std::cout << "f: " << f << std::endl;
    // std::cout << "x: " << z << std::endl;
    // std::cout << "cmd: "<<cmd<<std::endl;

    // if(f > 1.e5){
    //   std::cout << "f: " << f << std::endl;
    //   std::cout << "x: " << z << std::endl;
    //   std::cout << "cmd: "<<cmd<<std::endl;

    //   printf("G:\n");
    //   std::cout<<G<<std::endl;
    //   printf("g0:\n");
    //   std::cout<<g0<<std::endl;

    //   printf("CE:\n");
    //   std::cout<<CE<<std::endl;
    //   printf("ce0:\n");
    //   std::cout<<ce0<<std::endl;

    //   printf("CI:\n");
    //   std::cout<<CI<<std::endl;
    //   printf("ci0:\n");
    //   std::cout<<ci0<<std::endl;
    // }
}

void WBLC::_Build_Inequality_Constraint() {
    Cieq_ = Eigen::MatrixXd::Zero(dim_ieq_cstr_, dim_opt_);
    dieq_ = Eigen::VectorXd::Zero(dim_ieq_cstr_);
    int row_idx(0);

    Cieq_.block(row_idx, num_qdot_, Uf_.rows(), dim_rf_) = Uf_;
    dieq_.head(Uf_.rows()) = Fr_ieq_;
    row_idx += Uf_.rows();

    Cieq_.block(row_idx, 0, num_act_joint_, num_qdot_) = Sa_ * A_;
    Cieq_.block(row_idx, num_qdot_, num_act_joint_, dim_rf_) =
        -Sa_ * Jc_.transpose();
    dieq_.segment(row_idx, num_act_joint_) =
        data_->tau_min_ - Sa_ * (cori_ + grav_ + A_ * qddot_);
    row_idx += num_act_joint_;

    Cieq_.block(row_idx, 0, num_act_joint_, num_qdot_) = -Sa_ * A_;
    Cieq_.block(row_idx, num_qdot_, num_act_joint_, dim_rf_) =
        Sa_ * Jc_.transpose();
    dieq_.segment(row_idx, num_act_joint_) =
        -data_->tau_max_ + Sa_ * (cori_ + grav_ + A_ * qddot_);

    // myUtils::pretty_print(Cieq_, std::cout, "C ieq");
    // myUtils::pretty_print(dieq_, std::cout, "d ieq");
}

void WBLC::_Build_Equality_Constraint() {
    Aeq_ = Eigen::MatrixXd::Zero(dim_eq_cstr_, dim_opt_);
    beq_ = Eigen::VectorXd::Zero(dim_eq_cstr_);

    // passive joint
    Aeq_.block(0, 0, num_passive_, num_qdot_) = Sv_ * A_;
    Aeq_.block(0, num_qdot_, num_passive_, dim_rf_) = -Sv_ * Jc_.transpose();
    beq_.head(num_passive_) = -Sv_ * (A_ * qddot_ + cori_ + grav_);

    // xddot
    Aeq_.block(num_passive_, 0, dim_rf_, num_qdot_) = Jc_;
    Aeq_.bottomRightCorner(dim_rf_, dim_rf_) =
        -Eigen::MatrixXd::Identity(dim_rf_, dim_rf_);
    beq_.tail(dim_rf_) = -Jc_ * qddot_ - JcDotQdot_;

    // myUtils::pretty_print(Aeq_, std::cout, "Aeq");
    // myUtils::pretty_print(beq_, std::cout, "beq");
}

void WBLC::_BuildContactMtxVect(const std::vector<ContactSpec*>& contact_list) {
    ContactSpec* contact = contact_list[0];
    contact->getContactJacobian(Jc_);
    contact->getJcDotQdot(JcDotQdot_);
    contact->getRFConstraintMtx(Uf_);
    contact->getRFConstraintVec(Fr_ieq_);

    Eigen::MatrixXd Jc_i, Uf_i;
    Eigen::VectorXd Fr_ieq_i, JcDotQdot_i;

    dim_rf_ = Jc_.rows();
    int num_rows_Uf = Uf_.rows();
    for (int i(1); i < contact_list.size(); ++i) {
        contact = contact_list[i];
        contact->getContactJacobian(Jc_i);
        contact->getJcDotQdot(JcDotQdot_i);
        contact->getRFConstraintMtx(Uf_i);
        contact->getRFConstraintVec(Fr_ieq_i);

        Jc_.conservativeResize(dim_rf_ + Jc_i.rows(), num_qdot_);
        Jc_.block(dim_rf_, 0, Jc_i.rows(), num_qdot_) = Jc_i;

        JcDotQdot_.conservativeResize(dim_rf_ + Jc_i.rows());
        JcDotQdot_.tail(Jc_i.rows()) = JcDotQdot_i;

        Uf_.conservativeResize(num_rows_Uf + Uf_i.rows(),
                               dim_rf_ + Uf_i.cols());
        (Uf_.topRightCorner(num_rows_Uf, Uf_i.cols())).setZero();
        (Uf_.bottomLeftCorner(Uf_i.rows(), dim_rf_)).setZero();
        Uf_.block(num_rows_Uf, dim_rf_, Uf_i.rows(), Uf_i.cols()) = Uf_i;

        Fr_ieq_.conservativeResize(num_rows_Uf + Uf_i.rows());
        Fr_ieq_.tail(Uf_i.rows()) = Fr_ieq_i;

        dim_rf_ += Jc_i.rows();
        num_rows_Uf += Uf_i.rows();
    }
    // myUtils::pretty_print(Jc_, std::cout, "Jc");
    // myUtils::pretty_print(Uf_, std::cout, "Uf");
    // myUtils::pretty_print(JcDotQdot_, std::cout, "JcDotQdot");
    // myUtils::pretty_print(Fr_ieq_, std::cout, "Fr_ieq");
}

void WBLC::_OptimizationPreparation(const Eigen::MatrixXd& Aeq,
                                    const Eigen::VectorXd& beq,
                                    const Eigen::MatrixXd& Cieq,
                                    const Eigen::VectorXd& dieq) {
    G.resize(dim_opt_, dim_opt_);
    g0.resize(dim_opt_);
    CE.resize(dim_opt_, dim_eq_cstr_);
    ce0.resize(dim_eq_cstr_);
    CI.resize(dim_opt_, dim_ieq_cstr_);
    ci0.resize(dim_ieq_cstr_);

    for (int i(0); i < dim_opt_; ++i) {
        for (int j(0); j < dim_opt_; ++j) {
            G[i][j] = 0.;
        }
        g0[i] = 0.;
    }
    // Set Cost
    for (int i(0); i < num_qdot_; ++i) {
        G[i][i] = data_->W_qddot_[i];
    }
    int idx_offset = num_qdot_;
    for (int i(0); i < dim_rf_; ++i) {
        G[i + idx_offset][i + idx_offset] = data_->W_rf_[i];
    }
    idx_offset += dim_rf_;
    for (int i(0); i < dim_rf_; ++i) {
        G[i + idx_offset][i + idx_offset] = data_->W_xddot_[i];
    }

    for (int i(0); i < dim_eq_cstr_; ++i) {
        for (int j(0); j < dim_opt_; ++j) {
            CE[j][i] = Aeq(i, j);
        }
        ce0[i] = -beq[i];
    }

    for (int i(0); i < dim_ieq_cstr_; ++i) {
        for (int j(0); j < dim_opt_; ++j) {
            CI[j][i] = Cieq(i, j);
        }
        ci0[i] = -dieq[i];
    }
    // printf("G:\n");
    // std::cout << G << std::endl;
    // printf("g0:\n");
    // std::cout << g0 << std::endl;

    // printf("CE:\n");
    // std::cout << CE << std::endl;
    // printf("ce0:\n");
    // std::cout << ce0 << std::endl;

    // printf("CI:\n");
    // std::cout << CI << std::endl;
    // printf("ci0:\n");
    // std::cout << ci0 << std::endl;
}

void WBLC::makeTorque(const std::vector<Task*>& task_list,
                      const std::vector<ContactSpec*>& contact_list,
                      Eigen::VectorXd& cmd, void* extra_input) {}

void WBLC::_GetSolution(Eigen::VectorXd& cmd) {
    Eigen::VectorXd delta_qddot(num_qdot_);
    for (int i(0); i < num_qdot_; ++i) delta_qddot[i] = z[i];
    data_->Fr_ = Eigen::VectorXd(dim_rf_);
    for (int i(0); i < dim_rf_; ++i) data_->Fr_[i] = z[i + num_qdot_];

    Eigen::VectorXd tau = A_ * (qddot_ + delta_qddot) + cori_ + grav_ -
                          Jc_.transpose() * data_->Fr_;

    data_->qddot_ = qddot_ + delta_qddot;
    cmd = Sa_ * tau;

    // myUtils::pretty_print(qddot_, std::cout, "qddot_");
    // myUtils::pretty_print(delta_qddot, std::cout, "delta_qddot");
    // myUtils::pretty_print(data_->Fr_, std::cout, "Fr");
    // myUtils::pretty_print(tau, std::cout, "total tau");
    // Eigen::VectorXd x_check = Jc_ * (qddot_ + delta_qddot) + JcDotQdot_;
    // myUtils::pretty_print(x_check, std::cout, "x check");
}


/*
 * BSD 3-Clause License
 * Copyright (c) 2020, Levente Kiss
 * All rights reserved.
 *
 * You may obtain a copy of the License at
 * https://opensource.org/licenses/BSD-3-Clause
 */

#ifndef OLP_LOAMSLAMCALCULATOR_HPP
#define OLP_LOAMSLAMCALCULATOR_HPP

#include <thread>

#include <boost/thread/barrier.hpp>

#include <pcl/common/io.h>
#include <pcl/common/transforms.h>
#include <pcl/kdtree/kdtree_flann.h>

#include "../../helpers/ViewerHelper.h"
#include "Calculator.h"

namespace olp
{
namespace compute
{
template<typename PointType>
class LOAMSLAMCalculator : public Calculator<PointType>
{
public:
    LOAMSLAMCalculator(TransformData data) : Calculator<PointType>(data), BEAMCOUNT_(16), _barrier(BEAMCOUNT_ + 1)
    {
        _previousCloud = pcl::PointCloud<PointType>().makeShared();
    }

    TransformData calculate(typename pcl::PointCloud<PointType>::ConstPtr input) override;

    const std::string stringId() const override
    {
        return "LOAM";
    }

protected:
    unsigned const BEAMCOUNT_;
    const double SCAN_PERIOD_ = 0.1;
    const float THRESHOLD_ = 0.1;
    const double NEARBY_SCAN_ = 2.5;
private:
    boost::barrier _barrier;
    typename pcl::PointCloud<PointType>::Ptr _previousCloud;

    inline int _pointID(const PointType& point);

    void _calculateDistortion(typename pcl::PointCloud<PointType>::Ptr points);

    void _transformToStart(PointType const* const pi,
                           PointType* const po,
                           const Eigen::Vector3d& translation,
                           const Eigen::Vector3d& rotation);

    void _findPlanarAndEdgePoint(const typename pcl::PointCloud<PointType>::ConstPtr input,
                                 std::vector<int>& planarPointIndexes,
                                 std::vector<int>& edgePointIndexes,
                                 int maxEdgePoints, int maxPlanarPoints,
                                 float threshold);

    void _findCorrespondances(const typename pcl::PointCloud<PointType>::ConstPtr previousCloud,
                              const typename pcl::PointCloud<PointType>::ConstPtr currentCloud,
                              const std::vector<int>& planarPointIndexes,
                              const std::vector<int>& edgePointIndexes,
                              Eigen::Vector3d& rotation,
                              Eigen::Vector3d& translation,
                              float threshold);
};

template<typename PointType>
TransformData LOAMSLAMCalculator<PointType>::calculate(typename pcl::PointCloud<PointType>::ConstPtr input)
{
    typename pcl::PointCloud<PointType>::Ptr correctedIn(new pcl::PointCloud<PointType>);
    pcl::copyPointCloud(*input, *correctedIn);
    std::vector<int> planarPoints;
    std::vector<int> edgePoints;
    std::shared_ptr<std::map<int, float>> cValues(new std::map<int, float>);

    _calculateDistortion(correctedIn);
    _findPlanarAndEdgePoint(correctedIn, planarPoints, edgePoints, 2, 4, THRESHOLD_);

    if (_previousCloud->size() > 0)
    {
        Eigen::Vector3d rotation, translation;
        _findCorrespondances(_previousCloud, correctedIn, planarPoints, edgePoints, rotation, translation, 25);
        TransformData result = TransformData(70);
        result.transform *= Eigen::AngleAxisd(rotation(2), Eigen::Vector3d::UnitZ());
        result.transform.translation() = translation;
        return result;
    }
    pcl::copyPointCloud(*input, *_previousCloud);
    return TransformData();
}

template<typename PointType>
inline int LOAMSLAMCalculator<PointType>::_pointID(const PointType& point)
{
    float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;
    int scanID = 0;
    scanID = int((angle + 15) / 2 + 0.5);
    if (scanID > (BEAMCOUNT_ - 1) || scanID < 0)
    {
        return -1;
    }
    return scanID;
}

template<typename PointType>
void LOAMSLAMCalculator<PointType>::_calculateDistortion(typename pcl::PointCloud<PointType>::Ptr points)
{
    bool halfPassed = false;
    float startOri = -atan2(points->points[0].y, points->points[0].x);
    float endOri = -atan2(points->points[points->points.size() - 1].y,
                          points->points[points->points.size() - 1].x) +
                   2 * M_PI;
    if (endOri - startOri > 3 * M_PI)
    {
        endOri -= 2 * M_PI;
    }
    else if (endOri - startOri < M_PI)
    {
        endOri += 2 * M_PI;
    }
    int cloudSize = points->points.size();
    for (int i = 0; i < cloudSize; i++)
    {
        int scanID = _pointID(points->points[i]);

        float ori = -atan2(points->points[i].y, points->points[i].x);
        if (!halfPassed)
        {
            if (ori < startOri - M_PI / 2)
            {
                ori += 2 * M_PI;
            }
            else if (ori > startOri + M_PI * 3 / 2)
            {
                ori -= 2 * M_PI;
            }

            if (ori - startOri > M_PI)
            {
                halfPassed = true;
            }
        }
        else
        {
            ori += 2 * M_PI;
            if (ori < endOri - M_PI * 3 / 2)
            {
                ori += 2 * M_PI;
            }
            else if (ori > endOri + M_PI / 2)
            {
                ori -= 2 * M_PI;
            }
        }

        float relTime = (ori - startOri) / (endOri - startOri);
        points->points[i].intensity = scanID + SCAN_PERIOD_ * relTime;
    }
}

template<typename PointType>
void LOAMSLAMCalculator<PointType>::_transformToStart(
    PointType const* const pi,
    PointType* const po,
    const Eigen::Vector3d& translation,
    const Eigen::Vector3d& rotation)
{
    //interpolation ratio
    double s = (pi->intensity - int(pi->intensity)) / SCAN_PERIOD_;

    auto scaledT = translation * s;
    auto scaledR = rotation * s;

    float rx = s * scaledR[0];
    float ry = s * scaledR[1];
    float rz = s * scaledR[2];
    float tx = s * scaledT[0];
    float ty = s * scaledT[1];
    float tz = s * scaledT[2];

    float x1 = cos(rz) * (pi->x - tx) + sin(rz) * (pi->y - ty);
    float y1 = -sin(rz) * (pi->x - tx) + cos(rz) * (pi->y - ty);
    float z1 = (pi->z - tz);

    float x2 = x1;
    float y2 = cos(rx) * y1 + sin(rx) * z1;
    float z2 = -sin(rx) * y1 + cos(rx) * z1;

    po->x = cos(ry) * x2 - sin(ry) * z2;
    po->y = y2;
    po->z = sin(ry) * x2 + cos(ry) * z2;
    po->intensity = pi->intensity;
}

template<typename PointType>
void LOAMSLAMCalculator<PointType>::_findPlanarAndEdgePoint(
    const typename pcl::PointCloud<PointType>::ConstPtr input,
    std::vector<int>& planarPointIndexes,
    std::vector<int>& edgePointIndexes,
    int maxEdgePoints, int maxPlanarPoints,
    float threshold)
{
    const float MAXDISTANCE = 0.05;
    std::vector<std::shared_ptr<std::vector<std::pair<int, Eigen::Vector3f>>>> pointsPerBeam;
    for (auto i = 0; i < BEAMCOUNT_; ++i)
    {
        pointsPerBeam.push_back(std::shared_ptr<std::vector<std::pair<int, Eigen::Vector3f>>>(
            new std::vector<std::pair<int, Eigen::Vector3f>>()));
    }
    for (auto i = 0; i < input->size(); ++i)
    {
        int beamIndex = _pointID(input->points[i]);
        Eigen::Vector3f tmpPt(input->points[i].x, input->points[i].y, input->points[i].z);
        pointsPerBeam[beamIndex]->push_back(std::pair<int, Eigen::Vector3f>(i, tmpPt));

    }
    std::vector<std::shared_ptr<std::vector<int>>> beamEdgesPoints;
    std::vector<std::shared_ptr<std::vector<int>>> beamPlanarPoints;

    auto f = [this, &threshold, &maxEdgePoints, &maxPlanarPoints, &MAXDISTANCE](
        const std::vector<std::pair<int, Eigen::Vector3f>>& points,
        std::shared_ptr<std::vector<int>> selectedEdgePoints,
        std::shared_ptr<std::vector<int>> selectedPlanarPoints)
    {
        std::vector<float> cloudCurvature(points.size());
        std::vector<int> cloudSortInd(points.size());
        std::vector<int> cloudNeighborPicked(points.size());
        std::vector<int> cloudLabel(points.size());
        if (points.size() > 11)
        {
            for (int i = 5; i < points.size() - 6; i++)
            {
                float diffX =
                    points.at(i - 5).second.x() + points.at(i - 4).second.x() + points.at(i - 3).second.x() +
                    points.at(i - 2).second.x() + points.at(i - 1).second.x() - 10 * points.at(i).second.x() +
                    points.at(i + 1).second.x() + points.at(i + 2).second.x() + points.at(i + 3).second.x() +
                    points.at(i + 4).second.x() + points.at(i + 5).second.x();
                float diffY =
                    points.at(i - 5).second.y() + points.at(i - 4).second.y() + points.at(i - 3).second.y() +
                    points.at(i - 2).second.y() + points.at(i - 1).second.y() - 10 * points.at(i).second.y() +
                    points.at(i + 1).second.y() + points.at(i + 2).second.y() + points.at(i + 3).second.y() +
                    points.at(i + 4).second.y() + points.at(i + 5).second.y();
                float diffZ =
                    points.at(i - 5).second.z() + points.at(i - 4).second.z() + points.at(i - 3).second.z() +
                    points.at(i - 2).second.z() + points.at(i - 1).second.z() - 10 * points.at(i).second.z() +
                    points.at(i + 1).second.z() + points.at(i + 2).second.z() + points.at(i + 3).second.z() +
                    points.at(i + 4).second.z() + points.at(i + 5).second.z();
                float c = diffX * diffX + diffY * diffY + diffZ * diffZ;
                cloudCurvature[i] = c;
                cloudSortInd[i] = i;
                cloudNeighborPicked[i] = 0;
                cloudLabel[i] = 0;
            }

            for (int i = 0; i < 6; ++i)
            {
                int startIndex = 5 + (points.size() - 10) * i / 6;
                int endIndex = 5 + (points.size() - 10) * (i + 1) / 6 - 1;
                std::sort(cloudSortInd.begin() + startIndex, cloudSortInd.begin() + endIndex + 1,
                          [&cloudCurvature](int i, int j)
                          {
                              return cloudCurvature[i] < cloudCurvature[j];
                          });
                int largestPickedNum = 0;
                for (int k = endIndex; k >= startIndex; k--)
                {
                    int ind = cloudSortInd[k];

                    if (cloudNeighborPicked[ind] == 0 &&
                        cloudCurvature[ind] > threshold)
                    {

                        largestPickedNum++;

                        if (largestPickedNum <= maxEdgePoints)
                        {
                            cloudLabel[ind] = 1;
                            selectedEdgePoints->push_back(points[ind].first);
                        }
                        else
                        {
                            break;
                        }

                        cloudNeighborPicked[ind] = 1;
                        //not selecting close points
                        for (int l = 1; l <= 5; l++)
                        {
                            float diffX = points.at(ind + l).second.x() - points.at(ind + l - 1).second.x();
                            float diffY = points.at(ind + l).second.y() - points.at(ind + l - 1).second.y();
                            float diffZ = points.at(ind + l).second.z() - points.at(ind + l - 1).second.z();
                            if (diffX * diffX + diffY * diffY + diffZ * diffZ > MAXDISTANCE)
                            {
                                break;
                            }

                            cloudNeighborPicked[ind + l] = 1;
                        }
                        for (int l = -1; l >= -5; l--)
                        {
                            float diffX = points.at(ind + l).second.x() - points.at(ind + l + 1).second.x();
                            float diffY = points.at(ind + l).second.y() - points.at(ind + l + 1).second.y();
                            float diffZ = points.at(ind + l).second.z() - points.at(ind + l + 1).second.z();
                            if (diffX * diffX + diffY * diffY + diffZ * diffZ > MAXDISTANCE)
                            {
                                break;
                            }

                            cloudNeighborPicked[ind + l] = 1;
                        }
                    }

                }

                int smallestPickedNum = 0;
                for (int k = startIndex; k <= endIndex; k++)
                {
                    int ind = cloudSortInd[k];

                    if (ind < 1)
                    {
                        continue;
                    }

                    if (cloudNeighborPicked[ind] == 0 &&
                        cloudCurvature[ind] < threshold)
                    {

                        cloudLabel[ind] = -1;
                        selectedPlanarPoints->push_back(points.at(ind).first);

                        smallestPickedNum++;
                        if (smallestPickedNum >= maxPlanarPoints)
                        {
                            break;
                        }

                        cloudNeighborPicked[ind] = 1;
                        for (int l = 1; l <= 5; l++)
                        {
                            float diffX = points.at(ind + l).second.x() - points.at(ind + l - 1).second.x();
                            float diffY = points.at(ind + l).second.y() - points.at(ind + l - 1).second.y();
                            float diffZ = points.at(ind + l).second.z() - points.at(ind + l - 1).second.z();
                            if (diffX * diffX + diffY * diffY + diffZ * diffZ > MAXDISTANCE)
                            {
                                break;
                            }

                            cloudNeighborPicked[ind + l] = 1;
                        }
                        for (int l = -1; l >= -5; l--)
                        {
                            float diffX = points.at(ind + l).second.x() - points.at(ind + l + 1).second.x();
                            float diffY = points.at(ind + l).second.y() - points.at(ind + l + 1).second.y();
                            float diffZ = points.at(ind + l).second.z() - points.at(ind + l + 1).second.z();
                            if (diffX * diffX + diffY * diffY + diffZ * diffZ > MAXDISTANCE)
                            {
                                break;
                            }

                            cloudNeighborPicked[ind + l] = 1;
                        }
                    }
                }
            }
        }
        _barrier.wait();
    };
    int i = 0;
    for (auto beamPoints : pointsPerBeam)
    {
        auto edgeTmp = std::shared_ptr<std::vector<int>>(new std::vector<int>);
        beamEdgesPoints.push_back(edgeTmp);
        auto planeTmp = std::shared_ptr<std::vector<int>>(new std::vector<int>);
        beamPlanarPoints.push_back(planeTmp);
        std::thread(f, *beamPoints, edgeTmp, planeTmp).detach();
        ++i;
    }
    _barrier.wait();
    for (auto edgePoints : beamEdgesPoints)
    {
        edgePointIndexes.insert(edgePointIndexes.end(), edgePoints->begin(), edgePoints->end());
    }
    for (auto planarPoints : beamPlanarPoints)
    {
        planarPointIndexes.insert(planarPointIndexes.end(), planarPoints->begin(), planarPoints->end());
    }
}

template<typename PointType>
void LOAMSLAMCalculator<PointType>::_findCorrespondances(
    const typename pcl::PointCloud<PointType>::ConstPtr previousCloud,
    const typename pcl::PointCloud<PointType>::ConstPtr currentCloud,
    const std::vector<int>& planarPointIndexes,
    const std::vector<int>& edgePointIndexes,
    Eigen::Vector3d& rotation,
    Eigen::Vector3d& translation,
    float threshold)
{
    Eigen::Vector3d tr = Eigen::Vector3d::Zero();
    Eigen::Vector3d rt = Eigen::Vector3d::Zero();

    typename pcl::KdTreeFLANN<PointType>::Ptr kdtreeCornerLast(new pcl::KdTreeFLANN<PointType>());
    typename pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfLast(new pcl::KdTreeFLANN<PointType>());
    kdtreeCornerLast->setInputCloud(previousCloud);
    kdtreeSurfLast->setInputCloud(previousCloud);

    int cornerPointsSharpNum = edgePointIndexes.size();
    int surfPointsFlatNum = planarPointIndexes.size();

    // neighbour edgePoint indices (J,K)
    std::map<int, int> edgeJPointIndices;
    std::map<int, int> edgeLPointIndices;

    // neighbour planarPoint indices (J,K,L)
    std::map<int, int> planarJPointIndices;
    std::map<int, int> planarLPointIndices;
    std::map<int, int> planarMPointIndices;

    PointType coeff;
    typename pcl::PointCloud<PointType>::Ptr coeffSel(new pcl::PointCloud<PointType>());
    typename pcl::PointCloud<PointType>::Ptr laserCloudOri(new pcl::PointCloud<PointType>());

    if (planarPointIndexes.size() > 100 && edgePointIndexes.size() > 10)
    {
        for (int iterationCount = 0; iterationCount < 25; ++iterationCount)
        {
            std::vector<int> pointSearchInd(1);
            std::vector<float> pointSearchSqDis(1);

            for (auto idx : planarPointIndexes)
            {
                PointType currentPt = currentCloud->points[idx];
                _transformToStart(&currentCloud->points[idx], &currentPt, tr, rt);
                if (iterationCount % 5 == 0)
                {
                    kdtreeSurfLast->nearestKSearch(currentPt, 1, pointSearchInd, pointSearchSqDis);
                    int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;
                    if (pointSearchSqDis[0] < threshold)
                    {
                        closestPointInd = pointSearchInd[0];
                        int closestPointScan = _pointID(previousCloud->points[closestPointInd]);

                        float pointSqDis, minPointSqDis2 = threshold, minPointSqDis3 = threshold;
                        for (int j = closestPointInd + 1; j < surfPointsFlatNum; j++)
                        {
                            int currentPointID = _pointID(previousCloud->points[closestPointInd]);
                            if (currentPointID > closestPointScan + NEARBY_SCAN_)
                            {
                                break;
                            }
                            pointSqDis = (previousCloud->points[j].x - currentPt.x) *
                                         (previousCloud->points[j].x - currentPt.x) +
                                         (previousCloud->points[j].y - currentPt.y) *
                                         (previousCloud->points[j].y - currentPt.y) +
                                         (previousCloud->points[j].z - currentPt.z) *
                                         (previousCloud->points[j].z - currentPt.z);
                            if (currentPointID <= closestPointScan && pointSqDis < minPointSqDis2)
                            {
                                minPointSqDis2 = pointSqDis;
                                minPointInd2 = j;
                            }
                            else if (currentPointID > closestPointScan && pointSqDis < minPointSqDis3)
                            {
                                minPointSqDis3 = pointSqDis;
                                minPointInd3 = j;
                            }
                        }
                        for (int j = closestPointInd - 1; j >= 0; j--)
                        {
                            int currentPointID = _pointID(previousCloud->points[closestPointInd]);
                            if (currentPointID < closestPointScan - NEARBY_SCAN_)
                            {
                                break;
                            }
                            pointSqDis = (previousCloud->points[j].x - currentPt.x) *
                                         (previousCloud->points[j].x - currentPt.x) +
                                         (previousCloud->points[j].y - currentPt.y) *
                                         (previousCloud->points[j].y - currentPt.y) +
                                         (previousCloud->points[j].z - currentPt.z) *
                                         (previousCloud->points[j].z - currentPt.z);
                            if (currentPointID >= closestPointScan && pointSqDis < minPointSqDis2)
                            {
                                minPointSqDis2 = pointSqDis;
                                minPointInd2 = j;
                            }
                            else if (_pointID(previousCloud->points[j]) < closestPointScan &&
                                     pointSqDis < minPointSqDis3)
                            {
                                minPointSqDis3 = pointSqDis;
                                minPointInd3 = j;
                            }
                        }
                    }

                    planarJPointIndices[idx] = closestPointInd;
                    planarLPointIndices[idx] = minPointInd2;
                    planarMPointIndices[idx] = minPointInd3;
                    pointSearchInd.clear();
                    pointSearchSqDis.clear();
                }

                if (planarMPointIndices[idx] >= 0 && planarLPointIndices[idx] >= 0)
                {
                    Eigen::Vector3f J = previousCloud->points[planarJPointIndices[idx]].getVector3fMap();
                    Eigen::Vector3f L = previousCloud->points[planarLPointIndices[idx]].getVector3fMap();
                    Eigen::Vector3f M = previousCloud->points[planarMPointIndices[idx]].getVector3fMap();

                    float pa = (L[1] - J[1]) * (M[2] - J[2])
                               - (M[1] - J[1]) * (L[2] - L[2]);

                    float pb = (L[2] - J[2]) * (M[0] - J[0])
                               - (M[2] - J[2]) * (L[0] - J[0]);

                    float pc = (L[0] - J[0]) * (M[1] - J[1])
                               - (M[0] - J[0]) * (L[1] - J[1]);
                    float pd = -(pa * J[0] + pb * J[1] + pc * J[2]);

                    float ps = sqrt(pa * pa + pb * pb + pc * pc);

                    pa /= ps;
                    pb /= ps;
                    pc /= ps;
                    pd /= ps;

                    float pd2 = pa * currentPt.x + pb * currentPt.y + pc * currentPt.z + pd;

                    float s = 1;
                    if (iterationCount >= 5)
                    {
                        s = 1 - 1.8 * fabs(pd2) / sqrt(sqrt(currentPt.x * currentPt.x
                                                            + currentPt.y * currentPt.y +
                                                            currentPt.z * currentPt.z));
                    }

                    coeff.x = s * pa;
                    coeff.y = s * pb;
                    coeff.z = s * pc;
                    coeff.intensity = s * pd2;

                    if (s > 0.1 && pd2 != 0)
                    {

                        laserCloudOri->push_back(currentCloud->points[idx]);
                        coeffSel->push_back(coeff);
                    }
                }
            }

            for (auto idx : edgePointIndexes)
            {
                PointType currentPt = currentCloud->points[idx];
                _transformToStart(&currentCloud->points[idx], &currentPt, tr, rt);
                if (iterationCount % 5 == 0)
                {
                    kdtreeCornerLast->nearestKSearch(currentPt, 1, pointSearchInd, pointSearchSqDis);
                    int closestPointInd = -1, minPointInd2 = -1;
                    if (pointSearchSqDis[0] < threshold)
                    {
                        closestPointInd = pointSearchInd[0];
                        int closestPointScan = _pointID(previousCloud->points[closestPointInd]);

                        float pointSqDis, minPointSqDis2 = threshold;
                        for (int j = closestPointInd + 1; j < cornerPointsSharpNum; j++)
                        {
                            int currentPointID = _pointID(previousCloud->points[j]);
                            if (currentPointID > closestPointScan + NEARBY_SCAN_)
                            {
                                break;
                            }

                            if (currentPointID <= closestPointScan)
                            {
                                continue;
                            }
                            pointSqDis = (previousCloud->points[j].x - currentPt.x) *
                                         (previousCloud->points[j].x - currentPt.x) +
                                         (previousCloud->points[j].y - currentPt.y) *
                                         (previousCloud->points[j].y - currentPt.y) +
                                         (previousCloud->points[j].z - currentPt.z) *
                                         (previousCloud->points[j].z - currentPt.z);

                            if (pointSqDis < minPointSqDis2)
                            {
                                minPointSqDis2 = pointSqDis;
                                minPointInd2 = j;
                            }
                        }

                        for (int j = closestPointInd - 1; j >= 0; j--)
                        {
                            int currentPointID = _pointID(previousCloud->points[j]);
                            if (currentPointID < closestPointScan - NEARBY_SCAN_)
                            {
                                break;
                            }
                            if (currentPointID >= closestPointScan)
                            {
                                continue;
                            }

                            pointSqDis = (previousCloud->points[j].x - currentPt.x) *
                                         (previousCloud->points[j].x - currentPt.x) +
                                         (previousCloud->points[j].y - currentPt.y) *
                                         (previousCloud->points[j].y - currentPt.y) +
                                         (previousCloud->points[j].z - currentPt.z) *
                                         (previousCloud->points[j].z - currentPt.z);

                            if (pointSqDis < minPointSqDis2)
                            {
                                minPointSqDis2 = pointSqDis;
                                minPointInd2 = j;
                            }
                        }
                    }
                    /*********************************************/

                    edgeJPointIndices[idx] = closestPointInd;
                    edgeLPointIndices[idx] = minPointInd2;
                    pointSearchInd.clear();
                    pointSearchSqDis.clear();
                }

                if (edgeLPointIndices[idx] >= 0)
                {
                    Eigen::Vector3f I = currentPt.getVector3fMap();
                    Eigen::Vector3f J = previousCloud->points[edgeJPointIndices[idx]].getVector3fMap();
                    Eigen::Vector3f L = previousCloud->points[edgeLPointIndices[idx]].getVector3fMap();

                    float num = (I - J).cross(I - L).norm();

                    float deNum = (J - L).norm();

                    float d = num / deNum;

                    float la = ((J[1] - L[1]) * ((I[0] - J[0]) * (I[1] - L[1]) - (I[0] - L[0]) * (I[1] - J[1]))
                                + (J[2] - L[2]) * ((I[0] - J[0]) * (I[2] - L[2]) - (I[0] - L[0]) * (I[2] - J[2]))) /
                               num / deNum;

                    float lb = -((J[0] - L[0]) * ((I[0] - J[0]) * (I[1] - L[1]) - (I[0] - L[0]) * (I[1] - J[1]))
                                 -
                                 (J[2] - L[2]) * ((I[1] - J[1]) * (I[2] - L[2]) - (I[1] - L[1]) * (I[2] - J[2]))) /
                               num / deNum;

                    float lc = -((J[0] - L[0]) * ((I[0] - J[0]) * (I[2] - L[2]) - (I[0] - L[0]) * (I[2] - J[2]))
                                 +
                                 (J[1] - L[1]) * ((I[1] - J[1]) * (I[2] - L[2]) - (I[1] - L[1]) * (I[2] - J[2]))) /
                               num / deNum;

                    float s = 1;
                    if (iterationCount >= 5)
                    {
                        s = 1 - 1.8 * fabs(d);
                    }

                    coeff.x = s * la;
                    coeff.y = s * lb;
                    coeff.z = s * lc;
                    coeff.intensity = s * d;

                    if (s > 0.1 && d != 0)
                    {
                        laserCloudOri->push_back(currentCloud->points[idx]);
                        coeffSel->push_back(coeff);
                    }
                }
            }
            int pointSelNum = laserCloudOri->points.size();

            if (pointSelNum < 10)
            {
                continue;
            }
            Eigen::MatrixXf matA = Eigen::MatrixXf::Zero(pointSelNum, 6);
            Eigen::MatrixXf matAt = Eigen::MatrixXf::Zero(6, pointSelNum);
            Eigen::MatrixXf matAtA = Eigen::MatrixXf::Zero(6, 6);
            Eigen::VectorXf matB = Eigen::VectorXf::Zero(pointSelNum);
            Eigen::VectorXf matAtB = Eigen::VectorXf::Zero(6);
            Eigen::VectorXf matX = Eigen::VectorXf::Zero(6);

            Eigen::MatrixXf matP = Eigen::MatrixXf::Zero(6, 6);
            bool isDegenerate = false;

            for (int i = 0; i < pointSelNum; i++)
            {
                PointType pointOri = laserCloudOri->points[i];
                coeff = coeffSel->points[i];

                float s = 1;

                float srx = sin(s * rt[0]);
                float crx = cos(s * rt[0]);
                float sry = sin(s * rt[1]);
                float cry = cos(s * rt[1]);
                float srz = sin(s * rt[2]);
                float crz = cos(s * rt[2]);
                float tx = s * tr[0];
                float ty = s * tr[1];
                float tz = s * tr[2];

                float arx = (-s * crx * sry * srz * pointOri.x + s * crx * crz * sry * pointOri.y +
                             s * srx * sry * pointOri.z
                             + s * tx * crx * sry * srz - s * ty * crx * crz * sry - s * tz * srx * sry) * coeff.x
                            + (s * srx * srz * pointOri.x - s * crz * srx * pointOri.y + s * crx * pointOri.z
                               + s * ty * crz * srx - s * tz * crx - s * tx * srx * srz) * coeff.y
                            + (s * crx * cry * srz * pointOri.x - s * crx * cry * crz * pointOri.y -
                               s * cry * srx * pointOri.z
                               + s * tz * cry * srx + s * ty * crx * cry * crz - s * tx * crx * cry * srz) *
                              coeff.z;

                float ary = ((-s * crz * sry - s * cry * srx * srz) * pointOri.x
                             + (s * cry * crz * srx - s * sry * srz) * pointOri.y - s * crx * cry * pointOri.z
                             + tx * (s * crz * sry + s * cry * srx * srz) +
                             ty * (s * sry * srz - s * cry * crz * srx)
                             + s * tz * crx * cry) * coeff.x
                            + ((s * cry * crz - s * srx * sry * srz) * pointOri.x
                               + (s * cry * srz + s * crz * srx * sry) * pointOri.y - s * crx * sry * pointOri.z
                               + s * tz * crx * sry - ty * (s * cry * srz + s * crz * srx * sry)
                               - tx * (s * cry * crz - s * srx * sry * srz)) * coeff.z;

                float arz = ((-s * cry * srz - s * crz * srx * sry) * pointOri.x +
                             (s * cry * crz - s * srx * sry * srz) * pointOri.y
                             + tx * (s * cry * srz + s * crz * srx * sry) -
                             ty * (s * cry * crz - s * srx * sry * srz)) * coeff.x
                            + (-s * crx * crz * pointOri.x - s * crx * srz * pointOri.y
                               + s * ty * crx * srz + s * tx * crx * crz) * coeff.y
                            + ((s * cry * crz * srx - s * sry * srz) * pointOri.x +
                               (s * crz * sry + s * cry * srx * srz) * pointOri.y
                               + tx * (s * sry * srz - s * cry * crz * srx) -
                               ty * (s * crz * sry + s * cry * srx * srz)) * coeff.z;

                float atx = -s * (cry * crz - srx * sry * srz) * coeff.x + s * crx * srz * coeff.y
                            - s * (crz * sry + cry * srx * srz) * coeff.z;

                float aty = -s * (cry * srz + crz * srx * sry) * coeff.x - s * crx * crz * coeff.y
                            - s * (sry * srz - cry * crz * srx) * coeff.z;

                float atz = s * crx * sry * coeff.x - s * srx * coeff.y - s * crx * cry * coeff.z;

                float d2 = coeff.intensity;

                matA(i, 0) = arx;
                matA(i, 1) = ary;
                matA(i, 2) = arz;
                matA(i, 3) = atx;
                matA(i, 4) = aty;
                matA(i, 5) = atz;
                matB(i) = -0.05 * d2;
            }
            matAt = matA.transpose();

            matAtA = matAt * matA;
            matAtB = matAt * matB;
            matX = matAtA.householderQr().solve(matAtB);

            if (iterationCount == 0)
            {

                Eigen::VectorXf matE = Eigen::VectorXf::Zero(6);
                Eigen::MatrixXf matV = Eigen::MatrixXf::Zero(6, 6);
                Eigen::MatrixXf matV2 = Eigen::MatrixXf::Zero(6, 6);

                Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> solver(matAtA);
                matV = solver.eigenvectors();
                matE = solver.eigenvalues();
                matV2 = matV;

                isDegenerate = false;
                float eignThre[6] = {10, 10, 10, 10, 10, 10};
                for (int i = 5; i >= 0; i--)
                {
                    if (matE.transpose()(i) < eignThre[i])
                    {
                        for (int j = 0; j < 6; j++)
                        {
                            matV2(i, j) = 0;
                        }
                        isDegenerate = true;
                    }
                    else
                    {
                        break;
                    }
                }

                matP = matV.inverse() * matV2;
            }

            if (isDegenerate)
            {
                Eigen::VectorXf matX2(matX);
                matX = matP * matX2;
            }

            rt[0] += matX[0];
            rt[1] += matX[1];
            rt[2] += matX[2];
            tr[0] += matX[3];
            tr[1] += matX[4];
            tr[2] += matX[5];

            for (int i = 0; i < 3; i++)
            {
                if (isnan(tr[i]))
                    tr[i] = 0;
                if (isnan(rt[i]))
                    rt[i] = 0;
            }

            float deltaR = sqrt(
                pow((matX[0] / M_PI * 180.0), 2) +
                pow((matX[1] / M_PI * 180.0), 2) +
                pow((matX[2] / M_PI * 180.0), 2));
            float deltaT = sqrt(
                pow(matX[3] * 100, 2) +
                pow(matX[4] * 100, 2) +
                pow(matX[5] * 100, 2));

            //  std::cout << "delta R: " << deltaR << " delta T: " << deltaT << std::endl;

            if (deltaR < 0.1 && deltaT < 0.1)
            {
                break;
            }
        }
    }
    else
    {
        std::cerr << "Not enough features." << std::endl;
    }

    rotation = rt;
    translation = tr;
}

} // compute
} // olp

#endif //OLP_LOAMSLAMCALCULATOR_HPP


#include <iostream>
#include <vector>

//#define NUMBER_DOUBLE 1
#define NUMBER_DOUBLE_DOUBLE 1

#include "Nbodies/summation.h"
#include "Integration/methods.h"
#include "Writer/writer.h"
#include "Nbodies/energy.h"

#include <boost/multiprecision/float128.hpp>

#ifdef NUMBER_DOUBLE_DOUBLE
#include <qd/dd_real.h>
#include <qd/fpu.h>
#endif

#include <string>

using namespace std;
using namespace boost::multiprecision;

#include <chrono>
int main() {

#ifdef NUMBER_DOUBLE_DOUBLE
    unsigned int oldcw;
    fpu_fix_start(&oldcw);
#endif

    using current_type = dd_real;

    std::vector<Body<current_type>> bodies;

    //make_universe(bodies, 40, current_type(0.0), current_type(0.0), current_type(0.0));
/*
    bodies.push_back(Body<current_type>({current_type(0),current_type(0),current_type(0)},{current_type(0),current_type(0),current_type(0)},current_type(2e14)));
    bodies.push_back(Body<current_type>({ current_type(0), current_type(1.4e3), current_type(0) },{ current_type(3),current_type(0),current_type(0) },current_type(6)));
    bodies.push_back(Body<current_type>({ current_type(0), current_type(1.3e3), current_type(0) },{ current_type(3),current_type(0),current_type(0) },current_type(6)));
    bodies.push_back(Body<current_type>({ current_type(0), current_type(1.2e3), current_type(0) },{ current_type(3),current_type(0),current_type(0) },current_type(6)));
    bodies.push_back(Body<current_type>({ current_type(0), current_type(1.1e3), current_type(0) },{ current_type(3),current_type(0),current_type(0) },current_type(6)));
    bodies.push_back(Body<current_type>({ current_type(0), current_type(1.56e3), current_type(0) },{ current_type(3),current_type(0),current_type(0) },current_type(6)));
    bodies.push_back(Body<current_type>({ current_type(0), current_type(1.5e3), current_type(0) },{ current_type(3),current_type(0),current_type(0) },current_type(6)));
    bodies.push_back(Body<current_type>({ current_type(0), current_type(1.51e3), current_type(0) },{ current_type(3),current_type(0),current_type(0) },current_type(6)));
*/

    current_type a("0");
    current_type b("3");
    current_type c("6");
    current_type mm("2e14");
    current_type diss("1.3e3");

    bodies.push_back(Body<current_type>({a,a,a},{a,a,a},mm));
    bodies.push_back(Body<current_type>({ current_type("0"), current_type("1.3e3"), current_type("0") },{ current_type("3"),current_type("0"),current_type("0") },current_type("6")));
    bodies.push_back(Body<current_type>({ current_type("0"), current_type("1.2e3"), current_type("0") },{ current_type("3"),current_type("0"),current_type("0") },current_type("6")));
    bodies.push_back(Body<current_type>({ current_type("0"), current_type("1.1e3"), current_type("0") },{ current_type("3"),current_type("0"),current_type("0") },current_type("6")));
    bodies.push_back(Body<current_type>({ current_type("0"), current_type("1.56e3"), current_type("0") },{ current_type("3"),current_type("0"),current_type("0") },current_type("6")));
    bodies.push_back(Body<current_type>({ current_type("0"), current_type("1.5e3"), current_type("0") },{ current_type("3"),current_type("0"),current_type("0") },current_type("6")));
    bodies.push_back(Body<current_type>({ current_type("0"), current_type("1.51e3"), current_type("0") },{ current_type("3"),current_type("0"),current_type("0") },current_type("6")));
    bodies.push_back(Body<current_type>({ current_type("0"), current_type("1.4e3"), current_type("0") },{ current_type("3"),current_type("0"),current_type("0") },current_type("6")));

    current_type init_energy = summation<current_type, kinetic_energy_proxy<current_type>>(kinetic_energy_proxy(bodies), bodies.size()) / current_type(2) +
                         summation<current_type, potential_energy_proxy<current_type>>(potential_energy_proxy(bodies), bodies.size() * bodies.size()) / current_type(2) ;

    vec<current_type> init_impulse_moment = summation<vec<current_type>, impulse_moment_proxy<vec<current_type>,current_type>>(impulse_moment_proxy<vec<current_type>,current_type>(bodies), bodies.size());

    current_type total_mass = summation<current_type, total_mass_proxy<current_type>>(total_mass_proxy<current_type>(bodies), bodies.size());
    vec<current_type> init_center_mass = summation<vec<current_type>, mass_center_proxy<vec<current_type>, current_type>>(mass_center_proxy<vec<current_type>, current_type>(bodies), bodies.size());
    init_center_mass = init_center_mass / total_mass;
    vec<current_type> init_vel_mass = summation<vec<current_type>, mass_vel_proxy<vec<current_type>, current_type>>(mass_vel_proxy<vec<current_type>, current_type>(bodies), bodies.size());
    init_vel_mass = init_vel_mass / total_mass;

    for( int i = 0; i < bodies.size(); i++){
       // bodies[i].r -= init_center_mass;
       // bodies[i].v -= init_vel_mass;
    }

    json data_energy, data_impulse_moment, data_center, data_bodies;
    current_type h("0.1");
    int iterations = 100000;

    std::vector<current_type> coefs = initDDCoef<current_type>();
    auto start = std::chrono::high_resolution_clock::now();

    for (int i = 0; i < iterations; i++) {
         dormanPrince8(bodies, h, coefs);
         //RungeKutta4(bodies, h);
        if(true ) {
            for(int j = 0; j < bodies.size(); j++){

#ifdef NUMBER_DOUBLE_DOUBLE
                    data_bodies[j]["X"][i] = bodies[j].r.X._hi();
                    data_bodies[j]["Y"][i] = bodies[j].r.Y._hi();
                    data_bodies[j]["Z"][i] = bodies[j].r.Z._hi();
#endif
#ifdef NUMBER_DOUBLE

              data_bodies[j]["X"][i] = bodies[j].r.X;
              data_bodies[j]["Y"][i] = bodies[j].r.Y;
              data_bodies[j]["Z"][i] = bodies[j].r.Z;
                    /*
                    data_bodies[j]["X"][i] = bodies[j].r.X.str(32);
                    data_bodies[j]["Y"][i] = bodies[j].r.Y.str(32);
                    data_bodies[j]["Z"][i] = bodies[j].r.Z.str(32);
                     */
#endif
            }

            vec<current_type> center_mass = summation<vec<current_type>, mass_center_proxy<vec<current_type>, current_type>>(mass_center_proxy<vec<current_type>, current_type>(bodies), bodies.size());
            center_mass = center_mass / total_mass;

            current_type energy = summation<current_type, kinetic_energy_proxy<current_type>>(kinetic_energy_proxy(bodies), bodies.size()) / current_type(2) +
                            summation<current_type, potential_energy_proxy<current_type>>(potential_energy_proxy(bodies),bodies.size() * bodies.size()) / current_type(2);

            vec<current_type> impulse_moment = summation<vec<current_type>, impulse_moment_proxy<vec<current_type>,current_type>>(impulse_moment_proxy<vec<current_type>,current_type>(bodies), bodies.size());

            data_energy["n"].push_back(i);
#ifdef NUMBER_DOUBLE_DOUBLE
            data_energy["energy"].push_back((abs((energy - init_energy)/init_energy)).to_string());
#endif
#ifdef NUMBER_DOUBLE
            //data_energy["energy"].push_back(abs((energy - init_energy)/init_energy).str(32));
          data_energy["energy"].push_back(abs((energy - init_energy)/init_energy));
#endif

            data_impulse_moment["n"].push_back(i);
#ifdef NUMBER_DOUBLE_DOUBLE
            data_impulse_moment["moment"].push_back(abs((impulse_moment - init_impulse_moment).Len() / init_impulse_moment.Len()).to_string());
#endif
#ifdef NUMBER_DOUBLE
            //data_impulse_moment["moment"].push_back(abs((impulse_moment - init_impulse_moment).Len() / init_impulse_moment.Len()).str(32));
          data_impulse_moment["moment"].push_back(abs((impulse_moment - init_impulse_moment).Len() / init_impulse_moment.Len()));
#endif

                data_center["n"].push_back(i);
#ifdef NUMBER_DOUBLE_DOUBLE
                data_center["center"].push_back(abs((init_center_mass - center_mass).Len()).to_string());
#endif
#ifdef NUMBER_DOUBLE
                //data_center["center"].push_back(abs((init_center_mass - center_mass).Len()).str(32));
                data_center["center"].push_back(abs((init_center_mass - center_mass).Len()));
#endif

        }
    }
    auto stop = std::chrono::high_resolution_clock::now();
    auto duration = duration_cast<std::chrono::microseconds>(stop - start);
    printf("time: %lld microseconds \n", duration.count());

    writer<current_type> w;
    w.writeRes("../log.json", data_energy);
    w.writeRes("../bodies.json", data_bodies);
    w.writeRes("../moment.json", data_impulse_moment);
    w.writeRes("../center.json", data_center);

#ifdef NUMBER_DOUBLE_DOUBLE
    fpu_fix_end(&oldcw);
#endif
    return 0;
}


/**
 * Copyright (C) 2016, Wu Tao. All rights reserved.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#include <string>
#include <boost/functional/hash.hpp>
#include "FilterStrategy.h"
#include "Slice.h"

namespace lessdb {

class BloomFilterStrategy : public FilterStrategy {
  //
  // m: the total number of bits our bloom filter has.
  // n: the number of elements.
  // k: the number of hash functions.
  // For a given m and n, the value of k that minimizes the false positive
  // probability is ln2*(m/n).
  //
  // Use double-hashing to generate a sequence of hash values.
  // See analysis in [Kirsch,Mitzenmacher 2006].
  // The double-hashing functions is of the form gi(x) = h1(x) + i*h2(x).
  //

  const size_t kMinBloomFilterLength = 64;

 public:
  BloomFilterStrategy(size_t bits_per_key) : bits_per_byte_(bits_per_key) {
    k_ = static_cast<size_t>(bits_per_key * 0.69);  // ln2 ~= 0.69
  }

  inline void Put(const Slice &key, Slice &bits) override {
    size_t h1 = boost::hash_range(key.RawData(), key.RawData() + key.Len());
    size_t h2 = (h1 >> 17) | (h1 << 15);  // Rotate right 17 bits
    for (size_t i = 0; i < k_; ++i) {
      size_t g = (h1 + i * h2) % bits.Len();
      bits[g / 8] |= (1 << (g % 8));
    }
  }

  inline bool MightContain(const Slice &key, const Slice &bits) const override {
    size_t h1 = boost::hash_range(key.RawData(), key.RawData() + key.Len());
    size_t h2 = (h1 >> 17) | (h1 << 15);  // Rotate right 17 bits
    for (size_t i = 0; i < k_; ++i) {
      size_t g = (h1 + i * h2) % bits.Len();
      if (!(bits[g / 8] & (1 << (g % 8))))
        return false;
    }
    return true;
  }

 private:
  size_t bits_per_byte_;
  size_t k_;
};

FilterStrategy *FilterStrategy::Default(size_t bits_per_bytes) {
  return new BloomFilterStrategy(bits_per_bytes);
}

}  // namespace lessdb


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2009-2012 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#define BOOST_GEOMETRY_NO_BOOST_TEST

#include <test_overlay_p_q.hpp>

#include <boost/program_options.hpp>
#include <boost/timer.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>

struct star_params
{
    int count; // points of ellipse, not of star
    double factor_1;
    double factor_2;
    double center_x;
    double center_y;
    double rotation;
    star_params(int c, double f1, double f2, double x, double y, double r = 0)
        : count(c)
        , factor_1(f1)
        , factor_2(f2)
        , center_x(x)
        , center_y(y)
        , rotation(r)
    {}
};

template <typename Polygon>
inline void make_star(Polygon& polygon, star_params const& p)
{
    typedef typename bg::point_type<Polygon>::type P;
    typedef typename bg::select_most_precise
        <
            typename bg::coordinate_type<Polygon>::type,
            long double
        >::type coordinate_type;

    // Create star
    coordinate_type cx = 25.0;
    coordinate_type cy = 25.0;

    coordinate_type dx = 50.0;
    coordinate_type dy = 50.0;

    coordinate_type half = 0.5;
    coordinate_type two = 2.0;

    coordinate_type a1 = coordinate_type(p.factor_1) * half * dx;
    coordinate_type b1 = coordinate_type(p.factor_1) * half * dy;
    coordinate_type a2 = coordinate_type(p.factor_2) * half * dx;
    coordinate_type b2 = coordinate_type(p.factor_2) * half * dy;

    coordinate_type pi = boost::math::constants::pi<long double>();
    coordinate_type delta = pi * two / coordinate_type(p.count - 1);
    coordinate_type angle = coordinate_type(p.rotation) * delta;
    for (int i = 0; i < p.count - 1; i++, angle += delta)
    {
        bool even = i % 2 == 0;
        coordinate_type s = sin(angle);
        coordinate_type c = cos(angle);
        coordinate_type x = p.center_x + cx + (even ? a1 : a2) * s;
        coordinate_type y = p.center_y + cy + (even ? b1 : b2) * c;
        bg::exterior_ring(polygon).push_back(bg::make<P>(x, y));

    }
    bg::exterior_ring(polygon).push_back(bg::exterior_ring(polygon).front());
    bg::correct(polygon);
}

template <typename T, bool Clockwise, bool Closed>
void test_star_ellipse(int seed, int index, star_params const& par_p,
            star_params const& par_q, p_q_settings const& settings)
{
    typedef bg::model::d2::point_xy<T> point_type;
    typedef bg::model::polygon<point_type, Clockwise, Closed> polygon;

    polygon p, q;
    make_star(p, par_p);
    make_star(q, par_q);

    std::ostringstream out;
    out << "rse_" << seed << "_" << index;
    test_overlay_p_q<polygon, T>(out.str(), p, q, settings);
}

template <typename T, bool Clockwise, bool Closed>
void test_type(int seed, int count, p_q_settings const& settings)
{
    boost::timer t;

    typedef boost::minstd_rand base_generator_type;

    //boost::uniform_real<> random_factor(0.5, 1.2);
    //boost::uniform_real<> random_location(-10.0, 10.0);
    //boost::uniform_int<> random_points(5, 20);

    // This set (next 4 lines) are now solved for the most part
    // 2009-12-03, 3 or 4 errors in 1000000 calls
    // 2009-12-07,     no errors in 1000000 calls
    //boost::uniform_real<> random_factor(1.0 - 1e-3, 1.0 + 1e-3);
    //boost::uniform_real<> random_location(-1e-3, 1e-3);
    //boost::uniform_real<> random_rotation(-1e-3, 1e-3);
    //boost::uniform_int<> random_points(3, 3);

    // 2009-12-08, still errors, see notes
    // 2009-12-09, (probably) solved by order on side
    // 2010-01-16: solved (no errors in 1000000 calls)
    //boost::uniform_real<> random_factor(1.0 - 1e-3, 1.0 + 1e-3);
    //boost::uniform_real<> random_location(-1e-3, -1e-3);
    //boost::uniform_real<> random_rotation(-1e-3, 1e-3);
    //boost::uniform_int<> random_points(3, 4);

    // This set (next 4 lines) are now solved ("distance-zero"/"merge iiii" problem)
    // 2009-12-03: 5,50 -> 2:1 000 000 wrong (2009-12-03)
    // 2010-01-16: solved (no errors in 10000000 calls)
    boost::uniform_real<> random_factor(0.3, 1.2);
    boost::uniform_real<> random_location(-20.0, +20.0); // -25.0, +25.0
    boost::uniform_real<> random_rotation(0, 0.5);
    boost::uniform_int<> random_points(5, 15);

    base_generator_type generator(seed);

    boost::variate_generator<base_generator_type&, boost::uniform_real<> >
        factor_generator(generator, random_factor);

    boost::variate_generator<base_generator_type&, boost::uniform_real<> >
        location_generator(generator, random_location);

    boost::variate_generator<base_generator_type&, boost::uniform_real<> >
        rotation_generator(generator, random_rotation);

    boost::variate_generator<base_generator_type&, boost::uniform_int<> >
        int_generator(generator, random_points);

    for(int i = 0; i < count; i++)
    {
        test_star_ellipse<T, Clockwise, Closed>(seed, i + 1,
            star_params(int_generator() * 2 + 1,
                    factor_generator(), factor_generator(),
                    location_generator(), location_generator(), rotation_generator()),
            star_params(int_generator() * 2 + 1,
                    factor_generator(), factor_generator(),
                    location_generator(), location_generator(), rotation_generator()),
            settings);
    }
    std::cout
        << "type: " << string_from_type<T>::name()
        << " time: " << t.elapsed()  << std::endl;
}

template <bool Clockwise, bool Closed>
void test_all(std::string const& type, int seed, int count, p_q_settings settings)
{
    if (type == "float")
    {
        test_type<float, Clockwise, Closed>(seed, count, settings);
    }
    else if (type == "double")
    {
        test_type<double, Clockwise, Closed>(seed, count, settings);
    }
#if defined(HAVE_TTMATH)
    else if (type == "ttmath")
    {
        test_type<ttmath_big, Clockwise, Closed>(seed, count, settings);
    }
#endif
}

int main(int argc, char** argv)
{
    BoostGeometryWriteTestConfiguration();
    try
    {
        namespace po = boost::program_options;
        po::options_description description("=== random_ellipses_stars ===\nAllowed options");

        int count = 1;
        int seed = static_cast<unsigned int>(std::time(0));
        std::string type = "float";
        bool ccw = false;
        bool open = false;
        p_q_settings settings;

        description.add_options()
            ("help", "Help message")
            ("seed", po::value<int>(&seed), "Initialization seed for random generator")
            ("count", po::value<int>(&count)->default_value(1), "Number of tests")
            ("diff", po::value<bool>(&settings.also_difference)->default_value(false), "Include testing on difference")
            ("ccw", po::value<bool>(&ccw)->default_value(false), "Counter clockwise polygons")
            ("open", po::value<bool>(&open)->default_value(false), "Open polygons")
            ("type", po::value<std::string>(&type)->default_value("float"), "Type (float,double)")
            ("wkt", po::value<bool>(&settings.wkt)->default_value(false), "Create a WKT of the inputs, for all tests")
            ("svg", po::value<bool>(&settings.svg)->default_value(false), "Create a SVG for all tests")
        ;

        po::variables_map varmap;
        po::store(po::parse_command_line(argc, argv, description), varmap);
        po::notify(varmap);

        if (varmap.count("help"))
        {
            std::cout << description << std::endl;
            return 1;
        }

        if (ccw && open)
        {
            test_all<false, false>(type, seed, count, settings);
        }
        else if (ccw)
        {
            test_all<false, true>(type, seed, count, settings);
        }
        else if (open)
        {
            test_all<true, false>(type, seed, count, settings);
        }
        else
        {
            test_all<true, true>(type, seed, count, settings);
        }
    }
    catch(std::exception const& e)
    {
        std::cout << "Exception " << e.what() << std::endl;
    }
    catch(...)
    {
        std::cout << "Other exception" << std::endl;
    }

    return 0;
}