# include "data.h"

int find_matches2d (Vector *X1, Vector *Y1, Vector *X2, Vector *Y2, double Radius, Vector *index1, Vector *index2);
int find_matches2d_closest (Vector *X1, Vector *Y1, Vector *X2, Vector *Y2, double Radius, Vector *index);

// match2d (X1) (Y1) (X2) (Y2) (Radius) [-index1 (index1)] [-index2 (index2)] [-nomatch1 nomatch1] [-nomatch2 nomatch2]
// X1[Index1] <=> X2[Index2] (etc)
int match2d (int argc, char **argv) {
  
  int N, CLOSEST;
  double Radius;
  char *endptr;
  Vector *X1vec, *Y1vec, *X2vec, *Y2vec;
  Vector *index1, *index2;

  CLOSEST = FALSE;
  if ((N = get_argument (argc, argv, "-closest"))) {
    remove_argument (N, &argc, argv);
    CLOSEST = TRUE;
  }

  if ((N = get_argument (argc, argv, "-index1"))) {
    remove_argument (N, &argc, argv);
    if ((index1 = SelectVector (argv[N], ANYVECTOR, TRUE)) == NULL) return (FALSE);    
    remove_argument (N, &argc, argv);
  } else {
    if ((index1 = SelectVector ("index1", ANYVECTOR, TRUE)) == NULL) return (FALSE);    
  }

  if ((N = get_argument (argc, argv, "-index2"))) {
    remove_argument (N, &argc, argv);
    if ((index2 = SelectVector (argv[N], ANYVECTOR, TRUE)) == NULL) return (FALSE);    
    remove_argument (N, &argc, argv);
  } else {
    if ((index2 = SelectVector ("index2", ANYVECTOR, TRUE)) == NULL) return (FALSE);    
  }

  if (argc != 6) {
    gprint (GP_ERR, "USAGE: match2d X1 Y1 X2 Y2 Radius [-index1 (index1)] [-index2 (index2)] [-closest]\n");
    gprint (GP_ERR, "  if -closest is provided, index1 & index2 will have the same length as X1 and X2 (respectively)\n");
    gprint (GP_ERR, "    with either the index of the match or a value of -1 for non-matches\n");
    return (FALSE);
  }

  /*
    we have two modes of operation:  

    without -closest, we are finding all matched pairs within the match radius.  in this
    case, the two index vectors have the same length, one entry per matched pair.
    x1[index1],y1[index1] matches to x2[index2],y2[index2].

    with -closest selected, we are finding the closest element of set 1 to each of set 2
    and vice versa.  in this case, index1 is always the same length as x1,y1, while index2
    is the same lengths as x2,y2.  x2[index1],y2[index1] matches x1,y1 while
    x1[index2],y1[index2] matches x2,y2

   */

  if ((X1vec = SelectVector (argv[1], OLDVECTOR, TRUE)) == NULL) return (FALSE);    
  if ((Y1vec = SelectVector (argv[2], OLDVECTOR, TRUE)) == NULL) return (FALSE);    
  if ((X2vec = SelectVector (argv[3], OLDVECTOR, TRUE)) == NULL) return (FALSE);    
  if ((Y2vec = SelectVector (argv[4], OLDVECTOR, TRUE)) == NULL) return (FALSE);    

  if (X1vec[0].Nelements != Y1vec[0].Nelements) {
    gprint (GP_ERR, "X1 & Y1 vectors must have same length\n");
    return (FALSE);
  }
  if (X2vec[0].Nelements != Y2vec[0].Nelements) {
    gprint (GP_ERR, "X2 & Y2 vectors must have same length\n");
    return (FALSE);
  }

  REQUIRE_VECTOR_FLT (X1vec, FALSE); 
  REQUIRE_VECTOR_FLT (Y1vec, FALSE); 
  REQUIRE_VECTOR_FLT (X2vec, FALSE); 
  REQUIRE_VECTOR_FLT (Y2vec, FALSE); 

  Radius = strtod (argv[5], &endptr);
  if (*endptr) {
    gprint (GP_ERR, "Radius must be numerical (%s)\n", argv[5]);
    return (FALSE);
  }

  if (CLOSEST) {
      find_matches2d_closest (X1vec, Y1vec, X2vec, Y2vec, Radius, index1);
      find_matches2d_closest (X2vec, Y2vec, X1vec, Y1vec, Radius, index2);
  } else {
      find_matches2d (X1vec, Y1vec, X2vec, Y2vec, Radius, index1, index2);
  }

  return (TRUE);
}

