#ifndef VEC3_H
#define VEC3_H

#include <cmath>
#include <iostream>

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
// you need to add the following libraries to your project : gsl, gslcblas

class Vec3 {
private:
    float mVals[3];
public:
    Vec3() {}
    Vec3( float x , float y , float z ) {
       mVals[0] = x; mVals[1] = y; mVals[2] = z;
    }
    float & operator [] (unsigned int c) { return mVals[c]; }
    float operator [] (unsigned int c) const { return mVals[c]; }
    void operator = (Vec3 const & other) {
       mVals[0] = other[0] ; mVals[1] = other[1]; mVals[2] = other[2];
    }
    float squareLength() const {
       return mVals[0]*mVals[0] + mVals[1]*mVals[1] + mVals[2]*mVals[2];
    }
    float length() const { return sqrt( squareLength() ); }
    void normalize() { float L = length(); mVals[0] /= L; mVals[1] /= L; mVals[2] /= L; }
    static float dot( Vec3 const & a , Vec3 const & b ) {
       return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
    }
    static Vec3 cross( Vec3 const & a , Vec3 const & b ) {
       return Vec3( a[1]*b[2] - a[2]*b[1] ,
                    a[2]*b[0] - a[0]*b[2] ,
                    a[0]*b[1] - a[1]*b[0] );
    }
    void operator += (Vec3 const & other) {
        mVals[0] += other[0];
        mVals[1] += other[1];
        mVals[2] += other[2];
    }
    void operator -= (Vec3 const & other) {
        mVals[0] -= other[0];
        mVals[1] -= other[1];
        mVals[2] -= other[2];
    }
    void operator *= (float s) {
        mVals[0] *= s;
        mVals[1] *= s;
        mVals[2] *= s;
    }
    void operator /= (float s) {
        mVals[0] /= s;
        mVals[1] /= s;
        mVals[2] /= s;
    }

    static Vec3 Rand(float magnitude = 1.f) {
        return Vec3( magnitude * (-1.f + 2.f * rand() / (float)(RAND_MAX)) , magnitude * (-1.f + 2.f * rand() / (float)(RAND_MAX)) , magnitude * (-1.f + 2.f * rand() / (float)(RAND_MAX)) );
    }

    Vec3 getOrthogonal() const {
        // CAREFUL ! THE NORM IS NOT PRESERVED !!!!
        if( mVals[0] == 0 ) {
            return Vec3( 0 , mVals[2] , -mVals[1] );
        }
        else if( mVals[1] == 0 ) {
            return Vec3( mVals[2] , 0 , -mVals[0] );
        }
        return Vec3( mVals[1] , -mVals[0] , 0 );
    }
};

static inline Vec3 operator + (Vec3 const & a , Vec3 const & b) {
   return Vec3(a[0]+b[0] , a[1]+b[1] , a[2]+b[2]);
}
static inline Vec3 operator - (Vec3 const & a , Vec3 const & b) {
   return Vec3(a[0]-b[0] , a[1]-b[1] , a[2]-b[2]);
}
static inline Vec3 operator * (float a , Vec3 const & b) {
   return Vec3(a*b[0] , a*b[1] , a*b[2]);
}
static inline Vec3 operator / (Vec3 const &  a , float b) {
   return Vec3(a[0]/b , a[1]/b , a[2]/b);
}
static inline std::ostream & operator << (std::ostream & s , Vec3 const & p) {
    s << p[0] << " " << p[1] << " " << p[2];
    return s;
}
static inline std::istream & operator >> (std::istream & s , Vec3 & p) {
    s >> p[0] >> p[1] >> p[2];
    return s;
}

class Mat3
{
public:
    ////////////         CONSTRUCTORS          //////////////
    Mat3()
    {
        vals[0] = 0;
        vals[1] = 0;
        vals[2] = 0;
        vals[3] = 0;
        vals[4] = 0;
        vals[5] = 0;
        vals[6] = 0;
        vals[7] = 0;
        vals[8] = 0;
    }
    Mat3( float v1 , float v2 , float v3 , float v4 , float v5 , float v6 , float v7 , float v8 , float v9)
    {
        vals[0] = v1;
        vals[1] = v2;
        vals[2] = v3;
        vals[3] = v4;
        vals[4] = v5;
        vals[5] = v6;
        vals[6] = v7;
        vals[7] = v8;
        vals[8] = v9;
    }
    Mat3( const Mat3 & m )
    {
        for(int i = 0 ; i < 3 ; ++i )
            for(int j = 0 ; j < 3 ; ++j )
                (*this)(i,j) = m(i,j);
    }

    bool isnan() const {
        return std::isnan(vals[0]) || std::isnan(vals[1]) || std::isnan(vals[2])
                 || std::isnan(vals[3]) || std::isnan(vals[4]) || std::isnan(vals[5])
                 || std::isnan(vals[6]) || std::isnan(vals[7]) || std::isnan(vals[8]);
    }

    void operator = (const Mat3 & m)
    {
        for(int i = 0 ; i < 3 ; ++i )
            for(int j = 0 ; j < 3 ; ++j )
                (*this)(i,j) = m(i,j);
    }

    void operator += (const Mat3 & m)
    {
        for(int i = 0 ; i < 3 ; ++i )
            for(int j = 0 ; j < 3 ; ++j )
                (*this)(i,j) += m(i,j);
    }
    void operator -= (const Mat3 & m)
    {
        for(int i = 0 ; i < 3 ; ++i )
            for(int j = 0 ; j < 3 ; ++j )
                (*this)(i,j) -= m(i,j);
    }
    void operator /= (double s)
    {
        for( unsigned int c = 0 ; c < 9 ; ++c )
            vals[c] /= s;
    }

    Mat3 operator - (const Mat3 & m2)
    {
        return Mat3( (*this)(0,0)-m2(0,0) , (*this)(0,1)-m2(0,1) , (*this)(0,2)-m2(0,2) , (*this)(1,0)-m2(1,0) , (*this)(1,1)-m2(1,1) , (*this)(1,2)-m2(1,2) , (*this)(2,0)-m2(2,0) , (*this)(2,1)-m2(2,1) , (*this)(2,2)-m2(2,2) );
    }
    Mat3 operator + (const Mat3 & m2)
    {
        return Mat3( (*this)(0,0)+m2(0,0) , (*this)(0,1)+m2(0,1) , (*this)(0,2)+m2(0,2) , (*this)(1,0)+m2(1,0) , (*this)(1,1)+m2(1,1) , (*this)(1,2)+m2(1,2) , (*this)(2,0)+m2(2,0) , (*this)(2,1)+m2(2,1) , (*this)(2,2)+m2(2,2) );
    }

    Mat3 operator / (float s)
    {
        return Mat3( (*this)(0,0)/s , (*this)(0,1)/s , (*this)(0,2)/s , (*this)(1,0)/s , (*this)(1,1)/s , (*this)(1,2)/s , (*this)(2,0)/s , (*this)(2,1)/s , (*this)(2,2)/s );
    }
    Mat3 operator * (float s)
    {
        return Mat3( (*this)(0,0)*s , (*this)(0,1)*s , (*this)(0,2)*s , (*this)(1,0)*s , (*this)(1,1)*s , (*this)(1,2)*s , (*this)(2,0)*s , (*this)(2,1)*s , (*this)(2,2)*s );
    }

    Vec3 operator * (const Vec3 & p) // computes m.p
    {
        return Vec3(
                    (*this)(0,0)*p[0] + (*this)(0,1)*p[1] + (*this)(0,2)*p[2],
                    (*this)(1,0)*p[0] + (*this)(1,1)*p[1] + (*this)(1,2)*p[2],
                    (*this)(2,0)*p[0] + (*this)(2,1)*p[1] + (*this)(2,2)*p[2]);
    }

    Mat3 operator * (const Mat3 & m2)
    {
        return Mat3(
                    (*this)(0,0)*m2(0,0) + (*this)(0,1)*m2(1,0) + (*this)(0,2)*m2(2,0) ,
                    (*this)(0,0)*m2(0,1) + (*this)(0,1)*m2(1,1) + (*this)(0,2)*m2(2,1) ,
                    (*this)(0,0)*m2(0,2) + (*this)(0,1)*m2(1,2) + (*this)(0,2)*m2(2,2) ,
                    (*this)(1,0)*m2(0,0) + (*this)(1,1)*m2(1,0) + (*this)(1,2)*m2(2,0) ,
                    (*this)(1,0)*m2(0,1) + (*this)(1,1)*m2(1,1) + (*this)(1,2)*m2(2,1) ,
                    (*this)(1,0)*m2(0,2) + (*this)(1,1)*m2(1,2) + (*this)(1,2)*m2(2,2) ,
                    (*this)(2,0)*m2(0,0) + (*this)(2,1)*m2(1,0) + (*this)(2,2)*m2(2,0) ,
                    (*this)(2,0)*m2(0,1) + (*this)(2,1)*m2(1,1) + (*this)(2,2)*m2(2,1) ,
                    (*this)(2,0)*m2(0,2) + (*this)(2,1)*m2(1,2) + (*this)(2,2)*m2(2,2)
                    );
    }

    ////////        ACCESS TO COORDINATES      /////////
    float operator () (unsigned int i , unsigned int j) const
    { return vals[3*i + j]; }
    float & operator () (unsigned int i , unsigned int j)
    { return vals[3*i + j]; }

    ////////        BASICS       /////////
    inline float sqrnorm()
    {
        return vals[0]*vals[0] + vals[1]*vals[1] + vals[2]*vals[2]
                + vals[3]*vals[3] + vals[4]*vals[4] + vals[5]*vals[5]
                + vals[6]*vals[6] +  vals[7]*vals[7] + vals[8]*vals[8];
    }

    inline float norm()
    { return sqrt( sqrnorm() ); }

    inline float determinant() const
    {
        return vals[0] * ( vals[4] * vals[8] - vals[7] * vals[5] )
                - vals[1] * ( vals[3] * vals[8] - vals[6] * vals[5] )
                + vals[2] * ( vals[3] * vals[7] - vals[6] * vals[4] );
    }

    static
    Mat3 pseudoInverse( Mat3 const & m , bool & isRealInverse , double defaultValueForInverseSingularValue = 0.0 )
    {
        float det = m.determinant();
        if( fabs(det) != 0.0 )
        {
            isRealInverse = true;
            return Mat3( m(1,1)*m(2,2) - m(2,1)*m(1,2) , m(0,2)*m(2,1) - m(0,1)*m(2,2) , m(0,1)*m(1,2) - m(0,2)*m(1,1) ,
                             m(1,2)*m(2,0) - m(1,0)*m(2,2) , m(0,0)*m(2,2) - m(0,2)*m(2,0) , m(0,2)*m(1,0) - m(0,0)*m(1,2) ,
                             m(1,0)*m(2,1) - m(1,1)*m(2,0) , m(0,1)*m(2,0) - m(0,0)*m(2,1) , m(0,0)*m(1,1) - m(0,1)*m(1,0) ) / det ;
        }

        // otherwise:
        isRealInverse = false;
        Mat3 U ; float sx ; float sy ; float sz ; Mat3 Vt;
        m.SVD(U,sx,sy,sz,Vt);
        float sxInv = sx == 0.0 ? 1.0 / sx : defaultValueForInverseSingularValue;
        float syInv = sy == 0.0 ? 1.0 / sy : defaultValueForInverseSingularValue;
        float szInv = sz == 0.0 ? 1.0 / sz : defaultValueForInverseSingularValue;
        return Vt.getTranspose() * Mat3::diag(sxInv , syInv , szInv) * U.getTranspose();
    }

    inline float trace() const
    { return vals[0] + vals[4] + vals[8]; }

    ////////        TRANSPOSE       /////////
    inline
    void transpose()
    {
        float xy = vals[1] , xz = vals[2] , yz = vals[5];
        vals[1] = vals[3];
        vals[3] = xy;
        vals[2] = vals[6];
        vals[6] = xz;
        vals[5] = vals[7];
        vals[7] = yz;
    }
    Mat3 getTranspose() const
    {
        return Mat3(vals[0],vals[3],vals[6],vals[1],vals[4],vals[7],vals[2],vals[5],vals[8]);
    }

    // ---------- ROTATION <-> AXIS/ANGLE ---------- //
    template< class point_t >
    void getAxisAndAngleFromRotationMatrix( point_t & axis , float & angle )
    {
        angle = acos( (trace() - 1.f) / 2.f );
        axis[0] = vals[7] - vals[5];
        axis[1] = vals[2] - vals[6];
        axis[2] = vals[3] - vals[1];
        axis.normalize();
    }

    template< class point_t >
    inline static
    Mat3 getRotationMatrixFromAxisAndAngle( const point_t & axis , float angle )
    {
        Mat3 w = vectorial(axis);
        return Identity() +  w * std::sin(angle) +  w * w * ((1.0) - std::cos(angle));
    }

    // ---------- STATIC STANDARD MATRICES ---------- //
    inline static Mat3 Identity()
    {  return Mat3(1,0,0  ,  0,1,0  ,  0,0,1);  }

    inline static Mat3 Zero()
    {  return Mat3(0,0,0  ,  0,0,0  ,  0,0,0);  }

    template< typename T2 >
    inline static Mat3 diag( T2 x , T2 y ,T2 z )
    {  return Mat3(x,0,0  ,  0,y,0  ,  0,0,z);  }

    template< class point_t >
    inline static Mat3 getFromCols(const point_t & c1 , const point_t & c2 , const point_t & c3)
    {
        // 0 1 2
        // 3 4 5
        // 6 7 8
        return Mat3( c1[0] , c2[0] , c3[0] ,
                         c1[1] , c2[1] , c3[1] ,
                         c1[2] , c2[2] , c3[2] );
    }
    template< class point_t >
    inline static Mat3 getFromRows(const point_t & r1 , const point_t & r2 , const point_t & r3)
    {
        // 0 1 2
        // 3 4 5
        // 6 7 8
        return Mat3( r1[0] , r1[1] , r1[2] ,
                         r2[0] , r2[1] , r2[2] ,
                         r3[0] , r3[1] , r3[2] );
    }

