/* rstat/rquantile.c
 * 
 * Copyright (C) 2015 Patrick Alken
 * 
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 * 
 * 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.
 */

#include <config.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_sort.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_rstat.h>

/*
 * Running quantile calculation based on the paper
 *
 * [1] R. Jain and I. Chlamtac, "The P^2 algorithm for dynamic
 *     calculation of quantiles and histograms without storing
 *     observations", Communications of the ACM, October 1985
 */

static double calc_psq(const double qp1, const double q, const double qm1,
                       const double d, const double np1, const double n, const double nm1);

gsl_rstat_quantile_workspace *
gsl_rstat_quantile_alloc(const double p)
{
  gsl_rstat_quantile_workspace *w;

  w = calloc(1, sizeof(gsl_rstat_quantile_workspace));
  if (w == 0)
    {
      GSL_ERROR_NULL ("failed to allocate space for workspace", GSL_ENOMEM);
    }

  w->p = p;

  gsl_rstat_quantile_reset(w);

  return w;
} /* gsl_rstat_quantile_alloc() */

void
gsl_rstat_quantile_free(gsl_rstat_quantile_workspace *w)
{
  free(w);
} /* gsl_rstat_quantile_free() */

int
gsl_rstat_quantile_reset(gsl_rstat_quantile_workspace *w)
{
  const double p = w->p;
  size_t i;

  /* initialize positions n */
  for (i = 0; i < 5; ++i)
    w->npos[i] = i + 1;

  /* initialize n' */
  w->np[0] = 1.0;
  w->np[1] = 1.0 + 2.0 * p;
  w->np[2] = 1.0 + 4.0 * p;
  w->np[3] = 3.0 + 2.0 * p;
  w->np[4] = 5.0;

  /* initialize dn' */
  w->dnp[0] = 0.0;
  w->dnp[1] = 0.5 * p;
  w->dnp[2] = p;
  w->dnp[3] = 0.5 * (1.0 + p);
  w->dnp[4] = 1.0;

  w->n = 0;

  return GSL_SUCCESS;
}

int
gsl_rstat_quantile_add(const double x, gsl_rstat_quantile_workspace *w)
{
  if (w->n < 5)
    {
      w->q[w->n] = x;
    }
  else
    {
      int i;
      int k = -1;

      if (w->n == 5)
        {
          /* initialization: sort the first five heights */
          gsl_sort(w->q, 1, w->n);
        }

      /* step B1: find k such that q_k <= x < q_{k+1} */
      if (x < w->q[0])
        {
          w->q[0] = x;
          k = 0;
        }
      else if (x >= w->q[4])
        {
          w->q[4] = x;
          k = 3;
        }
      else
        {
          for (i = 0; i <= 3; ++i)
            {
              if (w->q[i] <= x && x < w->q[i + 1])
                {
                  k = i;
                  break;
                }
            }
        }

      if (k < 0)
        {
          /* we could get here if x is nan */
          GSL_ERROR ("invalid input argument x", GSL_EINVAL);
        }

      /* step B2(a): update n_i */
      for (i = k + 1; i <= 4; ++i)
        ++(w->npos[i]);

      /* step B2(b): update n_i' */
      for (i = 0; i < 5; ++i)
        w->np[i] += w->dnp[i];

      /* step B3: update heights */
      for (i = 1; i <= 3; ++i)
        {
          double ni = (double) w->npos[i];
          double d = w->np[i] - ni;

          if ((d >= 1.0 && (w->npos[i + 1] - w->npos[i] > 1)) ||
              (d <= -1.0 && (w->npos[i - 1] - w->npos[i] < -1)))
            {
              int dsign = (d > 0.0) ? 1 : -1;
              double qp1 = w->q[i + 1];
              double qi = w->q[i];
              double qm1 = w->q[i - 1];
              double np1 = (double) w->npos[i + 1];
              double nm1 = (double) w->npos[i - 1];
              double qp = calc_psq(qp1, qi, qm1, (double) dsign,
                                   np1, ni, nm1);

              if (qm1 < qp && qp < qp1)
                w->q[i] = qp;
              else
                {
                  /* use linear formula */
                  w->q[i] += dsign * (w->q[i + dsign] - qi) / ((double) w->npos[i + dsign] - ni);
                }

              w->npos[i] += dsign;
            }
        }
    }

  ++(w->n);

  return GSL_SUCCESS;
} /* gsl_rstat_quantile_add() */

double
gsl_rstat_quantile_get(gsl_rstat_quantile_workspace *w)
{
  if (w->n >= 5)
    {
      return w->q[2];
    }
  else
    {
      /* not yet initialized */
      gsl_sort(w->q, 1, w->n);
      return gsl_stats_quantile_from_sorted_data(w->q, 1, w->n, w->p);
    }
} /* gsl_rstat_quantile_get() */

static double
calc_psq(const double qp1, const double q, const double qm1,
         const double d, const double np1, const double n, const double nm1)
{
  double outer = d / (np1 - nm1);
  double inner_left = (n - nm1 + d) * (qp1 - q) / (np1 - n);
  double inner_right = (np1 - n - d) * (q - qm1) / (n - nm1);

  return q + outer * (inner_left + inner_right);
} /* calc_psq() */