/*
 * Copyright 2014 Marc Normandin
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 *     Unless required by applicable law or agreed to in writing, software
 *     distributed under the License is distributed on an "AS IS" BASIS,
 *     WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *     See the License for the specific language governing permissions and
 *     limitations under the License.
 */

/*
 * rng.h
 *
 *  Created on: Jul 3, 2013
 *      Author: marc
 */

#ifndef RNG_H_
#define RNG_H_

#include <climits> // ULONG_MAX
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <iostream>

// Seeds for GNU GSL random number generators
typedef unsigned long int gslseed_t;

class RandomNumberGenerator
{
public:
    /*
    RandomNumberGenerator()
    : mRngType( gsl_rng_default ), mSeed( time(0) ) {
    	initializeRNG();
    }*/

    RandomNumberGenerator(const gslseed_t& seed)
        : mRngType( gsl_rng_default ), mSeed( seed )
    {
        initializeRNG();
    }

    RandomNumberGenerator(const RandomNumberGenerator& rhs)
    {
        mRngType = rhs.mRngType;
        mRng = gsl_rng_clone( rhs.mRng );
        mSeed = rhs.randomSeed();
        gsl_rng_set (mRng, mSeed);
    }

    ~RandomNumberGenerator()
    {
        gsl_rng_free( mRng );
    }

    // Random uniform number between 0 and 1.
    double uniform() const
    {
        return gsl_rng_uniform( mRng );
    }

    // Random uniform number between min and max
    double uniform(const double min, const double max) const
    {
        const double u = uniform();
        return ( min + u * (max - min) );
    }

    double gaussian(const double sigma) const
    {
        return ( gsl_ran_gaussian (mRng, sigma) );
    }

    // Beta distribution. Random x between 0 and 1.
    double beta(const double alpha, const double beta) const
    {
        return ( gsl_ran_beta(mRng, alpha, beta) );
    }

    gslseed_t randomSeed() const
    {
    	// I wrote the following after looking into the GSL code itself.
        const unsigned long int offset = mRng->type->min;
        const unsigned long int range = mRng->type->max - offset;
        gslseed_t rseed = 1 + gsl_rng_uniform_int(mRng, range-2);
        //std::cerr << "generated random seed = " << rseed << std::endl;
        return rseed;
    }

protected:
    void initializeRNG()
    {
        mRng = gsl_rng_alloc (mRngType);
        gsl_rng_set (mRng, mSeed);
    }

private:
    // Prevent copying and assignment
    //RandomNumberGenerator(const RandomNumberGenerator&);
    void operator=(const RandomNumberGenerator&);

    const gsl_rng_type* mRngType;
    gslseed_t			mSeed;
    gsl_rng* 			mRng;
};

#endif /* RNG_H_ */


/* Copyright (c) 2011-2012, Jérémy Fix. All rights reserved. */

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

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

/* THIS SOFTWARE IS PROVIDED BY THE AUTHOR 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. */

#ifndef EKF_H
#define EKF_H

#include "ekf_types.h"
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>

namespace ekf
{

  void ekf_init(ekf_param &p, ekf_state &s)
  {

    // Matrices for the state
    s.xk = gsl_vector_alloc(p.n);
    gsl_vector_set_zero(s.xk);

    s.xkm = gsl_vector_alloc(p.n);
    gsl_vector_set_zero(s.xkm);

    s.Pxk = gsl_matrix_alloc(p.n, p.n);
    gsl_matrix_set_zero(s.Pxk);

    s.Fxk = gsl_matrix_alloc(p.n, p.n);
    gsl_matrix_set_zero(s.Fxk);

    s.Rv = gsl_matrix_alloc(p.n, p.n);
    gsl_matrix_set_zero(s.Rv);

    // Matrices for the observations
    s.yk = gsl_vector_alloc(p.no);
    gsl_vector_set_zero(s.yk);

    s.ino_yk = gsl_vector_alloc(p.no);
    gsl_vector_set_zero(s.ino_yk);

    s.Hyk = gsl_matrix_alloc(p.no, p.n);
    gsl_matrix_set_zero(s.Hyk);

    s.Rn = gsl_matrix_alloc(p.no, p.no);
    gsl_matrix_set_zero(s.Rn);

    // Matrices for the kalman gain and the updates
    s.Kk = gsl_matrix_alloc(p.n, p.no);
    gsl_matrix_set_zero(s.Kk);

    // Temporary matrices
    s.temp_n_n = gsl_matrix_alloc(p.n, p.n);
    gsl_matrix_set_zero(s.temp_n_n);

    s.temp_n_1 = gsl_matrix_alloc(p.n, 1);
    gsl_matrix_set_zero(s.temp_n_1);

    s.temp_no_no = gsl_matrix_alloc(p.no, p.no);
    gsl_matrix_set_zero(s.temp_no_no);

    s.temp_n_no = gsl_matrix_alloc(p.n, p.no);
    gsl_matrix_set_zero(s.temp_n_no);

    s.temp_2_n_n = gsl_matrix_alloc(p.n, p.n);
    gsl_matrix_set_zero(s.temp_2_n_n);

    s.temp_no = gsl_vector_alloc(p.no);
    gsl_vector_set_zero(s.temp_no);

    // Initialize the noises
    //gsl_matrix_set_identity(s.Rv);
    //gsl_matrix_scale(s.Rv, p.evolution_noise);
    p.evolution_noise->init(p, s);
    //printf("Evolution noise : max = %e , min = %e \n", gsl_matrix_max(s.Rv), gsl_matrix_min(s.Rv));

    gsl_matrix_set_identity(s.Rn);
    gsl_matrix_scale(s.Rn, p.observation_noise);

    // Initialize the covariance of the parameters
    gsl_matrix_set_identity(s.Pxk);
    gsl_matrix_scale(s.Pxk, p.prior_pk);

  }

  void ekf_free(ekf_param &p, ekf_state &s)
  {
    gsl_vector_free(s.xk);
    gsl_vector_free(s.xkm);
    gsl_matrix_free(s.Pxk);
    gsl_matrix_free(s.Fxk);
    gsl_matrix_free(s.Rv);

    gsl_vector_free(s.yk);
    gsl_vector_free(s.ino_yk);
    gsl_matrix_free(s.Hyk);
    gsl_matrix_free(s.Rn);

    gsl_matrix_free(s.Kk);

    gsl_matrix_free(s.temp_n_n);
    gsl_matrix_free(s.temp_n_1);
    gsl_matrix_free(s.temp_no_no);
    gsl_matrix_free(s.temp_n_no);
    gsl_matrix_free(s.temp_2_n_n);
    gsl_vector_free(s.temp_no);
  }
  template<typename FFunc,
    typename DFFunc,
    typename HFunc,
    typename DHFunc>
    void ekf_iterate(ekf_param &p, ekf_state &s,
		     FFunc f, DFFunc df,
		     HFunc h, DHFunc dh,
		     gsl_vector* yk)
  {
    /****************************/
    /***** Prediction step  *****/
    /****************************/

    // Compute the Jacobian of the evolution
    // Eq. 2.34
    df(s.params, s.xk, s.Fxk);

    // Compute the predicted state mean and covariance
    // Eq. 2.36
    // s.xk will now hold the predicted state !
    f(s.params, s.xk, s.xkm);

    // Eq. 2.37
    // s.Pxk will now hold Pxk^-
    gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, s.Pxk, s.Fxk, 0.0, s.temp_n_n);
    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, s.Fxk, s.temp_n_n, 0.0, s.Pxk);
    gsl_matrix_add(s.Pxk, s.Rv);

    /****************************/
    /***** Correction step  *****/
    /****************************/

    // Compute the Jacobian of the observation model
    // Eq. 2.38
    dh(s.params, s.xkm, s.Hyk);

    if(!p.observation_gradient_is_diagonal)
      {
	// Update the estimates
	// Eq 2.40
	// 1 - Compute H.P^-.H^T + Rn
	gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, s.Pxk, s.Hyk, 0.0, s.temp_n_no);
	gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, s.Hyk, s.temp_n_no, 0.0, s.temp_no_no);
	gsl_matrix_add(s.temp_no_no, s.Rn);
	// 2 - Compute its inverse
	gsl_linalg_cholesky_decomp(s.temp_no_no);
	gsl_linalg_cholesky_invert(s.temp_no_no);

	// 3 - Compute P^-.H ^T.( H P H^T + R)^-1
	gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, s.Hyk, s.temp_no_no, 0.0, s.temp_n_no);
	gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, s.Pxk, s.temp_n_no, 0.0, s.Kk);
      }
    else
      {
	// We can make some simplifications when computing H P H^T = H P H
	// and also when computing P . H . (H P H^T + R) ^-1

	// Update the estimates
	// Eq 2.40
	// 1 - Compute H.P^-.H^T + Rn
	for(int i = 0 ; i < p.no ; ++i)
	  {
	    for(int j = 0 ; j < p.no ; ++j)
	      {
		gsl_matrix_set(s.temp_no_no, i, j, gsl_matrix_get(s.Hyk, i, i) * gsl_matrix_get(s.Hyk, j, j) * gsl_matrix_get(s.Pxk, i, j) + gsl_matrix_get(s.Rn, i, j));
	      }
	  }
	// Compute its inverse : U = (H P H^T + R) ^-1
	gsl_linalg_cholesky_decomp(s.temp_no_no);
	gsl_linalg_cholesky_invert(s.temp_no_no);

	// 3 - Compute P^- H^T . U

	// Compute H^T . U
	for(int i = 0 ; i < p.no; ++i)
	  for(int j = 0 ; j < p.no ; ++j)
	    gsl_matrix_set(s.temp_n_no, i, j, gsl_matrix_get(s.Hyk, i,i) * gsl_matrix_get(s.temp_no_no, i, j));
	for(int i = p.no ; i < p.n; ++i)
	  for(int j = 0 ; j < p.no ; ++j)
	    gsl_matrix_set(s.temp_n_no, i, j, 0.0);
	// Compute P^- . (H^T . U)
	gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, s.Pxk, s.temp_n_no, 0.0, s.Kk);

      }
    // Update the current estimate
    // Eq 2.41
    // 1 - We need the observations
    h(s.params, s.xkm, s.yk);
    gsl_vector_memcpy(s.ino_yk, yk);
    gsl_vector_sub(s.ino_yk, s.yk);
    gsl_vector_memcpy(s.xk, s.xkm);
    gsl_blas_dgemv(CblasNoTrans, 1.0, s.Kk, s.ino_yk, 1.0,s.xk);

    // Update the variance/covariance matrix
    // Compute  -Kk * Hk
    if(!p.observation_gradient_is_diagonal)
      {
	gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, s.Kk, s.Hyk, 0.0, s.temp_n_n);

	// Add identity : I - Kk * Hk
	for(int i = 0 ; i < p.n ; ++i)
	  gsl_matrix_set(s.temp_n_n, i, i,gsl_matrix_get(s.temp_n_n,i,i) + 1.0);
      }
    else
      {
	// We can make some simplifications when H is diagonal
	for(int i = 0 ; i < p.n ; ++i)
	  {
	    for(int j = 0 ; j < p.no ; ++j)
	      {
		gsl_matrix_set(s.temp_n_n, i, j, (i==j?1.0:0.0) - gsl_matrix_get(s.Kk, i,j) * gsl_matrix_get(s.Hyk, j,j) );
	      }
	    for(int j = p.no ; j < p.n ; ++j)
	      {
		gsl_matrix_set(s.temp_n_n, i, j, (i == j ? 1.0 : 0.0));
	      }
	  }
      }

    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, s.temp_n_n, s.Pxk, 0.0, s.temp_2_n_n);
    gsl_matrix_memcpy(s.Pxk, s.temp_2_n_n);

    /***********************************/
    /***** Evolution noise update  *****/
    /***********************************/

    p.evolution_noise->updateEvolutionNoise(p, s);
  }
}

#endif // EKF_H


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

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

#include "ccl.h"

