/* test2_heapsort.c  sort array x,y,z closest to xp,yp,zp*/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "heapsort.h"

/* sample user program with a data structure and call on  heapsort */

typedef struct {double x; double y; double z;} xyz;
static xyz vert[6] = {{ 1.0, 1.0, 1.0},
                      { 2.0, 1.0, 1.0},
                      { 1.0, 2.0, 1.0},
                      { 1.0, 1.0, 2.0},
                      { 2.0, 2.0, 2.0},
                      { 2.0, 2.0, 1.0}}; 
double xp = 1.6;
double yp = 1.8;
double zp = 1.7;

static int compd(void *elem1, void *elem2);   /* using distance */
static double dist(double x, double y, double z,
                   double xp, double yp, double zp);

int main(int argc, char *argv[])
{
  int i, n;

  printf("test2_heapsort.c 3D data with 4th key \n");
  n = 6;
  printf("before sort of n=%d doubles \n",n);
  printf("pt  %f  %f  %f\n", xp, yp, zp);
  for(i=0; i<n; i++) printf("%2d %f  %f  %f  dist=%f\n",
                          i, vert[i].x, vert[i].y, vert[i].z,
                          dist(vert[i].x, vert[i].y, vert[i].z, xp, yp, zp));


  heapsort((void *)vert, n, sizeof(xyz), compd);

  printf("after sort of n=%d struct, closest to \n",n);
  printf("pt  %f  %f  %f\n", xp, yp, zp);
  for(i=0; i<n; i++) printf("%2d %f  %f  %f  dist=%f\n",
                          i, vert[i].x, vert[i].y, vert[i].z,
                          dist(vert[i].x, vert[i].y, vert[i].z, xp, yp, zp));

  printf("end test2_heapsort.c \n");
  return 0;
} /* end test_heapsort main */

int compd(void *elem1, void *elem2)
{
  double x1 = (*(xyz *)elem1).x;
  double y1 = (*(xyz *)elem1).y;
  double z1 = (*(xyz *)elem1).z;
  double d1;
  double x2 = (*(xyz *)elem2).x;
  double y2 = (*(xyz *)elem2).y;
  double z2 = (*(xyz *)elem2).z;
  double d2;

  d1 = dist(x1, y1, z1, xp, yp, zp);
  d2 = dist(x2, y2, z2, xp, yp, zp);
  printf("d1 = %f, d2=%f \n", d1, d2);
  if(d1 < d2) return -1;
  return 1;
}

static double dist(double x, double y, double z,
                   double xp, double yp, double zp)
{
  return sqrt((x-xp)*(x-xp) + (y-yp)*(y-yp) + (z-zp)*(z-zp));
}

/* end test2_heapsort.c */