/* ode-initval2/rk1imp.c
 *
 * Copyright (C) 2009, 2010 Tuomo Keskitalo
 *
 * 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.
 */

/* Implicit Euler a.k.a backward Euler method. */

/* Reference: Ascher, U.M., Petzold, L.R., Computer methods for
   ordinary differential and differential-algebraic equations, SIAM,
   Philadelphia, 1998.
*/

#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_odeiv2.h>

#include "odeiv_util.h"
#include "rksubs.c"
#include "modnewton1.c"

/* Stage of method */
#define RK1IMP_STAGE 1

typedef struct
{
  gsl_matrix *A;                /* Runge-Kutta coefficients */
  double *y_onestep;            /* Result with one step */
  double *y_twostep;            /* Result with two half steps */
  double *ytmp;                 /* Temporary work space */
  double *y_save;               /* Backup space */
  double *YZ;                   /* Runge-Kutta points */
  double *fYZ;                  /* Derivatives at YZ */
  gsl_matrix *dfdy;             /* Jacobian matrix */
  double *dfdt;                 /* time derivative of f */
  modnewton1_state_t *esol;     /* nonlinear equation solver */
  double *errlev;               /* desired error level of y */
  const gsl_odeiv2_driver *driver;      /* pointer to driver object */
}
rk1imp_state_t;

static void *
rk1imp_alloc (size_t dim)
{
  rk1imp_state_t *state = (rk1imp_state_t *) malloc (sizeof (rk1imp_state_t));

  if (state == 0)
    {
      GSL_ERROR_NULL ("failed to allocate space for rk1imp_state",
                      GSL_ENOMEM);
    }

  state->A = gsl_matrix_alloc (RK1IMP_STAGE, RK1IMP_STAGE);

  if (state->A == 0)
    {
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for A", GSL_ENOMEM);
    }

  state->y_onestep = (double *) malloc (dim * sizeof (double));

  if (state->y_onestep == 0)
    {
      gsl_matrix_free (state->A);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for y_onestep", GSL_ENOMEM);
    }

  state->y_twostep = (double *) malloc (dim * sizeof (double));

  if (state->y_twostep == 0)
    {
      free (state->y_onestep);
      gsl_matrix_free (state->A);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for y_onestep", GSL_ENOMEM);
    }

  state->ytmp = (double *) malloc (dim * sizeof (double));

  if (state->ytmp == 0)
    {
      free (state->y_twostep);
      free (state->y_onestep);
      gsl_matrix_free (state->A);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for ytmp", GSL_ENOMEM);
    }

  state->y_save = (double *) malloc (dim * sizeof (double));

  if (state->y_save == 0)
    {
      free (state->ytmp);
      free (state->y_twostep);
      free (state->y_onestep);
      gsl_matrix_free (state->A);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for y_save", GSL_ENOMEM);
    }

  state->YZ = (double *) malloc (dim * RK1IMP_STAGE * sizeof (double));

  if (state->YZ == 0)
    {
      free (state->y_save);
      free (state->ytmp);
      free (state->y_twostep);
      free (state->y_onestep);
      gsl_matrix_free (state->A);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for YZ", GSL_ENOMEM);
    }

  state->fYZ = (double *) malloc (dim * RK1IMP_STAGE * sizeof (double));

  if (state->fYZ == 0)
    {
      free (state->YZ);
      free (state->y_save);
      free (state->ytmp);
      free (state->y_twostep);
      free (state->y_onestep);
      gsl_matrix_free (state->A);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for fYZ", GSL_ENOMEM);
    }

  state->dfdt = (double *) malloc (dim * sizeof (double));

  if (state->dfdt == 0)
    {
      free (state->fYZ);
      free (state->YZ);
      free (state->y_save);
      free (state->ytmp);
      free (state->y_twostep);
      free (state->y_onestep);
      gsl_matrix_free (state->A);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for dfdt", GSL_ENOMEM);
    }

  state->dfdy = gsl_matrix_alloc (dim, dim);

  if (state->dfdy == 0)
    {
      free (state->dfdt);
      free (state->fYZ);
      free (state->YZ);
      free (state->y_save);
      free (state->ytmp);
      free (state->y_twostep);
      free (state->y_onestep);
      gsl_matrix_free (state->A);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for dfdy", GSL_ENOMEM);
    }

  state->esol = modnewton1_alloc (dim, RK1IMP_STAGE);

  if (state->esol == 0)
    {
      gsl_matrix_free (state->dfdy);
      free (state->dfdt);
      free (state->fYZ);
      free (state->YZ);
      free (state->y_save);
      free (state->ytmp);
      free (state->y_twostep);
      free (state->y_onestep);
      gsl_matrix_free (state->A);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for esol", GSL_ENOMEM);
    }

  state->errlev = (double *) malloc (dim * sizeof (double));

  if (state->errlev == 0)
    {
      modnewton1_free (state->esol);
      gsl_matrix_free (state->dfdy);
      free (state->dfdt);
      free (state->fYZ);
      free (state->YZ);
      free (state->y_save);
      free (state->ytmp);
      free (state->y_twostep);
      free (state->y_onestep);
      gsl_matrix_free (state->A);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for errlev", GSL_ENOMEM);
    }

  state->driver = NULL;

  return state;
}

static int
rk1imp_apply (void *vstate, size_t dim, double t, double h,
              double y[], double yerr[],
              const double dydt_in[], double dydt_out[],
              const gsl_odeiv2_system * sys)
{
  /* Makes an implicit Euler step with size h and estimates the local
     error of the step by step doubling.
   */

  rk1imp_state_t *state = (rk1imp_state_t *) vstate;

  double *const y_onestep = state->y_onestep;
  double *const y_twostep = state->y_twostep;
  double *const ytmp = state->ytmp;
  double *const y_save = state->y_save;
  double *const YZ = state->YZ;
  double *const fYZ = state->fYZ;
  gsl_matrix *const dfdy = state->dfdy;
  double *const dfdt = state->dfdt;
  double *const errlev = state->errlev;

  const modnewton1_state_t *esol = state->esol;

  /* Runge-Kutta coefficients */

  gsl_matrix *A = state->A;
  const double b[] = { 1.0 };
  const double c[] = { 1.0 };
  gsl_matrix_set (A, 0, 0, 1.0);

  if (esol == NULL)
    {
      GSL_ERROR ("no non-linear equation solver speficied", GSL_EINVAL);
    }

  /* Get desired error levels via gsl_odeiv2_control object through driver
     object, which is a requirement for this stepper.
   */

  if (state->driver == NULL)
    {
      return GSL_EFAULT;
    }
  else
    {
      size_t i;

      for (i = 0; i < dim; i++)
        {
          if (dydt_in != NULL)
            {
              gsl_odeiv2_control_errlevel (state->driver->c, y[i],
                                           dydt_in[i], h, i, &errlev[i]);
            }
          else
            {
              gsl_odeiv2_control_errlevel (state->driver->c, y[i],
                                           0.0, h, i, &errlev[i]);
            }
        }
    }

  /* Evaluate Jacobian for modnewton1 */

  {
    int s = GSL_ODEIV_JA_EVAL (sys, t, y, dfdy->data, dfdt);

    if (s != GSL_SUCCESS)
      {
        return s;
      }
  }

  /* Calculate a single step with size h */

  {
    int s = modnewton1_init ((void *) esol, A, h, dfdy, sys);

    if (s != GSL_SUCCESS)
      {
        return s;
      }
  }

  {
    int s = modnewton1_solve ((void *) esol, A, c, t, h, y,
                              sys, YZ, errlev);

    if (s != GSL_SUCCESS)
      {
        return s;
      }
  }

  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + c[0] * h, YZ, fYZ);

    if (s != GSL_SUCCESS)
      {
        return s;
      }
  }

  {
    int s = rksubs (y_onestep, h, y, fYZ, b, RK1IMP_STAGE, dim);

    if (s != GSL_SUCCESS)
      return s;
  }

  /* Error estimation by step doubling */

  {
    int s = modnewton1_init ((void *) esol, A, h / 2.0, dfdy, sys);

    if (s != GSL_SUCCESS)
      {
        return s;
      }
  }

  /* 1st half step */

  {
    int s = modnewton1_solve ((void *) esol, A, c, t, h / 2.0, y,
                              sys, YZ, errlev);

    if (s != GSL_SUCCESS)
      {
        return s;
      }
  }

  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + c[0] * h / 2.0, YZ, fYZ);
    if (s != GSL_SUCCESS)
      {
        return s;
      }
  }

  {
    int s = rksubs (ytmp, h / 2.0, y, fYZ, b, RK1IMP_STAGE, dim);

    if (s != GSL_SUCCESS)
      return s;
  }

  /* Save original y values in case of error */

  DBL_MEMCPY (y_save, y, dim);

  /* 2nd half step */

  {
    int s = modnewton1_solve ((void *) esol, A, c, t + h / 2.0, h / 2.0,
                              ytmp, sys, YZ, errlev);

    if (s != GSL_SUCCESS)
      {
        return s;
      }
  }

  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + h / 2.0 + c[0] * h / 2.0, YZ, fYZ);
    if (s != GSL_SUCCESS)
      {
        return s;
      }
  }

  {
    /* Note: rk1imp returns y using the results from two half steps
       instead of the single step since the results are freely
       available and more precise.
     */

    int s = rksubs (y_twostep, h / 2.0, ytmp, fYZ, b, RK1IMP_STAGE, dim);

    if (s != GSL_SUCCESS)
      {
        DBL_MEMCPY (y, y_save, dim);
        return s;
      }
  }

  DBL_MEMCPY (y, y_twostep, dim);

  /* Error estimation */

  {
    size_t i;
    for (i = 0; i < dim; i++)
      {
        yerr[i] = ODEIV_ERR_SAFETY * 0.5 * fabs (y_twostep[i] - y_onestep[i]);
      }
  }

  /* Derivatives at output */

  if (dydt_out != NULL)
    {
      int s = GSL_ODEIV_FN_EVAL (sys, t + h, y, dydt_out);

      if (s != GSL_SUCCESS)
        {
          /* Restore original values */
          DBL_MEMCPY (y, y_save, dim);

          return s;
        }
    }

  return GSL_SUCCESS;
}

static int
rk1imp_set_driver (void *vstate, const gsl_odeiv2_driver * d)
{
  rk1imp_state_t *state = (rk1imp_state_t *) vstate;

  state->driver = d;

  return GSL_SUCCESS;
}

static int
rk1imp_reset (void *vstate, size_t dim)
{
  rk1imp_state_t *state = (rk1imp_state_t *) vstate;

  DBL_ZERO_MEMSET (state->y_onestep, dim);
  DBL_ZERO_MEMSET (state->y_twostep, dim);
  DBL_ZERO_MEMSET (state->ytmp, dim);
  DBL_ZERO_MEMSET (state->y_save, dim);
  DBL_ZERO_MEMSET (state->YZ, dim);
  DBL_ZERO_MEMSET (state->fYZ, dim);

  return GSL_SUCCESS;
}

static unsigned int
rk1imp_order (void *vstate)
{
  rk1imp_state_t *state = (rk1imp_state_t *) vstate;
  state = 0;                    /* prevent warnings about unused parameters */
  return 1;
}

