#include "wirth_select.h"
#include <math.h>
#include <stdlib.h>

void compute_sigma_clipped_bounds(double data_buffer[], int count, int use_median,
                                  int use_mad_std, int maxiters, double sigma_lower,
                                  double sigma_upper, double *lower_bound,
                                  double *upper_bound, double  mad_buffer[]) {

  double mean, std, median = 0, cen;
  int i, new_count, iteration = 0;

  while (1) {

    if (use_median || use_mad_std) {
      median = wirth_median(data_buffer, count);
    }

    // Note that we don't use an else clause here, because the mean
    // might be needed even if use_median is used, but use_mad_std is not.
    if (!use_median || !use_mad_std) {
      mean = 0;
      for (i = 0; i < count; i++) {
        mean += data_buffer[i];
      }
      mean /= count;
    }

    if (use_median) {
      cen = median;
    } else {
      cen = mean;
    }

    if (use_mad_std) {

      for (i = 0; i < count; i++) {
        mad_buffer[i] = fabs(data_buffer[i] - median);
      }
      std = wirth_median(mad_buffer, count) * 1.482602218505602;

    } else {

      std = 0;
      for (i = 0; i < count; i++) {
        std += (data_buffer[i] - mean) * (data_buffer[i] - mean);
      }
      std = sqrt(std / count);

    }

    *lower_bound = cen - sigma_lower * std;
    *upper_bound = cen + sigma_upper * std;

    // We now exclude values from the buffer using these
    // limits and shift values so that we end up with a
    // packed array of 'valid' values
    new_count = 0;
    for (i = 0; i < count; i++) {
      if (data_buffer[i] >= *lower_bound && data_buffer[i] <= *upper_bound) {
        data_buffer[new_count] = data_buffer[i];
        new_count += 1;
      }
    }

    if (new_count == count)
      return;

    count = new_count;

    iteration += 1;

    if (maxiters != -1 && iteration >= maxiters)
      return;
  }
}