/* This file contains some utility functions for building the Gaussian
   Process model code */

/* Used by readnetCDF() below for error handling */
#define ERRCODE 2
#define ERR(e) {printf("Error: %s\n", nc_strerror(e)); exit(ERRCODE);}

void *reallocate_S_element(void *S_element, size_t newsize) {
  /* Grow the state struct's array size for adding more
     data. Since the grid is large, we cannot afford allocating very
     liberally, instead we need to allocate only what we need, with
     some tolerance, to not excessively often need to realloc

     This function can also be used as a simplification to realloc any
     dynamically allocated array.
  */

  float *tmp;

  tmp = realloc(S_element, newsize);
  if (!tmp) {
    printf("Error reserving memory for a grid variable!\n");
    return NULL;
  } else {
    return tmp;
  }
}

void reallocate_S(struct state *S, struct config *E, size_t N) {
  /* Reallocates the grid variables in S by adding N to the arrays to
     make space to continue appending. */

  size_t newsize = S->allocated_for_all_obs + N;
  size_t ns = newsize*sizeof(float);

  S->x = (float*) reallocate_S_element(S->x, ns);
  S->y = (float*) reallocate_S_element(S->y, ns);
  S->z = (float*) reallocate_S_element(S->z, ns);
  S->t = (float*) reallocate_S_element(S->t, ns);
  S->dv = (float*) reallocate_S_element(S->dv, ns);
  S->dv_unc = (float*) reallocate_S_element(S->dv_unc, ns);
  if (E->use_wind) {
    S->u = (float*) reallocate_S_element(S->u, ns);
    S->v = (float*) reallocate_S_element(S->v, ns);
  }

  S->allocated_for_all_obs = newsize;
}


int daynumber_from_t(size_t t) {
  /* Returns the daynumber corresponding to the datatime t. The times
     t should be seconds from start of 1970 - 1400000000 */
  return (int) ((double) t - 10004800 + 43200)/86400;
}

float xy_to_phi(float x, float y) {
  return atan2f(y,x);
}