static void
rk1imp_free (void *vstate)
{
  rk1imp_state_t *state = (rk1imp_state_t *) vstate;

  free (state->errlev);
  modnewton1_free (state->esol);
  gsl_matrix_free (state->dfdy);
  free (state->dfdt);
  free (state->fYZ);
  free (state->YZ);
  free (state->y_save);
  free (state->ytmp);
  free (state->y_twostep);
  free (state->y_onestep);
  gsl_matrix_free (state->A);
  free (state);
}

static const gsl_odeiv2_step_type rk1imp_type = {
  "rk1imp",                     /* name */
  1,                            /* can use dydt_in? */
  1,                            /* gives exact dydt_out? */
  &rk1imp_alloc,
  &rk1imp_apply,
  &rk1imp_set_driver,
  &rk1imp_reset,
  &rk1imp_order,
  &rk1imp_free
};

const gsl_odeiv2_step_type *gsl_odeiv2_step_rk1imp = &rk1imp_type;


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

#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>

/*
 * This module contains routines for estimating the condition number
 * of matrices in the 1-norm. The algorithm is based on the paper,
 *
 * [1] N. J. Higham, "FORTRAN codes for estimating the one-norm of
 * a real or complex matrix, with applications to condition estimation",
 * ACM Trans. Math. Soft., vol. 14, no. 4, pp. 381-396, December 1988.
 */

static double condest_tri_norm1(CBLAS_UPLO_t Uplo, const gsl_matrix * A);
static int condest_tri_rcond(CBLAS_UPLO_t Uplo, const gsl_matrix * A,
                             double * rcond, gsl_vector * work);
static int condest_same_sign(const gsl_vector * x, const gsl_vector * y);
static int condest_invtriu(CBLAS_TRANSPOSE_t TransA, gsl_vector * x, void * params);
static int condest_invtril(CBLAS_TRANSPOSE_t TransA, gsl_vector * x, void * params);

/*
gsl_linalg_tri_upper_rcond()
  Estimate reciprocal condition number of upper triangular matrix

Inputs: A     - upper triangular matrix, N-by-N
        rcond - (output) reciprocal condition number estimate
        work  - workspace, length 3*N

Return: success/error
*/

int
gsl_linalg_tri_upper_rcond(const gsl_matrix * A, double * rcond, gsl_vector * work)
{
  int status = condest_tri_rcond(CblasUpper, A, rcond, work);
  return status;
}

/*
gsl_linalg_tri_lower_rcond()
  Estimate reciprocal condition number of lower triangular matrix

Inputs: A     - lower triangular matrix, N-by-N
        rcond - (output) reciprocal condition number estimate
        work  - workspace, length 3*N

Return: success/error
*/

int
gsl_linalg_tri_lower_rcond(const gsl_matrix * A, double * rcond, gsl_vector * work)
{
  int status = condest_tri_rcond(CblasLower, A, rcond, work);
  return status;
}

/*
gsl_linalg_invnorm1()
  Estimate the 1-norm of ||A^{-1}||, where A is a square
N-by-N matrix

Inputs: N        - size of matrix
        Ainvx    - pointer to function which calculates:
                   x := A^{-1} x or x := A^{-t} x
        params   - parameters to pass to Ainvx
        Ainvnorm - (output) estimate of ||A^{-1}||_1
        work     - workspace, length 3*N
*/

int
gsl_linalg_invnorm1(const size_t N,
                    int (* Ainvx)(CBLAS_TRANSPOSE_t TransA, gsl_vector * x, void * params),
                    void * params, double * Ainvnorm, gsl_vector * work)
{
  if (work->size != 3 * N)
    {
      GSL_ERROR ("work vector must have length 3*N", GSL_EBADLEN);
    }
  else
    {
      const size_t maxit = 5;
      gsl_vector_view x = gsl_vector_subvector(work, 0, N);
      gsl_vector_view v = gsl_vector_subvector(work, N, N);
      gsl_vector_view xi = gsl_vector_subvector(work, 2*N, N);
      double gamma, gamma_old, temp;
      size_t i, k;

      for (i = 0; i < N; ++i)
        gsl_vector_set(&x.vector, i, 1.0 / (double) N);

      /* compute v = A^{-1} x */
      gsl_vector_memcpy(&v.vector, &x.vector);
      (*Ainvx)(CblasNoTrans, &v.vector, params);

      /* gamma = ||v||_1 */
      gamma = gsl_blas_dasum(&v.vector);

      /* xi = sign(v) */
      for (i = 0; i < N; ++i)
        {
          double vi = gsl_vector_get(&v.vector, i);
          gsl_vector_set(&xi.vector, i, GSL_SIGN(vi));
        }

      /* x = A^{-t} xi */
      gsl_vector_memcpy(&x.vector, &xi.vector);
      (*Ainvx)(CblasTrans, &x.vector, params);

      for (k = 0; k < maxit; ++k)
        {
          size_t j = (size_t) gsl_blas_idamax(&x.vector);

          /* v := A^{-1} e_j */
          gsl_vector_set_zero(&v.vector);
          gsl_vector_set(&v.vector, j, 1.0);
          (*Ainvx)(CblasNoTrans, &v.vector, params);

          gamma_old = gamma;
          gamma = gsl_blas_dasum(&v.vector);

          /* check for repeated sign vector (algorithm has converged) */
          if (condest_same_sign(&v.vector, &xi.vector) || (gamma < gamma_old))
            break;

          /* xi = sign(v) */
          for (i = 0; i < N; ++i)
            {
              double vi = gsl_vector_get(&v.vector, i);
              gsl_vector_set(&xi.vector, i, GSL_SIGN(vi));
            }

          /* x = A^{-t} sign(v) */
          gsl_vector_memcpy(&x.vector, &xi.vector);
          (*Ainvx)(CblasTrans, &x.vector, params);
        }

      temp = 1.0; /* (-1)^i */
      for (i = 0; i < N; ++i)
        {
          double term = 1.0 + (double) i / (N - 1.0);
          gsl_vector_set(&x.vector, i, temp * term);
          temp = -temp;
        }

      /* x := A^{-1} x */
      (*Ainvx)(CblasNoTrans, &x.vector, params);

      temp = 2.0 * gsl_blas_dasum(&x.vector) / (3.0 * N);
      if (temp > gamma)
        {
          gsl_vector_memcpy(&v.vector, &x.vector);
          gamma = temp;
        }

      *Ainvnorm = gamma;

      return GSL_SUCCESS;
    }
}

static int
condest_tri_rcond(CBLAS_UPLO_t Uplo, const gsl_matrix * A, double * rcond, gsl_vector * work)
{
  const size_t M = A->size1;
  const size_t N = A->size2;

  if (M != N)
    {
      GSL_ERROR ("matrix must be square", GSL_ENOTSQR);
    }
  else if (work->size != 3 * N)
    {
      GSL_ERROR ("work vector must have length 3*N", GSL_EBADLEN);
    }
  else
    {
      int status;
      double Anorm = condest_tri_norm1(Uplo, A); /* ||A||_1 */
      double Ainvnorm;                           /* ||A^{-1}||_1 */

      *rcond = 0.0;

      /* don't continue if matrix is singular */
      if (Anorm == 0.0)
        return GSL_SUCCESS;

      /* estimate ||A^{-1}||_1 */
      if (Uplo == CblasUpper)
        status = gsl_linalg_invnorm1(N, condest_invtriu, (void *) A, &Ainvnorm, work);
      else
        status = gsl_linalg_invnorm1(N, condest_invtril, (void *) A, &Ainvnorm, work);

      if (status)
        return status;

      if (Ainvnorm != 0.0)
        *rcond = (1.0 / Anorm) / Ainvnorm;

      return GSL_SUCCESS;
    }
}

/* calculate 1 norm of triangular matrix */
static double
condest_tri_norm1(CBLAS_UPLO_t Uplo, const gsl_matrix * A)
{
  const size_t N = A->size2;
  double max = 0.0;
  size_t i, j;

  if (Uplo == CblasUpper)
    {
      for (j = 0; j < N; ++j)
        {
          double sum = 0.0;
          for (i = 0; i <= j; ++i)
            {
              double Aij = gsl_matrix_get(A, i, j);
              sum += fabs(Aij);
            }

          max = GSL_MAX(max, sum);
        }
    }
  else
    {
      for (j = 0; j < N; ++j)
        {
          double sum = 0.0;
          for (i = j; i < N; ++i)
            {
              double Aij = gsl_matrix_get(A, i, j);
              sum += fabs(Aij);
            }

          max = GSL_MAX(max, sum);
        }
    }

  return max;
}

/* return 1 if sign(x) = sign(y), 0 otherwise */
static int
condest_same_sign(const gsl_vector * x, const gsl_vector * y)
{
  const size_t n = x->size;
  size_t i;

  for (i = 0; i < n; ++i)
    {
      double xi = gsl_vector_get(x, i);
      double yi = gsl_vector_get(y, i);
      if (GSL_SIGN(xi) != GSL_SIGN(yi))
        return 0;
    }

  return 1;
}

/* x := A^{-1} x, A upper triangular */
static int
condest_invtriu(CBLAS_TRANSPOSE_t TransA, gsl_vector * x, void * params)
{
  gsl_matrix * A = (gsl_matrix *) params;
  return gsl_blas_dtrsv(CblasUpper, TransA, CblasNonUnit, A, x);
}

/* x := A^{-1} x, A lower triangular */
static int
condest_invtril(CBLAS_TRANSPOSE_t TransA, gsl_vector * x, void * params)
{
  gsl_matrix * A = (gsl_matrix *) params;
  return gsl_blas_dtrsv(CblasLower, TransA, CblasNonUnit, A, x);
}


#include <gsl/gsl_multifit.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include "2d_array.h"
#include "const.h"
#include "input.h"
#include "utilities.h"
#include "misc.h"

