/* multifit_nlinear/trust.c
 *
 * Copyright (C) 2016 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 <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_multifit_nlinear.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_permutation.h>

#include "common.c"
#include "nielsen.c"

/*
 * This module contains a high level driver for a general trust
 * region nonlinear least squares solver. This container handles
 * the computation of all of the quantities relevant to all trust
 * region methods, including:
 *
 * residual vector: f_k = f(x_k)
 * Jacobian matrix: J_k = J(x_k)
 * gradient vector: g_k = J_k^T f_k
 * scaling matrix:  D_k
 */

typedef struct
{
  size_t n;                  /* number of observations */
  size_t p;                  /* number of parameters */
  double delta;              /* trust region radius */
  double mu;                 /* LM parameter */
  long nu;                   /* for updating LM parameter */
  gsl_vector *diag;          /* D = diag(J^T J) */
  gsl_vector *x_trial;       /* trial parameter vector */
  gsl_vector *f_trial;       /* trial function vector */
  gsl_vector *workp;         /* workspace, length p */
  gsl_vector *workn;         /* workspace, length n */

  void *trs_state;           /* workspace for trust region subproblem */
  void *solver_state;        /* workspace for linear least squares solver */

  double avratio;            /* current |a| / |v| */

  /* tunable parameters */
  gsl_multifit_nlinear_parameters params;
} trust_state_t;

static void * trust_alloc (const gsl_multifit_nlinear_parameters * params,
                           const size_t n, const size_t p);
static void trust_free(void *vstate);
static int trust_init(void *vstate, const gsl_vector * swts,
                      gsl_multifit_nlinear_fdf *fdf, const gsl_vector *x,
                      gsl_vector *f, gsl_matrix *J, gsl_vector *g);
static int trust_iterate(void *vstate, const gsl_vector *swts,
                         gsl_multifit_nlinear_fdf *fdf,
                         gsl_vector *x, gsl_vector *f, gsl_matrix *J,
                         gsl_vector *g, gsl_vector *dx);
static int trust_rcond(double *rcond, void *vstate);
static double trust_avratio(void *vstate);
static void trust_trial_step(const gsl_vector * x, const gsl_vector * dx,
                             gsl_vector * x_trial);
static double trust_calc_rho(const gsl_vector * f, const gsl_vector * f_trial,
                             const gsl_vector * g, const gsl_matrix * J,
                             const gsl_vector * dx, trust_state_t * state);
static int trust_eval_step(const gsl_vector * f, const gsl_vector * f_trial,
                           const gsl_vector * g, const gsl_matrix * J,
                           const gsl_vector * dx, double * rho, trust_state_t * state);
static double trust_scaled_norm(const gsl_vector *D, const gsl_vector *a);

static void *
trust_alloc (const gsl_multifit_nlinear_parameters * params,
             const size_t n, const size_t p)
{
  trust_state_t *state;

  state = calloc(1, sizeof(trust_state_t));
  if (state == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate lm state", GSL_ENOMEM);
    }

  state->diag = gsl_vector_alloc(p);
  if (state->diag == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for diag", GSL_ENOMEM);
    }

  state->workp = gsl_vector_alloc(p);
  if (state->workp == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for workp", GSL_ENOMEM);
    }

  state->workn = gsl_vector_alloc(n);
  if (state->workn == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for workn", GSL_ENOMEM);
    }

  state->x_trial = gsl_vector_alloc(p);
  if (state->x_trial == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for x_trial", GSL_ENOMEM);
    }

  state->f_trial = gsl_vector_alloc(n);
  if (state->f_trial == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for f_trial", GSL_ENOMEM);
    }

  state->trs_state = (params->trs->alloc)(params, n, p);
  if (state->trs_state == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for trs state", GSL_ENOMEM);
    }

  state->solver_state = (params->solver->alloc)(n, p);
  if (state->solver_state == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for solver state", GSL_ENOMEM);
    }

  state->n = n;
  state->p = p;
  state->delta = 0.0;
  state->params = *params;

  return state;
}

static void
trust_free(void *vstate)
{
  trust_state_t *state = (trust_state_t *) vstate;
  const gsl_multifit_nlinear_parameters *params = &(state->params);

  if (state->diag)
    gsl_vector_free(state->diag);

  if (state->workp)
    gsl_vector_free(state->workp);

  if (state->workn)
    gsl_vector_free(state->workn);

  if (state->x_trial)
    gsl_vector_free(state->x_trial);

  if (state->f_trial)
    gsl_vector_free(state->f_trial);

  if (state->trs_state)
    (params->trs->free)(state->trs_state);

  if (state->solver_state)
    (params->solver->free)(state->solver_state);

  free(state);
}

/*
trust_init()
  Initialize trust region solver

Inputs: vstate - workspace
        swts   - sqrt(W) vector
        fdf    - user callback functions
        x      - initial parameter values
        f      - (output) f(x) vector
        J      - (output) J(x) matrix
        g      - (output) J(x)' f(x) vector

Return: success/error
*/

static int
trust_init(void *vstate, const gsl_vector *swts,
           gsl_multifit_nlinear_fdf *fdf, const gsl_vector *x,
           gsl_vector *f, gsl_matrix *J, gsl_vector *g)
{
  int status;
  trust_state_t *state = (trust_state_t *) vstate;
  const gsl_multifit_nlinear_parameters *params = &(state->params);
  double Dx;

  /* evaluate function and Jacobian at x and apply weight transform */
  status = gsl_multifit_nlinear_eval_f(fdf, x, swts, f);
  if (status)
   return status;

  status = gsl_multifit_nlinear_eval_df(x, f, swts, params->h_df,
                                        params->fdtype, fdf, J, state->workn);
  if (status)
    return status;

  /* compute g = J^T f */
  gsl_blas_dgemv(CblasTrans, 1.0, J, f, 0.0, g);

  /* initialize diagonal scaling matrix D */
  (params->scale->init)(J, state->diag);

  /* compute initial trust region radius */
  Dx = trust_scaled_norm(state->diag, x);
  state->delta = 0.3 * GSL_MAX(1.0, Dx);

  /* initialize LM parameter */
  status = nielsen_init(J, state->diag, &(state->mu), &(state->nu));
  if (status)
    return status;

  /* initialize trust region method solver */
  {
    gsl_multifit_nlinear_trust_state trust_state;

    trust_state.x = x;
    trust_state.f = f;
    trust_state.g = g;
    trust_state.J = J;
    trust_state.diag = state->diag;
    trust_state.sqrt_wts = swts;
    trust_state.mu = &(state->mu);
    trust_state.params = params;
    trust_state.solver_state = state->solver_state;
    trust_state.fdf = fdf;
    trust_state.avratio = &(state->avratio);

    status = (params->trs->init)(&trust_state, state->trs_state);

    if (status)
      return status;
  }

  /* set default parameters */

  state->avratio = 0.0;

  return GSL_SUCCESS;
}

/*
trust_iterate()
  This function performs 1 iteration of the trust region algorithm.
It calls a user-specified method for computing the next step
(LM or dogleg), then tests if the computed step is acceptable.

Args: vstate - trust workspace
      swts   - data weights (NULL if unweighted)
      fdf    - function and Jacobian pointers
      x      - on input, current parameter vector
               on output, new parameter vector x + dx
      f      - on input, f(x)
               on output, f(x + dx)
      J      - on input, J(x)
               on output, J(x + dx)
      g      - on input, g(x) = J(x)' f(x)
               on output, g(x + dx) = J(x + dx)' f(x + dx)
      dx     - (output only) parameter step vector

Return:
1) GSL_SUCCESS if we found a step which reduces the cost
function

2) GSL_ENOPROG if 15 successive attempts were to made to
find a good step without success

3) If a scaling matrix D is used, inputs and outputs are
set to the unscaled quantities (ie: J and g)
*/

static int
trust_iterate(void *vstate, const gsl_vector *swts,
              gsl_multifit_nlinear_fdf *fdf, gsl_vector *x,
              gsl_vector *f, gsl_matrix *J, gsl_vector *g,
              gsl_vector *dx)
{
  int status;
  trust_state_t *state = (trust_state_t *) vstate;
  const gsl_multifit_nlinear_parameters *params = &(state->params);
  const gsl_multifit_nlinear_trs *trs = params->trs;
  gsl_multifit_nlinear_trust_state trust_state;
  gsl_vector *x_trial = state->x_trial;       /* trial x + dx */
  gsl_vector *f_trial = state->f_trial;       /* trial f(x + dx) */
  gsl_vector *diag = state->diag;             /* diag(D) */
  double rho;                                 /* ratio actual_reduction/predicted_reduction */
  int foundstep = 0;                          /* found step dx */
  int bad_steps = 0;                          /* consecutive rejected steps */

  /* store all state parameters needed by low level methods */
  trust_state.x = x;
  trust_state.f = f;
  trust_state.g = g;
  trust_state.J = J;
  trust_state.diag = state->diag;
  trust_state.sqrt_wts = swts;
  trust_state.mu = &(state->mu);
  trust_state.params = params;
  trust_state.solver_state = state->solver_state;
  trust_state.fdf = fdf;
  trust_state.avratio = &(state->avratio);

  /* initialize trust region subproblem with this Jacobian */
  status = (trs->preloop)(&trust_state, state->trs_state);
  if (status)
    return status;

  /* loop until we find an acceptable step dx */
  while (!foundstep)
    {
      /* calculate new step */
      status = (trs->step)(&trust_state, state->delta, dx, state->trs_state);

      /* occasionally the iterative methods (ie: CG Steihaug) can fail to find a step,
       * so in this case skip rho calculation and count it as a rejected step */

      if (status == GSL_SUCCESS)
        {
          /* compute x_trial = x + dx */
          trust_trial_step(x, dx, x_trial);

          /* compute f_trial = f(x + dx) */
          status = gsl_multifit_nlinear_eval_f(fdf, x_trial, swts, f_trial);
          if (status)
            return status;

          /* check if step should be accepted or rejected */
          status = trust_eval_step(f, f_trial, g, J, dx, &rho, state);
          if (status == GSL_SUCCESS)
            foundstep = 1;
        }
      else
        {
          /* an iterative TRS method failed to find a step vector */
          rho = -1.0;
        }

      /*
       * update trust region radius: if rho is large,
       * then the quadratic model is a good approximation
       * to the objective function, enlarge trust region.
       * If rho is small (or negative), the model function
       * is a poor approximation so decrease trust region. This
       * can happen even if the step is accepted.
       */
      if (rho > 0.75)
        state->delta *= params->factor_up;
      else if (rho < 0.25)
        state->delta /= params->factor_down;

      if (foundstep)
        {
          /* step was accepted */

          /* compute J <- J(x + dx) */
          status = gsl_multifit_nlinear_eval_df(x_trial, f_trial, swts,
                                                params->h_df, params->fdtype,
                                                fdf, J, state->workn);
          if (status)
            return status;

          /* update x <- x + dx */
          gsl_vector_memcpy(x, x_trial);

          /* update f <- f(x + dx) */
          gsl_vector_memcpy(f, f_trial);

          /* compute new g = J^T f */
          gsl_blas_dgemv(CblasTrans, 1.0, J, f, 0.0, g);

          /* update scaling matrix D */
          (params->scale->update)(J, diag);

          /* step accepted, decrease LM parameter */
          status = nielsen_accept(rho, &(state->mu), &(state->nu));
          if (status)
            return status;

          bad_steps = 0;
        }
      else
        {
          /* step rejected, increase LM parameter */
          status = nielsen_reject(&(state->mu), &(state->nu));
          if (status)
            return status;

          if (++bad_steps > 15)
            {
              /* if more than 15 consecutive rejected steps, report no progress */
              return GSL_ENOPROG;
            }
        }
    }

  return GSL_SUCCESS;
} /* trust_iterate() */

