text:
#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);
}

max_stars_repo_name: byurhanbeyzat/vsc-material-theme

max_issues_repo_name: byurhanbeyzat/vsc-material-theme

max_forks_repo_name: byurhanbeyzat/vsc-material-theme


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

#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_ieee_utils.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_rng.h>

static double
test_ldlt_norm1(const gsl_matrix * m)
{
  const size_t N = m->size2;
  double value = 0.0;
  size_t j;

  for (j = 0; j < N; ++j)
    {
      gsl_vector_const_view v = gsl_matrix_const_column(m, j);
      double sum = gsl_blas_dasum(&v.vector);
      value = GSL_MAX(value, sum);
    }

  return value;
}

static int
test_ldlt_decomp_eps(const gsl_matrix * m, const double expected_rcond,
                     const double eps, const char * desc)
{
  int s = 0;
  size_t i, j, N = m->size2;

  gsl_matrix * V  = gsl_matrix_alloc(N, N);
  gsl_matrix * A  = gsl_matrix_alloc(N, N);
  gsl_matrix * L  = gsl_matrix_calloc(N, N);
  gsl_matrix * LT = gsl_matrix_calloc(N, N);
  gsl_vector_view d;

  gsl_matrix_memcpy(V, m);

  s += gsl_linalg_ldlt_decomp(V);

  /* compute L and LT */
  gsl_matrix_tricpy(CblasLower, CblasUnit, L, V);
  d = gsl_matrix_diagonal(L);
  gsl_vector_set_all(&d.vector, 1.0);
  gsl_matrix_transpose_tricpy(CblasLower, CblasNonUnit, LT, L);

  /* compute L <- L D */
  d = gsl_matrix_diagonal(V);
  for (i = 0; i < N; ++i)
    {
      gsl_vector_view c = gsl_matrix_column(L, i);
      double di = gsl_vector_get(&d.vector, i);
      gsl_vector_scale(&c.vector, di);
    }

  /* compute A = L D LT */
  gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1.0, L, LT, 0.0, A);

  for (i = 0; i < N; i++)
    {
      for (j = 0; j < N; j++)
        {
          double Aij = gsl_matrix_get(A, i, j);
          double mij = gsl_matrix_get(m, i, j);

          gsl_test_rel(Aij, mij, eps,
                       "%s: (%3lu,%3lu)[%lu,%lu]: %22.18g   %22.18g\n",
                       desc, N, N, i, j, Aij, mij);
        }
    }

  if (expected_rcond > 0)
    {
      gsl_vector *work = gsl_vector_alloc(3 * N);
      double rcond;

      gsl_linalg_ldlt_rcond(V, &rcond, work);

      gsl_test_rel(rcond, expected_rcond, 1.0e-6,
                   "%s rcond: (%3lu,%3lu): %22.18g   %22.18g\n",
                   desc, N, N, rcond, expected_rcond);

      gsl_vector_free(work);
    }

  gsl_matrix_free(V);
  gsl_matrix_free(A);
  gsl_matrix_free(L);
  gsl_matrix_free(LT);

  return s;
}

static int
test_ldlt_decomp(gsl_rng * r)
{
  int s = 0;
  const size_t N_max = 50;
  size_t N;

  for (N = 1; N <= N_max; ++N)
    {
      gsl_matrix * m = gsl_matrix_alloc(N, N);

      create_posdef_matrix(m, r);

      test_ldlt_decomp_eps(m, -1.0, 1.0e2 * N * GSL_DBL_EPSILON, "ldlt_decomp random");

      if (N <= 12)
        {
          double expected_rcond = -1.0;

          if (hilb_rcond[N - 1] > 1.0e-12)
            expected_rcond = hilb_rcond[N - 1];

          create_hilbert_matrix2(m);
          test_ldlt_decomp_eps(m, expected_rcond, N * GSL_DBL_EPSILON, "ldlt_decomp hilbert");
        }

      gsl_matrix_free(m);
    }

  return s;
}

int
test_ldlt_solve_eps(const gsl_matrix * m, const gsl_vector * rhs,
                    const gsl_vector * sol, const double eps,
                    const char * desc)
{
  int s = 0;
  size_t i, N = m->size1;
  gsl_matrix * u  = gsl_matrix_alloc(N, N);
  gsl_vector * x = gsl_vector_calloc(N);

  gsl_matrix_memcpy(u, m);

  s += gsl_linalg_ldlt_decomp(u);
  s += gsl_linalg_ldlt_solve(u, rhs, x);

  for (i = 0; i < N; i++)
    {
      double xi = gsl_vector_get(x, i);
      double yi = gsl_vector_get(sol, i);

      gsl_test_rel(xi, yi, eps,
                   "%s: %3lu[%lu]: %22.18g   %22.18g\n",
                   desc, N, i, xi, yi);
    }

  gsl_vector_free(x);
  gsl_matrix_free(u);

  return s;
}

static int
test_ldlt_solve(gsl_rng * r)
{
  int s = 0;
  const size_t N_max = 50;
  size_t N;

  for (N = 1; N <= N_max; ++N)
    {
      gsl_matrix * m = gsl_matrix_alloc(N, N);
      gsl_vector * rhs = gsl_vector_alloc(N);
      gsl_vector * sol = gsl_vector_alloc(N);

      create_posdef_matrix(m, r);
      create_random_vector(sol, r);
      gsl_blas_dsymv(CblasLower, 1.0, m, sol, 0.0, rhs);

      test_ldlt_solve_eps(m, rhs, sol, 64.0 * N * GSL_DBL_EPSILON, "ldlt_solve random");

      if (N <= 3)
        {
          create_hilbert_matrix2(m);
          gsl_blas_dsymv(CblasLower, 1.0, m, sol, 0.0, rhs);
          test_ldlt_solve_eps(m, rhs, sol, 1024.0 * N * GSL_DBL_EPSILON, "ldlt_solve hilbert");
        }

      gsl_matrix_free(m);
      gsl_vector_free(rhs);
      gsl_vector_free(sol);
    }

  return s;
}