/******************************************************************************
MODULE:  greenband_test

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 greenband_test
(
    int *clrx,
    float **clry,
    int start,
    int end,
    float rmse,
    float n_t,
    int *bl_ids,
    float *C0,
    float *C1
)
{
    char FUNC_NAME[] = "greenband_test";
    int i;
    float **x;
    float pred;
    int nums;
    float coefs[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);

    *C0 = coefs[0];
    *C1 = coefs[1];

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

    for (i = 0; i < nums; i++)
    {
        pred = coefs[0] + coefs[1] * (float)clrx[i+start];
        if (clry[1][i+start]-pred > (n_t * rmse))
        {
            // int testy = clry[1][i+start];
            // int testx = clrx[i+start];
            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);
    }
    x = NULL;

    return (SUCCESS);
}

/******************************************************************************
MODULE:  nirband_test

PURPOSE:  nirband_test used to filter out shadow

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

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

NOTES:
******************************************************************************/
int nirband_test
(
    int *clrx,
    float **clry,
    int start,
    int end,
    float rmse,
    float n_t,
    int *bl_ids,
    float *C0,
    float *C1
)
{
    char FUNC_NAME[] = "nirband_test";
    int i;
    float **x;
    float pred;
    int nums;
    float coefs[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 4 */
    /*                                                                */
    /******************************************************************/

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

    *C0 = coefs[0];
    *C1 = coefs[1];

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

    for (i = 0; i < nums; i++)
    {
        pred = coefs[0] + coefs[1] * (float)clrx[i+start];
        if (clry[3][i+start]-pred < -(n_t * rmse))
        {
            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);
    }
    x = NULL;

    return (SUCCESS);
}

/******************************************************************************
MODULE:  average_compositing

PURPOSE:  Running average compositing for pixel-based time series

RETURN VALUE:
Type = int (SUCCESS, ERROR or FAILURE)

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

int average_compositing
(
    short int **buf,            /* I:  pixel-based time series           */
    int *valid_date_array,      /* I: valid date time series               */
    int valid_date_count,       /* I: the number of valid dates               */
    int lower_ordinal,
    int upper_ordinal,
    int i_col,
    short int **out_compositing           /* O: outputted compositing results for four bands */
)
{
    int i, j;
    double index_sum[TOTAL_IMAGE_BANDS];
    int valid_count_window= 0;
    for(i = 0; i < TOTAL_IMAGE_BANDS; i++)
        index_sum[i] = 0;

    for(i = 0; i < valid_date_count; i++)
    {
        if((valid_date_array[i] > lower_ordinal - 1) && (valid_date_array[i] < upper_ordinal + 1))
        {
            for(j = 0; j < TOTAL_IMAGE_BANDS; j++)
            {
                index_sum[j] = index_sum[j] +  buf[j][i];
            }
            valid_count_window++;

        }

    }

    if(valid_count_window==0)
    {
        for(i = 0; i < TOTAL_IMAGE_BANDS; i++)
        {
            out_compositing[i][i_col] = -9999;
        }
        return SUCCESS;
    }

    for(j = 0; j < TOTAL_IMAGE_BANDS; j++)
    {
        out_compositing[j][i_col] = (short int)(index_sum[j] / valid_count_window);
    }
}