static int
trust_rcond(double *rcond, void *vstate)
{
  int status;
  trust_state_t *state = (trust_state_t *) vstate;
  const gsl_multifit_nlinear_parameters *params = &(state->params);

  status = (params->solver->rcond)(rcond, state->solver_state);

  return status;
}

static double
trust_avratio(void *vstate)
{
  trust_state_t *state = (trust_state_t *) vstate;
  return state->avratio;
}

/* compute x_trial = x + dx */
static void
trust_trial_step(const gsl_vector * x, const gsl_vector * dx,
                 gsl_vector * x_trial)
{
  size_t i, N = x->size;

  for (i = 0; i < N; i++)
    {
      double dxi = gsl_vector_get (dx, i);
      double xi = gsl_vector_get (x, i);
      gsl_vector_set (x_trial, i, xi + dxi);
    }
}

/*
trust_calc_rho()
  Calculate ratio of actual reduction to predicted
reduction.

rho = actual_reduction / predicted_reduction

actual_reduction = 1 - ( ||f+|| / ||f|| )^2
predicted_reduction = -2 g^T dx / ||f||^2 - ( ||J*dx|| / ||f|| )^2
                    = -2 fhat . beta - ||beta||^2

where: beta = J*dx / ||f||

Inputs: f        - f(x)
        f_trial  - f(x + dx)
        g        - gradient J^T f
        J        - Jacobian
        dx       - proposed step, size p
        state    - workspace

Return: rho = actual_reduction / predicted_reduction
If actual_reduction is < 0, return rho = -1
*/

static double
trust_calc_rho(const gsl_vector * f, const gsl_vector * f_trial,
               const gsl_vector * g, const gsl_matrix * J,
               const gsl_vector * dx, trust_state_t * state)
{
  int status;
  const gsl_multifit_nlinear_parameters *params = &(state->params);
  const gsl_multifit_nlinear_trs *trs = params->trs;
  const double normf = gsl_blas_dnrm2(f);
  const double normf_trial = gsl_blas_dnrm2(f_trial);
  gsl_multifit_nlinear_trust_state trust_state;
  double rho;
  double actual_reduction;
  double pred_reduction;
  double u;

  /* if ||f(x+dx)|| > ||f(x)|| reject step immediately */
  if (normf_trial >= normf)
    return -1.0;

  trust_state.x = NULL;
  trust_state.f = f;
  trust_state.g = g;
  trust_state.J = J;
  trust_state.diag = state->diag;
  trust_state.sqrt_wts = NULL;
  trust_state.mu = &(state->mu);
  trust_state.params = params;
  trust_state.solver_state = state->solver_state;
  trust_state.fdf = NULL;
  trust_state.avratio = &(state->avratio);

  /* compute numerator of rho (actual reduction) */
  u = normf_trial / normf;
  actual_reduction = 1.0 - u*u;

  /*
   * compute denominator of rho (predicted reduction); this is calculated
   * inside each trust region subproblem, since it depends on the local
   * model used, which can vary according to each TRS
   */
  status = (trs->preduction)(&trust_state, dx, &pred_reduction, state->trs_state);
  if (status)
    return -1.0;

  if (pred_reduction > 0.0)
    rho = actual_reduction / pred_reduction;
  else
    rho = -1.0;

  return rho;
}

/*
trust_eval_step()
  Evaluate proposed step to determine if it should be
accepted or rejected
*/

static int
trust_eval_step(const gsl_vector * f, const gsl_vector * f_trial,
                const gsl_vector * g, const gsl_matrix * J,
                const gsl_vector * dx, double * rho, trust_state_t * state)
{
  int status = GSL_SUCCESS;
  const gsl_multifit_nlinear_parameters *params = &(state->params);

  if (params->trs == gsl_multifit_nlinear_trs_lmaccel)
    {
      /* reject step if acceleration is too large compared to velocity */
      if (state->avratio > params->avmax)
        status = GSL_FAILURE;
    }

  /* compute rho */
  *rho = trust_calc_rho(f, f_trial, g, J, dx, state);
  if (*rho <= 0.0)
    status = GSL_FAILURE;

  return status;
}

/* compute || diag(D) a || */
static double
trust_scaled_norm(const gsl_vector *D, const gsl_vector *a)
{
  const size_t n = a->size;
  double e2 = 0.0;
  size_t i;

  for (i = 0; i < n; ++i)
    {
      double Di = gsl_vector_get(D, i);
      double ai = gsl_vector_get(a, i);
      double u = Di * ai;

      e2 += u * u;
    }

  return sqrt (e2);
}

static const gsl_multifit_nlinear_type trust_type =
{
  "trust-region",
  trust_alloc,
  trust_init,
  trust_iterate,
  trust_rcond,
  trust_avratio,
  trust_free
};

const gsl_multifit_nlinear_type *gsl_multifit_nlinear_trust = &trust_type;


#pragma once

#include <memory>

#include <gsl/span>

#include "graphics/math.h"
#include "infrastructure/macros.h"

namespace gfx {
	using MdfRenderMaterialPtr = std::shared_ptr<class MdfRenderMaterial>;

	class RenderingDevice;
	class Texture;
	using TextureRef = std::shared_ptr<Texture>;

	struct Line2d {
		XMFLOAT2 from;
		XMFLOAT2 to;
		XMCOLOR diffuse;

		Line2d() {}

		Line2d(XMFLOAT2 from, XMFLOAT2 to, XMCOLOR color)
			: from(from), to(to), diffuse(color) {}
	};

#pragma pack(push, 1)
	struct Vertex2d {
		XMFLOAT4 pos;
		XMFLOAT4 normal;
		XMCOLOR diffuse;
		XMFLOAT2 uv;
	};
#pragma pack(pop)

	enum class SamplerType2d {
		CLAMP,
		WRAP,
		POINT
	};

	class SamplerState;

	/*
		Renders shapes in 2d screen space.
	*/
	class ShapeRenderer2d {
	public:
		explicit ShapeRenderer2d(RenderingDevice& g);
		~ShapeRenderer2d();

		void DrawRectangle(float x, float y, float width, float height, gfx::Texture& texture, uint32_t color = 0xFFFFFFFF, SamplerType2d samplerType = SamplerType2d::CLAMP) {
			DrawRectangle(x, y, width, height, &texture, color, samplerType);
		}

		void DrawRectangle(float x, float y, float width, float height, uint32_t color) {
			DrawRectangle(x, y, width, height, nullptr, color);
		}

		void DrawRectangle(gsl::span<Vertex2d, 4> corners,
			gfx::Texture* texture,
			gfx::Texture* mask = nullptr,
			SamplerType2d samplerType = SamplerType2d::CLAMP,
			bool blending = true);

		void DrawRectangle(gsl::span<Vertex2d, 4> corners,
			const gfx::MdfRenderMaterialPtr &material);

		void DrawRectangle(gsl::span<Vertex2d, 4> corners);

		void DrawLines(gsl::span<Line2d> lines);

		void DrawRectangleOutline(XMFLOAT2 topLeft, XMFLOAT2 bottomRight, XMCOLOR color);

		void DrawFullScreenQuad();

		/**
		 * Renders a circle/pie segment for use with the radial menu.
  		 */
		void DrawPieSegment(int segments,
			int x, int y,
			float angleCenter, float angleWidth,
			int innerRadius, int innerOffset,
			int outerRadius, int outerOffset,
			XMCOLOR color1, XMCOLOR color2);

		NO_COPY_OR_MOVE(ShapeRenderer2d);

	private:

		void DrawRectangle(
			float x, float y, float width, float height,
			gfx::Texture* texture,
			uint32_t color = 0xFFFFFFFF,
			SamplerType2d samplerType = SamplerType2d::CLAMP
		);

		SamplerState &getSamplerState(SamplerType2d type) const;

		struct Impl;
		std::unique_ptr<Impl> mImpl;
	};

}


/*
 *  vbHmmPc.c
 *  Model-specific core functions for VB-HMM-PC.
 *
 *  Created by OKAMOTO Kenji, SAKO Yasushi and RIKEN
 *  Copyright 2011-2015
 *  Cellular Informatics Laboratory, Advance Science Institute, RIKEN, Japan.
 *  All rights reserved.
 *
 *  Ver. 1.1.0
 *  Last modified on 2015.09.17
 */