struct daydata* readnetCDF(char* ifile, struct config *E,
			   float dataread_minlat, float dataread_maxlat){
  /* Reads a netcdf datafile and returns a pointer to the daydata
     struct with all the data. Only adds data that is sufficiently
     close to the geographical domain of the experiment. Modify this
     file to read data other than OCO-2, by changing variable names
     etc. below. You may need to add other variables if needed. */

  /* The daydata return variable */
  struct daydata *D = NULL;

  /* misc ints */
  size_t i, j, k;
  long int allocated;
  int ret;
  /* daynumber tells how manyth day from start of experiment we are
     talking about */
  int dn;

  /* netCDF related variables */
  int ncid, latid, lonid, sounding_id, dvid, dv_uncid, timeid, uid, vid, qfid, ncid_Retrieval;

  /* The temporary arrays where we put latitudes and longitudes,
     concentrations, and winds */
  float *lat = NULL; /* latitudes */
  float *lon = NULL; /* longitudes */
  float *dv = NULL; /* data value (e.g. XCO2) */
  float *dv_unc = NULL; /* data value standard deviation */
  float *u = NULL; /* wind speed east-west */
  float *v = NULL; /* wind speed north-south */
  int *qf = NULL; /* quality flag */

  /* Temporarily here this is double as the values are huge. Values
     are made lower later */
  double *dtime;

  /* Number of points inside the grid area */
  size_t ndata;
  size_t newsize;
  /* 1d array so only one element in array. netCDF peculiarities */
  size_t start[1] = {0};
  size_t end[1] = {0};

  /* Maximum radius tolerance in degrees */
  float max_rad_lat;

  /* Malloc the data structure that you return in the end */
  D = malloc(sizeof(struct daydata));
  D->lat = NULL;
  D->lon = NULL;
  D->data = NULL;
  /* Number of variables in data. We need to record this to know how
     much to free. */
  D->nvars = (E->use_wind) ? 5 : 3;

  /* Open file, get variable id's, then get data. Change here if you
     want to read in other variables. */
  if ((ret = nc_open(ifile, NC_NOWRITE, &ncid))) ERR(ret);
  if ((ret = nc_inq_varid(ncid, "latitude", &latid))) ERR(ret);
  if ((ret = nc_inq_varid(ncid, "longitude", &lonid))) ERR(ret);
  if ((ret = nc_inq_varid(ncid, "xco2", &dvid))) ERR(ret);
  if ((ret = nc_inq_varid(ncid, "xco2_uncertainty", &dv_uncid))) ERR(ret);
  if ((ret = nc_inq_varid(ncid, "time", &timeid))) ERR(ret);
  if ((ret = nc_inq_varid(ncid, "xco2_quality_flag", &qfid))) ERR(ret);

  if (E->use_wind) { /* Also this conforms to OCO-2 data format */
    if ((ret = nc_inq_ncid(ncid, "Meteorology", &ncid_Retrieval))) ERR(ret);
    if ((ret = nc_inq_varid(ncid_Retrieval, "windspeed_u_met", &uid))) ERR(ret);
    if ((ret = nc_inq_varid(ncid_Retrieval, "windspeed_v_met", &vid))) ERR(ret);
  }

  if ((ret = nc_inq_dimid(ncid, "sounding_id", &sounding_id))) ERR(ret);
  if ((ret = nc_inq_dimlen(ncid, sounding_id, &ndata))) ERR(ret);
  end[0] = ndata;

  if (DEBUG) {printf("Reading variables...");}
  dtime = malloc(sizeof(double) * ndata);
  if ((ret = nc_get_vara_double(ncid, timeid, start, end, dtime))) ERR(ret);

  /* If first observation is outside time domain, we discard everything. */
  dn = daynumber_from_t(dtime[0] - 1400000000);
  printf("daynumber %d/max %zu. ", dn, E->maxdays-1);

  if (dn > E->maxdays - 1) { /* Indexing starts from zero, and we want  at most E->maxdays data */
    printf("Found daynumber %d/%zu outside day range, skipping file %s.\n", dn+1, E->maxdays, ifile);
    D->ngobs = 0;
    free(dtime);
    ret = nc_close(ncid);
    if (ret != 0) {
      exit(ret);
    }
    return D;
  }

  /* Allocate memory for data */
  lat = malloc(sizeof(float) * ndata);
  lon = malloc(sizeof(float) * ndata);
  dv = malloc(sizeof(float) * ndata);
  dv_unc = malloc(sizeof(float) * ndata);
  qf = malloc(sizeof(int) * ndata);

  if (E->use_wind) {
    u = malloc(sizeof(float) * ndata);
    v = malloc(sizeof(float) * ndata);
  }

  if ((ret = nc_get_vara_float(ncid, latid, start, end, lat))) ERR(ret);
  if ((ret = nc_get_vara_float(ncid, lonid, start, end, lon))) ERR(ret);
  if ((ret = nc_get_vara_float(ncid, dvid, start, end, dv))) ERR(ret);
  if ((ret = nc_get_vara_float(ncid, dv_uncid, start, end, dv_unc))) ERR(ret);
  if ((ret = nc_get_vara_int(ncid, qfid, start, end, qf))) ERR(ret);

  if (E->use_wind) {
    if ((ret = nc_get_vara_float(ncid_Retrieval, uid, start, end, u))) ERR(ret);
    if ((ret = nc_get_vara_float(ncid_Retrieval, vid, start, end, v))) ERR(ret);
  }
  if (DEBUG) {printf("done!\n");}

  ret = nc_close(ncid);
  if (ret != 0) {
    exit(ret);
  }

  D->first_datatime = dtime[0];

  /* malloc the data array in the daydata struct*/
  D->data = malloc(sizeof(float*) * D->nvars);
  for (i=0; i<D->nvars; i++) {
    D->data[i] = malloc(sizeof(float) * E->malloc_chunk);
  }
  D->lat = malloc(sizeof(float) * E->malloc_chunk);
  D->lon = malloc(sizeof(float) * E->malloc_chunk);

  allocated = E->malloc_chunk;

  /* Populate the data object. Note that we restrict the
     observations to those that are over 380 ppm. Anything below
     that is very very likely a measurement error due to aerosols
     etc. 

     N.B. For filtering data, do it here. You should probably remove
     the dv[i] > 380 condition to include all data independent of the
     data value.

     N.B.We take only a given percentage of observations at random
     for performance reasons! That is dependent on obs_dist thinning
     parameter in gpconfig.h */

  float prev_lat = -100;
  float prev_lon = 1000;

  j = 0;
  max_rad_lat = E->max_rad/M_PI*180;
  for (i=0; i<ndata; i++) {
    float mr = 1.*random()/RAND_MAX;
    float dist = sqrtf(powf((lat[i] - prev_lat)/E->gridres, 2) + powf(((lon[i] - prev_lon)/cosf(lat[i]*M_PI/180.)/E->gridres), 2))/E->obs_dist;

    if (((E->mode) || (dv[i] > 380)) && /* Discard CO2 readings under 380 ppm */
	(mr < dist) &&
	(lat[i] < dataread_maxlat + max_rad_lat) &&
	(lat[i] > dataread_minlat - max_rad_lat) &&
	(!qf[i]) &&
	(lon[i] > (E->minlon - 180 - max_rad_lat/cosf(lat[i]/180*M_PI))) &&
	(lon[i] < (E->maxlon - 180 + max_rad_lat/cosf(lat[i]/180*M_PI)))) {

      // printf("Added index/lat/lon observation: %d/%f/%f\n", i, lat[i], lon[i]);
      prev_lat = lat[i];
      prev_lon = lon[i];

      /* Allocate more space if we need */
      if (!(j - allocated)) {
	newsize = (allocated + E->malloc_chunk)*sizeof(float);
	D->lat = (float*) reallocate_S_element(D->lat, newsize);
	D->lon = (float*) reallocate_S_element(D->lon, newsize);
	for (k=0; k<D->nvars; k++) {
	  D->data[k] = (float*) reallocate_S_element(D->data[k], newsize);
	}
	allocated += E->malloc_chunk;
      }

      if (E->dependent_variable == 1) {
	D->data[0][j] = u[i];
	D->data[1][j] = 1.; /* FIXME HARD-CODED */
      } else if (E->dependent_variable == 2) {
	D->data[0][j] = v[i];
	D->data[1][j] = 1.; /* FIXME HARD-CODED */
      } else if (!E->dependent_variable) {
	D->data[0][j] = dv[i];
	D->data[1][j] = dv_unc[i];
      }
      D->data[2][j] = (float)(dtime[i] - 1400000000);
      if (E->use_wind) {
	D->data[3][j] = u[i];
	D->data[4][j] = v[i];
      }
      /* Times were already processed for this
	 FIXME SHOULD DEFINITELY BE MADE SOMEHOW MORE ELEGANT */
      if (E->mode) {D->data[2][j] += 1400000000;}
      D->lat[j] = lat[i];
      D->lon[j] = lon[i];
      j++;
    }
  }
  D->ngobs = j;
  printf("Added %zu/%zu observations.\n", j,i);
  /* Deallocate the arrays that are not returned in *data */
  free(lat);
  free(lon);
  free(qf);
  free(dv);
  free(dv_unc);
  free(dtime);
  if (E->use_wind) {
    free(u);
    free(v);
  }
  return D;
}

