/*
 * Copyright (c) 2018-2021 Aleksas Mazeliauskas, Stefan Floerchinger,
 *                    Eduardo Grossi, and Derek Teaney
 * All rights reserved.
 *
 * FastReso is distributed under MIT license;
 * see the LICENSE file that should be present in the root
 * of the source distribution, or alternately available at:
 * https://github.com/amazeliauskas/FastReso/
 */
#ifndef FASTRESO_TFastReso_formulas_h
#define FASTRESO_TFastReso_formulas_h
#include <gsl/gsl_integration.h>
#include "TParticle.h"
#include <vector>
// List of parameters for GSL integration procedure.
#include "qag_params.h"
#include "grid_params.h"

#include <gsl/gsl_errno.h>
//using namespace std;
//! Eabc -- energy of b in restframe of a in a->b+c decay
double get_Eabc(double ma,double mb,double mc) { return (ma*ma+mb*mb-mc*mc)/(2*ma);}
//! particle momenta
double get_p(double E, double m) { return sqrt(E*E-m*m);}
//! pabc -- momentum of b or c in restframe of a in a->b+c decay
double get_pabc(double ma,double mb,double mc) { return get_p(get_Eabc(ma,mb,mc),mb);}

//! returns thermal Bose-Einstein, Fermi-Dirac or Boltzmann distribution
double get_thermal_F(double Ebar, double T, double QMu, EParticleType type);
//! returns derivative of thermal Bose-Einstein, Fermi-Dirac or Boltzmann distribution
double get_thermal_dF(double Ebar, double T, double QMu, EParticleType type);

//! Initialize f_j on the freeze-out surface
//! Here you can add additional fj's
double get_initial_Fj(EFjIndex j, double Ebar, double T, double QMu, EParticleType type, double m, double Cs2)
{
  double pbar = sqrt(Ebar*Ebar-m*m);
  switch(j){
    case EFjIndex::kFeq1: //feq 1
      return pbar*get_thermal_F(Ebar,T, QMu, type);
      break;
    case EFjIndex::kFeq2: //feq 2
      return pbar*get_thermal_F(Ebar,T, QMu, type);
      break;
    case EFjIndex::kFshear1: //fshear 1
      return pbar*pbar*pbar*get_thermal_dF(Ebar,T, QMu, type);
      break;
    case EFjIndex::kFshear2: //fshear 2
      return pbar*pbar*pbar*get_thermal_dF(Ebar,T, QMu, type);
      break;
    case EFjIndex::kFshear3: //fshear 3
      return pbar*pbar*pbar*get_thermal_dF(Ebar,T, QMu, type);
      break;
    case EFjIndex::kFbulk1: //fbulk 1
      return pbar*get_thermal_dF(Ebar,T, QMu, type)*(1./3.*m*m/T/Ebar-Ebar/T*(1./3.-Cs2));
      break;
    case EFjIndex::kFbulk2: //fbulk 2
      return pbar*get_thermal_dF(Ebar,T, QMu, type)*(1./3.*m*m/T/Ebar-Ebar/T*(1./3.-Cs2));
      break;
    case EFjIndex::kFtemp1: //ftemperature. 1
      return pbar*get_thermal_dF(Ebar,T, QMu, type)*Ebar/T;
      break;
    case EFjIndex::kFtemp2: //ftemperature 2
      return pbar*get_thermal_dF(Ebar,T, QMu, type)*Ebar/T;
      break;
    case EFjIndex::kFvel1: //fvelocity  1
      return pbar*pbar*get_thermal_dF(Ebar,T, QMu, type);
      break;
    case EFjIndex::kFvel2: //fvelocity 2
      return pbar*pbar*get_thermal_dF(Ebar,T, QMu, type);
      break;
    case EFjIndex::kFvel3: //fvelocity 3
      return pbar*pbar*get_thermal_dF(Ebar,T, QMu, type);
      break;
    default:
      std::cerr  << "\033[1mTFastReso.cpp::get_initial_Fj\033[0m : \033[1;31merror\033[0m :  wrong index "  << (int) j << std::endl;
      exit(EXIT_FAILURE);
  }
}
//! Transformation rule factor A for Fj in a 2-body decay
//! Here you can add additional rules for fj's
double get_factor_Aj(EFjIndex j, double Qw, double Ew, double Ebar, double pbar, double Ma, double pw)
{

//  double A= (Ma*Eabc/Mb/Mb-w*Ma*pabc/Mb/Mb*Ebar/pbar);
//  double Ew= (Ma*Eabc*Ebar/Mb/Mb-w*Ma*pabc*pbar/Mb/Mb);

  switch( j){
    case EFjIndex::kFeq1: // feq 1
      return Qw/pw;
      break;
    case EFjIndex::kFeq2: // feq 2
      return pbar/pw*Ew/Ebar;
      break;
    case EFjIndex::kFshear1: // fshear 1
      return Qw/pw*(2.5*Qw/pw*Qw/pw-1.5);
      break;
    case EFjIndex::kFshear2: // fshear 2
      return Qw/pw;
      break;
    case EFjIndex::kFshear3: // fshear 3
      return (1.5*Qw/pw*Qw/pw-0.5)*Ew/Ebar*pbar/pw;
      break;
    case EFjIndex::kFbulk1: // fbulk 1
      return Qw/pw;
      break;
    case EFjIndex::kFbulk2: // fbulk 2
      return pbar/pw*Ew/Ebar;
      break;
    case EFjIndex::kFtemp1: // ftemperature 1
      return Qw/pw;
      break;
    case EFjIndex::kFtemp2: // ftemperature 2
      return pbar/pw*Ew/Ebar;
      break;
    case EFjIndex::kFvel1: // fvelocity 1
      return 1.5*Qw/pw*Qw/pw-0.5;
      break;
    case EFjIndex::kFvel2: // fvelocity 2
      return 1;
      break;
    case EFjIndex::kFvel3: // fvelocity 3
      return pbar/pw*Qw/pw*Ew/Ebar;
      break;
    default:
      std::cerr  << "\033[1mTFastReso.cpp::get_factor_Aj\033[0m : \033[1;31merror\033[0m :  wrong index "  << (int) j << std::endl;
      exit(EXIT_FAILURE);
  }
}

//! Parameters for integration of thermal distribution
struct thermal_params{
  double m;
  double Tfo;
  double QMu;
  EParticleType type;
};
//! Thermal distribution function for integration over vbar=[0,1]
double get_F_p_dvbar(double vbar, void * p) {
  double &m           = ((struct thermal_params *) p)->m;
  double &Tfo           = ((struct thermal_params *) p)->Tfo;
  double &QMu           = ((struct thermal_params *) p)->QMu;
  EParticleType &type           = ((struct thermal_params *) p)->type;
  double Ebar=m/sqrt(1-vbar*vbar);
  double pbar=vbar*Ebar;
  double F_p = get_thermal_F(Ebar,Tfo,QMu, type);
  return F_p*pbar*pbar*4*M_PI*m/pow(2*M_PI*sqrt(1-vbar*vbar),3);
}
//! Parameters for integration of two body decay function
struct twobody_params{
  double Ebar;
  double Ma;
  double Mb;
  double Mc;
  TParticle * Parent;
  EFjIndex index;
};
//! Integrand of two body decay formula for f_j components
double get_F_pu_dw(double w, void * p) {
  double &Ebar           = ((struct twobody_params *) p)->Ebar;
  double &Ma             = ((struct twobody_params *) p)->Ma;
  double &Mb             = ((struct twobody_params *) p)->Mb;
  double &Mc             = ((struct twobody_params *) p)->Mc;
  TParticle *Parent      = ((struct twobody_params *) p)->Parent;
  EFjIndex &index             = ((struct twobody_params *) p)->index;

  double Eabc = get_Eabc(Ma,Mb,Mc);
  double pabc = get_p(Eabc, Mb);
  //double pbar = GSL_MAX(get_p(Ebar, Mb),DBL_EPSILON);
  double pbar = get_p(Ebar, Mb);// ,DBL_EPSILON);
  double Ebar_old = Ebar*Eabc*Ma/Mb/Mb-w*pbar*pabc*Ma/Mb/Mb;
  //double Ebar_old = pbar*pabc*Ma/Mb/Mb*(1-w);
  //double Ebar_old = pbar*pabc/Ma*u;
  double F_old=Parent->get_Fj( (int) index, Ebar_old);
  double Qw= (Ma*Eabc*pbar/Mb/Mb-w*Ma*pabc/Mb/Mb*Ebar);
  //double Qw = pabc/Ma*u;
//  double Ew= (Ma*Eabc*Ebar/Mb/Mb-w*Ma*pabc*pbar/Mb/Mb);
  double A = get_factor_Aj(index, Qw, Ebar_old, Ebar, pbar, Ma,sqrt(Qw*Qw+(1-w*w)*Ma*Ma/Mb/Mb*pabc*pabc));
  return F_old*A;
}
double get_F_pu_du(double u, void * p) {
  double &Ebar           = ((struct twobody_params *) p)->Ebar;
  double &Ma             = ((struct twobody_params *) p)->Ma;
  double &Mb             = ((struct twobody_params *) p)->Mb;
  double &Mc             = ((struct twobody_params *) p)->Mc;
  TParticle *Parent      = ((struct twobody_params *) p)->Parent;
  EFjIndex &index             = ((struct twobody_params *) p)->index;

  double Eabc = get_Eabc(Ma,Mb,Mc);
  double pabc = get_p(Eabc, Mb);
  //double pbar = GSL_MAX(get_p(Ebar, Mb),DBL_EPSILON);
  double pbar = get_p(Ebar, Mb);// ,DBL_EPSILON);
  //double Ebar_old = Ebar*Eabc*Ma/Mb/Mb-w*pbar*pabc*Ma/Mb/Mb;
  //double Ebar_old = pbar*pabc*Ma/Mb/Mb*(1-w);
  if (pbar==0) { return 0;};
  double Ebar_old = Ma/2*(pbar/pabc+pabc/pbar)+u*Ma;
  double F_old=Parent->get_Fj((int) index, Ebar_old);
  //double Qw= (Ma*Eabc/Mb/Mb-w*Ma*pabc/Mb/Mb*Ebar/pbar);
  double Qw = Ma/2/pbar*(pbar*pbar/pabc-pabc)+Ma*u;
//  double Ew= (Ma*Eabc*Ebar/Mb/Mb-w*Ma*pabc*pbar/Mb/Mb);
  double A = get_factor_Aj(index, Qw, Ebar_old, Ebar, pbar, Ma,sqrt(Qw*Qw+2*u*pabc/pbar*Ma*Ma));
  return F_old*A/(pbar*pabc/Ma/Ma);
}

//! Parameters for three body decay mass integral
struct threebody_params{
  gsl_function  *twobody_function;
  double Mc;
  double Md;
  gsl_integration_workspace *twobody_workspace;
};

//! Integrand for three body decay normalization
double get_B_dm(double mct, void * p) {
  gsl_function *twobody_function = ((struct threebody_params *) p)->twobody_function;
  double &Ma = ((struct twobody_params *) twobody_function->params)->Ma;
  double &Mb = ((struct twobody_params *) twobody_function->params)->Mb;
  double &Mc             = ((struct threebody_params *) p)->Mc;
  double &Md             = ((struct threebody_params *) p)->Md;
  return get_pabc(Ma,Mb,mct)*get_pabc(mct, Mc, Md);
}
//! Integrand for three body decay function
double get_F_pu_dm(double mct, void * p) {
  gsl_function *twobody_function = ((struct threebody_params *) p)->twobody_function;
  ((struct twobody_params *) twobody_function->params)->Mc = mct;
  gsl_integration_workspace *twobody_workspace = ((struct threebody_params *) p)->twobody_workspace;
  double &Ma = ((struct twobody_params *) twobody_function->params)->Ma;
  double &Mb = ((struct twobody_params *) twobody_function->params)->Mb;
  double &Mc             = ((struct threebody_params *) p)->Mc;
  double &Md             = ((struct threebody_params *) p)->Md;
  double result,error;

  using namespace qag_params;
  int status = gsl_integration_qag (twobody_function, -1, 1, fEpsAbs,fEpsRel, fLimit,fKey ,twobody_workspace, &result, &error);
  // Check status of integration and reduce accuracy if failing
  if (status) {
    status = gsl_integration_qag (twobody_function, -1, 1, fEpsAbs,1e-4, fLimit,GSL_INTEG_GAUSS15,twobody_workspace, &result, &error);
    if (status) {
      status = gsl_integration_qag (twobody_function, -1, 1, 1e-4,1e-4, fLimit,GSL_INTEG_GAUSS15,twobody_workspace, &result, &error);
      if (status) {      std::cerr << "\033[1mTFastReso_formulas.h\033[0m : \033[1;31merror\033[0m : integration failure! status = " <<gsl_strerror ( status )<< ". " << result << "+_" << error << std::endl;
        exit(EXIT_FAILURE);
      }else {
        std::cerr << "\033[1mTFastReso_formulas.h\033[0m : \033[1;31mwarning\033[0m : reduced relative and absolute error (1e-4) integration. Result = " << result << "+-" << error <<std::endl;
      }
    } else {
      std::cerr << "\033[1mTFastReso_formulas.h\033[0m : \033[1;31mwarning\033[0m : reduced relative error (1e-4) integration. Result = " << result << "+-" << error <<std::endl;
    }
  }
  return result*get_pabc(Ma,Mb,mct)*get_pabc(mct, Mc,Md);
}
double get_F_pu_dm_u(double mct, void * p) {
  gsl_function *twobody_function = ((struct threebody_params *) p)->twobody_function;
  ((struct twobody_params *) twobody_function->params)->Mc = mct;
  gsl_integration_workspace *twobody_workspace = ((struct threebody_params *) p)->twobody_workspace;
  double &Ma = ((struct twobody_params *) twobody_function->params)->Ma;
  double &Mb = ((struct twobody_params *) twobody_function->params)->Mb;
  double &Mc             = ((struct threebody_params *) p)->Mc;
  double &Md             = ((struct threebody_params *) p)->Md;
  double result,error;

  using namespace qag_params;
  int status = gsl_integration_qagiu (twobody_function, 0.0, fEpsAbs,fEpsRel, fLimit,twobody_workspace, &result, &error);
  // Check status of integration and reduce accuracy if failing
  if (status) {
  status = gsl_integration_qagiu (twobody_function, 0.0, fEpsAbs,1e-4, fLimit,twobody_workspace, &result, &error);
    if (status) {
  status = gsl_integration_qagiu (twobody_function, 0.0, 1e-4,1e-4, fLimit,twobody_workspace, &result, &error);
      if (status) {      std::cerr << "\033[1mTFastReso_formulas.h\033[0m : \033[1;31merror\033[0m : integration failure! status = " << gsl_strerror (status )<< ". " << result << "+_" << error << std::endl;
        exit(EXIT_FAILURE);
      }else {
        std::cerr << "\033[1mTFastReso_formulas.h\033[0m : \033[1;31mwarning\033[0m : reduced relative and absolute error (1e-4) integration. Result = " << result << "+-" << error <<std::endl;
      }
    } else {
      std::cerr << "\033[1mTFastReso_formulas.h\033[0m : \033[1;31mwarning\033[0m : reduced relative error (1e-4) integration. Result = " << result << "+-" << error <<std::endl;
    }
  }

  return result*get_pabc(Ma,Mb,mct)*get_pabc(mct, Mc,Md);
}