ccl_f2d_t *ccl_f2d_t_copy(ccl_f2d_t *f2d_o, int *status)
{
  int s2dstatus=0;
  ccl_f2d_t *f2d = malloc(sizeof(ccl_f2d_t));
  if (f2d == NULL)
    *status = CCL_ERROR_MEMORY;

  if(*status==0) {
    f2d->lkmin = f2d_o->lkmin;
    f2d->lkmax = f2d_o->lkmax;
    f2d->amin = f2d_o->amin;
    f2d->amax = f2d_o->amax;
    f2d->is_factorizable = f2d_o->is_factorizable;
    f2d->is_k_constant = f2d_o->is_k_constant;
    f2d->is_a_constant = f2d_o->is_a_constant;
    f2d->extrap_linear_growth = f2d_o->extrap_linear_growth;
    f2d->extrap_order_lok = f2d_o->extrap_order_lok;
    f2d->extrap_order_hik = f2d_o->extrap_order_hik;
    f2d->is_log = f2d_o->is_log;
    f2d->growth = f2d_o->growth;
    f2d->growth_factor_0 = f2d_o->growth_factor_0;
    f2d->growth_exponent = f2d_o->growth_exponent;

    if(f2d_o->fk != NULL) {
      f2d->fk = gsl_spline_alloc(gsl_interp_cspline,
                                 f2d_o->fk->size);
      if(f2d->fk == NULL)
        *status = CCL_ERROR_MEMORY;

      if(*status==0) {
        s2dstatus |= gsl_spline_init(f2d->fk, f2d_o->fk->x,
                                     f2d_o->fk->y, f2d_o->fk->size);
        if(s2dstatus)
          *status = CCL_ERROR_SPLINE;
      }
    }
    else
      f2d->fk = NULL;
  }

  if(*status==0) {
    if(f2d_o->fa != NULL) {
      f2d->fa = gsl_spline_alloc(gsl_interp_cspline,
                                 f2d_o->fa->size);
      if(f2d->fa == NULL)
        *status = CCL_ERROR_MEMORY;

      if(*status==0) {
        s2dstatus |= gsl_spline_init(f2d->fa, f2d_o->fa->x,
                                     f2d_o->fa->y, f2d_o->fa->size);
        if(s2dstatus)
          *status = CCL_ERROR_SPLINE;
      }
    }
    else
      f2d->fa = NULL;
  }

  if(*status==0) {
    if(f2d_o->fka != NULL) {
      f2d->fka = gsl_spline2d_alloc(gsl_interp2d_bicubic,
                                    f2d_o->fka->interp_object.xsize,
                                    f2d_o->fka->interp_object.ysize);
      if(f2d->fka == NULL)
        *status = CCL_ERROR_MEMORY;

      if(*status==0) {
        s2dstatus |= gsl_spline2d_init(f2d->fka, f2d_o->fka->xarr,
                                       f2d_o->fka->yarr, f2d_o->fka->zarr,
                                       f2d_o->fka->interp_object.xsize,
                                       f2d_o->fka->interp_object.ysize);
        if(s2dstatus)
          *status = CCL_ERROR_SPLINE;
      }
    }
    else
      f2d->fka = NULL;
  }

  return f2d;
}

ccl_f2d_t *ccl_f2d_t_new(int na,double *a_arr,
                         int nk,double *lk_arr,
                         double *fka_arr,
                         double *fk_arr,
                         double *fa_arr,
                         int is_factorizable,
                         int extrap_order_lok,
                         int extrap_order_hik,
                         ccl_f2d_extrap_growth_t extrap_linear_growth,
                         int is_fka_log,
                         double (*growth)(double),
                         double growth_factor_0,
                         int growth_exponent,
                         ccl_f2d_interp_t interp_type,
                         int *status) {
  int s2dstatus=0;
  ccl_f2d_t *f2d = malloc(sizeof(ccl_f2d_t));
  if (f2d == NULL)
    *status = CCL_ERROR_MEMORY;

  if (*status == 0) {
    is_factorizable = is_factorizable || (a_arr == NULL) || (lk_arr == NULL) || (fka_arr == NULL);
    f2d->is_factorizable = is_factorizable;
    f2d->is_k_constant = ((lk_arr == NULL) || ((fka_arr == NULL) && (fk_arr == NULL)));
    f2d->is_a_constant = ((a_arr == NULL) || ((fka_arr == NULL) && (fa_arr == NULL)));
    f2d->extrap_order_lok = extrap_order_lok;
    f2d->extrap_order_hik = extrap_order_hik;
    f2d->extrap_linear_growth = extrap_linear_growth;
    f2d->is_log = is_fka_log;
    f2d->growth = growth;
    f2d->growth_factor_0 = growth_factor_0;
    f2d->growth_exponent = growth_exponent;
    f2d->fka = NULL;
    f2d->fk = NULL;
    f2d->fa = NULL;

    if (!(f2d->is_k_constant)) { //If it's not constant
      f2d->lkmin = lk_arr[0];
      f2d->lkmax = lk_arr[nk-1];
    }
    if (!(f2d->is_a_constant)) {
      f2d->amin = a_arr[0];
      f2d->amax = a_arr[na-1];
    }
  }

  if ((extrap_order_lok > 2) || (extrap_order_lok < 0) || (extrap_order_hik > 2) || (extrap_order_hik < 0))
    *status = CCL_ERROR_INCONSISTENT;

  if ((extrap_linear_growth != ccl_f2d_cclgrowth) &&
      (extrap_linear_growth != ccl_f2d_customgrowth) &&
      (extrap_linear_growth != ccl_f2d_constantgrowth) &&
      (extrap_linear_growth != ccl_f2d_no_extrapol))
    *status = CCL_ERROR_INCONSISTENT;

  if(*status == 0) {
    switch(interp_type) {
    case(ccl_f2d_3):
      if (f2d->is_factorizable) {
        // Do not allocate spline if constant
        if(f2d->is_k_constant)
          f2d->fk = NULL;
        else { //Otherwise allocate and check
          f2d->fk = gsl_spline_alloc(gsl_interp_cspline, nk);
          if(f2d->fk == NULL)
            *status = CCL_ERROR_MEMORY;
        }

        // Do not allocate spline if constant
        if (f2d->is_a_constant)
          f2d->fa = NULL;
        else { //Otherwise allocate and check
          f2d->fa = gsl_spline_alloc(gsl_interp_cspline, na);
          if (f2d->fa == NULL)
            *status = CCL_ERROR_MEMORY;
        }
      }
      else {
        // Do not allocate spline if constant
        if ((f2d->is_k_constant) || (f2d->is_a_constant))
          f2d->fka = NULL;
        else { //Otherwise allocate and check
          f2d->fka = gsl_spline2d_alloc(gsl_interp2d_bicubic, nk, na);
          if (f2d->fka == NULL)
            *status = CCL_ERROR_MEMORY;
        }
      }
      break;

    default:
      f2d->fk = NULL;
      f2d->fa = NULL;
      f2d->fka = NULL;
    }
  }

  if (*status == 0) {
    if (f2d->is_factorizable) {
      if (f2d->fk != NULL)
        s2dstatus |= gsl_spline_init(f2d->fk, lk_arr, fk_arr, nk);
      if (f2d->fa != NULL)
        s2dstatus |= gsl_spline_init(f2d->fa, a_arr, fa_arr, na);
    }
    else {
      if (f2d->fka != NULL)
        s2dstatus=gsl_spline2d_init(f2d->fka, lk_arr, a_arr, fka_arr, nk, na);
    }
    if (s2dstatus)
      *status = CCL_ERROR_SPLINE;
  }

  return f2d;
}

double ccl_f2d_t_eval(ccl_f2d_t *f2d,double lk,double a,void *cosmo, int *status) {
  int is_hiz, is_loz;
  double a_ev = a;
  if (f2d->is_a_constant) {
    is_hiz = 0;
    is_loz = 0;
  }
  else {
    is_hiz = a < f2d->amin;
    is_loz = a > f2d->amax;
    if (is_loz) { // Are we above the interpolation range in a?
      if (f2d->extrap_linear_growth == ccl_f2d_no_extrapol) {
        *status=CCL_ERROR_SPLINE_EV;
        return NAN;
      }
      a_ev = f2d->amax;
    }
    else if (is_hiz) { // Are we below the interpolation range in a?
      if (f2d->extrap_linear_growth == ccl_f2d_no_extrapol) {
        *status=CCL_ERROR_SPLINE_EV;
        return NAN;
      }
      a_ev = f2d->amin;
    }
  }

  int is_hik, is_lok;
  double fka_pre, fka_post;
  double lk_ev = lk;
  if (f2d->is_k_constant) {
    is_hik = 0;
    is_lok = 0;
  }
  else {
    is_hik = lk > f2d->lkmax;
    is_lok = lk < f2d->lkmin;
    if (is_hik) // Are we above the interpolation range in k?
      lk_ev = f2d->lkmax;
    else if (is_lok) // Are we below the interpolation range in k?
      lk_ev = f2d->lkmin;
  }

  // Evaluate spline
  int spstatus=0;
  if (f2d->is_factorizable) {
    double fk, fa;
    if (f2d->fk == NULL) {
      if (f2d->is_log)
        fk = 0;
      else
        fk = 1;
    }
    else
      spstatus |= gsl_spline_eval_e(f2d->fk, lk_ev, NULL, &fk);

    if (f2d->fa == NULL) {
      if (f2d->is_log)
        fa = 0;
      else
        fa = 1;
    }
    else
      spstatus |= gsl_spline_eval_e(f2d->fa, a_ev, NULL, &fa);
      if (f2d->is_log)
        fka_pre = fk+fa;
      else
        fka_pre = fk*fa;
  }
  else {
    if (f2d->fka == NULL) {
      if (f2d->is_log)
        fka_pre = 0;
      else
        fka_pre = 1;
    }
    else
      spstatus = gsl_spline2d_eval_e(f2d->fka, lk_ev, a_ev, NULL, NULL, &fka_pre);
  }

  if (spstatus) {
    *status = CCL_ERROR_SPLINE_EV;
    return NAN;
  }

  // Now extrapolate in k if needed
  if (is_hik) {
    fka_post = fka_pre;
    if (f2d->extrap_order_hik > 0) {
      double pd;
      double dlk = lk-lk_ev;
      if (f2d->is_factorizable)
        spstatus = gsl_spline_eval_deriv_e(f2d->fk, lk_ev, NULL, &pd);
      else
        spstatus = gsl_spline2d_eval_deriv_x_e(f2d->fka, lk_ev, a_ev, NULL, NULL, &pd);
      if (spstatus) {
        *status = CCL_ERROR_SPLINE_EV;
        return NAN;
      }
      fka_post += pd*dlk;
      if (f2d->extrap_order_hik > 1) {
        if (f2d->is_factorizable)
          spstatus = gsl_spline_eval_deriv2_e(f2d->fk, lk_ev, NULL, &pd);
        else
          spstatus = gsl_spline2d_eval_deriv_xx_e(f2d->fka, lk_ev, a_ev, NULL, NULL, &pd);
        if (spstatus) {
          *status=CCL_ERROR_SPLINE_EV;
          return NAN;
        }
        fka_post += pd*dlk*dlk*0.5;
      }
    }
  }
  else if (is_lok) {
    fka_post = fka_pre;
    if (f2d->extrap_order_lok > 0) {
      double pd;
      double dlk = lk-lk_ev;
      if (f2d->is_factorizable)
        spstatus = gsl_spline_eval_deriv_e(f2d->fk, lk_ev, NULL, &pd);
      else
        spstatus = gsl_spline2d_eval_deriv_x_e(f2d->fka, lk_ev, a_ev, NULL, NULL, &pd);
      if (spstatus) {
        *status = CCL_ERROR_SPLINE_EV;
        return NAN;
      }
      fka_post += pd*dlk;

      if (f2d->extrap_order_lok > 1) {
        if (f2d->is_factorizable)
          spstatus = gsl_spline_eval_deriv2_e(f2d->fk, lk_ev, NULL, &pd);
        else
          spstatus = gsl_spline2d_eval_deriv_xx_e(f2d->fka, lk_ev, a_ev, NULL, NULL, &pd);
        if (spstatus) {
          *status = CCL_ERROR_SPLINE_EV;
          return NAN;
        }
        fka_post += pd*dlk*dlk*0.5;
      }
    }
  }
  else
    fka_post = fka_pre;

  // Exponentiate if needed
  if (f2d->is_log)
    fka_post = exp(fka_post);

  // Extrapolate in a if needed
  if (is_hiz) {
    double gz;
    if (f2d->extrap_linear_growth == ccl_f2d_cclgrowth) { // Use CCL's growth function
      ccl_cosmology *csm = (ccl_cosmology *)cosmo;
      if (!csm->computed_growth) {
        *status = CCL_ERROR_GROWTH_INIT;
        ccl_cosmology_set_status_message(
          csm,
          "ccl_f2d.c: ccl_f2d_t_eval(): growth factor splines have not been precomputed!");
        return NAN;
      }
      gz = (
        ccl_growth_factor(csm, a, status) /
        ccl_growth_factor(csm, a_ev, status));
    }
    else if (f2d->extrap_linear_growth == ccl_f2d_customgrowth) // Use internal growth function
      gz = f2d->growth(a) / f2d->growth(a_ev);
    else // Use constant growth factor
      gz = f2d->growth_factor_0;

    fka_post *= pow(gz, f2d->growth_exponent);
  }

  return fka_post;
}

void ccl_f2d_t_free(ccl_f2d_t *f2d)
{
  if(f2d != NULL) {
    if(f2d->fka != NULL)
      gsl_spline2d_free(f2d->fka);
    if(f2d->fk != NULL)
      gsl_spline_free(f2d->fk);
    if(f2d->fa != NULL)
      gsl_spline_free(f2d->fa);
    free(f2d);
  }
}


