/* tz_3dgeom_utils.c
 *
 * 17-Jan-2008 Initial write: Ting Zhao
 */
#include <stdio.h>
#include <math.h>
#include "tz_geo3d_utils.h"
#ifdef HAVE_LIBGSL
#  if defined(HAVE_INLINE)
#    undef HAVE_INLINE
#    define INLINE_SUPPRESSED
#  endif
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_eigen.h>
#  if defined(INLINE_SUPPRESSED)
#    define HAVE_INLINE
#  endif
#endif
#include "tz_error.h"
#include "tz_utilities.h"
#include "tz_constant.h"
#include "tz_geoangle_utils.h"
#include "tz_geometry.h"
#include "tz_3dgeom.h"
#include "tz_darray.h"
#include "tz_math.h"

#define TZ_DIST_3D_EPS 0.0000000000001 /* rounding torelance */

/* Geo3d_Translate_Coordinate(): 3D coordinate translation.
 *
 * Args: x - x coordinate to translate. It stores the result after return;
 *       y - y coordinate to translate. It stores the result after return;
 *       z - z coordinate to translate. It stores the result after return;
 *       dx - x axis translation;
 *       dy - y axis translation;
 *       dz - z axis translation.
 *
 * Return: void.
 */
void Geo3d_Translate_Coordinate(double *x, double *y, double *z,
				double dx, double dy, double dz)
{
  *x += dx;
  *y += dy;
  *z += dz;
}

/* Geo3d_Coordinate_Offset(): 3D coordinates offset.
 *
 * Args: x1, y1, z1 - start coordinate;
 *       x2, y2, z2 - end coordinate;
 *       dx, dy, dz - store the result.
 *
 * Return: void.
 */
void Geo3d_Coordinate_Offset(double x1, double y1, double z1,
			     double x2, double y2, double z2,
			     double *dx, double *dy, double *dz)
{
  *dx = x2 - x1;
  *dy = y2 - y1;
  *dz = z2 - z1;
}

/*
 * Geo3d_Rotate_Coordinate(): 3D  rotation.
 *
 * Args: x, y, z - the point to be rotated;
 *       theta - angle around the X axis;
 *       psi - angle around the Z axis;
 *       reverse - reverse the rotation or not.
 *
 * Return: void.
 */
void Geo3d_Rotate_Coordinate(double *x, double *y, double *z,
			     double theta, double psi, BOOL reverse)
{
  double p[3] = {*x, *y, *z};
  Rotate_XZ(p, p, 1, theta, psi, reverse);
  *x = p[0];
  *y = p[1];
  *z = p[2];
}

/* Geo3d_Orientation_Normal(): calculate the normal vector at a certain
 *                             direction.
 *
 * Args: theta - radian around X axis;
 *       psi - radian around Z axis;
 *       x - resulted x coordinate;
 *       y - resulted y coordinate;
 *       z - resulted z coordinate.
 *
 * Return: void.
 */
void Geo3d_Orientation_Normal(double theta, double psi,
			      double *x, double *y, double *z)
{
  /* rotaion of the vector (0, 0, 1) */
  double sin_theta = sin(theta);
  *x = sin_theta * sin(psi);
  *y = -sin_theta * cos(psi);
  //*z = cos(theta);
  *z = sqrt(1.0 - sin_theta * sin_theta);
}

void Geo3d_Normal_Orientation(double x, double y, double z,
			      double *theta, double *psi)
{
  /*
  *theta = acos(z);
  *psi = Vector_Angle(x, y) + TZ_PI_2;
  */
  //*psi = Vector_Angle(x, y) + TZ_PI_2 * 3.0;

  /* The range of theta will be [-pi, pi] and
   * the range of psi will be [-pi/2, pi/2]. */
  *theta = acos(z);

  if (*theta < GEOANGLE_COMPARE_EPS) {
    *psi = 0.0;
  } else {
    if (y >= 0.0) {
      *theta = -*theta;
      *psi = Vector_Angle(x, y) - TZ_PI_2;
    } else {
      *psi = Vector_Angle(x, y) - TZ_PI_2 * 3.0;
    }
  }
}

void Geo3d_Coord_Orientation(double x, double y, double z,
			      double *theta, double *psi)
{
  double r = Geo3d_Orgdist(x, y, z);
  if (r > TZ_DIST_3D_EPS) {
    x /= r;
    y /= r;
    z /= r;
    Geo3d_Normal_Orientation(x, y, z, theta, psi);
  } else {
    *theta = 0.0;
    *psi = 0.0;
  }
}

void Geo3d_Rotate_Orientation(double rtheta, double rpsi,
			      double *theta, double *psi)
{
  double coord[3];
  Geo3d_Orientation_Normal(*theta, *psi, coord, coord + 1, coord + 2);
  Rotate_XZ(coord, coord, 1, rtheta, rpsi, 0);
  Geo3d_Normal_Orientation(coord[0], coord[1], coord[2], theta, psi);
}

double Geo3d_Dot_Product(double x1, double y1, double z1,
			 double x2, double y2, double z2)
{
  return x1 * x2 + y1 * y2 + z1 * z2;
}

void Geo3d_Cross_Product(double x1, double y1, double z1,
			 double x2, double y2, double z2,
			 double *x, double *y, double *z)
{
  *x = y1 * z2 - y2 * z1;
  *y = x2 * z1 - x1 * z2;
  *z = x1 * y2 - x2 * y1;
}

double Geo3d_Orgdist_Sqr(double x, double y, double z)
{
  return x * x + y * y + z * z;
}

double Geo3d_Orgdist(double x, double y, double z)
{
  return sqrt(x * x + y * y + z * z);
}

double Geo3d_Dist_Sqr(double x1, double y1, double z1,
		      double x2, double y2, double z2)
{
  double dx = x1 - x2;
  double dy = y1 - y2;
  double dz = z1 - z2;

  return dx * dx + dy * dy + dz * dz;
}

double Geo3d_Dist(double x1, double y1, double z1,
		  double x2, double y2, double z2)
{
  double dx = x1 - x2;
  double dy = y1 - y2;
  double dz = z1 - z2;

  return sqrt(dx * dx + dy * dy + dz * dz);
}

double Geo3d_Angle2(double x1, double y1, double z1,
		    double x2, double y2, double z2)
{
  double d1 = Geo3d_Orgdist_Sqr(x1, y1, z1);
  if (d1 == 0.0) {
    return 0.0;
  }

  double d2 = Geo3d_Orgdist_Sqr(x2, y2, z2);
  if (d2 == 0.0) {
    return 0.0;
  }

  double d12 = Geo3d_Dot_Product(x1, y1, z1, x2, y2, z2) / sqrt(d1 * d2);

  /* to avoid invalid value caused by rounding error */
  d12 = (fabs(d12) > 1.0) ? round(d12) : d12;

  return acos(d12);
}

void Geo3d_Lineseg_Break(const double *line_start, const double *line_end,
			 double lambda, double *point)
{
  int i;
  for (i = 0; i < 3; i++) {
    point[i] = (1 - lambda) * line_start[i] + lambda * line_end[i];
  }
}

#define GEO3D_LINESEG_DIST2_EPS TZ_DIST_3D_EPS

/*
 * reference: http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline3d/
 */
static int geo3d_line_line_dist(const double *line1_start,
				const double *line1_end,
				const double *line2_start,
				const double *line2_end,
				double *dc1, double *dc2,
				double *intersect1, double *intersect2,
				double eps)
{
  double ds[3];  /*13*/
  double de[3];  /*24*/

  int i;
  for (i = 0; i < 3; i++) {
    dc1[i] = line1_end[i] - line1_start[i]; /*21*/
    dc2[i] = line2_end[i] - line2_start[i]; /*43*/
    ds[i] = line1_start[i] - line2_start[i];
    de[i] = line1_end[i] - line2_end[i];
  }

  double d2121 = dc1[0] * dc1[0] + dc1[1] * dc1[1] + dc1[2] * dc1[2];
  double d4343 = dc2[0] * dc2[0] + dc2[1] * dc2[1] + dc2[2] * dc2[2];

  if (d2121 < eps) {
    if (d4343 < eps) {
      return -3;
    }

    return -1;
  }

  if (d4343 < eps) {
    return -2;
  }

  double d4321 = dc2[0] * dc1[0] + dc2[1] * dc1[1] + dc2[2] * dc1[2];

  double denom = d2121 * d4343 - d4321 * d4321;

  double d1343 = ds[0] * dc2[0] + ds[1] * dc2[1] + ds[2] * dc2[2];
  double d1321 = ds[0] * dc1[0] + ds[1] * dc1[1] + ds[2] * dc1[2];

  double numer = d1343 * d4321 - d1321 * d4343;

  /* parallel */
  if (denom <= eps) {
    return 0;
  }

  *intersect1 = numer / denom;
  *intersect2 = (d1343 + d4321 * (*intersect1)) / d4343;

  return 1;
}

static int geo3d_point_line_dist(const double *point,
				 const double *line_start,
				 const double *line_end,
				 double *lamda, double eps)
{
  double line_vec[3];
  double point_vec[3];
  int i;
  for (i = 0; i < 3; i++) {
    line_vec[i] = line_end[i] - line_start[i];
    point_vec[i] = point[i] - line_start[i];
  }

  double length_sqr = Geo3d_Orgdist_Sqr(line_vec[0], line_vec[1], line_vec[2]);

  if (length_sqr < eps) {
    return 0;
  }

  /* projection from point to line */
  double dot = point_vec[0] * line_vec[0] + point_vec[1] * line_vec[1] +
    point_vec[2] * line_vec[2];

  *lamda = dot / length_sqr;

  return 1;
}

double Geo3d_Line_Line_Dist(double line1_start[], double line1_end[],
			    double line2_start[], double line2_end[])
{
  double intersect1, intersect2;
  double dc1[3], dc2[3];

  double lamda;

  switch (geo3d_line_line_dist(line1_start, line1_end, line2_start, line2_end,
			       dc1, dc2, &intersect1, &intersect2,
			       TZ_DIST_3D_EPS)) {
  case -3: /* point to point */
    return Geo3d_Dist(line1_start[0], line1_start[1], line1_start[2],
		      line2_start[0], line2_start[1], line2_start[2]);
  case -1: /* point to line2 */
    geo3d_point_line_dist(line1_start, line2_start, line2_end, &lamda,
			  TZ_DIST_3D_EPS);
    return Geo3d_Dist(line1_start[0], line1_start[1], line1_start[2],
		      line2_start[0] + lamda * dc2[0],
		      line2_start[1] + lamda * dc2[1],
		      line2_start[2] + lamda * dc2[2]);
  case -2: /* point to line 1 */
    geo3d_point_line_dist(line2_start, line1_start, line1_end, &lamda,
			  TZ_DIST_3D_EPS);
    return Geo3d_Dist(line2_start[0], line2_start[1], line2_start[2],
		      line1_start[0] + lamda * dc1[0],
		      line1_start[1] + lamda * dc1[1],
		      line1_start[2] + lamda * dc1[2]);
  case 0: /* parallel */
    geo3d_point_line_dist(line1_start, line2_start, line2_end, &lamda,
			  TZ_DIST_3D_EPS);
    return Geo3d_Dist(line1_start[0], line1_start[1], line1_start[2],
		      line2_start[0] + lamda * dc2[0],
		      line2_start[1] + lamda * dc2[1],
		      line2_start[2] + lamda * dc2[2]);
  default: /* line to line */
    return sqrt(Geo3d_Dist_Sqr(line1_start[0] + intersect1 * dc1[0],
			       line1_start[1] + intersect1 * dc1[1],
			       line1_start[2] + intersect1 * dc1[2],
			       line2_start[0] + intersect2 * dc2[0],
			       line2_start[1] + intersect2 * dc2[1],
			       line2_start[2] + intersect2 * dc2[2]));
  }
}

