/* ============================================================ *
 * halomodel.h							*
 * Martin Kilbinger 2006-2009					*
 * ============================================================ */

#ifndef __HALOMODEL_H
#define __HALOMODEL_H

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <assert.h>
#include <string.h>
#include <fftw3.h>

#include <gsl/gsl_sf_erf.h>

#include "io.h"
#include "errorlist.h"
#include "config.h"
#include "maths.h"

#include "cosmo.h"
#include "nofz.h"

#define hm_base     -1900
#define hm_hodtype   hm_base + 1
#define hm_Mmin      hm_base + 2
#define hm_pofk      hm_base + 3
#define hm_nfw       hm_base + 4
#define hm_par       hm_base + 5
#define hm_overflow  hm_base + 6
#define hm_io        hm_base + 7
#define hm_zbin      hm_base + 8
#define hm_alpha     hm_base + 9
#define hm_negative  hm_base + 10
#define hm_zmean_2h  hm_base + 11
#define hm_halo_bias hm_base + 12
#define hm_undef     hm_base + 13
#define hm_gsl_int   hm_base + 14

/* Ranges of interpolation tables */
#define k_max_HOD     3336.0

/* Present critical density [M_sol h^2 / Mpc^3] */
#define rho_c0  2.7754e11

/* Mass limits for integration over mass functions */
#define logMmin (3.0*log(10.0))
#define logMmax (16.0*log(10.0))

/* Number of steps for scale-factor-integration (redshift) */
#define Na_hm 20

/* Bit-coded power spectrum types */
typedef enum {pofk_undef=-1, pl=1, pnl=2, p1hdm=4, p2hdm=8, pthdm=16, p1hg=32, p2hg=64, pthg=128,
	      p1hgcs=256, p1hgss=512, pstellar=1024} pofk_t;

/* Halo mass function type */
typedef enum {ps, st, st2, j01} massfct_t;
#define smassfct_t(i) ( \
  i==ps  ? "ps" : \
  i==st  ? "st" : \
  i==st2 ? "st2" : \
  i==j01 ? "j01" : \
  "")
#define Nmassfct_t 4

/* Halo bias type */
typedef enum {halo_bias_sc, halo_bias_tinker05, halo_bias_tinker10} halo_bias_t;
#define shalo_bias_t(i) ( \
   i==halo_bias_sc       ? "halo_bias_sc" : \
   i==halo_bias_tinker05 ? "halo_bias_tinker05" : \
   i==halo_bias_tinker10 ? "halo_bias_tinker10" : \
   "")
#define Nhalo_bias_t 3

/* HOD (Halo occupation distribution) type */
#define Nhod_t 5
typedef enum {hod_none, hamana04, berwein02, berwein02_hexcl, leauthaud11} hod_t;
#define shod_t(i) (		\
 i==hod_none  ? "hod_none" :	\
 i==hamana04  ? "hamana04" :	\
 i==berwein02 ? "berwein02" :	\
 i==berwein02_hexcl ? "berwein02_hexcl" :\
 i==leauthaud11 ? "leauthaud11" :	\
 "")

/* ---------------------------------------------------------------- *
 * Global variables and functions                                   *
 * ---------------------------------------------------------------- */

double FFTLog_TMP;

typedef struct FFTLog_complex
{
  double re;
  double im;
  double amp;
  double arg;
}  FFTLog_complex;

typedef struct {
  int N;
  fftw_plan p_forward;
  fftw_plan p_backward;
  fftw_complex *an;
  fftw_complex *ak;
  fftw_complex *cm;
  fftw_complex *um;
  fftw_complex *cmum;
  double min;
  double max;
  double q;
  double mu;
  double kr;
} FFTLog_config;

typedef struct {

  cosmo *cosmo;
  redshift_t *redshift;
  double zmin, zmax;

  /* Dark matter halo profile                                                      */
  double c0;			/* concentration parameter                         */
  double alpha_NFW;		/* density slope                                   */
  double beta_NFW;		/* concentration slope as fct of mass              */
  massfct_t massfct;            /* halo mass function				   */
  halo_bias_t halo_bias;        /* Halo bias                                       */

  /* Mass function parameters (Sheth&Torman). Do not set manually, they are set    *
   * in set_massfct() according to enum massfct.				   */
  double nmz_a;                 /* Called q in CS02                                */
  double nmz_p;                 /* a=1, p=1/2 is Press-Schechter mass fct.         */

  /* HOD (halo occupation distribution) parameters				   */
  hod_t hod;                    /* HOD type                                        */
  double M1, M0, sigma_log_M;
  double M_min;
  double alpha;
  double pi_max;
  double eta;                   /* central galaxy proportion                       */

  /* galaxy-galaxy lensing and wp(rp) */
  double log10Mhalo;
  double coord_phys;

  /* For Leauthaud11 model                                                         */
  double beta,delta,gamma,Mstar0;
  double beta_sat,B_sat,beta_cut,B_cut;
  double x;                     /* any parameter to propagate if needed            */
  double Mstellar_min, Mstellar_max;
  double fcen1, fcen2;

  /* Precomputed stuff */
  double A;			/* Mass function normalisation                     */
  double Mstar;                 /* M_*(a=1.0)					   */
  interTable2D *Pthdm;
  interTable *xir;
  interTable *xi_dm;
  interTable2D *rhohat;
  splineTable* sigRsqr;
  double a_xir;

  /* FFTLOG flag - OBSOLETE */
  int FFTLog;

} cosmo_hm;

typedef struct {

  cosmo    *cosmo;
  cosmo_hm *model;
  double   a, r, k, ng, ngp, eps, c;
  double  logMlim, bias_fac, Mh, Mstellar, Mstellar_min, Mstellar_max;
  double  M, r_vir, *kk;
  error    **err;

  double   logrmin, logrmax, rp, xi;
  gsl_interp_accel *acc;
  gsl_spline *spline;

  int i, j, type, asymptotic, logintegrate;

  double   (*bias_func)(double, void *);
} cosmo_hm_params;

typedef struct gsl_int_params
{
  void *params;
  funcwithpars func;
  error **err;

} gsl_int_params;

typedef struct {
  double *z;
  double *fac;
  double *ypn; /* for spline interpolation */
  double zm;   /* average weighted redshift*/
  int nbins;
} nz_t;

cosmo_hm* init_parameters_hm(double OMEGAM, double OMEGADE, double W0_DE, double W1_DE,
			     double *W_POLY_DE, int N_POLY_DE,
			     double H100, double OMEGAB, double OMEGANUMASS,
			     double NEFFNUMASS, double NORM, double NSPEC,
			     int Nzbin, const int *Nnz, const nofz_t *nofz, double *par_nz,
			     double zmin, double zmax,
			     nonlinear_t NONLINEAR, transfer_t TRANSFER, growth_t GROWTH,
			     de_param_t DEPARAM, norm_t normmode,
			     double C0, double ALPHANFW, double BETANFW, massfct_t MASSFCT, halo_bias_t HALO_BIAS,
			     double M_min, double M1, double M0, double sigma_log_M, double alpha,
			     double Mstar0, double beta, double delta, double gamma, double B_cut, double B_sat,
			     double beta_cut, double beta_sat, double Mstellar_min, double Mstellar_max, double eta,
			     double fcen1, double fcen2,
			     hod_t HOD, double pi_max, error **err);

cosmo_hm* copy_parameters_hm_only(cosmo_hm* source, error **err);
cosmo_hm *copy_parameters_hm(cosmo_hm *source, error **err);
void read_cosmological_parameters_hm(cosmo_hm **model, FILE *F, error **err);
cosmo_hm *set_cosmological_parameters_to_default_hm(error **err);
void free_parameters_hm(cosmo_hm** model);

void set_massfct(massfct_t massfct, double *nmz_a, double *nmz_p, error **err);
void dump_param_only_hm(cosmo_hm* model, FILE *F);
void dump_param_hm(cosmo_hm* model, FILE *F, error **err);

double sm2_rtbis(double (*func)(double, void *, error **), double x1, double x2,
		 double xacc, void *param, error **err);

/* From nrcomplex.h,c */
#ifndef _DCOMPLEX_DECLARE_T_
typedef struct DCOMPLEX {double r,i;} dcomplex;
#define _DCOMPLEX_DECLARE_T_
#endif /* _DCOMPLEX_DECLARE_T_ */
dcomplex Complex(double re, double im);
dcomplex Cadd(dcomplex a, dcomplex b);
dcomplex Cmul(dcomplex a, dcomplex b);
dcomplex Cdiv(dcomplex a, dcomplex b);
dcomplex RCmul(double x, dcomplex a);
void sm2_cisi(double x, double *ci, double *si, error **err);

double delta_c(cosmo *model, double a, error **err);
double bis_Mstar(double logM, void *param, error **err);
double bis_Mstar_a(double logM, void *param, error **err);
double Mstar(cosmo_hm *model, error **err);
double Mstar_a(cosmo_hm *model, double a, error **err);
double concentration(cosmo_hm *model, double Mh, double a, error **err);
double Delta_vir(cosmo_hm *model, double a);
double dsigma_R_sqr_dR(cosmo_hm *model, double R, error **err);
double nufnu(cosmo_hm *model, double nu, int asymptotic, error **err);
double nufnu_j01(double x);
double sigma_R_sqr(cosmo_hm *model, double R, error **err);
double sigmasqr_M(cosmo_hm *model, double M, error **err);

double dsigma_m1_dlnM(cosmo_hm *model, double M, error **err);
double dnu_dlnM(cosmo_hm *model, double M, double a, error **err);
double dn_dlnM_lnM(double logM, void *intpar, error **err);
double dn_dlnM_uf(double M, cosmo_hm *model, double a, error **err);
double dn_dlnM(double M, void *intpar, error **err);

double r_vir(cosmo_hm *model, double M, double a, error **err);
double M_vir(cosmo_hm *model, double r_vir, double a, error **err);
double Delta_h(cosmo_hm *model, double a, error **err);
double rho_crit(cosmo_hm *model, double a, error **err);
double rho_crit_halo(cosmo_hm *model, double a, error **err);
double Omega_m_halo(cosmo_hm *model, double a, error **err);
double rho_halo(cosmo_hm *model, double r, double a, double Mh, double c, error **err);

double DeltaSigma_WB2000(cosmo_hm *model, double r, const double a, const double M,  double c,  double Delta, error **err);
double g_inf(double x, error **err);
double g_sup(double x, error **err);

double int_for_rhohat(double, void *, error **err);
double rhohat_halo(cosmo_hm *model, double k, double M, double a, double c, error **err);

double halo_bias(cosmo_hm *model, double M, double a, int k, error **err);
double bias(cosmo_hm *model, double M, double a, int k, error **err);
double bias_tinker(cosmo_hm *model, double M, double a, error **err);
double bias_tinker10(cosmo_hm *model, double M, double a, error **err);
double int_for_bias_norm(double logM, void *intpar, error **err);
double bias_norm(cosmo_hm *model, double a, error **err);

double int_for_M_ij(double, void *, error **);
double M_ij(cosmo_hm *model, int i, int j, double a, const double *k, error **err);
double P1h_dm(cosmo_hm *model, double a, double k, error **err);
double P2h_dm(cosmo_hm *model, double a, double k, error **err);

double xi_dm_NL_OBSOLETE(cosmo_hm *model, double a, double r, error **err);  // non-linear DM xi
double int_for_xi_dm_NL_OBSOLETE(double k, void *intpar,  error **err);  // non-linear DM xi

#define CHANGE(fct) int change_##fct(cosmo_hm*, cosmo_hm*)

/* ---------------------------------------------------------------- *
 * Utils                                                            *
 * ---------------------------------------------------------------- */

double int_gsl(funcwithpars func,void *params, double a, double b, double eps, error **err);
double integrand_gsl(double x,void *p);

CHANGE(massfct);
CHANGE(massfct_params);
CHANGE(halo_bias);
CHANGE(sigma_R_sqr);
CHANGE(Mstar);
CHANGE(rhohat_halo);
CHANGE(Pth);

#undef CHANGE

#endif


/**
 *
 * @file dsungesv.c
 *
 *  PLASMA computational routines
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @version 2.6.0
 * @author Emmanuel Agullo
 * @date 2010-11-15
 * @generated ds Tue Jan  7 11:45:09 2014
 *
 **/
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <lapacke.h>
#include "common.h"

#define PLASMA_dlag2s(_descA, _descSB)                \
  plasma_parallel_call_4(plasma_pdlag2s,              \
                         PLASMA_desc,      (_descA),  \
                         PLASMA_desc,      (_descSB), \
                         PLASMA_sequence*, sequence,  \
                         PLASMA_request*,  request)