void do_2bodydecay(TParticle * Parent, TParticle * Child1, TParticle * Child2, double BranchingRatio);
void do_3bodydecay(TParticle * Parent, TParticle * Child1, TParticle * Child2, TParticle * Child3, double BranchingRatio);

//! Initialize thermal distribution
double get_thermal_F(double Ebar, double T, double QMu, EParticleType type)
{
  switch(type){
    case EParticleType::kBoson:
      return 1.0/(exp((Ebar-QMu)/T)-1);
      break;
    case EParticleType::kFermion:
      return 1.0/(exp((Ebar-QMu)/T)+1);
      break;
    case EParticleType::kBoltzman:
      return exp(-(Ebar-QMu)/T);
      break;
    default:
      std::cerr << "\033[1mTFastReso.cpp::get_thermal_F\033[0m : \033[1;31merror\033[0m : unrecognined particle type!" <<std::endl;
      exit(EXIT_FAILURE);}
}
//! Initialize  derivative of t hermal distribution (With a minus sign)
double get_thermal_dF(double Ebar, double T, double QMu, EParticleType type)
{
  switch(type){
    case EParticleType::kBoson:
      return 1.0/(exp((Ebar-QMu)/T)-1)*(1+1.0/(exp((Ebar-QMu)/T)-1));
      break;
    case EParticleType::kFermion:
      return 1.0/(exp((Ebar-QMu)/T)+1)*(1-1.0/(exp((Ebar-QMu)/T)+1));
      break;
    case EParticleType::kBoltzman:
      return exp(-(Ebar-QMu)/T);
      break;
    default:
      std::cerr << "\033[1mTFastReso.cpp::get_thermal_F_pu\033[0m : \033[1;31merror\033[0m : unrecognined particle type!" <<std::endl;
      exit(EXIT_FAILURE);}
}

//! Perform two body decay integral: Parent->Child1 + Child2
void do_2bodydecay(TParticle * Parent, TParticle * Child1, TParticle * Child2, double BranchingRatio){
  using namespace qag_params;

  gsl_integration_workspace * fWorkspace2body ;
  fWorkspace2body = gsl_integration_workspace_alloc (fLimit);
  Parent->lock();
  double Ma = Parent->getM();
  double Mb = Child1->getM();
  double Mc = Child2->getM();
  double nua = Parent->getNu();
  double nub = Child1->getNu();
  double Ebar=0.0;
  Child1->clean_buffer();
  if (Ma < Mb+Mc) {
    std::cerr  << "\033[1mTFastReso.cpp::do_2bodydecays\033[0m : \033[1;31merror\033[0m : father particle lighter than childern! " << Ma << " < " << Mb+Mc << std::endl;
    exit(EXIT_FAILURE);
  }
  gsl_function func2body;
      if (Mb==0) {
  func2body.function = get_F_pu_du;
      } else {
  func2body.function = get_F_pu_dw;
      }
  twobody_params params = {Ebar, Ma, Mb, Mc, Parent, EFjIndex::kFeq1};
  func2body.params = &params;
  double error, result ;
  int sym=1 ;
  if (Child1==Child2 ){ sym=2; } else { sym=1; }

  double Nfather = Parent->getN();
  //! add fraction of the total yield to child
  Child1->addN(BranchingRatio*Nfather*sym);
  for (int i=0; i < Child1->getNpbar(); i++){
    double Ebar =Child1->getEbar(i);
    params.Ebar =Ebar;

    //for (int j=0; j < grid_params::fNf ; j++){
    for(auto j: Parent->fComponents) {
      params.index =j;
      double fac = 0;
      if (Mb==0) {
int status = gsl_integration_qagiu (&func2body, 0.0, fEpsAbs,fEpsRel, fLimit ,fWorkspace2body, &result, &error);

  // Check status of integration and reduce accuracy if failing
  if (status) {
status = gsl_integration_qagiu (&func2body, 0.0, fEpsAbs,1e-4, fLimit ,fWorkspace2body, &result, &error);
    if (status) {
status = gsl_integration_qagiu (&func2body, 0.0, 1e-4,1e-4, fLimit ,fWorkspace2body, &result, &error);
      if (status) {      std::cerr << "\033[1mTFastReso_formulas.h\033[0m : \033[1;31merror\033[0m : integration failure! status = " << gsl_strerror (status )<< ". " << result << "+_" << error << std::endl;
        exit(EXIT_FAILURE);
      }else {
        std::cerr << "\033[1mTFastReso_formulas.h\033[0m : \033[1;31mwarning\033[0m : reduced relative and absolute error (1e-4) integration. Result = " << result << "+-" << error <<std::endl;
      }
    } else {
      std::cerr << "\033[1mTFastReso_formulas.h\033[0m : \033[1;31mwarning\033[0m : reduced relative error (1e-4) integration. Result = " << result << "+-" << error <<std::endl;
    }
  }

      fac = sym*BranchingRatio*nua/nub/2;
      }
      else {
int status = gsl_integration_qag (&func2body, -1, 1, fEpsAbs,fEpsRel, fLimit,fKey ,fWorkspace2body, &result, &error);

  // Check status of integration and reduce accuracy if failing
  if (status) {
status = gsl_integration_qag (&func2body, -1, 1, fEpsAbs,1e-4, fLimit, GSL_INTEG_GAUSS15,fWorkspace2body, &result, &error);
    if (status) {
status = gsl_integration_qag (&func2body, -1, 1, 1e-4,1e-4, fLimit, GSL_INTEG_GAUSS15 ,fWorkspace2body, &result, &error);
      if (status) {      std::cerr << "\033[1mTFastReso_formulas.h\033[0m : \033[1;31merror\033[0m : integration failure! status = " << gsl_strerror (status) << ". " << result << "+_" << error << std::endl;
        exit(EXIT_FAILURE);
      }else {
        std::cerr << "\033[1mTFastReso_formulas.h\033[0m : \033[1;31mwarning\033[0m : reduced relative and absolute error (1e-4) integration. Result = " << result << "+-" << error <<std::endl;
      }
    } else {
      std::cerr << "\033[1mTFastReso_formulas.h\033[0m : \033[1;31mwarning\033[0m : reduced relative error (1e-4) integration. Result = " << result << "+-" << error <<std::endl;
    }
  }

      fac = sym*BranchingRatio*nua/nub*pow(Ma/Mb,2)/2;

      }
      Child1->addFj((int) j,i, fac*result);

    }
  }
  //if (Child1=="pi0139plu"  ) { getParticle(Child1)->print_buffer(Child1+"_"+Parent+"_"+Child2);}
  gsl_integration_workspace_free (fWorkspace2body);

}

//! Perform three body decay integral Parent->Child1 + Child2 + Child3
void do_3bodydecay(TParticle * Parent, TParticle * Child1, TParticle * Child2, TParticle * Child3, double BranchingRatio){
  using namespace qag_params;
  gsl_integration_workspace * fWorkspace2body ;
  gsl_integration_workspace * fWorkspace3body ;
  fWorkspace2body = gsl_integration_workspace_alloc (fLimit);
  fWorkspace3body = gsl_integration_workspace_alloc (fLimit);
  Parent->lock();
  double Ma = Parent->getM();
  double Mb = Child1->getM();
  double Mc = Child2->getM();
  double Md = Child3->getM();
  double nua = Parent->getNu();
  double nub = Child1->getNu();
  double Ebar=0.0;

  Child1->clean_buffer();
  if (Ma < Mb+Mc+Md) {
    std::cerr  << "\033[1mTFastReso.cpp::do_3bdoydecays\033[0m : \033[1;31merror\033[0m : father particle lighter than childern! " << Ma << " < " << Mb+Mc+Md << std::endl;
    exit(EXIT_FAILURE);
  }
  gsl_function func2body;
  gsl_function func3body;
      if (Mb==0) {
  func2body.function = get_F_pu_du;
      } else {
  func2body.function = get_F_pu_dw;
      }
  func3body.function = get_B_dm;
  twobody_params params2 = {Ebar, Ma, Mb, Mc+Md, Parent, EFjIndex::kFeq1};
  func2body.params = &params2;
  threebody_params params3 = {&func2body, Mc,Md, fWorkspace2body};
  func3body.params = &params3;
  double error, result ;
  int status =gsl_integration_qag (&func3body, Mc+Md, Ma-Mb, fEpsAbs,fEpsRel, fLimit,fKey ,fWorkspace3body, &result, &error);
  // Check status of integration and reduce accuracy if failing
  if (status) {
  status =gsl_integration_qag (&func3body, Mc+Md, Ma-Mb, fEpsAbs,1e-4, fLimit,GSL_INTEG_GAUSS15 ,fWorkspace3body, &result, &error);
    if (status) {
  status =gsl_integration_qag (&func3body, Mc+Md, Ma-Mb, 1e-4,1e-4, fLimit,GSL_INTEG_GAUSS15 ,fWorkspace3body, &result, &error);
      if (status) {      std::cerr << "\033[1mTFastReso_formulas.h\033[0m : \033[1;31merror\033[0m : integration failure! status = " << gsl_strerror (status )<< ". " << result << "+_" << error << std::endl;
        exit(EXIT_FAILURE);
      }else {
        std::cerr << "\033[1mTFastReso_formulas.h\033[0m : \033[1;31mwarning\033[0m : reduced relative and absolute error (1e-4) integration. Result = " << result << "+-" << error <<std::endl;
      }
    } else {
      std::cerr << "\033[1mTFastReso_formulas.h\033[0m : \033[1;31mwarning\033[0m : reduced relative error (1e-4) integration. Result = " << result << "+-" << error <<std::endl;
    }
  }

  double B3body = result;
      if (Mb==0) {
  func3body.function = get_F_pu_dm_u;
      } else {
  func3body.function = get_F_pu_dm;
      }

  int sym = 1 ;
  if (Child1==Child2 and Child2==Child3 ){ sym=3; } else if (Child1==Child2 or Child1==Child3 ){ sym=2;} else { sym=1; }
  double Nfather = Parent->getN();
  // add fraction of yield to child
  Child1->addN(BranchingRatio*Nfather*sym);
  for (int i=0; i < Child1->getNpbar(); i++){
    double Ebar =Child1->getEbar(i);
    params2.Ebar =Ebar;
    //for (int j=0; j < grid_params::fNf ; j++){
    for(auto j: Parent->fComponents) {
      params2.index =j;
      int status = gsl_integration_qag (&func3body, Mc+Md, Ma-Mb, fEpsAbs,fEpsRel, fLimit,fKey ,fWorkspace3body, &result, &error);
  // Check status of integration and reduce accuracy if failing
  if (status) {
      status = gsl_integration_qag (&func3body, Mc+Md, Ma-Mb, fEpsAbs,1e-4, fLimit,GSL_INTEG_GAUSS15 ,fWorkspace3body, &result, &error);
    if (status) {
      status = gsl_integration_qag (&func3body, Mc+Md, Ma-Mb, 1e-4,1e-4, fLimit,GSL_INTEG_GAUSS15 ,fWorkspace3body, &result, &error);
      if (status) {      std::cerr << "\033[1mTFastReso_formulas.h\033[0m : \033[1;31merror\033[0m : integration failure! status = " << gsl_strerror (status )<< ". " << result << "+_" << error << std::endl;
        exit(EXIT_FAILURE);
      }else {
        std::cerr << "\033[1mTFastReso_formulas.h\033[0m : \033[1;31mwarning\033[0m : reduced relative and absolute error (1e-4) integration. Result = " << result << "+-" << error <<std::endl;
      }
    } else {
      std::cerr << "\033[1mTFastReso_formulas.h\033[0m : \033[1;31mwarning\033[0m : reduced relative error (1e-4) integration. Result = " << result << "+-" << error <<std::endl;
    }
  }

      double fac =0;
      if (Mb==0) {
        fac = sym*BranchingRatio*nua/nub/2/B3body;
      }
      else {
        fac = sym*BranchingRatio*nua/nub*pow(Ma/Mb,2)/2/B3body;
      }
      Child1->addFj((int) j,i, fac*result);

    }
  }

  //if (Child1=="pi0139plu" ) { getParticle(Child1)->print_buffer(Child1+"_"+Parent+"_"+Child2+"_"+Child3);}
  gsl_integration_workspace_free (fWorkspace2body);
  gsl_integration_workspace_free (fWorkspace3body);
}

#endif


/*
 * Adapted from bits and pieces in https://code.google.com/p/eq-polar-alignment/
 * Could not find copyright statement from there, but license seems to be GPLv3...
 *
 * Copyright 2014, Kalle Vahlman, zuh@iki.fi
 * Licensed under the GNU GPL v3
 *
 */

#include <stdio.h>
#include <time.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_multiroots.h>
#include <astrometry/anwcs.h>
#include <astrometry/sip.h>
#include <astrometry/sip_qfits.h>

struct coord
{
    double ra;
    double dec;
    double x;
    double y;
};

struct state
{
    anwcs_t* h_wcs;
    anwcs_t* v_wcs;
    double pixel_scale;
    struct coord ncp;
    struct coord polaris;
    struct coord lambda;
    struct coord axis;

};

double julian_date()
{
    time_t now = time(NULL);
    struct tm *t = localtime(&now);
    int day = t->tm_mday;
    int month = t->tm_mon+1;
    int year = 1900 + t->tm_year;

    return 367*year-7*(year+(month+9)/12)/4-3*((year+(month-9)/7)/100+1)/4+275*month/9+day+1721029;
}

int calculate_ncp (double julian, struct coord *c)
{
    double t, zeta, theta;
    if (c == NULL)
        return 0;

    t = (julian-2451545)/36525;
    zeta = (2306.2181*t+0.30188*t*t+0.017998*t*t*t)/3600;
    theta = (2004.3109*t-0.42665*t*t-0.041833*t*t*t)/3600;
    c->dec = 90 - theta;
    c->ra = -zeta;

    return 1;
}

int solve_field(struct state *ps, const char *dir, const char *in, const char *out)
{
    char run[512];
    // TODO: Run the solving with code rather than system()
#define SOLVE "solve-field -B none -M none -S none -R none -U none -N none " \
              "--no-plots --temp-dir %s --dir %s --out %s " \
              "--downsample 2 --ra %.4f --dec %.4f --radius 10 %s"
    sprintf(run, SOLVE, dir, dir, out, ps->ncp.ra, ps->ncp.dec, in);
    printf("%s\n", run);
    return system(run);
}

int fvec(const gsl_vector *x, void *params, gsl_vector *f)
{
    struct state *s = (struct state *)params;
    double ra, dec, xp, yp;
    double xi = gsl_vector_get(x, 0);
    double yi = gsl_vector_get(x, 1);

    anwcs_pixelxy2radec(s->h_wcs, xi, yi, &ra, &dec);
    anwcs_radec2pixelxy(s->v_wcs, ra, dec, &xp, &yp);
    xp = xp - xi;
    yp = yp - yi;
    gsl_vector_set(f, 0, xp);
    gsl_vector_set(f, 1, yp);

    return GSL_SUCCESS;
}

