#ifndef DEPTH_TO_BEARING_H_ZXFA9HGG
#define DEPTH_TO_BEARING_H_ZXFA9HGG

#include <cmath>
#include <gsl/gsl>
#include <limits>
#include <sens_loc/camera_models/concepts.h>
#include <sens_loc/camera_models/utility.h>
#include <sens_loc/conversion/util.h>
#include <sens_loc/math/constants.h>
#include <sens_loc/math/coordinate.h>
#include <sens_loc/math/image.h>
#include <sens_loc/math/triangles.h>
#include <sens_loc/util/correctness_util.h>
#include <taskflow/taskflow.hpp>

namespace sens_loc::conversion {

/// Convert the image \p depth_image to an bearing angle image.
/// This function returns a new image with the same dimension as \p depth_image.
///
/// \tparam Direction the direction for neighbourhood-relationship between
/// pixels that form the bearing angle
/// \tparam Real precision to calculate in, needs to be floating-point
/// \tparam Intrinsic camera model that projects pixel to the unit sphere
/// \param depth_image range image that was taken by a sensor with the
/// calibration from \p intrinsic
/// \param intrinsic camera model to calculate the angle between light
/// rays that correspond to pixels
/// \returns cv::Mat<Real> with each pixel the bearing angle (radians) in
/// \p Direction. Invalid depth values result in 0 as result.
//
/// \note Depth images are orthografic and require conversion first!
/// \sa conversion::depth_to_laserscan
/// \sa camera_models::is_intrinsic_v
///
/// \pre \p depth_image to have 1 channel
/// \pre \p depth_image == laser-scan like image!
/// \pre \p depth_image is not empty
/// \pre \p the underlying type of \p depth_image matches \p PixelType
/// \pre \p intrinsic matches sensor that took the image, prior preprocessing
/// \post each bearing angle is in the range \f$[0, \pi)\f$
/// is of course possible with matching changes in the \p depth_image.
template <direction Direction,
          template <typename>
          typename Intrinsic,
          typename Real = float>
math::image<Real> depth_to_bearing(const math::image<Real>& depth_image,
                                   const Intrinsic<Real>&   intrinsic) noexcept;

/// This function provides are parallelized version of the conversion
/// functionality.
///
/// This function creates a taskflow for the row-wise parallel calculation
/// of the bearing angle image.
/// Only differences are documented here.
/// \param[in] depth_image,intrinsic the same
/// \param[out] ba_image result image that will be created with parallel
/// processing
/// \param[inout] flow parallel flow type that is used to parallelize the outer
/// for loop over all rows.
/// \returns synchronization points before and after the calculation of the
/// bearing angle image.
/// \sa depth_to_bearing
template <direction Direction,
          template <typename>
          typename Intrinsic,
          typename Real = float>
std::pair<tf::Task, tf::Task>
par_depth_to_bearing(const math::image<Real>& depth_image,
                     const Intrinsic<Real>&   intrinsic,
                     math::image<Real>&       ba_image,
                     tf::Taskflow&            flow) noexcept;

/// Convert a bearing angle image to an image with integer types.
/// This function scales the bearing angles between
/// [PixelType::min, PixelType::max] for the angles in range (0, PI).
///
/// \tparam Real underyling type of the \p bearing_image, floating-point
/// \tparam PixelType target type for the converted image that is returned,
/// arithmetic type (integer or floating-point)
/// \param bearing_image calculated
/// bearing angle image
/// \returns the bearing angle image is converted to a "normal" image that can
/// be displayed and stored with normal image visualization tools.
/// \pre the underlying type of \p bearing_image is \p Real
/// \pre range of pixel is in \p bearinge_image \f$[0, \pi)\f$
/// \post the underlying type of the result is \p PixelType
/// \post range of pixel in result is \f$[PixelType_{min}, PixelType_{max}]\f$
/// \sa conversion::depth_to_bearing
template <typename Real = float, typename PixelType = ushort>
math::image<PixelType>
convert_bearing(const math::image<Real>& bearing_image) noexcept;

namespace detail {
inline int get_du(direction dir) {
    switch (dir) {
    case direction::antidiagonal:
    case direction::diagonal:
    case direction::horizontal: return -1;
    case direction::vertical: return 0;
    }
    UNREACHABLE("Only 4 directions are possible");  // LCOV_EXCL_LINE
}

inline int get_dv(direction dir) {
    switch (dir) {
    case direction::diagonal:
    case direction::vertical: return -1;
    case direction::horizontal: return 0;
    case direction::antidiagonal: return 1;
    }
    UNREACHABLE("Only 4 directions are possible");  // LCOV_EXCL_LINE
}

/// Provide a general accessor for the pixels that provide depth information
/// to calculate the bearing angle.
template <typename Real, direction Direction>
class pixel {
  public:
    pixel()
        : _du{get_du(Direction)}
        , _dv{get_dv(Direction)} {}
    /// \returns (u, v) for the prior point depending on the direction.
    math::pixel_coord<int> operator()(const math::pixel_coord<int>& p) const
        noexcept {
        return {p.u() + _du, p.v() + _dv};
    }

  private:
    const int _du = 0;
    const int _dv = 0;
};

inline int get_x_start(direction dir) {
    switch (dir) {
    case direction::horizontal:
    case direction::antidiagonal:
    case direction::diagonal: return 1;
    case direction::vertical: return 0;
    }
    UNREACHABLE("Only 4 directions are possible");  // LCOV_EXCL_LINE
}

inline int get_y_start(direction dir) {
    switch (dir) {
    case direction::horizontal:
    case direction::antidiagonal: return 0;
    case direction::diagonal:
    case direction::vertical: return 1;
    }
    UNREACHABLE("Only 4 directions are possible");  // LCOV_EXCL_LINE
}

inline int get_dy_end(direction dir) {
    switch (dir) {
    case direction::antidiagonal: return 1;
    case direction::horizontal:
    case direction::diagonal:
    case direction::vertical: return 0;
    }
    UNREACHABLE("Only 4 directions are possible");  // LCOV_EXCL_LINE
}

/// Define the start and end iterator for each bearing angle direction.
template <direction Direction>
struct pixel_range {
    pixel_range(const cv::Mat& depth_image) noexcept
        : x_start{get_x_start(Direction)}
        , x_end{depth_image.cols}
        , y_start{get_y_start(Direction)}
        , y_end{depth_image.rows - get_dy_end(Direction)} {}

    const int x_start;
    const int x_end;
    const int y_start;
    const int y_end;
};

template <typename Real,
          typename RangeLimits,
          typename PriorAccess,
          template <typename>
          typename Intrinsic>
inline void bearing_inner(const RangeLimits&       r,
                          const PriorAccess&       prior_accessor,
                          const int                v,
                          const math::image<Real>& depth_image,
                          const Intrinsic<Real>&   intrinsic,
                          math::image<Real>&       ba_image) {
    for (int u = r.x_start; u < r.x_end; ++u) {
        const math::pixel_coord<int> central(u, v);
        const math::pixel_coord<int> prior = prior_accessor(central);

        const auto d_i = depth_image.at(central);
        const auto d_j = depth_image.at(prior);

        Expects(d_i >= Real(0.));
        Expects(d_j >= Real(0.));

        const Real d_phi = camera_models::phi(intrinsic, central, prior);

        // A depth==0 means there is no measurement at this pixel.
        const Real angle =
            (d_i == Real(0.) || d_j == Real(0.))
                ? Real(0.)
                : math::bearing_angle<Real>(d_i, d_j, std::cos(d_phi));

        Ensures(angle >= Real(0.));
        Ensures(angle < math::pi<Real>);

        ba_image.at(central) = angle;
    }
}
}  // namespace detail

template <direction Direction,
          template <typename>
          typename Intrinsic,
          typename Real>
inline math::image<Real>
depth_to_bearing(const math::image<Real>& depth_image,
                 const Intrinsic<Real>&   intrinsic) noexcept {
    static_assert(camera_models::is_intrinsic_v<Intrinsic, Real>);
    static_assert(std::is_floating_point_v<Real>);

    Expects(depth_image.w() == intrinsic.w());
    Expects(depth_image.h() == intrinsic.h());

    using namespace detail;
    const pixel<Real, Direction> prior_accessor;
    const pixel_range<Direction> r{depth_image.data()};

    // Image of Reals, that will be converted after the full calculation.
    cv::Mat ba(depth_image.h(), depth_image.w(),
               math::detail::get_opencv_type<Real>());
    ba = Real(0.);
    math::image<Real> ba_image(std::move(ba));

    for (int v = r.y_start; v < r.y_end; ++v)
        detail::bearing_inner(r, prior_accessor, v, depth_image, intrinsic,
                              ba_image);

    Ensures(ba_image.h() == depth_image.h());
    Ensures(ba_image.w() == depth_image.w());

    return ba_image;
}

template <direction Direction,
          template <typename>
          typename Intrinsic,
          typename Real>
inline std::pair<tf::Task, tf::Task>
par_depth_to_bearing(const math::image<Real>& depth_image,
                     const Intrinsic<Real>&   intrinsic,
                     math::image<Real>&       ba_image,
                     tf::Taskflow&            flow) noexcept {
    static_assert(camera_models::is_intrinsic_v<Intrinsic, Real>);
    static_assert(std::is_floating_point_v<Real>);

    using namespace detail;
    Expects(depth_image.w() == intrinsic.w());
    Expects(depth_image.h() == intrinsic.h());
    Expects(ba_image.w() == depth_image.w());
    Expects(ba_image.h() == depth_image.h());

    const pixel<Real, Direction> prior_accessor;
    const pixel_range<Direction> r{depth_image.data()};

    auto sync_points = flow.parallel_for(
        r.y_start, r.y_end, 1,
        [prior_accessor, r, &depth_image, &intrinsic, &ba_image](int v) {
            detail::bearing_inner(r, prior_accessor, v, depth_image, intrinsic,
                                  ba_image);
        });

    return sync_points;
}

template <typename Real, typename PixelType>
inline math::image<PixelType>
convert_bearing(const math::image<Real>& bearing_image) noexcept {
    static_assert(std::is_floating_point_v<Real>);
    static_assert(std::is_arithmetic_v<PixelType>);

    using detail::scaling_factor;
    cv::Mat img(bearing_image.h(), bearing_image.w(),
                math::detail::get_opencv_type<PixelType>());
    auto [scale, offset] =
        scaling_factor<Real, PixelType>(/*max_angle = */ math::pi<Real>);
    bearing_image.data().convertTo(
        img, math::detail::get_opencv_type<PixelType>(), scale, offset);

    Ensures(img.cols == bearing_image.w());
    Ensures(img.rows == bearing_image.h());
    Ensures(img.type() == math::detail::get_opencv_type<PixelType>());
    Ensures(img.channels() == 1);

    return math::image<PixelType>(std::move(img));
}
}  // namespace sens_loc::conversion

#endif /* end of include guard: DEPTH_TO_BEARING_H_ZXFA9HGG */


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

#include <gsl/gsl_errno.h>
#include <gsl/gsl_integration.h>

#include "ccl.h"

typedef struct{
  double l;
  ccl_cosmology *cosmo;
  ccl_cl_tracer_collection_t *trc1;
  ccl_cl_tracer_collection_t *trc2;
  ccl_f2d_t *psp;
  int *status;
} integ_cl_par;

typedef struct{
  int chipow;
  double l1;
  double l2;
  ccl_cosmology *cosmo;
  ccl_cl_tracer_collection_t *trc1;
  ccl_cl_tracer_collection_t *trc2;
  ccl_cl_tracer_collection_t *trc3;
  ccl_cl_tracer_collection_t *trc4;
  ccl_f3d_t *tsp;
  ccl_f1d_t *ker_extra;
  ccl_a_finder *finda;
  int *status;
} integ_cov_par;

static void update_chi_limits(ccl_cl_tracer_collection_t *trc,
                              double *chimin, double *chimax,
                              int is_union)
{
  int itr;
  double chimin_h=1E15;
  double chimax_h=-1E15;
  for(itr=0; itr < trc->n_tracers; itr++) {
    if (trc->ts[itr]->chi_min < chimin_h)
      chimin_h = trc->ts[itr]->chi_min;
    if (trc->ts[itr]->chi_max > chimax_h)
      chimax_h = trc->ts[itr]->chi_max;
  }

  if(is_union) {
    if(chimin_h < *chimin)
      *chimin = chimin_h;
    if(chimax_h > *chimax)
      *chimax = chimax_h;
  }
  else {
    if(chimin_h > *chimin)
      *chimin = chimin_h;
    if(chimax_h < *chimax)
      *chimax = chimax_h;
  }
}

static void get_k_interval(ccl_cosmology *cosmo,
                           ccl_cl_tracer_collection_t *trc1,
                           ccl_cl_tracer_collection_t *trc2,
                           double l, double *lkmin, double *lkmax) {
  int itr;

  // Loop through all tracers and find distance bounds
  double chi_min1 = 1E15;
  double chi_max1 = -1E15;
  for (itr=0; itr < trc1->n_tracers; itr++) {
    if (trc1->ts[itr]->chi_min < chi_min1)
      chi_min1 = trc1->ts[itr]->chi_min;
    if (trc1->ts[itr]->chi_max > chi_max1)
      chi_max1 = trc1->ts[itr]->chi_max;
  }

  double chi_min2 = 1E15;
  double chi_max2 = -1E15;
  for (itr=0; itr < trc2->n_tracers; itr++) {
    if (trc2->ts[itr]->chi_min < chi_min2)
      chi_min2 = trc2->ts[itr]->chi_min;
    if (trc2->ts[itr]->chi_max > chi_max2)
      chi_max2 = trc2->ts[itr]->chi_max;
  }

  // Find maximum of minima and minimum of maxima
  // (i.e. edges where the product of both kernels will have support).
  double chi_min = fmax(chi_min1, chi_min2);
  double chi_max = fmin(chi_max1, chi_max2);

  if (chi_min <= 0)
    chi_min = 0.5*(l+0.5)/cosmo->spline_params.K_MAX;

  // Don't go beyond kmax
  *lkmax = log(fmin(cosmo->spline_params.K_MAX, 2*(l+0.5)/chi_min));
  *lkmin = log(fmax(cosmo->spline_params.K_MIN, (l+0.5)/chi_max));
}