double Geo3d_Lineseg_Lineseg_Dist(double line1_start[], double line1_end[],
				  double line2_start[], double line2_end[],
				  double *intersect1, double *intersect2,
				  int *cond)
{
  double dc1[3]; /*21*/
  double dc2[3]; /*43*/

  double d1, d2;

  switch (geo3d_line_line_dist(line1_start, line1_end, line2_start, line2_end,
			       dc1, dc2, intersect1, intersect2,
			       TZ_DIST_3D_EPS)) {
  case 0: /* parallel */
    *cond = 9;
    d1 = Geo3d_Point_Lineseg_Dist(line1_end, line2_start, line2_end,
				  intersect1);
    d2 = Geo3d_Point_Lineseg_Dist(line1_start, line2_start, line2_end,
				  intersect2);
    if (d1 <= d2) {
      *intersect2 = *intersect1;
      *intersect1 = 1.0;
      return d1;
    } else {
      *intersect1 = 0.0;
      return d2;
    }
  case -1:
    *intersect1 = 0.0;
    *cond = 10;
    return Geo3d_Point_Lineseg_Dist(line1_start, line2_start, line2_end,
				    intersect2);
  case -2:
    *intersect2= 0.0;
    *cond = 10;
    return Geo3d_Point_Lineseg_Dist(line2_start, line1_start, line1_end,
				    intersect1);
  case -3:
    *cond = 10;
    *intersect1 = 0.0;
    *intersect2 = 0.0;
    return Geo3d_Dist(line1_start[0], line1_start[1], line1_start[2],
		      line2_start[0], line2_start[1], line2_start[2]);
  default:
#if defined BREAK_IS_IN_RANGE
#  undef BREAK_IS_IN_RANGE
#endif
#define BREAK_IS_IN_RANGE(mu) ((mu >= 0.0) && (mu <= 1.0))

    if (BREAK_IS_IN_RANGE(*intersect1) && BREAK_IS_IN_RANGE(*intersect2)) {
      *cond = 0;
      return sqrt(Geo3d_Dist_Sqr(line1_start[0] + *intersect1 * dc1[0],
				 line1_start[1] + *intersect1 * dc1[1],
				 line1_start[2] + *intersect1 * dc1[2],
				 line2_start[0] + *intersect2 * dc2[0],
				 line2_start[1] + *intersect2 * dc2[1],
				 line2_start[2] + *intersect2 * dc2[2]));
    } else if ((*intersect1 < 0.0) && BREAK_IS_IN_RANGE(*intersect2)) {
      *cond = 1;
      *intersect1 = 0.0;
      return Geo3d_Point_Lineseg_Dist(line1_start, line2_start, line2_end,
				      intersect2);
    } else if ((*intersect1 > 0.0) && BREAK_IS_IN_RANGE(*intersect2)) {
      *cond = 2;
      *intersect1 = 1.0;
      return Geo3d_Point_Lineseg_Dist(line1_end, line2_start, line2_end,
				      intersect2);
    } else if ((*intersect2 < 0.0) && BREAK_IS_IN_RANGE(*intersect1)) {
      *cond = 3;
      *intersect2 = 0.0;
      return Geo3d_Point_Lineseg_Dist(line2_start, line1_start, line1_end,
				      intersect1);
    } else if ((*intersect2 > 1.0) && BREAK_IS_IN_RANGE(*intersect1)) {
      *cond = 4;
      *intersect2 = 1.0;
      return Geo3d_Point_Lineseg_Dist(line2_end, line1_start, line1_end,
				      intersect1);
    } else if ((*intersect1 < 0.0) && (*intersect2 < 0.0)) {
      *cond = 5;
      d1 = Geo3d_Point_Lineseg_Dist(line1_start, line2_start, line2_end,
				    intersect2);
      d2 = Geo3d_Point_Lineseg_Dist(line2_start, line1_start, line1_end,
				    intersect1);
      if (d1 <= d2) {
	*intersect1 = 0.0;
	return d1;
      } else {
	*intersect2 = 0.0;
	return d2;
      }
    } else if ((*intersect1 > 1.0) && (*intersect2 < 0.0)) {
      *cond = 6;
      d1 = Geo3d_Point_Lineseg_Dist(line1_end, line2_start, line2_end,
				    intersect2);
      d2 = Geo3d_Point_Lineseg_Dist(line2_start, line1_start, line1_end,
				    intersect1);
      if (d1 <= d2) {
	*intersect1 = 1.0;
	return d1;
      } else {
	*intersect2 = 0.0;
	return d2;
      }
    } else if ((*intersect1 < 0.0) && (*intersect2 > 1.0)) {
      *cond = 7;
      d1 = Geo3d_Point_Lineseg_Dist(line1_start, line2_start, line2_end,
				    intersect2);
      d2 = Geo3d_Point_Lineseg_Dist(line2_end, line1_start, line1_end,
				    intersect1);
      if (d1 <= d2) {
	*intersect1 = 0.0;
	return d1;
      } else {
	*intersect2 = 1.0;
	return d2;
      }
    } else if ((*intersect1 > 1.0) && (*intersect2 > 1.0)) {
      *cond = 8;
      d1 = Geo3d_Point_Lineseg_Dist(line1_end, line2_start, line2_end,
				    intersect2);
      d2 = Geo3d_Point_Lineseg_Dist(line2_end, line1_start, line1_end,
				    intersect1);
      if (d1<= d2) {
	*intersect1 = 1.0;
	return d1;
      } else {
	*intersect2 = 1.0;
	return d2;
      }
    }
  }

  return -1.0;
}

double Geo3d_Lineseg_Dist2(double line1_start[], double line1_end[],
			   double line2_start[], double line2_end[],
			   double *intersect1, double *intersect2,
			   int *cond)
{
  double dc1[3]; /*21*/
  double dc2[3]; /*43*/
  double ds[3];  /*13*/
  double de[3];  /*24*/

  int i;
  for (i = 0; i < 3; i++) {
    dc1[i] = line1_end[i] - line1_start[i];
    dc2[i] = line2_end[i] - line2_start[i];
    ds[i] = line1_start[i] - line2_start[i];
    de[i] = line1_end[i] - line2_end[i];
  }

  double d2121 = dc1[0] * dc1[0] + dc1[1] * dc1[1] + dc1[2] * dc1[2];
  double d4343 = dc2[0] * dc2[0] + dc2[1] * dc2[1] + dc2[2] * dc2[2];

  ASSERT(d2121 > GEO3D_LINESEG_DIST2_EPS, "invalid length");
  ASSERT(d4343 > GEO3D_LINESEG_DIST2_EPS, "invalid length");

  double d4321 = dc2[0] * dc1[0] + dc2[1] * dc1[1] + dc2[2] * dc1[2];

  double denom = d2121 * d4343 - d4321 * d4321;

  double d1343 = ds[0] * dc2[0] + ds[1] * dc2[1] + ds[2] * dc2[2];
  double d1321 = ds[0] * dc1[0] + ds[1] * dc1[1] + ds[2] * dc1[2];

  double numer = d1343 * d4321 - d1321 * d4343;

  if (denom <= GEO3D_LINESEG_DIST2_EPS) {
    *cond = 9;
    return
      dmin2(Geo3d_Point_Lineseg_Dist(line1_end, line2_start, line2_end,
				     intersect1),
	    Geo3d_Point_Lineseg_Dist(line1_start, line2_start, line2_end,
				     intersect2)
	    );
  }

  *intersect1 = numer / denom;
  *intersect2 = (d1343 + d4321 * (*intersect1)) / d4343;

#if defined BREAK_IS_IN_RANGE
#  undef BREAK_IS_IN_RANGE
#endif
#define BREAK_IS_IN_RANGE(mu) ((mu >= 0.0) && (mu <= 1.0))

  if (BREAK_IS_IN_RANGE(*intersect1) && BREAK_IS_IN_RANGE(*intersect2)) {
    *cond = 0;
    return sqrt(Geo3d_Dist_Sqr(line1_start[0] + *intersect1 * dc1[0],
			       line1_start[1] + *intersect1 * dc1[1],
			       line1_start[2] + *intersect1 * dc1[2],
			       line2_start[0] + *intersect2 * dc2[0],
			       line2_start[1] + *intersect2 * dc2[1],
			       line2_start[2] + *intersect2 * dc2[2]));
  } else if ((*intersect1 < 0.0) && BREAK_IS_IN_RANGE(*intersect2)) {
    *cond = 1;
    return Geo3d_Point_Lineseg_Dist(line1_start, line2_start, line2_end,
				    intersect2);
  } else if ((*intersect1 > 0.0) && BREAK_IS_IN_RANGE(*intersect2)) {
    *cond = 2;
    return Geo3d_Point_Lineseg_Dist(line1_end, line2_start, line2_end,
				    intersect2);
  } else if ((*intersect2 < 0.0) && BREAK_IS_IN_RANGE(*intersect1)) {
    *cond = 3;
    return Geo3d_Point_Lineseg_Dist(line2_start, line1_start, line1_end,
				    intersect1);
  } else if ((*intersect2 > 1.0) && BREAK_IS_IN_RANGE(*intersect1)) {
    *cond = 4;
    return Geo3d_Point_Lineseg_Dist(line2_end, line1_start, line1_end,
				    intersect1);
  } else if ((*intersect1 < 0.0) && (*intersect2 < 0.0)) {
    *cond = 5;
    return
      dmin2(Geo3d_Point_Lineseg_Dist(line1_start, line2_start, line2_end,
				     intersect2),
	    Geo3d_Point_Lineseg_Dist(line2_start, line1_start, line1_end,
				     intersect1)
	    );
  } else if ((*intersect1 > 1.0) && (*intersect2 < 0.0)) {
    *cond = 6;
    return
      dmin2(Geo3d_Point_Lineseg_Dist(line1_end, line2_start, line2_end,
				     intersect2),
	    Geo3d_Point_Lineseg_Dist(line2_start, line1_start, line1_end,
				     intersect1)
	    );
  } else if ((*intersect1 < 0.0) && (*intersect2 > 1.0)) {
    *cond = 7;
    return
      dmin2(Geo3d_Point_Lineseg_Dist(line1_start, line2_start, line2_end,
				     intersect2),
	    Geo3d_Point_Lineseg_Dist(line2_end, line1_start, line1_end,
				     intersect1)
	    );
  } else if ((*intersect1 > 1.0) && (*intersect2 > 1.0)) {
    *cond = 8;
    return
      dmin2(Geo3d_Point_Lineseg_Dist(line1_end, line2_start, line2_end,
				     intersect2),
	    Geo3d_Point_Lineseg_Dist(line2_end, line1_start, line1_end,
				     intersect1)
	    );
  }

  return -1.0;
}