#define PLASMA_slag2d(_descSA, _descB)                \
  plasma_parallel_call_4(plasma_pslag2d,              \
                         PLASMA_desc,      (_descSA), \
                         PLASMA_desc,      (_descB),  \
                         PLASMA_sequence*, sequence,  \
                         PLASMA_request*,  request)

#define PLASMA_dlange(_norm, _descA, _result, _work)   \
  _result = 0;                                         \
  plasma_parallel_call_6(plasma_pdlange,               \
                         PLASMA_enum,      (_norm),    \
                         PLASMA_desc,      (_descA),   \
                         double*,          (_work),    \
                         double*,          &(_result), \
                         PLASMA_sequence*, sequence,   \
                         PLASMA_request*,  request);

#define PLASMA_dlacpy(_descA, _descB)                        \
  plasma_parallel_call_5(plasma_pdlacpy,                     \
                         PLASMA_enum,      PlasmaUpperLower, \
                         PLASMA_desc,      (_descA),         \
                         PLASMA_desc,      (_descB),         \
                         PLASMA_sequence*, sequence,         \
                         PLASMA_request*,  request)

#define PLASMA_dgeadd(_alpha, _descA, _descB)                 \
  plasma_parallel_call_5(plasma_pdgeadd,                      \
                         double, (_alpha),        \
                         PLASMA_desc,        (_descA),        \
                         PLASMA_desc,        (_descB),        \
                         PLASMA_sequence*,   sequence,        \
                         PLASMA_request*,    request)

/***************************************************************************//**
 *
 * @ingroup double
 *
 *  PLASMA_dsungesv - Solves overdetermined or underdetermined linear systems involving an M-by-N
 *  matrix A using the QR or the LQ factorization of A.  It is assumed that A has full rank.
 *  The following options are provided:
 *
 *  # trans = PlasmaNoTrans and M >= N: find the least squares solution of an overdetermined
 *    system, i.e., solve the least squares problem: minimize || B - A*X ||.
 *
 *  # trans = PlasmaNoTrans and M < N:  find the minimum norm solution of an underdetermined
 *    system A * X = B.
 *
 *  Several right hand side vectors B and solution vectors X can be handled in a single call;
 *  they are stored as the columns of the M-by-NRHS right hand side matrix B and the N-by-NRHS
 *  solution matrix X.
 *
 *  PLASMA_dsungesv first attempts to factorize the matrix in COMPLEX and use this
 *  factorization within an iterative refinement procedure to produce a
 *  solution with COMPLEX*16 normwise backward error quality (see below).
 *  If the approach fails the method switches to a COMPLEX*16
 *  factorization and solve.
 *
 *  The iterative refinement is not going to be a winning strategy if
 *  the ratio COMPLEX performance over COMPLEX*16 performance is too
 *  small. A reasonable strategy should take the number of right-hand
 *  sides and the size of the matrix into account. This might be done
 *  with a call to ILAENV in the future. Up to now, we always try
 *  iterative refinement.
 *
 *  The iterative refinement process is stopped if ITER > ITERMAX or
 *  for all the RHS we have: RNRM < N*XNRM*ANRM*EPS*BWDMAX
 *  where:
 *
 *  - ITER is the number of the current iteration in the iterative refinement process
 *  - RNRM is the infinity-norm of the residual
 *  - XNRM is the infinity-norm of the solution
 *  - ANRM is the infinity-operator-norm of the matrix A
 *  - EPS is the machine epsilon returned by DLAMCH('Epsilon').
 *
 *  Actually, in its current state (PLASMA 2.1.0), the test is slightly relaxed.
 *
 *  The values ITERMAX and BWDMAX are fixed to 30 and 1.0D+00 respectively.
 *
 *  We follow Bjorck's algorithm proposed in "Iterative Refinement of Linear
 *  Least Squares solutions I", BIT, 7:257-278, 1967.4
 *
 *******************************************************************************
 *
 * @param[in] trans
 *          Intended usage:
 *          = PlasmaNoTrans:   the linear system involves A;
 *          = PlasmaTrans: the linear system involves A**H.
 *          Currently only PlasmaNoTrans is supported.
 *
 * @param[in] N
 *          The number of columns of the matrix A. N >= 0.
 *
 * @param[in] NRHS
 *          The number of right hand sides, i.e., the number of columns of the matrices B and X.
 *          NRHS >= 0.
 *
 * @param[in] A
  *          The M-by-N matrix A. This matrix is not modified.
 *
 * @param[in] LDA
 *          The leading dimension of the array A. LDA >= max(1,M).
 *
 * @param[in] B
 *          The M-by-NRHS matrix B of right hand side vectors, stored columnwise. Not modified.
 *
 * @param[in] LDB
 *          The leading dimension of the array B. LDB >= MAX(1,M,N).
 *
 * @param[out] X
 *          If return value = 0, the solution vectors, stored columnwise.
 *          if M >= N, rows 1 to N of B contain the least squares solution vectors; the residual
 *          sum of squares for the solution in each column is given by the sum of squares of the
 *          modulus of elements N+1 to M in that column;
 *          if M < N, rows 1 to N of B contain the minimum norm solution vectors;
 *
 * @param[in] LDX
 *          The leading dimension of the array B. LDB >= MAX(1,M,N).
 *
 * @param[out] ITER
 *          The number of the current iteration in the iterative refinement process
 *
 *******************************************************************************
 *
 * @return
 *          \retval PLASMA_SUCCESS successful exit
 *          \retval <0 if -i, the i-th argument had an illegal value
 *
 *******************************************************************************
 *
 * @sa PLASMA_dsungesv_Tile
 * @sa PLASMA_dsungesv_Tile_Async
 * @sa PLASMA_dsungesv
 * @sa PLASMA_dgels
 *
 ******************************************************************************/
int PLASMA_dsungesv(PLASMA_enum trans, int N, int NRHS,
                    double *A, int LDA,
                    double *B, int LDB,
                    double *X, int LDX, int *ITER)
{
    int NB;
    int status;
    PLASMA_desc  descA;
    PLASMA_desc  descB;
    PLASMA_desc *descT;
    PLASMA_desc  descX;
    plasma_context_t *plasma;
    PLASMA_sequence *sequence = NULL;
    PLASMA_request request = PLASMA_REQUEST_INITIALIZER;

    plasma = plasma_context_self();
    if (plasma == NULL) {
        plasma_fatal_error("PLASMA_dsungesv", "PLASMA not initialized");
        return PLASMA_ERR_NOT_INITIALIZED;
    }

    *ITER = 0;

    /* Check input arguments */
    if (trans != PlasmaNoTrans   &&
        trans != PlasmaTrans &&
        trans != PlasmaTrans )
    {
        plasma_error("PLASMA_dsungesv", "illegal value of trans");
        return -1;
    }
    if (trans != PlasmaNoTrans) {
        plasma_error("PLASMA_dsungesv", "only PlasmaNoTrans supported");
        return PLASMA_ERR_NOT_SUPPORTED;
    }
    if (N < 0) {
        plasma_error("PLASMA_dsungesv", "illegal value of N");
        return -2;
    }
    if (NRHS < 0) {
        plasma_error("PLASMA_dsungesv", "illegal value of NRHS");
        return -3;
    }
    if (LDA < max(1, N)) {
        plasma_error("PLASMA_dsungesv", "illegal value of LDA");
        return -5;
    }
    if (LDB < max(1, N)) {
        plasma_error("PLASMA_dsungesv", "illegal value of LDB");
        return -7;
    }
    if (LDX < max(1, N)) {
        plasma_error("PLASMA_dsungesv", "illegal value of LDX");
        return -9;
    }

    /* Quick return */
    if ( N == 0 )
        return PLASMA_SUCCESS;

    /* Tune NB & IB depending on M, N & NRHS; Set NBNB */
    status = plasma_tune(PLASMA_FUNC_DSGELS, N, N, NRHS);
    if (status != PLASMA_SUCCESS) {
        plasma_error("PLASMA_dsungesv", "plasma_tune() failed");
        return status;
    }

    NB = PLASMA_NB;

    plasma_sequence_create(plasma, &sequence);

    /* DOUBLE PRECISION INITIALIZATION */
    if ( PLASMA_TRANSLATION == PLASMA_OUTOFPLACE ) {
        plasma_dooplap2tile( descA, A, NB, NB, LDA, N,    0, 0, N, N,    sequence, &request,
                             plasma_desc_mat_free(&(descA)) );
        plasma_dooplap2tile( descB, B, NB, NB, LDB, NRHS, 0, 0, N, NRHS, sequence, &request,
                             plasma_desc_mat_free(&(descA)); plasma_desc_mat_free(&(descB)) );
        plasma_ddesc_alloc(  descX, NB, NB, N, NRHS, 0, 0, N, NRHS, plasma_desc_mat_free(&(descA)); plasma_desc_mat_free(&(descB)); plasma_desc_mat_free(&(descX)) );
    } else {
        plasma_diplap2tile( descA, A, NB, NB, LDA, N,    0, 0, N, N,
                            sequence, &request);
        plasma_diplap2tile( descB, B, NB, NB, LDB, NRHS, 0, 0, N, NRHS,
                            sequence, &request);

        descX = plasma_desc_init(
            PlasmaRealDouble, NB, NB, (NB*NB),
            LDX, NRHS, 0, 0, N, NRHS);
        descX.mat = X;
    }

    /* Allocate workspace */
    PLASMA_Alloc_Workspace_dgels_Tile(N, N, &descT);

    /* Call the native interface */
    status = PLASMA_dsungesv_Tile_Async(PlasmaNoTrans, &descA, descT, &descB, &descX, ITER,
                                        sequence, &request);

    if (status == PLASMA_SUCCESS) {
        if ( PLASMA_TRANSLATION == PLASMA_OUTOFPLACE ) {
            plasma_dooptile2lap( descX, X, NB, NB, LDX, NRHS,  sequence, &request);
            plasma_dynamic_sync();
            plasma_desc_mat_free(&descA);
            plasma_desc_mat_free(&descB);
            plasma_desc_mat_free(&descX);
        } else {
            plasma_diptile2lap( descA, A, NB, NB, LDA, N,     sequence, &request);
            plasma_diptile2lap( descB, B, NB, NB, LDB, NRHS,  sequence, &request);
            plasma_diptile2lap( descX, X, NB, NB, LDX, NRHS,  sequence, &request);
            plasma_dynamic_sync();
        }
    }

    PLASMA_Dealloc_Handle_Tile(&descT);
    plasma_sequence_destroy(plasma, sequence);
    return status;
}

/***************************************************************************//**
 *
 * @ingroup double_Tile
 *
 *  PLASMA_dsungesv_Tile - Solves symmetric linear system of equations using the tile QR
 *  or the tile LQ factorization and mixed-precision iterative refinement.
 *  Tile equivalent of PLASMA_dsungesv().
 *  Operates on matrices stored by tiles.
 *  All matrices are passed through descriptors.
 *  All dimensions are taken from the descriptors.
 *
 *******************************************************************************
 *
 * @param[in] trans
 *          Intended usage:
 *          = PlasmaNoTrans:   the linear system involves A;
 *          = PlasmaTrans: the linear system involves A**H.
 *          Currently only PlasmaNoTrans is supported.
 *
 * @param[in,out] A
 *          - If the iterative refinement converged, A is not modified;
 *          - otherwise, it fell back to double precision solution, and
 *          on exit the M-by-N matrix A contains:
 *          if M >= N, A is overwritten by details of its QR factorization as returned by
 *                     PLASMA_dgeqrf;
 *          if M < N, A is overwritten by details of its LQ factorization as returned by
 *                      PLASMA_dgelqf.
 *
 * @param[out] T
 *          On exit:
 *          - if the iterative refinement converged, T is not modified;
 *          - otherwise, it fell back to double precision solution,
 *          and then T is an auxiliary factorization data.
 *
 * @param[in,out] B
 *          On entry, the M-by-NRHS matrix B of right hand side vectors, stored columnwise;
 * @param[in] B
 *          The N-by-NRHS matrix of right hand side matrix B.
 *
 * @param[out] X
 *          If return value = 0, X is the solution vectors, stored columnwise:
 *          if M >= N, rows 1 to N of X contain the least squares solution vectors; the residual
 *          sum of squares for the solution in each column is given by the sum of squares of the
 *          modulus of elements N+1 to M in that column;
 *          if M < N, rows 1 to N of X contain the minimum norm solution vectors;
 *
 * @param[out] ITER
 *          The number of the current iteration in the iterative refinement process
 *
 *******************************************************************************
 *
 * @return
 *          \retval PLASMA_SUCCESS successful exit
 *
 *******************************************************************************
 *
 * @sa PLASMA_dsungesv
 * @sa PLASMA_dsungesv_Tile_Async
 * @sa PLASMA_dsungesv_Tile
 * @sa PLASMA_dgels_Tile
 *
 ******************************************************************************/
