/* Functions to aid with optimization algorithms etc.*/

float get_starting_point(float low, float high, int mode) {
  /* See get_cfc_idx_and_limits() */
  return (mode == 1) ?  low + 1.*random()/RAND_MAX*(high - low) : (high + low)/2. ;
}

void get_cfc_idx_and_limits(struct covfunconfig *cfc, uint *npars, uint *argidx,
			    float *x_low, float *x_high, int x0mode, float *x0) {
  /*
    This is a setup routine for optimization, MCMC, etc. Given a
    compound kernel *cfc, it sets the following variables:

    npars: Number of parameters whose upper and lower limits differ,
    meaning that they will be part of any optimization / estimation
    run.

    argidx: a list of integers such that the parameter vector from an
    optimization algorithm would be unpacked to those indexes of a
    full array (containing all possible indexes, see
    get_current_parameter_vector() for format). Also see
    set_values_in_argidx() below.

    x_low, x_high: low and high limits of the npar parameters that are
    getting calibrated.

    x0: A starting point proposal: If x0mode == 0, the current value
    in cfc is returned, if x0mode == 1, a random value in the interval
    is returned, and else the middle point of the interval between
    x_high and x_low is returned.
  */
  int i;
  int ind = 0;

  assert(cfc->covftype < 0); /* Ensure we have a compound kernel */

  /* argidx = calloc(-cfc->covftype*6, sizeof(uint)); */
  /* x_low = calloc(-cfc->covftype*6, sizeof(float)); */
  /* x_high = calloc(-cfc->covftype*6, sizeof(float)); */
  /* x0 = calloc(-cfc->covftype*6, sizeof(float)); */

  for (i=0; i<-cfc->covftype; i++) {
    if (cfc->kernels[i]->tau_high - cfc->kernels[i]->tau_low) {
      x_low[ind] = cfc->kernels[i]->tau_low;
      x_high[ind] = cfc->kernels[i]->tau_high;
      x0[ind] =  (!x0mode) ? cfc->kernels[i]->tau :
	get_starting_point(x_low[ind], x_high[ind], x0mode);
      argidx[ind++] = 0 + 6*i;
    }
    if (cfc->kernels[i]->llat_high - cfc->kernels[i]->llat_low) {
      x_low[ind] = cfc->kernels[i]->llat_low;
      x_high[ind] = cfc->kernels[i]->llat_high;
      x0[ind] = (!x0mode) ? cfc->kernels[i]->llat :
	get_starting_point(x_low[ind], x_high[ind], x0mode);
      argidx[ind++] = 1 + 6*i;
    }
    if (cfc->kernels[i]->llon_high - cfc->kernels[i]->llon_low) {
      x_low[ind] = cfc->kernels[i]->llon_low;
      x_high[ind] = cfc->kernels[i]->llon_high;
      x0[ind] = (!x0mode) ? cfc->kernels[i]->llon :
	get_starting_point(x_low[ind], x_high[ind], x0mode);
      argidx[ind++] = 2 + 6*i;
    }
    if (cfc->kernels[i]->lt_high - cfc->kernels[i]->lt_low) {
      x_low[ind] = cfc->kernels[i]->lt_low;
      x_high[ind] = cfc->kernels[i]->lt_high;
      x0[ind] = (!x0mode) ? cfc->kernels[i]->lt :
	get_starting_point(x_low[ind], x_high[ind], x0mode);
      argidx[ind++] = 3 + 6*i;
    }
    if (cfc->kernels[i]->lperiodic_high - cfc->kernels[i]->lperiodic_low) {
      x_low[ind] = cfc->kernels[i]->lperiodic_low;
      x_high[ind] = cfc->kernels[i]->lperiodic_high;
      x0[ind] = (!x0mode) ? cfc->kernels[i]->lperiodic :
	get_starting_point(x_low[ind], x_high[ind], x0mode);
      argidx[ind++] = 4 + 6*i;
    }
    if (cfc->kernels[i]->rho_high - cfc->kernels[i]->rho_low) {
      x_low[ind] = cfc->kernels[i]->rho_low;
      x_high[ind] = cfc->kernels[i]->rho_high;
      x0[ind] = (!x0mode) ? cfc->kernels[i]->rho :
	get_starting_point(x_low[ind], x_high[ind], x0mode);
      argidx[ind++] = 5 + 6*i;
    }
  }
  *npars = ind;
}

float *get_current_full_parameter_vector(struct covfunconfig *cfc) {
  /* Returns the full parameter vector of a compound kernel that can
     be changed according to any values that we want to change and
     then applied with set_kernel_parameters(). This means that the
     length is always -6 * cfc->covftype. Note that cfc needs to be a
     compound kernel, as usual. */

  int i;
  int ind = 0;
  float *current_pars = calloc(-6*cfc->covftype, sizeof(float));

  assert(cfc->covftype < 0); /* Only compound kernels accepted */

  for (i=0; i<-cfc->covftype; i++) {
    current_pars[ind++] = cfc->kernels[i]->tau;
    current_pars[ind++] = cfc->kernels[i]->llat;
    current_pars[ind++] = cfc->kernels[i]->llon;
    current_pars[ind++] = cfc->kernels[i]->lt;
    current_pars[ind++] = cfc->kernels[i]->lperiodic;
    current_pars[ind++] = cfc->kernels[i]->rho;
  }
  return current_pars;
}

void set_cfpars_based_on_argidx(float *x, uint *argidx, uint npars, struct config *E) {
  /* Sets values of covariance parameters in the kernel of config E
     based on the positions in argindex. The function also sets the
     maximum radius-parameters that depend on covariance parameters
     and are used in picking observations when creating a
     cholstruct. */
  int i;

  /* Get the parameter vector containing all the possible parametes,
     including those that are not to be changed. This form is utilized
     when using set_kernel_parameters */
  float *x_full = get_current_full_parameter_vector(E->cfc);

  /* Set values in x into the appropriate index in the full vector */
  for (i=0; i<npars; i++) {
    x_full[argidx[i]] = x[i];
  }

  for (i=0; i<-E->cfc->covftype; i++) {
    set_kernel_parameters(E->cfc->kernels[i], x_full[0+i*6], x_full[1+i*6],
			  x_full[2+i*6], x_full[3+i*6], x_full[4+i*6], x_full[5+i*6]);
  }
  set_max_distances_in_config(E);
}

/* Trivial helper functions for dealing with parameter limits so that
   we can do unconstrained/constrained optimization as wanted */

double logit(double x) {
  return log(x/(1.-x));
}

double expit(double x) {
  return 1./(1+exp(-x));
}

double reals_to_par_range(double x, double hlim, double llim) {
  return expit(x)*(hlim-llim)+llim;
}

double par_range_to_reals(double x, double hlim, double llim) {
  return logit((x - llim)/(hlim-llim));
}