/******************************************************************************
MODULE:  average_compositing

PURPOSE:  Running average compositing for pixel-based time series

RETURN VALUE:
Type = int (SUCCESS, ERROR or FAILURE)

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

int median_compositing
(
    short int **buf,            /* I:  pixel-based time series           */
    int *valid_date_array,      /* I: valid date time series               */
    int valid_date_count,       /* I: the number of valid dates               */
    int i_col,
    short int **out_compositing           /* O: outputted compositing results for four bands */
)
{
    char FUNC_NAME[] = "median_compositing";
    short int *var;         /* pointer for allocation variable memory            */
    int i, j, m;

    if (valid_date_count == 1)
    {
        for (i = 0; i < TOTAL_IMAGE_BANDS; i++)
          out_compositing[i][i_col] = buf[i][0];
        return SUCCESS;
    }

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

    for (i = 0; i < TOTAL_IMAGE_BANDS; i++)
    {
        for (j = 0; j < valid_date_count; j++)
        {
            var[j] = buf[i][j];

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

    }

    free(var);
    return SUCCESS;

}

/******************************************************************************
MODULE:  hot_compositing

PURPOSE:  Running compositing for pixel-based time series

RETURN VALUE:
Type = int (SUCCESS, ERROR or FAILURE)

HISTORY:
Date        Programmer       Reason
--------    ---------------  -------------------------------------
05/02/2019   Su Ye         Original Development
******************************************************************************/
int hot_compositing
(
    short int **buf,            /* I:  pixel-based time series           */
    int *valid_date_array,      /* I: valid date time series               */
    int valid_date_count,       /* I: the number of valid dates               */
    int lower_ordinal,
    int upper_ordinal,
    int i_col,
    short int **out_compositing           /* O: outputted compositing results for four bands */
)
{
    int i, j;
    double wt;
    double index_sum[TOTAL_IMAGE_BANDS];
    for(i = 0; i < TOTAL_IMAGE_BANDS; i++)
        index_sum[i] = 0;
    double wt_sum = 0;
    int valid_count_window = 0;

    for(i = 0; i < valid_date_count; i++)
    {
        if((valid_date_array[i] > lower_ordinal - 1) && (valid_date_array[i] < upper_ordinal + 1))
        {
            wt = (double)1.0/((buf[BLUE_INDEX][i] - 0.5 * buf[RED_INDEX][i]) * (buf[BLUE_INDEX][i] - 0.5 * buf[RED_INDEX][i]));
            //wt = (float)1/(abs(buf[BLUE_INDEX][i] - 0.5 * buf[RED_INDEX][i]));
            for(j = 0; j < TOTAL_IMAGE_BANDS; j++)
            {
                index_sum[j] = index_sum[j] + buf[j][i] * wt;
            }
            wt_sum = wt_sum + wt;
            valid_count_window++;
        }
    }

    /********************************************/
    /*    condition 1: zero valid observation   */
    /********************************************/
    if(valid_count_window==0)
    {
        for(i = 0; i < TOTAL_IMAGE_BANDS; i++)
        {
            out_compositing[i][i_col] = -9999;
        }
        return SUCCESS;
    }

    /********************************************/
    /*    condition 2: standard procedures      */
    /********************************************/

    for(j = 0; j < TOTAL_IMAGE_BANDS; j++)
    {
        out_compositing[j][i_col] = (short int)(index_sum[j] / wt_sum);
        //out_compositing[j][i_col] = (short int)valid_count_window;
    }

    return SUCCESS;

}

/******************************************************************************
MODULE:  modified_hot_compositing

PURPOSE:  Running compositing for pixel-based time series by adding shadow consideration

RETURN VALUE:
Type = int (SUCCESS, ERROR or FAILURE)

HISTORY:
Date        Programmer       Reason
--------    ---------------  -------------------------------------
06/22/2019   Su Ye         Original Development
******************************************************************************/
int modified_hot_compositing
(
    short int **buf,            /* I:  pixel-based time series           */
    int *valid_date_array,      /* I: valid date time series               */
    int valid_date_count,       /* I: the number of valid dates               */
    int lower_ordinal,
    int upper_ordinal,
    int i_col,
    short int **out_compositing           /* O: outputted compositing results for four bands */
)
{
    int i, j;
    int status;
    double wt;
    double wt_shadow;
    double wt_cloud;
    double index_sum[TOTAL_IMAGE_BANDS];
    for(i = 0; i < TOTAL_IMAGE_BANDS; i++)
        index_sum[i] = 0;
    double wt_sum = 0;
    int valid_count_window = 0;
    short int** ts_subset;
    short int* ts_subset_selected_shadow;
    short int* ts_subset_selected_blue;
    short int variogram_shadow;
    short int medium_shadow;

    char FUNC_NAME[] = "modified_hot_compositing";

    ts_subset = (short int**)allocate_2d_array(TOTAL_IMAGE_BANDS, valid_date_count, sizeof(short int));
    if(ts_subset == NULL)
    {
        RETURN_ERROR ("Allocating ts_subset memory", FUNC_NAME, ERROR);
    }

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

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

    for(i = 0; i < valid_date_count; i++)
    {
        if((valid_date_array[i] > lower_ordinal - 1) && (valid_date_array[i] < upper_ordinal + 1))
        {
            for(j = 0; j < TOTAL_IMAGE_BANDS; j++)
            {
                ts_subset[j][valid_count_window] = buf[j][i];
                if(j == NIR_INDEX)
                {
                   //ts_subset_selected_shadow[valid_count_window] = (buf[BLUE_INDEX][i] + buf[RED_INDEX][i] + buf[GREEN_INDEX][i])/3;
		   ts_subset_selected_shadow[valid_count_window] = buf[NIR_INDEX][i];
                    //printf("%i\n", ts_subset_selected[valid_count_window]);
                }
                if(j == BLUE_INDEX)
                {
                    ts_subset_selected_blue[valid_count_window] = buf[BLUE_INDEX][i];
                    //printf("%i\n", ts_subset_selected[valid_count_window]);
                }
            }

            valid_count_window++;

        }
    }

    /********************************************/
    /*    condition 1: zero valid observation   */
    /********************************************/
    if(valid_count_window < 3)
    {
        for(i = 0; i < TOTAL_IMAGE_BANDS; i++)
        {
            out_compositing[i][i_col] = -9999;
        }
        return SUCCESS;
    }

    //single_mean_rmse(ts_subset_selected, 0, valid_count_window - 1, &rmse, &mean);
    //quick_sort_shortint(ts_subset_selected, 0, valid_count_window - 1);

    //m = valid_count_window / 2;

    //medium_shadow = (ts_subset_selected[m] + ts_subset_selected[m - 1] + ts_subset_selected[m + 1])/3;
    single_median_variogram(ts_subset_selected_shadow, 0, valid_count_window - 1, &variogram_shadow, &medium_shadow);
    //single_median_quantile(ts_subset_selected_blue, 0, valid_count_window - 1, &quantile_blue, &medium_blue);
    //single_median_variogram(ts_subset_selected_hot, 0, valid_count_window - 1, &variogram_hot, &medium_hot);
    //penalty_slope = - 9.0 / (1000.0 * variogram);
//    penalty_slope = - 33.0 / (50.0 * variogram);
//    penalty_intercept = PENALTY_INTERCEPT;

//    for(i = 0; i < valid_count_window; i++)
//    {

//        if (ts_subset_selected[i] - medium < - 1.5 * variogram)
//        {
//            ts_subset_id[i] = 1;
//        }
//        else if (ts_subset_selected[i] - medium < - 2 * variogram)
//        {
//            ts_subset_id[i] = 2;
//        }
//        else if (ts_subset_selected[i] - medium < - 3 * variogram)
//        {
//            ts_subset_id[i] = 3;
//        }
//        else
//        {
//            ts_subset_id[i] = 0;
//        }

//    }
//    for(i = 0; i < valid_count_window; i++)
//    {

//            if (ts_subset_selected[i] - medium < - 1.5 * variogram)
//            {
//                ts_subset_id[i] = 1;
//            }
//            else
//            {
//                ts_subset_id[i] = 0;
//            }

//    }

    /********************************************/
    /*    condition 2: standard procedures      */
    /********************************************/
    double ratio;
    for(i = 0; i < valid_count_window; i++)
    {
         wt_cloud = (double) 1.0 / (ts_subset_selected_blue[i] * ts_subset_selected_blue[i]);

         if (ts_subset_selected_shadow[i] < medium_shadow)
         {
             ratio = (double)ts_subset_selected_shadow[i] / medium_shadow;
             wt_shadow = ratio * ratio * ratio * ratio;
         }
         else
             wt_shadow = 1.0;

         wt = wt_cloud * wt_shadow;
        //wt = (float)1/(abs(buf[BLUE_INDEX][i] - 0.5 * buf[RED_INDEX][i]));
        for(j = 0; j < TOTAL_IMAGE_BANDS; j++)
        {
            index_sum[j] = index_sum[j] + ts_subset[j][i] * wt;
        }
        wt_sum = wt_sum + wt;
    }

    for(j = 0; j < TOTAL_IMAGE_BANDS; j++)
    {
        out_compositing[j][i_col] = (short int)(index_sum[j] / wt_sum);
        //out_compositing[j][i_col] = (short int)valid_count_window;
    }

    /* free memory*/
    status = free_2d_array(ts_subset);
    if (status != SUCCESS)
    {
        RETURN_ERROR ("Freeing memory: ts_subset\n",
                      FUNC_NAME, FAILURE);
    }
    free(ts_subset_selected_shadow);
    free(ts_subset_selected_blue);

    return SUCCESS;

}

/******************************************************************************
MODULE:  valid_obs_count

PURPOSE:  count valid observation for each pixels

RETURN VALUE:
Type = int (SUCCESS, ERROR or FAILURE)

HISTORY:
Date        Programmer       Reason
--------    ---------------  -------------------------------------
06/22/2019   Su Ye         Original Development
******************************************************************************/
int valid_obs_count
(
    short int **buf,            /* I:  pixel-based time series           */
    int *valid_date_array,      /* I: valid date time series               */
    int valid_date_count,       /* I: the number of valid dates               */
    int lower_ordinal,
    int upper_ordinal,
    int i_col,
    short int **out_compositing           /* O: outputted compositing results for four bands */
)
{
    int index_sum[TOTAL_IMAGE_BANDS];
    int i, j, status;
    for(i = 0; i < TOTAL_IMAGE_BANDS; i++)
        index_sum[i] = 0;
    int valid_count_window = 0;
    short int** ts_subset;
    short int* ts_subset_selected;

    char FUNC_NAME[] = "modified_hot_compositing";

    ts_subset = (short int**)allocate_2d_array(TOTAL_IMAGE_BANDS, valid_date_count, sizeof(short int));
    if(ts_subset == NULL)
    {
        RETURN_ERROR ("Allocating ts_subset memory", FUNC_NAME, ERROR);
    }

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

    for(i = 0; i < valid_date_count; i++)
    {
        if((valid_date_array[i] > lower_ordinal - 1) && (valid_date_array[i] < upper_ordinal + 1))
        {
            for(j = 0; j < TOTAL_IMAGE_BANDS; j++)
            {
                ts_subset[j][valid_count_window] = buf[j][i];
                if(j == NIR_INDEX)
                {
                    ts_subset_selected[valid_count_window] = buf[NIR_INDEX][i];
                    //printf("%i\n", ts_subset_selected[valid_count_window]);
                }
            }
            valid_count_window++;

        }
    }

    for(j = 0; j < TOTAL_IMAGE_BANDS; j++)
    {

        out_compositing[j][i_col] = (short int)valid_count_window;
    }

    status = free_2d_array((void **)ts_subset);
    if (status != SUCCESS)
    {
        RETURN_ERROR ("Freeing memory: ts_subset\n",
                      FUNC_NAME, FAILURE);
    }
    free(ts_subset_selected);

    return SUCCESS;

}

/******************************************************************************
MODULE:  medium_compositing

PURPOSE:  Running compositing for pixel-based time series by adding shadow consideration

RETURN VALUE:
Type = int (SUCCESS, ERROR or FAILURE)

HISTORY:
Date        Programmer       Reason
--------    ---------------  -------------------------------------
06/22/2019   Su Ye         Original Development
******************************************************************************/
int medium_compositing
(
    short int **buf,            /* I:  pixel-based time series           */
    int *valid_date_array,      /* I: valid date time series               */
    int valid_date_count,       /* I: the number of valid dates               */
    int lower_ordinal,
    int upper_ordinal,
    int i_col,
    short int **out_compositing           /* O: outputted compositing results for four bands */
)
{
    int i, j, m;
    int status;
    double wt;
    double index_sum[TOTAL_IMAGE_BANDS];
    for(i = 0; i < TOTAL_IMAGE_BANDS; i++)
        index_sum[i] = 0;
    double wt_sum = 0;
    int valid_count_window = 0;
    short int** ts_subset;
    short int* ts_subset_selected;
    int* ts_subset_selected_index;
    char FUNC_NAME[] = "medium_compositing";

    ts_subset = (short int*)allocate_2d_array(TOTAL_IMAGE_BANDS, valid_date_count, sizeof(short int));
    if(ts_subset == NULL)
    {
        RETURN_ERROR ("Allocating ts_subset memory", FUNC_NAME, ERROR);
    }

    for(i = 0; i < valid_date_count; i++)
    {
        if((valid_date_array[i] > lower_ordinal - 1) && (valid_date_array[i] < upper_ordinal + 1))
        {
            for(j = 0; j < TOTAL_IMAGE_BANDS; j++)
            {
                ts_subset[j][valid_count_window] = buf[j][i];
            }
            valid_count_window++;
        }
    }

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

    ts_subset_selected_index = (int*)malloc(valid_count_window*sizeof(int));
    if(ts_subset_selected == NULL)
    {
        RETURN_ERROR ("Allocating ts_subset_selected_index memory", FUNC_NAME, ERROR);
    }

    for(i = 0; i < valid_count_window; i++)
    {

        ts_subset_selected[i] = ts_subset[NIR_INDEX][i];
        ts_subset_selected_index[i] = i;
            //printf("%i\n", ts_subset_selected[valid_count_window]);
    }

    /********************************************/
    /*    condition 1: zero valid observation   */
    /********************************************/
    if(valid_count_window == 0)
    {
        for(i = 0; i < TOTAL_IMAGE_BANDS; i++)
        {
            out_compositing[i][i_col] = -9999;
        }
        return SUCCESS;
    }

    quick_sort_shortint_index(ts_subset_selected, ts_subset_selected_index,
                              0, valid_count_window - 1);
//    for (j = 0; j < valid_count_window; j++)
//    {
//       printf("%i  %i\n", ts_subset_selected[j], ts_subset_selected_index[j]);
//    }

     m = valid_count_window / 2;
    if (valid_count_window % 2 == 0)
    {
        //printf("%f\n", var[m-1]);
       //printf("%f\n", var[m]);
        for(i = 0; i < TOTAL_IMAGE_BANDS; i++)
        {
           out_compositing[i][i_col] = (short int)((ts_subset[i][ts_subset_selected_index[m-1]] +
                   ts_subset[i][ts_subset_selected_index[m]]) / 2);
        }

    }
    else
    {
        for(i = 0; i < TOTAL_IMAGE_BANDS; i++)
        {
           out_compositing[i][i_col] = (short int)(ts_subset[i][ts_subset_selected_index[m]]);
        }
        //printf("%i\n", out_compositing[i][i_col]);
    }

    status = free_2d_array((void**)ts_subset);
    if (status != SUCCESS)
    {
        RETURN_ERROR ("Freeing memory: ts_subset\n",
                      FUNC_NAME, FAILURE);
    }
    free(ts_subset_selected);

    return SUCCESS;

}

/******************************************************************************
MODULE:  compositing_scanline

PURPOSE:  Running compositing for scanline-based time series

RETURN VALUE:
Type = int (SUCCESS, ERROR or FAILURE)

HISTORY:
Date        Programmer       Reason
--------    ---------------  -------------------------------------
05/02/2019   Su Ye         Original Development
******************************************************************************/
int compositing_scanline
(
    short int **buf,            /* I:  scanline-based time series           */
    int **valid_datearray_scanline,      /* I: valid date time series               */
    int *valid_datecount_scanline,       /* I: the number of valid dates               */
    int lower_ordinal,                /* I: lower ordinal date               */
    int upper_ordinal,                   /* I: upper_ordinal for temporal range of composition   */
    int num_samples,                /* I: the pixel number in a row              */
    int num_scenes,                 /* I: the number of scenes               */
    short int **out_compositing,           /* O: outputted compositing results for four bands */
    int method                       /* I: the compositing method{1 - fitting-weighted; 2 - fitting-normal; 3 - hot; 4 - average}  */
)
{
    int  j;
    int i_col;
    short int **tmp_buf;                   /* This is the image bands buffer, valid pixel only*/
    char FUNC_NAME[] = "compositing_scanline";
    int b_diagnosis = FALSE;
    Output_t* rec_c;

    tmp_buf = (short int **) allocate_2d_array (TOTAL_IMAGE_BANDS, num_scenes, sizeof (short int));
    if(tmp_buf == NULL)
    {
        RETURN_ERROR("ERROR allocating tmp_buf memory", FUNC_NAME, FAILURE);
    }

    rec_c = malloc(sizeof(Output_t));
    if(rec_c == NULL)
    {
        RETURN_ERROR("ERROR allocating rec_c memory", FUNC_NAME, FAILURE);
    }

    for(i_col = 0; i_col < num_samples; i_col++)
    {
        for(j = 0; j < TOTAL_IMAGE_BANDS; j++)
        {
           tmp_buf[j]  = buf[j] + i_col * num_scenes;
        }

        /*weighted fitting*/
        if (1==method)
        {
            fitting_compositing(tmp_buf, valid_datearray_scanline[i_col],
                        valid_datecount_scanline[i_col], lower_ordinal,
                        upper_ordinal, i_col, out_compositing, TRUE, TRUE, b_diagnosis, rec_c);
        }
        /*normal fitting*/
        else if (2==method)
        {
            fitting_compositing(tmp_buf, valid_datearray_scanline[i_col],
                        valid_datecount_scanline[i_col], lower_ordinal,
                        upper_ordinal, i_col, out_compositing, TRUE, FALSE, b_diagnosis, rec_c);
        }
        else if (3==method)
        {
            hot_compositing(tmp_buf, valid_datearray_scanline[i_col],
                        valid_datecount_scanline[i_col], lower_ordinal,
                        upper_ordinal, i_col, out_compositing);
        }
        else if (4==method)
        {
            average_compositing(tmp_buf, valid_datearray_scanline[i_col],
                        valid_datecount_scanline[i_col], lower_ordinal,
                        upper_ordinal, i_col, out_compositing);
        }
        else if (5==method)
        {
            fitting_compositing(tmp_buf, valid_datearray_scanline[i_col],
                        valid_datecount_scanline[i_col], lower_ordinal,
                        upper_ordinal, i_col, out_compositing, FALSE, FALSE, b_diagnosis, rec_c);
        }
        else if (6==method)
        {
            modified_hot_compositing(tmp_buf, valid_datearray_scanline[i_col],
                        valid_datecount_scanline[i_col], lower_ordinal,
                        upper_ordinal, i_col, out_compositing);
        }

        else if (7==method)
        {
            medium_compositing(tmp_buf, valid_datearray_scanline[i_col],
                               valid_datecount_scanline[i_col], lower_ordinal,
                               upper_ordinal, i_col, out_compositing);
        }
        else if (8==method)
        {
            valid_obs_count(tmp_buf, valid_datearray_scanline[i_col],
                            valid_datecount_scanline[i_col], lower_ordinal,
                            upper_ordinal, i_col, out_compositing);
        }
    }

    free(rec_c);
    free_2d_array((void **)tmp_buf);

    return SUCCESS;
}

/******************************************************************************
MODULE:  fitting_compositing

PURPOSE:  Running compositing for pixel-based time series

RETURN VALUE:
Type = int (SUCCESS, ERROR or FAILURE)

HISTORY:
Date        Programmer       Reason
--------    ---------------  -------------------------------------
05/02/2019   Su Ye         Original Development
******************************************************************************/
int fitting_compositing
(
    short int **buf,            /* I:  pixel-based time series           */
    int *valid_date_array,      /* I: valid date time series               */
    int valid_date_count,       /* I: the number of valid dates               */
    int lower_ordinal,
    int upper_ordinal,
    int i_col,
    short int **out_compositing,           /* O: outputted compositing results for four bands */
    int bfit,
    int bweighted,
    int b_diagnosis,
    Output_t* rec_c
)
{
    char FUNC_NAME[] = "fitting_compositing";
    int status;
    float date_vario;           /* I: median date                                          */
    float max_date_difference;   /* I: maximum difference between two neighbor dates        */
    float adj_rmse[TOTAL_IMAGE_BANDS]; /* Adjusted RMSE for all bands          */
    int *bl_ids;
    int n_clr;
    int n_clr_1;
    int n_outlier_1;
    int n_clr_2;
    int n_outlier_2;
    int k, b;
    int* clrx;
    float **clry;
    int* clrx_1;
    float **clry_1;
    int* clrx_2;
    float **clry_2;
    int i;
    float C0; // intercept from each test output
    float C1; // slope from each test output

    clrx = (int*)calloc(valid_date_count, sizeof(int));
    clry = (float **) allocate_2d_array (TOTAL_IMAGE_BANDS, valid_date_count,
                                             sizeof (float));
    if (clry == NULL)
    {
        RETURN_ERROR ("Allocating clry memory", FUNC_NAME, FAILURE);
    }

    /**************************************************************/
    /*                                                            */
    /*      select observations in the observation window          */
    /*                                                            */
    /**************************************************************/
    n_clr = 0;
    for(i = 0; i < valid_date_count; i++)
    {
        if((valid_date_array[i] > lower_ordinal - 1) && (valid_date_array[i] < upper_ordinal + 1))
        {
            clrx[n_clr] = valid_date_array[i];
            for(b = 0; b < TOTAL_IMAGE_BANDS; b++)
            {
                clry[b][n_clr] = (float)buf[b][i];
            }
            n_clr++;
        }
    }

    /********************************************/
    /*    condition 1: zero valid observation   */
    /********************************************/
    if(n_clr==0)
    {
        for(i = 0; i < TOTAL_IMAGE_BANDS; i++)
        {
            out_compositing[i][i_col] = -9999;
        }

        if(TRUE == b_diagnosis)
        {
            rec_c->condition = NOOBS_CONDITION;
        }

        free(clrx);
        status = free_2d_array((void **)clry);
        if (status != SUCCESS)
        {
            RETURN_ERROR ("Freeing memory: clry\n",
                          FUNC_NAME, FAILURE);
        }

        return SUCCESS;
    }

    /**********************************************/
    /*    condition 2: inefficient observations   */
    /**********************************************/
    else if (n_clr < MIN_SAMPLE)
    {
        median_compositing(buf, valid_date_array, valid_date_count,
                           i_col, out_compositing);

        if(TRUE == b_diagnosis)
        {
            rec_c->condition = INEFFICIENT_CONDITION;
        }

        free(clrx);
        status = free_2d_array((void **)clry);
        if (status != SUCCESS)
        {
            RETURN_ERROR ("Freeing memory: clry\n",
                          FUNC_NAME, FAILURE);
        }

        return SUCCESS;
    }
    else
    {
        if(TRUE == b_diagnosis)
        {
            rec_c->condition = NORMAL_CONDITION;
        }
    }

    /**********************************************/
    /*    condition 3: standard procedure         */
    /**********************************************/

    bl_ids = (int *)calloc(n_clr, sizeof(int));
    if (bl_ids == NULL)
    {
        RETURN_ERROR("ERROR allocating bl_ids memory", FUNC_NAME, FAILURE);
    }

    clrx_1 = (int *)calloc(n_clr, sizeof(int));
    clry_1 = (float **) allocate_2d_array (TOTAL_IMAGE_BANDS, n_clr,
                                             sizeof (float));
    if (clry_1 == NULL)
    {
        RETURN_ERROR ("Allocating clry_1 memory", FUNC_NAME, FAILURE);
    }

    clrx_2 = (int *)calloc(n_clr, sizeof(int));
    clry_2 = (float **) allocate_2d_array (TOTAL_IMAGE_BANDS, n_clr,
                                             sizeof (float));
    if (clry_2 == NULL)
    {
        RETURN_ERROR ("Allocating clry_2 memory", FUNC_NAME, FAILURE);
    }

    /**************************************************************/
    /*                                                            */
    /*      calculate variogram for each band and dates.          */
    /*                                                            */
    /**************************************************************/
    status = adjust_median_variogram(clrx, clry, TOTAL_IMAGE_BANDS,
                                     0, n_clr-1, &date_vario,
                                     &max_date_difference, adj_rmse);
    if (status != SUCCESS)
    {
            RETURN_ERROR("ERROR calling median_variogram routine", FUNC_NAME,
                         FAILURE);
    }

    status = greenband_test(clrx, clry, 0, n_clr-1, adj_rmse[1], T_CONST_SINGLETAIL_9999,
            bl_ids, &C0, &C1);

    if (status != SUCCESS)
    {
        RETURN_ERROR("ERROR calling greenband_test",
                      FUNC_NAME, FAILURE);
    }

    if(TRUE == b_diagnosis)
    {
        rec_c->C0_green = C0;
        rec_c->C1_green = C1;
    }

    /**************************************************/
    /*                                                */
    /*         remove outliers.                       */
    /*                                                */
    /**************************************************/
    n_clr_1 = 0;
    n_outlier_1 = 0;
    for(i = 0; i < n_clr; i++)
    {
        if(bl_ids[i] == 0)
        {
            clrx_1[n_clr_1] = clrx[i];
            for (b = 0; b < TOTAL_IMAGE_BANDS; b++)
            {
                clry_1[b][n_clr_1] = clry[b][i];
            }
            n_clr_1 = n_clr_1 + 1;
        }
        else
        {
            if(TRUE == b_diagnosis)
            {
                rec_c->outlier_dates_green[n_outlier_1] = clrx[i];
                n_outlier_1 = n_outlier_1 + 1;
            }
        }
    }

    rec_c->n_outlier_green = n_outlier_1;

    /* if n_clr_1 < MIN_SAMPLE, means that green test failed, need to reset*/
    if (n_clr_1 < MIN_SAMPLE)
    {
        n_clr_1 = 0;
        for(i = 0; i < n_clr; i++)
        {

            clrx_1[n_clr_1] = clrx[i];
            for (b = 0; b < TOTAL_IMAGE_BANDS; b++)
            {
                clry_1[b][n_clr_1] = (float)clry[b][i];
            }
            n_clr_1 = n_clr_1 + 1;

        }
        if(TRUE == b_diagnosis)
            rec_c->b_success_green = FAILURE;
    }
    else{
        if(TRUE == b_diagnosis)
            rec_c->b_success_green = SUCCESS;
    }

    /**************************************************/
    /*                                                */
    /* nir band test                                  */
    /*                                                */
    /**************************************************/

    for (k = 0; k < n_clr_1; k++)
       bl_ids[k] = 0;

    status = nirband_test(clrx_1, clry_1, 0, n_clr_1-1, adj_rmse[3], T_CONST_SINGLETAIL_9999,
            bl_ids, &C0, &C1);

    if (status != SUCCESS)
    {
        RETURN_ERROR("ERROR calling nirband_test",
                      FUNC_NAME, FAILURE);
    }

    if(TRUE == b_diagnosis)
    {
        rec_c->C0_nir = C0;
        rec_c->C1_nir = C1;
    }

    /**************************************************/
    /*                                                */
    /*         remove outliers.                       */
    /*                                                */
    /**************************************************/
    n_clr_2 = 0;
    n_outlier_2 = 0;
    for(i = 0; i < n_clr_1; i++)
    {
        if(bl_ids[i] == 0)
        {
            clrx_2[n_clr_2] = clrx_1[i];
            for (b = 0; b < TOTAL_IMAGE_BANDS; b++)
            {
                clry_2[b][n_clr_2] = clry_1[b][i];
            }
            n_clr_2 = n_clr_2 + 1;
        }
        else
        {
            if(TRUE == b_diagnosis)
            {
                rec_c->outlier_dates_nir[n_outlier_2] = clrx_1[i];
                n_outlier_2 = n_outlier_2 + 1;
            }
        }
    }

    rec_c->n_outlier_nir = n_outlier_2;

    /* if n_clr_1 < MIN_SAMPLE, means that nir test failed, reset*/
    if (n_clr_2 < MIN_SAMPLE)
    {
        for(i = 0; i < n_clr_1; i++)
        {
            n_clr_2 = 0;
            clrx_2[n_clr_2] = clrx_1[i];
            for (b = 0; b < TOTAL_IMAGE_BANDS; b++)
            {
                clry_2[b][n_clr_2] = clry_1[b][i];
            }
            n_clr_2 = n_clr_2 + 1;
        }

        if(TRUE == b_diagnosis)
            rec_c->b_success_nir = FAILURE;
    }
    else{
        if(TRUE == b_diagnosis)
            rec_c->b_success_nir = SUCCESS;
    }

    if(bfit == TRUE)
        linear_fit_centerdate(clrx_2, clry_2, n_clr_2, 0, (lower_ordinal + upper_ordinal)/2,i_col,
                          out_compositing, bweighted, rec_c->C0_final, rec_c->C1_final);
    else
    {
        float wt;
        int j;
        double index_sum[TOTAL_IMAGE_BANDS];
        for(i = 0; i < TOTAL_IMAGE_BANDS; i++)
            index_sum[i] = 0;
        float wt_sum = 0;

        for(i = 0; i < n_clr_2; i++)
        {
            wt = (float)1/((clry_2[BLUE_INDEX][i] - 0.5 * clry_2[RED_INDEX][i]) * (clry_2[BLUE_INDEX][i] - 0.5 * clry_2[RED_INDEX][i]));
            for(j = 0; j < TOTAL_IMAGE_BANDS; j++)
            {
                index_sum[j] = index_sum[j] + clry_2[j][i] * wt;
            }
            wt_sum = wt_sum + wt;
        }

        /********************************************/
        /*    condition 2: standard procedures      */
        /********************************************/

        for(j = 0; j < TOTAL_IMAGE_BANDS; j++)
        {
            out_compositing[j][i_col] = (short int)(index_sum[j] / wt_sum);
        }

    }

    free(bl_ids);
    free(clrx);
    status = free_2d_array((void **)clry);
    if (status != SUCCESS)
    {
        RETURN_ERROR ("Freeing memory: clry\n",
                      FUNC_NAME, FAILURE);
    }

    free(clrx_1);
    status = free_2d_array((void **)clry_1);
    if (status != SUCCESS)
    {
        RETURN_ERROR ("Freeing memory: clry_1\n",
                      FUNC_NAME, FAILURE);
    }

    free(clrx_2);
    status = free_2d_array((void **)clry_2);
    if (status != SUCCESS)
    {
        RETURN_ERROR ("Freeing memory: clry_2\n",
                      FUNC_NAME, FAILURE);
    }

    return SUCCESS;

}

//int fitting_compositing_scanline
//(
//    short int **buf,            /* I:  scanline-based time series           */
//    int **valid_datearray_scanline,      /* I: valid date time series               */
//    int *valid_datecount_scanline,       /* I: the number of valid dates               */
//    int lower_ordinal,                /* I: lower ordinal dates               */
//    int upper_ordinal,                   /* I: upper_ordinal for temporal range of composition   */
//    int num_samples,                /* I: the pixel number in a row              */
//    int num_scenes,                 /* I: the number of scenes               */
//    short int **out_compositing           /* O: outputted compositing results for four bands */
//)
//{
//    int i, j;
//    int i_col;
//    short int **tmp_buf;                   /* This is the image bands buffer, valid pixel only*/
//    int result;
//    char FUNC_NAME[] = "compositing_scanline";
//    tmp_buf = (short int **) allocate_2d_array (TOTAL_IMAGE_BANDS, num_scenes, sizeof (short int));
//    if(tmp_buf == NULL)
//    {
//        RETURN_ERROR("ERROR allocating tmp_buf memory", FUNC_NAME, FAILURE);
//    }

//    for(i_col = 0; i_col < num_samples; i_col++)
//    {
//        for(j = 0; j < TOTAL_IMAGE_BANDS; j++)
//        {
//           tmp_buf[j]  = buf[j] + i_col * num_scenes;
//        }

//        fitting_compositing(tmp_buf, valid_datearray_scanline[i_col],
//                    valid_datecount_scanline[i_col], lower_ordinal,
//                    upper_ordinal, i_col, out_compositing);
//    }

//    free_2d_array((void **)tmp_buf);

//    return SUCCESS;
//}


/* linalg/gsl_linalg.h
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2006, 2007 Gerard Jungman, Brian Gough, 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.
 */

#ifndef __GSL_LINALG_H__
#define __GSL_LINALG_H__

#include <gsl/gsl_mode.h>
#include <gsl/gsl_permutation.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>

#undef __BEGIN_DECLS
#undef __END_DECLS
#ifdef __cplusplus
#define __BEGIN_DECLS extern "C" {
#define __END_DECLS }
#else
#define __BEGIN_DECLS           /* empty */
#define __END_DECLS             /* empty */
#endif

__BEGIN_DECLS

typedef enum
  {
    GSL_LINALG_MOD_NONE = 0,
    GSL_LINALG_MOD_TRANSPOSE = 1,
    GSL_LINALG_MOD_CONJUGATE = 2
  }
gsl_linalg_matrix_mod_t;

/* Note: You can now use the gsl_blas_dgemm function instead of matmult */

/* Simple implementation of matrix multiply.
 * Calculates C = A.B
 *
 * exceptions: GSL_EBADLEN
 */
int gsl_linalg_matmult (const gsl_matrix * A,
                        const gsl_matrix * B,
                        gsl_matrix * C);

/* Simple implementation of matrix multiply.
 * Allows transposition of either matrix, so it
 * can compute A.B or Trans(A).B or A.Trans(B) or Trans(A).Trans(B)
 *
 * exceptions: GSL_EBADLEN
 */
int gsl_linalg_matmult_mod (const gsl_matrix * A,
                            gsl_linalg_matrix_mod_t modA,
                            const gsl_matrix * B,
                            gsl_linalg_matrix_mod_t modB,
                            gsl_matrix * C);

/* Calculate the matrix exponential by the scaling and
 * squaring method described in Moler + Van Loan,
 * SIAM Rev 20, 801 (1978). The mode argument allows
 * choosing an optimal strategy, from the table
 * given in the paper, for a given precision.
 *
 * exceptions: GSL_ENOTSQR, GSL_EBADLEN
 */
int gsl_linalg_exponential_ss(
  const gsl_matrix * A,
  gsl_matrix * eA,
  gsl_mode_t mode
  );

/* Householder Transformations */

double gsl_linalg_householder_transform (gsl_vector * v);
gsl_complex gsl_linalg_complex_householder_transform (gsl_vector_complex * v);

int gsl_linalg_householder_hm (double tau,
                               const gsl_vector * v,
                               gsl_matrix * A);

int gsl_linalg_householder_mh (double tau,
                               const gsl_vector * v,
                               gsl_matrix * A);

int gsl_linalg_householder_hv (double tau,
                               const gsl_vector * v,
                               gsl_vector * w);

int gsl_linalg_householder_hm1 (double tau,
                                gsl_matrix * A);

int gsl_linalg_complex_householder_hm (gsl_complex tau,
                                       const gsl_vector_complex * v,
                                       gsl_matrix_complex * A);

int gsl_linalg_complex_householder_mh (gsl_complex tau,
                                       const gsl_vector_complex * v,
                                       gsl_matrix_complex * A);

int gsl_linalg_complex_householder_hv (gsl_complex tau,
                                       const gsl_vector_complex * v,
                                       gsl_vector_complex * w);

/* Hessenberg reduction */

int gsl_linalg_hessenberg_decomp(gsl_matrix *A, gsl_vector *tau);
int gsl_linalg_hessenberg_unpack(gsl_matrix * H, gsl_vector * tau,
                                 gsl_matrix * U);
int gsl_linalg_hessenberg_unpack_accum(gsl_matrix * H, gsl_vector * tau,
                                       gsl_matrix * U);
int gsl_linalg_hessenberg_set_zero(gsl_matrix * H);
int gsl_linalg_hessenberg_submatrix(gsl_matrix *M, gsl_matrix *A,
                                    size_t top, gsl_vector *tau);

/* To support gsl-1.9 interface: DEPRECATED */
int gsl_linalg_hessenberg(gsl_matrix *A, gsl_vector *tau);

/* Hessenberg-Triangular reduction */

int gsl_linalg_hesstri_decomp(gsl_matrix * A, gsl_matrix * B,
                              gsl_matrix * U, gsl_matrix * V,
                              gsl_vector * work);

/* Singular Value Decomposition

 * exceptions:
 */

int
gsl_linalg_SV_decomp (gsl_matrix * A,
                      gsl_matrix * V,
                      gsl_vector * S,
                      gsl_vector * work);

int
gsl_linalg_SV_decomp_mod (gsl_matrix * A,
                          gsl_matrix * X,
                          gsl_matrix * V,
                          gsl_vector * S,
                          gsl_vector * work);

int gsl_linalg_SV_decomp_jacobi (gsl_matrix * A,
                                 gsl_matrix * Q,
                                 gsl_vector * S);

int
gsl_linalg_SV_solve (const gsl_matrix * U,
                     const gsl_matrix * Q,
                     const gsl_vector * S,
                     const gsl_vector * b,
                     gsl_vector * x);

int gsl_linalg_SV_leverage(const gsl_matrix *U, gsl_vector *h);

/* LU Decomposition, Gaussian elimination with partial pivoting
 */

int gsl_linalg_LU_decomp (gsl_matrix * A, gsl_permutation * p, int *signum);

int gsl_linalg_LU_solve (const gsl_matrix * LU,
                         const gsl_permutation * p,
                         const gsl_vector * b,
                         gsl_vector * x);

int gsl_linalg_LU_svx (const gsl_matrix * LU,
                       const gsl_permutation * p,
                       gsl_vector * x);

int gsl_linalg_LU_refine (const gsl_matrix * A,
                          const gsl_matrix * LU,
                          const gsl_permutation * p,
                          const gsl_vector * b,
                          gsl_vector * x,
                          gsl_vector * residual);

int gsl_linalg_LU_invert (const gsl_matrix * LU,
                          const gsl_permutation * p,
                          gsl_matrix * inverse);

double gsl_linalg_LU_det (gsl_matrix * LU, int signum);
double gsl_linalg_LU_lndet (gsl_matrix * LU);
int gsl_linalg_LU_sgndet (gsl_matrix * lu, int signum);

/* Complex LU Decomposition */

int gsl_linalg_complex_LU_decomp (gsl_matrix_complex * A,
                                  gsl_permutation * p,
                                  int *signum);

int gsl_linalg_complex_LU_solve (const gsl_matrix_complex * LU,
                                 const gsl_permutation * p,
                                 const gsl_vector_complex * b,
                                 gsl_vector_complex * x);

int gsl_linalg_complex_LU_svx (const gsl_matrix_complex * LU,
                               const gsl_permutation * p,
                               gsl_vector_complex * x);

int gsl_linalg_complex_LU_refine (const gsl_matrix_complex * A,
                                  const gsl_matrix_complex * LU,
                                  const gsl_permutation * p,
                                  const gsl_vector_complex * b,
                                  gsl_vector_complex * x,
                                  gsl_vector_complex * residual);

int gsl_linalg_complex_LU_invert (const gsl_matrix_complex * LU,
                                  const gsl_permutation * p,
                                  gsl_matrix_complex * inverse);

gsl_complex gsl_linalg_complex_LU_det (gsl_matrix_complex * LU,
                                       int signum);

double gsl_linalg_complex_LU_lndet (gsl_matrix_complex * LU);

gsl_complex gsl_linalg_complex_LU_sgndet (gsl_matrix_complex * LU,
                                          int signum);

/* QR decomposition */

int gsl_linalg_QR_decomp (gsl_matrix * A,
                          gsl_vector * tau);

int gsl_linalg_QR_solve (const gsl_matrix * QR,
                         const gsl_vector * tau,
                         const gsl_vector * b,
                         gsl_vector * x);

int gsl_linalg_QR_svx (const gsl_matrix * QR,
                       const gsl_vector * tau,
                       gsl_vector * x);

int gsl_linalg_QR_lssolve (const gsl_matrix * QR,
                           const gsl_vector * tau,
                           const gsl_vector * b,
                           gsl_vector * x,
                           gsl_vector * residual);

int gsl_linalg_QR_QRsolve (gsl_matrix * Q,
                           gsl_matrix * R,
                           const gsl_vector * b,
                           gsl_vector * x);

int gsl_linalg_QR_Rsolve (const gsl_matrix * QR,
                          const gsl_vector * b,
                          gsl_vector * x);

int gsl_linalg_QR_Rsvx (const gsl_matrix * QR,
                        gsl_vector * x);

int gsl_linalg_QR_update (gsl_matrix * Q,
                          gsl_matrix * R,
                          gsl_vector * w,
                          const gsl_vector * v);

int gsl_linalg_QR_QTvec (const gsl_matrix * QR,
                         const gsl_vector * tau,
                         gsl_vector * v);

int gsl_linalg_QR_Qvec (const gsl_matrix * QR,
                        const gsl_vector * tau,
                        gsl_vector * v);

int gsl_linalg_QR_QTmat (const gsl_matrix * QR,
                         const gsl_vector * tau,
                         gsl_matrix * A);

int gsl_linalg_QR_unpack (const gsl_matrix * QR,
                          const gsl_vector * tau,
                          gsl_matrix * Q,
                          gsl_matrix * R);

int gsl_linalg_R_solve (const gsl_matrix * R,
                        const gsl_vector * b,
                        gsl_vector * x);

int gsl_linalg_R_svx (const gsl_matrix * R,
                      gsl_vector * x);

/* Q R P^T decomposition */

int gsl_linalg_QRPT_decomp (gsl_matrix * A,
                            gsl_vector * tau,
                            gsl_permutation * p,
                            int *signum,
                            gsl_vector * norm);

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);