int PLASMA_dsungesv_Tile(PLASMA_enum trans, PLASMA_desc *A, PLASMA_desc *T,
                         PLASMA_desc *B, PLASMA_desc *X, int *ITER)
{
    plasma_context_t *plasma;
    PLASMA_sequence *sequence = NULL;
    PLASMA_request request = PLASMA_REQUEST_INITIALIZER;
    int status;

    plasma = plasma_context_self();
    if (plasma == NULL) {
        plasma_fatal_error("PLASMA_dsungesv_Tile", "PLASMA not initialized");
        return PLASMA_ERR_NOT_INITIALIZED;
    }
    plasma_sequence_create(plasma, &sequence);
    status = PLASMA_dsungesv_Tile_Async(trans, A, T, B, X, ITER, sequence, &request);
    if (status != PLASMA_SUCCESS)
        return status;
    plasma_dynamic_sync();
    status = sequence->status;
    plasma_sequence_destroy(plasma, sequence);
    return status;
}

/***************************************************************************//**
 *
 * @ingroup double_Tile_Async
 *
 *  PLASMA_dsungesv_Tile_Async - Solves symmetric linear system of equations using
 *  the tile QR or the tile LQ factorization and mixed-precision iterative refinement.
 *  Non-blocking equivalent of PLASMA_dsungesv_Tile().
 *  May return before the computation is finished.
 *  Allows for pipelining of operations at runtime.
 *
 *******************************************************************************
 *
 * @param[in] sequence
 *          Identifies the sequence of function calls that this call belongs to
 *          (for completion checks and exception handling purposes).
 *
 * @param[out] request
 *          Identifies this function call (for exception handling purposes).
 *
 *******************************************************************************
 *
 * @sa PLASMA_dsungesv
 * @sa PLASMA_dsungesv_Tile
 * @sa PLASMA_dsungesv_Tile_Async
 * @sa PLASMA_dgels_Tile_Async
 *
 ******************************************************************************/
int PLASMA_dsungesv_Tile_Async(PLASMA_enum trans, PLASMA_desc *A, PLASMA_desc *T,
                               PLASMA_desc *B, PLASMA_desc *X, int *ITER,
                               PLASMA_sequence *sequence, PLASMA_request *request)
{
    int N, NB, IB;
    PLASMA_desc descA;
    PLASMA_desc descT;
    PLASMA_desc descB;
    PLASMA_desc descX;
    PLASMA_desc descR, descSA, descST, descSX;
    plasma_context_t *plasma;
    double *work;

    const int    itermax = 30;
    const double bwdmax  = 1.0;
    const double negone = -1.0;
    const double one = 1.0;
    int iiter;
    double Anorm, cte, eps, Rnorm, Xnorm;
    *ITER=0;

    plasma = plasma_context_self();
    if (plasma == NULL) {
        plasma_fatal_error("PLASMA_dsungesv_Tile", "PLASMA not initialized");
        return PLASMA_ERR_NOT_INITIALIZED;
    }
    if (sequence == NULL) {
        plasma_fatal_error("PLASMA_dsungesv_Tile", "NULL sequence");
        return PLASMA_ERR_UNALLOCATED;
    }
    if (request == NULL) {
        plasma_fatal_error("PLASMA_dsungesv_Tile", "NULL request");
        return PLASMA_ERR_UNALLOCATED;
    }
    /* Check sequence status */
    if (sequence->status == PLASMA_SUCCESS)
        request->status = PLASMA_SUCCESS;
    else
        return plasma_request_fail(sequence, request, PLASMA_ERR_SEQUENCE_FLUSHED);
    /* Check descriptors for correctness */
    if (plasma_desc_check(A) != PLASMA_SUCCESS) {
        plasma_error("PLASMA_dsungesv_Tile", "invalid first descriptor");
        return PLASMA_ERR_ILLEGAL_VALUE;
    } else {
        descA = *A;
    }
    if (plasma_desc_check(T) != PLASMA_SUCCESS) {
        plasma_error("PLASMA_dsungesv_Tile", "invalid second descriptor");
        return PLASMA_ERR_ILLEGAL_VALUE;
    } else {
        descT = *T;
    }
    if (plasma_desc_check(B) != PLASMA_SUCCESS) {
        plasma_error("PLASMA_dsungesv_Tile", "invalid third descriptor");
        return PLASMA_ERR_ILLEGAL_VALUE;
    } else {
        descB = *B;
    }
    if (plasma_desc_check(X) != PLASMA_SUCCESS) {
        plasma_error("PLASMA_dsungesv_Tile", "invalid fourth descriptor");
        return PLASMA_ERR_ILLEGAL_VALUE;
    } else {
        descX = *X;
    }
    /* Check input arguments */
    if ( (descA.nb != descA.mb) || (descB.nb != descB.mb) || (descX.nb != descX.mb) ||
         (descA.mb != descB.mb) || (descB.mb != descX.mb) ) {
        plasma_error("PLASMA_dsungesv_Tile", "only square tiles of same size are supported");
        return PLASMA_ERR_ILLEGAL_VALUE;
    }
    if (trans != PlasmaNoTrans) {
        plasma_error("PLASMA_dsungesv_Tile", "only PlasmaNoTrans supported");
        return PLASMA_ERR_NOT_SUPPORTED;
    }

    /* Set N, NRHS, NB */
    N  = descA.m;
    NB = descA.nb;
    IB = descT.mb;

    work = (double *)plasma_shared_alloc(plasma, PLASMA_SIZE, PlasmaRealDouble);
    if (work == NULL) {
        plasma_error("PLASMA_dsungesv", "plasma_shared_alloc() failed");
        plasma_shared_free(plasma, work);
        return PLASMA_ERR_OUT_OF_RESOURCES;
    }

    plasma_ddesc_alloc( descR,  NB, NB, descB.m, descB.n, 0, 0, descB.m, descB.n, plasma_shared_free( plasma, work ); plasma_desc_mat_free(&descR) );
    plasma_sdesc_alloc( descSA, NB, NB, descA.m, descA.n, 0, 0, descA.m, descA.n, plasma_shared_free( plasma, work ); plasma_desc_mat_free(&descR); plasma_desc_mat_free(&descSA) );
    plasma_sdesc_alloc( descST, IB, NB, descT.m, descT.n, 0, 0, descT.m, descT.n, plasma_shared_free( plasma, work ); plasma_desc_mat_free(&descR); plasma_desc_mat_free(&descSA); plasma_desc_mat_free(&descST) );
    plasma_sdesc_alloc( descSX, NB, NB, descX.m, descX.n, 0, 0, descX.m, descX.n, plasma_shared_free( plasma, work ); plasma_desc_mat_free(&descR); plasma_desc_mat_free(&descSA); plasma_desc_mat_free(&descST); plasma_desc_mat_free(&descSX) );

    /* Compute some constants */
    PLASMA_dlange(PlasmaInfNorm, descA, Anorm, work);
    eps = LAPACKE_dlamch_work('e');

    /* Convert B from double precision to single precision and store
       the result in SX. */
    PLASMA_dlag2s(descB, descSX);
    if (sequence->status != PLASMA_SUCCESS)
        return plasma_request_fail(sequence, request, PLASMA_ERR_SEQUENCE_FLUSHED);

    /* Convert A from double precision to single precision and store
       the result in SA. */
    PLASMA_dlag2s(descA, descSA);
    if (sequence->status != PLASMA_SUCCESS)
        return plasma_request_fail(sequence, request, PLASMA_ERR_SEQUENCE_FLUSHED);

    /* Compute the QR factorization of SA */
    plasma_parallel_call_4(plasma_psgeqrf,
        PLASMA_desc, descSA,
        PLASMA_desc, descST,
        PLASMA_sequence*, sequence,
        PLASMA_request*, request);

    /* Compute the solve in simple */
    plasma_parallel_call_7(plasma_psormqr,
        PLASMA_enum, PlasmaLeft,
        PLASMA_enum, PlasmaTrans,
        PLASMA_desc, descSA,
        PLASMA_desc, descSX,
        PLASMA_desc, descST,
        PLASMA_sequence*, sequence,
        PLASMA_request*, request);

    plasma_parallel_call_9(plasma_pstrsm,
        PLASMA_enum, PlasmaLeft,
        PLASMA_enum, PlasmaUpper,
        PLASMA_enum, PlasmaNoTrans,
        PLASMA_enum, PlasmaNonUnit,
        float, 1.0,
        PLASMA_desc, descSA,
        PLASMA_desc, descSX,
        PLASMA_sequence*, sequence,
        PLASMA_request*, request);

    /* Convert SX back to double precision */
    PLASMA_slag2d(descSX, descX);

    /* Compute R = B - AX. */
    PLASMA_dlacpy(descB, descR);

    plasma_parallel_call_9(plasma_pdgemm,
        PLASMA_enum, PlasmaNoTrans,
        PLASMA_enum, PlasmaNoTrans,
        double, negone,
        PLASMA_desc, descA,
        PLASMA_desc, descX,
        double, one,
        PLASMA_desc, descR,
        PLASMA_sequence*, sequence,
        PLASMA_request*, request);

    /* Check whether the NRHS normwise backward error satisfies the
       stopping criterion. If yes return. Note that ITER=0 (already set). */
    PLASMA_dlange(PlasmaInfNorm, descX, Xnorm, work);
    PLASMA_dlange(PlasmaInfNorm, descR, Rnorm, work);

    /* Wait for the end of Anorm, Xnorm and Bnorm computations */
    plasma_dynamic_sync();

    cte = Anorm*eps*((double) N)*bwdmax;
    if (Rnorm < Xnorm * cte){
        /* The NRHS normwise backward errors satisfy the
           stopping criterion. We are good to exit. */
        plasma_desc_mat_free(&descSA);
        plasma_desc_mat_free(&descST);
        plasma_desc_mat_free(&descSX);
        plasma_desc_mat_free(&descR);
        plasma_shared_free(plasma, work);
        return PLASMA_SUCCESS;
    }

    /* Iterative refinement */
    for (iiter = 0; iiter < itermax; iiter++){

        /* Convert R from double precision to single precision
           and store the result in SX. */
        PLASMA_dlag2s(descR, descSX);

        plasma_parallel_call_7(plasma_psormqr,
            PLASMA_enum, PlasmaLeft,
            PLASMA_enum, PlasmaTrans,
            PLASMA_desc, descSA,
            PLASMA_desc, descSX,
            PLASMA_desc, descST,
            PLASMA_sequence*, sequence,
            PLASMA_request*, request);

        plasma_parallel_call_9(plasma_pstrsm,
            PLASMA_enum, PlasmaLeft,
            PLASMA_enum, PlasmaUpper,
            PLASMA_enum, PlasmaNoTrans,
            PLASMA_enum, PlasmaNonUnit,
            float, (float)1.0,
            PLASMA_desc, descSA,
            PLASMA_desc, descSX,
            PLASMA_sequence*, sequence,
            PLASMA_request*, request);

        /* Convert SX back to double precision and update the current
           iterate. */
        PLASMA_slag2d(descSX, descR);
        PLASMA_dgeadd(one, descR, descX);

        /* Compute R = B - AX. */
        PLASMA_dlacpy(descB,descR);
        plasma_parallel_call_9(plasma_pdgemm,
            PLASMA_enum, PlasmaNoTrans,
            PLASMA_enum, PlasmaNoTrans,
            double, negone,
            PLASMA_desc, descA,
            PLASMA_desc, descX,
            double, one,
            PLASMA_desc, descR,
            PLASMA_sequence*, sequence,
            PLASMA_request*, request);

        /* Check whether the NRHS normwise backward errors satisfy the
           stopping criterion. If yes, set ITER=IITER>0 and return. */
        PLASMA_dlange(PlasmaInfNorm, descX, Xnorm, work);
        PLASMA_dlange(PlasmaInfNorm, descR, Rnorm, work);

        /* Wait for the end of Xnorm and Bnorm computations */
        plasma_dynamic_sync();

        if (Rnorm < Xnorm * cte){
            /* The NRHS normwise backward errors satisfy the
               stopping criterion. We are good to exit. */
            *ITER = iiter;

            plasma_desc_mat_free(&descSA);
            plasma_desc_mat_free(&descST);
            plasma_desc_mat_free(&descSX);
            plasma_desc_mat_free(&descR);
            plasma_shared_free(plasma, work);
            return PLASMA_SUCCESS;
        }
    }

    /* We have performed ITER=itermax iterations and never satisified
       the stopping criterion, set up the ITER flag accordingly and
       follow up on double precision routine. */
    *ITER = -itermax - 1;

    plasma_desc_mat_free(&descSA);
    plasma_desc_mat_free(&descST);
    plasma_desc_mat_free(&descSX);
    plasma_desc_mat_free(&descR);
    plasma_shared_free(plasma, work);

    /* Single-precision iterative refinement failed to converge to a
       satisfactory solution, so we restart to double precision. */
    PLASMA_dlacpy(descB, descX);

    plasma_parallel_call_4(plasma_pdgeqrf,
        PLASMA_desc, descA,
        PLASMA_desc, descT,
        PLASMA_sequence*, sequence,
        PLASMA_request*, request);

    plasma_parallel_call_7(plasma_pdormqr,
        PLASMA_enum, PlasmaLeft,
        PLASMA_enum, PlasmaTrans,
        PLASMA_desc, descA,
        PLASMA_desc, descX,
        PLASMA_desc, descT,
        PLASMA_sequence*, sequence,
        PLASMA_request*, request);

    plasma_parallel_call_9(plasma_pdtrsm,
        PLASMA_enum, PlasmaLeft,
        PLASMA_enum, PlasmaUpper,
        PLASMA_enum, PlasmaNoTrans,
        PLASMA_enum, PlasmaNonUnit,
        double, (double)1.0,
        PLASMA_desc, descA,
        PLASMA_desc, descX,
        PLASMA_sequence*, sequence,
        PLASMA_request*, request);

    return PLASMA_SUCCESS;
}