    inline static Mat3 RandRotation()
    {
        Vec3 axis(-1.0 + 2.0* (float)(rand()) / (float)( RAND_MAX )  ,
                       -1.0 + 2.0* (float)(rand()) / (float)( RAND_MAX )  ,
                       -1.0 + 2.0* (float)(rand()) / (float)( RAND_MAX )  );
        axis.normalize();
        float angle = 2.0 * M_PI * ((float)(rand()) / (float)( RAND_MAX )   - 0.5 );

        return Mat3::getRotationMatrixFromAxisAndAngle( axis , angle );
    }

    inline static Mat3 RandRotation( float maxAngle )
    {
        Vec3 axis(-1.0 + 2.0* (float)(rand()) / (float)( RAND_MAX )  ,
                       -1.0 + 2.0* (float)(rand()) / (float)( RAND_MAX )  ,
                       -1.0 + 2.0* (float)(rand()) / (float)( RAND_MAX )  );
        axis.normalize();
        float angle =  maxAngle * ((float)(rand()) / (float)( RAND_MAX )   - 0.5 );

        return Mat3::getRotationMatrixFromAxisAndAngle( axis , angle );
    }

    inline static Mat3 RandRotation( Vec3 twistAxis , double maxTwist , double maxRotation )
    {
        twistAxis.normalize();
        Vec3 u = twistAxis.getOrthogonal();
        u.normalize();
        const Vec3 & v = Vec3::cross(u,twistAxis);

        double uv_axis_angle = 2.0*M_PI * (double)(rand()) / (double)(RAND_MAX);
        const Vec3 & uv = cos(uv_axis_angle)*u + sin(uv_axis_angle)*v;

        double rotation_angle = maxRotation * ( ( 2.0 *(double)(rand()) / (double)(RAND_MAX) ) - 1.0 );
        double twist_angle = maxTwist * ( ( 2.0 *(double)(rand()) / (double)(RAND_MAX) ) - 1.0 );

        return Mat3::getRotationMatrixFromAxisAndAngle(uv , rotation_angle) * Mat3::getRotationMatrixFromAxisAndAngle(twistAxis , twist_angle);
    }

    void SVD( Mat3 & U , float & sx , float & sy , float & sz , Mat3 & Vt ) const
    {
        gsl_matrix * u = gsl_matrix_alloc(3,3);
        for(unsigned int i = 0 ; i < 3; ++i)
            for(unsigned int j = 0 ; j < 3; ++j)
                gsl_matrix_set( u , i , j , (*this)(i,j) );

        gsl_matrix * v = gsl_matrix_alloc(3,3);
        gsl_vector * s = gsl_vector_alloc(3);
        gsl_vector * work = gsl_vector_alloc(3);

        gsl_linalg_SV_decomp (u,
                              v,
                              s,
                              work);

        sx = s->data[0];
        sy = s->data[1];
        sz = s->data[2];
        for(unsigned int i = 0 ; i < 3; ++i)
        {
            for(unsigned int j = 0 ; j < 3; ++j)
            {
                U(i,j) = gsl_matrix_get( u , i , j );
                Vt(i,j) = gsl_matrix_get( v , j , i );
            }
        }

        gsl_matrix_free(u);
        gsl_matrix_free(v);
        gsl_vector_free(s);
        gsl_vector_free(work);

        // a transformation float is given as R.B.S.Bt, R = rotation , B = local basis (rotation matrix), S = scales in the basis B
        // it can be obtained from the svd decomposition of float = U Sigma Vt :
        // B = V
        // S = Sigma
        // R = U.Vt
    }

    // ---------- Projections onto Rotations ----------- //

    void setRotation()
    {
        Mat3 U,Vt;
        float sx,sy,sz;
        SVD(U,sx,sy,sz,Vt);
        const Mat3 & res = U*Vt;
        if( res.determinant() < 0 )
        {
            U(0,2) = -U(0,2);
            U(1,2) = -U(1,2);
            U(2,2) = -U(2,2);
            *this = (U*Vt);
            return;
        }
        // else
        *this = (res);
    }

    template< class point_t >
    inline static
    Mat3 tensor( const point_t & p1 , const point_t & p2 )
    {
        return Mat3(
                    p1[0]*p2[0] , p1[0]*p2[1] , p1[0]*p2[2],
                    p1[1]*p2[0] , p1[1]*p2[1] , p1[1]*p2[2],
                    p1[2]*p2[0] , p1[2]*p2[1] , p1[2]*p2[2]);
    }

    template< class point_t >
    inline static
    Mat3 vectorial( const point_t & p )
    {
        return Mat3(
                    0      , -p[2]  , p[1]     ,
                    p[2]   , 0      , - p[0]   ,
                    - p[1] , p[0]   , 0
                    );
    }

    Mat3 operator - () const
    {
        return Mat3( - vals[0],- vals[1],- vals[2],- vals[3],- vals[4],- vals[5],- vals[6],- vals[7],- vals[8] );
    }

private:
    float vals[9];
    // will be noted as :
    // 0 1 2
    // 3 4 5
    // 6 7 8
};

inline static
Mat3 operator * (float s , const Mat3 & m)
{
    return Mat3( m(0,0)*s , m(0,1)*s , m(0,2)*s , m(1,0)*s , m(1,1)*s , m(1,2)*s , m(2,0)*s , m(2,1)*s , m(2,2)*s );
}

inline static std::ostream & operator << (std::ostream & s , Mat3 const & m)
{
    s << m(0,0) << " \t" << m(0,1) << " \t" << m(0,2) << std::endl << m(1,0) << " \t" << m(1,1) << " \t" << m(1,2) << std::endl << m(2,0) << " \t" << m(2,1) << " \t" << m(2,2) << std::endl;
    return s;
}

#endif


/*****************************************************************************
* This program allows the reverberation time of a venue to be determined
* blindly, using a sample of music or speech that is recorded in the venue
* C implementation of Paul Kendrick's Matlab algorithm:
*
* Kendrick, Paul, et al. "Blind estimation of reverberation parameters for
* non-diffuse rooms." Acta Acustica united with Acustica 93.5 (2007): 760-770.
*
* C implementation by Kim Jones <K.Jones20@edu.salford.ac.uk>
*****************************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <complex.h>
#include <float.h>
#include <time.h>
#include <sndfile.h>
#include <libswresample/swresample.h>
#include <libavutil/channel_layout.h>
#include <libavcodec/avcodec.h>
#include <liquid/liquid.h>
#include <nlopt.h>
#include <svdlib.h>
#include "butter_params.h"
#include <omp.h>

#define	MAX_PATH_SZ	150
#define SPLIT		20		/* Process wav in SPLIT segment(s) */
#define MAX_CHUNK_SIZE	1024 * 1024 * 1024
#define WINDOW_WIDTH	0.5
#define OVERLAP		0.98
#define USE_HARD_BUTTER	1		/* Use butter_params.h if set */
#define OCT_FILT_ORDER	5
#define ENV_FILT_ORDER	4
#define N_OCT 		OCT_FILT_ORDER * 2
#define R_OCT 		N_OCT % 2
#define L_OCT 		(N_OCT - R_OCT) / 2

#define N_ENV 		ENV_FILT_ORDER
#define R_ENV 		N_ENV % 2
#define L_ENV 		(N_ENV - R_ENV) / 2
#define LOW_PASS_CUTOFF	80.0
#define DAMP_SEG_SIZE	0.5
#define DAMP_STEP_SIZE	0.002
#define MAX_RT		100.0
#define MIN_RT		0.005
#define	BEST_S_NUM	20000		/* Set to 100000 in Matlab */
#define PAR_UP_BOUND	0.99999
#define PAR_LOW_BOUND	0.99
#define SQP_STEP	5
//#define COARSE_DR_EST	1
#define SEG_LEN		3 * 60		/* 3 minutes */
#define RT_DECAY	25		/* 25 = T20 decay; 35 = T30 decay */
#define DISCARD_THRESH	0		/* Discard decay segs if <= this */

/* Functions */
void usage_func(void);
int get_wav_data(char *filename);
int compute_rt(int samp, int array_size, int *store_start, int *store_end,
	double *dr, double *a, double *b, double *alpha, double *mean_rt,
	double *rt_sd, int band, FILE *fh);
int optimum_model(double a, double b, double alpha, double dr, int samp, int n,
	double *chan_2);
int optimum_model_2(double **chan, int n, int chan_sz, int samp, double *lin);
void hanning(int len, double *hann);
int process_wav_data(int band, float *wav_data, SF_INFO input_info,
	SNDFILE *input, unsigned long long frames, double *p_alpha,
	double *p_a, double *p_b, double *p_dr, int *p_start, int *p_end, int
	*filt_frames);
float * resamp_wav_data(SwrContext *resamp, int in_rate, uint64_t num_frames,
	int samp_freq, const uint8_t **wav_data, int *resamp_frms);
float * oct_filt_data(float *output_data, int band_num, float samp_freq,
	int resamp_frames);
float * get_envelope(float samp_freq, float *resampled_wav,
	int resamp_frames);
int ** apply_polyfit(float samp_freq, int resamp_frames, float *env,
	int *s_e_size);
int ** choose_segments(float *var, int step_size, int len, int not_dumped,
	int *seg_num_els);
int perform_ml(int **start_end, float *env, int s_e_size,
	float *filtered_wav, int resamp_frames, int samp_freq,
	double **p_alpha, double **p_a_par, double **p_b_par, double **p_dr,
	int **p_start, int **p_end);
int ml_fit(float *data_seg, int len, float samp_freq, double *a_par,
	double *b_par, double *alph_par);
double get_decay_region(int c_len, double a, double b, double alpha,
		int len_store);
double alpha_opt(int n, const double *a, double *grad, void *nldv);
double par_3_opt(int n, const double *a, double *grad, void *nldv);

/* Structs */
struct nl_extra_data {
	float *data_seg;
	int len;
	double a_val;
	double b_val;
};

/* Globals */
int seg_len_val = SEG_LEN;
int num_bands = 8;
int octave_bands[] = {63, 125, 250, 500, 1000, 2000, 4000, 8000};
int samp_freq_per_band[] = {3000, 3000, 3000, 3000, 3000, 6000, 12000, 24000};
const char fname[] = "./blind_rts.log";

#if OCT_FILT_ORDER >= ENV_FILT_ORDER
	float butt_b[3 * (L_OCT + R_OCT)] = {0};
	float butt_a[3 * (L_OCT + R_OCT)] = {0};
#else
	float butt_b[3 * (L_ENV + R_ENV)] = {0};
	float butt_a[3 * (L_ENV + R_ENV)] = {0};
#endif

int main(int argc, char *argv[])
{
	double ret;
	char filename[MAX_PATH_SZ];

	if (argc < 2 || argc > 3) {
		usage_func();
		return 0;
	}

	/* Read optional seg_len_val amendment */
	if (argc == 3) {
		if (atoi(argv[2]) > 0 && atoi(argv[2]) < 10) {
			seg_len_val = atoi(argv[2]);
			printf("Changing segment length to %d minutes\n",
				seg_len_val);
		}

	}

	if (snprintf(filename, MAX_PATH_SZ, "%s", argv[1]) >=  MAX_PATH_SZ) {
		printf("File path too long, please shorten the path / filename\n"
			"Filename was truncated: %s\n", filename);
		return 0;
	}

	ret = get_wav_data(filename);

	if (ret < 0) {
		printf("Error returned during calculations...\n");
		return -1;
	} else {
		printf("Results exported to log file: %s\n", fname);
		printf("Exiting gracefully...\n");
        }
	return 0;
}

void usage_func(void) {
	printf("Usage:\n"
		"./blind_rt_est <path to wav> [segment length]\n\n"
		"Wav file must be 16 bit signed and in mono channel format.\n\n"
		"Segment length is an optional command line parameter to\n"
		"change the segment length from the default value (3 mins).\n"
		"The audio is divided into segments for obtaining the RT;\n"
		"there should be 36 decays of at least -25 dB in each "
		"segment.\nIf you are unsure, just ignore this parameter and "
		"use the default value.\n");
}