#define GEO3D_POINT_LINESEG_EPS 0.0000000000001
double Geo3d_Point_Lineseg_Dist(const double *point, const double *line_start,
				const double *line_end, double *lamda)
{
  double tmp_lamda;
  int status = geo3d_point_line_dist(point, line_start, line_end, &tmp_lamda,
				     GEO3D_POINT_LINESEG_EPS);

  double dist;
  if (status == 0) {
    dist = Geo3d_Dist(line_start[0], line_start[1], line_start[2],
		      point[0], point[1], point[2]);
  } else {
    if ((tmp_lamda >= 0.0) && (tmp_lamda <= 1.0)) {
      dist =
	Geo3d_Dist(point[0], point[1], point[2],
		   (1.0 - tmp_lamda) * line_start[0] + tmp_lamda * line_end[0],
		   (1.0 - tmp_lamda) * line_start[1] + tmp_lamda * line_end[1],
		   (1.0 - tmp_lamda) * line_start[2] + tmp_lamda * line_end[2]);
    } else if (tmp_lamda < 0.0) {
      tmp_lamda = 0.0;
      dist = sqrt(Geo3d_Dist_Sqr(line_start[0], line_start[1], line_start[2],
				 point[0], point[1], point[2]));
    } else {
      tmp_lamda = 1.0;
      dist = sqrt(Geo3d_Dist_Sqr(line_end[0], line_end[1], line_end[2],
				 point[0], point[1], point[2]));
    }
  }

  /*
  double line_vec[3];
  double point_vec[3];
  int i;
  for (i = 0; i < 3; i++) {
    line_vec[i] = line_end[i] - line_start[i];
    point_vec[i] = point[i] - line_start[i];
  }
  double length_sqr = Geo3d_Orgdist_Sqr(line_vec[0], line_vec[1], line_vec[2]);

  double tmp_lamda = 0.0;
  double dist = -1.0;

  if (length_sqr <= GEO3D_POINT_LINESEG_EPS) {
    dist = sqrt(Geo3d_Dist_Sqr(line_start[0], line_start[1], line_start[2],
			       point[0], point[1], point[2]));
  } else {
    double dot = point_vec[0] * line_vec[0] + point_vec[1] * line_vec[1] +
      point_vec[2] * line_vec[2];

    double point_vec_lensqr = Geo3d_Orgdist_Sqr(point_vec[0], point_vec[1],
					      point_vec[2]);

    if (point_vec_lensqr <= GEO3D_POINT_LINESEG_EPS) {
      dist = 0.0;
    } else {
      tmp_lamda = dot / length_sqr;
      if ((tmp_lamda >= 0.0) && (tmp_lamda <= 1.0)) {
	dist = sqrt(point_vec_lensqr - dot * dot / length_sqr);
      } else if (tmp_lamda < 0.0) {
	dist = sqrt(Geo3d_Dist_Sqr(line_start[0], line_start[1], line_start[2],
				   point[0], point[1], point[2]));
      } else {
	dist = sqrt(Geo3d_Dist_Sqr(line_end[0], line_end[1], line_end[2],
				   point[0], point[1], point[2]));
      }
    }
  }
  */
  if (lamda != NULL) {
    *lamda = tmp_lamda;
  }

  return dist;
}

void Geo3d_Cov_Orientation(double *cov, double *vec, double *ext)
{
#ifdef HAVE_LIBGSL
  gsl_matrix_view gmv = gsl_matrix_view_array(cov, 3, 3);

  gsl_vector *eval = gsl_vector_alloc(3);
  gsl_matrix *evec = gsl_matrix_alloc(3,3);
  gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(3);

  gsl_eigen_symmv(&(gmv.matrix), eval, evec, w);
  gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_VAL_DESC);

  if(ext != NULL) {
    darraycpy(ext, gsl_vector_const_ptr(eval, 0), 0, 3);
  }

  gsl_vector_view v = gsl_vector_view_array(vec, 3);
  gsl_matrix_get_col(&(v.vector), evec, 0);

  gsl_vector_free(eval);
  gsl_matrix_free(evec);
  gsl_eigen_symmv_free(w);
#else
  double eigv[3];
  Matrix_Eigen_Value_Cs(cov[0], cov[4], cov[8], cov[1], cov[2], cov[5], eigv);
  Matrix_Eigen_Vector_Cs(cov[0], cov[4], cov[8], cov[1], cov[2], cov[5],
      eigv[0], vec);
  if(ext != NULL) {
    darraycpy(ext, eigv, 0, 3);
  }
#endif
}


/*
 # This file is part of the Astrometry.net suite.
 # Licensed under a 3-clause BSD style license - see LICENSE
 */

#include <stdarg.h>

#include "compat.h"
#include "gslutils.h"

#include <gsl/gsl_matrix_double.h>
#include <gsl/gsl_vector_double.h>
#include <gsl/gsl_permutation.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_errno.h>

int gslutils_solve_leastsquares_v(gsl_matrix* A, int NB, ...) {
    int i, res;
    gsl_vector**  B = malloc(NB * sizeof(gsl_vector*));
    // Whoa, three-star programming!
    gsl_vector*** X = malloc(NB * sizeof(gsl_vector**));
    gsl_vector*** R = malloc(NB * sizeof(gsl_vector**));

    gsl_vector** Xtmp = malloc(NB * sizeof(gsl_vector*));
    gsl_vector** Rtmp = malloc(NB * sizeof(gsl_vector*));

    va_list va;
    va_start(va, NB);
    for (i=0; i<NB; i++) {
        B[i] = va_arg(va, gsl_vector*);
        X[i] = va_arg(va, gsl_vector**);
        R[i] = va_arg(va, gsl_vector**);
    }
    va_end(va);

    res = gslutils_solve_leastsquares(A, B, Xtmp, Rtmp, NB);
    for (i=0; i<NB; i++) {
        if (X[i])
            *(X[i]) = Xtmp[i];
        else
            gsl_vector_free(Xtmp[i]);
        if (R[i])
            *(R[i]) = Rtmp[i];
        else
            gsl_vector_free(Rtmp[i]);
    }
    free(Xtmp);
    free(Rtmp);
    free(X);
    free(R);
    free(B);
    return res;
}

int gslutils_solve_leastsquares(gsl_matrix* A, gsl_vector** B,
                                gsl_vector** X, gsl_vector** resids,
                                int NB) {
    int i;
    gsl_vector *tau, *resid = NULL;
    int ret;
    size_t M, N;

    M = A->size1;
    N = A->size2;

    for (i=0; i<NB; i++) {
        assert(B[i]);
        assert(B[i]->size == M);
    }

    tau = gsl_vector_alloc(MIN(M, N));
    assert(tau);

    ret = gsl_linalg_QR_decomp(A, tau);
    assert(ret == 0);
    // A,tau now contains a packed version of Q,R.

    for (i=0; i<NB; i++) {
        if (!resid) {
            resid = gsl_vector_alloc(M);
            assert(resid);
        }
        X[i] = gsl_vector_alloc(N);
        assert(X[i]);
        ret = gsl_linalg_QR_lssolve(A, tau, B[i], X[i], resid);
        assert(ret == 0);
        if (resids) {
            resids[i] = resid;
            resid = NULL;
        }
    }

    gsl_vector_free(tau);
    if (resid)
        gsl_vector_free(resid);

    return 0;
}


/*
 Copyright (C) 2002 M. Marques, A. Castro, A. Rubio, G. Bertsch

 This program is free software; you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation; either version 2, or (at your option)
 any later version.

 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with this program; if not, write to the Free Software
 Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
 02110-1301, USA.

 $Id$
*/

#include <config.h>
#include <stdlib.h>
#include <math.h>
#include <float.h>
#include <fortran_types.h>

#include <gsl/gsl_sf_legendre.h>

/* normalization constants for the spherical harmonics */
static double sph_cnsts[9] = {
	0.282094791773878, /* l = 0 */
	0.488602511902920, 0.488602511902920, 0.488602511902920, /* l = 1 */
	0.182091405098680, 0.364182810197360, 0.630783130505040, 0.364182810197360, 0.182091405098680
};

/* Computes real spherical harmonics ylm in the direction of vector r:
	 ylm = c * plm( cos(theta) ) * sin(m*phi)   for   m <  0
	 ylm = c * plm( cos(theta) ) * cos(m*phi)   for   m >= 0
	 with (theta,phi) the polar angles of r, c a positive normalization
	 constant and plm associated legendre polynomials. */

void FC_FUNC_(oct_ylm, OCT_YLM)
     (const fint * np, const double *x, const double *y, const double *z, const fint *l, const fint *m, double * restrict ylm)
{
  double r, r2, rr, rx, ry, rz, cosphi, sinphi, cosm, sinm, phase;
  int i, ip;

  if(l[0] == 0) {
    for (ip = 0; ip < np[0]; ip++) ylm[ip] = sph_cnsts[0];
    return;
  }

  for (ip = 0; ip < np[0]; ip++){

    r2 = x[ip]*x[ip] + y[ip]*y[ip] + z[ip]*z[ip];

    /* if r=0, direction is undefined => make ylm=0 except for l=0 */
    if(r2 < 1.0e-15){
      ylm[ip] = 0.0;
      continue;
    }

    switch(l[0]){
    case 1:
      rr = 1.0/sqrt(r2);
      switch(m[0]){
      case -1: ylm[ip] = -sph_cnsts[1]*rr*y[ip]; continue;
      case  0: ylm[ip] =  sph_cnsts[2]*rr*z[ip]; continue;
      case  1: ylm[ip] = -sph_cnsts[3]*rr*x[ip]; continue;
      }
    case 2:
      switch(m[0]){
      case -2: ylm[ip] =  sph_cnsts[4]*6.0*x[ip]*y[ip]/r2;
	continue;
      case -1: ylm[ip] = -sph_cnsts[5]*3.0*y[ip]*z[ip]/r2;
	continue;
      case  0: ylm[ip] =  sph_cnsts[6]*0.5*(3.0*z[ip]*z[ip]/r2 - 1.0);
	continue;
      case  1: ylm[ip] = -sph_cnsts[7]*3.0*x[ip]*z[ip]/r2;
	continue;
      case  2: ylm[ip] =  sph_cnsts[8]*3.0*(x[ip]*x[ip] - y[ip]*y[ip])/r2;
	continue;
      }
    }

    /* get phase */
    rr = 1.0/sqrt(r2);

    rx = x[ip]*rr;
    ry = y[ip]*rr;
    rz = z[ip]*rr;

    r = hypot(rx, ry);
    if(r < 1e-20) r = 1e-20; /* one never knows... */

    cosphi = rx/r;
    sinphi = ry/r;

    /* compute sin(mphi) and cos(mphi) by adding cos/sin */
    cosm = 1.; sinm=0.;
    for(i = 0; i < abs(m[0]); i++){
      double a = cosm, b = sinm;
      cosm = a*cosphi - b*sinphi;
      sinm = a*sinphi + b*cosphi;
    }
    phase = m[0] < 0 ? sinm : cosm;
    phase = m[0] == 0 ? phase : sqrt(2.0)*phase;

    /* adding small number (~= 10^-308) to avoid floating invalids */
    rz = rz + DBL_MIN;

    r = gsl_sf_legendre_sphPlm(l[0], abs(m[0]), rz);

    /* I am not sure whether we are including the Condon-Shortley factor (-1)^m */
    ylm[ip] = r*phase;
  }
}