#include <stdio.h>
#include <gsl/gsl_matrix.h>
#include "bstrlib/bstrlib.h"
#include "paths.h"
#include "ParseSCF.h"
#include "HTightBinding.h"
#include "BandEnergy.h"
#include "SpinOrbit.h"

int main(int argc, char *argv[]) {
    if (argc < 8) {
        printf("SOC-induced anisotropy calculation -- invoke with:\n");
        printf("anisotropy.out (system_name) (na) (nb) (nc) (soc_strength) (theta1) (phi1) (theta2) (phi2)\n");
        return 1;
    }
    char *system_name = argv[1];
    int na = atoi(argv[2]);
    int nb = atoi(argv[3]);
    int nc = atoi(argv[4]);
    double soc_strength = atof(argv[5]);
    double theta1 = atof(argv[6]);
    double phi1 = atof(argv[7]);
    double theta2 = atof(argv[8]);
    double phi2 = atof(argv[9]);

    char *scf_path = cwannier_data_path(system_name, "wannier", "\0", "scf.out");
    double num_electrons, alat;
    gsl_matrix *R = gsl_matrix_alloc(3, 3);

    int err = ParseSCF(scf_path, &num_electrons, &alat, R);
    if (err != CWANNIER_PARSESCF_OK) {
        printf("Error code = %d returned from ParseSCF\n", err);
        return err;
    }
    bcstrfree(scf_path);

    char *hr_up_path = cwannier_data_path(system_name, "wannier", system_name, "_up_hr.dat");
    HTightBinding *Hrs_up = ExtractHTightBinding(hr_up_path);
    bcstrfree(hr_up_path);

    char *hr_dn_path = cwannier_data_path(system_name, "wannier", system_name, "_dn_hr.dat");
    HTightBinding *Hrs_dn = ExtractHTightBinding(hr_dn_path);
    bcstrfree(hr_dn_path);

    printf("Hamiltonian loaded.\n");

    HTightBinding *Hrs_soc_1 = HamiltonianWithSOC(soc_strength, theta1, phi1, Hrs_up, Hrs_dn);
    HTightBinding *Hrs_soc_2 = HamiltonianWithSOC(soc_strength, theta2, phi2, Hrs_up, Hrs_dn);

    bool use_cache = true;
    double E_Fermi_1 = 0.0;
    double E_Fermi_2 = 0.0;

    double energy1 = BandEnergy(&E_Fermi_1, Hrs_soc_1, R, num_electrons, na, nb, nc, use_cache);
    printf("Got energy1 = %f\n", energy1);

    double energy2 = BandEnergy(&E_Fermi_2, Hrs_soc_2, R, num_electrons, na, nb, nc, use_cache);
    printf("Got energy2 = %f\n", energy2);

    printf("energy1 - energy2 = %e\n", energy1 - energy2);

    FreeHTightBinding(Hrs_up);
    FreeHTightBinding(Hrs_dn);
    FreeHTightBinding(Hrs_soc_1);
    FreeHTightBinding(Hrs_soc_2);
    return 0;
}


#include <gsl/blas/cblas.h>

/**
	All the methods below are obtained directly from the ATLAS implementation of BLAS, available at: http://www.netlib.org/clapack/cblas/
*/

#include "f2c.h"

doublereal ddot(integer *n, doublereal *dx, integer *incx, doublereal *dy, integer *incy){

    /* System generated locals */
    integer i__1;
    doublereal ret_val;

    /* Local variables */
    static integer i, m;
    static doublereal dtemp;
    static integer ix, iy, mp1;

/*     forms the dot product of two vectors.
       uses unrolled loops for increments equal to one.
       jack dongarra, linpack, 3/11/78.
       modified 12/3/93, array(1) declarations changed to array(*)

   Parameter adjustments
       Function Body */
#define DY(I) dy[(I)-1]
#define DX(I) dx[(I)-1]

    ret_val = 0.;
    dtemp = 0.;
    if (*n <= 0) {
	return ret_val;
    }
    if (*incx == 1 && *incy == 1) {
	goto L20;
    }

/*        code for unequal increments or equal increments
            not equal to 1 */

    ix = 1;
    iy = 1;
    if (*incx < 0) {
	ix = (-(*n) + 1) * *incx + 1;
    }
    if (*incy < 0) {
	iy = (-(*n) + 1) * *incy + 1;
    }
    i__1 = *n;
    for (i = 1; i <= *n; ++i) {
	dtemp += DX(ix) * DY(iy);
	ix += *incx;
	iy += *incy;
/* L10: */
    }
    ret_val = dtemp;
    return ret_val;

/*        code for both increments equal to 1

          clean-up loop */

L20:
    m = *n % 5;
    if (m == 0) {
	goto L40;
    }
    i__1 = m;
    for (i = 1; i <= m; ++i) {
	dtemp += DX(i) * DY(i);
/* L30: */
    }
    if (*n < 5) {
	goto L60;
    }
L40:
    mp1 = m + 1;
    i__1 = *n;
    for (i = mp1; i <= *n; i += 5) {
	dtemp = dtemp + DX(i) * DY(i) + DX(i + 1) * DY(i + 1) + DX(i + 2) *
		DY(i + 2) + DX(i + 3) * DY(i + 3) + DX(i + 4) * DY(i + 4);
/* L50: */
    }
L60:
    ret_val = dtemp;
    return ret_val;
} /* ddot_ */

doublereal dnrm2(integer *n, doublereal *x, integer *incx)
{
    /* System generated locals */
    integer i__1, i__2;
    doublereal ret_val, d__1;

    /* Builtin functions */
    double sqrt(doublereal);

    /* Local variables */
    static doublereal norm, scale, absxi;
    static integer ix;
    static doublereal ssq;

/*  DNRM2 returns the euclidean norm of a vector via the function
    name, so that

       DNRM2 := sqrt( x'*x )

    -- This version written on 25-October-1982.
       Modified on 14-October-1993 to inline the call to DLASSQ.
       Sven Hammarling, Nag Ltd.

   Parameter adjustments
       Function Body */
#define X(I) x[(I)-1]

    if (*n < 1 || *incx < 1) {
	norm = 0.;
    } else if (*n == 1) {
	norm = abs(X(1));
    } else {
	scale = 0.;
	ssq = 1.;
/*        The following loop is equivalent to this call to the LAPACK

          auxiliary routine:
          CALL DLASSQ( N, X, INCX, SCALE, SSQ ) */

	i__1 = (*n - 1) * *incx + 1;
	i__2 = *incx;
	for (ix = 1; *incx < 0 ? ix >= (*n-1)**incx+1 : ix <= (*n-1)**incx+1; ix += *incx) {
	    if (X(ix) != 0.) {
		absxi = (d__1 = X(ix), abs(d__1));
		if (scale < absxi) {
/* Computing 2nd power */
		    d__1 = scale / absxi;
		    ssq = ssq * (d__1 * d__1) + 1.;
		    scale = absxi;
		} else {
/* Computing 2nd power */
		    d__1 = absxi / scale;
		    ssq += d__1 * d__1;
		}
	    }
/* L10: */
	}
	norm = scale * sqrt(ssq);
    }

    ret_val = norm;
    return ret_val;

/*     End of DNRM2. */

} /* dnrm2_ */

int dswap(integer *n, doublereal *dx, integer *incx, doublereal *dy, integer *incy)
{

    /* System generated locals */
    integer i__1;

    /* Local variables */
    static integer i, m;
    static doublereal dtemp;
    static integer ix, iy, mp1;

/*     interchanges two vectors.
       uses unrolled loops for increments equal one.
       jack dongarra, linpack, 3/11/78.
       modified 12/3/93, array(1) declarations changed to array(*)

   Parameter adjustments
       Function Body */
#define DY(I) dy[(I)-1]
#define DX(I) dx[(I)-1]

    if (*n <= 0) {
	return 0;
    }
    if (*incx == 1 && *incy == 1) {
	goto L20;
    }

/*       code for unequal increments or equal increments not equal
           to 1 */

    ix = 1;
    iy = 1;
    if (*incx < 0) {
	ix = (-(*n) + 1) * *incx + 1;
    }
    if (*incy < 0) {
	iy = (-(*n) + 1) * *incy + 1;
    }
    i__1 = *n;
    for (i = 1; i <= *n; ++i) {
	dtemp = DX(ix);
	DX(ix) = DY(iy);
	DY(iy) = dtemp;
	ix += *incx;
	iy += *incy;
/* L10: */
    }
    return 0;

/*       code for both increments equal to 1

         clean-up loop */

L20:
    m = *n % 3;
    if (m == 0) {
	goto L40;
    }
    i__1 = m;
    for (i = 1; i <= m; ++i) {
	dtemp = DX(i);
	DX(i) = DY(i);
	DY(i) = dtemp;
/* L30: */
    }
    if (*n < 3) {
	return 0;
    }
L40:
    mp1 = m + 1;
    i__1 = *n;
    for (i = mp1; i <= *n; i += 3) {
	dtemp = DX(i);
	DX(i) = DY(i);
	DY(i) = dtemp;
	dtemp = DX(i + 1);
	DX(i + 1) = DY(i + 1);
	DY(i + 1) = dtemp;
	dtemp = DX(i + 2);
	DX(i + 2) = DY(i + 2);
	DY(i + 2) = dtemp;
/* L50: */
    }
    return 0;
} /* dswap_ */

int dcopy(integer *n, doublereal *dx, integer *incx, doublereal *dy, integer *incy)
{

    /* System generated locals */
    integer i__1;

    /* Local variables */
    static integer i, m, ix, iy, mp1;

/*     copies a vector, x, to a vector, y.
       uses unrolled loops for increments equal to one.
       jack dongarra, linpack, 3/11/78.
       modified 12/3/93, array(1) declarations changed to array(*)

   Parameter adjustments
       Function Body */
#define DY(I) dy[(I)-1]
#define DX(I) dx[(I)-1]

    if (*n <= 0) {
	return 0;
    }
    if (*incx == 1 && *incy == 1) {
	goto L20;
    }

/*        code for unequal increments or equal increments
            not equal to 1 */

    ix = 1;
    iy = 1;
    if (*incx < 0) {
	ix = (-(*n) + 1) * *incx + 1;
    }
    if (*incy < 0) {
	iy = (-(*n) + 1) * *incy + 1;
    }
    i__1 = *n;
    for (i = 1; i <= *n; ++i) {
	DY(iy) = DX(ix);
	ix += *incx;
	iy += *incy;
/* L10: */
    }
    return 0;

/*        code for both increments equal to 1

          clean-up loop */

L20:
    m = *n % 7;
    if (m == 0) {
	goto L40;
    }
    i__1 = m;
    for (i = 1; i <= m; ++i) {
	DY(i) = DX(i);
/* L30: */
    }
    if (*n < 7) {
	return 0;
    }
L40:
    mp1 = m + 1;
    i__1 = *n;
    for (i = mp1; i <= *n; i += 7) {
	DY(i) = DX(i);
	DY(i + 1) = DX(i + 1);
	DY(i + 2) = DX(i + 2);
	DY(i + 3) = DX(i + 3);
	DY(i + 4) = DX(i + 4);
	DY(i + 5) = DX(i + 5);
	DY(i + 6) = DX(i + 6);
/* L50: */
    }
    return 0;
} /* dcopy_ */