#include <math.h>
#include <string.h>
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_sf_psi.h>
#include <gsl/gsl_sf_pow_int.h>
#include <string.h>
#include "vbHmmPc.h"
#include "rand.h"

#ifdef _OPENMP
#include "omp.h"
#endif

#define  MAX(a,b)  ((a)>(b)?(a):(b))
#define  MIN(a,b)  ((a)<(b)?(a):(b))

//static int isGlobalAnalysis = 0;

void setFunctions_pc(){
    commonFunctions funcs;
    funcs.newModelParameters    = newModelParameters_pc;
    funcs.freeModelParameters   = freeModelParameters_pc;
    funcs.newModelStats         = newModelStats_pc;
    funcs.freeModelStats        = freeModelStats_pc;
    funcs.initializeVbHmm       = initializeVbHmm_pc;
    funcs.pTilde_z1             = pTilde_z1_pc;
    funcs.pTilde_zn_zn1         = pTilde_zn_zn1_pc;
    funcs.pTilde_xn_zn          = pTilde_xn_zn_pc;
    funcs.calcStatsVars         = calcStatsVars_pc;
    funcs.maximization          = maximization_pc;
    funcs.varLowerBound         = varLowerBound_pc;
    funcs.reorderParameters     = reorderParameters_pc;
    funcs.outputResults         = outputResults_pc;
    setFunctions( funcs );
}

//void setGFunctions_pc(){
//    gCommonFunctions funcs;
//    funcs.newModelParameters    = newModelParameters_pc;
//    funcs.freeModelParameters   = freeModelParameters_pc;
//    funcs.newModelStats         = newModelStats_pc;
//    funcs.freeModelStats        = freeModelStats_pc;
//    funcs.newModelStatsG        = newModelStatsG_pc;
//    funcs.freeModelStatsG       = freeModelStatsG_pc;
//    funcs.initializeVbHmmG      = initializeVbHmmG_pc;
//    funcs.pTilde_z1             = pTilde_z1_pc;
//    funcs.pTilde_zn_zn1         = pTilde_zn_zn1_pc;
//    funcs.pTilde_xn_zn          = pTilde_xn_zn_pc;
//    funcs.calcStatsVarsG        = calcStatsVarsG_pc;
//    funcs.maximizationG         = maximizationG_pc;
//    funcs.varLowerBoundG        = varLowerBoundG_pc;
//    funcs.reorderParametersG    = reorderParametersG_pc;
//    funcs.outputResultsG        = outputResultsG_pc;
//    setGFunctions( funcs );
//    isGlobalAnalysis = 1;
//}

void outputResults_pc( xn, gv, iv, logFP )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
FILE *logFP;
{
    outputPcResults( xn, gv, iv, logFP );
}

//void outputResultsG_pc( xns, gv, ivs, logFP )
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//FILE *logFP;
//{
//    outputPcResultsG( xns, gv, ivs, logFP );
//}

void *newModelParameters_pc( xn, sNo )
xnDataSet *xn;
int sNo;
{
    int i;
    pcParameters *p = (void*)malloc( sizeof(pcParameters) );

    p->uPiArr = (double*)malloc( sNo * sizeof(double) );
    p->sumUPi = 0.0;
    p->uAMat = (double**)malloc( sNo * sizeof(double*) );
    for( i = 0 ; i < sNo ; i++ ){
        p->uAMat[i] = (double*)malloc( sNo * sizeof(double) );
    }
    p->sumUAArr = (double*)malloc( sNo * sizeof(double) );

    p->avgPi = (double *)malloc( sNo * sizeof(double) );
    p->avgLnPi = (double *)malloc( sNo * sizeof(double) );
    p->avgA = (double **)malloc( sNo * sizeof(double*) );
    p->avgLnA = (double **)malloc( sNo * sizeof(double*) );
    for( i = 0 ; i < sNo ; i++ ){
        p->avgA[i] = (double *)malloc( sNo * sizeof(double) );
        p->avgLnA[i] = (double *)malloc( sNo * sizeof(double) );
    }

    p->avgI = (double *)malloc( sNo * sizeof(double) );
    p->avgLnI = (double *)malloc( sNo * sizeof(double) );

    p->aIArr = (double*)malloc( sNo * sizeof(double) );
    p->bIArr = (double*)malloc( sNo * sizeof(double) );

    return p;
}

void freeModelParameters_pc( p, xn, sNo )
void **p;
xnDataSet *xn;
int sNo;
{
    pcParameters *gp = *p;
    int i;

    free( gp->uPiArr );
    for( i = 0 ; i < sNo ; i++ ){
        free( gp->uAMat[i] );
    }
    free( gp->uAMat );
    free( gp->sumUAArr );

    free( gp->avgPi );
    free( gp->avgLnPi );
    for( i = 0 ; i < sNo ; i++ ){
        free( gp->avgA[i] );
        free( gp->avgLnA[i] );
    }
    free( gp->avgA );
    free( gp->avgLnA );

    free( gp->avgI );
    free( gp->avgLnI );

    free( gp->aIArr );
    free( gp->bIArr );

    free( *p );
    *p = NULL;
}

void *newModelStats_pc( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
//    if( isGlobalAnalysis == 0 ){
        int sNo = gv->sNo;
        pcStats *s = (pcStats*)malloc( sizeof(pcStats) );

        int i;
        s->Ni = (double *)malloc( sNo * sizeof(double) );
        s->Nij = (double **)malloc( sNo * sizeof(double*) );
        for( i = 0 ; i < sNo ; i++ )
        {   s->Nij[i] = (double *)malloc( sNo * sizeof(double) );   }
        s->Ci = (double *)malloc( sNo * sizeof(double) );
        s->Mi = (double *)malloc( sNo * sizeof(double) );

        return s;

//    } else {
//
//        return NULL;
//
//    }
}

void freeModelStats_pc( s, xn, gv, iv )
void **s;
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
//    if( isGlobalAnalysis == 0 ){
        int sNo = gv->sNo;
        pcStats *gs = *s;
        int i;
        free( gs->Ni );
        for( i = 0 ; i < sNo ; i++ )
        {   free( gs->Nij[i] );   }
        free( gs->Nij );
        free( gs->Ci );
        free( gs->Mi );

        free( gs );
        *s = NULL;
//    }
}

//void *newModelStatsG_pc( xns, gv, ivs)
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//{
//    int sNo = gv->sNo;
//    pcGlobalStats *gs = (pcGlobalStats*)malloc( sizeof(pcGlobalStats) );
//
//    return gs;
//}

//void freeModelStatsG_pc( gs, xns, gv, ivs )
//void **gs;
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//{
//    int sNo = gv->sNo;
//    pcGlobalStats *ggs = *gs;
//
//    free( *gs );
//    *gs = NULL;
//}

void initializeVbHmm_pc( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    pcData *d = xn->data;
    size_t dLen = xn->N;
    int sNo = gv->sNo;
    pcParameters *p = gv->params;

    int i, j;
    size_t totalC = 0;
    for( i = 0 ; i < dLen ; i++ ){
        totalC += d->counts[i];
    }
    double meanI = (double)totalC / (double)dLen;

    // hyperparameter for p( pi(k) )
    p->sumUPi = 0.0;
    for( i = 0 ; i < sNo ; i++ ){
        p->uPiArr[i] = 1.0;
        p->sumUPi += p->uPiArr[i];
    }

    // hyperparameter for p( A(i,j) )
    for( i = 0 ; i < sNo ; i++ ){
        p->sumUAArr[i] = 0.0;
        for( j = 0 ; j < sNo ; j++ ){
            if( j == i ){
                p->uAMat[i][j] = 100.0;
            } else {
                p->uAMat[i][j] = 1.0;
            }
            p->sumUAArr[i] += p->uAMat[i][j];
        }
    }

    // hyperparameter for p( I(k) )
    for( i = 0 ; i < sNo ; i++ ){
        p->aIArr[i] = 1.0;
        p->bIArr[i] = 1.0 / meanI;
    }

    initialize_indVars_pc( xn, gv, iv );

    calcStatsVars_pc( xn, gv, iv );
    maximization_pc( xn, gv, iv );
}

//void initializeVbHmmG_pc( xns, gv, ivs )
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//{
//}

void initialize_indVars_pc( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    size_t dLen = xn->N;
    int sNo = gv->sNo;
    double **gmMat = iv->gmMat;

    int i;
    size_t n;
    double sumPar;
    for( n = 0 ; n < dLen ; n++ ){
        sumPar = 0.0;
        for( i = 0 ; i < sNo ; i++ ){
            gmMat[n][i] = enoise(1.0) + 1.0;
            sumPar += gmMat[n][i];
        }
        for( i = 0 ; i < sNo ; i++ ){
            gmMat[n][i] /= sumPar;
        }
    }
}

xnDataSet *newXnDataSet_pc( filename )
const char *filename;
{
    xnDataSet *xn = (xnDataSet*)malloc( sizeof(xnDataSet) );
    xn->name = (char*)malloc( strlen(filename) + 2 );
    strncpy( xn->name, filename, strlen(filename)+1 );
    xn->data = (pcData*)malloc( sizeof(pcData) );
    pcData *d = (pcData*)xn->data;
    d->binSize = 0.0;
    d->counts = NULL;
    return xn;
}

void freeXnDataSet_pc( xn )
xnDataSet **xn;
{
    pcData *d = (pcData*)(*xn)->data;
    free( d->counts );
    free( (*xn)->data );
    free( (*xn)->name );
    free( *xn );
    *xn = NULL;
}

double pTilde_z1_pc( i, params )
int i;
void *params;
{
    pcParameters *p = (pcParameters*)params;
    return exp( p->avgLnPi[i] );
}

double pTilde_zn_zn1_pc( i, j, params )
int i, j;
void *params;
{
    pcParameters *p = (pcParameters*)params;
    return exp( p->avgLnA[i][j] );
}