/*
	Simbicon 1.5 Controller Editor Framework,
	Copyright 2009 Stelian Coros, Philippe Beaudoin and Michiel van de Panne.
	All rights reserved. Web: www.cs.ubc.ca/~van/simbicon_cef

	This file is part of the Simbicon 1.5 Controller Editor Framework.

	Simbicon 1.5 Controller Editor Framework is free software: you can
	redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.

	Simbicon 1.5 Controller Editor Framework is distributed in the hope
	that it will be useful, but WITHOUT ANY WARRANTY; without even the
	implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
	See the GNU General Public License for more details.

	You should have received a copy of the GNU General Public License
	along with Simbicon 1.5 Controller Editor Framework.
	If not, see <http://www.gnu.org/licenses/>.
*/

#pragma once

#include <stdlib.h>
#include <stdio.h>
#include <string.h>

#include <MathLib/MathLib.h>
#include <MathLib/ThreeTuple.h>

#include <gsl/matrix/gsl_matrix.h>

#define MATRIX_AT(m, i, j) (*((m->data + ((i) * m->tda + (j)))))

class Vector3d;

/*====================================================================================================================================================================*
 | This class will be used to represent matrices of arbitrary sizes (m rows by n columns) that have elements of type double. The underlying data strucutre used by    |
 | this class is gsl's (Gnu Scientific Library) matrix class. This class also makes use of the ATLAS implementation of BLAS for some operations such as matrix-matrix |
 | multiplication. This class is meant to improve performance, not necessarily ease of use.                                                                           |
 *====================================================================================================================================================================*/

class Matrix  {
friend class Vector;
protected:

	//this data structure holds all the matrix data
	gsl_matrix *matrix;

public:
	/**
		constructor - creates an m rows by n columns matrix that is not initialized to any particular values
	*/
	Matrix(int m, int n);

	/**
		default constructor
	*/
	Matrix();

	/**
		copy constructor - performs a deep copy of the matrix passed in as a parameter.
	*/
	Matrix(const Matrix& other);

	/**
		destructor.
	*/
        virtual ~Matrix();

	/**
		loads the matrix with all zero values.
	*/
	void loadZero();

	/**
		loads the matrix with 1's on the diagonal, 0's everywhere else - note: the matrix doesn't have to be square.
	*/
	void loadIdentity();

	/**
		this method sets the current matrix to a 3x3 matrix that is equal to the outer product of the vectors a and b
	*/
	void setToOuterproduct(const Vector3d& a, const Vector3d& b);

	/**
		this method resizes the current matrix to have m rows and n cols. If not enough space is allocated for it, then a new matrix of
		correct dimensions is allocated. There is no guarantee with regards to the data that is contained in the matrix after a resize operation.
	*/
	void resizeTo(int m, int n);

	/**
		copy operator - performs a deep copy of the matrix passed in as a parameter.
	*/
	Matrix& operator=(const Matrix &other);

	/**
		this method performs a shallow copy of the matrix that is passed in as a parameter.
	*/
	void shallowCopy(const Matrix& other, int startRow = 0, int startCol = 0, int endRow = -1, int endCol = -1);

	/**
		this method performs a deep copy of the matrix that is passed in as a paramerer.
	*/
	void deepCopy(const Matrix& other);

	/**
		Returns the number of columns
	*/
	int getColumnCount() const;

	/**
		Returns the number of rows
	*/
	int getRowCount()const;

	/**
		Multiplies each element in the current matrix by a constant
	*/
	void multiplyBy(const double val);

	/**
		This method sets the current matrix to be equal to one of the products: a * b, a'*b, a*b' or a'*b'.
		The values of transA and transB indicate which of the matrices are tranposed and which ones are not.
	*/
	void setToProductOf(const Matrix& a, const Matrix& b, bool transA = false, bool transB = false);

	/**
		This method computes the inverse of the matrix a and writes it over the current matrix. The implementation
		for the inverse of a matrix was obtained from Graphite. The parameter t is used as a threshold value for
		determinants, etc so that we still get a result for the inverse of our matrix even if it is very poorly conditioned.
	*/
	void setToInverseOf(const Matrix &a, double t = 0);

        /**
                This method prints the contents of the matrix - testing purpose only.
        */
        void printMatrix() const;

        /**
                This method prints the contents of the matrix - testing purpose only.
        */
        std::string toString() const;

	/**
		This method sets the current matrix to be a sub-matrix (starting at (i,j) and ending at (i+rows, j+cols)
		of the one that is passed in as a parameter - shallow copy only.
	*/
	void setToSubmatrix(const Matrix &a, int i, int j, int rows, int cols);

	/**
		This method returns a copy of the value of the matrix at (i,j)
	*/
	double get(int i, int j) const;

	/**
		This method sets the value of the matrix at (i,j) to newVal.
	*/
	void set(int i, int j, double newVal);

	/**
		This method is used to set the values in the matrix to the ones that are passed in the array of doubles.
		It is assumed that the array contains the right number of elements and that there is no space between consecutive rows (tda == nrCols).
	*/
	void setValues(double* vals);

	/**
		this method returns a pointer to its internal matrix data structure
	*/
	gsl_matrix* getMatrixPointer() const;

	/**
		Implement this operator to have a quick way of multiplying 3x3 matrices by vectors - used for dynamics for instance
	*/
	Vector3d operator * (const Vector3d &other);

	/**
		Implement a potentially faster function for 3x3 matrix - vector3d multiplication. Result and v need to be different!!
	*/
	void postMultiplyVector(const Vector3d& v, Vector3d& result);

	/**
		Still need to do (if need be): get row/col vector. Initialize a matrix from a vector so that we can create a 1xn matrix easily.
		make sure dgemv works properly, implement vector class, etc, swap, add, scale matrices, etc
	*/

	/**
		this method adds the matrix that is passed in as a parameter to the current matrix. In addition, it scales, both matrices by the two
		numbers that are passed in as parameters:
		*this = a * *this + b * other.
	*/
	void add(const Matrix& other, double scaleA = 1.0, double scaleB = 1.0);

	/**
		this method subtracts the matrix that is passed in as a parameter from the current matrix. In addition, it scales, both matrices by the two
		numbers that are passed in as parameters:
		*this = a * *this - b * other.
	*/
	void sub(const Matrix& other, double scaleA = 1.0, double scaleB = 1.0);

};

void testMatrixClass();


#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <assert.h>
#include <time.h>
#include <sys/time.h>
#include <gsl/gsl_math.h>

#include "cosmocalc.h"

double wtime(void)
{
  struct timeval tp;
  gettimeofday(&tp,NULL);
  return ((double) (tp.tv_sec)) + ((double) (tp.tv_usec))/1e6;
}

#define EPS 3.0e-11
void gauleg(double x1, double x2, double x[], double w[], int n)
{
  int m,j,i;
  double z1,z,xm,xl,pp,p3,p2,p1;

  m=(n+1)/2;
  xm=0.5*(x2+x1);
  xl=0.5*(x2-x1);
  for (i=1;i<=m;i++) {
    z=cos(3.141592654*(i-0.25)/(n+0.5));
    do {
      p1=1.0;
      p2=0.0;
      for (j=1;j<=n;j++) {
	p3=p2;
	p2=p1;
	p1=((2.0*j-1.0)*z*p2-(j-1.0)*p3)/j;
      }
      pp=n*(z*p1-p2)/(z*z-1.0);
      z1=z;
      z=z1-p1/pp;
    } while (fabs(z-z1) > EPS);
    x[i]=xm-xl*z;
    x[n+1-i]=xm+xl*z;
    w[i]=2.0*xl/((1.0-z*z)*pp*pp);
    w[n+1-i]=w[i];
  }
}
#undef EPS


/*
 * Copyright 2020 Makani Technologies LLC
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "lib/util/random_matrix.h"

#include <assert.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_vector.h>
#include <stdint.h>

#include "common/c_math/linalg.h"

void MatArrRandomGaussian(int32_t nr, int32_t nc, const gsl_rng *rng,
                          double *d) {
  for (int32_t i = 0; i < nr; ++i) {
    for (int32_t j = 0; j < nc; ++j) {
      d[i*nc + j] = gsl_ran_gaussian(rng, 1.0);
    }
  }
}

void MatArrRandomOrthogonal(int32_t n, const gsl_rng *rng, double *d) {
  MatArrRandomGaussian(n, n, rng, d);
  gsl_matrix *Q = gsl_matrix_alloc((size_t)n, (size_t)n);
  gsl_matrix *R = gsl_matrix_alloc((size_t)n, (size_t)n);
  gsl_vector *tau = gsl_vector_alloc((size_t)n);

  gsl_matrix_view A = gsl_matrix_view_array(d, (size_t)n, (size_t)n);
  gsl_linalg_QR_decomp(&A.matrix, tau);
  gsl_linalg_QR_unpack(&A.matrix, tau, Q, R);
  gsl_matrix_memcpy(&A.matrix, Q);

  gsl_vector_free(tau);
  gsl_matrix_free(R);
  gsl_matrix_free(Q);
}

void MatArrRandomWithRank(int32_t nr, int32_t nc, int32_t rank,
                          const gsl_rng *rng, double *d) {
  assert(rank <= nr && rank <= nc);

  MatArrZero(nr, nc, d);
  double *u = (double *)malloc((size_t)nr * sizeof(*d));
  double *v = (double *)malloc((size_t)nc * sizeof(*d));
  for (int32_t k = 0; k < rank; ++k) {
    MatArrRandomGaussian(1, nr, rng, u);
    MatArrRandomGaussian(1, nc, rng, v);

    for (int32_t i = 0; i < nr; ++i) {
      for (int32_t j = 0; j < nc; ++j) {
        d[i*nc + j] += u[i] * v[j];
      }
    }
  }
  free(u);
  free(v);
}


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2012-2017 by Synge Todo <wistaria@phys.s.u-tokyo.ac.jp>
*
* Distributed under 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)
*
*****************************************************************************/

#include <cblas.h>
#include <lapacke.h>
#include <rokko/cmatrix.h>

int imin(int x, int y) { return (x < y) ? x : y; }
int imax(int x, int y) { return (x > y) ? x : y; }