void latlon_to_xyz(float lat, float lon, double *x, double *y, double *z)  {

  float phi, theta;

  theta = (90 - lat)/180*M_PI;
  phi = lon/360*2*M_PI;
  *x = (double) sinf(theta)*cosf(phi);
  *y = (double) sinf(theta)*sinf(phi);
  *z = (double) cosf(theta);
}

void xyz_to_latlon(float x, float y, float z, float *lat, float *lon) {

  float theta, phi;

  theta = acosf(z);
  phi = xy_to_phi(x,y);
  *lat = 90 - theta*180/M_PI;
  /* lon wanted between 0 and 360, like E_mf->minlon. The + sign is
     needed to make argument to fmod positive (+ 180 chosen instead
     of - 180)*/
  *lon = fmodf(phi*360./2./M_PI + 180., 360.);
}

void latlon_to_latidxlonidx(float lat, float lon, uint *latidx, uint *lonidx, struct config *refconfig) {
  /* Transforms lat-lon coordinates into latidx and lonidx values,
     returning the index that is closest to the point. Reference grid
     config in refconfig holds the information about the target
     grid. */

  assert(refconfig->mode == 0); /* Refconfig needs to be a grid specification */

  *latidx = (uint) ((lat - refconfig->lat[0])/refconfig->gridres + 0.4999);
  /* Now lon and refconfig->minlon are between 0 and 360 but refconfig->lon is still
     between -180 and 180*/
  *lonidx = (uint) ((lon - refconfig->lon[0])/refconfig->gridres + 0.4999);

  /* If the lat-lon value is outside the grid limits, the closest
     point within the grid will be returned.
  */
  if ((*latidx > refconfig->nlat-1) || (*lonidx > refconfig->nlon-1) || (*latidx < 0) || (*lonidx < 0)) {
    if (DEBUG) {
      printf("BEFORE CLIPPING: lat, lon, latidx, lonidx, minlat, minlon: %f, %f, %i, %i, %f, %f\n",
	     lat, lon, *latidx, *lonidx, refconfig->minlat, refconfig->minlon);
    }
    *latidx = *latidx > refconfig->nlat - 1 ? refconfig->nlat - 1 : *latidx;
    *lonidx = *lonidx > refconfig->nlon - 1 ? refconfig->nlon - 1 : *lonidx;
    *latidx = *latidx < 0 ? 0 : *latidx;
    *lonidx = *lonidx < 0 ? 0 : *lonidx;
    if (DEBUG) {
      printf("CLIPPED TO     : lat, lon, latidx, lonidx, minlat, minlon: %f, %f, %i, %i, %f, %f\n",
	     lat, lon, *latidx, *lonidx, refconfig->minlat, refconfig->minlon);
    }
  }
}