int get_wav_data(char *filename) {
	SF_INFO input_info;
	float *wav_data;
	unsigned long long long_ret = 0;
	int *store_start, *store_end;
	int i, j, k, band, array_size, current_sz = 0, ret = 0;
	int filt_frm_tot, filt_frames;
	double *a, *b, *alpha, *dr, mean_rt, rt_sd;
	time_t ct = time(NULL);
	FILE *fh = fopen(fname, "a+");

	if (fh == NULL) {
		printf("Error opening log file!\n");
		return 0;
	}

	/* Open file */
	input_info.format = 0;
	SNDFILE *input = sf_open(filename, SFM_READ, &input_info);

	if (input == NULL) {
		printf("Error opening Wav file!\n");
		usage_func();
		return 0;
	} else
		printf("Reading %s\n", filename);

	/* Print header data */
	printf("Frames: %llu\nSample Rate: %d\nChannels: %d\nFormat: 0x%X\n"
		"Sections: %d\nSeekable: %d\n",(unsigned long long)
		input_info.frames, input_info.samplerate, input_info.channels,
		input_info.format, input_info.sections, input_info.seekable);

	/* Check to make sure that the wav file is in the expected format:
		mono channel and 16 bit signed wav */
	if ((input_info.format & 0xFF000F) != 0x10002 &&
			(input_info.format & 0xFF000F) != 0x130002) {
		printf("Program requires 16-bit signed wav.\nMake sure format "
			"is 0x10002 or 0x130002.\nExiting...\n");
		sf_close(input);
		return 0;
	}
	if (input_info.channels != 1) {
		printf("Input must be a single channel wav file. Exiting...\n");
		sf_close(input);
		return 0;
	}

	fprintf(fh, "=====================================================\n");
	fprintf(fh, "Reverberation Time calculations: %s\n", filename);
	fprintf(fh, "Time and Date: %s\n", ctime(&ct));

	/* Create arrays large enough to store values */
	a = calloc(BEST_S_NUM, sizeof(double));
	b = calloc(BEST_S_NUM, sizeof(double));
	alpha = calloc(BEST_S_NUM, sizeof(double));
	dr = calloc(BEST_S_NUM, sizeof(double));
	store_start = calloc(BEST_S_NUM, sizeof(int));
	store_end = calloc(BEST_S_NUM, sizeof(int));

	/* Store data in array */
	wav_data = (float *) calloc(input_info.frames / SPLIT, sizeof(float));

	if (wav_data == NULL) {
		printf("Memory allocation error\nExiting...\n");
		sf_close(input);
		return -1;
	}

	/* Octave band filtering */
	for (band = 0; band < 8; band++) {
		printf("\n\nCalculations of Reverberation Time for %d Hz "
			"octave band.\n", octave_bands[band]);
		fprintf(fh, "\nCalculations of Reverberation Time for %d Hz "
			"octave band.\n", octave_bands[band]);

		array_size = 0;
		current_sz = 0;
		filt_frm_tot = 0;
		filt_frames = 0;

		/* Single channel, so can call this way */
		for (i = 0; i < SPLIT; i++) {
			printf("\nPart %d of %d\n", i + 1, SPLIT);

			long_ret = sf_read_float(input, wav_data,
					floor(input_info.frames / SPLIT));

			/* Process the wav data */
			ret = process_wav_data(band, wav_data, input_info,
				input, long_ret, alpha, a, b,
				dr, store_start, store_end, &filt_frames);

			/* If ret < 0, error */
			/* If >= 0, ret is the size of the returned arrays */
			if (ret < 0) {
				free(wav_data);
				sf_close(input);
				return -1;
			} else {
				array_size += ret;
				current_sz = ret;
				filt_frm_tot += filt_frames;
			}

			for (k = 0; k < current_sz; k++) {
				store_start[k] += (filt_frm_tot - filt_frames);
				store_end[k] += (filt_frm_tot - filt_frames);
			}

			a += current_sz;
			b += current_sz;
			alpha += current_sz;
			dr += current_sz;
			store_start += current_sz;
			store_end += current_sz;

			/* Reset bytes to zero */
			memset(wav_data, 0, sizeof(float) *
				(input_info.frames / SPLIT));
		}

		/* Return the pointers to zero position */
		if (sf_seek(input, 0, SEEK_SET) < 0) {
			printf("Error returning to start of wav!\n"
				"Exiting...\n");
			free(wav_data);
			sf_close(input);
			return ret;
		}

		a -= array_size;
		b -= array_size;
		alpha -= array_size;
		dr -= array_size;
		store_start -= array_size;
		store_end -= array_size;

		/* Compute the reverberation time (RT) */
		ret = compute_rt(samp_freq_per_band[band],
			array_size, store_start, store_end, dr, a, b,
			alpha, &mean_rt, &rt_sd, octave_bands[band], fh);

		fprintf(fh, "=============================================\n");

		/* Reset bytes to zero */
		memset(a, 0, sizeof(double) * BEST_S_NUM);
		memset(b, 0, sizeof(double) * BEST_S_NUM);
		memset(alpha, 0, sizeof(double) * BEST_S_NUM);
		memset(dr, 0, sizeof(double) * BEST_S_NUM);
		memset(store_start, 0, sizeof(int) * BEST_S_NUM);
		memset(store_end, 0, sizeof(int) * BEST_S_NUM);
	}

	/* Clean up */
	sf_close(input);
	free(wav_data);
	free(a);
	free(b);
	free(alpha);
	free(dr);
	free(store_start);
	free(store_end);
	fclose(fh);

	return ret;
}

/*
 * Compute RT for room
 */
int compute_rt(int samp, int array_size, int *store_start, int *store_end,
		double *dr, double *a, double *b, double *alpha,
		double *mean_rt, double *rt_sd, int band, FILE *fh) {
	int n_seg, file_len, pos_to, pos_from, seg_len_n, i, j, k, l_reg, ret_sz;
	int min_5dB_index, min_rtdB_index, nan_count = 0;
	int n_chan = 4 * samp, l = 0, discard = 0;
	double **chan, *chan_opt, *chan_opt_log, sd_tmp;
	double *sd_rev_time, max, min_5dB, min_rtdB, **mat_a, mat_a_mul[2];

	/* SVDLIBC stuff */
	SVDRec svd_mat = svdNewSVDRec();
	DMat dmat_a_mat;
	SVDVerbosity = 0;	/* 1: normal verbosity; 3: very verbose */

	/* Initialise */
	*mean_rt = 0.0;
	*rt_sd = 0.0;

	printf("Computing the Reverberation Time...\n");

	chan = calloc(array_size, sizeof(double *));
	chan_opt = calloc(n_chan, sizeof(double));
	chan_opt_log = calloc(n_chan, sizeof(double));

	for (i = 0; i < array_size; i++)
		chan[i] = calloc(n_chan, sizeof(double));

	/* Find the end position of the last decay phase by finding max
	 *	of store_end[] */
	file_len = store_end[0];
	for (i = 1; i < array_size; i++) {
		if (store_end[i] > file_len)
			file_len = store_end[i];
	}

	/* seg_len_n is the number of samples in the length of time
	 *	chosen, seg_len. n_seg is the number of chunks, For
	 *	example, if seg_len_val = 180 secs and file_len = 6200
	 *	samples, then n_seg = 3 chunks (2 chunks of 180 secs and
	 *	1 shorter chunk). */
	seg_len_n = seg_len_val * samp;
	n_seg = ceil((double)file_len / seg_len_n);

	/* Create array to store standard deviation values */
	sd_rev_time = calloc(n_seg, sizeof(double));

	for (i = 0; i < n_seg; i++) {
		/* Get start and end positions of each chunk */
		pos_from = i * seg_len_n;
		pos_to = (i+1) * seg_len_n - 1;

		/* In case of partial chunk */
		if (i == n_seg - 1)
			pos_to = file_len;

		k = 0;
		for (j = 0; j < array_size; j++) {
			/* Find decay rates < -25 dB */
			if (dr[j] < -25.0) {
				/* Does decay rate occur in this chunk? */
				if (store_start[j] >= pos_from &&
						store_end[j] < pos_to) {
					//printf("DR: %lf\n", dr[j]);
					optimum_model(a[j], b[j], alpha[j], dr[j],
							samp, n_chan,
							&chan[k][0]);

					k++;
					l++;
				}
			}
		}

		printf("Segment: %d; Num Valid Decays: %d\n", i, k);
		fprintf(fh, "Segment: %d; Num Valid Decays: %d\n", i, k);

		if (k <= DISCARD_THRESH) {
			discard++;
			continue;
		}

		ret_sz = optimum_model_2(chan, n_chan, k, samp, chan_opt);

		/* Get cumulative sum */
		chan_opt_log[ret_sz - 1] = pow(chan_opt[ret_sz - 1], 2);
		for (j = ret_sz - 2; j >= 0; j--) {
			chan_opt_log[j] = pow(chan_opt[j], 2) +
				chan_opt_log[j + 1];
		}

		// DEBUG note: chan_opt_log[0] close to matlab

		/* Get max before getting log */
		max = fabs(chan_opt_log[0]);
		for (j = 1; j < ret_sz; j++) {
			if (fabs(chan_opt_log[j]) > max)
				max = fabs(chan_opt_log[j]);
		}

		for (j = 0; j < ret_sz; j++) {
			/* Convert to decibels */
			chan_opt_log[j] = 10 * log10(chan_opt_log[j] / max);

			/* Find -5 dB and -RT_DECAY dB decay points */
			if (j == 0) {
				min_5dB = fabs(chan_opt_log[0] - (-5));
				min_rtdB = fabs(chan_opt_log[0] - (-RT_DECAY));
				min_5dB_index = 0;
				min_rtdB_index = 0;
			} else {
				if (fabs(chan_opt_log[j] - (-5)) < min_5dB) {
					min_5dB = fabs(chan_opt_log[j] - (-5));
					min_5dB_index = j;
				}

				if (fabs(chan_opt_log[j]-(-RT_DECAY)) < min_rtdB) {
					min_rtdB = fabs(chan_opt_log[j] -
						(-RT_DECAY));
					min_rtdB_index = j;
				}
			}
		}

		l_reg = min_rtdB_index - min_5dB_index + 1;

		/* Allocate space for mat_a */
		mat_a = calloc(l_reg, sizeof(double *));
		for (k = 0; k < l_reg; k++)
			mat_a[k] = calloc(2, sizeof(double));

		/* Get the RT */
		/* Note: due to (possible bug?) in SVDLIBC, I need to normalise
			the 2nd column of the matrix. I will multiply the RT
			at the end to compensate for the normalisation */
		for (j = 0; j < l_reg; j++) {
			mat_a[j][0] = 1.0;
			mat_a[j][1] = ((double) (min_5dB_index + j)) /
				min_rtdB_index;
		}

		/* Get the SVD of mat_a */
		dmat_a_mat = svdNewDMat(l_reg, 2);
		dmat_a_mat->value = mat_a;

		svd_mat = svdLAS2A(svdConvertDtoS(dmat_a_mat), 0);

		/* Get reciprocal of S */
		for (k = 0; k < 2; k++)
			svd_mat->S[k] = 1.0 / svd_mat->S[k];

		/* Multiply modified S val by V */
		for (k = 0; k < 2; k++)
			for (j = 0; j < 2; j++)
				svd_mat->Vt->value[k][j] *= svd_mat->S[k];

		/* Matrix multiplcation of modified V with U */
		for (j = 0; j < l_reg; j++) {
			mat_a[j][0] = svd_mat->Ut->value[0][j] *
				svd_mat->Vt->value[0][0];

			mat_a[j][1] = svd_mat->Ut->value[1][j] *
				svd_mat->Vt->value[1][1];

			mat_a[j][0] += svd_mat->Ut->value[1][j] *
					svd_mat->Vt->value[1][0];

			mat_a[j][1] += svd_mat->Ut->value[0][j] *
					svd_mat->Vt->value[0][1];

			// DEBUG: MATLAB matches output when same values are
			//	passed to Matlab
        	}

		/* Multiply pseudoinverse by sig */
		mat_a_mul[0] = 0.0;
		mat_a_mul[1] = 0.0;
		for (j = 0; j < l_reg; j++) {
			mat_a_mul[0] += mat_a[j][0] *
				chan_opt_log[j + min_5dB_index];
			mat_a_mul[1] += mat_a[j][1] *
				chan_opt_log[j + min_5dB_index];
		}

		/*
		 * If RT for segment != nan, add to rev_time
		 */
		if(!isnan(-60.0 / (samp * mat_a_mul[1]))) {
			sd_rev_time[i - nan_count - discard] =
				((double) (-60.0 * min_rtdB_index)) /
				(samp * mat_a_mul[1]);

			*mean_rt += sd_rev_time[i - nan_count - discard];
		} else
			nan_count++;

		/* Free array */
		for (k = 0; k < l_reg; k++)
			free(mat_a[k]);

		free(mat_a);
	}

	printf("n_seg: %d; nan_count: %d; discarded seg: %d\n",
		n_seg, nan_count, discard);
	printf("There are %d decays of at least -25 dB.\n", l);
	fprintf(fh, "n_seg: %d; nan_count: %d; discarded seg: %d\n",
		n_seg, nan_count, discard);
	fprintf(fh, "There are %d decays of at least -25 dB.\n", l);

	/* Get mean RT and standard dev of RT if num of valid RTs > 1 */
	if (n_seg - nan_count - discard > 1) {
		*mean_rt /= (n_seg - nan_count - discard);

		for (sd_tmp = 0.0, i = 0; i < n_seg - nan_count - discard; i++)
			sd_tmp += pow(fabs(sd_rev_time[i] - *mean_rt), 2);

		*rt_sd = sqrt(sd_tmp / (n_seg - nan_count - discard - 1));

		/* Get standard error from std dist (97.5 percentile) */
		*rt_sd = (*rt_sd / sqrt(n_seg - nan_count - discard)) * 1.96;
	}

	printf("Mean RT for %d band: %lf\n", band, *mean_rt);
	printf("RT Standard Dev for %d band: %lf\n", band, *rt_sd);
	fprintf(fh, "Mean RT for %d band: %lf\n", band, *mean_rt);
	fprintf(fh, "RT Standard Dev for %d band: %lf\n\n", band, *rt_sd);

	/* Free stuff */
	for (i = 0; i < array_size; i++)
		free(chan[i]);

	free(chan);
	free(chan_opt);
	free(chan_opt_log);
	free(sd_rev_time);
	svdFreeSVDRec(svd_mat);

	return 0;
}

int optimum_model(double a, double b, double alpha, double dr, int samp, int n,
		double *chan_2) {
	int i;
	double a_pow, b_pow, max;

	for (i = 0; i < n; i++) {
		a_pow = pow(a, i);
		b_pow = pow(b, i);
		chan_2[i] = alpha * a_pow + (1 - alpha) * b_pow;

		if (i == 0)
			max = fabs(chan_2[i]);
		else {
			if (fabs(chan_2[i]) > max)
				max = fabs(chan_2[i]);
		}
	}

	for (i = 0; i < n; i++)
		chan_2[i] /= max;

	return 0;
}