int main(int argc, char** argv) {
  int m = 3;
  int n = 5;
  int r, lwork, i, j, info;
  double norm;
  double *s, *work;
  double **a, **u, **vt, **t;

  if (argc > 2) {
    m = atoi(argv[1]);
    n = atoi(argv[2]);
  }
  r = imin(m, n);

  /* generate matrix */
  a = alloc_dmatrix(m, n);
  for (j = 0; j < n; ++j) {
    for (i = 0; i < m; ++i) {
      mat_elem(a, i, j) = n - imax(i, j);
    }
  }
  printf("Matrix A: ");
  fprint_dmatrix(stdout, m, n, a);

  /* singular value decomposition */
  u = alloc_dmatrix(m, r);
  vt = alloc_dmatrix(r, n);
  s = alloc_dvector(r);
  t = alloc_dmatrix(m, n);
  cblas_dcopy(m * n, mat_ptr(a), 1, mat_ptr(t), 1);
  lwork = imax(3 * r + imax(m, n), 5 * r);
  work = alloc_dvector(lwork);
  info = LAPACKE_dgesvd_work(LAPACK_COL_MAJOR, 'S', 'S', m, n, mat_ptr(t), m,
                             vec_ptr(s), mat_ptr(u), m, mat_ptr(vt), r,
                             vec_ptr(work), lwork);
  if (info != 0) {
    fprintf(stderr, "Error: dgesvd fails\n");
    exit(255);
  }
  free_dmatrix(t);
  printf("Matrix U: ");
  fprint_dmatrix(stdout, m, r, u);
  printf("Singular values: ");
  fprint_dvector(stdout, r, s);
  printf("Matrix Vt: ");
  fprint_dmatrix(stdout, r, n, vt);

  /* orthogonality check */
  t = alloc_dmatrix(r, r);
  cblas_dgemm(CblasColMajor, CblasTrans, CblasNoTrans, r, r, m, 1, mat_ptr(u), m,
              mat_ptr(u), m, 0, mat_ptr(t), r);
  for (i = 0; i < r; ++i) mat_elem(t, i, i) -= 1;
  norm = LAPACKE_dlange(LAPACK_COL_MAJOR, 'F', r, r, mat_ptr(t), r);
  printf("|| U^t U - I || = %e\n", norm);
  if (norm > 1e-10) {
    fprintf(stderr, "Error: orthogonality check\n");
    exit(255);
  }

  cblas_dgemm(CblasColMajor, CblasNoTrans, CblasTrans, r, r, n, 1, mat_ptr(vt), r,
              mat_ptr(vt), r, 0, mat_ptr(t), r);
  for (i = 0; i < r; ++i) mat_elem(t, i, i) -= 1;
  norm = LAPACKE_dlange(LAPACK_COL_MAJOR, 'F', r, r, mat_ptr(t), r);
  printf("|| V^t V - I || = %e\n", norm);
  if (norm > 1e-10) {
    fprintf(stderr, "Error: orthogonality check\n");
    exit(255);
  }
  free_dmatrix(t);

  /* solution check */
  t = alloc_dmatrix(m, r);
  cblas_dgemm(CblasColMajor, CblasNoTrans, CblasTrans, m, r, n, 1, mat_ptr(a), m,
              mat_ptr(vt), r, 0, mat_ptr(t), m);
  free_dmatrix(vt);
  vt = alloc_dmatrix(r, r);
  cblas_dgemm(CblasColMajor, CblasTrans, CblasNoTrans, r, r, m, 1, mat_ptr(u), m,
              mat_ptr(t), m, 0, mat_ptr(vt), r);
  for (i = 0; i < r; ++i) mat_elem(vt, i, i) -= s[i];
  norm = LAPACKE_dlange(LAPACK_COL_MAJOR, 'F', r, r, mat_ptr(vt), r);
  printf("|| U^t A V - diag(S) || = %e\n", norm);
  if (norm > 1e-10) {
    fprintf(stderr, "Error: solution check\n");
    exit(255);
  }
  free_dmatrix(t);

  free_dmatrix(a);
  free_dmatrix(u);
  free_dmatrix(vt);
  free_dvector(s);
  return 0;
}


/*
 # This file is part of the Astrometry.net suite.
 # Licensed under a 3-clause BSD style license - see LICENSE
 */
#include <math.h>
#include <stdlib.h>
#include <assert.h>
#include <string.h>

#include <gsl/gsl_matrix_double.h>
#include <gsl/gsl_vector_double.h>

#include "os-features.h"
#include "sip-utils.h"
#include "gslutils.h"
#include "starutil.h"
#include "mathutil.h"
#include "errors.h"
#include "log.h"

double wcs_pixel_center_for_size(double size) {
    return 0.5 + 0.5 * size;
}

void tan_rotate(const tan_t* tanin, tan_t* tanout, double angle) {
    double s,c;
    double newcd[4];
    memmove(tanout, tanin, sizeof(tan_t));
    s = sin(deg2rad(angle));
    c = cos(deg2rad(angle));
    newcd[0] = c*tanin->cd[0][0] + s*tanin->cd[1][0];
    newcd[1] = c*tanin->cd[0][1] + s*tanin->cd[1][1];
    newcd[2] = -s*tanin->cd[0][0] + c*tanin->cd[1][0];
    newcd[3] = -s*tanin->cd[0][1] + c*tanin->cd[1][1];
    tanout->cd[0][0] = newcd[0];
    tanout->cd[0][1] = newcd[1];
    tanout->cd[1][0] = newcd[2];
    tanout->cd[1][1] = newcd[3];
}

int sip_ensure_inverse_polynomials(sip_t* sip) {
    if ((sip->a_order == 0 && sip->b_order == 0) ||
        (sip->ap_order > 0  && sip->bp_order > 0)) {
        return 0;
    }
    sip->ap_order = sip->bp_order = MAX(sip->a_order, sip->b_order) + 1;
    return sip_compute_inverse_polynomials(sip, 0, 0, 0, 0, 0, 0);
}

int sip_compute_inverse_polynomials(sip_t* sip, int NX, int NY,
                                    double xlo, double xhi,
                                    double ylo, double yhi) {
    int inv_sip_order;
    int M, N;
    int i, j, p, q, gu, gv;
    double maxu, maxv, minu, minv;
    double u, v, U, V;
    gsl_matrix *mA;
    gsl_vector *b1, *b2, *x1, *x2;
    tan_t* tan;

    assert(sip->a_order == sip->b_order);
    assert(sip->ap_order == sip->bp_order);
    tan = &(sip->wcstan);

    logverb("sip_compute-inverse_polynomials: A %i, AP %i\n",
            sip->a_order, sip->ap_order);

    /*
     basic idea: lay down a grid in image, for each gridpoint, push
     through the polynomial to get yourself into warped image
     coordinate (but not yet lifted onto the sky).  Then, using the
     set of warped gridpoints as inputs, fit back to their original
     grid locations as targets.
     */
    inv_sip_order = sip->ap_order;

    // Number of grid points to use:
    if (NX == 0)
        NX = 10 * (inv_sip_order + 1);
    if (NY == 0)
        NY = 10 * (inv_sip_order + 1);
    if (xhi == 0)
        xhi = tan->imagew;
    if (yhi == 0)
        yhi = tan->imageh;

    logverb("NX,NY %i,%i, x range [%f, %f], y range [%f, %f]\n",
            NX,NY, xlo, xhi, ylo, yhi);

    // Number of coefficients to solve for:
    // We only compute the upper triangle polynomial terms
    N = (inv_sip_order + 1) * (inv_sip_order + 2) / 2;

    // Number of samples to fit.
    M = NX * NY;

    mA = gsl_matrix_alloc(M, N);
    b1 = gsl_vector_alloc(M);
    b2 = gsl_vector_alloc(M);
    assert(mA);
    assert(b1);
    assert(b2);

    /*
     *  Rearranging formula (4), (5), and (6) from the SIP paper gives the
     *  following equations:
     *
     *    +----------------------- Linear pixel coordinates in PIXELS
     *    |                        before SIP correction
     *    |                   +--- Intermediate world coordinates in DEGREES
     *    |                   |
     *    v                   v
     *                   -1
     *    U = [CD11 CD12]   * x
     *    V   [CD21 CD22]     y
     *
     *    +---------------- PIXEL distortion delta from telescope to
     *    |                 linear coordinates
     *    |    +----------- Linear PIXEL coordinates before SIP correction
     *    |    |       +--- Polynomial U,V terms in powers of PIXELS
     *    v    v       v
     *
     *    -f(u1,v1) =  p11 p12 p13 p14 p15 ... * ap1
     *    -f(u2,v2) =  p21 p22 p23 p24 p25 ...   ap2
     *    ...
     *
     *    -g(u1,v1) =  p11 p12 p13 p14 p15 ... * bp1
     *    -g(u2,v2) =  p21 p22 p23 p24 p25 ...   bp2
     *    ...
     *
     *  which recovers the A and B's.
     */

    minu = xlo - tan->crpix[0];
    maxu = xhi - tan->crpix[0];
    minv = ylo - tan->crpix[1];
    maxv = yhi - tan->crpix[1];

    // Sample grid locations.
    i = 0;
    for (gu=0; gu<NX; gu++) {
        for (gv=0; gv<NY; gv++) {
            double fuv, guv;
            // Calculate grid position in original image pixels
            u = (gu * (maxu - minu) / (NX-1)) + minu;
            v = (gv * (maxv - minv) / (NY-1)) + minv;
            // compute U=u+f(u,v) and V=v+g(u,v)
            sip_calc_distortion(sip, u, v, &U, &V);
            fuv = U - u;
            guv = V - v;
            // Polynomial terms...
            j = 0;
            for (p = 0; p <= inv_sip_order; p++)
                for (q = 0; q <= inv_sip_order; q++) {
                    if (p + q > inv_sip_order)
                        continue;
                    assert(j < N);
                    gsl_matrix_set(mA, i, j,
                                   pow(U, (double)p) * pow(V, (double)q));
                    j++;
                }
            assert(j == N);
            gsl_vector_set(b1, i, -fuv);
            gsl_vector_set(b2, i, -guv);
            i++;
        }
    }
    assert(i == M);

    // Solve the linear equation.
    if (gslutils_solve_leastsquares_v(mA, 2, b1, &x1, NULL, b2, &x2, NULL)) {
        ERROR("Failed to solve SIP inverse matrix equation!");
        return -1;
    }

    // Extract the coefficients
    j = 0;
    for (p = 0; p <= inv_sip_order; p++)
        for (q = 0; q <= inv_sip_order; q++) {
            if ((p + q > inv_sip_order))
                continue;
            assert(j < N);
            sip->ap[p][q] = gsl_vector_get(x1, j);
            sip->bp[p][q] = gsl_vector_get(x2, j);
            j++;
        }
    assert(j == N);

    // Check that we found values that actually invert the polynomial.
    // The error should be particularly small at the grid points.
    if (log_get_level() > LOG_VERB) {
        // rms error accumulators:
        double sumdu = 0;
        double sumdv = 0;
        int Z;
        for (gu = 0; gu < NX; gu++) {
            for (gv = 0; gv < NY; gv++) {
                double newu, newv;
                // Calculate grid position in original image pixels
                u = (gu * (maxu - minu) / (NX-1)) + minu;
                v = (gv * (maxv - minv) / (NY-1)) + minv;
                sip_calc_distortion(sip, u, v, &U, &V);
                sip_calc_inv_distortion(sip, U, V, &newu, &newv);
                sumdu += square(u - newu);
                sumdv += square(v - newv);
            }
        }
        sumdu /= (NX*NY);
        sumdv /= (NX*NY);
        debug("RMS error of inverting a distortion (at the grid points, in pixels):\n");
        debug("  du: %g\n", sqrt(sumdu));
        debug("  dv: %g\n", sqrt(sumdu));
        debug("  dist: %g\n", sqrt(sumdu + sumdv));

        sumdu = 0;
        sumdv = 0;
        Z = 1000;
        for (i=0; i<Z; i++) {
            double newu, newv;
            u = uniform_sample(minu, maxu);
            v = uniform_sample(minv, maxv);
            sip_calc_distortion(sip, u, v, &U, &V);
            sip_calc_inv_distortion(sip, U, V, &newu, &newv);
            sumdu += square(u - newu);
            sumdv += square(v - newv);
        }
        sumdu /= Z;
        sumdv /= Z;
        debug("RMS error of inverting a distortion (at random points, in pixels):\n");
        debug("  du: %g\n", sqrt(sumdu));
        debug("  dv: %g\n", sqrt(sumdu));
        debug("  dist: %g\n", sqrt(sumdu + sumdv));
    }

    gsl_matrix_free(mA);
    gsl_vector_free(b1);
    gsl_vector_free(b2);
    gsl_vector_free(x1);
    gsl_vector_free(x2);

    return 0;
}

