// https://github.com/attractivechaos/matmul
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <stdio.h>

/**********************************
 * Pseudo-random number generator *
 **********************************/

static uint64_t mat_rng[2] = { 11ULL, 1181783497276652981ULL };

static inline uint64_t xorshift128plus(uint64_t s[2])
{
	uint64_t x, y;
	x = s[0], y = s[1];
	s[0] = y;
	x ^= x << 23;
	s[1] = x ^ y ^ (x >> 17) ^ (y >> 26);
	y += s[1];
	return y;
}

double mat_drand(void)
{
	return (xorshift128plus(mat_rng)>>11) * (1.0/9007199254740992.0);
}

/*******************************************
 * Helper routines for matrix manipulation *
 *******************************************/

float **mat_init(int n_rows, int n_cols)
{
	float **m;
	int i;
	m = (float**)malloc(n_rows * sizeof(float*));
	m[0] = (float*)calloc(n_rows * n_cols, sizeof(float));
	for (i = 1; i < n_rows; ++i)
		m[i] = m[i-1] + n_cols;
	return m;
}

void mat_destroy(float **m)
{
	free(m[0]); free(m);
}

float **mat_gen_random(int n_rows, int n_cols)
{
	float **m;
	int i, j;
	m = mat_init(n_rows, n_cols);
	for (i = 0; i < n_rows; ++i)
		for (j = 0; j < n_cols; ++j)
			m[i][j] = mat_drand();
	return m;
}

float **mat_transpose(int n_rows, int n_cols, float *const* a)
{
	int i, j;
	float **m;
	m = mat_init(n_cols, n_rows);
	for (i = 0; i < n_rows; ++i)
		for (j = 0; j < n_cols; ++j)
			m[j][i] = a[i][j];
	return m;
}

float sdot_1(int n, const float *x, const float *y)
{
	int i;
	float s = 0.0f;
	for (i = 0; i < n; ++i) s += x[i] * y[i];
	return s;
}

float sdot_8(int n, const float *x, const float *y)
{
	int i, n8 = n>>3<<3;
	float s, t[8];
	t[0] = t[1] = t[2] = t[3] = t[4] = t[5] = t[6] = t[7] = 0.0f;
	for (i = 0; i < n8; i += 8) {
		t[0] += x[i+0] * y[i+0];
		t[1] += x[i+1] * y[i+1];
		t[2] += x[i+2] * y[i+2];
		t[3] += x[i+3] * y[i+3];
		t[4] += x[i+4] * y[i+4];
		t[5] += x[i+5] * y[i+5];
		t[6] += x[i+6] * y[i+6];
		t[7] += x[i+7] * y[i+7];
	}
	for (s = 0.0f; i < n; ++i) s += x[i] * y[i];
	s += t[0] + t[1] + t[2] + t[3] + t[4] + t[5] + t[6] + t[7];
	return s;
}

#ifdef __SSE__
#include <xmmintrin.h>

float sdot_sse(int n, const float *x, const float *y)
{
	int i, n8 = n>>3<<3;
	__m128 vs1, vs2;
	float s, t[4];
	vs1 = _mm_setzero_ps();
	vs2 = _mm_setzero_ps();
	for (i = 0; i < n8; i += 8) {
		__m128 vx1, vx2, vy1, vy2;
		vx1 = _mm_loadu_ps(&x[i]);
		vx2 = _mm_loadu_ps(&x[i+4]);
		vy1 = _mm_loadu_ps(&y[i]);
		vy2 = _mm_loadu_ps(&y[i+4]);
		vs1 = _mm_add_ps(vs1, _mm_mul_ps(vx1, vy1));
		vs2 = _mm_add_ps(vs2, _mm_mul_ps(vx2, vy2));
	}
	for (s = 0.0f; i < n; ++i) s += x[i] * y[i];
	_mm_storeu_ps(t, vs1);
	s += t[0] + t[1] + t[2] + t[3];
	_mm_storeu_ps(t, vs2);
	s += t[0] + t[1] + t[2] + t[3];
	return s;
}
#endif

/*************************
 * Matrix multiplication *
 *************************/

float **mat_mul0(int n_a_rows, int n_a_cols, float *const *a, int n_b_cols, float *const *b)
{
	int i, j, k;
	float **m;
	m = mat_init(n_a_rows, n_b_cols);
	for (i = 0; i < n_a_rows; ++i) {
		for (j = 0; j < n_b_cols; ++j) {
			float t = 0.0;
			for (k = 0; k < n_a_cols; ++k)
				t += a[i][k] * b[k][j];
			m[i][j] = t;
		}
	}
	return m;
}

float **mat_mul1(int n_a_rows, int n_a_cols, float *const *a, int n_b_cols, float *const *b)
{
	int i, j, k, n_b_rows = n_a_cols;
	float **m, **bT;
	m = mat_init(n_a_rows, n_b_cols);
	bT = mat_transpose(n_b_rows, n_b_cols, b);
	for (i = 0; i < n_a_rows; ++i) {
		const float *ai = a[i];
		float *mi = m[i];
		for (j = 0; j < n_b_cols; ++j) {
			float t = 0.0f, *bTj = bT[j];
			for (k = 0; k < n_a_cols; ++k)
				t += ai[k] * bTj[k];
			mi[j] = t;
		}
	}
	mat_destroy(bT);
	return m;
}

#ifdef __SSE__
float **mat_mul2(int n_a_rows, int n_a_cols, float *const *a, int n_b_cols, float *const *b)
{
	int i, j, n_b_rows = n_a_cols;
	float **m, **bT;
	m = mat_init(n_a_rows, n_b_cols);
	bT = mat_transpose(n_b_rows, n_b_cols, b);
	for (i = 0; i < n_a_rows; ++i)
		for (j = 0; j < n_b_cols; ++j)
			m[i][j] = sdot_sse(n_a_cols, a[i], bT[j]);
	mat_destroy(bT);
	return m;
}
float **mat_mul7(int n_a_rows, int n_a_cols, float *const *a, int n_b_cols, float *const *b)
{
	int i, j, ii, jj, x = 16, n_b_rows = n_a_cols;
	float **m, **bT;
	m = mat_init(n_a_rows, n_b_cols);
	bT = mat_transpose(n_b_rows, n_b_cols, b);
	for (i = 0; i < n_a_rows; i += x) {
		for (j = 0; j < n_b_cols; j += x) {
			int je = n_b_cols < j + x? n_b_cols : j + x;
			int ie = n_a_rows < i + x? n_a_rows : i + x;
			for (ii = i; ii < ie; ++ii)
				for (jj = j; jj < je; ++jj)
					m[ii][jj] += sdot_sse(n_a_cols, a[ii], bT[jj]);
		}
	}
	mat_destroy(bT);
	return m;
}
#endif

float **mat_mul3(int n_a_rows, int n_a_cols, float *const *a, int n_b_cols, float *const *b)
{
	int i, j, n_b_rows = n_a_cols;
	float **m, **bT;
	m = mat_init(n_a_rows, n_b_cols);
	bT = mat_transpose(n_b_rows, n_b_cols, b);
	for (i = 0; i < n_a_rows; ++i)
		for (j = 0; j < n_b_cols; ++j)
			m[i][j] = sdot_8(n_a_cols, a[i], bT[j]);
	mat_destroy(bT);
	return m;
}

float **mat_mul4(int n_a_rows, int n_a_cols, float *const *a, int n_b_cols, float *const *b)
{
	int i, j, n_b_rows = n_a_cols;
	float **m, **bT;
	m = mat_init(n_a_rows, n_b_cols);
	bT = mat_transpose(n_b_rows, n_b_cols, b);
	for (i = 0; i < n_a_rows; ++i)
		for (j = 0; j < n_b_cols; ++j)
			m[i][j] = sdot_1(n_a_cols, a[i], bT[j]);
	mat_destroy(bT);
	return m;
}

#ifdef HAVE_CBLAS
#include <cblas.h>

float **mat_mul5(int n_a_rows, int n_a_cols, float *const *a, int n_b_cols, float *const *b)
{
	int i, j, n_b_rows = n_a_cols;
	float **m, **bT;
	m = mat_init(n_a_rows, n_b_cols);
	bT = mat_transpose(n_b_rows, n_b_cols, b);
	for (i = 0; i < n_a_rows; ++i)
		for (j = 0; j < n_b_cols; ++j)
			m[i][j] = cblas_sdot(n_a_cols, a[i], 1, bT[j], 1);
	mat_destroy(bT);
	return m;
}

float **mat_mul6(int n_a_rows, int n_a_cols, float *const *a, int n_b_cols, float *const *b)
{
	float **m, n_b_rows = n_a_cols;
	m = mat_init(n_a_rows, n_b_cols);
	cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, n_a_rows, n_b_cols, n_a_cols, 1.0f, a[0], n_a_rows, b[0], n_b_rows, 0.0f, m[0], n_a_rows);
	return m;
}
#endif

/*****************
 * Main function *
 *****************/

#include <unistd.h>
#include <time.h>