void add_datapoint_to_S(float *lat, float *lon, float *t, float *dv,
			 float *dv_unc, float *u, float *v,
			 struct config *E, struct state *S) {

  /* Adds data to state struct S. */
  assert(!E->mode);

  uint latidx, lonidx;
  double x, y, z;
  int i, j, dn;
  size_t gi, r, oi;


  int max_gridlatidx, min_gridlatidx, max_gridlonidx, min_gridlonidx;
  min_gridlatidx = S->gi0/E->nlon;
  max_gridlatidx = S->gi1/E->nlon;
  min_gridlonidx = 0; /* Assume that we go through at least a full longitude per sweep */
  max_gridlonidx = E->nlon - 1;

  dn = daynumber_from_t(*t);
  latlon_to_latidxlonidx(*lat, *lon, &latidx, &lonidx, E);
  if ((latidx > max_gridlatidx + E->max_rad_equator_degrees/E->gridres) ||
      (latidx < min_gridlatidx - E->max_rad_equator_degrees/E->gridres) ||
      (lonidx > max_gridlonidx) || (lonidx < min_gridlonidx)) {
    return;
  }

  if (S->nobs == S->allocated_for_all_obs) {
    reallocate_S(S, E, E->malloc_chunk);
  }

  oi = S->nobs++; /* index of this observation, note the ++ */
  latlon_to_xyz(*lat, *lon, &x, &y, &z);

  /* Add the datapoint to S here, references to it are added below in
     a for-loop */
  S->x[oi] = x;
  S->y[oi] = y;
  S->z[oi] = z;
  S->dv[oi] = *dv;
  S->dv_unc[oi] = *dv_unc;
  S->t[oi] = *t;
  if (E->use_wind) {
    S->u[oi] = *u;
    S->v[oi] = *v;
  }

  i = latidx;
  j = lonidx;
  r = i*E->nlon + j;

  // if ((r<S->gi0) || (r>=S->gi1)) {printf("error?");} /* Not in region of current gi batch */
  gi = r%E->gi_inc;
  /* Get more space if needed */
  // printf("%zu/%zu: %zu, %zu\n", gi, dn, S->griddaycounts[gi][dn], S->allocated[gi][dn]);
  if (!(S->griddaycounts[gi][dn] - S->allocated[gi][dn])) {
    size_t newsize = S->allocated[gi][dn] + E->malloc_chunk;
    size_t ns = newsize*sizeof(size_t);
    S->gridobsind[gi][dn] = (size_t*) reallocate_S_element(S->gridobsind[gi][dn], ns);
    S->allocated[gi][dn] = newsize;
  }
  /* Add data reference */
  S->gridobsind[gi][dn][S->griddaycounts[gi][dn]] = oi;
  S->griddaycounts[gi][dn]++;
}

int compare(const size_t *i1, const size_t *i2, float *arr) {
  return (arr[*i1] > arr[*i2]) - (arr[*i1] < arr[*i2]);
}

int compare_inverse(const void * restrict i1, const void * restrict i2, void * restrict arr) {
  /* For sorting with qsort in descending order */
  size_t * ii1 = (size_t*) i1;
  size_t * ii2 = (size_t*) i2;
  float * aarr = (float*) arr;

  return (aarr[*ii1] < aarr[*ii2]) - (aarr[*ii1] > aarr[*ii2]);
}

int isprime(size_t N) {
  size_t i;
  for (i=2; i<(int)sqrt(N) + 1; i++) {
    if (!(N%i)) {
      return 0;
    }
  }
  return 1;
}

size_t get_largest_prime_under(size_t N) {
  /* Returns largest prime number of size max N-1 */
  N--;
  while (!isprime(N)) {
    N--;
  }
  return N;
}

void thin_data_double(double *arr, size_t ndata_before, size_t ndata_after) {

  /* j is the order number of the picked observations */
  size_t i, j;

  /* Sanity checks */
  assert(ndata_before >= ndata_after);
  if (ndata_before == ndata_after) {return;}

  j = 0;
  for (i=0; i<ndata_before; i++) {
    /* Take ndata_after observations evenly spaced from the range */
    if (i == ((ndata_before*j)/ndata_after)) {
      arr[j] = arr[i];
      j++;
    }
  }
}

void thin_data_size_t(size_t *arr, size_t ndata_before, size_t ndata_after) {

  /* j is the order number of the picked observations */
  size_t i, j;

  /* Sanity checks */
  assert(ndata_before >= ndata_after);
  if (ndata_before == ndata_after) {return;}

  j = 0;
  for (i=0; i<ndata_before; i++) {
    /* Take ndata_after observations evenly spaced from the range */
    if (i == ((ndata_before*j)/ndata_after)) {
      arr[j] = arr[i];
      j++;
    }
  }
}