int solve_axis(struct state *ps)
{
    int s;
    size_t iter=0;
    const size_t n=2;
    gsl_multiroot_fsolver *solver;
    gsl_multiroot_function f = { &fvec, n, ps};
    gsl_vector *x = gsl_vector_alloc(n);

    gsl_vector_set(x, 0, ps->polaris.x);
    gsl_vector_set(x, 1, ps->polaris.y);
    solver = gsl_multiroot_fsolver_alloc(gsl_multiroot_fsolver_hybrids, 2);
    gsl_multiroot_fsolver_set(solver, &f, x);
    do {
        iter++;
        s = gsl_multiroot_fsolver_iterate(solver);
        if (s) break;
        s = gsl_multiroot_test_residual(solver->f,1e-7);
    } while (s == GSL_CONTINUE && iter < 1000);
    ps->axis.x = gsl_vector_get(solver->x, 0);
    ps->axis.y = gsl_vector_get(solver->x, 1);

    gsl_multiroot_fsolver_free(solver);
    gsl_vector_free(x);
    return s;
}

#define MAX(a, b) a < b ? b : a

int plot(struct state *ps, char *in, char *out)
{
    char convert[2048];
    double arcmin = 60/ps->pixel_scale;
    double cropwidth = MAX(400, 120 * arcmin);
    double cropheight = MAX(400, 120 * arcmin);
    double cropx = ps->ncp.x - cropwidth/2;
    double cropy = ps->ncp.y - cropheight/2;
    int fontsize = ps->pixel_scale < 24 ? 16 : 8;

#define CONVERT "convert -fill none " \
    "-pointsize %d " \
    "-font Courier -stroke cyan " \
    "-draw 'point %.0f,%.0f' " \
    "-draw 'circle %.0f,%.0f %.0f,%.0f' " \
    "-draw 'circle %.0f,%.0f %.0f,%.0f' " \
    "-draw 'circle %.0f,%.0f %.0f,%.0f' " \
    "-draw 'circle %.0f,%.0f %.0f,%.0f' " \
    "-draw 'circle %.0f,%.0f %.0f,%.0f' " \
    "-draw \"text %.0f,%.0f '%s'\" " \
    "-draw \"text %.0f,%.0f '%s'\" " \
    "-draw \"text %.0f,%.0f '%s'\" " \
    "-draw \"text %.0f,%.0f '%s'\" " \
    "-draw \"text %.0f,%.0f '%s'\" " \
    "-stroke red " \
    "-draw 'line %.0f,%.0f %.0f,%.0f' " \
    "-draw 'line %.0f,%.0f %.0f,%.0f' " \
    "-draw 'circle %.0f,%.0f %.0f,%.0f' " \
    "-draw 'circle %.0f,%.0f %.0f,%.0f' " \
    "-stroke white " \
    "-draw 'line %.0f,%.0f %.0f,%.0f' " \
    "-draw 'line %.0f,%.0f %.0f,%.0f' " \
    "-font Symbol -fill white " \
    "-draw \"text %.0f,%.0f '%s'\" " \
    "-fill none " \
    "-stroke orange " \
    "-draw 'line %.0f,%.0f %.0f,%.0f' " \
    "-draw 'line %.0f,%.0f %.0f,%.0f' " \
    "-font Symbol -fill orange " \
    "-draw \"text %.0f,%.0f '%s'\" " \
    "-fill none " \
    "-font Courier -stroke white -fill white " \
    "-draw \"text %.0f,%.0f 'Current pixel offset from NCP: %.0f, %.0f'\" " \
    "-draw \"text %.0f,%.0f '%s %s %s %s'\" " \
    "-crop '%.0fx%.0f+%.0f+%.0f' " \
    "%s %s"

    sprintf(convert, CONVERT, fontsize,
        // Target circles
        ps->ncp.x, ps->ncp.y,
        ps->ncp.x, ps->ncp.y, ps->ncp.x+2*arcmin, ps->ncp.y,
        ps->ncp.x, ps->ncp.y, ps->ncp.x+5*arcmin, ps->ncp.y,
        ps->ncp.x, ps->ncp.y, ps->ncp.x+10*arcmin, ps->ncp.y,
        ps->ncp.x, ps->ncp.y, ps->ncp.x+20*arcmin, ps->ncp.y,
        ps->ncp.x, ps->ncp.y, ps->ncp.x+40*arcmin, ps->ncp.y,
        ps->ncp.x-2*arcmin+2, ps->ncp.y, ps->pixel_scale < 24 ? "2\\'" : "",
        ps->ncp.x-5*arcmin+2, ps->ncp.y, ps->pixel_scale < 24 ? "5\\'" : "",
        ps->ncp.x-10*arcmin+2, ps->ncp.y, ps->pixel_scale < 24 ? "10\\'" : "",
        ps->ncp.x-20*arcmin+2, ps->ncp.y, "20\\'",
        ps->ncp.x-40*arcmin+2, ps->ncp.y, "40\\'",
        // Current axis
        ps->axis.x-5*arcmin, ps->axis.y-5*arcmin, ps->axis.x+5*arcmin, ps->axis.y+5*arcmin,
        ps->axis.x-5*arcmin, ps->axis.y+5*arcmin, ps->axis.x+5*arcmin, ps->axis.y-5*arcmin,
        ps->axis.x, ps->axis.y, ps->axis.x+2*arcmin, ps->axis.y,
        ps->axis.x, ps->axis.y, ps->axis.x+5*arcmin, ps->axis.y,
        // Polaris
        ps->polaris.x, ps->polaris.y-1*arcmin,
            ps->polaris.x, ps->polaris.y+1*arcmin,
        ps->polaris.x-1*arcmin, ps->polaris.y,
            ps->polaris.x+1*arcmin, ps->polaris.y,
        ps->polaris.x+1*arcmin, ps->polaris.y-1*arcmin, "a",
        // λ UMi
        ps->lambda.x, ps->lambda.y-1*arcmin,
            ps->lambda.x, ps->lambda.y+1*arcmin,
        ps->lambda.x-1*arcmin, ps->lambda.y,
            ps->lambda.x+1*arcmin, ps->lambda.y,
        ps->lambda.x+1*arcmin, ps->lambda.y-1*arcmin, "l",
        // Offset report
        cropx + 16, cropy + 32, ps->ncp.x-ps->axis.x, ps->ncp.y-ps->axis.y,
        cropx + 16, cropy + 64,
            (ps->ncp.x == ps->axis.x && ps->ncp.y == ps->axis.y)
                ? "Well done, perfect alignment!"
                : "Adjust mount alignment to",
            (ps->ncp.x == ps->axis.x && ps->ncp.y == ps->axis.y)
                ? ""
                : ps->ncp.x < ps->axis.x ? "left" : "right",
            (ps->ncp.x == ps->axis.x && ps->ncp.y == ps->axis.y) ? "" : "and",
            (ps->ncp.x == ps->axis.x && ps->ncp.y == ps->axis.y)
                ? ""
                : ps->ncp.y < ps->axis.y ? "up" : "down",
        // Crop
        cropwidth, cropheight, cropx, cropy,
        in, out);

    printf("%s\n", convert);
    return system(convert);
}

int main (int argc, char **argv)
{
    char dir[20] = "/tmp/polar-XXXXXX";
    char h_dst[30];
    char v_dst[30];
    sip_t *sip;
    struct state ps = { NULL, NULL, 0,
                      { 0, 0, 0, 0 }, // NCP
                      { 37.9529, 89.2642, 0, 0 }, // Polaris
                      { 259.2367, 89.0378, 0, 0 }, // λ UMi
                      { 0, 0, 0, 0 } // axis
                    };

    if (argc < 3) {
        printf("E: Not enough parameters!\n");
        printf("Usage: %s <horizontal image> <vertical image> [output]\n", argv[0]);
        return -1;
    }

    if (mkdtemp(dir) == NULL) {
        printf("E: Could not create working directory!\n");
        return -1;
    }

    // Calculate current ra,dec position of North Celestial Pole
    calculate_ncp(julian_date(), &ps.ncp);

    // Solve horizontal and vertical images of Polaris and Lambda UMi
    sprintf(h_dst, "%s/h.wcs", dir);
    sprintf(v_dst, "%s/v.wcs", dir);
    solve_field(&ps, dir, argv[1], "h.wcs");
    solve_field(&ps, dir, argv[2], "v.wcs");

    // Pick pixel coordinates from horizontal image
    ps.h_wcs = anwcs_open(h_dst, 0);
    anwcs_radec2pixelxy(ps.h_wcs, ps.polaris.ra, ps.polaris.dec, &ps.polaris.x, &ps.polaris.y);
    anwcs_radec2pixelxy(ps.h_wcs, ps.lambda.ra, ps.lambda.dec, &ps.lambda.x, &ps.lambda.y);
    anwcs_radec2pixelxy(ps.h_wcs, ps.ncp.ra, ps.ncp.dec, &ps.ncp.x, &ps.ncp.y);

    // Solve rotational axis in pixel coordinates of the horizontal image
    ps.v_wcs = anwcs_open(v_dst, 0);
    solve_axis(&ps);
    anwcs_pixelxy2radec(ps.h_wcs, ps.axis.x, ps.axis.y, &ps.axis.ra, &ps.axis.dec);
    anwcs_free(ps.v_wcs);

    // Grab pixel scale from horizontal image
    sip = anwcs_get_sip(ps.h_wcs);
    ps.pixel_scale = sip_pixel_scale(sip);
    anwcs_free(ps.h_wcs);

    // Print what we found
    printf("Pixel scale: %.4f\n", ps.pixel_scale);
    printf("Polaris: %+9.4f ra, %+9.4f dec (%4.0f, %4.0f)\n",
        ps.lambda.ra, ps.lambda.dec, ps.lambda.x, ps.lambda.y);
    printf("λ UMi  : %+9.4f ra, %+9.4f dec (%4.0f, %4.0f)\n",
        ps.polaris.ra, ps.polaris.dec, ps.polaris.x, ps.polaris.y);
    printf("NCP    : %+9.4f ra, %+9.4f dec (%4.0f, %4.0f)\n",
        ps.ncp.ra, ps.ncp.dec, ps.ncp.x, ps.ncp.y);
    printf("Axis   : %+9.4f ra, %+9.4f dec (%4.0f, %4.0f)\n",
        ps.axis.ra, ps.axis.dec, ps.axis.x, ps.axis.y);
    printf("Offset : %4.0f x, %4.0f y\n",
        ps.ncp.x-ps.axis.x, ps.ncp.y-ps.axis.y);

    // Plot a chart of the polar alignment, if user asks for it
    if (argc == 4)
        plot(&ps, argv[1], argv[3]);

    return 0;
}


/*
Ballistic: a software to benchmark ballistic models.

AUTHORS: Javier Burguete Tolosa.

Copyright 2018, AUTHORS.

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

  1. Redistributions of source code must retain the above copyright notice,
    this list of conditions and the following disclaimer.

  2. Redistributions in binary form must reproduce the above copyright notice,
    this list of conditions and the following disclaimer in the
    documentation and/or other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY AUTHORS ``AS IS'' AND ANY EXPRESS OR IMPLIED
WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
SHALL AUTHORS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
OF SUCH DAMAGE.
*/

/**
 * \file multi-steps.c
 * \brief Source file to define the multi-steps method data and functions.
 * \author Javier Burguete Tolosa.
 * \copyright Copyright 2018.
 */
#define _GNU_SOURCE
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <gsl/gsl_rng.h>
#include <libxml/parser.h>
#include "config.h"
#include "utils.h"
#include "equation.h"
#include "method.h"
#include "runge-kutta.h"
#include "multi-steps.h"

#define DEBUG_MULTI_STEPS 0     ///< macro to debug the multi-steps functions.

///> array of a coefficients of the 2nd order multi-steps method.
const long double ms_a2[3] = { 0.75L, 0.L, 0.25L };

///> array of c coefficients of the 2nd order multi-steps method.
const long double ms_c2[3] = { 2.L, 0.L, 0.L };

///> array of error a coefficients of the 2nd order multi-steps method.
const long double ms_ea2[3] = { 0.25L, 0.L, -0.25L };

///> array of error b coefficients of the 2nd order multi-steps method.
const long double ms_eb2[3] = { 0.5L, 0.L, 0.L };

///> array of a coefficients of the 3rd order multi-steps method.
const long double ms_a3[4] = { 16.L / 27.L, 0.L, 0.L, 11.L / 27.L };

///> array of c coefficients of the 3rd order multi-steps method.
const long double ms_c3[4] = { 3.L, 0.L, 0.L, 12.L / 11.L };

///> array of error a coefficients of the 3rd order multi-steps method.
const long double ms_ea3[4] = { 17.L / 108.L, 0.L, 0.25L, -11.L / 27.L };

///> array of error b coefficients of the 3rd order multi-steps method.
const long double ms_eb3[4] = { -5.L / 18.L, 0.L, 0.L, -4.L / 9.L };

/**
 * Function to init the multi-steps method coefficients.
 *
 * \return 1 on success, 0 on error.
 */
int
multi_steps_init (MultiSteps * ms)      ///< MultiSteps struct.
{
  Method *m;
#if DEBUG_MULTI_STEPS
  fprintf (stderr, "multi_steps_init: start\n");
#endif
  m = MULTI_STEPS_METHOD (ms);
  switch (ms->type)
    {
    case 1:
      method_init (m, 3, 2);
      ms->a = ms_a2;
      ms->c = ms_c2;
      ms->ea = ms_ea2;
      ms->eb = ms_eb2;
      break;
    case 2:
      method_init (m, 4, 3);
      ms->a = ms_a3;
      ms->c = ms_c3;
      ms->ea = ms_ea3;
      ms->eb = ms_eb3;
      break;
    default:
      printf ("Error reading multi-steps data\n");
#if DEBUG_MULTI_STEPS
      fprintf (stderr, "multi_steps_init: error\n");
      fprintf (stderr, "multi_steps_init: end\n");
#endif
      return 0;
    }
#if DEBUG_MULTI_STEPS
  fprintf (stderr, "multi_steps_init: success\n");
  fprintf (stderr, "multi_steps_init: end\n");
#endif
  return 1;
}

/**
 * Function to init the variables used by the multi-steps methods.
 */
void
multi_steps_init_variables (MultiSteps * ms)
{
#if DEBUG_MULTI_STEPS
  fprintf (stderr, "multi_steps_init_variables: start\n");
#endif
  runge_kutta_init_variables (MULTI_STEPS_RUNGE_KUTTA (ms));
  method_init_variables (MULTI_STEPS_METHOD (ms));
#if DEBUG_MULTI_STEPS
  fprintf (stderr, "multi_steps_init_variables: end\n");
#endif
}

/**
 * Function to perform a step of the multi-steps method.
 */
