// @(#)root/matrix:$Id$
// Authors: Fons Rademakers, Eddy Offermann   Nov 2003

/*************************************************************************
 * Copyright (C) 1995-2000, Rene Brun and Fons Rademakers.               *
 * All rights reserved.                                                  *
 *                                                                       *
 * For the licensing terms see $ROOTSYS/LICENSE.                         *
 * For the list of contributors see $ROOTSYS/README/CREDITS.             *
 *************************************************************************/

#ifndef ROOT_TMatrixT
#define ROOT_TMatrixT

//////////////////////////////////////////////////////////////////////////
//                                                                      //
// TMatrixT                                                             //
//                                                                      //
// Template class of a general matrix in the linear algebra package     //
//                                                                      //
//////////////////////////////////////////////////////////////////////////

#include "TMatrixTBase.h"
#include "TMatrixTUtils.h"

#ifdef CBLAS
#include <vecLib/vBLAS.h>
//#include <cblas.h>
#endif

#include "Rtypes.h"
#include "TError.h"

template<class Element> class TMatrixTSym;
template<class Element> class TMatrixTSparse;
template<class Element> class TMatrixTLazy;

template<class Element> class TMatrixT : public TMatrixTBase<Element> {

protected:

   Element  fDataStack[TMatrixTBase<Element>::kSizeMax]; //! data container
   Element *fElements;                                   //[fNelems] elements themselves

   Element *New_m   (Int_t size);
   void     Delete_m(Int_t size,Element*&);
   Int_t    Memcpy_m(Element *newp,const Element *oldp,Int_t copySize,
                      Int_t newSize,Int_t oldSize);
   void     Allocate(Int_t nrows,Int_t ncols,Int_t row_lwb = 0,Int_t col_lwb = 0,Int_t init = 0,
                     Int_t /*nr_nonzeros*/ = -1);

public:

   enum {kWorkMax = 100};
   enum EMatrixCreatorsOp1 { kZero,kUnit,kTransposed,kInverted,kAtA };
   enum EMatrixCreatorsOp2 { kMult,kTransposeMult,kInvMult,kMultTranspose,kPlus,kMinus };

   TMatrixT(): fDataStack(), fElements(0) { }
   TMatrixT(Int_t nrows,Int_t ncols);
   TMatrixT(Int_t row_lwb,Int_t row_upb,Int_t col_lwb,Int_t col_upb);
   TMatrixT(Int_t nrows,Int_t ncols,const Element *data,Option_t *option="");
   TMatrixT(Int_t row_lwb,Int_t row_upb,Int_t col_lwb,Int_t col_upb,const Element *data,Option_t *option="");
   TMatrixT(const TMatrixT      <Element> &another);
   TMatrixT(const TMatrixTSym   <Element> &another);
   TMatrixT(const TMatrixTSparse<Element> &another);
   template <class Element2> TMatrixT(const TMatrixT<Element2> &another): fElements(0)
   {
      R__ASSERT(another.IsValid());
      Allocate(another.GetNrows(),another.GetNcols(),another.GetRowLwb(),another.GetColLwb());
      *this = another;
   }

   TMatrixT(EMatrixCreatorsOp1 op,const TMatrixT<Element> &prototype);
   TMatrixT(const TMatrixT    <Element> &a,EMatrixCreatorsOp2 op,const TMatrixT   <Element> &b);
   TMatrixT(const TMatrixT    <Element> &a,EMatrixCreatorsOp2 op,const TMatrixTSym<Element> &b);
   TMatrixT(const TMatrixTSym <Element> &a,EMatrixCreatorsOp2 op,const TMatrixT   <Element> &b);
   TMatrixT(const TMatrixTSym <Element> &a,EMatrixCreatorsOp2 op,const TMatrixTSym<Element> &b);
   TMatrixT(const TMatrixTLazy<Element> &lazy_constructor);

   virtual ~TMatrixT() { Clear(); }

   // Elementary constructors

   void Plus (const TMatrixT   <Element> &a,const TMatrixT   <Element> &b);
   void Plus (const TMatrixT   <Element> &a,const TMatrixTSym<Element> &b);
   void Plus (const TMatrixTSym<Element> &a,const TMatrixT   <Element> &b) { Plus(b,a); }

   void Minus(const TMatrixT   <Element> &a,const TMatrixT   <Element> &b);
   void Minus(const TMatrixT   <Element> &a,const TMatrixTSym<Element> &b);
   void Minus(const TMatrixTSym<Element> &a,const TMatrixT   <Element> &b) { Minus(b,a); }

   void Mult (const TMatrixT   <Element> &a,const TMatrixT   <Element> &b);
   void Mult (const TMatrixT   <Element> &a,const TMatrixTSym<Element> &b);
   void Mult (const TMatrixTSym<Element> &a,const TMatrixT   <Element> &b);
   void Mult (const TMatrixTSym<Element> &a,const TMatrixTSym<Element> &b);

   void TMult(const TMatrixT   <Element> &a,const TMatrixT   <Element> &b);
   void TMult(const TMatrixT   <Element> &a,const TMatrixTSym<Element> &b);
   void TMult(const TMatrixTSym<Element> &a,const TMatrixT   <Element> &b) { Mult(a,b); }
   void TMult(const TMatrixTSym<Element> &a,const TMatrixTSym<Element> &b) { Mult(a,b); }

   void MultT(const TMatrixT   <Element> &a,const TMatrixT   <Element> &b);
   void MultT(const TMatrixT   <Element> &a,const TMatrixTSym<Element> &b) { Mult(a,b); }
   void MultT(const TMatrixTSym<Element> &a,const TMatrixT   <Element> &b);
   void MultT(const TMatrixTSym<Element> &a,const TMatrixTSym<Element> &b) { Mult(a,b); }

   virtual const Element *GetMatrixArray  () const;
   virtual       Element *GetMatrixArray  ();
   virtual const Int_t   *GetRowIndexArray() const { return 0; }
   virtual       Int_t   *GetRowIndexArray()       { return 0; }
   virtual const Int_t   *GetColIndexArray() const { return 0; }
   virtual       Int_t   *GetColIndexArray()       { return 0; }

   virtual       TMatrixTBase<Element> &SetRowIndexArray(Int_t * /*data*/) { MayNotUse("SetRowIndexArray(Int_t *)"); return *this; }
   virtual       TMatrixTBase<Element> &SetColIndexArray(Int_t * /*data*/) { MayNotUse("SetColIndexArray(Int_t *)"); return *this; }

   virtual void Clear(Option_t * /*option*/ ="") { if (this->fIsOwner) Delete_m(this->fNelems,fElements);
                                                   else fElements = 0;
                                                   this->fNelems = 0; }

           TMatrixT    <Element> &Use     (Int_t row_lwb,Int_t row_upb,Int_t col_lwb,Int_t col_upb,Element *data);
   const   TMatrixT    <Element> &Use     (Int_t row_lwb,Int_t row_upb,Int_t col_lwb,Int_t col_upb,const Element *data) const
                                            { return (const TMatrixT<Element>&)
                                                     ((const_cast<TMatrixT<Element> *>(this))->Use(row_lwb,row_upb,col_lwb,col_upb, const_cast<Element *>(data))); }
           TMatrixT    <Element> &Use     (Int_t nrows,Int_t ncols,Element *data);
   const   TMatrixT    <Element> &Use     (Int_t nrows,Int_t ncols,const Element *data) const;
           TMatrixT    <Element> &Use     (TMatrixT<Element> &a);
   const   TMatrixT    <Element> &Use     (const TMatrixT<Element> &a) const;

   virtual TMatrixTBase<Element> &GetSub  (Int_t row_lwb,Int_t row_upb,Int_t col_lwb,Int_t col_upb,
                                           TMatrixTBase<Element> &target,Option_t *option="S") const;
           TMatrixT    <Element>  GetSub  (Int_t row_lwb,Int_t row_upb,Int_t col_lwb,Int_t col_upb,Option_t *option="S") const;
   virtual TMatrixTBase<Element> &SetSub  (Int_t row_lwb,Int_t col_lwb,const TMatrixTBase<Element> &source);

   virtual TMatrixTBase<Element> &ResizeTo(Int_t nrows,Int_t ncols,Int_t /*nr_nonzeros*/ =-1);
   virtual TMatrixTBase<Element> &ResizeTo(Int_t row_lwb,Int_t row_upb,Int_t col_lwb,Int_t col_upb,Int_t /*nr_nonzeros*/ =-1);
   inline  TMatrixTBase<Element> &ResizeTo(const TMatrixT<Element> &m) {
                                            return ResizeTo(m.GetRowLwb(),m.GetRowUpb(),m.GetColLwb(),m.GetColUpb());
                                 }

   virtual Double_t Determinant  () const;
   virtual void     Determinant  (Double_t &d1,Double_t &d2) const;

           TMatrixT<Element> &Invert      (Double_t *det=0);
           TMatrixT<Element> &InvertFast  (Double_t *det=0);
           TMatrixT<Element> &Transpose   (const TMatrixT<Element> &source);
   inline  TMatrixT<Element> &T           () { return this->Transpose(*this); }
           TMatrixT<Element> &Rank1Update (const TVectorT<Element> &v,Element alpha=1.0);
           TMatrixT<Element> &Rank1Update (const TVectorT<Element> &v1,const TVectorT<Element> &v2,Element alpha=1.0);
           Element            Similarity  (const TVectorT<Element> &v) const;

   TMatrixT<Element> &NormByColumn(const TVectorT<Element> &v,Option_t *option="D");
   TMatrixT<Element> &NormByRow   (const TVectorT<Element> &v,Option_t *option="D");

   // Either access a_ij as a(i,j)
   inline       Element                     operator()(Int_t rown,Int_t coln) const;
   inline       Element                    &operator()(Int_t rown,Int_t coln);

   // or as a[i][j]
   inline const TMatrixTRow_const<Element>  operator[](Int_t rown) const { return TMatrixTRow_const<Element>(*this,rown); }
   inline       TMatrixTRow      <Element>  operator[](Int_t rown)       { return TMatrixTRow      <Element>(*this,rown); }

   TMatrixT<Element> &operator= (const TMatrixT      <Element> &source);
   TMatrixT<Element> &operator= (const TMatrixTSym   <Element> &source);
   TMatrixT<Element> &operator= (const TMatrixTSparse<Element> &source);
   TMatrixT<Element> &operator= (const TMatrixTLazy  <Element> &source);
   template <class Element2> TMatrixT<Element> &operator= (const TMatrixT<Element2> &source)
   {
      if (!AreCompatible(*this,source)) {
         Error("operator=(const TMatrixT2 &)","matrices not compatible");
         return *this;
      }

     TObject::operator=(source);
     const Element2 * const ps = source.GetMatrixArray();
           Element  * const pt = this->GetMatrixArray();
     for (Int_t i = 0; i < this->fNelems; i++)
        pt[i] = ps[i];
     this->fTol = source.GetTol();
     return *this;
   }

   TMatrixT<Element> &operator= (Element val);
   TMatrixT<Element> &operator-=(Element val);
   TMatrixT<Element> &operator+=(Element val);
   TMatrixT<Element> &operator*=(Element val);

   TMatrixT<Element> &operator+=(const TMatrixT   <Element> &source);
   TMatrixT<Element> &operator+=(const TMatrixTSym<Element> &source);
   TMatrixT<Element> &operator-=(const TMatrixT   <Element> &source);
   TMatrixT<Element> &operator-=(const TMatrixTSym<Element> &source);

   TMatrixT<Element> &operator*=(const TMatrixT            <Element> &source);
   TMatrixT<Element> &operator*=(const TMatrixTSym         <Element> &source);
   TMatrixT<Element> &operator*=(const TMatrixTDiag_const  <Element> &diag);
   TMatrixT<Element> &operator/=(const TMatrixTDiag_const  <Element> &diag);
   TMatrixT<Element> &operator*=(const TMatrixTRow_const   <Element> &row);
   TMatrixT<Element> &operator/=(const TMatrixTRow_const   <Element> &row);
   TMatrixT<Element> &operator*=(const TMatrixTColumn_const<Element> &col);
   TMatrixT<Element> &operator/=(const TMatrixTColumn_const<Element> &col);

   const TMatrixT<Element> EigenVectors(TVectorT<Element> &eigenValues) const;

   ClassDef(TMatrixT,4) // Template of General Matrix class
};