int main(int argc, char *argv[])
{
	int c, n = 1000, algo = 2;
	clock_t t;
	float **a, **b, **m = 0;

	while ((c = getopt(argc, argv, "n:a:h")) >= 0) {
		if (c == 'n') n = atoi(optarg);
		else if (c == 'a') algo = atoi(optarg);
		else if (c == 'h') {
			fprintf(stderr, "Usage: mat-eval [options]\n");
			fprintf(stderr, "Options:\n");
			fprintf(stderr, "  -n INT    size of the square matrix [%d]\n", n);
			fprintf(stderr, "  -a INT    matrix multiplication implementation [%d]\n", algo);
			fprintf(stderr, "            0: naive - no optimization\n");
			fprintf(stderr, "            1: transposing the second matrix\n");
#ifdef __SSE__
			fprintf(stderr, "            2: explicitly vectorized sdot() with SSE\n");
			fprintf(stderr, "            7: explicitly SSE sdot() plus loop tiling\n");
#endif
			fprintf(stderr, "            3: implicitly vectorized sdot()\n");
			fprintf(stderr, "            4: no vectorization hints\n");
#ifdef HAVE_CBLAS
			fprintf(stderr, "            5: with sdot() from an external CBLAS library\n");
			fprintf(stderr, "            6: with sgemm() from an external CBLAS library\n");
#endif
			fprintf(stderr, "  -h        this help message\n");
			return 1;
		}
	}

	a = mat_gen_random(n, n);
	b = mat_gen_random(n, n);

	t = clock();
	if (algo == 0) {
		m = mat_mul0(n, n, a, n, b);
	} else if (algo == 1) {
		m = mat_mul1(n, n, a, n, b);
#ifdef __SSE__
	} else if (algo == 2) {
		m = mat_mul2(n, n, a, n, b);
	} else if (algo == 7) {
		m = mat_mul7(n, n, a, n, b);
#endif
	} else if (algo == 3) {
		m = mat_mul3(n, n, a, n, b);
	} else if (algo == 4) {
		m = mat_mul4(n, n, a, n, b);
#ifdef HAVE_CBLAS
	} else if (algo == 5) {
		m = mat_mul5(n, n, a, n, b);
	} else if (algo == 6) {
		m = mat_mul6(n, n, a, n, b);
#endif
	} else {
		fprintf(stderr, "ERROR: unknown algorithm %d\n", algo);
		return 1;
	}
	fprintf(stderr, "CPU time: %g\n", (double)(clock() - t) / CLOCKS_PER_SEC);
	fprintf(stderr, "Central cell: %g\n", m[n/2][n/2]);

	if (m) mat_destroy(m);
	mat_destroy(b); mat_destroy(a);
	return 0;
}


#include <assert.h>
#include <gsl/gsl_sf_gamma.h>
#include <inttypes.h>
#include <math.h>
#include <stdlib.h>

#include "index.h"
#include "parse_mmaps.h"
#include "probability.h"

double reference_probability(const struct mmap_info* mmap_info,
			     struct revision_assignment* parent,
			     struct revision_assignment* child,
			     int disagrees,
			     const struct index_patch* index_patch) {
  double action_prob = 1.0;
  if (parent != NULL && parent->topic >= 0 && parent->pov >= 0
      && child->topic >= 0 && child->pov >= 0) {
    struct revision_assignment_header* revision_assignment_header
      = (struct revision_assignment_header*)mmap_info->revision_assignment_mmap;
    struct topic_summary* topic_summary;
    struct pov_summary* pov_dist;
    get_topic_summary(mmap_info, child->topic, &topic_summary, &pov_dist, NULL);
    if (parent->topic == child->topic) {
      if (parent->pov != child->pov) {
	// POV reference
	struct pov_summary* pov_summary = get_ant_pov(mmap_info, pov_dist,
						      child->pov, parent->pov);
	double denom = (double)(pov_summary->norevert_count
				+ pov_summary->revert_count)
	  + revision_assignment_header->psi_alpha
	  + revision_assignment_header->psi_beta;
	int correction = 0;
	if (index_patch != NULL
	    && index_patch->topic == child->topic
	    && index_patch->pov == child->pov
	    && index_patch->parent_pov == parent->pov
	    && index_patch->parent_topic == index_patch->topic) {
	  assert(denom -= 1.0 >= 0.0);
	  if (!disagrees == !(index_patch->disagrees)) {
	    correction += 1;
	  }
	}
	if (index_patch != NULL
	    && index_patch->child_topic == child->topic
	    && index_patch->child_pov == child->pov
	    && index_patch->pov == parent->pov
	    && index_patch->topic == index_patch->child_topic) {
	  assert(denom -= 1.0 >= 0.0);
	  if (!disagrees == !(index_patch->child_disagrees)) {
	    correction += 1;
	  }
	}
	if (disagrees) {
	  action_prob *= ((double)(pov_summary->revert_count
				   - correction)
			  + revision_assignment_header->psi_alpha) / denom;
	} else {
	  action_prob *= ((double)(pov_summary->norevert_count
				   - correction)
			  + revision_assignment_header->psi_beta) / denom;
	}
      } else {
	// topic reference
	double denom = (double)(topic_summary->revert_topic_count
				+ topic_summary->norevert_topic_count
				+ revision_assignment_header->gamma_alpha
				+ revision_assignment_header->gamma_beta);
	int correction = 0;
	if (index_patch != NULL
	    && index_patch->topic == child->topic
	    && index_patch->parent_pov == index_patch->pov
	    && index_patch->parent_topic == index_patch->topic) {
	  assert(denom -= 1.0 >= 0.0);
	  if (!disagrees == !(index_patch->disagrees)) {
	    correction += 1;
	  }
	}
	if (index_patch != NULL
	    && index_patch->child_topic == child->topic
	    && index_patch->pov == index_patch->child_pov
	    && index_patch->topic == index_patch->child_topic) {
	  assert(denom -= 1.0 >= 0.0);
	  if (!disagrees == !(index_patch->child_disagrees)) {
	    correction += 1;
	  }
	}
	if (disagrees) {
	  action_prob *= ((double)(topic_summary->revert_topic_count
				   - correction)
			  + revision_assignment_header->gamma_alpha) / denom;
	} else {
	  action_prob *= ((double)(topic_summary->norevert_topic_count
				   - correction)
			  + revision_assignment_header->gamma_beta) / denom;
	}
      }
    } else {
      // general reference
      double denom = (double)(topic_summary->revert_general_count
			      + topic_summary->norevert_general_count)
	+ revision_assignment_header->gamma_alpha
	+ revision_assignment_header->gamma_beta;
      int correction = 0;
      if (index_patch != NULL
	  && index_patch->parent_topic >= 0
	  && index_patch->topic == child->topic
	  && index_patch->parent_topic != index_patch->topic) {
	assert(denom -= 1.0 >= 0.0);
	if (!disagrees == !(index_patch->disagrees)) {
	  correction += 1;
	}
      }
      if (index_patch != NULL
	  && index_patch->topic >= 0
	  && index_patch->child_topic == child->topic
	  && index_patch->topic != index_patch->child_topic) {
	assert(denom -= 1.0 >= 0.0);
	if (!disagrees == !(index_patch->child_disagrees)) {
	  correction = 1;
	}
      }
      if (disagrees) {
	action_prob *= ((double)(topic_summary->revert_general_count
				 - correction)
			+ revision_assignment_header->gamma_alpha) / denom;
      } else {
	action_prob *= ((double)(topic_summary->norevert_general_count
				 - correction)
			+ revision_assignment_header->gamma_beta) / denom;
      }
    }
  } // Else, a constant not dependant on POV or topic
  assert(action_prob > 0.0);
  return action_prob;
}

double revision_probability(const struct mmap_info* mmap_info, int64_t revision_id,
			    int32_t topic, int32_t pov,
			    int include_child,
			    int include_user_topic_pov,
			    const struct index_patch* index_patch) {
  const struct revision* revision = get_revision(mmap_info, revision_id);
  struct revision_assignment current_assignment;
  current_assignment.topic = topic;
  current_assignment.pov = pov;
  double ret = 1.0;
  // Revert probability
  if (revision->parent >= 0) {
    ret *= reference_probability(mmap_info,
				 get_revision_assignment(mmap_info, revision->parent),
				 &current_assignment, revision->disagrees,
				 index_patch);
  }
  if (revision->child >= 0 && include_child) {
    ret *= reference_probability(mmap_info, &current_assignment,
				 get_revision_assignment(mmap_info, revision->child),
				 get_revision(mmap_info, revision->child)->disagrees,
				 index_patch);
  }

  // Topic, pov probability
  struct revision_assignment_header* revision_assignment_header
    = (struct revision_assignment_header*)mmap_info->revision_assignment_mmap;
  if (include_user_topic_pov) {
    int pov_correction = 0;
    if (index_patch
	&& index_patch->user == revision->user
	&& pov == index_patch->pov
	&& topic == index_patch->topic) {
      pov_correction = 1;
    }

    double* user_pov_dist;
    get_user_topics(mmap_info, revision->user, &user_pov_dist);
    // Alpha is already added to this array. The normalizing constant is
    // independant of topic/pov, so we exclude it here.
    ret *= (user_pov_dist[topic * revision_assignment_header->pov_per_topic + pov]
	    - pov_correction);
  }

  // Page probability
  struct topic_summary_header* topic_summary_header
    = (struct topic_summary_header*)mmap_info->topic_index_mmap;
  int64_t* page_dist;
  struct topic_summary* topic_summary;
  get_topic_summary(mmap_info, topic, &topic_summary, NULL, &page_dist);
  int64_t topic_page_revisions = page_dist[revision->article];
  int64_t topic_revisions = topic_summary->total_revisions;
  if (index_patch && index_patch->topic == topic) {
    topic_revisions -= 1;
    if (index_patch->page == revision->article) {
      topic_page_revisions -= 1;
    }
  }
  ret *= ((double)(topic_page_revisions) + revision_assignment_header->beta)
    / ((double)(topic_revisions)
       + revision_assignment_header->beta * topic_summary_header->num_pages);
  assert(ret > 0.0);
  return ret;
}