/* normal.c
 *
 * Copyright (C) 2015, 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 <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_multifit.h>
#include <gsl/gsl_multilarge.h>

typedef struct
{
  size_t p;              /* number of columns of LS matrix */
  gsl_matrix *ATA;       /* A^T A, p-by-p */
  gsl_vector *ATb;       /* A^T b, p-by-1 */
  double normb;          /* || b || */
  gsl_matrix *work_ATA;  /* workspace for chol(ATA), p-by-p */
  gsl_vector *workp;     /* workspace size p */
  gsl_vector *work3p;    /* workspace size 3*p */
  gsl_vector *D;         /* scale factors for ATA, size p */
  gsl_vector *c;         /* solution vector for L-curve */
  int eigen;             /* 1 if eigenvalues computed */
  double eval_min;       /* minimum eigenvalue */
  double eval_max;       /* maximum eigenvalue */
  gsl_eigen_symm_workspace *eigen_p;
} normal_state_t;

static void *normal_alloc(const size_t p);
static void normal_free(void *vstate);
static int normal_reset(void *vstate);
static int normal_accumulate(gsl_matrix * A, gsl_vector * b,
                             void * vstate);
static int normal_solve(const double lambda, gsl_vector * x,
                        double * rnorm, double * snorm,
                        void * vstate);
static int normal_rcond(double * rcond, void * vstate);
static int normal_lcurve(gsl_vector * reg_param, gsl_vector * rho,
                         gsl_vector * eta, void * vstate);
static const gsl_matrix * normal_ATA(const void * vstate);
static const gsl_vector * normal_ATb(const void * vstate);
static int normal_solve_system(const double lambda, gsl_vector * x,
                               normal_state_t *state);
static int normal_solve_cholesky(gsl_matrix * ATA, const gsl_vector * ATb,
                                 gsl_vector * x, normal_state_t *state);
static int normal_calc_norms(const gsl_vector *x, double *rnorm,
                             double *snorm, normal_state_t *state);
static int normal_eigen(normal_state_t *state);

/*
normal_alloc()
  Allocate workspace for solving large linear least squares
problems using the normal equations approach

Inputs: p    - number of columns of LS matrix

Return: pointer to workspace
*/

static void *
normal_alloc(const size_t p)
{
  normal_state_t *state;

  if (p == 0)
    {
      GSL_ERROR_NULL("p must be a positive integer",
                     GSL_EINVAL);
    }

  state = calloc(1, sizeof(normal_state_t));
  if (!state)
    {
      GSL_ERROR_NULL("failed to allocate normal state", GSL_ENOMEM);
    }

  state->p = p;

  state->ATA = gsl_matrix_alloc(p, p);
  if (state->ATA == NULL)
    {
      normal_free(state);
      GSL_ERROR_NULL("failed to allocate ATA matrix", GSL_ENOMEM);
    }

  state->work_ATA = gsl_matrix_alloc(p, p);
  if (state->work_ATA == NULL)
    {
      normal_free(state);
      GSL_ERROR_NULL("failed to allocate temporary ATA matrix", GSL_ENOMEM);
    }

  state->ATb = gsl_vector_alloc(p);
  if (state->ATb == NULL)
    {
      normal_free(state);
      GSL_ERROR_NULL("failed to allocate ATb vector", GSL_ENOMEM);
    }

  state->D = gsl_vector_alloc(p);
  if (state->D == NULL)
    {
      normal_free(state);
      GSL_ERROR_NULL("failed to allocate D vector", GSL_ENOMEM);
    }

  state->workp = gsl_vector_alloc(p);
  if (state->workp == NULL)
    {
      normal_free(state);
      GSL_ERROR_NULL("failed to allocate temporary ATb vector", GSL_ENOMEM);
    }

  state->work3p = gsl_vector_alloc(3 * p);
  if (state->work3p == NULL)
    {
      normal_free(state);
      GSL_ERROR_NULL("failed to allocate work3p", GSL_ENOMEM);
    }

  state->c = gsl_vector_alloc(p);
  if (state->c == NULL)
    {
      normal_free(state);
      GSL_ERROR_NULL("failed to allocate c vector", GSL_ENOMEM);
    }

  state->eigen_p = gsl_eigen_symm_alloc(p);
  if (state->eigen_p == NULL)
    {
      normal_free(state);
      GSL_ERROR_NULL("failed to allocate eigen workspace", GSL_ENOMEM);
    }

  normal_reset(state);

  return state;
}

static void
normal_free(void *vstate)
{
  normal_state_t *state = (normal_state_t *) vstate;

  if (state->ATA)
    gsl_matrix_free(state->ATA);

  if (state->work_ATA)
    gsl_matrix_free(state->work_ATA);

  if (state->ATb)
    gsl_vector_free(state->ATb);

  if (state->D)
    gsl_vector_free(state->D);

  if (state->workp)
    gsl_vector_free(state->workp);

  if (state->work3p)
    gsl_vector_free(state->work3p);

  if (state->c)
    gsl_vector_free(state->c);

  if (state->eigen_p)
    gsl_eigen_symm_free(state->eigen_p);

  free(state);
}

static int
normal_reset(void *vstate)
{
  normal_state_t *state = (normal_state_t *) vstate;

  gsl_matrix_set_zero(state->ATA);
  gsl_vector_set_zero(state->ATb);
  state->normb = 0.0;
  state->eigen = 0;
  state->eval_min = 0.0;
  state->eval_max = 0.0;

  return GSL_SUCCESS;
}

/*
normal_accumulate()
  Add a new block of rows to the normal equations system

Inputs: A      - new block of rows, n-by-p
        b      - new rhs vector n-by-1
        vstate - workspace

Return: success/error
*/

static int
normal_accumulate(gsl_matrix * A, gsl_vector * b, void * vstate)
{
  normal_state_t *state = (normal_state_t *) vstate;
  const size_t n = A->size1;

  if (A->size2 != state->p)
    {
      GSL_ERROR("columns of A do not match workspace", GSL_EBADLEN);
    }
  else if (n != b->size)
    {
      GSL_ERROR("A and b have different numbers of rows", GSL_EBADLEN);
    }
  else
    {
      int s;

      /* ATA += A^T A, using only the lower half of the matrix */
      s = gsl_blas_dsyrk(CblasLower, CblasTrans, 1.0, A, 1.0, state->ATA);
      if (s)
        return s;

      /* ATb += A^T b */
      s = gsl_blas_dgemv(CblasTrans, 1.0, A, b, 1.0, state->ATb);
      if (s)
        return s;

      /* update || b || */
      state->normb = gsl_hypot(state->normb, gsl_blas_dnrm2(b));

      return GSL_SUCCESS;
    }
}

/*
normal_solve()
  Solve normal equations system:

(A^T A + \lambda^2 I) x = A^T b

using Cholesky decomposition

Inputs: lambda - regularization parameter
        x      - (output) solution vector p-by-1
        rnorm  - (output) residual norm ||b - A x||
        snorm  - (output) solution norm ||x||
        vstate - workspace

Return: success/error
*/

static int
normal_solve(const double lambda, gsl_vector * x,
             double * rnorm, double * snorm,
             void * vstate)
{
  normal_state_t *state = (normal_state_t *) vstate;

  if (x->size != state->p)
    {
      GSL_ERROR("solution vector does not match workspace", GSL_EBADLEN);
    }
  else
    {
      int status;

      /* solve system (A^T A) x = A^T b */
      status = normal_solve_system(lambda, x, state);
      if (status)
        {
          GSL_ERROR("failed to solve normal equations", status);
        }

      /* compute residual norm ||y - X c|| and solution norm ||x|| */
      normal_calc_norms(x, rnorm, snorm, state);

      return GSL_SUCCESS;
    }
}

static int
normal_rcond(double * rcond, void * vstate)
{
  normal_state_t *state = (normal_state_t *) vstate;
  int status = GSL_SUCCESS;
  double rcond_ATA;

  status = gsl_linalg_cholesky_rcond(state->work_ATA, &rcond_ATA, state->work3p);
  if (status == GSL_SUCCESS)
    *rcond = sqrt(rcond_ATA);

  return status;
}

/*
normal_lcurve()
  Compute L-curve of least squares system

Inputs: reg_param - (output) vector of regularization parameters
        rho       - (output) vector of residual norms
        eta       - (output) vector of solution norms
        vstate    - workspace

Return: success/error
*/

static int
normal_lcurve(gsl_vector * reg_param, gsl_vector * rho,
              gsl_vector * eta, void * vstate)
{
  normal_state_t *state = (normal_state_t *) vstate;
  int status;
  double smin, smax; /* minimum/maximum singular values */
  size_t i;

  if (state->eigen == 0)
    {
      status = normal_eigen(state);
      if (status)
        return status;
    }

  if (state->eval_max < 0.0)
    {
      GSL_ERROR("matrix is not positive definite", GSL_EDOM);
    }

  /* compute singular values which are sqrts of eigenvalues */
  smax = sqrt(state->eval_max);
  if (state->eval_min > 0.0)
    smin = sqrt(state->eval_min);
  else
    smin = 0.0;

  /* compute vector of regularization parameters */
  gsl_multifit_linear_lreg(smin, smax, reg_param);

  /* solve normal equations for each regularization parameter */
  for (i = 0; i < reg_param->size; ++i)
    {
      double lambda = gsl_vector_get(reg_param, i);
      double rnorm, snorm;

      status = normal_solve_system(lambda, state->c, state);
      if (status)
        return status;

      /* compute ||y - X c|| and ||c|| */
      normal_calc_norms(state->c, &rnorm, &snorm, state);

      gsl_vector_set(rho, i, rnorm);
      gsl_vector_set(eta, i, snorm);
    }

  return GSL_SUCCESS;
}

static const gsl_matrix *
normal_ATA(const void * vstate)
{
  const normal_state_t *state = (const normal_state_t *) vstate;
  return state->ATA;
}

static const gsl_vector *
normal_ATb(const void * vstate)
{
  const normal_state_t *state = (const normal_state_t *) vstate;
  return state->ATb;
}

/*
normal_solve_system()
  Compute solution to normal equations:

(A^T A + lambda^2*I) x = A^T b

using LDL decomposition.

Inputs: x     - (output) solution vector
        state - workspace

Return: success/error
*/

static int
normal_solve_system(const double lambda, gsl_vector * x, normal_state_t *state)
{
  int status;
  const double lambda_sq = lambda * lambda;
  gsl_vector_view d = gsl_matrix_diagonal(state->work_ATA);

  /* copy ATA matrix to temporary workspace and regularize */
  gsl_matrix_tricpy(CblasLower, CblasNonUnit, state->work_ATA, state->ATA);
  gsl_vector_add_constant(&d.vector, lambda_sq);

  /* solve with Cholesky decomposition */
  status = normal_solve_cholesky(state->work_ATA, state->ATb, x, state);
  if (status)
    return status;

  return status;
}

static int
normal_solve_cholesky(gsl_matrix * ATA, const gsl_vector * ATb,
                      gsl_vector * x, normal_state_t *state)
{
  int status;

  status = gsl_linalg_cholesky_decomp2(ATA, state->D);
  if (status)
    return status;

  status = gsl_linalg_cholesky_solve2(ATA, state->D, ATb, x);
  if (status)
    return status;

  return GSL_SUCCESS;
}

/*
normal_calc_norms()
  Compute residual norm ||y - X c|| and solution
norm ||c||

Inputs: x     - solution vector
        rnorm - (output) residual norm ||y - X c||
        snorm - (output) solution norm ||c||
        state - workspace
*/

static int
normal_calc_norms(const gsl_vector *x, double *rnorm,
                  double *snorm, normal_state_t *state)
{
  double r2;

  /* compute solution norm ||x|| */
  *snorm = gsl_blas_dnrm2(x);

  /* compute residual norm ||b - Ax|| */

  /* compute: A^T A x - 2 A^T b */
  gsl_vector_memcpy(state->workp, state->ATb);
  gsl_blas_dsymv(CblasLower, 1.0, state->ATA, x, -2.0, state->workp);

  /* compute: x^T A^T A x - 2 x^T A^T b */
  gsl_blas_ddot(x, state->workp, &r2);

  /* add b^T b */
  r2 += state->normb * state->normb;

  *rnorm = sqrt(r2);

  return GSL_SUCCESS;
}

/*
normal_eigen()
  Compute eigenvalues of A^T A matrix, which
are stored in state->workp on output. Also,
state->eval_min and state->eval_max are set
to the minimum/maximum eigenvalues
*/

static int
normal_eigen(normal_state_t *state)
{
  int status;

  /* copy lower triangle of ATA to temporary workspace */
  gsl_matrix_tricpy(CblasLower, CblasNonUnit, state->work_ATA, state->ATA);

  /* compute eigenvalues of ATA */
  status = gsl_eigen_symm(state->work_ATA, state->workp, state->eigen_p);
  if (status)
    return status;

  gsl_vector_minmax(state->workp, &state->eval_min, &state->eval_max);

  state->eigen = 1;

  return GSL_SUCCESS;
}

static const gsl_multilarge_linear_type normal_type =
{
  "normal",
  normal_alloc,
  normal_reset,
  normal_accumulate,
  normal_solve,
  normal_rcond,
  normal_lcurve,
  normal_ATA,
  normal_ATb,
  normal_free
};

