#include <stdio.h>
#include <getopt.h>
#include <omp.h>
#include <math.h>
#include <string.h>

#ifdef __APPLE__

#include <cblas.h>

#define set_num_threads(x) openblas_set_num_threads(x)
#define get_num_threads() openblas_get_num_threads()
#else
#include <mkl.h>
#define set_num_threads(x) mkl_set_num_threads(x)
#define get_num_threads() mkl_get_num_threads()
#endif

double norm_inf_mat_Y11(double *X, int n, int extra);

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

    int n, i, j;
    char *filename = NULL;
    double tolerance = 1e-8;
    int option = 0;
    int threads_num = 1;
    while ((option = getopt(argc, argv, "t:f:")) != -1) {
        switch (option) {
            case 'f':
                filename = optarg;
                break;
            case 't':
                threads_num = atoi(optarg);
                break;
            default:
                printf("Usage: mexp -f string \n");
                return 0;
        }
    }
    if (filename == NULL) {
        printf("Usage: mexp -t num_threads -f string \n");
        return 0;
    }

    double *A = NULL, *R = NULL, *B = NULL, *W = NULL;
    double *x, *r, *p, *w;
    FILE *file = fopen(filename, "rb");
    if (!file) {
        printf("Unable to open file!");
        return 0;
    }

    fread(&n, sizeof(int), 1, file);
    A = (double *) malloc((n + 1) * (n + 1) * sizeof(double));
    R = (double *) malloc((n + 1) * (n + 1) * sizeof(double));
    B = (double *) malloc((n + 1) * sizeof(double));
    W = (double *) malloc(n * sizeof(double));
    x = (double *) malloc(n * sizeof(double));
    r = (double *) malloc(n * sizeof(double));
    p = (double *) malloc(n * sizeof(double));
    w = (double *) malloc(n * sizeof(double));

    printf("set_threads_num:%d\t", threads_num++);

    fseek(file, 0, SEEK_SET);
    fread(&n, sizeof(int), 1, file);

    //fseek(file, sizeof(int), SEEK_SET);
    memset(A, 0, sizeof(double) * (n + 1) * (n + 1));
    memset(R, 0, sizeof(double) * (n + 1) * (n + 1));
    for (i = 0; i < n; ++i) {
        fread(A + i * (n + 1), sizeof(double), (size_t) n, file);
    }
    memset(B, 0, sizeof(double) * (n + 1));
    fread(B, sizeof(double), (size_t) n, file);

    set_num_threads(threads_num);
    omp_set_num_threads(threads_num);

    cblas_dcopy(n, B, 1, A + n, n + 1);

    double omp_time = omp_get_wtime();
    double lambda = norm_inf_mat_Y11(A, n, 1);

    //init Y matrix
    for (i = 0; i < n; ++i) {
        double *A_T = A + i * (n + 1);
#pragma omp parallel for
        for (j = 0; j < n; ++j) {
            A_T[j] = -A_T[j] / lambda;
        }
    }
#pragma omp parallel for
    for (i = 0; i < (n + 1) * (n + 1); i += n + 2) {
        A[i] += 1;
    }
#pragma omp parallel for
    for (i = n; i < (n + 1) * n; i += n + 1) {
        A[i] /= lambda;
    }
#pragma omp parallel for
    for (i = 0; i < n; ++i) {
        double *A_T = A + n * (n + 1);
        A_T[i] = 0.0;
    }
    A[(n + 1) * (n + 1)] = 1.0;

    //loop
    double a, b, *T;
    size_t index;
    int itern = 0;
    while (1) {
        a = norm_inf_mat_Y11(A, n, 1);
        index = cblas_idamax(n, A + n, n + 1);
        b = A[(index + 1) * (n + 1) - 1];
        if (a / b < tolerance)
            break;
        else {
            //R = A*A
            cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, n + 1, n + 1,
                        n + 1, 1.0, A, n + 1, A, n + 1, 0.0, R, n + 1);
            T = A;
            A = R;
            R = T;
            itern++;
        }
    }

    omp_time = omp_get_wtime() - omp_time;

    cblas_dcopy(n, A + n, n + 1, B, 1);

    /*
    * check result
    */
    for (i = 0; i < n; ++i) {
        if (fabs(B[i] - 1) > 1e-3) {
            printf("MEXP ERR %f\t", B[i]);
            break;
        }
    }
    printf("mexp_iter:%d\t", itern);
    printf("omp_time:%f\t", omp_time);

    fseek(file, 0, SEEK_SET);
    fread(&n, sizeof(int), 1, file);

    fread(A, sizeof(double), (size_t) (n * n), file);
    memset(B, 0, sizeof(double) * (n + 1));
    fread(B, sizeof(double), (size_t) n, file);
#pragma omp parallel for
    for (i = 0; i < n; ++i) {
        W[i] = 1.0;
        //B[i] /= A[i*(n+1)];
        //cblas_dscal(n,1/A[i*(n+1)],A+i*n,1);
    }

    omp_time = omp_get_wtime();
    int iteration = 0;

    double alpha, beta, rho_new = 0.0, rho_old;
    memset(x, 0, sizeof(double) * n);
    cblas_dcopy(n, B, 1, r, 1); //r <- B

    rho_new = cblas_ddot(n, r, 1, r, 1);
    double w_norm2 = sqrt(rho_new);
    while (1) {
        if (sqrt(rho_new) / w_norm2 < tolerance) {
            break;
        }
        iteration++;
        if (iteration == 1) {
            cblas_dcopy(n, r, 1, p, 1); //p <- r
        } else {
            beta = rho_new / rho_old;
            cblas_dscal(n, beta, p, 1); // p <- beta * p
            cblas_daxpy(n, 1.0, r, 1, p, 1); // p <- r + pcd
        }
        cblas_dsymv(CblasRowMajor, CblasUpper, n, 1.0, A, n, p, 1, 0.0, w, 1);
        alpha = rho_new / cblas_ddot(n, p, 1, w, 1);
        cblas_daxpy(n, alpha, p, 1, x, 1);
        cblas_daxpy(n, -alpha, w, 1, r, 1);  // r = r - alpha* w
        rho_old = rho_new;
        rho_new = cblas_ddot(n, r, 1, r, 1); //rho_new = r' dot r;
    }

    printf("cg_it: %d\t", iteration);
    printf("cg_omp_time: %f", omp_get_wtime() - omp_time);

    /*
    * check result
    */
    for (i = 0; i < n; ++i) {
        if (fabs(x[i] - 1) > 1e-3) {
            printf(" CG ERR %f\t", x[i]);
            break;
        }
    }
    printf("\n");

    free(A);
    free(B);
    free(W);
    free(R);
    free(x);
    free(r);
    free(p);
    free(w);
    fclose(file);
}

double norm_inf_mat_Y11(double *X, int n, int extra) {
    double lambda = 0.0, lambda_t;
    int i;
    for (i = 0; i < n; ++i) {
        lambda_t = cblas_dasum(n, X + (i * (n + extra)), 1);
        if (lambda_t > lambda)
            lambda = lambda_t;
    }
    return lambda;
}


/* linalg/qrpt.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman, Brian Gough
 *
 * 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 2 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_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_permute_vector.h>
#include <gsl/gsl_blas.h>

#include <gsl/gsl_linalg.h>

#define REAL double

#include "givens.c"
#include "apply_givens.c"

/* Factorise a general M x N matrix A into
 *
 *   A P = Q R
 *
 * where Q is orthogonal (M x M) and R is upper triangular (M x N).
 * When A is rank deficient, r = rank(A) < n, then the permutation is
 * used to ensure that the lower n - r rows of R are zero and the first
 * r columns of Q form an orthonormal basis for A.
 *
 * Q is stored as a packed set of Householder transformations in the
 * strict lower triangular part of the input matrix.
 *
 * R is stored in the diagonal and upper triangle of the input matrix.
 *
 * P: column j of P is column k of the identity matrix, where k =
 * permutation->data[j]
 *
 * 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
 *
 *       v_i = [1, m(i+1,i), m(i+2,i), ... , m(M,i)]
 *
 * This storage scheme is the same as in LAPACK.  See LAPACK's
 * dgeqpf.f for details.
 *
 */