#ifndef __CINT__
// When building with -fmodules, it instantiates all pending instantiations,
// instead of delaying them until the end of the translation unit.
// We 'got away with' probably because the use and the definition of the
// explicit specialization do not occur in the same TU.
//
// In case we are building with -fmodules, we need to forward declare the
// specialization in order to compile the dictionary G__Matrix.cxx.
template <> TClass *TMatrixT<double>::Class();
#endif // __CINT__

template <class Element> inline const Element           *TMatrixT<Element>::GetMatrixArray() const { return fElements; }
template <class Element> inline       Element           *TMatrixT<Element>::GetMatrixArray()       { return fElements; }

template <class Element> inline       TMatrixT<Element> &TMatrixT<Element>::Use           (Int_t nrows,Int_t ncols,Element *data)
                                                                                { return Use(0,nrows-1,0,ncols-1,data); }
template <class Element> inline const TMatrixT<Element> &TMatrixT<Element>::Use           (Int_t nrows,Int_t ncols,const Element *data) const
                                                                                { return Use(0,nrows-1,0,ncols-1,data); }
template <class Element> inline       TMatrixT<Element> &TMatrixT<Element>::Use           (TMatrixT &a)
                                                                                {
                                                                                R__ASSERT(a.IsValid());
                                                                                return Use(a.GetRowLwb(),a.GetRowUpb(),
                                                                                a.GetColLwb(),a.GetColUpb(),a.GetMatrixArray());
                                                                                }
template <class Element> inline const TMatrixT<Element> &TMatrixT<Element>::Use           (const TMatrixT &a) const
                                                                                {
                                                                                R__ASSERT(a.IsValid());
                                                                                return Use(a.GetRowLwb(),a.GetRowUpb(),
                                                                                a.GetColLwb(),a.GetColUpb(),a.GetMatrixArray());
                                                                                }

template <class Element> inline       TMatrixT<Element>  TMatrixT<Element>::GetSub        (Int_t row_lwb,Int_t row_upb,Int_t col_lwb,Int_t col_upb,
                                                                                Option_t *option) const
                                                                                {
                                                                                TMatrixT tmp;
                                                                                this->GetSub(row_lwb,row_upb,col_lwb,col_upb,tmp,option);
                                                                                return tmp;
                                                                                }

template <class Element> inline Element TMatrixT<Element>::operator()(Int_t rown,Int_t coln) const
{
   R__ASSERT(this->IsValid());
   const Int_t arown = rown-this->fRowLwb;
   const Int_t acoln = coln-this->fColLwb;
   if (arown >= this->fNrows || arown < 0) {
      Error("operator()","Request row(%d) outside matrix range of %d - %d",rown,this->fRowLwb,this->fRowLwb+this->fNrows);
      return TMatrixTBase<Element>::NaNValue();
   }
   if (acoln >= this->fNcols || acoln < 0) {
      Error("operator()","Request column(%d) outside matrix range of %d - %d",coln,this->fColLwb,this->fColLwb+this->fNcols);
      return TMatrixTBase<Element>::NaNValue();

   }
   return (fElements[arown*this->fNcols+acoln]);
}

template <class Element> inline Element &TMatrixT<Element>::operator()(Int_t rown,Int_t coln)
{
   R__ASSERT(this->IsValid());
   const Int_t arown = rown-this->fRowLwb;
   const Int_t acoln = coln-this->fColLwb;
   if (arown >= this->fNrows || arown < 0) {
      Error("operator()","Request row(%d) outside matrix range of %d - %d",rown,this->fRowLwb,this->fRowLwb+this->fNrows);
      return TMatrixTBase<Element>::NaNValue();
   }
   if (acoln >= this->fNcols || acoln < 0) {
      Error("operator()","Request column(%d) outside matrix range of %d - %d",coln,this->fColLwb,this->fColLwb+this->fNcols);
      return TMatrixTBase<Element>::NaNValue();
   }
   return (fElements[arown*this->fNcols+acoln]);
}

template <class Element> TMatrixT<Element>  operator+  (const TMatrixT   <Element> &source1,const TMatrixT   <Element> &source2);
template <class Element> TMatrixT<Element>  operator+  (const TMatrixT   <Element> &source1,const TMatrixTSym<Element> &source2);
template <class Element> TMatrixT<Element>  operator+  (const TMatrixTSym<Element> &source1,const TMatrixT   <Element> &source2);
template <class Element> TMatrixT<Element>  operator+  (const TMatrixT   <Element> &source ,      Element               val    );
template <class Element> TMatrixT<Element>  operator+  (      Element               val    ,const TMatrixT   <Element> &source );
template <class Element> TMatrixT<Element>  operator-  (const TMatrixT   <Element> &source1,const TMatrixT   <Element> &source2);
template <class Element> TMatrixT<Element>  operator-  (const TMatrixT   <Element> &source1,const TMatrixTSym<Element> &source2);
template <class Element> TMatrixT<Element>  operator-  (const TMatrixTSym<Element> &source1,const TMatrixT   <Element> &source2);
template <class Element> TMatrixT<Element>  operator-  (const TMatrixT   <Element> &source ,      Element               val    );
template <class Element> TMatrixT<Element>  operator-  (      Element               val    ,const TMatrixT   <Element> &source );
template <class Element> TMatrixT<Element>  operator*  (      Element               val    ,const TMatrixT   <Element> &source );
template <class Element> TMatrixT<Element>  operator*  (const TMatrixT   <Element> &source ,      Element               val    );
template <class Element> TMatrixT<Element>  operator*  (const TMatrixT   <Element> &source1,const TMatrixT   <Element> &source2);
template <class Element> TMatrixT<Element>  operator*  (const TMatrixT   <Element> &source1,const TMatrixTSym<Element> &source2);
template <class Element> TMatrixT<Element>  operator*  (const TMatrixTSym<Element> &source1,const TMatrixT   <Element> &source2);
template <class Element> TMatrixT<Element>  operator*  (const TMatrixTSym<Element> &source1,const TMatrixTSym<Element> &source2);
// Preventing warnings with -Weffc++ in GCC since overloading the || and && operators was a design choice.
#if (__GNUC__ * 10000 + __GNUC_MINOR__ * 100 + __GNUC_PATCHLEVEL__) >= 40600
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Weffc++"
#endif
template <class Element> TMatrixT<Element>  operator&& (const TMatrixT   <Element> &source1,const TMatrixT   <Element> &source2);
template <class Element> TMatrixT<Element>  operator&& (const TMatrixT   <Element> &source1,const TMatrixTSym<Element> &source2);
template <class Element> TMatrixT<Element>  operator&& (const TMatrixTSym<Element> &source1,const TMatrixT   <Element> &source2);
template <class Element> TMatrixT<Element>  operator|| (const TMatrixT   <Element> &source1,const TMatrixT   <Element> &source2);
template <class Element> TMatrixT<Element>  operator|| (const TMatrixT   <Element> &source1,const TMatrixTSym<Element> &source2);
template <class Element> TMatrixT<Element>  operator|| (const TMatrixTSym<Element> &source1,const TMatrixT   <Element> &source2);
#if (__GNUC__ * 10000 + __GNUC_MINOR__ * 100 + __GNUC_PATCHLEVEL__) >= 40600
#pragma GCC diagnostic pop
#endif
template <class Element> TMatrixT<Element>  operator>  (const TMatrixT   <Element> &source1,const TMatrixT   <Element> &source2);
template <class Element> TMatrixT<Element>  operator>  (const TMatrixT   <Element> &source1,const TMatrixTSym<Element> &source2);
template <class Element> TMatrixT<Element>  operator>  (const TMatrixTSym<Element> &source1,const TMatrixT   <Element> &source2);
template <class Element> TMatrixT<Element>  operator>= (const TMatrixT   <Element> &source1,const TMatrixT   <Element> &source2);
template <class Element> TMatrixT<Element>  operator>= (const TMatrixT   <Element> &source1,const TMatrixTSym<Element> &source2);
template <class Element> TMatrixT<Element>  operator>= (const TMatrixTSym<Element> &source1,const TMatrixT   <Element> &source2);
template <class Element> TMatrixT<Element>  operator<= (const TMatrixT   <Element> &source1,const TMatrixT   <Element> &source2);
template <class Element> TMatrixT<Element>  operator<= (const TMatrixT   <Element> &source1,const TMatrixTSym<Element> &source2);
template <class Element> TMatrixT<Element>  operator<= (const TMatrixTSym<Element> &source1,const TMatrixT   <Element> &source2);
template <class Element> TMatrixT<Element>  operator<  (const TMatrixT   <Element> &source1,const TMatrixT   <Element> &source2);
template <class Element> TMatrixT<Element>  operator<  (const TMatrixT   <Element> &source1,const TMatrixTSym<Element> &source2);
template <class Element> TMatrixT<Element>  operator<  (const TMatrixTSym<Element> &source1,const TMatrixT   <Element> &source2);
template <class Element> TMatrixT<Element>  operator!= (const TMatrixT   <Element> &source1,const TMatrixT   <Element> &source2);
template <class Element> TMatrixT<Element>  operator!= (const TMatrixT   <Element> &source1,const TMatrixTSym<Element> &source2);
template <class Element> TMatrixT<Element>  operator!= (const TMatrixTSym<Element> &source1,const TMatrixT   <Element> &source2);

template <class Element> TMatrixT<Element> &Add        (TMatrixT<Element> &target,      Element               scalar,const TMatrixT   <Element> &source);
template <class Element> TMatrixT<Element> &Add        (TMatrixT<Element> &target,      Element               scalar,const TMatrixTSym<Element> &source);
template <class Element> TMatrixT<Element> &ElementMult(TMatrixT<Element> &target,const TMatrixT   <Element> &source);
template <class Element> TMatrixT<Element> &ElementMult(TMatrixT<Element> &target,const TMatrixTSym<Element> &source);
template <class Element> TMatrixT<Element> &ElementDiv (TMatrixT<Element> &target,const TMatrixT   <Element> &source);
template <class Element> TMatrixT<Element> &ElementDiv (TMatrixT<Element> &target,const TMatrixTSym<Element> &source);

template <class Element> void AMultB (const Element * const ap,Int_t na,Int_t ncolsa,
                                      const Element * const bp,Int_t nb,Int_t ncolsb,Element *cp);
template <class Element> void AtMultB(const Element * const ap,Int_t ncolsa,
                                      const Element * const bp,Int_t nb,Int_t ncolsb,Element *cp);
template <class Element> void AMultBt(const Element * const ap,Int_t na,Int_t ncolsa,
                                      const Element * const bp,Int_t nb,Int_t ncolsb,Element *cp);

#endif


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

#include <gsl/gsl_errno.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_const_mksa.h>
#include <gsl/gsl_roots.h>

#include "ccl.h"
#include "ccl_params.h"

// Global variable to hold the neutrino phase-space spline
gsl_spline* nu_spline=NULL;

/* ------- ROUTINE: nu_integrand ------
INPUTS: x: dimensionless momentum, *r: pointer to a dimensionless mass / temperature
TASK: Integrand of phase-space massive neutrino integral
*/
static double nu_integrand(double x, void *r)
{
  double rat=*((double*)(r));
  return sqrt(x*x+rat*rat)/(exp(x)+1.0)*x*x;
}

/* ------- ROUTINE: ccl_calculate_nu_phasespace_spline ------
TASK: Get the spline of the result of the phase-space integral required for massive neutrinos.
*/