static int
test_ldlt_band_decomp_eps(const size_t p, const gsl_matrix * m, const double eps, const char * desc)
{
  int s = 0;
  size_t i, j, N = m->size2;
  double rcond, rcond_expected;

  gsl_matrix * V  = gsl_matrix_alloc(N, p + 1);
  gsl_matrix * A  = gsl_matrix_alloc(N, N);
  gsl_matrix * L  = gsl_matrix_calloc(N, N);
  gsl_matrix * LT = gsl_matrix_calloc(N, N);
  gsl_vector * D = gsl_vector_alloc(N);
  gsl_vector * work = gsl_vector_alloc(3 * N);

  /* convert m to packed banded format */
  symm2band_matrix(p, m, V);

  s += gsl_linalg_ldlt_band_decomp(V);

  /* compute L and LT */
  gsl_linalg_ldlt_band_unpack(V, L, D);
  gsl_matrix_transpose_tricpy(CblasLower, CblasNonUnit, LT, L);

  /* compute L <- L D */
  for (i = 0; i < N; ++i)
    {
      gsl_vector_view c = gsl_matrix_column(L, i);
      double di = gsl_vector_get(D, i);
      gsl_vector_scale(&c.vector, di);
    }

  /* compute A = L D LT */
  gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1.0, L, LT, 0.0, A);

  for (i = 0; i < N; i++)
    {
      for (j = 0; j < N; j++)
        {
          double Aij = gsl_matrix_get(A, i, j);
          double mij = gsl_matrix_get(m, i, j);

          gsl_test_rel(Aij, mij, eps,
                       "%s: (p=%zu,N=%zu)[%lu,%lu]: %22.18g   %22.18g\n",
                       desc, p, N, i, j, Aij, mij);
        }
    }

  /* test 1-norm calculation */
  if (p > 0)
    {
      double norm1_expected = test_ldlt_norm1(m);
      double norm1 = gsl_matrix_get(V, N - 1, p);

      gsl_test_rel(norm1, norm1_expected, eps,
                   "%s: (p=%zu,N=%zu) 1-norm: %22.18g   %22.18g\n",
                   desc, p, N, norm1, norm1_expected);
    }

  /* test rcond */
  gsl_matrix_memcpy(A, m);
  s += gsl_linalg_ldlt_decomp(A);
  s += gsl_linalg_ldlt_rcond(A, &rcond_expected, work);
  s += gsl_linalg_ldlt_band_rcond(V, &rcond, work);
  gsl_test_rel(rcond, rcond_expected, eps,
               "%s: (p=%zu,N=%zu) rcond: %22.18g   %22.18g\n",
               desc, p, N, rcond, rcond_expected);

  gsl_matrix_free(V);
  gsl_matrix_free(A);
  gsl_matrix_free(L);
  gsl_matrix_free(LT);
  gsl_vector_free(D);
  gsl_vector_free(work);

  return s;
}

static int
test_ldlt_band_decomp(gsl_rng * r)
{
  int s = 0;
  const size_t N_max = 50;
  size_t N, p;

  for (N = 1; N <= N_max; ++N)
    {
      gsl_matrix * m = gsl_matrix_alloc(N, N);

      for (p = 0; p < GSL_MIN(N, 10); ++p)
        {
          create_posdef_band_matrix(p, m, r);
          s += test_ldlt_band_decomp_eps(p, m, 1.0e3 * N * GSL_DBL_EPSILON, "ldlt_band_decomp random");
        }

      gsl_matrix_free(m);
    }

  return s;
}

int
test_ldlt_band_solve_eps(const size_t p, const gsl_matrix * m, const gsl_vector * rhs,
                         const gsl_vector * sol, const double eps, const char * desc)
{
  int s = 0;
  size_t i, N = m->size1;
  gsl_matrix * u  = gsl_matrix_alloc(N, p + 1);
  gsl_vector * x = gsl_vector_alloc(N);

  /* convert m to packed banded format */
  symm2band_matrix(p, m, u);

  s += gsl_linalg_ldlt_band_decomp(u);
  s += gsl_linalg_ldlt_band_solve(u, rhs, x);

  for (i = 0; i < N; i++)
    {
      double xi = gsl_vector_get(x, i);
      double yi = gsl_vector_get(sol, i);

      gsl_test_rel(xi, yi, eps,
                   "%s: p=%zu N=%zu [%lu]: %22.18g   %22.18g\n",
                   desc, p, N, i, xi, yi);
    }

  gsl_vector_free(x);
  gsl_matrix_free(u);

  return s;
}

static int
test_ldlt_band_solve(gsl_rng * r)
{
  int s = 0;
  const size_t N_max = 50;
  size_t N, p;

  for (N = 1; N <= N_max; ++N)
    {
      gsl_matrix * m = gsl_matrix_alloc(N, N);
      gsl_vector * rhs = gsl_vector_alloc(N);
      gsl_vector * sol = gsl_vector_alloc(N);

      for (p = 0; p < GSL_MIN(N, 10); ++p)
        {
          create_posdef_band_matrix(p, m, r);
          create_random_vector(sol, r);
          gsl_blas_dsymv(CblasLower, 1.0, m, sol, 0.0, rhs);

          test_ldlt_band_solve_eps(p, m, rhs, sol, 64.0 * N * GSL_DBL_EPSILON, "ldlt_band_solve random");
        }

      gsl_matrix_free(m);
      gsl_vector_free(rhs);
      gsl_vector_free(sol);
    }

  return s;
}

ext: c

max_stars_repo_name: zzpwahaha/Chimera-Control-Trim

max_issues_repo_name: zzpwahaha/Chimera-Control-Trim

max_forks_repo_name: zzpwahaha/Chimera-Control-Trim