void print_config(struct config *E) {
  /* Print information about the experiment */

  printf("====Gaussian process configuration===\n");
  printf("Area min latitude: %f\n", E->minlat);
  printf("Area max latitude: %f\n", E->maxlat);
  printf("Area min longitude: %f\n", E->minlon);
  printf("Area min longitude: %f\n", E->maxlon);
  printf("Grid resolution: %f degrees\n", E->gridres);
  printf("Number of latitudes, longitudes, and gridpoints: %zu, %zu, %zu\n",
	 E->nlat, E->nlon, E->nlat * E->nlon);
  printf("Number of grid points calculated per data reading: %i\n", E->gi_inc);
  printf("Experiment number of days: %zu\n", E->maxdays);
  printf("Gaussian process min. covariance: %f\n", E->min_cov);
  printf("Max time window for observations is hence %f days.\n", 2*E->max_tdist+1);
  printf("Gaussian process corresponding radius on unit ball: %f\n", E->max_rad);
  printf("Memory allocation chunk size: %zu\n", E->malloc_chunk);
  printf("Obs dist in E: %f\n", E->obs_dist);
}

void print_vector(void *v, int N, char *vectorname, int datatype) {
  /* Prints the vector v of length N in a human readable form.*/
  int j;
  double *vv = (double*) v;

  printf("----- START VECTOR %s -----\n", vectorname);
  for (j=0; j<N; j++) {
    printf("%f ", vv[j]);
  }
  printf("\n");
  printf("----- END VECTOR %s -----\n", vectorname);
}

void print_matrix(void *K, int nrows, int ncols, char *matrixname, int python_format) {
  /* Prints the row major matrix K in a human readable form - the
     matrix is nrows x ncols. matrixname is a string describing what
     matrix we are printing. If python_format is non-zero, then a
     numpy-copy-pasteable array is printed. Otherwise, a raw text in
     nrows x ncols format is printed. */

  float *k = (float*) K;
  int i, j;

  if (python_format) {
    printf("----- START MATRIX %s = np.array([\n", matrixname);
  } else {
    printf("----- START MATRIX %s -----\n", matrixname);
  }
   for (i=0; i<nrows; i++) {
    for (j=0; j<ncols; j++) {
      printf("%f, ", k[i*ncols+j]);
    }
    if (!python_format) {
      printf("\n");
    }
  }
  if (python_format) {
    printf("]).reshape((%d, %d)) END MATRIX %s -----\n", nrows, ncols, matrixname);
  } else {
    printf("----- END MATRIX %s -----\n", matrixname);
  }
}

void write_1d_array_to_txt(char *fname, void *arr, int nrows, size_t ncols, int datatype) {
  /* FIXME misleading function name.  The 1d refers to that arr is
     just an array of floats, not an array of pointers to pointer
     arrays. With nrows 1, a 1-d array can be written, otherwise it
     actually writes nrows elements of size ncols.
     
     FIXME the datatype variable does nothing
  */
  size_t i, j;
  FILE *f = fopen(fname, "a");
  float *a = (float*) arr;

  if (f==NULL) {
    printf("Opening output file failed.\n");
    exit(4);
  }

  for (i=0; i<nrows; i++) {
    for (j=0; j<ncols; j++) {
      fprintf(f, "%f ", a[i*ncols+j]);
    }
    fprintf(f, "\n");
  }
  fclose(f);
}


void write_1d_array_to_txt_double(char *fname, void *arr, int ndays, size_t ngp, int datatype) {
  /* FIXME FIGURE _Generic use for proper overloading.*/
  size_t i, j;
  FILE *f = fopen(fname, "a");
  double *a = (double*) arr;

  if (f==NULL) {
    printf("Opening output file failed.\n");
    exit(4);
  }

  for (i=0; i<ndays; i++) {
    for (j=0; j<ngp; j++) {
      fprintf(f, "%g ", a[i*ngp+j]);
    }
    fprintf(f, "\n");
  }
  fclose(f);
}

void write_2d_array_to_txt(char *fname, float **arr, int ndays, size_t ngp, int datatype) {
  /* Fixme the datatype variable does nothing */

  size_t i, j;
  FILE *f = fopen(fname, "w");

  if (f==NULL) {
    printf("Opening output file failed.\n");
    exit(3);
  }
  for (i=0; i<ndays; i++) {
    for (j=0; j<ngp; j++) {
      fprintf(f, "%f ", arr[j][i]);
    }
    fprintf(f, "\n");
  }
  fclose(f);
}

void save_chosen_daydata_to_txt(struct daydata *D, int dn) {

  size_t i;

  /* Construct the file name */
  char fname[120] = "daydatas/day_";
  char dns[8]; /* daynumber string */
  sprintf(dns, "%d", dn);
  strcat(fname, dns);
  strcat(fname, ".txt");
  if (DEBUG) {printf("Saving observations from file %s\n", fname);}

  FILE *f = fopen(fname, "w");

  if (f==NULL) {
    printf("Opening output file failed.\n");
    exit(5);
  }

  for (i=0; i<D->ngobs; i++) {
    fprintf(f, "%f ", D->lat[i]);
    fprintf(f, "%f ", D->lon[i]);
    fprintf(f, "%f ", D->data[0][i]);
    fprintf(f, "%f ", D->data[1][i]);
    fprintf(f, "%f ", D->data[2][i]);
    fprintf(f, "\n");
  }

  fclose(f);
}