static double transfer_limber_single(ccl_cl_tracer_t *tr, double l, double lk,
                                     double k, double chi_l, double a_l,
                                     ccl_cosmology *cosmo, ccl_f2d_t *psp,
                                     int ignore_jbes_deriv,
                                     int *status) {
  double dd = 0;

  // Kernel and transfer evaluated at chi_l
  double w = ccl_cl_tracer_t_get_kernel(tr, chi_l, status);
  double t = ccl_cl_tracer_t_get_transfer(tr, lk,a_l, status);
  double fl = ccl_cl_tracer_t_get_f_ell(tr, l, status);

  if (tr->der_bessel < 1) { //We don't need l+1
    dd = w*t;
    if (tr->der_bessel == -1) { //If we divide by (chi*k)^2
      double lp1h = l+0.5;
      dd /= (lp1h*lp1h);
    }
  }
  else { // We will need l+1
    if(ignore_jbes_deriv)
      dd = 0;
    else {
      // Compute chi_{l+1} and a_{l+1}
      double lp1h = l+0.5;
      double lp3h = l+1.5;
      double chi_lp = lp3h/k;
      double a_lp = ccl_scale_factor_of_chi(cosmo, chi_lp, status);

      // Compute power spectrum ratio there
      double pk_ratio = fabs(ccl_f2d_t_eval(psp, lk, a_lp, cosmo, status) /
                             ccl_f2d_t_eval(psp, lk, a_l, cosmo, status));

      // Compute kernel and trasfer at chi_{l+1}
      double w_p = ccl_cl_tracer_t_get_kernel(tr, chi_lp, status);
      double t_p = ccl_cl_tracer_t_get_transfer(tr, lk,a_lp, status);

      // sqrt(2l+1/2l+3)
      double sqell = sqrt(lp1h*pk_ratio/lp3h);
      if (tr->der_bessel == 1)
        dd = l*w*t/lp1h-sqell*w_p*t_p;
      else //we assume der_bessel=2 here to avoid extra if clause
        dd = sqell*2*w_p*t_p/lp3h - (0.25+2*l)*w*t/(lp1h*lp1h);
    }
  }
  return dd*fl;
}

static double transfer_limber_wrap(double l,double lk, double k, double chi,
                                   double a, ccl_cl_tracer_collection_t *trc,
                                   ccl_cosmology *cosmo,ccl_f2d_t *psp,
                                   int ignore_jbes_deriv, int *status) {
  int itr;
  double transfer = 0;

  for (itr=0; itr < trc->n_tracers; itr++) {
    transfer += transfer_limber_single(
      trc->ts[itr], l, lk, k, chi, a, cosmo, psp, ignore_jbes_deriv, status);
    if (*status != 0)
      return -1;
  }
  return transfer;
}

static double cl_integrand(double lk, void *params) {
  double d1, d2;
  integ_cl_par *p = (integ_cl_par *)params;
  double k = exp(lk);
  double chi = (p->l+0.5)/k;
  double a = ccl_scale_factor_of_chi(p->cosmo, chi, p->status);

  d1 = transfer_limber_wrap(p->l, lk, k, chi, a, p->trc1,
                            p->cosmo, p->psp, 0, p->status);
  if (d1 == 0)
    return 0;

  d2 = transfer_limber_wrap(p->l, lk, k, chi, a, p->trc2,
                            p->cosmo, p->psp, 0, p->status);

  if (d2 == 0)
    return 0;

  double pk = ccl_f2d_t_eval(p->psp, lk, a, p->cosmo, p->status);

  return k*pk*d1*d2;
}

static void integ_cls_limber_spline(ccl_cosmology *cosmo,
				    integ_cl_par *ipar,
				    double lkmin, double lkmax,
				    double *result, int *status) {
  int ik;
  int nk = (int)(fmax((lkmax - lkmin) / cosmo->spline_params.DLOGK_INTEGRATION + 0.5,
		      1))+1;
  double *fk_arr = NULL;
  double *lk_arr = NULL;
  lk_arr = ccl_linear_spacing(lkmin, lkmax, nk);
  if(lk_arr == NULL)
    *status = CCL_ERROR_LOGSPACE;

  if(*status == 0) {
    fk_arr = malloc(nk * sizeof(double));
    if(fk_arr == NULL)
      *status = CCL_ERROR_MEMORY;
  }

  if(*status == 0) {
    for(ik=0; ik<nk; ik++) {
      fk_arr[ik] = cl_integrand(lk_arr[ik], ipar);
      if(*(ipar->status)) {
	*status = *(ipar->status);
	break;
      }
    }
  }

  if(*status == 0) {
    ccl_integ_spline(1, nk, lk_arr, &fk_arr,
                     1, -1, result, gsl_interp_akima,
                     status);
  }
  free(fk_arr);
  free(lk_arr);
}

static void integ_cls_limber_qag_quad(ccl_cosmology *cosmo,
				      gsl_function *F,
				      double lkmin, double lkmax,
				      gsl_integration_workspace *w,
				      double *result, double *eresult,
				      int *status) {
  int gslstatus;
  size_t nevals;
  gsl_integration_cquad_workspace *w_cquad = NULL;
  // Integrate
  gslstatus = gsl_integration_qag(F, lkmin, lkmax, 0,
				  cosmo->gsl_params.INTEGRATION_LIMBER_EPSREL,
				  cosmo->gsl_params.N_ITERATION,
				  cosmo->gsl_params.INTEGRATION_LIMBER_GAUSS_KRONROD_POINTS,
				  w, result, eresult);

  // Test if a round-off error occured in the evaluation of the integral
  // If so, try another integration function, more robust but potentially slower
  if (gslstatus == GSL_EROUND) {
    ccl_raise_gsl_warning(gslstatus,
			  "ccl_cls.c: integ_cls_limber_qag_quad(): "
			  "Default GSL integration failure, attempting backup method.");
    w_cquad = gsl_integration_cquad_workspace_alloc(cosmo->gsl_params.N_ITERATION);
    if (w_cquad == NULL)
      *status = CCL_ERROR_MEMORY;

    if (*status == 0) {
      nevals = 0;
      gslstatus = gsl_integration_cquad(F, lkmin, lkmax, 0,
					cosmo->gsl_params.INTEGRATION_LIMBER_EPSREL,
					w_cquad, result, eresult, &nevals);
    }
  }
  gsl_integration_cquad_workspace_free(w_cquad);
  if(*status == 0)
    *status = gslstatus;
}

void ccl_angular_cls_limber(ccl_cosmology *cosmo,
			    ccl_cl_tracer_collection_t *trc1,
			    ccl_cl_tracer_collection_t *trc2,
			    ccl_f2d_t *psp,
			    int nl_out, double *l_out, double *cl_out,
			    ccl_integration_t integration_method,
			    int *status) {

  // make sure to init core things for safety
  if (!cosmo->computed_distances) {
    *status = CCL_ERROR_DISTANCES_INIT;
    ccl_cosmology_set_status_message(
      cosmo,
      "ccl_cls.c: ccl_angular_cls_limber(): distance splines have not been precomputed!");
    return;
  }

  #pragma omp parallel shared(cosmo, trc1, trc2, l_out, cl_out, \
                              nl_out, status, psp, integration_method) \
                       default(none)
  {
    int clastatus, lind;
    integ_cl_par ipar;
    gsl_integration_workspace *w = NULL;
    int local_status = *status;
    gsl_function F;
    double lkmin, lkmax, l, result, eresult;

    if (local_status == 0) {
      // Set up integrating function parameters
      ipar.cosmo = cosmo;
      ipar.trc1 = trc1;
      ipar.trc2 = trc2;
      ipar.psp = psp;
      ipar.status = &clastatus;
    }

    if(integration_method == ccl_integration_qag_quad) {
      if (local_status == 0) {
	w = gsl_integration_workspace_alloc(cosmo->gsl_params.N_ITERATION);
	if (w == NULL) {
	  local_status = CCL_ERROR_MEMORY;
	}
      }

      if (local_status == 0) {
	// Set up integrating function
	F.function = &cl_integrand;
	F.params = &ipar;
      }
    }

    #pragma omp for schedule(dynamic)
    for (lind=0; lind < nl_out; ++lind) {
      if (local_status == 0) {
        l = l_out[lind];
        clastatus = 0;
        ipar.l = l;

        // Get integration limits
        get_k_interval(cosmo, trc1, trc2, l, &lkmin, &lkmax);

	// Integrate
	if(integration_method == ccl_integration_qag_quad) {
	  integ_cls_limber_qag_quad(cosmo, &F, lkmin, lkmax, w,
				    &result, &eresult, &local_status);
	}
	else if(integration_method == ccl_integration_spline) {
	  integ_cls_limber_spline(cosmo, &ipar, lkmin, lkmax,
				  &result, &local_status);
	}
	else
	  local_status = CCL_ERROR_NOT_IMPLEMENTED;

        if ((*ipar.status == 0) && (local_status == 0)) {
          cl_out[lind] = result / (l+0.5);
        }
        else {
          ccl_raise_gsl_warning(local_status, "ccl_cls.c: ccl_angular_cls_limber():");
          cl_out[lind] = NAN;
          local_status = CCL_ERROR_INTEG;
        }
      }
    }

    gsl_integration_workspace_free(w);

    if (local_status) {
      #pragma omp atomic write
      *status = local_status;
    }
  }

  if (*status) {
    ccl_cosmology_set_status_message(
      cosmo,
      "ccl_cls.c: ccl_angular_cls_limber(); integration error\n");
  }
}

void ccl_angular_cls_nonlimber(ccl_cosmology *cosmo,
                               ccl_cl_tracer_collection_t *trc1,
                               ccl_cl_tracer_collection_t *trc2,
                               ccl_f2d_t *psp,
                               int nl_out, int *l_out, double *cl_out,
                               int *status) {
  *status = CCL_ERROR_INCONSISTENT;
  ccl_cosmology_set_status_message(
    cosmo,
    "ccl_cls.c: ccl_angular_cls_nonlimber(); non-Limber integrator not implemented yet\n");
}

static double cov_integrand(double chi, void *params)
{
  double d1, d2, d3, d4, tkk, ker=1;
  integ_cov_par *p = (integ_cov_par *)params;
  double k1=(p->l1+0.5)/chi;
  double k2=(p->l2+0.5)/chi;
  double lk1=log(k1);
  double lk2=log(k2);
  double a = ccl_scale_factor_of_chi(p->cosmo, chi, p->status);

  d1 = transfer_limber_wrap(p->l1, lk1, k1, chi, a, p->trc1,
                            p->cosmo, NULL, 1, p->status);
  if (d1 == 0)
    return 0;
  d2 = transfer_limber_wrap(p->l1, lk1, k1, chi, a, p->trc2,
                            p->cosmo, NULL, 1, p->status);
  if (d2 == 0)
    return 0;
  d3 = transfer_limber_wrap(p->l2, lk2, k2, chi, a, p->trc3,
                            p->cosmo, NULL, 1, p->status);
  if (d3 == 0)
    return 0;
  d4 = transfer_limber_wrap(p->l2, lk2, k2, chi, a, p->trc4,
                            p->cosmo, NULL, 1, p->status);
  if (d4 == 0)
    return 0;

  tkk = ccl_f3d_t_eval(p->tsp, lk1, lk2, a,
                       p->finda, p->cosmo, p->status);
  if(p->ker_extra!=NULL)
    ker = ccl_f1d_t_eval(p->ker_extra, chi);

  return d1*d2*d3*d4*tkk*ker/pow(chi, p->chipow);
}

static void integ_cov_limber_spline(ccl_cosmology *cosmo,
				    integ_cov_par *ipar,
				    double chimin, double chimax,
				    double *result, int *status)
{
  int ichi;
  int nchi = (int)(fmax((chimax - chimin) / cosmo->spline_params.DCHI_INTEGRATION + 0.5,
		      1))+1;
  double *fchi_arr = NULL;
  double *chi_arr = NULL;
  chi_arr = ccl_linear_spacing(chimin, chimax, nchi);
  if(chi_arr == NULL)
    *status = CCL_ERROR_LOGSPACE;

  if(*status == 0) {
    fchi_arr = malloc(nchi * sizeof(double));
    if(fchi_arr == NULL)
      *status = CCL_ERROR_MEMORY;
  }

  if(*status == 0) {
    for(ichi=0; ichi<nchi; ichi++) {
      fchi_arr[ichi] = cov_integrand(chi_arr[ichi], ipar);
      if(*(ipar->status)) {
	*status = *(ipar->status);
	break;
      }
    }
  }

  if(*status == 0) {
    ccl_integ_spline(1, nchi, chi_arr, &fchi_arr,
                     1, -1, result, gsl_interp_akima,
                     status);
  }

  free(fchi_arr);
  free(chi_arr);
}

static void integ_cov_limber_qag_quad(ccl_cosmology *cosmo,
				      gsl_function *F,
				      double chimin, double chimax,
				      gsl_integration_workspace *w,
				      double *result, double *eresult,
				      int *status) {
  int gslstatus;
  size_t nevals;
  gsl_integration_cquad_workspace *w_cquad = NULL;
  // Integrate
  gslstatus = gsl_integration_qag(F, chimin, chimax, 0,
				  cosmo->gsl_params.INTEGRATION_LIMBER_EPSREL,
				  cosmo->gsl_params.N_ITERATION,
				  cosmo->gsl_params.INTEGRATION_LIMBER_GAUSS_KRONROD_POINTS,
				  w, result, eresult);

  // Test if a round-off error occured in the evaluation of the integral
  // If so, try another integration function, more robust but potentially slower
  if (gslstatus == GSL_EROUND) {
    ccl_raise_gsl_warning(gslstatus,
			  "ccl_cls.c: ccl_angular_cov_limber(): "
			  "Default GSL integration failure, attempting backup method.");
    w_cquad = gsl_integration_cquad_workspace_alloc(cosmo->gsl_params.N_ITERATION);
    if (w_cquad == NULL)
      *status = CCL_ERROR_MEMORY;

    if (*status == 0) {
      nevals = 0;
      gslstatus = gsl_integration_cquad(F, chimin, chimax, 0,
					cosmo->gsl_params.INTEGRATION_LIMBER_EPSREL,
					w_cquad, result, eresult, &nevals);
    }
  }
  gsl_integration_cquad_workspace_free(w_cquad);
  if(*status == 0)
    *status = gslstatus;
}