text:
/*****************************************
Emitting C Generated Code
*******************************************/
#include <cblas.h>
#include <stdlib.h>
#include <stdio.h>
#include <stdint.h>
#include <stdbool.h>
/**************** Snippet ****************/
void Snippet(int x0) {
  float x1[3] = { 1.0, 2.0, 3.0 };
  float x2[6] = { 1.0, 2.0, 3.0, 4.0, 5.0, 6.0 };
  float x3[6] = { 1.0, 2.0, 3.0, 4.0, 5.0, 6.0 };
  float* x4 = (float*)malloc(1 * sizeof(float));
  int x5 = 0;
  while (x5 != 3) {
    int x6 = x5;
    x4[0] = x4[0] + x1[x6] * x1[x6];
    x5 = x5 + 1;
  }
  int x7 = 0;
  while (x7 != 1) {
    printf("%f ", x4[x7]);
    x7 = x7 + 1;
  }
  float* x8 = (float*)malloc(2 * sizeof(float));
  cblas_sgemv(CblasRowMajor, CblasNoTrans, 2, 3, 1.0, x2, 3, x1, 1, 0.0, x8, 1);
  int x9 = 0;
  while (x9 != 2) {
    printf("%f ", x8[x9]);
    x9 = x9 + 1;
  }
  float* x10 = (float*)malloc(4 * sizeof(float));
  cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, 2, 2, 3, 1.0, x2, 3, x3, 2, 0.0, x10, 2);
  int x11 = 0;
  while (x11 != 4) {
    printf("%f ", x10[x11]);
    x11 = x11 + 1;
  }
}
/*****************************************
End of C Generated Code
*******************************************/
int main(int argc, char *argv[]) {
  if (argc != 2) {
    printf("usage: %s <arg>\n", argv[0]);
    return 0;
  }
  Snippet(atoi(argv[1]));
  return 0;
}

max_stars_repo_name: Shangyint/lms-clean

max_issues_repo_name: Shangyint/lms-clean

max_forks_repo_name: Shangyint/lms-clean


text:
/* -*- mode: C; c-basic-offset: 4 -*- */
/* ex: set shiftwidth=4 tabstop=4 expandtab: */
/*
 * Copyright (c) 2013, Georgia Tech Research Corporation
 * Copyright (c) 2015, Rice University
 * Copyright (c) 2018-2019, Colorado School of Mines
 * All rights reserved.
 *
 * Author(s): Neil T. Dantam <ntd@gatech.edu>
 * Georgia Tech Humanoid Robotics Lab
 * Under Direction of Prof. Mike Stilman <mstilman@cc.gatech.edu>
 *
 *
 * This file is provided under the following "BSD-style" License:
 *
 *
 *   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.
 *
 *   * Neither the name of the Rice University nor the names of its
 *     contributors may 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 HOLDER 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.
 *
 */

#include <nlopt.h>
#include "amino.h"
#include "amino/diffeq.h"

/* static double */
/* s_nlobj_jpinv(unsigned n, const double *q, double *dq, void *vcx) */
/* { */
/*     struct kin_solve_cx *cx = (struct kin_solve_cx*)vcx; */
/*     void *ptrtop = aa_mem_region_ptr(cx->reg); */

/*     struct aa_dvec vq = AA_DVEC_INIT(n,(double*)q,1); */
/*     aa_rx_fk_sub(cx->fk, cx->ssg, &vq); */
/*     double *E_act = aa_rx_fk_ref(cx->fk, cx->frame); */

/*     if( dq ) { */
/*         struct aa_dvec v_dq = AA_DVEC_INIT(n,dq,1); */
/*         s_ksol_jpinv(cx,q, &v_dq); */
/*         aa_dvec_scal(-1,&v_dq); */
/*     } */

/*     double x = s_serr( E_act, cx->TF_ref->data ); */

/*     aa_mem_region_pop(cx->reg, ptrtop); */
/*     return x; */

/* } */

static double s_nlobj_dq_fd_helper( void *vcx, const struct aa_dvec *x)
{
    struct kin_solve_cx *cx = (struct kin_solve_cx*)vcx;
    void *ptrtop = aa_mem_region_ptr(cx->reg);
    assert(1 == x->inc);

    aa_rx_fk_sub(cx->fk, cx->ssg, x);
    double *E_act = aa_rx_fk_ref(cx->fk, cx->frame);

    double S_act[8], S_ref[8], S_err[8], S_ln[8];
    aa_tf_qutr2duqu(E_act, S_act);
    aa_tf_qutr2duqu(cx->TF_ref->data, S_ref);
    aa_tf_duqu_cmul(S_act,S_ref,S_err);
    aa_tf_duqu_minimize(S_err);
    aa_tf_duqu_ln(S_err, S_ln);
    double result = aa_la_dot(8,S_ln,S_ln);

    aa_mem_region_pop(cx->reg, ptrtop);
    return result;
}

AA_API double
aa_rx_ik_opt_err_dqln_fd( void *vcx, const double *q, double *dq )
{
    struct kin_solve_cx *cx = (struct kin_solve_cx*)vcx;
    size_t n = aa_rx_sg_sub_config_count(cx->ssg);

    struct aa_dvec vq = AA_DVEC_INIT(n,(double*)q,1);
    double x = s_nlobj_dq_fd_helper(vcx, &vq);

    if( dq ) {
        struct aa_dvec vdq = AA_DVEC_INIT(n,dq,1);
        // TODO: make epsilon a parameter
        double eps = 1e-6;
        aa_de_grad_fd( s_nlobj_dq_fd_helper, vcx,
                       &vq, eps, &vdq );
    }

    return x;
}

static void
mv_block_helper (const double *A, const double *x, double *y)
{
    cblas_dgemv(CblasColMajor, CblasTrans, 8, 4,
                1, A, 8, x, 1,
                0, y, 1);
    cblas_dgemv(CblasColMajor, CblasTrans, 4, 4,
                1, A, 8, x+4, 1,
                0, y+4, 1);
}

static void
duqu_rmul_helper( const double *Sr, const double *x, double *y )
{
    double M[8*8];
    aa_tf_duqu_matrix_r(Sr,M,8);
    mv_block_helper(M, x, y);
}