const gsl_multilarge_linear_type * gsl_multilarge_linear_normal =
  &normal_type;


/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

/*
 Contains collection of low-level routines for handling lists,
 one-hot matrices, evaluation metrics (f1-scores), and other.

 Dimitris Berberidis
 University of Minnesota 2017-2018
*/

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

#include <stddef.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <time.h>
#include <inttypes.h>
#include <cblas.h>
#include <sys/stat.h>
#include <stdbool.h>

#include "my_utils.h"
#include "my_defs.h"

void assert_all_nodes_present(csr_graph graph, const sz_long* seed_indices, sz_med num_seeds){

	for(sz_med i=0;i<num_seeds;i++){
		if(seed_indices[i]>graph.num_nodes){
			printf("ERROR: Seed node index does not appear in edgelist (probably an isolated node)\n");
		        exit(EXIT_FAILURE);
		}
	}

}

sz_long rand_lim(sz_long limit) {
	/* return a random number between 0 and limit inclusive.
	 */
	int divisor = RAND_MAX/((int)limit+1);
	int retval;
	do {
		retval = rand() / divisor;
	} while (retval > limit);
	return retval;
}

//Draw random samples with replacement from 0 to N-1
void random_sample( sz_long* seeds, abstract_labels labels, abstract_labels all_labels, sz_med num_seeds, sz_long N){

	sz_long temp;
	sz_short flag;

	//Draw seed indexes
	seeds[0]=rand_lim(N-1);
	for(sz_med i=1;i<num_seeds;i++){
		do{
			temp=rand_lim(N-1);
			flag=0;
			for(sz_med j=0;j<i;j++){
				if(temp==seeds[j])
					flag=1;
			}
		}while(flag==1);
		seeds[i]=temp;
	}

	//Draw corresponding labels
	if(all_labels.is_multilabel){
		for(sz_med i=0;i<num_seeds;i++){
			for(sz_short j=0; j<all_labels.mlabel.num_class;j++)
				labels.mlabel.bin[j][i] = all_labels.mlabel.bin[j][seeds[i]];
		}
	}else{

		for(sz_med i=0;i<num_seeds;i++){
			labels.mclass[i]=all_labels.mclass[seeds[i]];
			//printf("%d\n",seeds[i]);
		}
	}

	// +1 seed indexes !
	for(sz_med i=0;i<num_seeds;i++){seeds[i]+=1;}

}

//My comparator for two collumn array. Sorts second col according to first
int compare ( const void *pa, const void *pb )
{
	const sz_long *a = *(const sz_long **)pa;
	const sz_long *b = *(const sz_long **)pb;
	if(a[0] == b[0])
		return a[1] - b[1];
	else
		return a[0] - b[0];
}

//My comparator for a struct with double and index
int compare2 ( const void *pa, const void *pb )
{
  val_and_ind *a1 = (val_and_ind*)pa;
  val_and_ind *a2 = (val_and_ind*)pb;
  if((*a1).val>(*a2).val)return -1;
  else if((*a1).val<(*a2).val)return 1;
  else return 0;
}

//frobenious norm of double-valued square matrix
double frob_norm(double* A, sz_med dim){
	double norm=0.0f;

	for(sz_med i=0;i<dim*dim;i++){
		norm+=pow(A[i],2.0f);
	}

	return sqrt(norm);
}

//mean of double array
double mean(double* a, int len){
	double sum=0;
	for(int i=0;i<len;i++) sum+=a[i];
	return sum/(double)len;
}

//Simple linear system solver using the LU decomposition
/* INPUT: A,P filled in LUPDecompose; b - rhs vector; N - dimension
 * OUTPUT: x - solution vector of A*x=b
 */
void LUPSolve(double **A, int *P, double *b, int N, double *x){

    for (int i = 0; i < N; i++) {
        x[i] = b[P[i]];

        for (int k = 0; k < i; k++)
            x[i] -= A[i][k] * x[k];
    }

    for (int i = N - 1; i >= 0; i--) {
        for (int k = i + 1; k < N; k++)
            x[i] -= A[i][k] * x[k];

        x[i] = x[i] / A[i][i];
    }
}

//Simple LU decomposition routine
/* INPUT: A - array of pointers to rows of a square matrix having dimension N
 *        Tol - small tolerance number to detect failure when the matrix is near degenerate
 * OUTPUT: Matrix A is changed, it contains both matrices L-E and U as A=(L-E)+U such that P*A=L*U.
 *        The permutation matrix is not stored as a matrix, but in an integer vector P of size N+1
 *        containing column indexes where the permutation matrix has "1". The last element P[N]=S+N,
 *        where S is the number of row exchanges needed for determinant computation, det(P)=(-1)^S
 */
int LUPDecompose(double **A, int N, double Tol, int *P) {

    int i, j, k, imax;
    double maxA, *ptr, absA;

    for (i = 0; i <= N; i++)
        P[i] = i; //Unit permutation matrix, P[N] initialized with N

    for (i = 0; i < N; i++) {
        maxA = 0.0;
        imax = i;

        for (k = i; k < N; k++)
            if ((absA = fabs(A[k][i])) > maxA) {
                maxA = absA;
                imax = k;
            }

        if (maxA < Tol) return 0; //failure, matrix is degenerate

        if (imax != i) {
            //pivoting P
            j = P[i];
            P[i] = P[imax];
            P[imax] = j;

            //pivoting rows of A
            ptr = A[i];
            A[i] = A[imax];
            A[imax] = ptr;

            //counting pivots starting from N (for determinant)
            P[N]++;
        }

        for (j = i + 1; j < N; j++) {
            A[j][i] /= A[i][i];

            for (k = i + 1; k < N; k++)
                A[j][k] -= A[j][i] * A[i][k];
        }
    }

    return 1;  //decomposition done
}

//Interface for CBLAS matrix vector product
void matvec(double*y, double* A, double* x, sz_med M, sz_med N ){

	for(int i=0;i<M;i++){y[i]=0.0f;}

	cblas_dgemv( CblasRowMajor , CblasNoTrans , (int)M , (int)N, 1.0f, A, (int)M, x, 1, 0.0f, y, 1);

}

//Interface for CBLAS matrix vector product
void matvec_trans(double*y, double* A, double* x, sz_med M, sz_med N ){

	for(sz_med i=0;i<M;i++){y[i]=0.0f;}

	cblas_dgemv( CblasRowMajor , CblasTrans , (int)M , (int)N, 1.0f, A, (int)M, x, 1, 0.0f, y, 1);

}

//Interface for CBLAS trnaspose-matrix vector product
void matvec_trans_long( double* y , double* A, double* x, sz_long N, sz_med p ){

	cblas_dgemv (CblasRowMajor, CblasTrans, (int) p, (int)N , 1.0f, A,  (int) N , x, 1 , 0.0f, y, 1);

}

//Interface for CBLAS mutrix matrix product
void matrix_matrix_product(double*C, double* A, double* B, sz_long m, sz_med k , sz_short n){

	cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, (int)m,
		     (int)n, (int)k, 1.0f, A, (int)k, B, (int)n, 0.0f, C, (int)n);

}

//Project vector onto simplex by alternating projections onto line and positive quadrant
//Operation happens in place
void project_to_simplex( double* x, sz_med N ){
	double sum,a;
	sz_short flag;

	do{
		flag=0;
		sum=0.0f;

		for(sz_med i=0; i<N; i++) sum+=x[i];

		a=(sum - 1.0f)/(double)N;

		for(sz_med i=0; i<N; i++){
			x[i]-=a;
			if(x[i]<= - PROJ_TOL){
				x[i]=0.0f;
				flag=1;}
		}

	}while(flag==1);
}

//find l_max norm between vecors of known length
double max_diff(double* x, double* y , sz_med L){
	double dif;
	double max_dif=0.0f;
	for(sz_med i=0;i<L;i++){
		dif=fabs( x[i] - y[i] );
		max_dif = ( dif > max_dif ) ? dif : max_dif ;
	}
	return max_dif;
}

//Print array (double)
void print_array_1D(double* arr, sz_long N, sz_long M){
	sz_long i,j;
	printf("Array: \n");
	for(i=0;i<N;i++){
		printf("\n");
		for(j=0;j<M;j++) printf("%lf  ",arr[i*M + j]);
	}
	printf("\n");
}

//Pradict labels from largest soft label
void predict_labels( class_t* label_out, double* soft_labels, class_t* class,
	             sz_long graph_size, sz_short num_class ){
	sz_short max_ind;
	double max_val;

	for(sz_long i=0;i<graph_size;i++){
		max_val=-100.0f;
		for(sz_short j=0;j<num_class;j++){
			if(max_val<soft_labels[i*num_class + j]){
				max_val=soft_labels[i*num_class + j];
				max_ind=j;}
		}
		label_out[i]=class[max_ind];
	}
}

//Pradict labels from largest soft label (Soft labels are transposed array)
void predict_labels_type2( class_t* label_out, double* soft_labels, class_t* class,
			   sz_long graph_size, sz_short num_class ){

	sz_short max_ind;
	double max_val;

	for(sz_long j=0;j<graph_size;j++){
		max_val=-100.0f;
		for(sz_short i=0;i<num_class;i++){
			if(max_val<soft_labels[i*graph_size + j]){
				max_val=soft_labels[i*graph_size + j];
				max_ind=i;}
		}
		label_out[j]=class[max_ind];
	}
}

//Print the two collumn array of edges
void print_edge_list(const sz_long** edge_list, sz_long len){
	printf("EDGE LIST: \n");
	for(sz_long i=0;i<len;i++)
		printf("%"PRIu64"  %"PRIu64"\n", (uint64_t) edge_list[i][0], (uint64_t) edge_list[i][1]);
}

//Print array (double)
void print_predictions(class_t* arr, sz_long N){
	printf("Predicted labels: \n");
	for(sz_long i=0;i<N;i++) printf("%"PRIi16"\n", (int16_t) arr[i]);
}

//Check if file is valid
int file_isreg(char *path) {
    struct stat st;

    if (stat(path, &st) < 0)
        return -1;

    return S_ISREG(st.st_mode);
}

//Elementwise subtract b from a and store in c
void my_array_sub(double* c, double* a, double* b, sz_long N ){
	for(sz_long i=0;i<N;i++) c[i] = a[i] -b[i];
}

//Max of array

sz_short max_u8( sz_short* a, sz_long N ){
	sz_short max= 0;
	for(sz_long i=0; i<N ; i++) max = ( a[i]>max ) ? a[i] : max ;
	return max;
}

sz_long max_u64( sz_long* a, sz_long N ){
	sz_long max= 0;
	for(sz_long i=0; i<N ; i++) max = ( a[i]>max ) ? a[i] : max ;
	return max;
}

//Return edge list and count to main

sz_long** give_edge_list( char* file_name, sz_long* count ){

	sz_long** buffer= (sz_long **)malloc(EDGE_BUFF_SIZE * sizeof(sz_long *));

	for(sz_long i=0;i<EDGE_BUFF_SIZE;i++)
		buffer[i]= (sz_long*) malloc(2*sizeof(sz_long));

	FILE* file= fopen(file_name, "r");

	if(!file) printf("ERROR: Cannot open graph file");

	// Read adjacency into buffer into buffer and return length count=edges
	*count= read_adjacency_to_buffer(buffer,file);
	printf("Number of edges: %"PRIu64"\n", (uint64_t) *count);

	//print_edge_list( buffer, *count);

	//Free excess memory
	for(sz_long i=*count+1;i<EDGE_BUFF_SIZE;i++)
	{free(buffer[i]);}
	buffer=realloc(buffer,(*count)*sizeof(sz_long*));

	return buffer;
}

//Read .txt file into buffer
sz_long read_adjacency_to_buffer(sz_long** buffer, FILE* file){
        sz_long count = 0;
	for (; count < EDGE_BUFF_SIZE; ++count)
	{
		int got = fscanf(file, "%"SCNu64"\t%"SCNu64"\n", &buffer[count][0] , &buffer[count][1]);
		if ((got != 2)||( (buffer[count][0]==0) && (buffer[count][1]==0))) break;
		// Stop scanning if wrong number of tokens (maybe end of file) or zero input
	}
	fclose(file);
	return count;
}

//Read class.txt file into buffer (ignore first collumn here)
class_t* read_labels(char* filename, sz_long* label_count){
	sz_long count = 0;
	sz_long* indexes = (sz_long*) malloc(sizeof(sz_long)*CLASS_BUFF_SIZE);
	class_t* buffer = (class_t*) malloc(sizeof(class_t)*CLASS_BUFF_SIZE);
	FILE* file= fopen(filename, "r");

	if(!file) printf("ERROR: Cannot open label file");

	for (; count < CLASS_BUFF_SIZE; ++count)
	{
		int got = fscanf(file, "%"SCNu64"%"SCNu8"", &indexes[count] , &buffer[count]);
		if (got != 2) break; // wrong number of tokens - maybe end of file
	}
	fclose(file);

	*label_count = count;
	buffer = realloc(buffer,sizeof(class_t)*count);
	indexes = realloc(indexes,sizeof(sz_long)*count);

	my_relative_sorting( indexes, buffer, count );

	free(indexes);
	return buffer;
}