void fill_index_patch(const struct mmap_info* mmap_info,
		      int64_t revision_id,
		      struct index_patch* index_patch) {
  const struct revision* revision = get_revision(mmap_info, revision_id);
  index_patch->user = revision->user;
  index_patch->page = revision->article;
  const struct revision_assignment* revision_assignment
    = get_revision_assignment(mmap_info, revision_id);
  index_patch->topic = revision_assignment->topic;
  index_patch->pov = revision_assignment->pov;
  index_patch->disagrees = revision->disagrees;
  if (revision->parent >= 0) {
    const struct revision_assignment* parent_assignment
      = get_revision_assignment(mmap_info, revision->parent);
    index_patch->parent_topic = parent_assignment->topic;
    index_patch->parent_pov = parent_assignment->pov;
  } else {
    index_patch->parent_topic = -1;
    index_patch->parent_pov = -1;
  }
  if (revision->child >= 0) {
    const struct revision_assignment* child_assignment
      = get_revision_assignment(mmap_info, revision->child);
    index_patch->child_topic = child_assignment->topic;
    index_patch->child_pov = child_assignment->pov;
    index_patch->child_disagrees = get_revision(mmap_info,
						revision->child)->disagrees;
  } else {
    index_patch->child_topic = -1;
    index_patch->child_pov = -1;
    index_patch->child_disagrees = -1;
  }
}

double users_pages_probability_modn(const struct mmap_info* mmap_info, int sample, int modn) {
  struct revision_assignment_header* revision_assignment_header
    = (struct revision_assignment_header*)mmap_info->revision_assignment_mmap;
  struct user_topic_header* user_topic_header = (struct user_topic_header*)mmap_info->user_topic_mmap;
  double log_likelihood = 0.0;
  int64_t user_edits;
  double* user_topic_pov_dist;
  int topic_pov_count
    = revision_assignment_header->num_topics * revision_assignment_header->pov_per_topic;
  for (int64_t user_num = sample; user_num < user_topic_header->num_users; user_num += modn) {
    get_user(mmap_info, user_num, &user_edits, NULL);
    get_user_topics(mmap_info, user_num, &user_topic_pov_dist);
    if (user_edits <= 0) {
      // This user does not actually exist
      continue;
    }
    log_likelihood -= gsl_sf_lngamma(user_edits + revision_assignment_header->alpha * topic_pov_count);
    for (int topic = 0; topic < revision_assignment_header->num_topics; ++topic) {
      for (int pov = 0; pov < revision_assignment_header->pov_per_topic; ++pov) {
	log_likelihood += gsl_sf_lngamma(user_topic_pov_dist[topic * revision_assignment_header->pov_per_topic
							     + pov]);
      }
    }
  }
  struct topic_summary_header* topic_summary_header
    = (struct topic_summary_header*)mmap_info->topic_index_mmap;
  int64_t* page_dist;
  for (int topic = 0; topic < revision_assignment_header->num_topics; ++topic) {
    get_topic_summary(mmap_info, topic, NULL, NULL, &page_dist);
    for (int64_t page = sample; page < topic_summary_header->num_pages; page += modn) {
      log_likelihood += gsl_sf_lngamma(page_dist[page] + revision_assignment_header->beta);
    }
  }
  return log_likelihood;
}

double log_likelihood(const struct mmap_info* mmap_info) {
  return log_likelihood_gamma(mmap_info, 1);
}

double log_likelihood_gamma(const struct mmap_info* mmap_info, int include_users_pages) {
  struct user_topic_header* user_topic_header = (struct user_topic_header*)mmap_info->user_topic_mmap;
  struct revision_assignment_header* revision_assignment_header
    = (struct revision_assignment_header*)mmap_info->revision_assignment_mmap;
  int topic_pov_count
    = revision_assignment_header->num_topics * revision_assignment_header->pov_per_topic;
  double log_likelihood = 0.0;
  log_likelihood += user_topic_header->num_users
    * gsl_sf_lngamma(revision_assignment_header->alpha * topic_pov_count);
  log_likelihood -= user_topic_header->num_users * topic_pov_count
    * gsl_sf_lngamma(revision_assignment_header->alpha);
  if (include_users_pages) {
    log_likelihood += users_pages_probability_modn(mmap_info, 0, 1);
  }

  struct topic_summary_header* topic_summary_header
    = (struct topic_summary_header*)mmap_info->topic_index_mmap;
  struct topic_summary* topic_summary;
  struct pov_summary* pov_dist;
  struct pov_summary* pov_summary;
  log_likelihood += revision_assignment_header->num_topics
    * gsl_sf_lngamma(revision_assignment_header->beta * topic_summary_header->num_pages);
  log_likelihood -= revision_assignment_header->num_topics * topic_summary_header->num_pages
    * gsl_sf_lngamma(revision_assignment_header->beta);
  for (int topic = 0; topic < revision_assignment_header->num_topics; ++topic) {
    get_topic_summary(mmap_info, topic, &topic_summary, &pov_dist, NULL);
    log_likelihood -= gsl_sf_lngamma(topic_summary->total_revisions + revision_assignment_header->beta
				     * topic_summary_header->num_pages);
    // General reverts
    log_likelihood += gsl_sf_lngamma(topic_summary->revert_general_count
				     + revision_assignment_header->gamma_alpha);
    log_likelihood += gsl_sf_lngamma(topic_summary->norevert_general_count
				     + revision_assignment_header->gamma_beta);
    log_likelihood -= gsl_sf_lngamma(topic_summary->revert_general_count
				     + topic_summary->norevert_general_count
				     + revision_assignment_header->gamma_alpha
				     + revision_assignment_header->gamma_beta);
    // Topic reverts
    log_likelihood += gsl_sf_lngamma(topic_summary->revert_topic_count
				     + revision_assignment_header->gamma_alpha);
    log_likelihood += gsl_sf_lngamma(topic_summary->norevert_topic_count
				     + revision_assignment_header->gamma_beta);
    log_likelihood -= gsl_sf_lngamma(topic_summary->revert_topic_count
				     + topic_summary->norevert_topic_count
				     + revision_assignment_header->gamma_alpha
				     + revision_assignment_header->gamma_beta);

    // POV reverts
    for (int pov = 0; pov < revision_assignment_header->pov_per_topic; ++pov) {
      for (int ant_pov = 0; ant_pov < revision_assignment_header->pov_per_topic - 1; ++ant_pov) {
	pov_summary = pov_dist + pov * (revision_assignment_header->pov_per_topic - 1) + ant_pov;
	log_likelihood += gsl_sf_lngamma(pov_summary->revert_count
					 + revision_assignment_header->psi_alpha);
	log_likelihood += gsl_sf_lngamma(pov_summary->norevert_count
					 + revision_assignment_header->psi_beta);
	log_likelihood -= gsl_sf_lngamma(pov_summary->revert_count
					 + pov_summary->norevert_count
					 + revision_assignment_header->psi_alpha
					 + revision_assignment_header->psi_beta);
      }
    }
  }
  log_likelihood += 2 * revision_assignment_header->num_topics
    * gsl_sf_lngamma(revision_assignment_header->gamma_alpha + revision_assignment_header->gamma_beta);
  log_likelihood -= 2 * revision_assignment_header->num_topics
    * gsl_sf_lngamma(revision_assignment_header->gamma_alpha);
  log_likelihood -= 2 * revision_assignment_header->num_topics
    * gsl_sf_lngamma(revision_assignment_header->gamma_beta);

  log_likelihood += revision_assignment_header->pov_per_topic
    * (revision_assignment_header->pov_per_topic - 1)
    * revision_assignment_header->num_topics
    * gsl_sf_lngamma(revision_assignment_header->psi_alpha + revision_assignment_header->psi_beta);
  log_likelihood -= revision_assignment_header->pov_per_topic
    * (revision_assignment_header->pov_per_topic - 1)
    * revision_assignment_header->num_topics
    * gsl_sf_lngamma(revision_assignment_header->psi_alpha);
  log_likelihood -= revision_assignment_header->pov_per_topic
    * (revision_assignment_header->pov_per_topic - 1)
    * revision_assignment_header->num_topics
    * gsl_sf_lngamma(revision_assignment_header->psi_beta);

  return log_likelihood;
}


/* specfunc/zeta.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2004 Gerard Jungman
 *
 * 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.
 */

/* Author:  G. Jungman */

/* This file was taken from the GNU Scientific Library. Some modifications
 * were done in order to make it independent from the rest of GSL
 */

/*
#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sf_elementary.h>
#include <gsl/gsl_sf_exp.h>
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_sf_pow_int.h>
#include <gsl/gsl_sf_zeta.h>

#include "error.h"

#include "chebyshev.h"
#include "cheb_eval.c"
*/

#include <math.h>
#include <stdio.h>
#include "error.h"

/*-*-*-*-*-*-*-*-*-*- From gsl_machine.h -*-*-*-*-*-*-*-*-*-*-*-*-*/

#define GSL_LOG_DBL_MIN   (-7.0839641853226408e+02)
#define GSL_LOG_DBL_MAX    7.0978271289338397e+02
#define GSL_DBL_EPSILON        2.2204460492503131e-16

/*-*-*-*-*-*-*-*-*-* From gsl_sf_result.h *-*-*-*-*-*-*-*-*-*-*-*/