AA_API double
aa_rx_ik_opt_err_dqln( void *vcx, double *q, double *dq ) {
    struct kin_solve_cx *cx = (struct kin_solve_cx*)vcx;
    void *ptrtop = aa_mem_region_ptr(cx->reg);

    size_t n = aa_rx_sg_sub_config_count(cx->ssg);
    struct aa_dvec vq = AA_DVEC_INIT(n,(double*)q,1);
    aa_rx_fk_sub(cx->fk, cx->ssg, &vq );
    double *E_act = aa_rx_fk_ref(cx->fk, cx->frame);

    double S_act[8], S_ref[8], S_err[8], S_ln[8];
    aa_tf_qutr2duqu(E_act, S_act);
    aa_tf_qutr2duqu(cx->TF_ref->data, S_ref);
    aa_tf_duqu_cmul(S_act,S_ref,S_err);
    // Apply a negative factor in gradient when we have to minimze the
    // error quaternion
    int needs_min = (S_err[AA_TF_DUQU_REAL_W] < 0 );
    if( needs_min ) {
        for( size_t i = 0; i < 8; i ++ ) S_err[i] *= -1;
    } else {
    }
    aa_tf_duqu_ln(S_err, S_ln);
    double result = aa_la_dot(8,S_ln,S_ln);

    if( dq ) {
        if( needs_min ) {
            for( size_t i = 0; i < 8; i ++ ) S_ln[i] *= -1;
        }
        struct aa_dvec v_dq = AA_DVEC_INIT(n,dq,1);

        /*
         * g_sumsq * J_ln*[S_ref]_r*J_conj*J_S
         * -> J_ln^T g_sumsq * [S_ref]_r*J_conj*J_S
         * -> [S_ref]_r^T (J_ln^T g_sumsq) *J_conj*J_S
         * -> J_conj^T*[S_ref]_r^T (J_ln^T g_sumsq) * J_S
         * -> J_S^T J_conj^T * [S_ref]_r^T * J_ln^T g_sumsq
         */

        double a[8], b[8], dJ[8*8];
        struct aa_dmat vJ_8x8 = AA_DMAT_INIT(8,8,dJ,8);
        struct aa_dmat *J_8x8 = &vJ_8x8;

        {
            struct aa_dmat *J_ln = J_8x8;
            aa_tf_duqu_ln_jac(S_err,J_ln);
            /*
             *  struct aa_dvec v_S_ln = AA_DVEC_INIT(8,S_ln,1);
             * //aa_dmat_gemv( CblasTrans, 2, J_8x8, &v_S_ln, 0, &va );
             *
             * Avoid multiplying by the zero block:
             *
             * J_ln = [J_r   0]     J_ln^T = [J_r^T J_d^T]
             *        [J_d J_r]              [  0   J_r^T]
             *
             * J_ln^T * del(S_ln^T*S_ln)^T = 2*J_ln^T S_ln
             *
             * 2*J_ln^T S_ln = 2*[J_r^T  J_d^T] (S_ln_r)
             *                   [   0   J_r^T] (S_ln_d)
             *
             */
            mv_block_helper(dJ, S_ln, a);
        }

        /*
         * dS/dphi = [S]_R V J
         * g [S]_R V J
         * -> [S]_R^T g V J
         * -> V^T [S]_R^T g J
         * -> J^T V [S]_R^T g
         */

        duqu_rmul_helper( S_ref, a, b );
        aa_tf_duqu_conj1(b);

        duqu_rmul_helper( S_act, b, a );  /* TODO: Pure result, some
                                           * extra multiplies here. */
        // a is now a pure dual quaternion
        {
            double c[6];
            struct aa_dvec vc = AA_DVEC_INIT(6,c,1);
            // Using Twist Jacobian
            aa_tf_duqu2pure(a,c);
            struct aa_dmat *Jtw = aa_rx_sg_sub_jac_twist_get(cx->ssg, cx->reg, cx->fk);
            aa_dmat_gemv( CblasTrans, 1, Jtw, &vc, 0, &v_dq );

            // Using Velocity Jacobian
            /* aa_tf_cross_a(a+AA_TF_DUQU_DUAL_XYZ,E_act+AA_TF_QUTR_V,a); */
            /* aa_tf_duqu2pure(a,c); */
            /* struct aa_dmat *J_vel = aa_rx_sg_sub_get_jacobian(cx->ssg,cx->reg,cx->TF); */
            /* aa_dmat_gemv( CblasTrans, 1, J_vel, &vc, 0, &v_dq ); */

        }

        /* { */
        /*     struct aa_dvec *v_dq_fd = aa_dvec_alloc(cx->reg,n); */
        /*     double eps = 1e-6; */
        /*     aa_de_grad_fd( s_nlobj_dq_fd_helper, vcx, */
        /*                    &vq, eps, v_dq_fd ); */

        /*     printf("--\n"); */
        /*     printf("needs min: %d\n", needs_min); */
        /*     printf("ad: "); */
        /*     aa_dump_vec(stdout,v_dq_fd->data,n); */
        /*     printf("an: "); */
        /*     aa_dump_vec(stdout,dq,n); */

        /*     assert( aa_dvec_ssd(v_dq_fd, &v_dq) < 1e-3 ); */
        /* } */
    }

    aa_mem_region_pop(cx->reg, ptrtop);
    return result;
}

static double s_nlobj_qv_fd_trans_helper( void *vcx, const struct aa_dvec *x)
{
    struct kin_solve_cx *cx = (struct kin_solve_cx*)vcx;
    assert(1 == x->inc);

    aa_rx_fk_sub(cx->fk, cx->ssg, x);
    double *E_act = aa_rx_fk_ref(cx->fk, cx->frame);
    double *v_act = E_act + AA_TF_QUTR_V;

    double *E_ref = cx->TF_ref->data;
    double *v_ref = E_ref + AA_TF_QUTR_V;

    double E_err[7];
    double *v_err = E_err + AA_TF_QUTR_V;

    for(size_t i = 0; i < 3; i ++ ) v_err[i] = v_act[i] - v_ref[i];

    double result = aa_tf_vdot(v_err,v_err);

    return result;
}

AA_API double
aa_rx_ik_opt_err_trans_fd( void *vcx, const double *q, double *dq )
{
    struct kin_solve_cx *cx = (struct kin_solve_cx*)vcx;
    size_t n = aa_rx_sg_sub_config_count(cx->ssg);

    struct aa_dvec vq = AA_DVEC_INIT(n,(double*)q,1);
    double x = s_nlobj_qv_fd_trans_helper(vcx, &vq);

    if( dq ) {
        struct aa_dvec vdq = AA_DVEC_INIT(n,dq,1);
        // TODO: make epsilon a parameter
        double eps = 1e-6;
        aa_de_grad_fd( s_nlobj_qv_fd_trans_helper, vcx,
                       &vq, eps, &vdq );
    }
    /* fprintf(stdout, "fd error: %2.2f\n", x);  */
    return x;
}

