# include "data.h"
int find_matches2d (Vector *X1, Vector *Y1, Vector *X2, Vector *Y2, double Radius, Vector *index1, Vector *index2);

// 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;
  double Radius;
  char *endptr;
  Vector *X1vec, *Y1vec, *X2vec, *Y2vec;
  Vector *index1, *index2;

  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, "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);
  }

  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);
  }

  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);
}