void ccl_angular_cl_covariance(ccl_cosmology *cosmo,
                               ccl_cl_tracer_collection_t *trc1,
                               ccl_cl_tracer_collection_t *trc2,
                               ccl_cl_tracer_collection_t *trc3,
                               ccl_cl_tracer_collection_t *trc4,
                               ccl_f3d_t *tsp,
                               int nl1_out, double *l1_out,
                               int nl2_out, double *l2_out,
                               double *cov_out,
                               ccl_integration_t integration_method,
                               int chi_exponent, ccl_f1d_t *kernel_extra,
                               double prefactor_extra, int *status)
{
  if(!cosmo->computed_distances) {
    *status = CCL_ERROR_DISTANCES_INIT;
    ccl_cosmology_set_status_message(
      cosmo,
      "ccl_cls.c: ccl_angular_cl_limber(): distance splines have not been precomputed!");
    return;
  }

  #pragma omp parallel shared(cosmo, trc1, trc2, trc3, trc4, tsp, \
                              nl1_out, l1_out, nl2_out, l2_out, cov_out, \
                              integration_method, chi_exponent, \
                              kernel_extra, prefactor_extra, status) \
                       default(none)
  {
    int clastatus, lind1,lind2;
    integ_cov_par ipar;
    gsl_integration_workspace *w = NULL;
    int local_status = *status;
    gsl_function F;
    double chimin, chimax;
    double l1, l2, result, eresult;
    ccl_a_finder *finda = ccl_a_finder_new_from_f3d(tsp);

    // Find integration limits
    chimin = 1E15;
    chimax = -1E15;
    update_chi_limits(trc1, &chimin, &chimax, 1);
    update_chi_limits(trc2, &chimin, &chimax, 0);
    update_chi_limits(trc3, &chimin, &chimax, 0);
    update_chi_limits(trc4, &chimin, &chimax, 0);

    if (local_status == 0) {
      // Set up integrating function parameters
      ipar.cosmo = cosmo;
      ipar.trc1 = trc1;
      ipar.trc2 = trc2;
      ipar.trc3 = trc3;
      ipar.trc4 = trc4;
      ipar.tsp = tsp;
      ipar.ker_extra = kernel_extra;
      ipar.finda = finda;
      ipar.status = &clastatus;
      ipar.chipow = chi_exponent;
    }

    if(integration_method == ccl_integration_qag_quad) {
      if (local_status == 0) {
	w = gsl_integration_workspace_alloc(cosmo->gsl_params.N_ITERATION);
	if (w == NULL) {
	  local_status = CCL_ERROR_MEMORY;
	}
      }

      if (local_status == 0) {
	// Set up integrating function
	F.function = &cov_integrand;
	F.params = &ipar;
      }
    }

    #pragma omp for schedule(dynamic)
    for (lind1=0; lind1 < nl1_out; ++lind1) {
      l1 = l1_out[lind1];
      ipar.l1 = l1;

      for (lind2=0; lind2 < nl2_out; ++lind2) {
        if (local_status == 0) {
          l2 = l2_out[lind2];
          clastatus = 0;
          ipar.l2 = l2;

          // Integrate
          if(integration_method == ccl_integration_qag_quad) {
            integ_cov_limber_qag_quad(cosmo, &F, chimin, chimax, w,
                                      &result, &eresult, &local_status);
          }
          else if(integration_method == ccl_integration_spline) {
            integ_cov_limber_spline(cosmo, &ipar, chimin, chimax,
                                    &result, &local_status);
          }
          else
            local_status = CCL_ERROR_NOT_IMPLEMENTED;

          if ((*ipar.status == 0) && (local_status == 0)) {
            cov_out[lind1+nl1_out*lind2] = result * prefactor_extra;
          }
          else {
            ccl_raise_gsl_warning(local_status, "ccl_cls.c: ccl_angular_cov_limber():");
            cov_out[lind1+nl1_out*lind2] = NAN;
            local_status = CCL_ERROR_INTEG;
          }
        }
      }
    }

    gsl_integration_workspace_free(w);

    if (local_status) {
      #pragma omp atomic write
        *status = local_status;
    }
    ccl_a_finder_free(finda);
  }

  if (*status) {
    ccl_cosmology_set_status_message(
      cosmo,
      "ccl_cls.c: ccl_angular_cov_limber(); integration error\n");
  }
}


// for license information, see the accompanying LICENSE file

#include <stdio.h>

#include <stdlib.h>

#include <math.h>

#include <assert.h>

// CBLAS, LAPACKE are f2c translated Netlib F77 reference routines.
// 'cblas.h' is widely supported, and is the f2c translated Netlib F77 refernce version.
// There are known issues with name mangling schemes regarding wrapped code and inter-language operability between Fortran and C.
//
// Netlib CBLAS / LAPACKE target
// #define LISE_LA_REF
// #include <lapacke.h>
// #include <cblas.h>
//
// IBM ESSL target
// #define LISE_LA_ESSL
// #include <essl.h>
//
// Intel MKL target
// #define LISE_LA_MKL
// #include <mkl.h>

#ifdef LISE_LA_MKL
#include <mkl.h>
double ddot(const int *, const double *, const int *, const double *, const int *);
void daxpy(const int *, const double *, const double *, const int *, double *, const int *);
void dgemv(const char *, const int *, const int *, const double *, const double *, const int *, const double *, const int *, const double *, double *, const int *);
void dcopy(const int *, const double *, const int *, double *, const int *);
void dgemm(const char *, const char *, const int *, const int *, const int *, const double *, const double *, const int *, const double *, const int *, const double *, double *, const int *);
void dgesdd(const char *, const int *, const int *, double *, const int *, double *, double *, const int *, double *, const int *, double *, const int *, int *, int *);
void dger(const int *, const int *, const double *, const double *, const int *, const double *, const int *, double *, const int *);
#endif

#ifdef LISE_LA_ESSL
#include <essl.h>
double ddot(int, const double *, int, const double *,int);
void daxpy(int,  double,  double *, int,  double *,  int);
void dgemv(const char *, int, int,  double, const void *, int, const  double *, int,  double,  double *, int);
void dcopy(int,  double *, int,  double *, int);
void dgemm(const char *, const char *, int, int, int,  double, const void *, int, const void *, int,  double, void *, int);
void dgesdd(const char *, int, int, void *, int, double *, void *, int, void *, int, double *, int, int *, int *);
void dger(int, int,  double, const  double *, int, const  double *, int, void *, int);
// void dger1(int, int,  double, const  double *, int, const  double *, int, void *, int);
#endif

#ifdef LISE_LA_REF
#include <lapacke.h>
#include <cblas.h>
double cblas_ddot(const int N, const double *X, const int incX, const double *Y, const int incY);
void cblas_daxpy(const int N, const double alpha, const double *X, const int incX, double *Y, const int incY);
void cblas_dgemv(CBLAS_LAYOUT layout, CBLAS_TRANSPOSE TransA, const int M, const int N, const double alpha, const double *A, const int lda, const double *X, const int incX, const double beta, double *Y, const int incY);
void cblas_dcopy(const int N, const double *X, const int incX, double *Y, const int incY);
void cblas_dgemm(CBLAS_LAYOUT layout, CBLAS_TRANSPOSE TransA, CBLAS_TRANSPOSE TransB, const int M, const int N, const int K, const double alpha, const double *A, const int lda, const double *B, const int ldb, const double beta, double *C, const int ldc);
void cblas_dger(CBLAS_LAYOUT layout, const int M, const int N, const double alpha, const double *X, const int incX, const double *Y, const int incY, double *A, const int lda);
lapack_int LAPACKE_dgesdd( int matrix_layout, char jobz, lapack_int m, lapack_int n, double* a, lapack_int lda, double* s, double* u, lapack_int ldu, double* vt, lapack_int ldvt );
#endif

double ddotII( int * , double * , int * , double * , int * ) ;
void daxpyII( int * , double * , double * , int * , double * , int * ) ;
int broyden_minB( double * v_in , double * v_in_old , double * f_old , double * b , double * v_out , const int n , int * it_out , const double alpha ) ;
int broyden_min( double * v_in , double * v_in_old , double * f_old , double * b_bra , double * b_ket , double * d , double * v_out , const int n , int * it_out , const double alpha ) ;
int broydenMod_min( double * v_in , double * v_in_old , double * f_old , double * b_bra , double * b_ket , double * d , double * v_out , const int n , int * it_out , const double alpha ) ;

void broyden_min_( double * v_in , double * v_in_old , double * f_old , double * b_bra , double * b_ket , double * d , double * v_out , int * n , int * it_out , double * alpha )

{
  broydenMod_min( v_in , v_in_old , f_old , b_bra , b_ket , d , v_out , * n , it_out , * alpha ) ;
}

void broyden_minb_( double * v_in , double * v_in_old , double * f_old , double * b_bra , double * v_out , int * n , int * it_out , double * alpha )

{

  broyden_minB( v_in , v_in_old , f_old , b_bra , v_out , * n , it_out , * alpha ) ;

}

int broyden_min( double * v_in , double * v_in_old , double * f_old , double * b_bra , double * b_ket , double * d , double * v_out , const int n , int * it_out , const double alpha )

{

  double * f , * dv , * df ;

  int i , ii , it1 ;

  int ione = 1 ;

  double norm , norm1 , norm2 ;

  int n1 , ishift ;

  int it ;

  it = * it_out ;

  n1 = n ;

  if( it == 0 )

    {

      * it_out = 1 ;

      for( i = 0 ; i < n ; ++i ) {

	v_in_old[ i ] = v_in[ i ] ;

	f_old[ i ] = alpha * ( v_in[ i ] - v_out[ i ] ) ;

	v_in[ i ]  -= f_old[ i ] ;

	v_out[ i ] = v_in[ i ] ;

      }

      return( 0 ) ;

    }

  assert( f = malloc( n * sizeof( double ) ) ) ;

  assert( df = malloc( n * sizeof( double ) ) ) ;

  assert( dv = malloc( n * sizeof( double ) ) ) ;

  it1 = it - 1 ;

  ishift = it1 * n ;

  norm = 0. ; /* || F || */

  norm1 = 0. ; /* || F_old || */

  for( i = 0 ; i < n ; i++ )

    {

      f[ i ] =  alpha * ( v_in[ i ] - v_out[ i ] ) ;

      norm += f[ i ] * f[ i ] ;

      norm1 += f_old[ i ] * f_old[ i ] ;

      dv[ i ] = v_in[ i ] - v_in_old[ i ] ;

      df[ i ] = f[ i ] - f_old[ i ] ;

      b_ket[ ishift + i ] = df[ i ] ;

      b_bra[ ishift + i ] = dv[ i ] ;

    }

  for( ii = 0 ; ii < it1 ; ++ii )

    {

#ifdef LISE_LA_MKL
      norm =  d[ ii ] * ddot( &n1 , df , &ione , b_bra + ii * n , &ione ) ;
      daxpy( &n1 , &norm , b_ket + ii * n , &ione , b_ket + ishift , &ione ) ;
      norm = d[ ii ] * ddot( &n1 , dv , &ione , b_ket + ii * n , &ione ) ;
      daxpy( &n1 , &norm , b_bra + ii * n , &ione , b_bra + ishift ,&ione ) ;
#endif
#ifdef LISE_LA_ESSL
      norm =  d[ ii ] * ddot( n1 , df , ione , b_bra + ii * n , ione ) ;
      daxpy( n1 , norm , b_ket + ii * n , ione , b_ket + ishift , ione ) ;
      norm = d[ ii ] * ddot( n1 , dv , ione , b_ket + ii * n , ione ) ;
      daxpy( n1 , norm , b_bra + ii * n , ione , b_bra + ishift ,ione ) ;
#endif
#ifdef LISE_LA_REF
//double cblas_ddot(const int N, const double *X, const int incX, const double *Y, const int incY);
//void cblas_daxpy(const int N, const double alpha, const double *X, const int incX, double *Y, const int incY);
      norm =  d[ ii ] * cblas_ddot( n1 , df , ione , b_bra + ii * n , ione ) ;
      cblas_daxpy( n1 , norm , b_ket + ii * n , ione , b_ket + ishift , ione ) ;
      norm = d[ ii ] * cblas_ddot( n1 , dv , ione , b_ket + ii * n , ione ) ;
      cblas_daxpy( n1 , norm , b_bra + ii * n , ione , b_bra + ishift ,ione ) ;
#endif

    }

#ifdef LISE_LA_MKL
  norm = ddot(  &n1 , dv , &ione , b_ket + ishift , &ione ) ;
#endif
#ifdef LISE_LA_ESSL
  norm = ddot(  n1 , dv , ione , b_ket + ishift , ione ) ;
#endif
#ifdef LISE_LA_REF
  norm = cblas_ddot(  n1 , dv , ione , b_ket + ishift , ione ) ;
#endif

  printf( "<dv|B|df> =%12.6e\n", norm ) ;

  if( fabs( norm ) < 1.e-15 ) return( 0 ) ;

  for( i = 0 ; i < n ; ++i )

    b_ket[ ishift + i ] = ( dv[ i ] - b_ket[ ishift + i ] ) / norm ;

#ifdef LISE_LA_MKL
  norm1 = sqrt( ddot( &n1 , b_ket + ishift , &ione , b_ket + ishift , &ione ) ) ;
  norm2 = sqrt( ddot( &n1 , b_bra + ishift , &ione , b_bra + ishift , &ione ) ) ;
#endif
#ifdef LISE_LA_ESSL
  norm1 = sqrt( ddot( n1 , b_ket + ishift , ione , b_ket + ishift , ione ) ) ;
  norm2 = sqrt( ddot( n1 , b_bra + ishift , ione , b_bra + ishift , ione ) ) ;
#endif
#ifdef LISE_LA_REF
  norm1 = sqrt( cblas_ddot( n1 , b_ket + ishift , ione , b_ket + ishift , ione ) ) ;
  norm2 = sqrt( cblas_ddot( n1 , b_bra + ishift , ione , b_bra + ishift , ione ) ) ;
#endif

  for( i = 0 ; i < n ; i++ )

    {

      b_ket[ ishift + i ] = b_ket[ ishift + i ] / norm1 ;

      b_bra[ ishift + i ] = b_bra[ ishift + i ] / norm2 ;

    }

  d[ it1 ] = norm1 * norm2 ;

  for( i = 0 ; i < n ; i++)
    {

      v_in_old[ i ] = v_in[ i ] ;

      v_in[ i ] -= f[ i ] ;

    }

  for( ii = 0 ; ii < it ; ii++ )

    {

#ifdef LISE_LA_MKL
      norm = - d[ ii ] * ddot( &n1 , b_bra + ii * n , &ione , f , &ione ) ;
      daxpy( &n1 , &norm , b_ket + ii * n , &ione , v_in , &ione ) ;
#endif
#ifdef LISE_LA_ESSL
      norm = - d[ ii ] * ddot( n1 , b_bra + ii * n , ione , f , ione ) ;
      daxpy( n1 , norm , b_ket + ii * n , ione , v_in , ione ) ;
#endif
#ifdef LISE_LA_REF
      norm = - d[ ii ] * cblas_ddot( n1 , b_bra + ii * n , ione , f , ione ) ;
      cblas_daxpy( n1 , norm , b_ket + ii * n , ione , v_in , ione ) ;
#endif

    }

  for( i = 0 ; i < n ; i++ )

    {

      f_old[ i ] = f[ i ] ;

      v_out[ i ] = v_in[ i ] ;

    }

  ( * it_out ) ++ ;

  free( f ) ; free( dv ) ; free( df ) ;

  return( 0 ) ;

}