int dgemm(char *transa, char *transb, integer *m, integer *	n, integer *k, doublereal *alpha, doublereal *a, integer *lda, doublereal *b, integer *ldb, doublereal *beta, doublereal *c, integer *ldc)
{

    /* System generated locals */
    integer a_dim1, a_offset, b_dim1, b_offset, c_dim1, c_offset, i__1, i__2,
	    i__3;

    /* Local variables */
    static integer info;
    static logical nota, notb;
    static doublereal temp;
    static integer i, j, l, ncola;
    extern logical lsame_(char *, char *);
    static integer nrowa, nrowb;
    extern /* Subroutine */ int xerbla_(char *, integer *);

/*  Purpose
    =======

    DGEMM  performs one of the matrix-matrix operations

       C := alpha*op( A )*op( B ) + beta*C,

    where  op( X ) is one of

       op( X ) = X   or   op( X ) = X',

    alpha and beta are scalars, and A, B and C are matrices, with op( A )

    an m by k matrix,  op( B )  a  k by n matrix and  C an m by n matrix.

    Parameters
    ==========

    TRANSA - CHARACTER*1.
             On entry, TRANSA specifies the form of op( A ) to be used in

             the matrix multiplication as follows:

                TRANSA = 'N' or 'n',  op( A ) = A.

                TRANSA = 'T' or 't',  op( A ) = A'.

                TRANSA = 'C' or 'c',  op( A ) = A'.

             Unchanged on exit.

    TRANSB - CHARACTER*1.
             On entry, TRANSB specifies the form of op( B ) to be used in

             the matrix multiplication as follows:

                TRANSB = 'N' or 'n',  op( B ) = B.

                TRANSB = 'T' or 't',  op( B ) = B'.

                TRANSB = 'C' or 'c',  op( B ) = B'.

             Unchanged on exit.

    M      - INTEGER.
             On entry,  M  specifies  the number  of rows  of the  matrix

             op( A )  and of the  matrix  C.  M  must  be at least  zero.

             Unchanged on exit.

    N      - INTEGER.
             On entry,  N  specifies the number  of columns of the matrix

             op( B ) and the number of columns of the matrix C. N must be

             at least zero.
             Unchanged on exit.

    K      - INTEGER.
             On entry,  K  specifies  the number of columns of the matrix

             op( A ) and the number of rows of the matrix op( B ). K must

             be at least  zero.
             Unchanged on exit.

    ALPHA  - DOUBLE PRECISION.
             On entry, ALPHA specifies the scalar alpha.
             Unchanged on exit.

    A      - DOUBLE PRECISION array of DIMENSION ( LDA, ka ), where ka is

             k  when  TRANSA = 'N' or 'n',  and is  m  otherwise.
             Before entry with  TRANSA = 'N' or 'n',  the leading  m by k

             part of the array  A  must contain the matrix  A,  otherwise

             the leading  k by m  part of the array  A  must contain  the

             matrix A.
             Unchanged on exit.

    LDA    - INTEGER.
             On entry, LDA specifies the first dimension of A as declared

             in the calling (sub) program. When  TRANSA = 'N' or 'n' then

             LDA must be at least  max( 1, m ), otherwise  LDA must be at

             least  max( 1, k ).
             Unchanged on exit.

    B      - DOUBLE PRECISION array of DIMENSION ( LDB, kb ), where kb is

             n  when  TRANSB = 'N' or 'n',  and is  k  otherwise.
             Before entry with  TRANSB = 'N' or 'n',  the leading  k by n

             part of the array  B  must contain the matrix  B,  otherwise

             the leading  n by k  part of the array  B  must contain  the

             matrix B.
             Unchanged on exit.

    LDB    - INTEGER.
             On entry, LDB specifies the first dimension of B as declared

             in the calling (sub) program. When  TRANSB = 'N' or 'n' then

             LDB must be at least  max( 1, k ), otherwise  LDB must be at

             least  max( 1, n ).
             Unchanged on exit.

    BETA   - DOUBLE PRECISION.
             On entry,  BETA  specifies the scalar  beta.  When  BETA  is

             supplied as zero then C need not be set on input.
             Unchanged on exit.

    C      - DOUBLE PRECISION array of DIMENSION ( LDC, n ).
             Before entry, the leading  m by n  part of the array  C must

             contain the matrix  C,  except when  beta  is zero, in which

             case C need not be set on entry.
             On exit, the array  C  is overwritten by the  m by n  matrix

             ( alpha*op( A )*op( B ) + beta*C ).

    LDC    - INTEGER.
             On entry, LDC specifies the first dimension of C as declared

             in  the  calling  (sub)  program.   LDC  must  be  at  least

             max( 1, m ).
             Unchanged on exit.

    Level 3 Blas routine.

    -- Written on 8-February-1989.
       Jack Dongarra, Argonne National Laboratory.
       Iain Duff, AERE Harwell.
       Jeremy Du Croz, Numerical Algorithms Group Ltd.
       Sven Hammarling, Numerical Algorithms Group Ltd.

       Set  NOTA  and  NOTB  as  true if  A  and  B  respectively are not

       transposed and set  NROWA, NCOLA and  NROWB  as the number of rows

       and  columns of  A  and the  number of  rows  of  B  respectively.

   Parameter adjustments
       Function Body */

#define A(I,J) a[(I)-1 + ((J)-1)* ( *lda)]
#define B(I,J) b[(I)-1 + ((J)-1)* ( *ldb)]
#define C(I,J) c[(I)-1 + ((J)-1)* ( *ldc)]

    nota = lsame_(transa, "N");
    notb = lsame_(transb, "N");
    if (nota) {
	nrowa = *m;
	ncola = *k;
    } else {
	nrowa = *k;
	ncola = *m;
    }
    if (notb) {
	nrowb = *k;
    } else {
	nrowb = *n;
    }

/*     Test the input parameters. */

    info = 0;
    if (! nota && ! lsame_(transa, "C") && ! lsame_(transa, "T")) {
	info = 1;
    } else if (! notb && ! lsame_(transb, "C") && ! lsame_(transb,
	    "T")) {
	info = 2;
    } else if (*m < 0) {
	info = 3;
    } else if (*n < 0) {
	info = 4;
    } else if (*k < 0) {
	info = 5;
    } else if (*lda < max(1,nrowa)) {
	info = 8;
    } else if (*ldb < max(1,nrowb)) {
	info = 10;
    } else if (*ldc < max(1,*m)) {
	info = 13;
    }
    if (info != 0) {
	xerbla_("DGEMM ", &info);
	return 0;
    }

/*     Quick return if possible. */

    if (*m == 0 || *n == 0 || (*alpha == 0. || *k == 0) && *beta == 1.) {
	return 0;
    }

/*     And if  alpha.eq.zero. */

    if (*alpha == 0.) {
	if (*beta == 0.) {
	    i__1 = *n;
	    for (j = 1; j <= *n; ++j) {
		i__2 = *m;
		for (i = 1; i <= *m; ++i) {
		    C(i,j) = 0.;
/* L10: */
		}
/* L20: */
	    }
	} else {
	    i__1 = *n;
	    for (j = 1; j <= *n; ++j) {
		i__2 = *m;
		for (i = 1; i <= *m; ++i) {
		    C(i,j) = *beta * C(i,j);
/* L30: */
		}
/* L40: */
	    }
	}
	return 0;
    }

/*     Start the operations. */

    if (notb) {
	if (nota) {

/*           Form  C := alpha*A*B + beta*C. */

	    i__1 = *n;
	    for (j = 1; j <= *n; ++j) {
		if (*beta == 0.) {
		    i__2 = *m;
		    for (i = 1; i <= *m; ++i) {
			C(i,j) = 0.;
/* L50: */
		    }
		} else if (*beta != 1.) {
		    i__2 = *m;
		    for (i = 1; i <= *m; ++i) {
			C(i,j) = *beta * C(i,j);
/* L60: */
		    }
		}
		i__2 = *k;
		for (l = 1; l <= *k; ++l) {
		    if (B(l,j) != 0.) {
			temp = *alpha * B(l,j);
			i__3 = *m;
			for (i = 1; i <= *m; ++i) {
			    C(i,j) += temp * A(i,l);
/* L70: */
			}
		    }
/* L80: */
		}
/* L90: */
	    }
	} else {

/*           Form  C := alpha*A'*B + beta*C */

	    i__1 = *n;
	    for (j = 1; j <= *n; ++j) {
		i__2 = *m;
		for (i = 1; i <= *m; ++i) {
		    temp = 0.;
		    i__3 = *k;
		    for (l = 1; l <= *k; ++l) {
			temp += A(l,i) * B(l,j);
/* L100: */
		    }
		    if (*beta == 0.) {
			C(i,j) = *alpha * temp;
		    } else {
			C(i,j) = *alpha * temp + *beta * C(i,j);
		    }
/* L110: */
		}
/* L120: */
	    }
	}
    } else {
	if (nota) {

/*           Form  C := alpha*A*B' + beta*C */

	    i__1 = *n;
	    for (j = 1; j <= *n; ++j) {
		if (*beta == 0.) {
		    i__2 = *m;
		    for (i = 1; i <= *m; ++i) {
			C(i,j) = 0.;
/* L130: */
		    }
		} else if (*beta != 1.) {
		    i__2 = *m;
		    for (i = 1; i <= *m; ++i) {
			C(i,j) = *beta * C(i,j);
/* L140: */
		    }
		}
		i__2 = *k;
		for (l = 1; l <= *k; ++l) {
		    if (B(j,l) != 0.) {
			temp = *alpha * B(j,l);
			i__3 = *m;
			for (i = 1; i <= *m; ++i) {
			    C(i,j) += temp * A(i,l);
/* L150: */
			}
		    }
/* L160: */
		}
/* L170: */
	    }
	} else {

/*           Form  C := alpha*A'*B' + beta*C */

	    i__1 = *n;
	    for (j = 1; j <= *n; ++j) {
		i__2 = *m;
		for (i = 1; i <= *m; ++i) {
		    temp = 0.;
		    i__3 = *k;
		    for (l = 1; l <= *k; ++l) {
			temp += A(l,i) * B(j,l);
/* L180: */
		    }
		    if (*beta == 0.) {
			C(i,j) = *alpha * temp;
		    } else {
			C(i,j) = *alpha * temp + *beta * C(i,j);
		    }
/* L190: */
		}
/* L200: */
	    }
	}
    }

    return 0;

/*     End of DGEMM . */

} /* dgemm_ */