gsl_spline* calculate_nu_phasespace_spline(int *status) {
  int N=CCL_NU_MNUT_N;
  double *mnut = ccl_linear_spacing(log(CCL_NU_MNUT_MIN),log(CCL_NU_MNUT_MAX),N);
  double *y=malloc(sizeof(double)*CCL_NU_MNUT_N);
  if (y ==NULL) {
    // Not setting a status_message here because we can't easily pass a cosmology to this function - message printed in ccl_error.c.
    *status = CCL_ERROR_NU_INT;
  }

  // Check whether ccl_splines and ccl_gsl exist; exit gracefully if they
  // can't be loaded
  if(ccl_splines==NULL || ccl_gsl==NULL) ccl_cosmology_read_config();
  if(ccl_splines==NULL || ccl_gsl==NULL) {
    ccl_raise_exception(CCL_ERROR_MISSING_CONFIG_FILE,
                        "ccl_neutrinos.c: Failed to read config file.");
    *status = CCL_ERROR_MISSING_CONFIG_FILE;
    return NULL;
  }

  int stat=0, gslstatus;
  gsl_integration_cquad_workspace * workspace = \
                gsl_integration_cquad_workspace_alloc(ccl_gsl->N_ITERATION);
  gsl_function F;
  F.function = &nu_integrand;
  for (int i=0; i<CCL_NU_MNUT_N; i++) {
    double mnut_=exp(mnut[i]);
    F.params = &(mnut_);
    gslstatus = gsl_integration_cquad(&F, 0, 1000.0,
                                      ccl_gsl->INTEGRATION_NU_EPSABS,
                                      ccl_gsl->INTEGRATION_NU_EPSREL,
                                      workspace,&y[i], NULL, NULL);
    if(gslstatus != GSL_SUCCESS) {
      ccl_raise_gsl_warning(gslstatus, "ccl_neutrinos.c: calculate_nu_phasespace_spline():");
      stat |= gslstatus;
    }
  }
  gsl_integration_cquad_workspace_free(workspace);
  double renorm=1./y[0];
  for (int i=0; i<CCL_NU_MNUT_N; i++)
    y[i]*=renorm;
  gsl_spline* spl = gsl_spline_alloc(A_SPLINE_TYPE, CCL_NU_MNUT_N);
  stat |= gsl_spline_init(spl, mnut, y, CCL_NU_MNUT_N);

  // Check for errors in creating the spline
  if (stat) {
    // Not setting a status_message here because we can't easily pass a cosmology to this function - message printed in ccl_error.c.
    *status = CCL_ERROR_NU_INT;
    free(mnut);
    free(y);
    gsl_spline_free(spl);
  }
  free(mnut);
  free(y);
  return spl;
}

/* ------- ROUTINE: ccl_nu_phasespace_intg ------
INPUTS: accel: pointer to an accelerator which will evaluate the neutrino phasespace spline if defined, mnuOT: the dimensionless mass / temperature of a single massive neutrino
TASK: Get the value of the phase space integral at mnuOT
*/

double nu_phasespace_intg(gsl_interp_accel* accel, double mnuOT, int* status)
{
  // Check if the global variable for the phasespace spline has been defined yet:
  if (nu_spline==NULL) nu_spline =calculate_nu_phasespace_spline(status);
  ccl_check_status_nocosmo(status);

  double integral_value =0.;

  // First check the cases where we are in the limits.
  if (mnuOT<CCL_NU_MNUT_MIN) {
    return 7./8.;
  }
  else if (mnuOT>CCL_NU_MNUT_MAX) {
    return 0.2776566337*mnuOT;
  }

  // Evaluate the spline - this will use the accelerator if it has been defined.
  int gslstatus = gsl_spline_eval_e(nu_spline, log(mnuOT),accel, &integral_value);
  if(gslstatus != GSL_SUCCESS) {
    ccl_raise_gsl_warning(gslstatus, "ccl_neutrinos.c: nu_phasespace_intg():");
    *status |= gslstatus;
  }
  return integral_value*7./8.;
}

/* -------- ROUTINE: Omeganuh2 ---------
INPUTS: a: scale factor, Nnumass: number of massive neutrino species, mnu: total mass in eV of neutrinos, T_CMB: CMB temperature, accel: pointer to an accelerator which will evaluate the neutrino phasespace spline if defined, status: pointer to status integer.
TASK: Compute Omeganu * h^2 as a function of time.
!! To all practical purposes, Neff is simply N_nu_mass !!
*/

double ccl_Omeganuh2 (double a, int N_nu_mass, double* mnu, double T_CMB, gsl_interp_accel* accel, int* status)
{
  double Tnu, a4, prefix_massless, mnuone, OmNuh2;
  double Tnu_eff, mnuOT, intval, prefix_massive;
  double total_mass; // To check if this is the massless or massive case.

  // First check if N_nu_mass is 0
  if (N_nu_mass==0) return 0.0;

  Tnu=T_CMB*pow(4./11.,1./3.);
  a4=a*a*a*a;
  // Check if mnu=0. We assume that in the massless case mnu is a pointer to a single element and that element is 0. This should in principle never be called.
  if (mnu[0] < 0.00017) {  // Limit taken from Lesgourges et al. 2012
    prefix_massless = NU_CONST  * Tnu * Tnu * Tnu * Tnu;
    return N_nu_mass*prefix_massless*7./8./a4;
  }

  // And the remaining massive case.
  // Tnu_eff is used in the massive case because CLASS uses an effective temperature of nonLCDM components to match to mnu / Omeganu =93.14eV. Tnu_eff = T_ncdm * T_CMB = 0.71611 * T_CMB
  Tnu_eff = Tnu * TNCDM / (pow(4./11.,1./3.));

  // Define the prefix using the effective temperature (to get mnu / Omega = 93.14 eV) for the massive case:
  prefix_massive = NU_CONST * Tnu_eff * Tnu_eff * Tnu_eff * Tnu_eff;

  OmNuh2 = 0.; // Initialize to 0 - we add to this for each massive neutrino species.
  for(int i=0; i<N_nu_mass; i++){
	// Get mass over T (mass (eV) / ((kb eV/s/K) Tnu_eff (K))
	// This returns the density normalized so that we get nuh2 at a=0
	mnuOT = mnu[i] / (Tnu_eff/a) * (EV_IN_J / (KBOLTZ));

	// Get the value of the phase-space integral
	intval=nu_phasespace_intg(accel,mnuOT, status);
	OmNuh2 = intval*prefix_massive/a4 + OmNuh2;
  }

  return OmNuh2;
}

/* -------- ROUTINE: Omeganuh2_to_Mnu ---------
INPUTS: OmNuh2: neutrino mass density today Omeganu * h^2, label: how you want to split up the masses, see ccl_neutrinos.h for options, T_CMB: CMB temperature, accel: pointer to an accelerator which will evaluate the neutrino phasespace spline if defined, status: pointer to status integer.
TASK: Given Omeganuh2 today, the method of splitting into masses, and the temperature of the CMB, output a pointer to the array of neutrino masses (may be length 1 if label asks for sum)
*/

double* ccl_nu_masses(double OmNuh2, ccl_neutrino_mass_splits mass_split, double T_CMB,  int* status){

  double sumnu;

  sumnu = 93.14 * OmNuh2;

  // Now split the sum up into three masses depending on the label given:

  if(mass_split==ccl_nu_normal){

     double *mnu;
	 mnu = malloc(3*sizeof(double));

     // See CCL note for how we get these expressions for the neutrino masses in normal and inverted hierarchy.
	 mnu[0] = 2./3.* sumnu - 1./6. * pow(-6. * DELTAM12_sq + 12. * DELTAM13_sq_pos + 4. * sumnu*sumnu, 0.5)
	         - 0.25 * DELTAM12_sq / (2./3.* sumnu - 1./6. * pow(-6. * DELTAM12_sq + 12. * DELTAM13_sq_pos + 4. * sumnu*sumnu, 0.5));
	 mnu[1] = 2./3.* sumnu - 1./6. * pow(-6. * DELTAM12_sq + 12. * DELTAM13_sq_pos + 4. * sumnu*sumnu, 0.5)
	         + 0.25 * DELTAM12_sq / (2./3.* sumnu - 1./6. * pow(-6. * DELTAM12_sq + 12. * DELTAM13_sq_pos + 4. * sumnu*sumnu, 0.5));
	 mnu[2] = -1./3. * sumnu + 1./3 * pow(-6. * DELTAM12_sq + 12. * DELTAM13_sq_pos + 4. * sumnu*sumnu, 0.5);
	 if (mnu[0]<0 || mnu[1]<0 || mnu[2]<0){
	    // The user has provided a sum that is below the physical limit.
	    if (sumnu<1e-14){
			mnu[0] = 0.;
			mnu[1] = 0.;
			mnu[2] = 0.;
	    }else{
		 *status = CCL_ERROR_MNU_UNPHYSICAL;
	 }
     }
	 ccl_check_status_nocosmo(status);
	 return mnu;

  } else if (mass_split==ccl_nu_inverted){

	double *mnu;
	mnu = malloc(3*sizeof(double));
	mnu[0] = 2./3.* sumnu - 1./6. * pow(-6. * DELTAM12_sq + 12. * DELTAM13_sq_neg + 4. * sumnu * sumnu, 0.5)
	         - 0.25 * DELTAM12_sq / (2./3.* sumnu - 1./6. * pow(-6. * DELTAM12_sq + 12. * DELTAM13_sq_neg + 4. * sumnu * sumnu, 0.5));
	mnu[1] = 2./3.* sumnu - 1./6. * pow(-6. * DELTAM12_sq + 12. * DELTAM13_sq_neg + 4. * sumnu*sumnu, 0.5)
	         + 0.25 * DELTAM12_sq / (2./3.* sumnu - 1./6. * pow(-6. * DELTAM12_sq + 12. * DELTAM13_sq_neg + 4. * sumnu*sumnu, 0.5));
	mnu[2] = -1./3. * sumnu + 1./3 * pow(-6. * DELTAM12_sq + 12. * DELTAM13_sq_neg + 4. * sumnu*sumnu, 0.5);
	if(mnu[0]<0 || mnu[1]<0 || mnu[2]<0){
	// The user has provided a sum that is below the physical limit.
	    if (sumnu<1e-14){
			mnu[0] = 0.;
			mnu[1] = 0.;
			mnu[2] = 0.;;
        }else{
		*status = CCL_ERROR_MNU_UNPHYSICAL;
	    }
	}

	ccl_check_status_nocosmo(status);
	return mnu;

  } else if (mass_split==ccl_nu_equal){
	  double *mnu;
	  mnu = malloc(3*sizeof(double));
	  mnu[0] = sumnu/3.;
	  mnu[1] = sumnu/3.;
	  mnu[2] = sumnu/3.;

      return mnu;

  } else if (mass_split == ccl_nu_sum){

	  double *mnu;
	  mnu = malloc(sizeof(double));

      mnu[0] = sumnu;

      return mnu;

  } else{

	  printf("WARNING:  mass option = %d not yet supported\n continuing with normal hierarchy\n", mass_split);
	  double *mnu;
	  mnu = malloc(3*sizeof(double));

	 if (sumnu>0.59){
		 mnu[0] = (sumnu - 0.59) / 3.;
		 mnu[1] = mnu[0] + 0.009;
		 mnu[2] = mnu[0] + 0.05;
	 }else{
		 *status = CCL_ERROR_MNU_UNPHYSICAL;
	 }

	 ccl_check_status_nocosmo(status);
	 return mnu;
  }
}


/*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
**                                                                                **
** This file forms part of the Underworld geophysics modelling application.         **
**                                                                                **
** For full license and copyright information, please refer to the LICENSE.md file  **
** located at the project root, or contact the authors.                             **
**                                                                                **
**~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*/
#include <petsc.h>
#include <petscvec.h>
#include <petscmat.h>
#include <petscksp.h>

#include "common-driver-utils.h"