struct gsl_sf_result_struct {
  double val;
  double err;
};
typedef struct gsl_sf_result_struct gsl_sf_result;

/*-*-*-*-*-*-*-*-*-*-*-* Private Section *-*-*-*-*-*-*-*-*-*-*-*/

/* coefficients for Maclaurin summation in hzeta()
 * B_{2j}/(2j)!
 */
static double hzeta_c[15] = {
  1.00000000000000000000000000000,
  0.083333333333333333333333333333,
 -0.00138888888888888888888888888889,
  0.000033068783068783068783068783069,
 -8.2671957671957671957671957672e-07,
  2.0876756987868098979210090321e-08,
 -5.2841901386874931848476822022e-10,
  1.3382536530684678832826980975e-11,
 -3.3896802963225828668301953912e-13,
  8.5860620562778445641359054504e-15,
 -2.1748686985580618730415164239e-16,
  5.5090028283602295152026526089e-18,
 -1.3954464685812523340707686264e-19,
  3.5347070396294674716932299778e-21,
 -8.9535174270375468504026113181e-23
};

/*-*-*-*-*-*-*-*-*-*-*-* Functions with Error Codes *-*-*-*-*-*-*-*-*-*-*-*/

static int gsl_sf_hzeta_e(const double s, const double q, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(s <= 1.0 || q <= 0.0) {
	PLFIT_ERROR("s must be larger than 1.0 and q must be larger than zero", PLFIT_EINVAL);
  }
  else {
    const double max_bits = 54.0;
    const double ln_term0 = -s * log(q);

    if(ln_term0 < GSL_LOG_DBL_MIN + 1.0) {
	  PLFIT_ERROR("underflow", PLFIT_UNDRFLOW);
    }
    else if(ln_term0 > GSL_LOG_DBL_MAX - 1.0) {
	  PLFIT_ERROR("overflow", PLFIT_OVERFLOW);
    }
    else if((s > max_bits && q < 1.0) || (s > 0.5*max_bits && q < 0.25)) {
      result->val = pow(q, -s);
      result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
      return PLFIT_SUCCESS;
    }
    else if(s > 0.5*max_bits && q < 1.0) {
      const double p1 = pow(q, -s);
      const double p2 = pow(q/(1.0+q), s);
      const double p3 = pow(q/(2.0+q), s);
      result->val = p1 * (1.0 + p2 + p3);
      result->err = GSL_DBL_EPSILON * (0.5*s + 2.0) * fabs(result->val);
      return PLFIT_SUCCESS;
    }
    else {
      /* Euler-Maclaurin summation formula
       * [Moshier, p. 400, with several typo corrections]
       */
      const int jmax = 12;
      const int kmax = 10;
      int j, k;
      const double pmax  = pow(kmax + q, -s);
      double scp = s;
      double pcp = pmax / (kmax + q);
      double ans = pmax*((kmax+q)/(s-1.0) + 0.5);

      for(k=0; k<kmax; k++) {
        ans += pow(k + q, -s);
      }

      for(j=0; j<=jmax; j++) {
        double delta = hzeta_c[j+1] * scp * pcp;
        ans += delta;
        if(fabs(delta/ans) < 0.5*GSL_DBL_EPSILON) break;
        scp *= (s+2*j+1)*(s+2*j+2);
        pcp /= (kmax + q)*(kmax + q);
      }

      result->val = ans;
      result->err = 2.0 * (jmax + 1.0) * GSL_DBL_EPSILON * fabs(ans);
      return PLFIT_SUCCESS;
    }
  }
}

/*-*-*-*-*-*-*-*-*-* Functions w/ Natural Prototypes *-*-*-*-*-*-*-*-*-*-*/

double gsl_sf_hzeta(const double s, const double a)
{
  gsl_sf_result result;
  gsl_sf_hzeta_e(s, a, &result);
  return result.val;
}


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <pthread.h>

#include <gsl/gsl_sf.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_roots.h>

#include "LSAbundance.h"

static int  verbose  = FALSE;

static int  nLines   = 9;

static char *usage[] = {"LSAbundance - \n",
                        "Required parameters:\n",
			"   -a    abundance data\n",
			"   -in   filename      parameter file  \n",
                        "Optional:\n",
			"   -b    integer       length of sample file to ignore\n",
			"   -s    integer       sampling frequency\n",
			"   -seed long          seed random number generator\n",
			"   -v                  verbose\n"};

void updateExpectations(double* adExpect, int nMax, double dMDash, double dV, double dNu, int nS, t_Data *ptData)
{
  int i = 0;

  for(i = 1; i <= nMax; i++){
    int nA = i;
    double dLog = 0.0, dP = 0.0;

    if(nA < MAX_QUAD){
      dLog = logLikelihoodQuad(nA, dMDash, dV, dNu);
    }
    else{
      dLog = logLikelihoodRampal(nA, dMDash, dV, dNu);
    }

    dP = exp(dLog);

    adExpect[i - 1]+= dP*nS;
  }
}

int main(int argc, char* argv[]){
  t_Params    tParams;
  t_LSParams* atLSParams;
  int         i = 0, nSamples = 0, nMax = 0;
  t_Data      tData;
  double*     adExpect = NULL;

  gsl_set_error_handler_off();

  getCommandLineParams(&tParams, argc, argv);

  /*allocate memory for samples*/
  atLSParams = (t_LSParams *) malloc(MAX_SAMPLES*sizeof(t_LSParams));
  if(!atLSParams)
    goto memoryError;

  /*read in Monte-Carlo samples*/
  readSamples(&tParams, atLSParams, &nSamples);

  readAbundanceData(tParams.szAbundFile, &tData);

  nMax = tData.aanAbund[tData.nNA - 1][0];
  nMax = floor(pow(2.0,ceil(log((double) nMax)/log(2.0)) + 2.0) + 1.0e-7);

  adExpect = (double *) malloc(sizeof(double)*nMax);

  for(i = 0; i < nMax; i++){
    adExpect[i] = 0.0;
  }

  for(i = 0; i < nSamples; i++){
    updateExpectations(adExpect, nMax,
		       atLSParams[i].dMDash,
		       atLSParams[i].dV,
		       atLSParams[i].dNu,
		       atLSParams[i].nS,
		       &tData);
  }

  for(i = 1; i <= nMax; i++){
    printf("%d %f\n",i,adExpect[i - 1]/((double) nSamples));
  }

  free(adExpect);
  exit(EXIT_SUCCESS);

 memoryError:
  fprintf(stderr, "Failed to allocate memory in main aborting ...\n");
  fflush(stderr);
  exit(EXIT_FAILURE);
}

int intCompare(const void *pvA, const void *pvB)
{
  int* pnA = (int *) pvA, *pnB = (int *) pvB;

  if(*pnA < *pnB)
    return +1;
  else if(*pnA == *pnB){
    return 0;
  }
  else{
    return -1;
  }
}

int doubleCompare(const void *pvA, const void *pvB)
{
  double* pnA = (double *) pvA, *pnB = (double *) pvB;

  if(*pnA < *pnB)
    return +1;
  else if(*pnA == *pnB){
    return 0;
  }
  else{
    return -1;
  }
}

void writeUsage(FILE* ofp)
{
  int i = 0;
  char *line;

  for(i = 0; i < nLines; i++){
    line = usage[i];
    fputs(line,ofp);
  }
}

char *extractParameter(int argc, char **argv, char *param,int when)
{
  int i = 0;

  while((i < argc) && (strcmp(param,argv[i]))){
    i++;
  }

  if(i < argc - 1){
    return(argv[i + 1]);
  }

  if((i == argc - 1) && (when == OPTION)){
    return "";
  }

  if(when == ALWAYS){
    fprintf(stdout,"Can't find asked option %s\n",param);
  }

  return (char *) NULL;
}

void getCommandLineParams(t_Params *ptParams,int argc,char *argv[])
{
  char *szTemp = NULL;
  char *cError = NULL;

  /*get parameter file name*/
  ptParams->szInputFile  = extractParameter(argc,argv, INPUT_FILE,ALWAYS);
  if(ptParams->szInputFile == NULL)
    goto error;

 /*get abundance file name*/
  ptParams->szAbundFile  = extractParameter(argc,argv, ABUND_FILE,ALWAYS);
  if(ptParams->szAbundFile == NULL)
    goto error;

  /*get long seed*/
  szTemp  = extractParameter(argc,argv,SEED,OPTION);
  if(szTemp != NULL){
    ptParams->lSeed = strtol(szTemp,&cError,10);
    if(*cError != '\0'){
      goto error;
    }
  }
  else{
    ptParams->lSeed = 0;
  }

  /*get burn*/
  szTemp  = extractParameter(argc, argv, BURN, OPTION);
  if(szTemp != NULL){
    ptParams->nBurn = strtol(szTemp,&cError,10);
    if(*cError != '\0'){
      goto error;
    }
  }
  else{
    ptParams->nBurn = DEF_BURN;
  }

 /*get long seed*/
  szTemp  = extractParameter(argc, argv, SAMPLE, OPTION);
  if(szTemp != NULL){
    ptParams->nSample = strtol(szTemp,&cError,10);
    if(*cError != '\0'){
      goto error;
    }
  }
  else{
    ptParams->nSample = DEF_SAMPLE;
  }

  /*verbosity*/
  szTemp = extractParameter(argc, argv, VERBOSE, OPTION);
  if(szTemp != NULL){
    verbose = TRUE;
  }

  return;

 error:
  writeUsage(stdout);
  exit(EXIT_FAILURE);
}