anbool tan_pixel_is_inside_image(const tan_t* wcs, double x, double y) {
    return (x >= 1 && x <= wcs->imagew && y >= 1 && y <= wcs->imageh);
}

anbool sip_pixel_is_inside_image(const sip_t* wcs, double x, double y) {
    return tan_pixel_is_inside_image(&(wcs->wcstan), x, y);
}

anbool sip_is_inside_image(const sip_t* wcs, double ra, double dec) {
    double x,y;
    if (!sip_radec2pixelxy(wcs, ra, dec, &x, &y))
        return FALSE;
    return sip_pixel_is_inside_image(wcs, x, y);
}

anbool tan_is_inside_image(const tan_t* wcs, double ra, double dec) {
    double x,y;
    if (!tan_radec2pixelxy(wcs, ra, dec, &x, &y))
        return FALSE;
    return tan_pixel_is_inside_image(wcs, x, y);
}

int* sip_filter_stars_in_field(const sip_t* sip, const tan_t* tan,
                               const double* xyz, const double* radec,
                               int N, double** p_xy, int* inds, int* p_Ngood) {
    int i, Ngood;
    int W, H;
    double* xy = NULL;
    anbool allocd = FALSE;

    assert(sip || tan);
    assert(xyz || radec);
    assert(p_Ngood);

    Ngood = 0;
    if (!inds) {
        inds = malloc(N * sizeof(int));
        allocd = TRUE;
    }

    if (p_xy)
        xy = malloc(N * 2 * sizeof(double));

    if (sip) {
        W = sip->wcstan.imagew;
        H = sip->wcstan.imageh;
    } else {
        W = tan->imagew;
        H = tan->imageh;
    }

    for (i=0; i<N; i++) {
        double x, y;
        if (xyz) {
            if (sip) {
                if (!sip_xyzarr2pixelxy(sip, xyz + i*3, &x, &y))
                    continue;
            } else {
                if (!tan_xyzarr2pixelxy(tan, xyz + i*3, &x, &y))
                    continue;
            }
        } else {
            if (sip) {
                if (!sip_radec2pixelxy(sip, radec[i*2], radec[i*2+1], &x, &y))
                    continue;
            } else {
                if (!tan_radec2pixelxy(tan, radec[i*2], radec[i*2+1], &x, &y))
                    continue;
            }
        }
        // FIXME -- check half- and one-pixel FITS issues.
        if ((x < 0) || (y < 0) || (x >= W) || (y >= H))
            continue;

        inds[Ngood] = i;
        if (xy) {
            xy[Ngood * 2 + 0] = x;
            xy[Ngood * 2 + 1] = y;
        }
        Ngood++;
    }

    if (allocd)
        inds = realloc(inds, Ngood * sizeof(int));

    if (xy)
        xy = realloc(xy, Ngood * 2 * sizeof(double));
    if (p_xy)
        *p_xy = xy;

    *p_Ngood = Ngood;

    return inds;
}

void sip_get_radec_center(const sip_t* wcs,
                          double* p_ra, double* p_dec) {
    double px = wcs_pixel_center_for_size(wcs->wcstan.imagew);
    double py = wcs_pixel_center_for_size(wcs->wcstan.imageh);
    sip_pixelxy2radec(wcs, px, py, p_ra, p_dec);
}

void tan_get_radec_center(const tan_t* wcs,
                          double* p_ra, double* p_dec) {
    double px = wcs_pixel_center_for_size(wcs->imagew);
    double py = wcs_pixel_center_for_size(wcs->imageh);
    tan_pixelxy2radec(wcs, px, py, p_ra, p_dec);
}

double sip_get_radius_deg(const sip_t* wcs) {
    return arcsec2deg(sip_pixel_scale(wcs) * hypot(wcs->wcstan.imagew, wcs->wcstan.imageh)/2.0);
}

double tan_get_radius_deg(const tan_t* wcs) {
    return arcsec2deg(tan_pixel_scale(wcs) * hypot(wcs->imagew, wcs->imageh)/2.0);
}

void sip_get_radec_center_hms(const sip_t* wcs,
                              int* rah, int* ram, double* ras,
                              int* decsign, int* decd, int* decm, double* decs) {
    double ra, dec;
    sip_get_radec_center(wcs, &ra, &dec);
    ra2hms(ra, rah, ram, ras);
    dec2dms(dec, decsign, decd, decm, decs);
}

void sip_get_radec_center_hms_string(const sip_t* wcs,
                                     char* rastr, char* decstr) {
    double ra, dec;
    sip_get_radec_center(wcs, &ra, &dec);
    ra2hmsstring(ra, rastr);
    dec2dmsstring(dec, decstr);
}

void sip_get_field_size(const sip_t* wcs,
                        double* pw, double* ph,
                        char** units) {
    double minx = 0.5;
    double maxx = (wcs->wcstan.imagew + 0.5);
    double midx = (minx + maxx) / 2.0;
    double miny = 0.5;
    double maxy = (wcs->wcstan.imageh + 0.5);
    double midy = (miny + maxy) / 2.0;
    double ra1, dec1, ra2, dec2, ra3, dec3;
    double w, h;

    // measure width through the middle
    sip_pixelxy2radec(wcs, minx, midy, &ra1, &dec1);
    sip_pixelxy2radec(wcs, midx, midy, &ra2, &dec2);
    sip_pixelxy2radec(wcs, maxx, midy, &ra3, &dec3);
    w = arcsec_between_radecdeg(ra1, dec1, ra2, dec2) +
        arcsec_between_radecdeg(ra2, dec2, ra3, dec3);
    // measure height through the middle
    sip_pixelxy2radec(wcs, midx, miny, &ra1, &dec1);
    sip_pixelxy2radec(wcs, midx, midy, &ra2, &dec2);
    sip_pixelxy2radec(wcs, midx, maxy, &ra3, &dec3);
    h = arcsec_between_radecdeg(ra1, dec1, ra2, dec2) +
        arcsec_between_radecdeg(ra2, dec2, ra3, dec3);

    if (MIN(w, h) < 60.0) {
        *units = "arcseconds";
        *pw = w;
        *ph = h;
    } else if (MIN(w, h) < 3600.0) {
        *units = "arcminutes";
        *pw = w / 60.0;
        *ph = h / 60.0;
    } else {
        *units = "degrees";
        *pw = w / 3600.0;
        *ph = h / 3600.0;
    }
}

void sip_walk_image_boundary(const sip_t* wcs, double stepsize,
                             void (*callback)(const sip_t* wcs, double x, double y, double ra, double dec, void* token),
                             void* token) {
    int i, side;
    // Walk the perimeter of the image in steps of stepsize pixels
    double W = wcs->wcstan.imagew;
    double H = wcs->wcstan.imageh;
    {
        double Xmin = 0.5;
        double Xmax = W + 0.5;
        double Ymin = 0.5;
        double Ymax = H + 0.5;
        double offsetx[] = { Xmin, Xmax, Xmax, Xmin };
        double offsety[] = { Ymin, Ymin, Ymax, Ymax };
        double stepx[] = { +stepsize, 0, -stepsize, 0 };
        double stepy[] = { 0, +stepsize, 0, -stepsize };
        int Nsteps[] = { ceil(W/stepsize), ceil(H/stepsize), ceil(W/stepsize), ceil(H/stepsize) };

        for (side=0; side<4; side++) {
            for (i=0; i<Nsteps[side]; i++) {
                double ra, dec;
                double x, y;
                x = MIN(Xmax, MAX(Xmin, offsetx[side] + i * stepx[side]));
                y = MIN(Ymax, MAX(Ymin, offsety[side] + i * stepy[side]));
                sip_pixelxy2radec(wcs, x, y, &ra, &dec);
                callback(wcs, x, y, ra, dec, token);
            }
        }
    }
}

struct radecbounds {
    double rac, decc;
    double ramin, ramax, decmin, decmax;
};

static void radec_bounds_callback(const sip_t* wcs, double x, double y, double ra, double dec, void* token) {
    struct radecbounds* b = token;
    b->decmin = MIN(b->decmin, dec);
    b->decmax = MAX(b->decmax, dec);
    if (ra - b->rac > 180)
        // wrap-around: racenter < 180, ra has gone < 0 but been wrapped around to > 180
        ra -= 360;
    if (b->rac - ra > 180)
        // wrap-around: racenter > 180, ra has gone > 360 but wrapped around to > 0.
        ra += 360;

    b->ramin = MIN(b->ramin, ra);
    b->ramax = MAX(b->ramax, ra);
}

void sip_get_radec_bounds(const sip_t* wcs, int stepsize,
                          double* pramin, double* pramax,
                          double* pdecmin, double* pdecmax) {
    struct radecbounds b;

    sip_get_radec_center(wcs, &(b.rac), &(b.decc));
    b.ramin  = b.ramax = b.rac;
    b.decmin = b.decmax = b.decc;
    sip_walk_image_boundary(wcs, stepsize, radec_bounds_callback, &b);

    // Check for poles...
    // north pole
    if (sip_is_inside_image(wcs, 0, 90)) {
        b.ramin = 0;
        b.ramax = 360;
        b.decmax = 90;
    }
    if (sip_is_inside_image(wcs, 0, -90)) {
        b.ramin = 0;
        b.ramax = 360;
        b.decmin = -90;
    }

    if (pramin) *pramin = b.ramin;
    if (pramax) *pramax = b.ramax;
    if (pdecmin) *pdecmin = b.decmin;
    if (pdecmax) *pdecmax = b.decmax;
}

void sip_shift(const sip_t* sipin, sip_t* sipout,
               double xlo, double xhi, double ylo, double yhi) {
    memmove(sipout, sipin, sizeof(sip_t));
    tan_transform(&(sipin->wcstan), &(sipout->wcstan),
                  xlo, xhi, ylo, yhi, 1.0);
}

void tan_transform(const tan_t* tanin, tan_t* tanout,
                   double xlo, double xhi, double ylo, double yhi,
                   double scale) {
    memmove(tanout, tanin, sizeof(tan_t));
    tanout->imagew = (xhi - xlo + 1) * scale;
    tanout->imageh = (yhi - ylo + 1) * scale;
    tanout->crpix[0] = (tanout->crpix[0] - (xlo - 1)) * scale;
    tanout->crpix[1] = (tanout->crpix[1] - (ylo - 1)) * scale;
    tanout->cd[0][0] /= scale;
    tanout->cd[0][1] /= scale;
    tanout->cd[1][0] /= scale;
    tanout->cd[1][1] /= scale;
}

void tan_scale(const tan_t* tanin, tan_t* tanout,
               double scale) {
    memmove(tanout, tanin, sizeof(tan_t));
    tanout->imagew *= scale;
    tanout->imageh *= scale;

    tanout->crpix[0] = 0.5 + scale * (tanin->crpix[0] - 0.5);
    tanout->crpix[1] = 0.5 + scale * (tanin->crpix[1] - 0.5);
    tanout->cd[0][0] /= scale;
    tanout->cd[0][1] /= scale;
    tanout->cd[1][0] /= scale;
    tanout->cd[1][1] /= scale;
}