static inline void
multi_steps_step (MultiSteps * ms,      ///< MultiSteps struct.
                  Equation * eq,        ///< Equation struct.
                  long double t,        ///< actual time.
                  long double dt)       ///< time step size.
{
  long double msr0[3], msr1[3];
  Method *m;
  const long double *a;
  const long double *c;
  unsigned int i, n;
#if DEBUG_MULTI_STEPS
  fprintf (stderr, "multi_steps_step: start\n");
#endif
  m = MULTI_STEPS_METHOD (ms);
#if DEBUG_MULTI_STEPS
  for (i = 0; i < 3; ++i)
    fprintf (stderr, "multi_steps_step: r0[0][%u]=%Lg\n", i, m->r0[0][i]);
  for (i = 0; i < 3; ++i)
    fprintf (stderr, "multi_steps_step: r1[0][%u]=%Lg\n", i, m->r1[0][i]);
  for (i = 0; i < 3; ++i)
    fprintf (stderr, "multi_steps_step: r2[0][%u]=%Lg\n", i, m->r2[0][i]);
#endif
  n = m->nsteps;
  a = ms->a;
  c = ms->c;
#if DEBUG_MULTI_STEPS
  for (i = 0; i < n; ++i)
    fprintf (stderr, "multi_steps_step: a%u=%Lg\n", i, a[i]);
  for (i = 0; i < n; ++i)
    fprintf (stderr, "multi_steps_step: c%u=%Lg\n", i, c[i]);
#endif
  msr0[0] = a[0] * (r0[0] + dt * c[0] * r1[0]);
  msr0[1] = a[0] * (r0[1] + dt * c[0] * r1[1]);
  msr0[2] = a[0] * (r0[2] + dt * c[0] * r1[2]);
  msr1[0] = a[0] * (r1[0] + dt * c[0] * r2[0]);
  msr1[1] = a[0] * (r1[1] + dt * c[0] * r2[1]);
  msr1[2] = a[0] * (r1[2] + dt * c[0] * r2[2]);
  for (i = 1; i < n; ++i)
    {
#if DEBUG_MULTI_STEPS
      fprintf (stderr, "multi_steps_step: r0[%u][0]=%Lg\n", i, m->r0[i][0]);
      fprintf (stderr, "multi_steps_step: r0[%u][1]=%Lg\n", i, m->r0[i][1]);
      fprintf (stderr, "multi_steps_step: r0[%u][2]=%Lg\n", i, m->r0[i][2]);
      fprintf (stderr, "multi_steps_step: r1[%u][0]=%Lg\n", i, m->r1[i][0]);
      fprintf (stderr, "multi_steps_step: r1[%u][1]=%Lg\n", i, m->r1[i][1]);
      fprintf (stderr, "multi_steps_step: r1[%u][2]=%Lg\n", i, m->r1[i][2]);
      fprintf (stderr, "multi_steps_step: r2[%u][0]=%Lg\n", i, m->r2[i][0]);
      fprintf (stderr, "multi_steps_step: r2[%u][1]=%Lg\n", i, m->r2[i][1]);
      fprintf (stderr, "multi_steps_step: r2[%u][2]=%Lg\n", i, m->r2[i][2]);
#endif
      msr0[0] += a[i] * (m->r0[i][0] + dt * c[i] * m->r1[i][0]);
      msr0[1] += a[i] * (m->r0[i][1] + dt * c[i] * m->r1[i][1]);
      msr0[2] += a[i] * (m->r0[i][2] + dt * c[i] * m->r1[i][2]);
      msr1[0] += a[i] * (m->r1[i][0] + dt * c[i] * m->r2[i][0]);
      msr1[1] += a[i] * (m->r1[i][1] + dt * c[i] * m->r2[i][1]);
      msr1[2] += a[i] * (m->r1[i][2] + dt * c[i] * m->r2[i][2]);
    }
#if DEBUG_MULTI_STEPS
  for (i = 0; i < 3; ++i)
    fprintf (stderr, "multi_steps_step: r0[0][%u]=%Lg\n", i, msr0[i]);
  for (i = 0; i < 3; ++i)
    fprintf (stderr, "multi_steps_step: r1[0][%u]=%Lg\n", i, msr1[i]);
#endif
  memcpy (m->r0[0], r0, 3 * sizeof (long double));
  memcpy (m->r1[0], r1, 3 * sizeof (long double));
  memcpy (m->r2[0], r2, 3 * sizeof (long double));
  for (i = n; --i > 0;)
    {
      memcpy (m->r0[i], m->r0[i - 1], 3 * sizeof (long double));
      memcpy (m->r1[i], m->r1[i - 1], 3 * sizeof (long double));
      memcpy (m->r2[i], m->r2[i - 1], 3 * sizeof (long double));
    }
  memcpy (r0, msr0, 3 * sizeof (long double));
  memcpy (r1, msr1, 3 * sizeof (long double));
  equation_acceleration (eq, r0, r1, r2, t + dt);
#if DEBUG_MULTI_STEPS
  for (i = 0; i < 3; ++i)
    fprintf (stderr, "multi_steps_step: r0[0][%u]=%Lg\n", i, r0[i]);
  for (i = 0; i < 3; ++i)
    fprintf (stderr, "multi_steps_step: r1[0][%u]=%Lg\n", i, r1[i]);
  fprintf (stderr, "multi_steps_step: end\n");
#endif
}

/**
 * Function to estimate the error on a multi-steps method step.
 */
static inline void
multi_steps_error (MultiSteps * ms,     ///< MultiSteps struct.
                   long double dt)      ///< time step size.
{
  long double e0[3], e1[3];
  Method *m;
  unsigned int i;
#if DEBUG_MULTI_STEPS
  fprintf (stderr, "multi_steps_error: start\n");
#endif
  m = MULTI_STEPS_METHOD (ms);
  e0[0] = e0[1] = e0[2] = e1[0] = e1[1] = e1[2] = 0.L;
  for (i = 0; i < m->nsteps; ++i)
    {
      e0[0] += ms->ea[i] * m->r0[i][0] + dt * ms->eb[i] * m->r1[i][0];
      e0[1] += ms->ea[i] * m->r0[i][1] + dt * ms->eb[i] * m->r1[i][1];
      e0[2] += ms->ea[i] * m->r0[i][2] + dt * ms->eb[i] * m->r1[i][2];
      e1[0] += ms->ea[i] * m->r1[i][0] + dt * ms->eb[i] * m->r2[i][0];
      e1[1] += ms->ea[i] * m->r1[i][1] + dt * ms->eb[i] * m->r2[i][1];
      e1[2] += ms->ea[i] * m->r1[i][2] + dt * ms->eb[i] * m->r2[i][2];
    }
  m->e0 = sqrtl (e0[0] * e0[0] + e0[1] * e0[1] + e0[2] * e0[2]);
  m->e1 = sqrtl (e1[0] * e1[0] + e1[1] * e1[1] + e1[2] * e1[2]);
  m->et0 += m->e0;
  m->et1 += m->e1;
#if DEBUG_MULTI_STEPS
  fprintf (stderr, "multi_steps_error: e0=%Lg et0=%Lg\n", m->e0, m->et0);
  fprintf (stderr, "multi_steps_error: e1=%Lg et1=%Lg\n", m->e1, m->et1);
  fprintf (stderr, "multi_steps_error: end\n");
#endif
}

/**
 * Function to run the multi-steps method bucle.
 *
 * \return final time.
 */
long double
multi_steps_run (MultiSteps * ms,       ///< MultiSteps struct.
                 Equation * eq) ///< Equation struct.
{
  RungeKutta *rk;
  Method *m, *mrk;
  long double t, dt, to, dto, et0o, et1o;
  unsigned int i, n;

#if DEBUG_MULTI_STEPS
  fprintf (stderr, "multi_steps_run: start\n");
#endif

  // variables backup
  rk = MULTI_STEPS_RUNGE_KUTTA (ms);
  mrk = RUNGE_KUTTA_METHOD (rk);
  m = MULTI_STEPS_METHOD (ms);
  memcpy (ro0, r0, 3 * sizeof (long double));
  memcpy (ro1, r1, 3 * sizeof (long double));
  memcpy (ro2, r2, 3 * sizeof (long double));

  // Runge-Kutta first steps
  n = m->nsteps;
  for (t = 0.L, i = n; --i > 0;)
    {

      // time step size
      if (t > 0.L && mrk->error_dt)
        {
          dto = dt;
          dt = method_dt (mrk, dt);

          // revert the step if big error
          if (dt < mrk->beta * dto)
            {
              ++i;
              t = to;
              mrk->et0 = et0o;
              mrk->et1 = et1o;
              memcpy (r0, ro0, 3 * sizeof (long double));
              memcpy (r1, ro1, 3 * sizeof (long double));
              memcpy (r2, ro2, 3 * sizeof (long double));
            }
        }
      else
        dt = equation_step_size (eq);

      // checking trajectory end
      to = t;
      if (equation_land (eq, to, &t, &dt))
        goto end;
#if DEBUG_MULTI_STEPS
      fprintf (stderr, "multi_steps_run: t=%Lg dt=%Lg\n", t, dt);
#endif

      // saving step
      memcpy (m->r0[i], r0, 3 * sizeof (long double));
      memcpy (m->r1[i], r1, 3 * sizeof (long double));
      memcpy (m->r2[i], r2, 3 * sizeof (long double));
      memcpy (ro0, r0, 3 * sizeof (long double));
      memcpy (ro1, r1, 3 * sizeof (long double));
      memcpy (ro2, r2, 3 * sizeof (long double));

      // Runge-Kutta step
      runge_kutta_step (rk, eq, to, dt);

      // error estimate
      if (mrk->error_dt)
        {
          et0o = mrk->et0;
          et1o = mrk->et1;
          runge_kutta_error (rk, dt);
        }
    }

  // saving last step
  memcpy (m->r0[0], r0, 3 * sizeof (long double));
  memcpy (m->r1[0], r1, 3 * sizeof (long double));
  memcpy (m->r2[0], r2, 3 * sizeof (long double));

  // initing errors
  m->et0 = mrk->et0;
  m->et1 = mrk->et1;

  // temporal bucle
  while (1)
    {

      // time step size
      if (m->error_dt)
        {
          dto = dt;
          dt = method_dt (m, dt);

          // revert the step if big error
          if (dt < mrk->beta * dto)
            {
              ++i;
              t = to;
              m->et0 = et0o;
              m->et1 = et1o;
              memcpy (r0, ro0, 3 * sizeof (long double));
              memcpy (r1, ro1, 3 * sizeof (long double));
              memcpy (r2, ro2, 3 * sizeof (long double));
            }
        }
      else
        dt = equation_step_size (eq);

      // checking trajectory end
      to = t;
      dto = dt;
      if (equation_land (eq, to, &t, &dt))
        break;
#if DEBUG_MULTI_STEPS
      fprintf (stderr, "multi_steps_run: t=%Lg dt=%Lg\n", t, dt);
#endif

      // variables backup
      memcpy (ro0, r0, 3 * sizeof (long double));
      memcpy (ro1, r1, 3 * sizeof (long double));
      memcpy (ro2, r2, 3 * sizeof (long double));

      // multi-steps step
      if (dto == dt)
        multi_steps_step (ms, eq, to, dt);
      else
        runge_kutta_step (rk, eq, to, dt);

      // error estimate
      if (m->error_dt)
        {
          et0o = m->et0;
          et1o = m->et1;
          multi_steps_error (ms, dt);
        }
    }
end:
#if DEBUG_MULTI_STEPS
  fprintf (stderr, "multi_steps_run: end\n");
#endif
  return t;
}

/**
 * Function to free the memory used by a MultiSteps struct.
 */
void
multi_steps_delete (MultiSteps * ms)    ///< RungeKutta struct.
{
#if DEBUG_MULTI_STEPS
  fprintf (stderr, "multi_steps_delete: start\n");
#endif
  runge_kutta_delete (MULTI_STEPS_RUNGE_KUTTA (ms));
  method_delete (MULTI_STEPS_METHOD (ms));
#if DEBUG_MULTI_STEPS
  fprintf (stderr, "multi_steps_delete: end\n");
#endif
}

/**
 * Function to read on a XML node the multi-steps method input data.
 *
 * \return 1 on success, 0 on error.
 */
int
multi_steps_read_xml (MultiSteps * ms,  ///< MultiSteps struct.
                      xmlNode * node)   ///< XML node.
{
	char *message[] = {
		NULL,
		"Bad steps number",
		"Bad numerical method data",
		"No Runge-Kutta XML node",
		"Bad Runge-Kutta XML node",
		"Bad Runge-Kutta method data",
		"Bad multi-steps data"
	};
  int e, error_code;
#if DEBUG_MULTI_STEPS
  fprintf (stderr, "multi_steps_read_xml: start\n");
#endif
  ms->type = xml_node_get_uint (node, XML_TYPE, &error_code);
  if (error_code || !ms->type)
    e = 1;
	else if (!method_read_xml (MULTI_STEPS_METHOD (ms), node))
    e = 2;
  else
	  {
			node = node->children;
			if (!node)
				e = 3;
			else if (xmlStrcmp (node->name, XML_RUNGE_KUTTA))
				e = 4;
			else if (!runge_kutta_read_xml (MULTI_STEPS_RUNGE_KUTTA (ms), node))
				e = 5;
			else if (!multi_steps_init (ms))
				e = 6;
			else
				e = 0;
		}
	if (e)
	{
		error_add (message[e]);
		e = 0;
	}
	else
		e = 1;
#if DEBUG_MULTI_STEPS
  if (e)
    fprintf (stderr, "multi_steps_read: success\n");
  else
    fprintf (stderr, "multi_steps_read: error\n");
  fprintf (stderr, "multi_steps_read: end\n");
#endif
  return e;
}


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

#include <gsl/gsl_math.h>

#include "imshift.h"
#include "conv1.h"

void showMatrix(double * I, size_t M, size_t N)
{
  for(size_t kk = 0; kk<N; kk++) {
    for(size_t ll = 0; ll<M; ll++)
    {
      printf("%5.2f ", I[ll*M+kk]);
    }
    printf("\n");
  }
}

int imshift3(double * V, size_t M, size_t N, size_t P,
    double dx, double dy, double dz, int method)
{

  int status = 0;

  double * Kx = NULL; size_t nKx;
  double * Ky = NULL; size_t nKy;
  double * Kz = NULL; size_t nKz;

  status = generateShift(&Kx, dx, &nKx, method);
  if(status != 0)
    return -1;

  status = generateShift(&Ky, dy, &nKy, method);
  if(status != 0) {
    free(Kx);
    return -2;
  }

  status = generateShift(&Kz, dz, &nKz, method);
  if(status != 0) {
    free(Kx);
    free(Ky);
    return -3;
  }

  status = conv1_3(V, M, N, P,
      Kx, nKx,
      Ky, nKy,
      Kz, nKz);

      free(Kz);
      free(Ky);
      free(Kx);

      return status;
}