double pTilde_xn_zn_pc( xnWv, n, i, params )
xnDataSet *xnWv;
size_t n;
int i;
void *params;
{
    pcParameters *p = (pcParameters*)params;
    pcData *xn = (pcData*)xnWv->data;
    return exp( p->avgLnI[i] * (double)xn->counts[n] - p->avgI[i] ) / gsl_sf_fact( xn->counts[n] );
}

void calcStatsVars_pc( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    pcData *d = (pcData*)xn->data;
    pcStats *s = (pcStats*)iv->stats;
    size_t dLen = xn->N;
    int sNo = gv->sNo;
    double **gmMat = iv->gmMat, ***xiMat = iv->xiMat;
    double *Ni = s->Ni, **Nij = s->Nij;
    double *Mi = s->Mi, *Ci = s->Ci;
    size_t n;
    int i, j;

    for( i = 0 ; i < sNo ; i++ ){
        Ni[i] = 1e-10;
        Ci[i] = 1e-10;
        Mi[i] = 1e-10;
        for( j = 0 ; j < sNo ; j++ ){
            Nij[i][j] = 1e-10;
        }
    }
    for( n = 0 ; n < dLen ; n++ ){
        for( i = 0 ; i < sNo ; i++ ){
            Ni[i]  += gmMat[n][i];
            Ci[i]  += gmMat[n][i] * (double)d->counts[n];
            for( j = 0 ; j < sNo ; j++ ){
                Mi[i]     += xiMat[n][i][j];
                Nij[i][j] += xiMat[n][i][j];
            }
        }
    }
}

//void calcStatsVarsG_pc( xns, gv, ivs )
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//{
//}

void maximization_pc( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    pcParameters *p = (pcParameters*)gv->params;
    pcStats *s = (pcStats*)iv->stats;
    int sNo = gv->sNo;
    double **gmMat = iv->gmMat;
    double *avgPi = p->avgPi, *avgLnPi = p->avgLnPi, **avgA = p->avgA, **avgLnA = p->avgLnA;
    double *avgI = p->avgI, *avgLnI = p->avgLnI;
    double *uPiArr = p->uPiArr, sumUPi = p->sumUPi, *aIArr = p->aIArr, *bIArr = p->bIArr;
    double **uAMat = p->uAMat, *sumUAArr = p->sumUAArr;
    double *Ni = s->Ni, **Nij = s->Nij;
    double *Mi = s->Mi, *Ci = s->Ci;
    int i, j;

    for( i = 0 ; i < sNo ; i++ ){
        avgPi[i] = ( uPiArr[i] + gmMat[0][i] ) / ( sumUPi + 1.0 );
        avgLnPi[i] = gsl_sf_psi( uPiArr[i] + gmMat[0][i] ) - gsl_sf_psi( sumUPi + 1.0 );

        for( j = 0 ; j < sNo ; j++ ){
            avgA[i][j] = ( uAMat[i][j] + Nij[i][j] ) / ( sumUAArr[i] + Mi[i] );
            avgLnA[i][j] = gsl_sf_psi( uAMat[i][j] + Nij[i][j] ) - gsl_sf_psi( sumUAArr[i] + Mi[i] );
        }

        avgI[i] = (Ci[i] + aIArr[i]) / (Ni[i] + bIArr[i]);
        avgLnI[i] = gsl_sf_psi( Ci[i] + aIArr[i] ) - log( Ni[i] + bIArr[i] );
#ifdef  INTENSITY_CAP
        size_t n, totalC = 0;
        pcData *pc = xnWv->data;
        for( n = 0 ; n < xnWv->N ; n++ ){
            totalC += pc->counts[n];
        }
        double meanI = (double)totalC / (double)xnWv->N;
        avgI[i] = MIN( avgI[i], maxIntensityRatio * meanI );
        avgLnI[i] = MIN( avgLnI[i], log(maxIntensityRatio * meanI) );
#endif
    }
}

//void maximizationG_pc( xns, gv, ivs )
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//{
//}

double varLowerBound_pc( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    pcParameters *p = (pcParameters*)gv->params;
    pcStats *s = (pcStats*)iv->stats;
    size_t dLen = xn->N;
    int sNo = gv->sNo;
    double **gmMat = iv->gmMat, *cn = iv->cn;
    double *avgLnPi = p->avgLnPi, **avgLnA = p->avgLnA;
    double *avgI = p->avgI, *avgLnI = p->avgLnI;
    double *uPiArr = p->uPiArr, sumUPi = p->sumUPi, *aIArr = p->aIArr, *bIArr = p->bIArr;
    double **uAMat = p->uAMat, *sumUAArr = p->sumUAArr;
    double *Ni = s->Ni, **Nij = s->Nij;
    double *Mi = s->Mi, *Ci = s->Ci;
    size_t n;
    int i, j;

    double lnpPi = gsl_sf_lngamma(sumUPi);
    double lnpA = 0.0;
    double lnpI = 0.0;
    double lnqPi = gsl_sf_lngamma(sumUPi + 1.0);
    double lnqA = 0.0;
    double lnqI = 0.0;
    for( i = 0 ; i < sNo ; i++ ){
        lnpPi += (uPiArr[i]-1.0) * avgLnPi[i] - gsl_sf_lngamma(uPiArr[i]);

        lnpI += aIArr[i] * log(bIArr[i]) - gsl_sf_lngamma(aIArr[i]);
        lnpI += (aIArr[i] - 1.0) * avgLnI[i] - bIArr[i] * avgI[i];

        lnqPi += (uPiArr[i]+gmMat[0][i]-1.0) * (gsl_sf_psi(uPiArr[i]+gmMat[0][i]) - gsl_sf_psi(sumUPi+1.0));
        lnqPi -= gsl_sf_lngamma(uPiArr[i] + gmMat[0][i]);

        lnqI += (Ci[i] + aIArr[i]) * log(Ni[i] + bIArr[i]) - gsl_sf_lngamma(Ci[i] + aIArr[i]);
        lnqI += (Ci[i] + aIArr[i] - 1.0) * avgLnI[i] - (Ni[i] + bIArr[i]) * avgI[i];

        lnpA += gsl_sf_lngamma(sumUAArr[i]);
        lnqA += gsl_sf_lngamma(sumUAArr[i] + Mi[i]);
        for( j = 0 ; j < sNo ; j++ ){
            lnpA += (uAMat[i][j]-1.0) * avgLnA[i][j] - gsl_sf_lngamma(uAMat[i][j]);

            lnqA += (uAMat[i][j] + Nij[i][j] - 1.0) * (gsl_sf_psi(uAMat[i][j]+Nij[i][j]) - gsl_sf_psi( sumUAArr[i]+Mi[i]) );
            lnqA -= gsl_sf_lngamma( uAMat[i][j] + Nij[i][j] );
        }
    }

    double lnpX = 0.0;
    for( n = 0 ; n < dLen ; n++ ){
        lnpX += log( cn[n] );
    }

    double val;
    val  = lnpPi + lnpA + lnpI;
    val -= lnqPi + lnqA + lnqI;
    val += lnpX;
    val += log(gsl_sf_fact(sNo));

    return val;
}

//double varLowerBoundG_pc( xns, gv, ivs )
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//{
//}

void reorderParameters_pc( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    pcParameters *p = (pcParameters*)gv->params;
    pcStats *s = (pcStats*)iv->stats;
    size_t dLen = xn->N;
    int sNo = gv->sNo;
    double **gmMat = iv->gmMat, ***xiMat = iv->xiMat;
    double *avgPi = p->avgPi, *avgLnPi = p->avgLnPi, **avgA = p->avgA, **avgLnA = p->avgLnA;
    double *avgI = p->avgI, *avgLnI = p->avgLnI;
    double *Ni = s->Ni, *Ci = s->Ci;
    size_t n;
    int i, j;

    int *index = (int*)malloc( sNo * sizeof(int) );
    double *store = (double*)malloc( sNo * sizeof(double) );
    double **s2D = (double**)malloc( sNo * sizeof(double*) );
    for( i = 0 ; i < sNo ; i++ )
    {   s2D[i] = (double*)malloc( sNo * sizeof(double) );   }

    // index indicates order of avgI values (0=biggest avgI -- sNo=smallest avgI).
    for( i = 0 ; i < sNo ; i++ ){
        index[i] = sNo - 1;
        for( j = 0 ; j < sNo ; j++ ){
            if( j != i ){
                if( avgI[i] < avgI[j] ){
                    index[i]--;
                } else if( avgI[i] == avgI[j] ){
                    if( i < j )
                    {   index[i]--;   }
                }
            }
        }
    }

    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = avgPi[i];   }
    for( i = 0 ; i < sNo ; i++ ){   avgPi[i] = store[i];   }

    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = avgLnPi[i];   }
    for( i = 0 ; i < sNo ; i++ ){   avgLnPi[i] = store[i];   }

    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = avgI[i];   }
    for( i = 0 ; i < sNo ; i++ ){   avgI[i] = store[i];   }

    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = avgLnI[i];   }
    for( i = 0 ; i < sNo ; i++ ){   avgLnI[i] = store[i];   }

    for( j = 0 ; j < sNo ; j++ ){
        for( i = 0 ; i < sNo ; i++ ){   s2D[index[i]][index[j]] = avgA[i][j];   }
    }
    for( j = 0 ; j < sNo ; j++ ){
        for( i = 0 ; i < sNo ; i++ ){   avgA[i][j] = s2D[i][j];   }
    }

    for( j = 0 ; j < sNo ; j++ ){
        for( i = 0 ; i < sNo ; i++ ){   s2D[index[i]][index[j]] = avgLnA[i][j];   }
    }
    for( j = 0 ; j < sNo ; j++ ){
        for( i = 0 ; i < sNo ; i++ ){   avgLnA[i][j] = s2D[i][j];   }
    }

    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = Ni[i];   }
    for( i = 0 ; i < sNo ; i++ ){   Ni[i] = store[i];   }

    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = Ci[i];   }
    for( i = 0 ; i < sNo ; i++ ){   Ci[i] = store[i];   }

    for( n = 0 ; n < dLen ; n++ ){
        for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = gmMat[n][i];   }
        for( i = 0 ; i < sNo ; i++ ){   gmMat[n][i] = store[i];   }
    }

    for( n = 0 ; n < dLen ; n++ ){
        for( j = 0 ; j < sNo ; j++ ){
            for( i = 0 ; i < sNo ; i++ ){   s2D[index[i]][index[j]] = xiMat[n][i][j];   }
        }
        for( j = 0 ; j < sNo ; j++ ){
            for( i = 0 ; i < sNo ; i++ ){   xiMat[n][i][j] = s2D[i][j];   }
        }
    }

    for( i = 0 ; i < sNo ; i++ ){   free( s2D[i] );   }
    free( s2D );
    free( store );
    free( index );
}