void readSamples(t_Params *ptParams, t_LSParams *atLSParams, int *pnSamples)
{
  int  nSamples = 0;
  char *szInputFile = ptParams->szInputFile;
  char szLine[MAX_LINE_LENGTH];
  FILE *ifp = NULL;

  ifp = fopen(szInputFile, "r");

  if(ifp){
    while(fgets(szLine, MAX_LINE_LENGTH, ifp)){
      char *szTok = NULL, *szBrk = NULL, *pcError = NULL;
      int  nTime  = 0;

      /*remove trailing new line*/
      szBrk = strpbrk(szLine, "\n"); (*szBrk) = '\0';

      szTok = strtok(szLine, DELIM);

      nTime = strtol(szTok, &pcError, 10);
      if(*pcError != '\0') goto fileFormatError;

      if(nTime > ptParams->nBurn && nTime % ptParams->nSample == 0){

	szTok = strtok(NULL,DELIM);
	atLSParams[nSamples].dMDash = strtod(szTok, &pcError);
	if(*pcError != '\0') goto fileFormatError;

	szTok = strtok(NULL,DELIM);
	atLSParams[nSamples].dV = strtod(szTok, &pcError);
	if(*pcError != '\0') goto fileFormatError;

	szTok = strtok(NULL,DELIM);
	atLSParams[nSamples].dNu = strtod(szTok, &pcError);
	if(*pcError != '\0') goto fileFormatError;

	szTok = strtok(NULL,DELIM);
	atLSParams[nSamples].nS = strtol(szTok, &pcError, 10);
	if(*pcError != '\0') goto fileFormatError;

	nSamples++;
      }
    }

    fclose(ifp);
  }
  else{
    fprintf(stderr, "Failed to open file %s aborting\n", szInputFile);
    fflush(stderr);
    exit(EXIT_FAILURE);
  }

  (*pnSamples) = nSamples;
  return;

 fileFormatError:
  fprintf(stderr, "Incorrectly formatted input file aborting\n");
  fflush(stderr);
  exit(EXIT_FAILURE);
}

double f1(double x, void *pvParams)
{
  t_LSParams *ptLSParams = (t_LSParams *) pvParams;
  double dMDash = ptLSParams->dMDash, dV = ptLSParams->dV, dNu = ptLSParams->dNu;
  int n = ptLSParams->n;
  double t = ((x - dMDash)*(x - dMDash))/dV;
  double dExp  = x*((double) n) - exp(x);
  double dF  = pow(1.0 + t/dNu, -0.5*(dNu + 1.0));

  return exp(dExp)*dF;
}

double f1Log(double x, void *pvParams)
{
  t_LNParams *ptLNParams = (t_LNParams *) pvParams;
  double dMDash = ptLNParams->dMDash, dV = ptLNParams->dV;
  int n = ptLNParams->n;
  double dTemp = (x - dMDash);
  double dExp  = x*((double) n) - exp(x) - 0.5*((dTemp*dTemp)/dV);
  double dRet  = exp(dExp);

  return dRet;
}

double derivExponent(double x, void *pvParams)
{
  t_LNParams *ptLNParams = (t_LNParams *) pvParams;
  double dMDash = ptLNParams->dMDash, dV = ptLNParams->dV, n = ptLNParams->n;
  double dTemp = (x - dMDash)/dV, dRet = 0.0;

  dRet = ((double) n) - exp(x) - dTemp;

  return dRet;
}

double logStirlingsGamma(double dZ)
{
  return 0.5*log(2.0*M_PI) + (dZ - 0.5)*log(dZ) - dZ;
}

double logLikelihoodQuad(int n, double dMDash, double dV, double dNu)
{
  gsl_integration_workspace *ptGSLWS =
    gsl_integration_workspace_alloc(1000);
  double dLogFac1   = 0.0, dLogFacN  = 0.0;
  double dN = (double) n, dResult = 0.0, dError = 0.0, dPrecision = 0.0;
  gsl_function tGSLF;
  t_LSParams tLSParams;
  double dEst = dMDash + ((double)n)*dV, dA = 0.0, dB = 0.0;

  tLSParams.n = n; tLSParams.dMDash = dMDash; tLSParams.dV = dV; tLSParams.dNu = dNu;

  tGSLF.function = &f1;
  tGSLF.params   = (void *) &tLSParams;

  if(dNu < MAX_MU_GAMMA){
    dLogFac1 = gsl_sf_lngamma(0.5*(dNu + 1.0)) - gsl_sf_lngamma(0.5*dNu) - 0.5*log(M_PI*dNu);
  }
  else{
    dLogFac1 = 0.5*dNu*(log(0.5*(dNu + 1.0)) - log(0.5*dNu)) -0.5*log(2.0*M_PI) - 0.5;
  }

  if(n < 50){
    dLogFacN = gsl_sf_fact(n);
    dLogFacN = log(dLogFacN);
  }
  else if(n < 100){
    dLogFacN = gsl_sf_lngamma(dN + 1.0);
  }
  else{
    dLogFacN = logStirlingsGamma(dN + 1.0);
  }

  dA = -100.0; dB = 100.0;

  if(n < 10){
    dPrecision = HI_PRECISION;
  }
  else{
    dPrecision = LO_PRECISION;
  }

  gsl_integration_qag(&tGSLF, dA, dB, dPrecision, 0.0, 1000, GSL_INTEG_GAUSS61, ptGSLWS, &dResult, &dError);

  //printf("%f %f\n", dResult, dError);

  gsl_integration_workspace_free(ptGSLWS);

  return log(dResult) - dLogFacN + dLogFac1 - 0.5*log(dV);
}

int solveF(double x_lo, double x_hi, double (*f)(double, void*),
	   void* params, double tol, double *xsolve)
{
  int status, iter = 0, max_iter = 100;
  const gsl_root_fsolver_type *T;
  gsl_root_fsolver *s;
  double r = 0;
  gsl_function F;
  t_LNParams *ptLNParams = (t_LNParams *) params;
  F.function = f;
  F.params = params;

  //printf("%f %f %d %f %f\n",ptLNParams->dMDash, ptLNParams->dV, ptLNParams->n, x_lo, x_hi);
  T = gsl_root_fsolver_brent;
  s = gsl_root_fsolver_alloc (T);
  gsl_root_fsolver_set (s, &F, x_lo, x_hi);

  do{
    iter++;
    status = gsl_root_fsolver_iterate (s);
    r = gsl_root_fsolver_root (s);
    x_lo = gsl_root_fsolver_x_lower (s);
    x_hi = gsl_root_fsolver_x_upper (s);

    status = gsl_root_test_interval (x_lo, x_hi, 0, tol);
  }
  while (status == GSL_CONTINUE && iter < max_iter);

  (*xsolve) = gsl_root_fsolver_root (s);
  gsl_root_fsolver_free (s);

  return status;
}

double logLikelihoodLNQuad(int n, double dMDash, double dV)
{
  gsl_integration_workspace *ptGSLWS =
    gsl_integration_workspace_alloc(1000);
  double dLogFac1   = 0.0, dLogFacN  = 0.0;
  double dResult = 0.0, dError = 0.0, dPrecision = 0.0;
  gsl_function tGSLF;
  t_LNParams tLNParams;
  double dEst = dMDash + ((double)n)*dV, dA = 0.0, dB = 0.0;

  tLNParams.n = n; tLNParams.dMDash = dMDash; tLNParams.dV = dV;

  tGSLF.function = &f1Log;
  tGSLF.params   = (void *) &tLNParams;

  dLogFac1 = log(2.0*M_PI*dV);

  if(n < 50){
    dLogFacN = gsl_sf_fact(n);
    dLogFacN = log(dLogFacN);
  }
  else{
    dLogFacN = gsl_sf_lngamma(((double) n) + 1.0);
  }

  if(dEst > dV){
    double dMax = 0.0;
    double dUpper = (((double) n) + (dMDash/dV) - 1.0)/(1.0 + 1.0/dV);
    double dVar   = 0.0;

    if(fabs(dUpper) > 1.0e-7){
      solveF(0.0, dUpper, derivExponent, (void *) &tLNParams, 1.0e-5, &dMax);
    }

    dVar = sqrt(1.0/((1.0/dV) + exp(dMax)));

    dA = dMax - V_MULT*dVar; dB = dMax + V_MULT*dVar;
  }
  else{
    double dMax = 0.0;
    double dLower = dEst - dV;
    double dUpper = (((double) n) + (dMDash/dV) - 1.0)/(1.0 + 1.0/dV);
    double dVar   = 0.0;

    if(fabs(dUpper - dLower) > 1.0e-7){
      solveF(dLower, dUpper, derivExponent, (void *) &tLNParams, 1.0e-5, &dMax);
    }
    else{
      dMax = 0.5*(dLower + dUpper);
    }
    dVar = sqrt(1.0/((1.0/dV) + exp(dMax)));

    dA = dMax - V_MULT*dVar; dB = dMax + V_MULT*dVar;
  }

  if(n < 10){
    dPrecision = HI_PRECISION;
  }
  else{
    dPrecision = LO_PRECISION;
  }

  gsl_integration_qag(&tGSLF, dA, dB, dPrecision, 0.0, 1000, GSL_INTEG_GAUSS61, ptGSLWS, &dResult, &dError);

  gsl_integration_workspace_free(ptGSLWS);

  return log(dResult) - dLogFacN -0.5*dLogFac1;
}