int optimum_model_2(double **chan, int n, int chan_sz, int samp, double *lin) {
	double winn = 0.1;
	double over = 0.5;
	int nn = winn * samp;
	int i, j, k, n1, min_index, l_reg;
	int n0 = floor((1.0 - over) * nn);
	int n_sect = floor(((double)n - nn) / n0);
	double n2, min, chan_sum[chan_sz], *last;
	double win[nn * n_sect];

	last = calloc(n, sizeof(double));

	for (i = 0, n1 = 0; i < n_sect; i++) {
		n2 = n1 + nn;
		l_reg = n2 - n1;

		for (j = 0; j < chan_sz; j++) {
			chan_sum[j] = 0;
			for (k = n1; k < n2; k++)
				chan_sum[j] += pow(chan[j][k], 2);

			/* Get the minimum */
			if (j == 0) {
				min = chan_sum[0];
				min_index = 0;
			}
			else {
				if (chan_sum[j] < min) {
					min = chan_sum[j];
					min_index = j;
				}
			}
		}

		// DEBUG note: min and min_index match matlab 250 Hz

		if (i == 0) {
			for (k = 0; k < n; k++) {
				if (k >= n1 && k < n2)
					lin[k] = chan[min_index][k];

				last[k] = chan[min_index][k];
			}
		} else {
			hanning(l_reg, win);

			for (k = l_reg / 2 - 1; k < l_reg; k++)
				win[k] = 1.0;

			for (k = 0; k < n; k++) {
				if (k >= n1 && k < n2) {
					lin[k] = last[k] * fabs(win[k] - 1.0) +
						chan[min_index][k] * win[k];
				}

				last[k] = chan[min_index][k];
			}
		}
		n1 += n0;
	}
	//DEBUG note: lin[0-9] matches MATLAB

	free(last);
	return (int) n2;
}

void hanning(int len, double *hann) {
	int n;

	for (n = 0; n < len; n++)
		hann[n] = 0.5 * (1.0 - cos(2.0 * M_PI * n / (len - 1.0)));

	return;
}

int process_wav_data(int band, float *wav_data, SF_INFO input_info,
		SNDFILE *input, unsigned long long frames, double *p_alpha,
		double *p_a, double *p_b, double *p_dr,
		int *p_start, int *p_end, int *filt_frames) {
	int i, ret, s_e_size, resamp_frames = 0, **start_end;
	float *resampled_wav, *filtered_wav, *env;
	double *alpha, *a, *b, *dr;
	int *store_start, *store_end;

	/* Specifications that apply to all subbands */
	SwrContext *resamp = swr_alloc();

	av_opt_set_int(resamp, "in_channel_layout", AV_CH_LAYOUT_MONO, 0);
	av_opt_set_int(resamp, "out_channel_layout", AV_CH_LAYOUT_MONO, 0);
	av_opt_set_int(resamp, "in_sample_rate", input_info.samplerate, 0);
	av_opt_set_sample_fmt(resamp, "in_sample_fmt", AV_SAMPLE_FMT_FLT, 0);
	av_opt_set_sample_fmt(resamp, "out_sample_fmt", AV_SAMPLE_FMT_FLT, 0);

	av_opt_set_int(resamp, "out_sample_rate",
		samp_freq_per_band[band], 0);

	ret = swr_init(resamp);

	if (ret < 0) {
		printf("Resample initialisation error\nExiting...\n");
		return -1;
	}

	/* Resample the wav file */
	resampled_wav = resamp_wav_data(resamp, input_info.samplerate,
		(uint64_t) frames, samp_freq_per_band[band],
		(const uint8_t **) &wav_data, &resamp_frames);

	swr_free(&resamp);
	if (resamp_frames <= 0 || resampled_wav == NULL) {
		printf("Resample error\nExiting...\n");
		return -1;
	}

	*filt_frames = resamp_frames;

	/* Octave band filtering */
	filtered_wav = oct_filt_data(resampled_wav,
		band, (float) samp_freq_per_band[band],
		resamp_frames);

	/* Free memory no longer needed */
	av_free((uint8_t **) &resampled_wav[0]);
	resampled_wav = NULL;

	/* Obtain signal envelope */
	env = get_envelope((float) samp_freq_per_band[band], filtered_wav,
		resamp_frames);

	/* Polyfit algorithm & get decay segments */
	start_end = apply_polyfit((float) samp_freq_per_band[band],
		resamp_frames, env, &s_e_size);

	/* Perform Maximum Likelihood Estimation */
	ret = perform_ml(start_end, env, s_e_size, filtered_wav,
		resamp_frames, samp_freq_per_band[band], &alpha, &a, &b, &dr,
		&store_start, &store_end);

	if (ret >= 0) {
		printf("Maximum Likelihood calculations have completed!\n%d "
			"potential segments analysed, %d may be valid\n",
			s_e_size, ret);

		/* Return pointers to caller */
		for (i = 0; i < ret; i++) {
			*(p_alpha + i) = alpha[i];
			*(p_a + i) = a[i];
			*(p_b + i) = b[i];
			*(p_dr + i) = dr[i];
			*(p_start + i) = store_start[i];
			*(p_end + i) = store_end[i];
		}
	}

	/* Free memory no longer needed */
	free(filtered_wav);
	filtered_wav = NULL;

	free(env);
	env = NULL;

	for (i = 0; i < 2; i++)
		free(start_end[i]);
	free(start_end);
	start_end = NULL;

	free(alpha);
	alpha = NULL;
	free(a);
	a = NULL;
	free(b);
	b = NULL;
	free(dr);
	dr = NULL;
	free(store_start);
	store_start = NULL;
	free(store_end);
	store_end = NULL;

	/* Return */
	if (ret < 0)
		return -1;
	else
		return ret;
}

float * resamp_wav_data(SwrContext *resamp, int in_rate, uint64_t num_frames, int samp_freq, const uint8_t **wav_data, int *resamp_frms) {
	uint8_t *output_data;
	int ret, resamp_frames;

	printf("Resampling wav...\n");
	resamp_frames = av_rescale_rnd(swr_get_delay(resamp, in_rate) +
		num_frames, samp_freq, in_rate, AV_ROUND_UP);

	ret = av_samples_alloc(&output_data, NULL, 1, resamp_frames,
		AV_SAMPLE_FMT_FLT, 0);

	if (ret < 0) {
		printf("Output buffer alloc failure\nExiting...\n");
		return NULL;
	}

	resamp_frames = swr_convert(resamp, &output_data, resamp_frames,
		wav_data, num_frames);

	if (resamp_frames < 0) {
		printf("Resampling failure\nExiting...\n");
		av_free(&output_data[0]);
		return NULL;
	}

	/* Store # frames in pointer */
	*resamp_frms = resamp_frames;

	/* Return pointer to beginning of data */
	return (float *) output_data;
}

float * oct_filt_data(float *resamp_data, int band_num, float samp_freq, int resamp_frames) {
	int i, flag = 0;
	float *filtered = calloc(resamp_frames, sizeof(float));
	float band = octave_bands[band_num];

	memset(butt_b, 0, sizeof(butt_b));
	memset(butt_a, 0, sizeof(butt_a));

	printf("Applying octave band filter...\n");

#ifdef USE_HARD_BUTTER
	/* Only accurate when numbers are not tiny */
	if (fabs(butter_b[band_num][0]) > 1e-5)
		flag = 1;
#endif

	if (flag == 1) {
		iirfilt_rrrf f_obj_oct1 = iirfilt_rrrf_create(
			butter_b[band_num], 2 * OCT_FILT_ORDER + 1,
			butter_a[band_num], 2 * OCT_FILT_ORDER + 1);

		/* Filter data */
		for (i = 0; i < resamp_frames; i++)
			iirfilt_rrrf_execute(f_obj_oct1, resamp_data[i],
				&filtered[i]);

		/* Free filter and reset coefficient arrays */
		iirfilt_rrrf_destroy(f_obj_oct1);
	} else {
		/* Create bandpass filter - 7th and 8th parameter are ignored */
		liquid_iirdes(LIQUID_IIRDES_BUTTER, LIQUID_IIRDES_BANDPASS,
			LIQUID_IIRDES_SOS, OCT_FILT_ORDER,
			band / (sqrt(2.0) * samp_freq),
			band / samp_freq, 1.0, 1.0, butt_b, butt_a);
		//printf("res: %d\n\n", iirdes_isstable(butt_b, butt_a,
		//	3 * (L_OCT + R_OCT)));
		iirfilt_rrrf f_obj_oct2 = iirfilt_rrrf_create_sos(
			butt_b, butt_a, L_OCT + R_OCT);

		/* Filter data */
		for (i = 0; i < resamp_frames; i++)
			iirfilt_rrrf_execute(f_obj_oct2, resamp_data[i],
				&filtered[i]);

		/* Free filter and reset coefficient arrays */
		iirfilt_rrrf_destroy(f_obj_oct2);
	}

	/* fiiltered data: return to previous function */
	return filtered;
}

float * get_envelope(float samp_freq, float *filtered_wav,
		int resamp_frames) {
	float complex *filt_complex = calloc(resamp_frames, sizeof(float complex));
	float complex *tmp = calloc(resamp_frames, sizeof(float complex));
	float complex *tmp2 = calloc(resamp_frames, sizeof(float complex));
	float complex *hilb = calloc(resamp_frames, sizeof(float complex));
	float *env = calloc(resamp_frames, sizeof(float));
	int i, j;

	//FIXME add calloc NULL check

	memset(butt_b, 0, sizeof(butt_b));
	memset(butt_a, 0, sizeof(butt_a));

	/* Create Low Pass Filter - 6th, 7th and 8th param are ignored */
	printf("Obtaining signal envelope...\n");
	liquid_iirdes(LIQUID_IIRDES_BUTTER, LIQUID_IIRDES_LOWPASS,
		LIQUID_IIRDES_TF, ENV_FILT_ORDER,
		LOW_PASS_CUTOFF / samp_freq, 0.1,
		1.0, 1.0, butt_b, butt_a);

	iirfilt_crcf f_obj_env = iirfilt_crcf_create(butt_b,
		ENV_FILT_ORDER+1, butt_a, ENV_FILT_ORDER+1);

	fftplan fft_pln = fft_create_plan(resamp_frames, (float complex *)
		filt_complex, (float complex *) tmp, LIQUID_FFT_FORWARD, 0);
	fftplan ifft_pln = fft_create_plan(resamp_frames, (float complex *) tmp,
 		(float complex *) hilb, LIQUID_FFT_BACKWARD, 0);

	/* Get the Hilbert transform - really it's the analytic signal as the
		Hilbert transform is stored in the imaginary part of the soln */
	for (i = 0; i < resamp_frames; i++)
		filt_complex[i] = (float complex) filtered_wav[i];

	fft_execute(fft_pln);

	for (i = 0; i < resamp_frames; i+= 1) {
		j = i + 1;

		if (resamp_frames % 2 == 0) {
			/* Even number of samples */
			/* Set if value should be non-zero (array contains zeros)*/
			if (j == 1 || j == ((resamp_frames / 2) + 1))
				tmp2[i] = 1.0;
			else if (j >= 2 && j <= (resamp_frames / 2))
				tmp2[i] = 2.0;
		} else {
			/* Odd number of samples */
			/* Set if value should be non-zero (array contains zeros)*/
			if (j == 1)
				tmp2[i] = 1.0;
			else if (j >= 2 && j <= (resamp_frames + 1) / 2)
				tmp2[i] = 2.0;
		}

		tmp[i] *= tmp2[i];
	}

	fft_execute(ifft_pln);

	for (i = 0; i < resamp_frames; i++) {
		/* Normalisation correction */
		hilb[i] *= (1.0 / resamp_frames);

		/* Get envelope */
		hilb[i] = csqrtf(filt_complex[i] * filt_complex[i] +
			hilb[i] * hilb[i]);

		/* Apply low pass filter to envelope */
		iirfilt_crcf_execute(f_obj_env, hilb[i], &tmp[i]);

		env[i] = cabsf(tmp[i]);
	}

	free(tmp2);
	free(tmp);
	free(hilb);
	free(filt_complex);
	iirfilt_crcf_destroy(f_obj_env);
	fft_destroy_plan(ifft_pln);
	fft_destroy_plan(fft_pln);

	/* envelope pointer: return to previous function */
	return env;
}

