#include "ccv.h"
#include "ccv_internal.h"
#include <sys/time.h>
#ifdef HAVE_GSL
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#endif
#ifdef USE_OPENMP
#include <omp.h>
#endif

const ccv_bbf_param_t ccv_bbf_default_params = {
    .interval = 5,
    .min_neighbors = 2,
    .accurate = 1,
    .flags = 0,
    .size = {
        24,
        24,
    },
};

#define _ccv_width_padding(x) (((x) + 3) & -4)

static inline int _ccv_run_bbf_feature(ccv_bbf_feature_t *feature, int *step, unsigned char **u8)
{
#define pf_at(i) (*(u8[feature->pz[i]] + feature->px[i] + feature->py[i] * step[feature->pz[i]]))
#define nf_at(i) (*(u8[feature->nz[i]] + feature->nx[i] + feature->ny[i] * step[feature->nz[i]]))
  unsigned char pmin = pf_at(0), nmax = nf_at(0);
  /* check if every point in P > every point in N, and take a shortcut */
  if (pmin <= nmax)
    return 0;
  int i;
  for (i = 1; i < feature->size; i++)
  {
    if (feature->pz[i] >= 0)
    {
      int p = pf_at(i);
      if (p < pmin)
      {
        if (p <= nmax)
          return 0;
        pmin = p;
      }
    }
    if (feature->nz[i] >= 0)
    {
      int n = nf_at(i);
      if (n > nmax)
      {
        if (pmin <= n)
          return 0;
        nmax = n;
      }
    }
  }
#undef pf_at
#undef nf_at
  return 1;
}

static int _ccv_read_bbf_stage_classifier(const char *file, ccv_bbf_stage_classifier_t *classifier)
{
  FILE *r = fopen(file, "r");
  if (r == 0)
    return -1;
  int stat = 0;
  stat |= fscanf(r, "%d", &classifier->count);
  union {
    float fl;
    int i;
  } fli;
  stat |= fscanf(r, "%d", &fli.i);
  classifier->threshold = fli.fl;
  classifier->feature = (ccv_bbf_feature_t *)ccmalloc(classifier->count * sizeof(ccv_bbf_feature_t));
  classifier->alpha = (float *)ccmalloc(classifier->count * 2 * sizeof(float));
  int i, j;
  for (i = 0; i < classifier->count; i++)
  {
    stat |= fscanf(r, "%d", &classifier->feature[i].size);
    for (j = 0; j < classifier->feature[i].size; j++)
    {
      stat |= fscanf(r, "%d %d %d", &classifier->feature[i].px[j], &classifier->feature[i].py[j], &classifier->feature[i].pz[j]);
      stat |= fscanf(r, "%d %d %d", &classifier->feature[i].nx[j], &classifier->feature[i].ny[j], &classifier->feature[i].nz[j]);
    }
    union {
      float fl;
      int i;
    } flia, flib;
    stat |= fscanf(r, "%d %d", &flia.i, &flib.i);
    classifier->alpha[i * 2] = flia.fl;
    classifier->alpha[i * 2 + 1] = flib.fl;
  }
  fclose(r);
  return 0;
}

#ifdef HAVE_GSL

static unsigned int _ccv_bbf_time_measure()
{
  struct timeval tv;
  gettimeofday(&tv, 0);
  return tv.tv_sec * 1000000 + tv.tv_usec;
}

#define less_than(a, b, aux) ((a) < (b))
CCV_IMPLEMENT_QSORT(_ccv_sort_32f, float, less_than)
#undef less_than

static void _ccv_bbf_eval_data(ccv_bbf_stage_classifier_t *classifier, unsigned char **posdata, int posnum, unsigned char **negdata, int negnum, ccv_size_t size, float *peval, float *neval)
{
  int i, j;
  int steps[] = {_ccv_width_padding(size.width),
                 _ccv_width_padding(size.width >> 1),
                 _ccv_width_padding(size.width >> 2)};
  int isizs0 = steps[0] * size.height;
  int isizs01 = isizs0 + steps[1] * (size.height >> 1);
  for (i = 0; i < posnum; i++)
  {
    unsigned char *u8[] = {posdata[i], posdata[i] + isizs0, posdata[i] + isizs01};
    float sum = 0;
    float *alpha = classifier->alpha;
    ccv_bbf_feature_t *feature = classifier->feature;
    for (j = 0; j < classifier->count; ++j, alpha += 2, ++feature)
      sum += alpha[_ccv_run_bbf_feature(feature, steps, u8)];
    peval[i] = sum;
  }
  for (i = 0; i < negnum; i++)
  {
    unsigned char *u8[] = {negdata[i], negdata[i] + isizs0, negdata[i] + isizs01};
    float sum = 0;
    float *alpha = classifier->alpha;
    ccv_bbf_feature_t *feature = classifier->feature;
    for (j = 0; j < classifier->count; ++j, alpha += 2, ++feature)
      sum += alpha[_ccv_run_bbf_feature(feature, steps, u8)];
    neval[i] = sum;
  }
}

static int _ccv_prune_positive_data(ccv_bbf_classifier_cascade_t *cascade, unsigned char **posdata, int posnum, ccv_size_t size)
{
  float *peval = (float *)ccmalloc(posnum * sizeof(float));
  int i, j, k, rpos = posnum;
  for (i = 0; i < cascade->count; i++)
  {
    _ccv_bbf_eval_data(cascade->stage_classifier + i, posdata, rpos, 0, 0, size, peval, 0);
    k = 0;
    for (j = 0; j < rpos; j++)
      if (peval[j] >= cascade->stage_classifier[i].threshold)
      {
        posdata[k] = posdata[j];
        ++k;
      }
      else
      {
        ccfree(posdata[j]);
      }
    rpos = k;
  }
  ccfree(peval);
  return rpos;
}

static int _ccv_prepare_background_data(ccv_bbf_classifier_cascade_t *cascade, char **bgfiles, int bgnum, unsigned char **negdata, int negnum)
{
  int t, i, j, k, q;
  int negperbg;
  int negtotal = 0;
  int steps[] = {_ccv_width_padding(cascade->size.width),
                 _ccv_width_padding(cascade->size.width >> 1),
                 _ccv_width_padding(cascade->size.width >> 2)};
  int isizs0 = steps[0] * cascade->size.height;
  int isizs1 = steps[1] * (cascade->size.height >> 1);
  int isizs2 = steps[2] * (cascade->size.height >> 2);
  int *idcheck = (int *)ccmalloc(negnum * sizeof(int));

  gsl_rng_env_setup();

  gsl_rng *rng = gsl_rng_alloc(gsl_rng_default);
  gsl_rng_set(rng, (unsigned long int)idcheck);

  ccv_size_t imgsz = cascade->size;
  int rneg = negtotal;
  for (t = 0; negtotal < negnum; t++)
  {
    PRINT(CCV_CLI_INFO, "preparing negative data ...  0%%");
    for (i = 0; i < bgnum; i++)
    {
      negperbg = (t < 2) ? (negnum - negtotal) / (bgnum - i) + 1 : negnum - negtotal;
      ccv_dense_matrix_t *image = 0;
      ccv_read(bgfiles[i], &image, CCV_IO_GRAY | CCV_IO_ANY_FILE);
      assert((image->type & CCV_C1) && (image->type & CCV_8U));
      if (image == 0)
      {
        PRINT(CCV_CLI_ERROR, "\n%s file corrupted\n", bgfiles[i]);
        continue;
      }
      if (t % 2 != 0)
        ccv_flip(image, 0, 0, CCV_FLIP_X);
      if (t % 4 >= 2)
        ccv_flip(image, 0, 0, CCV_FLIP_Y);
      ccv_bbf_param_t params = {.interval = 3, .min_neighbors = 0, .accurate = 1, .flags = 0, .size = cascade->size};
      ccv_array_t *detected = ccv_bbf_detect_objects(image, &cascade, 1, params);
      memset(idcheck, 0, ccv_min(detected->rnum, negperbg) * sizeof(int));
      for (j = 0; j < ccv_min(detected->rnum, negperbg); j++)
      {
        int r = gsl_rng_uniform_int(rng, detected->rnum);
        int flag = 1;
        ccv_rect_t *rect = (ccv_rect_t *)ccv_array_get(detected, r);
        while (flag)
        {
          flag = 0;
          for (k = 0; k < j; k++)
            if (r == idcheck[k])
            {
              flag = 1;
              r = gsl_rng_uniform_int(rng, detected->rnum);
              break;
            }
          rect = (ccv_rect_t *)ccv_array_get(detected, r);
          if ((rect->x < 0) || (rect->y < 0) || (rect->width + rect->x > image->cols) || (rect->height + rect->y > image->rows))
          {
            flag = 1;
            r = gsl_rng_uniform_int(rng, detected->rnum);
          }
        }
        idcheck[j] = r;
        ccv_dense_matrix_t *temp = 0;
        ccv_dense_matrix_t *imgs0 = 0;
        ccv_dense_matrix_t *imgs1 = 0;
        ccv_dense_matrix_t *imgs2 = 0;
        ccv_slice(image, (ccv_matrix_t **)&temp, 0, rect->y, rect->x, rect->height, rect->width);
        ccv_resample(temp, &imgs0, 0, imgsz.height, imgsz.width, CCV_INTER_AREA);
        assert(imgs0->step == steps[0]);
        ccv_matrix_free(temp);
        ccv_sample_down(imgs0, &imgs1, 0, 0, 0);
        assert(imgs1->step == steps[1]);
        ccv_sample_down(imgs1, &imgs2, 0, 0, 0);
        assert(imgs2->step == steps[2]);

        negdata[negtotal] = (unsigned char *)ccmalloc(isizs0 + isizs1 + isizs2);
        unsigned char *u8s0 = negdata[negtotal];
        unsigned char *u8s1 = negdata[negtotal] + isizs0;
        unsigned char *u8s2 = negdata[negtotal] + isizs0 + isizs1;
        unsigned char *u8[] = {u8s0, u8s1, u8s2};
        memcpy(u8s0, imgs0->data.u8, imgs0->rows * imgs0->step);
        ccv_matrix_free(imgs0);
        memcpy(u8s1, imgs1->data.u8, imgs1->rows * imgs1->step);
        ccv_matrix_free(imgs1);
        memcpy(u8s2, imgs2->data.u8, imgs2->rows * imgs2->step);
        ccv_matrix_free(imgs2);

        flag = 1;
        ccv_bbf_stage_classifier_t *classifier = cascade->stage_classifier;
        for (k = 0; k < cascade->count; ++k, ++classifier)
        {
          float sum = 0;
          float *alpha = classifier->alpha;
          ccv_bbf_feature_t *feature = classifier->feature;
          for (q = 0; q < classifier->count; ++q, alpha += 2, ++feature)
            sum += alpha[_ccv_run_bbf_feature(feature, steps, u8)];
          if (sum < classifier->threshold)
          {
            flag = 0;
            break;
          }
        }
        if (!flag)
          ccfree(negdata[negtotal]);
        else
        {
          ++negtotal;
          if (negtotal >= negnum)
            break;
        }
      }
      ccv_array_free(detected);
      ccv_matrix_free(image);
      ccv_drain_cache();
      PRINT(CCV_CLI_INFO, "\rpreparing negative data ... %2d%%", 100 * negtotal / negnum);
      fflush(0);
      if (negtotal >= negnum)
        break;
    }
    if (rneg == negtotal)
      break;
    rneg = negtotal;
    PRINT(CCV_CLI_INFO, "\nentering additional round %d\n", t + 1);
  }
  gsl_rng_free(rng);
  ccfree(idcheck);
  ccv_drain_cache();
  PRINT(CCV_CLI_INFO, "\n");
  return negtotal;
}