/*

Stokes output:
  ---------------------------------
  Operator summary:
    K
    G
    f,
    h
    u
    p
  ---------------------------------
  Solution summary:
    max_u
    min_u
    average_u

    |r_1|
    |r_2|
---------------------------------
  Solver summary:
    name

---------------------------------
  Petsc build summary:

*/
PetscErrorCode BSSCR_stokes_output( PetscViewer v, Mat stokes_A, Vec stokes_b, Vec stokes_x, KSP ksp, PetscInt monitor_index )
{
	Mat K,G,D,C;
	Vec f,h, u,p;

	K = G = D = C = PETSC_NULL;
	f = h = PETSC_NULL;
	u = p = PETSC_NULL;

	MatNestGetSubMat( stokes_A, 0,0, &K );
	MatNestGetSubMat( stokes_A, 0,1, &G );
	MatNestGetSubMat( stokes_A, 1,0, &D );
	MatNestGetSubMat( stokes_A, 1,1, &C );

	VecNestGetSubVec( stokes_x, 0, &u );
	VecNestGetSubVec( stokes_x, 1, &p );

	VecNestGetSubVec( stokes_b, 0, &f );
	VecNestGetSubVec( stokes_b, 1, &h );

	PetscViewerASCIIPrintf( v, "Stokes Output:\n");
	PetscViewerASCIIPushTab( v );
	/*--------------------------------------------------------------------------------------------*/
	PetscViewerASCIIPrintf( v, "--------------------------------------------------\n");
	PetscViewerASCIIPrintf( v, "Operator summary:\n");
	PetscViewerASCIIPushTab( v );

	if (K) {	BSSCR_MatInfoLog(v,K, "stokes_A11");		PetscViewerASCIIPrintf( v, "\n");	}
	if (G) {	BSSCR_MatInfoLog(v,G, "stokes_A12");		PetscViewerASCIIPrintf( v, "\n");	}
	if (D){		BSSCR_MatInfoLog(v,D, "stokes_A21"); 		PetscViewerASCIIPrintf( v, "\n");	}
	if (C) {	BSSCR_MatInfoLog(v,C, "stokes_A22"); 		PetscViewerASCIIPrintf( v, "\n");	}

	if (f) {	BSSCR_VecInfoLog(v,f,"stokes_b1");		PetscViewerASCIIPrintf( v, "\n");	}
	if (h) {	BSSCR_VecInfoLog(v,h,"stokes_b2");		PetscViewerASCIIPrintf( v, "\n");	}

	PetscViewerASCIIPopTab( v );

	/*--------------------------------------------------------------------------------------------*/
	PetscViewerASCIIPrintf( v, "--------------------------------------------------\n");
	PetscViewerASCIIPrintf( v, "Solution summary:\n");
	PetscViewerASCIIPushTab( v );

	if (u) {	BSSCR_VecInfoLog(v,u,"x1");		PetscViewerASCIIPrintf( v, "\n");	}
	if (p) {	BSSCR_VecInfoLog(v,p,"x2");		PetscViewerASCIIPrintf( v, "\n");	}

	{
		PetscScalar s,sum;
		PetscReal r,max,min;
		PetscInt N, loc;
		double r1,r2;
		Vec K_d;
		PetscInt loc_max, loc_min;

		VecGetSize( u, &N );
		VecMax( u, &loc, &r );		PetscViewerASCIIPrintf( v, "u_max: %1.12e [%d] \n", r, loc );
		VecMin( u, &loc, &r );		PetscViewerASCIIPrintf( v, "u_min: %1.12e [%d] \n", r, loc );

		VecDot( u,u, &s );			PetscViewerASCIIPrintf( v, "u_rms: %1.12e \n", sqrt( PetscRealPart(s) )/N );

		VecDuplicate( u, &K_d );
		MatGetDiagonal( K, K_d );
		VecMax( K_d, &loc_max, &max );
		VecMin( K_d, &loc_min, &min );
		PetscViewerASCIIPrintf( v,"Algebraic contrast: max(K_d)=%.3e [%d] , min(K_d)=%.3e [%d] , max(K_d)/min(K_d) = %.8e \n", max,loc_max, min,loc_min, max/min );

		MatGetRowMax( K, K_d, PETSC_NULL );
		VecMax( K_d, &loc_max, &max );
		MatGetRowMin( K, K_d, PETSC_NULL );
		VecAbs( K_d );
		VecMin( K_d, &loc_min, &min );
		PetscViewerASCIIPrintf( v,"Algebraic contrast:   max(K)=%.3e [%d] , |min(K)|=%.3e [%d]  ,   max(K)/|min(K)| = %.8e \n", max,loc_max, min,loc_min, max/min );
		Stg_VecDestroy(&K_d );

		PetscViewerASCIIPrintf( v, "\n");

		VecGetSize( p, &N );
		VecMax( p, &loc, &r );		PetscViewerASCIIPrintf( v, "p_max:  %1.12e [%d] \n", r, loc );
		VecMin( p, &loc, &r );		PetscViewerASCIIPrintf( v, "p_min:  %1.12e [%d] \n", r, loc );

		VecDot( p,p, &s );			PetscViewerASCIIPrintf( v, "p_rms:  %1.12e \n", sqrt( PetscRealPart(s) )/N );

		VecSum( p, &sum );			PetscViewerASCIIPrintf( v, "sum(p): %1.12e \n", sum );

		PetscViewerASCIIPrintf( v, "\n");

		r1 = BSSCR_StokesMomentumResidual( K,G,f, u,p );
		PetscViewerASCIIPrintf( v, "|r1| = %1.12e <momentum> \n", r1 );
		r2 = BSSCR_StokesContinuityResidual( G,C,h, u,p );
		PetscViewerASCIIPrintf( v, "|r2| = %1.12e <continuity> \n", r2 );
		PetscViewerASCIIPrintf( v, "\n");
	}
	PetscViewerASCIIPopTab( v );

	/*--------------------------------------------------------------------------------------------*/
	if (ksp) {
		PetscViewerASCIIPrintf( v, "--------------------------------------------------\n");
		PetscViewerASCIIPrintf( v, "Solver summary:\n");
		PetscViewerASCIIPushTab( v );

		BSSCR_KSPLogSolve( v, monitor_index, ksp );
		BSSCR_BSSCR_KSPLogSolveSummary( v, monitor_index, ksp );
		PetscViewerASCIIPrintf( v, "\n");
		PetscViewerASCIIPopTab( v );
	}

	/*--------------------------------------------------------------------------------------------*/
	PetscViewerASCIIPrintf( v, "--------------------------------------------------\n");
	PetscViewerASCIIPrintf( v, "Petsc build summary:\n");
	PetscViewerASCIIPushTab( v );

	BSSCR_GeneratePetscHeader_for_viewer( v );
	PetscViewerASCIIPrintf( v, "\n");

	PetscViewerASCIIPopTab( v );
	/*--------------------------------------------------------------------------------------------*/

	PetscViewerASCIIPopTab(v);

	PetscFunctionReturn(0);
}


/**
 * @file ssygvd.c
 *
 *  PLASMA computational routines
 *  Release Date: November, 15th 2009
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @version 2.6.0
 * @author Azzam Haidar
 * @author Hatem Ltaief
 * @date 2010-11-15
 * @generated s Tue Jan  7 11:45:09 2014
 *
 **/
#include <lapacke.h>
#include "common.h"

/***************************************************************************//**
 *
 * @ingroup float
 *
 *  PLASMA_ssygvd - Computes all eigenvalues and, optionally,
 *  eigenvectors of a complex generalized Hermitian-definite
 *  eigenproblem of the form: A*x=(lambda)*B*x, A*Bx=(lambda)*x, or
 *  B*A*x=(lambda)*x.
 *  Here A and B are assumed to be Hermitian and B is also positive
 *  definite.
 *
 *******************************************************************************
 *
 * @param[in] itype
 *          Intended usage:
 *          = 1: A*x=(lambda)*B*x
 *          = 2: A*Bx=(lambda)*x
 *          = 3: B*A*x=(lambda)*x
 *
 * @param[in] jobz
 *          Intended usage:
 *          = PlasmaNoVec: computes eigenvalues only;
 *          = PlasmaVec: computes eigenvalues and eigenvectors.
 *
 * @param[in] uplo
 *          Specifies whether the matrix A is upper triangular or
 *          lower triangular:
 *          = PlasmaUpper: Upper triangle of A and B are stored;
 *          = PlasmaLower: Lower triangle of A and B are stored.
 *
 * @param[in] N
 *          The order of the matrix A. N >= 0.
 *
 * @param[in,out] A
 *          On entry, the symmetric (or Hermitian) matrix A.
 *          If uplo = PlasmaUpper, the leading N-by-N upper triangular
 *          part of A contains the upper triangular part of the matrix
 *          A, and the strictly lower triangular part of A is not
 *          referenced.
 *          If uplo = PlasmaLower, the leading N-by-N lower triangular
 *          part of A contains the lower triangular part of the matrix
 *          A, and the strictly upper triangular part of A is not
 *          referenced.
 *          On exit, if jobz = PlasmaVec, then if return value = 0, A
 *          contains the matrix Z of eigenvectors.
 *          The eigenvectors are normalized as follows:
 *          if ITYPE = 1 or 2, Z**T*B*Z = I;
 *          if ITYPE = 3,      Z**T*inv(B)*Z = I.
 *          If jobz = PlasmaNoVec, then on exit the lower triangle (if
 *          uplo = PlasmaLower) or the upper triangle (if uplo =
 *          PlasmaUpper) of A, including the diagonal, is destroyed.
 *
 * @param[in] LDA
 *          The leading dimension of the array A. LDA >= max(1,N).
 *
 * @param[in,out] B
 *          On entry, the symmetric (or Hermitian) positive definite
 *          matrix B.
 *          If uplo = PlasmaUpper, the leading N-by-N upper triangular
 *          part of B contains the upper triangular part of the matrix
 *          B, and the strictly lower triangular part of B is not
 *          referenced.
 *          If uplo = PlasmaLower, the leading N-by-N lower triangular
 *          part of B contains the lower triangular part of the matrix
 *          B, and the strictly upper triangular part of B is not
 *          referenced.
 *          On exit, if return value <= N, the part of B containing
 *          the matrix is overwritten by the triangular factor U or L
 *          from the Cholesky factorization B = U**T*U or B = L*L**T.
 *
 * @param[in] LDB
 *          The leading dimension of the array B. LDA >= max(1,N).
 *
 * @param[out] W
 *          On exit, if info = 0, the eigenvalues.
 *
 * @param[in, out] descT
 *          On entry, descriptor as return by PLASMA_Alloc_Workspace_ssygvd
 *          On exit, contains auxiliary factorization data.
 *
 * @param[out] Q
 *          On exit, if jobz = PlasmaVec and info = 0, the eigenvectors.
 *
 * @param[in] LDQ
 *          The leading dimension of Q.
 *
 *******************************************************************************
 *
 * @return
 *          \retval PLASMA_SUCCESS successful exit
 *          \retval <0 if -i, the i-th argument had an illegal value
 *          \retval <=N if INFO = i, plasma_ssygvd failed to converge; i
 *               off-diagonal elements of an intermediate tridiagonal
 *               form did not converge to zero.
 *          \retval >N if INFO = N + i, for 1 <= i <= N, then the leading
 *                     minor of order i of B is not positive definite.
 *                     The factorization of B could not be completed and
 *                     no eigenvalues or eigenvectors were computed.
 *
 *******************************************************************************
 *
 * @sa PLASMA_ssygvd_Tile
 * @sa PLASMA_ssygvd_Tile_Async
 * @sa PLASMA_chegvd
 * @sa PLASMA_dsygvd
 * @sa PLASMA_ssygvd
 *
 ******************************************************************************/