double logLikelihoodLNRampal(int n, double dMDash, double dV)
{
  double dN = (double) n;
  double dLogLik = 0.0, dTemp = gsl_pow_int(log(dN) - dMDash,2), dTemp3 = gsl_pow_int(log(dN) - dMDash,3);

  dLogLik = -0.5*log(2.0*M_PI*dV) - log(dN) - (dTemp/(2.0*dV));

  dLogLik += log(1.0 + 1.0/(2.0*dN*dV)*(dTemp/dV + log(dN) - dMDash - 1.0)
		 + 1.0/(6.0*dN*dN*dV*dV*dV)*(3.0*dV*dV - (3.0*dV - 2.0*dV*dV)*(dMDash - log(dN))
		 - 3.0*dV*dTemp + dTemp3));

  return dLogLik;
}

double logLikelihoodRampal(int n, double dMDash, double dV, double dNu)
{
  double dGamma = 0.5*(dNu + 1.0), dN = (double) n, dRN = 1.0/dN, dRSV = 1.0/(sqrt(dV)*sqrt(dNu));
  double dZ = (log(dN) - dMDash)*dRSV;
  double dDZDX = dRN*dRSV, dDZDX2 = -dRN*dRN*dRSV;
  double dF = (1.0 + dZ*dZ);
  double dA = 0.0, dB = 0.0, dTemp = 0.0;
  double dLogFac1 = 0.0;

  if(dNu < MAX_MU_GAMMA){
    dLogFac1 = gsl_sf_lngamma(0.5*(dNu + 1.0)) - gsl_sf_lngamma(0.5*dNu) - 0.5*log(M_PI*dNu);
  }
  else{
    dLogFac1 = 0.5*dNu*(log(0.5*(dNu + 1.0)) - log(0.5*dNu)) -0.5*log(2.0*M_PI) - 0.5;
  }

  dA = 4.0*dZ*dZ*dDZDX*dDZDX*dGamma*(dGamma + 1.0);
  dA /= dF*dF;

  dB = -2.0*dGamma*(dDZDX*dDZDX + dZ*dDZDX2);
  dB /= dF;

  dTemp = dRN + dA + dB;

  return -dGamma*log(dF) + log(dTemp) + dLogFac1 - 0.5*log(dV);
}

void readAbundanceData(const char *szFile, t_Data *ptData)
{
  int **aanAbund = NULL;
  int  i = 0, nNA = 0, nA = 0, nC = 0;
  int  nL = 0, nJ = 0;
  char szLine[MAX_LINE_LENGTH];
  FILE* ifp = NULL;

  ifp = fopen(szFile, "r");

  if(ifp){
    char* szTok   = NULL;
    char* pcError = NULL;

    fgets(szLine, MAX_LINE_LENGTH, ifp);

    szTok = strtok(szLine, DELIM2);

    nNA = strtol(szTok,&pcError,10);
    if(*pcError != '\0'){
      goto formatError;
    }

    aanAbund = (int **) malloc(nNA*sizeof(int*));

    for(i = 0; i < nNA; i++){
      aanAbund[i] = (int *) malloc(sizeof(int)*2);

      fgets(szLine, MAX_LINE_LENGTH, ifp);

      szTok = strtok(szLine, DELIM2);

      nA = strtol(szTok,&pcError,10);
      if(*pcError != '\0'){
	goto formatError;
      }

      szTok = strtok(NULL, DELIM2);

      nC = strtol(szTok,&pcError,10);
      if(*pcError != '\0'){
	goto formatError;
      }

      nL += nC;
      nJ += nC*nA;

      aanAbund[i][0]  = nA;
      aanAbund[i][1]  = nC;
    }
  }
  else{
    fprintf(stderr, "Failed to open abundance data file %s aborting\n", szFile);
    fflush(stderr);
    exit(EXIT_FAILURE);
  }

  ptData->nJ          = nJ;
  ptData->nL          = nL;
  ptData->aanAbund    = aanAbund;
  ptData->nNA         = nNA;
  return;

 formatError:
  fprintf(stderr, "Incorrectly formatted abundance data file\n");
  fflush(stderr);
  exit(EXIT_FAILURE);
}


/**
 * @file bblas_cutil.c
 *
 * @brief BBLAS testing utilities for float _Complex routines.
 *
 * BBLAS is a software package provided by Univ. of Manchester,
 * Univ. of Tennessee.
 *
 * @version 1.0.0
 * @author  Samuel  D. Relton
 * @author  Pedro   V. Lara
 * @author  Mawussi Zounon
 * @date    2016-02-20
 *
 * Contains routines used in the testing to modify randomly generated matrices
 * and compute the average error over an entire batch etc.
 *
 **/

#ifndef DOXYGEN_SHOULD_SKIP_THIS
/**
 * Code generation
 * @generated from bblas_zutil.c normal z -> c, Mon Jun  6 09:44:14 2016
 **/
#endif

#include "bblas_common.h"
#if defined(BBLAS_WITH_MKL)
#include <mkl_lapacke.h>
#else
#include <lapacke.h>
#endif

/** Include complex functions since using float complex precision **/
#define COMPLEX

/** Quick access to the matrix elements **/
#define A(i,j)  A[i + j*lda]

/**
 * Make a matrix symmetric/Hermitian.  Makes diagonal real.
 * Sets Aji = conj( Aij ) for j < i, that is, copy & conjugate
 * lower triangle to upper triangle.
 **/

void bblas_cmake_hermitian(int lda, int N, BBLAS_Complex32_t* A)
{
    int i, j;
    for( i=0; i < N; ++i ) {
        A(i,i) = creal( A(i,i) );
        for( j=0; j < i; ++j ) {
            A(j,i) = conj( A(i,j) );
        }
    }
}

#ifdef COMPLEX

/**
 * Make a matrix complex-symmetric
 * Does NOT make diagonal real.
 * Sets Aji = Aij for j < i, that is,
 * copy lower triangle to upper triangle.
 **/

void bblas_cmake_symmetric(int lda, int N, BBLAS_Complex32_t* A)
{
    int i, j;
    for( i=0; i < N; ++i ) {
        for( j=0; j < i; ++j ) {
            A(j,i) =  A(i,j);
        }
    }
}
#endif

/**
 * irandRange generates a random value (int)
 * in the range min_n and max_n
 **/

int irandRange(int min_n, int max_n)
{
    return rand() % (max_n - min_n + 1) + min_n;
}

/**
 * bblas_crandRange generates a random value (BBLAS_Complex32_t)
 * in the range [0,max_n]: TODO replace by generic
 */

BBLAS_Complex32_t bblas_crandRange( int max_n )
{
    return ( BBLAS_Complex32_t )rand()/( BBLAS_Complex32_t )( RAND_MAX/max_n );
}

/**
 * Computes statistics of the relative errors to summarise them for the user.
 **/

void bblas_cstatistic(bblas_ctest_t *test)
{

    enum BBLAS_ROUTINE routine  = test->routine;

    /*Compute avg(M), avg(N), avg(K) */
    if(test->batch_opts == BBLAS_VARIABLE)
    {
	if ((routine == BBLAS_GEMM) || (routine == BBLAS_SYMM) ||
	    (routine == BBLAS_HEMM) || (routine == BBLAS_TRMM) ||
	    (routine == BBLAS_TRSM))
	{
	    test->avgM = bblas_avgarrayI(test->M, test->batch_count);
	}

	if ((routine == BBLAS_GEMM) || (routine == BBLAS_SYRK)  ||
	    (routine == BBLAS_HERK) || (routine == BBLAS_SYR2K) ||
	    (routine == BBLAS_HER2K))
	{
	    test->avgK = bblas_avgarrayI(test->K, test->batch_count);
	}

	test->avgN = bblas_avgarrayI(test->N, test->batch_count);

    } else if (test->batch_opts == BBLAS_FIXED)
    {

	if ((routine == BBLAS_GEMM) || (routine == BBLAS_SYMM) ||
	    (routine == BBLAS_HEMM) || (routine == BBLAS_TRMM) ||
	    (routine == BBLAS_TRSM))
	{
	    test->avgM = test->M[0];
	}

	if ((routine == BBLAS_GEMM) || (routine == BBLAS_SYRK)  ||
	    (routine == BBLAS_HERK) || (routine == BBLAS_SYR2K) ||
	    (routine == BBLAS_HER2K))
	{
	    test->avgK = test->K[0];
	}
	test->avgN = test->N[0];

    } else
    {
	bblas_error("testing_cgemm_batch.c", "wrong batch_opts value");
    }

    /*Statistics on the error */
    switch(test->target)
    {
    case BBLAS_MKL:
      test->mkl_min_error = bblas_cminarrayD(test->mkl_error, test->batch_count);
      test->mkl_avg_error = bblas_cavgarrayD(test->mkl_error, test->batch_count);
      test->mkl_max_error = bblas_cmaxarrayD(test->mkl_error, test->batch_count);
      test->mkl_std_error = bblas_cstdarrayD(test->mkl_error, test->batch_count);
      break;

    case BBLAS_CUBLAS:
    case BBLAS_MAGMA:
      test->device_min_error = bblas_cminarrayD(test->device_error, test->batch_count);
      test->device_avg_error = bblas_cavgarrayD(test->device_error, test->batch_count);
      test->device_max_error = bblas_cmaxarrayD(test->device_error, test->batch_count);
      test->device_std_error = bblas_cstdarrayD(test->device_error, test->batch_count);
      break;

    case BBLAS_OTHER:
      test->other_min_error = bblas_cminarrayD(test->other_error, test->batch_count);
      test->other_avg_error = bblas_cavgarrayD(test->other_error, test->batch_count);
      test->other_max_error = bblas_cmaxarrayD(test->other_error, test->batch_count);
      test->other_std_error = bblas_cstdarrayD(test->other_error, test->batch_count);
      break;

    default:
      printf("In bblas_cstatistic(): Target no defined\n");
      exit(EXIT_FAILURE);
    }
}