void sip_scale(const sip_t* wcsin, sip_t* wcsout,
               double scale) {
    int i, j;
    memmove(wcsout, wcsin, sizeof(sip_t));
    tan_scale(&(wcsin->wcstan), &(wcsout->wcstan), scale);
    for (i=0; i<=wcsin->a_order; i++) {
        for (j=0; j<=wcsin->a_order; j++) {
            if (i + j > wcsin->a_order)
                continue;
            wcsout->a[i][j] *= pow(scale, 1 - (i+j));
        }
    }
    for (i=0; i<=wcsin->b_order; i++) {
        for (j=0; j<=wcsin->b_order; j++) {
            if (i + j > wcsin->b_order)
                continue;
            wcsout->b[i][j] *= pow(scale, 1 - (i+j));
        }
    }
    for (i=0; i<=wcsin->ap_order; i++) {
        for (j=0; j<=wcsin->ap_order; j++) {
            if (i + j > wcsin->ap_order)
                continue;
            wcsout->ap[i][j] *= pow(scale, 1 - (i+j));
        }
    }
    for (i=0; i<=wcsin->bp_order; i++) {
        for (j=0; j<=wcsin->bp_order; j++) {
            if (i + j > wcsin->bp_order)
                continue;
            wcsout->bp[i][j] *= pow(scale, 1 - (i+j));
        }
    }
}


//#include <config.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_interp.h>

static double fun(double x)
{
    return x + cos(x * x);
}

int main(void)
{
    const double a = 1.0;
    const double b = 10.0;
    const int steps = 10;
    double xi, yi, x[100], y[100], dx;
    FILE *input, *output;
    int i;

    input = fopen("wartosci.txt", "w");
    output = fopen("inter-cspline.txt", "w");

    dx = (b - a) / (double)steps;

    for (i = 0; i <= steps; ++i)
    {
        x[i] = a + (double)i * dx + 0.5 * sin((double)i * dx);
        y[i] = fun(x[i]);
        fprintf(input, "%g %g\n", x[i], y[i]);
    }

    {
        gsl_interp_accel *acc = gsl_interp_accel_alloc();

        gsl_spline *spline = gsl_spline_alloc(gsl_interp_cspline, steps + 1);

        gsl_spline_init(spline, x, y, steps + 1);

        for (xi = a; xi <= b; xi += 0.01)
        {
            yi = gsl_spline_eval(spline, xi, acc);
            fprintf(output, "%g %g\n", xi, yi);
        }

        gsl_spline_free(spline);
        gsl_interp_accel_free(acc);
    }
    return 0;
}


#ifndef BIO_BAYESIAN_HIERARCHICAL_CLUSTERING_H_
#define BIO_BAYESIAN_HIERARCHICAL_CLUSTERING_H_

#include "bio/defs.h"

#include <boost/graph/adjacency_list.hpp>
#include <boost/io/ios_state.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/storage.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <set>
#include <list>
#include <limits>
#include <iterator>
#include <algorithm>
#include <iostream>

#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_sf_exp.h>
#include <gsl/gsl_sf_log.h>
#include <gsl/gsl_math.h>

BIO_NS_START

/** @file
See http://www.gatsby.ucl.ac.uk/~heller/bhc.pdf for the motivation for these algorithms.
*/

/** Aggregates a data set with its associated score. */
template <class DataIt, class Score>
struct Cluster
{
    typedef std::set<DataIt> data_set_t;

    data_set_t data_set;
    Score score;
};

/** A Normal-Wishart prior.

Each datum should be an iterator that points to a sequence of reals.

I am not convinced this is implemented correctly...
*/
struct NormalWishartPrior
{
    typedef double real_t;
    typedef boost::numeric::ublas::vector<real_t> vector_t;
    typedef boost::numeric::ublas::matrix<real_t> matrix_t;
    matrix_t s;
    vector_t m;
    real_t r;
    real_t v;
    unsigned get_num_dims() const { return s.size1(); }

    NormalWishartPrior(unsigned num_dimensions = 0)
        : s(num_dimensions,  num_dimensions)
        , m(num_dimensions)
    {
    }

    template <class DataIt>
    real_t
    operator()(
        DataIt data_begin,
        DataIt data_end) const
    {
        using namespace boost::numeric::ublas;

        //if we have no data
        if (data_end == data_begin)
        {
            return real_t(1);
        }

        //calculate the number of data points
        unsigned n = 0; //std::count_if(data_begin, data_end, true);
        for (DataIt di = data_begin;
            data_end != di;
            ++di)
        {
            ++n;
        }

        //put the data set in a matrix
        matrix_t x(get_num_dims(), n);
        unsigned i = 0;
        for (DataIt di = data_begin;
            data_end != di;
            ++di, ++i)
        {
            for (unsigned j = 0;
                get_num_dims() != j;
                ++j)
            {
                x(j,i) = (**di)[j];
            }
        }

        //calculate the sum of the data in each dimension
        vector_t x_sum(get_num_dims());
        for (unsigned j = 0;
            get_num_dims() != j;
            ++j)
        {
            x_sum(j) = 0;
            for (unsigned i = 0;
                n != i;
                ++i)
            {
                x_sum(j) += x(j, i);
            }
        }

        //calculate s_dash - the first term is s
        matrix_t s_dash(s);
        {
            //add X.XT to s_dash
            s_dash += prod(x, trans(x));

            //add m.mT term to s_dash
            s_dash += (r * n / (n + r)) * outer_prod(m, m);

            //add x_sum.x_sumT term to s_dash
            s_dash += (1.0 / (n + r)) * outer_prod(x_sum, x_sum);

            //subtract m.x_sum term from s_dash
            s_dash -= (r / (n + r)) * (outer_prod(m, x_sum) + outer_prod(x_sum, m));
        }

        //std::cout << s << std::endl;
        //std::cout << s_dash << std::endl;
        //std::cout << prod(x, trans(x)) << std::endl;

        //calculate the determinants
        const real_t s_det = get_determinant(s);
        const real_t s_dash_det = get_determinant(s_dash);

        //v_dash
        const real_t v_dash = v + n;

        //the number of dimensions
        const real_t k = get_num_dims();

        //the number of data points
        const real_t N = n;

        //the natural logarithm of the result
        real_t ln_p_D_given_H1 = 0.0;

        ln_p_D_given_H1 -= N * k / 2 * gsl_sf_log(2 * M_PI);
        ln_p_D_given_H1 += k / 2 * gsl_sf_log(r / (N + r));
        ln_p_D_given_H1 += v / 2 * gsl_sf_log(fabs(s_det));
        ln_p_D_given_H1 -= v_dash / 2 * gsl_sf_log(fabs(s_dash_det));
        ln_p_D_given_H1 += v_dash * k / 2 * gsl_sf_log(2);
        ln_p_D_given_H1 -= v * k / 2 * gsl_sf_log(2);

        //add/subtract the pi terms
        for (unsigned d = 0; k != d; ++d)
        {
            ln_p_D_given_H1 +=
                gsl_sf_lngamma((v_dash + 1 - d) / 2)
                - gsl_sf_lngamma((v + 1 - d) / 2);

        }

        return gsl_sf_exp(ln_p_D_given_H1);
    }

    template <class M>
    static real_t
    get_determinant(const M & m)
    {
        BOOST_ASSERT(m.size1() == m.size2());

        matrix_t lu(m);
        //permutation_matrix<unsigned> p(m.size());

        lu_factorize(lu);

        real_t result = 0;
        for (unsigned j = 0;
            lu.size1() != j;
            ++j)
        {
            result += lu(j,j);
        }
        return result;
    }
};

/** A Bernoulli - Beta prior.

Each datum should be an iterator that points to a sequence of bools (or convertible types).
*/
struct BernoulliBetaPrior
{
    typedef double real_t;
    typedef std::vector<real_t> param_vec_t;
    param_vec_t alpha;
    param_vec_t beta;

    BernoulliBetaPrior(unsigned num_dimensions = 0)
        : alpha(num_dimensions)
        , beta(num_dimensions)
    {
    }

    template <class DataIt>
    real_t
    operator()(
        DataIt data_begin,
        DataIt data_end)
    {
        if (data_end == data_begin)
        {
            return real_t(1);
        }

        assert(alpha.size() == beta.size());
        assert((*data_begin)->size() == alpha.size());

        //the log of p(D|H1)
        double ln_p_D_H1 = 0.0;

        unsigned n = 0;
        for (DataIt i = data_begin; data_end != i; ++i)
        {
            ++n;
        }

        //for each dimension
        for (size_t d = 0; alpha.size() != d; ++d)
        {
            //work out m_d
            unsigned m_d = 0;
            for (DataIt i = data_begin; data_end != i; ++i)
            {
                assert((*i)->size() == alpha.size());

                if ((**i)[d])
                {
                    ++m_d;
                }
            }

            ln_p_D_H1 +=
                gsl_sf_lngamma(alpha[d] + beta[d])
                + gsl_sf_lngamma(alpha[d] + m_d)
                + gsl_sf_lngamma(beta[d] + n - m_d)
                - gsl_sf_lngamma(alpha[d])
                - gsl_sf_lngamma(beta[d])
                - gsl_sf_lngamma(alpha[d] + beta[d] + n);
        }

        return gsl_sf_exp(ln_p_D_H1);
    }
};

/** A Multinomial Dirichlet prior.

Each datum should be an iterator that points to a sequence of bools (or convertible types).
*/
struct MultinomialDirichletPrior
{
    typedef double real_t;
    typedef std::vector<real_t> param_vec_t;
    param_vec_t alpha;

    MultinomialDirichletPrior(unsigned num_dimensions = 0)
        : alpha(num_dimensions)
    {
    }

    template <class DataIt>
    real_t
    operator()(
        DataIt data_begin,
        DataIt data_end)
    {
        if (data_end == data_begin)
        {
            return real_t(1);
        }

        assert((*data_begin)->size() == alpha.size());

        //the log of p(D|H1) ie log of the result
        double ln_p_D_H1 = 0.0;

        //TODO - not implemented

        return gsl_sf_exp(ln_p_D_H1);
    }
};

/** The score of a cluster based on a prior model and the score of the two clusters
it is composed of. */
struct BayesianClusterScore
{
    typedef double real_t;

    real_t p_Dk_given_H1k;
    real_t p_Dk_given_Tk;

    BayesianClusterScore(real_t p_Dk_given_H1k = 1, real_t p_Dk_given_Tk = 1)
        : p_Dk_given_H1k(p_Dk_given_H1k)
        , p_Dk_given_Tk(p_Dk_given_Tk)
    {
    }

    bool operator<(const BayesianClusterScore & rhs) const
    {
        if (real_t(0) == p_Dk_given_Tk)
        {
            return false;
        }
        if (real_t(0) == rhs.p_Dk_given_Tk)
        {
            return false;
        }
        return operator()() < rhs();
    }

    real_t operator()() const
    {
        return p_Dk_given_H1k / p_Dk_given_Tk;
    }
};

BIO_NS_END

std::ostream &
operator<<(std::ostream & os, const BIO_NS::BayesianClusterScore & score)
{
    boost::io::ios_all_saver ias(os);
    os.precision(3);

    os << score.p_Dk_given_H1k << "/" << score.p_Dk_given_Tk << "=" << score();
    return os;
}

BIO_NS_START

/** Scores clusters according to http://www.gatsby.ucl.ac.uk/~heller/bhcnew.pdf */
template <class Model>
struct BayesianClusterScorer
{
    typedef double real_t;
    typedef Model model_t;

    model_t & model;
    BayesianClusterScorer(model_t & model)
        : model(model)
    {
    }

    typedef BayesianClusterScore score_t;
    static score_t min_score() { return score_t(-1, 1); }