//Read seed and label file when in operation mode
sz_long* read_seed_file( char* filename, sz_med* num_seeds, sz_short* num_class, abstract_labels* label_in ){

        //First read file into buffers
	sz_long count = 0;
	sz_long* index_buffer = (sz_long*) malloc(sizeof(sz_long)*CLASS_BUFF_SIZE);
	class_t* label_buffer = (class_t*) malloc(sizeof(class_t)*CLASS_BUFF_SIZE);
	FILE* file= fopen(filename, "r");

	if(!file) printf("ERROR: Cannot open seed file");

	for (; count < CLASS_BUFF_SIZE; ++count)
	{
		int got = fscanf(file, "%"SCNu64"%"SCNd8"", &index_buffer[count] , &label_buffer[count]);
		if (got != 2) break; // wrong number of tokens - maybe end of file
	}
	fclose(file);
	label_buffer = realloc(label_buffer,sizeof(class_t)*count);
	index_buffer = realloc(index_buffer,sizeof(sz_long)*count);

	//prepare input labels and seeds for is_multilabel or multi_class

	sz_long* seed_indices;

	if(label_in->is_multilabel){
		*num_class = max_u8( (sz_short*) label_buffer, count);
		my_relative_sorting( index_buffer, label_buffer, count );
		seed_indices = find_unique_from_sorted( index_buffer, count , num_seeds );

		label_in->mlabel = init_one_hot(*num_class , *num_seeds);
		sz_long j=0;
		for(sz_long i=0;i<count;i++){
			if(i>0) j = (index_buffer[i] == index_buffer[i-1] ) ? j : j+1;
			label_in->mlabel.bin[label_buffer[i]-1][j]=1;
		}

	        free(index_buffer);
		free(label_buffer);
	}else{
		seed_indices = index_buffer;
		label_in->mclass = label_buffer;
		*num_seeds = (sz_med) count;
	}

	return seed_indices;
}

//write predicted labels (or ranking in multilabel case) to output file
void save_predictions(char* filename, abstract_label_output label_out, sz_long len, sz_short num_class){

	FILE* file = fopen(filename, "w");

	if(!file) printf("ERROR: Cannot open outfile");

	if(label_out.is_multilabel){
		val_and_ind line_of_out[num_class];
		for(sz_long i=0; i<len; i++){
			for(sz_short j=0; j<num_class; j++)
				line_of_out[j] = (val_and_ind) {.val = label_out.mlabel[j*len + i], .ind=(int)j};

			qsort( line_of_out, num_class, sizeof(line_of_out[0]), compare2);

			fprintf(file, "%"SCNu64":\t", (uint64_t) i+1 );

			for(sz_short j=0; j<num_class; j++)
				fprintf(file, "%"PRIu16" ", (uint16_t) line_of_out[j].ind +1 );

			fprintf(file, "\n");
		}
	}else{
		for(sz_long i=0; i<len; i++)
			fprintf(file, "%"SCNu64"\t%"SCNd16"\n", (uint64_t) i+1 , (int16_t) label_out.mclass[i]);
	}

	fclose(file);
}

//Read class.txt file into one_hot_matrix (ignore first collumn here)
one_hot_mat read_one_hot_mat(char* filename, sz_long* label_count){
	sz_long count = 0;
	sz_long* indexes = (sz_long*) malloc(sizeof(sz_long)*CLASS_BUFF_SIZE);
	sz_short* buffer = (sz_short*) malloc(sizeof(sz_short)*CLASS_BUFF_SIZE);
	FILE* file = fopen(filename, "r");

	if(!file) printf("ERROR: Cannot open label file");

	for (; count < CLASS_BUFF_SIZE; ++count)
	{
		int got = fscanf(file, "%"SCNu64"\t%"SCNu8"", &indexes[count] , &buffer[count]);
		if (got != 2) break; // wrong number of tokens - maybe end of file
	}
	fclose(file);

	buffer = realloc(buffer,sizeof(sz_short)*count);
	indexes = realloc(indexes,sizeof(sz_long)*count);

	sz_short num_class = max_u8( buffer, count);

	sz_long length = max_u64( indexes, count);

//	printf("LENGTH %"PRIu64"\n",length);

	one_hot_mat all_labels = init_one_hot(num_class , length);

	for(sz_long i=0;i<count;i++) all_labels.bin[buffer[i]-1][indexes[i]-1]=1;

	*label_count = length;

	free(indexes);
	free(buffer);
	return all_labels;
}

// Sort A and ind with respect to indexes in ind
void my_relative_sorting( sz_long* ind, class_t* A, sz_long len ){
	sz_long* temp = (sz_long*)malloc(len*sizeof(sz_long));
	sz_long* sorted_inds = (sz_long*)malloc(len*sizeof(sz_long));

	for(sz_long i=0;i<len;i++){ temp[i] = i ;}

	sz_long** zipped_arrays = zip_arrays(ind,temp,len);

        qsort( zipped_arrays , len, sizeof(zipped_arrays[0]), compare);

        unzip_array(zipped_arrays, temp, sorted_inds, len );

        rearange(sorted_inds, (void*) A, "class_t" , len );

	free(temp);
	free(sorted_inds);

}

//Zip two arrays into a list of length-2 lists
sz_long** zip_arrays(sz_long* A, sz_long* B, sz_long len){
	sz_long** zipped = (sz_long**)malloc(len*sizeof(sz_long*));

	for(sz_long i=0;i<len;i++){
		zipped[i] = (sz_long*)malloc(2*sizeof(sz_long));
                ** (zipped +i) = A[i];
                *(* (zipped +i)+1) = B[i];
	}
	return zipped;
}

//Unzip two arrays into input pointers and destroy zipped array
void unzip_array(sz_long** zipped, sz_long* unzip_1, sz_long* unzip_2, sz_long len ){

	for(sz_long i=0;i<len;i++){
		unzip_1[i] = zipped[i][0];
		unzip_2[i] = zipped[i][1];
		free(*(zipped+i));
	}

	free(zipped);
}

//Rearange elements of array A accortding to given indexes
//Works for any type of array as long as the type is provided
void rearange(sz_long* ind, void* A, char* type , sz_long len ){

	if(strcmp(type,"double")==0){
		double A_temp[len];
		memcpy(A_temp, A, len*sizeof(double));
		for(sz_long i=0;i<len;i++){ *((double*)A + i) = A_temp[ind[i]];}
	}else if(strcmp(type,"sz_long")==0){
		sz_long A_temp[len];
		memcpy(A_temp, A, len*sizeof(sz_long));
		for(sz_long i=0;i<len;i++){ *((sz_long*)A + i) = A_temp[ind[i]];}
	}else if(strcmp(type,"uint32_t")==0){
		uint32_t A_temp[len];
		memcpy(A_temp, A, len*sizeof(uint32_t));
		for(sz_long i=0;i<len;i++){ *((uint32_t*)A + i) = A_temp[ind[i]];}
	}else if(strcmp(type,"sz_med")==0){
		sz_med A_temp[len];
		memcpy(A_temp, A, len*sizeof(sz_med));
		for(sz_long i=0;i<len;i++){ *((sz_med*)A + i) = A_temp[ind[i]];}
	}else if(strcmp(type,"sz_short")==0){
		sz_short A_temp[len];
		memcpy(A_temp, A, len*sizeof(sz_short));
		for(sz_long i=0;i<len;i++){ *((sz_short*)A + i) = A_temp[ind[i]];}
	}else if(strcmp(type,"int32_t")==0){
		int32_t A_temp[len];
		memcpy(A_temp, A, len*sizeof(int32_t));
		for(sz_long i=0;i<len;i++){ *((int32_t*)A + i) = A_temp[ind[i]];}
	}else if(strcmp(type,"int16_t")==0){
		int16_t A_temp[len];
		memcpy(A_temp, A, len*sizeof(int16_t));
		for(sz_long i=0;i<len;i++){ *((int16_t*)A + i) = A_temp[ind[i]];}
	}else if(strcmp(type,"class_t")==0){
		class_t A_temp[len];
		memcpy(A_temp, A, len*sizeof(class_t));
		for(sz_long i=0;i<len;i++){ *((class_t*)A + i) = A_temp[ind[i]];}
	}else if(strcmp(type,"long double")==0){
		long double A_temp[len];
		memcpy(A_temp, A, len*sizeof(long double));
		for(sz_long i=0;i<len;i++){ *((long double*)A + i) = A_temp[ind[i]];}
	}else if(strcmp(type,"int")==0){
		int A_temp[len];
		memcpy(A_temp, A, len*sizeof(int));
		for(sz_long i=0;i<len;i++){ *((int*)A + i) = A_temp[ind[i]];}
	}else{
		printf("Unknown rearange type\n");
		exit(EXIT_FAILURE);
	}

}

//Remove items of given indexes from array (list)
// Return result in NEW  list
sz_long* remove_from_list(const sz_long* list, const sz_long* indexes_to_be_removed,
			   sz_long len, sz_long num_removed ){

	sz_long* new_list = (sz_long*) malloc((len-num_removed)*sizeof(sz_long));

	int mask[len];

	memset(mask, 0, len*sizeof(int));

	for(sz_long i =0; i<num_removed; i++){ mask[indexes_to_be_removed[i]] =1 ;}

	sz_long k=0;
	for(sz_long i =0; i<len; i++){
		if(mask[i]==0){
		    new_list[k++] = list[i];
		}
	}

	return new_list;
}

//Find unique elements among known number of entries in buffer
//Also eturn number of unique entries
sz_short find_unique(class_t* unique_elements, const class_t* buffer,sz_med N){

	sz_short new,end=1;

	unique_elements[0]=buffer[0];
	for(sz_med i=0;i<N;i++){
		new=1;
		for(sz_short j=0;j<end;j++)
		{if(unique_elements[j]==buffer[i])
			new=0;}

		if(new)
			unique_elements[end++]=buffer[i];

	}
	return end;
}

//Find unique entries from list of sorted (unisgned indexes)
sz_long* find_unique_from_sorted( sz_long* sorted_buffer, sz_long len , sz_med* num_unique ){
	sz_long* unique = (sz_long*) malloc(len*sizeof(sz_long));
	*num_unique = 1;

	unique[0] = sorted_buffer[0];
	for(sz_long i=1;i<len;i++){
		if(!(sorted_buffer[i] == sorted_buffer[i-1] )){
			*num_unique += 1;
			unique[*num_unique-1] = sorted_buffer[i];
		}
	}
	unique = realloc(unique,*num_unique*sizeof(sz_long));
	return unique;
}

// Converts list to one hot binary matrix of one_hot_mat type
// Label count is number of indexes with known labels
// label_count<=length
// If label_count<=length, then rows of the one_hot matrix
// without a coresponding labeled index will be [0...0]
one_hot_mat list_to_one_hot( sz_long* ind , class_t* labels, sz_short num_class ,
			     class_t* class ,  sz_long label_count ,sz_long length){

	one_hot_mat one_hot = init_one_hot( num_class , length);

	for(sz_long i=0;i<label_count;i++){
		for(sz_short j=0;j<num_class;j++){
			if( labels[i] == class[j] ){
				one_hot.bin[j][ind[i]]=1;
				break;
			}
		}
	}

	return one_hot;
}

//Allocate one_hot_mat
one_hot_mat init_one_hot(sz_short num_class ,sz_long length){
	one_hot_mat one_hot;
	one_hot.bin = (sz_short**) malloc(num_class*sizeof(sz_short*));

	for(sz_short i=0;i<num_class;i++){
		one_hot.bin[i] = (sz_short*) malloc(length*sizeof(sz_short));
		for(sz_long j=0;j<length;j++){
			one_hot.bin[i][j] =0 ;
		}
	}
	one_hot.num_class = num_class;
	one_hot.length = length;
	return one_hot;
}

// Destroy (free) one_hot matrix
void destroy_one_hot(one_hot_mat one_hot){
	for(sz_short i=0;i<one_hot.num_class;i++) free(one_hot.bin[i]);
	free(one_hot.bin);
}

// Return array with number of non-zero entries in each row of one-hot-matrix
sz_short* return_num_labels_per_node( one_hot_mat one_hot ){
	sz_short* num_labels = (sz_short*)malloc(one_hot.length*sizeof(sz_short));
	for(sz_long i=0;i<one_hot.length;i++){
		num_labels[i]=0;
		for(sz_short j=0;j<one_hot.num_class;j++) num_labels[i]+= one_hot.bin[j][i];
	}
	return num_labels;
}