float *read_2d_array_from_txt(char *fname, size_t ngp, int ndays) {
  /* Read a 2d array for whatever purpose: size is days*ngp */

  size_t i,j;

  float *arr = malloc(ngp*ndays*sizeof(float));
  FILE *f = NULL;

  f = fopen(fname, "r");
  if (!f) {
    printf("EXITING, FAILED TO READ ARRAY!\n");
    exit(-5);
  }

  for (i=0; i<ndays; i++) {
    for (j=0; j<ngp; j++) {
      fscanf(f, "%f", &arr[i*ngp+j]);
      if (DEBUG) {
	if (1) {
	  printf("read item %zu = %f\n", i*ngp+j, arr[i*ngp+j]);
	}
      }
    }
  }
  fclose(f);

  return arr;
}

float *read_1d_array_from_txt(char *fname, size_t ndata) {
  /* Read a 2d array for whatever purpose: size is days*ngp */

  float *arr;
  arr = read_2d_array_from_txt(fname,  ndata, 1);
  return arr;
}

int *read_daylist(char *fname, int *ndays) {

  const size_t limit = 1000000;
  int *arr = malloc(limit*sizeof(int));
  int ret;
  int c, d;

  FILE *f = NULL;

  d = 0;
  f = fopen(fname, "r");
  printf("Reading daylist: ");
  while (d < limit) {
    ret = fscanf(f, "%d", &c);
    printf("%d ", c);
    if (ret == EOF) {
      break;
    }
    arr[d++] = c;
  }
  printf("\n");
  *ndays = d;
  fclose(f);
  arr = realloc(arr, d*sizeof(int));
  return arr;
}

void gaussian_convolve_1darray(float *arr, int stride, size_t firstindex,
			       float width, size_t axislength) {
  /* convolves the input 1d array with a Gaussian, with
     sigma=width. The convolution will take into consideration
     "axislength" number of elements at intervals "stride" starting at
     "firstindex". In python the array in question would be
     arr[firstindex:firstindex + axislength*stride][::stride]. The
     code is written so, that it preserves edges by always normalizing
     the kernel.*/

  const int l = 1 + 2*((int) width*3);
  int i, k;
  int j0, j1;
  float s, p;
  float ker[l];
  float res[axislength];

  for (i=0; i<l; i++) {
    ker[i] = exp(-.5*pow(((float)i - l/2), 2)/width/width);
  }

  for (i=0; i<axislength; i++) {
    j0 = (l/2-i < 0) ? 0 : l/2-i;
    j1 = (axislength - i + l/2 > l) ? l : axislength - i + l/2;
    s = 0;
    p = 0; /* For normalization */
    for (k=j0; k<j1; k++) {
      s += ker[k]*arr[firstindex + (i + k - l/2)*stride];
      p += ker[k];
    }
    res[i] = s/p;
  }

  for (i=0; i<axislength; i++) {
    arr[firstindex + i*stride] = res[i];
  }

  /* Python 3 version of the code would be:
  for i in range(axislength):
    j0 = max(0, l//2-i)
    j1 = min(l, 100 - i + l//2)
    s = 0
    p = 0
    for k in range(j0, j1):
	s += ker[k]*x[i+k-l//2]
	p += ker[k]
    y[i] = s/p
  */

}

void gaussian_convolve_array(float *arr, int nlat, int nlon, int ndays,
			     float width_t, float width_lat, float width_lon) {

  /* Convolves the input array, with dimensions (time, lat, lon), with
     the Gaussian kernel with sigma "width_x". If some of the width
     arguments is zero, then convolution with respect to that axis is
     not performed. */

  size_t gi, i, j;

  if (width_t) {
    printf("Convolving array along time axis using a Gaussian kernel with sigma %f...", width_t);
    for (gi=0; gi<nlat*nlon; gi++) {
      gaussian_convolve_1darray(arr, nlat*nlon, gi, width_t, ndays);
    }
    printf("done!\n");
  }
  if (width_lat) {
    printf("Convolving array along latitudes using a Gaussian kernel with sigma %f...", width_lat);
    for (i=0; i<ndays; i++) {
      for (j=0; j<nlon; j++) {
	  gaussian_convolve_1darray(arr, nlon, i*nlat*nlon + j, width_lat, nlat);
      }
    }
    printf("done!\n");
  }
  if (width_lon) {
    printf("Convolving array along longitudes using a Gaussian kernel with sigma %f...", width_lon);
    for (i=0; i<ndays; i++) {
      for (j=0; j<nlat; j++) {
	  gaussian_convolve_1darray(arr, 1, i*nlat*nlon + j*nlon, width_lon, nlon);
      }
    }
    printf("done!\n");
  }
}