static void _ccv_prepare_positive_data(ccv_dense_matrix_t **posimg, unsigned char **posdata, ccv_size_t size, int posnum)
{
  PRINT(CCV_CLI_INFO, "preparing positive data ...  0%%");
  int i;
  for (i = 0; i < posnum; i++)
  {
    ccv_dense_matrix_t *imgs0 = posimg[i];
    ccv_dense_matrix_t *imgs1 = 0;
    ccv_dense_matrix_t *imgs2 = 0;
    assert((imgs0->type & CCV_C1) && (imgs0->type & CCV_8U) && imgs0->rows == size.height && imgs0->cols == size.width);
    ccv_sample_down(imgs0, &imgs1, 0, 0, 0);
    ccv_sample_down(imgs1, &imgs2, 0, 0, 0);
    int isizs0 = imgs0->rows * imgs0->step;
    int isizs1 = imgs1->rows * imgs1->step;
    int isizs2 = imgs2->rows * imgs2->step;

    posdata[i] = (unsigned char *)ccmalloc(isizs0 + isizs1 + isizs2);
    memcpy(posdata[i], imgs0->data.u8, isizs0);
    memcpy(posdata[i] + isizs0, imgs1->data.u8, isizs1);
    memcpy(posdata[i] + isizs0 + isizs1, imgs2->data.u8, isizs2);

    PRINT(CCV_CLI_INFO, "\rpreparing positive data ... %2d%%", 100 * (i + 1) / posnum);
    fflush(0);

    ccv_matrix_free(imgs1);
    ccv_matrix_free(imgs2);
  }
  ccv_drain_cache();
  PRINT(CCV_CLI_INFO, "\n");
}

typedef struct
{
  double fitness;
  int pk, nk;
  int age;
  double error;
  ccv_bbf_feature_t feature;
} ccv_bbf_gene_t;

static inline void _ccv_bbf_genetic_fitness(ccv_bbf_gene_t *gene)
{
  gene->fitness = (1 - gene->error) * exp(-0.01 * gene->age) * exp((gene->pk + gene->nk) * log(1.015));
}

static inline int _ccv_bbf_exist_gene_feature(ccv_bbf_gene_t *gene, int x, int y, int z)
{
  int i;
  for (i = 0; i < gene->pk; i++)
    if (z == gene->feature.pz[i] && x == gene->feature.px[i] && y == gene->feature.py[i])
      return 1;
  for (i = 0; i < gene->nk; i++)
    if (z == gene->feature.nz[i] && x == gene->feature.nx[i] && y == gene->feature.ny[i])
      return 1;
  return 0;
}

static inline void _ccv_bbf_randomize_gene(gsl_rng *rng, ccv_bbf_gene_t *gene, int *rows, int *cols)
{
  int i;
  do
  {
    gene->pk = gsl_rng_uniform_int(rng, CCV_BBF_POINT_MAX - 1) + 1;
    gene->nk = gsl_rng_uniform_int(rng, CCV_BBF_POINT_MAX - 1) + 1;
  } while (gene->pk + gene->nk < CCV_BBF_POINT_MIN); /* a hard restriction of at least 3 points have to be examed */
  gene->feature.size = ccv_max(gene->pk, gene->nk);
  gene->age = 0;
  for (i = 0; i < CCV_BBF_POINT_MAX; i++)
  {
    gene->feature.pz[i] = -1;
    gene->feature.nz[i] = -1;
  }
  int x, y, z;
  for (i = 0; i < gene->pk; i++)
  {
    do
    {
      z = gsl_rng_uniform_int(rng, 3);
      x = gsl_rng_uniform_int(rng, cols[z]);
      y = gsl_rng_uniform_int(rng, rows[z]);
    } while (_ccv_bbf_exist_gene_feature(gene, x, y, z));
    gene->feature.pz[i] = z;
    gene->feature.px[i] = x;
    gene->feature.py[i] = y;
  }
  for (i = 0; i < gene->nk; i++)
  {
    do
    {
      z = gsl_rng_uniform_int(rng, 3);
      x = gsl_rng_uniform_int(rng, cols[z]);
      y = gsl_rng_uniform_int(rng, rows[z]);
    } while (_ccv_bbf_exist_gene_feature(gene, x, y, z));
    gene->feature.nz[i] = z;
    gene->feature.nx[i] = x;
    gene->feature.ny[i] = y;
  }
}

static inline double _ccv_bbf_error_rate(ccv_bbf_feature_t *feature, unsigned char **posdata, int posnum, unsigned char **negdata, int negnum, ccv_size_t size, double *pw, double *nw)
{
  int i;
  int steps[] = {_ccv_width_padding(size.width),
                 _ccv_width_padding(size.width >> 1),
                 _ccv_width_padding(size.width >> 2)};
  int isizs0 = steps[0] * size.height;
  int isizs01 = isizs0 + steps[1] * (size.height >> 1);
  double error = 0;
  for (i = 0; i < posnum; i++)
  {
    unsigned char *u8[] = {posdata[i], posdata[i] + isizs0, posdata[i] + isizs01};
    if (!_ccv_run_bbf_feature(feature, steps, u8))
      error += pw[i];
  }
  for (i = 0; i < negnum; i++)
  {
    unsigned char *u8[] = {negdata[i], negdata[i] + isizs0, negdata[i] + isizs01};
    if (_ccv_run_bbf_feature(feature, steps, u8))
      error += nw[i];
  }
  return error;
}

#define less_than(fit1, fit2, aux) ((fit1).fitness >= (fit2).fitness)
static CCV_IMPLEMENT_QSORT(_ccv_bbf_genetic_qsort, ccv_bbf_gene_t, less_than)
#undef less_than

    static ccv_bbf_feature_t _ccv_bbf_genetic_optimize(unsigned char **posdata, int posnum, unsigned char **negdata, int negnum, int ftnum, ccv_size_t size, double *pw, double *nw)
{
  ccv_bbf_feature_t best;
  /* seed (random method) */
  gsl_rng_env_setup();
  gsl_rng *rng = gsl_rng_alloc(gsl_rng_default);
  union {
    unsigned long int li;
    double db;
  } dbli;
  dbli.db = pw[0] + nw[0];
  gsl_rng_set(rng, dbli.li);
  int i, j;
  int pnum = ftnum * 100;
  assert(pnum > 0);
  ccv_bbf_gene_t *gene = (ccv_bbf_gene_t *)ccmalloc(pnum * sizeof(ccv_bbf_gene_t));
  int rows[] = {size.height, size.height >> 1, size.height >> 2};
  int cols[] = {size.width, size.width >> 1, size.width >> 2};
  for (i = 0; i < pnum; i++)
    _ccv_bbf_randomize_gene(rng, &gene[i], rows, cols);
  unsigned int timer = _ccv_bbf_time_measure();
#ifdef USE_OPENMP
#pragma omp parallel for private(i) schedule(dynamic)
#endif
  for (i = 0; i < pnum; i++)
    gene[i].error = _ccv_bbf_error_rate(&gene[i].feature, posdata, posnum, negdata, negnum, size, pw, nw);
  timer = _ccv_bbf_time_measure() - timer;
  for (i = 0; i < pnum; i++)
    _ccv_bbf_genetic_fitness(&gene[i]);
  double best_err = 1;
  int rnum = ftnum * 39; /* number of randomize */
  int mnum = ftnum * 40; /* number of mutation */
  int hnum = ftnum * 20; /* number of hybrid */
  /* iteration stop crit : best no change in 40 iterations */
  int it = 0, t;
  for (t = 0; it < 40; ++it, ++t)
  {
    int min_id = 0;
    double min_err = gene[0].error;
    for (i = 1; i < pnum; i++)
      if (gene[i].error < min_err)
      {
        min_id = i;
        min_err = gene[i].error;
      }
    min_err = gene[min_id].error = _ccv_bbf_error_rate(&gene[min_id].feature, posdata, posnum, negdata, negnum, size, pw, nw);
    if (min_err < best_err)
    {
      best_err = min_err;
      memcpy(&best, &gene[min_id].feature, sizeof(best));
      PRINT(CCV_CLI_INFO, "best bbf feature with error %f\n|-size: %d\n|-positive point: ", best_err, best.size);
      for (i = 0; i < best.size; i++)
        PRINT(CCV_CLI_INFO, "(%d %d %d), ", best.px[i], best.py[i], best.pz[i]);
      PRINT(CCV_CLI_INFO, "\n|-negative point: ");
      for (i = 0; i < best.size; i++)
        PRINT(CCV_CLI_INFO, "(%d %d %d), ", best.nx[i], best.ny[i], best.nz[i]);
      PRINT(CCV_CLI_INFO, "\n");
      it = 0;
    }
    PRINT(CCV_CLI_INFO, "minimum error achieved in round %d(%d) : %f with %d ms\n", t, it, min_err, timer / 1000);
    _ccv_bbf_genetic_qsort(gene, pnum, 0);
    for (i = 0; i < ftnum; i++)
      ++gene[i].age;
    for (i = ftnum; i < ftnum + mnum; i++)
    {
      int parent = gsl_rng_uniform_int(rng, ftnum);
      memcpy(gene + i, gene + parent, sizeof(ccv_bbf_gene_t));
      /* three mutation strategy : 1. add, 2. remove, 3. refine */
      int pnm, pn = gsl_rng_uniform_int(rng, 2);
      int *pnk[] = {&gene[i].pk, &gene[i].nk};
      int *pnx[] = {gene[i].feature.px, gene[i].feature.nx};
      int *pny[] = {gene[i].feature.py, gene[i].feature.ny};
      int *pnz[] = {gene[i].feature.pz, gene[i].feature.nz};
      int x, y, z;
      int victim, decay = 1;
      do
      {
        switch (gsl_rng_uniform_int(rng, 3))
        {
        case 0: /* add */
          if (gene[i].pk == CCV_BBF_POINT_MAX && gene[i].nk == CCV_BBF_POINT_MAX)
            break;
          while (*pnk[pn] + 1 > CCV_BBF_POINT_MAX)
            pn = gsl_rng_uniform_int(rng, 2);
          do
          {
            z = gsl_rng_uniform_int(rng, 3);
            x = gsl_rng_uniform_int(rng, cols[z]);
            y = gsl_rng_uniform_int(rng, rows[z]);
          } while (_ccv_bbf_exist_gene_feature(&gene[i], x, y, z));
          pnz[pn][*pnk[pn]] = z;
          pnx[pn][*pnk[pn]] = x;
          pny[pn][*pnk[pn]] = y;
          ++(*pnk[pn]);
          gene[i].feature.size = ccv_max(gene[i].pk, gene[i].nk);
          decay = gene[i].age = 0;
          break;
        case 1:                                             /* remove */
          if (gene[i].pk + gene[i].nk <= CCV_BBF_POINT_MIN) /* at least 3 points have to be examed */
            break;
          while (*pnk[pn] - 1 <= 0) // || *pnk[pn] + *pnk[!pn] - 1 < CCV_BBF_POINT_MIN)
            pn = gsl_rng_uniform_int(rng, 2);
          victim = gsl_rng_uniform_int(rng, *pnk[pn]);
          for (j = victim; j < *pnk[pn] - 1; j++)
          {
            pnz[pn][j] = pnz[pn][j + 1];
            pnx[pn][j] = pnx[pn][j + 1];
            pny[pn][j] = pny[pn][j + 1];
          }
          pnz[pn][*pnk[pn] - 1] = -1;
          --(*pnk[pn]);
          gene[i].feature.size = ccv_max(gene[i].pk, gene[i].nk);
          decay = gene[i].age = 0;
          break;
        case 2: /* refine */
          pnm = gsl_rng_uniform_int(rng, *pnk[pn]);
          do
          {
            z = gsl_rng_uniform_int(rng, 3);
            x = gsl_rng_uniform_int(rng, cols[z]);
            y = gsl_rng_uniform_int(rng, rows[z]);
          } while (_ccv_bbf_exist_gene_feature(&gene[i], x, y, z));
          pnz[pn][pnm] = z;
          pnx[pn][pnm] = x;
          pny[pn][pnm] = y;
          decay = gene[i].age = 0;
          break;
        }
      } while (decay);
    }
    for (i = ftnum + mnum; i < ftnum + mnum + hnum; i++)
    {
      /* hybrid strategy: taking positive points from dad, negative points from mum */
      int dad, mum;
      do
      {
        dad = gsl_rng_uniform_int(rng, ftnum);
        mum = gsl_rng_uniform_int(rng, ftnum);
      } while (dad == mum || gene[dad].pk + gene[mum].nk < CCV_BBF_POINT_MIN); /* at least 3 points have to be examed */
      for (j = 0; j < CCV_BBF_POINT_MAX; j++)
      {
        gene[i].feature.pz[j] = -1;
        gene[i].feature.nz[j] = -1;
      }
      gene[i].pk = gene[dad].pk;
      for (j = 0; j < gene[i].pk; j++)
      {
        gene[i].feature.pz[j] = gene[dad].feature.pz[j];
        gene[i].feature.px[j] = gene[dad].feature.px[j];
        gene[i].feature.py[j] = gene[dad].feature.py[j];
      }
      gene[i].nk = gene[mum].nk;
      for (j = 0; j < gene[i].nk; j++)
      {
        gene[i].feature.nz[j] = gene[mum].feature.nz[j];
        gene[i].feature.nx[j] = gene[mum].feature.nx[j];
        gene[i].feature.ny[j] = gene[mum].feature.ny[j];
      }
      gene[i].feature.size = ccv_max(gene[i].pk, gene[i].nk);
      gene[i].age = 0;
    }
    for (i = ftnum + mnum + hnum; i < ftnum + mnum + hnum + rnum; i++)
      _ccv_bbf_randomize_gene(rng, &gene[i], rows, cols);
    timer = _ccv_bbf_time_measure();
#ifdef USE_OPENMP
#pragma omp parallel for private(i) schedule(dynamic)
#endif
    for (i = 0; i < pnum; i++)
      gene[i].error = _ccv_bbf_error_rate(&gene[i].feature, posdata, posnum, negdata, negnum, size, pw, nw);
    timer = _ccv_bbf_time_measure() - timer;
    for (i = 0; i < pnum; i++)
      _ccv_bbf_genetic_fitness(&gene[i]);
  }
  ccfree(gene);
  gsl_rng_free(rng);
  return best;
}