int gsl_linalg_QRPT_solve (const gsl_matrix * QR,
                           const gsl_vector * tau,
                           const gsl_permutation * p,
                           const gsl_vector * b,
                           gsl_vector * x);

int gsl_linalg_QRPT_svx (const gsl_matrix * QR,
                         const gsl_vector * tau,
                         const gsl_permutation * p,
                         gsl_vector * x);

int gsl_linalg_QRPT_QRsolve (const gsl_matrix * Q,
                             const gsl_matrix * R,
                             const gsl_permutation * p,
                             const gsl_vector * b,
                             gsl_vector * x);

int gsl_linalg_QRPT_Rsolve (const gsl_matrix * QR,
                             const gsl_permutation * p,
                             const gsl_vector * b,
                             gsl_vector * x);

int gsl_linalg_QRPT_Rsvx (const gsl_matrix * QR,
                           const gsl_permutation * p,
                           gsl_vector * x);

int gsl_linalg_QRPT_update (gsl_matrix * Q,
                            gsl_matrix * R,
                            const gsl_permutation * p,
                            gsl_vector * u,
                            const gsl_vector * v);

/* LQ decomposition */

int gsl_linalg_LQ_decomp (gsl_matrix * A, gsl_vector * tau);

int gsl_linalg_LQ_solve_T (const gsl_matrix * LQ, const gsl_vector * tau,
			 const gsl_vector * b, gsl_vector * x);