int
gsl_linalg_QRPT_decomp (gsl_matrix * A, gsl_vector * tau, gsl_permutation * p, int *signum, gsl_vector * norm)
{
  const size_t M = A->size1;
  const size_t N = A->size2;

  if (tau->size != GSL_MIN (M, N))
    {
      GSL_ERROR ("size of tau must be MIN(M,N)", GSL_EBADLEN);
    }
  else if (p->size != N)
    {
      GSL_ERROR ("permutation size must be N", GSL_EBADLEN);
    }
  else if (norm->size != N)
    {
      GSL_ERROR ("norm size must be N", GSL_EBADLEN);
    }
  else
    {
      size_t i;

      *signum = 1;

      gsl_permutation_init (p); /* set to identity */

      /* Compute column norms and store in workspace */

      for (i = 0; i < N; i++)
        {
          gsl_vector_view c = gsl_matrix_column (A, i);
          double x = gsl_blas_dnrm2 (&c.vector);
          gsl_vector_set (norm, i, x);
        }

      for (i = 0; i < GSL_MIN (M, N); i++)
        {
          /* Bring the column of largest norm into the pivot position */

          double max_norm = gsl_vector_get(norm, i);
          size_t j, kmax = i;

          for (j = i + 1; j < N; j++)
            {
              double x = gsl_vector_get (norm, j);

              if (x > max_norm)
                {
                  max_norm = x;
                  kmax = j;
                }
            }

          if (kmax != i)
            {
              gsl_matrix_swap_columns (A, i, kmax);
              gsl_permutation_swap (p, i, kmax);
              gsl_vector_swap_elements(norm,i,kmax);

              (*signum) = -(*signum);
            }

          /* Compute the Householder transformation to reduce the j-th
             column of the matrix to a multiple of the j-th unit vector */

          {
            gsl_vector_view c_full = gsl_matrix_column (A, i);
            gsl_vector_view c = gsl_vector_subvector (&c_full.vector,
                                                      i, M - i);
            double tau_i = gsl_linalg_householder_transform (&c.vector);

            gsl_vector_set (tau, i, tau_i);

            /* Apply the transformation to the remaining columns */

            if (i + 1 < N)
              {
                gsl_matrix_view m = gsl_matrix_submatrix (A, i, i + 1, M - i, N - (i+1));

                gsl_linalg_householder_hm (tau_i, &c.vector, &m.matrix);
              }
          }

          /* Update the norms of the remaining columns too */

          if (i + 1 < M)
            {
              for (j = i + 1; j < N; j++)
                {
                  double x = gsl_vector_get (norm, j);

                  if (x > 0.0)
                    {
                      double y = 0;
                      double temp= gsl_matrix_get (A, i, j) / x;

                      if (fabs (temp) >= 1)
                        y = 0.0;
                      else
                        y = x * sqrt (1 - temp * temp);

                      /* recompute norm to prevent loss of accuracy */

                      if (fabs (y / x) < sqrt (20.0) * GSL_SQRT_DBL_EPSILON)
                        {
                          gsl_vector_view c_full = gsl_matrix_column (A, j);
                          gsl_vector_view c =
                            gsl_vector_subvector(&c_full.vector,
                                                 i+1, M - (i+1));
                          y = gsl_blas_dnrm2 (&c.vector);
                        }

                      gsl_vector_set (norm, j, y);
                    }
                }
            }
        }

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_QRPT_decomp2 (const gsl_matrix * A, gsl_matrix * q, gsl_matrix * r, gsl_vector * tau, gsl_permutation * p, int *signum, gsl_vector * norm)
{
  const size_t M = A->size1;
  const size_t N = A->size2;

  if (q->size1 != M || q->size2 !=M)
    {
      GSL_ERROR ("q must be M x M", GSL_EBADLEN);
    }
  else if (r->size1 != M || r->size2 !=N)
    {
      GSL_ERROR ("r must be M x N", GSL_EBADLEN);
    }
  else if (tau->size != GSL_MIN (M, N))
    {
      GSL_ERROR ("size of tau must be MIN(M,N)", GSL_EBADLEN);
    }
  else if (p->size != N)
    {
      GSL_ERROR ("permutation size must be N", GSL_EBADLEN);
    }
  else if (norm->size != N)
    {
      GSL_ERROR ("norm size must be N", GSL_EBADLEN);
    }

  gsl_matrix_memcpy (r, A);

  gsl_linalg_QRPT_decomp (r, tau, p, signum, norm);

  /* FIXME:  aliased arguments depends on behavior of unpack routine! */

  gsl_linalg_QR_unpack (r, tau, q, r);

  return GSL_SUCCESS;
}

/* Solves the system A x = b using the Q R P^T factorisation,

   R z = Q^T b

   x = P z;

   to obtain x. Based on SLATEC code. */

int
gsl_linalg_QRPT_solve (const gsl_matrix * QR,
                       const gsl_vector * tau,
                       const gsl_permutation * p,
                       const gsl_vector * b,
                       gsl_vector * x)
{
  if (QR->size1 != QR->size2)
    {
      GSL_ERROR ("QR matrix must be square", GSL_ENOTSQR);
    }
  else if (QR->size1 != p->size)
    {
      GSL_ERROR ("matrix size must match permutation size", GSL_EBADLEN);
    }
  else if (QR->size1 != b->size)
    {
      GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
    }
  else if (QR->size2 != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else
    {
      gsl_vector_memcpy (x, b);

      gsl_linalg_QRPT_svx (QR, tau, p, x);

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_QRPT_svx (const gsl_matrix * QR,
                     const gsl_vector * tau,
                     const gsl_permutation * p,
                     gsl_vector * x)
{
  if (QR->size1 != QR->size2)
    {
      GSL_ERROR ("QR matrix must be square", GSL_ENOTSQR);
    }
  else if (QR->size1 != p->size)
    {
      GSL_ERROR ("matrix size must match permutation size", GSL_EBADLEN);
    }
  else if (QR->size2 != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else
    {
      /* compute sol = Q^T b */

      gsl_linalg_QR_QTvec (QR, tau, x);

      /* Solve R x = sol, storing x inplace in sol */

      gsl_blas_dtrsv (CblasUpper, CblasNoTrans, CblasNonUnit, QR, x);

      gsl_permute_vector_inverse (p, x);

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_QRPT_QRsolve (const gsl_matrix * Q, const gsl_matrix * R,
                         const gsl_permutation * p,
                         const gsl_vector * b,
                         gsl_vector * x)
{
  if (Q->size1 != Q->size2 || R->size1 != R->size2)
    {
      return GSL_ENOTSQR;
    }
  else if (Q->size1 != p->size || Q->size1 != R->size1
           || Q->size1 != b->size)
    {
      return GSL_EBADLEN;
    }
  else
    {
      /* compute b' = Q^T b */

      gsl_blas_dgemv (CblasTrans, 1.0, Q, b, 0.0, x);

      /* Solve R x = b', storing x inplace */

      gsl_blas_dtrsv (CblasUpper, CblasNoTrans, CblasNonUnit, R, x);

      /* Apply permutation to solution in place */

      gsl_permute_vector_inverse (p, x);

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_QRPT_Rsolve (const gsl_matrix * QR,
                        const gsl_permutation * p,
                        const gsl_vector * b,
                        gsl_vector * x)
{
  if (QR->size1 != QR->size2)
    {
      GSL_ERROR ("QR matrix must be square", GSL_ENOTSQR);
    }
  else if (QR->size1 != b->size)
    {
      GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
    }
  else if (QR->size2 != x->size)
    {
      GSL_ERROR ("matrix size must match x size", GSL_EBADLEN);
    }
  else if (p->size != x->size)
    {
      GSL_ERROR ("permutation size must match x size", GSL_EBADLEN);
    }
  else
    {
      /* Copy x <- b */

      gsl_vector_memcpy (x, b);

      /* Solve R x = b, storing x inplace */

      gsl_blas_dtrsv (CblasUpper, CblasNoTrans, CblasNonUnit, QR, x);

      gsl_permute_vector_inverse (p, x);

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_QRPT_Rsvx (const gsl_matrix * QR,
                      const gsl_permutation * p,
                      gsl_vector * x)
{
  if (QR->size1 != QR->size2)
    {
      GSL_ERROR ("QR matrix must be square", GSL_ENOTSQR);
    }
  else if (QR->size2 != x->size)
    {
      GSL_ERROR ("matrix size must match x size", GSL_EBADLEN);
    }
  else if (p->size != x->size)
    {
      GSL_ERROR ("permutation size must match x size", GSL_EBADLEN);
    }
  else
    {
      /* Solve R x = b, storing x inplace */

      gsl_blas_dtrsv (CblasUpper, CblasNoTrans, CblasNonUnit, QR, x);

      gsl_permute_vector_inverse (p, x);

      return GSL_SUCCESS;
    }
}

/* Update a Q R P^T factorisation for A P= Q R ,  A' = A + u v^T,

   Q' R' P^-1 = QR P^-1 + u v^T
   = Q (R + Q^T u v^T P ) P^-1
   = Q (R + w v^T P) P^-1

   where w = Q^T u.

   Algorithm from Golub and Van Loan, "Matrix Computations", Section
   12.5 (Updating Matrix Factorizations, Rank-One Changes)  */

int
gsl_linalg_QRPT_update (gsl_matrix * Q, gsl_matrix * R,
                        const gsl_permutation * p,
                        gsl_vector * w, const gsl_vector * v)
{
  if (Q->size1 != Q->size2 || R->size1 != R->size2)
    {
      return GSL_ENOTSQR;
    }
  else if (R->size1 != Q->size2 || v->size != Q->size2 || w->size != Q->size2)
    {
      return GSL_EBADLEN;
    }
  else
    {
      size_t j, k;
      const size_t M = Q->size1;
      const size_t N = Q->size2;
      double w0;

      /* Apply Given's rotations to reduce w to (|w|, 0, 0, ... , 0)

         J_1^T .... J_(n-1)^T w = +/- |w| e_1

         simultaneously applied to R,  H = J_1^T ... J^T_(n-1) R
         so that H is upper Hessenberg.  (12.5.2) */

      for (k = N - 1; k > 0; k--)
        {
          double c, s;
          double wk = gsl_vector_get (w, k);
          double wkm1 = gsl_vector_get (w, k - 1);

          create_givens (wkm1, wk, &c, &s);
          apply_givens_vec (w, k - 1, k, c, s);
          apply_givens_qr (M, N, Q, R, k - 1, k, c, s);
        }

      w0 = gsl_vector_get (w, 0);

      /* Add in w v^T  (Equation 12.5.3) */

      for (j = 0; j < N; j++)
        {
          double r0j = gsl_matrix_get (R, 0, j);
          size_t p_j = gsl_permutation_get (p, j);
          double vj = gsl_vector_get (v, p_j);
          gsl_matrix_set (R, 0, j, r0j + w0 * vj);
        }

      /* Apply Givens transformations R' = G_(n-1)^T ... G_1^T H
         Equation 12.5.4 */

      for (k = 1; k < N; k++)
        {
          double c, s;
          double diag = gsl_matrix_get (R, k - 1, k - 1);
          double offdiag = gsl_matrix_get (R, k, k - 1);

          create_givens (diag, offdiag, &c, &s);
          apply_givens_qr (M, N, Q, R, k - 1, k, c, s);
        }

      return GSL_SUCCESS;
    }
}


#include <math.h>
#include <stdlib.h>
#include <assert.h>
#include <cconfigspace.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_cdf.h>

#define NUM_DISTRIBS 2
#define NUM_SAMPLES 10000

void test_create_multivariate_distribution() {
	const size_t            num_distribs = NUM_DISTRIBS;
	ccs_distribution_t      distrib = NULL;
	ccs_distribution_t      distribs[NUM_DISTRIBS];
	ccs_distribution_t      distribs_ret[NUM_DISTRIBS];
	ccs_result_t            err = CCS_SUCCESS;
	int32_t                 refcount;
	ccs_object_type_t       otype;
	ccs_distribution_type_t dtype;
	ccs_numeric_type_t      data_types[NUM_DISTRIBS];
	ccs_interval_t          intervals[NUM_DISTRIBS];
	size_t                  num_distribs_ret;

	err = ccs_create_uniform_distribution(
		CCS_NUM_FLOAT,
		CCSF(-5.0),
		CCSF(5.0),
		CCS_LINEAR,
		CCSF(0.0),
		distribs);
	assert( err == CCS_SUCCESS );

	err = ccs_create_uniform_distribution(
		CCS_NUM_INTEGER,
		CCSI(-5),
		CCSI(5),
		CCS_LINEAR,
		CCSI(0),
		distribs + 1);
	assert( err == CCS_SUCCESS );

	err = ccs_create_multivariate_distribution(
		num_distribs,
		distribs,
		&distrib);
	assert( err == CCS_SUCCESS );

	err = ccs_object_get_type(distrib, &otype);
	assert( err == CCS_SUCCESS );
	assert( otype == CCS_DISTRIBUTION );

	err = ccs_distribution_get_type(distrib, &dtype);
	assert( err == CCS_SUCCESS );
	assert( dtype == CCS_MULTIVARIATE );

	err = ccs_distribution_get_data_types(distrib, data_types);
	assert( err == CCS_SUCCESS );
	assert( data_types[0] == CCS_NUM_FLOAT );
	assert( data_types[1] == CCS_NUM_INTEGER );

        err = ccs_distribution_get_bounds(distrib, intervals);
	assert( err == CCS_SUCCESS );
	assert( intervals[0].type == CCS_NUM_FLOAT );
	assert( intervals[0].lower.f == -5.0 );
	assert( intervals[0].lower_included == CCS_TRUE );
	assert( intervals[0].upper.f == 5.0 );
	assert( intervals[0].upper_included == CCS_FALSE );
	assert( intervals[1].type == CCS_NUM_INTEGER );
	assert( intervals[1].lower.i == -5 );
	assert( intervals[1].lower_included == CCS_TRUE );
	assert( intervals[1].upper.i == 5 );
	assert( intervals[1].upper_included == CCS_FALSE );

	err = ccs_multivariate_distribution_get_num_distributions(distrib, &num_distribs_ret);
	assert( err == CCS_SUCCESS );
	assert( num_distribs_ret == num_distribs );

	err = ccs_multivariate_distribution_get_distributions(distrib, num_distribs, distribs_ret, &num_distribs_ret);
	assert( err == CCS_SUCCESS );
	assert( num_distribs_ret == num_distribs );
	for (size_t i = 0; i < num_distribs; i++)
		assert( distribs_ret[i] == distribs[i] );

	err = ccs_object_get_refcount(distrib, &refcount);
	assert( err == CCS_SUCCESS );
	assert( refcount == 1 );

	for (size_t i = 0; i < num_distribs; i++) {
		err = ccs_release_object(distribs[i]);
		assert( err == CCS_SUCCESS );
	}
	err = ccs_release_object(distrib);
	assert( err == CCS_SUCCESS );
}

void test_multivariate_distribution() {
	ccs_distribution_t distrib = NULL, distribs[NUM_DISTRIBS];
	ccs_rng_t          rng = NULL;
	ccs_result_t       err = CCS_SUCCESS;
	const size_t       num_distribs = NUM_DISTRIBS;
	const size_t       num_samples = NUM_SAMPLES;
	ccs_numeric_t      samples[NUM_SAMPLES*NUM_DISTRIBS];

	err = ccs_rng_create(&rng);
	assert( err == CCS_SUCCESS );

	err = ccs_create_uniform_distribution(
		CCS_NUM_FLOAT,
		CCSF(-5.0),
		CCSF(5.0),
		CCS_LINEAR,
		CCSF(0.0),
		distribs);
	assert( err == CCS_SUCCESS );

	err = ccs_create_uniform_distribution(
		CCS_NUM_INTEGER,
		CCSI(-5),
		CCSI(5),
		CCS_LINEAR,
		CCSI(0),
		distribs + 1);
	assert( err == CCS_SUCCESS );

	err = ccs_create_multivariate_distribution(
		num_distribs,
		distribs,
		&distrib);
	assert( err == CCS_SUCCESS );

	err = ccs_distribution_samples(distrib, rng, num_samples, samples);
	assert( err == CCS_SUCCESS );

	for (size_t i = 0; i < num_samples; i++) {
		assert( samples[NUM_DISTRIBS*i].f >= -5.0 );
		assert( samples[NUM_DISTRIBS*i].f <   5.0 );
		assert( samples[NUM_DISTRIBS*i + 1].i >= -5 );
		assert( samples[NUM_DISTRIBS*i + 1].i <   5 );
	}
	double mean = gsl_stats_mean((double*)samples, NUM_DISTRIBS, num_samples);
	assert( mean < 0.0 + 0.1 );
	assert( mean > 0.0 - 0.1 );

	for (size_t i = 0; i < num_distribs; i++) {
		err = ccs_release_object(distribs[i]);
		assert( err == CCS_SUCCESS );
	}
	err = ccs_release_object(distrib);
	assert( err == CCS_SUCCESS );
	err = ccs_release_object(rng);
	assert( err == CCS_SUCCESS );
}

void test_multivariate_distribution_strided_samples() {
	ccs_distribution_t distrib = NULL, distribs[NUM_DISTRIBS];
	ccs_rng_t          rng = NULL;
	ccs_result_t       err = CCS_SUCCESS;
	const size_t       num_distribs = NUM_DISTRIBS;
	const size_t       num_samples = NUM_SAMPLES;
	ccs_numeric_t      samples[NUM_SAMPLES*(NUM_DISTRIBS+1)];

	err = ccs_rng_create(&rng);
	assert( err == CCS_SUCCESS );

	err = ccs_create_uniform_distribution(
		CCS_NUM_FLOAT,
		CCSF(-5.0),
		CCSF(5.0),
		CCS_LINEAR,
		CCSF(0.0),
		distribs);
	assert( err == CCS_SUCCESS );

	err = ccs_create_uniform_distribution(
		CCS_NUM_INTEGER,
		CCSI(-5),
		CCSI(5),
		CCS_LINEAR,
		CCSI(0),
		distribs + 1);
	assert( err == CCS_SUCCESS );

	err = ccs_create_multivariate_distribution(
		num_distribs,
		distribs,
		&distrib);
	assert( err == CCS_SUCCESS );

	err = ccs_distribution_strided_samples(distrib, rng, num_samples, NUM_DISTRIBS+1, samples);
	assert( err == CCS_SUCCESS );

	for (size_t i = 0; i < num_samples; i++) {
		assert( samples[(NUM_DISTRIBS+1)*i].f >= -5.0 );
		assert( samples[(NUM_DISTRIBS+1)*i].f <   5.0 );
		assert( samples[(NUM_DISTRIBS+1)*i + 1].i >= -5 );
		assert( samples[(NUM_DISTRIBS+1)*i + 1].i <   5 );
	}
	double mean = gsl_stats_mean((double*)samples, NUM_DISTRIBS+1, num_samples);
	assert( mean < 0.0 + 0.1 );
	assert( mean > 0.0 - 0.1 );

	for (size_t i = 0; i < num_distribs; i++) {
		err = ccs_release_object(distribs[i]);
		assert( err == CCS_SUCCESS );
	}
	err = ccs_release_object(distrib);
	assert( err == CCS_SUCCESS );
	err = ccs_release_object(rng);
	assert( err == CCS_SUCCESS );
}

void test_multivariate_distribution_soa_samples() {
	ccs_distribution_t distrib = NULL, distribs[NUM_DISTRIBS];
	ccs_rng_t          rng = NULL;
	ccs_result_t       err = CCS_SUCCESS;
	const size_t       num_distribs = NUM_DISTRIBS;
	const size_t       num_samples = NUM_SAMPLES;
	ccs_numeric_t      samples1[NUM_SAMPLES];
	ccs_numeric_t      samples2[NUM_SAMPLES];
	ccs_numeric_t     *samples[] = { samples1, samples2 };

	err = ccs_rng_create(&rng);
	assert( err == CCS_SUCCESS );

	err = ccs_create_uniform_distribution(
		CCS_NUM_FLOAT,
		CCSF(-5.0),
		CCSF(5.0),
		CCS_LINEAR,
		CCSF(0.0),
		distribs);
	assert( err == CCS_SUCCESS );

	err = ccs_create_uniform_distribution(
		CCS_NUM_INTEGER,
		CCSI(-5),
		CCSI(5),
		CCS_LINEAR,
		CCSI(0),
		distribs + 1);
	assert( err == CCS_SUCCESS );

	err = ccs_create_multivariate_distribution(
		num_distribs,
		distribs,
		&distrib);
	assert( err == CCS_SUCCESS );

	err = ccs_distribution_soa_samples(distrib, rng, num_samples, samples);
	assert( err == CCS_SUCCESS );

	for (size_t i = 0; i < num_samples; i++) {
		assert( samples1[i].f >= -5.0 );
		assert( samples1[i].f <   5.0 );
		assert( samples2[i].i >= -5 );
		assert( samples2[i].i <   5 );
	}
	double mean = gsl_stats_mean((double*)samples1, 1, num_samples);
	assert( mean < 0.0 + 0.1 );
	assert( mean > 0.0 - 0.1 );

	for (size_t i = 0; i < num_distribs; i++) {
		err = ccs_release_object(distribs[i]);
		assert( err == CCS_SUCCESS );
	}
	err = ccs_release_object(distrib);
	assert( err == CCS_SUCCESS );
	err = ccs_release_object(rng);
	assert( err == CCS_SUCCESS );
}

void test_distribution_hyperparameters_sample() {
	const size_t         num_distribs = NUM_DISTRIBS;
	ccs_distribution_t   distrib = NULL, distribs[NUM_DISTRIBS];
	ccs_hyperparameter_t params[NUM_DISTRIBS];
	ccs_rng_t            rng = NULL;
	ccs_result_t         err = CCS_SUCCESS;
	const size_t         num_samples = NUM_SAMPLES;
	ccs_datum_t          samples[NUM_SAMPLES*NUM_DISTRIBS];

	err = ccs_rng_create(&rng);
	assert( err == CCS_SUCCESS );

	err = ccs_create_uniform_distribution(
		CCS_NUM_FLOAT,
		CCSF(-4.0),
		CCSF(4.0),
		CCS_LINEAR,
		CCSF(0.0),
		distribs);
	assert( err == CCS_SUCCESS );

	err = ccs_create_uniform_distribution(
		CCS_NUM_FLOAT,
		CCSF(-3.0),
		CCSF(5.0),
		CCS_LINEAR,
		CCSF(0.0),
		distribs + 1);
	assert( err == CCS_SUCCESS );

	err = ccs_create_numerical_hyperparameter("param1", CCS_NUM_FLOAT,
	                                          CCSF(-5.0), CCSF(5.0),
	                                          CCSF(0.0), CCSF(0.0),
	                                          NULL, params);
	assert( err == CCS_SUCCESS );

	err = ccs_create_numerical_hyperparameter("param2", CCS_NUM_FLOAT,
	                                          CCSF(-4.0), CCSF(6.0),
	                                          CCSF(0.0), CCSF(1.0),
	                                          NULL, params + 1);
	assert( err == CCS_SUCCESS );

	err = ccs_create_multivariate_distribution(
		num_distribs,
		distribs,
		&distrib);
	assert( err == CCS_SUCCESS );

	err = ccs_distribution_hyperparameters_samples(distrib, rng, params, num_samples, samples);
	assert( err == CCS_SUCCESS );

	for (size_t i = 0; i < num_samples; i++) {
		assert( samples[NUM_DISTRIBS*i].type == CCS_FLOAT );
		assert( samples[NUM_DISTRIBS*i].value.f >= -5.0 );
		assert( samples[NUM_DISTRIBS*i].value.f <   5.0 );
		assert( samples[NUM_DISTRIBS*i + 1].type == CCS_FLOAT );
		assert( samples[NUM_DISTRIBS*i + 1].value.f >= -4.0 );
		assert( samples[NUM_DISTRIBS*i + 1].value.f <   6.0 );
	}
	double mean = gsl_stats_mean((double*)samples, NUM_DISTRIBS*2, num_samples);
	assert( mean < 0.0 + 0.1 );
	assert( mean > 0.0 - 0.1 );

	mean = gsl_stats_mean((double*)samples + 2, NUM_DISTRIBS*2, num_samples);
	assert( mean < 1.0 + 0.1 );
	assert( mean > 1.0 - 0.1 );

	for (size_t i = 0; i < num_distribs; i++) {
		err = ccs_release_object(distribs[i]);
		assert( err == CCS_SUCCESS );
		err = ccs_release_object(params[i]);
		assert( err == CCS_SUCCESS );
	}
	err = ccs_release_object(distrib);
	assert( err == CCS_SUCCESS );
	err = ccs_release_object(rng);
	assert( err == CCS_SUCCESS );
}

void test_distribution_hyperparameters_sample_oversampling() {
	const size_t         num_distribs = NUM_DISTRIBS;
	ccs_distribution_t   distrib = NULL, distribs[NUM_DISTRIBS];
	ccs_hyperparameter_t params[NUM_DISTRIBS];
	ccs_rng_t            rng = NULL;
	ccs_result_t         err = CCS_SUCCESS;
	const size_t         num_samples = NUM_SAMPLES;
	ccs_datum_t          samples[NUM_SAMPLES*NUM_DISTRIBS];

	err = ccs_rng_create(&rng);
	assert( err == CCS_SUCCESS );

	err = ccs_create_normal_distribution(
		CCS_NUM_FLOAT,
		0.0,
		4.0,
		CCS_LINEAR,
		CCSF(0.0),
		distribs);
	assert( err == CCS_SUCCESS );

	err = ccs_create_normal_distribution(
		CCS_NUM_FLOAT,
		1.0,
		4.0,
		CCS_LINEAR,
		CCSF(0.0),
		distribs + 1);
	assert( err == CCS_SUCCESS );

	err = ccs_create_numerical_hyperparameter("param1", CCS_NUM_FLOAT,
	                                          CCSF(-5.0), CCSF(5.0),
	                                          CCSF(0.0), CCSF(0.0),
	                                          NULL, params);
	assert( err == CCS_SUCCESS );

	err = ccs_create_numerical_hyperparameter("param2", CCS_NUM_FLOAT,
	                                          CCSF(-4.0), CCSF(6.0),
	                                          CCSF(0.0), CCSF(1.0),
	                                          NULL, params + 1);
	assert( err == CCS_SUCCESS );

	err = ccs_create_multivariate_distribution(
		num_distribs,
		distribs,
		&distrib);
	assert( err == CCS_SUCCESS );

	err = ccs_distribution_hyperparameters_samples(distrib, rng, params, num_samples, samples);
	assert( err == CCS_SUCCESS );

	for (size_t i = 0; i < num_samples; i++) {
		assert( samples[NUM_DISTRIBS*i].type == CCS_FLOAT );
		assert( samples[NUM_DISTRIBS*i].value.f >= -5.0 );
		assert( samples[NUM_DISTRIBS*i].value.f <   5.0 );
		assert( samples[NUM_DISTRIBS*i + 1].type == CCS_FLOAT );
		assert( samples[NUM_DISTRIBS*i + 1].value.f >= -4.0 );
		assert( samples[NUM_DISTRIBS*i + 1].value.f <   6.0 );
	}
	double mean = gsl_stats_mean((double*)samples, NUM_DISTRIBS*2, num_samples);
	assert( mean < 0.0 + 0.1 );
	assert( mean > 0.0 - 0.1 );

	mean = gsl_stats_mean((double*)samples + 2, NUM_DISTRIBS*2, num_samples);
	assert( mean < 1.0 + 0.1 );
	assert( mean > 1.0 - 0.1 );

	for (size_t i = 0; i < num_distribs; i++) {
		err = ccs_release_object(distribs[i]);
		assert( err == CCS_SUCCESS );
		err = ccs_release_object(params[i]);
		assert( err == CCS_SUCCESS );
	}
	err = ccs_release_object(distrib);
	assert( err == CCS_SUCCESS );
	err = ccs_release_object(rng);
	assert( err == CCS_SUCCESS );
}

int main() {
	ccs_init();
	test_create_multivariate_distribution();
	test_multivariate_distribution();
	test_multivariate_distribution_strided_samples();
	test_multivariate_distribution_soa_samples();
	test_distribution_hyperparameters_sample();
	test_distribution_hyperparameters_sample_oversampling();
	ccs_fini();
	return 0;
}


/* movstat/test_Qn.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 <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_sort.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_movstat.h>

/* calculate Q_n statistic for input vector using slow/naive algorithm */
static int
slow_movQn(const gsl_movstat_end_t etype, const gsl_vector * x, gsl_vector * y,
           const int H, const int J)
{
  const size_t n = x->size;
  const int K = H + J + 1;
  double *window = malloc(K * sizeof(double));
  double *work = malloc(3 * K * sizeof(double));
  int *work_int = malloc(5 * K * sizeof(int));
  size_t i;

  for (i = 0; i < n; ++i)
    {
      size_t wsize = gsl_movstat_fill(etype, x, i, H, J, window);
      double Qn;

      gsl_sort(window, 1, wsize);
      Qn = gsl_stats_Qn_from_sorted_data(window, 1, wsize, work, work_int);
      gsl_vector_set(y, i, Qn);
    }

  free(window);
  free(work);
  free(work_int);

  return GSL_SUCCESS;
}

static double
func_Qn(const size_t n, double x[], void * params)
{
  double *work = malloc(3 * n * sizeof(double));
  int *work_int = malloc(5 * n * sizeof(int));
  double Qn;

  (void) params;

  gsl_sort(x, 1, n);
  Qn = gsl_stats_Qn_from_sorted_data(x, 1, n, work, work_int);

  free(work);
  free(work_int);

  return Qn;
}

static void
test_Qn_proc(const double tol, const size_t n, const size_t H, const size_t J,
             const gsl_movstat_end_t etype, gsl_rng *rng_p)
{
  gsl_movstat_workspace *w;
  gsl_vector *x = gsl_vector_alloc(n);
  gsl_vector *y = gsl_vector_alloc(n);
  gsl_vector *z = gsl_vector_alloc(n);
  gsl_movstat_function F;
  char buf[2048];

  F.function = func_Qn;
  F.params = NULL;

  if (H == J)
    w = gsl_movstat_alloc(2*H + 1);
  else
    w = gsl_movstat_alloc2(H, J);

  /* test moving median with random input */
  random_vector(x, rng_p);

  /* y = Q_n(x) with slow brute force algorithm */
  slow_movQn(etype, x, y, H, J);

  /* z = Q_n(x) */
  gsl_movstat_Qn(etype, x, z, w);

  /* test y = z */
  sprintf(buf, "n=%zu H=%zu J=%zu endtype=%u Qn random", n, H, J, etype);
  compare_vectors(tol, z, y, buf);

  /* z = Q_n(x) in-place */
  gsl_vector_memcpy(z, x);
  gsl_movstat_Qn(etype, z, z, w);

  sprintf(buf, "n=%zu H=%zu J=%zu endtype=%u Qn random in-place", n, H, J, etype);
  compare_vectors(tol, z, y, buf);

  /* z = Q_n(x) with user-defined function */
  gsl_movstat_apply(etype, &F, x, z, w);

  sprintf(buf, "n=%zu H=%zu J=%zu endtype=%u Qn user", n, H, J, etype);
  compare_vectors(tol, z, y, buf);

  gsl_vector_free(x);
  gsl_vector_free(y);
  gsl_vector_free(z);
  gsl_movstat_free(w);
}

static void
test_Qn(gsl_rng * rng_p)
{
  test_Qn_proc(GSL_DBL_EPSILON, 1000, 0, 0, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_Qn_proc(GSL_DBL_EPSILON, 1000, 5, 5, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_Qn_proc(GSL_DBL_EPSILON, 1000, 5, 2, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_Qn_proc(GSL_DBL_EPSILON, 1000, 2, 5, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_Qn_proc(GSL_DBL_EPSILON, 2000, 50, 0, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_Qn_proc(GSL_DBL_EPSILON, 2000, 0, 50, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_Qn_proc(GSL_DBL_EPSILON, 20, 50, 50, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_Qn_proc(GSL_DBL_EPSILON, 20, 1, 50, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_Qn_proc(GSL_DBL_EPSILON, 20, 50, 1, GSL_MOVSTAT_END_PADZERO, rng_p);

  test_Qn_proc(GSL_DBL_EPSILON, 1000, 0, 0, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_Qn_proc(GSL_DBL_EPSILON, 1000, 5, 5, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_Qn_proc(GSL_DBL_EPSILON, 1000, 5, 2, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_Qn_proc(GSL_DBL_EPSILON, 1000, 2, 5, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_Qn_proc(GSL_DBL_EPSILON, 2000, 50, 0, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_Qn_proc(GSL_DBL_EPSILON, 2000, 0, 50, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_Qn_proc(GSL_DBL_EPSILON, 20, 50, 50, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_Qn_proc(GSL_DBL_EPSILON, 20, 1, 50, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_Qn_proc(GSL_DBL_EPSILON, 20, 50, 1, GSL_MOVSTAT_END_PADVALUE, rng_p);

  test_Qn_proc(GSL_DBL_EPSILON, 1000, 0, 0, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_Qn_proc(GSL_DBL_EPSILON, 1000, 5, 5, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_Qn_proc(GSL_DBL_EPSILON, 1000, 5, 2, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_Qn_proc(GSL_DBL_EPSILON, 1000, 2, 5, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_Qn_proc(GSL_DBL_EPSILON, 2000, 50, 0, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_Qn_proc(GSL_DBL_EPSILON, 2000, 0, 50, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_Qn_proc(GSL_DBL_EPSILON, 20, 50, 50, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_Qn_proc(GSL_DBL_EPSILON, 20, 1, 50, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_Qn_proc(GSL_DBL_EPSILON, 20, 50, 1, GSL_MOVSTAT_END_TRUNCATE, rng_p);
}


/* movstat/test_median.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 <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_movstat.h>
#include <gsl/gsl_statistics.h>

/* compute filtered data by explicitely constructing window, sorting it and finding median */
int
slow_movmedian(const gsl_movstat_end_t etype, const gsl_vector * x, gsl_vector * y,
               const int H, const int J)
{
  const size_t n = x->size;
  const int K = H + J + 1;
  double *window = malloc(K * sizeof(double));
  size_t i;

  for (i = 0; i < n; ++i)
    {
      size_t wsize = gsl_movstat_fill(etype, x, i, H, J, window);
      double yi = gsl_stats_median(window, 1, wsize);

      gsl_vector_set(y, i, yi);
    }

  free(window);

  return GSL_SUCCESS;
}

/* test root sequence */
static void
test_median_root(const double tol, const size_t n, const size_t K, const gsl_movstat_end_t etype)
{
  gsl_movstat_workspace *w = gsl_movstat_alloc(K);
  gsl_vector *x = gsl_vector_alloc(n);
  gsl_vector *y = gsl_vector_alloc(n);
  char buf[2048];
  size_t i;

  /* test a root sequence (square input): x = [zero one zero] */
  gsl_vector_set_all(x, 0.0);

  for (i = n / 3; i <= n / 2; ++i)
    gsl_vector_set(x, i, 1.0);

  /* compute y = median(x) and test y = x */
  gsl_movstat_median(etype, x, y, w);

  sprintf(buf, "n=%zu K=%zu endtype=%u SMF root sequence", n, K, etype);
  compare_vectors(tol, y, x, buf);

  gsl_vector_free(x);
  gsl_vector_free(y);
  gsl_movstat_free(w);
}

static double
func_median(const size_t n, double x[], void * params)
{
  (void) params;
  return gsl_stats_median(x, 1, n);
}

static void
test_median_proc(const double tol, const size_t n, const size_t H, const size_t J,
                 const gsl_movstat_end_t etype, gsl_rng *rng_p)
{
  gsl_movstat_workspace *w;
  gsl_vector *x = gsl_vector_alloc(n);
  gsl_vector *y = gsl_vector_alloc(n);
  gsl_vector *z = gsl_vector_alloc(n);
  gsl_movstat_function F;
  char buf[2048];

  F.function = func_median;
  F.params = NULL;

  if (H == J)
    w = gsl_movstat_alloc(2*H + 1);
  else
    w = gsl_movstat_alloc2(H, J);

  /* test moving median with random input */
  random_vector(x, rng_p);

  /* y = median(x) with slow brute force algorithm */
  slow_movmedian(etype, x, y, H, J);

  /* z = median(x) */
  gsl_movstat_median(etype, x, z, w);

  /* test y = z */
  sprintf(buf, "n=%zu H=%zu J=%zu endtype=%u median random", n, H, J, etype);
  compare_vectors(tol, z, y, buf);

  /* z = median(x) in-place */
  gsl_vector_memcpy(z, x);
  gsl_movstat_median(etype, z, z, w);

  sprintf(buf, "n=%zu H=%zu J=%zu endtype=%u median random in-place", n, H, J, etype);
  compare_vectors(tol, z, y, buf);

  /* z = median(x) with user-defined function */
  gsl_movstat_apply(etype, &F, x, z, w);

  sprintf(buf, "n=%zu H=%zu J=%zu endtype=%u median user", n, H, J, etype);
  compare_vectors(tol, z, y, buf);

  gsl_vector_free(x);
  gsl_vector_free(y);
  gsl_vector_free(z);
  gsl_movstat_free(w);
}

static void
test_median(gsl_rng * rng_p)
{
  test_median_root(GSL_DBL_EPSILON, 1000, 3, GSL_MOVSTAT_END_PADZERO);
  test_median_root(GSL_DBL_EPSILON, 200, 15, GSL_MOVSTAT_END_PADVALUE);
  test_median_root(GSL_DBL_EPSILON, 100, 5, GSL_MOVSTAT_END_TRUNCATE);

  test_median_proc(GSL_DBL_EPSILON, 1000, 0, 0, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 1000, 1, 1, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 100, 150, 150, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 5000, 8, 8, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 5000, 0, 5, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 5000, 5, 0, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 5000, 15, 10, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 5000, 10, 15, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 50, 100, 150, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 50, 150, 100, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 50, 100, 100, GSL_MOVSTAT_END_PADZERO, rng_p);

  test_median_proc(GSL_DBL_EPSILON, 1000, 0, 0, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 1000, 1, 1, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 100, 150, 150, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 5000, 8, 8, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 5000, 0, 5, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 5000, 5, 0, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 5000, 15, 10, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 5000, 10, 15, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 50, 100, 150, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 50, 150, 100, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 50, 100, 100, GSL_MOVSTAT_END_PADVALUE, rng_p);

  test_median_proc(GSL_DBL_EPSILON, 1000, 0, 0, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 1000, 1, 1, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 100, 150, 150, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 5000, 8, 8, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 5000, 0, 5, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 5000, 5, 0, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 5000, 15, 10, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 5000, 10, 15, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 50, 100, 150, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 50, 150, 100, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_median_proc(GSL_DBL_EPSILON, 50, 100, 100, GSL_MOVSTAT_END_TRUNCATE, rng_p);
}


#include <stdio.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

#define NRAND 1000000

#define HIST_SIZE 25
#define HIST_MIN 0
#define HIST_MAX (HIST_SIZE - 1)

int hist[HIST_SIZE];

int main (void)
{
	const gsl_rng_type * T;
	gsl_rng * r;
	double mu = 2.0;

	/* create a generator chosen by the
	   environment variable GSL_RNG_TYPE */
	gsl_rng_env_setup();
	T = gsl_rng_default;
	r = gsl_rng_alloc (T);

	printf("Distribution with mean of %f\n", mu);

	/* print N random variates chosen from
	   the poisson distribution with mean
	   parameter mu */
	for (int i = 0; i < NRAND; i++) {
		/* unsigned int k = gsl_ran_poisson(r, mu) + 10; */
		unsigned int k = gsl_ran_gaussian(r, mu) + 10;
		if (HIST_MIN <= k && k <= HIST_MAX) {
			hist[k]++;
		} else {
			printf ("outside history range: %u\n", k);
		}
	}

	for (int i = 0; i < HIST_SIZE; i++) {
		printf("[%d] ", i);
		for (int j = 0; j < hist[i] / (NRAND / 100); j++) {
			putchar('*');
		}
		printf("\n");
	}

	gsl_rng_free (r);
	return 0;
}


//====---- Sudoku/Size.h                                              ----====//
//
// The size definition that rules them all.
//====--------------------------------------------------------------------====//
#pragma once

#include <gsl/gsl>

#include <limits>

namespace Sudoku
{
template<int N>
struct Size
{
	using index                 = gsl::index;
	static constexpr index base = N;           // 3 for default 9*9 board
	static constexpr index elem = base * base; // 9 for default
	static constexpr index full = elem * elem; // 81 for default

	static_assert(
		1 < base && base < elem && base < full && elem < full,
		"struct Size: Board size out of bounds.");
	static_assert(
		N < std::numeric_limits<index>::max() / base &&
			N < std::numeric_limits<index>::max() / elem &&
			N < std::numeric_limits<index>::max() / elem / base &&
			N < std::numeric_limits<index>::max() / full &&
			N < std::numeric_limits<index>::max() / full / base,
		"struct Size: Board size too large for system integer size.");
	// fails at N > 35 when int less than 32-bits.
};

template<int N>
inline constexpr gsl::index base_size = Size<N>::base; // default 3
template<int N>
inline constexpr gsl::index elem_size = Size<N>::elem; // default 9
template<int N>
inline constexpr gsl::index full_size = Size<N>::full; // default 81

} // namespace Sudoku


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

#include <gsl/gsl_interp.h>
#include <gsl/gsl_spline.h>

#include "ccl.h"

/* BCM correction */
// See Schneider & Teyssier (2015) for details of the model.
double ccl_bcm_model_fka(ccl_cosmology * cosmo, double k, double a, int *status) {
  double fkz;
  double b0;
  double bfunc, bfunc4;
  double kg;
  double gf,scomp;
  double kh;
  double z;

  z = 1./a - 1.;
  kh = k / cosmo->params.h;
  b0 = 0.105*cosmo->params.bcm_log10Mc - 1.27;
  bfunc = b0 / (1. + pow(z/2.3, 2.5));
  bfunc4 = (1-bfunc) * (1-bfunc) * (1-bfunc) * (1-bfunc);
  kg = 0.7 * bfunc4 * pow(cosmo->params.bcm_etab, -1.6);
  gf = bfunc / (1 + pow(kh/kg, 3.)) + 1. - bfunc; //k in h/Mpc
  scomp = 1 + (kh / cosmo->params.bcm_ks) * (kh / cosmo->params.bcm_ks); //k in h/Mpc
  fkz = gf * scomp;
  return fkz;
}

void ccl_bcm_correct(ccl_cosmology *cosmo, ccl_f2d_t *psp, int *status)
{
  size_t nk, na;
  double *x, *z, *y2d=NULL;

  //Find lk array
  if(psp->fk != NULL) {
    nk = psp->fk->size;
    x = psp->fk->x;
  }
  else {
    nk = psp->fka->interp_object.xsize;
    x = psp->fka->xarr;
  }

  //Find a array
  if(psp->fa != NULL) {
    na = psp->fa->size;
    z = psp->fa->x;
  }
  else {
    na = psp->fka->interp_object.ysize;
    z = psp->fka->yarr;
  }

  //Allocate pka array
  y2d = malloc(nk * na * sizeof(double));
  if (y2d == NULL) {
    *status = CCL_ERROR_MEMORY;
    ccl_cosmology_set_status_message(cosmo,
                                     "ccl_bcm.c: ccl_bcm_correct(): "
                                     "memory allocation\n");
  }

  if (*status == 0) {
    for (int j = 0; j<na; j++) {
      for (int i=0; i<nk; i++) {
        if (*status == 0) {
          double pk = ccl_f2d_t_eval(psp, x[i], z[j], cosmo, status);
          double fbcm = ccl_bcm_model_fka(cosmo, exp(x[i]), z[j], status);
          if(psp->is_log)
            y2d[j*nk + i] = log(pk*fbcm);
          else
            y2d[j*nk + i] = pk*fbcm;
        }
      }
    }
  }

  if (*status == 0) {
    gsl_spline2d *fka = gsl_spline2d_alloc(gsl_interp2d_bicubic, nk, na);

    if (fka == NULL) {
      *status = CCL_ERROR_MEMORY;
      ccl_cosmology_set_status_message(cosmo,
                                       "ccl_bcm.c: ccl_bcm_correct(): "
                                       "memory allocation\n");
    }
    if(*status == 0) {
      int spstatus = gsl_spline2d_init(fka, x, z, y2d, nk, na);
      if(spstatus) {
        *status = CCL_ERROR_MEMORY;
        ccl_cosmology_set_status_message(cosmo,
                                         "ccl_bcm.c: ccl_bcm_correct(): "
                                         "Error initializing spline\n");
      }
    }
    if(*status == 0) {
      if(psp->fa != NULL)
        gsl_spline_free(psp->fa);
      if(psp->fk != NULL)
        gsl_spline_free(psp->fk);
      if(psp->fka != NULL)
        gsl_spline2d_free(psp->fka);
      psp->fka = fka;
      psp->is_factorizable = 0;
      psp->is_k_constant = 0;
      psp->is_a_constant = 0;
    }
    else
      gsl_spline2d_free(fka);
  }

  free(y2d);
}


#include <math.h>
#include "2d_array.h"
#include "const.h"
#include "misc.h"
#include "utilities.h"
#include <gsl/gsl_multifit.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_fit.h>

/******************************************************************************
MODULE:  rmse_from_square_root_mean

PURPOSE:  simulate matlab calculate rmse from square root mean

RETURN VALUE:
Type = void
Value           Description
-----           -----------

HISTORY:
Date        Programmer       Reason
--------    ---------------  -------------------------------------
5/28/2019   Su Ye            Original Development

******************************************************************************/
void rmse_from_square_root_mean
(
    float **array,      /* I: input array */
    float fit_cft,      /* I: input fit_cft value */
    int dim1_index,     /* I: dimension 1 index in input array */
    int dim2_len,       /* I: dimension 2 length */
    float *rmse         /* O: output rmse */
)
{
    int i;
    float sum = 0.0;

    for (i = 0; i < dim2_len; i++)
    {
         sum += (array[dim1_index][i] - fit_cft) *
                (array[dim1_index][i] - fit_cft);
    }
    *rmse = sqrt(sum / dim2_len);
}

/******************************************************************************
MODULE:  dofit

PURPOSE: Declare data type and allocate memory and do multiple linear robust
         fit used for auto_robust_fit

RETURN VALUE: None

HISTORY:
Date        Programmer       Reason
--------    ---------------  -------------------------------------
5/28/2019   Su Ye            Original Development

NOTES:
******************************************************************************/
void dofit(const gsl_multifit_robust_type *T,
      const gsl_matrix *X, const gsl_vector *y,
      gsl_vector *c, gsl_matrix *cov)
{
  gsl_multifit_robust_workspace * work
    = gsl_multifit_robust_alloc (T, X->size1, X->size2);
  gsl_multifit_robust (X, y, c, cov, work);
  gsl_multifit_robust_free (work);
  work = NULL;
}

void dofit_linear(const gsl_matrix *X, const gsl_vector *y,
      gsl_vector *c, gsl_matrix *cov)
{
    double chisq;
    gsl_multifit_linear_workspace * work
        = gsl_multifit_linear_alloc ( X->size1, X->size2);
    gsl_multifit_linear(X, y, c, cov, &chisq,work);
    gsl_multifit_linear_free (work);
    // work = NULL; // SY 03242019
}

/******************************************************************************
MODULE:  auto_robust_fit

PURPOSE:  Robust fit for one band

RETURN VALUE: None

HISTORY:
Date        Programmer       Reason
--------    ---------------  -------------------------------------
5/28/2019   Su Ye            Original Development

NOTES:
******************************************************************************/
void auto_robust_fit
(
    float **clrx,
    float **clry,
    int nums,
    int start,
    int band_index,
    float *coefs
)
{
    int i, j;
    const int p = 2; /* linear fit */
    gsl_matrix *x, *cov;
    gsl_vector *y, *c;

    /******************************************************************/
    /*                                                                */
    /* Defines the inputs/outputs for robust fitting                  */
    /*                                                                */
    /******************************************************************/

    x = gsl_matrix_alloc (nums, p);
    y = gsl_vector_alloc (nums);

    c = gsl_vector_alloc (p);
    cov = gsl_matrix_alloc (p, p);

    /******************************************************************/
    /*                                                                */
    /* construct design matrix x for linear fit                       */
    /*                                                                */
    /******************************************************************/

    for (i = 0; i < nums; ++i)
    {
        for (j = 0; j < p; j++)
        {
            if (j == 0)
            {
                gsl_matrix_set (x, i, j, 1.0);
            }
            else
            {
                gsl_matrix_set (x, i, j, (double)clrx[i][j-1]);
            }
        }
        gsl_vector_set(y,i,(double)clry[band_index][i+start]);
    }

    /******************************************************************/
    /*                                                                */
    /* perform robust fit                                             */
    /*                                                                */
    /******************************************************************/

    dofit(gsl_multifit_robust_bisquare, x, y, c, cov);

    for (j = 0; j < (int)c->size; j++)
    {
        coefs[j] = gsl_vector_get(c, j);
    }

    /******************************************************************/
    /*                                                                */
    /* Free the memories                                              */
    /*                                                                */
    /******************************************************************/

    gsl_matrix_free (x);
    x = NULL;
    gsl_vector_free (y);
    y = NULL;
    gsl_vector_free (c);
    c = NULL;
    gsl_matrix_free (cov);
    cov = NULL;
}

/******************************************************************************
MODULE:  auto_mask

PURPOSE:  Multitemporal cloud, cloud shadow, & snow masks (global version)

RETURN VALUE:
Type = int
ERROR error out due to memory allocation
SUCCESS no error encounted

HISTORY:
Date        Programmer       Reason
--------    ---------------  -------------------------------------
05282019    Su Ye

NOTES:
******************************************************************************/
int auto_mask
(
    int *clrx,
    float **clry,
    int start,
    int end,
    float years,
    float t_b1,
    float t_b2,
    float n_t,
    int *bl_ids
)
{
    char FUNC_NAME[] = "auto_mask";
    int i;
    float **x;
    float pred_b2, pred_b4;
    int nums;
    float coefs[ROBUST_COEFFS];
    float coefs2[ROBUST_COEFFS];

    nums = end - start + 1;
    /* Allocate memory */
    x = (float **)allocate_2d_array(nums, ROBUST_COEFFS - 1, sizeof(float));
    if (x == NULL)
    {
        RETURN_ERROR("ERROR allocating x memory", FUNC_NAME, ERROR);
    }

    for (i = 0; i < nums; i++)
    {
        x[i][0] = (float)clrx[i+start];
    }

    /******************************************************************/
    /*                                                                */
    /* Do robust fitting for band 2 */
    /*                                                                */
    /******************************************************************/

    auto_robust_fit(x, clry, nums, start, 1, coefs);

    /******************************************************************/
    /*                                                                */
    /* Do robust fitting for band 4 */
    /*                                                                */
    /******************************************************************/

    auto_robust_fit(x, clry, nums, start, 3, coefs2);

    /******************************************************************/
    /*                                                                */
    /* predict band 2 and band 4 refs, bl_ids value of 0 is clear and   */
    /* 1 otherwise                                                    */
    /*                                                                */
    /******************************************************************/

    for (i = 0; i < nums; i++)
    {
        pred_b2 = coefs[0] + coefs[1] * (float)clrx[i+start];
        pred_b4 = coefs2[0] + coefs2[1] * (float)clrx[i+start];
        if (((clry[1][i+start]-pred_b2) > (n_t * t_b1)) ||
            ((clry[3][i+start]-pred_b4) < -(n_t * t_b2)))
        {
            bl_ids[i] = 1;
        }
        else
        {
            bl_ids[i] = 0;
        }
    }

    /* Free allocated memory */
    if (free_2d_array ((void **) x) != SUCCESS)
    {
        RETURN_ERROR ("Freeing memory: x\n", FUNC_NAME, ERROR);
    }

    return (SUCCESS);
}

/******************************************************************************
MODULE:  adjust_median_variogram

PURPOSE:  calculate absolute variogram for auto_mask

RETURN VALUE:
Type = void
Value           Description
-----           -----------

HISTORY:
Date        Programmer       Reason
--------    ---------------  -------------------------------------
28/05/2019   Su Ye         Original Development

NOTES:
******************************************************************************/
int adjust_median_variogram
(
    int *clrx,                  /* I: dates                                          */
    float **array,              /* I: input array                                    */
    int dim1_len,               /* I: dimension 1 length in input array              */
    int dim2_start,             /* I: dimension 2 start index                        */
    int dim2_end,               /* I: dimension 2 end index                          */
    float *date_vario,          /* O: outputted median variogran for dates           */
    float *max_neighdate_diff,  /* O: maximum difference for two neighbor times       */
    float *output_array        /* O: output array                                   */
)
{
    int i, j;           /* loop indecies                                     */
    float *var;         /* pointer for allocation variable memory            */
    int dim2_len = dim2_end - dim2_start + 1; /* perhaps should get defined  */
    char FUNC_NAME[] = "adjust_median_variogram"; /* for error messages             */
    int max_freq;
    int m;

//    for (i = 0; i < dim2_len; i++)
//    {
//        for (j = 0; j < TOTAL_IMAGE_BANDS; j++)
//        {
//           printf("%f\n", (float)array[j][i]);
//        }

//    }

    if (dim2_len == 1)
    {
        for (i = 0; i < dim1_len; i++)
        {
            output_array[i] = array[i][dim2_start];
            *date_vario = clrx[dim2_start];
            return (SUCCESS);
        }
    }

    var = malloc((dim2_len-1) * sizeof(float));
    if (var == NULL)
    {
        RETURN_ERROR ("Allocating var memory", FUNC_NAME, ERROR);
    }

    for (j = dim2_start; j < dim2_end; j++)
    {
        var[j] = abs(clrx[j+1] - clrx[j]);
    }
    quick_sort_float(var, dim2_start, dim2_end-1);
    m = (dim2_len-1) / 2;
    if ((dim2_len-1) % 2 == 0)
    {

        *date_vario = (var[m-1] + var[m]) / 2.0;
    }
    else
        *date_vario = var[m];

    *max_neighdate_diff = var[dim2_len-2];

    for (i = 0; i < dim1_len; i++)
    {
        for (j = dim2_start; j < dim2_end; j++)
        {
            var[j] = abs(array[i][j+1] - array[i][j]);
            //printf("%d var for band %d: %f\n", j, i+1, (float)var[j]);

        }
        quick_sort_float(var, dim2_start, dim2_end-1);
//            for (j = 0; j < dim2_end; j++)
//            {
//               printf("%f\n", var[j]);
//            }
        m = (dim2_len-1) / 2;
        if ((dim2_len-1) % 2 == 0)
        {
            //printf("%f\n", var[m-1]);
           //printf("%f\n", var[m]);
            output_array[i] = (var[m-1] + var[m]) / 2.0;
        }
        else
            output_array[i] = var[m];

    }

    free(var);

    return (SUCCESS);
}

/******************************************************************************
 *
MODULE:  single_median_variogram
PURPOSE:  calculate absolute variogram for a single band

RETURN VALUE:
Type = void
Value           Description
-----           -----------

HISTORY:
Date        Programmer       Reason
--------    ---------------  -------------------------------------
22/06/2019   Su Ye         Original Development

NOTES:
******************************************************************************/
int single_median_variogram
(
    short int *array,              /* I: input array                                    */
    int i_start,
    int i_end,
    short int *variogram,
    short int *mediam_value          /* O: outputted mediam value                         */
)
{
    int j, m;           /* loop indecies                                     */
    char FUNC_NAME[] = "single_median_variogram"; /* for error messages             */
    short int *var;         /* pointer for allocation variable memory            */
    int obs_num;
    int var_count = 0;
    short int *array_cpy;

    obs_num = i_end - i_start + 1;

//    for (j = 0; j < obs_num; j++)
//    {
//        printf("%i\n", array[j]);

//    }

    if (obs_num == 1)
    {
        *variogram = 0;
        *mediam_value = array[0];
        return (SUCCESS);

    }

    var = malloc((obs_num-1) * sizeof(short int));
    if (var == NULL)
    {
        RETURN_ERROR ("Allocating var memory", FUNC_NAME, ERROR);
    }

    array_cpy = malloc(obs_num * sizeof(short int));
    if (array_cpy == NULL)
    {
        RETURN_ERROR ("Allocating array_cpy memory", FUNC_NAME, ERROR);
    }

    for (j = i_start; j < i_end; j++)
    {
        var[j] = (short int)abs(array[j+1] - array[j]);
        //printf("%d var for band %d: %f\n", j, i+1, (float)var[j]);

    }

    for (j = i_start; j < i_end+1; j++)
    {
        array_cpy[j] = array[j];
        //printf("%d var for band %d: %f\n", j, i+1, (float)var[j]);

    }
    quick_sort_shortint(var, 0, obs_num-2);
//            for (j = 0; j < obs_num-1; j++)
//            {
//               printf("%d\n", var[j]);
//            }

    /* compute variogram */
    m = (obs_num-1) / 2;
    if ((obs_num-1) % 2 == 0)
    {
        *variogram = (short int) ((var[m-1] + var[m]) / 2.0);
    }
    else
        *variogram = var[m];

    /* compute mediam value */
    quick_sort_shortint(array_cpy, i_start, i_end);
//    for (j = 0; j < obs_num; j++)
//    {
//       printf("%d\n", array_cpy[j]);
//    }

    m = obs_num / 2;
    if (obs_num % 2 == 0)
    {
        *mediam_value = (short int)((array_cpy[m-1] + array_cpy[m]) / 2.0);
    }
    else
        *mediam_value = array_cpy[m];

    free(var);
    free(array_cpy);

    return (SUCCESS);
}

/******************************************************************************
 *
MODULE:  single_median_variogram
PURPOSE:  calculate absolute variogram for a single band

RETURN VALUE:
Type = void
Value           Description
-----           -----------

HISTORY:
Date        Programmer       Reason
--------    ---------------  -------------------------------------
22/06/2019   Su Ye         Original Development

NOTES:
******************************************************************************/
int single_median_quantile
(
    short int *array,              /* I: input array                                    */
    int i_start,
    int i_end,
    short int *quantile,          /* O: outputted quantile value                         */
    short int *mediam_value
)
{
    int j, m;           /* loop indecies                                     */
    char FUNC_NAME[] = "single_median_quantile"; /* for error messages             */
    short int *var;         /* pointer for allocation variable memory            */
    int obs_num;
    int var_count = 0;
    short int *array_cpy;

    obs_num = i_end - i_start + 1;

//    for (j = 0; j < obs_num; j++)
//    {
//        printf("%i\n", array[j]);

//    }

    if (obs_num == 1)
    {
        *mediam_value = array[0];
        *quantile = array[0];
        return (SUCCESS);

    }

    var = malloc((obs_num-1) * sizeof(short int));
    if (var == NULL)
    {
        RETURN_ERROR ("Allocating var memory", FUNC_NAME, ERROR);
    }

    array_cpy = malloc(obs_num * sizeof(short int));
    if (array_cpy == NULL)
    {
        RETURN_ERROR ("Allocating array_cpy memory", FUNC_NAME, ERROR);
    }

    for (j = i_start; j < i_end+1; j++)
    {
        array_cpy[j] = array[j];
        //printf("%d var for band %d: %f\n", j, i+1, (float)var[j]);

    }

    /* compute mediam value */
    quick_sort_shortint(array_cpy, i_start, i_end);
//    for (j = 0; j < obs_num; j++)
//    {
//       printf("%d\n", array_cpy[j]);
//    }

    m = obs_num / 2;
    if (obs_num % 2 == 0)
    {
        *mediam_value = (short int)((array_cpy[m-1] + array_cpy[m]) / 2.0);
    }
    else
        *mediam_value = array_cpy[m];

    *quantile = array_cpy[obs_num / 4];

    free(var);
    free(array_cpy);

    return (SUCCESS);
}
/******************************************************************************
 *
MODULE:  single_mean_rmse
PURPOSE:  calculate absolute variogram for a single band

RETURN VALUE:
Type = void
Value           Description
-----           -----------

HISTORY:
Date        Programmer       Reason
--------    ---------------  -------------------------------------
22/06/2019   Su Ye         Original Development

NOTES:
******************************************************************************/
int single_mean_rmse
(
    short int *array,              /* I: input array                                    */
    int i_start,
    int i_end,
    float *rmse,
    float *mean          /* O: outputted mediam value                         */
)
{
    int j, m;           /* loop indecies                                     */
    char FUNC_NAME[] = "single_mean_rse"; /* for error messages             */
    short int *var;         /* pointer for allocation variable memory            */
    int obs_num;
    int sum = 0;
    int rmse_sum = 0;

    obs_num = i_end - i_start + 1;

//    for (j = 0; j < obs_num; j++)
//    {
//        printf("%i\n", array[j]);

//    }

    if (obs_num == 1)
    {
        *rmse = 0;
        *mean = array[0];
        return (SUCCESS);

    }

    var = malloc((obs_num-1) * sizeof(short int));
    if (var == NULL)
    {
        RETURN_ERROR ("Allocating var memory", FUNC_NAME, ERROR);
    }

    for (j = i_start; j < i_end + 1; j++)
    {
        sum = sum + (int)array[j];
    }

    *mean = (float)((float)sum / (float)obs_num);

    for (j = i_start; j < i_end + 1; j++)
    {
        rmse_sum = rmse_sum + (array[j] - *mean) * (array[j] - *mean);
    }

    *rmse = (float)sqrt((double)rmse_sum/(double)obs_num);
    return (SUCCESS);
}

/******************************************************************************
MODULE:  linear_fit_centerdate

PURPOSE:  Robust fit for one band

RETURN VALUE: None

HISTORY:
Date        Programmer       Reason
--------    ---------------  -------------------------------------
5/31/2019   Su Ye            Original Development

NOTES:
******************************************************************************/
void linear_fit_centerdate
(
    int *clrx,
    float **clry,
    int nums,
    int start,
    int center_date,
    int i_col,
    short int **composites,
    int bweighted,
    float *C0,
    float *C1
)
{
    char FUNC_NAME[] = "linear_fit_centerdate";
    double c0;
    double c1;
    double cov00;
    double cov01;
    double cov11;
    double sumsq;
    int i;
    double* x;
    double* y_b1;
    double* y_b2;
    double* y_b3;
    double* y_b4;
    double* w;
    float coefs[ROBUST_COEFFS];
    float** x_t;

    x = (double*)malloc(nums * sizeof(double));
    if (x == NULL)
    {
        RETURN_ERROR("ERROR allocating x memory", FUNC_NAME, FAILURE);
    }

    y_b1 = (double*)malloc(nums * sizeof(double));
    if (y_b1 == NULL)
    {
        RETURN_ERROR("ERROR allocating y_b1 memory", FUNC_NAME, FAILURE);
    }

    y_b2 = (double*)malloc(nums * sizeof(double));
    if (y_b2 == NULL)
    {
        RETURN_ERROR("ERROR allocating y_b2 memory", FUNC_NAME, FAILURE);
    }

    y_b3 = (double*)malloc(nums * sizeof(double));
    if (y_b3 == NULL)
    {
        RETURN_ERROR("ERROR allocating y_b3 memory", FUNC_NAME, FAILURE);
    }

    y_b4 = (double*)malloc(nums * sizeof(double));
    if (y_b4 == NULL)
    {
        RETURN_ERROR("ERROR allocating y_b4 memory", FUNC_NAME, FAILURE);
    }

    w = (double*)malloc(nums * sizeof(double));
    if (w == NULL)
    {
        RETURN_ERROR("ERROR allocating y_b4 memory", FUNC_NAME, FAILURE);
    }

    x_t = (float **)allocate_2d_array(nums, ROBUST_COEFFS - 1, sizeof(float));
    if (x_t == NULL)
    {
        RETURN_ERROR("ERROR allocating x_t memory", FUNC_NAME, ERROR);
    }

    for (i = 0; i < nums; i++)
    {
        x_t[i][0] = (float)clrx[i+start];
    }

    /******************************************************************/
    /*                                                                */
    /* Defines the inputs/outputs for robust fitting                  */
    /*                                                                */
    /******************************************************************/

    /******************************************************************/
    /*                                                                */
    /* construct design matrix x for linear fit                       */
    /*                                                                */
    /******************************************************************/

    for (i = 0; i < nums; ++i)
    {
        x[i] = (double)clrx[i];
        y_b1[i] = (double)clry[0][i];
        y_b2[i] = (double)clry[1][i];
        y_b3[i] = (double)clry[2][i];
        y_b4[i] = (double)clry[3][i];

    }

    /******************************************************************/
    /*                                                                */
    /* perform robust fit                                             */
    /*                                                                */
    /******************************************************************/

    if (bweighted == TRUE)
    {
        /* create hot weights */
        for (i = 0; i < nums; ++i)
        {
             w[i] = (double)1/(abs(y_b1[i] - 0.5 * y_b3[i]));
        }

        gsl_fit_wlinear (x, 1, w, 1, y_b1, 1, nums, &c0, &c1, &cov00, &cov01, &cov11, &sumsq);
        composites[0][i_col] = (short int)(c0 + c1 * center_date);
        C0[0] = (float)c0;
        C1[0] = (float)c1;

        gsl_fit_wlinear (x, 1, w, 1, y_b2, 1, nums, &c0, &c1, &cov00, &cov01, &cov11, &sumsq);
        composites[1][i_col] = (short int)(c0 + c1 * center_date);
        C0[1] = (float)c0;
        C1[1] = (float)c1;

        gsl_fit_wlinear (x, 1, w, 1, y_b3, 1, nums, &c0, &c1, &cov00, &cov01, &cov11, &sumsq);
        composites[2][i_col] = (short int)(c0 + c1 * center_date);
        C0[2] = (float)c0;
        C1[2] = (float)c1;

        gsl_fit_wlinear (x, 1, w, 1, y_b4, 1, nums, &c0, &c1, &cov00, &cov01, &cov11, &sumsq);
        composites[3][i_col] = (short int)(c0 + c1 * center_date);
        C0[3] = (float)c0;
        C1[3] = (float)c1;
    }
    else
    {
        /* OLS*/
//        gsl_fit_linear (x, 1, y_b1, 1, nums, &c0, &c1, &cov00, &cov01, &cov11, &sumsq);
//        composites[0][i_col] = (short int)(c0 + c1 * center_date);
//        C0[0] = (float)c0;
//        C1[0] = (float)c1;

//        gsl_fit_linear (x, 1, y_b2, 1, nums, &c0, &c1, &cov00, &cov01, &cov11, &sumsq);
//        composites[1][i_col] = (short int)(c0 + c1 * center_date);
//        C0[1] = (float)c0;
//        C1[1] = (float)c1;

//        gsl_fit_linear (x, 1, y_b3, 1, nums, &c0, &c1, &cov00, &cov01, &cov11, &sumsq);
//        composites[2][i_col] = (short int)(c0 + c1 * center_date);
//        C0[2] = (float)c0;
//        C1[2] = (float)c1;

//        gsl_fit_linear (x, 1, y_b4, 1, nums, &c0, &c1, &cov00, &cov01, &cov11, &sumsq);
//        composites[3][i_col] = (short int)(c0 + c1 * center_date);
//        C0[3] = (float)c0;
//        C1[3] = (float)c1;
        /* robust regression */
        for(i = 0; i < TOTAL_IMAGE_BANDS; i++)
        {
            auto_robust_fit(x_t, clry, nums, start, i, coefs);
            composites[i][i_col] = (short int)(coefs[0] + coefs[1] * center_date);
            C0[i] = (float)coefs[0];
            C1[i] = (float)coefs[1];

        }
    }

    /******************************************************************/
    /*                                                                */
    /* Free the memories                                              */
    /*                                                                */
    /******************************************************************/

    free (x);
    free (y_b1);
    free (y_b2);
    free (y_b3);
    free (y_b4);
    free (w);
    /* Free allocated memory */
    if (free_2d_array ((void **) x_t) != SUCCESS)
    {
        RETURN_ERROR ("Freeing memory: x_t\n", FUNC_NAME, ERROR);
    }

}

/******************************************************************************
MODULE:  median_filter

PURPOSE:  calculate mediam filtering for three buf scanline

RETURN VALUE: None

HISTORY:
Date        Programmer       Reason
--------    ---------------  -------------------------------------
6/12/2019   Su Ye            Original Development

NOTES:
******************************************************************************/
int median_filter
(
    short int **buf1,
    short int **buf2,
    short int **buf3,
    short int **buf,
    int *valid_datecount_scanline,       /* I: the number of valid dates               */
    int num_scenes,
    int n_cols
)
{
    int i, b, j;
    short int window[9];

    for(i = 0; i < n_cols; i++)
    {
        for(j = 0; j < valid_datecount_scanline[i]; j++)
        {
            if(i==0)
            {
                for(b = 0; b < TOTAL_IMAGE_BANDS; b++)
                {
                    window[0] = buf1[b][i * num_scenes + j];
                    window[1] = buf1[b][i * num_scenes + j];
                    window[2] = buf1[b][(i+1) * num_scenes + j];
                    window[3] = buf2[b][i * num_scenes + j];
                    window[4] = buf2[b][i * num_scenes + j];
                    window[5] = buf2[b][(i+1) * num_scenes + j];
                    window[6] = buf3[b][i * num_scenes + j];
                    window[7] = buf3[b][i * num_scenes + j];
                    window[8] = buf3[b][(i+1) * num_scenes + j];

                    quick_sort_shortint(window, 0, 8);

                    buf[b][i * num_scenes + j] = window[4];

                }
            }
            else if (i == n_cols - 1)
            {
                for(b = 0; b < TOTAL_IMAGE_BANDS; b++)
                {
                    window[0] = buf1[b][(i-1) * num_scenes + j];
                    window[1] = buf1[b][i * num_scenes + j];
                    window[2] = buf1[b][i * num_scenes + j];
                    window[3] = buf2[b][(i-1) * num_scenes + j];
                    window[4] = buf2[b][i * num_scenes + j];
                    window[5] = buf2[b][i * num_scenes + j];
                    window[6] = buf3[b][(i-1) * num_scenes + j];
                    window[7] = buf3[b][i * num_scenes + j];
                    window[8] = buf3[b][i * num_scenes + j];

                    quick_sort_shortint(window, 0, 8);

                    buf[b][i * num_scenes + j] = window[4];

                }
            }
            else
            {
                for(b = 0; b < TOTAL_IMAGE_BANDS; b++)
                {
                    window[0] = buf1[b][(i-1) * num_scenes + j];
                    window[1] = buf1[b][i * num_scenes + j];
                    window[2] = buf1[b][(i+1) * num_scenes + j];
                    window[3] = buf2[b][(i-1) * num_scenes + j];
                    window[4] = buf2[b][i * num_scenes + j];
                    window[5] = buf2[b][(i+1) * num_scenes + j];
                    window[6] = buf3[b][(i-1) * num_scenes + j];
                    window[7] = buf3[b][i * num_scenes + j];
                    window[8] = buf3[b][(i+1) * num_scenes + j];

                    quick_sort_shortint(window, 0, 8);

                    buf[b][i * num_scenes + j] = window[4];

                }
            }

        }

    }

}


#include "utilities.h"
#include "compute_signal.h"
#include <gsl/gsl_rng.h>
#include <gsl/gsl_histogram.h>
#include <fftw3.h>
#include <stdlib.h>
#include <stdio.h>

#include "parameters.h"

int main(void){
  gsl_rng * r = init_rng();

  double * signal, * spectrum;
  allocate_arrays(&signal, &spectrum, LENGTH);

  gsl_histogram * hist = init_histogram(HIST_MIN, HIST_MAX, BIN_N);

  load_wisdom("wisdom");

  fftw_plan plan = make_plan(signal, LENGTH);

  for(size_t i = 0; i < COUNT; i++){
    printf("---- pass %zu ----\n", i);

    compute_signal(signal, LENGTH, DELTA, r, hist);
    if(i==0) output_signal("signal.dat", signal, LENGTH, DELTA, N_POINTS);

    remove_mean(signal, LENGTH);
    make_fft(plan);
    compute_spectrum(signal, spectrum, LENGTH);

    puts("Done.");
  }

  final_spectrum(spectrum, LENGTH, COUNT, DELTA, SMOOTH_NEIGHBORS);

  output_spectrum("spectrum.dat", spectrum, LENGTH, DELTA, N_POINTS);
  output_distribution("distribution.dat", hist);

  cleanup(signal, spectrum, plan, r, hist);
  return EXIT_SUCCESS;
}