int ** apply_polyfit(float samp_freq, int resamp_frames, float *env, int *s_e_size) {
	int i, j, seg_size, step_size, num_segs, poly_coeff = 2, dump = 0;
	int **seg_index, invalid_coeff = 0, actual_seg_num_els = 0, max = 0;
	int **start_end;
	float *poly_param, **poly_res, *poly_seg, max_poly_res, min_poly_res;
	float *var;
	float *log_env = calloc(resamp_frames, sizeof(float));
	//FIXME: add calloc NULL check

	printf("Applying polyfit algorithm...\n");

	for (i = 0; i < resamp_frames; i++)
		log_env[i] = log10f(env[i]);

	seg_size = (int) floorf(samp_freq * DAMP_SEG_SIZE);

	step_size = (int) floorf(samp_freq * DAMP_STEP_SIZE);

	num_segs = (int) floorf((resamp_frames - seg_size) / step_size + 1);

	//FIXME: add calloc NULL check
	poly_param = calloc(seg_size, sizeof(float));
	poly_res = calloc(num_segs, sizeof(float *));
	poly_seg = calloc(seg_size, sizeof(float));
	var = calloc(num_segs, sizeof(float));

	for (i = 0; i < num_segs; i++)
		poly_res[i] = calloc(poly_coeff, sizeof(float));

	/* Fill in Polyfit function parameter */
	for (i = 0; i < seg_size; i++)
		poly_param[i] = i + 1;

	max_poly_res = -log10f(exp(6.91 / MAX_RT / samp_freq));
	min_poly_res = -log10f(exp(6.91 / MIN_RT / samp_freq));

	for (i = 0; i < num_segs; i++) {
		for (j = i * step_size; j < i * step_size + seg_size; j++)
			poly_seg[j - i * step_size] = log_env[j];

		polyf_fit(poly_param, poly_seg, seg_size, &poly_res[i][0],
			poly_coeff);

		//TODO: CHECK MATLAB POLYFIT COEFFS
		//TODO: CHECK MATLAB VALUES UP TO HERE
		//DONE ABOVE - SEEMED FINE BUT POLY COEFFS REVERESED...
//		for (j = 0; j < poly_coeff; j++) {
//			printf("%lf", poly_res[i][j]);
//			if (j == 0)
//				printf(",");
//			else
//				printf("\n");
//		}

		for (j = 0; j < poly_coeff; j++) {
			//FIXME eeeh floating point comparisons are DODGEY in C
			if (poly_res[i][j] < min_poly_res ||
				poly_res[i][j] > max_poly_res) {
				invalid_coeff++;
			}
		}
		if (invalid_coeff  == poly_coeff) {
			var[i] = -1.0;
			dump++;
		}
		invalid_coeff = 0;
	}

	/* Choose segments */
	seg_index = choose_segments(var, step_size, num_segs, num_segs - dump,
		&actual_seg_num_els);

	/* Number of decay segments should not be greater than BEST_S_NUM */
	if (actual_seg_num_els > BEST_S_NUM)
		actual_seg_num_els = BEST_S_NUM;

	start_end = calloc(2, sizeof(int *));
	for (i = 0; i < 2; i++)
		start_end[i] = calloc(actual_seg_num_els, sizeof(int));

	for (i = 0; i < actual_seg_num_els; i++) {
		max = 0;
		for (j = 0; j < actual_seg_num_els; j++) {
			if (seg_index[j][1] > seg_index[max][1])
				max = j;
		}

		start_end[0][i] = seg_index[max][0] * step_size;
		start_end[1][i] = seg_index[max][0] * step_size +
			seg_size + seg_index[max][1] * step_size - 1;

		/* Remove longest segment - it has been chosen */
		seg_index[max][1] = 0;

	}

	/* Free arrays */
	for (i = 0; i < num_segs; i++)
		free(poly_res[i]);
	free(poly_res);

	for (i = 0; i < num_segs - dump; i++)
		free(seg_index[i]);
	free(seg_index);
	seg_index = NULL;

	free(poly_seg);
	free(var);
	free(poly_param);
	free(log_env);

	*s_e_size = actual_seg_num_els;

	return start_end;
}

int ** choose_segments(float *var, int step_size, int len, int not_dumped,
		int *seg_num_els) {
	int i, k, j = 0;
	int **seg_index;

	/* Max possible size of seg_index */
	seg_index = calloc(not_dumped, sizeof(int *));

	for (i = 0; i < not_dumped; i++)
		seg_index[i] = calloc(2, sizeof(int));

	/* Set decay segments and number of continuous segments in array */
	for (i = 0; i < len - 1; i += k) {
		k = 1;

		/* Floating point method of checking if var[i] == 0 */
		if (fabs(var[i]) < 0.0001) {
			seg_index[j][0] = i;
			seg_index[j][1] = 0;

			while (((i + k) < len) && fabs(var[i + k]) < 0.0001) {
				seg_index[j][1]++;
				k++;
			}
			j++;
			k += step_size;
		}
	}

	*seg_num_els = j;
	/* FIXME: don't free seg_index... what am I supposed to do about it? */
	return seg_index;
}

int perform_ml(int **start_end, float *env, int s_e_size, float *filtered_wav,
		int resamp_frames, int samp_freq, double **p_alpha,
		double **p_a_par, double **p_b_par, double **p_dr,
		int **p_start, int **p_end) {
	int sz, i, all_ret = 0;
	float *abs_filtered = calloc(resamp_frames, sizeof(float));
	int *store_start = calloc(s_e_size, sizeof(int));
	int *store_end = calloc(s_e_size, sizeof(int));
	int *len_store = calloc(s_e_size, sizeof(int));
	double *a_par = calloc(s_e_size, sizeof(double));
	double *b_par = calloc(s_e_size, sizeof(double));
	double *alpha = calloc(s_e_size, sizeof(double));
	double *dr = calloc(s_e_size, sizeof(double));

	printf("Performing Maximum Likelihood calculations\n"
		"This will take a while...\n");

	/* Get absolute vaule of filtered wav */
	for (i = 0; i < resamp_frames; i++)
		abs_filtered[i] = fabs(filtered_wav[i]);

	/* Largest possible size of arrays - first pair of elements */
	sz = start_end[1][0] - start_end[0][0];

	#pragma omp parallel for schedule(static)
	for (i = 0; i < s_e_size; i++) {
		float *segment = calloc(sz, sizeof(float));
		float *env_seg = calloc(sz, sizeof(float));
		float *abs_filt_seg = calloc(sz, sizeof(float));
		float *seg_seg2 = calloc(sz + sz - roundf((DAMP_SEG_SIZE / 2) *
			samp_freq), sizeof(float));
		float max_seg2;
		int k, j, len = abs(start_end[1][i] - start_end[0][i]);
		int min_abs_filt_seg, max_abs_filt_seg, ret = 0;

		/* Store decay segment in new array */
		for (k = 0, j = start_end[0][i]; j < start_end[1][i]; j++, k++)
		{
			segment[k] = filtered_wav[j];
			env_seg[k] = env[j];
			abs_filt_seg[k] = abs_filtered[j];
		}
		// DEBUG: above values match matlab!

		/* Get location of minimum */
		min_abs_filt_seg = len - round((DAMP_SEG_SIZE / 2) *
			samp_freq) - 1;

		for (j = min_abs_filt_seg; j < len; j++) {
			if (abs_filt_seg[j] < abs_filt_seg[min_abs_filt_seg])
				min_abs_filt_seg = j;
		}

		/* Get location of maximum */
		max_abs_filt_seg = 0;
		for (j = 0; j < round((DAMP_SEG_SIZE / 2) * samp_freq); j++) {
			if (abs_filt_seg[j] > abs_filt_seg[max_abs_filt_seg])
				max_abs_filt_seg = j;
		}

		/* Store new fine-tuned decay segment */
		for (k = 0, j = max_abs_filt_seg; j <= min_abs_filt_seg; k++,
			j++)
			seg_seg2[k] = segment[j];

		/* NB: len_store[] contains INDEX to last element, not # of
			elements in array */
		len_store[i] = k - 1;

		k = 0;
		for (j = 0; j <= len_store[i]; j++) {
			if (fabs(seg_seg2[j]) > fabs(seg_seg2[k]))
				k = j;
		}

		max_seg2 = fabs(seg_seg2[k]);
		for (j = 0; j <= len_store[i]; j++)
			seg_seg2[j] /= max_seg2;

		/* Save fine-tuned start and end locations */
		// FIXME: are the -1's OK here? Compare to MATLAB
		store_start[i] = start_end[0][i] + max_abs_filt_seg;
		store_end[i] = start_end[0][i] + max_abs_filt_seg +
				len_store[i] - 1;

		/* ML fitting of decay model to the data */
		ret = ml_fit(seg_seg2, len_store[i], (float) samp_freq,
			&a_par[i], &b_par[i], &alpha[i]);

		free(segment);
		free(seg_seg2);
		free(env_seg);
		free(abs_filt_seg);

		if (ret == -1) {
			/* DR being discarded */
			dr[i] = 0;
			continue;
		} else if (ret < -1) {
			/* Error */
			all_ret = -1;
		}

		/* If code gets to here, DR was possibly valid */
		/* Compute decay curves using ML-calculated params */
		dr[i] = get_decay_region(6 * samp_freq, a_par[i], b_par[i],
			alpha[i], len_store[i]);
	}

	/* Pointers used by caller cannot be freed. They are alpha[], a[],
		b[], store_start[], store_end[] and dr[] */
	*p_alpha = alpha;
	*p_a_par = a_par;
	*p_b_par = b_par;
	*p_dr = dr;
	*p_start = store_start;
        *p_end = store_end;

	/* Free pointers that are no longer needed */
	free(abs_filtered);
	free(len_store);

	if (all_ret != 0) {
		printf("Error during ML calculations!\n");
		return -1;
	} else
		return s_e_size;
}

double get_decay_region(int c_len, double a, double b, double alpha,
		int len_store) {
	double *chan = calloc(c_len, sizeof(double));
	double *y = calloc(c_len, sizeof(double));
	double dr;
	int j, max_y;

	/* Compute decay curves using ML-calculated params */
	for (j = 0; j < c_len; j++)
		chan[j] = alpha * pow(a, j) + (1.0 - alpha) * pow(b, j);

	/* Get the reverse cumsum of chan[] and assign to y in reverse */
	y[c_len - 1] = pow(chan[c_len - 1], 2);
	for (j = c_len - 2; j >= 0; j--)
		y[j] = pow(chan[j], 2) + y[j + 1];

	/* Get max of fabs(y) */
	max_y = fabs(y[0]);
	for (j = 1; j < c_len; j++) {
		if (fabs(y[j]) > max_y)
			max_y = fabs(y[j]);
	}

	/* Normalise y first value will be max; stores whole cumsum */
	for (j = 0; j < c_len; j++)
		y[j] = 10.0 * log10(y[j] / max_y);

	dr = y[len_store];

	free(chan);
	free(y);

	return dr;
}

int ml_fit(float *data_seg, int len, float samp_freq, double *a_par,
		double *b_par, double *alph_par) {
	double min, max, interval, j, gmax_val, x_fine[3], fine_val, dr;
	double *coarse_grid, lb[3], ub[3];
	double like[SQP_STEP][SQP_STEP] = {0}, alpha[SQP_STEP][SQP_STEP] = {0};
	int gmax_pos[2], i, k, el_cg, ret = 0;
	struct nl_extra_data nld;

	lb[0] = 0.0;
	ub[0] = 1.0;

	nld.data_seg = data_seg;
	nld.len = len;

	nlopt_opt nl_obj1 = nlopt_create(NLOPT_LN_COBYLA, 1);
	nlopt_set_lower_bounds1(nl_obj1, lb[0]);
	nlopt_set_upper_bounds1(nl_obj1, ub[0]);
	nlopt_set_maxeval(nl_obj1, 100);
	nlopt_set_min_objective(nl_obj1, (nlopt_func) alpha_opt, (void *) &nld);

	min = (-6.91 / log(PAR_LOW_BOUND)) / 3000.0;
	min = exp(-6.91 / (samp_freq * min));

	max = (-6.91 / log(PAR_UP_BOUND)) / 3000.0;
	max = exp(-6.91 / (samp_freq * max));

	interval = (max - min) / ((double) SQP_STEP - 1.0);
	el_cg = round((max - min) / interval) + 1;
	coarse_grid = calloc(el_cg, sizeof(double));

	/* Fill in coarse grid */
	for (i = 0, j = min; i < el_cg; i++, j += interval)
		coarse_grid[i] = j;

	// There is another max(abs(x)) division here in MATLAB
	// 	Makes no diff - dividing by 1.0 - leaving out

	/* Coarse grid minimisation search */
	for (i = 0; i < SQP_STEP; i++) {
		nld.b_val = coarse_grid[i];

		for (k = 0; k < SQP_STEP; k++) {
			alpha[k][i] = 0.5;
			nld.a_val = coarse_grid[k];

			ret = nlopt_optimize(nl_obj1, &alpha[k][i],
				&like[k][i]);

			/* -4 is an acceptable nlopt return code */
			if (ret < 0 && ret != -4) {
				nlopt_destroy(nl_obj1);
				free(coarse_grid);
				return -2;
			}

			like[k][i] *= -1;
		}
	}

	for (i = 0; i < SQP_STEP; i++) {
		for (k = 0; k < SQP_STEP; k++) {
			/* Store value and position ([][]) of global max */
			if (i == 0 && k == 0) {
				gmax_val = like[k][i];
				gmax_pos[0] = k;
				gmax_pos[1] = i;
			} else {
				if (like[k][i] > gmax_val) {
					gmax_val = like[k][i];
					gmax_pos[0] = k;
					gmax_pos[1] = i;
				}
			}

			//printf("k: %d, i: %d, ret: %d, a: %lf like: %le coarse: %lf\n", k+1, i+1, ret, alpha[k][i], like[k][i], coarse_grid[k]);
		}
	}

	/* No longer need nl_obj1; coarse search is done */
	nlopt_destroy(nl_obj1);

#ifdef COARSE_DR_EST
	dr = get_decay_region(6 * samp_freq, coarse_grid[gmax_pos[0]],
		coarse_grid[gmax_pos[1]], alpha[gmax_pos[0]][gmax_pos[1]], len);

	if (dr > -1.0) {
		free(coarse_grid);
		return -1;
	}
#endif

	/* Create new nlopt object for fine search */
	nlopt_opt nl_obj3 = nlopt_create(NLOPT_LN_COBYLA, 3);

	lb[0] = lb[1] = coarse_grid[0];
	ub[0] = ub[1] = coarse_grid[el_cg - 1];
	lb[2] = 0.0;
	ub[2] = 1.0;

	nlopt_set_lower_bounds(nl_obj3, lb);
	nlopt_set_upper_bounds(nl_obj3, ub);
	nlopt_set_maxeval(nl_obj3, 300);
	nlopt_set_min_objective(nl_obj3, (nlopt_func) par_3_opt, (void *) &nld);

	/* Initial values for fine search */
	x_fine[0] = coarse_grid[gmax_pos[0]];
	x_fine[1] = coarse_grid[gmax_pos[1]];
	x_fine[2] = alpha[gmax_pos[0]][gmax_pos[1]];
	//printf("B4 XVAL: %lf %lf %lf %le\n", x_fine[0], x_fine[1], x_fine[2], gmax_val);

	ret = nlopt_optimize(nl_obj3, x_fine, &fine_val);
	//printf("DEBUG FINE RESULTS - ret: %d XVAL: %lf %lf %lf FINE_VAL: %le\n",
		//ret, x_fine[0], x_fine[1], x_fine[2], fine_val);

	/* Return a, b, alpha in pointers */
	*a_par = x_fine[0];
	*b_par = x_fine[1];
	*alph_par = x_fine[2];

	free(coarse_grid);
	nlopt_destroy(nl_obj3);

	if (ret < 0 && ret != -4)
		return -2;
	else
		return 0;
}