int PLASMA_ssygvd(PLASMA_enum itype, PLASMA_enum jobz, PLASMA_enum uplo, int N,
                 float *A, int LDA,
                 float *B, int LDB,
                 float *W,
                 PLASMA_desc *descT,
                 float *Q, int LDQ)
{
    int NB;
    int status;
    plasma_context_t *plasma;
    PLASMA_sequence *sequence = NULL;
    PLASMA_request request = PLASMA_REQUEST_INITIALIZER;
    PLASMA_desc descA, descB, descQ;

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

    /* Check input arguments */
    if (itype != 1 && itype != 2 && itype != 3) {
        plasma_error("PLASMA_ssygvd", "Illegal value of itype");
        return -1;
    }
    if (jobz != PlasmaNoVec && jobz != PlasmaVec) {
        plasma_error("PLASMA_ssygvd", "illegal value of jobz");
        return -2;
    }
    if (uplo != PlasmaLower && uplo!= PlasmaUpper) {
        plasma_error("PLASMA_ssygvd", "only PlasmaLower supported");
        return -3;
    }
    if (N < 0) {
        plasma_error("PLASMA_ssygvd", "illegal value of N");
        return -4;
    }
    if (LDA < max(1, N)) {
        plasma_error("PLASMA_ssygvd", "illegal value of LDA");
        return -6;
    }
    if (LDB < max(1, N)) {
        plasma_error("PLASMA_ssygvd", "illegal value of LDB");
        return -8;
    }
    if (LDQ < max(1, N)) {
        plasma_error("PLASMA_ssygvd", "illegal value of LDQ");
        return -12;
    }

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

    /* Tune NB & IB depending on N; Set NBNB */
    status = plasma_tune(PLASMA_FUNC_SSYGV, N, N, 0);
    if (status != PLASMA_SUCCESS) {
        plasma_error("PLASMA_ssygvd", "plasma_tune() failed");
        return status;
    }

    /* Set NT */
    NB = PLASMA_NB;

    plasma_sequence_create(plasma, &sequence);

    if ( PLASMA_TRANSLATION == PLASMA_OUTOFPLACE ) {
        plasma_sooplap2tile( descA, A, NB, NB, LDA, N, 0, 0, N, N, sequence, &request,
                             plasma_desc_mat_free(&(descA)) );
        plasma_sooplap2tile( descB, B, NB, NB, LDB, N, 0, 0, N, N, sequence, &request,
                             plasma_desc_mat_free(&(descA)); plasma_desc_mat_free(&(descB)) );
        /* No conversion for Q */
    } else {
        plasma_siplap2tile( descA, A, NB, NB, LDA, N, 0, 0, N, N,
                            sequence, &request);
        plasma_siplap2tile( descB, B, NB, NB, LDB, N, 0, 0, N, N,
                            sequence, &request);
    }

    /* Q is used only as an output */
    descQ = plasma_desc_init(
        PlasmaRealFloat, NB, NB, NB*NB,
        LDQ, N, 0, 0, N, N);
    descQ.mat = Q;

    /* Call the tile interface */
    PLASMA_ssygvd_Tile_Async(itype, jobz, uplo,
                            &descA, &descB, W,
                            descT, &descQ,
                            sequence, &request);

    plasma_siptile2lap( descQ, Q, NB, NB, LDQ, N,  sequence, &request);
    if ( PLASMA_TRANSLATION == PLASMA_OUTOFPLACE ) {
        plasma_sooptile2lap( descA, A, NB, NB, LDA, N, sequence, &request);
        plasma_sooptile2lap( descB, B, NB, NB, LDB, N, sequence, &request);
        plasma_dynamic_sync();
        plasma_desc_mat_free(&descA);
        plasma_desc_mat_free(&descB);
    } else {
        plasma_siptile2lap( descA, A, NB, NB, LDA, N, sequence, &request);
        plasma_siptile2lap( descB, B, NB, NB, LDB, N, sequence, &request);
        plasma_dynamic_sync();
    }

    status = sequence->status;
    plasma_sequence_destroy(plasma, sequence);
    return status;
}
/***************************************************************************//**
 *
 * @ingroup float_Tile
 *
 *  PLASMA_ssygvd_Tile - Computes all eigenvalues and, optionally,
 *  eigenvectors of a complex generalized Hermitian-definite
 *  eigenproblem of the form: A*x=(lambda)*B*x, A*Bx=(lambda)*x, or
 *  B*A*x=(lambda)*x.
 *  Here A and B are assumed to be Hermitian and B is also
 *  positive definite.
 *
 *  Tile equivalent of PLASMA_ssygvd().
 *  Operates on matrices stored by tiles.
 *  All matrices are passed through descriptors.
 *  All dimensions are taken from the descriptors.
 *
 *******************************************************************************
 *
 * @param[in] itype
 *          Intended usage:
 *          = 1: A*x=(lambda)*B*x
 *          = 2: A*Bx=(lambda)*x
 *          = 3: B*A*x=(lambda)*x
 *
 * @param[in] jobz
 *          Intended usage:
 *          = PlasmaNoVec: computes eigenvalues only;
 *          = PlasmaVec: computes eigenvalues and eigenvectors.
 *
 * @param[in] uplo
 *          Specifies whether the matrix A is upper triangular or
 *          lower triangular:
 *          = PlasmaUpper: Upper triangle of A and B are stored;
 *          = PlasmaLower: Lower triangle of A and B are stored.
 *
 * @param[in,out] A
 *          On entry, the symmetric (or Hermitian) matrix A.
 *          If uplo = PlasmaUpper, the leading N-by-N upper triangular
 *          part of A contains the upper triangular part of the matrix
 *          A, and the strictly lower triangular part of A is not
 *          referenced.
 *          If uplo = PlasmaLower, the leading N-by-N lower triangular
 *          part of A contains the lower triangular part of the matrix
 *          A, and the strictly upper triangular part of A is not
 *          referenced.
 *          On exit, if jobz = PlasmaVec, then if return value = 0, A
 *          contains the matrix Z of eigenvectors.
 *          The eigenvectors are normalized as follows:
 *          if ITYPE = 1 or 2, Z**T*B*Z = I;
 *          if ITYPE = 3,      Z**T*inv(B)*Z = I.
 *          If jobz = PlasmaNoVec, then on exit the lower triangle (if
 *          uplo = PlasmaLower) or the upper triangle (if uplo =
 *          PlasmaUpper) of A, including the diagonal, is destroyed.
 *
 * @param[in,out] B
 *          On entry, the symmetric (or Hermitian) positive definite
 *          matrix B.
 *          If uplo = PlasmaUpper, the leading N-by-N upper triangular
 *          part of B contains the upper triangular part of the matrix
 *          B, and the strictly lower triangular part of B is not
 *          referenced.
 *          If uplo = PlasmaLower, the leading N-by-N lower triangular
 *          part of B contains the lower triangular part of the matrix
 *          B, and the strictly upper triangular part of B is not
 *          referenced.
 *          On exit, if return value <= N, the part of B containing
 *          the matrix is overwritten by the triangular factor U or L
 *          from the Cholesky factorization B = U**T*U or B = L*L**T.
 *
 * @param[in,out] T
 *          On entry, descriptor as return by
 *          PLASMA_Alloc_Workspace_ssygvd
 *          On exit, contains auxiliary factorization data.
 *
 * @param[out] W
 *          On exit, if info = 0, the eigenvalues.
 *
 * @param[out] Q
 *          On exit, if jobz = PlasmaVec and info = 0, the eigenvectors.
 *
 *******************************************************************************
 *
 * @return
 *          \retval PLASMA_SUCCESS successful exit
 *          \retval <0 if -i, the i-th argument had an illegal value
 *          \retval <=N if INFO = i, plasma_ssygvd failed to converge; i
 *               off-diagonal elements of an intermediate tridiagonal
 *               form did not converge to zero.
 *          \retval >N if INFO = N + i, for 1 <= i <= N, then the leading
 *                     minor of order i of B is not positive definite.
 *                     The factorization of B could not be completed and
 *                     no eigenvalues or eigenvectors were computed.
 *
 *******************************************************************************
 *
 * @sa PLASMA_ssygvd
 * @sa PLASMA_ssygvd_Tile_Async
 * @sa PLASMA_chegvd_Tile
 * @sa PLASMA_dsygvd_Tile
 * @sa PLASMA_ssygvd_Tile
 *
 ******************************************************************************/
int PLASMA_ssygvd_Tile(PLASMA_enum itype, PLASMA_enum jobz, PLASMA_enum uplo,
                      PLASMA_desc *A, PLASMA_desc *B, float *W,
                      PLASMA_desc *T, PLASMA_desc *Q)
{
    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_ssygvd_Tile", "PLASMA not initialized");
        return PLASMA_ERR_NOT_INITIALIZED;
    }
    plasma_sequence_create(plasma, &sequence);
    PLASMA_ssygvd_Tile_Async(itype, jobz, uplo, A, B, W, T, Q, sequence, &request);
    plasma_dynamic_sync();
    status = sequence->status;
    plasma_sequence_destroy(plasma, sequence);
    return status;
}

/***************************************************************************//**
 *
 * @ingroup float_Tile_Async
 *
 *  PLASMA_ssygvd_Tile - Computes all eigenvalues and, optionally,
 *  eigenvectors of a complex generalized Hermitian-definite
 *  eigenproblem of the form: A*x=(lambda)*B*x, A*Bx=(lambda)*x, or
 *  B*A*x=(lambda)*x.
 *  Here A and B are assumed to be Hermitian and B is also
 *  positive definite.
 *
 *  Non-blocking equivalent of PLASMA_ssygvd_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_ssygvd
 * @sa PLASMA_ssygvd_Tile
 * @sa PLASMA_chegvd_Tile_Async
 * @sa PLASMA_dsygvd_Tile_Async
 * @sa PLASMA_ssygvd_Tile_Async
 *
 ******************************************************************************/