int generateShift(double ** K, double delta, size_t * size, int order)
  /** Allocate space for a 1D shifting kernel, K,
   * shifted by delta in [-1,1]
   * returns the size of the kernel in size
   * order sets the polynomial order, 1 = linear, 2 = quadratic
   *
   * Errors:
   *  -1, K != NULL
   *  -2, no valid interpolation method selected
   *  -3, unable to allocate memory for the kernel
   *
   * */
{

  if(K[0]!=NULL)
    return -1;

  if(order == 1)
  {
    size_t N = 3;
    size[0] = N;
    K[0] = malloc(N*sizeof(double));
    if(K[0] == NULL)
      return(-3);

    for(size_t kk = 0; kk<N; kk++)
      K[0][kk] = GSL_MAX(0, 1-fabs(kk-(1-delta)));
    return 0;
  }

  if(order == 3)
  {
    size_t N = 5;
    size[0] = N;
    K[0] = malloc(N*sizeof(double));
    if(K[0] == NULL)
      return -3;

    // set up and solve polynomial equation here
    double a = -.5;

    for(size_t kk = 0; kk < N; kk++)
    {
      double x = kk-(2-delta);
      double ax = fabs(x);
      if(ax<=1)
        K[0][kk] = (a+2)*ax*ax*ax - (a+3)*ax*ax + 1;
      if(ax>1)
        K[0][kk] = a*ax*ax*ax -5*a*ax*ax  +8*a*ax -4*a;
      if(ax>2)
        K[0][kk] = 0;
    }
    return 0;
  }

  return -2;
}

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

  printf("Running %s\n", argv[0]);

  if(argc>1)
    printf("Warning: unused command line arguments\n");

  {
    printf("--> Testing kernels\n");
    double * K;
    double sum;

    for(double delta = -.5; delta < .6; delta+=.1) {
    for(int method = 1; method < 4; method = method +2)
    {
      K = NULL;
      size_t nK = 0;
      assert(generateShift(&K, delta, &nK, 3)==0);

      sum = 0;
      for(int kk = 0; kk<(int) nK; kk++)
        sum += K[kk];

      printf("delta: %f order: %d sum: %f\n", delta, method, sum);

     free(K);
    }
    }
  }

  {
    printf("--> Testing 1D\n");
    double delta = .3;
    size_t N = 10;
    double * V = malloc(N*sizeof(double));

    for(size_t kk=0; kk<N; kk++)
      V[kk] = kk%2;

    double * K = NULL;
    size_t nK = 0;
    assert(generateShift(&K, delta, &nK, 3)==0);

    printf("Kernel:\n");
    for(size_t kk = 0; kk<nK; kk++)
      printf("%.1f ", K[kk]);
    printf("\n");
    printf("Input:\n");
    for(size_t kk = 0; kk<N; kk++)
      printf("%.1f ", V[kk]);
    printf("\n");

    double * buff = calloc(N,sizeof(double));

    conv1(V, buff, N, K, nK, 1);
    printf("Output\n");
    for(size_t kk = 0; kk<N; kk++)
      printf("%.1f ", V[kk]);
    printf("\n");

    free(buff);
    free(K);
    free(V);
  }

  {
    printf("Testing 2D\n");
    size_t M = 11;
    size_t N = 11;

    double * Kx = NULL;
    double * Ky = NULL;
    size_t nKx;
    size_t nKy;

    double deltax = .1;
    double deltay = .6;

    int method = 1;
    assert(generateShift(&Kx, deltax, &nKx, method)==0);
    assert(generateShift(&Ky, deltay, &nKy, method)==0);

    printf("Kernels:\n");
double sum = 0;
    printf(" deltax: %f\n", deltax);
    for(size_t kk = 0; kk<nKx; kk++) {
      printf("%5.1f ", Kx[kk]);
      sum += Kx[kk];
    }
    printf("s: %.2f\n", sum);
    sum = 0;
    printf(" deltay: %f\n", deltay);
    for(size_t kk = 0; kk<nKy; kk++) {
      printf("%5.1f ", Ky[kk]);
      sum += Ky[kk];
    }
    printf("s: %.2f\n", sum);

    double * V = calloc(M*N, sizeof(double));
    V[(M*N-1)/2] = 1;

    printf("V:\n");
    showMatrix(V, M, N);
    double * C1 = malloc(M*N*sizeof(double));
    double * C2 = malloc(M*N*sizeof(double));
    memcpy(C1,V, M*N*sizeof(double));
    memcpy(C2,V, M*N*sizeof(double));

    conv1_3(C1,  M, N, 1, Kx, nKx, Ky, nKy, NULL, 0);
    conv1_3(C2,  M, N, 1, Ky, nKy, Kx, nKx, NULL, 0);

    printf("C1\n");
    showMatrix(C1, M, N);

    printf("C2\n");
    showMatrix(C2, M, N);

    printf("Testing symmetry, C1=C2' ... ");
    for(size_t kk = 0; kk<M; kk++)
      for(size_t ll = 0; ll<N; ll++)
        assert(C1[kk+M*ll] == C2[ll+M*kk]);
    printf("ok!\n");

    free(V);
    free(Ky);
    free(Kx);
    free(C1);
    free(C2);

  }

  {
    printf("Testing 3D\n");
    double delta = .3;
    size_t M = 11;
    size_t N = 11;
    size_t P = 11;
    double * V3 = calloc(M*N*P,sizeof(double));
    V3[(M*N*P-1)/2] = 1;
    double * K = NULL;
    size_t nK = 0;
    assert(generateShift(&K, delta, &nK, 1)==0);

    clock_t t1 = clock();
    conv1_3(V3, M, N,P, K,nK,K,nK,K,nK);
    clock_t t2 = clock();

    printf("V3(:,:,6):\n");
    showMatrix(V3 + M*N*6, M, N);

    printf("Time: %f ms\n", (double) 1000*(t2-t1)/CLOCKS_PER_SEC);

    free(V3);
    free(K);
  }

  return 0;
}


/*
    This code derives from the R wavelets package and it is modified by Davide
    Albanese <albanese@fbk.it>.

    The Python interface is written by  Davide Albanese <albanese@fbk.it>.
    (C) 2009 mlpy Developers.

    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, see <http://www.gnu.org/licenses/>.
*/

#include <Python.h>
#include <numpy/arrayobject.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_wavelet.h>
#include <gsl/gsl_math.h>

#define SQRT_2 1.4142135623730951

void
uwt_forward (double *V, int n, int j,
	      double *h, double *g, int l,
	      double *Wj, double *Vj)
{
  int t, k, z;
  double k_div;

  for(t = 0; t < n; t++)
    {
      k = t;
      Wj[t] = h[0] * V[k];
      Vj[t] = g[0] * V[k];

      for(z = 1; z < l; z++)
	{
	  k -= (int) pow(2, (j - 1));

	  k_div = -k / (double) n;

	  if(k < 0)
	    k += (int) ceil(k_div) * n;

	  Wj[t] += h[z] * V[k];
	  Vj[t] += g[z] * V[k];
	}
    }
}

void
uwt_backward (double *W, double *V, int j,
	       int n, double *h, double *g,
	       int l, double *Vj)
{
  int t, k, z;
  double k_div;

  for(t = 0; t < n; t++)
    {
      k = t;
      Vj[t] = h[0] * W[k] + g[0] * V[k];

      for(z = 1; z < l; z++)
	{
	  k += (int) pow(2, (j - 1));
	  k_div = (double) k / (double) n;

	  if(k >= n)
	    k -= (int) floor(k_div) * n;

	  Vj[t] += h[z] * W[k] + g[z] * V[k];
	}
    }
}

static PyObject *uwt_uwt(PyObject *self, PyObject *args, PyObject *keywds)
{
  PyObject *x = NULL; PyObject *xa = NULL;
  PyObject *Xa = NULL;
  int levels = 0;
  char wf;

  int i, k, j, n, J;

  double *_x;
  double *_X;

  double *v;
  double *wj, *vj;
  double *h, *g;

  npy_intp Xa_dims[2];

  gsl_wavelet *wave;

  /* Parse Tuple*/
  static char *kwlist[] = {"x", "wf", "k", "levels", NULL};
  if (!PyArg_ParseTupleAndKeywords(args, keywds, "Oci|i", kwlist, &x, &wf, &k, &levels))
    return NULL;

  xa = PyArray_FROM_OTF(x, NPY_DOUBLE, NPY_IN_ARRAY);
  if (xa == NULL) return NULL;

  n = (int) PyArray_DIM(xa, 0);
  _x = (double *) PyArray_DATA(xa);

  switch (wf)
    {
    case 'd':
      wave = gsl_wavelet_alloc (gsl_wavelet_daubechies_centered, k);
      break;

    case 'h':
      wave = gsl_wavelet_alloc (gsl_wavelet_haar_centered, k);
      break;

    case 'b':
      wave = gsl_wavelet_alloc (gsl_wavelet_bspline_centered, k);
      break;

    default:
      PyErr_SetString(PyExc_ValueError, "wavelet family is not valid");
      return NULL;
    }

  h = (double *) malloc(wave->nc * sizeof(double));
  g = (double *) malloc(wave->nc * sizeof(double));

  for(i=0; i<wave->nc; i++)
    {
      h[i] = wave->h1[i] / SQRT_2;
      g[i] = wave->g1[i] / SQRT_2;
    }

  if (levels == 0)
    J = (int) floor(log(((n-1) / (wave->nc-1)) + 1) / log(2));
  else
    J = levels;

  Xa_dims[0] = (npy_intp) (2 * J);
  Xa_dims[1] = PyArray_DIM(xa, 0);
  Xa = PyArray_SimpleNew(2, Xa_dims, NPY_DOUBLE);
  _X = (double *) PyArray_DATA(Xa);

  v = _x;
  for(j=0; j<J; j++)
    {
      wj = _X +(j * n);
      vj = _X +((j + J) * n);
      uwt_forward(v, n, j+1, g, h, wave->nc, wj,  vj);
      v = vj;
    }

  gsl_wavelet_free(wave);
  free(h);
  free(g);
  Py_DECREF(xa);

  return Py_BuildValue("N", Xa);
}

static PyObject *uwt_iuwt(PyObject *self, PyObject *args, PyObject *keywds)
{
  PyObject *X = NULL; PyObject *Xa = NULL;

  PyObject *xa = NULL;

  char wf;
  int i, k, n, J;
  double *w1, *v1;

  double *_X;
  double *_x;
  double *h, *g;

  npy_intp xa_dims[1];

  gsl_wavelet *wave;

  /* Parse Tuple*/
  static char *kwlist[] = {"X", "wf", "k", NULL};
  if (!PyArg_ParseTupleAndKeywords(args, keywds, "Oci", kwlist, &X, &wf, &k))
    return NULL;

  Xa = PyArray_FROM_OTF(X, NPY_DOUBLE, NPY_IN_ARRAY);
  if (Xa == NULL) return NULL;

  n = (int) PyArray_DIM(Xa, 1);
  J = ((int) PyArray_DIM(Xa, 0)) / 2;

  _X = (double *) PyArray_DATA(Xa);

  switch (wf)
    {
    case 'd':
      wave = gsl_wavelet_alloc (gsl_wavelet_daubechies, k);
      break;

    case 'h':
      wave = gsl_wavelet_alloc (gsl_wavelet_haar, k);
      break;

    case 'b':
      wave = gsl_wavelet_alloc (gsl_wavelet_bspline, k);
      break;

    default:
      PyErr_SetString(PyExc_ValueError, "wavelet family is not valid");
      return NULL;
    }

  h = (double *) malloc(wave->nc * sizeof(double));
  g = (double *) malloc(wave->nc * sizeof(double));

  for(i=0; i<wave->nc; i++)
    {
      h[i] = wave->h2[i] / SQRT_2;
      g[i] = wave->g2[i] / SQRT_2;
    }

  w1 = _X;
  v1 = _X + (J * n);

  xa_dims[0] = (npy_intp) n;
  xa = PyArray_SimpleNew(1, xa_dims, NPY_DOUBLE);
  _x = (double *) PyArray_DATA(xa);

  uwt_backward (w1, v1, 1, n, g, h, wave->nc, _x);

  gsl_wavelet_free(wave);
  free(h);
  free(g);
  Py_DECREF(Xa);

  return Py_BuildValue("N", xa);
}

/* Doc strings: */
static char module_doc[]  = "Undecimated Wavelet Transform Module";

static char uwt_uwt_doc[] =
  "Undecimated Wavelet Tranform\n\n"
  ":Parameters:\n"
  "   x : 1d array_like object (the length is restricted to powers of two)\n"
  "      data\n"
  "   wf : string ('d': daubechies, 'h': haar, 'b': bspline)\n"
  "      wavelet family\n"
  "   k : int\n"
  "      member of the wavelet family\n\n"
  "         * daubechies: k = 4, 6, ..., 20 with k even\n"
  "         * haar: the only valid choice of k is k = 2\n"
  "         * bspline: k = 103, 105, 202, 204, 206, 208, 301, 303, 305 307, 309\n\n"
  "   levels : int\n"
  "      level of the decomposition (J).\n"
  "      If levels = 0 this is the value J such that the length of X\n"
  "      is at least as great as the length of the level J wavelet filter,\n"
  "      but less than the length of the level J+1 wavelet filter.\n"
  "      Thus, j <= log_2((n-1)/(l-1)+1), where n is the length of x\n\n"
  ":Returns:\n"
  "   X : 2d numpy array (2J * len(x))\n"
  "      misaligned scaling and wavelet coefficients::\n\n"
  "         [[wavelet coefficients W_1]\n"
  "          [wavelet coefficients W_2]\n"
  "                        :\n"
  "          [wavelet coefficients W_J]\n"
  "          [scaling coefficients V_1]\n"
  "          [scaling coefficients V_2]\n"
  "                       :\n"
  "          [scaling coefficients V_J]]"
  ;

static char uwt_iuwt_doc[] =
  "Inverse Undecimated Wavelet Tranform\n\n"
  ":Parameters:\n"
  "   X : 2d array_like object (the length is restricted to powers of two)\n"
  "      misaligned scaling and wavelet coefficients\n"
  "   wf : string ('d': daubechies, 'h': haar, 'b': bspline)\n"
  "      wavelet family\n"
  "   k : int\n"
  "      member of the wavelet family\n\n"
  "         * daubechies: k = 4, 6, ..., 20 with k even\n"
  "         * haar: the only valid choice of k is k = 2\n"
  "         * bspline: k = 103, 105, 202, 204, 206, 208, 301, 303, 305 307, 309\n\n"
  ":Returns:\n"
  "   x : 1d numpy array\n"
  "      data"
  ;

/* Method table */
static PyMethodDef uwt_methods[] = {
  {"uwt",
   (PyCFunction)uwt_uwt,
   METH_VARARGS | METH_KEYWORDS,
   uwt_uwt_doc},
  {"iuwt",
   (PyCFunction)uwt_iuwt,
   METH_VARARGS | METH_KEYWORDS,
   uwt_iuwt_doc},
  {NULL, NULL, 0, NULL}
};

#if PY_MAJOR_VERSION >= 3

static struct PyModuleDef moduledef = {
  PyModuleDef_HEAD_INIT,
  "_uwt",
  module_doc,
  -1,
  uwt_methods,
  NULL, NULL, NULL, NULL
};

PyObject *PyInit__uwt(void)
{
  PyObject *m;
  m = PyModule_Create(&moduledef);
  if (!m) {
    return NULL;
  }

  import_array();

  return m;
}

#else

PyMODINIT_FUNC init_uwt(void)
{
  PyObject *m;

  m = Py_InitModule3("_uwt", uwt_methods, module_doc);
  if (m == NULL) {
    return;
  }

  import_array();
}

#endif


/*
  NAME:
     calc_splitnmerge
  PURPOSE:
     calculates the split and merge hierarchy after an proj_EM convergence
  CALLING SEQUENCE:
     calc_splitnmerge(struct datapoint * data,int N,
     struct gaussian * gaussians, int K, gsl_matrix * qij,
     int * snmhierarchy){
  INPUT:
     data      - the data
     N         - number of data points
     gaussians - model gaussians
     K         - number of gaussians
     qij       - matrix of log(posterior likelihoods)
     bs        - bs from previous EM estimate
  OUTPUT:
     snmhierarchy - the hierarchy, first row has the highest prioriry,
                    goes down from there
  REVISION HISTORY:
     2008-09-21 - Written Bovy
*/
#include <stdio.h>
#include <math.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_blas.h>
#include <proj_gauss_mixtures.h>