#define less_than(fit1, fit2, aux) ((fit1).error < (fit2).error)
static CCV_IMPLEMENT_QSORT(_ccv_bbf_best_qsort, ccv_bbf_gene_t, less_than)
#undef less_than

    static ccv_bbf_gene_t _ccv_bbf_best_gene(ccv_bbf_gene_t *gene, int pnum, int point_min, unsigned char **posdata, int posnum, unsigned char **negdata, int negnum, ccv_size_t size, double *pw, double *nw)
{
  int i;
  unsigned int timer = _ccv_bbf_time_measure();
#ifdef USE_OPENMP
#pragma omp parallel for private(i) schedule(dynamic)
#endif
  for (i = 0; i < pnum; i++)
    gene[i].error = _ccv_bbf_error_rate(&gene[i].feature, posdata, posnum, negdata, negnum, size, pw, nw);
  timer = _ccv_bbf_time_measure() - timer;
  _ccv_bbf_best_qsort(gene, pnum, 0);
  int min_id = 0;
  double min_err = gene[0].error;
  for (i = 0; i < pnum; i++)
    if (gene[i].nk + gene[i].pk >= point_min)
    {
      min_id = i;
      min_err = gene[i].error;
      break;
    }
  PRINT(CCV_CLI_INFO, "local best bbf feature with error %f\n|-size: %d\n|-positive point: ", min_err, gene[min_id].feature.size);
  for (i = 0; i < gene[min_id].feature.size; i++)
    PRINT(CCV_CLI_INFO, "(%d %d %d), ", gene[min_id].feature.px[i], gene[min_id].feature.py[i], gene[min_id].feature.pz[i]);
  PRINT(CCV_CLI_INFO, "\n|-negative point: ");
  for (i = 0; i < gene[min_id].feature.size; i++)
    PRINT(CCV_CLI_INFO, "(%d %d %d), ", gene[min_id].feature.nx[i], gene[min_id].feature.ny[i], gene[min_id].feature.nz[i]);
  PRINT(CCV_CLI_INFO, "\nthe computation takes %d ms\n", timer / 1000);
  return gene[min_id];
}

static ccv_bbf_feature_t _ccv_bbf_convex_optimize(unsigned char **posdata, int posnum, unsigned char **negdata, int negnum, ccv_bbf_feature_t *best_feature, ccv_size_t size, double *pw, double *nw)
{
  ccv_bbf_gene_t best_gene;
  /* seed (random method) */
  gsl_rng_env_setup();
  gsl_rng *rng = gsl_rng_alloc(gsl_rng_default);
  union {
    unsigned long int li;
    double db;
  } dbli;
  dbli.db = pw[0] + nw[0];
  gsl_rng_set(rng, dbli.li);
  int i, j, k, q, p, g, t;
  int rows[] = {size.height, size.height >> 1, size.height >> 2};
  int cols[] = {size.width, size.width >> 1, size.width >> 2};
  int pnum = rows[0] * cols[0] + rows[1] * cols[1] + rows[2] * cols[2];
  ccv_bbf_gene_t *gene = (ccv_bbf_gene_t *)ccmalloc((pnum * (CCV_BBF_POINT_MAX * 2 + 1) * 2 + CCV_BBF_POINT_MAX * 2 + 1) * sizeof(ccv_bbf_gene_t));
  if (best_feature == 0)
  {
    /* bootstrapping the best feature, start from two pixels, one for positive, one for negative
		 * the bootstrapping process go like this: first, it will assign a random pixel as positive
		 * and enumerate every possible pixel as negative, and pick the best one. Then, enumerate every
		 * possible pixel as positive, and pick the best one, until it converges */
    memset(&best_gene, 0, sizeof(ccv_bbf_gene_t));
    for (i = 0; i < CCV_BBF_POINT_MAX; i++)
      best_gene.feature.pz[i] = best_gene.feature.nz[i] = -1;
    best_gene.pk = 1;
    best_gene.nk = 0;
    best_gene.feature.size = 1;
    best_gene.feature.pz[0] = gsl_rng_uniform_int(rng, 3);
    best_gene.feature.px[0] = gsl_rng_uniform_int(rng, cols[best_gene.feature.pz[0]]);
    best_gene.feature.py[0] = gsl_rng_uniform_int(rng, rows[best_gene.feature.pz[0]]);
    for (t = 0;; ++t)
    {
      g = 0;
      if (t % 2 == 0)
      {
        for (i = 0; i < 3; i++)
          for (j = 0; j < cols[i]; j++)
            for (k = 0; k < rows[i]; k++)
              if (i != best_gene.feature.pz[0] || j != best_gene.feature.px[0] || k != best_gene.feature.py[0])
              {
                gene[g] = best_gene;
                gene[g].pk = gene[g].nk = 1;
                gene[g].feature.nz[0] = i;
                gene[g].feature.nx[0] = j;
                gene[g].feature.ny[0] = k;
                g++;
              }
      }
      else
      {
        for (i = 0; i < 3; i++)
          for (j = 0; j < cols[i]; j++)
            for (k = 0; k < rows[i]; k++)
              if (i != best_gene.feature.nz[0] || j != best_gene.feature.nx[0] || k != best_gene.feature.ny[0])
              {
                gene[g] = best_gene;
                gene[g].pk = gene[g].nk = 1;
                gene[g].feature.pz[0] = i;
                gene[g].feature.px[0] = j;
                gene[g].feature.py[0] = k;
                g++;
              }
      }
      PRINT(CCV_CLI_INFO, "bootstrapping round : %d\n", t);
      ccv_bbf_gene_t local_gene = _ccv_bbf_best_gene(gene, g, 2, posdata, posnum, negdata, negnum, size, pw, nw);
      if (local_gene.error >= best_gene.error - 1e-10)
        break;
      best_gene = local_gene;
    }
  }
  else
  {
    best_gene.feature = *best_feature;
    best_gene.pk = best_gene.nk = best_gene.feature.size;
    for (i = 0; i < CCV_BBF_POINT_MAX; i++)
      if (best_feature->pz[i] == -1)
      {
        best_gene.pk = i;
        break;
      }
    for (i = 0; i < CCV_BBF_POINT_MAX; i++)
      if (best_feature->nz[i] == -1)
      {
        best_gene.nk = i;
        break;
      }
  }
  /* after bootstrapping, the float search technique will do the following permutations:
	 * a). add a new point to positive or negative
	 * b). remove a point from positive or negative
	 * c). move an existing point in positive or negative to another position
	 * the three rules applied exhaustively, no heuristic used. */
  for (t = 0;; ++t)
  {
    g = 0;
    for (i = 0; i < 3; i++)
      for (j = 0; j < cols[i]; j++)
        for (k = 0; k < rows[i]; k++)
          if (!_ccv_bbf_exist_gene_feature(&best_gene, j, k, i))
          {
            /* add positive point */
            if (best_gene.pk < CCV_BBF_POINT_MAX - 1)
            {
              gene[g] = best_gene;
              gene[g].feature.pz[gene[g].pk] = i;
              gene[g].feature.px[gene[g].pk] = j;
              gene[g].feature.py[gene[g].pk] = k;
              gene[g].pk++;
              gene[g].feature.size = ccv_max(gene[g].pk, gene[g].nk);
              g++;
            }
            /* add negative point */
            if (best_gene.nk < CCV_BBF_POINT_MAX - 1)
            {
              gene[g] = best_gene;
              gene[g].feature.nz[gene[g].nk] = i;
              gene[g].feature.nx[gene[g].nk] = j;
              gene[g].feature.ny[gene[g].nk] = k;
              gene[g].nk++;
              gene[g].feature.size = ccv_max(gene[g].pk, gene[g].nk);
              g++;
            }
            /* refine positive point */
            for (q = 0; q < best_gene.pk; q++)
            {
              gene[g] = best_gene;
              gene[g].feature.pz[q] = i;
              gene[g].feature.px[q] = j;
              gene[g].feature.py[q] = k;
              g++;
            }
            /* add positive point, remove negative point */
            if (best_gene.pk < CCV_BBF_POINT_MAX - 1 && best_gene.nk > 1)
            {
              for (q = 0; q < best_gene.nk; q++)
              {
                gene[g] = best_gene;
                gene[g].feature.pz[gene[g].pk] = i;
                gene[g].feature.px[gene[g].pk] = j;
                gene[g].feature.py[gene[g].pk] = k;
                gene[g].pk++;
                for (p = q; p < best_gene.nk - 1; p++)
                {
                  gene[g].feature.nz[p] = gene[g].feature.nz[p + 1];
                  gene[g].feature.nx[p] = gene[g].feature.nx[p + 1];
                  gene[g].feature.ny[p] = gene[g].feature.ny[p + 1];
                }
                gene[g].feature.nz[gene[g].nk - 1] = -1;
                gene[g].nk--;
                gene[g].feature.size = ccv_max(gene[g].pk, gene[g].nk);
                g++;
              }
            }
            /* refine negative point */
            for (q = 0; q < best_gene.nk; q++)
            {
              gene[g] = best_gene;
              gene[g].feature.nz[q] = i;
              gene[g].feature.nx[q] = j;
              gene[g].feature.ny[q] = k;
              g++;
            }
            /* add negative point, remove positive point */
            if (best_gene.pk > 1 && best_gene.nk < CCV_BBF_POINT_MAX - 1)
            {
              for (q = 0; q < best_gene.pk; q++)
              {
                gene[g] = best_gene;
                gene[g].feature.nz[gene[g].nk] = i;
                gene[g].feature.nx[gene[g].nk] = j;
                gene[g].feature.ny[gene[g].nk] = k;
                gene[g].nk++;
                for (p = q; p < best_gene.pk - 1; p++)
                {
                  gene[g].feature.pz[p] = gene[g].feature.pz[p + 1];
                  gene[g].feature.px[p] = gene[g].feature.px[p + 1];
                  gene[g].feature.py[p] = gene[g].feature.py[p + 1];
                }
                gene[g].feature.pz[gene[g].pk - 1] = -1;
                gene[g].pk--;
                gene[g].feature.size = ccv_max(gene[g].pk, gene[g].nk);
                g++;
              }
            }
          }
    if (best_gene.pk > 1)
      for (q = 0; q < best_gene.pk; q++)
      {
        gene[g] = best_gene;
        for (i = q; i < best_gene.pk - 1; i++)
        {
          gene[g].feature.pz[i] = gene[g].feature.pz[i + 1];
          gene[g].feature.px[i] = gene[g].feature.px[i + 1];
          gene[g].feature.py[i] = gene[g].feature.py[i + 1];
        }
        gene[g].feature.pz[gene[g].pk - 1] = -1;
        gene[g].pk--;
        gene[g].feature.size = ccv_max(gene[g].pk, gene[g].nk);
        g++;
      }
    if (best_gene.nk > 1)
      for (q = 0; q < best_gene.nk; q++)
      {
        gene[g] = best_gene;
        for (i = q; i < best_gene.nk - 1; i++)
        {
          gene[g].feature.nz[i] = gene[g].feature.nz[i + 1];
          gene[g].feature.nx[i] = gene[g].feature.nx[i + 1];
          gene[g].feature.ny[i] = gene[g].feature.ny[i + 1];
        }
        gene[g].feature.nz[gene[g].nk - 1] = -1;
        gene[g].nk--;
        gene[g].feature.size = ccv_max(gene[g].pk, gene[g].nk);
        g++;
      }
    gene[g] = best_gene;
    g++;
    PRINT(CCV_CLI_INFO, "float search round : %d\n", t);
    ccv_bbf_gene_t local_gene = _ccv_bbf_best_gene(gene, g, CCV_BBF_POINT_MIN, posdata, posnum, negdata, negnum, size, pw, nw);
    if (local_gene.error >= best_gene.error - 1e-10)
      break;
    best_gene = local_gene;
  }
  ccfree(gene);
  gsl_rng_free(rng);
  return best_gene.feature;
}