float interp2d(float x, float y, float x1, float y1, float x2, float y2,
	       float v1, float v2, float v3,  float v4) {
  /* Bilinear interpolation to obtain the value at location (x,y), by
     first interpolating wrt x axis and then to y. The values at (x1,
     y1)...(x2,y2) correspond to the values at the corners of the
     rectangle around the point (x,y).*/

  float tmp1, tmp2, res;

  tmp1 = (x2 - x)/(x2 - x1)*v1 + (x - x1)/(x2 - x1)*v2;
  tmp2 = (x2 - x)/(x2 - x1)*v3 + (x - x1)/(x2 - x1)*v4;
  res = (y2 - y)/(y2 - y1)*tmp1 + (y - y1)/(y2 - y1)*tmp2;

  return res;
}

float fillmeans(float *v1, float *v2, float *v3, float *v4) {
  /* Returns the average of the nonzero arguments. If everything is
     zero, we return zero. */
  int n = 4;
  n -= (!*v1) + (!*v2) + (!*v3) + (!*v4);
  return (n) ? (*v1 + *v2 + *v3 + *v4)/n : 0;
}


float interp_latloncoords(float lat, float lon, float *data, struct config *E) {
  /* Finds the interpolated values corresponding to spatial location
     (lat, lon) in *data using information of the grid available in E. */

  float latidx_fract, lonidx_fract, res;
  int gi1, gi2, gi3, gi4;
  int latidx0, latidx1, lonidx0, lonidx1;


  /* The interpolation fails if we are outside the grid, since the
     indexes to E.lon and/or E.lat will go out of bounds and return
     garbage. Therefore we tune the lat and lon to be just barely
     inside the grid in those corner cases. This amounts to
     interpolating just based on those values then. */
  // printf("lon, lat: %f, %f\n", lon, lat); fflush(stdout);
  lon = fmin(lon, E->lon[E->nlon - 1] - 1e-3);
  lon = fmax(lon, E->lon[0] + 1e-3);
  lat = fmin(lat, E->lat[E->nlat - 1] - 1e-3);
  lat = fmax(lat, E->lat[0] + 1e-3);

  latidx_fract = (lat - E->lat[0])/E->gridres;

  /* E->minlon is 0..360, and that's why the +180 */
  lonidx_fract = (lon - E->lon[0])/E->gridres;

  latidx0 = floorf(latidx_fract);
  latidx1 = latidx0 + 1;
  lonidx0 = floorf(lonidx_fract);
  lonidx1 = lonidx0 + 1;

  /* grid indexes of the corners */
  gi1 = E->nlon*latidx0 + lonidx0;
  gi2 = E->nlon*latidx0 + lonidx1;
  gi3 = E->nlon*latidx1 + lonidx0;
  gi4 = E->nlon*latidx1 + lonidx1;

  float a1, a2, a3, a4;
  a1 = data[gi1];
  a2 = data[gi2];
  a3 = data[gi3];
  a4 = data[gi4];

  /* FIXME still (maybe not any more?)  gives nans, don't know why */
  if (!isnormal(a1)) {a1 = 0;}
  if (!isnormal(a2)) {a2 = 0;}
  if (!isnormal(a3)) {a3 = 0;}
  if (!isnormal(a4)) {a4 = 0;}

  if (!a1 || !a2 || !a3 || !a4) {
    if (DEBUG) {
      printf("We have zeros: ");
      printf("interpolate_xyz v1-4: \n%f, %f, %f, %f\n", a1, a2, a3, a4);
    }
    a1 = (a1) ? a1 : fillmeans(&a1, &a2, &a3, &a4); //? a3 : a1;
    a2 = (a2) ? a2 : fillmeans(&a1, &a2, &a3, &a4); //? a3 : a1;
    a3 = (a3) ? a3 : fillmeans(&a1, &a2, &a3, &a4); //? a3 : a1;
    a4 = (a4) ? a4 : fillmeans(&a1, &a2, &a3, &a4); //? a3 : a1;

    if (DEBUG) {
      printf("Corrected: \n%f, %f, %f, %f\n", a1, a2, a3, a4);
    }
  }

  res = interp2d(lon, lat,
		 E->lon[lonidx0], E->lat[latidx0], E->lon[lonidx1], E->lat[latidx1],
		 a1, a2, a3, a4);
  return res;
}


float interp_xyzcoords(float x, float y, float z, float *data, struct config *E) {
  /* Finds the interpolated values corresponding to spatial location
     (x,y,z) in *data using information of the grid available in E. */

  float theta, phi, lat, lon;

  theta = acosf(z);
  phi = xy_to_phi(x,y);

  lat = 90 - theta*180/M_PI;

  /* Will give points -180...+180, so Japan is close to 150... */
  lon = phi*360/2/M_PI;

  return interp_latloncoords(lat, lon, data, E);
}