/* Optimise with respect to alpha */
double alpha_opt(int n, const double *a, double *grad, void *nldv)
{
	/* FIXME ADD THE STUFF IN HERE TO DO WITH READING nld */
	int i;
	double alpha = *a;
	struct nl_extra_data *nld = (struct nl_extra_data *) nldv;
	double sigma_tot = 0, *sigma = calloc(nld->len, sizeof(double));
	double like_a_tot = 0;
	double like_b_tot = 0, *like_b = calloc(nld->len, sizeof(double));

	for (i = 0; i < nld->len; i++) {
		/* Get sigma */
		sigma[i] = alpha * pow(nld->a_val, i) +
			(1.0 - alpha) * pow(nld->b_val, i);

		sigma[i] = -1.0 / pow(sigma[i], 2);

		sigma[i] *= pow(nld->data_seg[i], 2);

		sigma_tot += sigma[i];
	}

	sigma_tot = sqrt(-sigma_tot / ( (double) nld->len ));

	for (i = 0; i < nld->len; i++) {
		/* Get likelihood*/
		like_a_tot += log( alpha * pow(nld->a_val, i) +
			(1.0 - alpha) * pow(nld->b_val, i) );

		like_b[i] = alpha * pow(nld->a_val, i) +
			(1.0 - alpha) * pow(nld->b_val, i);

		like_b[i] = pow(like_b[i], -2);

		like_b[i] *= pow(nld->data_seg[i], 2) /
			(2.0 * pow(sigma_tot, 2));

		like_b_tot += like_b[i];
	}

	like_b_tot = -like_a_tot - like_b_tot -
		(double) nld->len * log(2 * M_PI * pow(sigma_tot, 2)) / 2.0;

	free(like_b);
	free(sigma);

	return -like_b_tot;
}

/* Optimise all three parameters */
double par_3_opt(int n, const double *a, double *grad, void *nldv)
{
	int i;
        struct nl_extra_data *nld = (struct nl_extra_data *) nldv;
	double a_val = a[0], b_val = a[1], alpha = a[2], sigma_tot = 0;
	double *env = calloc(nld->len, sizeof(double));
	double like = 0, x = 0;

	for (i = 0; i < nld->len; i++) {
		env[i] = alpha * pow(a_val, i) + (1.0 - alpha) * pow(b_val, i);

		sigma_tot -= (-1.0 / pow(env[i], 2)) * pow(nld->data_seg[i], 2);
	}

	sigma_tot = sqrt(sigma_tot / (double) nld->len);

	for (i = 0; i < nld->len; i++) {
		like -= log(env[i]);

		x += pow(env[i], -2) * pow(nld->data_seg[i], 2) /
			(2.0 * pow(sigma_tot, 2));
	}

	like -= (x + (double) nld->len * log(2.0 * M_PI *
			pow(sigma_tot, 2)) / 2.0);

	free(env);
	return -like;
}


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

int
main(void)
{
  double data[5] = {17.2, 18.1, 16.5, 18.3, 12.6};
  double mean, variance, largest, smallest, sd,
         rms, sd_mean, median, skew, kurtosis;
  gsl_rstat_workspace *rstat_p = gsl_rstat_alloc();
  size_t i, n;

  /* add data to rstat accumulator */
  for (i = 0; i < 5; ++i)
    gsl_rstat_add(data[i], rstat_p);

  mean     = gsl_rstat_mean(rstat_p);
  variance = gsl_rstat_variance(rstat_p);
  largest  = gsl_rstat_max(rstat_p);
  smallest = gsl_rstat_min(rstat_p);
  median   = gsl_rstat_median(rstat_p);
  sd       = gsl_rstat_sd(rstat_p);
  sd_mean  = gsl_rstat_sd_mean(rstat_p);
  skew     = gsl_rstat_skew(rstat_p);
  rms      = gsl_rstat_rms(rstat_p);
  kurtosis = gsl_rstat_kurtosis(rstat_p);
  n        = gsl_rstat_n(rstat_p);

  printf ("The dataset is %g, %g, %g, %g, %g\n",
         data[0], data[1], data[2], data[3], data[4]);

  printf ("The sample mean is %g\n", mean);
  printf ("The estimated variance is %g\n", variance);
  printf ("The largest value is %g\n", largest);
  printf ("The smallest value is %g\n", smallest);
  printf( "The median is %g\n", median);
  printf( "The standard deviation is %g\n", sd);
  printf( "The root mean square is %g\n", rms);
  printf( "The standard devation of the mean is %g\n", sd_mean);
  printf( "The skew is %g\n", skew);
  printf( "The kurtosis %g\n", kurtosis);
  printf( "There are %zu items in the accumulator\n", n);

  gsl_rstat_reset(rstat_p);
  n = gsl_rstat_n(rstat_p);
  printf( "There are %zu items in the accumulator\n", n);

  gsl_rstat_free(rstat_p);

  return 0;
}


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <gsl/gsl_cblas.h>
#include <lapacke.h>

#include "allvars.h"
#include "proto.h"
/*
 * note that for a matrix A(mxn) in the row-major order, the first dimension is n.
 */

/* C(nxn) = A(nxn)*B(nxn) */
void multiply_mat(double * a, double *b, double *c, int n)
{
  cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, n, n, n, 1.0f
                             , a, n, b, n, 0.0f, c, n);
}
/* C(nxn) = A^T(nxn)*B(nxn) */
void multiply_mat_transposeA(double * a, double *b, double *c, int n)
{
  cblas_dgemm(CblasRowMajor, CblasTrans, CblasNoTrans, n, n, n, 1.0f
                             , a, n, b, n, 0.0f, c, n);
}
/* C(nxn) = A(nxn)*B^T(nxn) */
void multiply_mat_transposeB(double * a, double *b, double *c, int n)
{
  cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasTrans, n, n, n, 1.0f
                             , a, n, b, n, 0.0f, c, n);
}
/* C(m*n) = A(m*k) * B(k*n) */
void multiply_mat_MN(double * a, double *b, double *c, int m, int n, int k)
{
  cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, m, n, k, 1.0f
                             , a, k, b, n, 0.0f, c, n);
}
/* C(m*n) = A^T(m*k) * B(k*n) */
void multiply_mat_MN_transposeA(double * a, double *b, double *c, int m, int n, int k)
{
  cblas_dgemm(CblasRowMajor, CblasTrans, CblasNoTrans, m, n, k, 1.0f
                             , a, m, b, n, 0.0f, c, n);
}
/* C(m*n) = A(m*k) * B^T(k*n) */
void multiply_mat_MN_transposeB(double * a, double *b, double *c, int m, int n, int k)
{
  cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasTrans, m, n, k, 1.0f
                             , a, k, b, k, 0.0f, c, n);
}
/* y(n) = A(nxn)* x(n) */
void multiply_matvec(double *a, double *x, int n, double *y)
{
  cblas_dgemv(CblasRowMajor, CblasNoTrans, n, n, 1.0f, a, n, x, 1, 0.0f, y, 1);
}
/* y(n) = A^T(nxn)* x(n) */
void multiply_matvec_transposeA(double *a, double *x, int n, double *y)
{
  cblas_dgemv(CblasRowMajor, CblasTrans, n, n, 1.0f, a, n, x, 1, 0.0f, y, 1);
}
/* y(m) = A(m, n) * x(n) */
void multiply_matvec_MN(double * a, int m, int n, double *x, double *y)
{
  cblas_dgemv(CblasRowMajor, CblasNoTrans, m, n, 1.0f, a, n, x, 1, 0.0f, y, 1);
}
/* A(mxm)^-1 * B(mxn), store the output in B
 * note that A will be changed on exit. */
int multiply_mat_MN_inverseA(double * a, double *b, int m, int n)
{
  int * ipiv, info;
  ipiv=malloc(m*sizeof(int));

  info=LAPACKE_dgetrf(LAPACK_ROW_MAJOR, m, m, a, m, ipiv);
  if(info!=0)
  {
    strcpy(str_error_exit, "multiply_mat_MN_inverseA 1.\n this usually caused by improper nc.\n increase the low limit of nc");
    error_exit(9);
  }
  info = LAPACKE_dgetrs(LAPACK_ROW_MAJOR, 'N', m, n, a, m, ipiv, b, n);
  if(info!=0)
  {
    strcpy(str_error_exit, "multiply_mat_MN_inverseA 2\n this usually caused by improper nc.\n increase the low limit of nc");
    error_exit(9);
  }

  free(ipiv);
  return info;
}
/* A^-1 */
void inverse_mat(double * a, int n, int *info)
{
  int * ipiv;
  ipiv=malloc(n*sizeof(int));

//  dgetrf_(&n, &n, a, &n, ipiv, info);
//  dgetri_(&n, a, &n, ipiv, work, &lwork, info);

  *info = LAPACKE_dgetrf(LAPACK_ROW_MAJOR, n, n, a, n, ipiv);
  if(*info!=0)
  {
    strcpy(str_error_exit, "inverse_mat");
    error_exit(9);
  }
  *info = LAPACKE_dgetri(LAPACK_ROW_MAJOR, n, a, n, ipiv);
  if(*info!=0)
  {
    strcpy(str_error_exit, "inverse_mat");
    error_exit(9);
  }
  free(ipiv);
  return;
}
/* eigen vector and eigen values */
void eigen_sym_mat(double *a, int n, double *val, int *info)
{
    char jobz='V', uplo='U';

/* store the eigenvectors  in a by rows.
 * store the eigenvalues in val in ascending order.
 */
//    dsyev_(&jobz, &uplo, &n, a, &n, val, work, &lwork, info);

/* store the eigenvectors  in a by columns.
 * store the eigenvalues in val in ascending order.
 */
    LAPACKE_dsyev(LAPACK_ROW_MAJOR, jobz, uplo, n, a, n, val);
    return;
}
/* A(nxn) = x^T(n) * x(n) */
void multiply_vec2mat(double * x, double * a, int n)
{
//  cblas_dsyr(CblasRowMajor, CblasUpper, n, 1.0f, x, 1, a, n);
  int i, j;
  for(i=0; i<n; i++)
    for(j=0; j<=i; j++)
    {
      a[i*n+j] = a[j*n+i] = x[i]*x[j];
    }
}
/* determinant of matrix A
 * note that A is changed on exit. */
double det_mat(double *a, int n, int *info)
{
  int *ipiv;
  int i;
  double det;

  ipiv = workspace_ipiv;

//  dgetrf_(&n, &n, a, &n, ipiv, info);
  *info=LAPACKE_dgetrf(LAPACK_ROW_MAJOR, n, n, a, n, ipiv);

  if(*info!=0)
  {
    strcpy(str_error_exit, "det_mat");
    error_exit(9);
  }

  det = 1.0;
  for(i=0; i<n; i++)
  {
    det *= a[i*n+i];
    if (ipiv[i] != i+1)
    {
      det = -det;
    }
  }
  return det;
}
/* natural logarithm of determinant of A
 * note that A is changed on exit. */
double lndet_mat(double *a, int n, int *info)
{
  int * ipiv;
  int i;
  double lndet;
  ipiv=workspace_ipiv;

//  dgetrf_(&n, &n, a, &n, ipiv, info);
  *info=LAPACKE_dgetrf(LAPACK_ROW_MAJOR, n, n, a, n, ipiv);
  if(*info!=0)
  {
    strcpy(str_error_exit, "lndet_mat");
    error_exit(9);
  }
  lndet = 0.0;
  for(i=0; i<n; i++)
  {
      lndet += log(fabs(a[i*n+i]));
  }
  return lndet;
}

/* natural logarithm of determinant of A
 * if any sign of the eigen values is negtive, return sign=-1
 * note that A is changed on exit
 */
double lndet_mat2(double *a, int n, int *info, int *sign)
{
  int * ipiv;
  int i, *sign_all;
  double lndet;
  //ipiv=malloc(n*sizeof(int));
  //sign_all=malloc(n*sizeof(int));
  ipiv = workspace_ipiv;
  sign_all = ipiv + n;

//  dgetrf_(&n, &n, a, &n, ipiv, info);
  *info=LAPACKE_dgetrf(LAPACK_ROW_MAJOR, n, n, a, n, ipiv);
  if(*info!=0)
  {
    strcpy(str_error_exit, "lndet_mat2");
    error_exit(9);
  }

  lndet = 0.0;
  for(i=0; i<n; i++)
  {
    lndet += log(fabs(a[i*n+i]));
    //*sign *= (a[i*n+i]>=0?1:-1);
    sign_all[i] = (a[i*n+i]>=0?1:-1);

    if(ipiv[i]!=i+1)
    {
      //printf("%e %d\n", a[i*n+i], n);
      //ipiv[ipiv[i]] = ipiv[i];
      //*sign *= -1;
      sign_all[i] *= -1;
    }
  }

  for(i=0; i<n;i++)
  {
    if(sign_all[i]==-1)
    {
      *sign=-1;      // if any sign is negtive, return sign=-1
      break;
    }
  }
  return lndet;
}
/* natural logarithm of determinant of A
 * sign of all the eigen values
 * note that A is changed on exit
 */
double lndet_mat3(double *a, int n, int *info, int *sign)
{
  int *ipiv;
  int i;
  double lndet;
  //ipiv=malloc(n*sizeof(int));
  //sign_all=malloc(n*sizeof(int));
  ipiv = workspace_ipiv;

//  dgetrf_(&n, &n, a, &n, ipiv, info);
  *info=LAPACKE_dgetrf(LAPACK_ROW_MAJOR, n, n, a, n, ipiv);
  if(*info!=0)
  {
    strcpy(str_error_exit, "lndet_mat3");
    error_exit(9);
  }

  lndet = 0.0;
  *sign = 1;
  for(i=0; i<n; i++)
  {
    lndet += log(fabs(a[i*n+i]));
    *sign *= (a[i*n+i]>=0?1:-1);

    if(ipiv[i]!=i+1)
    {
      //printf("%e %d\n", a[i*n+i], n);
      //ipiv[ipiv[i]] = ipiv[i];
      *sign *= -1;
    }
  }

  return lndet;
}
/* Cholesky decomposition of A, i.e., A = M^T* M
 * store M in A */