static int _ccv_write_bbf_stage_classifier(const char *file, ccv_bbf_stage_classifier_t *classifier)
{
  FILE *w = fopen(file, "wb");
  if (w == 0)
    return -1;
  fprintf(w, "%d\n", classifier->count);
  union {
    float fl;
    int i;
  } fli;
  fli.fl = classifier->threshold;
  fprintf(w, "%d\n", fli.i);
  int i, j;
  for (i = 0; i < classifier->count; i++)
  {
    fprintf(w, "%d\n", classifier->feature[i].size);
    for (j = 0; j < classifier->feature[i].size; j++)
    {
      fprintf(w, "%d %d %d\n", classifier->feature[i].px[j], classifier->feature[i].py[j], classifier->feature[i].pz[j]);
      fprintf(w, "%d %d %d\n", classifier->feature[i].nx[j], classifier->feature[i].ny[j], classifier->feature[i].nz[j]);
    }
    union {
      float fl;
      int i;
    } flia, flib;
    flia.fl = classifier->alpha[i * 2];
    flib.fl = classifier->alpha[i * 2 + 1];
    fprintf(w, "%d %d\n", flia.i, flib.i);
  }
  fclose(w);
  return 0;
}

static int _ccv_read_background_data(const char *file, unsigned char **negdata, int *negnum, ccv_size_t size)
{
  int stat = 0;
  FILE *r = fopen(file, "rb");
  if (r == 0)
    return -1;
  stat |= fread(negnum, sizeof(int), 1, r);
  int i;
  int isizs012 = _ccv_width_padding(size.width) * size.height +
                 _ccv_width_padding(size.width >> 1) * (size.height >> 1) +
                 _ccv_width_padding(size.width >> 2) * (size.height >> 2);
  for (i = 0; i < *negnum; i++)
  {
    negdata[i] = (unsigned char *)ccmalloc(isizs012);
    stat |= fread(negdata[i], 1, isizs012, r);
  }
  fclose(r);
  return 0;
}

static int _ccv_write_background_data(const char *file, unsigned char **negdata, int negnum, ccv_size_t size)
{
  FILE *w = fopen(file, "w");
  if (w == 0)
    return -1;
  fwrite(&negnum, sizeof(int), 1, w);
  int i;
  int isizs012 = _ccv_width_padding(size.width) * size.height +
                 _ccv_width_padding(size.width >> 1) * (size.height >> 1) +
                 _ccv_width_padding(size.width >> 2) * (size.height >> 2);
  for (i = 0; i < negnum; i++)
    fwrite(negdata[i], 1, isizs012, w);
  fclose(w);
  return 0;
}

static int _ccv_resume_bbf_cascade_training_state(const char *file, int *i, int *k, int *bg, double *pw, double *nw, int posnum, int negnum)
{
  int stat = 0;
  FILE *r = fopen(file, "r");
  if (r == 0)
    return -1;
  stat |= fscanf(r, "%d %d %d", i, k, bg);
  int j;
  union {
    double db;
    int i[2];
  } dbi;
  for (j = 0; j < posnum; j++)
  {
    stat |= fscanf(r, "%d %d", &dbi.i[0], &dbi.i[1]);
    pw[j] = dbi.db;
  }
  for (j = 0; j < negnum; j++)
  {
    stat |= fscanf(r, "%d %d", &dbi.i[0], &dbi.i[1]);
    nw[j] = dbi.db;
  }
  fclose(r);
  return 0;
}

static int _ccv_save_bbf_cacade_training_state(const char *file, int i, int k, int bg, double *pw, double *nw, int posnum, int negnum)
{
  FILE *w = fopen(file, "w");
  if (w == 0)
    return -1;
  fprintf(w, "%d %d %d\n", i, k, bg);
  int j;
  union {
    double db;
    int i[2];
  } dbi;
  for (j = 0; j < posnum; ++j)
  {
    dbi.db = pw[j];
    fprintf(w, "%d %d ", dbi.i[0], dbi.i[1]);
  }
  fprintf(w, "\n");
  for (j = 0; j < negnum; ++j)
  {
    dbi.db = nw[j];
    fprintf(w, "%d %d ", dbi.i[0], dbi.i[1]);
  }
  fprintf(w, "\n");
  fclose(w);
  return 0;
}