// Return a one_hot_mat with non-zeros for each row given to
// the k-largest (specified by num_lpn) corresponding soft labels
// Does not do sorting and has O(kC) complexity per node instead
// of O(C logC). Will be slow if C and k large
one_hot_mat top_k_mlabel( double* soft_labels , sz_short* num_lpn, sz_long length, sz_short num_class ){
	double max_max,max,val;
	sz_short max_ind;
	one_hot_mat label_out = init_one_hot(num_class ,length);

	for(sz_long i=0; i<length;i++ ){
		max_max =1.1f;
		for(sz_short k = 0; k<num_lpn[i];k++){
			max = 0.0f;
			for(sz_short j=0; j<num_class ;j++ ){
				val = soft_labels[j*length +i];
				if( ( val < max_max) && (val > max) ){
					max = val;
					max_ind = j;
				}
			}
			max_max = max;
			label_out.bin[max_ind][i]=1;
		}
	}

	return label_out;
}

// Computes rate of true positive, False positive and false negative
// A binary mask determines which values I am interested in (usually unlabeled entries..)
detector_stats get_detector_stats( sz_short* true_val, sz_short* pred_val, sz_long len, int* mask ){

	detector_stats stats = {.true_pos = 0.0,
			        .true_neg =0.0,
			        .false_pos = 0.0,
			        .false_neg = 0.0};

	sz_long k=0;
	for(sz_long i=0;i<len;i++){
	    if(mask[i]!=0){
	    	k++;
		if(true_val[i]==1){
			if(pred_val[i]==1){
				stats.true_pos+=1.0;
			}else{
				stats.false_neg+=1.0;
			}
		}else{
			if(pred_val[i]==1){
				stats.false_pos+=1.0;
			}else{
				stats.true_neg+=1.0;
			}
		}
	    }
	}

	stats.true_pos/=(double) k;
	stats.true_neg/=(double) k;
	stats.false_pos/=(double) k;
	stats.false_neg/=(double) k;

	return stats;
}

// Harmonic mean of 2 real numbers
double harmonic_mean( double x, double y ){
	if(x+y == 0.0f){
		printf("ERROR: Harmonic mean evaluated at 0\n ");
		exit(EXIT_FAILURE);
	}
	return (2.0f*(x*y))/(x+y);
}

// Produce micro and macro averaged precision and recall
// Input is the more simple detector_stats for different classes
classifier_stats  get_class_stats(detector_stats* all_stats , sz_short num_class ){

	classifier_stats stats = {.micro_precision=0.0f,
				  .macro_precision=0.0f,
				  .micro_recall=0.0f,
				  .macro_recall=0.0f };

	double sum_of_true_pos = 0.0f, micro_prec_denom = 0.0f, micro_recall_denom = 0.0f ;

	for(sz_short i=0; i<num_class;i++){
		sum_of_true_pos += all_stats[i].true_pos;
		micro_prec_denom += all_stats[i].true_pos + all_stats[i].false_pos;
		micro_recall_denom += all_stats[i].true_pos + all_stats[i].false_neg;

		stats.macro_precision += all_stats[i].true_pos/( all_stats[i].true_pos + all_stats[i].false_pos );
		stats.macro_recall += all_stats[i].true_pos/( all_stats[i].true_pos + all_stats[i].false_neg );
	}

	stats.micro_precision = sum_of_true_pos/micro_prec_denom;
	stats.micro_recall = sum_of_true_pos/micro_recall_denom;

	stats.macro_precision /= (double) num_class;
	stats.macro_recall /= (double) num_class;

	return stats;
}

// Takes array of detector statistics as input and returns array with f-1 scores for every class

void get_per_class_f1_scores(double* f1_per_class, detector_stats* all_stats, sz_short num_class ){

	for(sz_short i=0;i<num_class;i++){
		double TP = all_stats[i].true_pos;
		double FN = all_stats[i].false_neg;
		double FP = all_stats[i].false_pos;
		f1_per_class[i] = 2.0*TP / ( 2.0*TP + FN + FP );
	}
}

// Computes Micro and Macro f1-score
// Inputs must be one_hot structs
// f1_scores are only evaluated on the indexes denoted by unlabeled

f1_scores get_averaged_f1_scores( one_hot_mat true_labels, one_hot_mat pred_labels,
	                          sz_long* unlabeled , sz_long num_unlabeled ){

	if( (true_labels.num_class != pred_labels.num_class) || (true_labels.length != pred_labels.length) ){
		printf("Classifier stats ERROR: One-hot matrixes dimensions dont match\n");
		exit(EXIT_FAILURE);
	}

	int mask[true_labels.length];
	memset(mask, 0 , true_labels.length*sizeof(int));
	for(sz_long i=0;i< num_unlabeled; i++ ){ mask[unlabeled[i]]=1; }

        // Start by gathering detection statistics per each class

	detector_stats all_stats[true_labels.num_class];

	for(sz_short i=0;i<true_labels.num_class;i++){
		all_stats[i] = get_detector_stats( true_labels.bin[i] , pred_labels.bin[i] , true_labels.length , mask );
	}

	// Use detection stats to get Micro and Macro averaged F1 scores

	f1_scores scores = {.micro = 0.0f,
			    .macro = 0.0f };

	classifier_stats class_stats = get_class_stats(all_stats , true_labels.num_class );

	double f1_per_class[true_labels.num_class];

	get_per_class_f1_scores(f1_per_class, all_stats, true_labels.num_class);

	scores.micro = harmonic_mean(class_stats.micro_precision, class_stats.micro_recall );
        scores.macro = mean( f1_per_class, (int)true_labels.num_class );
/*
        This definition of Macro F1 (using macro averaged precision and recall) does not seem to work well
	scores.macro = harmonic_mean(class_stats.macro_precision, class_stats.macro_recall ); //
*/
	return scores;
}

//Compute error rate

double accuracy(class_t* true_label, class_t* pred,  sz_long* unlabeled, sz_long num_unlabeled){
	double sum=0.0;
	for(sz_long i=0;i<num_unlabeled;i++){
			if(true_label[unlabeled[i]]==pred[unlabeled[i]])
				sum+=1.0;
		}

	return sum/(double)num_unlabeled;
}


/*
 * Evan Lezar
 * An implementation of a dense eigensolver that uses the ARPACK Fortran backend.
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <omp.h>
#include <cblas.h>
// #include <acml.h>

// Include the interfaces to the Fortran routines.
#include "interface.h"

#define ARPACK_CALL 0
#define SHIFT 1
#define SGETRF_S 2
#define SGEMV 9

//!
//! A data structure used to pass the eigenproblem configuration to Fortran and
//! back to C again.
struct data_struct {
    float* A;           //! A pointer to the matrix representing the eigenproblem.
    double sgemv_time;  //! The total time required to calculate the matrix-vector products.
    int sgemv_calls;    //! The number of times the matrix-vector product was called.
    int LDA;            //! The leading dimension of the matrix.
};
typedef struct data_struct data_struct;

//!
//! A utility function to print the structure representing the eigenproblem.
void print_data ( const char* desc, data_struct DATA )
{
    printf ( "%s: A = %p LDA = %d\n", desc, DATA.A, DATA.LDA );
}

//!
//! Calculate the matrix-vector product  y <- Ax for the eigensystem defined by DATA. This
//! routine is called from the Fortran backend whenever ARPACK requires a matrix-vector
//! product to be calculated.
//! \param[in] N The dimension of the eigensystem.
//! \param[in,out] DATA The structure representing the eigenproblem.
//! \param[in] x The vector that must be multiplied by A.
//! \param[out] y The vector that must store the result.
void sgemv_wrapper ( int N, data_struct* DATA, float* x, float* y )
{
    checkpoint t0 = tic();
    // read the relevant data from the struct
    float* A = DATA->A;
    int LDA = DATA->LDA;
    // calculate y <-- Ax
    cblas_sgemv ( CblasColMajor, CblasNoTrans, N, N, 1.0, A, LDA, x, 1, 0.0, y, 1);
    DATA->sgemv_time += toc ( t0 );
    DATA->sgemv_calls += 1;
}

//!
//! Initialise the struct representing the eigensystem.
void init_data ( data_struct* DATA, float* A, int LDMAT )
{
    DATA->A = A;
    DATA->LDA = LDMAT;
    DATA->sgemv_calls = 0;
    DATA->sgemv_time = 0.0;
}

//!
//! Solve the actual eigensystem. This takes the eigensystem defined in the structure DATA and allocates
//! the required temporary workspaces before calling the Fortran backend.
void calculate_eigen_values ( int N, void* DATA, int NEV, float* eigenvalues, float* eigenvectors, char* which )
{
    int use_N_ev = 2*NEV;
    if ( use_N_ev > ( N/2 - 1 ) )
        use_N_ev = N/2 - 1;
    // select the number of Arnoldi vectors to generate
    int NCV = 2*use_N_ev + 1;
    if ( NCV > N )
        NCV = N;

    // allocate temporary storage for the vectors
    float* temp_ev = (float*)malloc ( NCV*2*sizeof(float) );
    float* temp_vectors = (float*) malloc (N*NCV*sizeof(float));
    float* temp_residuals = (float*)malloc ( (NCV )*sizeof(float));

    // solve the eigenvalue problem using ARPACK
    arpack_ssev(N, (void*)DATA, use_N_ev, NCV, temp_ev, temp_vectors, temp_residuals, which );

    // Copy the resultant eigenvalues to the previously allocated space.
    memcpy(eigenvalues, temp_ev, NEV*2*sizeof(float));
    memcpy(eigenvectors, temp_vectors, NEV*N*sizeof(float));

    // free the temporary storage
    free ( temp_ev );
    free ( temp_vectors );
    free ( temp_residuals );
}

//!
//! Solve the standard eigensystem Ax = lx.eigen
//! \param[in] N The number of columns in the matrix A
//! \param[in] A The matrix representing the eigensystem to be solved.
//! \param[in] NEV The number of eigenvalues to calculate.
//! \param[out] eigenvalues A vector of the NEV eigenvalues. Note that the eigenvectors are complex.
//! \param[out] eigenvectors An NxNEV matrix with the eigenvectors as columns.
//! \param[out] timing_data_10 A 10-vector representing the timing data for various phases of the process.
//! \param[out] int_data_10 A 10-vector containing some run information.
//! \return A non-zero error code if an error occured.
int dense_seev ( int N, float* A, int LDMAT, int NEV, float* eigenvalues, float* eigenvectors, double* timing_data_10, int* int_data_10 )
{
    checkpoint t0;
    int result = 0;

    // Initialise the data structure that is passed to the ARPACK routines.
    data_struct DATA;
    init_data( &DATA, A, LDMAT );

    t0 = tic();
    // Call a C wrapper function that allows for the calculation of the NEV largest eigenvalues.
    calculate_eigen_values ( N, &DATA, NEV, eigenvalues, eigenvectors, "LM" );
    timing_data_10[ARPACK_CALL] = toc( t0 );

    timing_data_10[SGEMV] = DATA.sgemv_time;
    int_data_10[SGEMV] = DATA.sgemv_calls;
    return result;
}


#include <gsl/gsl_test.h>
#include <gsl/gsl_ieee_utils.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_cblas.h>

#include "tests.h"

void
test_hpr2 (void) {
const double flteps = 1e-4, dbleps = 1e-6;
  {
   int order = 101;
   int uplo = 121;
   int N = 1;
   float alpha[2] = {-1.0f, 0.0f};
   float Ap[] = { 0.159f, -0.13f };
   float X[] = { 0.854f, 0.851f };
   int incX = -1;
   float Y[] = { 0.526f, -0.267f };
   int incY = -1;
   float Ap_expected[] = { -0.284974f, 0.0f };
   cblas_chpr2(order, uplo, N, alpha, X, incX, Y, incY, Ap);
   {
     int i;
     for (i = 0; i < 1; i++) {
       gsl_test_rel(Ap[2*i], Ap_expected[2*i], flteps, "chpr2(case 1458) real");
       gsl_test_rel(Ap[2*i+1], Ap_expected[2*i+1], flteps, "chpr2(case 1458) imag");
     };
   };
  };

  {
   int order = 101;
   int uplo = 122;
   int N = 1;
   float alpha[2] = {-1.0f, 0.0f};
   float Ap[] = { 0.159f, -0.13f };
   float X[] = { 0.854f, 0.851f };
   int incX = -1;
   float Y[] = { 0.526f, -0.267f };
   int incY = -1;
   float Ap_expected[] = { -0.284974f, 0.0f };
   cblas_chpr2(order, uplo, N, alpha, X, incX, Y, incY, Ap);
   {
     int i;
     for (i = 0; i < 1; i++) {
       gsl_test_rel(Ap[2*i], Ap_expected[2*i], flteps, "chpr2(case 1459) real");
       gsl_test_rel(Ap[2*i+1], Ap_expected[2*i+1], flteps, "chpr2(case 1459) imag");
     };
   };
  };

  {
   int order = 102;
   int uplo = 121;
   int N = 1;
   float alpha[2] = {-1.0f, 0.0f};
   float Ap[] = { 0.159f, -0.13f };
   float X[] = { 0.854f, 0.851f };
   int incX = -1;
   float Y[] = { 0.526f, -0.267f };
   int incY = -1;
   float Ap_expected[] = { -0.284974f, 0.0f };
   cblas_chpr2(order, uplo, N, alpha, X, incX, Y, incY, Ap);
   {
     int i;
     for (i = 0; i < 1; i++) {
       gsl_test_rel(Ap[2*i], Ap_expected[2*i], flteps, "chpr2(case 1460) real");
       gsl_test_rel(Ap[2*i+1], Ap_expected[2*i+1], flteps, "chpr2(case 1460) imag");
     };
   };
  };

  {
   int order = 102;
   int uplo = 122;
   int N = 1;
   float alpha[2] = {-1.0f, 0.0f};
   float Ap[] = { 0.159f, -0.13f };
   float X[] = { 0.854f, 0.851f };
   int incX = -1;
   float Y[] = { 0.526f, -0.267f };
   int incY = -1;
   float Ap_expected[] = { -0.284974f, 0.0f };
   cblas_chpr2(order, uplo, N, alpha, X, incX, Y, incY, Ap);
   {
     int i;
     for (i = 0; i < 1; i++) {
       gsl_test_rel(Ap[2*i], Ap_expected[2*i], flteps, "chpr2(case 1461) real");
       gsl_test_rel(Ap[2*i+1], Ap_expected[2*i+1], flteps, "chpr2(case 1461) imag");
     };
   };
  };

  {
   int order = 101;
   int uplo = 121;
   int N = 1;
   double alpha[2] = {-0.3, 0.1};
   double Ap[] = { 0.772, 0.997 };
   double X[] = { -0.173, -0.839 };
   int incX = -1;
   double Y[] = { 0.941, -0.422 };
   int incY = -1;
   double Ap_expected[] = { 0.829742, 0.0 };
   cblas_zhpr2(order, uplo, N, alpha, X, incX, Y, incY, Ap);
   {
     int i;
     for (i = 0; i < 1; i++) {
       gsl_test_rel(Ap[2*i], Ap_expected[2*i], dbleps, "zhpr2(case 1462) real");
       gsl_test_rel(Ap[2*i+1], Ap_expected[2*i+1], dbleps, "zhpr2(case 1462) imag");
     };
   };
  };

  {
   int order = 101;
   int uplo = 122;
   int N = 1;
   double alpha[2] = {-0.3, 0.1};
   double Ap[] = { 0.772, 0.997 };
   double X[] = { -0.173, -0.839 };
   int incX = -1;
   double Y[] = { 0.941, -0.422 };
   int incY = -1;
   double Ap_expected[] = { 0.829742, 0.0 };
   cblas_zhpr2(order, uplo, N, alpha, X, incX, Y, incY, Ap);
   {
     int i;
     for (i = 0; i < 1; i++) {
       gsl_test_rel(Ap[2*i], Ap_expected[2*i], dbleps, "zhpr2(case 1463) real");
       gsl_test_rel(Ap[2*i+1], Ap_expected[2*i+1], dbleps, "zhpr2(case 1463) imag");
     };
   };
  };

  {
   int order = 102;
   int uplo = 121;
   int N = 1;
   double alpha[2] = {-0.3, 0.1};
   double Ap[] = { 0.772, 0.997 };
   double X[] = { -0.173, -0.839 };
   int incX = -1;
   double Y[] = { 0.941, -0.422 };
   int incY = -1;
   double Ap_expected[] = { 0.829742, 0.0 };
   cblas_zhpr2(order, uplo, N, alpha, X, incX, Y, incY, Ap);
   {
     int i;
     for (i = 0; i < 1; i++) {
       gsl_test_rel(Ap[2*i], Ap_expected[2*i], dbleps, "zhpr2(case 1464) real");
       gsl_test_rel(Ap[2*i+1], Ap_expected[2*i+1], dbleps, "zhpr2(case 1464) imag");
     };
   };
  };

  {
   int order = 102;
   int uplo = 122;
   int N = 1;
   double alpha[2] = {-0.3, 0.1};
   double Ap[] = { 0.772, 0.997 };
   double X[] = { -0.173, -0.839 };
   int incX = -1;
   double Y[] = { 0.941, -0.422 };
   int incY = -1;
   double Ap_expected[] = { 0.829742, 0.0 };
   cblas_zhpr2(order, uplo, N, alpha, X, incX, Y, incY, Ap);
   {
     int i;
     for (i = 0; i < 1; i++) {
       gsl_test_rel(Ap[2*i], Ap_expected[2*i], dbleps, "zhpr2(case 1465) real");
       gsl_test_rel(Ap[2*i+1], Ap_expected[2*i+1], dbleps, "zhpr2(case 1465) imag");
     };
   };
  };

}


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#ifndef NOMPI
#include <mpi.h>
#endif
#include <gsl/gsl_math.h>

#include "allvars.h"
#include "proto.h"

/*! \file predict.c
 *  \brief drift particles by a small time interval
 *
 *  This function contains code to implement a drift operation on all the
 *  particles, which represents one part of the leapfrog integration scheme.
 */