static double s_nlobj_qv_fd_helper( void *vcx, const struct aa_dvec *x)
{
    struct kin_solve_cx *cx = (struct kin_solve_cx*)vcx;
    void *ptrtop = aa_mem_region_ptr(cx->reg);
    assert(1 == x->inc);

    aa_rx_fk_sub(cx->fk, cx->ssg, x);
    double *E_act = aa_rx_fk_ref(cx->fk, cx->frame);
    double *v_act = E_act + AA_TF_QUTR_V;
    double *q_act = E_act + AA_TF_QUTR_Q;

    double *E_ref = cx->TF_ref->data;
    double *v_ref = E_ref + AA_TF_QUTR_V;
    double *q_ref = E_ref + AA_TF_QUTR_Q;

    double E_err[7];
    double *v_err = E_err + AA_TF_QUTR_V;
    double *q_err = E_err + AA_TF_QUTR_Q;

    aa_tf_qcmul(q_act,q_ref,q_err);

    double q_ln[4];
    aa_tf_qminimize(q_err);
    aa_tf_duqu_ln(q_err, q_ln);

    for(size_t i = 0; i < 3; i ++ ) v_err[i] = v_act[i] - v_ref[i];

    double result = ( aa_tf_qdot(q_ln,q_ln)
                      +
                      aa_tf_vdot(v_err,v_err) );

    aa_mem_region_pop(cx->reg, ptrtop);
    return result;
}

AA_API double
aa_rx_ik_opt_err_qlnpv_fd( void *vcx, const double *q, double *dq )
{
    struct kin_solve_cx *cx = (struct kin_solve_cx*)vcx;
    size_t n = aa_rx_sg_sub_config_count(cx->ssg);

    struct aa_dvec vq = AA_DVEC_INIT(n,(double*)q,1);
    double x = s_nlobj_qv_fd_helper(vcx, &vq);

    if( dq ) {
        struct aa_dvec vdq = AA_DVEC_INIT(n,dq,1);
        // TODO: make epsilon a parameter
        double eps = 1e-6;
        aa_de_grad_fd( s_nlobj_qv_fd_helper, vcx,
                       &vq, eps, &vdq );
    }

    return x;
}

AA_API double
aa_rx_ik_opt_err_trans( void *vcx, const double *q, double *dq )
{
    struct kin_solve_cx *cx = (struct kin_solve_cx*)vcx;
    void *ptrtop = aa_mem_region_ptr(cx->reg);

    size_t n = aa_rx_sg_sub_config_count(cx->ssg);
    struct aa_dvec vq = AA_DVEC_INIT(n,(double*)q,1);
    aa_rx_fk_sub(cx->fk, cx->ssg, &vq );

    double *E_act = aa_rx_fk_ref(cx->fk, cx->frame);
    double *v_act = E_act + AA_TF_QUTR_V;

    double *E_ref = cx->TF_ref->data;
    double *v_ref = E_ref + AA_TF_QUTR_V;

    double E_err[7];
    double *v_err = E_err + AA_TF_QUTR_V;

    // Apply a negative factor in gradient when we have to minimze the
    // error quaternion

    for(size_t i = 0; i < 3; i ++ ) v_err[i] = v_act[i] - v_ref[i];

    double result = aa_tf_vdot(v_err,v_err);

    if( dq ) {

        struct aa_dvec v_dq = AA_DVEC_INIT(n,dq,1);

        struct aa_dmat *Jvel = aa_rx_sg_sub_jac_vel_get(cx->ssg, cx->reg, cx->fk);
        struct aa_dmat Jr, Jv;
        aa_dmat_view_block(&Jv, Jvel, AA_TF_DX_V, 0, 3, Jvel->cols);
        aa_dmat_view_block(&Jr, Jvel, AA_TF_DX_W, 0, 3, Jvel->cols);

        // Translational Part
        struct aa_dvec v_v_err = AA_DVEC_INIT(3,v_err,1);
        aa_dmat_gemv(CblasTrans, 2, &Jv, &v_v_err, 0, &v_dq);
    }

    aa_mem_region_pop(cx->reg, ptrtop);
    /* printf("err: %f\n", result); */
    return result;
}

static void
q_rmul_helper( const double *q, const struct aa_dvec *x, struct aa_dvec *y )
{
    double dM[4*4];
    struct aa_dmat M = AA_DMAT_INIT(4,4,dM,4);
    aa_tf_qmat_r(q,&M);
    aa_dmat_gemv(CblasTrans, 1, &M, x, 0, y);
}