void Chol_decomp_U(double *a, int n, int *info)
{
  int i,j;
  char uplo = 'L';
//  dpotrf_(&uplo, &n, a, &n, info);
  *info=LAPACKE_dpotrf(LAPACK_ROW_MAJOR, uplo, n, a, n);
  if(*info<0)
  {
    strcpy(str_error_exit, "Chol_decomp_U");
    fprintf(stderr, "The %d-th argument had an illegal value!\n", *info);
    error_exit(9);
  }
  else if (*info>0)
  {
    strcpy(str_error_exit, "Chol_decomp_U");
    fprintf(stderr, "The leading minor of order %d is not positive definite, and the factorization could not be completed.\n", *info);
    error_exit(9);
  }
  for(i=0;i<n;i++)
    for(j=i+1;j<n;j++)
      a[i*n+j] = 0.0;
  return;
}
void display_mat(double *a, int m, int n)
{
    int i, j;
    for(i=0;i<m;i++)
    {
        for(j=0;j<n;j++)
        {
            printf("%f\t", a[i*n+j]);
        }
        printf("\n");
    }
}

double ** matrix_malloc(int n1, int n2)
{
  double **mat;
  int i;

  if(!(mat = malloc(n1*sizeof(double*))))
  {
    fprintf(stderr, "Unable to allocate the matrix!\n");
    error_exit(7);
  }

  for(i=0; i<n1; i++)
  {
    if(!(mat[i] = malloc(n2*sizeof(double))))
    {
      strcpy(str_error_exit, "matrix");
      error_exit(7);
    }
  }
  return mat;
}

double * array_malloc(int n)
{
  double *array;

  if(!(array = malloc(n*sizeof(double))))
  {
    strcpy(str_error_exit, "array");
    error_exit(7);
  }

  return array;
}


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

#include <gsl/gsl_blas.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_vector.h>
#include <qdm.h>

gsl_vector *
qdm_knots_vector(size_t spline_df, gsl_vector *interior_knots)
{
  size_t size = spline_df * 2 + interior_knots->size;
  gsl_vector *knots = gsl_vector_alloc(size);

  // Fill front with zeros.
  for (size_t i = 0; i < spline_df; i++) {
    gsl_vector_set(knots, i, 0);
  }

  // Fill middle with interior knots.
  gsl_vector_view view = gsl_vector_subvector(knots, spline_df, interior_knots->size);
  gsl_blas_dcopy(interior_knots, &view.vector);

  // Fill end with ones.
  for (size_t i = spline_df + interior_knots->size; i < knots->size; i++) {
    gsl_vector_set(knots, i, 1);
  }

  return knots;
}

int
qdm_knots_rss(
  double *rss,

  gsl_vector *sorted_data,
  gsl_vector *middle,
  gsl_vector *interior_knots,

  size_t spline_df
)
{
  int status = 0;

  gsl_vector *m_knots = qdm_knots_vector(spline_df, interior_knots);

  size_t m = spline_df + interior_knots->size;
  gsl_matrix *ix = gsl_matrix_alloc(middle->size, m + 1);

  gsl_vector *emperical_quantiles = qdm_vector_quantile(sorted_data, middle);
  gsl_vector *theta = gsl_vector_alloc(ix->size2);
  gsl_vector *spline_quantiles = gsl_vector_alloc(emperical_quantiles->size);

  /* Build the true quantile function. */
  qdm_ispline_matrix(ix, middle, spline_df, m_knots);

  /* Check if the matrix is full rank. */
  double det = 0;
  status = qdm_matrix_det_tmm(ix, &det);
  if (status != 0) {
    goto cleanup;
  }

  if (det <= 0) {
    goto cleanup;
  }

  status = qdm_theta_optimize(theta, emperical_quantiles, ix);
  if (status != 0) {
    goto cleanup;
  }

  status = gsl_blas_dgemv(CblasNoTrans , 1.0, ix, theta, 0.0, spline_quantiles);
  if (status != 0) {
    goto cleanup;
  }

  *rss = qdm_vector_rss(emperical_quantiles, spline_quantiles);

cleanup:
  gsl_vector_free(spline_quantiles);
  gsl_vector_free(theta);
  gsl_vector_free(emperical_quantiles);
  gsl_matrix_free(ix);
  gsl_vector_free(m_knots);

  return status;
}

int
qdm_knots_optimize(
  gsl_vector *result,

  gsl_rng *rng,

  gsl_vector *sorted_data,
  gsl_vector *middle,
  gsl_vector *possible_knots,

  size_t iterate_n,
  size_t spline_df
)
{
  int status = 0;
  double rss = INFINITY;
  double rss_found = 0;

  gsl_vector *interior_knots = gsl_vector_alloc(result->size);

  for (size_t i = 0; i < iterate_n; i++) {
    status = gsl_ran_choose(
        rng,
        interior_knots->data,
        interior_knots->size,
        possible_knots->data,
        possible_knots->size,
        sizeof(double)
    );
    if (status != 0) {
      goto cleanup;
    }

    status = qdm_knots_rss(
        &rss_found,

        sorted_data,
        middle,
        interior_knots,

        spline_df
    );
    if (status != 0) {
      goto cleanup;
    }

    if (rss_found < rss) {
      rss = rss_found;

      status = gsl_vector_memcpy(result, interior_knots);
      if (status != 0) {
        goto cleanup;
      }
    }
  }

cleanup:
  gsl_vector_free(interior_knots);

  return status;
}


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
//#include <cem.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>

void least_square_solver(double *matA, int n_row, int n_col, double *y, double *sol_x) {

    int i, j, k, tmp;
    double sum;
    gsl_matrix * A = gsl_matrix_alloc (n_row,n_col);
    gsl_matrix * V = gsl_matrix_alloc (n_col,n_col);
    gsl_vector * S = gsl_vector_alloc (n_col);
    gsl_vector * work = gsl_vector_alloc (n_col);
//    gsl_matrix * U = gsl_matrix_alloc (n_row-1,n_col_ids);
    gsl_vector * b = gsl_vector_alloc (n_row);
    gsl_vector * x = gsl_vector_alloc (n_col);

/*    //printf A and b
    FILE *fpA, *fpU, *fpb;
    fpA = fopen("A.dat","w");
    fpU = fopen("U.dat","w");
    fpb = fopen("b.dat","w");*/

    // need to copy matA to A, y to b!
    for (i=0;i<n_row;i++) {
        for (j=0; j<n_col; j++) {
            gsl_matrix_set (A, i, j, *(matA+i*n_col+j));
        }
        gsl_vector_set (b, i, y[i]);
    }
/*    for (i=0;i<n_row;i++) {
        for (j=0; j<n_col; j++) {
            fprintf(fpA,"%f ", gsl_matrix_get (A, i, j));
        }
        fprintf(fpA,"\n");
        fprintf(fpb,"%f\n", gsl_vector_get(b,i));
    }*/
    gsl_linalg_SV_decomp(A, V, S, work);    // on output, A is replaced by U!
/*    for (i=0;i<n_row;i++) {
        for (j=0; j<n_col; j++) {
            fprintf(fpU,"%f ", *(matA+i*n_col+j));
        }
        fprintf(fpU,"\n");
    }*/
    gsl_linalg_SV_solve (A, V, S, b, x);    // So, A should be used instead of U, here.

    for (i=0;i<n_col;i++) {
        sol_x[i] = gsl_vector_get(x,i);
    }

    gsl_matrix_free(A);
    gsl_matrix_free(V);
    gsl_vector_free(S);
    gsl_vector_free(work);
    gsl_vector_free(b);
    gsl_vector_free(x);

}


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

#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_complex_math.h>
#include <gsl/gsl_permute_vector.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_complex_math.h>

#include <gsl/gsl_linalg.h>

/* Factorise a general N x N complex matrix A into,
 *
 *   P A = L U
 *
 * where P is a permutation matrix, L is unit lower triangular and U
 * is upper triangular.
 *
 * L is stored in the strict lower triangular part of the input
 * matrix. The diagonal elements of L are unity and are not stored.
 *
 * U is stored in the diagonal and upper triangular part of the
 * input matrix.
 *
 * P is stored in the permutation p. Column j of P is column k of the
 * identity matrix, where k = permutation->data[j]
 *
 * signum gives the sign of the permutation, (-1)^n, where n is the
 * number of interchanges in the permutation.
 *
 * See Golub & Van Loan, Matrix Computations, Algorithm 3.4.1 (Gauss
 * Elimination with Partial Pivoting).
 */