    template <class Cluster>
    score_t
    operator()(
        const Cluster & cluster) const
    {
        const real_t marginal_likelihood = model(cluster.data_set.begin(), cluster.data_set.end());
        return score_t(marginal_likelihood, marginal_likelihood);
    }

    /** Returns the score of the data set resulting from merging the two data sets. */
    template <class Cluster>
    score_t
    operator()(
        const Cluster & cluster_1,
        const Cluster & cluster_2) const
    {
        //merge the two data sets
        typename Cluster::data_set_t merged_data_set;
        set_union(
            cluster_1.data_set.begin(),
            cluster_1.data_set.end(),
            cluster_2.data_set.begin(),
            cluster_2.data_set.end(),
            std::inserter(merged_data_set, merged_data_set.begin()));

        score_t result;
        const real_t pi_k = 0.5; //?????
        result.p_Dk_given_H1k =
            pi_k * model(merged_data_set.begin(), merged_data_set.end());
        result.p_Dk_given_Tk =
            result.p_Dk_given_H1k
            + (1 - pi_k) * cluster_1.score.p_Dk_given_Tk * cluster_2.score.p_Dk_given_Tk;

        return result;
    }
};

/** Defines some types for given data types and scorers. */
template <class DataIt, class Scorer>
struct ClusterTraits
{
    typedef DataIt data_it;
    typedef Scorer scorer_t;

    typedef typename data_it::value_type data_t;
    typedef std::set<data_it> data_set_t;

    typedef Cluster<data_it, typename scorer_t::score_t> cluster_t;
    typedef typename cluster_t::data_set_t::iterator cluster_data_it;

    typedef std::list<cluster_t> cluster_list_t;
    typedef typename cluster_list_t::iterator cluster_it;
};

/** Clusters the data points according to pairs' scores. */
template <class DataIt, class Scorer, class Output>
void
cluster(
    DataIt data_begin,
    DataIt data_end,
    Scorer & scorer,
    Output & output)
{
    using namespace boost;
    using namespace std;

    typedef ClusterTraits<DataIt, Scorer> traits_t;

    //initialise the list of clusters with one entry for every data point
    typename traits_t::cluster_list_t cluster_list;
    for (DataIt d = data_begin;
        data_end != d;
        ++d)
    {
        typename traits_t::cluster_t cluster;
        cluster.data_set.insert(d);
        cluster.score = scorer(cluster);
        output(cluster_list.insert(cluster_list.begin(), cluster));
    }

    //while we have at least 2 clusters to merge
    while (1 < cluster_list.size())
    {
        //look for the best pair of clusters to merge in the hierarchy
        typename std::pair<typename traits_t::cluster_it, typename traits_t::cluster_it> best_clusters =
            make_pair(cluster_list.end(), cluster_list.end());
        typename Scorer::score_t best_score = Scorer::min_score();

        //for each pair of clusters, c1 and c2
        for (typename traits_t::cluster_it c1 = cluster_list.begin();
            cluster_list.end() != c1;
            ++c1)
        {
            typename traits_t::cluster_it c2 = c1;
            ++c2;
            for ( ;
                cluster_list.end() != c2;
                ++c2)
            {
                //score the pair of clusters
                const typename Scorer::score_t score = scorer(*c1, *c2);

                //is it the best score so far?
                if (best_score < score)
                {
                    //update best
                    best_score = score;
                    best_clusters.first = c1;
                    best_clusters.second = c2;
                }
            }
        }
        assert(best_clusters.first != best_clusters.second);
        assert(cluster_list.end() != best_clusters.first);
        assert(cluster_list.end() != best_clusters.second);

        //create and add the merged cluster
        typename traits_t::cluster_t merged_cluster;
        merged_cluster.score = best_score;
        set_union(
            best_clusters.first->data_set.begin(),
            best_clusters.first->data_set.end(),
            best_clusters.second->data_set.begin(),
            best_clusters.second->data_set.end(),
            inserter(merged_cluster.data_set, merged_cluster.data_set.begin()));

        //let the output know
        output(
            best_clusters.first,
            best_clusters.second,
            cluster_list.insert(cluster_list.begin(), merged_cluster));

        //remove the old data sets and add the merged one
        cluster_list.erase(best_clusters.first);
        cluster_list.erase(best_clusters.second);
    }
}

struct LoggingClusterer
{
    std::ostream & os;

    LoggingClusterer(std::ostream & os) : os(os)
    {
    }

    template <class ClusterIt>
    void
    operator()(
        const ClusterIt & cluster)
    {
        using namespace std;

        os << "Initial cluster: ";
        for (typename ClusterIt::value_type::data_set_t::const_iterator d = cluster->data_set.begin();
            cluster->data_set.end() != d;
            ++d)
        {
            os << **d << ",";
        }
        os << " score:" << cluster->score << endl;
    }

    template <class ClusterIt>
    void
    operator()(
        ClusterIt cluster_1,
        ClusterIt cluster_2,
        ClusterIt merged_cluster)
    {
        using namespace std;

        //print what we are doing
        cout << "Clustering: ";
        for (typename ClusterIt::value_type::data_set_t::const_iterator d = cluster_1->data_set.begin();
            cluster_1->data_set.end() != d;
            ++d)
        {
            os << **d << ",";
        }
        cout << " with ";
        for (typename ClusterIt::value_type::data_set_t::const_iterator d = cluster_2->data_set.begin();
            cluster_2->data_set.end() != d;
            ++d)
        {
            os << **d << ",";
        }
        os << " score:" << merged_cluster->score << endl;
    }
};

template <class DataIt, class Scorer>
struct TreeBuildingClusterer
{
    typedef DataIt data_it;
    typedef Scorer scorer_t;
    typedef ClusterTraits<data_it, scorer_t> traits_t;

    typedef std::pair<bool, typename traits_t::data_it> vertex_property;
    typedef boost::adjacency_list<
        boost::listS,
        boost::vecS,
        boost::directedS,
        vertex_property,
        typename Scorer::score_t> tree_t;
    typedef typename boost::graph_traits<tree_t>::vertex_iterator tree_vertex_it;
    typedef typename boost::graph_traits<tree_t>::vertex_descriptor tree_vertex;
    tree_t tree;

    /** A map from clusters to tree nodes. */
    typedef std::map<typename traits_t::cluster_t *, tree_vertex> cluster_vertex_map_t;
    cluster_vertex_map_t cluster_vertex_map;

    LoggingClusterer logger;
    bool logging_on;

    TreeBuildingClusterer(bool logging_on = false) : logger(std::cout), logging_on(logging_on)
    {
    }

    void
    reset()
    {
        cluster_vertex_map.clear();
        tree.clear();
    }

    void
    operator()(
        typename traits_t::cluster_it cluster)
    {
        using namespace std;
        using namespace boost;

        if (logging_on)
        {
            logger(cluster);
        }

        cluster_vertex_map[&*cluster] = add_vertex(make_pair(true, *cluster->data_set.begin()), tree);
    }

    void
    operator()(
        typename traits_t::cluster_it cluster_1,
        typename traits_t::cluster_it cluster_2,
        typename traits_t::cluster_it merged_cluster)
    {
        using namespace std;
        using namespace boost;

        if (logging_on)
        {
            logger(cluster_1, cluster_2, merged_cluster);
        }

        //find the vertex descriptors for cluster_1 and cluster_2
        assert(cluster_vertex_map.end() != cluster_vertex_map.find(&*cluster_1));
        assert(cluster_vertex_map.end() != cluster_vertex_map.find(&*cluster_2));

        cluster_vertex_map[&*merged_cluster] = add_vertex(make_pair(false, typename traits_t::data_it()), tree);

        add_edge(cluster_vertex_map[&*merged_cluster], cluster_vertex_map[&*cluster_1], tree);
        add_edge(cluster_vertex_map[&*merged_cluster], cluster_vertex_map[&*cluster_2], tree);
    }
};

BIO_NS_END

#endif //BIO_BAYESIAN_HIERARCHICAL_CLUSTERING_H_


/* randist/bigauss.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2007 James Theiler, Brian Gough
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include <config.h>
#include <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

/* The Bivariate Gaussian probability distribution is

   p(x,y) dxdy = (1/(2 pi sigma_x sigma_y sqrt(c)))
    exp(-((x/sigma_x)^2 + (y/sigma_y)^2 - 2 r (x/sigma_x)(y/sigma_y))/2c) dxdy

   where c = 1-r^2
*/

void
gsl_ran_bivariate_gaussian (const gsl_rng * r,
                            double sigma_x, double sigma_y, double rho,
                            double *x, double *y)
{
  double u, v, r2, scale;

  do
    {
      /* choose x,y in uniform square (-1,-1) to (+1,+1) */

      u = -1 + 2 * gsl_rng_uniform (r);
      v = -1 + 2 * gsl_rng_uniform (r);

      /* see if it is in the unit circle */
      r2 = u * u + v * v;
    }
  while (r2 > 1.0 || r2 == 0);

  scale = sqrt (-2.0 * log (r2) / r2);

  *x = sigma_x * u * scale;
  *y = sigma_y * (rho * u + sqrt(1 - rho*rho) * v) * scale;
}

double
gsl_ran_bivariate_gaussian_pdf (const double x, const double y,
                                const double sigma_x, const double sigma_y,
                                const double rho)
{
  double u = x / sigma_x ;
  double v = y / sigma_y ;
  double c = 1 - rho*rho ;
  double p = (1 / (2 * M_PI * sigma_x * sigma_y * sqrt(c)))
    * exp (-(u * u - 2 * rho * u * v + v * v) / (2 * c));
  return p;
}


/*
    hyper.c
*/
#include <stdlib.h>
#include <stdio.h>
#include <gsl/gsl_sf_psi.h>

static double update_hyper(double param, int *vec, int **mat, int nrow, int ncol){
    double numer = 0.0;
    double denom = 0.0;
    int i, j;

    for(i = 0;i < nrow;i++)
        for(j = 0;j < ncol;j++)
            numer += gsl_sf_psi((double)mat[i][j] + param);
    numer -= (double)nrow * (double)ncol * gsl_sf_psi(param);

    for(i = 0;i < nrow;i++)
        denom += (double)ncol * gsl_sf_psi((double)vec[i] + param * (double)ncol);
    denom -= (double)nrow * (double)ncol * gsl_sf_psi(param * (double)ncol);

    return param * numer / denom;
}

void update_alpha(double *alpha, int *n_m, int **n_mz, int ndocs, int nclass){
    double sum_alpha = 0.0;
    double numer = 0.0;
    double denom = 0.0;
    int d, k;
    for(k = 0;k < nclass;k++)
        sum_alpha += alpha[k];
    for(d = 0;d < ndocs;d++)
        denom += gsl_sf_psi((double)n_m[d] + sum_alpha);
    denom -= (double)ndocs * gsl_sf_psi(sum_alpha);

    for(k = 0;k < nclass;k++){
        numer = 0.0;
        for(d = 0;d < ndocs;d++)
            numer += gsl_sf_psi((double)n_mz[d][k] + alpha[k]);
        numer -= (double)ndocs * gsl_sf_psi(alpha[k]);
        alpha[k] = alpha[k] * numer / denom;
    }
}

double update_beta(double beta, int *n_z, int **n_zw, int nclass, int nlex){
    double new_beta = update_hyper(beta, n_z, n_zw, nclass, nlex);
    return new_beta;
}