//void reorderParametersG_pc( xns, gv, ivs )
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//{
//}

void outputPcResults( xn, gv, iv, logFP )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
FILE *logFP;
{
    pcParameters *p = (pcParameters*)gv->params;
    int sNo = gv->sNo;

    int i, j;
    fprintf(logFP, "  results: K = %d \n", sNo);

    fprintf(logFP, "   intensity: ( %g", p->avgI[0]);
    for( i = 1 ; i < sNo ; i++ )
    {   fprintf(logFP, ", %g", p->avgI[i]);   }
    fprintf(logFP, " ) \n");

    fprintf(logFP, "   pi: ( %g", p->avgPi[0]);
    for( i = 1 ; i < sNo ; i++ ){
        fprintf(logFP, ", %g", p->avgPi[i]);
    }
    fprintf(logFP, " ) \n");

    fprintf(logFP, "   A_matrix: [");
    for( i = 0 ; i < sNo ; i++ ){
        fprintf(logFP, " ( %g", p->avgA[i][0]);
        for( j = 1 ; j < sNo ; j++ )
        {   fprintf(logFP, ", %g", p->avgA[i][j]);   }
        fprintf(logFP, ")");
    }
    fprintf(logFP, " ] \n\n");

    char fn[256];
    FILE *fp;
    size_t n;

    sprintf( fn, "%s.param%03d", xn->name, sNo );
    if( (fp = fopen( fn, "w")) != NULL ){
        fprintf(fp, "I, pi");
        for( i = 0 ; i < sNo ; i++ )
        {   fprintf(fp, ", A%dx", i);   }
        fprintf(fp, "\n");

        for( i = 0 ; i < sNo ; i++ ){
            fprintf(fp, "%g, %g", p->avgI[i], p->avgPi[i]);
            for( j = 0 ; j < sNo ; j++ )
            {   fprintf(fp, ", %g", p->avgA[j][i]);   }
            fprintf(fp, "\n");
        }
        fclose(fp);
    }

    sprintf( fn, "%s.Lq%03d", xn->name, sNo );
    if( (fp = fopen( fn, "w")) != NULL ){
        for( n = 0 ; n < gv->iteration ; n++ ){
            fprintf( fp, "%24.20e\n", gv->LqArr[n] );
        }
        fclose(fp);
    }

    sprintf( fn, "%s.maxS%03d", xn->name, sNo );
    if( (fp = fopen( fn, "w")) != NULL ){
        for( n = 0 ; n < xn->N ; n++ ){
            fprintf( fp, "%d\n", iv->stateTraj[n] );
        }
        fclose(fp);
    }

}

//void outputPcResultsG( xns, gv, ivs, logFP )
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//FILE *logFP;
//{
//}

//


/*
    learn.h
*/
#ifndef LEARN_H
#define LEARN_H
#include <stdlib.h>
#include <gsl/gsl_rng.h>
#include "feature.h"

//#define RANDOM ((double)rand()/(double)RAND_MAX)

int sampling_multinomial(gsl_rng *r, double *p, double *cum_sum_p, int len_p);

extern void mvslda_learn(document *data,
        double **resp,
        double *alpha,
        double beta,
        double nu2, double sigma2,
        int nclass, int nlex, int dlenmax, int nresp,
        int maxiter,
        double **phi, double **theta, double **eta,
        int **n_mz, int **n_zw,
        FILE *likp, FILE *hyperp,
        unsigned long int random_seed);

#endif


#ifndef CTETRA_WEIGHTS_H
#define CTETRA_WEIGHTS_H

#include <stdbool.h>
#include <gsl/gsl_vector.h>
#include "ecache.h"
#include "dos.h"

void WeightsAtK(double E_Fermi, int i, int j, int k, EnergyCache *Ecache, double *result);

void sortEsKs(double *Es, int **ks);

void WeightContrib(double E_Fermi, double E1, double E2, double E3, double E4, double num_tetra, double *ws);

void addCurvatureCorrection(double E_Fermi, double E1, double E2, double E3, double E4, double num_tetra, double *ws);

#endif // CTETRA_WEIGHTS_H


/*
 * Copyright (c) 2016-2021 lymastee, All rights reserved.
 * Contact: lymastee@hotmail.com
 *
 * This file is part of the gslib project.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#pragma once

#ifndef rtree_77e9ff20_6040_46f3_b87d_ab64a4ac506a_h
#define rtree_77e9ff20_6040_46f3_b87d_ab64a4ac506a_h

#include <gslib/error.h>
#include <gslib/tree.h>
#include <gslib/utility.h>

__gslib_begin__

class __gs_novtable rtree_spec abstract
{
public:
    enum type
    {
        spec_line,
        /* more... */
    };

    virtual ~rtree_spec() {}
    virtual type get_type() const = 0;
    virtual bool query_overlap(const rectf& rc) const = 0;
    virtual bool query_overlap(const pointf& p1, const pointf& p2) const = 0;
};

class rtree_spec_line:
    public rtree_spec
{
public:
    rtree_spec_line(const pointf& p1, const pointf& p2): _p1(p1), _p2(p2) {}
    virtual type get_type() const override { return spec_line; }
    virtual bool query_overlap(const rectf& rc) const override { return is_line_rect_overlapped(_p1, _p2, rc); }
    virtual bool query_overlap(const pointf& p1, const pointf& p2) const override
    {
        vec2 intsp, d1, d2;
        d1.sub(_p2, _p1);
        d2.sub(p2, p1);
        if(is_parallel(d1, d2))
            return false;
        intersectp_linear_linear(intsp, _p1, p1, d1, d2);
        float t1 = linear_reparameterize(_p1, _p2, intsp);
        if(t1 < 0.f || t1 > 1.f)
            return false;
        float t2 = linear_reparameterize(p1, p2, intsp);
        return t2 >= 0.f && t2 <= 1.f;
    }

private:
    pointf          _p1, _p2;
};

template<class _bind, int _bsize = sizeof(_bind)>
class rtree_entity
{
public:
    typedef rtree_entity<_bind, _bsize> myref;
    typedef _bind bind_type;
    typedef const _bind& bind_arg;

protected:
    rectf           _rect;
    bind_type       _bind_arg;
    rtree_spec*     _spec = nullptr;

public:
    rtree_entity() {}
    rtree_entity(bind_arg ba, const rectf& rc): _rect(rc), _bind_arg(ba) {}
    ~rtree_entity() { set_spec(nullptr); }
    const rectf& const_rect() const { return _rect; }
    rectf& get_rect() { return _rect; }
    bind_arg get_bind_arg() const { return _bind_arg; }
    rtree_spec* get_spec() const { return _spec; }
    void set_rect(const rectf& rc) { _rect = rc; }
    void set_bind_arg(bind_arg ba) { _bind_arg = ba; }
    void set_spec(rtree_spec* spec)
    {
        if(_spec)
            delete _spec;
        _spec = spec;
    }
    rtree_spec* detach_spec()
    {
        auto* spec = _spec;
        _spec = nullptr;
        return spec;
    }
};

template<class _bind>
class rtree_entity<_bind, sizeof(void*)>
{
public:
    typedef rtree_entity<_bind, sizeof(void*)> myref;
    typedef _bind bind_type;
    typedef _bind bind_arg;

protected:
    rectf           _rect;
    bind_type       _bind_arg = 0;
    rtree_spec*     _spec = nullptr;

public:
    rtree_entity() {}
    rtree_entity(bind_arg ba, const rectf& rc): _rect(rc), _bind_arg(ba) {}
    ~rtree_entity() { set_spec(nullptr); }
    const rectf& const_rect() const { return _rect; }
    rectf& get_rect() { return _rect; }
    bind_arg get_bind_arg() const { return _bind_arg; }
    rtree_spec* get_spec() const { return _spec; }
    void set_rect(const rectf& rc) { _rect = rc; }
    void set_bind_arg(bind_arg ba) { _bind_arg = ba; }
    void set_spec(rtree_spec* spec)
    {
        if(_spec)
            delete _spec;
        _spec = spec;
    }
    rtree_spec* detach_spec()
    {
        auto* spec = _spec;
        _spec = nullptr;
        return spec;
    }
};

template<class _entity>
using rtree_node = _treenode_cpy_wrapper<_entity>;

template<class _rect>
static float calc_enlarge_area(const _rect& rc1, const _rect& rc2, float s)
{
    assert(s == rc1.area());
    float s2 = rc2.area();
    _rect u, d;
    union_rect(u, rc1, rc2);
    if(intersect_rect(d, rc1, rc2))
        return u.area() - s - s2 + d.area();
    return u.area() - s - s2;
}

template<class _rect>
static float calc_enlarge_area(const _rect& rc1, const _rect& rc2)
{
    return calc_enlarge_area(rc1, rc2, rc1.area());
}

inline float calc_orientation(const float coef[3], const pointf& p)
{
    assert(coef);
    return coef[0] * p.x + coef[1] * p.y + coef[2];
}