AA_API double
aa_rx_ik_opt_err_qlnpv( void *vcx, const double *q, double *dq )
{
    struct kin_solve_cx *cx = (struct kin_solve_cx*)vcx;
    void *ptrtop = aa_mem_region_ptr(cx->reg);

    size_t n = aa_rx_sg_sub_config_count(cx->ssg);
    struct aa_dvec vq = AA_DVEC_INIT(n,(double*)q,1);
    aa_rx_fk_sub(cx->fk, cx->ssg, &vq );

    double *E_act = aa_rx_fk_ref(cx->fk, cx->frame);
    double *v_act = E_act + AA_TF_QUTR_V;
    double *q_act = E_act + AA_TF_QUTR_Q;

    double *E_ref = cx->TF_ref->data;
    double *v_ref = E_ref + AA_TF_QUTR_V;
    double *q_ref = E_ref + AA_TF_QUTR_Q;

    double E_err[7];
    double *v_err = E_err + AA_TF_QUTR_V;
    double *q_err = E_err + AA_TF_QUTR_Q;

    aa_tf_qcmul(q_act,q_ref,q_err);

    // Apply a negative factor in gradient when we have to minimze the
    // error quaternion
    int needs_min = (E_err[AA_TF_QUAT_W] < 0 );
    if( needs_min ) {
        for( size_t i = 0; i < 4; i ++ ) E_err[i] *= -1;
    }

    double q_ln[4];
    aa_tf_qln(E_err, q_ln);

    for(size_t i = 0; i < 3; i ++ ) v_err[i] = v_act[i] - v_ref[i];

    double result = ( aa_tf_qdot(q_ln,q_ln) +
                      + aa_tf_vdot(v_err,v_err) );

    if( dq ) {

        struct aa_dvec v_dq = AA_DVEC_INIT(n,dq,1);

        struct aa_dmat *Jvel = aa_rx_sg_sub_jac_vel_get(cx->ssg, cx->reg, cx->fk);
        struct aa_dmat Jr, Jv;
        aa_dmat_view_block(&Jv, Jvel, AA_TF_DX_V, 0, 3, Jvel->cols);
        aa_dmat_view_block(&Jr, Jvel, AA_TF_DX_W, 0, 3, Jvel->cols);

        // Translational Part
        struct aa_dvec v_v_err = AA_DVEC_INIT(3,v_err,1);
        aa_dmat_gemv(CblasTrans, 2, &Jv, &v_v_err, 0, &v_dq);

        // Rotational Part
        if( needs_min ) {
            for( size_t i = 0; i < 4; i ++ ) q_ln[i] *= -1;
        }

        double a[4], b[4], dJ[4*4];
        struct aa_dvec va = AA_DVEC_INIT(4,a,1);
        struct aa_dvec vb = AA_DVEC_INIT(4,b,1);
        struct aa_dvec vln = AA_DVEC_INIT(4,q_ln,1);
        struct aa_dmat vJ_4x4 = AA_DMAT_INIT(4,4,dJ,4);
        struct aa_dmat *J_4x4 = &vJ_4x4;

        {
            struct aa_dmat *J_ln = J_4x4;
            aa_tf_qln_jac(q_err,J_ln);
            aa_dmat_gemv(CblasTrans, 1, J_ln, &vln, 0, &va);
        }

        q_rmul_helper( q_ref, &va, &vb );
        aa_tf_qconj1(b);

        q_rmul_helper( q_act, &vb, &va ); /* TODO: Pure result, some
                                           * extra multiplies here. */

        // a is now a pure dual quaternion */
        {
            struct aa_dvec va3 = AA_DVEC_INIT(3,a,1);
            aa_dmat_gemv( CblasTrans, 1, &Jr, &va3, 1, &v_dq );
        }

        /* { */
        /*     struct aa_dvec *v_dq_fd = aa_dvec_alloc(cx->reg,n); */
        /*     double eps = 1e-6; */
        /*     aa_de_grad_fd( s_nlobj_qv_fd_helper, vcx, */
        /*                    &vq, eps, v_dq_fd ); */

        /*     printf("--\n"); */
        /*     printf("needs min: %d\n", needs_min); */
        /*     printf("ad: "); */
        /*     aa_dump_vec(stdout,v_dq_fd->data,n); */
        /*     printf("an: "); */
        /*     aa_dump_vec(stdout,dq,n); */

        /*     assert( aa_dvec_ssd(v_dq_fd, &v_dq) < 1e-3 ); */
        /* } */
    }

    aa_mem_region_pop(cx->reg, ptrtop);
    //printf("err: %f\n", result);
    return result;
}

// TODO: weighted error

AA_API double
aa_rx_ik_opt_err_jcenter( void *vcx, const double *q, double *dq )
{
    struct kin_solve_cx *cx = (struct kin_solve_cx*)vcx;
    struct aa_mem_region *reg = cx->reg;
    void *ptrtop = aa_mem_region_ptr(reg);

    size_t n = aa_rx_sg_sub_config_count(cx->ssg);
    struct aa_dvec *q_center = aa_dvec_alloc(reg,n);
    aa_rx_sg_sub_center_configv(cx->ssg,q_center);

    double result = 0;

    if( dq ) { // error and gradient
        for( size_t i = 0; i < n; i ++ ) {
            double d = q[i] - AA_DVEC_REF(q_center,i);
            result += (d*d);
            dq[i] = d;
        }
        //printf("--\n");
        //printf("q:  "); aa_dump_vec(stdout,q,n);
        //printf("qc: "); aa_dump_vec(stdout,q_center->data,n);
        //printf("dq: "); aa_dump_vec(stdout,dq,n);
    } else { // error only
        for( size_t i = 0; i < n; i ++ ) {
            double d = q[i] - AA_DVEC_REF(q_center,i);
            result += (d*d);
        }
    }

    aa_mem_region_pop(cx->reg, ptrtop);

    return result / 2;
}

struct err_cx {
    void *cx;
    aa_rx_ik_opt_fun *fun;
};

static double
s_err_cx_dispatch(unsigned n, const double *q, double *dq, void *vcx)
{
    (void)n;
    struct err_cx *cx = (struct err_cx *)vcx;
    return cx->fun(cx->cx, q, dq);
}

static int
s_ik_nlopt( struct kin_solve_cx *cx,
            struct aa_dvec *q )
{
    struct aa_mem_region *reg = cx->reg;
    void *ptrtop = aa_mem_region_ptr(reg);

    const struct aa_rx_sg_sub *ssg = cx->ssg;
    const struct aa_rx_sg *sg = cx->ssg->scenegraph;

    if( 1 != cx->q_sub->inc || 1 != cx->q_all->inc ) {
        return AA_RX_INVALID_PARAMETER;
    }

    size_t n_sub = cx->q_sub->len;
    nlopt_algorithm alg = NLOPT_LD_SLSQP;
    nlopt_opt opt = nlopt_create(alg, (unsigned)n_sub); /* algorithm and dimensionality */

    struct err_cx ocx, eqctcx;
    ocx.cx = cx;
    ocx.fun = cx->ik_cx->opts->obj_fun;
    nlopt_set_min_objective(opt, s_err_cx_dispatch, &ocx);

    if( cx->ik_cx->opts->eqct_fun ) {
        eqctcx.cx = cx;
        eqctcx.fun = cx->ik_cx->opts->eqct_fun;
        nlopt_add_equality_constraint(opt, s_err_cx_dispatch, &eqctcx,
                                      cx->ik_cx->opts->eqct_tol);
    }

    //nlopt_set_xtol_rel(opt, 1e-4); // TODO: make a parameter
    if( cx->opts->tol_obj_abs >= 0 )
        nlopt_set_ftol_abs(opt, cx->opts->tol_obj_abs );
    if( cx->opts->tol_obj_rel >= 0 )
        nlopt_set_ftol_rel(opt, cx->opts->tol_obj_rel );
    if( cx->opts->tol_dq >= 0 )
        nlopt_set_xtol_rel(opt, cx->opts->tol_dq );

