# include "data.h"

int fit2d (int argc, char **argv) {
  
  double **c, **b, **s, X, Y;
  int k, K, i, j, nx, ny, nterm, mterm, wterm, order;
  int N, Weight;
  float *x, *y, *z; 
  char name[64];
  Vector *xvec, *yvec, *zvec, *dyvec;

  Weight = FALSE;
  if (N = get_argument (argc, argv, "-dy")) {
    remove_argument (N, &argc, argv);
    if ((dyvec = SelectVector (argv[N], OLDVECTOR, TRUE)) == NULL) return (FALSE);    
    remove_argument (N, &argc, argv);
    Weight = TRUE;
    fprintf (stderr, "warning: weighting currently ignored!\n");
  }

  if (argc != 5) {
    fprintf (stderr, "USAGE: fit x y z order [-dy`z wt]\n");
    return (FALSE);
  }

  if ((xvec = SelectVector (argv[1], OLDVECTOR, TRUE)) == NULL) return (FALSE);    
  if ((yvec = SelectVector (argv[2], OLDVECTOR, TRUE)) == NULL) return (FALSE);    
  if ((zvec = SelectVector (argv[3], OLDVECTOR, TRUE)) == NULL) return (FALSE);    

  order = atof (argv[4]);
  nterm = order + 1;
  wterm = nterm*(nterm + 1)/2;
  mterm = 2*order + 1;

  if (xvec[0].Nelements != yvec[0].Nelements) {
    fprintf (stderr, "vectors must have same length\n");
    return (FALSE);
  }
  if (xvec[0].Nelements != zvec[0].Nelements) {
    fprintf (stderr, "vectors must have same length\n");
    return (FALSE);
  }
  if (Weight && xvec[0].Nelements != dyvec[0].Nelements) {
    fprintf (stderr, "vectors must have same length\n");
    return (FALSE);
  }
  
  x = xvec[0].elements;
  y = yvec[0].elements;
  z = zvec[0].elements;
  /* dy = dyvec[0].elements; */

  /* allocate the summation matrices */
  ALLOCATE (b, double *, wterm);
  ALLOCATE (c, double *, wterm);
  for (i = 0; i < wterm; i++) {
    ALLOCATE (c[i], double, wterm);
    bzero (c[i], wterm*sizeof(double));
    ALLOCATE (b[i], double, 1);
    bzero (b[i], sizeof(double));
  }
  ALLOCATE (s, double *, mterm);
  for (i = 0; i < mterm; i++) {
    ALLOCATE (s[i], double, mterm);
    bzero (s[i], mterm*sizeof(double));
  }

  /* add up the x,y values */
  for (i = 0; i < xvec[0].Nelements; i++, x++, y++) {
    Y = X = 1;
    for (ny = 0; ny < mterm; ny++) {
      X = Y;
      for (nx = 0; nx < mterm - ny; nx++) {
	s[nx][ny] += X;
	X = X * (*x);
      }
      Y = Y * (*y);
    }
  }

  /* add up the z values */
  x = xvec[0].elements;
  y = yvec[0].elements;
  z = zvec[0].elements;
  for (i = 0; i < xvec[0].Nelements; i++, x++, y++, z++) {
    Y = X = *z;
    for (N = 0, ny = 0; ny < nterm; ny++) {
      X = Y;
      for (nx = 0; nx < nterm - ny; nx++, N++) {
	b[N][0] += X;
	X = X * (*x);
      }
      Y = Y * (*y);
    }
  }

  /* re-sort mterm x mterm matrix to wterm matrix */
  for (k = j = 0; j < nterm; j++) {
    for (i = 0; i < nterm - j; i++, k++) {
      for (K = ny = 0; ny < nterm; ny++) {
	for (nx = 0; nx < nterm - ny; nx++, K++) {
	  c[K][k] = s[nx+i][ny+j];
	}
      }
    }
  }

  gaussj (c, wterm, b, 1);

  fprintf (stderr, "z = ");
  for (N = ny = 0; ny < nterm; ny++) {
    for (nx = 0; nx < nterm - ny; nx++, N++) {
      sprintf (name, "CX%dY%d", nx, ny);
      set_variable (name, b[N][0]);
      fprintf (stderr, "%f x^%d y^%d  ", b[N][0], nx, ny);
    }
  }
  sprintf (name, "Cnn");
  set_variable (name, (double) order);
  
  fprintf (stderr, "\n");
  return (TRUE);

}