inline void calc_perpendicular_bisector(float coef[3], const pointf& p1, const pointf& p2)
{
    assert(coef);
    pointf c;
    c.x = (p1.x + p2.x) * 0.5f;
    c.y = (p1.y + p2.y) * 0.5f;
    float d[2];
    d[0] = p1.y - p2.y;
    d[1] = p1.x - p2.x;
    if(abs(d[0]) < 1e-5f) {
        coef[0] = 1.f;
        coef[1] = -c.x;
        coef[2] = 0.f;
    }
    else if(abs(d[1]) < 1e-5f) {
        coef[0] = -c.y;
        coef[1] = 1.f;
        coef[2] = 0.f;
    }
    else {
        coef[0] = d[1] / d[0];
        coef[1] = 1.f;
        coef[2] = -c.x * coef[0] - c.y;
    }
    /* test orientation */
    float t = calc_orientation(coef, p1);
    if(t > 0.f) {
        coef[0] = -coef[0];
        coef[1] = -coef[1];
        coef[2] = -coef[2];
    }
}

template<class _tree>
class rtree_finder
{
public:
    typedef _tree mytree;
    typedef typename _tree::value value;
    typedef typename _tree::iterator iterator;
    typedef typename _tree::const_iterator const_iterator;
    typedef typename value::bind_arg bind_arg;

protected:
    mytree&         _mytree;

public:
    rtree_finder(mytree& mt): _mytree(mt) {}
    iterator find(bind_arg ba, const rectf& rc) const { return find(_mytree.get_root(), ba, rc); }

protected:
    iterator find(iterator p, bind_arg ba, const rectf& rc) const
    {
        assert(p.is_valid());
        if(!is_rect_intersected(rc, p->const_rect()))
            return iterator(nullptr);
        if(p.is_leaf())
            return (p->get_bind_arg() == ba) ? p : iterator(nullptr);
        for(auto i = p.child(); i.is_valid(); i.to_next()) {
            auto f = find(i, ba, rc);
            if(f.is_valid())
                return f;
        }
        return iterator(nullptr);
    }
};

/*
 * Here we implement an algorithm for the bulk loading. For a set of input inserting into
 * the rtree, original insert was a waste.
 * The algorithm was very similar to the STR insert as the following paper stated:
 * http://www.dtic.mil/dtic/tr/fulltext/u2/a324493.pdf
 */
template<int _max_record, int _min_record, class _tree>
class rtree_str_load
{
public:
    typedef _tree mytree;
    typedef typename _tree::value value;
    typedef typename _tree::wrapper wrapper;
    typedef typename _tree::iterator iterator;
    typedef typename _tree::const_iterator const_iterator;
    typedef typename _tree::value value;
    typedef typename value::bind_arg bind_arg;
    typedef list<value> value_list;
    static const int max_record = _max_record;
    static const int min_record = _min_record;
    struct bulk_node;
    typedef list<bulk_node*> bulk_list;
    typedef list<wrapper*> wrapper_list;

    struct bulk_node:
        public value
    {
        pointf      center;
        wrapper*    assoc_wrapper;

    public:
        bulk_node(const value& v):
            value(v)
        {
            auto& rc = v.const_rect();
            center.x = 0.5f * (rc.left + rc.right);
            center.y = 0.5f * (rc.top + rc.bottom);
            assoc_wrapper = nullptr;
        }
        bulk_node(wrapper* w):
            value(w->get_ref())
        {
            auto& v = w->get_ref();
            auto& rc = v.const_rect();
            center.x = 0.5f * (rc.left + rc.right);
            center.y = 0.5f * (rc.top + rc.bottom);
            assoc_wrapper = w;
        }
    };

public:
    rtree_str_load(mytree& tr): _mytree(tr) {}
    template<class _vsl>
    void load(const _vsl& input)
    {
        wrapper_list in, out;
        for(auto& p : input) {
            auto* w = new wrapper;
            w->born();
            auto& v = w->get_ref();
            v.set_bind_arg(p.get_bind_arg());
            v.set_rect(p.const_rect());
            v.set_spec(p.detach_spec());
            in.push_back(w);
        }
        for(;;) {
            int s = packing(out, in);
            if(s == 1) {
                _mytree.set_root(out.front());
                return;
            }
            out.swap(in);
            out.clear();
        }
    }

protected:
    mytree&         _mytree;

protected:
    void set_as_children(wrapper* p, wrapper_list& ch)
    {
        assert(p);
        assert(!ch.empty());
        int size = (int)ch.size();
        if(size == 1) {
            p->acquire_children(wrapper::children(ch.front()));
            return;
        }
        auto i = ch.begin(), j = std::next(i), end = ch.end();
        for(; j != end; i = j ++)
            wrapper::children::join(*i, *j);
        p->acquire_children(wrapper::children(ch.front(), ch.back(), (int)ch.size()));
    }
    void set_bound_rect(wrapper* p)
    {
        assert(p);
        float l = FLT_MAX, t = FLT_MAX, r = -FLT_MAX, b = -FLT_MAX;
        p->for_each([&](wrapper* w) {
            assert(w);
            auto& v = w->get_ref();
            auto& rc = v.get_rect();
            l = gs_min(l, rc.left);
            t = gs_min(t, rc.top);
            r = gs_max(r, rc.right);
            b = gs_max(b, rc.bottom);
        });
        p->born();
        auto& v = p->get_ref();
        rectf rc;
        rc.set_ltrb(l, t, r, b);
        v.set_rect(rc);
    }
    int packing(wrapper_list& output, wrapper_list& input)
    {
        assert(output.empty());
        assert(!input.empty());
        int size = (int)input.size();
        if(size <= max_record) {
            auto* p = new wrapper;
            set_as_children(p, input);
            set_bound_rect(p);
            output.push_back(p);
            return 1;
        }
        list<bulk_node> bulks;
        for(auto* w : input)
            bulks.push_back(bulk_node(w));
        bulks.sort([](const bulk_node& n1, const bulk_node& n2)-> bool {
            return n1.center.x < n2.center.x;
        });
        float rough_slice_count = sqrtf((float)size / max_record);
        float rough_slice_size = (float)size / rough_slice_count;
        int slice_size = (int)(rough_slice_size + 0.5f);
        int full_slice_count = size / slice_size;
        int slice_count = (size % slice_size) ? full_slice_count + 1 : full_slice_count;
        auto* slices = new list<bulk_node*>[slice_count];
        assert(slices);
        auto i = bulks.begin(), end = bulks.end();
        bool ascend = true;
        auto asc_pr = [](const bulk_node* n1, const bulk_node* n2)-> bool { return n1->center.y < n2->center.y; };
        auto desc_pr = [](const bulk_node* n1, const bulk_node* n2)-> bool { return n1->center.y > n2->center.y; };
        for(int j = 0, k = 0; i != end; ++ i) {
            assert(k < slice_count);
            slices[k].push_back(&(*i));
            if(++ j >= slice_size) {
                assert((int)slices[k].size() == slice_size);
                ascend ? slices[k].sort(asc_pr) : slices[k].sort(desc_pr);
                ascend = !ascend;
                j = 0, k ++;
            }
        }
        if(slice_count != full_slice_count) {
            assert(full_slice_count + 1 == slice_count);
            ascend ? slices[full_slice_count].sort(asc_pr) : slices[full_slice_count].sort(desc_pr);
        }
        /* join the list */
        vector<bulk_node*> sorted;
        for(int j = 0; j < slice_count; j ++)
            sorted.insert(sorted.end(), slices[j].begin(), slices[j].end());
        divide(output, sorted);
        delete [] slices;
        return (int)output.size();
    }
    void divide(wrapper_list& output, vector<bulk_node*>& sorted)
    {
        int size = (int)sorted.size();
        for(int i = 0; i < size; i += max_record) {
            int left = size - i;
            if(left < max_record + min_record) {
                if(left <= max_record) {
                    wrapper_list ch;
                    for(int j = i; j < size; j ++)
                        ch.push_back(sorted.at(j)->assoc_wrapper);
                    auto* w = new wrapper;
                    set_as_children(w, ch);
                    set_bound_rect(w);
                    output.push_back(w);
                }
                else {
                    int size1 = left / 2;
                    int size2 = left - size1;
                    wrapper_list ch1, ch2;
                    for(int j = 0; j < size1; j ++)
                        ch1.push_back(sorted.at(j + i)->assoc_wrapper);
                    for(int j = i + size1; j < size; j ++)
                        ch2.push_back(sorted.at(j)->assoc_wrapper);
                    auto* w1 = new wrapper;
                    auto* w2 = new wrapper;
                    set_as_children(w1, ch1);
                    set_bound_rect(w1);
                    set_as_children(w2, ch2);
                    set_bound_rect(w2);
                    output.push_back(w1);
                    output.push_back(w2);
                }
                return;
            }
            wrapper_list ch;
            for(int j = 0; j < max_record; j ++)
                ch.push_back(sorted.at(i + j)->assoc_wrapper);
            auto* w = new wrapper;
            set_as_children(w, ch);
            set_bound_rect(w);
            output.push_back(w);
        }
    }
};