void calc_splitnmerge(struct datapoint * data,int N,
		      struct gaussian * gaussians, int K,
		      gsl_matrix * qij, int * snmhierarchy){
  gsl_matrix * tempqij = gsl_matrix_alloc(N,K);
  gsl_matrix_memcpy(tempqij,qij);
  gsl_matrix * Jmerge = gsl_matrix_alloc(K,K);
  gsl_matrix_set_all(Jmerge,-1.);
  int kk1, kk2, kk, ii,maxsnm= K*(K-1)*(K-2)/2;
  int d = (gaussians->VV)->size1;//dim of mm
  double temp1,temp2,temp;
  for (kk1 = 0; kk1 != K; ++kk1)
    for (kk2 = kk1+1; kk2 != K; ++kk2){
      temp = 0.;
      for (ii=0; ii != N; ++ii){
	//make them all exps
	temp1 = exp(gsl_matrix_get(qij,ii,kk1));
	temp2 = exp(gsl_matrix_get(qij,ii,kk2));
	temp += temp1*temp2;
      }
      gsl_matrix_set(Jmerge,kk1,kk2,temp);
    }

  //Then calculate Jsplit
  gsl_vector * Jsplit = gsl_vector_alloc(K);
  gsl_vector * Jsplit_temp = gsl_vector_alloc(K);
  gsl_vector_set_all(Jsplit,-1.);
  //if there is missing data, fill in the missing data
  struct missingdatapoint{
    gsl_vector *ww;
  };
  struct missingdatapoint * missingdata;
  missingdata = (struct missingdatapoint *) malloc(N * sizeof (struct missingdatapoint) );
  for (ii=0; ii != N; ++ii){
    missingdata->ww = gsl_vector_alloc(d);
    ++missingdata;
  }
  missingdata -= N;

  gsl_matrix * tempRR,* tempVV;
  gsl_vector * tempSS,* tempwork;
  gsl_vector * expectedww = gsl_vector_alloc(d);
  //gsl_vector_view tempUcol;
  double lambda;
  int di,signum;
  for (ii=0; ii != N; ++ii){
    //First check whether there is any missing data
    if ((data->ww)->size == d){
      gsl_vector_memcpy(missingdata->ww,data->ww);
      ++missingdata;
      ++data;
      continue;
    }

    /*AS IT STANDS THE MISSING DATA PART IS *NOT* IMPLEMENTED CORRECTLY:
      A CORRECT IMPLEMENTATION NEEDS THE NULL SPACE OF THE PROJECTION
      MATRIX WHICH CAN BE FOUND FROM THE FULL SINGULAR VALUE DECOMPOSITIIN,
      UNFORTUNATELY GSL DOES NOT COMPUTE THE FULL SVD, BUT ONLY THE THIN SVD.
      LAPACK MIGHT DO, BUT MIGHT NOT BE INSTALLED (?) AND THIS MIGHT BE HARD TO IMPLEMENT.

      INDICATED BELOW ARE THE SECTION THAT WOULD HAVE TO BE FIXED TO MAKE THIS WORK
     */

    //calculate expectation, for this we need to calculate the bbijs (EXACTLY THE SAME AS IN PROJ_EM, SHOULD WRITE GENERAL FUNCTION TO DO THIS)
    gsl_vector_set_zero(expectedww);
    for (kk = 0; kk != K; ++kk){
      //prepare...
      di = (data->SS)->size1;
      p = gsl_permutation_alloc (di);
      wminusRm = gsl_vector_alloc (di);
      gsl_vector_memcpy(wminusRm,data->ww);
      TinvwminusRm = gsl_vector_alloc (di);
      Tij = gsl_matrix_alloc(di,di);
      gsl_matrix_memcpy(Tij,data->SS);
      Tij_inv = gsl_matrix_alloc(di,di);
      VRT = gsl_matrix_alloc(d,di);
      VRTTinv = gsl_matrix_alloc(d,di);
      Rtrans = gsl_matrix_alloc(d,di);
      //Calculate Tij
      gsl_matrix_transpose_memcpy(Rtrans,data->RR);
      gsl_blas_dsymm(CblasLeft,CblasUpper,1.0,gaussians->VV,Rtrans,0.0,VRT);//Only the upper right part of VV is calculated
      gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,data->RR,VRT,1.0,Tij);//This is Tij
      //Calculate LU decomp of Tij and Tij inverse
      gsl_linalg_LU_decomp(Tij,p,&signum);
      gsl_linalg_LU_invert(Tij,p,Tij_inv);
      //Calculate Tijinv*(w-Rm)
      gsl_blas_dgemv(CblasNoTrans,-1.0,data->RR,gaussians->mm,1.0,wminusRm);
      gsl_blas_dsymv(CblasUpper,1.0,Tij_inv,wminusRm,0.0,TinvwminusRm);
      //Now calculate bij and Bij
      gsl_vector_memcpy(bs->bbij,gaussians->mm);
      gsl_blas_dgemv(CblasNoTrans,1.0,VRT,TinvwminusRm,1.0,bs->bbij);
      //..and add the result to expectedww
      gsl_vector_scale(bs->bbij,exp(gsl_matrix_get(qij,ii,kk)));
      gsl_vector_add(expectedww,bs->bbij);
      //Clean up
      gsl_permutation_free (p);
      gsl_vector_free(wminusRm);
      gsl_vector_free(TinvwminusRm);
      gsl_matrix_free(Tij);
      gsl_matrix_free(Tij_inv);
      gsl_matrix_free(VRT);
      gsl_matrix_free(VRTTinv);
      gsl_matrix_free(Rtrans);
      ++gaussians;
    }
    gaussians -= K;
    //if missing, fill in the missing data
    tempRR = gsl_matrix_alloc((data->RR)->size2,(data->RR)->size1);//will hold the transpose of RR
    //tempVV = gsl_matrix_alloc((data->RR)->size1,(data->RR)->size1);
    //tempSS = gsl_vector_alloc((data->RR)->size1);
    //tempwork = gsl_vector_alloc((data->RR)->size1);
    gsl_matrix_transpose_memcpy(tempRR,data->RR);
    gsl_blas_dgemv(CblasNoTrans,1.,tempRR,data->ww,0.,missingdata->ww);
    //gsl_linalg_SV_decomp(tempRR,tempVV,tempSS,tempwork);
    //compute the missing data THIS PART IS NOT IMPLEMENTED CORRECTLY
    //for (kk = 0; kk != d-(data->ww)->size; ++kk){
      //tempUcol = gsl_matrix_column(tempRR,d-1-kk);
    //gsl_blas_ddot(&(tempUcol.vector),expectedww,&lambda);
    //gsl_vector_scale(&(tempUcol.vector),lambda);
    //gsl_vector_add(missingdata->ww,&(tempUcol.vector));
    //}
    ++missingdata;
    ++data;
    //free
    gsl_matrix_free(tempRR);
    //gsl_matrix_free(tempVV);
    //gsl_vector_free(tempSS);
    //gsl_vector_free(tempwork);
  }
  data -= N;
  missingdata -= N;

  //then for every gaussian, calculate the KL divergence between the local data density and the l-th gaussian
  double tempsplit;
  p = gsl_permutation_alloc (d);
  tempVV= gsl_matrix_alloc(d,d);
  tempRR= gsl_matrix_alloc(d,d);
  tempSS= gsl_vector_alloc(d);
  tempwork= gsl_vector_alloc(d);
  for (kk = 0; kk != K; ++kk){
    //calculate qil/ql factors
    normalize_row(tempqij,kk,false,true,0.);
    //calculate inverse of V and det(V)
    gsl_matrix_memcpy(tempVV,gaussians->VV);
    gsl_linalg_LU_decomp(tempVV,p,&signum);
    gsl_linalg_LU_invert(tempVV,p,tempRR);//tempRR now has the inverse of VV
    tempsplit = d * halflogtwopi + 0.5 * gsl_linalg_LU_lndet(tempVV);
    for (ii=0; ii != N; ++ii){
      if (exp(gsl_matrix_get(tempqij,ii,kk)) == 0.){
	++missingdata;
	continue;
      }
      tempsplit += gsl_matrix_get(tempqij,ii,kk) * exp(gsl_matrix_get(tempqij,ii,kk));
      gsl_vector_memcpy(tempSS,gaussians->mm);
      gsl_vector_scale(tempSS,-1.);
      gsl_vector_add(tempSS,missingdata->ww);
      gsl_blas_dgemv(CblasNoTrans,1.0,tempRR,tempSS,0.,tempwork);
      gsl_blas_ddot(tempSS,tempwork,&lambda);
      tempsplit += 0.5 * exp(gsl_matrix_get(tempqij,ii,kk)) * lambda;
      ++missingdata;
    }
    gsl_vector_set(Jsplit,kk,tempsplit);
    //printf("Jsplit for gaussian %i = %f\n",kk,tempsplit);
    missingdata -= N;
    ++gaussians;
  }
  gaussians -= K;

  //free
  gsl_permutation_free(p);
  gsl_matrix_free(tempRR);
  gsl_matrix_free(tempVV);
  gsl_vector_free(tempSS);
  gsl_vector_free(tempwork);

  //and put everything in the hierarchy
  size_t maxj, maxk, maxl;
  for (kk1 = 0; kk1 != maxsnm; kk1 += (K-2)){
    gsl_matrix_max_index(Jmerge,&maxj,&maxk);
    gsl_vector_memcpy(Jsplit_temp,Jsplit);
    gsl_vector_set(Jsplit_temp,maxj,-1.);
    gsl_vector_set(Jsplit_temp,maxk,-1.);
    for (kk2=0; kk2 != K-2; ++kk2){
      maxl = gsl_vector_max_index(Jsplit_temp);
      gsl_vector_set(Jsplit_temp,maxl,-1.);
      *(snmhierarchy++)= maxj;
      *(snmhierarchy++)= maxk;
      *(snmhierarchy++)= maxl;
      //printf("j = %i, k = %i, l = %i\n",(int)maxj,(int)maxk,(int)maxl);
    }
    //then set it to zero and find the next
    gsl_matrix_set(Jmerge,maxj,maxk,-1.);
  }
  snmhierarchy -= 3*maxsnm;

  //clean up
  gsl_matrix_free(Jmerge);
  gsl_vector_free(Jsplit);
  gsl_vector_free(Jsplit_temp);

  return ;
}


/**
 * \author Sylvain Marsat, University of Maryland - NASA GSFC
 *
 * \brief C code for the implementation of the Fourier domain response for LIGO-VIRGO detectors.
 *
 */

#define _XOPEN_SOURCE 500

#ifdef __GNUC__
#define UNUSED __attribute__ ((unused))
#else
#define UNUSED
#endif

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <complex.h>
#include <time.h>
#include <unistd.h>
#include <getopt.h>
#include <stdbool.h>
#include <string.h>

#include <gsl/gsl_errno.h>
#include <gsl/gsl_bspline.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_min.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_complex.h>

#include "constants.h"
#include "LLVgeometry.h"
#include "struct.h"
#include "LLVFDresponse.h"
#include "timeconversion.h"

/************************************************************/
/********* Functions setting detector geometry **************/

/* Function setting the response matrix of a given detector, in cartesian coordinates */
void SetMatrixD(
  gsl_matrix* D,                       /* Output: matrix of the detector response Dij */
  const Detectortag tag)               /* Tag identifying the detector */
{
  /* Allocating and defining the cartesian unit vectors along the two arms */
  gsl_vector* nx = gsl_vector_alloc(3);
  gsl_vector* ny = gsl_vector_alloc(3);
  if(tag==LHO) {
    gsl_vector_set(nx, 0, LAL_LHO_4K_ARM_X_DIRECTION_X);
    gsl_vector_set(nx, 1, LAL_LHO_4K_ARM_X_DIRECTION_Y);
    gsl_vector_set(nx, 2, LAL_LHO_4K_ARM_X_DIRECTION_Z);
    gsl_vector_set(ny, 0, LAL_LHO_4K_ARM_Y_DIRECTION_X);
    gsl_vector_set(ny, 1, LAL_LHO_4K_ARM_Y_DIRECTION_Y);
    gsl_vector_set(ny, 2, LAL_LHO_4K_ARM_Y_DIRECTION_Z);
  }
  else if(tag==LLO) {
    gsl_vector_set(nx, 0, LAL_LLO_4K_ARM_X_DIRECTION_X);
    gsl_vector_set(nx, 1, LAL_LLO_4K_ARM_X_DIRECTION_Y);
    gsl_vector_set(nx, 2, LAL_LLO_4K_ARM_X_DIRECTION_Z);
    gsl_vector_set(ny, 0, LAL_LLO_4K_ARM_Y_DIRECTION_X);
    gsl_vector_set(ny, 1, LAL_LLO_4K_ARM_Y_DIRECTION_Y);
    gsl_vector_set(ny, 2, LAL_LLO_4K_ARM_Y_DIRECTION_Z);
  }
  else if(tag==VIRGO) {
    gsl_vector_set(nx, 0, LAL_VIRGO_ARM_X_DIRECTION_X);
    gsl_vector_set(nx, 1, LAL_VIRGO_ARM_X_DIRECTION_Y);
    gsl_vector_set(nx, 2, LAL_VIRGO_ARM_X_DIRECTION_Z);
    gsl_vector_set(ny, 0, LAL_VIRGO_ARM_Y_DIRECTION_X);
    gsl_vector_set(ny, 1, LAL_VIRGO_ARM_Y_DIRECTION_Y);
    gsl_vector_set(ny, 2, LAL_VIRGO_ARM_Y_DIRECTION_Z);
  }
  else {
    printf("Error: invalid detector tag\n");
    exit(1);
  }

  /* Setting the components of the matrix D = 1/2 (nx nx - ny ny) */
  for(int i=0; i<3; i++) {
    for(int j=0; j<3; j++) {
      gsl_matrix_set(D, i, j, 1./2*(gsl_vector_get(nx, i)*gsl_vector_get(nx, j) - gsl_vector_get(ny, i)*gsl_vector_get(ny, j)));
    }
  }
  gsl_vector_free(nx);
  gsl_vector_free(ny);
}

/* Function setting the position of a detector, in cartesian coordinates */
void SetVectorXd(
  gsl_vector* Xd,                      /* Output: position vector of the detector */
  const Detectortag tag)               /* Tag identifying the detector */
{
  if(tag==LHO) {
    gsl_vector_set(Xd, 0, LAL_LHO_4K_VERTEX_LOCATION_X_SI);
    gsl_vector_set(Xd, 1, LAL_LHO_4K_VERTEX_LOCATION_Y_SI);
    gsl_vector_set(Xd, 2, LAL_LHO_4K_VERTEX_LOCATION_Z_SI);
  }
  else if(tag==LLO) {
    gsl_vector_set(Xd, 0, LAL_LLO_4K_VERTEX_LOCATION_X_SI);
    gsl_vector_set(Xd, 1, LAL_LLO_4K_VERTEX_LOCATION_Y_SI);
    gsl_vector_set(Xd, 2, LAL_LLO_4K_VERTEX_LOCATION_Z_SI);
  }
  else if(tag==VIRGO) {
    gsl_vector_set(Xd, 0, LAL_VIRGO_VERTEX_LOCATION_X_SI);
    gsl_vector_set(Xd, 1, LAL_VIRGO_VERTEX_LOCATION_Y_SI);
    gsl_vector_set(Xd, 2, LAL_VIRGO_VERTEX_LOCATION_Z_SI);
  }
  else {
    printf("Error: invalid detector tag\n");
    exit(1);
  }
}