int broyden_minB( double * v_in , double * v_in_old , double * f_old , double * b , double * v_out , const int n , int * it_out , const double alpha )

{

  double * f , * dv , * df , * bdf , * bdv ;

  int i , ii , it1 ;

  int ione = 1 ;

  double norm , norm1 , norm2 ;

  int n1 , ishift ;

  int it , j ;

  double * u , * vt , * s , * work , wk_[ 2 ] ;

  int lwork = -1 , info , * iwork ;

  it = * it_out ;

  printf( "Starting Broyden %d \n\n" , it ) ;

  n1 = n ;

  if( it == 0 )

    {

      * it_out = 1 ;

      for( i = 0 ; i < n ; ++i ) {

	v_in_old[ i ] = v_in[ i ] ;

	f_old[ i ] = alpha * ( v_in[ i ] - v_out[ i ] ) ;

	v_in[ i ]  -= f_old[ i ] ;

	v_out[ i ] = v_in[ i ] ;

	for( j = 0 ; j < n ; j++ )

	  b[ i + n * j ] = 0. ;

	b[ i + n * i ] = 1. ;

      }

      return( 0 ) ;

    }

  assert( f = malloc( n * sizeof( double ) ) ) ;

  assert( df = malloc( n * sizeof( double ) ) ) ;

  assert( dv = malloc( n * sizeof( double ) ) ) ;

  assert( bdv = malloc( n * sizeof( double ) ) ) ;

  assert( bdf = malloc( n * sizeof( double ) ) ) ;

  it1 = it - 1 ;

  ishift = it1 * n ;

  for( i = 0 ; i < n ; i++ )

    {

      f[ i ] =  alpha * ( v_in[ i ] - v_out[ i ] ) ;

      dv[ i ] = v_in[ i ] - v_in_old[ i ] ;

      df[ i ] = f[ i ] - f_old[ i ] ;

    }

  norm = 0. ;

  for( i = 0 ; i < n ; i ++ ) {

    bdv[ i ] = 0. ;

    bdf[ i ] = dv[ i ] ;

    for( j = 0 ; j < n ; j++ ){

      bdv[ i ] += b[ j + n * i ] * dv[ j ] ;

      bdf[ i ] -= b[ i + n * j ] * df[ j ] ;

    }

    norm += df[ i ] * bdv[ i ] ;

  }

  printf( "<dv|B|df> =%12.6e\n", norm ) ;

  norm = 1./norm ;

#ifdef LISE_LA_REF
  cblas_dger( CBLASColMajor, n1 , n1 , norm , bdf , ione , bdv , ione , b , n1 ) ;
#endif
#ifdef LISE_LA_ESSL
  dger( n1 , n1 , norm , bdf , ione , bdv , ione , b , n1 ) ;
#endif
#ifdef LISE_LA_MKL
// void dger(const int *, const int *, const double *, const double *, const int *, const double *, const int *, void *, const int *);
  dger( &n1 , &n1 , &norm , bdf , &ione , bdv , &ione , b , &n1 ) ;
#endif

  if( it > 5 ){

  /* add a SVD decomposition of matrix B */

    assert( s = malloc( n * sizeof( double ) ) ) ;

    assert( u = malloc( n * n  * sizeof( double ) ) ) ;

    assert( vt = malloc( n * n * sizeof( double ) ) ) ;

    assert( iwork = malloc( 8 * n * sizeof( double ) ) ) ;

#ifdef LISE_LA_MKL
    dgesdd( "A", &n1 , &n1 , b , &n1 , s , u , &n1 , vt , &n1 , wk_ , &lwork , iwork , &info ) ;
#endif
#ifdef LISE_LA_ESSL
    dgesdd( "A", n1 , n1 , b , n1 , s , u , n1 , vt , n1 , wk_ , lwork , iwork , &info ) ;
#endif
#ifdef LISE_LA_REF
    info = LAPACKE_dgesdd(LAPACK_COL_MAJOR, 'A', n1 , n1 , b , n1 , s , u , n1 , vt , n1 ) ;
#endif

    lwork = ( int ) wk_[ 0 ] ;

    assert( work = malloc( lwork * sizeof( double ) ) ) ;

#ifdef LISE_LA_ESSL
    dgesdd( "A" , n1 , n1 , b , n1 , s , u , n1 , vt , n1 , work , lwork , iwork , &info ) ;
#endif
#ifdef LISE_LA_REF
    info = LAPACKE_dgesdd(LAPACK_COL_MAJOR, 'A', n1 , n1 , b , n1 , s , u , n1 , vt , n1 ) ;
#endif
#ifdef LISE_LA_MKL
    dgesdd( "A", &n1 , &n1 , b , &n1 , s , u , &n1 , vt , &n1 , work , &lwork , iwork , &info ) ;
#endif

    free( work ) ; free( iwork ) ;

    for( i = 0 ; i < n ; i++ ){

      if( s[ i ] < 1.e-6 ) {

	printf( " s[ %d ] = %g \n" , i , s[ i ] ) ;

	s[ i ] = 0. ;

      }

      for( j = 0 ; j < n ; j++ )

	vt[ i + j * n ] *= s[ i ] ;

    }

    free( s ) ;

    norm = 0. ;

    norm1 = 1. ;

#ifdef LISE_LA_ESSL
    dgemm( "N" , "N" , n1 , n1 , n1 , norm1 , u , n1 , vt , n1 , norm , b , n1 ) ;
#endif
#ifdef LISE_LA_MKL
// void dgemm(const char *, const char *, const int *, const int *, const int *, const double *, const double *, const int *, const double *, const int *, const double *, double *, const int *);
    dgemm( "N" , "N" , &n1 , &n1 , &n1 , &norm1 , u , &n1 , vt , &n1 , &norm , b , &n1 ) ;
#endif
#ifdef LISE_LA_REF
    cblas_dgemm(CBLAS_COL_MAJOR,"N" , "N" , n1 , n1 , n1 , norm1 , u , n1 , vt , n1 , norm , b , n1);
#endif

    free( u ) ; free( vt ) ;

  }

#ifdef LISE_LA_ESSL
  dcopy( n1 , v_in , ione , v_in_old , ione ) ;
#endif
#ifdef LISE_LA_MKL
//void dcopy(const int *, const double *, const int *, double *, const int *);
  dcopy( &n1 , v_in , &ione , v_in_old , &ione ) ;
#endif
#ifdef LISE_LA_REF
  cblas_dcopy( n1 , v_in , ione , v_in_old , ione ) ;
#endif

  norm1 = -1. ;

  norm = 1. ;

#ifdef LISE_LA_MKL
//void dgemv(const char *, const int *, const int *, const double *, const double *, const int *, const double *, const int *, const double *, double *, const int *);
  dgemv( "N" , &n1 , &n1 , &norm1 , b , &n1 , f , &ione , &norm , v_in , &ione ) ;
#endif
#ifdef LISE_LA_ESSL
  dgemv( "N" , n1 , n1 , norm1 , b , n1 , f , ione , norm , v_in , ione ) ;
#endif
#ifdef LISE_LA_REF
  cblas_dgemv(CBLAS_COL_MAJOR, "N" , n1 , n1 , norm1 , b , n1 , f , ione , norm , v_in , ione ) ;
#endif

  for( i = 0 ; i < n ; i++ )

    {

      f_old[ i ] = f[ i ] ;

      v_out[ i ] = v_in[ i ] ;

    }

  ( * it_out ) ++ ;

  free( f ) ; free( dv ) ; free( df ) ; free( bdf ) ; free( bdv ) ;

  printf( "Done with Broyden \n\n" ) ;

  return( 0 ) ;

}

int broydenMod_min( double * v_in , double * v_in_old , double * f_old , double * b_bra , double * b_ket , double * d , double * v_out , const int n , int * it_out , const double alpha )
/*

Change in the formula. Use Eq. (10) in J.Chem.Phys. 134 (2011) 134109

 */

{

  double * f , * dv , * df ;

  int i , ii , it1 ;

  int ione = 1 ;

  double norm , norm1 , norm2 ;

  int n1 , ishift ;

  int it ;

  it = * it_out ;

  printf( "Starting Broyden %d \n\n" , it ) ;

  n1 = n ;

  if( it == 0 )

    {

      * it_out = 1 ;

      for( i = 0 ; i < n ; ++i ) {

	v_in_old[ i ] = v_in[ i ] ;

	f_old[ i ] = alpha * ( v_in[ i ] - v_out[ i ] ) ;

	v_in[ i ]  -= f_old[ i ] ;

	v_out[ i ] = v_in[ i ] ;

      }

      return( 0 ) ;

    }

  assert( f = malloc( n * sizeof( double ) ) ) ;

  assert( df = malloc( n * sizeof( double ) ) ) ;

  assert( dv = malloc( n * sizeof( double ) ) ) ;

  it1 = it - 1 ;

  ishift = it1 * n ;

  norm = 0. ; /* || F || */

  norm1 = 0. ; /* || F_old || */

  for( i = 0 ; i < n ; i++ )

    {

      f[ i ] =  alpha * ( v_in[ i ] - v_out[ i ] ) ;

      norm += f[ i ] * f[ i ] ;

      norm1 += f_old[ i ] * f_old[ i ] ;

      dv[ i ] = v_in[ i ] - v_in_old[ i ] ;

      df[ i ] = f[ i ] - f_old[ i ] ;

      b_ket[ ishift + i ] = df[ i ] ;

      b_bra[ ishift + i ] = df[ i ] ;

    }

  for( ii = 0 ; ii < it1 ; ++ii )

    {

#ifdef LISE_LA_MKL
      norm =  d[ ii ] * ddot( &n1 , df , &ione , b_bra + ii * n , &ione ) ;
      daxpy( &n1 , &norm , b_ket + ii * n , &ione , b_ket + ishift , &ione ) ;
#endif
#ifdef LISE_LA_ESSL
      norm =  d[ ii ] * ddot( n1 , df , ione , b_bra + ii * n , ione ) ;
      daxpy( n1 , norm , b_ket + ii * n , ione , b_ket + ishift , ione ) ;
#endif
#ifdef LISE_LA_REF
      norm =  d[ ii ] * cblas_ddot( n1 , df , ione , b_bra + ii * n , ione ) ;
      cblas_daxpy( n1 , norm , b_ket + ii * n , ione , b_ket + ishift , ione ) ;
#endif

    }

#ifdef LISE_LA_MKL
  norm = ddot( &n1 , df , &ione , df , &ione ) ;
#endif
#ifdef LISE_LA_ESSL
  norm = ddot( n1 , df , ione , df , ione ) ;
#endif
#ifdef LISE_LA_REF
  norm = cblas_ddot( n1 , df , ione , df , ione ) ;
#endif

  printf( "<df|df> =%12.6e\n", norm ) ;

  for( i = 0 ; i < n ; ++i )

    b_ket[ ishift + i ] = ( dv[ i ] - b_ket[ ishift + i ] ) / norm ;

#ifdef LISE_LA_MKL
  norm1 = sqrt( ddot( &n1 , b_ket + ishift , &ione , b_ket + ishift , &ione ) ) ;
  norm2 = sqrt( ddot( &n1 , b_bra + ishift , &ione , b_bra + ishift ,&ione ) ) ;
#endif
#ifdef LISE_LA_ESSL
  norm1 = sqrt( ddot( n1 , b_ket + ishift , ione , b_ket + ishift , ione ) ) ;
  norm2 = sqrt( ddot( n1 , b_bra + ishift , ione , b_bra + ishift ,ione ) ) ;
#endif
#ifdef LISE_LA_REF
  norm1 = sqrt( cblas_ddot( n1 , b_ket + ishift , ione , b_ket + ishift , ione ) ) ;
  norm2 = sqrt( cblas_ddot( n1 , b_bra + ishift , ione , b_bra + ishift ,ione ) ) ;
#endif

  for( i = 0 ; i < n ; i++ )

    {

      b_ket[ ishift + i ] = b_ket[ ishift + i ] / norm1 ;

      b_bra[ ishift + i ] = b_bra[ ishift + i ] / norm2 ;

    }

  d[ it1 ] = norm1 * norm2 ;

  for( i = 0 ; i < n ; i++)
    {

      v_in_old[ i ] = v_in[ i ] ;

      v_in[ i ] -= f[ i ] ;

    }

  for( ii = 0 ; ii < it ; ii++ )

    {

#ifdef LISE_LA_MKL
      norm = - d[ ii ] * ddot( &n1 , b_bra + ii * n , &ione , f , &ione ) ;
      daxpy( &n1 , &norm , b_ket + ii * n , &ione , v_in , &ione ) ;
#endif
#ifdef LISE_LA_ESSL
      norm = - d[ ii ] * ddot( n1 , b_bra + ii * n , ione , f , ione ) ;
      daxpy( n1 , norm , b_ket + ii * n , ione , v_in , ione ) ;
#endif
#ifdef LISE_LA_REF
      norm = - d[ ii ] * cblas_ddot( n1 , b_bra + ii * n , ione , f , ione ) ;
      cblas_daxpy( n1 , norm , b_ket + ii * n , ione , v_in , ione ) ;
#endif

    }

  for( i = 0 ; i < n ; i++ )

    {

      f_old[ i ] = f[ i ] ;

      v_out[ i ] = v_in[ i ] ;

    }

  ( * it_out ) ++ ;

  free( f ) ; free( dv ) ; free( df ) ;

  printf( "Done with Broyden \n\n" ) ;

  return( 0 ) ;

}