void ccv_bbf_classifier_cascade_new(ccv_dense_matrix_t **posimg, int posnum, char **bgfiles, int bgnum, int negnum, ccv_size_t size, const char *dir, ccv_bbf_new_param_t params)
{
  int i, j, k;
  /* allocate memory for usage */
  ccv_bbf_classifier_cascade_t *cascade = (ccv_bbf_classifier_cascade_t *)ccmalloc(sizeof(ccv_bbf_classifier_cascade_t));
  cascade->count = 0;
  cascade->size = size;
  cascade->stage_classifier = (ccv_bbf_stage_classifier_t *)ccmalloc(sizeof(ccv_bbf_stage_classifier_t));
  unsigned char **posdata = (unsigned char **)ccmalloc(posnum * sizeof(unsigned char *));
  unsigned char **negdata = (unsigned char **)ccmalloc(negnum * sizeof(unsigned char *));
  double *pw = (double *)ccmalloc(posnum * sizeof(double));
  double *nw = (double *)ccmalloc(negnum * sizeof(double));
  float *peval = (float *)ccmalloc(posnum * sizeof(float));
  float *neval = (float *)ccmalloc(negnum * sizeof(float));
  double inv_balance_k = 1. / params.balance_k;
  /* balance factor k, and weighted with 0.01 */
  params.balance_k *= 0.01;
  inv_balance_k *= 0.01;

  int steps[] = {_ccv_width_padding(cascade->size.width),
                 _ccv_width_padding(cascade->size.width >> 1),
                 _ccv_width_padding(cascade->size.width >> 2)};
  int isizs0 = steps[0] * cascade->size.height;
  int isizs01 = isizs0 + steps[1] * (cascade->size.height >> 1);

  i = 0;
  k = 0;
  int bg = 0;
  int cacheK = 10;
  /* state resume code */
  char buf[1024];
  sprintf(buf, "%s/stat.txt", dir);
  _ccv_resume_bbf_cascade_training_state(buf, &i, &k, &bg, pw, nw, posnum, negnum);
  if (i > 0)
  {
    cascade->count = i;
    ccfree(cascade->stage_classifier);
    cascade->stage_classifier = (ccv_bbf_stage_classifier_t *)ccmalloc(i * sizeof(ccv_bbf_stage_classifier_t));
    for (j = 0; j < i; j++)
    {
      sprintf(buf, "%s/stage-%d.txt", dir, j);
      _ccv_read_bbf_stage_classifier(buf, &cascade->stage_classifier[j]);
    }
  }
  if (k > 0)
    cacheK = k;
  int rpos, rneg = 0;
  if (bg)
  {
    sprintf(buf, "%s/negs.txt", dir);
    _ccv_read_background_data(buf, negdata, &rneg, cascade->size);
  }

  for (; i < params.layer; i++)
  {
    if (!bg)
    {
      rneg = _ccv_prepare_background_data(cascade, bgfiles, bgnum, negdata, negnum);
      /* save state of background data */
      sprintf(buf, "%s/negs.txt", dir);
      _ccv_write_background_data(buf, negdata, rneg, cascade->size);
      bg = 1;
    }
    double totalw;
    /* save state of cascade : level, weight etc. */
    sprintf(buf, "%s/stat.txt", dir);
    _ccv_save_bbf_cacade_training_state(buf, i, k, bg, pw, nw, posnum, negnum);
    ccv_bbf_stage_classifier_t classifier;
    if (k > 0)
    {
      /* resume state of classifier */
      sprintf(buf, "%s/stage-%d.txt", dir, i);
      _ccv_read_bbf_stage_classifier(buf, &classifier);
    }
    else
    {
      /* initialize classifier */
      for (j = 0; j < posnum; j++)
        pw[j] = params.balance_k;
      for (j = 0; j < rneg; j++)
        nw[j] = inv_balance_k;
      classifier.count = k;
      classifier.threshold = 0;
      classifier.feature = (ccv_bbf_feature_t *)ccmalloc(cacheK * sizeof(ccv_bbf_feature_t));
      classifier.alpha = (float *)ccmalloc(cacheK * 2 * sizeof(float));
    }
    _ccv_prepare_positive_data(posimg, posdata, cascade->size, posnum);
    rpos = _ccv_prune_positive_data(cascade, posdata, posnum, cascade->size);
    PRINT(CCV_CLI_INFO, "%d postivie data and %d negative data in training\n", rpos, rneg);
    /* reweight to 1.00 */
    totalw = 0;
    for (j = 0; j < rpos; j++)
      totalw += pw[j];
    for (j = 0; j < rneg; j++)
      totalw += nw[j];
    for (j = 0; j < rpos; j++)
      pw[j] = pw[j] / totalw;
    for (j = 0; j < rneg; j++)
      nw[j] = nw[j] / totalw;
    for (;; k++)
    {
      /* get overall true-positive, false-positive rate and threshold */
      double tp = 0, fp = 0, etp = 0, efp = 0;
      _ccv_bbf_eval_data(&classifier, posdata, rpos, negdata, rneg, cascade->size, peval, neval);
      _ccv_sort_32f(peval, rpos, 0);
      classifier.threshold = peval[(int)((1. - params.pos_crit) * rpos)] - 1e-6;
      for (j = 0; j < rpos; j++)
      {
        if (peval[j] >= 0)
          ++tp;
        if (peval[j] >= classifier.threshold)
          ++etp;
      }
      tp /= rpos;
      etp /= rpos;
      for (j = 0; j < rneg; j++)
      {
        if (neval[j] >= 0)
          ++fp;
        if (neval[j] >= classifier.threshold)
          ++efp;
      }
      fp /= rneg;
      efp /= rneg;
      PRINT(CCV_CLI_INFO, "stage classifier real TP rate : %f, FP rate : %f\n", tp, fp);
      PRINT(CCV_CLI_INFO, "stage classifier TP rate : %f, FP rate : %f at threshold : %f\n", etp, efp, classifier.threshold);
      if (k > 0)
      {
        /* save classifier state */
        sprintf(buf, "%s/stage-%d.txt", dir, i);
        _ccv_write_bbf_stage_classifier(buf, &classifier);
        sprintf(buf, "%s/stat.txt", dir);
        _ccv_save_bbf_cacade_training_state(buf, i, k, bg, pw, nw, posnum, negnum);
      }
      if (etp > params.pos_crit && efp < params.neg_crit)
        break;
      /* TODO: more post-process is needed in here */

      /* select the best feature in current distribution through genetic algorithm optimization */
      ccv_bbf_feature_t best;
      if (params.optimizer == CCV_BBF_GENETIC_OPT)
      {
        best = _ccv_bbf_genetic_optimize(posdata, rpos, negdata, rneg, params.feature_number, cascade->size, pw, nw);
      }
      else if (params.optimizer == CCV_BBF_FLOAT_OPT)
      {
        best = _ccv_bbf_convex_optimize(posdata, rpos, negdata, rneg, 0, cascade->size, pw, nw);
      }
      else
      {
        best = _ccv_bbf_genetic_optimize(posdata, rpos, negdata, rneg, params.feature_number, cascade->size, pw, nw);
        best = _ccv_bbf_convex_optimize(posdata, rpos, negdata, rneg, &best, cascade->size, pw, nw);
      }
      double err = _ccv_bbf_error_rate(&best, posdata, rpos, negdata, rneg, cascade->size, pw, nw);
      double rw = (1 - err) / err;
      totalw = 0;
      /* reweight */
      for (j = 0; j < rpos; j++)
      {
        unsigned char *u8[] = {posdata[j], posdata[j] + isizs0, posdata[j] + isizs01};
        if (!_ccv_run_bbf_feature(&best, steps, u8))
          pw[j] *= rw;
        pw[j] *= params.balance_k;
        totalw += pw[j];
      }
      for (j = 0; j < rneg; j++)
      {
        unsigned char *u8[] = {negdata[j], negdata[j] + isizs0, negdata[j] + isizs01};
        if (_ccv_run_bbf_feature(&best, steps, u8))
          nw[j] *= rw;
        nw[j] *= inv_balance_k;
        totalw += nw[j];
      }
      for (j = 0; j < rpos; j++)
        pw[j] = pw[j] / totalw;
      for (j = 0; j < rneg; j++)
        nw[j] = nw[j] / totalw;
      double c = log(rw);
      PRINT(CCV_CLI_INFO, "coefficient of feature %d: %f\n", k + 1, c);
      classifier.count = k + 1;
      /* resizing classifier */
      if (k >= cacheK)
      {
        ccv_bbf_feature_t *feature = (ccv_bbf_feature_t *)ccmalloc(cacheK * 2 * sizeof(ccv_bbf_feature_t));
        memcpy(feature, classifier.feature, cacheK * sizeof(ccv_bbf_feature_t));
        ccfree(classifier.feature);
        float *alpha = (float *)ccmalloc(cacheK * 4 * sizeof(float));
        memcpy(alpha, classifier.alpha, cacheK * 2 * sizeof(float));
        ccfree(classifier.alpha);
        classifier.feature = feature;
        classifier.alpha = alpha;
        cacheK *= 2;
      }
      /* setup new feature */
      classifier.feature[k] = best;
      classifier.alpha[k * 2] = -c;
      classifier.alpha[k * 2 + 1] = c;
    }
    cascade->count = i + 1;
    ccv_bbf_stage_classifier_t *stage_classifier = (ccv_bbf_stage_classifier_t *)ccmalloc(cascade->count * sizeof(ccv_bbf_stage_classifier_t));
    memcpy(stage_classifier, cascade->stage_classifier, i * sizeof(ccv_bbf_stage_classifier_t));
    ccfree(cascade->stage_classifier);
    stage_classifier[i] = classifier;
    cascade->stage_classifier = stage_classifier;
    k = 0;
    bg = 0;
    for (j = 0; j < rpos; j++)
      ccfree(posdata[j]);
    for (j = 0; j < rneg; j++)
      ccfree(negdata[j]);
  }

  ccfree(neval);
  ccfree(peval);
  ccfree(nw);
  ccfree(pw);
  ccfree(negdata);
  ccfree(posdata);
  ccfree(cascade);
}
#else
void ccv_bbf_classifier_cascade_new(ccv_dense_matrix_t **posimg, int posnum, char **bgfiles, int bgnum, int negnum, ccv_size_t size, const char *dir, ccv_bbf_new_param_t params)
{
  fprintf(stderr, " ccv_bbf_classifier_cascade_new requires libgsl support, please compile ccv with libgsl.\n");
}
#endif

static int _ccv_is_equal(const void *_r1, const void *_r2, void *data)
{
  const ccv_comp_t *r1 = (const ccv_comp_t *)_r1;
  const ccv_comp_t *r2 = (const ccv_comp_t *)_r2;
  int distance = (int)(r1->rect.width * 0.25 + 0.5);

  return r2->rect.x <= r1->rect.x + distance &&
         r2->rect.x >= r1->rect.x - distance &&
         r2->rect.y <= r1->rect.y + distance &&
         r2->rect.y >= r1->rect.y - distance &&
         r2->rect.width <= (int)(r1->rect.width * 1.5 + 0.5) &&
         (int)(r2->rect.width * 1.5 + 0.5) >= r1->rect.width;
}

static int _ccv_is_equal_same_class(const void *_r1, const void *_r2, void *data)
{
  const ccv_comp_t *r1 = (const ccv_comp_t *)_r1;
  const ccv_comp_t *r2 = (const ccv_comp_t *)_r2;
  int distance = (int)(r1->rect.width * 0.25 + 0.5);

  return r2->classification.id == r1->classification.id &&
         r2->rect.x <= r1->rect.x + distance &&
         r2->rect.x >= r1->rect.x - distance &&
         r2->rect.y <= r1->rect.y + distance &&
         r2->rect.y >= r1->rect.y - distance &&
         r2->rect.width <= (int)(r1->rect.width * 1.5 + 0.5) &&
         (int)(r2->rect.width * 1.5 + 0.5) >= r1->rect.width;
}