/**
 * Print a matrix.
 **/
void bblas_cprintmatrix(BBLAS_Complex32_t *matrix, int row, int col)
{
    /*Local variables */
    int i, j;

    for (i=0; i < row; i++)
    {
	printf("\n\n");
	for (j=0; j < col; j++)
	{
#ifdef COMPLEX
	    printf("%1.2f + %1.2f\t", creal(matrix[i*col+j]), cimag(matrix[i*col+j]));
#else
	    printf("%1.2f",matrix[i*col+j]);
#endif
	}
    }
    printf("\n");
}

/**
 * Decide whether a batch is fixed or variable.
 **/

char* bblas_getoption(enum BBLAS_OPTS opts)
{
   /*Local variable */
    char funcname[] = "bblas_getoption";

    switch(opts)
    {
	case BBLAS_VARIABLE:
	    return "BATCH OPTION: VARIABLE";
	    break;

	case BBLAS_FIXED:
	    return "BATCH OPTION: FIXED";
	    break;

	default:
	    printf("ERROR in %s, undefined bblas routine name\n",funcname);
	    exit(EXIT_FAILURE);
    }

}

/**
 * Get the name of the current test routine.
 **/

char* bblas_getroutine(enum BBLAS_ROUTINE routine)
{
    /*Local variable */
    char funcname[] = "bblas_getroutine";

    switch(routine)
    {
	case BBLAS_GEMM:
	    return "CGEMM";
	    break;

	case BBLAS_HEMM:
	    return "CHEMM";
	    break;

	case BBLAS_HER2K:
	    return "CHER2K";
	    break;

	case BBLAS_HERK:
	    return "CHERK";
	    break;

	case BBLAS_SYMM:
	    return "CSYMM";
	    break;

	case BBLAS_SYR2K:
	    return "CSYR2K";
	    break;

	case BBLAS_SYRK:
	    return "CSYRK";
	    break;

	case BBLAS_TRMM:
	    return "CTRMM";
	    break;

	case BBLAS_TRSM:
	    return "CTRSM";
	    break;

	default:
	    printf("ERROR in %s, undefined bblas routine name\n",funcname);
	    exit(EXIT_FAILURE);
    }
}

/**
 * Computes the maximum value of an array of real floats.
 **/

float bblas_cmaxarrayD(float *myArray, int size)
{
    int iter;
    float maxValue = myArray[0];

    for (iter = 0; iter < size; ++iter)
    {
        if ( myArray[iter] > maxValue )
	{
            maxValue = myArray[iter];
        }
    }
    return maxValue;
}

/**
 * Computes the minimum value of an array of real floats.
 **/

float bblas_cminarrayD(float *myArray, int size)
{
    int iter;
    float minValue = myArray[0];

    for (iter = 0; iter < size; ++iter)
    {
        if ( myArray[iter] < minValue )
	{
            minValue = myArray[iter];
        }
    }
    return minValue;
}

/**
 * Computes the mean value of an array of real floats.
 **/

float bblas_cavgarrayD(float *myArray, int size)
{
    int iter;
    float avg = 0.;

    for (iter = 0; iter < size; ++iter)
    {
	avg += myArray[iter];
    }
    return avg/size;
}

/**
 * Computes the standard deviation of an array of real floats.
 **/

float bblas_cstdarrayD(float *myArray, int size)
{
    int iter;
    float avg, sd=0.;

    avg =  bblas_cavgarrayD(myArray, size);

    for (iter = 0; iter < size; ++iter)
    {
	sd += (myArray[iter] -avg)*(myArray[iter] -avg);
    }
    return sd/size;
}

/**
 * Computes the minimum value of an array of integers.
 **/

int bblas_minarrayI(int *myArray, int size)
{
    int iter;
    int minValue = myArray[0];

    for (iter = 0; iter < size; ++iter)
    {
        if ( myArray[iter] < minValue )
	{
            minValue = myArray[iter];
        }
    }
    return minValue;
}

/**
 * Computes the mean of an array of integers.
 **/

int bblas_avgarrayI(int *myArray, int size)
{
    int iter;
    int avg = 0;

    for (iter = 0; iter < size; ++iter)
    {
	avg += myArray[iter];
    }
    return avg/size;
}

/**
 * Transform BBLAS enum values for <tt>trans</tt>, <tt>uplo</tt> etc. to human-readable strings.
 **/

char* bblas_op2char(unsigned int op)
{
    char *opname = (char*)malloc(30*sizeof(char));
    switch(op)
    {
        case BblasNoTrans:
            strcpy(opname,"CblasNoTrans");
            break;

        case BblasTrans:
	    strcpy(opname,"CblasTrans");
            break;

        case BblasConjTrans:
            strcpy(opname,"CblasConjTrans");
            break;

        case BblasLower:
            strcpy(opname,"CblasLower");
            break;

        case BblasUpper:
	    strcpy(opname,"CblasUpper");
            break;

        case BblasNonUnit:
	    strcpy(opname,"CblasNonUnit");
            break;

        case BblasUnit:
	   strcpy(opname,"CblasUnit");
            break;

        case BblasLeft:
	   strcpy(opname,"CblasLeft");
            break;

        case BblasRight:
	    strcpy(opname,"CblasRight");
            break;

        default:
            return 0;
            exit(EXIT_FAILURE);
    }
    return opname;
}

/**
 * Get the amount of data needed.
 **/

int bblas_cnbdata(bblas_ctest_t *test)
{
    enum BBLAS_OPTS batch_opts        = test->batch_opts;
    int nb_data;
    char function_name[NAME_LENGTH]   ="bblas_getdatacount";

    switch(batch_opts)
    {
	case BBLAS_VARIABLE:
	    nb_data =  test->batch_count;
	    break;

	case BBLAS_FIXED:
	    nb_data = 1;
	    break;

	default:
	    bblas_fatal_error(function_name, "wrong batch_opts value");
    }

    return nb_data;
}

/**
 * Inject an error into the computation if this is set in the input file.
 **/

void bblas_cset_error(bblas_ctest_t *test)
{
    int   nb_data =  bblas_cnbdata(test);
    int   routine = test->routine;
    int   error_index = irandRange(0, nb_data);

    if (test->global_error){
	test->batch_count = -1;
	return;
    }

    if (test->batch_opts == BBLAS_FIXED)
    {
	error_index = 0;
    }

    if (test->set_error)
    {
	if ((routine == BBLAS_GEMM) || (routine == BBLAS_SYMM) ||
	    (routine == BBLAS_HEMM) || (routine == BBLAS_TRMM) ||
	    (routine == BBLAS_TRSM))
	{
	    test->M[error_index] = -1;
	}else if ((routine == BBLAS_SYRK) || (routine == BBLAS_HERK) ||
		  (routine == BBLAS_SYR2K)|| (routine == BBLAS_HER2K))
	{
	    test->K[error_index] = -1;
	}
    }
}

/**
 * Check whether a computation has passed or failed our accuracy test.
 * When new_accuracy=1 in the input file this uses an appropriate
 * forward/backward error bound,
 * otherwise this looks at the relative error.
 **/

void bblas_cpassed_failed(bblas_ctest_t  *test, float error, char *result, int info)
{
    float eps = LAPACKE_slamch_work('e');

	/* Use our new accuracy test based on forward/backward error analysis*/
    if (test->new_accuracy)
    {
	if( (error > 1) || (info))
	{
	    strcpy(result, "FAILED");

	}else
	{
	    strcpy(result, "PASSED");
	}
    }else
    {
		/* Use old accuracy test based on the relative error */
	if((error > eps*test->tolerance) || (info))
	{
	    strcpy(result, "FAILED");

	}else
	{
	    strcpy(result, "PASSED");
	}
    }
}

/**
 * Set the batch_count.
 **/

void bblas_cset_batch_count(bblas_ctest_t *test)
{

  test->batch_count = test->minbatch_count* (test->current_iter+1);
}

#undef COMPLEX


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

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

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

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

#ifndef OPTIMIZE_H_
#define OPTIMIZE_H_

#include <ANN.h>
#include "gicp.h"
#include <vector>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_multimin.h>
#include <gsl/gsl_multifit_nlin.h>

namespace dgc {
namespace gicp {
inline void print_gsl_matrix(gsl_matrix *mat, const char * name) {
    std::cout << name << "= [";
    for(unsigned int i = 0; i < mat->size1; i++) {
        for(unsigned int j = 0; j < mat->size2; j++) {
            std::cout << gsl_matrix_get(mat, i, j) << " ";
        }
        std::cout << ";" << std::endl;
    }
    std::cout << "]" << std::endl;
}

struct GICPOptData {
    GICPPointSet *p1;
    GICPPointSet *p2;
    ANNidx *nn_indecies; // nearest point indecies
    gicp_mat_t *M;      // mahalanobis matrices for each pair
    dgc_transform_t base_t;
    int num_matches;
    bool solve_rotation;
};

class GICPOptimizer {
public:
    GICPOptimizer();
    ~GICPOptimizer();

    int Iterations() { return iter; }
    const char* Status() { return gsl_strerror(status); }