double ddotII( int * n , double *v1 , int * i1 , double * v2 , int * i2 )

{

  double sum = 0. ;

  int i ;

  for( i = 0 ; i < * n ; i++ )

    sum += v1[ i ] * v2[ i ] ;

  return( sum ) ;

}

void daxpyII( int * n , double * alpha , double * x , int * i1 , double * y , int * i2 )

{

  int i ;

  for( i = 0 ; i < * n ; i++ )

    y[ i ] += *alpha * x[ i ] ;

}


/*--------------------------------------------------------------------------------------------------
  FUNCTION: ssblock_lu

  DESCRIPTION:
--------------------------------------------------------------------------------------------------
*/

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

#ifdef OPENBLAS
#include <cblas.h>
#elif MKL
#include <mkl.h>
#endif

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

extern void getlvl2minors_lu_(int*, const int*, double*, int*, double*, int*);

void ssblock_lu(double *csc, int no, const int nv1, const int nv2, const int ns1, const int ns2, double *wf1, double *wf2, double *l1minor, double *ss, const int print_level)
{
	int ld_csc;
        int ld_msum, ld_tmp;
        int ld_l2minors;
	int tmp_cols;
        int i, o1, o2;
        int pos_tmp, pos_msum;
        int st1, st2;
	int num_threads;
	double *msum = NULL;  //Auxiliary matrix to store computed determinants
	double *tmp = NULL;  // Auxiliary matrix for intermediate results
	double *l2minors = NULL;
	double zero = 0.0, one = 1.0, neg_one = -1.0;
	int    one_i = 1;
	double coef;
	double ss_local;
	int v1;

        /* Timing routines */
        double start, finish;
        double time00, time0;
        double time_l2m, time_det, time_sum, time_tot;

	/* Auxiliary variables */
	int ndevices;
	int devices[10];

	if (print_level >= 2) {
	    time00 = omp_get_wtime();
            printf("\n         ---- start ssblock subroutine ----\n");
	}

	/* Set times to 0 */
	time_l2m = 0.0;
	time_det = 0.0;
	time_sum = 0.0;

        /* Set leading dimensions */
        ld_csc =  no + nv1;
        ld_l2minors = no*no*no;
	ld_tmp = max(no, nv1);
	tmp_cols = nv2;
	ld_msum = nv1;

        /* Allocate arrays */
        msum = (double*) malloc (ld_msum * nv2 * sizeof(double));
        tmp = (double*) malloc(ld_tmp * tmp_cols * ns2 * sizeof(double));
	l2minors = (double*) malloc(ld_l2minors * no * sizeof(double));

#ifdef OPENBLAS
	num_threads = openblas_get_num_threads();
#elif MKL
	num_threads = mkl_get_max_threads();
#endif

	/* Compute the determinants of all possible l2minors of the reference matrix. Pass through all combinations of rows and columns (r1, c1, r2, c2) to removed in order to form a l2minor */
	time0 = omp_get_wtime();
	getlvl2minors_lu_(&no, &nv2, csc, &ld_csc, l2minors, &ld_l2minors);
	time_l2m = omp_get_wtime() - time0;

        /* Scale lower csc matrix block with alternating array (1,-1,1,-1...) i.e. starting from the second
         * column, scale every second column with -1.
         * Arrays wf1 and wf2 are used as auxiliary arrays and are not referenced (unchaged on exit).
	 */
	cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, nv1, no/2, nv2, zero, wf1, nv1, wf2, nv2, neg_one, &csc[no+nv1 + no], 2*ld_csc);

	/* Scale ss matrix to zero */
        cblas_dscal(ns1*ns2, zero, ss, 1);

	/* Start iterate through the blocks */
	for( o1 = 0; o1 < no; o1++ ){
	    for( o2 = 0; o2 < no; o2++ ){

	/*	printf("[ssblock_lu] Computing block (%d, %d)\n", o1, o2); */

		/* Apply rows from the right of the ref matrix, cscmat positions (1:no, no+1:np+nv2)
		 * Compute first product l2minors * csc(1:no, no+1:no+nv2) */
        	time0 = omp_get_wtime();
		pos_tmp = o1*no*no + o2*no;
		ld_tmp = no;

        	cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, no, nv2, no, one, &l2minors[pos_tmp], ld_l2minors, &csc[no*ld_csc], ld_csc, zero, tmp, ld_tmp);

                pos_msum = o1*no*nv1 + o2*nv1;

		cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, nv1, nv2, no, pow(neg_one, o1+2), &csc[no], ld_csc, tmp, ld_tmp, zero, msum, ld_msum);

		/* Get l1minor determinants and apply permutation sign */
		coef = pow(neg_one, o1+o2) * l1minor[o2*no + o1];

		/* Get coefficients (no+1:no+nv1, no+1:no+nv2) into auxiliary storage msum */
		for( i = 0; i < nv2; i++ ){
			cblas_daxpy(nv1, coef, &csc[(no+i)*ld_csc+no], 1, &msum[i*ld_msum], 1);
		}
        	time_det += omp_get_wtime() - time0;

	        time0 = omp_get_wtime();
		/* Compute ss matrix  */
		for( v1 = 0; v1 < nv1; v1++ ){
			cblas_dgemv( CblasColMajor, CblasTrans, nv2, ns2, one, &wf2[o2*nv2], no*nv2, &msum[v1], nv1, zero, tmp, 1 );
			cblas_dger( CblasColMajor, ns1, ns2, one, &wf1[o1*nv1+v1], no*nv1, tmp, one, ss, ns1 );
		}
        	finish = omp_get_wtime();
		time_sum +=  omp_get_wtime() - time0;
	    }
	}

	/* Free allocated spaces */
	free(msum);
	free(tmp);
	free(l2minors);

	/* Print timings */
	if (print_level >= 2) {
            printf("\n         ssblock time:\n");
	    printf("             L2 minors                  - time (sec): %.4lf \n",  time_l2m);
	    printf("             Determinants from minors   - time (sec): %.4lf \n",  time_det);
	    printf("             Final sum                  - time (sec): %.4lf \n",  time_sum);
	    printf("                                                     --------------\n");
	    printf("             Total                      - time (sec): %.4lf \n",  time_tot);
            printf("         ---- end ssblock subroutine ----\n");
	}

}


/**
 *
 * @file testing_zcungesv.c
 *
 *  PLASMA testing routines
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @version 2.6.0
 * @author Emmanuel Agullo
 * @date 2010-11-15
 * @precisions mixed zc -> ds
 *
 **/
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>

#include <plasma.h>
#include <cblas.h>
#include <lapacke.h>
#include <core_blas.h>
#include "testing_zmain.h"

static int check_solution(int, int, PLASMA_Complex64_t*, int, PLASMA_Complex64_t*, PLASMA_Complex64_t*, int, double);

int testing_zcungesv(int argc, char **argv)
{
    /* Check for number of arguments*/
    if (argc != 4){
        USAGE("CUNGESV", "N LDA NRHS LDB",
              "   - N    : the size of the matrix\n"
              "   - LDA  : leading dimension of the matrix A\n"
              "   - NRHS : number of RHS\n"
              "   - LDB  : leading dimension of the RHS B\n");
        return -1;
    }

    int N     = atoi(argv[0]);
    int LDA   = atoi(argv[1]);
    int NRHS  = atoi(argv[2]);
    int LDB   = atoi(argv[3]);
    int ITER;

    double eps;
    int info, info_solution;
    int i,j;
    int LDAxN    = LDA*N;
    int LDBxNRHS = LDB*NRHS;

    PLASMA_Complex64_t *A1 = (PLASMA_Complex64_t *)malloc(LDA*N   *sizeof(PLASMA_Complex64_t));
    PLASMA_Complex64_t *A2 = (PLASMA_Complex64_t *)malloc(LDA*N   *sizeof(PLASMA_Complex64_t));
    PLASMA_Complex64_t *B1 = (PLASMA_Complex64_t *)malloc(LDB*NRHS*sizeof(PLASMA_Complex64_t));
    PLASMA_Complex64_t *B2 = (PLASMA_Complex64_t *)malloc(LDB*NRHS*sizeof(PLASMA_Complex64_t));

    /* Check if unable to allocate memory */
    if ( (!A1) || (!A2) || (!B1) || (!B2) ) {
        printf("Out of Memory \n ");
        exit(0);
    }

    eps = LAPACKE_dlamch_work('e');

    /*----------------------------------------------------------
    *  TESTING ZCGELS
    */

    /* Initialize A1 and A2 */
    LAPACKE_zlarnv_work(IONE, ISEED, LDAxN, A1);
    for (i = 0; i < N; i++)
        for (j = 0; j < N; j++)
            A2[LDA*j+i] = A1[LDA*j+i] ;

    /* Initialize B1 and B2 */
    LAPACKE_zlarnv_work(IONE, ISEED, LDBxNRHS, B1);
    for (i = 0; i < N; i++)
        for (j = 0; j < NRHS; j++)
             B2[LDB*j+i] = B1[LDB*j+i] ;

    printf("\n");
    printf("------ TESTS FOR PLASMA ZCUNGESV ROUTINE -------  \n");
    printf("            Size of the Matrix %d by %d\n", N, N);
    printf("\n");
    printf(" The matrix A is randomly generated for each test.\n");
    printf("============\n");
    printf(" The relative machine precision (eps) is to be %e \n",eps);
    printf(" Computational tests pass if scaled residuals are less than 60.\n");

    /* PLASMA ZCUNGESV */
    info = PLASMA_zcungesv(PlasmaNoTrans, N, NRHS, A2, LDA, B1, LDB, B2, LDB, &ITER);
    if (info != PLASMA_SUCCESS ) {
        printf("PLASMA_zcposv is not completed: info = %d\n", info);
        info_solution = 1;
    } else {
        printf(" Solution obtained with %d iterations\n", ITER);

        /* Check the orthogonality, factorization and the solution */
        info_solution = check_solution(N, NRHS, A1, LDA, B1, B2, LDB, eps);
    }

    if (info_solution == 0) {
        printf("***************************************************\n");
        printf(" ---- TESTING ZCUNGESV.................... PASSED !\n");
        printf("***************************************************\n");
    }
    else {
        printf("************************************************\n");
        printf(" - TESTING ZCUNGESV .. FAILED !\n");
        printf("************************************************\n");
    }

    free(A1); free(A2); free(B1);  free(B2);

    return 0;
}

/*--------------------------------------------------------------
 * Check the solution
 */

static int check_solution(int N, int NRHS, PLASMA_Complex64_t *A1, int LDA, PLASMA_Complex64_t *B1, PLASMA_Complex64_t *B2, int LDB, double eps )
{
    int info_solution;
    double Rnorm, Anorm, Xnorm, Bnorm, result;
    PLASMA_Complex64_t alpha, beta;
    double *work = (double *)malloc(N*sizeof(double));

    alpha = 1.0;
    beta  = -1.0;

    Anorm = LAPACKE_zlange_work(LAPACK_COL_MAJOR, lapack_const(PlasmaInfNorm), N, N,    A1, LDA, work);
    Xnorm = LAPACKE_zlange_work(LAPACK_COL_MAJOR, lapack_const(PlasmaInfNorm), N, NRHS, B2, LDB, work);
    Bnorm = LAPACKE_zlange_work(LAPACK_COL_MAJOR, lapack_const(PlasmaInfNorm), N, NRHS, B1, LDB, work);

    cblas_zgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, N, NRHS, N, CBLAS_SADDR(alpha), A1, LDA, B2, LDB, CBLAS_SADDR(beta), B1, LDB);
    Rnorm = LAPACKE_zlange_work(LAPACK_COL_MAJOR, lapack_const(PlasmaInfNorm), N, NRHS, B1, LDB, work);

    if (getenv("PLASMA_TESTING_VERBOSE"))
      printf( "||A||_oo=%f\n||X||_oo=%f\n||B||_oo=%f\n||A X - B||_oo=%e\n", Anorm, Xnorm, Bnorm, Rnorm );

    result = Rnorm / ( (Anorm*Xnorm+Bnorm)*N*eps ) ;
    printf("============\n");
    printf("Checking the Residual of the solution \n");
    printf("-- ||Ax-B||_oo/((||A||_oo||x||_oo+||B||_oo).N.eps) = %e \n", result);

    if (  isnan(Xnorm) || isinf(Xnorm) || isnan(result) || isinf(result) || (result > 60.0) ) {
        printf("-- The solution is suspicious ! \n");
        info_solution = 1;
     }
    else{
        printf("-- The solution is CORRECT ! \n");
        info_solution = 0;
    }

    free(work);

    return info_solution;
}


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <math.h>
#include <float.h>
#include "parmt_postProcess.h"
#include "beachball.h"
#ifdef PARMT_USE_INTEL
#include <mkl_cblas.h>
#else
#include <cblas.h>
#endif
#include "compearth.h"

