/* movstat/medacc.c

 * 

 * Copyright (C) 2018 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.

 *

 * Original copyright notice:

 * Copyright (c) 2011 ashelly.myopenid.com under <http://www.opensource.org/licenses/mit-license>

 */

#include <config.h>

#include <stdlib.h>

#include <math.h>

#include <gsl/gsl_math.h>

#include <gsl/gsl_errno.h>

#include <gsl/gsl_movstat.h>

#define ItemLess(a,b)  ((a)<(b))

#define ItemMean(a,b)  (((a)+(b))/2)

#define minCt(m) (((m)->ct-1)/2) /* count of items in minheap */

#define maxCt(m) (((m)->ct)/2)   /* count of items in maxheap */

typedef double medacc_type_t;

typedef struct

{

  int n;                /* window size */

  int idx;              /* position in circular queue */

  int ct;               /* count of items in queue */

  medacc_type_t *data;  /* circular queue of values, size k */

  int *pos;             /* index into `heap` for each value, size 2*k */

  int *heap;            /* max/median/min heap holding indices into `data` */

} medacc_state_t;

static size_t medacc_size(const size_t n);

static int medacc_init(const size_t n, void * vstate);

static int medacc_insert(const medacc_type_t x, void * vstate);

static int medacc_delete(void * vstate);

static int medacc_get(void * params, medacc_type_t * result, const void * vstate);

static int mmless(const medacc_state_t * state, const int i, const int j);

static int mmexchange(medacc_state_t * state, const int i, const int j);

static int mmCmpExch(medacc_state_t * state, const int i, const int j);

static void minSortDown(medacc_state_t * state, int i);

static void maxSortDown(medacc_state_t * state, int i);

static int minSortUp(medacc_state_t * state, int i);

static int maxSortUp(medacc_state_t * state, int i);

static size_t

medacc_size(const size_t n)

{

  size_t size = 0;

  size += sizeof(medacc_state_t);

  size += n * sizeof(medacc_type_t);

  size += 2 * n * sizeof(int);

  return size;

}

static int

medacc_init(const size_t n, void * vstate)

{

  medacc_state_t * state = (medacc_state_t *) vstate;

  int k = (int) n;

  state->n = n;

  state->ct = 0;

  state->idx = 0;

  state->data = (medacc_type_t *) ((unsigned char *) vstate + sizeof(medacc_state_t));

  state->pos = (int *) ((unsigned char *) state->data + n * sizeof(medacc_type_t));

  state->heap = state->pos + n + (n/2); /* points to middle of storage */

  /* set up initial heap fill pattern: median,max,min,max,... */

  while (k--)

    {

      state->pos[k] = ((k + 1)/2) * ((k & 1) ? -1 : 1);

      state->heap[state->pos[k]] = k;

    }

  return GSL_SUCCESS;

}

static int

medacc_insert(const medacc_type_t x, void * vstate)

{

  medacc_state_t * state = (medacc_state_t *) vstate;

  int isNew = (state->ct < (int) state->n);

  int p = state->pos[state->idx];

  medacc_type_t old = state->data[state->idx];

  state->data[state->idx] = x;

  state->idx = (state->idx + 1) % state->n;

  state->ct += isNew;

  if (p > 0)       /* new item is in minHeap */

    {

      if (!isNew && ItemLess(old, x))

        minSortDown(state, p * 2);

      else if (minSortUp(state, p))

        maxSortDown(state, -1);

    }

  else if (p < 0)  /* new item is in maxHeap */

    {

      if (!isNew && ItemLess(x, old))

        maxSortDown(state, p * 2);

      else if (maxSortUp(state, p))

        minSortDown(state, 1);

    }

  else             /* new item is at median */

    {

      if (maxCt(state))

        maxSortDown(state, -1);

      if (minCt(state))

        minSortDown(state, 1);

    }

  return GSL_SUCCESS;

}

static int

medacc_delete(void * vstate)

{

  medacc_state_t * state = (medacc_state_t *) vstate;

  if (state->ct > 0)

    {

      int p = state->pos[(state->idx - state->ct + state->n) % state->n];

      if (p > 0)        /* oldest item is in minHeap */

        {

          mmexchange(state, p, minCt(state));

          --(state->ct);

          minSortDown(state, 2 * p);

        }

      else if (p < 0)   /* oldest item is in maxHeap */

        {

          mmexchange(state, p, maxCt(state));

          --(state->ct);

          maxSortDown(state, 2 * p);

        }

      else if (p == 0)  /* oldest item is at median */

        {

        }

    }

  return GSL_SUCCESS;

}

/* returns median (or average of 2 when item count is even) */

static int

medacc_get(void * params, medacc_type_t * result, const void * vstate)

{

  const medacc_state_t * state = (const medacc_state_t *) vstate;

  medacc_type_t median = state->data[state->heap[0]];

  (void) params;

  if ((state->ct & 1) == 0)

    median = ItemMean(median, state->data[state->heap[-1]]);

  *result = median;

  return GSL_SUCCESS;

}

/* returns 1 if heap[i] < heap[j] */

static int

mmless(const medacc_state_t * state, const int i, const int j)

{

  return ItemLess(state->data[state->heap[i]], state->data[state->heap[j]]);

}

/* swaps items i and j in heap, maintains indexes */

static int

mmexchange(medacc_state_t * state, const int i, const int j)

{

  int t = state->heap[i];

  state->heap[i] = state->heap[j];

  state->heap[j] = t;

  state->pos[state->heap[i]] = i;

  state->pos[state->heap[j]] = j;

  return 1;

}

/* swaps items i and j if i < j; returns true if swapped */

static int

mmCmpExch(medacc_state_t * state, const int i, const int j)

{

  return (mmless(state, i, j) && mmexchange(state , i, j));

}

/* maintains minheap property for all items below i/2. */

static void

minSortDown(medacc_state_t * state, int i)

{

  for (; i <= minCt(state); i *= 2)

    {

      if (i > 1 && i < minCt(state) && mmless(state, i + 1, i))

        ++i;

      if (!mmCmpExch(state, i, i / 2))

        break;

   }

}

/* maintains maxheap property for all items below i/2. (negative indexes) */

static void

maxSortDown(medacc_state_t * state, int i)

{

  for (; i >= -maxCt(state); i *= 2)

    {

      if (i < -1 && i > -maxCt(state) && mmless(state, i, i - 1))

        --i;

      if (!mmCmpExch(state, i / 2, i))

        break;

   }

}

/* maintains minheap property for all items above i, including median

   returns true if median changed */

static int

minSortUp(medacc_state_t * state, int i)

{

  while (i > 0 && mmCmpExch(state, i, i / 2))

    i /= 2;

  return (i == 0);

}

/* maintains maxheap property for all items above i, including median

   returns true if median changed */

static int

maxSortUp(medacc_state_t * state, int i)

{

  while (i<0 && mmCmpExch(state, i / 2, i))

    i /= 2;

  return (i == 0);

}

static const gsl_movstat_accum median_accum_type =

{

  medacc_size,

  medacc_init,

  medacc_insert,

  NULL, /* XXX FIXME */

  medacc_get

};

const gsl_movstat_accum *gsl_movstat_accum_median = &median_accum_type;