int gsl_linalg_LQ_svx_T (const gsl_matrix * LQ, const gsl_vector * tau,
                         gsl_vector * x);

int gsl_linalg_LQ_lssolve_T (const gsl_matrix * LQ, const gsl_vector * tau,
			   const gsl_vector * b, gsl_vector * x,
			   gsl_vector * residual);

int gsl_linalg_LQ_Lsolve_T (const gsl_matrix * LQ, const gsl_vector * b,
			  gsl_vector * x);

int gsl_linalg_LQ_Lsvx_T (const gsl_matrix * LQ, gsl_vector * x);

int gsl_linalg_L_solve_T (const gsl_matrix * L, const gsl_vector * b,
			gsl_vector * x);

int gsl_linalg_LQ_vecQ (const gsl_matrix * LQ, const gsl_vector * tau,
			gsl_vector * v);

int gsl_linalg_LQ_vecQT (const gsl_matrix * LQ, const gsl_vector * tau,
			 gsl_vector * v);

int gsl_linalg_LQ_unpack (const gsl_matrix * LQ, const gsl_vector * tau,
			  gsl_matrix * Q, gsl_matrix * L);

int gsl_linalg_LQ_update (gsl_matrix * Q, gsl_matrix * R,
			  const gsl_vector * v, gsl_vector * w);