static void argsort3(const double *__restrict__ x, int *__restrict__ iperm);
static void sortUL(const double *__restrict__ lam,
                   const double *__restrict__ U,
                   double *__restrict__ lamSort,
                   double *__restrict__ Usort);
static void eigenvector2PrincipalAxis(const double eig,
                                      const double *__restrict__ ev,
                                      double *__restrict__ paxis);
static void vecstrd(const double str, const double dip, double r[3]);
static double dot3(const double *__restrict__ x,
                   const double *__restrict__ y);
static void gemv3(const double *__restrict__ A, const double *__restrict__ x,
                  double *__restrict__ y);
static void crossp(const double *__restrict__ u,
                   const double *__restrict__ v,
                   double *__restrict__ n);
static void strdvec(double r[3], double *str, double *dip);

int postprocess_tnp2beachballPolarity(const int nxp,
                                      const double xc, const double yc,
                                      const double rad,
                                      const double *__restrict__ pAxis,
                                      const double *__restrict__ nAxis,
                                      const double *__restrict__ tAxis,
                                      double *__restrict__ xw1,
                                      double *__restrict__ yw1,
                                      int8_t *__restrict__ pn1)
{
    double ax1[3] __attribute__ ((aligned(64)));
    double ax2[3] __attribute__ ((aligned(64)));
    double ax3[3] __attribute__ ((aligned(64)));
    double rm[9] __attribute__ ((aligned(64)));
    double rmt[9] __attribute__ ((aligned(64)));
    double tm[9] __attribute__ ((aligned(64)));
    double pt[3] __attribute__ ((aligned(64)));
    double ptnew[3] __attribute__ ((aligned(64)));
    double ampMax, az, baz, baz1, bdp, bdp1, bev, bevI, dp,
           dx, dxc, dy, dyc, evI,
           fclvd, fIso, paz, paz1, pdp, pdp1, pev, pevI, plusMin,
           rad2, rpt, rpt2,
           taz, taz1, tdp, tdp1, tev, tevI, theta, x, x0, y, y0;
    int ix, iy, k, nx, ny;
    const double degradi = 180.0/M_PI;
    const double third = 1.0/3.0;
    const double sqrt2 = 1.4142135623730951;
    const double sqrt2i = 1.0/sqrt2;
    taz1 = tAxis[0];
    tdp1 = tAxis[1];
    tevI = tAxis[2];
    baz1 = nAxis[0];
    bdp1 = nAxis[1];
    bevI = nAxis[2];
    paz1 = pAxis[0];
    pdp1 = pAxis[1];
    pevI = pAxis[2];
    // Avoid overwriting memory
    taz = taz1;
    tdp = tdp1;
    baz = baz1;
    bdp = bdp1;
    paz = paz1;
    pdp = pdp1;
    // Step 1: set up axes:
    //  - D axis ('dominant' 1-axis - with largest absolute eigenvalue)
    //  - M axis ('minor'    3-axis)
    //  - B axis (           2-axis)
    evI = (tevI + bevI + pevI)*third;
    tev = tevI - evI;
    bev = bevI - evI;
    pev = pevI - evI;
    vecstrd(paz, pdp, ax1);
    vecstrd(taz, tdp, ax3);
    fclvd = fabs(bev/pev);
    fIso = evI/pev;
    if (fabs(tev) > fabs(pev))
    {
        vecstrd(taz, tdp, ax1);
        vecstrd(paz, pdp, ax3);
        fclvd = fabs(bev/tev);
        fIso = evI/tev;
    }
    //if (fabs(evI) > .03)
    //{
    //    write(6,901) evI
    //}
    crossp(ax1, ax3, ax2);
    strdvec(ax2, &baz, &bdp);
    cliffsNodes_mtensor(taz, tdp, paz, pdp, tevI, bevI, pevI,
                        tm, rm, rmt, &ampMax);
    nx = nxp;
    ny = nxp;
    memset(xw1, 0, (size_t) (nx*ny)*sizeof(double));
    memset(yw1, 0, (size_t) (nx*ny)*sizeof(double));
    memset(pn1, 0, (size_t) (nx*ny)*sizeof(int8_t));
    dx = 2.0*rad/(double) (nx - 1);
    dy = 2.0*rad/(double) (ny - 1);
    x0 = xc - rad;
    y0 = yc - rad;
    rad2 = rad*rad; // saves a sqrt computation when comparing distance
    // loop on pixel grid
    for (iy=0; iy<ny; iy++)
    {
        for (ix=0; ix<nx; ix++)
        {
            x = x0 + (double) ix*dx;
            y = y0 + (double) iy*dy;
            k = iy*nx + ix;
            pn1[k] = 0;
            xw1[k] = x;
            yw1[k] = y;
            // require (x,y) be in the focal sphere
            dxc = x - xc;
            dyc = y - yc;
            rpt2 = dxc*dxc + dyc*dyc;
            if (rpt2 < rad2)
            {
                rpt = sqrt(rpt2);
                // compute the azimuth
                theta = atan2(y - yc, x - xc);  // measure from x axis
                az = (M_PI_2 - theta)*degradi;  // az is measured from north
                if (az < 0.0){az = az + 360.0;} // convention is [0,360]
                // compute the dip
                dp = (M_PI_2 - 2.0*asin(rpt*sqrt2i))*degradi;
                vecstrd(az, dp, pt);
                gemv3(tm, pt, ptnew);
                plusMin = dot3(pt, ptnew);
                pn1[k] =-1;
                if (plusMin > 100.0*DBL_EPSILON*ampMax){pn1[k] = 1;}
            } // end check on location
        } // loop on x
    } // loop on y
    return 0;
}
/*!
 * @brief Converts a Tape and Tape 2015 tensor to tension, null, and
 *        plunge eigenvectors in USE coordinates for plotting with
 *        cliffsnodes
 * @param[in] beta    colatitude (radians)
 * @param[in] gamma   longitude (radians)
 * @param[in] kappa   strike angle (radians)
 * @param[in] sigma   slip angle (radians)
 * @param[in] theta   dip angle (radians)
 *
 * @param[out] pAxis  (azimuth, plunge, eigenvalue) of pressure axis [3]
 * @param[out] nAxis  (azimuth, plunge, eigenvalue) of null axis [3]
 * @param[out] tAxis  (azimuth, plunge, eigenvalue) of tension axis [3]
 *
 * @result 0 indicate success
 *
 * @author Ben Baker
 *
 * @copyright ISTI distributed under Apache 2
 *
 */
int postprocess_tt2tnp(const double beta, const double gamma,
                       const double kappa, const double sigma,
                       const double theta,
                       double *__restrict__ pAxis,
                       double *__restrict__ nAxis,
                       double *__restrict__ tAxis)
{
    const double R[9] = {
             0.7071067811865476, 0.0,                -0.7071067811865476,
            -0.4082482904638631, 0.8164965809277261, -0.4082482904638631,
             0.5773502691896258, 0.5773502691896258,  0.5773502691896258};
    const double Yrot[9] = {
             0.7071067811865475, 0.0, 0.7071067811865475,
             0.0, 1.0, 0.0,
            -0.7071067811865475, 0.0, 0.7071067811865475};
    // Convert NWU to USE
    const double Rot[9] = {0, -1, 0, 0, 0, -1, 1, 0, 0};
    //const double RotInv[9] = {0, 0, 1, -1, 0, 0, 0, -1, 0};
    double Zsigma[9], Xtheta[9], Zkappa[9], ZX[9], V[9],
           U[9], Uuse[9], Usort[9], Uw[9],
           lambda[3], v3[3], lamSort[3],
           cosb, cosg, sinb, sing, nKappaDeg, sigmaDeg, thetaDeg;
    const double toDeg = 180.0/M_PI;
    // Compute the eigenvalues from Eqn 7
    sinb = sin(beta);
    sing = sin(gamma);
    cosb = cos(beta);
    cosg = cos(gamma);
    v3[0] = sinb*cosg;
    v3[1] = sinb*sing;
    v3[2] = cosb;
    // Compute R*lam
    lambda[0] = R[0]*v3[0] + R[3]*v3[1] + R[6]*v3[2];
    lambda[1] = R[1]*v3[0] + R[4]*v3[1] + R[7]*v3[2];
    lambda[2] = R[2]*v3[0] + R[5]*v3[1] + R[8]*v3[2];
    // Eqn 9-10
    sigmaDeg  = sigma*toDeg;
    thetaDeg  = theta*toDeg;
    nKappaDeg =-kappa*toDeg;
    compearth_eulerUtil_rotmat(1, &sigmaDeg, 3, Zsigma);
    compearth_eulerUtil_rotmat(1, &thetaDeg, 1, Xtheta);
    compearth_eulerUtil_rotmat(1, &nKappaDeg, 3, Zkappa);
    //compearth_eulerUtil_rotmatGen(1,-45.0, 2, Yrot);
    cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans,
                3, 3, 3, 1.0, Zkappa, 3, Xtheta, 3, 0.0, ZX, 3);
    cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans,
                3, 3, 3, 1.0, ZX, 3, Zsigma, 3, 0.0, V, 3);
    cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans,
                3, 3, 3, 1.0, V, 3, Yrot, 3, 0.0, U, 3);
    // M could be computed by computing M = U*Lambda*inv(U) hence
    // U corresponds to the eigenvectors and lambda the scaling.
    // However, the current basis is NWU which must be converted to USE.
    // If performing a proper rotation of M to M_{use} we would compute:
    //   R M inv(R) = R U Lambda inv(U) inv(R)
    // thus what we must be do is transform the eigenvectors into the
    // apropriate frame.  Intuitively it is the rotation of each
    // eigenvector (column) of the U matrix which is indeed the matrix
    // matrix multiplication of R*U.
    cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans,
                3, 3, 3, 1.0, Rot, 3, U, 3, 0.0, Uuse, 3);
    // If I want to finish this I could compute:
    //   R U Lambda inv(U) inv(R) = R U Lambda U^T R^T
/*
    double lamMat[9], Temp[9], M[9], UtRt[9];
    memset(lamMat, 0, 9*sizeof(double));
    lamMat[0] = lambda[0]; lamMat[4] = lambda[1]; lamMat[8] = lambda[2];
    cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans,
                3, 3, 3, 1.0, Uuse, 3, lamMat, 3, 0.0, Temp, 3);
    cblas_dgemm(CblasColMajor, CblasTrans, CblasTrans,
                3, 3, 3, 1.0, U, 3, Rot, 3, 0.0, UtRt, 3);
    cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans,
                3, 3, 3, 1.0, Temp, 3, UtRt, 3, 0.0, M, 3);
    for (int i=0; i<3; i++)
    {
        for (int j=0; j<3; j++)
        {
            printf("%f ", M[3*j+i]);
        }
        printf("\n");
     }
*/
    sortUL(lambda, Uuse, lamSort, Usort);
//printf("%f %f %f\n", lamSort[0], lamSort[1], lamSort[2]);
    // Compute pressure, null, and tension axes
    eigenvector2PrincipalAxis(lamSort[0], &Usort[0], pAxis);
    eigenvector2PrincipalAxis(lamSort[1], &Usort[3], nAxis);
    eigenvector2PrincipalAxis(lamSort[2], &Usort[6], tAxis);
    // Explosion - simplification (all positive eigenvalues)
    if (lamSort[0] > 0.0)
    {
        pAxis[2] = 1.0;
        nAxis[2] = 1.0;
        tAxis[2] = 1.0;
    }
    // Implosion - simplification (all negative eigenvalues)
    if (lamSort[2] < 0.0)
    {
        pAxis[0] =-1.0;
        nAxis[1] =-1.0;
        tAxis[2] =-1.0;
    }
    //printf("%f %f %f\n", pAxis[0], pAxis[1], pAxis[2]);
    //printf("%f %f %f\n", nAxis[0], nAxis[1], nAxis[2]);
    //printf("%f %f %f\n", tAxis[0], tAxis[1], tAxis[2]);
    return 0;
}

/*!
 * @brief Convert pressure, null, and tension principal axes to azimuth,
 *        plunge, and length.
 */
static void eigenvector2PrincipalAxis(const double eig,
                                      const double *__restrict__ ev,
                                      double *__restrict__ paxis)
{
    const char *fcnm = "cmopad_Eigenvector2PrincipalAxis\0";
    double v[3], az, plunge;
    double twopi = 2.0*M_PI;
    double pi180i = 180.0/M_PI;
    int ierr;
    //------------------------------------------------------------------------//    //
    // Compute azimuth and plunge angles where ev is USE coordinates
    plunge = asin(-ev[0]);        //arcsin(-z/r)
    az     = atan2(ev[2],-ev[1]); //atan(x/y)
    if (plunge <= 0.0)
    {
        plunge =-plunge;
        az = az + M_PI;
    }
    if (az < 0.0){az = az + twopi;}   //Shift to [0,360]
    if (az > twopi){az = az - twopi;} //Shift to [0,360]
    // Convert to degrees
    plunge = plunge*pi180i;
    az = az*pi180i;
    // Copy back result
    paxis[0] = az;     //First value is azimuth (degrees)
    paxis[1] = plunge; //Second value is plunge (degrees)
    paxis[2] = eig;    //Final value is eigenvalue (distance)
    return;
}
//===========================================================================//
static void sortUL(const double *__restrict__ lam,
                   const double *__restrict__ U,
                   double *__restrict__ lamSort,
                   double *__restrict__ Usort)
{
    int perm[3], j;
    argsort3(lam, perm);
    lamSort[0] = lam[perm[0]];
    lamSort[1] = lam[perm[1]];
    lamSort[2] = lam[perm[2]];
    for (j=0; j<3; j++)
    {
        Usort[3*j+0] = U[3*perm[j]+0];
        Usort[3*j+1] = U[3*perm[j]+1];
        Usort[3*j+2] = U[3*perm[j]+2];
    }
    return;
}

