#ifndef UTIL_H
#define UTIL_H

#define PRECISION 2
#define MAX_LINE_LENGTH 9046
#define USE_CBLAS
#define USE_LAPACK

#include <assert.h>
#include <math.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/time.h>
#include <time.h>

#ifdef USE_CBLAS
#include <cblas.h>
#endif

#ifdef USE_LAPACK
#include <lapacke.h>
#endif

/******************************************************************************
 *                                LOGGING
 ******************************************************************************/

/* DEBUG */
#ifdef NDEBUG
#define DEBUG(M, ...)
#else
#define DEBUG(M, ...) \
  fprintf(stderr, "[DEBUG] %s:%d: " M "\n", __func__, __LINE__, ##__VA_ARGS__)
#endif

/* LOG */
#define LOG_ERROR(M, ...) \
  fprintf(stderr, "[ERROR] [%s] " M "\n", __func__, ##__VA_ARGS__)
#define LOG_WARN(M, ...) fprintf(stderr, "[WARN] " M "\n", ##__VA_ARGS__)
#define LOG_INFO(M, ...) fprintf(stderr, "[INFO] " M "\n", ##__VA_ARGS__)

/* FATAL */
#define FATAL(M, ...)                                \
  fprintf(stderr, "[FATAL] " M "\n", ##__VA_ARGS__); \
  exit(-1);

/* CHECK */
#define CHECK(A, M, ...)       \
  if (!(A)) {                  \
    log_err(M, ##__VA_ARGS__); \
    goto error;                \
  }

/******************************************************************************
 *                                   DATA
 ******************************************************************************/

#if PRECISION == 1
typedef float real_t;
#elif PRECISION == 2
typedef double real_t;
#else
#error "Precision not defined!"
#endif

char *malloc_string(const char *s);
int dsv_rows(const char *fp);
int dsv_cols(const char *fp, const char delim);
char **dsv_fields(const char *fp, const char delim, int *nb_fields);
real_t **dsv_data(const char *fp, const char delim, int *nb_rows, int *nb_cols);
real_t **csv_data(const char *fp, int *nb_rows, int *nb_cols);
int **load_iarrays(const char *csv_path, int *nb_arrays);
real_t **load_darrays(const char *csv_path, int *nb_arrays);
real_t *load_vector(const char *file_path);

/******************************************************************************
 *                                 MATHS
 ******************************************************************************/

#define UNUSED(expr) \
  do {               \
    (void)(expr);    \
  } while (0)

#ifndef M_PI
#define M_PI (3.14159265358979323846)
#endif

#define MIN(x, y) ((x) < (y) ? (x) : (y))
#define MAX(x, y) ((x) > (y) ? (x) : (y))
#define SIGN(a, b) ((b) >= 0.0 ? fabs(a) : -fabs(a))

float randf(float a, float b);
real_t deg2rad(const real_t d);
real_t rad2deg(const real_t r);
int fltcmp(const real_t x, const real_t y);
real_t pythag(const real_t a, const real_t b);
real_t lerp(const real_t a, const real_t b, const real_t t);
void lerp3(const real_t *a, const real_t *b, const real_t t, real_t *x);
real_t sinc(const real_t x);

/******************************************************************************
 *                              LINEAR ALGEBRA
 ******************************************************************************/

void print_matrix(const char *prefix, const real_t *data, const size_t m,
                  const size_t n);
void print_vector(const char *prefix, const real_t *data, const size_t length);

void eye(real_t *A, const size_t m, const size_t n);
void ones(real_t *A, const size_t m, const size_t n);
void zeros(real_t *A, const size_t m, const size_t n);

real_t *mat_new(const size_t m, const size_t n);
int mat_cmp(const real_t *A, const real_t *B, const size_t m, const size_t n);
int mat_equals(const real_t *A, const real_t *B, const size_t m, const size_t n,
               const real_t tol);
int mat_save(const char *save_path, const real_t *A, const int m, const int n);
real_t *mat_load(const char *save_path, int *nb_rows, int *nb_cols);
void mat_set(real_t *A, const size_t stride, const size_t i, const size_t j,
             const real_t val);
real_t mat_val(const real_t *A, const size_t stride, const size_t i,
               const size_t j);
void mat_copy(const real_t *src, const int m, const int n, real_t *dest);
void mat_block_get(const real_t *A, const size_t stride, const size_t rs,
                   const size_t cs, const size_t re, const size_t ce,
                   real_t *block);
void mat_block_set(real_t *A, const size_t stride, const size_t rs,
                   const size_t cs, const size_t re, const size_t ce,
                   const real_t *block);
void mat_diag_get(const real_t *A, const int m, const int n, real_t *d);
void mat_diag_set(real_t *A, const int m, const int n, const real_t *d);
void mat_triu(const real_t *A, const size_t n, real_t *U);
void mat_tril(const real_t *A, const size_t n, real_t *L);
real_t mat_trace(const real_t *A, const size_t m, const size_t n);
void mat_transpose(const real_t *A, size_t m, size_t n, real_t *A_t);
void mat_add(const real_t *A, const real_t *B, real_t *C, size_t m, size_t n);
void mat_sub(const real_t *A, const real_t *B, real_t *C, size_t m, size_t n);
void mat_scale(real_t *A, const size_t m, const size_t n, const real_t scale);

real_t *vec_new(const size_t length);
void vec_copy(const real_t *src, const size_t length, real_t *dest);
int vec_equals(const real_t *x, const real_t *y, const size_t length);
void vec_add(const real_t *x, const real_t *y, real_t *z, size_t length);
void vec_sub(const real_t *x, const real_t *y, real_t *z, size_t length);
void vec_scale(real_t *x, const size_t length, const real_t scale);
real_t vec_norm(const real_t *x, const size_t length);

void dot(const real_t *A, const size_t A_m, const size_t A_n, const real_t *B,
         const size_t B_m, const size_t B_n, real_t *C);
void skew(const real_t x[3], real_t A[3 * 3]);
void fwdsubs(const real_t *L, const real_t *b, real_t *y, const size_t n);
void bwdsubs(const real_t *U, const real_t *y, real_t *x, const size_t n);
int check_jacobian(const char *jac_name, const real_t *fdiff, const real_t *jac,
                   const size_t m, const size_t n, const real_t tol,
                   const int print);

#ifdef USE_CBLAS
void cblas_dot(const real_t *A, const size_t A_m, const size_t A_n,
               const real_t *B, const size_t B_m, const size_t B_n, real_t *C);
#endif

/******************************************************************************
 *                                   SVD
 ******************************************************************************/

int svd(real_t *A, int m, int n, real_t *U, real_t *s, real_t *V_t);
int svdcomp(real_t *A, int m, int n, real_t *w, real_t *V);
int pinv(real_t *A, const int m, const int n, real_t *A_inv);

#ifdef USE_LAPACK

#endif

/******************************************************************************
 *                                   CHOL
 ******************************************************************************/

void chol(const real_t *A, const size_t n, real_t *L);
void chol_solve(const real_t *A, const real_t *b, real_t *x, const size_t n);

#ifdef USE_LAPACK
void lapack_chol_solve(const real_t *A, const real_t *b, real_t *x,
                       const size_t n);
#endif

/******************************************************************************
 *                                   TIME
 ******************************************************************************/

typedef uint64_t timestamp_t;

struct timespec tic();
float toc(struct timespec *tic);
float mtoc(struct timespec *tic);
float time_now();

/******************************************************************************
 *                                TRANSFORMS
 ******************************************************************************/

void tf(const real_t params[7], real_t T[4 * 4]);
void tf_params(const real_t T[4 * 4], real_t params[7]);
void tf_rot_set(real_t T[4 * 4], const real_t C[3 * 3]);
void tf_trans_set(real_t T[4 * 4], const real_t r[3]);
void tf_trans_get(const real_t T[4 * 4], real_t r[3]);
void tf_rot_get(const real_t T[4 * 4], real_t C[3 * 3]);
void tf_quat_get(const real_t T[4 * 4], real_t q[4]);
void tf_inv(const real_t T[4 * 4], real_t T_inv[4 * 4]);
void tf_point(const real_t T[4 * 4], const real_t p[3], real_t retval[3]);
void tf_hpoint(const real_t T[4 * 4], const real_t p[4], real_t retval[4]);
void tf_perturb_rot(real_t T[4 * 4], const real_t step_size, const int i);
void tf_perturb_trans(real_t T[4 * 4], const real_t step_size, const int i);
void rvec2rot(const real_t *rvec, const real_t eps, real_t *R);
void euler321(const real_t euler[3], real_t C[3 * 3]);
void rot2quat(const real_t C[3 * 3], real_t q[4]);
void quat2euler(const real_t q[4], real_t euler[3]);
void quat2rot(const real_t q[4], real_t C[3 * 3]);
void quat_lmul(const real_t p[4], const real_t q[4], real_t r[4]);
void quat_rmul(const real_t p[4], const real_t q[4], real_t r[4]);
void quat_mul(const real_t p[4], const real_t q[4], real_t r[4]);
void quat_delta(const real_t dalpha[3], real_t dq[4]);

/******************************************************************************
 *                                 IMAGE
 ******************************************************************************/

typedef struct image_t {
  int width;
  int height;
  uint8_t *data;
} image_t;

void image_init(image_t *img, uint8_t *data, int width, int height);

/******************************************************************************
 *                                  CV
 ******************************************************************************/

/******************************** RADTAN **************************************/

void radtan4_distort(const real_t params[4], const real_t p[2], real_t p_d[2]);
void radtan4_point_jacobian(const real_t params[4], const real_t p[2],
                            real_t J_point[2 * 2]);
void radtan4_params_jacobian(const real_t params[4], const real_t p[2],
                             real_t J_param[2 * 4]);

/********************************* EQUI ***************************************/

void equi4_distort(const real_t params[4], const real_t p[2], real_t p_d[2]);
void equi4_point_jacobian(const real_t params[4], const real_t p[2],
                          real_t J_point[2 * 2]);
void equi4_params_jacobian(const real_t params[4], const real_t p[2],
                           real_t J_param[2 * 4]);

/******************************** PINHOLE *************************************/

real_t pinhole_focal(const int image_width, const real_t fov);
int pinhole_project(const real_t params[4], const real_t p_C[3], real_t x[2]);

void pinhole_point_jacobian(const real_t params[4], real_t J_point[2 * 3]);
void pinhole_params_jacobian(const real_t params[4], const real_t x[2],
                             real_t J[2 * 4]);

/**************************** PINHOLE-RADTAN4 *********************************/

void pinhole_radtan4_project(const real_t params[8], const real_t p_C[3],
                             real_t x[2]);
void pinhole_radtan4_project_jacobian(const real_t params[8],
                                      const real_t p_C[3], real_t J[2 * 3]);
void pinhole_radtan4_params_jacobian(const real_t params[8],
                                     const real_t p_C[3], real_t J[2 * 8]);

/***************************** PINHOLE-EQUI4 **********************************/

void pinhole_equi4_project(const real_t params[8], const real_t p_C[3],
                           real_t x[2]);
void pinhole_equi4_project_jacobian(const real_t params[8], const real_t p_C[3],
                                    real_t J[2 * 3]);
void pinhole_equi4_params_jacobian(const real_t params[8], const real_t p_C[3],
                                   real_t J[2 * 8]);

#endif  // ZERO_H


/*
 * Copyright 2008-2016 Jan Gasthaus
 *
 * 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.
 */

/**
 * Functions for computing and tabulating the generalized stirling numbers
 * of type (-1,-d,0).
 *
 */

#ifndef STIRLING_H_
#define STIRLING_H_
#include <cmath>
#include <vector>
#include <string>
#include <iostream>
#include <cassert>
#include <gsl/gsl_sf_gamma.h>

namespace gatsby { namespace libplump {

typedef std::vector<double> d_vec;
typedef std::vector<d_vec> d_vec_vec;

/**
 * computes log(exp(a) + exp(b)) while avoiding numerical
 * instabilities.
 */
inline double fast_logsumexp(double a, double b) {
  if (a>b) {
    return log(1+exp(b-a)) + a;
  } else {
    return log(1+exp(a-b)) + b;
  }
}

/**
 * computes log(exp(a) - exp(b)) while avoiding numerical
 * instabilities.
 */
inline double fast_logminusexp(double a, double b) {
  if (a>b) {
    return log(1-exp(b-a)) + a;
  } else {
    return log(1-exp(a-b)) + b;
  }
}

/**
 * Compute S_d(c,t) recursively by directly applying the recursion
 * S_d(c,t) = S_d(c-1,t-1) + (c-1 - t*d)S_d(c-1,t)
 *
 * This does a _lot_ of duplicate work and should only be used for debugging!
 */
double gen_stirling_recursive(double d, int c, int t);

/**
 * Compute S_d(c,t) recursively in log space by directly applying the recursion
 * S_d(c,t) = S_d(c-1,t-1) + (c-1 - t*d)S_d(c-1,t)
 *
 * This does a _lot_ of duplicate work and should only be used for debugging!
 */
double log_gen_stirling_recursive(double d, int c, int t);

/**
 * Compute S_d(c,t) directly in log space using the recursion
 * S_d(c,t) = S_d(c-1,t-1) + (c-1 - t*d)S_d(c-1,t)
 */
double log_gen_stirling_direct(double d, int c, int t);

double log_gen_stirling_ratio(double d, int c, int t);

d_vec_vec log_gen_stirling_table(double d, int c);

void log_gen_stirling_table_extend(double d, int c, d_vec_vec& table);

double log_get_stirling_from_table(d_vec_vec& table, int c, int t);

class stirling_generator_recompute_log {
  public:
    stirling_generator_recompute_log(double d, int c, int t);

    double ratio(int c, int t);

    static std::string statsToString();

  private:
    double d;
};

class stirling_generator_fast_log {

  public:
    stirling_generator_fast_log(double d, int c, int t);

    double get(int c, int t);

    double ratio(int c, int t);

    static std::string statsToString();

  private:
    void incC();
    void decC();

    double d;
    int current_c;
    d_vec row, col, prev_col;
};

/**
 * Class to encapsulate the generation of ratios of stirling numbers
 * for removing customers from restaurants.
 *
 * One of these objects should be constructed for each restaurant,
 * providing the current number of tables, and the total number of customers.
 */
class stirling_generator_full_log {
  public:
    stirling_generator_full_log(double d, int c, int t);

    double ratio(int c, int t);

    double getLog(int c, int t);

    static std::string statsToString();

  private:

    static int global_c_max, num_ratio_calls, num_extends, num_construct;

    d_vec_vec table;
    int c_max;
    double d;
};

inline double log_get_stirling_from_table(d_vec_vec& table, int c, int t) {
  // c and t must be non-negative
  assert(c >= 0 && t>= 0);

  if ((c == 1  && t == 1) || (c == 0 && t == 0))
    return 0;
  if (c == 0 || t == 0)
    return -INFINITY;
  if (t > c)
    return -INFINITY;
  if(c == t)
    return 0;
  if (t > (int)table.size() || (c - t) > (int)table[t-1].size()) {
    std::cout<< c << ", " << t << std::endl;
  }
  return table[t-1][c-t-1];
}

inline double log_stirling_asymptotic(double d, int c, int t) {
  return   gsl_sf_lngamma(c)
         - gsl_sf_lngamma(1 - d)
         - gsl_sf_lngamma(t)
         - (t-1) * log(d)
         - d * log(c);
}

// class log_gen_stirling_table {
//     private:
//         std::vector<std::vector<double> > table;
//         size_t c_cur, t_cur;
//
//         /*
//          * Grow the arrays to the required size.
//          */
//         void _grow(size_t c_max, size_t t_max) {
//             for(int i=t_cur;i<t_max;i++) {
//                 std::vector<double> tmp(c_cur,0);
//                 table.push_back(std::vector<double>());
//             }
//         }
//
//         /*
//          * Tabulate up to the specified limits
//          */
//         void _tabulate(size_t c_max, size_t t_max) {
//             for t in xrange(1,max_t):
//         for c in xrange(t,max_c):
//             a[c,t] = a[c-1,t-1] + (c-1-d*t)*a[c-1,t]
//
//         }
//
//     public:
//         double d;
//         gen_stirling_table(double d) : d(d), c_cur(0), t_cur(0)  {}
//
// };

}} // namespace gatsby::libplump

#endif /* STIRLING_H_ */


/**
 *
 * @file core_clarfy.c
 *
 *  PLASMA core_blas kernel
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @version 2.6.0
 * @author Azzam Haidar
 * @date 2011-05-15
 * @generated c Tue Jan  7 11:44:49 2014
 *
 **/
#include <math.h>
#include <lapacke.h>
#include "common.h"

#undef REAL
#define COMPLEX

/***************************************************************************//**
 *
 * @ingroup CORE_PLASMA_Complex32_t
 *
 *  CORE_clarfy applies an elementary reflector, or Householder matrix, H,
 *  to a N-by-N hermitian matrix C, from both the left and the right.
 *
 *  H is represented in the form
 *
 *     H = I - tau * v * v'
 *
 *  where  tau  is a scalar and  v  is a vector.
 *
 *  If tau is zero, then H is taken to be the unit matrix.
 *
 *******************************************************************************
 *
 * @param[in] N
 *          The number of rows and columns of the matrix C.  N >= 0.
 *
 * @param[in,out] A
 *          COMPLEX*16 array, dimension (LDA, N)
 *          On entry, the Hermetian matrix A.
 *          On exit, A is overwritten by H * A * H'.
 *
 * @param[in] LDA
 *         The leading dimension of the array A.  LDA >= max(1,N).
 *
 * @param[in] V
 *          The vector V that contains the Householder reflectors.
 *
 * @param[in] TAU
 *          The value tau.
 *
 * @param[out] WORK
 *          Workspace.
 *
 ******************************************************************************/
void
CORE_clarfy(int N,
            PLASMA_Complex32_t *A, int LDA,
            const PLASMA_Complex32_t *V,
            const PLASMA_Complex32_t *TAU,
            PLASMA_Complex32_t *WORK)
{
    static PLASMA_Complex32_t zzero =  0.0;
    static PLASMA_Complex32_t zmone = -1.0;

    int j;
    PLASMA_Complex32_t dtmp;

    /* Compute dtmp = X'*V */
    /* X = AVtau */
    cblas_chemv(CblasColMajor, CblasLower,
                N, CBLAS_SADDR(*TAU), A, LDA,
                V, 1, CBLAS_SADDR(zzero), WORK, 1);

    /* cblas_cdotc_sub(N, WORK, 1, V, 1, &dtmp);*/
    dtmp = 0.;
    for (j = 0; j < N ; j++)
        dtmp = dtmp + conjf(WORK[j]) * V[j];

    /* Compute 1/2 X'*V*t = 1/2*dtmp*tau  */
    dtmp = -dtmp * 0.5 * (*TAU);

   /* Compute W=X-1/2VX'Vt = X - dtmp*V */
    cblas_caxpy(N, CBLAS_SADDR(dtmp),
                V, 1, WORK, 1);

    /*
     * Performs the symmetric rank 2 operation
     *    A := alpha*x*y' + alpha*y*x' + A
     */
    cblas_cher2(CblasColMajor, CblasLower, N,
                CBLAS_SADDR(zmone), WORK, 1,
                                    V,    1,
                                    A,    LDA);

    return;
}
#undef COMPLEX


//This takes the output of window_univar, and applies the 1D FFT to each row or col.
//The input dim is which dim to take the 1D FFT along (0->along cols, 1->along rows).
//The output Y is power (real-valued), i.e. the FFT squared, Y = X*conj(X) element-wise.
//The size of Y must be FxC if dim==0, and RxF if dim==1, where F = nfft/2 + 1.

//I tried parallel versions with OpenMP, but much slower (have to make fftw_plan P times!).

#include <stdio.h>
#include <cblas.h>
#include <fftw3.h>

#ifdef __cplusplus
namespace ov {
extern "C" {
#endif

int fft_squared_s (float *Y, const float *X, const int iscolmajor, const int R, const int C, const int dim, const int nfft);
int fft_squared_d (double *Y, const double *X, const int iscolmajor, const int R, const int C, const int dim, const int nfft);

int fft_squared_s (float *Y, const float *X, const int iscolmajor, const int R, const int C, const int dim, const int nfft)
{
    const float z = 0.0f;
    const int F = nfft/2 + 1;
    int r, c, f;
    float *X1, *Y1;
    fftwf_plan plan;
    //struct timespec tic, toc;

    //Checks
    if (R<1) { fprintf(stderr,"error in fft_squared_s: R (nrows X) must be positive\n"); return 1; }
    if (C<1) { fprintf(stderr,"error in fft_squared_s: C (ncols X) must be positive\n"); return 1; }
    if (nfft<R && dim==0) { fprintf(stderr,"error in fft_squared_s: nfft must be >= R (winlength)\n"); return 1; }
    if (nfft<C && dim==1) { fprintf(stderr,"error in fft_squared_s: nfft must be >= C (winlength)\n"); return 1; }

    //Initialize fftwf
    X1 = fftwf_alloc_real((size_t)nfft);
    Y1 = fftwf_alloc_real((size_t)nfft);
    plan = fftwf_plan_r2r_1d(nfft,X1,Y1,FFTW_R2HC,FFTW_ESTIMATE);
    if (!plan) { fprintf(stderr,"error in fft_squared_s: problem creating fftw plan\n"); return 1; }
    cblas_scopy(nfft-R,&z,0,&X1[R],1); //zero-pad

    if (dim==0u)
    {
        if (iscolmajor)
        {
            //clock_gettime(CLOCK_REALTIME,&tic);
            for (c=0; c<C; c++)
            {
                cblas_scopy(R,&X[c*R],1,&X1[0],1);
                fftwf_execute(plan);

                //n = c*F; Y[n++] = fmaf(Y1[0],Y1[0],0.0f); //this was ~25% slower
                //for (f=1; f<F-1; f++, n++) { Y[n] = fmaf(Y1[f],Y1[f],fmaf(Y1[nfft-f],Y1[nfft-f],0.0f)); }
                //Y[n] = (nfft%2) ? fmaf(Y1[f],Y1[f],fmaf(Y1[nfft-f],Y1[nfft-f],0.0f)) : fmaf(Y1[f],Y1[f],0.0f);

                //n = c*F; Y[n++] = Y1[0]*Y1[0];       //this was also ~25% slower
                //for (f=1; f<F-1; f++, n++) { Y[n] = Y1[f]*Y1[f] + Y1[nfft-f]*Y1[nfft-f]; }
                //Y[n] = (nfft%2) ? Y1[f]*Y1[f] + Y1[nfft-f]*Y1[nfft-f] : Y1[f]*Y1[f];

                for (f=0; f<nfft; f++) { Y1[f] *= Y1[f]; }
                for (f=1; f<F-1+nfft%2; f++) { Y1[f] += Y1[nfft-f]; }
                cblas_scopy(F,&Y[c*F],1,&Y1[0],1);
            }
            //clock_gettime(CLOCK_REALTIME,&toc);
            //fprintf(stderr,"elapsed time = %f ms\n",(toc.tv_sec-tic.tv_sec)*1e3+(toc.tv_nsec-tic.tv_nsec)/1e6);
        }
        else
        {
            for (c=0; c<C; c++)
            {
                cblas_scopy(R,&X[c],C,&X1[0],1);
                fftwf_execute(plan);
                for (f=0; f<nfft; f++) { Y1[f] *= Y1[f]; }
                for (f=1; f<F-1+nfft%2; f++) { Y1[f] += Y1[nfft-f]; }
                cblas_scopy(F,&Y[c],C,&Y1[0],1);
            }
        }
    }
    else if (dim==1u)
    {
        if (iscolmajor)
        {
            for (r=0; r<R; r++)
            {
                cblas_scopy(C,&X[r],R,&X1[0],1);
                fftwf_execute(plan);
                for (f=0; f<nfft; f++) { Y1[f] *= Y1[f]; }
                for (f=1; f<F-1+nfft%2; f++) { Y1[f] += Y1[nfft-f]; }
                cblas_scopy(F,&Y[r],R,&Y1[0],1);
            }
        }
        else
        {
            for (r=0; r<R; r++)
            {
                cblas_scopy(C,&X[r*C],1,&X1[0],1);
                fftwf_execute(plan);
                for (f=0; f<nfft; f++) { Y1[f] *= Y1[f]; }
                for (f=1; f<F-1+nfft%2; f++) { Y1[f] += Y1[nfft-f]; }
                cblas_scopy(F,&Y[r*F],1,&Y1[0],1);
            }
        }
    }
    else
    {
        fprintf(stderr,"error in fft_squared_s: dim must be 0 or 1.\n"); return 1;
    }

    fftwf_destroy_plan(plan); fftwf_free(X1); fftwf_free(Y1);
    return 0;
}

int fft_squared_d (double *Y, const double *X, const int iscolmajor, const int R, const int C, const int dim, const int nfft)
{
    const double z = 0.0;
    const int F = nfft/2 + 1;
    int r, c, f;
    double *X1, *Y1;
    fftw_plan plan;

    //Checks
    if (R<1) { fprintf(stderr,"error in fft_squared_d: R (nrows X) must be positive\n"); return 1; }
    if (C<1) { fprintf(stderr,"error in fft_squared_d: C (ncols X) must be positive\n"); return 1; }
    if (nfft<R && dim==0) { fprintf(stderr,"error in fft_squared_s: nfft must be >= R (winlength)\n"); return 1; }
    if (nfft<C && dim==1) { fprintf(stderr,"error in fft_squared_s: nfft must be >= C (winlength)\n"); return 1; }

    //Initialize fftw
    X1 = fftw_alloc_real((size_t)nfft);
    Y1 = fftw_alloc_real((size_t)nfft);
    plan = fftw_plan_r2r_1d(nfft,X1,Y1,FFTW_R2HC,FFTW_ESTIMATE);
    if (!plan) { fprintf(stderr,"error in fft_squared_d: problem creating fftw plan\n"); return 1; }
    cblas_dcopy(nfft,&z,0,&X1[0],1); //zero-pad

    if (dim==0u)
    {
        if (iscolmajor)
        {
            for (c=0; c<C; c++)
            {
                cblas_dcopy(R,&X[c*R],1,&X1[0],1);
                fftw_execute(plan);
                for (f=0; f<nfft; f++) { Y1[f] *= Y1[f]; }
                for (f=1; f<F-1+nfft%2; f++) { Y1[f] += Y1[nfft-f]; }
                cblas_dcopy(F,&Y[c*F],1,&Y1[0],1);
            }
        }
        else
        {
            for (c=0; c<C; c++)
            {
                cblas_dcopy(R,&X[c],C,&X1[0],1);
                fftw_execute(plan);
                for (f=0; f<nfft; f++) { Y1[f] *= Y1[f]; }
                for (f=1; f<F-1+nfft%2; f++) { Y1[f] += Y1[nfft-f]; }
                cblas_dcopy(F,&Y[c],C,&Y1[0],1);
            }
        }
    }
    else if (dim==1u)
    {
        if (iscolmajor)
        {
            for (r=0; r<R; r++)
            {
                cblas_dcopy(C,&X[r],R,&X1[0],1);
                fftw_execute(plan);
                for (f=0; f<nfft; f++) { Y1[f] *= Y1[f]; }
                for (f=1; f<F-1+nfft%2; f++) { Y1[f] += Y1[nfft-f]; }
                cblas_dcopy(F,&Y[r],R,&Y1[0],1);
            }
        }
        else
        {
            for (r=0; r<R; r++)
            {
                cblas_dcopy(C,&X[r*C],1,&X1[0],1);
                fftw_execute(plan);
                for (f=0; f<nfft; f++) { Y1[f] *= Y1[f]; }
                for (f=1; f<F-1+nfft%2; f++) { Y1[f] += Y1[nfft-f]; }
                cblas_dcopy(F,&Y[r*F],1,&Y1[0],1);
            }
        }
    }
    else
    {
        fprintf(stderr,"error in fft_squared_d: dim must be 0 or 1.\n"); return 1;
    }

    fftw_destroy_plan(plan); fftw_free(X1); fftw_free(Y1);
    return 0;
}

#ifdef __cplusplus
}
}
#endif


//Gets deltas (1st order) and delta-deltas (2nd order) differences of X,
//which is assumed to be pre-allocated of triple size,
//Thus, for dim==0, C%3 must equal 0, and C/3 is the original ncols of X.
//And,  for dim==1, R%3 must equal 0, and R/3 is the original nrows of X.

//I implement this just like FIR for speed and shorter code,
//except non-causal and mid-sample of B is 0,
//so I don't explicitly make B (e.g., B[n] just equals sc*n).

//Note that this may treat edge samples differently than other code.
//But this could be changed with a few lines of additional code here,
//without changing the super-efficient FIR implementation.
//That is, since out-of-range samps were assumed to be 0, nothing was
//added to Y for them, so can just add something later.

#include <stdio.h>
#include <cblas.h>

#ifdef __cplusplus
namespace ov {
extern "C" {
#endif

int add_delta_deltas_s (float *X, const int iscolmajor, const int R, const int C, const int dim, const int N);
int add_delta_deltas_d (double *X, const int iscolmajor, const int R, const int C, const int dim, const int N);

int add_delta_deltas_s (float *X, const int iscolmajor, const int R, const int C, const int dim, const int N)
{
    const float z = 0.0f;
    const int No = R*C/3;
    int r, c, n;
    float sc = 1.0f;

    //Checks
    if (R<1) { fprintf(stderr,"error in add_delta_deltas_s: R (nrows X) must be positive\n"); return 1; }
    if (C<1) { fprintf(stderr,"error in add_delta_deltas_s: C (ncols X) must be positive\n"); return 1; }
    if (N<1) { fprintf(stderr,"error in add_delta_deltas_s: N (delta winlength) must be positive\n"); return 1; }
    if (dim==0 && C%3!=0) { fprintf(stderr,"error in add_delta_deltas_s: for dim==0, C (ncols X) must be divisible by 3\n"); return 1; }
    if (dim==1 && R%3!=0) { fprintf(stderr,"error in add_delta_deltas_s: for dim==1, R (nrows X) must be divisible by 3\n"); return 1; }

    //Get sc (normalizer)
    for (n=2; n<=N; n++) { sc += n*n; }
    sc = 0.5f/sc;

    if (dim==0)
    {
        if (iscolmajor)
        {
            cblas_scopy(2*No,&z,0,&X[No],1);
            for (c=0; c<C/3; c++)
            {
                for (n=1; n<=N; n++)
                {
                    cblas_saxpy(n,-sc*n,&X[c*R],0,&X[No+c*R],1);             //beg edge samps
                    cblas_saxpy(R-n,-sc*n,&X[c*R],1,&X[No+c*R+n],1);         //past samps
                    cblas_saxpy(R-n,sc*n,&X[c*R+n],1,&X[No+c*R],1);          //future samps
                    cblas_saxpy(n,sc*n,&X[c*R+R-1],0,&X[No+c*R+R-n],1);      //end edge samps
                }
                for (n=1; n<=N; n++)
                {
                    cblas_saxpy(n,-sc*n,&X[No+c*R],0,&X[2*No+c*R],1);        //beg edge samps
                    cblas_saxpy(R-n,-sc*n,&X[No+c*R],1,&X[2*No+c*R+n],1);    //past samps
                    cblas_saxpy(R-n,sc*n,&X[No+c*R+n],1,&X[2*No+c*R],1);     //future samps
                    cblas_saxpy(n,sc*n,&X[No+c*R+R-1],0,&X[2*No+c*R+R-n],1); //end edge samps
                }
            }
        }
        else
        {
            for (c=0; c<C/3; c++)
            {
                cblas_scopy(R,&z,0,&X[C/3+c],C);
                for (n=1; n<=N; n++)
                {
                    cblas_saxpy(n,-sc*n,&X[c],0,&X[C/3+c],C);                      //beg edge samps
                    cblas_saxpy(R-n,-sc*n,&X[c],C,&X[C/3+c+n*C],C);                //past samps
                    cblas_saxpy(R-n,sc*n,&X[c+n*C],C,&X[C/3+c],C);                 //future samps
                    cblas_saxpy(n,sc*n,&X[c+C*(R-1)],0,&X[C/3+c+C*(R-n)],C);       //end edge samps
                }
                cblas_scopy(R,&z,0,&X[2*C/3+c],C);
                for (n=1; n<=N; n++)
                {
                    cblas_saxpy(n,-sc*n,&X[C/3+c],0,&X[2*C/3+c],C);                //beg edge samps
                    cblas_saxpy(R-n,-sc*n,&X[C/3+c],C,&X[2*C/3+c+n*C],C);          //past samps
                    cblas_saxpy(R-n,sc*n,&X[C/3+c+n*C],C,&X[2*C/3+c],C);           //future samps
                    cblas_saxpy(n,sc*n,&X[C/3+c+C*(R-1)],0,&X[2*C/3+c+C*(R-n)],C); //end edge samps
                }
            }
        }
    }
    else if (dim==1)
    {
        if (iscolmajor)
        {
            for (r=0; r<R/3; r++)
            {
                cblas_scopy(C,&z,0,&X[R/3+r],R);
                for (n=1; n<=N; n++)
                {
                    cblas_saxpy(n,-sc*n,&X[r],0,&X[R/3+r],R);                      //beg edge samps
                    cblas_saxpy(C-n,-sc*n,&X[r],R,&X[R/3+r+n*R],R);                //past samps
                    cblas_saxpy(C-n,sc*n,&X[r+n*R],R,&X[R/3+r],R);                 //future samps
                    cblas_saxpy(n,sc*n,&X[r+R*(C-1)],0,&X[R/3+r+R*(C-n)],R);       //end edge samps
                }
                cblas_scopy(C,&z,0,&X[2*R/3+r],R);
                for (n=1; n<=N; n++)
                {
                    cblas_saxpy(n,-sc*n,&X[R/3+r],0,&X[2*R/3+r],R);                //beg edge samps
                    cblas_saxpy(C-n,-sc*n,&X[R/3+r],R,&X[2*R/3+r+n*R],R);          //past samps
                    cblas_saxpy(C-n,sc*n,&X[R/3+r+n*R],R,&X[2*R/3+r],R);           //future samps
                    cblas_saxpy(n,sc*n,&X[R/3+r+R*(C-1)],0,&X[2*R/3+r+R*(C-n)],R); //end edge samps
                }
            }
        }
        else
        {
            cblas_scopy(2*No,&z,0,&X[No],1);
            for (r=0; r<R/3; r++)
            {
                for (n=1; n<=N; n++)
                {
                    cblas_saxpy(n,-sc*n,&X[r*C],0,&X[No+r*C],1);             //beg edge samps
                    cblas_saxpy(C-n,-sc*n,&X[r*C],1,&X[No+r*C+n],1);         //past samps
                    cblas_saxpy(C-n,sc*n,&X[r*C+n],1,&X[No+r*C],1);          //future samps
                    cblas_saxpy(n,sc*n,&X[r*C+C-1],0,&X[No+r*C+C-n],1);      //end edge samps
                }
                for (n=1; n<=N; n++)
                {
                    cblas_saxpy(n,-sc*n,&X[No+r*C],0,&X[2*No+r*C],1);        //beg edge samps
                    cblas_saxpy(C-n,-sc*n,&X[No+r*C],1,&X[2*No+r*C+n],1);    //past samps
                    cblas_saxpy(C-n,sc*n,&X[No+r*C+n],1,&X[2*No+r*C],1);     //future samps
                    cblas_saxpy(n,sc*n,&X[No+r*C+C-1],0,&X[2*No+r*C+C-n],1); //end edge samps
                }
            }
        }
    }
    else
    {
        fprintf(stderr,"error in add_delta_deltas_s: dim must be 0 or 1.\n"); return 1;
    }

    return 0;
}

int add_delta_deltas_d (double *X, const int iscolmajor, const int R, const int C, const int dim, const int N)
{
    const double z = 0.0;
    const int No = R*C/3;
    int r, c, n;
    double sc = 1.0;

    //Checks
    if (R<1) { fprintf(stderr,"error in add_delta_deltas_d: R (nrows X) must be positive\n"); return 1; }
    if (C<1) { fprintf(stderr,"error in add_delta_deltas_d: C (ncols X) must be positive\n"); return 1; }
    if (N<1) { fprintf(stderr,"error in add_delta_deltas_d: N (delta winlength) must be positive\n"); return 1; }
    if (dim==0 && C%3!=0) { fprintf(stderr,"error in add_delta_deltas_d: C (ncols X) must be divisible by 3\n"); return 1; }
    if (dim==1 && R%3!=0) { fprintf(stderr,"error in add_delta_deltas_d: R (nrows X) must be divisible by 3\n"); return 1; }

    //Get sc (normalizer)
    for (n=2; n<=N; n++) { sc += n*n; }
    sc = 0.5/sc;

    if (dim==0)
    {
        if (iscolmajor)
        {
            cblas_dcopy(2*No,&z,0,&X[No],1);
            for (c=0; c<C/3; c++)
            {
                for (n=1; n<=N; n++)
                {
                    cblas_daxpy(n,-sc*n,&X[c*R],0,&X[No+c*R],1);             //beg edge samps
                    cblas_daxpy(R-n,-sc*n,&X[c*R],1,&X[No+c*R+n],1);         //past samps
                    cblas_daxpy(R-n,sc*n,&X[c*R+n],1,&X[No+c*R],1);          //future samps
                    cblas_daxpy(n,sc*n,&X[c*R+R-1],0,&X[No+c*R+R-n],1);      //end edge samps
                }
                for (n=1; n<=N; n++)
                {
                    cblas_daxpy(n,-sc*n,&X[No+c*R],0,&X[2*No+c*R],1);        //beg edge samps
                    cblas_daxpy(R-n,-sc*n,&X[No+c*R],1,&X[2*No+c*R+n],1);    //past samps
                    cblas_daxpy(R-n,sc*n,&X[No+c*R+n],1,&X[2*No+c*R],1);     //future samps
                    cblas_daxpy(n,sc*n,&X[No+c*R+R-1],0,&X[2*No+c*R+R-n],1); //end edge samps
                }
            }
        }
        else
        {
            for (c=0; c<C/3; c++)
            {
                cblas_dcopy(R,&z,0,&X[C/3+c],C);
                for (n=1; n<=N; n++)
                {
                    cblas_daxpy(n,-sc*n,&X[c],0,&X[C/3+c],C);                      //beg edge samps
                    cblas_daxpy(R-n,-sc*n,&X[c],C,&X[C/3+c+n*C],C);                //past samps
                    cblas_daxpy(R-n,sc*n,&X[c+n*C],C,&X[C/3+c],C);                 //future samps
                    cblas_daxpy(n,sc*n,&X[c+C*(R-1)],0,&X[C/3+c+C*(R-n)],C);       //end edge samps
                }
                cblas_dcopy(R,&z,0,&X[2*C/3+c],C);
                for (n=1; n<=N; n++)
                {
                    cblas_daxpy(n,-sc*n,&X[C/3+c],0,&X[2*C/3+c],C);                //beg edge samps
                    cblas_daxpy(R-n,-sc*n,&X[C/3+c],C,&X[2*C/3+c+n*C],C);          //past samps
                    cblas_daxpy(R-n,sc*n,&X[C/3+c+n*C],C,&X[2*C/3+c],C);           //future samps
                    cblas_daxpy(n,sc*n,&X[C/3+c+C*(R-1)],0,&X[2*C/3+c+C*(R-n)],C); //end edge samps
                }
            }
        }
    }
    else if (dim==1)
    {
        if (iscolmajor)
        {
            for (r=0; r<R/3; r++)
            {
                cblas_dcopy(C,&z,0,&X[R/3+r],R);
                for (n=1; n<=N; n++)
                {
                    cblas_daxpy(n,-sc*n,&X[r],0,&X[R/3+r],R);                      //beg edge samps
                    cblas_daxpy(C-n,-sc*n,&X[r],R,&X[R/3+r+n*R],R);                //past samps
                    cblas_daxpy(C-n,sc*n,&X[r+n*R],R,&X[R/3+r],R);                 //future samps
                    cblas_daxpy(n,sc*n,&X[r+R*(C-1)],0,&X[R/3+r+R*(C-n)],R);       //end edge samps
                }
                cblas_dcopy(C,&z,0,&X[2*R/3+r],R);
                for (n=1; n<=N; n++)
                {
                    cblas_daxpy(n,-sc*n,&X[R/3+r],0,&X[2*R/3+r],R);                //beg edge samps
                    cblas_daxpy(C-n,-sc*n,&X[R/3+r],R,&X[2*R/3+r+n*R],R);          //past samps
                    cblas_daxpy(C-n,sc*n,&X[R/3+r+n*R],R,&X[2*R/3+r],R);           //future samps
                    cblas_daxpy(n,sc*n,&X[R/3+r+R*(C-1)],0,&X[2*R/3+r+R*(C-n)],R); //end edge samps
                }
            }
        }
        else
        {
            cblas_dcopy(2*No,&z,0,&X[No],1);
            for (r=0; r<R/3; r++)
            {
                for (n=1; n<=N; n++)
                {
                    cblas_daxpy(n,-sc*n,&X[r*C],0,&X[No+r*C],1);             //beg edge samps
                    cblas_daxpy(C-n,-sc*n,&X[r*C],1,&X[No+r*C+n],1);         //past samps
                    cblas_daxpy(C-n,sc*n,&X[r*C+n],1,&X[No+r*C],1);          //future samps
                    cblas_daxpy(n,sc*n,&X[r*C+C-1],0,&X[No+r*C+C-n],1);      //end edge samps
                }
                for (n=1; n<=N; n++)
                {
                    cblas_daxpy(n,-sc*n,&X[No+r*C],0,&X[2*No+r*C],1);        //beg edge samps
                    cblas_daxpy(C-n,-sc*n,&X[No+r*C],1,&X[2*No+r*C+n],1);    //past samps
                    cblas_daxpy(C-n,sc*n,&X[No+r*C+n],1,&X[2*No+r*C],1);     //future samps
                    cblas_daxpy(n,sc*n,&X[No+r*C+C-1],0,&X[2*No+r*C+C-n],1); //end edge samps
                }
            }
        }
    }
    else
    {
        fprintf(stderr,"error in add_delta_deltas_d: dim must be 0 or 1.\n"); return 1;
    }

    return 0;
}

#ifdef __cplusplus
}
}
#endif


/* specfunc/mathieu_coeff.c
 *
 * Copyright (C) 2002 Lowell Johnson
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

/* Author:  L. Johnson */

#include <config.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_sf_mathieu.h>

/*****************************************************************************
 * backward_recurse
 *
 * Purpose:
 ****************************************************************************/
static void backward_recurse_c(double aa, double qq, double xx, double *ff,
                               double *gx, int even_odd, int ni)
{
  int ii, nn;
  double g1;

  g1 = *gx;
  ff[ni] = xx;

  if (even_odd == 0)
  {
      for (ii=0; ii<ni; ii++)
      {
          nn = GSL_SF_MATHIEU_COEFF - ii - 1;
          ff[ni-ii-1] = -1.0/((4*nn*nn - aa)/qq + ff[ni-ii]);
      }
      if (ni == GSL_SF_MATHIEU_COEFF - 1)
          ff[0] *= 2.0;
  }
  else
  {
      for (ii=0; ii<ni; ii++)
      {
          nn = GSL_SF_MATHIEU_COEFF - ii - 1;
          ff[ni-ii-1] = -1.0/(((2*nn + 1)*(2*nn + 1) - aa)/qq + ff[ni-ii]);
      }
  }

  *gx = ff[0] - g1;
}

static void backward_recurse_s(double aa, double qq, double xx, double *ff,
                               double *gx, int even_odd, int ni)
{
  int ii, nn;
  double g1;

  g1 = *gx;
  ff[ni] = xx;

  if (even_odd == 0)
  {
      for (ii=0; ii<ni; ii++)
      {
          nn = GSL_SF_MATHIEU_COEFF - ii - 1;
          ff[ni-ii-1] = -1.0/((4*(nn + 1)*(nn + 1) - aa)/qq + ff[ni-ii]);
      }
  }
  else
  {
      for (ii=0; ii<ni; ii++)
      {
          nn = GSL_SF_MATHIEU_COEFF - ii - 1;
          ff[ni-ii-1] = -1.0/(((2*nn + 1)*(2*nn + 1) - aa)/qq + ff[ni-ii]);
      }
  }

  *gx = ff[0] - g1;
}

int gsl_sf_mathieu_a_coeff(int order, double qq, double aa, double coeff[])
{
  int ni, nn, ii, even_odd;
  double eps, g1, g2, x1, x2, e1, e2, de, xh, sum, ratio,
         ff[GSL_SF_MATHIEU_COEFF];

  eps = 1e-14;
  coeff[0] = 1.0;

  even_odd = 0;
  if (order % 2 != 0)
      even_odd = 1;

  /* If the coefficient array is not large enough to hold all necessary
     coefficients, error out. */
  if (order > GSL_SF_MATHIEU_COEFF)
      return GSL_FAILURE;

  /* Handle the trivial case where q = 0. */
  if (qq == 0.0)
  {
      for (ii=0; ii<GSL_SF_MATHIEU_COEFF; ii++)
          coeff[ii] = 0.0;

      coeff[order/2] = 1.0;

      return GSL_SUCCESS;
  }

  if (order < 5)
  {
      nn = 0;
      sum = 0.0;
      if (even_odd == 0)
          ratio = aa/qq;
      else
          ratio = (aa - 1 - qq)/qq;
  }
  else
  {
      if (even_odd == 0)
      {
          coeff[1] = aa/qq;
          coeff[2] = (aa - 4)/qq*coeff[1] - 2;
          sum = coeff[0] + coeff[1] + coeff[2];
          for (ii=3; ii<order/2+1; ii++)
          {
              coeff[ii] = (aa - 4*(ii - 1)*(ii - 1))/qq*coeff[ii-1] -
                                                                  coeff[ii-2];
              sum += coeff[ii];
          }
      }
      else
      {
          coeff[1] = (aa - 1)/qq - 1;
          sum = coeff[0] + coeff[1];
          for (ii=2; ii<order/2+1; ii++)
          {
              coeff[ii] = (aa - (2*ii - 1)*(2*ii - 1))/qq*coeff[ii-1] -
                                                                  coeff[ii-2];
              sum += coeff[ii];
          }
      }

      nn = ii - 1;

      ratio = coeff[nn]/coeff[nn-1];
  }

  ni = GSL_SF_MATHIEU_COEFF - nn - 1;

  /* Compute first two points to start root-finding. */
  if (even_odd == 0)
      x1 = -qq/(4.0*GSL_SF_MATHIEU_COEFF*GSL_SF_MATHIEU_COEFF);
  else
      x1 = -qq/((2.0*GSL_SF_MATHIEU_COEFF + 1.0)*(2.0*GSL_SF_MATHIEU_COEFF + 1.0));
  g1 = ratio;
  backward_recurse_c(aa, qq, x1, ff, &g1, even_odd, ni);
  x2 = g1;
  g2 = ratio;
  backward_recurse_c(aa, qq, x2, ff, &g2, even_odd, ni);

  /* Find the root. */
  while (1)
  {
      /* Compute the relative error. */
      e1 = g1 - x1;
      e2 = g2 - x2;
      de = e1 - e2;

      /* If we are close enough to the root, break... */
      if (fabs(de) < eps)
          break;

      /* Otherwise, determine the next guess and try again. */
      xh = (e1*x2 - e2*x1)/de;
      x1 = x2;
      g1 = g2;
      x2 = xh;
      g2 = ratio;
      backward_recurse_c(aa, qq, x2, ff, &g2, even_odd, ni);
  }

  /* Compute the rest of the coefficients. */
  sum += coeff[nn];
  for (ii=nn+1; ii<GSL_SF_MATHIEU_COEFF; ii++)
  {
      coeff[ii] = ff[ii-nn-1]*coeff[ii-1];
      sum += coeff[ii];

      /* If the coefficients are getting really small, set the remainder
         to zero. */
      if (fabs(coeff[ii]) < 1e-20)
      {
          for (; ii<GSL_SF_MATHIEU_COEFF;)
              coeff[ii++] = 0.0;
      }
  }

  /* Normalize the coefficients. */
  for (ii=0; ii<GSL_SF_MATHIEU_COEFF; ii++)
      coeff[ii] /= sum;

  return GSL_SUCCESS;
}

int gsl_sf_mathieu_b_coeff(int order, double qq, double aa, double coeff[])
{
  int ni, nn, ii, even_odd;
  double eps, g1, g2, x1, x2, e1, e2, de, xh, sum, ratio,
         ff[GSL_SF_MATHIEU_COEFF];

  eps = 1e-10;
  coeff[0] = 1.0;

  even_odd = 0;
  if (order % 2 != 0)
      even_odd = 1;

  /* If the coefficient array is not large enough to hold all necessary
     coefficients, error out. */
  if (order > GSL_SF_MATHIEU_COEFF)
      return GSL_FAILURE;

  /* Handle the trivial case where q = 0. */
  if (qq == 0.0)
  {
      for (ii=0; ii<GSL_SF_MATHIEU_COEFF; ii++)
          coeff[ii] = 0.0;

      coeff[(order-1)/2] = 1.0;

      return GSL_SUCCESS;
  }

  if (order < 5)
  {
      nn = 0;
      sum = 0.0;
      if (even_odd == 0)
          ratio = (aa - 4)/qq;
      else
          ratio = (aa - 1 - qq)/qq;
  }
  else
  {
      if (even_odd == 0)
      {
          coeff[1] = (aa - 4)/qq;
          sum = 2*coeff[0] + 4*coeff[1];
          for (ii=2; ii<order/2; ii++)
          {
              coeff[ii] = (aa - 4*ii*ii)/qq*coeff[ii-1] - coeff[ii-2];
              sum += 2*(ii + 1)*coeff[ii];
          }
      }
      else
      {
          coeff[1] = (aa - 1)/qq + 1;
          sum = coeff[0] + 3*coeff[1];
          for (ii=2; ii<order/2+1; ii++)
          {
              coeff[ii] = (aa - (2*ii - 1)*(2*ii - 1))/qq*coeff[ii-1] -
                                                                  coeff[ii-2];
              sum += (2*(ii + 1) - 1)*coeff[ii];
          }
      }

      nn = ii - 1;

      ratio = coeff[nn]/coeff[nn-1];
  }

  ni = GSL_SF_MATHIEU_COEFF - nn - 1;

  /* Compute first two points to start root-finding. */
  if (even_odd == 0)
      x1 = -qq/(4.0*(GSL_SF_MATHIEU_COEFF + 1.0)*(GSL_SF_MATHIEU_COEFF + 1.0));
  else
      x1 = -qq/((2.0*GSL_SF_MATHIEU_COEFF + 1.0)*(2.0*GSL_SF_MATHIEU_COEFF + 1.0));
  g1 = ratio;
  backward_recurse_s(aa, qq, x1, ff, &g1, even_odd, ni);
  x2 = g1;
  g2 = ratio;
  backward_recurse_s(aa, qq, x2, ff, &g2, even_odd, ni);

  /* Find the root. */
  while (1)
  {
      /* Compute the relative error. */
      e1 = g1 - x1;
      e2 = g2 - x2;
      de = e1 - e2;

      /* If we are close enough to the root, break... */
      if (fabs(de) < eps)
          break;

      /* Otherwise, determine the next guess and try again. */
      xh = (e1*x2 - e2*x1)/de;
      x1 = x2;
      g1 = g2;
      x2 = xh;
      g2 = ratio;
      backward_recurse_s(aa, qq, x2, ff, &g2, even_odd, ni);
  }

  /* Compute the rest of the coefficients. */
  sum += 2*(nn + 1)*coeff[nn];
  for (ii=nn+1; ii<GSL_SF_MATHIEU_COEFF; ii++)
  {
      coeff[ii] = ff[ii-nn-1]*coeff[ii-1];
      sum += 2*(ii + 1)*coeff[ii];

      /* If the coefficients are getting really small, set the remainder
         to zero. */
      if (fabs(coeff[ii]) < 1e-20)
      {
          for (; ii<GSL_SF_MATHIEU_COEFF;)
              coeff[ii++] = 0.0;
      }
  }

  /* Normalize the coefficients. */
  for (ii=0; ii<GSL_SF_MATHIEU_COEFF; ii++)
      coeff[ii] /= sum;

  return GSL_SUCCESS;
}


//
// Created by pedram pakseresht on 2/18/21.
//

#include <petsc.h>

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

    Vec a,b,c;
    DM packer;
    PetscErrorCode ierr;
    PetscInt i[4] = {0,1,2,3};
    PetscReal v[4] = {1.5,2.5,3.5,4.5};
    PetscReal w[4] = {5.5,6.5,7.5,8.5};

    // initialize Petsc ...
    ierr = PetscInitialize(&argc, &argv, NULL, "");CHKERRQ(ierr);

    ierr= VecCreate(PETSC_COMM_WORLD, &a); CHKERRQ(ierr);
    ierr= VecCreate(PETSC_COMM_WORLD, &b); CHKERRQ(ierr);

    ierr= VecSetSizes(a,PETSC_DECIDE,4); CHKERRQ(ierr);
    ierr = VecSetFromOptions(a); CHKERRQ(ierr);
    ierr= VecSetValues(a,4,i,v,INSERT_VALUES);CHKERRQ(ierr);
    ierr= VecAssemblyBegin(a);CHKERRQ(ierr);
    ierr = VecAssemblyEnd(a);CHKERRQ(ierr);
    ierr = VecView(a,PETSC_VIEWER_STDOUT_WORLD);CHKERRQ(ierr);

    ierr= VecSetSizes(b,PETSC_DECIDE,4); CHKERRQ(ierr);
    ierr = VecSetFromOptions(b); CHKERRQ(ierr);
    ierr= VecSetValues(b,4,i,w,INSERT_VALUES);CHKERRQ(ierr);
    ierr= VecAssemblyBegin(b);CHKERRQ(ierr);
    ierr = VecAssemblyEnd(b);CHKERRQ(ierr);
    ierr = VecView(b,PETSC_VIEWER_STDOUT_WORLD);CHKERRQ(ierr);

    ierr = PetscFinalize();exit(ierr);

}


/*
* -----------------------------------------------------------------
*  ODE Solver Library --- ode_lib.c
*  Version: 1.6180
*  Date: Jan 4, 2011
* -----------------------------------------------------------------
*  Programmer: Americo Barbosa da Cunha Junior
*              americo.cunhajr@gmail.com
* -----------------------------------------------------------------
*  Copyright (c) 2010 by Americo Barbosa da Cunha Junior
*
*  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.
*
*  A copy of the GNU General Public License is available in
*  LICENSE.txt or http://www.gnu.org/licenses/.
* -----------------------------------------------------------------
*  This is the implementation file of a library
*  with ODE solution tools.
* -----------------------------------------------------------------
*/

#include <stdlib.h>
#include <math.h>
#include <float.h>
#include <nvector/nvector_serial.h>
#include <cvode/cvode_dense.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>

#include "../include/ode_lib.h"

/*
*------------------------------------------------------------
*   uround
*
*   This function computes the machine unit roundoff
*   defined as the smallest u such that 1.0 + u > 1.0
*
*   Output:
*   uround - machine unit roundoff
*
*   last update: May 20, 2009
*------------------------------------------------------------
*/

double uround(void)
{
    double u = 1.0;
    double comp = 0.0;

    while(comp != 1.0)
    {
	u *= 0.5;
	comp = 1.0 + u;
    }

    return 2.0*u;
}
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
*   wnorm
*
*   This function computes v1 and v2 vector weight norm
*   defined as
*   wnorm = sqrt( (1/n)*(sum (v1[i]*v2[i])^2) )
*
*   Input:
*   n     - vectors dimension
*   v     - vector 1
*   v     - vector 2
*
*   Output:
*   wnorm - weight norm
*
*   last update: May 20, 2009
*------------------------------------------------------------
*/

double wnorm(int n,double *v1,double *v2)
{
    int i;
    double wnorm = 0.0;

    for ( i = 0; i < n; i++ )
	wnorm += v1[i]*v1[i]*v2[i]*v2[i];

    return sqrt( wnorm / (double)n );
}
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
*   ewset
*
*   This function creates a error weight vector
*   defined as ewv[i] = rtol*abs(v[i]) + atol
*
*   Input:
*   n    - vector dimension
*   v    - vector
*   atol - absolute tolerance
*   rtol - relative tolerance
*
*   Output:
*   ewv  - error weight vector
*
*   last update: May 22, 2009
*------------------------------------------------------------
*/

void ewtset(int n,double *v,double atol,double rtol,double *ewt)
{
    int i;

    for ( i = 0; i < n; i++ )
        ewt[i] =  rtol*fabs(v[i]) + atol;

    return;
}
/*------------------------------------------------------------*/

/*------------------------------------------------------------
*   jacobian
*
*   This function computes the jacobian matrix of ydot = f(y,t)
*   defiend as  Jij = dfi/dyj
*
*   Input:
*   n      - vector dimension
*   f_data - pointer to external data
*   Fy     - reaction mapping
*   y      - composition vector
*   t      - time step
*   atol   - absolute tolerance
*   rtol   - relative tolerance
*
*   Output:
*   J       - jacobian matrix
*   success or error
*
*   last update: Oct 7, 2009
------------------------------------------------------------*/

int jacobian(CVRhsFn f,void *f_data,gsl_vector *Fy,gsl_vector *y,
                    double t,double atol,double rtol,gsl_matrix *J)
{
    unsigned int i, j, N1, N2;
    double roundoff, min_inc_mult;
    double fnorm, minInc, inc, inc_inv, yjsaved, srur;
    double *yd_data  = NULL;
    double *Fyd_data = NULL;
    N_Vector yd      = NULL;
    N_Vector Fyd     = NULL;
    gsl_vector *ewt  = NULL;

    min_inc_mult = 1.0e3;

    /* vectors dimensions */
    N1 = y->size;
    N2 = Fy->size;

    /* error weight vector */
    ewt = gsl_vector_calloc(N1);

    /* computing the unit roundoff */
    roundoff = uround();

    /* memory allocation and startup of yd and Fyd */
    yd  = N_VNew_Serial(N1);
    Fyd = N_VNew_Serial(N2);
    if ( yd == NULL || Fyd == NULL )
        return GSL_ENOMEM;

    /* obtaining yd and Fyd compoments  */
    yd_data  = NV_DATA_S(yd);
    Fyd_data = NV_DATA_S(Fyd);

    /* setting yd equal y vector */
    for ( i = 0; i < N1; i++ )
        yd_data[i] = y->data[i];

    /* setting Fyd equal the null vector */
    for ( i = 0; i < N2; i++ )
        Fyd_data[i] = 0.0;

    /* defing error weight vector */
    ewtset(N1,y->data,atol,rtol,ewt->data);

    /* computing weight norm */
    fnorm = wnorm(N2,Fy->data,ewt->data);

    /* square root of the machine roundoff */
    srur = sqrt(roundoff);

    /* computing disturbance parameter */
    minInc = (fnorm != 0.0) ?
	    (min_inc_mult*fabs(t)*roundoff*((double)N1)*fnorm) : 1.0;

    for ( j = 0; j < N1; j++ )
    {
        /* saving y[j] value */
        yjsaved = yd_data[j];

        /* disturbance */
        inc = GSL_MAX(srur*fabs(yjsaved),minInc/ewt->data[j]);

    	/* disturbing y */
        yd_data[j] += inc;

        /* computing Fy disturbed */
        f(t,yd,Fyd,f_data);

	/* restoring yd[j] original value */
        yd_data[j] = yjsaved;

	/* computing the step */
        inc_inv = 1.0/inc;

        /* computing jacobian matrix column j */
    	for ( i = 0; i < N2; i++ )
            J->data[i*J->tda+j] = inc_inv*(Fyd_data[i] - Fy->data[i]);
    }

    /* releasing allocated memory */
    N_VDestroy_Serial(yd);
    N_VDestroy_Serial(Fyd);
    gsl_vector_free(ewt);
    yd       = NULL;
    Fyd      = NULL;
    yd_data  = NULL;
    Fyd_data = NULL;
    ewt      = NULL;

    return GSL_SUCCESS;
}
/*------------------------------------------------------------*/

/*------------------------------------------------------------
*   gradient
*
*   This function computes the gradient matrix of R(phi)
*   defiend as: Aij = d R_i/d phi_j
*
*   Input:
*   f       - right hand side function
*   f_data  - pointer to external data
*   t0      - initial time
*   delta_t - time step
*   atol    - absolute tolerance
*   rtol    - relative tolerance
*   phi     - composition vector
*   Rphi    - reac tion mapping
*
*   Output:
*   A       - gradient matrix
*   success or error
*
*   last update: Feb 19, 2010
------------------------------------------------------------*/

int gradient(CVRhsFn f,void *f_data,void *cvode_mem,double t0,double delta_t,
    double atol,double rtol,gsl_vector *phi,gsl_vector *Rphi,gsl_matrix *A)
{
    unsigned int i, j, flag;
    double inc, inc_inv, phijsaved, srur;
    gsl_vector *Rphid = NULL;

    /* memory allocation for Rphid */
    Rphid = gsl_vector_calloc(Rphi->size);

    /* square root of 1.0e6 times the machine roundoff */
    srur = sqrt(1.0e6*DBL_EPSILON);

    for ( j = 0; j < phi->size; j++ )
    {
        /* saving phi_j value */
        phijsaved = phi->data[j];

        /* disturbance */
        inc = phijsaved*srur + srur;

    	/* disturbing phi_j */
        phi->data[j] += inc;

        /* computing R(phi) disturbed */
	flag = odesolver_reinit(f,f_data,t0,atol,rtol,phi,cvode_mem);
        if ( flag != GSL_SUCCESS )
            return flag;

	flag = odesolver(cvode_mem,delta_t,Rphid);
        if ( flag != GSL_SUCCESS )
            return flag;

	/* restoring phi_j original value */
        phi->data[j] = phijsaved;

	/* computing the step */
        inc_inv = 1.0/inc;

        /* computing gradient matrix column j */
    	for ( i = 0; i < Rphi->size; i++ )
            A->data[i*A->tda+j] = inc_inv*(Rphid->data[i] - Rphi->data[i]);
    }

    /* releasing allocated memory */
    gsl_vector_free(Rphid);
    Rphid = NULL;

    return GSL_SUCCESS;
}
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
*   linear_approx
*
*   This function computes the linear approximation
*   for a vector function F(x) near the point x0.
*
*   F(x) = F(x0) + DF(x0)*(x-x0) + O(|x-x0|^2)
*
*   Input:
*   x    - vector x
*   x0   - vector x0
*   Fx0  - function at x0
*   DFx0 - jacobian matrix at x0
*
*   Output:
*   Fx   - linear approximation for F(x)
*
*   last update: Jun 9, 2009
*------------------------------------------------------------
*/

void linear_approx(gsl_vector *x,gsl_vector *x0,
                gsl_vector *Fx0,gsl_matrix *DFx0,gsl_vector *Fx)
{
    gsl_vector *aux = NULL;

    /* memory allocation */
    aux = gsl_vector_alloc(x0->size);

    /* aux := x */
    gsl_vector_memcpy(aux,x);

    /* aux := x - x0 */
    gsl_vector_sub(aux,x0);

    /* Fx := F(x0) */
    gsl_vector_memcpy(Fx,Fx0);

    /* Fx := F(x0) + DF(x0)*(x-x0) */
    gsl_blas_dgemv(CblasNoTrans,1.0,DFx0,aux,1.0,Fx);

    /* releasing allocated memory */
    gsl_vector_free(aux);
    aux = NULL;

    return;
}
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
*   odesolver_init
*
*   This function initiates the ODE solver workspace.
*
*   Input:
*   f         - right hand side function
*   f_data    - right hand side function data
*   t0        - initial time
*   mxsteps   - max # of solver iterations
*   atol      - absolute tolerance
*   rtol      - relative tolerance
*   x         - initial condition
*   cvode_mem - ODE solver workspace
*
*   Output:
*   success or error
*
*   last update: Jan 4, 2011
*------------------------------------------------------------
*/

int odesolver_init(CVRhsFn f,void *f_data,double t0,int mxsteps,
		   double atol,double rtol,gsl_vector *x,void *cvode_mem)
{
    int maxnef  = 100;
    int flag    = CV_SUCCESS;
    N_Vector y0 = NULL;

    /* memory allocation and startup of y0 */
    y0 = N_VMake_Serial(x->size,x->data);
    if ( y0 == NULL )
	return GSL_ENOMEM;

    /* memory allocation for CVODE */
    flag = CVodeMalloc(cvode_mem,f,t0,y0,CV_SS,rtol,&atol);
    if( flag != CV_SUCCESS )
    	return GSL_ENOMEM;

    /* setting user data for right hand side function */
    flag = CVodeSetFdata(cvode_mem,f_data);
    if( flag != CV_SUCCESS )
        return flag;

    /* setting max # of steps to be taken by the solver */
    flag = CVodeSetMaxNumSteps(cvode_mem,mxsteps);
    if( flag != CV_SUCCESS )
        return flag;

    /* setting max # of error test failures permitted */
    flag = CVodeSetMaxErrTestFails(cvode_mem,maxnef);
    if( flag != CV_SUCCESS )
        return flag;

    /* setting dense linear solver */
    flag = CVDense(cvode_mem,x->size);
    if( flag != CV_SUCCESS )
        return flag;

    /* releasing allocated memory */
    N_VDestroy_Serial(y0);
    y0 = NULL;

    return GSL_SUCCESS;
}
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
*   odesolver_reinit
*
*   This function reinitiates the ODE solver workspace.
*
*   Input:
*   f         - right hand side function
*   f_data    - right hand side function data
*   t0        - initial time
*   atol      - absolute tolerance
*   rtol      - relative tolerance
*   x         - initial condition
*   cvode_mem - ODE solver workspace
*
*   Output:
*   success or error
*
*   last update: Oct 31, 2009
*------------------------------------------------------------
*/

int odesolver_reinit(CVRhsFn f,void *f_data,double t0,
		   double atol,double rtol,gsl_vector *x,void *cvode_mem)
{
    int flag    = CV_SUCCESS;
    N_Vector y0 = NULL;

    /* memory allocation and startup of y0 */
    y0 = N_VMake_Serial(x->size,x->data);
    if ( y0 == NULL )
	return GSL_ENOMEM;

    /* restarting CVODE workspace */
    flag = CVodeReInit(cvode_mem,f,t0,y0,CV_SS,rtol,&atol);
    if( flag != CV_SUCCESS )
    	return flag;

    /* setting user data for right hand side function */
    flag = CVodeSetFdata(cvode_mem,f_data);
    if( flag != CV_SUCCESS )
        return flag;

    /* releasing allocated memory */
    N_VDestroy_Serial(y0);
    y0 = NULL;

    return GSL_SUCCESS;
}
/*------------------------------------------------------------*/

/*
*------------------------------------------------------------
*   odesolver
*
*   This function performs the direct integration of the
*   governing equations.
*
*   Input:
*   cvode_mem - ODE solver workspace
*   tf   - final time
*   Fx   - solution vector
*
*   Output:
*   success or error
*
*   last update: Oct 31, 2009
*------------------------------------------------------------
*/

int odesolver(void *cvode_mem,double tf,gsl_vector *Fx)
{
    double t;
    int flag   = CV_SUCCESS;
    N_Vector y = NULL;

    /* memory allocation and startup of y */
    y = N_VMake_Serial(Fx->size,Fx->data);
    if ( y == NULL )
	return GSL_ENOMEM;

    /* calling CVode solver */
    flag = CVode(cvode_mem,tf,y,&t,CV_NORMAL);
    if( flag != CV_SUCCESS )
        return flag;

    /* releasing allocated memory */
    N_VDestroy_Serial(y);
    y = NULL;

    return GSL_SUCCESS;
}
/*------------------------------------------------------------*/


/* eigen/gensymm.c
 *
 * Copyright (C) 2007 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 <stdlib.h>

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

/*
 * This module computes the eigenvalues of a real generalized
 * symmetric-definite eigensystem A x = \lambda B x, where A and
 * B are symmetric, and B is positive-definite.
 */

/*
gsl_eigen_gensymm_alloc()

Allocate a workspace for solving the generalized symmetric-definite
eigenvalue problem. The size of this workspace is O(2n).

Inputs: n - size of matrices

Return: pointer to workspace
*/

gsl_eigen_gensymm_workspace *
gsl_eigen_gensymm_alloc(const size_t n)
{
  gsl_eigen_gensymm_workspace *w;

  if (n == 0)
    {
      GSL_ERROR_NULL ("matrix dimension must be positive integer",
                      GSL_EINVAL);
    }

  w = (gsl_eigen_gensymm_workspace *) calloc (1, sizeof (gsl_eigen_gensymm_workspace));

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

  w->size = n;

  w->symm_workspace_p = gsl_eigen_symm_alloc(n);
  if (!w->symm_workspace_p)
    {
      gsl_eigen_gensymm_free(w);
      GSL_ERROR_NULL("failed to allocate space for symm workspace", GSL_ENOMEM);
    }

  return (w);
} /* gsl_eigen_gensymm_alloc() */

/*
gsl_eigen_gensymm_free()
  Free workspace w
*/

void
gsl_eigen_gensymm_free (gsl_eigen_gensymm_workspace * w)
{
  RETURN_IF_NULL (w);

  if (w->symm_workspace_p)
    gsl_eigen_symm_free(w->symm_workspace_p);

  free(w);
} /* gsl_eigen_gensymm_free() */

/*
gsl_eigen_gensymm()

Solve the generalized symmetric-definite eigenvalue problem

A x = \lambda B x

for the eigenvalues \lambda.

Inputs: A    - real symmetric matrix
        B    - real symmetric and positive definite matrix
        eval - where to store eigenvalues
        w    - workspace

Return: success or error
*/

int
gsl_eigen_gensymm (gsl_matrix * A, gsl_matrix * B, gsl_vector * eval,
                   gsl_eigen_gensymm_workspace * w)
{
  const size_t N = A->size1;

  /* check matrix and vector sizes */

  if (N != A->size2)
    {
      GSL_ERROR ("matrix must be square to compute eigenvalues", GSL_ENOTSQR);
    }
  else if ((N != B->size1) || (N != B->size2))
    {
      GSL_ERROR ("B matrix dimensions must match A", GSL_EBADLEN);
    }
  else if (eval->size != N)
    {
      GSL_ERROR ("eigenvalue vector must match matrix size", GSL_EBADLEN);
    }
  else if (w->size != N)
    {
      GSL_ERROR ("matrix size does not match workspace", GSL_EBADLEN);
    }
  else
    {
      int s;

      /* compute Cholesky factorization of B */
      s = gsl_linalg_cholesky_decomp(B);
      if (s != GSL_SUCCESS)
        return s; /* B is not positive definite */

      /* transform to standard symmetric eigenvalue problem */
      gsl_eigen_gensymm_standardize(A, B);

      s = gsl_eigen_symm(A, eval, w->symm_workspace_p);

      return s;
    }
} /* gsl_eigen_gensymm() */

/*
gsl_eigen_gensymm_standardize()
  Reduce the generalized symmetric-definite eigenproblem to
the standard symmetric eigenproblem by computing

C = L^{-1} A L^{-t}

where L L^t is the Cholesky decomposition of B

Inputs: A - (input/output) real symmetric matrix
        B - real symmetric, positive definite matrix in Cholesky form

Return: success

Notes: A is overwritten by L^{-1} A L^{-t}
*/

int
gsl_eigen_gensymm_standardize(gsl_matrix *A, const gsl_matrix *B)
{
  const size_t N = A->size1;
  size_t i;
  double a, b, c;

  for (i = 0; i < N; ++i)
    {
      /* update lower triangle of A(i:n, i:n) */

      a = gsl_matrix_get(A, i, i);
      b = gsl_matrix_get(B, i, i);
      a /= b * b;
      gsl_matrix_set(A, i, i, a);

      if (i < N - 1)
        {
          gsl_vector_view ai = gsl_matrix_subcolumn(A, i, i + 1, N - i - 1);
          gsl_matrix_view ma =
            gsl_matrix_submatrix(A, i + 1, i + 1, N - i - 1, N - i - 1);
          gsl_vector_const_view bi =
            gsl_matrix_const_subcolumn(B, i, i + 1, N - i - 1);
          gsl_matrix_const_view mb =
            gsl_matrix_const_submatrix(B, i + 1, i + 1, N - i - 1, N - i - 1);

          gsl_blas_dscal(1.0 / b, &ai.vector);

          c = -0.5 * a;
          gsl_blas_daxpy(c, &bi.vector, &ai.vector);

          gsl_blas_dsyr2(CblasLower, -1.0, &ai.vector, &bi.vector, &ma.matrix);

          gsl_blas_daxpy(c, &bi.vector, &ai.vector);

          gsl_blas_dtrsv(CblasLower,
                         CblasNoTrans,
                         CblasNonUnit,
                         &mb.matrix,
                         &ai.vector);
        }
    }

  return GSL_SUCCESS;
} /* gsl_eigen_gensymm_standardize() */


#include <stdio.h>
#include <cblas.h>

double A[] = { 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0 };

double x[] = { 1.0, -1.0, 2.0 };
double y[] = { -1.0, 0.0, 2.0 };

int main() {
  int i;
  cblas_dgemv(CblasRowMajor, CblasNoTrans, 3, 3, 2.5, A, 3, x, 1, 1.2, y, 1);
  for (i = 0; i < 3; ++i) { printf("%6.1f\n", y[i]); }

  return 0;
}


/* gsl_histogram_maxval.c
 * Copyright (C) 2000  Simone Piccardi
 *
 * This library 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 library; if not, write to the Free Software Foundation, Inc.,
 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */
/***************************************************************
 *
 * File gsl_histogram_maxval.c:
 * Routine to find maximum and minumum content of a hisogram.
 * Need GSL library and header.
 * Contains the routines:
 * gsl_histogram_max_val find max content values
 * gsl_histogram_min_val find min content values
 * gsl_histogram_bin_max find coordinates of max contents bin
 * gsl_histogram_bin_min find coordinates of min contents bin
 *
 * Author: S. Piccardi
 * Jan. 2000
 *
 ***************************************************************/
#include <config.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_histogram.h>

double
gsl_histogram_max_val (const gsl_histogram * h)
{
  const size_t n = h->n;
  size_t i;
  double max = h->bin[0];
  for (i = 0; i < n; i++)
    {
      if (h->bin[i] > max)
        {
          max = h->bin[i];
        }
    }
  return max;
}

size_t
gsl_histogram_max_bin (const gsl_histogram * h)
{
  size_t i;
  size_t imax = 0;
  double max = h->bin[0];
  for (i = 0; i < h->n; i++)
    {
      if (h->bin[i] > max)
        {
          max = h->bin[i];
          imax = i;
        }
    }
  return imax;
}

double
gsl_histogram_min_val (const gsl_histogram * h)
{
  size_t i;
  double min = h->bin[0];
  for (i = 0; i < h->n; i++)
    {
      if (h->bin[i] < min)
        {
          min = h->bin[i];
        }
    }
  return min;
}

size_t
gsl_histogram_min_bin (const gsl_histogram * h)
{
  size_t i;
  size_t imin = 0;
  double min = h->bin[0];
  for (i = 0; i < h->n; i++)
    {
      if (h->bin[i] < min)
        {
          min = h->bin[i];
          imin = i;
        }
    }
  return imin;
}


/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Set of C routines to use FFTW1.3 library
* 2D- FFT's for any size
*
*
* JLP
* Version 12/12/2006 (from my BORLAND version)
*--------------------------------------------------------*/
#include <stdio.h>
#include <math.h>
#include "jlp_fftw.h"
#include "jlp_num_rename.h"

/* JLP2001: To be compatible with "matlab"
* I should not normalize by sqrt(nx*ny): */
/*
#define NORMALIZE_SQRT
*/

/* Content of jlp_fftw.h:
#include <fftw.h>

int FFTW_1D_Y_FLT(float *re, float *im, INT4 *nx, INT4 *ny, INT4 *idim,
                   INT4 *direct);
int fftw_1D_Y_float(float *re, float *im, int nx, int ny, int direct);
int FFTW_1D_Y_DBLE(double *re, double *im, INT4 *nx, INT4 *ny, INT4 *idim,
             INT4 *direct);
int fftw_1D_Y_double(double *re, double *im, int nx, int ny, int direct);
int FFTW_2D_DBLE(double *re, double *im, int *nx, int *ny, int *direct);
int fftw_2D_double(double *re, double *im, int nx, int ny, int direct);
int FFTW_2D_FLT(float *re, float *im, INT4 *nx, INT4 *ny, INT4 *idim,
                  INT4 *kod);
int fftw_2D_float(float *re, float *im, int nx, int ny, int direct);
int fftw_setup(int nx, int ny);
int FFTW_SETUP(int *nx, int *ny);
int fftw_fast(FFTW_COMPLEX *image, int nx, int ny, int direct);
int fftw_shutdown();
*/

/* Static variables: */
static fftwnd_plan plan_fwd, plan_bkwd;

/*
#define MAIN_TEST
*/

#ifdef MAIN_TEST
main()
{
register int i;
int nx = 9, ny = 8;
double re[128], im[128];
FFTW_COMPLEX* image;
char s[80];

for(i = 0; i < nx * ny; i++)
  {
  re[i] = i;
  im[i] = -i;
  }

fftw_2D_double(re, im, nx, ny, 1);
fftw_2D_double(re, im, nx, ny, -1);

for(i = 0; i < nx * ny; i++)
  {
  printf(" i=%d re=%f im=%f \n", i, re[i], im[i]);
  }

printf(" Now going to fast fft...");
gets(s);

nx = 388; ny = 128;
image = (FFTW_COMPLEX *) malloc(nx * ny * sizeof(FFTW_COMPLEX));
for(i = 0; i < nx * ny; i++)
  {
  c_re(image[i]) = i;
  c_im(image[i]) = -i;
  }
  printf("OK, I go on, with nx=%d ny=%d",nx,ny);

fftw_setup(nx, ny);
fftw_fast(image, nx, ny, 1);
fftw_fast(image, nx, ny, -1);
fftw_shutdown();
for(i = 0; i < 200; i++)
  {
  printf(" i=%d re=%f im=%f \n", i, c_re(image[i]), c_im(image[i]));
  }

gets(s);
free(image);
}
#endif
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Interface with Fortran routines (with arrays of first dimension = idim)
* kod=1, or 2 direct
* kod=-1, or -2 inverse
* kod=-2, or 2: power spectrum, and phase
*--------------------------------------------------------------*/
int FFTW_2D_FLT(float *re, float *im, INT4 *nx, INT4 *ny, INT4 *idim,
                  INT4 *kod)
{
int nx1, ny1, direct, status;
if(*idim != *nx)
 {printf("FFTW_2D/Fatal error idim=%d while nx=%d \n", *idim, *nx);
  exit(-1);
 }
if(*kod != 1 && *kod != -1)
 {printf("FFTW_2D/Fatal error kod=%d (option not yet implemented)\n", *kod);
  exit(-1);
 }
nx1 = *nx; ny1 = *ny; direct = *kod;
status = fftw_2D_float(re, im, nx1, ny1, direct);
return(status);
}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Interface with Fortran routines
* kod=1, or 2 direct
* kod=-1, or -2 inverse
* kod=-2, or 2: power spectrum, and phase
* --------------------------------------------------------------*/
int FFTW_1D_Y_DBLE(double *re, double *im, INT4 *nx, INT4 *ny,
                   INT4 *idim, INT4 *kod)
{
int nx1, ny1, direct, status;
if(*idim != *nx)
 {printf("FFTW_1D_Y/Fatal error idim=%d while nx=%d \n", *idim, *nx);
  exit(-1);
 }
if(*kod != 1 && *kod != -1)
 {printf("FFTW_1D_Y/Fatal error kod=%d (option not yet implemented)\n", *kod);
  exit(-1);
 }
nx1 = *nx; ny1 = *ny; direct = *kod;
printf("FFTW_1D_Y/ nx1=%d ny1=%d direct=%d\n",nx1,ny1,direct);
status = fftw_1D_Y_double(re, im, nx1, ny1, direct);
return(status);
}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* Interface with Fortran routines
* kod=1, or 2 direct
* kod=-1, or -2 inverse
* kod=-2, or 2: power spectrum, and phase
* --------------------------------------------------------------*/
int FFTW_1D_Y_FLT(float *re, float *im, INT4 *nx, INT4 *ny,
                   INT4 *idim, INT4 *kod)
{
int nx1, ny1, direct, status;

#ifndef TOTO_
double sum;
register int i;
#endif

if(*idim != *nx)
 {printf("FFTW_1D_Y_FLT/Fatal error idim=%d while nx=%d \n", *idim, *nx);
  exit(-1);
 }
if(*kod != 1 && *kod != -1)
 {printf("FFTW_1D_Y_FLT/Fatal error kod=%d (option not yet implemented)\n", *kod);
  exit(-1);
 }
nx1 = *nx; ny1 = *ny; direct = *kod;
#ifdef DEBUG
printf("FFTW_1D_Y_FLT/ nx1=%d ny1=%d direct=%d (fftw)\n",nx1,ny1,direct);
#endif

#ifndef TOTO_
sum =0;
for(i=0; i < ny1; i++) sum += re[nx1/2 + i * nx1];
printf(" Sum of central line = %e and sum/sqrt(ny)=%e \n",
        sum,sum/sqrt((double)ny1));
#endif

status = fftw_1D_Y_float(re, im, nx1, ny1, direct);

/* JLP99: to check that FFT is correct: */
/* Not recentred yet !! */
#ifndef TOTO_
for(i=0; i <= 2; i++)
printf("re,im [ixc,%d]: %e %e \n",i,
        re[nx1/2 + i*nx1],im[nx1/2 + i*nx1]);
#endif

return(status);
}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* 2D FFT routine for double precision arrays
* direct = 1  : direct or forward
* direct = -1 : reverse or backward
* (FORTRAN version)
* --------------------------------------------------------------*/
int FFTW_2D_DBLE(double *re, double *im, int *nx, int *ny, int *direct)
{
int status;
status = fftw_2D_double(re, im, *nx, *ny, *direct);
return(status);
}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* 2D FFT routine for double precision arrays
* direct = 1  : direct or forward
* direct = -1 : reverse or backward
*
* --------------------------------------------------------------*/
int fftw_2D_double(double *re, double *im, int nx, int ny, int direct)
{
register int i;
double norm;
int isize;
fftwnd_plan plan;
fftw_direction dir;
FFTW_COMPLEX* in_out;

isize = nx * ny;
in_out = (FFTW_COMPLEX *) malloc(isize * sizeof(FFTW_COMPLEX));

/* Transfer to complex array structure: */
for(i = 0; i < nx * ny; i++)
  {
   c_re(in_out[i]) = re[i];
   c_im(in_out[i]) = im[i];
  }

/* direct = 1  : forward
   direct = -1 : backward
*/
dir = (direct == 1) ? FFTW_FORWARD : FFTW_BACKWARD;
/* Warning: inversion nx-ny here: */
plan = fftw2d_create_plan(ny, nx, dir,
                              FFTW_ESTIMATE | FFTW_IN_PLACE);
if(plan == NULL)
{printf("FFTW_2D_DBLE: fatal error creating plan\n"); exit(-1);}

/* Compute the FFT: */
   fftwnd(plan, 1, in_out, 1, 0, 0, 0, 0);

/* Transfer back to original arrays (and normalize by sqrt(nx * ny): */
#ifdef NORMALIZE_SQRT
norm = nx * ny; norm = sqrt(norm);
for(i = 0; i < nx * ny; i++)
  {
  re[i] = c_re(in_out[i]) /norm;
  im[i] = c_im(in_out[i]) /norm;
  }
#else
if(direct == 1)
  {
  for(i = 0; i < nx * ny; i++)
    {
    re[i] = c_re(in_out[i]);
    im[i] = c_im(in_out[i]);
    }
  }
else
  {
  norm = nx * ny;
  for(i = 0; i < nx * ny; i++)
    {
    re[i] = c_re(in_out[i]) /norm;
    im[i] = c_im(in_out[i]) /norm;
    }
  }
#endif

free(in_out);

/* Delete plan: */
  fftwnd_destroy_plan(plan);

return(0);
}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* 2D FFT routine for single precision arrays (for which idim=nx)
* (well suited to C arrays since nx=idim...)
* direct = 1  : direct or forward
* direct = -1 : reverse or backward
* --------------------------------------------------------------*/
int fftw_2D_float(float *re, float *im, int nx, int ny, int direct)
{
register int i;
double norm;
int isize;
fftwnd_plan plan;
fftw_direction dir;
FFTW_COMPLEX* in_out;

isize = nx * ny;
in_out = (FFTW_COMPLEX *) malloc(isize * sizeof(FFTW_COMPLEX));

/* Transfer to complex array structure: */
for(i = 0; i < nx * ny; i++)
  {
   c_re(in_out[i]) = re[i];
   c_im(in_out[i]) = im[i];
  }

/* direct = 1  : forward
   direct = -1 : backward
*/
dir = (direct == 1) ? FFTW_FORWARD : FFTW_BACKWARD;
/* Warning: inversion nx-ny here: */
plan = fftw2d_create_plan(ny, nx, dir,
                              FFTW_ESTIMATE | FFTW_IN_PLACE);
if(plan == NULL)
{printf("fftw_2D_float: fatal error creating plan\n"); exit(-1);}

/* Compute the FFT: */
   fftwnd(plan, 1, in_out, 1, 0, 0, 0, 0);

/* Transfer back to original arrays (and normalize by sqrt(nx * ny): */
#ifdef NORMALIZE_SQRT
norm = nx * ny; norm = sqrt(norm);
for(i = 0; i < nx * ny; i++)
  {
  re[i] = c_re(in_out[i]) /norm;
  im[i] = c_im(in_out[i]) /norm;
  }
#else
if(direct == 1)
  {
  for(i = 0; i < nx * ny; i++)
    {
    re[i] = c_re(in_out[i]);
    im[i] = c_im(in_out[i]);
    }
  }
else
  {
  norm = nx * ny;
  for(i = 0; i < nx * ny; i++)
    {
    re[i] = c_re(in_out[i]) /norm;
    im[i] = c_im(in_out[i]) /norm;
    }
  }
#endif

free(in_out);

/* Delete plan: */
  fftwnd_destroy_plan(plan);

return(0);
}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* 1D FFT routine for single precision arrays
* along the columns (for spectroscopic mode)
* direct = 1  : direct or forward
* direct = -1 : reverse or backward
*
* --------------------------------------------------------------*/
int fftw_1D_Y_float(float *re, float *im, int nx, int ny, int direct)
{
register int i, j;
double norm;
fftw_plan plan;
fftw_direction dir;
FFTW_COMPLEX* in_out;

in_out = (FFTW_COMPLEX *) malloc(ny * sizeof(FFTW_COMPLEX));
if(in_out == NULL)
{printf("fftw_1D_Y_float: fatal error allocating memory\n"); exit(-1);}

/* direct = 1  : forward
   direct = -1 : backward
*/
dir = (direct == 1) ? FFTW_FORWARD : FFTW_BACKWARD;
plan = fftw_create_plan(ny, dir, FFTW_ESTIMATE | FFTW_IN_PLACE);
if(plan == NULL)
{printf("fftw_1D_Y_float: fatal error creating plan\n"); exit(-1);}

for(i = 0; i < nx; i++)
{
/* Transfer to complex array structure: */
  for(j = 0; j < ny; j++)
  {
   c_re(in_out[j]) = re[i + j * nx];
   c_im(in_out[j]) = im[i + j * nx];
  }

/* Compute the FFT: */
   fftw(plan, 1, in_out, 1, 0, 0, 0, 0);

/* Transfer back to original arrays (and normalize by sqrt(ny): */
#ifdef NORMALIZE_SQRT
norm = sqrt((double)ny);
for(j = 0; j < ny; j++)
  {
  re[i + j * nx] = c_re(in_out[j]) /norm;
  im[i + j * nx] = c_im(in_out[j]) /norm;
  }
#else
if(direct == 1)
  {
  for(j = 0; j < ny; j++)
    {
    re[i + j * nx] = c_re(in_out[j]);
    im[i + j * nx] = c_im(in_out[j]);
    }
  }
else
  {
  norm = (double)ny;
  for(j = 0; j < ny; j++)
    {
    re[i + j * nx] = c_re(in_out[j]) /norm;
    im[i + j * nx] = c_im(in_out[j]) /norm;
    }
  }
#endif
/* End of loop on i (from 0 to nx-1) */
}

free(in_out);

/* Delete plan: */
  fftw_destroy_plan(plan);

return(0);
}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* 1D FFT routine for double precision arrays
* along the columns (for spectroscopic mode)
* direct = 1  : direct or forward
* direct = -1 : reverse or backward
*
* --------------------------------------------------------------*/
int fftw_1D_Y_double(double *re, double *im, int nx, int ny, int direct)
{
register int i, j;
double norm;
fftw_plan plan;
fftw_direction dir;
FFTW_COMPLEX* in_out;

in_out = (FFTW_COMPLEX *) malloc(ny * sizeof(FFTW_COMPLEX));

/* direct = 1  : forward
   direct = -1 : backward
*/
dir = (direct == 1) ? FFTW_FORWARD : FFTW_BACKWARD;
plan = fftw_create_plan(ny, dir, FFTW_ESTIMATE | FFTW_IN_PLACE);
if(plan == NULL)
{printf("fftw_1D_Y: fatal error creating plan\n"); exit(-1);}

for(i = 0; i < nx; i++)
{
/* Transfer to complex array structure: */
  for(j = 0; j < ny; j++)
  {
   c_re(in_out[j]) = re[i + j * nx];
   c_im(in_out[j]) = im[i + j * nx];
  }

/* Compute the FFT: */
   fftw(plan, 1, in_out, 1, 0, 0, 0, 0);

/* Transfer back to original arrays (and normalize by sqrt(ny): */
#ifdef NORMALIZE_SQRT
norm = sqrt((double)ny);
for(j = 0; j < ny; j++)
  {
  re[i + j * nx] = c_re(in_out[j]) /norm;
  im[i + j * nx] = c_im(in_out[j]) /norm;
  }
#else
if(direct == 1)
  {
  for(j = 0; j < ny; j++)
    {
    re[i + j * nx] = c_re(in_out[j]);
    im[i + j * nx] = c_im(in_out[j]);
    }
  }
else
  {
  norm = (double)ny;
  for(j = 0; j < ny; j++)
    {
    re[i + j * nx] = c_re(in_out[j]) /norm;
    im[i + j * nx] = c_im(in_out[j]) /norm;
    }
  }
#endif
/* End of loop on i (from 0 to nx-1) */
}

free(in_out);

/* Delete plan: */
  fftw_destroy_plan(plan);

return(0);
}
/****************************************************************
* Fortran interface to fftw_setup
****************************************************************/
int FFTW_FSETUP(int *nx, int *ny)
{
fftw_setup(*nx, *ny);
return(0);
}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// fftw_setup, to initialize FFTW routines
// Create forward and backward plans
//
// --------------------------------------------------------------*/
int fftw_setup(int nx, int ny)
{
char fwd_name[60], bkwd_name[60];
FILE *fp_wisd;
int ToOutput;

/********************** With "forward"  ***************************/
#ifdef BORLAND
sprintf(fwd_name,"c:\\tc\\fftw13\\jlp\\fwd_%d.wis",nx);
#else
sprintf(fwd_name,"/d/fftw/fwd_%d%d.wis",nx,ny);
#endif

/* Check if wisdom file already exits: */
printf("fftw_setup/Read wisdom file: %s\n",fwd_name);
if((fp_wisd = fopen(fwd_name,"r")) == NULL)
  {
  printf("fftw_setup/Failure to open wisdom file: %s\n",fwd_name);
  ToOutput = 1;
  }
else
  {
  ToOutput = 0;
  if(fftw_import_wisdom_from_file(fp_wisd) == FFTW_FAILURE) ToOutput = 1;
  fclose(fp_wisd);
  }

/* Create  "plan" for fftw (speed is wanted for subsequent FFT's) */
/* Warning: inversion nx-ny here: */
plan_fwd = fftw2d_create_plan(ny, nx, FFTW_FORWARD,
                              FFTW_MEASURE | FFTW_IN_PLACE | FFTW_USE_WISDOM);

/* Output wisdom file if needed: */
if(ToOutput)
  {
  printf("fftw_setup/Write wisdom file: %s\n",fwd_name);
  if((fp_wisd = fopen(fwd_name,"w")) != NULL)
                                       fftw_export_wisdom_to_file(fp_wisd);
  fclose(fp_wisd);
  }

/********************** With "backward"  ***************************/
#ifdef BORLAND
sprintf(bkwd_name,"c:\\tc\\fftw13\\jlp\\bk_%d%d.wis",nx,ny);
#else
sprintf(bkwd_name,"/d/fftw/bk_%d%d.wis",nx,ny);
#endif

/* Check if wisdom file already exits: */
printf("fftw_setup/Read wisdom file: %s\n",bkwd_name);
if((fp_wisd = fopen(bkwd_name,"r")) == NULL)
  {
  printf("fftw_setup/Failure to open wisdom file: %s\n",bkwd_name);
  ToOutput = 1;
  }
else
  {
  ToOutput = 0;
  if(fftw_import_wisdom_from_file(fp_wisd) == FFTW_FAILURE) ToOutput = 1;
  fclose(fp_wisd);
  }

/* Create  "plan" for fftw (speed is wanted for subsequent FFT's) */
/* Warning: inversion nx-ny here: */
plan_bkwd = fftw2d_create_plan(ny, nx, FFTW_BACKWARD,
                              FFTW_MEASURE | FFTW_IN_PLACE | FFTW_USE_WISDOM);

/* Output wisdom file if needed: */
if(ToOutput)
  {
  printf("fftw_setup/Write wisdom file: %s\n",bkwd_name);
  if((fp_wisd = fopen(bkwd_name,"w")) != NULL)
                                        fftw_export_wisdom_to_file(fp_wisd);
  fclose(fp_wisd);
  }

return(0);
}
/*************************************************************
*
**************************************************************/
int FFTW_SHUTDOWN()
{
/* Delete plans: */
  fftwnd_destroy_plan(plan_fwd);
  fftwnd_destroy_plan(plan_bkwd);
return(0);
}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
* 2D FFT routine for double precision fftw_complex arrays
* direction = 1  : forward
* direction = -1 : backward
*  Should be called after fftw_setup and before fftw_shutdown
* --------------------------------------------------------------*/
int fftw_fast(FFTW_COMPLEX *image, int nx, int ny, int dir)
{
register int i;
double norm;

/* Compute the FFT: */
if(dir == 1)
   fftwnd(plan_fwd, 1, image, 1, 0, 0, 0, 0);
else
   fftwnd(plan_bkwd, 1, image, 1, 0, 0, 0, 0);

/* Normalize by sqrt(nx * ny): */
#ifdef NORMALIZE_SQRT
norm = nx * ny; norm = sqrt(norm);
for(i = 0; i < nx * ny; i++)
  {
  c_re(image[i]) /= norm;
  c_im(image[i]) /= norm;
  }
#else
if(dir == -1)
  {
  norm = (double)(nx * ny);
  for(i = 0; i < nx * ny; i++)
    {
    c_re(image[i]) /= norm;
    c_im(image[i]) /= norm;
    }
  }
#endif

return(0);
}


/*
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_4_2.c
 * \brief Source file to optimize Runge-Kutta 4 steps 2nd 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_4_2.h"

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

/**
 * Function to obtain the coefficients of a 4 steps 2nd order Runge-Kutta
 * method.
 */
int
rk_tb_4_2 (Optimize * optimize) ///< Optimize struct.
{
  long double *tb, *r;
#if DEBUG_RK_4_2
  fprintf (stderr, "rk_tb_4_2: start\n");
#endif
  tb = optimize->coefficient;
  r = optimize->random_data;
  t4 (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];
  b41 (tb) = r[6];
  b42 (tb) = r[7];
  b43 (tb) = (0.5L - b41 (tb) * t1 (tb) - b42 (tb) * t2 (tb)) / t3 (tb);
  if (isnan (b43 (tb)))
    return 0;
  rk_b_4 (tb);
#if DEBUG_RK_4_2
  fprintf (stderr, "rk_tb_4_2: end\n");
#endif
  return 1;
}

/**
 * Function to obtain the coefficients of a 4 steps 2nd order, 3rd order in
 * equations depending only in time, Runge-Kutta method.
 */
int
rk_tb_4_2t (Optimize * optimize)        ///< Optimize struct.
{
  long double *tb, *r;
#if DEBUG_RK_4_2
  fprintf (stderr, "rk_tb_4_2t: start\n");
#endif
  tb = optimize->coefficient;
  r = optimize->random_data;
  t4 (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];
  b41 (tb) = r[6];
  b42 (tb) = (1.L / 3.L - 0.5L * t3 (tb)
              - b41 (tb) * t1 (tb) * (t1 (tb) - t3 (tb)))
    / (t2 (tb) * (t2 (tb) - t3 (tb)));
  if (isnan (b42 (tb)))
    return 0;
  b43 (tb) = (0.5L - b41 (tb) * t1 (tb) - b42 (tb) * t2 (tb)) / t3 (tb);
  if (isnan (b43 (tb)))
    return 0;
  rk_b_4 (tb);
#if DEBUG_RK_4_2
  fprintf (stderr, "rk_tb_4_2t: end\n");
#endif
  return 1;
}

/**
 * Function to obtain the coefficients of a 4 steps 1st-2nd order Runge-Kutta
 * pair.
 */
int
rk_tb_4_2p (Optimize * optimize)        ///< Optimize struct.
{
  long double *tb;
#if DEBUG_RK_4_2
  fprintf (stderr, "rk_tb_4_2p: start\n");
#endif
  if (!rk_tb_4_2 (optimize))
    return 0;
  tb = optimize->coefficient;
  e41 (tb) = e42 (tb) = 0.L;
  rk_e_4 (tb);
#if DEBUG_RK_4_2
  rk_print_e (optimize, "rk_tb_4_2p", stderr);
  fprintf (stderr, "rk_tb_4_2p: end\n");
#endif
  return 1;
}

/**
 * Function to obtain the coefficients of a 4 steps 1st-2nd order, 1st-3rd order
 * in equations depending only in time, Runge-Kutta method.
 */
int
rk_tb_4_2tp (Optimize * optimize)       ///< Optimize struct.
{
  long double *tb;
#if DEBUG_RK_4_2
  fprintf (stderr, "rk_tb_4_2tp: start\n");
#endif
  if (!rk_tb_4_2t (optimize))
    return 0;
  tb = optimize->coefficient;
  e41 (tb) = e42 (tb) = 0.L;
  rk_e_4 (tb);
#if DEBUG_RK_4_2
  rk_print_tb (optimize, "rk_tb_4_2tp", stderr);
  fprintf (stderr, "rk_tb_4_2pt: end\n");
#endif
  return 1;
}

/**
 * Function to calculate the objective function of a 4 steps 2nd order
 * Runge-Kutta method.
 *
 * \return objective function value.
 */
long double
rk_objective_tb_4_2 (RK * rk)   ///< RK struct.
{
  long double *tb;
  long double o;
#if DEBUG_RK_4_2
  fprintf (stderr, "rk_objective_tb_4_2: 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 (b43 (tb) < 0.L)
    o += b43 (tb);
  if (o < 0.L)
    {
      o = 40.L - o;
      goto end;
    }
  o = 30.L + fmaxl (1.L, fmaxl (t1 (tb), fmaxl (t2 (tb), t3 (tb))));
  if (rk->strong)
    {
      rk_bucle_ac (rk);
      o = fminl (o, *rk->ac0->optimal);
    }
end:
#if DEBUG_RK_4_2
  fprintf (stderr, "rk_objective_tb_4_2: optimal=%Lg\n", o);
  fprintf (stderr, "rk_objective_tb_4_2: end\n");
#endif
  return o;
}

/**
 * Function to calculate the objective function of a 4 steps 2nd order, 3rd
 * order in equations depending only in time, Runge-Kutta method.
 *
 * \return objective function value.
 */
long double
rk_objective_tb_4_2t (RK * rk)  ///< RK struct.
{
  long double *tb;
  long double o;
#if DEBUG_RK_4_2
  fprintf (stderr, "rk_objective_tb_4_2t: 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 (b42 (tb) < 0.L)
    o += b42 (tb);
  if (b43 (tb) < 0.L)
    o += b43 (tb);
  if (o < 0.L)
    {
      o = 40.L - o;
      goto end;
    }
  o = 30.L + fmaxl (1.L, fmaxl (t1 (tb), fmaxl (t2 (tb), t3 (tb))));
  if (rk->strong)
    {
      rk_bucle_ac (rk);
      o = fminl (o, *rk->ac0->optimal);
    }
end:
#if DEBUG_RK_4_2
  fprintf (stderr, "rk_objective_tb_4_2t: optimal=%Lg\n", o);
  fprintf (stderr, "rk_objective_tb_4_2t: end\n");
#endif
  return o;
}


#include <stdio.h>
#include <gsl/gsl_sf_bessel.h>

int main (void)
{
  double x, y;
  x = 5.0;
  y = gsl_sf_bessel_J0 (x);
  printf ("J0(%g) = %.18e\n", x, y);
  return 0;
}