int gsl_linalg_LQ_LQsolve (gsl_matrix * Q, gsl_matrix * L,
			   const gsl_vector * b, gsl_vector * x);

/* P^T L Q decomposition */

int gsl_linalg_PTLQ_decomp (gsl_matrix * A, gsl_vector * tau,
			    gsl_permutation * p, int *signum,
			    gsl_vector * norm);

int gsl_linalg_PTLQ_decomp2 (const gsl_matrix * A, gsl_matrix * q,
			     gsl_matrix * r, gsl_vector * tau,
			     gsl_permutation * p, int *signum,
			     gsl_vector * norm);

int gsl_linalg_PTLQ_solve_T (const gsl_matrix * QR,
			   const gsl_vector * tau,
			   const gsl_permutation * p,
			   const gsl_vector * b,
			   gsl_vector * x);

int gsl_linalg_PTLQ_svx_T (const gsl_matrix * LQ,
                           const gsl_vector * tau,
                           const gsl_permutation * p,
                           gsl_vector * x);

int gsl_linalg_PTLQ_LQsolve_T (const gsl_matrix * Q, const gsl_matrix * L,
			     const gsl_permutation * p,
			     const gsl_vector * b,
			     gsl_vector * x);

int gsl_linalg_PTLQ_Lsolve_T (const gsl_matrix * LQ,
			    const gsl_permutation * p,
			    const gsl_vector * b,
			    gsl_vector * x);

int gsl_linalg_PTLQ_Lsvx_T (const gsl_matrix * LQ,
			  const gsl_permutation * p,
			  gsl_vector * x);

int gsl_linalg_PTLQ_update (gsl_matrix * Q, gsl_matrix * L,
			    const gsl_permutation * p,
			    const gsl_vector * v, gsl_vector * w);

/* Cholesky Decomposition */

int gsl_linalg_cholesky_decomp (gsl_matrix * A);

int gsl_linalg_cholesky_solve (const gsl_matrix * cholesky,
                               const gsl_vector * b,
                               gsl_vector * x);

int gsl_linalg_cholesky_svx (const gsl_matrix * cholesky,
                             gsl_vector * x);

int gsl_linalg_cholesky_invert(gsl_matrix * cholesky);

/* Cholesky decomposition with unit-diagonal triangular parts.
 *   A = L D L^T, where diag(L) = (1,1,...,1).
 *   Upon exit, A contains L and L^T as for Cholesky, and
 *   the diagonal of A is (1,1,...,1). The vector Dis set
 *   to the diagonal elements of the diagonal matrix D.
 */
int gsl_linalg_cholesky_decomp_unit(gsl_matrix * A, gsl_vector * D);

/* Complex Cholesky Decomposition */

int gsl_linalg_complex_cholesky_decomp (gsl_matrix_complex * A);

int gsl_linalg_complex_cholesky_solve (const gsl_matrix_complex * cholesky,
                                       const gsl_vector_complex * b,
                                       gsl_vector_complex * x);

int gsl_linalg_complex_cholesky_svx (const gsl_matrix_complex * cholesky,
                                     gsl_vector_complex * x);

int gsl_linalg_complex_cholesky_invert(gsl_matrix_complex * cholesky);

/* Symmetric to symmetric tridiagonal decomposition */

int gsl_linalg_symmtd_decomp (gsl_matrix * A,
                              gsl_vector * tau);

int gsl_linalg_symmtd_unpack (const gsl_matrix * A,
                              const gsl_vector * tau,
                              gsl_matrix * Q,
                              gsl_vector * diag,
                              gsl_vector * subdiag);

int gsl_linalg_symmtd_unpack_T (const gsl_matrix * A,
                                gsl_vector * diag,
                                gsl_vector * subdiag);

/* Hermitian to symmetric tridiagonal decomposition */

int gsl_linalg_hermtd_decomp (gsl_matrix_complex * A,
                              gsl_vector_complex * tau);

int gsl_linalg_hermtd_unpack (const gsl_matrix_complex * A,
                              const gsl_vector_complex * tau,
                              gsl_matrix_complex * U,
                              gsl_vector * diag,
                              gsl_vector * sudiag);

int gsl_linalg_hermtd_unpack_T (const gsl_matrix_complex * A,
                                gsl_vector * diag,
                                gsl_vector * subdiag);

/* Linear Solve Using Householder Transformations

 * exceptions:
 */

int gsl_linalg_HH_solve (gsl_matrix * A, const gsl_vector * b, gsl_vector * x);
int gsl_linalg_HH_svx (gsl_matrix * A, gsl_vector * x);

/* Linear solve for a symmetric tridiagonal system.

 * The input vectors represent the NxN matrix as follows:
 *
 *     diag[0]  offdiag[0]             0    ...
 *  offdiag[0]     diag[1]    offdiag[1]    ...
 *           0  offdiag[1]       diag[2]    ...
 *           0           0    offdiag[2]    ...
 *         ...         ...           ...    ...
 */
int gsl_linalg_solve_symm_tridiag (const gsl_vector * diag,
                                   const gsl_vector * offdiag,
                                   const gsl_vector * b,
                                   gsl_vector * x);

/* Linear solve for a nonsymmetric tridiagonal system.

 * The input vectors represent the NxN matrix as follows:
 *
 *       diag[0]  abovediag[0]              0    ...
 *  belowdiag[0]       diag[1]   abovediag[1]    ...
 *             0  belowdiag[1]        diag[2]    ...
 *             0             0   belowdiag[2]    ...
 *           ...           ...            ...    ...
 */
int gsl_linalg_solve_tridiag (const gsl_vector * diag,
                                   const gsl_vector * abovediag,
                                   const gsl_vector * belowdiag,
                                   const gsl_vector * b,
                                   gsl_vector * x);

/* Linear solve for a symmetric cyclic tridiagonal system.

 * The input vectors represent the NxN matrix as follows:
 *
 *      diag[0]  offdiag[0]             0   .....  offdiag[N-1]
 *   offdiag[0]     diag[1]    offdiag[1]   .....
 *            0  offdiag[1]       diag[2]   .....
 *            0           0    offdiag[2]   .....
 *          ...         ...
 * offdiag[N-1]         ...
 */
int gsl_linalg_solve_symm_cyc_tridiag (const gsl_vector * diag,
                                       const gsl_vector * offdiag,
                                       const gsl_vector * b,
                                       gsl_vector * x);

/* Linear solve for a nonsymmetric cyclic tridiagonal system.

 * The input vectors represent the NxN matrix as follows:
 *
 *        diag[0]  abovediag[0]             0   .....  belowdiag[N-1]
 *   belowdiag[0]       diag[1]  abovediag[1]   .....
 *              0  belowdiag[1]       diag[2]
 *              0             0  belowdiag[2]   .....
 *            ...           ...
 * abovediag[N-1]           ...
 */
int gsl_linalg_solve_cyc_tridiag (const gsl_vector * diag,
                                  const gsl_vector * abovediag,
                                  const gsl_vector * belowdiag,
                                  const gsl_vector * b,
                                  gsl_vector * x);

/* Bidiagonal decomposition */

int gsl_linalg_bidiag_decomp (gsl_matrix * A,
                              gsl_vector * tau_U,
                              gsl_vector * tau_V);

int gsl_linalg_bidiag_unpack (const gsl_matrix * A,
                              const gsl_vector * tau_U,
                              gsl_matrix * U,
                              const gsl_vector * tau_V,
                              gsl_matrix * V,
                              gsl_vector * diag,
                              gsl_vector * superdiag);

int gsl_linalg_bidiag_unpack2 (gsl_matrix * A,
                               gsl_vector * tau_U,
                               gsl_vector * tau_V,
                               gsl_matrix * V);

int gsl_linalg_bidiag_unpack_B (const gsl_matrix * A,
                                gsl_vector * diag,
                                gsl_vector * superdiag);

/* Balancing */

int gsl_linalg_balance_matrix (gsl_matrix * A, gsl_vector * D);
int gsl_linalg_balance_accum (gsl_matrix * A, gsl_vector * D);
int gsl_linalg_balance_columns (gsl_matrix * A, gsl_vector * D);

__END_DECLS

#endif /* __GSL_LINALG_H__ */


/* siman/siman.c
 *
 * Copyright (C) 2007 Brian Gough
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Mark Galassi
 *
 * 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 <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>

#include <gsl/gsl_machine.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_siman.h>

static inline double
boltzmann(double E, double new_E, double T, gsl_siman_params_t *params)
{
  double x = -(new_E - E) / (params->k * T);
  /* avoid underflow errors for large uphill steps */
  return (x < GSL_LOG_DBL_MIN) ? 0.0 : exp(x);
}

static inline void
copy_state(void *src, void *dst, size_t size, gsl_siman_copy_t copyfunc)
{
  if (copyfunc) {
    copyfunc(src, dst);
  } else {
    memcpy(dst, src, size);
  }
}

/* implementation of a basic simulated annealing algorithm */