/*! This function drifts all particles from the current time to the future:
 *  time0 - > time1
 *
 *  If there is no explicit tree construction in the following timestep, the
 *  tree nodes are also drifted and updated accordingly. Note: For periodic
 *  boundary conditions, the mapping of coordinates onto the interval
 *  [0,All.BoxSize] is only done before the domain decomposition, or for
 *  outputs to snapshot files.  This simplifies dynamic tree updates, and
 *  allows the domain decomposition to be carried out only every once in a
 *  while.
 */
void move_particles(int time0, int time1)
{
  int i, j;
  double dt_drift, dt_gravkick, dt_hydrokick, dt_entr;
  double t0, t1;

  t0 = second();

  if(All.ComovingIntegrationOn)
    {
      dt_drift = get_drift_factor(time0, time1);
      dt_gravkick = get_gravkick_factor(time0, time1);
      dt_hydrokick = get_hydrokick_factor(time0, time1);
    }
  else
    {
      dt_drift = dt_gravkick = dt_hydrokick = (time1 - time0) * All.Timebase_interval;
    }

  for(i = 0; i < NumPart; i++)
    {
      for(j = 0; j < 3; j++)
	P[i].Pos[j] += P[i].Vel[j] * dt_drift;

      if(P[i].Type == 0)
	{
#ifdef PMGRID
	  for(j = 0; j < 3; j++)
	    SphP[i].VelPred[j] +=
	      (P[i].GravAccel[j] + P[i].GravPM[j]) * dt_gravkick + SphP[i].HydroAccel[j] * dt_hydrokick;
#else
	  for(j = 0; j < 3; j++)
	    SphP[i].VelPred[j] += P[i].GravAccel[j] * dt_gravkick + SphP[i].HydroAccel[j] * dt_hydrokick;
#endif
	  SphP[i].Density *= exp(-SphP[i].DivVel * dt_drift);
	  SphP[i].Hsml *= exp(0.333333333333 * SphP[i].DivVel * dt_drift);

	  if(SphP[i].Hsml < All.MinGasHsml)
	    SphP[i].Hsml = All.MinGasHsml;

	  dt_entr = (time1 - (P[i].Ti_begstep + P[i].Ti_endstep) / 2) * All.Timebase_interval;

	  SphP[i].Pressure = (SphP[i].Entropy + SphP[i].DtEntropy * dt_entr) * pow(SphP[i].Density, GAMMA);

#ifdef MORRIS97VISC
          SphP[i].Alpha += SphP[i].DAlphaDt * dt_drift;
#endif
	}
    }

  /* if domain-decomp and tree are not going to be reconstructed, update dynamically.  */
  if(All.NumForcesSinceLastDomainDecomp < All.TotNumPart * All.TreeDomainUpdateFrequency)
    {
      for(i = 0; i < Numnodestree; i++)
	for(j = 0; j < 3; j++)
	  Nodes[All.MaxPart + i].u.d.s[j] += Extnodes[All.MaxPart + i].vs[j] * dt_drift;

      force_update_len();

      force_update_pseudoparticles();
    }

  t1 = second();

  All.CPU_Predict += timediff(t0, t1);
}

/*! This function makes sure that all particle coordinates (Pos) are
 *  periodically mapped onto the interval [0, BoxSize].  After this function
 *  has been called, a new domain decomposition should be done, which will
 *  also force a new tree construction.
 */
#ifdef PERIODIC
void do_box_wrapping(void)
{
  int i, j;
  double boxsize[3];

  for(j = 0; j < 3; j++)
    boxsize[j] = All.BoxSize;

#ifdef LONG_X
  boxsize[0] *= LONG_X;
#endif
#ifdef LONG_Y
  boxsize[1] *= LONG_Y;
#endif
#ifdef LONG_Z
  boxsize[2] *= LONG_Z;
#endif

  for(i = 0; i < NumPart; i++)
    for(j = 0; j < 3; j++)
      {
	while(P[i].Pos[j] < 0)
	  P[i].Pos[j] += boxsize[j];

	while(P[i].Pos[j] >= boxsize[j])
	  P[i].Pos[j] -= boxsize[j];
      }
}
#endif


/*
ODE: a program to get optime Runge-Kutta and multi-steps methods.

Copyright 2011-2019, Javier Burguete Tolosa.

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 Javier Burguete Tolosa ``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 Javier Burguete Tolosa 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.
*/

/**
 * \file rk_5_3.c
 * \brief Source file to optimize Runge-Kutta 5 steps 3rd order methods.
 * \author Javier Burguete Tolosa.
 * \copyright Copyright 2011-2019.
 */
#define _GNU_SOURCE
#include <string.h>
#include <math.h>
#include <libxml/parser.h>
#include <glib.h>
#include <libintl.h>
#include <gsl/gsl_rng.h>
#include "config.h"
#include "utils.h"
#include "optimize.h"
#include "rk.h"
#include "rk_5_3.h"

#define DEBUG_RK_5_3 0          ///< macro to debug.

/**
 * Function to obtain the coefficients of a 5 steps 3rd order Runge-Kutta
 * method.
 */
int
rk_tb_5_3 (Optimize * optimize) ///< Optimize struct.
{
  long double A[3], B[3], C[3], D[3];
  long double *tb, *r;
#if DEBUG_RK_5_3
  fprintf (stderr, "rk_tb_5_3: start\n");
#endif
  tb = optimize->coefficient;
  r = optimize->random_data;
  t5 (tb) = 1.L;
  t1 (tb) = r[0];
  t2 (tb) = r[1];
  b21 (tb) = r[2];
  t3 (tb) = r[3];
  b31 (tb) = r[4];
  b32 (tb) = r[5];
  t4 (tb) = r[6];
  b41 (tb) = r[7];
  b42 (tb) = r[8];
  b43 (tb) = r[9];
  b54 (tb) = r[10];
  A[0] = t1 (tb);
  B[0] = t2 (tb);
  C[0] = t3 (tb);
  D[0] = 0.5L - b54 (tb) * t4 (tb);
  A[1] = A[0] * t1 (tb);
  B[1] = B[0] * t2 (tb);
  C[1] = C[0] * t3 (tb);
  D[1] = 1.L / 3.L - b54 (tb) * sqr (t4 (tb));
  A[2] = 0.L;
  B[2] = b21 (tb) * t1 (tb);
  C[2] = b31 (tb) * t1 (tb) + b32 (tb) * t2 (tb);
  D[2] = 1.L / 6.L - b54 (tb) * (b41 (tb) * t1 (tb) + b42 (tb) * t2 (tb)
                                 + b43 (tb) * t3 (tb));
  solve_3 (A, B, C, D);
  if (isnan (D[0]) || isnan (D[1]) || isnan (D[2]))
    return 0;
  b53 (tb) = D[2];
  b52 (tb) = D[1];
  b51 (tb) = D[0];
  rk_b_5 (tb);
#if DEBUG_RK_5_3
  fprintf (stderr, "rk_tb_5_3: end\n");
#endif
  return 1;
}

/**
 * Function to obtain the coefficients of a 5 steps 3rd order, 4th order in
 * equations depending only in time, Runge-Kutta method.
 */