int dgemv(char *trans, integer *m, integer *n, doublereal *	alpha, doublereal *a, integer *lda, doublereal *x, integer *incx, doublereal *beta, doublereal *y, integer *incy)
{

    /* System generated locals */
    integer a_dim1, a_offset, i__1, i__2;

    /* Local variables */
    static integer info;
    static doublereal temp;
    static integer lenx, leny, i, j;
    extern logical lsame_(char *, char *);
    static integer ix, iy, jx, jy, kx, ky;
    extern /* Subroutine */ int xerbla_(char *, integer *);

/*  Purpose
    =======

    DGEMV  performs one of the matrix-vector operations

       y := alpha*A*x + beta*y,   or   y := alpha*A'*x + beta*y,

    where alpha and beta are scalars, x and y are vectors and A is an
    m by n matrix.

    Parameters
    ==========

    TRANS  - CHARACTER*1.
             On entry, TRANS specifies the operation to be performed as
             follows:

                TRANS = 'N' or 'n'   y := alpha*A*x + beta*y.

                TRANS = 'T' or 't'   y := alpha*A'*x + beta*y.

                TRANS = 'C' or 'c'   y := alpha*A'*x + beta*y.

             Unchanged on exit.

    M      - INTEGER.
             On entry, M specifies the number of rows of the matrix A.
             M must be at least zero.
             Unchanged on exit.

    N      - INTEGER.
             On entry, N specifies the number of columns of the matrix A.

             N must be at least zero.
             Unchanged on exit.

    ALPHA  - DOUBLE PRECISION.
             On entry, ALPHA specifies the scalar alpha.
             Unchanged on exit.

    A      - DOUBLE PRECISION array of DIMENSION ( LDA, n ).
             Before entry, the leading m by n part of the array A must
             contain the matrix of coefficients.
             Unchanged on exit.

    LDA    - INTEGER.
             On entry, LDA specifies the first dimension of A as declared

             in the calling (sub) program. LDA must be at least
             max( 1, m ).
             Unchanged on exit.

    X      - DOUBLE PRECISION array of DIMENSION at least
             ( 1 + ( n - 1 )*abs( INCX ) ) when TRANS = 'N' or 'n'
             and at least
             ( 1 + ( m - 1 )*abs( INCX ) ) otherwise.
             Before entry, the incremented array X must contain the
             vector x.
             Unchanged on exit.

    INCX   - INTEGER.
             On entry, INCX specifies the increment for the elements of
             X. INCX must not be zero.
             Unchanged on exit.

    BETA   - DOUBLE PRECISION.
             On entry, BETA specifies the scalar beta. When BETA is
             supplied as zero then Y need not be set on input.
             Unchanged on exit.

    Y      - DOUBLE PRECISION array of DIMENSION at least
             ( 1 + ( m - 1 )*abs( INCY ) ) when TRANS = 'N' or 'n'
             and at least
             ( 1 + ( n - 1 )*abs( INCY ) ) otherwise.
             Before entry with BETA non-zero, the incremented array Y
             must contain the vector y. On exit, Y is overwritten by the

             updated vector y.

    INCY   - INTEGER.
             On entry, INCY specifies the increment for the elements of
             Y. INCY must not be zero.
             Unchanged on exit.

    Level 2 Blas routine.

    -- Written on 22-October-1986.
       Jack Dongarra, Argonne National Lab.
       Jeremy Du Croz, Nag Central Office.
       Sven Hammarling, Nag Central Office.
       Richard Hanson, Sandia National Labs.

       Test the input parameters.

   Parameter adjustments
       Function Body */
#define X(I) x[(I)-1]
#define Y(I) y[(I)-1]

#define A(I,J) a[(I)-1 + ((J)-1)* ( *lda)]

    info = 0;
    if (! lsame_(trans, "N") && ! lsame_(trans, "T") && !
	    lsame_(trans, "C")) {
	info = 1;
    } else if (*m < 0) {
	info = 2;
    } else if (*n < 0) {
	info = 3;
    } else if (*lda < max(1,*m)) {
	info = 6;
    } else if (*incx == 0) {
	info = 8;
    } else if (*incy == 0) {
	info = 11;
    }
    if (info != 0) {
	xerbla_("DGEMV ", &info);
	return 0;
    }

/*     Quick return if possible. */

    if (*m == 0 || *n == 0 || *alpha == 0. && *beta == 1.) {
	return 0;
    }

/*     Set  LENX  and  LENY, the lengths of the vectors x and y, and set

       up the start points in  X  and  Y. */

    if (lsame_(trans, "N")) {
	lenx = *n;
	leny = *m;
    } else {
	lenx = *m;
	leny = *n;
    }
    if (*incx > 0) {
	kx = 1;
    } else {
	kx = 1 - (lenx - 1) * *incx;
    }
    if (*incy > 0) {
	ky = 1;
    } else {
	ky = 1 - (leny - 1) * *incy;
    }

/*     Start the operations. In this version the elements of A are
       accessed sequentially with one pass through A.

       First form  y := beta*y. */

    if (*beta != 1.) {
	if (*incy == 1) {
	    if (*beta == 0.) {
		i__1 = leny;
		for (i = 1; i <= leny; ++i) {
		    Y(i) = 0.;
/* L10: */
		}
	    } else {
		i__1 = leny;
		for (i = 1; i <= leny; ++i) {
		    Y(i) = *beta * Y(i);
/* L20: */
		}
	    }
	} else {
	    iy = ky;
	    if (*beta == 0.) {
		i__1 = leny;
		for (i = 1; i <= leny; ++i) {
		    Y(iy) = 0.;
		    iy += *incy;
/* L30: */
		}
	    } else {
		i__1 = leny;
		for (i = 1; i <= leny; ++i) {
		    Y(iy) = *beta * Y(iy);
		    iy += *incy;
/* L40: */
		}
	    }
	}
    }
    if (*alpha == 0.) {
	return 0;
    }
    if (lsame_(trans, "N")) {

/*        Form  y := alpha*A*x + y. */

	jx = kx;
	if (*incy == 1) {
	    i__1 = *n;
	    for (j = 1; j <= *n; ++j) {
		if (X(jx) != 0.) {
		    temp = *alpha * X(jx);
		    i__2 = *m;
		    for (i = 1; i <= *m; ++i) {
			Y(i) += temp * A(i,j);
/* L50: */
		    }
		}
		jx += *incx;
/* L60: */
	    }
	} else {
	    i__1 = *n;
	    for (j = 1; j <= *n; ++j) {
		if (X(jx) != 0.) {
		    temp = *alpha * X(jx);
		    iy = ky;
		    i__2 = *m;
		    for (i = 1; i <= *m; ++i) {
			Y(iy) += temp * A(i,j);
			iy += *incy;
/* L70: */
		    }
		}
		jx += *incx;
/* L80: */
	    }
	}
    } else {

/*        Form  y := alpha*A'*x + y. */

	jy = ky;
	if (*incx == 1) {
	    i__1 = *n;
	    for (j = 1; j <= *n; ++j) {
		temp = 0.;
		i__2 = *m;
		for (i = 1; i <= *m; ++i) {
		    temp += A(i,j) * X(i);
/* L90: */
		}
		Y(jy) += *alpha * temp;
		jy += *incy;
/* L100: */
	    }
	} else {
	    i__1 = *n;
	    for (j = 1; j <= *n; ++j) {
		temp = 0.;
		ix = kx;
		i__2 = *m;
		for (i = 1; i <= *m; ++i) {
		    temp += A(i,j) * X(ix);
		    ix += *incx;
/* L110: */
		}
		Y(jy) += *alpha * temp;
		jy += *incy;
/* L120: */
	    }
	}
    }

    return 0;

/*     End of DGEMV . */

} /* dgemv_ */

logical lsame_(char *ca, char *cb)
{

    /* System generated locals */
    logical ret_val;

    /* Local variables */
    static integer inta, intb, zcode;

/*  -- LAPACK auxiliary routine (version 2.0) --
       Univ. of Tennessee, Univ. of California Berkeley, NAG Ltd.,
       Courant Institute, Argonne National Lab, and Rice University
       January 31, 1994

    Purpose
    =======

    LSAME returns .TRUE. if CA is the same letter as CB regardless of
    case.

    Arguments
    =========

    CA      (input) CHARACTER*1
    CB      (input) CHARACTER*1
            CA and CB specify the single characters to be compared.

   =====================================================================

       Test if the characters are equal */

    ret_val = *(unsigned char *)ca == *(unsigned char *)cb;
    if (ret_val) {
	return ret_val;
    }

/*     Now test for equivalence if both characters are alphabetic. */

    zcode = 'Z';

/*     Use 'Z' rather than 'A' so that ASCII can be detected on Prime
       machines, on which ICHAR returns a value with bit 8 set.
       ICHAR('A') on Prime machines returns 193 which is the same as
       ICHAR('A') on an EBCDIC machine. */

    inta = *(unsigned char *)ca;
    intb = *(unsigned char *)cb;

    if (zcode == 90 || zcode == 122) {

/*        ASCII is assumed - ZCODE is the ASCII code of either lower o
r
          upper case 'Z'. */

	if (inta >= 97 && inta <= 122) {
	    inta += -32;
	}
	if (intb >= 97 && intb <= 122) {
	    intb += -32;
	}

    } else if (zcode == 233 || zcode == 169) {

/*        EBCDIC is assumed - ZCODE is the EBCDIC code of either lower
 or
          upper case 'Z'. */

	if (inta >= 129 && inta <= 137 || inta >= 145 && inta <= 153 || inta
		>= 162 && inta <= 169) {
	    inta += 64;
	}
	if (intb >= 129 && intb <= 137 || intb >= 145 && intb <= 153 || intb
		>= 162 && intb <= 169) {
	    intb += 64;
	}

    } else if (zcode == 218 || zcode == 250) {

/*        ASCII is assumed, on Prime machines - ZCODE is the ASCII cod
e
          plus 128 of either lower or upper case 'Z'. */

	if (inta >= 225 && inta <= 250) {
	    inta += -32;
	}
	if (intb >= 225 && intb <= 250) {
	    intb += -32;
	}
    }
    ret_val = inta == intb;

/*     RETURN

       End of LSAME */

    return ret_val;
} /* lsame_ */
int xerbla_(char *srname, integer *info)
{
/*  -- LAPACK auxiliary routine (version 2.0) --
       Univ. of Tennessee, Univ. of California Berkeley, NAG Ltd.,
       Courant Institute, Argonne National Lab, and Rice University
       September 30, 1994

    Purpose
    =======

    XERBLA  is an error handler for the LAPACK routines.
    It is called by an LAPACK routine if an input parameter has an
    invalid value.  A message is printed and execution stops.

    Installers may consider modifying the STOP statement in order to
    call system-specific exception-handling facilities.

    Arguments
    =========

    SRNAME  (input) CHARACTER*6
            The name of the routine which called XERBLA.

    INFO    (input) INTEGER
            The position of the invalid parameter in the parameter list

            of the calling routine.

   =====================================================================
*/

    printf("** On entry to %6s, parameter number %2i had an illegal value\n",
		srname, *info);

/*     End of XERBLA */

    return 0;
} /* xerbla_ */


#ifndef CHEVALIER_H
#define CHEVALIER_H

#include <math.h>
#include <gsl/gsl_roots.h>
#include <gsl/gsl_multiroots.h>

#define parsec_in_cm 3.085678e+18 //parsec in cm
#define parsec_in_km 3.085678e+13 //parsec in km
#define msun_in_g    1.988920e+33 //msun in g
#define year_in_sec  3.155760e+07 //year in s

typedef struct Chevalier
{
    double M_dot;               //mass input rate
    double E_dot;               //energy input rate
    double R;                   //limit of mass and energy input
    double gamma;               //adiabatic index
    double V;                   //Volume of input area
    double q;                   //M_dot / V
    double Q;                   //E_dot / V
    gsl_root_fsolver *grsolve;  //GSL root solver
} Chevalier;

// Function declarations for Chevalier struct
void SetChevalier(Chevalier *c, double M_dot, double E_dot, double gamma, double R);
double MachNumber(Chevalier *c, double r);      //mach number vs. radius
double MomentumDensity(Chevalier *c, double r); //momentum density vs. radius in Msun/pc^3 * km/s
double Pressure(Chevalier *c, double r);        //Pressure in dyn cm^-2
double Density(Chevalier *c, double r);         //density in g cm^-3
double Density_Msunpc3(Chevalier *c, double r); //density in Msun/pc3
double WindVelocity(Chevalier *c, double r);    //wind velocity in km/s
double EnergyIntegral(Chevalier *c, double r);  //msun/yr (km/s)^2 / pc^3
double u_star(Chevalier *c, double r);          //normalized wind velocity
double rho_star(Chevalier *c, double r);        //normalized density
double P_star(Chevalier *c, double r);          //normalized pressure

double mach_crossing_A(double M, void *fp);     //functions for Mach number root finding
double mach_crossing_B(double M, void *fp);

// Function definitions
void SetChevalier(Chevalier *c, double M_dot, double E_dot, double gamma, double R) {
    // M_dot = Mass input in Msun/yr
    // E_dot = Energy input in erg / s
    // gamma = Adiabatic index
    // R     = Radius of the wind region in parsec

    //Set model parameters
    c->M_dot = M_dot;
    c->E_dot = E_dot;
    c->gamma = gamma;
    c->R     = R;
    c->V     = (4.*M_PI/3. * R*R*R); //volume in pc^3
    //compute input density rate (msun / yr / pc^3)
    c->q     = c->M_dot / c->V;
    //compute input energy density rate (msun/yr (km/s)^2 / pc^3)
    c->Q     = (c->E_dot/msun_in_g*year_in_sec / 1.0e10) / c->V;
    //define the GSL root solver to use
    const gsl_root_fsolver_type *grsolve_T = gsl_root_fsolver_brent;
    c->grsolve = gsl_root_fsolver_alloc(grsolve_T);
}

double u_star(Chevalier *c, double r) {
    // Returns dimensionless wind velocity
    double u = WindVelocity(c, r);
    double u_prime = sqrt(c->E_dot)/sqrt(c->M_dot*msun_in_g/year_in_sec)/1.0e5;
    return u/u_prime;
}