/* Function setting the cartesian coordinates of the wave frame vectors (X,Y,Z), given the position in the sky and polarization */
void SetVectorsXYZ(
  gsl_vector* X,                       /* Output: cartesian vector of the wave frame unit vector X */
  gsl_vector* Y,                       /* Output: cartesian vector of the wave frame unit vector Y */
  gsl_vector* Z,                       /* Output: cartesian vector of the wave frame unit vector Z */
  const double theta,                  /* First angle for the position in the sky (Earth-based spherical angle) */
  const double phi,                    /* Second angle for the position in the sky (Earth-based spherical angle) */
  const double psi)                    /* Polarization angle */
{
  double cospsi = cos(psi);
  double sinpsi = sin(psi);
  double costheta = cos(theta);
  double sintheta = sin(theta);
  double cosphi = cos(phi);
  double sinphi = sin(phi);
  /* Unit vector X */
  gsl_vector_set(X, 0, -cospsi*sinphi+sinpsi*costheta*cosphi);
  gsl_vector_set(X, 1, cospsi*cosphi+sinpsi*costheta*sinphi);
  gsl_vector_set(X, 2, -sinpsi*sintheta);
  /* Unit vector Y */
  gsl_vector_set(Y, 0, sinpsi*sinphi+cospsi*costheta*cosphi);
  gsl_vector_set(Y, 1, -sinpsi*cosphi+cospsi*costheta*sinphi);
  gsl_vector_set(Y, 2, -cospsi*sintheta);
  /* Unit vector Z */
  gsl_vector_set(Z, 0, -sintheta*cosphi);
  gsl_vector_set(Z, 1, -sintheta*sinphi);
  gsl_vector_set(Z, 2, -costheta);
}

/***************************************/
/********* Core functions **************/

/* Function to convert string input network string to Networktag */
Networktag ParseNetworktag(char* string) {
  Networktag tag;
  if(strcmp(string, "L")==0) tag = L;
  else if(strcmp(string, "H")==0) tag = H;
  else if(strcmp(string, "V")==0) tag = V;
  else if(strcmp(string, "LH")==0) tag = LH;
  else if(strcmp(string, "LV")==0) tag = LV;
  else if(strcmp(string, "HV")==0) tag = HV;
  else if(strcmp(string, "LHV")==0) tag = LHV;
  else {
    printf("Error in ParseNetworktag: string not recognized.\n");
    exit(1);
  }
  return tag;
}

/* Function evaluating the Fourier-domain factors for LHV detectors */
/* Note: in case only one or two detectors are considered, amplitudes for the missing ones are set to 0 */
/* Contrarily to the LISA case, here returns trivially 0 or 1 */
/* (allows minimal changes from the old structure that assumed LHV - but not optimal) */
static int EvaluateDetectorFactor3Det(
  double* factor1,                       /* Output for factor for detector 1 */
  double* factor2,                       /* Output for factor for detector 2 */
  double* factor3,                       /* Output for factor for detector 3 */
  const Networktag networktag)                  /* Selector for the detector network */
{
  switch(networktag) {
    case L:
    *factor1 = 1.;
    *factor2 = 0;
    *factor3 = 0;
    break;
    case H:
    *factor1 = 0;
    *factor2 = 1.;
    *factor3 = 0;
    break;
    case V:
    *factor1 = 0;
    *factor2 = 0;
    *factor3 = 1.;
    break;
    case LH:
    *factor1 = 1.;
    *factor2 = 1.;
    *factor3 = 0;
    break;
    case LV:
    *factor1 = 1.;
    *factor2 = 0;
    *factor3 = 1.;
    break;
    case HV:
    *factor1 = 0;
    *factor2 = 1.;
    *factor3 = 1.;
    break;
    case LHV:
    *factor1 = 1.;
    *factor2 = 1.;
    *factor3 = 1.;
    break;
    default:
    printf("Error in EvaluateDetectorFactor3Det: networktag not recognized.\n");
    exit(1);
  }
  return SUCCESS;
}

/* Core function processing a signal (in the form of a list of modes) through the Fourier-domain LLV response (for a given detector), for given values of the inclination, position in the sky and polarization angle */
int LLVSimFDResponse(
  struct tagListmodesCAmpPhaseFrequencySeries **listhlm,  /* Input: list of modes in Frequency-domain amplitude and phase form as produced by the ROM */
  struct tagListmodesCAmpPhaseFrequencySeries **lists,    /* Output: list of contribution of each mode in the detector signal, in Frequency-domain amplitude and phase form, for the given detector and sky position */
  const double gpstime,                                   /* GPS time (s) when the signal at coalescence reaches geocenter */
  const double ra,                                        /* Position in the sky: J2000.0 right ascension (rad) */
  const double dec,                                       /* Position in the sky: J2000.0 declination (rad) */
  const double inclination,                               /* Inclination of the source (rad) */
  const double psi,                                       /* Polarization angle (rad) */
  const Detectortag tag)                                  /* Tag identifying the detector */
{
  /* Define matrix D and position vector Xd of the detector */
  gsl_matrix* D = gsl_matrix_alloc(3,3);
  gsl_vector* Xd = gsl_vector_alloc(3);
  SetVectorXd(Xd, tag);
  SetMatrixD(D, tag);

  /* Conversion from (ra, dec) to the Earth-based spherical angles (theta, phi) - neglecting nutation and precession, and identifying UT1 and UTC, so accurate roughly to a second of time */
  double gmst_angle = gmst_angle_from_gpstime(gpstime);
  double theta = PI/2 - dec;
  double phi = ra - gmst_angle;

  /* Define waveframe unit vectors (X,Y,Z) */
  gsl_vector* X = gsl_vector_alloc(3);
  gsl_vector* Y = gsl_vector_alloc(3);
  gsl_vector* Z = gsl_vector_alloc(3);
  SetVectorsXYZ(X, Y, Z, theta, phi, psi);

  /* Compute the delay from geocenter to the detector */
  double delaylength;
  gsl_blas_ddot(Xd, Z, &delaylength);
  double twopidelay = 2*PI*delaylength/C_SI;

  /* Compute the value of pattern functions Fplus, Fcross */
  gsl_vector* DX = gsl_vector_calloc(3); /* Temporary vector D.X, initialized to 0 */
  gsl_vector* DY = gsl_vector_calloc(3); /* Temporary vector D.Y, initialized to 0 */
  gsl_blas_dgemv( CblasNoTrans, 1., D, X, 0, DX);
  gsl_blas_dgemv( CblasNoTrans, 1., D, Y, 0, DY);
  double XDX;
  double XDY;
  double YDY;
  gsl_blas_ddot(X, DX, &XDX);
  gsl_blas_ddot(X, DY, &XDY);
  gsl_blas_ddot(Y, DY, &YDY);
  double Fplus = XDX - YDY;
  double Fcross = 2*XDY;

  /* Main loop over the modes - goes through all the modes present, stopping when encountering NULL */
  ListmodesCAmpPhaseFrequencySeries* listelement = *listhlm;
  while(listelement) {

    /* Definitions: l,m, frequency series and length */
    int l = listelement->l;
    int m = listelement->m;
    CAmpPhaseFrequencySeries* freqseries = listelement->freqseries;
    gsl_vector* freq = freqseries->freq;
    gsl_vector* amp_real = freqseries->amp_real;
    gsl_vector* amp_imag = freqseries->amp_imag;
    gsl_vector* phase = freqseries->phase;
    int len = (int) freq->size;
    double f;
    double complex camp;
    double complex camps;

    /* Computing the Ylm combined factors for plus and cross for this mode */
    /* Capital Phi is set to 0 by convention */
    double complex Yfactorplus;
    double complex Yfactorcross;
    if (!(l%2)) {
      Yfactorplus = 1./2 * (SpinWeightedSphericalHarmonic(inclination, 0., -2, l, m) + conj(SpinWeightedSphericalHarmonic(inclination, 0., -2, l, -m)));
      Yfactorcross = I/2 * (SpinWeightedSphericalHarmonic(inclination, 0., -2, l, m) - conj(SpinWeightedSphericalHarmonic(inclination, 0., -2, l, -m)));
    }
    else {
      Yfactorplus = 1./2 * (SpinWeightedSphericalHarmonic(inclination, 0., -2, l, m) - conj(SpinWeightedSphericalHarmonic(inclination, 0., -2, l, -m)));
      Yfactorcross = I/2 * (SpinWeightedSphericalHarmonic(inclination, 0., -2, l, m) + conj(SpinWeightedSphericalHarmonic(inclination, 0., -2, l, -m)));
    }

    /* Initializing frequency series structure for this mode, for the signal s = F+ h+ + Fx hx */
    CAmpPhaseFrequencySeries *modefreqseriess = NULL;
    CAmpPhaseFrequencySeries_Init(&modefreqseriess, len);
    gsl_vector* freqs = modefreqseriess->freq;
    gsl_vector* amp_reals = modefreqseriess->amp_real;
    gsl_vector* amp_imags = modefreqseriess->amp_imag;
    gsl_vector* phases = modefreqseriess->phase;

    /* Loop over the frequencies - multiplying by F+, Fx, and add phase due to the delay from geocenter to the detector */
    double complex factorcamp = Fplus*Yfactorplus + Fcross*Yfactorcross;
    for(int j=0; j<len; j++) {
      f = gsl_vector_get(freq, j);
      camp = gsl_vector_get(amp_real, j) + I*gsl_vector_get(amp_imag, j);
      camps = camp * factorcamp;
      gsl_vector_set(amp_reals, j, creal(camps));
      gsl_vector_set(amp_imags, j, cimag(camps));
      gsl_vector_set(phases, j, gsl_vector_get(phase, j) + twopidelay*f);
    }
    /* Copying the vectors of frequencies */
    gsl_vector_memcpy(freqs, freq);

    /* Append the modes to the ouput list-of-modes structures */
    *lists = ListmodesCAmpPhaseFrequencySeries_AddModeNoCopy(*lists, modefreqseriess, l, m);

    /* Going to the next mode in the list */
    listelement = listelement->next;
  }

  /* Cleaning */
  gsl_matrix_free(D);
  gsl_vector_free(Xd);
  gsl_vector_free(X);
  gsl_vector_free(Y);
  gsl_vector_free(Z);
  gsl_vector_free(DX);
  gsl_vector_free(DY);

  return SUCCESS;
}

