/*
  chronyd/chronyc - Programs for keeping computer clocks accurate.

 **********************************************************************
 * Copyright (C) Miroslav Lichvar  2009-2011, 2014, 2016, 2018
 * 
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of version 2 of the GNU General Public License as
 * published by the Free Software Foundation.
 * 
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, write to the Free Software Foundation, Inc.,
 * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 * 
 **********************************************************************

  =======================================================================

  Routines implementing a median sample filter.

  */

#include "config.h"

#include "local.h"
#include "logging.h"
#include "memory.h"
#include "regress.h"
#include "samplefilt.h"
#include "util.h"

#define MIN_SAMPLES 1
#define MAX_SAMPLES 256

struct SPF_Instance_Record {
  int min_samples;
  int max_samples;
  int index;
  int used;
  int last;
  int avg_var_n;
  double avg_var;
  double max_var;
  double combine_ratio;
  NTP_Sample *samples;
  int *selected;
  double *x_data;
  double *y_data;
  double *w_data;
};

/* ================================================== */

SPF_Instance
SPF_CreateInstance(int min_samples, int max_samples, double max_dispersion, double combine_ratio)
{
  SPF_Instance filter;

  filter = MallocNew(struct SPF_Instance_Record);

  min_samples = CLAMP(MIN_SAMPLES, min_samples, MAX_SAMPLES);
  max_samples = CLAMP(MIN_SAMPLES, max_samples, MAX_SAMPLES);
  max_samples = MAX(min_samples, max_samples);
  combine_ratio = CLAMP(0.0, combine_ratio, 1.0);

  filter->min_samples = min_samples;
  filter->max_samples = max_samples;
  filter->index = -1;
  filter->used = 0;
  filter->last = -1;
  /* Set the first estimate to the system precision */
  filter->avg_var_n = 0;
  filter->avg_var = SQUARE(LCL_GetSysPrecisionAsQuantum());
  filter->max_var = SQUARE(max_dispersion);
  filter->combine_ratio = combine_ratio;
  filter->samples = MallocArray(NTP_Sample, filter->max_samples);
  filter->selected = MallocArray(int, filter->max_samples);
  filter->x_data = MallocArray(double, filter->max_samples);
  filter->y_data = MallocArray(double, filter->max_samples);
  filter->w_data = MallocArray(double, filter->max_samples);

  return filter;
}

/* ================================================== */

void
SPF_DestroyInstance(SPF_Instance filter)
{
  Free(filter->samples);
  Free(filter->selected);
  Free(filter->x_data);
  Free(filter->y_data);
  Free(filter->w_data);
  Free(filter);
}

/* ================================================== */

/* Check that samples times are strictly increasing */

static int
check_sample(SPF_Instance filter, NTP_Sample *sample)
{
  if (filter->used <= 0)
    return 1;

  if (UTI_CompareTimespecs(&filter->samples[filter->last].time, &sample->time) >= 0) {
    DEBUG_LOG("filter non-increasing sample time %s", UTI_TimespecToString(&sample->time));
    return 0;
  }

  return 1;
}

/* ================================================== */

int
SPF_AccumulateSample(SPF_Instance filter, NTP_Sample *sample)
{
  if (!check_sample(filter, sample))
      return 0;

  filter->index++;
  filter->index %= filter->max_samples;
  filter->last = filter->index;
  if (filter->used < filter->max_samples)
    filter->used++;

  filter->samples[filter->index] = *sample;

  DEBUG_LOG("filter sample %d t=%s offset=%.9f peer_disp=%.9f",
            filter->index, UTI_TimespecToString(&sample->time),
            sample->offset, sample->peer_dispersion);
  return 1;
}

/* ================================================== */

int
SPF_GetLastSample(SPF_Instance filter, NTP_Sample *sample)
{
  if (filter->last < 0)
    return 0;

  *sample = filter->samples[filter->last];
  return 1;
}

/* ================================================== */

int
SPF_GetNumberOfSamples(SPF_Instance filter)
{
  return filter->used;
}

/* ================================================== */

int
SPF_GetMaxSamples(SPF_Instance filter)
{
  return filter->max_samples;
}

/* ================================================== */

double
SPF_GetAvgSampleDispersion(SPF_Instance filter)
{
  return sqrt(filter->avg_var);
}

/* ================================================== */