template<int _max_record, int _min_record, class _tree>
class rtree_alg_base
{
public:
    typedef _tree mytree;
    typedef typename _tree::value value;
    typedef typename _tree::wrapper wrapper;
    typedef typename _tree::iterator iterator;
    typedef typename _tree::const_iterator const_iterator;
    typedef typename _tree::value value;
    typedef typename value::bind_arg bind_arg;
    typedef rtree_finder<mytree> finder;
    typedef list<value> value_list;
    static const int max_record = _max_record;
    static const int min_record = _min_record;

public:
    rtree_alg_base(mytree& tr): _mytree(tr) {}

protected:
    mytree&         _mytree;

protected:
    static void self_transfer(mytree& mt, iterator i, iterator dp)
    {
        assert(i.is_valid() && dp.is_valid());
        mytree t;
        mt.detach(t, i);
        mt.attach(t, mt.birth_tail(dp));
    }
    static void update_subtree_rect(iterator p)
    {
        assert(p.is_valid() && !p.is_leaf());
        auto i = p.child();
        rectf rc = i->const_rect();
        for(i.to_next(); i.is_valid(); i.to_next())
            union_rect(rc, rc, i->const_rect());
        p->set_rect(rc);
    }
    static void update_rect_recursively(iterator p)
    {
        assert(p.is_valid() && !p.is_leaf());
        auto i = p.child();
        rectf rc = i->const_rect();
        for(i.to_next(); i.is_valid(); i.to_next())
            union_rect(rc, rc, i->const_rect());
        if(p->const_rect() != rc) {
            p->set_rect(rc);
            auto g = p.parent();
            if(g.is_valid())
                update_rect_recursively(g);
        }
    }
    static void update_rect_recursively(iterator p, iterator i)
    {
        assert(p.is_valid() && i.is_valid() && (i.parent() == p));
        rectf rc = p->const_rect();
        union_rect(rc, rc, i->const_rect());
        if(p->const_rect() != rc) {
            p->set_rect(rc);
            auto g = p.parent();
            if(g.is_valid())
                update_rect_recursively(g, p);
        }
    }
    static void collect_leaves(value_list& candidates, iterator i)
    {
        assert(i.is_valid());
        if(i.is_leaf()) {
            candidates.push_back(*i);
            return;
        }
        for(auto j = i.child(); j.is_valid(); j.to_next())
            collect_leaves(candidates, j);
    }

protected:
    void split_binary(iterator p, iterator i, iterator j)
    {
        assert(p.is_valid() && (p.childs() == max_record + 1));
        assert(i.is_valid() && j.is_valid() && (i.parent() == p) && (j.parent() == p));
        auto g = p.parent();
        iterator q;
        if(g.is_valid())
            q = _mytree.insert_after(p);
        else {
            mytree mt;
            auto r = mt.insert(iterator(nullptr));
            r->set_rect(p->const_rect());
            auto p1 = mt.birth(r);
            auto p2 = mt.birth_tail(r);
            mt.attach(_mytree, p1);
            mt.swap(_mytree);
            p = p1;
            q = p2;
        }
        int max_split = max_record + 1 - min_record;
        /* retrieve the split line */
        auto c1 = i->const_rect().center();
        auto c2 = j->const_rect().center();
        float coef[3];
        calc_perpendicular_bisector(coef, c1, c2);
        /* sort them by signed distance */
        typedef multimap<float, iterator> sort_by_sd;
        sort_by_sd sbsd;
        float muler = 1.f / sqrtf(coef[0] * coef[0] + coef[1] * coef[1]);
        for(auto m = p.child(); m.is_valid(); m.to_next()) {
            if(m != i && m != j) {
                float d = muler * calc_orientation(coef, m->const_rect().center());
                sbsd.insert(std::make_pair(d, m));
            }
        }
        /* dump to a random access container */
        typedef vector<iterator> sd_queue;
        assert(sbsd.size() == max_record - 1);
        int divpos = -1, idx = 0;
        sd_queue sdq;
        sdq.resize(sbsd.size());
        for(auto n = sbsd.begin(); n != sbsd.end(); ++ n, ++ idx) {
            sdq.at(idx) = n->second;
            if((divpos < 0) && (n->first >= 0.f))
                divpos = idx;
        }
        if(divpos < 0)
            divpos = 0;
        /* clamp */
        if(divpos < min_record)
            divpos = min_record;
        else if(divpos > max_split)
            divpos = max_split;
        /* transfer from p to q */
        self_transfer(_mytree, j, q);
        for(auto pos = divpos; pos != (int)sdq.size(); ++ pos)
            self_transfer(_mytree, sdq.at(pos), q);
        /* update p, q's rect */
        update_subtree_rect(p);
        update_subtree_rect(q);
    }
};

/*
 * It's a bit weird that the Guttman's paper said that:
 * "PickNext simply chooses any of the remaining entries"
 * Does this algorithm actually useful, or maybe I implemented it wrong.
 * Use quadratic split in preference.
 * plus TODO:
 * R* algorithm to be implemented, but undone.
 */
template<int _max_record, int _min_record, class _tree>
class linear_split_alg:
    public rtree_alg_base<_max_record, _min_record, _tree>
{
public:
    linear_split_alg(mytree& tr):
        rtree_alg_base<_max_record, _min_record, _tree>(tr)
    {
        /* better be static_assert! */
        assert((min_record * 2 <= max_record) &&
            "min_record should be less than half max_record."
            );
    }
    iterator insert(bind_arg ba, const rectf& rc)
    {
        if(!_mytree.is_valid()) {
            auto i = _mytree.insert(iterator(nullptr));
            *i = value(ba, rc);
            return i;
        }
        auto i = find_insert_position(_mytree.get_root());
        if(i.is_valid()) {
            assert(i.childs() < max_record);
            auto j = _mytree.birth_tail(i);
            *j = value(ba, rc);
            update_rect_recursively(i, j);
            return j;
        }
        i = find_first_leaf(_mytree.get_root());
        assert(i.is_valid() && i.is_leaf());
        auto p = i.parent();
        if(!p.is_valid()) {
            assert(i == _mytree.get_root());
            auto j = _mytree.birth(i);
            auto k = _mytree.birth_tail(i);
            *j = *i;
            *k = value(ba, rc);
            rectf new_rc;
            union_rect(new_rc, i->const_rect(), rc);
            i->set_bind_arg(0);
            i->set_rect(new_rc);
            return k;
        }
        assert(p.childs() == max_record);
        /* temporarily insert into p, then split */
        auto j = _mytree.birth_tail(p);
        *j = value(ba, rc);
        update_rect_recursively(p, j);
        do { p = adjust_tree(p); }
        while(p.is_valid());
        return j;
    }
    void remove(bind_arg ba, const rectf& rc)
    {
        auto f = finder(_mytree).find(ba, rc);
        if(!f.is_valid())
            return;
        assert(f.is_leaf());
        if(f.is_root()) {
            _mytree.clear();
            return;
        }
        auto p = f.parent();
        assert(p.is_valid());
        _mytree.erase(f);
        if(p.childs() >= min_record) {
            update_rect_recursively(p);
            return;
        }
        value_list candidates;
        do { p = condense_tree(candidates, p); }
        while(p.is_valid());
        reinsert(candidates);
    }

protected:
    iterator find_insert_position(iterator i)
    {
        assert(i.is_valid());
        if(i.is_leaf())
            return iterator(nullptr);
        auto j = i.child();
        if(j.is_leaf())
            return (i.childs() < max_record) ? i : iterator(nullptr);
        for(; j.is_valid(); j.to_next()) {
            auto r = find_insert_position(j);
            if(r.is_valid())
                return r;
        }
        return iterator(nullptr);
    }
    iterator find_first_leaf(iterator i)
    {
        assert(i.is_valid());
        return i.is_leaf() ? i : find_first_leaf(i.child());
    }
    iterator find_extreme_u(iterator p)
    {
        assert(p.is_valid() && !p.is_leaf());
        auto i = p.child();
        assert(i.is_valid());
        float m = i->const_rect().width();
        auto r = i;
        for(i.to_next(); i.is_valid(); i.to_next()) {
            float u = i->const_rect().width();
            if(u > m)
                r = i, m = u;
        }
        return r;
    }
    iterator find_extreme_v_but_not(iterator p, iterator c)
    {
        assert(p.is_valid() && !p.is_leaf());
        assert(c.is_valid() && (c.parent() == p));
        iterator r;
        float m = -FLT_MAX;
        for(auto i = p.child(); i.is_valid(); i.to_next()) {
            if(i == c)
                continue;
            float v = i->const_rect().height();
            if(v > m)
                r = i, m = v;
        }
        assert(r.is_valid());
        return r;
    }
    void split(iterator p)
    {
        assert(p.is_valid() && (p.childs() == max_record + 1));
        auto i = find_extreme_u(p);
        auto j = find_extreme_v_but_not(p, i);
        split_binary(p, i, j);
    }
    iterator adjust_tree(iterator i)
    {
        assert(i.is_valid() && !i.is_leaf());
        assert(i.childs() == max_record + 1);
        auto p = i.parent();
        split(i);
        if(!p.is_valid())
            return iterator(nullptr);
        return (p.childs() <= max_record) ? iterator(nullptr) : p;
    }
    iterator condense_tree(value_list& candidates, iterator i)
    {
        assert(i.is_valid() && !i.is_leaf());
        assert(i.childs() < min_record);
        if(i.is_root()) {
            if(i.childs() == 1) {
                /* height decrease */
                mytree t;
                _mytree.detach(t, i.child());
                _mytree.swap(t);
            }
            return iterator(nullptr);
        }
        collect_leaves(candidates, i);
        auto p = i.parent();
        assert(p.is_valid());
        _mytree.erase(i);
        if(p.childs() >= min_record) {
            update_subtree_rect(p);
            return iterator(nullptr);
        }
        return p;
    }
    void reinsert(value_list& candidates)
    {
        if(candidates.empty())
            return;
        std::for_each(candidates.begin(), candidates.end(), [this](const value& val) {
            this->insert(val.get_bind_arg(), val.const_rect());
        });
    }
};