    double *lb = AA_MEM_REGION_NEW_N(reg,double,n_sub);
    double *ub = AA_MEM_REGION_NEW_N(reg,double,n_sub);
    { // bounds
        for( size_t i = 0; i < n_sub; i ++ ) {
            aa_rx_config_id id = aa_rx_sg_sub_config(ssg,i);
            if( aa_rx_sg_get_limit_pos(sg, id, lb+i, ub+i) )
            {
                lb[i] = -DBL_MAX;
                ub[i] = DBL_MAX;
            }
        }
    }

    nlopt_set_lower_bounds(opt, lb);
    nlopt_set_upper_bounds(opt, ub);
    double minf;
    nlopt_result ores = nlopt_optimize(opt, cx->q_sub->data, &minf);
    if( cx->opts->debug ) {
        /* fprintf("AMINO IK NLOPT RESULT: %s (%d)", */
        /*         nlopt_result_to_string(ores), ores); */

        fprintf(stderr, "AMINO IK NLOPT RESULT: %d\n", ores);
    }
    aa_dvec_copy( cx->q_sub, q );

    aa_rx_fk_sub(cx->fk, cx->ssg, q);
    double *E_act = aa_rx_fk_ref(cx->fk, cx->frame);
    int result = s_check(cx->ik_cx, cx->TF_ref, E_act );
    nlopt_destroy(opt);
    aa_mem_region_pop(reg,ptrtop);

    return result;

}

max_stars_repo_name: dyalab/amino

max_issues_repo_name: dyalab/amino

max_forks_repo_name: dyalab/amino


text:
/***********************************************************
 *	Program Name: conv.
 *
 *	A program used to process the data from mcml -
 *	A Monte Carlo simulation of photon distribution in
 *	multi-layered turbid media in ANSI Standard C.
 ****
 *	Creation Date:	11/1991.
 *	Current Date:	6/1992.
 *
 *	Lihong Wang, Ph. D.
 *	Steven L. Jacques, Ph. D.
 *	Laser Biology Research Laboratory - 17
 *	M.D. Anderson Cancer Center
 *	University of Texas
 *	1515 Holcombe Blvd.
 *	Houston, TX 77030
 *	USA
 *
 ****
 *	General Naming Conventions:
 *	Preprocessor names: all capital letters,
 *		e.g. #define PREPROCESSORS
 *	Globals: first letter of each word is capital, no
 *		underscores,
 *		e.g. short GlobalVar;
 *	Dummy variables:  first letter of each word is capital,
 *		and words are connected by underscores,
 *		e.g. void NiceFunction(char Dummy_Var);
 *	Local variables:  all lower cases, words are connected
 *		by underscores,
 *		e.g. short local_var;
 *	Function names or data types:  same as Globals.
 *
 ****
 *	Dimension of length: cm.
 *
 ****/

#ifndef __MCML_CONV_H__
#define __MCML_CONV_H__

#include <math.h>
#include <new>
#include <string>
#include <algorithm>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_sf_bessel.h>
#include "mcml_model.h"

#define GAUSSLIMIT 4
#define PI 3.1415926

#define SIGN(x) ((x)>=0 ? 1:-1)

/****************** Classes *****************************/

class Beam {
/* Beam class - incident light beam class
        Parameters to describe a photon beam.
        Pencil: infinitely narrow beam. This is default for the
            beam from the mcml output.
        Flat:	Flat beam with radius R.
        Gaussian:	Gaussian with 1/e2 radius R.
        Others: general beam described by points with interpolation.
        Class instance variables:
            type - incident beam type, FLAT or GAUSSIAN
            P - total beam power/energy [W or J]
            R - beam radius, defined as 1/e^2 for Gaussian beam [cm]
        Methods:

*/
  public:
    enum BeamType {
        FLAT,
        GAUSSIAN
    };
    BeamType type;			// beam type
    double P;       // total power. [J or W]
    double R;       // radius. [cm]

};

class Node {
/* Node class - node link list binary tree class
        Data structures for the binary tree used to store part of
        the integrand evaluation.

        Class instance variables:
            x - x grid node position
            y - y grid node position
            left - left node pointer
            right - right node pointer
        Methods:
*/

  public:
    double x;
    double y;
    Node * left;
    Node * right;

    Node () : left (nullptr), right (nullptr) {};
    static Node * FillNode(double x, double y);
    static Node * SearchNode(Node * tree, double x);
    static void InsertNode(Node * tree, double x, double y);
    static void FreeTree(Node * tree);
};

class ConvVar {
/* ConvVar class - convoluation variables class
        A global structure to pass the current coordinate of the
        physical quantities being evaluated and the pointers of the
        input and output parameters to the integration function.

        Class instance variables:
            r - r position
            iz - iz index
            ia - ia index
            tree - A tree to store ITheta() & ExpBessI0().
        Methods:

*/

  public:
    double r;
    short iz;
    short ia;
    Node * tree;      // A tree to store ITheta() & ExpBessI0().

    ConvVar () : tree (nullptr) {};
};

class ConvInput {
/* ConvInput class - beam convolution input class
        Input parameters for each independent run.
        z and r are for the cylindrical coordinate system. [cm]
        a is for the angle alpha between the photon exiting
        direction and the surface normal. [radian]
        The grid line separations in z, r, and alpha
        directions are dz, dr, and da respectively.  The numbers
        of grid lines in z, r, and alpha directions are
        nz, nr, and na respectively.
        The member layerspecs will point to an array of
        structures which store parameters of each layer.
        This array has (number_layers + 2) elements. One
        element is for a layer.
        The layers 0 and (num_layers + 1) are for top ambient
        medium and the bottom ambient medium respectively.
        For convolution, the grid line separations in z, and alpha
        directions are still dz, and da respectively.  The numbers
        of grid lines in z, and alpha directions are still
        nz, and na respectively. However, the grid line separation
        and the number of grid lines in r direction are drc and
        nrc respectively.
        Class instance variables:
            beam - incident beam class instance object
            drc - convolution r grid separation.[cm]
            nrc - convolution array range 0..nrc-1.
            eps - relative error in convolution
        Methods:

*/

  public:
    Beam beam;    // incident beam of finite size
    double drc;
    short nrc;
    MCMLModel mcmlModel;
    ConvVar convVar;

    void SelectConvInput (MCMLModel mcmlModelSet,
          Beam::BeamType beamType = Beam::FLAT, double P = 1.0, double R = 0);
    void FreeConvInput();
};