ccv_array_t *ccv_bbf_detect_objects(ccv_dense_matrix_t *a, ccv_bbf_classifier_cascade_t **_cascade, int count, ccv_bbf_param_t params)
{
  int hr = a->rows / params.size.height;
  int wr = a->cols / params.size.width;
  double scale = pow(2., 1. / (params.interval + 1.));
  int next = params.interval + 1;
  int scale_upto = (int)(log((double)ccv_min(hr, wr)) / log(scale));
  ccv_dense_matrix_t **pyr = (ccv_dense_matrix_t **)alloca((scale_upto + next * 2) * 4 * sizeof(ccv_dense_matrix_t *));
  memset(pyr, 0, (scale_upto + next * 2) * 4 * sizeof(ccv_dense_matrix_t *));
  if (params.size.height != _cascade[0]->size.height || params.size.width != _cascade[0]->size.width)
    ccv_resample(a, &pyr[0], 0, a->rows * _cascade[0]->size.height / params.size.height, a->cols * _cascade[0]->size.width / params.size.width, CCV_INTER_AREA);
  else
    pyr[0] = a;
  int i, j, k, t, x, y, q;
  for (i = 1; i < ccv_min(params.interval + 1, scale_upto + next * 2); i++)
    ccv_resample(pyr[0], &pyr[i * 4], 0, (int)(pyr[0]->rows / pow(scale, i)), (int)(pyr[0]->cols / pow(scale, i)), CCV_INTER_AREA);
  for (i = next; i < scale_upto + next * 2; i++)
    ccv_sample_down(pyr[i * 4 - next * 4], &pyr[i * 4], 0, 0, 0);
  if (params.accurate)
    for (i = next * 2; i < scale_upto + next * 2; i++)
    {
      ccv_sample_down(pyr[i * 4 - next * 4], &pyr[i * 4 + 1], 0, 1, 0);
      ccv_sample_down(pyr[i * 4 - next * 4], &pyr[i * 4 + 2], 0, 0, 1);
      ccv_sample_down(pyr[i * 4 - next * 4], &pyr[i * 4 + 3], 0, 1, 1);
    }
  ccv_array_t *idx_seq;
  ccv_array_t *seq = ccv_array_new(sizeof(ccv_comp_t), 64, 0);
  ccv_array_t *seq2 = ccv_array_new(sizeof(ccv_comp_t), 64, 0);
  ccv_array_t *result_seq = ccv_array_new(sizeof(ccv_comp_t), 64, 0);
  /* detect in multi scale */
  for (t = 0; t < count; t++)
  {
    ccv_bbf_classifier_cascade_t *cascade = _cascade[t];
    float scale_x = (float)params.size.width / (float)cascade->size.width;
    float scale_y = (float)params.size.height / (float)cascade->size.height;
    ccv_array_clear(seq);
    for (i = 0; i < scale_upto; i++)
    {
      int dx[] = {0, 1, 0, 1};
      int dy[] = {0, 0, 1, 1};
      int i_rows = pyr[i * 4 + next * 8]->rows - (cascade->size.height >> 2);
      int steps[] = {pyr[i * 4]->step, pyr[i * 4 + next * 4]->step, pyr[i * 4 + next * 8]->step};
      int i_cols = pyr[i * 4 + next * 8]->cols - (cascade->size.width >> 2);
      int paddings[] = {pyr[i * 4]->step * 4 - i_cols * 4,
                        pyr[i * 4 + next * 4]->step * 2 - i_cols * 2,
                        pyr[i * 4 + next * 8]->step - i_cols};
      for (q = 0; q < (params.accurate ? 4 : 1); q++)
      {
        unsigned char *u8[] = {pyr[i * 4]->data.u8 + dx[q] * 2 + dy[q] * pyr[i * 4]->step * 2, pyr[i * 4 + next * 4]->data.u8 + dx[q] + dy[q] * pyr[i * 4 + next * 4]->step, pyr[i * 4 + next * 8 + q]->data.u8};
        for (y = 0; y < i_rows; y++)
        {
          for (x = 0; x < i_cols; x++)
          {
            float sum;
            int flag = 1;
            ccv_bbf_stage_classifier_t *classifier = cascade->stage_classifier;
            for (j = 0; j < cascade->count; ++j, ++classifier)
            {
              sum = 0;
              float *alpha = classifier->alpha;
              ccv_bbf_feature_t *feature = classifier->feature;
              for (k = 0; k < classifier->count; ++k, alpha += 2, ++feature)
                sum += alpha[_ccv_run_bbf_feature(feature, steps, u8)];
              if (sum < classifier->threshold)
              {
                flag = 0;
                break;
              }
            }
            if (flag)
            {
              ccv_comp_t comp;
              comp.rect = ccv_rect((int)((x * 4 + dx[q] * 2) * scale_x + 0.5), (int)((y * 4 + dy[q] * 2) * scale_y + 0.5), (int)(cascade->size.width * scale_x + 0.5), (int)(cascade->size.height * scale_y + 0.5));
              comp.neighbors = 1;
              comp.classification.id = t;
              comp.classification.confidence = sum;
              ccv_array_push(seq, &comp);
            }
            u8[0] += 4;
            u8[1] += 2;
            u8[2] += 1;
          }
          u8[0] += paddings[0];
          u8[1] += paddings[1];
          u8[2] += paddings[2];
        }
      }
      scale_x *= scale;
      scale_y *= scale;
    }

    /* the following code from OpenCV's haar feature implementation */
    if (params.min_neighbors == 0)
    {
      for (i = 0; i < seq->rnum; i++)
      {
        ccv_comp_t *comp = (ccv_comp_t *)ccv_array_get(seq, i);
        ccv_array_push(result_seq, comp);
      }
    }
    else
    {
      idx_seq = 0;
      ccv_array_clear(seq2);
      // group retrieved rectangles in order to filter out noise
      int ncomp = ccv_array_group(seq, &idx_seq, _ccv_is_equal_same_class, 0);
      ccv_comp_t *comps = (ccv_comp_t *)ccmalloc((ncomp + 1) * sizeof(ccv_comp_t));
      memset(comps, 0, (ncomp + 1) * sizeof(ccv_comp_t));

      // count number of neighbors
      for (i = 0; i < seq->rnum; i++)
      {
        ccv_comp_t r1 = *(ccv_comp_t *)ccv_array_get(seq, i);
        int idx = *(int *)ccv_array_get(idx_seq, i);

        if (comps[idx].neighbors == 0)
          comps[idx].classification.confidence = r1.classification.confidence;

        ++comps[idx].neighbors;

        comps[idx].rect.x += r1.rect.x;
        comps[idx].rect.y += r1.rect.y;
        comps[idx].rect.width += r1.rect.width;
        comps[idx].rect.height += r1.rect.height;
        comps[idx].classification.id = r1.classification.id;
        comps[idx].classification.confidence = ccv_max(comps[idx].classification.confidence, r1.classification.confidence);
      }

      // calculate average bounding box
      for (i = 0; i < ncomp; i++)
      {
        int n = comps[i].neighbors;
        if (n >= params.min_neighbors)
        {
          ccv_comp_t comp;
          comp.rect.x = (comps[i].rect.x * 2 + n) / (2 * n);
          comp.rect.y = (comps[i].rect.y * 2 + n) / (2 * n);
          comp.rect.width = (comps[i].rect.width * 2 + n) / (2 * n);
          comp.rect.height = (comps[i].rect.height * 2 + n) / (2 * n);
          comp.neighbors = comps[i].neighbors;
          comp.classification.id = comps[i].classification.id;
          comp.classification.confidence = comps[i].classification.confidence;
          ccv_array_push(seq2, &comp);
        }
      }

      // filter out small face rectangles inside large face rectangles
      for (i = 0; i < seq2->rnum; i++)
      {
        ccv_comp_t r1 = *(ccv_comp_t *)ccv_array_get(seq2, i);
        int flag = 1;

        for (j = 0; j < seq2->rnum; j++)
        {
          ccv_comp_t r2 = *(ccv_comp_t *)ccv_array_get(seq2, j);
          int distance = (int)(r2.rect.width * 0.25 + 0.5);

          if (i != j &&
              r1.classification.id == r2.classification.id &&
              r1.rect.x >= r2.rect.x - distance &&
              r1.rect.y >= r2.rect.y - distance &&
              r1.rect.x + r1.rect.width <= r2.rect.x + r2.rect.width + distance &&
              r1.rect.y + r1.rect.height <= r2.rect.y + r2.rect.height + distance &&
              (r2.neighbors > ccv_max(3, r1.neighbors) || r1.neighbors < 3))
          {
            flag = 0;
            break;
          }
        }

        if (flag)
          ccv_array_push(result_seq, &r1);
      }
      ccv_array_free(idx_seq);
      ccfree(comps);
    }
  }

  ccv_array_free(seq);
  ccv_array_free(seq2);

  ccv_array_t *result_seq2;
  /* the following code from OpenCV's haar feature implementation */
  if (params.flags & CCV_BBF_NO_NESTED)
  {
    result_seq2 = ccv_array_new(sizeof(ccv_comp_t), 64, 0);
    idx_seq = 0;
    // group retrieved rectangles in order to filter out noise
    int ncomp = ccv_array_group(result_seq, &idx_seq, _ccv_is_equal, 0);
    ccv_comp_t *comps = (ccv_comp_t *)ccmalloc((ncomp + 1) * sizeof(ccv_comp_t));
    memset(comps, 0, (ncomp + 1) * sizeof(ccv_comp_t));

    // count number of neighbors
    for (i = 0; i < result_seq->rnum; i++)
    {
      ccv_comp_t r1 = *(ccv_comp_t *)ccv_array_get(result_seq, i);
      int idx = *(int *)ccv_array_get(idx_seq, i);

      if (comps[idx].neighbors == 0 || comps[idx].classification.confidence < r1.classification.confidence)
      {
        comps[idx].classification.confidence = r1.classification.confidence;
        comps[idx].neighbors = 1;
        comps[idx].rect = r1.rect;
        comps[idx].classification.id = r1.classification.id;
      }
    }

    // calculate average bounding box
    for (i = 0; i < ncomp; i++)
      if (comps[i].neighbors)
        ccv_array_push(result_seq2, &comps[i]);

    ccv_array_free(result_seq);
    ccfree(comps);
  }
  else
  {
    result_seq2 = result_seq;
  }

  for (i = 1; i < scale_upto + next * 2; i++)
    ccv_matrix_free(pyr[i * 4]);
  if (params.accurate)
    for (i = next * 2; i < scale_upto + next * 2; i++)
    {
      ccv_matrix_free(pyr[i * 4 + 1]);
      ccv_matrix_free(pyr[i * 4 + 2]);
      ccv_matrix_free(pyr[i * 4 + 3]);
    }
  if (params.size.height != _cascade[0]->size.height || params.size.width != _cascade[0]->size.width)
    ccv_matrix_free(pyr[0]);

  return result_seq2;
}

ccv_bbf_classifier_cascade_t *ccv_bbf_read_classifier_cascade(const char *directory)
{
  char buf[1024];
  sprintf(buf, "%s/cascade.txt", directory);
  int s, i;
  FILE *r = fopen(buf, "r");
  if (r == 0)
    return 0;
  ccv_bbf_classifier_cascade_t *cascade = (ccv_bbf_classifier_cascade_t *)ccmalloc(sizeof(ccv_bbf_classifier_cascade_t));
  s = fscanf(r, "%d %d %d", &cascade->count, &cascade->size.width, &cascade->size.height);
  assert(s > 0);
  cascade->stage_classifier = (ccv_bbf_stage_classifier_t *)ccmalloc(cascade->count * sizeof(ccv_bbf_stage_classifier_t));
  for (i = 0; i < cascade->count; i++)
  {
    sprintf(buf, "%s/stage-%d.txt", directory, i);
    if (_ccv_read_bbf_stage_classifier(buf, &cascade->stage_classifier[i]) < 0)
    {
      cascade->count = i;
      break;
    }
  }
  fclose(r);
  return cascade;
}