int PLASMA_ssygvd_Tile_Async(PLASMA_enum itype, PLASMA_enum jobz, PLASMA_enum uplo,
                             PLASMA_desc *A, PLASMA_desc *B, float *W,
                             PLASMA_desc *T, PLASMA_desc *Q,
PLASMA_sequence *sequence, PLASMA_request *request)
{
    int status;
    PLASMA_desc descA;
    PLASMA_desc descB;
    PLASMA_desc descQ;
    PLASMA_enum trans;
    plasma_context_t *plasma;

    plasma = plasma_context_self();
    if (plasma == NULL) {
        plasma_fatal_error("PLASMA_ssygvd_Tile_Async", "PLASMA not initialized");
        return PLASMA_ERR_NOT_INITIALIZED;
    }
    if (sequence == NULL) {
        plasma_fatal_error("PLASMA_ssygvd_Tile_Async", "NULL sequence");
        return PLASMA_ERR_UNALLOCATED;
    }
    if (request == NULL) {
        plasma_fatal_error("PLASMA_ssygvd_Tile_Async", "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_ssygvd_Tile_Async", "invalid first descriptor");
        return plasma_request_fail(sequence, request, PLASMA_ERR_ILLEGAL_VALUE);
    } else {
        descA = *A;
    }
    if (plasma_desc_check(B) != PLASMA_SUCCESS) {
        plasma_error("PLASMA_ssygvd_Tile_Async", "invalid second descriptor");
        return plasma_request_fail(sequence, request, PLASMA_ERR_ILLEGAL_VALUE);
    } else {
        descB = *B;
    }
    if (plasma_desc_check(T) != PLASMA_SUCCESS) {
        plasma_error("PLASMA_ssygvd_Tile_Async", "invalid T descriptor");
        return plasma_request_fail(sequence, request, PLASMA_ERR_ILLEGAL_VALUE);
    }
    if (plasma_desc_check(Q) != PLASMA_SUCCESS) {
        plasma_error("PLASMA_ssygvd_Tile_Async", "invalid Q descriptor");
        return plasma_request_fail(sequence, request, PLASMA_ERR_ILLEGAL_VALUE);
    } else {
        descQ = *Q;
    }

    /* Check input arguments */
    if (itype != 1 && itype != 2 && itype != 3) {
        plasma_error("PLASMA_ssygvd_Tile_Async", "Illegal value of itype");
        return plasma_request_fail(sequence, request, PLASMA_ERR_ILLEGAL_VALUE);
    }
    if (jobz != PlasmaNoVec && jobz != PlasmaVec) {
        plasma_error("PLASMA_ssygvd_Tile_Async", "illegal value of jobz");
        return plasma_request_fail(sequence, request, PLASMA_ERR_ILLEGAL_VALUE);
    }
    if (uplo != PlasmaLower && uplo != PlasmaUpper) {
        plasma_error("PLASMA_ssygvd_Tile_Async", "illegal value of uplo");
        return plasma_request_fail(sequence, request, PLASMA_ERR_ILLEGAL_VALUE);
    }
    if (descA.nb != descA.mb) {
        plasma_error("PLASMA_ssygvd_Tile_Async", "only square tiles supported");
        return plasma_request_fail(sequence, request, PLASMA_ERR_ILLEGAL_VALUE);
    }
    if ( (jobz == PlasmaVec) && (descA.nb != descA.mb) ) {
        plasma_error("PLASMA_ssygv_Tile_Async", "only square tiles supported");
        return plasma_request_fail(sequence, request, PLASMA_ERR_ILLEGAL_VALUE);
    }

    /*
     * Form a Cholesky factorization of B.
     */
    plasma_parallel_call_4(plasma_pspotrf,
        PLASMA_enum,      uplo,
        PLASMA_desc,      descB,
        PLASMA_sequence*, sequence,
        PLASMA_request*,  request);

    status = sequence->status;
    if (status != 0){
       status = descA.n + status;
       return status;
    }

    /*
     * Transform problem to standard eigenvalue problem and solve.
     */
    plasma_dynamic_call_6(plasma_pssygst,
        PLASMA_enum,      itype,
        PLASMA_enum,      uplo,
        PLASMA_desc,      descA,
        PLASMA_desc,      descB,
        PLASMA_sequence*, sequence,
        PLASMA_request*,  request);

    /*
     * Call Eigensolver
     */
    status = PLASMA_ssyevd_Tile_Async( jobz, uplo,
                                       A, W, T,
                                       (float*)(Q->mat), Q->lm,
                                       sequence, request );

    /*
     * Backtransform eigenvectors to the original problem.
     */
    if( jobz == PlasmaVec ) {
        if ( status != PLASMA_SUCCESS ) {
            plasma_error("PLASMA_sheegvd", "ssygvd");
            return status;
        }

        /*
         * Convert back the lapack layout used in ssyev to tile layout
         */
        plasma_siplap2tile( descQ, (float*)Q->mat, descQ.mb, descQ.nb, descQ.lm, descQ.ln,
                            0, 0, descQ.m, descQ.n, sequence, request );

        if( (itype == 1) || (itype == 2) ) {
            /*
             * For A*x=(lambda)*B*x and A*B*x=(lambda)*x;
             * backtransform eigenvectors: x = inv(L)**T *y or inv(U)*y
             */
             if( uplo == PlasmaUpper )
                 trans = PlasmaNoTrans;
             else
                 trans = PlasmaTrans;

             plasma_parallel_call_9(plasma_pstrsm,
                 PLASMA_enum,        PlasmaLeft,
                 PLASMA_enum,        uplo,
                 PLASMA_enum,        trans,
                 PLASMA_enum,        PlasmaNonUnit,
                 float, 1.0,
                 PLASMA_desc,        descB,
                 PLASMA_desc,        descQ,
                 PLASMA_sequence*,   sequence,
                 PLASMA_request*,    request);
        }
        else if( itype == 3 ) {
            /*
             * For B*A*x=(lambda)*x;
             * backtransform eigenvectors: x = L*y or U**T *y
             */
             if( uplo == PlasmaUpper )
                 trans = PlasmaTrans;
             else
                 trans = PlasmaNoTrans;

             plasma_dynamic_call_9(plasma_pstrmm,
                 PLASMA_enum,        PlasmaLeft,
                 PLASMA_enum,        uplo,
                 PLASMA_enum,        trans,
                 PLASMA_enum,        PlasmaNonUnit,
                 float, 1.0,
                 PLASMA_desc,        descB,
                 PLASMA_desc,        descQ,
                 PLASMA_sequence*,   sequence,
                 PLASMA_request*,    request);
        }
    }

    return PLASMA_SUCCESS;
}


#include <math.h>
#include <string.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_roots.h>
#include <gsl/gsl_sf_hyperg.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>

#include <fastpm/libfastpm.h>
#include <fastpm/prof.h>
#include <fastpm/lightcone.h>
#include <fastpm/logging.h>
#include <gsl/gsl_linalg.h>

#include "pmpfft.h"
#include "pmghosts.h"

static void
_matrix_invert(double (*matrix)[4],
                    double (*matrix_inv)[4],
                    int matrix_size)
                                                                        //square matrix..
                                                                        //matrix_size fixed to 4
{
    double temp[16];

    memcpy(temp, matrix, sizeof(double) * 16);

    gsl_matrix_view m = gsl_matrix_view_array(temp, matrix_size, matrix_size);
    gsl_matrix_view inv = gsl_matrix_view_array(&matrix_inv[0][0],matrix_size, matrix_size);
    gsl_permutation * p = gsl_permutation_alloc (matrix_size);

    int signum;
    gsl_linalg_LU_decomp (&m.matrix, p, &signum);
    gsl_linalg_LU_invert (&m.matrix, p, &inv.matrix);

    gsl_permutation_free (p);
}

void
fastpm_lc_init(FastPMLightCone * lc)
{
    gsl_set_error_handler_off(); // Turn off GSL error handler

    /* Allocation */
    lc->horizon = malloc(sizeof(FastPMHorizon));
    fastpm_horizon_init(lc->horizon, lc->cosmology);
    _matrix_invert(lc->glmatrix,lc->glmatrix_inv,4);

}

void
fastpm_lc_destroy(FastPMLightCone * lc)
{
    fastpm_horizon_destroy(lc->horizon);
    free(lc->horizon);
}

void
fastpm_usmesh_init(FastPMUSMesh * mesh, FastPMLightCone * lc,
            double target_volume,
            FastPMStore * source,
            size_t np_upper,
            double (*tileshifts)[3],
            int ntiles,
            double amin,
            double amax)
{

    mesh->target_volume = target_volume;
    mesh->source = source;
    mesh->amin = amin;
    mesh->amax = amax;
    mesh->lc = lc;
    mesh->tileshifts = malloc(sizeof(tileshifts[0]) * ntiles);
    mesh->ntiles = ntiles;
    mesh->np_before = 0;

    memcpy(mesh->tileshifts, tileshifts, sizeof(tileshifts[0]) * ntiles);

    mesh->event_handlers = NULL;
    mesh->p = malloc(sizeof(FastPMStore));
    /* for saving the density with particles */
    fastpm_store_init(mesh->p, source->name, np_upper,
                  COLUMN_ID | COLUMN_POS | COLUMN_VEL | COLUMN_MASK
                | COLUMN_AEMIT,
                FASTPM_MEMORY_HEAP
    );

    mesh->p->meta.M0 = source->meta.M0;       // FIXME: change this for ncdm mass defn?
}

void fastpm_usmesh_destroy(FastPMUSMesh * mesh)
{
    fastpm_destroy_event_handlers(&mesh->event_handlers);
    fastpm_store_destroy(mesh->p);
    free(mesh->tileshifts);
    free(mesh->p);
}

struct funct_params {
    FastPMLightCone *lc;
    FastPMStore * p;
    FastPMDriftFactor * drift;
    ptrdiff_t i;
    double tileshift[4];
    double a1;
    double a2;
    void * context;
};

static double
funct(double a, void *params)
{
    struct funct_params *Fp = (struct funct_params *) params;

    FastPMLightCone *lc = Fp->lc;
    FastPMStore * p = Fp->p;
    FastPMDriftFactor * drift = Fp->drift;
    ptrdiff_t i = Fp->i;
    int d;
    double xi[4];
    double xo[4];

    xi[3] = 1;
    if(p->v) {
        fastpm_drift_one(drift, p, i, xi, a);
    } else {
        for(d = 0; d < 3; d ++) {
            xi[d] = p->x[i][d];
        }
    }
    for(d = 0; d < 4; d ++) {
        xi[d] += Fp->tileshift[d];
    }
    /* transform the coordinate */
    fastpm_gldot(lc->glmatrix, xi, xo);

    double distance = fastpm_lc_distance(lc, xo);

    /* XXX: may need to worry about periodic boundary */
    return distance - lc->speedfactor * HorizonDistance(a, lc->horizon);
}

double
fastpm_lc_distance(FastPMLightCone * lc, double x[3])
{
    double distance;
    if (lc->fov <= 0) {
        distance = x[2];
    } else {
        distance = 0;
        int d;
        for (d = 0; d < 3; d ++) {
            distance += x[d] * x[d];
        }
        distance = sqrt(distance);
    }
    return distance;
}

static int
_fastpm_usmesh_intersect_one(FastPMUSMesh * mesh,
        struct funct_params * params,
        ptrdiff_t i,
        double * solution)
{
    params->i = i;

    return fastpm_horizon_solve(mesh->lc->horizon,
        params->context,
        solution,
        params->a1, params->a2,
        funct, params);
}

static double
zangle(double * x) {
    double dxy = 0;
    double dz = x[2];
    dxy = x[0] * x[0] + x[1] * x[1];

    double rt = atan2(sqrt(dxy), dz) / M_PI * 180.;
    if (rt < 0) rt += 360.;
    return rt;
}

/* is a vector in the i-th octant.
 * The ordering of octants is in C ordering, with
 * the Z-axis the fast changing direction.
 *
 * Thus [0, 4) are x > 0.
 *
 * The tolerance is in the same unit as the vector.
 * */
static int
_in_octant(int i, double vec[3], double tol)
{
    static const int signs[][3] = {
        {1, 1, 1},
        {1, 1, -1},
        {1, -1, 1},
        {1, -1, -1},
        {-1, 1, 1},
        {-1, 1, -1},
        {-1, -1, 1},
        {-1, -1, -1},
    };
    int d;
    for(d = 0; d < 3; d ++) {
        double s = vec[d] * signs[i][d];
        if(s < -tol) {
            return 0;
        }
    }
    return 1;
}
int
fastpm_lc_inside(FastPMLightCone * lc, double vec[3])
{
    if(lc->fov > 0) {
        int r;
        if(lc->fov < 360) {
            r = zangle(vec) <= lc->fov * 0.5;
        } else {
            r = 1;
        }
        if(r) {
            double norm = 0;
            int d;
            for(d = 0; d < 3; d ++) {
                norm += vec[d] * vec[d];
            }
            norm = sqrt(norm);
            int i;
            for(i = 0; i < 8; i ++) {
                if(lc->octants[i] && _in_octant(i, vec, lc->tol * norm)) return 1;
            }
            return 0;
        } else {
            return 0;
        }
    } else {
        /* smesh takes care of this */
        return 1;
    }
}

/* Compute an approximate AABB of all particles
 * based on position at two times, assuming linear motion.
 * */
static void
fastpm_compute_bbox(FastPMStore * p,
        FastPMDriftFactor * drift,
        double a1,
        double a2,
        double padding,
        double xmin[3],
        double xmax[3],
        MPI_Comm comm
) {
    ptrdiff_t i;
    for(int d = 0; d < 3; d ++) {
        xmin[d] = 1e20;
        xmax[d] = -1e20;
    }
    for(i = 0; i < p->np; i ++) {
        int d;
        for(d = 0; d < 3; d ++) {
            double xo[3];
            fastpm_drift_one(drift, p, i, xo, a1);
            if(xo[d] < xmin[d]) xmin[d] = xo[d];
            if(xo[d] > xmax[d]) xmax[d] = xo[d];
            fastpm_drift_one(drift, p, i, xo, a2);
            if(xo[d] < xmin[d]) xmin[d] = xo[d];
            if(xo[d] > xmax[d]) xmax[d] = xo[d];
        }
    }
    // It is sufficient not doing a reduce; each rank can cull its own
    // volume.
    // MPI_Allreduce(MPI_IN_PLACE, xmin, 3, MPI_DOUBLE, MPI_MIN, comm);
    // MPI_Allreduce(MPI_IN_PLACE, xmax, 3, MPI_DOUBLE, MPI_MAX, comm);
    for(int d = 0; d < 3; d ++) {
        xmin[d] -= padding;
        xmax[d] += padding;
    }
}