static void
drop_samples(SPF_Instance filter, int keep_last)
{
  filter->index = -1;
  filter->used = 0;
  if (!keep_last)
    filter->last = -1;
}

/* ================================================== */

void
SPF_DropSamples(SPF_Instance filter)
{
  drop_samples(filter, 0);
}

/* ================================================== */

static const NTP_Sample *tmp_sort_samples;

static int
compare_samples(const void *a, const void *b)
{
  const NTP_Sample *s1, *s2;

  s1 = &tmp_sort_samples[*(int *)a];
  s2 = &tmp_sort_samples[*(int *)b];

  if (s1->offset < s2->offset)
    return -1;
  else if (s1->offset > s2->offset)
    return 1;
  return 0;
}

/* ================================================== */

static int
select_samples(SPF_Instance filter)
{
  int i, j, k, o, from, to, *selected;
  double min_dispersion;

  if (filter->used < filter->min_samples)
    return 0;

  selected = filter->selected;

  /* With 4 or more samples, select those that have peer dispersion smaller
     than 1.5x of the minimum dispersion */
  if (filter->used > 4) {
    for (i = 1, min_dispersion = filter->samples[0].peer_dispersion; i < filter->used; i++) {
      if (min_dispersion > filter->samples[i].peer_dispersion)
        min_dispersion = filter->samples[i].peer_dispersion;
    }

    for (i = j = 0; i < filter->used; i++) {
      if (filter->samples[i].peer_dispersion <= 1.5 * min_dispersion)
        selected[j++] = i;
    }
  } else {
    j = 0;
  }

  if (j < 4) {
    /* Select all samples */

    for (j = 0; j < filter->used; j++)
      selected[j] = j;
  }

  /* And sort their indices by offset */
  tmp_sort_samples = filter->samples;
  qsort(selected, j, sizeof (int), compare_samples);

  /* Select samples closest to the median */
  if (j > 2) {
    from = j * (1.0 - filter->combine_ratio) / 2.0;
    from = CLAMP(1, from, (j - 1) / 2);
  } else {
    from = 0;
  }

  to = j - from;

  /* Mark unused samples and sort the rest by their time */

  o = filter->used - filter->index - 1;

  for (i = 0; i < from; i++)
    selected[i] = -1;
  for (; i < to; i++)
    selected[i] = (selected[i] + o) % filter->used;
  for (; i < filter->used; i++)
    selected[i] = -1;

  for (i = from; i < to; i++) {
    j = selected[i];
    selected[i] = -1;
    while (j != -1 && selected[j] != j) {
      k = selected[j];
      selected[j] = j;
      j = k;
    }
  }

  for (i = j = 0; i < filter->used; i++) {
    if (selected[i] != -1)
      selected[j++] = (selected[i] + filter->used - o) % filter->used;
  }

  assert(j > 0 && j <= filter->max_samples);

  return j;
}

/* ================================================== */