ccv_bbf_classifier_cascade_t *ccv_bbf_classifier_cascade_read_binary(char *s)
{
  int i;
  ccv_bbf_classifier_cascade_t *cascade = (ccv_bbf_classifier_cascade_t *)ccmalloc(sizeof(ccv_bbf_classifier_cascade_t));
  memcpy(&cascade->count, s, sizeof(cascade->count));
  s += sizeof(cascade->count);
  memcpy(&cascade->size.width, s, sizeof(cascade->size.width));
  s += sizeof(cascade->size.width);
  memcpy(&cascade->size.height, s, sizeof(cascade->size.height));
  s += sizeof(cascade->size.height);
  ccv_bbf_stage_classifier_t *classifier = cascade->stage_classifier = (ccv_bbf_stage_classifier_t *)ccmalloc(cascade->count * sizeof(ccv_bbf_stage_classifier_t));
  for (i = 0; i < cascade->count; i++, classifier++)
  {
    memcpy(&classifier->count, s, sizeof(classifier->count));
    s += sizeof(classifier->count);
    memcpy(&classifier->threshold, s, sizeof(classifier->threshold));
    s += sizeof(classifier->threshold);
    classifier->feature = (ccv_bbf_feature_t *)ccmalloc(classifier->count * sizeof(ccv_bbf_feature_t));
    classifier->alpha = (float *)ccmalloc(classifier->count * 2 * sizeof(float));
    memcpy(classifier->feature, s, classifier->count * sizeof(ccv_bbf_feature_t));
    s += classifier->count * sizeof(ccv_bbf_feature_t);
    memcpy(classifier->alpha, s, classifier->count * 2 * sizeof(float));
    s += classifier->count * 2 * sizeof(float);
  }
  return cascade;
}

int ccv_bbf_classifier_cascade_write_binary(ccv_bbf_classifier_cascade_t *cascade, char *s, int slen)
{
  int i;
  int len = sizeof(cascade->count) + sizeof(cascade->size.width) + sizeof(cascade->size.height);
  ccv_bbf_stage_classifier_t *classifier = cascade->stage_classifier;
  for (i = 0; i < cascade->count; i++, classifier++)
    len += sizeof(classifier->count) + sizeof(classifier->threshold) + classifier->count * sizeof(ccv_bbf_feature_t) + classifier->count * 2 * sizeof(float);
  if (slen >= len)
  {
    memcpy(s, &cascade->count, sizeof(cascade->count));
    s += sizeof(cascade->count);
    memcpy(s, &cascade->size.width, sizeof(cascade->size.width));
    s += sizeof(cascade->size.width);
    memcpy(s, &cascade->size.height, sizeof(cascade->size.height));
    s += sizeof(cascade->size.height);
    classifier = cascade->stage_classifier;
    for (i = 0; i < cascade->count; i++, classifier++)
    {
      memcpy(s, &classifier->count, sizeof(classifier->count));
      s += sizeof(classifier->count);
      memcpy(s, &classifier->threshold, sizeof(classifier->threshold));
      s += sizeof(classifier->threshold);
      memcpy(s, classifier->feature, classifier->count * sizeof(ccv_bbf_feature_t));
      s += classifier->count * sizeof(ccv_bbf_feature_t);
      memcpy(s, classifier->alpha, classifier->count * 2 * sizeof(float));
      s += classifier->count * 2 * sizeof(float);
    }
  }
  return len;
}

void ccv_bbf_classifier_cascade_free(ccv_bbf_classifier_cascade_t *cascade)
{
  int i;
  for (i = 0; i < cascade->count; ++i)
  {
    ccfree(cascade->stage_classifier[i].feature);
    ccfree(cascade->stage_classifier[i].alpha);
  }
  ccfree(cascade->stage_classifier);
  ccfree(cascade);
}


#ifndef OPENDSS_H
#define OPENDSS_H
#include <petsc.h>
#include <complex.h>
extern PetscErrorCode OpenDSSRunCommand(const char*);
extern PetscErrorCode OpenDSSVsourcesSetPU(PetscScalar);
extern PetscErrorCode OpenDSSVsourcesSetAngleDeg(PetscScalar);
extern PetscErrorCode OpenDSSSolutionGetSolve(PetscInt*);
extern PetscErrorCode OpenDSSCircuitGetTotalPower(double complex**);
#endif


/*
 * optimize_tnc.h
 *
 *  Created on: Feb 9, 2010
 *      Author: smitty
 */

#ifndef _OPTIMIZE_CONT_MODELS_NLOPT_H_
#define _OPTIMIZE_CONT_MODELS_NLOPT_H_

#include <nlopt.h>

#include "cont_models.h"

#include <armadillo>
using namespace arma;

double nlopt_bm_sr(unsigned n, const double *x, double *grad, void *data);
double nlopt_bm_sr_log(unsigned n, const double *x, double *grad, void *data);
double nlopt_ou_sr_log(unsigned n, const double *x, double *grad, void *data);
double nlopt_bm_bl(unsigned n, const double *x, double *grad, void *data);
vector<double> optimize_single_rate_bm_nlopt(rowvec & _x, mat & _vcv,bool log);
vector<double> optimize_single_rate_bm_ou_nlopt(rowvec & _x, mat & _vcv);
vector<double> optimize_single_rate_bm_bl(Tree * tr);

#endif /* _OPTIMIZE_CONT_MODELS_NLOPT_H_ */


/*
 * The MIT License is a permissive free software license, which permits reuse
 * within both open source and proprietary software. The software is licensed
 * as is, and no warranty is given as to fitness for purpose or absence of
 * infringement of third parties' rights, such as patents. Generally use for
 * research activities is allowed regardless of any third party patents, but
 * commercial use may be subject to a separate license.
 *
 *
 * The MIT License (MIT)
 *
 * Copyright (c) 2015 Tuomo Raitio
 *
 * 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.
 *
 *
 *
 *
 * <><><><><><><><><><><><><><><><><><><><><><><><><><><><><><><><><>
 *                  GlottHMM Speech Synthesis
 * <><><><><><><><><><><><><><><><><><><><><><><><><><><><><><><><><>
 *
 * This program reads speech parameters and a glottal pulse/
 * pulse library, and synthesizes speech from them.
 *
 * This program has been written in Aalto University,
 * Department of Signal Processign and Acoustics, Espoo, Finland
 *
 * Main author: Tuomo Raitio
 * Acknowledgements: Antti Suni, Paavo Alku, Martti Vainio
 *
 * File Synthesis.c
 * Version: 1.1
 *
 */

/***********************************************/
/*                 INCLUDE                     */
/***********************************************/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <time.h>
#include <gsl/gsl_vector.h>		/* GSL, Vector */
#include <gsl/gsl_matrix.h>		/* GSL, Matrix */
#include <gsl/gsl_fft_real.h>		/* GSL, FFT */
#include <libconfig.h>				/* Configuration file */
#include "SynthesisFunctions.h"

/*******************************************************************/
/*                          MAIN                                   */
/*******************************************************************/