#include "spherebox.h"
void
rotate_vec3(vec3 v3, double glmatrix[4][4], int translate) {
    double xi[4];
    double xo[4];
    for(int d = 0; d < 3; d ++) {
        xi[d] = v3.v[d];
    }
    if(translate) xi[3] = 1;
    else xi[3] = 0;
    fastpm_gldot(glmatrix, xi, xo);
    for(int d = 0; d < 3; d ++) {
        v3.v[d] = xo[d];
    }
}
/*
 * if a shell in (radius1, radius2) intersects a AABB box at xmin to xmax.
 *
 * Apply glmatrix and tileshift to the AABB first to transform it to the intended location.
 * */
int
fastpm_shell_intersects_bbox(
    double xmin[3],
    double xmax[3],
    double glmatrix[4][4],
    double tileshift[3],
    double radius1,
    double radius2
)
{
    box bbox = make_box(xmin, xmax);

    /* rotate all planes, and apply tileshift */
    for(int i = 0; i < 6; i ++) {
        rotate_vec3(bbox.planes[i].position, glmatrix, 1);
        rotate_vec3(bbox.planes[i].direction, glmatrix, 0);
        for(int d = 0; d < 3; d ++) {
            bbox.planes[i].position.v[d] += tileshift[d];
        }
    }
    for(int i = 0; i < 8; i ++) {
        rotate_vec3(bbox.corners[i], glmatrix, 1);
        for(int d = 0; d < 3; d ++) {
            bbox.corners[i].v[d] += tileshift[d];
        }
    }
    sphere s1 = make_sphere0(radius1);
    if (box_inside_sphere(bbox, s1)) return 0;
    sphere s2 = make_sphere0(radius2);
    if (sphere_outside_box(s2, bbox)) return 0;

    return 1;
}
/* FIXME:
 * the function shall take ai, af as input,
 *
 * the function shall be able to interpolate potential as
 * well as position and velocity.
 * We need a more general representation of '*drift' and '*kick'.
 *
 * */
static int
fastpm_usmesh_intersect_tile(FastPMUSMesh * mesh, double * tileshift,
        double a1,
        double a2,
        FastPMDriftFactor * drift,
        FastPMKickFactor * kick,
        FastPMStore * p,
        FastPMStore * pout
)
{
    FastPMLightCone * lc = mesh->lc;
    struct funct_params params = {
        .lc = lc,
        .drift = drift,
        .p = p,
        .i = 0,
        .a1 = a1,
        .a2 = a2,
    };

    int a1_is_outside = (params.a1 > mesh->amax) || (params.a1 < mesh->amin);
    int a2_is_outside = (params.a2 > mesh->amax) || (params.a2 < mesh->amin);

    if(a1_is_outside && a2_is_outside) {
        return 0;
    }

    {
        int d;
        for(d = 0; d < 3; d ++) {
            params.tileshift[d] = tileshift[d];
        }
    }

    params.tileshift[3] = 0;

    ptrdiff_t i;

    #pragma omp parallel firstprivate(params)
    {
        params.context = fastpm_horizon_solve_start();
        #pragma omp for
        for(i = 0; i < p->np; i ++) {
            double a_emit = 0;
            if(0 == _fastpm_usmesh_intersect_one(mesh, &params, i, &a_emit)) continue;

            /* the event is outside the region we care, skip */
            if(a_emit > mesh->amax || a_emit < mesh->amin) continue;

            double xi[4];
            double xo[4];
            int d;

            xi[3] = 1;
            if(p->v) {
                /* can we drift? if we are using a fixed grid there is no v. */
                fastpm_drift_one(drift, p, i, xi, a_emit);
            } else {
                for(d = 0; d < 3; d ++) {
                    xi[d] = p->x[i][d];
                }
            }
            for(d = 0; d < 4; d ++) {
                xi[d] += params.tileshift[d];
            }
            /* transform the coordinate */
            fastpm_gldot(lc->glmatrix, xi, xo);

            /* does it fall into the field of view? */
            if(!fastpm_lc_inside(lc, xo)) continue;

            /* A solution is found */
            /* move the particle and store it. */
            ptrdiff_t next;
            #pragma omp atomic capture
                next = pout->np++;

            if(next >= pout->np_upper) {
                continue;
            }

            /* copy the position if desired */
            if(pout->x) {
                for(d = 0; d < 3; d ++) {
                    pout->x[next][d] = xo[d];
                }
            }

            float vo[4];
            float vi[4];
            if(p->v) {
                /* can we kick? if we are using a fixed grid there is no v */
                fastpm_kick_one(kick, p, i, vi, a_emit);
                vi[3] = 0;
                /* transform the coordinate */
                fastpm_gldotf(lc->glmatrix, vi, vo);

                if(pout->v) {
                    for(d = 0; d < 3; d ++) {
                        /* convert to peculiar velocity a dx / dt in kms */
                        pout->v[next][d] = vo[d] * HubbleConstant / a_emit;
                    }
                }
            }
            if(pout->id)
                pout->id[next] = p->id[i];
            if(pout->aemit)
                pout->aemit[next] = a_emit;
            if(pout->mask)
                pout->mask[next] = p->mask[i];

            double potfactor = 1.5 * lc->cosmology->Omega_cdm / (HubbleDistance * HubbleDistance);
            /* convert to dimensionless potential */
            if(pout->potential)
                pout->potential[next] = p->potential[i] / a_emit * potfactor;

            if(pout->tidal) {
                for(d = 0; d < 6; d++) {
                    pout->tidal[next][d] = p->tidal[i][d] / a_emit * potfactor;
                }
            }
        }
        fastpm_horizon_solve_end(params.context);
    }

    if(pout->np >= pout->np_upper) {
        fastpm_raise(-1, "Too many particles in the light cone; limit = %td, wanted = %td\n", pout->np_upper, pout->np);
    }
    return 0;
}

void
fastpm_usmesh_emit(FastPMUSMesh * mesh, int whence)
{
    /* a portion of light cone is ready between a0 and a1 */
    FastPMLCEvent lcevent[1];

    lcevent->p = mesh->p;
    lcevent->ai = mesh->ai;
    lcevent->af = mesh->af;
    lcevent->whence = whence;

    fastpm_emit_event(mesh->event_handlers,
            FASTPM_EVENT_LC_READY, FASTPM_EVENT_STAGE_AFTER,
            (FastPMEvent*) lcevent, mesh);

}

int
fastpm_usmesh_intersect(FastPMUSMesh * mesh, FastPMDriftFactor * drift, FastPMKickFactor * kick,
    double a1, double a2, int whence, MPI_Comm comm)
{
    {
        double t1 = a1, t2 = a2;
        a1 = fmin(t1, t2);
        a2 = fmax(t1, t2);
    }
    CLOCK(intersect);
    if (whence == TIMESTEP_START) {
        mesh->ai = a1;
        mesh->af = a1;
        fastpm_info("usmesh start event from %0.4f to %0.4f.\n", mesh->ai, mesh->af);
        mesh->np_before = 0;
        fastpm_usmesh_emit(mesh, whence);
    } else
    if (whence == TIMESTEP_CUR) {
        double xmin[3] = {0};
        double xmax[3] = {0};
        double padding = 0.5; /* rainwoodman: add a 500 Kpc/h padding; need a better estimate. */
        fastpm_compute_bbox(mesh->source, drift, a1, a2, padding, xmin, xmax, comm);

        fastpm_info("usmesh: bounding box computed for a = (%g, %g), AABB = [ %g %g %g ] - [ %g %g %g]",
                a1, a2,
                xmin[0], xmin[1], xmin[2],
                xmax[0], xmax[1], xmax[2]
            );

        /* for each tile */
        int t;
        ENTER(intersect);
        fastpm_info("usmesh intersection from %0.4f to %0.4f with %d tiles.\n", a1, a2, mesh->ntiles);

        double r1 = mesh->lc->speedfactor * HorizonDistance(a1, mesh->lc->horizon);
        double r2 = mesh->lc->speedfactor * HorizonDistance(a2, mesh->lc->horizon);

        double volume = 4 * M_PI / 3 * (pow(r1, 3) - pow(r2, 3));

        int steps = (int) ((volume / mesh->target_volume) + 0.5);
        if(steps == 0) steps = 1;
        double da = (a1 - a2) / steps;

        for(int i = 0; i < steps; i ++) {
            double ai = a1 + da * i;
            double af = (i + 1 == steps)?a2 : a1 + da * (i + 1);
            fastpm_info("usmesh: intersection step %d / %d a = %g %g .\n", i, steps, ai, af);

            int ntiles = 0;
            for(t = 0; t < mesh->ntiles; t ++) {
                /* for spherical geometry, skip if the tile does not intersects the lightcone. */
                if(mesh->lc->fov > 0 && !fastpm_shell_intersects_bbox(
                    xmin, xmax, mesh->lc->glmatrix, &mesh->tileshifts[t][0], r2, r1)) {
                    continue;
                }
                fastpm_usmesh_intersect_tile(mesh, &mesh->tileshifts[t][0],
                        ai, af,
                        drift, kick,
                        mesh->source,
                        mesh->p); /*Store particle to get density*/
                ntiles ++;
            }
            double ntiles_max, ntiles_min, ntiles_mean;
            MPIU_stats(comm, ntiles, "<->", &ntiles_min, &ntiles_mean, &ntiles_max);
            fastpm_info("usmesh: number of bounding box intersects shell r = (%g %g), min = %g max = %g, mean=%g",
                  r2, r1, ntiles_min, ntiles_max, ntiles_mean);
            LEAVE(intersect);
            mesh->af = af;
            if(MPIU_Any(comm, mesh->p->np > 0.5 * mesh->p->np_upper)) {
                fastpm_info("usmesh cur event from %0.4f to %0.4f.\n", mesh->ai, mesh->af);
                fastpm_usmesh_emit(mesh, whence);
                mesh->np_before += mesh->p->np;
                /* now purge the store. */
                mesh->p->np = 0;
                mesh->ai = mesh->af;
            }
        }
    } else
    if (whence == TIMESTEP_END) {
        mesh->af = a2;
        fastpm_info("usmesh end event from %0.4f to %0.4f.\n", mesh->ai, mesh->af);
        fastpm_usmesh_emit(mesh, whence);
        mesh->np_before += mesh->p->np;
        /* now purge the store. */
        mesh->p->np = 0;
        mesh->ai = mesh->af;
    }
    return 0;
}


/*
  @copyright Russell Standish 2000-2013
  @author Russell Standish
  This file is part of EcoLab

  Open source licensed under the MIT license. See LICENSE for details.
*/

#include <classdesc_access.h>
#include <pack_stl.h>
#include <analysis.h>

#if defined(GNUSL)
#include <gsl/gsl_errno.h>
#include <gsl/gsl_odeiv2.h>
#endif

using namespace ecolab;
typedef map<NautyRep,double> archetype;
using namespace classdesc;
using array_ns::array;

struct Gsl_matrix;
struct GaussianMarkovModel
{
  /// lags and replications used calculating causal density and ilk
  int lags;
  Gsl_matrix* runData;
  typedef ConcreteGraph<DiGraph> G;
  G& g;
  gaussrand gran;

  GaussianMarkovModel(G& g): lags(10), runData(NULL), g(g) {}
  ~GaussianMarkovModel();
  /// run the model for lags+repetitions steps to populate the matrices
  void run(TCL_args repetitions);
  /// output the rundata into a file: each node has separate row, each
  /// sample column
  void outputRunData(TCL_args);
  /// calculate the causal density for a Gaussian Markov process
  /// running on the network.
  double causalDensity();
  double bipartitionCausalDensity();
};