//============================================================================//
/*!
 * @brief Permutation to sort length 3 array into ascending order
 */
static void argsort3(const double *__restrict__ x, int *__restrict__ iperm)
{
    const char *fcnm = "__cmopad_argsort3\0";
    int i, temp;
    const int a = 0;
    const int b = 1;
    const int c = 2;
    // Copy
    iperm[a] = a;
    iperm[b] = b;
    iperm[c] = c;
    if (x[iperm[a]] > x[iperm[c]])
    {
        temp = iperm[c];
        iperm[c] = iperm[a];
        iperm[a] = temp;
    }
    if (x[iperm[a]] > x[iperm[b]])
    {
        temp = iperm[b];
        iperm[b] = iperm[a];
        iperm[a] = temp;
    }
    //Now the smallest element is the first one. Just check the 2-nd and 3-rd
    if (x[iperm[b]] > x[iperm[c]])
    {
        temp = iperm[c];
        iperm[c] = iperm[b];
        iperm[b] = temp;
    }
    return;
}
//============================================================================//
/*!
 * @brief Find components of downward pointing unit vector pole from the
 *        strike and dip angles.  This is based on Cliff Frolich's vecstrd.
 *
 * @param[in] str    strike angle of pole (degrees)
 * @param[in] dip    dip angle of pole (degrees)
 *
 * @param[out] r     corresponding unit vector pole (3)
 *
 * @author Ben Baker
 *
 * @copyright BSD
 *
 */
static void vecstrd(const double str, const double dip, double r[3])
{
    double cosdip, cosstr, diprad, sindip, sinstr, strrad;
    const double degrad = M_PI/180.0;
    // convert to radians
    strrad = str*degrad;
    diprad = dip*degrad;
    // compute cosines and sines
    sinstr = sin(strrad);
    cosstr = cos(strrad);
    cosdip = cos(diprad);
    sindip = sin(diprad);
    // compute (x,y,z) from strike and dip for unit radius
    r[0] = sinstr*cosdip;
    r[1] = cosstr*cosdip;
    r[2] =-sindip;
    return;
}
/*!
 * @brief Utility function for computing dot product of length 3 vector
 */
static double dot3(const double *__restrict__ x,
                   const double *__restrict__ y)
{
    double dot;
    dot = x[0]*y[0] + x[1]*y[1] + x[2]*y[2];
    return dot;
}
/*!
 * @brief Utility function for computing y = Ax for 3 x 3 matrix.
 */
static void gemv3(const double *__restrict__ A, const double *__restrict__ x,
                  double *__restrict__ y)
{
    y[0] = A[0]*x[0] + A[3]*x[1] + A[6]*x[2];
    y[1] = A[1]*x[0] + A[4]*x[1] + A[7]*x[2];
    y[2] = A[2]*x[0] + A[5]*x[1] + A[8]*x[2];
    return;
}
/*!
 * @brief Cross product of 2 length 3 vectors u and v.  Returns normal vector n.
 *
 * @param[in] u    first vector in cross product u x v [3]
 * @param[in] v    second vector in cross product u x v [3]
 *
 * @param[out] n   normal vector n = u x v [3]
 *
 * @author Ben Baker
 *
 * @copyright BSD
 *
 */
static void crossp(const double *__restrict__ u,
                   const double *__restrict__ v,
                   double *__restrict__ n)
{
    n[0] = u[1]*v[2] - u[2]*v[1];
    n[1] = u[2]*v[0] - u[0]*v[2];
    n[2] = u[0]*v[1] - u[1]*v[0];
    return;
}
/*!
 * @brief Finds the strike and dip of downward pointing unit vector.  This
 *        is based on Cliff Frolich's strdvec.
 *
 * @param[in,out] r      on input contains the downward pointing unit
 *                       vector.
 *                       if r[2] is > 0 then the unit vector is pointing
 *                       updward.  in this case then on output r3 will
 *                       be negated.
 *
 * @param[out] str       strike angle (degrees) \f$ s.t. \phi \in [0,360] \f$.
 * @param[out] dip       dip angle (degrees)
 *
 * @author Ben Baker
 *
 * @copyright BSD
 *
 */
static void strdvec(double r[3], double *str, double *dip)
{
    double rs;
    const double degradi = 180.0/M_PI;
    if (r[2] > 0.0)
    {
        r[0] =-r[0];
        r[1] =-r[1];
        r[2] =-r[2];
    }
    *str = atan2(r[0], r[1])*degradi;
    if (*str < 0.){*str = *str + 360.;}
    rs = sqrt(r[0]*r[0] + r[1]*r[1]);
    *dip=atan2(-r[2], rs)*degradi;
    return;
}


/*
 * SPDX-License-Identifier:  BSD-3-Clause
 *
 * example_10-StructOfArrays-CellLinkedList-OuterLoop-SymmetricalLoadBalancing.c :
 *      Example of SPH Density Calculation using
 *      fast neighbor search the main density loop via
 *      Cell Linked List method, Struct of Arrays (SoA)
 *      data layout, OpenMP parallelization at the
 *      cell-pair level, SIMD directives in the kernel
 *      and in the inner-most loop. It also implements
 *      symmetrical load balancing by moving the parallelism
 *      from iterating over cells to iterate over
 *      unique pairs of cell (i.e. i<j) and recyling the
 *      intermediary calculations.
 *
 * (C) Copyright 2021 José Hugo Elsas
 * Author: José Hugo Elsas <jhelsas@gmail.com>
 *
 * Command Line Options:
 *   -runs  <int>   : Set the number of repetitions (runs) for
 *                      calculating the density. The value of
 *                      the density is based on the last
 *                      iteration.
 *                    Default value: 1
 *   -run_seed <int>: Flag to set an alternative seed use for
 *                      for the PRNG. Instead of feeding seed
 *                      to the PRNG directly, it feeds
 *                      seed + iteration, as to generate different
 *                      configurations for each iteration.
 *                    Default value: 0 - (possible 0/1)
 *   -seed     <int>: Set the seed to use for the SPH particles
 *                      uniform position generation in the box
 *                    Default value: 123123123
 *
 *   -N        <int>: Set the number of SPH particles to be used
 *                    Default value: 1e5 = 100,000
 *   -h      <float>: Set the value of the smoothing kernel
 *                      parameter h, which corresponds to half
 *                      of the support of the kernel.
 *                    Default value: 0.05
 *
 *   -Nx       <int>: Set the number of Cells in the X direction
 *                    Default value: 10
 *   -Ny       <int>: Set the number of Cells in the Y direction
 *                    Default value: 10
 *   -Nz       <int>: Set the number of Cells in the Z direction
 *                    Default value: 10
 *
 *   -Xmin   <float>: Set the lower bound in the X direction for
 *                      the Cell Linked List box
 *                    Default value: 0.0
 *   -Ymin   <float>: Set the lower bound in the Y direction for
 *                    the Cell Linked List box
 *                    Default value: 0.0
 *   -Ymin   <float>: Set the lower bound in the Z direction for
 *                      the Cell Linked List box
 *                    Default value: 0.0
 *
 *   -Xmax   <float>: Set the lower bound in the X direction for
 *                      the Cell Linked List box
 *                    Default value: 1.0
 *   -Ymax   <float>: Set the lower bound in the Y direction for
 *                      the Cell Linked List box
 *                    Default value: 1.0
 *   -Zmax   <float>: Set the lower bound in the Z direction for
 *                      the Cell Linked List box
 *                    Default value: 1.0
 */

#include <math.h>
#include <ctype.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <limits.h>
#include <unistd.h>
#include <stdbool.h>
#include <sys/time.h>
#include <inttypes.h>

#include <omp.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_heapsort.h>

#include "sph_data_types.h"
#include "sph_linked_list.h"
#include "sph_utils.h"

#ifndef M_PI
#define M_PI (3.14159265358979323846)
#endif

#define COMPUTE_BLOCKS 5

#define dbg false

int main_loop(int run, bool run_seed, int64_t N, double h, long int seed,
              void *swap_arr,int64_t *temp_hash, linkedListBox *box, SPHparticle *lsph, double *times);

int compute_density_3d_symmetrical_load_ballance(int N, double h, SPHparticle *lsph, linkedListBox *box);

int compute_density_3d_chunk_symmetrical(int64_t node_begin, int64_t node_end,
                                         int64_t nb_begin, int64_t nb_end,double h,
                                         double* restrict x, double* restrict y,
                                         double* restrict z, double* restrict nu,
                                         double* restrict rhoi, double* restrict rhoj);

//void quickSort_int64_t2(int64_t *arr, int64_t low, int64_t high);

void quickSort_int64_t_2(int64_t *arr, int64_t low, int64_t high);

void mergeSort(int64_t arr[], int64_t l, int64_t r);

double w_bspline_3d_constant(double h);

#pragma omp declare simd
double w_bspline_3d_simd(double q);

#define ip_swap(a,b) (b)^=((a)^=((b)^=(a)))

KHASH_MAP_INIT_INT64(2, int64_t)

void counting_sort(int64_t N, linkedListBox *box, SPHparticle *lsph,void *swap_arr,int64_t *temp_hash);

int main(int argc, char **argv){
  bool run_seed = false;       // By default the behavior is is to use the same seed
  int runs = 1,err;            // it only runs once
  long int seed = 123123123;   // The default seed is 123123123
  int64_t N = 100000;          // The default number of particles is N = 1e5 = 100,000
  double h=0.05;               // The default kernel smoothing length is h = 0.05
  linkedListBox *box;          // Uninitialized Box containing the cells for the cell linked list method
  SPHparticle *lsph;           // Uninitialized array of SPH particles

  box = (linkedListBox*)malloc(1*sizeof(linkedListBox)); // Create a box representing the entire 3d domain

  // allow for command line customization of the run
  arg_parse(argc,argv,&N,&h,&seed,&runs,&run_seed,box);  // Parse the command line options
                                                         // line arguments and override default values

  err = SPHparticle_SoA_malloc(N,&lsph);
  if(err)
    fprintf(stderr,"error in SPHparticle_SoA_malloc\n");

  void *swap_arr = malloc(N*sizeof(double));
  int64_t *temp_hash = (int64_t*)malloc(2*N*sizeof(int64_t));
  double times[runs*COMPUTE_BLOCKS];

  for(int run=0;run<runs;run+=1)
    main_loop(run,run_seed,N,h,seed,swap_arr,temp_hash,box,lsph,times);

  bool is_cll = true;
  const char *prefix = "ex11,cll,SoA,outer,simd,symmLB,quicker";
  print_time_stats(prefix,is_cll,N,h,seed,runs,lsph,box,times);
  print_sph_particles_density(prefix,is_cll,N,h,seed,runs,lsph,box);

  SPHparticleSOA_safe_free(N,&lsph);
  safe_free_box(box);
  free(swap_arr);
  free(temp_hash);

  return 0;
}

/*
 *  Function main_loop:
 *    Runs the main loop of the program, including the particle array generation,
 *    density calculation and the timings annotations.
 *
 *    Arguments:
 *       run <int>            : index (or value) or the present iteration
 *       run_seed <bool>      : boolean defining whether to use run index for seed or not
 *       N <int>              : Number of SPH particles to be used in the run
 *       h <double>           : Smoothing Length for the Smoothing Kernel w_bspline
 *       seed <long int>      : seed for GSL PRNG generator to generate particle positions
 *       box  <linkedListBox> : Box of linked list cells, encapsulating the 3d domain
 *       lsph <SPHparticle>   : Array (pointer) of SPH particles to be updated
 *       times <double>       : Array to store the computation timings to be updated
 *    Returns:
 *       0                    : error code returned
 *       lsph <SPHparticle>   : SPH particle array is updated in the rho field by reference
 *       times <double>       : Times is updated by reference
 */

int main_loop(int run, bool run_seed, int64_t N, double h, long int seed,
              void *swap_arr, int64_t *temp_hash, linkedListBox *box, SPHparticle *lsph, double *times)
{
  int err;

  if(run_seed)
    err = gen_unif_rdn_pos_box(N,seed+run,box,lsph);
  else
    err = gen_unif_rdn_pos_box(N,seed,box,lsph);

  if(err)
    fprintf(stderr,"error in gen_unif_rdn_pos\n");

  // ------------------------------------------------------ //

  double t0,t1,t2,t3,t4,t5;

  t0 = omp_get_wtime();

  err = compute_hash_MC3D(N,lsph,box);                               // Compute Morton Z 3D hash based on the
  if(err)                                                            // cell index for each of the X, Y and Z
    fprintf(stderr,"error in compute_hash_MC3D\n");                  // directions, in which a given particle reside

  t1 = omp_get_wtime();

  counting_sort(N,box,lsph,swap_arr,temp_hash);

  t2 = omp_get_wtime();

  t3 = omp_get_wtime();

  t4 = omp_get_wtime();

  err = compute_density_3d_symmetrical_load_ballance(N,h,lsph,box);  // Compute the density of the particles based
  if(err)                                                            // on the cell linked list method for fast
    fprintf(stderr,"error in compute_density_3d_load_ballanced\n");  // neighbor search

  // --------------------------------------------------------------- //

  t5 = omp_get_wtime();

  kh_clear(0, box->hbegin);
  kh_clear(1, box->hend);

  times[COMPUTE_BLOCKS*run+0] = t1-t0;                               // Time for compute morton Z 3d hash
  times[COMPUTE_BLOCKS*run+1] = t2-t1;                               // Time for sorting the particles' hashes
  times[COMPUTE_BLOCKS*run+2] = t3-t2;                               // Time for reordering all other arrays accordingly
  times[COMPUTE_BLOCKS*run+3] = t4-t3;                               // Time for setting up the interval hash tables
  times[COMPUTE_BLOCKS*run+4] = t5-t4;                               // Time for computing the SPH particle densities

  return 0;
}