template<int _max_record, int _min_record, class _tree>
class quadratic_split_alg:
    public rtree_alg_base<_max_record, _min_record, _tree>
{
public:
    quadratic_split_alg(mytree& tr):
        rtree_alg_base<_max_record, _min_record, _tree>(tr)
    {
        /* better be static_assert! */
        assert((min_record * 2 <= max_record) &&
            "min_record should be less than half max_record."
            );
    }
    iterator insert(bind_arg ba, const rectf& rc)
    {
        if(!_mytree.is_valid()) {
            auto i = _mytree.insert(iterator(nullptr));
            *i = value(ba, rc);
            return i;
        }
        auto i = find_insert_position(_mytree.get_root(), rc);
        assert(i.is_valid());
        auto p = i.parent();
        if(!p.is_valid()) {
            assert(i == _mytree.get_root());
            auto j = _mytree.birth(i);
            auto k = _mytree.birth_tail(i);
            *j = *i;
            *k = value(ba, rc);
            rectf new_rc;
            union_rect(new_rc, i->const_rect(), rc);
            i->set_bind_arg(0);
            i->set_rect(new_rc);
            return k;
        }
        if(p.childs() < max_record) {
            auto j = _mytree.birth_tail(p);
            *j = value(ba, rc);
            update_rect_recursively(p, j);
            return j;
        }
        /* temporarily insert into p, then split */
        auto j = _mytree.birth_tail(p);
        *j = value(ba, rc);
        update_rect_recursively(p, j);
        do { p = adjust_tree(p); }
        while(p.is_valid());
        return j;
    }
    void remove(bind_arg ba, const rectf& rc)
    {
        auto f = finder(_mytree).find(ba, rc);
        if(!f.is_valid())
            return;
        assert(f.is_leaf());
        if(f.is_root()) {
            _mytree.clear();
            return;
        }
        auto p = f.parent();
        assert(p.is_valid());
        _mytree.erase(f);
        if(p.childs() >= min_record) {
            update_rect_recursively(p);
            return;
        }
        value_list candidates;
        do { p = condense_tree(candidates, p); }
        while(p.is_valid());
        reinsert(candidates);
    }

protected:
    iterator find_least_enlargement(iterator i, const rectf& rc)
    {
        assert(i.is_valid());
        auto c = i.child();
        auto ret = c;
        float enlargement = FLT_MAX;
        for(; c.is_valid(); c = c.next()) {
            const rectf& oldrc = c->const_rect();
            float oldarea = oldrc.area();
            rectf fr;
            union_rect(fr, oldrc, rc);
            float area = fr.area();
            float enlarge = area - oldarea;
            if(enlarge < enlargement) {
                enlargement = enlarge;
                ret = c;
            }
        }
        return ret;
    }
    float find_biggest_enlargement(iterator& i, iterator& j, iterator p)
    {
        assert(p.is_valid() && !p.is_leaf());
        assert(p.childs() == max_record + 1);
        iterator f = p.child(), g;
        assert(f.next().is_valid());
        float m = find_biggest_enlargement(g, f, f.next(), p.last_child());
        i = f, j = g;
        for(f.to_next(); f != p.last_child(); f.to_next()) {
            float n = find_biggest_enlargement(g, f, f.next(), p.last_child());
            if(n > m) {
                m = n;
                i = f, j = g;
            }
        }
        return m;
    }
    float find_biggest_enlargement(iterator& j, iterator i, iterator from, iterator to)
    {
        assert(i.is_valid() && from.is_valid() && to.is_valid());
        iterator t = from;
        float s = i->const_rect().area();
        float m = calc_enlarge_area(i->const_rect(), t->const_rect(), s);
        j = t;
        if(t == to)
            return m;
        for(t.to_next();; t.to_next()) {
            float n = calc_enlarge_area(i->const_rect(), t->const_rect(), s);
            if(n > m) { m = n, j = t; }
            if(t == to)
                break;
        }
        return m;
    }
    iterator find_insert_position(iterator i, const rectf& rc)
    {
        if(i.is_leaf())
            return i;
        auto trygetch = i.child();
        if(trygetch.is_leaf())
            return trygetch;
        auto j = find_least_enlargement(i, rc);
        assert(j.is_valid());
        return find_insert_position(j, rc);
    }
    void split(iterator p)
    {
        assert(p.is_valid() && (p.childs() == max_record + 1));
        iterator i, j;
        find_biggest_enlargement(i, j, p);
        split_binary(p, i, j);
    }
    iterator adjust_tree(iterator i)
    {
        assert(i.is_valid() && !i.is_leaf());
        assert(i.childs() == max_record + 1);
        auto p = i.parent();
        split(i);
        if(!p.is_valid())
            return iterator(nullptr);
        return (p.childs() <= max_record) ? iterator(nullptr) : p;
    }
    iterator condense_tree(value_list& candidates, iterator i)
    {
        assert(i.is_valid() && !i.is_leaf());
        assert(i.childs() < min_record);
        if(i.is_root()) {
            if(i.childs() == 1) {
                /* height decrease */
                mytree t;
                _mytree.detach(t, i.child());
                _mytree.swap(t);
            }
            return iterator(nullptr);
        }
        collect_leaves(candidates, i);
        auto p = i.parent();
        assert(p.is_valid());
        _mytree.erase(i);
        if(p.childs() >= min_record) {
            update_subtree_rect(p);
            return iterator(nullptr);
        }
        return p;
    }
    void reinsert(value_list& candidates)
    {
        if(candidates.empty())
            return;
        std::for_each(candidates.begin(), candidates.end(), [this](const value& val) {
            this->insert(val.get_bind_arg(), val.const_rect());
        });
    }
};

template<class _ty,
    class _alg,
    class _wrapper = rtree_node<_ty>,
    class _alloc = _tree_allocator<_wrapper>
    >
class rtree:
    public tree<_ty, _wrapper, _alloc>
{
public:
    typedef _ty value;
    typedef _wrapper wrapper;
    typedef _alloc alloc;
    typedef _alg alg;
    typedef rtree<_ty, _alg, _wrapper, _alloc> myref;
    typedef tree<_ty, _wrapper, _alloc> superref;
    typedef typename superref::iterator iterator;
    typedef typename superref::const_iterator const_iterator;
    typedef typename value::bind_arg bind_arg;
    typedef unordered_set<bind_arg> lookup_table;

public:
    template<class _querier, class _cont>
    int query(const _querier& q, _cont& out) const
    {
        if(!is_valid())
            return 0;
        lookup_table lookups;
        auto i = get_root();
        return query_overlapped(i, q, lookups, out);
    }
    template<class _cont>
    int query(const pointf& p1, const pointf& p2, _cont& out) const
    {
        if(!is_valid())
            return 0;
        lookup_table lookups;
        rectf rc;
        rc.set_by_pts(p1, p2);
        auto i = get_root();
        return query_overlapped(i, p1, p2, rc, lookups, out);
    }
    iterator insert(bind_arg ba, const rectf& rc) { return alg(*this).insert(ba, rc); }
    void remove(bind_arg ba, const rectf& rc) { alg(*this).remove(ba, rc); }
    bool empty() const { return !is_valid(); }
    iterator insert_line(bind_arg ba, const pointf& p1, const pointf& p2)
    {
        rectf rc;
        rc.set_by_pts(p1, p2);
        iterator i = insert(ba, rc);
        assert(i);
        i->set_spec(new rtree_spec_line(p1, p2));
        return i;
    }
    template<class _vsl>
    void insert(const _vsl& input)
    {
        if(input.size() == 1) {
            auto& p = input.front();
            insert(p.get_bind_arg(), p.const_rect());
            return;
        }
        rtree_str_load<alg::max_record, alg::min_record, alg::mytree>(*this).load(input);
    }
    void tracing() const
    {
        if(!is_valid())
            return;
        auto r = get_root();
        trace_subtree(r);
        trace_layer(r);
    }

protected:
    template<class _cont>
    int query_overlapped(const_iterator i, const rectf& q, lookup_table& lookups, _cont& out) const
    {
        assert(i.is_valid());
        const auto& rc = i->const_rect();
        if(!is_rect_intersected(rc, q))
            return 0;
        if(i.is_leaf()) {
            if(lookups.find(i->get_bind_arg()) != lookups.end())
                return 0;
            lookups.insert(i->get_bind_arg());
            out.push_back(i->get_bind_arg());
            return 1;
        }
        int c = 0;
        for(auto j = i.child(); j.is_valid(); j = j.next())
            c += query_overlapped(j, q, lookups, out);
        return c;
    }
    template<class _cont>
    int query_overlapped(const_iterator i, const pointf& q, lookup_table& lookups, _cont& out) const
    {
        assert(i.is_valid());
        const auto& rc = i->const_rect();
        if(!rc.in_rect(q))
            return 0;
        if(i.is_leaf()) {
            if(lookups.find(i->get_bind_arg()) != lookups.end())
                return 0;
            lookups.insert(i->get_bind_arg());
            out.push_back(i->get_bind_arg());
            return 1;
        }
        int c = 0;
        for(auto j = i.child(); j.is_valid(); j = j.next())
            c += query_overlapped(j, q, lookups, out);
        return c;
    }
    template<class _cont>
    int query_overlapped(const_iterator i, const pointf& p1, const pointf& p2, const rectf& rc, lookup_table& lookups, _cont& out) const
    {
        assert(i.is_valid());
        if(i.is_leaf()) {
            auto* spec = i->get_spec();
            bool isovl = spec ? spec->query_overlap(p1, p2) : is_rect_intersected(i->const_rect(), rc);
            if(isovl && lookups.find(i->get_bind_arg()) == lookups.end()) {
                lookups.insert(i->get_bind_arg());
                out.push_back(i->get_bind_arg());
                return 1;
            }
            return 0;
        }
        if(!is_rect_intersected(i->const_rect(), rc))
            return 0;
        int c = 0;
        for(auto j = i.child(); j.is_valid(); j = j.next())
            c += query_overlapped(j, p1, p2, rc, lookups, out);
        return c;
    }
    void trace_layer(const_iterator i) const
    {
        assert(i.is_valid());
        int r = rand() % 256;
        int g = rand() % 256;
        int b = rand() % 256;
        string cr;
        cr.format(_t("rgb(%d,%d,%d)"), r, g, b);
        trace(_t("@!\n"));
        trace(_t("@&strokeColor=%s;\n"), cr.c_str());
        trace(_t("@&withArrow=false;\n"));
        for(auto j = i; j.is_valid(); j = j.next()) {
            const auto& rc = j->const_rect();
            trace(_t("@rect %f, %f, %f, %f;\n"), rc.left, rc.top, rc.right, rc.bottom);
        }
        trace(_t("@@\n"));
    }
    void trace_subtree(const_iterator i) const
    {
        assert(i.is_valid());
        if(i.is_leaf())
            return;
        for(auto j = i.child(); j.is_valid(); j = j.next())
            trace_subtree(j);
        trace_layer(i.child());
    }
};

__gslib_end__

#endif