class MCMLConv : public ConvInput {
/* MCMLConv class - multi-layered photon scattering model beam convolution
        inherits from ConvInput beam setup
        Structures for scored physical quantities
        from mcml and to be convolved for photon
        beams of finite size.  Therefore, "Out"
        here means the output of both mcml and conv.
        The member allocated is used to keep the status
        of the arrays.  It is set to 1 if all the arrays
        are allocated and assigned values.  It is set to
        0 otherwise.
        z and r represent z and r coordinates of the
        cylindrical coordinate system. [cm]
        a is the angle alpha between the photon exiting
        direction and the normal to the surfaces. [radian]
        See comments of the InputStruct.
        See manual for the physcial quantities.
        Class instance variables:
            Rd_rac - convolved data. [J/(cm2 sr)]
            Rd_rc - 1D radial distribution of diffuse reflectance [J/cm2]
            A_rzc - 2D probability density in turbid media over r & z [J/cm3]
            Tt_rac - 2D distribution of total transmittance [J/(cm2 sr)]
            Tt_rc - 1D radial distribution of transmittance [J/cm2]
        Methods:

*/
  private:
    void ConvRd_ra ();
    void ConvRd_r ();
    void ConvA_rz ();
    void ConvTt_ra ();
    void ConvTt_r ();
    void ConvA2F();

  public:
    double ** Rd_rac;
    double * Rd_rc;
    double ** A_rzc;
    double ** Tt_rac;
    double * Tt_rc;
    double ** F_rzc;

    MCMLConv () : Rd_rac (nullptr), Rd_rc (nullptr), A_rzc (nullptr),
      Tt_rac (nullptr), Tt_rc (nullptr), F_rzc (nullptr) {};
    void SelectMCMLConv (MCMLModel mcmlModelSet, std::string beamType,
          double P, double R);
    void FreeMCMLConv ();
    void RunConv ();
    double CenterHalfMaxDepth ();
    double SurfaceHalfMaxWidth ();
};

#endif    //__MCML_CONV_H__

ext: h

max_stars_repo_name: harveyiliu/mcml-photon-scattering-cpp

max_issues_repo_name: harveyiliu/mcml-photon-scattering-cpp

max_forks_repo_name: harveyiliu/mcml-photon-scattering-cpp


text:
#ifndef PROGRESS_BAR_OBSERVER_H_0L8ZR7QT
#define PROGRESS_BAR_OBSERVER_H_0L8ZR7QT

#include "rang.hpp"

#include <atomic>
#include <cmath>
#include <cstdint>
#include <gsl/gsl>
#include <iomanip>
#include <sens_loc/util/console.h>
#include <taskflow/core/observer.hpp>

namespace sens_loc::util {

class progress_bar_observer : public tf::ExecutorObserverInterface {
  public:
    constexpr static int max_bars = 50;

    progress_bar_observer(std::int64_t total_tasks)
        : _total_tasks{total_tasks}
        , _done{0} {
        Expects(total_tasks >= 1);
    }

    progress_bar_observer(const progress_bar_observer&) = delete;
    progress_bar_observer(progress_bar_observer&&)      = delete;
    progress_bar_observer& operator=(const progress_bar_observer&) = delete;
    progress_bar_observer& operator=(progress_bar_observer&&) = delete;
    ~progress_bar_observer() override                         = default;

    void set_up(unsigned /*num_workers*/) override {}
    void on_entry(unsigned /*worker_id*/, tf::TaskView /*task_view*/) override;
    void on_exit(unsigned /*worker_id*/, tf::TaskView /*task_view*/) override;

  private:
    void print_bar(bool increment) noexcept;

    std::int64_t      _total_tasks;
    std::int64_t      _done;
    std::atomic<bool> _inital_output = false;
};
}  // namespace sens_loc::util

#endif /* end of include guard: PROGRESS_BAR_OBSERVER_H_0L8ZR7QT */

max_stars_repo_name: JonasToth/depth-conversions

max_issues_repo_name: JonasToth/depth-conversions

max_forks_repo_name: JonasToth/depth-conversions


text:
#include <gsl/gsl_math.h>
#include "gsl_cblas.h"
#include "cblas.h"

void
cblas_drotm (const int N, double *X, const int incX, double *Y,
	     const int incY, const double *P)
{
#define BASE double
#include "source_rotm.h"
#undef BASE
}


text:
#include <stdio.h>
#include <gsl/gsl_statistics.h>
#include <stdlib.h>
#include <float.h>

#ifndef N
#define N 32
#endif

#define K N/2

#include <klee/klee.h>
int
main(void)
{
  double data[K], w[K];
  float input[N];

  // symbolic inputs
  klee_make_symbolic(&input, sizeof(input), "input");

  int i;
  for (i = 0; i < K; i++){
    w[i] = input[i]  / FLT_MAX * 100;
  }
  for (i = 0; i < K; i++){
    data[i] = input[i+K]  / FLT_MAX * 100;
  }

  double wtss, wmean;
  wmean    = gsl_stats_wmean(w, 1, data, 1, K);
  wtss     = gsl_stats_wtss_m(w, 1, data, 1, K, wmean);

  printf ("The sample variance is %g\n", wtss);
  return 0;
}

max_stars_repo_name: snipekill/FPGen

max_issues_repo_name: snipekill/FPGen

max_forks_repo_name: snipekill/FPGen


text:
#include <stdio.h>
#include <math.h>
#include <gsl/gsl_integration.h>

void integral_gen (int nmin, int nmax, double vals[]);
double f (double x, void *params);

double f (double x, void *params)
{
    double n = (double) *(int *) params;
    double f = exp (-x) * pow (x, n);

    return f;
}

void integral_gen (int nmin, int nmax, double vals[])
{

    double a = 0., b = 1.;      // limits of integration
    double abserr = 0., relerr = 1.e-8; // requested errors
    double result;              // the integral value
    double error;               // the error estimate
    int n;

    size_t np = 1000;           // work area size
    gsl_integration_workspace *w = gsl_integration_workspace_alloc (np);

    gsl_function F;

    F.function = &f;
    F.params = &n;

    for (n = nmin; n <= nmax; n++)
    {
        gsl_integration_qag (&F, a, b, abserr, relerr, np, GSL_INTEG_GAUSS15,
            w, &result, &error);
        vals[n] = result;
    }

    gsl_integration_workspace_free (w);
}