/*
 *  Function compute_density_3d_symmetrical_load_ballance:
 *    Computes the SPH density from the particles using cell linked list with
 *    vectorization at the compute_density_3d_chunk level, but the parallelization
 *    done at the level of the outer-most loop of the compute_density_3d_cll_outerOmp
 *    function, not at the chunk level.
 *
 *    The parallelization is done at the level of unique cell pair instead of cells,
 *    with the indexes for the cell pairs pre-computed before parallelization.
 *
 *    Arguments:
 *       N <int>              : Number of SPH particles to be used in the run
 *       h <double>           : Smoothing Length for the Smoothing Kernel w_bspline
 *       lsph <SPHparticle>   : Array (pointer) of SPH particles to be updated
 *    Returns:
 *       0                    : error code returned
 *       lsph <SPHparticle>   : SPH particle array is updated in the rho field by reference
 */
int compute_density_3d_symmetrical_load_ballance(int N, double h, SPHparticle *lsph, linkedListBox *box){
  int64_t *node_begin,*node_end,*nb_begin,*nb_end;                        // Define the arrays for cell boundaries
  int64_t max_cell_pair_count = 0;                                        // and the number of cell pairs
  const double kernel_constant = w_bspline_3d_constant(h);

  max_cell_pair_count = count_box_pairs(box);                             // compute the maximum number of cell pairs

  node_begin = (int64_t*)malloc(max_cell_pair_count*sizeof(int64_t));     // allocate space for node_begin
  node_end   = (int64_t*)malloc(max_cell_pair_count*sizeof(int64_t));     // allocate space for node_end
  nb_begin   = (int64_t*)malloc(max_cell_pair_count*sizeof(int64_t));     // allocate space for nb_begin
  nb_end     = (int64_t*)malloc(max_cell_pair_count*sizeof(int64_t));     // allocate space for nb_end

  max_cell_pair_count = setup_unique_box_pairs(box,                       // set the values for cell pairs
                                               node_begin,node_end,
                                               nb_begin,nb_end);

  memset(lsph->rho,(int)0,N*sizeof(double));                              // Pre-initialize the density to zero

                                                                          // Parallelism was moved
                                                                          // to the level of unique pairs
  #pragma omp parallel for schedule(dynamic,5) proc_bind(master)          // Execute in parallel
  for(size_t i=0;i<max_cell_pair_count;i+=1){                             // over the unique pairs' array
    double local_rhoi[node_end[i] - node_begin[i]];                       // partial density array for node indexs
    double local_rhoj[  nb_end[i] -   nb_begin[i]];                       // partial density array for nb   indexs

    memset(local_rhoi,(int)0,(node_end[i]-node_begin[i])*sizeof(double)); // initialize node partial density to zero
    memset(local_rhoj,(int)0,    (nb_end[i]-nb_begin[i])*sizeof(double)); // initialize nb partial density to zero

    compute_density_3d_chunk_symmetrical(node_begin[i],node_end[i],       // Compute the density contribution
                                         nb_begin[i],nb_end[i],h,         // from this particular cell pair
                                         lsph->x,lsph->y,lsph->z,         // for both node and nb partial density
                                         lsph->nu,local_rhoi,
                                         local_rhoj);

    // merging the results can result in race conditions, therefore needs to be serialized
    #pragma omp critical                                                  // this serializes this code section
    {

      for(size_t ii=node_begin[i];ii<node_end[i];ii+=1){                  // iterate over the node_ cell
        lsph->rho[ii] += kernel_constant*local_rhoi[ii-node_begin[i]];    // add the partial density contribution
      }

      if(node_begin[i] != nb_begin[i])                                    // if sender and receiver are different
        for(size_t ii=nb_begin[i];ii<nb_end[i];ii+=1){                    // iterate over the nb_ cell
          lsph->rho[ii] += kernel_constant*local_rhoj[ii-nb_begin[i]];    // add the partial density contribution
        }
    }
  }

  free(node_begin);
  free(node_end);
  free(nb_begin);
  free(nb_end);

  return 0;
}

/*
 *  Function compute_density_3d_chunk_symmetrical:
 *    Computes the SPH density contribution to both the node_ cell and the nb_ cell.
 *    Vectorization in the inner-most loop, but no parallelization.
 *    The density contribution is symmetrical.
 *
 *    Arguments:
 *       N <int>              : Number of SPH particles to be used in the run
 *       h <double>           : Smoothing Length for the Smoothing Kernel w_bspline
 *       x       <double*>    : Array of particles' X positions
 *       y       <double*>    : Array of particles' Y positions
 *       z       <double*>    : Array of particles' Z positions
 *       nu      <double*>    : Array of particles' density weights (i.e. masses)
 *    Returns:
 *       0                    : error code returned
 *       rho       <double*>  : Array of particles' densities
 */
int compute_density_3d_chunk_symmetrical(int64_t node_begin, int64_t node_end,
                                         int64_t nb_begin, int64_t nb_end,double h,
                                         double* restrict x, double* restrict y,
                                         double* restrict z, double* restrict nu,
                                         double* restrict rhoi, double* restrict rhoj){
  const double inv_h = 1./h;

  for(int64_t ii=node_begin;ii<node_end;ii+=1){ // Iterate over the ii index of the chunk
    double xii = x[ii];                         // Load the X component of the ii particle position
    double yii = y[ii];                         // Load the Y component of the ii particle position
    double zii = z[ii];                         // Load the Z component of the ii particle position

    #pragma omp simd                            // Hint at the compiler to vectorize the inner most loop
    for(int64_t jj=nb_begin;jj<nb_end;jj+=1){   // Iterate over the each other particle in jj loop
      double q = 0.;                            // Initialize the distance

      double xij = xii-x[jj];                   // Load and subtract jj particle's X position component
      double yij = yii-y[jj];                   // Load and subtract jj particle's Y position component
      double zij = zii-z[jj];                   // Load and subtract jj particle's Z position component

      q += xij*xij;                             // Add the jj contribution to the ii distance in X
      q += yij*yij;                             // Add the jj contribution to the ii distance in Y
      q += zij*zij;                             // Add the jj contribution to the ii distance in Z

      q = sqrt(q)*inv_h;                        // Sqrt to compute the normalized distance, measured in h

      double wij = w_bspline_3d_simd(q);        // compute the smoothing kernel separately for re-use

      rhoi[ii-node_begin] += nu[jj]*wij;        // add the jj contribution to ii density
      rhoj[jj-nb_begin]   += nu[ii]*wij;        // add the ii contribution to jj density
    }
  }

  return 0;
}

/*
 *  Function w_bspline_3d_constant:
 *    Returns the 3d normalization constant for the cubic b-spline SPH smoothing kernel
 *
 *    Arguments:
 *       h <double>           : Smoothing Length for the Smoothing Kernel w_bspline
 *    Returns:
 *       3d bspline normalization density <double>
 */
double w_bspline_3d_constant(double h){
  return 3./(2.*M_PI*h*h*h);  // 3d normalization value for the b-spline kernel
}

/*
 *  Function w_bspline_3d_simd:
 *    Returns the un-normalized value of the cubic b-spline SPH smoothing kernel
 *
 *    Arguments:
 *       q <double>           : Distance between particles normalized by the smoothing length h
 *    Returns:
 *       wq <double>          : Unnormalized value of the kernel
 *
 *    Observation:
 *       Why not else if(q<2.)?
 *       Because if you use "else if", the compiler refuses to vectorize,
 *       This results in a large slowdown, as of 2.5x slower for example_04
 */
#pragma omp declare simd
double w_bspline_3d_simd(double q){
  double wq=0;
  double wq1 = (0.6666666666666666 - q*q + 0.5*q*q*q);             // The first polynomial of the spline
  double wq2 = 0.16666666666666666*(2.-q)*(2.-q)*(2.-q);           // The second polynomial of the spline

  if(q<2.)                                                         // If the distance is below 2
    wq = wq2;                                                      // Use the 2nd polynomial for the spline

  if(q<1.)                                                         // If the distance is below 1
    wq = wq1;                                                      // Use the 1st polynomial for the spline

  return wq;                                                       // return which ever value corresponds to the distance
}

int cmp_int64_t(const void *p,const void *q){
  int64_t *data1,*data2;
  data1 = (int64_t*)p;
  data2 = (int64_t*)q;

  if(*data1 < *data2)         // data[0] is the hash value,
    return -1;
  else if(*data1 == *data2)   // in the unsorted array
    return 0;
  else
    return 1;
}

#define swap_loop(N,lsph,temp_swap,member,type) for(int64_t i=0;i<(N);i+=1)                            \
                                                  (temp_swap)[i] = (lsph)->member[(lsph)->hash[2*i+1]];\
                                                memcpy((lsph)->member,temp_swap,(N)*sizeof(type))

/*
 *  Function counting_sort:
 *
 *
 *    Arguments:
 *     N            <int64_t>    :
 *     box      <linkedListBox*> :
 *     lsph       <SPHparticle*> :
 *     swap_arr     <double*>    :
 *     temp_hash   <int64_t*>    :
 *    Returns:
 *     box      <linkedListBox*> :
 *
 *    Observation:
 *       Why not else if(q<2.)?
 *       Because if you use "else if", the compiler refuses to vectorize,
 *       This results in a large slowdown, as of 2.5x slower for example_04
 */
void counting_sort(int64_t N, linkedListBox *box, SPHparticle *lsph,void *swap_arr,int64_t *temp_hash){

  double t0,t1,t2,t3,t4,t5,t6;
  double t31,t32,t33,t34;

  t0 = omp_get_wtime();

  for(int64_t i=0;i<N;i+=1){
    int ret;
    khiter_t k = kh_put(0, box->hbegin, lsph->hash[2*i+0], &ret);
    if(ret==1)
      kh_value(box->hbegin, k) = 1;
    else if(ret==0){
      int64_t val = kh_value(box->hbegin, k);
      kh_value(box->hbegin, k) = val + 1;
    }
    else{
      printf("error counting_sort init");
      return;
    }
  }

  t1 = omp_get_wtime();

  unsigned int dict_size = kh_size(box->hbegin);
  int64_t hash[dict_size];
  int64_t prefix[dict_size];
  int idx = 0;

  for (khiter_t k = kh_begin(box->hbegin); k != kh_end(box->hbegin); ++k){
    if (kh_exist(box->hbegin, k)){
      hash[idx] = kh_key(box->hbegin,k);
      idx+=1;
    }
  }

  t2 = omp_get_wtime();

  qsort(hash,dict_size,sizeof(int64_t),cmp_int64_t);

  t3 = omp_get_wtime();

  prefix[0] = 0;
  for(int i=1;i<dict_size;i+=1){
    prefix[i] = prefix[i-1];

    khiter_t k = kh_get(0, box->hbegin, hash[i-1]);
    prefix[i] += kh_value(box->hbegin, k);
  }

  t31 = omp_get_wtime();

  for(int i = 0; i< dict_size;i+=1){
    khiter_t kp = kh_get(0, box->hbegin, hash[i]);
    kh_value(box->hbegin, kp) = prefix[i];
  }

  t32 = omp_get_wtime();

  for (khiter_t k = kh_begin(box->hbegin); k != kh_end(box->hbegin); ++k){
    if (kh_exist(box->hbegin, k)){
      int ret;
      int64_t hash = kh_key(box->hbegin,k);
      khiter_t ke = kh_put(1, box->hend, hash, &ret);
      kh_value(box->hend, ke) = kh_value(box->hbegin, k);
    }
  }

  t33 = omp_get_wtime();

  for(int64_t i=0;i<N;i+=1){
    khiter_t kp = kh_get(1, box->hend, lsph->hash[2*i]);
    if (kh_exist(box->hend, kp)){
      lsph->hash[2*i+1] = kh_value(box->hend, kp);
      kh_value(box->hend, kp) += 1;
    }
    else{
      printf("there is an issue\n");
    }
  }

  t34 = omp_get_wtime();

  t4 = omp_get_wtime();

  for(int64_t i=0;i<N;i+=1){
    int64_t idx = lsph->hash[2*i+1];
    temp_hash[2*idx+0] = lsph->hash[2*i+0];
    temp_hash[2*idx+1] = i;
  }

  for(int64_t i=0;i<N;i+=1){
    lsph->hash[2*i+0] = temp_hash[2*i+0];
    lsph->hash[2*i+1] = temp_hash[2*i+1];
  }

  t4 = omp_get_wtime();

  int64_t *int64_temp_swap = (int64_t *)swap_arr;
  swap_loop(N,lsph,int64_temp_swap,id ,int64_t);
  double *double_temp_swap = (double *)swap_arr;
  swap_loop(N,lsph,double_temp_swap,nu ,double);
  swap_loop(N,lsph,double_temp_swap,rho,double);
  swap_loop(N,lsph,double_temp_swap,x  ,double);
  swap_loop(N,lsph,double_temp_swap,y  ,double);
  swap_loop(N,lsph,double_temp_swap,z  ,double);
  swap_loop(N,lsph,double_temp_swap,ux ,double);
  swap_loop(N,lsph,double_temp_swap,uy ,double);
  swap_loop(N,lsph,double_temp_swap,uz ,double);
  swap_loop(N,lsph,double_temp_swap,Fx ,double);
  swap_loop(N,lsph,double_temp_swap,Fy ,double);
  swap_loop(N,lsph,double_temp_swap,Fz ,double);

  t5 = omp_get_wtime();

  for(int64_t i=0;i<N;i+=1)
    lsph->hash[i] = lsph->hash[2*i];

  t6 = omp_get_wtime();

  if(dbg){
    printf("1: %lf \n",t1-t0);
    printf("2: %lf \n",t2-t1);
    printf("3: %lf \n",t3-t2);
    printf("4: %lf \n",t4-t3);
    printf("  31: %lf\n",t31-t3);
    printf("  32: %lf\n",t32-t31);
    printf("  33: %lf\n",t33-t32);
    printf("  34: %lf\n",t34-t33);
    printf("  35: %lf\n",t4-t34);
    printf("5: %lf \n",t5-t4);
    printf("6: %lf \n",t6-t5);
  }

  return;
}