/* Core function processing a signal (in the form of a list of modes) through the Fourier-domain LLV response for a given detector network, for given values of the inclination, position in the sky and polarization angle */
/* Note: as for now, asssumes the three detectors are L,H,V - amplitudes simply set to 0 in those that are not selected by networktag */
int LLVSimFDResponse3Det(
  struct tagListmodesCAmpPhaseFrequencySeries **list1,    /* Output: list of contributions of each mode in the signal of detector 1, in Frequency-domain amplitude and phase form */
  struct tagListmodesCAmpPhaseFrequencySeries **list2,    /* Output: list of contributions of each mode in the signal of detector 1, in Frequency-domain amplitude and phase form */
  struct tagListmodesCAmpPhaseFrequencySeries **list3,    /* Output: list of contributions of each mode in the signal of detector 1, in Frequency-domain amplitude and phase form */
  struct tagListmodesCAmpPhaseFrequencySeries **listhlm,  /* Input: list of modes in Frequency-domain amplitude and phase form as produced by the ROM */
  const double gpstime,                                   /* GPS time (s) when the signal at coalescence reaches geocenter */
  const double ra,                                        /* Position in the sky: J2000.0 right ascension (rad) */
  const double dec,                                       /* Position in the sky: J2000.0 declination (rad) */
  const double inclination,                               /* Inclination of the source (rad) */
  const double psi,                                       /* Polarization angle (rad) */
  const Networktag tag)                                   /* Tag identifying the network to use */
{
  /* Read which detectors are to be included in the network */
  double factor1 = 0;
  double factor2 = 0;
  double factor3 = 0;
  EvaluateDetectorFactor3Det(&factor1, &factor2, &factor3, tag);

  /* Define matrix D and position vector Xd for the 3 detectors */
  gsl_matrix* D1 = gsl_matrix_alloc(3,3);
  gsl_matrix* D2 = gsl_matrix_alloc(3,3);
  gsl_matrix* D3 = gsl_matrix_alloc(3,3);
  gsl_vector* Xd1 = gsl_vector_alloc(3);
  gsl_vector* Xd2 = gsl_vector_alloc(3);
  gsl_vector* Xd3 = gsl_vector_alloc(3);
  SetVectorXd(Xd1, LHO);
  SetVectorXd(Xd2, LLO);
  SetVectorXd(Xd3, VIRGO);
  SetMatrixD(D1, LHO);
  SetMatrixD(D2, LLO);
  SetMatrixD(D3, VIRGO);

  /* Conversion from (ra, dec) to the Earth-based spherical angles (theta, phi) - neglecting nutation and precession, and identifying UT1 and UTC, so accurate roughly to a second of time */
  double gmst_angle = gmst_angle_from_gpstime(gpstime);
  double theta = PI/2 - dec;
  double phi = ra - gmst_angle;

  /* Define waveframe unit vectors (X,Y,Z) */
  gsl_vector* X = gsl_vector_alloc(3);
  gsl_vector* Y = gsl_vector_alloc(3);
  gsl_vector* Z = gsl_vector_alloc(3);
  SetVectorsXYZ(X, Y, Z, theta, phi, psi);

  /* Compute the delays from geocenter to each detector */
  double delaylength1 = 0;
  double delaylength2 = 0;
  double delaylength3 = 0;
  gsl_blas_ddot(Xd1, Z, &delaylength1);
  gsl_blas_ddot(Xd2, Z, &delaylength2);
  gsl_blas_ddot(Xd3, Z, &delaylength3);
  double twopidelay1 = 2*PI*delaylength1/C_SI;
  double twopidelay2 = 2*PI*delaylength2/C_SI;
  double twopidelay3 = 2*PI*delaylength3/C_SI;

  /* Compute the value of pattern functions Fplus, Fcross for each detector */
  gsl_vector* DX1 = gsl_vector_calloc(3); /* Temporary vector D.X, initialized to 0 */
  gsl_vector* DY1 = gsl_vector_calloc(3); /* Temporary vector D.Y, initialized to 0 */
  gsl_vector* DX2 = gsl_vector_calloc(3); /* Temporary vector D.X, initialized to 0 */
  gsl_vector* DY2 = gsl_vector_calloc(3); /* Temporary vector D.Y, initialized to 0 */
  gsl_vector* DX3 = gsl_vector_calloc(3); /* Temporary vector D.X, initialized to 0 */
  gsl_vector* DY3 = gsl_vector_calloc(3); /* Temporary vector D.Y, initialized to 0 */
  gsl_blas_dgemv( CblasNoTrans, 1., D1, X, 0, DX1);
  gsl_blas_dgemv( CblasNoTrans, 1., D1, Y, 0, DY1);
  gsl_blas_dgemv( CblasNoTrans, 1., D2, X, 0, DX2);
  gsl_blas_dgemv( CblasNoTrans, 1., D2, Y, 0, DY2);
  gsl_blas_dgemv( CblasNoTrans, 1., D3, X, 0, DX3);
  gsl_blas_dgemv( CblasNoTrans, 1., D3, Y, 0, DY3);
  double XDX1 = 0.;
  double XDY1 = 0.;
  double YDY1 = 0.;
  double XDX2 = 0.;
  double XDY2 = 0.;
  double YDY2 = 0.;
  double XDX3 = 0.;
  double XDY3 = 0.;
  double YDY3 = 0.;
  gsl_blas_ddot(X, DX1, &XDX1);
  gsl_blas_ddot(X, DY1, &XDY1);
  gsl_blas_ddot(Y, DY1, &YDY1);
  gsl_blas_ddot(X, DX2, &XDX2);
  gsl_blas_ddot(X, DY2, &XDY2);
  gsl_blas_ddot(Y, DY2, &YDY2);
  gsl_blas_ddot(X, DX3, &XDX3);
  gsl_blas_ddot(X, DY3, &XDY3);
  gsl_blas_ddot(Y, DY3, &YDY3);
  double Fplus1 = XDX1 - YDY1;
  double Fcross1 = 2*XDY1;
  double Fplus2 = XDX2 - YDY2;
  double Fcross2 = 2*XDY2;
  double Fplus3 = XDX3 - YDY3;
  double Fcross3 = 2*XDY3;

  /* Main loop over the modes - goes through all the modes present, stopping when encountering NULL */
  ListmodesCAmpPhaseFrequencySeries* listelement = *listhlm;
  while(listelement) {

    /* Definitions: l,m, frequency series and length */
    int l = listelement->l;
    int m = listelement->m;
    CAmpPhaseFrequencySeries* freqseries = listelement->freqseries;
    gsl_vector* freq = freqseries->freq;
    gsl_vector* amp_real = freqseries->amp_real;
    gsl_vector* amp_imag = freqseries->amp_imag;
    gsl_vector* phase = freqseries->phase;
    int len = (int) freq->size;
    double f;
    double complex camp;
    double complex camp1;
    double complex camp2;
    double complex camp3;

    /* Computing the Ylm combined factors for plus and cross for this mode */
    /* Capital Phi is set to 0 by convention */
    double complex Yfactorplus;
    double complex Yfactorcross;
    if (!(l%2)) {
      Yfactorplus = 1./2 * (SpinWeightedSphericalHarmonic(inclination, 0., -2, l, m) + conj(SpinWeightedSphericalHarmonic(inclination, 0., -2, l, -m)));
      Yfactorcross = I/2 * (SpinWeightedSphericalHarmonic(inclination, 0., -2, l, m) - conj(SpinWeightedSphericalHarmonic(inclination, 0., -2, l, -m)));
    }
    else {
      Yfactorplus = 1./2 * (SpinWeightedSphericalHarmonic(inclination, 0., -2, l, m) - conj(SpinWeightedSphericalHarmonic(inclination, 0., -2, l, -m)));
      Yfactorcross = I/2 * (SpinWeightedSphericalHarmonic(inclination, 0., -2, l, m) + conj(SpinWeightedSphericalHarmonic(inclination, 0., -2, l, -m)));
    }

    /* Initializing frequency series structure for this mode, for the signal s = F+ h+ + Fx hx in each detector */
    CAmpPhaseFrequencySeries *modefreqseries1 = NULL;
    CAmpPhaseFrequencySeries *modefreqseries2 = NULL;
    CAmpPhaseFrequencySeries *modefreqseries3 = NULL;
    CAmpPhaseFrequencySeries_Init(&modefreqseries1, len);
    CAmpPhaseFrequencySeries_Init(&modefreqseries2, len);
    CAmpPhaseFrequencySeries_Init(&modefreqseries3, len);
    gsl_vector* freq1 = modefreqseries1->freq;
    gsl_vector* freq2 = modefreqseries2->freq;
    gsl_vector* freq3 = modefreqseries3->freq;
    gsl_vector* amp_real1 = modefreqseries1->amp_real;
    gsl_vector* amp_real2 = modefreqseries2->amp_real;
    gsl_vector* amp_real3 = modefreqseries3->amp_real;
    gsl_vector* amp_imag1 = modefreqseries1->amp_imag;
    gsl_vector* amp_imag2 = modefreqseries2->amp_imag;
    gsl_vector* amp_imag3 = modefreqseries3->amp_imag;
    gsl_vector* phase1 = modefreqseries1->phase;
    gsl_vector* phase2 = modefreqseries2->phase;
    gsl_vector* phase3 = modefreqseries3->phase;

    /* Loop over the frequencies - multiplying by F+, Fx, and add phase due to the delay from geocenter to the detector */
    double complex factorcamp1 = Fplus1*Yfactorplus + Fcross1*Yfactorcross;
    double complex factorcamp2 = Fplus2*Yfactorplus + Fcross2*Yfactorcross;
    double complex factorcamp3 = Fplus3*Yfactorplus + Fcross3*Yfactorcross;
    for(int j=0; j<len; j++) {
      f = gsl_vector_get(freq, j);
      camp = gsl_vector_get(amp_real, j) + I*gsl_vector_get(amp_imag, j);
      /* Note: include factor123 as detector selectors, which are simpy 0 or 1 whether the detector is in the array or not */
      camp1 = factor1 * camp * factorcamp1;
      camp2 = factor2 * camp * factorcamp2;
      camp3 = factor3 * camp * factorcamp3;
      gsl_vector_set(amp_real1, j, creal(camp1));
      gsl_vector_set(amp_real2, j, creal(camp2));
      gsl_vector_set(amp_real3, j, creal(camp3));
      gsl_vector_set(amp_imag1, j, cimag(camp1));
      gsl_vector_set(amp_imag2, j, cimag(camp2));
      gsl_vector_set(amp_imag3, j, cimag(camp3));
      gsl_vector_set(phase1, j, gsl_vector_get(phase, j) + twopidelay1*f);
      gsl_vector_set(phase2, j, gsl_vector_get(phase, j) + twopidelay2*f);
      gsl_vector_set(phase3, j, gsl_vector_get(phase, j) + twopidelay3*f);
    }
    /* Copying the vectors of frequencies */
    gsl_vector_memcpy(freq1, freq);
    gsl_vector_memcpy(freq2, freq);
    gsl_vector_memcpy(freq3, freq);

    /* Append the modes to the ouput list-of-modes structures */
    *list1 = ListmodesCAmpPhaseFrequencySeries_AddModeNoCopy(*list1, modefreqseries1, l, m);
    *list2 = ListmodesCAmpPhaseFrequencySeries_AddModeNoCopy(*list2, modefreqseries2, l, m);
    *list3 = ListmodesCAmpPhaseFrequencySeries_AddModeNoCopy(*list3, modefreqseries3, l, m);

    /* Going to the next mode in the list */
    listelement = listelement->next;
  }

  /* Cleaning */
  gsl_matrix_free(D1);
  gsl_matrix_free(D2);
  gsl_matrix_free(D3);
  gsl_vector_free(Xd1);
  gsl_vector_free(Xd2);
  gsl_vector_free(Xd3);
  gsl_vector_free(X);
  gsl_vector_free(Y);
  gsl_vector_free(Z);
  gsl_vector_free(DX1);
  gsl_vector_free(DX2);
  gsl_vector_free(DX3);
  gsl_vector_free(DY1);
  gsl_vector_free(DY2);
  gsl_vector_free(DY3);

  return SUCCESS;
}


/*! @copyright (c) 2017 King Abdullah University of Science and
 *                      Technology (KAUST). All rights reserved.
 *
 * STARS-H is a software package, provided by King Abdullah
 *             University of Science and Technology (KAUST)
 *
 * @file testing/rndtiled.c
 * @version 1.3.0
 * @author Aleksandr Mikhalev
 * @date 2017-11-07
 * */

#ifdef MKL
    #include <mkl.h>
#else
    #include <cblas.h>
    #include <lapacke.h>
#endif
#include <stdio.h>
#include <stdlib.h>
#include <omp.h>
#include <starsh.h>
#include <starsh-randtlr.h>

int main(int argc, char **argv)
{
    if(argc != 6)
    {
        printf("%d arguments provided, but 5 are needed\n", argc-1);
        printf("randtlr N NB decay maxrank tol\n");
        return 1;
    }
    int N = atoi(argv[1]);
    int block_size = atoi(argv[2]);
    double decay = atof(argv[3]);
    int maxrank = atoi(argv[4]);
    double tol = atof(argv[5]);
    int onfly = 0;
    char symm = 'N', dtype = 'd';
    STARSH_int shape[2] = {N, N};
    int info;
    srand(0);
    // Init STARS-H
    info = starsh_init();
    if(info != 0)
        return info;
    // Generate problem by random matrices
    STARSH_randtlr *data;
    STARSH_kernel *kernel;
    info = starsh_application((void **)&data, &kernel, N, dtype,
            STARSH_RANDTLR, STARSH_RANDTLR_KERNEL1,
            STARSH_RANDTLR_NB, block_size,
            STARSH_RANDTLR_DECAY, decay, STARSH_RANDTLR_DIAG, (double)N, 0);
    if(info != 0)
    {
        printf("Problem was NOT generated (wrong parameters)\n");
        return info;
    }
    // Init problem with given data and kernel and print short info
    STARSH_problem *P;
    info = starsh_problem_new(&P, 2, shape, symm, dtype, data, data, kernel,
            "Randomly generated tiled blr-matrix");
    if(info != 0)
        return info;
    starsh_problem_info(P);
    // Init plain clusterization and print info
    STARSH_cluster *C;
    info = starsh_cluster_new_plain(&C, data, N, block_size);
    if(info != 0)
        return info;
    starsh_cluster_info(C);
    // Init tlr division into admissible blocks and print short info
    STARSH_blrf *F;
    info = starsh_blrf_new_tlr(&F, P, symm, C, C);
    if(info != 0)
        return info;
    starsh_blrf_info(F);
    // Approximate each admissible block
    STARSH_blrm *M;
    double time1 = omp_get_wtime();
    info = starsh_blrm_approximate(&M, F, maxrank, tol, onfly);
    if(info != 0)
        return info;
    time1 = omp_get_wtime()-time1;
    // Print info about updated format and approximation
    starsh_blrf_info(F);
    starsh_blrm_info(M);
    printf("TIME TO APPROXIMATE: %e secs\n", time1);
    // Measure approximation error
    time1 = omp_get_wtime();
    double rel_err = starsh_blrm__dfe_omp(M);
    time1 = omp_get_wtime()-time1;
    printf("TIME TO MEASURE ERROR: %e secs\nRELATIVE ERROR: %e\n",
            time1, rel_err);
    if(rel_err/tol > 10.)
    {
        printf("Resulting relative error is too big\n");
        return 1;
    }
    // Free block low-rank matrix
    starsh_blrm_free(M);
    // Free block low-rank format
    starsh_blrf_free(F);
    // Free clusterization info
    starsh_cluster_free(C);
    // Free STARS_Problem instance
    starsh_problem_free(P);
    // Free randomly generated data
    starsh_randtlr_free(data);
    return 0;
}


/* ----------------------------------------------------------------------------
  JAM_AXI_RMS_WMMT

    Calculates weighted second moment.

    INPUTS
      xp    : projected x' [pc]
      yp    : projected y' [pc]
      nxy   : number of x' and y' values given
      incl  : inclination [radians]
      lum   : projected luminous MGE
      pot   : projected potential MGE
      beta  : velocity anisotropy (1 - vz^2 / vr^2)
      vv    : velocity integral selector (1=xx, 2=yy, 3=zz, 4=xy, 5=xz, 6=yz)

    NOTES
    * Based on janis2_weighted_second_moment_squared IDL code by Michele
      Cappellari.

  Mark den Brok
  Laura L Watkins [lauralwatkins@gmail.com]

  This code is released under a BSD 2-clause license.
  If you use this code for your research, please cite:
  Watkins et al. 2013, MNRAS, 436, 2598
  "Discrete dynamical models of omega Centauri"
  http://adsabs.harvard.edu/abs/2013MNRAS.436.2598W
---------------------------------------------------------------------------- */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_errno.h>
#include "jam.h"
#include "../mge/mge.h"
#include "../tools/tools.h"

double *jam_axi_rms_wmmt( double *xp, double *yp, int nxy, double incl, \
        struct multigaussexp *lum, struct multigaussexp *pot, \
        double *beta, int vv , int *gslFlag_rms) {

    struct params_rmsint p;
    struct multigaussexp ilum, ipot;
    double ci, si, *kani, *s2l, *q2l, *s2q2l, *s2p, *e2p;
    double result, error, *sb_mu2;
    int i;
    int status = 0;

    // convert from projected MGEs to intrinsic MGEs
    ilum = mge_deproject( lum, incl );
    ipot = mge_deproject( pot, incl );

    // angles
    ci = cos( incl );
    si = sin( incl );

    // mge component combinations

    kani = (double *) malloc ( lum->ntotal * sizeof( double ) );
    s2l = (double *) malloc( lum->ntotal * sizeof( double ) );
    q2l = (double *) malloc( lum->ntotal * sizeof( double ) );
    s2q2l = (double *) malloc( lum->ntotal * sizeof( double ) );
    s2p = (double *) malloc( pot->ntotal * sizeof( double ) );
    e2p = (double *) malloc( pot->ntotal * sizeof( double ) );

    for ( i = 0; i < ilum.ntotal; i++ ) {
        kani[i] = 1. / ( 1. - beta[i] );
        s2l[i] = pow( ilum.sigma[i], 2. );
        q2l[i] = pow( ilum.q[i], 2. );
        s2q2l[i] = s2l[i] * q2l[i];
    }

    for ( i = 0; i < ipot.ntotal; i++ ) {
        s2p[i] = pow( ipot.sigma[i], 2. );
        e2p[i] = 1. - pow( ipot.q[i], 2. );
    }

    // parameters for the integrand function
    p.ci2 = ci * ci;
    p.si2 = si * si;
    p.cisi = ci * si;
    p.lum = &ilum;
    p.pot = &ipot;
    p.kani = kani;
    p.s2l = s2l;
    p.q2l = q2l;
    p.s2q2l = s2q2l;
    p.s2p = s2p;
    p.e2p = e2p;
    p.vv = vv;

    // perform integration

    gsl_integration_workspace *w = gsl_integration_workspace_alloc( 1000 );
    gsl_function F;
    gsl_set_error_handler_off();
    F.function = &jam_axi_rms_mgeint;

    sb_mu2 = (double *) malloc( nxy * sizeof( double ) );
    for ( i = 0; i < nxy; i++ ) {
        p.x2 = xp[i] * xp[i];
        p.y2 = yp[i] * yp[i];
        p.xy = xp[i] * yp[i];
        F.params = &p;
        status += gsl_integration_qag( &F, 0., 1., 0., 1e-5, 1000, 6, w, \
            &result, &error );
        sb_mu2[i] = result;
    }

    /*// if you want to test the gsl flagging thing uncomment these two lines
    int test = 1;
    status += test;
    */
    if (status > 0) { *gslFlag_rms += 1;   }

    gsl_integration_workspace_free( w );

    free( kani );
    free( s2l );
    free( q2l );
    free( s2q2l );
    free( s2p );
    free( e2p );

    return sb_mu2;

}