int main(int argc, char *argv[]) {

	/* Start counter */
	double time1 = (double)clock();

	/* Check command line format */
	if(Check_command_line(argc) == EXIT_FAILURE)
		return EXIT_FAILURE;

	/* Read default configuration file and assign parameters */
	struct config_t *conf_def = Read_config(argv[2]);
	if(conf_def == NULL)
		return EXIT_FAILURE;
	PARAM params;
	if(Assign_config_parameters(argv[1],conf_def,&params,DEF_CONF) == EXIT_FAILURE) {
		config_destroy(conf_def);
		free(conf_def);
		return EXIT_FAILURE;
	}

	/* Read user configuration file and assign parameters */
	if(argc == 4) {
		struct config_t *conf_usr = Read_config(argv[3]);
		if(conf_usr == NULL)
			return EXIT_FAILURE;
		if(Assign_config_parameters(argv[1],conf_usr,&params,USR_CONF) == EXIT_FAILURE) {
			config_destroy(conf_usr);
			free(conf_usr);
			return EXIT_FAILURE;
		}
	}

	/* Print synthesis settings */
	Print_synthesis_settings_start(&params);

	/*******************************************************************/
	/*                 READ PULSE LIBRARY / PULSE                      */
	/*******************************************************************/

	/* Allocate space for pulse library and read data */
	gsl_matrix *pulses,*pulses_rs,*plsf,*ptilt,*pharm,*phnr,*pwaveform,*pca_pc,*pca_w_lib;
	gsl_vector *pgain,*pulse_lengths,*ph1h2,*pnaq,*pca_mean,*stoch_env,*stoch_sp;
	if(Read_pulse_library(&params,&pulses,&pulses_rs,&plsf,&ptilt,&pharm,&phnr,&pwaveform,&pca_pc,
			&pca_w_lib,&stoch_env,&stoch_sp,&pgain,&ph1h2,&pnaq,&pca_mean,&pulse_lengths) == EXIT_FAILURE)
		return EXIT_FAILURE;

	/* Read pulse file if pulse library is not used */
	gsl_vector *original_pulse = NULL;
	if(params.use_pulselib == 0) {
		original_pulse = ReadPulseFile(&params);
		if(original_pulse == NULL)
			return EXIT_FAILURE;
	}

	/* Read DNN pulse generation weights and allocate parameters */
	gsl_matrix **DNN_W = (gsl_matrix**)malloc(params.dnn_weight_dims->size/2*sizeof(gsl_matrix*));
	if(Read_DNN_weights(&params,DNN_W) == EXIT_FAILURE)
		return EXIT_FAILURE;
	gsl_vector **input_minmax = (gsl_vector**)malloc(sizeof(gsl_vector*));
	if(Read_input_minmax(&params,input_minmax) == EXIT_FAILURE)
			return EXIT_FAILURE;
	gsl_vector *dnnpulseindices = NULL;
	gsl_vector *dnnpulses = NULL;

	/* Define variables */
	gsl_vector *gain,*fundf,*excitation_voiced,*excitation_unvoiced;
	gsl_vector *gain_new,*pulse_clus_id,*h1h2,*naq,*resynthesis_pulse_index;
	gsl_matrix *LSF,*LSF2,*glflowsp,*glflowsp_new,*hnr,*hnr_new,*harmonics,*waveform,*pulse_clusters,*pca_w,*LSF_interp = NULL;
	int i;

	/****************************************************************************************/
	/*                          START SYNTHESING FILE(S)                                    */
	/****************************************************************************************/

	/* Start loop for synthesis list */
	for(i=0; i<params.synlistlen; i++) {

		/* Initialize params */
		if(Initialize_params(&params,i) == EXIT_FAILURE)
			return EXIT_FAILURE;

		/* Print synthesis settings */
		Print_synthesis_settings_middle(&params);

		/* Compatibility check */
		if(Compatibility_check(&params) == EXIT_FAILURE)
			return EXIT_FAILURE;

		/*****************************************************************************/
		/*                     LOAD AND MODIFY PARAMETERS                            */
		/*****************************************************************************/

		/* Allocate and read synthesis parameters, initialize pulse clustering if used */
		Allocate_params(&excitation_voiced,&excitation_unvoiced,&resynthesis_pulse_index,&gain_new,&glflowsp_new,&hnr_new,&params);
		if(Read_synthesis_parameters(&gain,&fundf,&LSF,&LSF2,&glflowsp,&hnr,&harmonics,&waveform,&h1h2,&naq,&pca_w,&params) == EXIT_FAILURE) continue;
		if(Pulse_clustering(&pulse_clus_id, &pulse_clusters, &params) == EXIT_FAILURE) continue;

		/* Miscellaneous operations */
		Convert_logF0_to_lin(fundf,&params);
		Merge_voiced_unvoiced_spectra(LSF,LSF2,fundf,&params);
		Integrate_LSFs(&LSF,&params);
		if(params.use_tilt == 1)
			Integrate_LSFs(&glflowsp,&params);
		LSF_fix_matrix(LSF);
		LSF_fix_matrix(glflowsp);
		Smooth_matrix(hnr,params.hnr_smooth_len);
		if(params.use_hmm == 0) Smooth_matrix(harmonics,params.harmonics_smooth_len);
		if(params.use_hmm == 0) MA(gain,params.gain_smooth_len);

		/* Modification for noise robust speech */
		Noise_robust_speech2(gain,harmonics,&params);

		/* Formant enhancement for LSFs */
		Postfilter(LSF,&params);

		/* Apply noise reduction */
		Noise_reduction(gain,&params);

		/* Normalize pulse library parameters according to synthesis parameters (normalize mean) */
		Normalize_pulse_library_var(plsf,ptilt,pharm,phnr,pwaveform,pgain,ph1h2,pnaq,LSF,glflowsp,
				harmonics,hnr,waveform,gain,h1h2,naq,fundf,&params);

		/* Adapt synthesis parameters according to pulse library parameters (normalize mean) */
		Adapt_synthesis_parameters_var(plsf,ptilt,pharm,phnr,pwaveform,pgain,ph1h2,pnaq,LSF,glflowsp,
				harmonics,hnr,waveform,gain,h1h2,naq,fundf,pulse_lengths,&params);

		/* Select vocal tract LSFs from pulse library */
		// TODO: Viterbi
		int winsize = 5;
		Select_LSFs_from_pulse_library(LSF,plsf,fundf,winsize,&params);

		/* Smooth and interpolate LSFs to signal length */
		LSF_interp = gsl_matrix_alloc(params.signal_length,params.lpc_order_vt);
		Smooth_interp_lsf(LSF_interp,LSF,params.signal_length,params.use_hmm,params.lsf_smooth_len);

		/* DNN pulse gen */
		if(params.use_dnn_pulsegen == 1) {
			dnnpulseindices = gsl_vector_alloc(params.signal_length);
			dnnpulses = gsl_vector_alloc(2*params.signal_length);
		}

		/*******************************************************************/
		/*                    CREATE EXCITATION                            */
		/*******************************************************************/

		printf("	- Creating excitation...\n");

		/* Create excitation for estimating the HNR of the voiced excitation */
		CreateExcitation(&params,excitation_voiced,excitation_unvoiced,fundf,gain,LSF,glflowsp,hnr,harmonics,waveform,h1h2,naq,
				original_pulse,pulses,pulses_rs,pulse_lengths,pgain,plsf,ptilt,phnr,pharm,pwaveform,ph1h2,pnaq,
				resynthesis_pulse_index,pulse_clus_id,pulse_clusters,gain,glflowsp_new,hnr_new,pca_mean,pca_pc,pca_w,pca_w_lib,
				stoch_env,stoch_sp,DNN_W,input_minmax,dnnpulseindices,dnnpulses);

		/* Compensate HNR etc */
		HNR_compensation(hnr,hnr_new,&params);
		Fill_pulse_indices(resynthesis_pulse_index);
		params.resynth = 1;

		/* Create excitation */
		CreateExcitation(&params,excitation_voiced,excitation_unvoiced,fundf,gain,LSF,glflowsp,hnr,harmonics,waveform,h1h2,naq,
				original_pulse,pulses,pulses_rs,pulse_lengths,pgain,plsf,ptilt,phnr,pharm,pwaveform,ph1h2,pnaq,
				resynthesis_pulse_index,pulse_clus_id,pulse_clusters,gain,glflowsp_new,hnr_new,pca_mean,pca_pc,pca_w,pca_w_lib,
				stoch_env,stoch_sp,DNN_W,input_minmax,dnnpulseindices,dnnpulses);
		Print_elapsed_time(&params);

		/* Compensate for the pre-emphasis during analysis (de-emphasis) */
		if(params.unvoiced_pre_emphasis == 1)
			Integrate(excitation_unvoiced, 0.97); // Slightly less than leaky factor

		/*******************************************************************/
		/*                    SPECTRAL MATCHING                            */
		/*******************************************************************/

		if((params.use_pulselib == 0 && params.use_dnn_pulsegen == 0) || (params.use_pulselib_pca == 1 && params.pca_spectral_matching == 1) ||
			 (params.use_dnn_pulsegen == 1 && params.use_dnn_specmatch == 1)) {
			printf("	- Spectral matching...\n");
			Spectral_match(excitation_voiced, glflowsp, glflowsp_new, &params);
			if(params.use_pulselib_pca == 0 && params.use_dnn_pulsegen == 0 && params.two_pitch_period_diff_pulse == 0)
				LipRadiation(excitation_voiced);
			Print_elapsed_time(&params);
		}

		/*******************************************************************/
		/*                    FILTER EXCITATION                            */
		/*******************************************************************/

		/* Combine excitations and filter */
		printf("	- Filtering excitation...\n");
		gsl_vector_add(excitation_voiced,excitation_unvoiced);
		Filter_excitation(excitation_voiced,LSF_interp,&params);
		Print_elapsed_time(&params);

		/*******************************************************************/
		/*              RESYNTHESIS FOR GAIN NORMALIZATION                 */
		/*******************************************************************/

		/* Evaluate new gain for re-synthesis */
		printf("	- Re-synthesis for gain normalization...\n");
		Evaluate_new_gain(excitation_voiced,gain_new,gain,fundf,&params);

		/* Re-synthesize with new gain */
		CreateExcitation(&params,excitation_voiced,excitation_unvoiced,fundf,gain_new,LSF,glflowsp,hnr,harmonics,waveform,h1h2,naq,
				original_pulse,pulses,pulses_rs,pulse_lengths,pgain,plsf,ptilt,phnr,pharm,pwaveform,ph1h2,pnaq,
				resynthesis_pulse_index,pulse_clus_id,pulse_clusters,gain,glflowsp_new,hnr_new,pca_mean,pca_pc,pca_w,pca_w_lib,
				stoch_env,stoch_sp,DNN_W,input_minmax,dnnpulseindices,dnnpulses);
		if(params.unvoiced_pre_emphasis == 1)
			Integrate(excitation_unvoiced, 0.97); // Slightly less than leaky factor

		/* Spectral matching */
		if((params.use_pulselib == 0 && params.use_dnn_pulsegen == 0) || (params.use_pulselib_pca == 1 && params.pca_spectral_matching == 1) ||
			 (params.use_dnn_pulsegen == 1 && params.use_dnn_specmatch == 1)) {
			Spectral_match(excitation_voiced, glflowsp, glflowsp_new, &params);
			if(params.use_pulselib_pca == 0 && params.use_dnn_pulsegen == 0 && params.two_pitch_period_diff_pulse == 0)
				LipRadiation(excitation_voiced);
		}

		/* Combine excitations and filter */
		gsl_vector_add(excitation_voiced,excitation_unvoiced);

		/* Save excitation to wav file */
		Save_excitation_to_wav(excitation_voiced,&params);

		/* Filter excitation */
		Filter_excitation(excitation_voiced,LSF_interp,&params);

		/*******************************************************************/
		/*              POSTPROCESSING AND SAVE TO FILE                    */
		/*******************************************************************/

		/* Filter out signal below F0 */
		Hp_filt_below_f0(excitation_voiced,fundf,&params);

		/* Compression of speech signal (only in noise robust speech) */
		Compression(excitation_voiced, params.compcoeff);

		/* Scale (if > 1) and save signal to file */
		Scale_signal(excitation_voiced,SCALE_IF_GREATER_THAN_ONE);
		Save_signal_to_file(excitation_voiced,&params,NULL);

		/* Report */
		Print_synthesis_settings_end(&params,time1);
	}

	/*******************************************************************/
	/*                     FREE MEMORY AND FINISH                      */
	/*******************************************************************/

	/* Free memory */
	Free_pulselib_variables(pulses,pulses_rs,pwaveform,pulse_lengths,plsf,ptilt,pharm,phnr,pgain,ph1h2,pnaq,pca_mean,pca_pc,pca_w_lib,stoch_env,stoch_sp,&params);
	Free_variables(original_pulse,excitation_voiced,excitation_unvoiced,fundf,gain,gain_new,LSF,LSF2,LSF_interp,glflowsp,glflowsp_new,
			hnr,hnr_new,harmonics,waveform,h1h2,naq,resynthesis_pulse_index,pulse_clus_id,pulse_clusters,pca_w,
			DNN_W,input_minmax,dnnpulseindices,dnnpulses,&params);

	/* Exit */
  	return EXIT_SUCCESS;
}

/***********/
/*   EOF   */
/***********/


/*************************************************************
  Generalized-ICP Copyright (c) 2009 Aleksandr Segal.
  All rights reserved.

  Redistribution and use in source and binary forms, with
  or without modification, are permitted provided that the
  following conditions are met:

* Redistributions of source code must retain the above
  copyright notice, this list of conditions and the
  following disclaimer.
* Redistributions in binary form must reproduce the above
  copyright notice, this list of conditions and the
  following disclaimer in the documentation and/or other
  materials provided with the distribution.
* The names of the contributors may not be used to endorse
  or promote products derived from this software
  without specific prior written permission.

  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
  CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
  WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
  WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
  PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
  INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
  PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
  OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
  DAMAGE.
*************************************************************/

#ifndef GICP_H_
#define GICP_H_

#include <ANN.h>
#include <vector>
#include <iostream>
//#include <gsl/gsl.h>
#include <semaphore.h>
#include "transform.h"

namespace dgc {
namespace gicp {
typedef double gicp_mat_t[3][3];

struct GICPPoint {
    double x, y, z;
    float range;
    gicp_mat_t C; // covariance matrix
};

class GICPPointSet {
public:
    GICPPointSet();
    ~GICPPointSet();
    void BuildKDTree();
    void ComputeMatrices();

    void SavePoints(const char *filename);
    void SaveMatrices(const char *filename);

    int NumPoints() { return (int)point_.size(); }
    void Clear(void);
    int Size() { return point_.size(); }
    inline void AppendPoint(GICPPoint const & pt) { point_.push_back(pt); }
    void SetMaxIteration(int iter) { max_iteration_ = iter; }
    void SetMaxIterationInner(int iter) { max_iteration_inner_ = iter; }
    void SetEpsilon(double eps) { epsilon_ = eps; }
    void SetSolveRotation(bool s) { solve_rotation_ = s; }
    void SetGICPEpsilon(double eps) { gicp_epsilon_ = eps; }
    void SetDebug(bool d) { debug_ = d; }

    GICPPoint & operator[](int i) { return point_[i]; }
    GICPPoint const& operator[](int i) const { return point_[i]; }

    // returns number of iterations it took to converge
    int AlignScan(GICPPointSet *scan, dgc_transform_t base_t, dgc_transform_t t, double max_match_dist, bool save_error_plot = 0);

private:
    std::vector <GICPPoint> point_;
    ANNpointArray kdtree_points_;
    ANNkd_tree *kdtree_;
    int max_iteration_;
    int max_iteration_inner_;
    double epsilon_;
    double epsilon_rot_;
    double gicp_epsilon_;
    bool debug_;
    bool solve_rotation_;
    bool matrices_done_;
    bool kdtree_done_;
    pthread_mutex_t mutex_;
};

}

}

#endif