double rho_star(Chevalier *c, double r) {
    // Returns dimensionless density
    double rho = Density(c,r); //g cm^-3
    double M_dot_prime = c->M_dot * msun_in_g/year_in_sec; // g/s
    double R_prime = c->R * parsec_in_cm; //cm
    double rho_prime = pow(M_dot_prime,1.5)/(sqrt(c->E_dot)*R_prime*R_prime);
    return rho/rho_prime;
}

double P_star(Chevalier *c, double r) {
    // Returns dimensionless pressure
    double P = Pressure(c,r); //dyn cm^-2
    double M_dot_prime = c->M_dot * msun_in_g/year_in_sec; // g/s
    double R_prime = c->R * parsec_in_cm; //cm
    double P_prime = sqrt(M_dot_prime)*sqrt(c->E_dot)/(R_prime*R_prime);
    return P/P_prime;
}

double Pressure(Chevalier *c, double r) {
    // Returns pressure in dyn cm^-2
    double Mach = MachNumber(c,r);          //mach number
    double u    = WindVelocity(c,r);        //km/s
    double cs   = u/Mach * 1.0e5;           //sound speed in cm/s
    u *= year_in_sec / parsec_in_km;        //to pc/yr
    double rho  = MomentumDensity(c,r)/u;   //Msun / pc^3
    rho *= msun_in_g;                       //g / pc^3
    rho /= pow(parsec_in_cm, 3);            //g / cm^3
    double P = cs*cs*rho/c->gamma;          //pressure in cgs
    return P;                               //in dyn cm^-2
}

double Density(Chevalier *c, double r) {
    // Returns density in g cm^-3
    double rho = Density_Msunpc3(c, r);     //Msun/pc^3
    rho *= msun_in_g;                       //g / pc^3
    rho /= pow(parsec_in_cm, 3);            //g / cm^3
    return rho;                             // in g/cm^3
}

double Density_Msunpc3(Chevalier *c, double r) {
    // Returns density in g cm^-3
    double u = WindVelocity(c, r);          //km/s
    u *= year_in_sec / parsec_in_km;        //to pc/yr
    return MomentumDensity(c,r)/u;          //Msun/pc^3;
}

double WindVelocity(Chevalier *c, double r) {
    // Returns wind velocity in km/s

    //First, find integrated energy input density
    double Qint = EnergyIntegral(c, r);     //msun/yr (km/s)^2 / pc^3
    //Second, find momentum density
    double rhou = MomentumDensity(c, r);     //msun/yr/pc^2
    //find sq of velocity (modulo gamma + Mach correction)
    double usq = Qint/rhou;                 //1/2 u^2 + (gamma/(gamma-1))*P/rho
                                            //1/2 u^2 + c^2 / (gamma-1)
                                            //1/2 u^2 + u^2 / (M^2 (gamma-1))
                                            //u^2 * ( 1/2 + 1/(M^2 (gamma-1)) )
    //get the mach number
    double Mach = MachNumber(c,r);
    //find the adjustment factor
    double fac = (0.5 + 1./(Mach*Mach*(c->gamma-1.)));
    return sqrt(usq/fac);	//km/s
}

double EnergyIntegral(Chevalier *c, double r) {
    double Qint = r<c->R ? 1./3.*c->Q*r : 1./3.*c->Q*c->R*c->R*c->R/(r*r);
    return Qint;    //msun/yr (km/s)^2 / pc^3
}

double MomentumDensity(Chevalier *c, double r) {
    double rhou = r<c->R ? 1./3.*c->q*r : 1./3.*c->q*c->R*c->R*c->R/(r*r) ;
    return rhou;    // in Msun/yr/pc^2
}

double MachNumber(Chevalier *c, double r) {

    int    status, iter=0, max_iter=100;
    double M_lo = 1.0e-5, M_hi = 5.;
    double Mx, answer = 0;
    double x = r/c->R;
    gsl_function func;
    double fp[2] = {c->gamma, x};

    //choose which solution to use
    if(x<=1.0) {
        M_lo = 1.0e-5;
        M_hi = 1.0;
        func.function = &mach_crossing_A;
    }
    else {
        M_lo = 1.0;
        M_hi = 10000.0;
        func.function = &mach_crossing_B;
    }
    func.params   = &fp[0];

    gsl_root_fsolver_set(c->grsolve, &func, M_lo, M_hi);

    while(++iter<max_iter){
        status = gsl_root_fsolver_iterate(c->grsolve);
        Mx = gsl_root_fsolver_root(c->grsolve);
        M_lo = gsl_root_fsolver_x_lower(c->grsolve);
        M_hi = gsl_root_fsolver_x_upper(c->grsolve);
        status = gsl_root_test_interval(M_lo,M_hi,0,1.0e-5);
        if(status==GSL_SUCCESS) {answer = Mx; break;}
    }

    return answer;
}

double mach_crossing_A(double M, void *fp) {
    double *g     = (double *) fp;
    double gamma  = g[0];
    double x      = g[1];
    double alpha  = -1.*(3.*gamma+1.)/(5.*gamma+1.);
    double beta   = (gamma+1.)/(2.*(5.*gamma+1.));
    double A      = pow( (3.*gamma + 1./(M*M))/(1.+3.*gamma), alpha);
    double B      = pow( (gamma-1.+2./(M*M))/(1.+gamma), beta);
    return A*B - x;
}

double mach_crossing_B(double M, void *fp) {
    double *g     = (double *) fp;
    double gamma = g[0];
    double x     = g[1];
    double alpha = 2./(gamma-1.);
    double beta  = (gamma+1.)/(2.*(gamma-1.));
    double A     = pow( M, alpha);
    double B     = pow( (gamma-1.+2./(M*M))/(1.+gamma), beta);
    return A*B - x*x;
}

#endif //CHEVALIER_H


/* multirobust.c
 *
 * Copyright (C) 2013 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.
 *
 * This module contains routines related to robust linear least squares. The
 * algorithm used closely follows the publications:
 *
 * [1] DuMouchel, W. and F. O'Brien (1989), "Integrating a robust
 * option into a multiple regression computing environment,"
 * Computer Science and Statistics:  Proceedings of the 21st
 * Symposium on the Interface, American Statistical Association
 *
 * [2] Street, J.O., R.J. Carroll, and D. Ruppert (1988), "A note on
 * computing robust regression estimates via iteratively
 * reweighted least squares," The American Statistician, v. 42,
 * pp. 152-154.
 */

//#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_multifit.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_sort.h>
#include <gsl/gsl_sort_vector.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>

#include "linear_common.c"

static int robust_test_convergence(const gsl_vector *c_prev, const gsl_vector *c,
                                   const double tol);
static double robust_madsigma(const gsl_vector *x, gsl_multifit_robust_workspace *w);
static double robust_robsigma(const gsl_vector *r, const double s,
                              const double tune, gsl_multifit_robust_workspace *w);
static double robust_sigma(const double s_ols, const double s_rob,
                           gsl_multifit_robust_workspace *w);
static int robust_covariance(const double sigma, gsl_matrix *cov,
                             gsl_multifit_robust_workspace *w);

/*
gsl_multifit_robust_alloc
  Allocate a robust workspace

Inputs: T - robust weighting algorithm
        n - number of observations
        p - number of model parameters

Return: pointer to workspace
*/

gsl_multifit_robust_workspace *
gsl_multifit_robust_alloc(const gsl_multifit_robust_type *T,
                          const size_t n, const size_t p)
{
  gsl_multifit_robust_workspace *w;

  if (n < p)
    {
      GSL_ERROR_VAL("observations n must be >= p", GSL_EINVAL, 0);
    }

  w = calloc(1, sizeof(gsl_multifit_robust_workspace));
  if (w == 0)
    {
      GSL_ERROR_VAL("failed to allocate space for multifit_robust struct",
                    GSL_ENOMEM, 0);
    }

  w->n = n;
  w->p = p;
  w->type = T;
  /* bdavis */
  //w->maxiter = 100; /* maximum iterations */
  w->maxiter = 5; /* maximum iterations */
  /* bdavis */
  w->tune = w->type->tuning_default;

  w->multifit_p = gsl_multifit_linear_alloc(n, p);
  if (w->multifit_p == 0)
    {
      GSL_ERROR_VAL("failed to allocate space for multifit_linear struct",
                    GSL_ENOMEM, 0);
    }

  w->r = gsl_vector_alloc(n);
  if (w->r == 0)
    {
      GSL_ERROR_VAL("failed to allocate space for residuals",
                    GSL_ENOMEM, 0);
    }

  w->weights = gsl_vector_alloc(n);
  if (w->weights == 0)
    {
      GSL_ERROR_VAL("failed to allocate space for weights", GSL_ENOMEM, 0);
    }

  w->c_prev = gsl_vector_alloc(p);
  if (w->c_prev == 0)
    {
      GSL_ERROR_VAL("failed to allocate space for c_prev", GSL_ENOMEM, 0);
    }

  w->resfac = gsl_vector_alloc(n);
  if (w->resfac == 0)
    {
      GSL_ERROR_VAL("failed to allocate space for residual factors",
                    GSL_ENOMEM, 0);
    }

  w->psi = gsl_vector_alloc(n);
  if (w->psi == 0)
    {
      GSL_ERROR_VAL("failed to allocate space for psi", GSL_ENOMEM, 0);
    }

  w->dpsi = gsl_vector_alloc(n);
  if (w->dpsi == 0)
    {
      GSL_ERROR_VAL("failed to allocate space for dpsi", GSL_ENOMEM, 0);
    }

  w->QSI = gsl_matrix_alloc(p, p);
  if (w->QSI == 0)
    {
      GSL_ERROR_VAL("failed to allocate space for QSI", GSL_ENOMEM, 0);
    }

  w->D = gsl_vector_alloc(p);
  if (w->D == 0)
    {
      GSL_ERROR_VAL("failed to allocate space for D", GSL_ENOMEM, 0);
    }

  w->workn = gsl_vector_alloc(n);
  if (w->workn == 0)
    {
      GSL_ERROR_VAL("failed to allocate space for workn", GSL_ENOMEM, 0);
    }

  w->stats.sigma_ols = 0.0;
  w->stats.sigma_mad = 0.0;
  w->stats.sigma_rob = 0.0;
  w->stats.sigma = 0.0;
  w->stats.Rsq = 0.0;
  w->stats.adj_Rsq = 0.0;
  w->stats.rmse = 0.0;
  w->stats.sse = 0.0;
  w->stats.dof = n - p;
  w->stats.weights = w->weights;
  w->stats.r = w->r;

  return w;
} /* gsl_multifit_robust_alloc() */

/*
gsl_multifit_robust_free()
  Free memory associated with robust workspace
*/