static int
combine_selected_samples(SPF_Instance filter, int n, NTP_Sample *result)
{
  double mean_peer_dispersion, mean_root_dispersion, mean_peer_delay, mean_root_delay;
  double mean_x, mean_y, disp, var, prev_avg_var;
  NTP_Sample *sample, *last_sample;
  int i, dof;

  last_sample = &filter->samples[filter->selected[n - 1]];

  /* Prepare data */
  for (i = 0; i < n; i++) {
    sample = &filter->samples[filter->selected[i]];

    filter->x_data[i] = UTI_DiffTimespecsToDouble(&sample->time, &last_sample->time);
    filter->y_data[i] = sample->offset;
    filter->w_data[i] = sample->peer_dispersion;
  }

  /* Calculate mean offset and interval since the last sample */
  for (i = 0, mean_x = mean_y = 0.0; i < n; i++) {
    mean_x += filter->x_data[i];
    mean_y += filter->y_data[i];
  }
  mean_x /= n;
  mean_y /= n;

  if (n >= 4) {
    double b0, b1, s2, sb0, sb1;

    /* Set y axis to the mean sample time */
    for (i = 0; i < n; i++)
      filter->x_data[i] -= mean_x;

    /* Make a linear fit and use the estimated standard deviation of the
       intercept as dispersion */
    RGR_WeightedRegression(filter->x_data, filter->y_data, filter->w_data, n,
                           &b0, &b1, &s2, &sb0, &sb1);
    var = s2;
    disp = sb0;
    dof = n - 2;
  } else if (n >= 2) {
    for (i = 0, disp = 0.0; i < n; i++)
      disp += (filter->y_data[i] - mean_y) * (filter->y_data[i] - mean_y);
    var = disp / (n - 1);
    disp = sqrt(var);
    dof = n - 1;
  } else {
    var = filter->avg_var;
    disp = sqrt(var);
    dof = 1;
  }

  /* Avoid working with zero dispersion */
  if (var < 1e-20) {
    var = 1e-20;
    disp = sqrt(var);
  }

  /* Drop the sample if the variance is larger than the maximum */
  if (filter->max_var > 0.0 && var > filter->max_var) {
    DEBUG_LOG("filter dispersion too large disp=%.9f max=%.9f",
              sqrt(var), sqrt(filter->max_var));
    return 0;
  }

  prev_avg_var = filter->avg_var;

  /* Update the exponential moving average of the variance */
  if (filter->avg_var_n > 50) {
    filter->avg_var += dof / (dof + 50.0) * (var - filter->avg_var);
  } else {
    filter->avg_var = (filter->avg_var * filter->avg_var_n + var * dof) /
      (dof + filter->avg_var_n);
    if (filter->avg_var_n == 0)
      prev_avg_var = filter->avg_var;
    filter->avg_var_n += dof;
  }

  /* Use the long-term average of variance instead of the estimated value
     unless it is significantly smaller in order to reduce the noise in
     sourcestats weights */
  if (var * dof / RGR_GetChi2Coef(dof) < prev_avg_var)
    disp = sqrt(filter->avg_var) * disp / sqrt(var);

  mean_peer_dispersion = mean_root_dispersion = mean_peer_delay = mean_root_delay = 0.0;

  for (i = 0; i < n; i++) {
    sample = &filter->samples[filter->selected[i]];

    mean_peer_dispersion += sample->peer_dispersion;
    mean_root_dispersion += sample->root_dispersion;
    mean_peer_delay += sample->peer_delay;
    mean_root_delay += sample->root_delay;
  }

  mean_peer_dispersion /= n;
  mean_root_dispersion /= n;
  mean_peer_delay /= n;
  mean_root_delay /= n;

  UTI_AddDoubleToTimespec(&last_sample->time, mean_x, &result->time);
  result->offset = mean_y;
  result->peer_dispersion = MAX(disp, mean_peer_dispersion);
  result->root_dispersion = MAX(disp, mean_root_dispersion);
  result->peer_delay = mean_peer_delay;
  result->root_delay = mean_root_delay;

  return 1;
}

/* ================================================== */

int
SPF_GetFilteredSample(SPF_Instance filter, NTP_Sample *sample)
{
  int n;

  n = select_samples(filter);

  DEBUG_LOG("selected %d from %d samples", n, filter->used);

  if (n < 1)
    return 0;

  if (!combine_selected_samples(filter, n, sample))
    return 0;

  drop_samples(filter, 1);

  return 1;
}

/* ================================================== */

static int
get_first_last(SPF_Instance filter, int *first, int *last)
{
  if (filter->last < 0)
    return 0;

  /* Always slew the last sample as it may be returned even if no new
     samples were accumulated */
  if (filter->used > 0) {
    *first = 0;
    *last = filter->used - 1;
  } else {
    *first = *last = filter->last;
  }

  return 1;
}


/* ================================================== */

void
SPF_SlewSamples(SPF_Instance filter, struct timespec *when, double dfreq, double doffset)
{
  int i, first, last;
  double delta_time;

  if (!get_first_last(filter, &first, &last))
    return;

  for (i = first; i <= last; i++) {
    UTI_AdjustTimespec(&filter->samples[i].time, when, &filter->samples[i].time,
                       &delta_time, dfreq, doffset);
    filter->samples[i].offset -= delta_time;
  }
}

/* ================================================== */

void
SPF_CorrectOffset(SPF_Instance filter, double doffset)
{
  int i, first, last;

  if (!get_first_last(filter, &first, &last))
    return;

  for (i = first; i <= last; i++)
    filter->samples[i].offset -= doffset;
}

/* ================================================== */

void
SPF_AddDispersion(SPF_Instance filter, double dispersion)
{
  int i;

  for (i = 0; i < filter->used; i++) {
    filter->samples[i].peer_dispersion += dispersion;
    filter->samples[i].root_dispersion += dispersion;
  }
}