float *create_interpolated_array(float *arr, uint nfields, struct config *E_from, struct config *E_to) {
  /* Interpolates data in *arr from a grid specification E_from to
     E_to. Returns a pointer to a newly created array.

     The second argument, nfields, is number of 2-d fields in arr to
     interpolate. For instance, for 4 beta coefficients one would set
     it to 4 so that interpolating needs to be done only once. For a
     single 2d-field this would be one.

     The nfield controls the outermost loop of the iteration, namely,
     indexes 0..E_from->ngp are first field etc.
*/

  float *newarr = malloc(E_to->ngp*sizeof(float));
  size_t i, j;
  for (j=0; j<nfields; j++) {
    for (i=0;i<E_to->ngp;i++) {
      newarr[i] = interp_latloncoords(E_to->lat[i], E_to->lon[i], arr, E_from);
    }
  }
  return newarr;
}

void fill_E_testdata_randomly(struct config *E, int max_days) {
  /* Fills random data to E->xyzt - these are meant to be reference
     points for balls within which likelihood function is evaluated,
     to check whether the chosen covariance parameters explain the
     data. So, these are centres of balls, around which point clouds
     are chosen. Data will be filled according to grid spec in E.*/

  int i;
  float lat, lon;
  double x, y, z;

  float *savearr;
  savearr = malloc(E->ngp*4*sizeof(float));

  for (i=0; i<E->ngp; i++) {
    lat = 1.*random()/RAND_MAX*(E->maxlat - E->minlat) + E->minlat;
    lon = 1.*random()/RAND_MAX*(E->maxlon - E->minlon) + E->minlon - 180.;
    latlon_to_xyz(lat, lon, &x, &y, &z);
    E->x[i] = savearr[i*4] = x;
    E->y[i] = savearr[i*4 + 1] = y;
    E->z[i] = savearr[i*4 + 2] = z;
    E->lat[i] = (double) lat;
    E->lon[i] = (double) lon;
    E->t[i] = savearr[i*4 + 3] = 1.*random()/RAND_MAX*24*3600*max_days + 10004800 - 43200;
  }
  write_1d_array_to_txt("refpoints.txt", savearr, E->ngp, 4, 1);
  free(savearr);
}


float normal(float mu, float sigma, struct boxmullerstruct *BM) {
  /* Box-muller algorithm; generate normal rv's with mean mu and std
     sigma */
  if (BM->counter--) {
    return sigma*BM->x + mu;
  } else {
    float u1 = 1.*random()/RAND_MAX;
    float u2 = 1.*random()/RAND_MAX;
    BM->x = sqrt(-2*log(u1))*sin(2*M_PI*u2);
    BM->counter++;
    return sigma*sqrt(-2*log(u1))*cos(2*M_PI*u2) + mu;
  }
}

void test_normal(float mu, float sigma, size_t N) {
  /* Generate N normals with mean mu and std sigma just to make sure
     we get normal rands. Sampling normals seems to work just fine;
     draw e.g histogram with test_normal(-2, 4, 10000000); */

  float *t = malloc(N*sizeof(float));
  struct boxmullerstruct *BM = initialize_boxmullerstruct();

  for (int i=0; i<N; i++) {
    t[i] = normal(mu, sigma, BM);
  }
  write_1d_array_to_txt("test_boxmuller.txt", t, 1, N, 1);
  free(t);
  free(BM);
}


/* Python version of the sample covariance calculation, written in
   C-like code, just for checking...

    def get_propcov(arr, npar, niter):
	m = zeros(npar)
	for i in range(niter):
	    for j in range(npar):
		m[j] += arr[i*npar + j]/niter
	C = zeros(npar*npar)
	for i in range(niter):
	    for j in range(npar):
		for k in range(npar):
		    C[j*npar + k] += (arr[i*npar + j] - m[j])*(arr[i*npar + k] - m[k])/(niter - 1)
	print(reshape(C, (npar, npar)))
	print(cov(reshape(arr, (niter, npar)).T)) */


void sample_covariance(double *C, double *data, uint npar, size_t ndata, double scaling) {
  /* Calculates the (npar x npar) sample covariance of a sample of
     size ndata of vectors npar. The array data has dimensions (ndata,
     npar). The parameter scaling can be used if the output is wanted
     to be scaled, like when calculating proposal covariance for the
     AM algorithm. */

  size_t i, j, k;
  double *mean = calloc(npar, sizeof(double));

  for (i=0; i<npar; i++) {
    mean[i] = 0;
    for (j=0; j<npar; j++) {
      C[i*npar + j] = 0;
    }
  }

  /* Construct sample mean vector */
  for (i=0; i<ndata; i++) {
    for (j=0; j<npar; j++) {
      mean[j] += data[i*npar + j]/ndata;
    }
  }
  for (i=0; i<ndata; i++) {
    for (j=0; j<npar; j++) {
      for (k=0; k<npar; k++) {
	C[j*npar + k] += scaling*(data[i*npar + j] - mean[j])*(data[i*npar + k] - mean[k])/(ndata - 1);
      }
    }
  }

  free(mean);
}