void
gsl_multifit_robust_free(gsl_multifit_robust_workspace *w)
{
  if (w->multifit_p)
    gsl_multifit_linear_free(w->multifit_p);

  if (w->r)
    gsl_vector_free(w->r);

  if (w->weights)
    gsl_vector_free(w->weights);

  if (w->c_prev)
    gsl_vector_free(w->c_prev);

  if (w->resfac)
    gsl_vector_free(w->resfac);

  if (w->psi)
    gsl_vector_free(w->psi);

  if (w->dpsi)
    gsl_vector_free(w->dpsi);

  if (w->QSI)
    gsl_matrix_free(w->QSI);

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

  if (w->workn)
    gsl_vector_free(w->workn);

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

int
gsl_multifit_robust_tune(const double tune, gsl_multifit_robust_workspace *w)
{
  w->tune = tune;
  return GSL_SUCCESS;
}

const char *
gsl_multifit_robust_name(const gsl_multifit_robust_workspace *w)
{
  return w->type->name;
}

gsl_multifit_robust_stats
gsl_multifit_robust_statistics(const gsl_multifit_robust_workspace *w)
{
  return w->stats;
}

/*
gsl_multifit_robust()
  Perform robust iteratively reweighted linear least squares
fit

Inputs: X     - design matrix of basis functions
        y     - right hand side vector
        c     - (output) model coefficients
        cov   - (output) covariance matrix
        w     - workspace
*/

int
gsl_multifit_robust(const gsl_matrix * X,
                    const gsl_vector * y,
                    gsl_vector * c,
                    gsl_matrix * cov,
                    gsl_multifit_robust_workspace *w)
{
  /* check matrix and vector sizes */
  if (X->size1 != y->size)
    {
      GSL_ERROR
        ("number of observations in y does not match rows of matrix X",
         GSL_EBADLEN);
    }
  else if (X->size2 != c->size)
    {
      GSL_ERROR ("number of parameters c does not match columns of matrix X",
                 GSL_EBADLEN);
    }
  else if (cov->size1 != cov->size2)
    {
      GSL_ERROR ("covariance matrix is not square", GSL_ENOTSQR);
    }
  else if (c->size != cov->size1)
    {
      GSL_ERROR
        ("number of parameters does not match size of covariance matrix",
         GSL_EBADLEN);
    }
  else if (X->size1 != w->n || X->size2 != w->p)
    {
      GSL_ERROR
        ("size of workspace does not match size of observation matrix",
         GSL_EBADLEN);
    }
  else
    {
      int s;
      double chisq;
      const double tol = GSL_SQRT_DBL_EPSILON;
      int converged = 0;
      size_t numit = 0;
      const size_t n = y->size;
      double sigy = gsl_stats_sd(y->data, y->stride, n);
      double sig_lower;
      size_t i;

      /*
       * if the initial fit is very good, then finding outliers by comparing
       * them to the residual standard deviation is difficult. Therefore we
       * set a lower bound on the standard deviation estimate that is a small
       * fraction of the standard deviation of the data values
       */
      sig_lower = 1.0e-6 * sigy;
      if (sig_lower == 0.0)
        sig_lower = 1.0;

      /* compute initial estimates using ordinary least squares */
      s = gsl_multifit_linear(X, y, c, cov, &chisq, w->multifit_p);
      if (s)
        return s;

      /* save Q S^{-1} of original matrix */
      gsl_matrix_memcpy(w->QSI, w->multifit_p->QSI);
      gsl_vector_memcpy(w->D, w->multifit_p->D);

      /* compute statistical leverage of each data point */
      s = gsl_linalg_SV_leverage(w->multifit_p->A, w->resfac);
      if (s)
        return s;

      /* correct residuals with factor 1 / sqrt(1 - h) */
      for (i = 0; i < n; ++i)
        {
          double h = gsl_vector_get(w->resfac, i);

          if (h > 0.9999)
            h = 0.9999;

          gsl_vector_set(w->resfac, i, 1.0 / sqrt(1.0 - h));
        }

      /* compute residuals from OLS fit r = y - X c */
      s = gsl_multifit_linear_residuals(X, y, c, w->r);
      if (s)
        return s;

      /* compute estimate of sigma from ordinary least squares */
      w->stats.sigma_ols = gsl_blas_dnrm2(w->r) / sqrt((double) w->stats.dof);

      while (!converged && ++numit <= w->maxiter)
        {
          double sig;

          /* adjust residuals by statistical leverage (see DuMouchel and O'Brien) */
          s = gsl_vector_mul(w->r, w->resfac);
          if (s)
            return s;

          /* compute estimate of standard deviation using MAD */
          sig = robust_madsigma(w->r, w);

          /* scale residuals by standard deviation and tuning parameter */
          gsl_vector_scale(w->r, 1.0 / (GSL_MAX(sig, sig_lower) * w->tune));

          /* compute weights using these residuals */
          s = w->type->wfun(w->r, w->weights);
          if (s)
            return s;

          gsl_vector_memcpy(w->c_prev, c);

          /* solve weighted least squares with new weights */
          s = gsl_multifit_wlinear(X, w->weights, y, c, cov, &chisq, w->multifit_p);
          if (s)
            return s;

          /* compute new residuals r = y - X c */
          s = gsl_multifit_linear_residuals(X, y, c, w->r);
          if (s)
            return s;

          converged = robust_test_convergence(w->c_prev, c, tol);
        }

      /* compute final MAD sigma */
      w->stats.sigma_mad = robust_madsigma(w->r, w);

      /* compute robust estimate of sigma */
      w->stats.sigma_rob = robust_robsigma(w->r, w->stats.sigma_mad, w->tune, w);

      /* compute final estimate of sigma */
      w->stats.sigma = robust_sigma(w->stats.sigma_ols, w->stats.sigma_rob, w);

      /* store number of iterations */
      w->stats.numit = numit;

      {
        double dof = (double) w->stats.dof;
        double rnorm = w->stats.sigma * sqrt(dof); /* see DuMouchel, sec 4.2 */
        double ss_err = rnorm * rnorm;
        double ss_tot = gsl_stats_tss(y->data, y->stride, n);

        /* compute R^2 */
        w->stats.Rsq = 1.0 - ss_err / ss_tot;

        /* compute adjusted R^2 */
        w->stats.adj_Rsq = 1.0 - (1.0 - w->stats.Rsq) * (n - 1.0) / dof;

        /* compute rmse */
        w->stats.rmse = sqrt(ss_err / dof);

        /* store SSE */
        w->stats.sse = ss_err;
      }

      /* calculate covariance matrix = sigma^2 (X^T X)^{-1} */
      s = robust_covariance(w->stats.sigma, cov, w);
      if (s)
        return s;

      /* raise an error if not converged */
/* bdavis */
      /* Eliminating this check is to avoid an error when iterations */
      /* exceed 5.  A better solution is probably recommended, such as */
      /* reverting to default of 100 if an input specification is not */
      /* enabled and defined. */
      /* bdavis@usgs.gov */
      /*
      if (numit > w->maxiter)
        {
          GSL_ERROR("maximum iterations exceeded", GSL_EMAXITER);
        }
      */
/* bdavis */

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

/* Estimation of values for given x */
int
gsl_multifit_robust_est(const gsl_vector * x, const gsl_vector * c,
                        const gsl_matrix * cov, double *y, double *y_err)
{
  int s = gsl_multifit_linear_est(x, c, cov, y, y_err);

  return s;
}

/***********************************
 * INTERNAL ROUTINES               *
 ***********************************/

/*
robust_test_convergence()
  Test for convergence in robust least squares

Convergence criteria:

|c_i^(k) - c_i^(k-1)| <= tol * max(|c_i^(k)|, |c_i^(k-1)|)

for all i. k refers to iteration number.

Inputs: c_prev - coefficients from previous iteration
        c      - coefficients from current iteration
        tol    - tolerance

Return: 1 if converged, 0 if not
*/

static int
robust_test_convergence(const gsl_vector *c_prev, const gsl_vector *c,
                        const double tol)
{
  size_t p = c->size;
  size_t i;

  for (i = 0; i < p; ++i)
    {
      double ai = gsl_vector_get(c_prev, i);
      double bi = gsl_vector_get(c, i);

      if (fabs(bi - ai) > tol * GSL_MAX(fabs(ai), fabs(bi)))
        return 0; /* not yet converged */
    }

  /* converged */
  return 1;
} /* robust_test_convergence() */

/*
robust_madsigma()
  Estimate the standard deviation of the residuals using
the Median-Absolute-Deviation (MAD) of the residuals,
throwing away the smallest p residuals.

See: Street et al, 1988

Inputs: r - vector of residuals
        w - workspace
*/

static double
robust_madsigma(const gsl_vector *r, gsl_multifit_robust_workspace *w)
{
  gsl_vector_view v;
  double sigma;
  size_t n = r->size;
  const size_t p = w->p;
  size_t i;

  /* copy |r| into workn */
  for (i = 0; i < n; ++i)
    {
      gsl_vector_set(w->workn, i, fabs(gsl_vector_get(r, i)));
    }

  gsl_sort_vector(w->workn);

  /*
   * ignore the smallest p residuals when computing the median
   * (see Street et al 1988)
   */
  v = gsl_vector_subvector(w->workn, p - 1, n - p + 1);
  sigma = gsl_stats_median_from_sorted_data(v.vector.data, v.vector.stride, v.vector.size) / 0.6745;

  return sigma;
} /* robust_madsigma() */

/*
robust_robsigma()
  Compute robust estimate of sigma so that
sigma^2 * inv(X' * X) is a reasonable estimate of
the covariance for robust regression. Based heavily
on the equations of Street et al, 1988.

Inputs: r    - vector of residuals y - X c
        s    - sigma estimate using MAD
        tune - tuning constant
        w    - workspace
*/

static double
robust_robsigma(const gsl_vector *r, const double s,
                const double tune, gsl_multifit_robust_workspace *w)
{
  double sigma;
  size_t i;
  const size_t n = w->n;
  const size_t p = w->p;
  const double st = s * tune;
  double a, b, lambda;

  /* compute u = r / sqrt(1 - h) / st */
  gsl_vector_memcpy(w->workn, r);
  gsl_vector_mul(w->workn, w->resfac);
  gsl_vector_scale(w->workn, 1.0 / st);

  /* compute w(u) and psi'(u) */
  w->type->wfun(w->workn, w->psi);
  w->type->psi_deriv(w->workn, w->dpsi);

  /* compute psi(u) = u*w(u) */
  gsl_vector_mul(w->psi, w->workn);

  /* Street et al, Eq (3) */
  a = gsl_stats_mean(w->dpsi->data, w->dpsi->stride, n);

  /* Street et al, Eq (5) */
  b = 0.0;
  for (i = 0; i < n; ++i)
    {
      double psi_i = gsl_vector_get(w->psi, i);
      double resfac = gsl_vector_get(w->resfac, i);
      double fac = 1.0 / (resfac*resfac); /* 1 - h */

      b += fac * psi_i * psi_i;
    }
  b /= (double) (n - p);

  /* Street et al, Eq (5) */
  lambda = 1.0 + ((double)p)/((double)n) * (1.0 - a) / a;

  sigma = lambda * sqrt(b) * st / a;

  return sigma;
} /* robust_robsigma() */

/*
robust_sigma()
  Compute final estimate of residual standard deviation, using
the OLS and robust sigma estimates.

This equation is taken from DuMouchel and O'Brien, sec 4.1:
\hat{\sigma_R}

Inputs: s_ols - OLS sigma
        s_rob - robust sigma
        w     - workspace

Return: final estimate of sigma
*/

static double
robust_sigma(const double s_ols, const double s_rob,
             gsl_multifit_robust_workspace *w)
{
  double sigma;
  const size_t p = w->p;
  const size_t n = w->n;

  /* see DuMouchel and O'Brien, sec 4.1 */
  sigma = GSL_MAX(s_rob,
                  sqrt((s_ols*s_ols*p*p + s_rob*s_rob*n) /
                       (p*p + n)));

  return sigma;
} /* robust_sigma() */

/*
robust_covariance()
  Calculate final covariance matrix, defined as:

  sigma * (X^T X)^{-1}

Inputs: sigma - residual standard deviation
        cov   - (output) covariance matrix
        w     - workspace
*/

static int
robust_covariance(const double sigma, gsl_matrix *cov,
                  gsl_multifit_robust_workspace *w)
{
  int s = 0;
  const size_t p = w->p;
  const double s2 = sigma * sigma;
  size_t i, j;
  gsl_matrix *QSI = w->QSI;
  gsl_vector *D = w->D;

  /* Form variance-covariance matrix cov = s2 * (Q S^-1) (Q S^-1)^T */

  for (i = 0; i < p; i++)
    {
      gsl_vector_view row_i = gsl_matrix_row (QSI, i);
      double d_i = gsl_vector_get (D, i);

      for (j = i; j < p; j++)
        {
          gsl_vector_view row_j = gsl_matrix_row (QSI, j);
          double d_j = gsl_vector_get (D, j);
          double s;

          gsl_blas_ddot (&row_i.vector, &row_j.vector, &s);

          gsl_matrix_set (cov, i, j, s * s2 / (d_i * d_j));
          gsl_matrix_set (cov, j, i, s * s2 / (d_i * d_j));
        }
    }

  return s;
} /* robust_covariance() */


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_matrix.h>
#include "globals.h"
#include "SimulationSteps.h"

//#define WDON

int P;
int Np;
gsl_matrix *LIFT;
gsl_matrix *VolMat;
gsl_matrix *MassMatrix;

int main(int argc, char **argv)
{
	if (argc != 2)
	{
		printf("Usage: ./Simulation 'Fort.14'\n");
		printf("Fort.14: Mesh for the channel\n");
		exit(EXIT_FAILURE);

	}

	char *Mesh = argv[1];

	//printf("Enter the polynomial approximation order:\n");
	//scanf("%d", &P);

	P = 1;
	Np = P+1;

	store_mesh(Mesh);

/*	for (int i = 0; i < NumEl; i++)
	{
		for (int j = 0; j < Np; j++)
			printf("x = %lf \t z = %lf \t S0 = %3.18lf\n", NodalX[i*Np+j] , Nodalz[i*Np+j], dz[i*Np+j]);
	}

	exit(1);
*/	// Create VolMat, LIFT and MassMatrix
	calculateLIFTVolMat(P, Np, &LIFT, &VolMat, &MassMatrix);

	double FinalTime;

	printf("Enter the time you would like to run the simulation till:\n");
	scanf("%lf", &FinalTime);

	initialize();

	/* Impose boundary conditions and couple the channels to the junction */
	boundary_conditions();

	/* Step through time */

	time_evolution(FinalTime);

	return(0);

}