int
gsl_linalg_complex_LU_decomp (gsl_matrix_complex * A, gsl_permutation * p, int *signum)
{
  if (A->size1 != A->size2)
    {
      GSL_ERROR ("LU decomposition requires square matrix", GSL_ENOTSQR);
    }
  else if (p->size != A->size1)
    {
      GSL_ERROR ("permutation length must match matrix size", GSL_EBADLEN);
    }
  else
    {
      const size_t N = A->size1;
      size_t i, j, k;

      *signum = 1;
      gsl_permutation_init (p);

      for (j = 0; j < N - 1; j++)
        {
          /* Find maximum in the j-th column */

          gsl_complex ajj = gsl_matrix_complex_get (A, j, j);
          double max = gsl_complex_abs (ajj);
          size_t i_pivot = j;

          for (i = j + 1; i < N; i++)
            {
              gsl_complex aij = gsl_matrix_complex_get (A, i, j);
              double ai = gsl_complex_abs (aij);

              if (ai > max)
                {
                  max = ai;
                  i_pivot = i;
                }
            }

          if (i_pivot != j)
            {
              gsl_matrix_complex_swap_rows (A, j, i_pivot);
              gsl_permutation_swap (p, j, i_pivot);
              *signum = -(*signum);
            }

          ajj = gsl_matrix_complex_get (A, j, j);

          if (!(GSL_REAL(ajj) == 0.0 && GSL_IMAG(ajj) == 0.0))
            {
              for (i = j + 1; i < N; i++)
                {
                  gsl_complex aij_orig = gsl_matrix_complex_get (A, i, j);
                  gsl_complex aij = gsl_complex_div (aij_orig, ajj);
                  gsl_matrix_complex_set (A, i, j, aij);

                  for (k = j + 1; k < N; k++)
                    {
                      gsl_complex aik = gsl_matrix_complex_get (A, i, k);
                      gsl_complex ajk = gsl_matrix_complex_get (A, j, k);

                      /* aik = aik - aij * ajk */

                      gsl_complex aijajk = gsl_complex_mul (aij, ajk);
                      gsl_complex aik_new = gsl_complex_sub (aik, aijajk);

                      gsl_matrix_complex_set (A, i, k, aik_new);
                    }
                }
            }
        }

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_complex_LU_solve (const gsl_matrix_complex * LU, const gsl_permutation * p, const gsl_vector_complex * b, gsl_vector_complex * x)
{
  if (LU->size1 != LU->size2)
    {
      GSL_ERROR ("LU matrix must be square", GSL_ENOTSQR);
    }
  else if (LU->size1 != p->size)
    {
      GSL_ERROR ("permutation length must match matrix size", GSL_EBADLEN);
    }
  else if (LU->size1 != b->size)
    {
      GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
    }
  else if (LU->size2 != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else
    {
      /* Copy x <- b */

      gsl_vector_complex_memcpy (x, b);

      /* Solve for x */

      gsl_linalg_complex_LU_svx (LU, p, x);

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_complex_LU_svx (const gsl_matrix_complex * LU, const gsl_permutation * p, gsl_vector_complex * x)
{
  if (LU->size1 != LU->size2)
    {
      GSL_ERROR ("LU matrix must be square", GSL_ENOTSQR);
    }
  else if (LU->size1 != p->size)
    {
      GSL_ERROR ("permutation length must match matrix size", GSL_EBADLEN);
    }
  else if (LU->size1 != x->size)
    {
      GSL_ERROR ("matrix size must match solution/rhs size", GSL_EBADLEN);
    }
  else
    {
      /* Apply permutation to RHS */

      gsl_permute_vector_complex (p, x);

      /* Solve for c using forward-substitution, L c = P b */

      gsl_blas_ztrsv (CblasLower, CblasNoTrans, CblasUnit, LU, x);

      /* Perform back-substitution, U x = c */

      gsl_blas_ztrsv (CblasUpper, CblasNoTrans, CblasNonUnit, LU, x);

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_complex_LU_refine (const gsl_matrix_complex * A, const gsl_matrix_complex * LU, const gsl_permutation * p, const gsl_vector_complex * b, gsl_vector_complex * x, gsl_vector_complex * residual)
{
  if (A->size1 != A->size2)
    {
      GSL_ERROR ("matrix a must be square", GSL_ENOTSQR);
    }
  if (LU->size1 != LU->size2)
    {
      GSL_ERROR ("LU matrix must be square", GSL_ENOTSQR);
    }
  else if (A->size1 != LU->size2)
    {
      GSL_ERROR ("LU matrix must be decomposition of a", GSL_ENOTSQR);
    }
  else if (LU->size1 != p->size)
    {
      GSL_ERROR ("permutation length must match matrix size", GSL_EBADLEN);
    }
  else if (LU->size1 != b->size)
    {
      GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
    }
  else if (LU->size1 != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else
    {
      /* Compute residual, residual = (A * x  - b) */

      gsl_vector_complex_memcpy (residual, b);

      {
        gsl_complex one = GSL_COMPLEX_ONE;
        gsl_complex negone = GSL_COMPLEX_NEGONE;
        gsl_blas_zgemv (CblasNoTrans, one, A, x, negone, residual);
      }

      /* Find correction, delta = - (A^-1) * residual, and apply it */

      gsl_linalg_complex_LU_svx (LU, p, residual);

      {
        gsl_complex negone= GSL_COMPLEX_NEGONE;
        gsl_blas_zaxpy (negone, residual, x);
      }

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_complex_LU_invert (const gsl_matrix_complex * LU, const gsl_permutation * p, gsl_matrix_complex * inverse)
{
  size_t i, n = LU->size1;

  int status = GSL_SUCCESS;

  gsl_matrix_complex_set_identity (inverse);

  for (i = 0; i < n; i++)
    {
      gsl_vector_complex_view c = gsl_matrix_complex_column (inverse, i);
      int status_i = gsl_linalg_complex_LU_svx (LU, p, &(c.vector));

      if (status_i)
        status = status_i;
    }

  return status;
}

gsl_complex
gsl_linalg_complex_LU_det (gsl_matrix_complex * LU, int signum)
{
  size_t i, n = LU->size1;

  gsl_complex det = gsl_complex_rect((double) signum, 0.0);

  for (i = 0; i < n; i++)
    {
      gsl_complex zi = gsl_matrix_complex_get (LU, i, i);
      det = gsl_complex_mul (det, zi);
    }

  return det;
}

double
gsl_linalg_complex_LU_lndet (gsl_matrix_complex * LU)
{
  size_t i, n = LU->size1;

  double lndet = 0.0;

  for (i = 0; i < n; i++)
    {
      gsl_complex z = gsl_matrix_complex_get (LU, i, i);
      lndet += log (gsl_complex_abs (z));
    }

  return lndet;
}

gsl_complex
gsl_linalg_complex_LU_sgndet (gsl_matrix_complex * LU, int signum)
{
  size_t i, n = LU->size1;

  gsl_complex phase = gsl_complex_rect((double) signum, 0.0);

  for (i = 0; i < n; i++)
    {
      gsl_complex z = gsl_matrix_complex_get (LU, i, i);

      double r = gsl_complex_abs(z);

      if (r == 0)
        {
          phase = gsl_complex_rect(0.0, 0.0);
          break;
        }
      else
        {
          z = gsl_complex_div_real(z, r);
          phase = gsl_complex_mul(phase, z);
        }
    }

  return phase;
}


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

#include <config.h>
#include <math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

void
gsl_ran_dir_2d (const gsl_rng * r, double *x, double *y)
{
  /* This method avoids trig, but it does take an average of 8/pi =
   * 2.55 calls to the RNG, instead of one for the direct
   * trigonometric method.  */

  double u, v, s;
  do
    {
      u = -1 + 2 * gsl_rng_uniform (r);
      v = -1 + 2 * gsl_rng_uniform (r);
      s = u * u + v * v;
    }
  while (s > 1.0 || s == 0.0);

  /* This is the Von Neumann trick. See Knuth, v2, 3rd ed, p140
   * (exercise 23).  Note, no sin, cos, or sqrt !  */

  *x = (u * u - v * v) / s;
  *y = 2 * u * v / s;

  /* Here is the more straightforward approach,
   *     s = sqrt (s);  *x = u / s;  *y = v / s;
   * It has fewer total operations, but one of them is a sqrt */
}

void
gsl_ran_dir_2d_trig_method (const gsl_rng * r, double *x, double *y)
{
  /* This is the obvious solution... */
  /* It ain't clever, but since sin/cos are often hardware accelerated,
   * it can be faster -- it is on my home Pentium -- than von Neumann's
   * solution, or slower -- as it is on my Sun Sparc 20 at work
   */
  double t = 6.2831853071795864 * gsl_rng_uniform (r);          /* 2*PI */
  *x = cos (t);
  *y = sin (t);
}

void
gsl_ran_dir_3d (const gsl_rng * r, double *x, double *y, double *z)
{
  double s, a;

  /* This is a variant of the algorithm for computing a random point
   * on the unit sphere; the algorithm is suggested in Knuth, v2,
   * 3rd ed, p136; and attributed to Robert E Knop, CACM, 13 (1970),
   * 326.
   */

  /* Begin with the polar method for getting x,y inside a unit circle
   */
  do
    {
      *x = -1 + 2 * gsl_rng_uniform (r);
      *y = -1 + 2 * gsl_rng_uniform (r);
      s = (*x) * (*x) + (*y) * (*y);
    }
  while (s > 1.0);

  *z = -1 + 2 * s;              /* z uniformly distributed from -1 to 1 */
  a = 2 * sqrt (1 - s);         /* factor to adjust x,y so that x^2+y^2
                                 * is equal to 1-z^2 */
  *x *= a;
  *y *= a;
}

void
gsl_ran_dir_nd (const gsl_rng * r, size_t n, double *x)
{
  double d;
  size_t i;
  /* See Knuth, v2, 3rd ed, p135-136.  The method is attributed to
   * G. W. Brown, in Modern Mathematics for the Engineer (1956).
   * The idea is that gaussians G(x) have the property that
   * G(x)G(y)G(z)G(...) is radially symmetric, a function only
   * r = sqrt(x^2+y^2+...)
   */
  d = 0;
  do
    {
      for (i = 0; i < n; ++i)
        {
          x[i] = gsl_ran_gaussian (r, 1.0);
          d += x[i] * x[i];
        }
    }
  while (d == 0);
  d = sqrt (d);
  for (i = 0; i < n; ++i)
    {
      x[i] /= d;
    }
}


-----------natrix mul---
/*
 * standard includes
 */
	#include <stdio.h>
	#include <stdlib.h>
/*
 *  includes for GSL components
 *  	- use double precision
 */
	#include <gsl/gsl_vector_double.h>
	#include <gsl/gsl_matrix_double.h>
	#include <gsl/gsl_rng.h>
	#include <gsl/gsl_randist.h>

/*
 * FUNCTIONS
 */

/*
 * simple Fibonacci sequence generator function, using recursion
 */
size_t fib(size_t k)
{
	if (k==0)
	{
		return 0;
	}
	else if (k==1)
	{
		return 1;
	}
	else /* k >= 2 */
	{
		return fib(k-1) + fib(k-2);
	}
}
gsl_matrix *embt_mm(const gsl_matrix *U,const gsl_matrix *V, size_t N)
{
	gsl_matrix *W=gsl_matrix_calloc(N,N);
	double sum=0;
	for (size_t i=0;i!=N;i++)
	{
		for(size_t j=0;j!=N;j++)
		{
			for (size_t k=0;k!=N;k++)
			{
				sum=sum+gsl_matrix_get(U,i,k)*gsl_matrix_get(V,k,j);
			}
			gsl_matrix_set(W,i,j,(double)sum);
			sum=0;
		}
	}
	return W;
}

gsl_matrix *embt_apb(gsl_vector *u, gsl_vector *v,size_t N)
{
	gsl_vector *w=gsl_vector_calloc(N);
	gsl_vector_add(w,u);
	gsl_vector_add(w,v);
	return w;
}

gsl_matrix *embt_m_print(const gsl_matrix *U, size_t N)
{
	for (size_t i = 0; i != N; ++i)
	{
		for (size_t j=0;j!=N;j++)
		{
			printf("%0.2f\t",gsl_matrix_get(U, i, j));
		}
		printf("\n");
	}
}
gsl_matrix *embt_v_print(const gsl_vector *u, size_t N)
{
	for (int i = 0; i != N; ++i)
	{
		printf("%0.2f\n",gsl_vector_get(u, i));
	}
}
int main()
{
	/*
	 * INITIALIZE PARAMETERS
	 */
		/* vectors parameters */
		size_t		N=3; /* index type, vector sizes */
		gsl_vector *a = gsl_vector_alloc(N); /* allocate vector from heap of size N */
		gsl_vector *b = gsl_vector_alloc(N); /* allocate vector from heap of size N */
		gsl_vector *c = gsl_vector_calloc(N); /* allocate vector of size N but initialize entries to zero */

		/* random number generator parameters */
		const gsl_rng_type *T;
		gsl_rng *r; /* handle for our random number generator */

		/* matrix parameters */
		gsl_matrix *A = gsl_matrix_alloc(N,N);
		gsl_matrix *B = gsl_matrix_alloc(N,N);
		gsl_matrix *C = gsl_matrix_calloc(N,N);

	/*
	 * SET UP RANDOM NUMBER GENERATION
	 */
		gsl_rng_env_setup();
		T = gsl_rng_default;
		r = gsl_rng_alloc(T);

	/*
	 *  VECTOR OPERATIONS
	 */
		/* set the vector elements */
		for (size_t i = 0; i != N; ++i)
		{
			gsl_vector_set(a, i, fib(i)); /* set element i of vector a to Fibonacci number i */
			gsl_vector_set(b, i, gsl_ran_flat(r,-1.0,+1.0)); /* set element of vector b to random number */
		}

		/* c = a + b */
		c=embt_apb(a,b,N); /* c += a */
		//gsl_vector_add(c, b); /* c += b */

		/* print results */

		int sum=0;
	/*	MATRIX OPERATIONS */
		for (size_t i=0;i!=N;i++)
			for (size_t j=0;j!=N;j++)
			{
				//size_t x=j+(i*N);
				gsl_matrix_set(A,i,j,(double)fib(j+i*N));
				gsl_matrix_set(B,i,j,gsl_ran_flat(r,-100,+100));
			}
		//gsl_matrix_mul_elements(C, A); /* c += a */
		//gsl_matrix_mul_elements(C, B); /* c += b */
		C=embt_mm(A,B,N);

		printf("a= \n");embt_v_print(a,N);
		printf("b= \n");embt_v_print(b,N);
		printf("c= \n");embt_v_print(c,N);
		printf("A= \n");embt_m_print(A,N);
		printf("B= \n");embt_m_print(B,N);
		printf("C= \n");embt_m_print(C,N);

	/* de-allocate memory */
	gsl_vector_free(a);
	gsl_vector_free(b);
	gsl_vector_free(c);
	gsl_matrix_free(A);
	gsl_matrix_free(B);
	gsl_matrix_free(C);
	return EXIT_SUCCESS;
}


/**
  * Copyright 2017 José Manuel Abuín Mosquera <josemanuel.abuin@usc.es>
  *
  * This file is part of Matrix Market Suite.
  *
  * Matrix Market Suite 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.
  *
  * Matrix Market Suite 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 Matrix Market Suite. If not, see <http://www.gnu.org/licenses/>.
  */

#include <cblas.h>
#include "JacobiSolver.h"

int JacobiSolver(unsigned long *II, unsigned long *J, double *A, unsigned long M, unsigned long N, unsigned long long nz, double *b, unsigned long M_Vector, unsigned long N_Vector, unsigned long long nz_vector,double *x2, int iterationNumber) {

	//Jacobi Method as shown in the example from https://en.wikipedia.org/wiki/Jacobi_method

	unsigned long 	k 	= 0;
	int 		stop 	= 0;

	unsigned int	i	= 0;

	double		result	= 0.0;

	//Initial solution
	double *x1=(double *) calloc(nz_vector,sizeof(double));
	//double *x2=(double *) calloc(nz_vector,sizeof(double));

	double *res=(double *) calloc(nz_vector,sizeof(double));

	double *LU=(double *) calloc(nz,sizeof(double));
	double *Dinv=(double *) calloc(nz,sizeof(double));

	double *T=(double *) calloc(nz,sizeof(double));
	double *C=(double *) calloc(nz_vector,sizeof(double));

	if(!isDiagonallyDominant(A,M,N,nz)){
		fprintf(stderr, "[%s] The matrix is not diagonally dominant\n",__func__);
		//return 0;
	}

	getLUValues(LU, A,M,N,nz);

	getDInvValues(Dinv, A,M,N,nz);

	for(i = 0; i< N; i++){
		x1[i] = 1.0;
	}

	//T=-D^{-1}(L+U)
	/*
	void cblas_dgemm (const CBLAS_LAYOUT Layout, const CBLAS_TRANSPOSE transa, const CBLAS_TRANSPOSE transb, const MKL_INT m, const MKL_INT n, const MKL_INT k, const double alpha, const double *a, const MKL_INT lda, const double *b, const MKL_INT ldb, const double beta, double *c, const MKL_INT ldc);
	*/
	cblas_dgemm(CblasRowMajor,CblasNoTrans,CblasNoTrans, M, N, N, -1.0, Dinv, N, LU, N, 0.0, T, N);
	//writeDenseCoordinateMatrixRowLine("stdout", T,M,N,nz);

	//C=-D^{-1}b
	cblas_dgemv(CblasRowMajor, CblasNoTrans, M,N , 1.0, Dinv, N, b, 1, 0.0, C, 1);

	//writeDenseVector("stdout", C,nz_vector,1, nz_vector);
	unsigned long maxIterations = M*2;

	if(iterationNumber != 0 ){
		maxIterations = iterationNumber;
	}

	while(!stop) {

		// x^{(1)}= Tx^{(0)}+C
		//x2 = T*x1
		cblas_dgemv(CblasRowMajor, CblasNoTrans, M,N , 1.0, T, N, x1, 1, 0.0, x2, 1);

		//x2 = x2+C
		cblas_daxpy(nz_vector,1.0,C, 1, x2, 1);

		//res = A*x - b
		memcpy(res, b, nz_vector*sizeof(double));
		cblas_dgemv(CblasRowMajor, CblasNoTrans, M,N , 1.0, A, N, x2, 1, -1.0, res, 1);

		result = vectorSumElements(res,N);
		if((fabs(result)<=EPSILON)||(k == maxIterations)){
			//fprintf(stderr,"Sum vector res is %lg\n",result);
			stop = 1;
		}

		memcpy(x1, x2, nz_vector*sizeof(double));
		//writeDenseVector("stdout", x1,nz_vector,1, nz_vector);
		k++;
	}

	//memcpy(b, x2, N*sizeof(double));

	free(x1);
	//free(x2);
	free(res);

	fprintf(stderr, "[%s] Number of iterations %lu\n",__func__,k);

	return 1;

}