// RAII data structure for GSL Runge-Kutta algorithm
struct RKdata
{
#if defined(GNUSL)
  gsl_odeiv2_system sys;
  gsl_odeiv2_driver* driver;
  template <class DynSysGen>
  RKdata(const DynSysGen& dynSys) {
    sys.function=DynSysGen::RKfunction;
    sys.dimension=dynSys.dim;
    sys.params=(void*)&dynSys;
    const gsl_odeiv2_step_type* stepper=gsl_odeiv2_step_rkf45;
    driver = gsl_odeiv2_driver_alloc_y_new
        (&sys, stepper, dynSys.stepMax, dynSys.epsAbs,
         dynSys.epsRel);
      gsl_odeiv2_driver_set_hmax(driver, dynSys.stepMax);
      gsl_odeiv2_driver_set_hmin(driver, dynSys.stepMin);
    }
    ~RKdata() {gsl_odeiv2_driver_free(driver);}
#else
  int sys, driver; // dummies, to keep classdesc happy
#endif
  };

// dynamic system network generators
template <class DynSys>
struct DynSysGen: public DynSys, public NetworkFromTimeSeries
{
  static int RKfunction(double t, const double x[], double f[], void *params)
  {
#if defined(GNUSL)
    if (params==NULL) return GSL_EBADFUNC;
    try
      {
        ((const DynSys*)params)->f(t,x,f);
      }
    catch (std::exception& e)
      {
        Tcl_AppendResult(interp(),e.what(),NULL);
        Tcl_AppendResult(interp(),"\n",NULL);
        return GSL_EBADFUNC;
      }
    return GSL_SUCCESS;
#else
    return 0;
#endif
  }
  // RK parameters
  double stepMin; // minimum step size
  double stepMax; // maximum step size
  double epsAbs;  // absolute error
  double epsRel;  // relative error
  DynSysGen(): stepMin(1e-5), stepMax(1e-3), epsAbs(1e-3), epsRel(1e-2)
  {
    resize(DynSys::dim);
    n.resize(DynSys::dim,100);
  }

  void fillNet(int nsteps) {
#if defined(GNUSL)
    RKdata rkData(*this);
    vector<double> x(DynSys::dim);
    for (size_t i=0; i<x.size(); ++i)
      x[i]=uni.rand();
    double t=0;
    gsl_odeiv2_driver_set_nmax(rkData.driver, 1);
    for (int i=0; i<nsteps; ++i)
      {
        gsl_odeiv2_driver_apply(rkData.driver,&t,1,&x[0]);
        for (size_t j=0; j<size(); ++j)
          (*this)[j]<<=x[j];
      }
    constructNet();
#endif
  }
  double complexity() const {return ::complexity(net);}
  urand uni;
  void random_rewire() {::random_rewire(net,uni);}
};

// example dynamical systems defined here
struct Lorenz
{
  static const unsigned dim=3;
  double sigma, rho, beta;
  Lorenz(): sigma(10), rho(28), beta(8/3.0) {}
  void f(double t, const double x[3], double y[3]) const
  {
    y[0]=sigma*(x[1]-x[0]);
    y[1]=x[0]*(rho-x[2])-x[1];
    y[2]=x[0]*x[1]-beta*x[2];
  }
};

struct HenonHeiles
{
  static const unsigned dim=4;
  double lambda;
  HenonHeiles(): lambda(1) {}
  void f(double t, const double x[4], double y[4]) const
  {
    y[0]=x[1];
    y[1]=-x[0]-2*lambda*x[0]*x[2];
    y[2]=x[3];
    y[3]=-x[2]-lambda*(x[0]*x[0]-x[2]*x[2]);
  }
};

// small 1D cellular automata
struct Small1DCA
{
  unsigned rule; ///< rule in 1..255
  unsigned nCells; ///< no cells < 0..8*sizeof(state)
  unsigned long state;
  /// 3 bit neighbourhood of cell i
  unsigned nbhd(unsigned i)
  {
    if (i==0)
      return ((state&3)<<1) | ((state>>(nCells-1)) & 1);
    else if (i==nCells-1)
      return ((state>>(i-1))&3) | ((state&1)<<2);
    else
      return state>>(i-1)&7;
  }
  // ratio of 1 bits out of 8
  double lambda() {
    int sum=0;
    for (int i=0; i<8; ++i)
      sum+=(rule>>i)&1;
    return sum/8.0;
  }
  void step() {
    unsigned long newState=0;
    for (unsigned i=0; i<nCells; ++i)
      {
        unsigned n=nbhd(i);
        newState |= ((rule>>n) & 1) << i;
      }
    state=newState;
  }
};

struct Larger1DCA
{
  // number of states a cell can take, and neighbourhood size (always odd)
  unsigned nStates;
  int nbhdSz;
  vector<unsigned> rule,cells;
  urand uni;
  size_t nbhd(unsigned i)
  {
    size_t r=0;
    for (int j=-nbhdSz/2; j<=nbhdSz/2; ++j)
      {
        int ii=i+j;
        if (ii<0) ii+=cells.size();
        if (ii>int(cells.size())-1) ii-=cells.size();
        r*=nStates;
        r+=cells[ii];
      }
    return r;
  }
  // ratio of non-quiescent states in rule to total
  double lambda() {
    int sum=0;
    for (unsigned i=0; i<rule.size(); ++i)
      sum+=rule[i]>0;
    return double(sum)/rule.size();
  }
  // resize rule vector, given nStates and nbhdSz
  void resizeRule() {
    rule.resize(pow(nStates,nbhdSz));
  }
  // installs a random rule matching a given lambda
  void initRandomRule(double lambda) {
    resizeRule();
    for (size_t i=0; i<rule.size(); ++i)
      if (uni.rand()>lambda)
        rule[i]=0;
      else
        rule[i]=(nStates-1)*uni.rand()+1;
  }
  void initRandom() {
    for (size_t i=0; i<cells.size(); ++i)
      cells[i]=nStates*uni.rand();
  }
  void step() {
    vector<unsigned> newState(cells.size());
    for (unsigned i=0; i<cells.size(); ++i)
      newState[i] = rule[nbhd(i)];
    cells.swap(newState);
  }

};

template <class T>
struct NodeMap: map<T,unsigned>
{
  unsigned nextId;
  NodeMap(): nextId(0) {}
  unsigned operator[](const T& x) {
    typename map<T,unsigned>::iterator i=this->find(x);
    if (i!=this->end())
      return i->second;
    else
      return map<T,unsigned>::operator[](x)=nextId++;
  }
};

struct NetworkFromSmall1DCA:
  public Small1DCA, public NetworkFromTimeSeries
{
  void fillNet(int nsteps, int transient) {
    resize(1); back().clear();
    state=pow(2,nCells)*uni.rand();
    NodeMap<unsigned long> nodeMap;

    // throw away the intial transient
    for (int i=0; i<transient; ++i) step();
    for (int i=0; i<nsteps; ++i)
      {
        step();
        back()<<=nodeMap[state];
 //       if (nodeMap.nextId>10)
 //         cout << back() << " ";
      }
    //   cout<<endl;
    n.resize(1); n[0]=nodeMap.nextId;
    constructNet();
  }

  double complexity() const {return ::complexity(net);}
  urand uni;
  void random_rewire() {::random_rewire(net,uni);}

};

struct NetworkFromLarger1DCA:
  public Larger1DCA, public NetworkFromTimeSeries
{
  void fillNet(int nsteps, int transient) {
    resize(1); back().clear();
    initRandom();
    // throw away the intial transient
    for (int i=0; i<transient; ++i) step();
    // map of state to node tag
    NodeMap<vector<unsigned> > nodeMap;
    unsigned tagid=1;
    for (int i=0; i<nsteps; ++i)
      {
        step();
        back()<<=nodeMap[cells];
      }
    n.resize(1); n[0]=nodeMap.nextId;
    constructNet();
  }

  double complexity() const {return ::complexity(net);}
  urand uni;
  void random_rewire() {::random_rewire(net,uni);}

};

class netc_t
{
  archetype archc;
  Iterator<archetype> ic;
  bool fill_curr();
  CLASSDESC_ACCESS(netc_t);
public:
  netc_t(): weight_dist("one"), gProcess(g) {}
  unsigned currc;
  void reset_arch() {archc.clear();}
  bool first();
  bool next();
  bool last() {return ic==archc.end();}
  unsigned size() {return archc.size();}
  double ODcomplexity() {return ::ODcomplexity(g);}
  double MA() {return ::MA(g);}
  void exhaust_search(TCL_args);
  //  void gen_uniq_rand(TCL_args);

  ConcreteGraph<DiGraph> g;
  Degrees degrees;
  void update_degrees() {degrees=::degrees(g);}
  urand uni;
  double complexity() {return ::complexity(g);}
  double lnomega() {BitRep eCanon; return canonical(eCanon,g);}
  double nauty_complexity() {return ::complexity(NautyRep(g));}
  double nauty_lnomega() {return NautyRep(g).lnomega();}
  double saucy_lnomega() {return ::saucy_lnomega(g);}

  /** race algorithms against each other in separate processes. Best
      run on a multicore machine with as many or more cores than there
      are algorithms (currently 4).

      @param time (in seconds) limiting the race
      @return the number of te winning algorithm, or 0 if the race times out
  */
  int automorphism_race(TCL_args);
  /**
     races variants of the bliss algorithm with different values of
     the splitting heuristic
     @param time (in seconds) limiting the  race
     @return the winning splitting heuristic, or -1 if the race times out
  */
  int bliss_race(TCL_args);
  /// returns a string representation of the algorithm given by the
  /// integer argument
  string automorphism_algorithm_name(TCL_args);

  TCL_obj_ref<random_gen> weight_dist;
  array<float> weights() {
    array<float> r;
    for (ConcreteGraph<DiGraph>::const_iterator e=g.begin(); e!=g.end(); ++e)
      r<<=e->weight;
    return r;
  }
  void genRandom(TCL_args args) {
    ErdosRenyi_generate(g,args[0],args[1],uni,*weight_dist);}
  void prefAttach(TCL_args args) {
    TCL_obj_ref<random_gen> indegree_dist;
    indegree_dist.set(args[1]);
    PreferentialAttach_generate(g,args[0],uni,*indegree_dist,*weight_dist);
  }
  void random_rewire() {::random_rewire(g,uni);}
  /// generate a fully connected directed graph, with each node having
  /// same indegree and outdegree. Only works if graph degree is odd.
  void genFullDirected(TCL_args args);
  bool testAgainstNauty();

  GaussianMarkovModel gProcess;

  DynSysGen<Lorenz> lorenzGen;
  DynSysGen<HenonHeiles> henonHeilesGen;
  NetworkFromSmall1DCA small1DCA;
  NetworkFromLarger1DCA large1DCA;
};


/* min/convergence.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 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 <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_min.h>

int
gsl_min_test_interval (double x_lower, double x_upper, double epsabs, double epsrel)
{
  const double lower = x_lower;
  const double upper = x_upper;

  const double abs_lower = fabs(lower) ;
  const double abs_upper = fabs(upper) ;

  double min_abs, tolerance;

  if (epsrel < 0.0)
    GSL_ERROR ("relative tolerance is negative", GSL_EBADTOL);

  if (epsabs < 0.0)
    GSL_ERROR ("absolute tolerance is negative", GSL_EBADTOL);

  if (lower > upper)
    GSL_ERROR ("lower bound larger than upper_bound", GSL_EINVAL);

  if ((lower > 0 && upper > 0) || (lower < 0 && upper < 0))
    {
      min_abs = GSL_MIN_DBL(abs_lower, abs_upper) ;
    }
  else
    {
      min_abs = 0;
    }

  tolerance = epsabs + epsrel * min_abs  ;

  if (fabs(upper - lower) < tolerance)
    return GSL_SUCCESS;

  return GSL_CONTINUE ;
}