    bool Optimize(dgc_transform_t t, GICPOptData &opt_data);
    bool OptimizeLM(dgc_transform_t t, GICPOptData &opt_data);

    void SetDebug(bool d) { debug = d; }

    void SetMaxIterations(int iter) { max_iter = iter; }

    void PlotError(dgc_transform_t t, GICPOptData &opt_data, const char* filename);

private:
    static double f(const gsl_vector * x, void * params);
    static void df(const gsl_vector * x, void * params, gsl_vector * g);
    static void fdf(const gsl_vector * x, void * params, double * f, gsl_vector * g);

    static void compute_dr(gsl_vector const* x, gsl_matrix const* gsl_temp_mat_r, gsl_vector *g);
    static double mat_inner_prod(gsl_matrix const* mat1, gsl_matrix const* mat2);
    static void apply_state(dgc_transform_t t, gsl_vector const* x);

    gsl_multimin_fdfminimizer *gsl_minimizer;
    gsl_vector *x;
    int max_iter;
    int iter;
    int status;
    bool debug;
    const static int N = 6;
    const gsl_multimin_fdfminimizer_type *T_min;
};

}
}

#endif


/* linalg/qr_band.c
 *
 * Copyright (C) 2020 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 <string.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>

/* Factorise a (p,q) banded M x N matrix A into
 *
 *   A = Q R
 *
 * where Q is orthogonal (M x M) and R is upper triangular (M x N).
 *
 * Example with M = 7, N = 6, (p,q) = (2,1)
 *
 * A = [ A11 A12  0   0   0   0  ]
 *     [ A21 A22 A23  0   0   0  ]
 *     [ A31 A32 A33 A34  0   0  ]
 *     [  0  A42 A43 A44 A45  0  ]
 *     [  0   0  A53 A54 A55 A56 ]
 *     [  0   0   0  A64 A65 A66 ]
 *     [  0   0   0   0  A75 A76 ]
 *
 * AB has dimensions N-by-(2p + q + 1)
 *
 * INPUT:                          OUTPUT:
 *
 * AB = [ *  *  *  A11 A21 A31 ]   AB = [  *   *   *  R11 V21 V31 ]
 *      [ *  * A12 A22 A32 A42 ]        [  *   *  R12 R22 V32 V42 ]
 *      [ *  0 A23 A33 A43 A53 ]        [  *  R13 R23 R33 V43 V53 ]
 *      [ 0  0 A34 A44 A54 A64 ]        [ R14 R24 R34 R44 V54 V64 ]
 *      [ 0  0 A45 A55 A65 A75 ]        [ R25 R35 R45 R55 V65 V75 ]
 *      [ 0  0 A56 A66 A76  *  ]        [ R36 R46 R56 R66 V76  *  ]
 *        -p-- -q- -1- ---p---            ---p--- -q- -1- ---p---
 *
 * The full matrix for Q can be obtained as the product
 *
 *       Q = Q_k .. Q_2 Q_1
 *
 * where k = MIN(M,N) and
 *
 *       Q_i = (I - tau_i * v_i * v_i')
 *
 * and where v_i is a Householder vector
 */

int
gsl_linalg_QR_band_decomp_L2 (const size_t M, const size_t p, const size_t q, gsl_matrix * AB, gsl_vector * tau)
{
  const size_t N = AB->size1;

  if (tau->size != N)
    {
      GSL_ERROR ("tau must have length N", GSL_EBADLEN);
    }
  else if (AB->size2 != 2*p + q + 1)
    {
      GSL_ERROR ("dimensions of AB are inconsistent with (p,q)", GSL_EBADLEN);
    }
  else
    {
      const size_t minMN = GSL_MIN(M, N);
      size_t j;

      /* set AB(:,1:p) to zero */
      if (p > 0)
        {
          gsl_matrix_view m = gsl_matrix_submatrix(AB, 0, 0, N, p);
          gsl_matrix_set_zero(&m.matrix);
        }

      for (j = 0; j < minMN; ++j)
        {
          /* Compute the Householder transformation to reduce the j-th
             column of the matrix to a multiple of the j-th unit vector */

          size_t k1 = GSL_MIN(p + 1, M - j);     /* number of non-zero elements of this column, including diagonal element */
          size_t k2 = GSL_MIN(p + q, N - j - 1); /* number of columns to update */
          gsl_vector_view c = gsl_matrix_subrow(AB, j, p + q, k1);
          double tau_j = gsl_linalg_householder_transform (&(c.vector));
          double * ptr = gsl_vector_ptr(&(c.vector), 0);

          gsl_vector_set (tau, j, tau_j);

          /* apply the transformation to the remaining columns */
          if (k2 > 0)
            {
              gsl_matrix_view m = gsl_matrix_submatrix (AB, j + 1, p + q - 1, k2, k1);
              gsl_vector_view work = gsl_vector_subvector(tau, j + 1, k2);
              double tmp = *ptr;

              m.matrix.tda -= 1; /* unskew matrix */

              /* we want to compute H*A(j:j+k1-1,j+1:j+k2), but due to our using row-major order, the
               * matrix m contains A(j:j+k1-1,j+1:j+k2)^T. So therefore we apply H from the right,
               *
               * [H*A]^T = A^T H^T = A^T H
               */

              *ptr = 1.0;
              gsl_linalg_householder_right(tau_j, &(c.vector), &(m.matrix), &(work.vector));
              *ptr = tmp;
            }
        }

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_QR_band_unpack_L2 (const size_t p, const size_t q, const gsl_matrix * QRB, const gsl_vector * tau,
                              gsl_matrix * Q, gsl_matrix * R)
{
  const size_t M = Q->size1;
  const size_t N = QRB->size1;

  if (Q->size2 != M)
    {
      GSL_ERROR ("Q matrix must be square", GSL_ENOTSQR);
    }
  else if (R->size1 != M || R->size2 != N)
    {
      GSL_ERROR ("R matrix must be M x N", GSL_ENOTSQR);
    }
  else if (tau->size < GSL_MIN (M, N))
    {
      GSL_ERROR ("size of tau must be at least MIN(M,N)", GSL_EBADLEN);
    }
  else if (QRB->size2 != 2*p + q + 1)
    {
      GSL_ERROR ("dimensions of QRB are inconsistent with (p,q)", GSL_EBADLEN);
    }
  else
    {
      size_t i;

      /* form matrix Q */
      gsl_matrix_set_identity (Q);

      for (i = GSL_MIN (M, N); i-- > 0;)
        {
          size_t k1 = GSL_MIN(p + 1, M - i);     /* number of non-zero elements of this column, including diagonal element */
          gsl_vector_const_view h = gsl_matrix_const_subrow(QRB, i, p + q, k1);
          gsl_matrix_view m = gsl_matrix_submatrix (Q, i, i, k1, M - i);
          double ti = gsl_vector_get (tau, i);
          gsl_vector_view work = gsl_matrix_subcolumn(R, 0, 0, M - i);
          double * ptr = gsl_vector_ptr((gsl_vector *) &h.vector, 0);
          double tmp = *ptr;

          *ptr = 1.0;
          gsl_linalg_householder_left (ti, &h.vector, &m.matrix, &work.vector);
          *ptr = tmp;
        }

      /* form matrix R */
      gsl_matrix_set_zero(R);

      for (i = 0; i <= GSL_MIN(p + q, N - 1); ++i)
        {
          gsl_vector_const_view src = gsl_matrix_const_subcolumn(QRB, p + q - i, i, GSL_MIN(M, N - i));
          gsl_vector_view dest = gsl_matrix_superdiagonal(R, i);
          gsl_vector_memcpy(&dest.vector, &src.vector);
        }

      return GSL_SUCCESS;
    }
}


#ifndef KEYPOINT_TRANSFORM_H_ARXFM8VJ
#define KEYPOINT_TRANSFORM_H_ARXFM8VJ

#include <gsl/gsl>
#include <sens_loc/camera_models/pinhole.h>
#include <sens_loc/camera_models/projection.h>
#include <sens_loc/math/pointcloud.h>

namespace sens_loc::apps {

template <template <typename> typename Model = sens_loc::camera_models::pinhole,
          typename Real                      = float>
math::pointcloud<Real>
keypoints_to_pointcloud(const std::vector<cv::KeyPoint>& kps,
                        const math::image<ushort>&       depth_image,
                        const Model<Real>&               c,
                        float                            unit_factor) noexcept {
    using camera_models::keypoint_to_coords;
    math::imagepoints<Real> pts   = keypoint_to_coords(kps);
    math::pointcloud<Real>  c_pts = project_to_sphere(c, pts);

    for (std::size_t i = 0; i < c_pts.size(); ++i) {
        auto orig_depth = depth_image.at(pts[i]);
        auto t_d        = unit_factor * gsl::narrow_cast<float>(orig_depth);
        c_pts[i]        = t_d * c_pts[i];
    }

    return c_pts;
}

/// Transform the pointcloud \c points_in_other_frame into the coordinate
/// frame of \c c and project these points into \c c's image plane.
/// \returns the backprojected points
/// \sa project_to_image
template <template <typename> typename Model = sens_loc::camera_models::pinhole,
          typename Real                      = float>
math::imagepoints<Real>
project_to_other_camera(const math::pose_t&           pose,
                        const math::pointcloud<Real>& points_in_other_frame,
                        const Model<Real>&            c) noexcept {
    math::pointcloud<Real>  transformed   = pose * points_in_other_frame;
    math::imagepoints<Real> backprojected = project_to_image(c, transformed);
    return backprojected;
}

}  // namespace sens_loc::apps

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