// we are not defining a relative offset DX,DY for now
int find_matches2d (Vector *X1, Vector *Y1, Vector *X2, Vector *Y2, double Radius, Vector *index1, Vector *index2) {
  
  off_t i, j, first_j, I, J, *N1, *N2, Nmatch, NMATCH, DMATCH;
  double dX, dY, dR, Radius2;

  NMATCH = MAX(MAX(0.01*X1->Nelements, 0.01*X2->Nelements), 100);
  DMATCH = NMATCH;

  ResetVector (index1, OPIHI_INT, NMATCH);
  ResetVector (index2, OPIHI_INT, NMATCH);

  ALLOCATE (N1, off_t, X1->Nelements);
  ALLOCATE (N2, off_t, X2->Nelements);

  for (i = 0; i < X1->Nelements; i++) { N1[i] = i; }
  for (i = 0; i < X2->Nelements; i++) { N2[i] = i; }

  sort_coords_indexonly (X1->elements.Flt, Y1->elements.Flt, N1, X1->Nelements);
  sort_coords_indexonly (X2->elements.Flt, Y2->elements.Flt, N2, X2->Nelements);

  Radius2 = Radius*Radius;

  Nmatch = 0;
  for (i = j = 0; (i < X1->Nelements) && (j < X2->Nelements);) {
    I = N1[i];
    J = N2[j];

    dX = X1->elements.Flt[I] - X2->elements.Flt[J];

    if (dX <= -1.02*Radius) { i++; continue; }
    if (dX >= +1.02*Radius) { j++; continue; }

    // look for all matches of list2() to list1(i)
    first_j = j;
    for (j = first_j; (dX > -1.02*Radius) && (j < X2->Nelements); j++) {
      J = N2[j];
      dX = X1->elements.Flt[I] - X2->elements.Flt[J];
      dY = Y1->elements.Flt[I] - Y2->elements.Flt[J];
      dR = dX*dX + dY*dY;
      if (dR < Radius2) {
	index1->elements.Int[Nmatch] = I;
	index2->elements.Int[Nmatch] = J;

	// XXX track matches 1 and 2 with internal vector, save new nomatch index vectors
	// after this loop

	Nmatch ++;
	if (Nmatch >= NMATCH) {
	  NMATCH += DMATCH;
	  REALLOCATE (index1->elements.Int, opihi_int, NMATCH);
	  REALLOCATE (index2->elements.Int, opihi_int, NMATCH);
	}
      }
    }
    j = first_j;
    i++;
  }
  index1->Nelements = Nmatch;
  index2->Nelements = Nmatch;

  free (N1);
  free (N2);

  return (TRUE);
}

// find the elements of X2,Y2 which are closest to each element of X1,Y1 (-1 if no match)
int find_matches2d_closest (Vector *X1, Vector *Y1, Vector *X2, Vector *Y2, double Radius, Vector *index) {
  
  off_t i, j, Jmin, Ji, I, J, *N1, *N2, NMATCH;
  double dX, dY, dR, Radius2, Rmin;

  NMATCH = X1->Nelements;
  ResetVector (index, OPIHI_INT, NMATCH);

  for (i = 0; i < index->Nelements; i++) { index->elements.Int[i] = -1; }

  ALLOCATE (N1, off_t, X1->Nelements);
  ALLOCATE (N2, off_t, X2->Nelements);

  for (i = 0; i < X1->Nelements; i++) { N1[i] = i; }
  for (i = 0; i < X2->Nelements; i++) { N2[i] = i; }

  sort_coords_indexonly (X1->elements.Flt, Y1->elements.Flt, N1, X1->Nelements);
  sort_coords_indexonly (X2->elements.Flt, Y2->elements.Flt, N2, X2->Nelements);

  Radius2 = Radius*Radius;

  // find the closest entry in list 2 to the current entry in list 1:
  for (i = j = 0; (i < X1->Nelements) && (j < X2->Nelements);) {
    I = N1[i];
    J = N2[j];

    dX = X1->elements.Flt[I] - X2->elements.Flt[J];

    if (dX <= -1.02*Radius) { 
      // no match in list 2 to this entry
      index->elements.Int[I] = -1;
      i++; 
      continue; 
    }
    if (dX >= +1.02*Radius) { j++; continue; }

    // look for closest matches of list2() to list1(i)
    Jmin = -1;
    Rmin = Radius2;
    for (Ji = j; (dX > -1.02*Radius) && (Ji < X2->Nelements); Ji++) {
      J = N2[Ji];
      dX = X1->elements.Flt[I] - X2->elements.Flt[J];
      dY = Y1->elements.Flt[I] - Y2->elements.Flt[J];
      dR = dX*dX + dY*dY;
      if (dR > Radius2) continue;
      if (dR < Rmin) {
	Rmin = dR;
	Jmin  = J;
      }
    }

    // no match in list 2 to this entry
    if (Jmin == -1) {
      index->elements.Int[I] = -1;
      i++;
      continue;
    }
    index->elements.Int[I] = Jmin;
    i++;
  }
  index->Nelements = NMATCH;

  free (N1);
  free (N2);

  return (TRUE);
}