int
rk_tb_5_3t (Optimize * optimize)        ///< Optimize struct.
{
  long double A[4], B[4], C[4], D[4], E[4];
  long double *tb, *r;
#if DEBUG_RK_5_3
  fprintf (stderr, "rk_tb_5_3t: start\n");
#endif
  tb = optimize->coefficient;
  r = optimize->random_data;
  t5 (tb) = 1.L;
  t1 (tb) = r[0];
  t2 (tb) = r[1];
  b21 (tb) = r[2];
  t3 (tb) = r[3];
  b31 (tb) = r[4];
  b32 (tb) = r[5];
  t4 (tb) = r[6];
  b41 (tb) = r[7];
  b42 (tb) = r[8];
  b43 (tb) = r[9];
  A[0] = t1 (tb);
  B[0] = t2 (tb);
  C[0] = t3 (tb);
  D[0] = t4 (tb);
  E[0] = 0.5L;
  A[1] = A[0] * t1 (tb);
  B[1] = B[0] * t2 (tb);
  C[1] = C[0] * t3 (tb);
  D[1] = D[0] * t4 (tb);
  E[1] = 1.L / 3.L;
  A[2] = A[1] * t1 (tb);
  B[2] = B[1] * t2 (tb);
  C[2] = C[1] * t3 (tb);
  D[2] = D[1] * t4 (tb);
  E[2] = 0.25L;
  A[3] = 0.L;
  B[3] = b21 (tb) * t1 (tb);
  C[3] = b31 (tb) * t1 (tb) + b32 (tb) * t2 (tb);
  D[3] = b41 (tb) * t1 (tb) + b42 (tb) * t2 (tb) + b43 (tb) * t3 (tb);
  E[3] = 1.L / 6.L;
  solve_4 (A, B, C, D, E);
  if (isnan (E[0]) || isnan (E[1]) || isnan (E[2]) || isnan (E[3]))
    return 0;
  b54 (tb) = E[3];
  b53 (tb) = E[2];
  b52 (tb) = E[1];
  b51 (tb) = E[0];
  rk_b_5 (tb);
#if DEBUG_RK_5_3
  fprintf (stderr, "rk_tb_5_3t: end\n");
#endif
  return 1;
}

/**
 * Function to obtain the coefficients of a 5 steps 2nd-3rd order Runge-Kutta
 * pair.
 */
int
rk_tb_5_3p (Optimize * optimize)        ///< Optimize struct.
{
  long double *tb;
#if DEBUG_RK_5_3
  fprintf (stderr, "rk_tb_5_3p: start\n");
#endif
  if (!rk_tb_5_3 (optimize))
    return 0;
  tb = optimize->coefficient;
  e51 (tb) = 0.5L / t1 (tb);
  e52 (tb) = e53 (tb) = 0.L;
  rk_e_5 (tb);
#if DEBUG_RK_5_3
  fprintf (stderr, "rk_tb_5_3p: end\n");
#endif
  return 1;
}

/**
 * Function to obtain the coefficients of a 5 steps 2nd-3rd order, 3rd-4th order
 * in equations depending only in time, Runge-Kutta pair.
 */
int
rk_tb_5_3tp (Optimize * optimize)       ///< Optimize struct.
{
  long double *tb;
#if DEBUG_RK_5_3
  fprintf (stderr, "rk_tb_5_3tp: start\n");
#endif
  if (!rk_tb_5_3t (optimize))
    return 0;
  tb = optimize->coefficient;
  e53 (tb) = 0.L;
  e52 (tb) = (1.L / 3.L - 0.5L * t1 (tb)) / (t2 (tb) * (t2 (tb) - t1 (tb)));
  if (isnan (e52 (tb)))
    return 0;
  e51 (tb) = (0.5L - e52 (tb) * t2 (tb)) / t1 (tb);
  if (isnan (e51 (tb)))
    return 0;
  rk_e_5 (tb);
#if DEBUG_RK_5_3
  fprintf (stderr, "rk_tb_5_3tp: end\n");
#endif
  return 1;
}

/**
 * Function to calculate the objective function of a 5 steps 3rd order
 * Runge-Kutta method.
 *
 * \return objective function value.
 */
long double
rk_objective_tb_5_3 (RK * rk)   ///< RK struct.
{
  long double *tb;
  long double o;
#if DEBUG_RK_5_3
  fprintf (stderr, "rk_objective_tb_5_3: start\n");
#endif
  tb = rk->tb->coefficient;
  o = fminl (0.L, b20 (tb));
  if (b30 (tb) < 0.L)
    o += b30 (tb);
  if (b40 (tb) < 0.L)
    o += b40 (tb);
  if (b50 (tb) < 0.L)
    o += b50 (tb);
  if (b51 (tb) < 0.L)
    o += b51 (tb);
  if (b52 (tb) < 0.L)
    o += b52 (tb);
  if (b53 (tb) < 0.L)
    o += b53 (tb);
  if (o < 0.L)
    {
      o = 40.L - o;
      goto end;
    }
  o = 30.L
    + fmaxl (1.L, fmaxl (t1 (tb), fmaxl (t2 (tb), fmaxl (t3 (tb), t4 (tb)))));
  if (rk->strong)
    {
      rk_bucle_ac (rk);
      o = fminl (o, *rk->ac0->optimal);
    }
end:
#if DEBUG_RK_5_3
  fprintf (stderr, "rk_objective_tb_5_3: optimal=%Lg\n", o);
  fprintf (stderr, "rk_objective_tb_5_3: end\n");
#endif
  return o;
}

/**
 * Function to calculate the objective function of a 5 steps 3rd order, 4th
 * order in equations depending only in time, Runge-Kutta method.
 *
 * \return objective function value.
 */
long double
rk_objective_tb_5_3t (RK * rk)  ///< RK struct.
{
  long double *tb;
  long double o;
#if DEBUG_RK_5_3
  fprintf (stderr, "rk_objective_tb_5_3t: start\n");
#endif
  tb = rk->tb->coefficient;
  o = fminl (0.L, b20 (tb));
  if (b30 (tb) < 0.L)
    o += b30 (tb);
  if (b40 (tb) < 0.L)
    o += b40 (tb);
  if (b50 (tb) < 0.L)
    o += b50 (tb);
  if (b51 (tb) < 0.L)
    o += b51 (tb);
  if (b52 (tb) < 0.L)
    o += b52 (tb);
  if (b53 (tb) < 0.L)
    o += b53 (tb);
  if (b54 (tb) < 0.L)
    o += b54 (tb);
  if (o < 0.L)
    {
      o = 40.L - o;
      goto end;
    }
  o = 30.L
    + fmaxl (1.L, fmaxl (t1 (tb), fmaxl (t2 (tb), fmaxl (t3 (tb), t4 (tb)))));
  if (rk->strong)
    {
      rk_bucle_ac (rk);
      o = fminl (o, *rk->ac0->optimal);
    }
end:
#if DEBUG_RK_5_3
  fprintf (stderr, "rk_objective_tb_5_3t: optimal=%Lg\n", o);
  fprintf (stderr, "rk_objective_tb_5_3t: end\n");
#endif
  return o;
}

/**
 * Function to calculate the objective function of a 5 steps 2nd-3rd order
 * Runge-Kutta pair.
 *
 * \return objective function value.
 */
long double
rk_objective_tb_5_3p (RK * rk)  ///< RK struct.
{
  long double *tb;
  long double o;
#if DEBUG_RK_5_3
  fprintf (stderr, "rk_objective_tb_5_3p: start\n");
#endif
  tb = rk->tb->coefficient;
  o = fminl (0.L, b20 (tb));
  if (b30 (tb) < 0.L)
    o += b30 (tb);
  if (b40 (tb) < 0.L)
    o += b40 (tb);
  if (b50 (tb) < 0.L)
    o += b50 (tb);
  if (b51 (tb) < 0.L)
    o += b51 (tb);
  if (b52 (tb) < 0.L)
    o += b52 (tb);
  if (b53 (tb) < 0.L)
    o += b53 (tb);
  if (e50 (tb) < 0.L)
    o += e50 (tb);
  if (e51 (tb) < 0.L)
    o += e51 (tb);
  if (o < 0.L)
    {
      o = 40.L - o;
      goto end;
    }
  o = 30.L
    + fmaxl (1.L, fmaxl (t1 (tb), fmaxl (t2 (tb), fmaxl (t3 (tb), t4 (tb)))));
  if (rk->strong)
    {
      rk_bucle_ac (rk);
      o = fminl (o, *rk->ac0->optimal);
    }
end:
#if DEBUG_RK_5_3
  fprintf (stderr, "rk_objective_tb_5_3p: optimal=%Lg\n", o);
  fprintf (stderr, "rk_objective_tb_5_3p: end\n");
#endif
  return o;
}

/**
 * Function to calculate the objective function of a 5 steps 2nd-3rd order,
 * 3rd-4th order in equations depending only in time, Runge-Kutta pair.
 *
 * \return objective function value.
 */
long double
rk_objective_tb_5_3tp (RK * rk) ///< RK struct.
{
  long double *tb;
  long double o;
#if DEBUG_RK_5_3
  fprintf (stderr, "rk_objective_tb_5_3tp: start\n");
#endif
  tb = rk->tb->coefficient;
  o = fminl (0.L, b20 (tb));
  if (b30 (tb) < 0.L)
    o += b30 (tb);
  if (b40 (tb) < 0.L)
    o += b40 (tb);
  if (b50 (tb) < 0.L)
    o += b50 (tb);
  if (b51 (tb) < 0.L)
    o += b51 (tb);
  if (b52 (tb) < 0.L)
    o += b52 (tb);
  if (b53 (tb) < 0.L)
    o += b53 (tb);
  if (b54 (tb) < 0.L)
    o += b54 (tb);
  if (e50 (tb) < 0.L)
    o += e50 (tb);
  if (e51 (tb) < 0.L)
    o += e51 (tb);
  if (e52 (tb) < 0.L)
    o += e52 (tb);
  if (o < 0.L)
    {
      o = 40.L - o;
      goto end;
    }
  o = 30.L
    + fmaxl (1.L, fmaxl (t1 (tb), fmaxl (t2 (tb), fmaxl (t3 (tb), t4 (tb)))));
  if (rk->strong)
    {
      rk_bucle_ac (rk);
      o = fminl (o, *rk->ac0->optimal);
    }
end:
#if DEBUG_RK_5_3
  fprintf (stderr, "rk_objective_tb_5_3tp: optimal=%Lg\n", o);
  fprintf (stderr, "rk_objective_tb_5_3tp: end\n");
#endif
  return o;
}


/*
 * Copyright 2020-2021 OpenDR European Project
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef SCANMATCHERPROCESSOR_H
#define SCANMATCHERPROCESSOR_H

#include <gmapping/log/sensorlog.h>
#include <gmapping/sensor/sensor_range/rangereading.h>
#include <gmapping/sensor/sensor_range/rangesensor.h>
//#include <gsl/gsl_eigen.h>
#include "gmapping/scanmatcher/scanmatcher.h"

namespace GMapping {

  class ScanMatcherProcessor {
  public:
    ScanMatcherProcessor(const ScanMatcherMap &m);

    ScanMatcherProcessor(double xmin, double ymin, double xmax, double ymax, double delta, double patchdelta);

    virtual ~ScanMatcherProcessor();

    virtual void processScan(const RangeReading &reading);

    void setSensorMap(const SensorMap &smap, std::string sensorName = "FLASER");

    void init();

    void setMatchingParameters(double urange, double range, double sigma, int kernsize, double lopt, double aopt,
                               int iterations, bool computeCovariance = false);

    void setRegistrationParameters(double regScore, double critScore);

    OrientedPoint getPose() const;

    inline const ScanMatcherMap &getMap() const { return m_map; }

    inline ScanMatcher &matcher() { return m_matcher; }

    inline void setmaxMove(double mmove) { m_maxMove = mmove; }

    bool useICP;

  protected:
    ScanMatcher m_matcher;
    bool m_computeCovariance;
    bool m_first;
    SensorMap m_sensorMap;
    double m_regScore, m_critScore;
    unsigned int m_beams;
    double m_maxMove;
    // state
    ScanMatcherMap m_map;
    OrientedPoint m_pose;
    OrientedPoint m_odoPose;
    int m_count;
    // gsl_eigen_symmv_workspace * m_eigenspace;
  };

};  // namespace GMapping

#endif


#ifndef TRANS_TYPE
#define TRANS_TYPE

#include <gsl/gsl_vector.h>

// corresponding python variable: element of 'list_n_original_diff'
typedef struct {
    gsl_vector *inc1;
    gsl_vector *inc2;
    gsl_vector *skp1;
    gsl_vector *skp2;
    int inclu_len;
    int skip_len;
    int flag;
    char* id;
} odiff;

typedef struct {
    int batch_size;
    void **datum;
} batch_datum;

// used to represent the reture value of
// MLE_marginal_iteration and MLE_marginal_iteration_constrain.
// corresponding python variable: '[current_sum,[psi1,psi2,beta_0,beta_1,var1,var2]]'
typedef struct {
    double sum;
    struct {
        gsl_vector *psi1;
        gsl_vector *psi2;
        double beta0;
        double beta1;
        double var1;
        double var2;
    } params;
} mle_result;

// we use a linked list to represent a python list.
// corresponding python variable: 'list_n_original_diff'
typedef struct dnode{
    odiff* data;
    struct dnode* next;
    struct dnode* end;
} diff_list_node;

typedef struct node {
    void* data;
    struct node* next;
    struct node* end;
} list_node;

// we use fortran l_bfgs_b routine to solve the numerical optimization problem.
// (i.e. f = 0.0 at the optimal solution.)
// only the minimal intersection of C's and Fortran's many data types can be relied on:
// the following are some essential mapping between c type and fortran type
// counterpart of f2c.h
typedef int integer;
typedef float real;
typedef double doublereal;
typedef long int logical;

#endif