void
gsl_siman_solve (const gsl_rng * r, void *x0_p, gsl_siman_Efunc_t Ef,
                 gsl_siman_step_t take_step,
                 gsl_siman_metric_t distance,
                 gsl_siman_print_t print_position,
                 gsl_siman_copy_t copyfunc,
                 gsl_siman_copy_construct_t copy_constructor,
                 gsl_siman_destroy_t destructor,
                 size_t element_size,
                 gsl_siman_params_t params)
{
  void *x, *new_x, *best_x;
  double E, new_E, best_E;
  int i;
  double T, T_factor;
  int n_evals = 1, n_iter = 0, n_accepts, n_rejects, n_eless;

  /* this function requires that either the dynamic functions (copy,
     copy_constructor and destrcutor) are passed, or that an element
     size is given */
  assert((copyfunc != NULL && copy_constructor != NULL && destructor != NULL)
         || (element_size != 0));

  distance = 0 ; /* This parameter is not currently used */
  E = Ef(x0_p);

  if (copyfunc) {
    x = copy_constructor(x0_p);
    new_x = copy_constructor(x0_p);
    best_x = copy_constructor(x0_p);
  } else {
    x = (void *) malloc (element_size);
    memcpy (x, x0_p, element_size);
    new_x = (void *) malloc (element_size);
    best_x =  (void *) malloc (element_size);
    memcpy (best_x, x0_p, element_size);
  }

  best_E = E;

  T = params.t_initial;
  T_factor = 1.0 / params.mu_t;

  if (print_position) {
    printf ("#-iter  #-evals   temperature     position   energy\n");
  }

  while (1) {

    n_accepts = 0;
    n_rejects = 0;
    n_eless = 0;

    for (i = 0; i < params.iters_fixed_T; ++i) {

      copy_state(x, new_x, element_size, copyfunc);

      take_step (r, new_x, params.step_size);
      new_E = Ef (new_x);

      if(new_E <= best_E){
        if (copyfunc) {
          copyfunc(new_x,best_x);
        } else {
          memcpy (best_x, new_x, element_size);
        }
        best_E=new_E;
      }

      ++n_evals;                /* keep track of Ef() evaluations */
      /* now take the crucial step: see if the new point is accepted
         or not, as determined by the boltzmann probability */
      if (new_E < E) {

	if (new_E < best_E) {
	  copy_state(new_x, best_x, element_size, copyfunc);
	  best_E = new_E;
	}

        /* yay! take a step */
	copy_state(new_x, x, element_size, copyfunc);
        E = new_E;
        ++n_eless;

      } else if (gsl_rng_uniform(r) < boltzmann(E, new_E, T, &params)) {
        /* yay! take a step */
	copy_state(new_x, x, element_size, copyfunc);
        E = new_E;
        ++n_accepts;

      } else {
        ++n_rejects;
      }
    }

    if (print_position) {
      /* see if we need to print stuff as we go */
      /*       printf("%5d %12g %5d %3d %3d %3d", n_iter, T, n_evals, */
      /*           100*n_eless/n_steps, 100*n_accepts/n_steps, */
      /*           100*n_rejects/n_steps); */
      printf ("%5d   %7d  %12g", n_iter, n_evals, T);
      print_position (x);
      printf ("  %12g  %12g\n", E, best_E);
    }

    /* apply the cooling schedule to the temperature */
    /* FIXME: I should also introduce a cooling schedule for the iters */
    T *= T_factor;
    ++n_iter;
    if (T < params.t_min) {
      break;
    }
  }

  /* at the end, copy the result onto the initial point, so we pass it
     back to the caller */
  copy_state(best_x, x0_p, element_size, copyfunc);

  if (copyfunc) {
    destructor(x);
    destructor(new_x);
    destructor(best_x);
  } else {
    free (x);
    free (new_x);
    free (best_x);
  }
}

/* implementation of a simulated annealing algorithm with many tries */

void
gsl_siman_solve_many (const gsl_rng * r, void *x0_p, gsl_siman_Efunc_t Ef,
                      gsl_siman_step_t take_step,
                      gsl_siman_metric_t distance,
                      gsl_siman_print_t print_position,
                      size_t element_size,
                      gsl_siman_params_t params)
{
  /* the new set of trial points, and their energies and probabilities */
  void *x, *new_x;
  double *energies, *probs, *sum_probs;
  double Ex;                    /* energy of the chosen point */
  double T, T_factor;           /* the temperature and a step multiplier */
  int i;
  double u;                     /* throw the die to choose a new "x" */
  int n_iter;

  if (print_position) {
    printf ("#-iter    temperature       position");
    printf ("         delta_pos        energy\n");
  }

  x = (void *) malloc (params.n_tries * element_size);
  new_x = (void *) malloc (params.n_tries * element_size);
  energies = (double *) malloc (params.n_tries * sizeof (double));
  probs = (double *) malloc (params.n_tries * sizeof (double));
  sum_probs = (double *) malloc (params.n_tries * sizeof (double));

  T = params.t_initial;
  T_factor = 1.0 / params.mu_t;

  memcpy (x, x0_p, element_size);

  n_iter = 0;
  while (1)
    {
      Ex = Ef (x);
      for (i = 0; i < params.n_tries - 1; ++i)
        {                       /* only go to N_TRIES-2 */
          /* center the new_x[] around x, then pass it to take_step() */
          sum_probs[i] = 0;
          memcpy ((char *)new_x + i * element_size, x, element_size);
          take_step (r, (char *)new_x + i * element_size, params.step_size);
          energies[i] = Ef ((char *)new_x + i * element_size);
          probs[i] = boltzmann(Ex, energies[i], T, &params);
        }
      /* now add in the old value of "x", so it is a contendor */
      memcpy ((char *)new_x + (params.n_tries - 1) * element_size, x, element_size);
      energies[params.n_tries - 1] = Ex;
      probs[params.n_tries - 1] = boltzmann(Ex, energies[i], T, &params);

      /* now throw biased die to see which new_x[i] we choose */
      sum_probs[0] = probs[0];
      for (i = 1; i < params.n_tries; ++i)
        {
          sum_probs[i] = sum_probs[i - 1] + probs[i];
        }
      u = gsl_rng_uniform (r) * sum_probs[params.n_tries - 1];
      for (i = 0; i < params.n_tries; ++i)
        {
          if (u < sum_probs[i])
            {
              memcpy (x, (char *) new_x + i * element_size, element_size);
              break;
            }
        }
      if (print_position)
        {
          printf ("%5d\t%12g\t", n_iter, T);
          print_position (x);
          printf ("\t%12g\t%12g\n", distance (x, x0_p), Ex);
        }
      T *= T_factor;
      ++n_iter;
      if (T < params.t_min)
	{
	  break;
        }
    }

  /* now return the value via x0_p */
  memcpy (x0_p, x, element_size);

  /*  printf("the result is: %g (E=%g)\n", x, Ex); */

  free (x);
  free (new_x);
  free (energies);
  free (probs);
  free (sum_probs);
}


/* Copyright (c) 2014, Giuseppe Argentieri <giuseppe.argentieri@ts.infn.it>

 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * 2. 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.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 */

/*
 * =====================================================================================
 *
 *       Filename:  Img_ss.c
 *
 *    Description:
 *
 *        Version:  1.0
 *        Created:  15/04/2013 16:44:51
 *       Revision:  none
 *       Compiler:  gcc
 *
 *         Author:  Giuseppe Argentieri (), argenti@ts.infn.it
 *   Organization:
 *
 * =====================================================================================
 */

#include "funcs.h"
/* #include <gsl/gsl_sf_expint.h> */

int im_gss ( void* params, double* val, double* error )
{
	struct f_params* pars = (struct f_params*) params ;
	double o_c, b, O, o_1, alpha ;
	assign_p ( pars, &o_c, &b, &O, &o_1 ) ;
	alpha = pars->alpha ;

	double e1, e2, E1, E2 ;
	double err1, err2, ERR1, ERR2 ;

	double beta1 = O + o_1 ;
	double beta2 = o_1 - O ;
	double mu = 1/o_c ;

	expi ( beta1*mu, &e1, &err1 ) ;
	expi_plus ( beta1*mu, &E1, &ERR1 ) ;
	expi_plus ( beta2*mu, &E2, &ERR2 ) ;
	expi ( beta2*mu, &e2, &err2 ) ;

/* 	e1 = - gsl_sf_expint_E1 ( beta1*mu ) ;
 * 	e2 = - gsl_sf_expint_E1 ( beta2*mu ) ;
 * 	E1 = gsl_sf_expint_Ei ( beta1*mu ) ;
 * 	E2 = gsl_sf_expint_Ei ( beta2*mu ) ;
 */

	double imgss = (alpha/4)*(beta1*(e1*exp(beta1*mu)-E1*exp(-beta1*mu)) +
				  beta2*(E2*exp(-beta2*mu)-e2*exp(beta2*mu))) ;
	*val = imgss ;

	double err = (alpha/4)*(beta1*(err1*exp(beta1*mu))+ERR1*exp(-beta1*mu) +
				  beta2*(ERR2*exp(-beta2*mu)+err2*exp(beta2*mu))) ;
	*error = err ;

	return 0;
}


#ifdef USE_MKL
#include <mkl.h>
#else
#include <cblas.h>
#endif

// contains all calls to C-interface BLAS functions
// e.g. http://www.netlib.org/lapack/explore-html/d1/dff/cblas__example1_8c_source.html

// returns the dot product of x and y
// http://www.netlib.org/lapack/explore-html/d5/df6/ddot_8f_source.html
double specex_dot(int n, const double *x, const double *y){
  return cblas_ddot(n, x, 1, y, 1);
}

// y += alpha*x
// http://www.netlib.org/lapack/explore-html/d9/dcd/daxpy_8f_source.html
void specex_axpy(int n, const double *alpha, const double *x, const double *y){
  cblas_daxpy(n, *alpha, x, 1, y, 1);
}

// A += alpha*x*x**T, where A is a symmetric matrx (only lower half is filled)
// http://www.netlib.org/lapack/explore-html/d3/d60/dsyr_8f_source.html
void specex_syr(int n, const double *alpha, const double *x, const double *A){
  cblas_dsyr(CblasColMajor, CblasLower, n, *alpha, x, 1, A, n);
}

// C = alpha*A**T + beta*C, where A is a symmetric matrx
// http://www.netlib.org/lapack/explore-html/dc/d05/dsyrk_8f_source.html
void specex_syrk(int n, int k, const double *alpha, const double *A, const double *beta,
		 const double *C){
  cblas_dsyrk(CblasColMajor, CblasLower, CblasNoTrans, n, k, *alpha, A, n, *beta, C, n);
}

// y = alpha*A*x + beta*y
// http://www.netlib.org/lapack/explore-html/dc/da8/dgemv_8f_source.html
void specex_gemv(int m, int n, const double *alpha, const double *A, const double *x,
		 const double *beta, const double *y){
  cblas_dgemv(CblasColMajor, CblasNoTrans, m, n, *alpha, A, m, x, 1, *beta, y, 1);

}

// C = alpha*A*B + beta*C
// http://www.netlib.org/lapack/explore-html/d7/d2b/dgemm_8f_source.html
void specex_gemm(int m, int n, int k, const double *alpha, const double *A, const double *B,
		 const double *beta, const double *C){
  cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, m, n, k, *alpha, A, m, B, k, *beta, C, m);
}