#ifndef COORDINATE_H_Y7FJINQO
#define COORDINATE_H_Y7FJINQO

#include <gsl/gsl>
#include <sens_loc/math/eigen_types.h>
#include <type_traits>

namespace sens_loc::math {

/// This enum allows to discrimate in which coordinate system or frame of
/// reference a vector lives. It is solely for code readability and correctness
/// evaluation.
enum class frame {
    world,   ///< 3D, Global, World Coordinate System. \f$(U, V, W)\f$.
    camera,  ///< 3D, Camera coordinate system, but the camera center is the
             ///< frame reference. \f$(X, Y, Z)\f$
    sphere,  ///< 3D, projection from image into space. \f$(X_s, Y_s, Z_s)\f$.
             ///< Equivalent to the direction of a lightray for a pixel.
    image,   ///< 2D, coordinate in an image as floating point. \f$(x, y)\f$
    pixel,   ///< 2D, usually a discrete coordinate system that refers to pixels
             ///< in an image. \f$(u, v)\f$
};

namespace detail {
/// Helper type-traits to examine if a reference frame is 2D. If its is not
/// 2D, the coordinate system is 3D.
/// \sa math::frame
template <frame Frame>
struct is_2d
    : std::bool_constant<Frame == frame::image || Frame == frame::pixel> {};

/// Small helper class that holds the data for 2 dimensional coordinates.
/// \sa math::coordinate
template <typename Real>
struct coordinate2d {
    static_assert(std::is_arithmetic_v<Real>);

    coordinate2d()
        : d(Real(0.), Real(0.)) {}
    coordinate2d(Real x1, Real x2)
        : d(x1, x2) {}

    vec<Real, 2> d;
};

/// Small helper class that holds the data for 3 dimensional coordinates.
/// \sa math::coordinate
template <typename Real>
struct coordinate3d {
    static_assert(std::is_arithmetic_v<Real>);

    coordinate3d()
        : d(Real(0.), Real(0.), Real(0.)) {}
    coordinate3d(Real x1, Real x2, Real x3)
        : d(x1, x2, x3) {}

    vec<Real, 3> d;
};
}  // namespace detail

/// This class is a strongly typed coordinate class to ensure the implemented
/// math does not mix up reference frames.
///
/// The overall goal of this slightly complicated construct is to increase
/// readability of the code **USING** this class and to improve correctness.
///
/// This class allows only access with the coordinate conventions for each
/// frame.
/// It prevents mixing different coordinates while assignment or other
/// operations and therefore forces conversion functions. Such conversions
/// are well defined through camera intrinsics for example.
///
/// There are helper typedefs, for easier writing:
/// \c pixel_coord<Real>, \c image_coord<Real>, \c sphere_coord<Real>,
/// \c camera_coord<Real> and \c world_coord<Real>.
///
/// \code
///   const pixel_coord<int> pixel(42, 32);
///   pixel.u(); // Accessing the u-coordinate for that pixel.
///   pixel.x(); // DOES NOT COMPILE - WRONG CONVENTION!
///
///   // Changing coordinate systems requires an explicit transformation, for
///   // example with the camera calibration as in this case.
///   const camera_models::pinhole<float> i;
///   const image_coord<float> transformed = i.transform_to_image(pixel);
///   transformed.x() // Accessing x-coordinate for that pixel.
///                   // Same point but different coordinate system.
///   transformed.u() // DOES NOT COMPILE - WRONG CONVENTION!
///
///   // It is not possible to mix coordinates up.
///   pixel_coord<float> sub_pixel(42.42, 42.42);
///   image_coord<float> transformed = sub_pixel; // DOES NOT COMPILE
///                                               // TYPE MISSMATCH!!
/// \endcode
///
/// \tparam Real underlying type of the coordinates
/// \tparam Frame type of reference frame, results in either 2 or 3 dimensional
/// vectors. The vector data is inherited.
template <typename Real, frame Frame>
class coordinate
    : private std::conditional_t<detail::is_2d<Frame>::value,
                                 detail::coordinate2d<Real>,
                                 detail::coordinate3d<Real>> {
    static_assert(std::is_arithmetic_v<Real>,
                  "Arithmetic type for Real required!");

    using base_class = std::conditional_t<detail::is_2d<Frame>::value,
                                          detail::coordinate2d<Real>,
                                          detail::coordinate3d<Real>>;

  public:
    using real = Real;

    // Reuse constructors of baseclass
    using base_class::base_class;

    /// Calculate the euclidean norm for the coordinate.
    /// \note This function is only enabled if the \p Real type is a floating
    /// point number.
    template <typename T = Real,
              typename   = std::enable_if_t<!std::is_integral_v<T>>>
    [[nodiscard]] Real norm() const noexcept {
        return base_class::d.norm();
    }

    /// Return the normalized vector of this coordinate.
    /// \note This function is only enabled if the \p Real type is a floating
    /// point number and the vector is in 3 dimensional space.
    template <typename T = Real,
              typename U = frame,
              typename   = std::enable_if_t<!std::is_integral_v<T> &&
                                          !detail::is_2d<Frame>::value>>
    [[nodiscard]] coordinate<Real, Frame> normalized() const noexcept {
        const auto res = base_class::d.normalized();
        return {res[0], res[1], res[2]};
    }

    /// Calculate the dot product with \p other.
    [[nodiscard]] Real dot(const coordinate<Real, Frame>& other) const
        noexcept {
        return base_class::d.dot(other.d);
    }
    /// Calculate the cross product with \p other.
    template <
        typename T = frame,
        typename = std::enable_if_t<!(Frame == T::pixel || Frame == T::image)>>
    [[nodiscard]] coordinate<Real, Frame>
    cross(const coordinate<Real, Frame>& other) const noexcept {
        const auto res = base_class::d.cross(other.d);
        return {res[0], res[1], res[2]};
    }

    // clang-format off
    template <typename T = frame,
              typename   = std::enable_if_t<Frame == T::pixel>>
    [[nodiscard]] Real u() const noexcept { return base_class::d[0]; }
    template <typename T = frame,
              typename   = typename std::enable_if<Frame == T::pixel>>
    [[nodiscard]] Real v() const noexcept { return base_class::d[1]; }

    template <typename T = frame,
              typename   = typename std::enable_if_t<Frame == T::image>>
    [[nodiscard]] Real x() const noexcept { return base_class::d[0]; }
    template <typename T = frame,
              typename   = typename std::enable_if<Frame == T::image>>
    [[nodiscard]] Real y() const noexcept { return base_class::d[1]; }

    template <typename T = frame,
              typename   = typename std::enable_if_t<Frame == T::sphere>>
    [[nodiscard]] Real Xs() const noexcept { return base_class::d[0]; }
    template <typename T = frame,
              typename   = typename std::enable_if<Frame == T::sphere>>
    [[nodiscard]] Real Ys() const noexcept { return base_class::d[1]; }
    template <typename T = frame,
              typename   = typename std::enable_if<Frame == T::sphere>>
    [[nodiscard]] Real Zs() const noexcept { return base_class::d[2]; }

    template <typename T = frame,
              typename   = typename std::enable_if_t<Frame == T::camera>>
    [[nodiscard]] Real X() const noexcept { return base_class::d[0]; }
    template <typename T = frame,
              typename   = typename std::enable_if<Frame == T::camera>>
    [[nodiscard]] Real Y() const noexcept { return base_class::d[1]; }
    template <typename T = frame,
              typename   = typename std::enable_if<Frame == T::camera>>
    [[nodiscard]] Real Z() const noexcept { return base_class::d[2]; }

    template <typename T = frame,
              typename   = typename std::enable_if_t<Frame == T::world>>
    [[nodiscard]] Real U() const noexcept { return base_class::d[0]; }
    template <typename T = frame,
              typename   = typename std::enable_if<Frame == T::world>>
    [[nodiscard]] Real V() const noexcept { return base_class::d[1]; }
    template <typename T = frame,
              typename   = typename std::enable_if<Frame == T::world>>
    [[nodiscard]] Real W() const noexcept { return base_class::d[2]; }
    // clang-format on

    coordinate<Real, Frame> operator-(const coordinate<Real, Frame>& p) const
        noexcept {
        coordinate<Real, Frame> res;
        auto                    difference = base_class::d - p.d;

        res.d = difference;
        return res;
    }
};

template <typename Real>
coordinate<Real, frame::camera>
operator*(Real factor, const coordinate<Real, frame::camera>& pt) noexcept {
    static_assert(std::is_floating_point_v<Real>,
                  "Real must be a floating point type for scaling operations!");

    coordinate<Real, frame::camera> result{factor * pt.X(), factor * pt.Y(),
                                           factor * pt.Z()};
    Ensures(std::abs(factor * pt.norm() - result.norm()) < Real(0.0001));
    return result;
}

/// Scaling spherical coordinates results in camera coordinates.
template <typename Real>
coordinate<Real, frame::camera>
operator*(Real factor, const coordinate<Real, frame::sphere>& pt) noexcept {
    static_assert(std::is_floating_point_v<Real>,
                  "Real must be a floating point type for scaling operations!");
    coordinate<Real, frame::camera> result{factor * pt.Xs(), factor * pt.Ys(),
                                           factor * pt.Zs()};
    Ensures(std::abs(factor * pt.norm() - result.norm()) < Real(0.0001));
    return result;
}

template <typename Real = int>
using pixel_coord = coordinate<Real, frame::pixel>;
template <typename Real = float>
using image_coord = coordinate<Real, frame::image>;
template <typename Real = float>
using sphere_coord = coordinate<Real, frame::sphere>;
template <typename Real = float>
using camera_coord = coordinate<Real, frame::camera>;
template <typename Real = float>
using world_coord = coordinate<Real, frame::world>;

}  // namespace sens_loc::math

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


#include <math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include "cconfigspace_internal.h"
#include "distribution_internal.h"

struct _ccs_distribution_normal_data_s {
	_ccs_distribution_common_data_t common_data;
	ccs_float_t                     mu;
	ccs_float_t                     sigma;
	ccs_scale_type_t                scale_type;
	ccs_numeric_t                   quantization;
	int                             quantize;
};
typedef struct _ccs_distribution_normal_data_s _ccs_distribution_normal_data_t;

static ccs_result_t
_ccs_distribution_del(ccs_object_t o) {
	(void)o;
	return CCS_SUCCESS;
}

static ccs_result_t
_ccs_distribution_normal_get_bounds(_ccs_distribution_data_t *data,
                                    ccs_interval_t           *interval_ret);

static ccs_result_t
_ccs_distribution_normal_samples(_ccs_distribution_data_t *data,
                                 ccs_rng_t                 rng,
                                 size_t                    num_values,
                                 ccs_numeric_t            *values);

static ccs_result_t
_ccs_distribution_normal_strided_samples(_ccs_distribution_data_t *data,
                                         ccs_rng_t                 rng,
                                         size_t                    num_values,
                                         size_t                    stride,
                                         ccs_numeric_t            *values);

static ccs_result_t
_ccs_distribution_normal_soa_samples(_ccs_distribution_data_t  *data,
                                     ccs_rng_t                  rng,
                                     size_t                     num_values,
                                     ccs_numeric_t            **values);

static _ccs_distribution_ops_t _ccs_distribution_normal_ops = {
	{ &_ccs_distribution_del },
	&_ccs_distribution_normal_samples,
	&_ccs_distribution_normal_get_bounds,
	&_ccs_distribution_normal_strided_samples,
	&_ccs_distribution_normal_soa_samples
};

static ccs_result_t
_ccs_distribution_normal_get_bounds(_ccs_distribution_data_t *data,
                                    ccs_interval_t           *interval_ret) {
	_ccs_distribution_normal_data_t *d = (_ccs_distribution_normal_data_t *)data;
	const ccs_numeric_type_t  data_type   = d->common_data.data_types[0];
	const ccs_scale_type_t scale_type     = d->scale_type;
	const ccs_numeric_t    quantization   = d->quantization;
	const int              quantize       = d->quantize;
	ccs_numeric_t          l;
	ccs_bool_t             li;
	ccs_numeric_t          u;
	ccs_bool_t             ui;

	if (scale_type == CCS_LOGARITHMIC) {
		if (data_type == CCS_NUM_FLOAT) {
			if (quantize) {
				l.f = quantization.f;
				li = CCS_TRUE;
			} else {
				l.f = 0.0;
				li = CCS_FALSE;
			}
			u.f = CCS_INFINITY;
			ui = CCS_FALSE;
		} else {
			if (quantize) {
				l.i = quantization.i;
				u.i = (CCS_INT_MAX/quantization.i)*quantization.i;
			} else {
				l.i = 1;
				u.i = CCS_INT_MAX;
			}
			li = CCS_TRUE;
			ui = CCS_TRUE;
		}
	} else {
		if (data_type == CCS_NUM_FLOAT) {
			l.f = -CCS_INFINITY;
			li = CCS_FALSE;
			u.f = CCS_INFINITY;
			ui = CCS_FALSE;
		} else {
			if (quantize) {
				l.i = (CCS_INT_MIN/quantization.i)*quantization.i;
				u.i = (CCS_INT_MAX/quantization.i)*quantization.i;
			} else {
				l.i = CCS_INT_MIN;
				u.i = CCS_INT_MAX;
			}
			li = CCS_TRUE;
			ui = CCS_TRUE;
		}
	}
	interval_ret->type = data_type;
	interval_ret->lower = l;
	interval_ret->upper = u;
	interval_ret->lower_included = li;
	interval_ret->upper_included = ui;
	return CCS_SUCCESS;
}

static inline ccs_result_t
_ccs_distribution_normal_samples_float(gsl_rng                *grng,
                                       const ccs_scale_type_t  scale_type,
                                       const ccs_float_t       quantization,
                                       const ccs_float_t       mu,
                                       const ccs_float_t       sigma,
                                       const int               quantize,
                                       size_t                  num_values,
                                       ccs_float_t            *values) {
	size_t i;
	if (scale_type == CCS_LOGARITHMIC && quantize) {
		ccs_float_t lq = log(quantization*0.5);
		if (mu - lq >= 0.0)
			//at least 50% chance to get a valid value
			for (i = 0; i < num_values; i++)
				do {
					values[i] = gsl_ran_gaussian(grng, sigma) + mu;
				} while (values[i] < lq);
		else
			//use tail distribution
			for (i = 0; i < num_values; i++)
				values[i] = gsl_ran_gaussian_tail(grng, lq - mu, sigma) + mu;
	} else
		for (i = 0; i < num_values; i++)
			values[i] = gsl_ran_gaussian(grng, sigma) + mu;
	if (scale_type == CCS_LOGARITHMIC)
		for (i = 0; i < num_values; i++)
			values[i] = exp(values[i]);
	if (quantize) {
			ccs_float_t rquantization = 1.0 / quantization;
			for (i = 0; i < num_values; i++)
				values[i] = round(values[i] * rquantization) * quantization;
	}
	return CCS_SUCCESS;
}

static inline ccs_result_t
_ccs_distribution_normal_samples_int(gsl_rng                *grng,
                                     const ccs_scale_type_t  scale_type,
                                     const ccs_int_t         quantization,
                                     const ccs_float_t       mu,
                                     const ccs_float_t       sigma,
                                     const int               quantize,
                                     size_t                  num_values,
                                     ccs_numeric_t          *values) {
	size_t i;
	ccs_float_t q;
	if (quantize)
		q = quantization*0.5;
	else
		q = 0.5;
	if (scale_type == CCS_LOGARITHMIC) {
		ccs_float_t lq = log(q);
		if (mu - lq >= 0.0)
			for (i = 0; i < num_values; i++)
				do {
					do {
						values[i].f = gsl_ran_gaussian(grng, sigma) + mu;
					} while (values[i].f < lq);
					values[i].f = exp(values[i].f);
				} while (CCS_UNLIKELY(values[i].f - q > (ccs_float_t)CCS_INT_MAX));
		else
			for (i = 0; i < num_values; i++)
				do {
					values[i].f = gsl_ran_gaussian_tail(grng, lq - mu, sigma) + mu;
					values[i].f = exp(values[i].f);
				} while (CCS_UNLIKELY(values[i].f - q > (ccs_float_t)CCS_INT_MAX));
	}
	else
		for (i = 0; i < num_values; i++)
			do {
				values[i].f = gsl_ran_gaussian(grng, sigma) + mu;
			} while (CCS_UNLIKELY(values[i].f - q > (ccs_float_t)CCS_INT_MAX || values[i].f + q < (ccs_float_t)CCS_INT_MIN));
	if (quantize) {
		ccs_float_t rquantization = 1.0 / quantization;
		for (i = 0; i < num_values; i++)
			values[i].i = (ccs_int_t)round(values[i].f * rquantization) * quantization;
	} else
		for (i = 0; i < num_values; i++)
			values[i].i = round(values[i].f);
	return CCS_SUCCESS;
}

static ccs_result_t
_ccs_distribution_normal_samples(_ccs_distribution_data_t *data,
                                  ccs_rng_t                 rng,
                                  size_t                    num_values,
                                  ccs_numeric_t            *values) {
	_ccs_distribution_normal_data_t *d = (_ccs_distribution_normal_data_t *)data;
	const ccs_numeric_type_t  data_type   = d->common_data.data_types[0];
	const ccs_scale_type_t scale_type     = d->scale_type;
	const ccs_numeric_t    quantization   = d->quantization;
	const ccs_float_t      mu             = d->mu;
	const ccs_float_t      sigma          = d->sigma;
	const int              quantize       = d->quantize;
	gsl_rng *grng;
	CCS_VALIDATE(ccs_rng_get_gsl_rng(rng, &grng));
	if (data_type == CCS_NUM_FLOAT)
		return _ccs_distribution_normal_samples_float(grng, scale_type,
                                                              quantization.f, mu,
                                                              sigma, quantize,
                                                              num_values,
                                                              (ccs_float_t*) values);
	else
		return _ccs_distribution_normal_samples_int(grng, scale_type,
                                                            quantization.i, mu,
                                                            sigma, quantize,
                                                            num_values, values);
}

static inline ccs_result_t
_ccs_distribution_normal_strided_samples_float(gsl_rng                *grng,
                                               const ccs_scale_type_t  scale_type,
                                               const ccs_float_t       quantization,
                                               const ccs_float_t       mu,
                                               const ccs_float_t       sigma,
                                               const int               quantize,
                                               size_t                  num_values,
                                               size_t                  stride,
                                               ccs_float_t            *values) {
	size_t i;
	if (scale_type == CCS_LOGARITHMIC && quantize) {
		ccs_float_t lq = log(quantization*0.5);
		if (mu - lq >= 0.0)
			//at least 50% chance to get a valid value
			for (i = 0; i < num_values; i++)
				do {
					values[i*stride] = gsl_ran_gaussian(grng, sigma) + mu;
				} while (values[i*stride] < lq);
		else
			//use tail distribution
			for (i = 0; i < num_values; i++)
				values[i*stride] = gsl_ran_gaussian_tail(grng, lq - mu, sigma) + mu;
	} else
		for (i = 0; i < num_values; i++)
			values[i*stride] = gsl_ran_gaussian(grng, sigma) + mu;
	if (scale_type == CCS_LOGARITHMIC)
		for (i = 0; i < num_values; i++)
			values[i*stride] = exp(values[i*stride]);
	if (quantize) {
			ccs_float_t rquantization = 1.0 / quantization;
			for (i = 0; i < num_values; i++)
				values[i*stride] = round(values[i*stride] * rquantization) * quantization;
	}
	return CCS_SUCCESS;
}

static inline ccs_result_t
_ccs_distribution_normal_strided_samples_int(gsl_rng                *grng,
                                             const ccs_scale_type_t  scale_type,
                                             const ccs_int_t         quantization,
                                             const ccs_float_t       mu,
                                             const ccs_float_t       sigma,
                                             const int               quantize,
                                             size_t                  num_values,
                                             size_t                  stride,
                                             ccs_numeric_t          *values) {
	size_t i;
	ccs_float_t q;
	if (quantize)
		q = quantization*0.5;
	else
		q = 0.5;
	if (scale_type == CCS_LOGARITHMIC) {
		ccs_float_t lq = log(q);
		if (mu - lq >= 0.0)
			for (i = 0; i < num_values; i++)
				do {
					do {
						values[i*stride].f = gsl_ran_gaussian(grng, sigma) + mu;
					} while (values[i*stride].f < lq);
					values[i*stride].f = exp(values[i*stride].f);
				} while (CCS_UNLIKELY(values[i*stride].f - q > (ccs_float_t)CCS_INT_MAX));
		else
			for (i = 0; i < num_values; i++)
				do {
					values[i*stride].f = gsl_ran_gaussian_tail(grng, lq - mu, sigma) + mu;
					values[i*stride].f = exp(values[i*stride].f);
				} while (CCS_UNLIKELY(values[i*stride].f - q > (ccs_float_t)CCS_INT_MAX));
	}
	else
		for (i = 0; i < num_values; i++)
			do {
				values[i*stride].f = gsl_ran_gaussian(grng, sigma) + mu;
			} while (CCS_UNLIKELY(values[i*stride].f - q > (ccs_float_t)CCS_INT_MAX || values[i*stride].f + q < (ccs_float_t)CCS_INT_MIN));
	if (quantize) {
		ccs_float_t rquantization = 1.0 / quantization;
		for (i = 0; i < num_values; i++)
			values[i*stride].i = (ccs_int_t)round(values[i*stride].f * rquantization) * quantization;
	} else
		for (i = 0; i < num_values; i++)
			values[i*stride].i = round(values[i*stride].f);
	return CCS_SUCCESS;
}

static ccs_result_t
_ccs_distribution_normal_strided_samples(_ccs_distribution_data_t *data,
                                         ccs_rng_t                 rng,
                                         size_t                    num_values,
                                         size_t                    stride,
                                         ccs_numeric_t            *values) {
	_ccs_distribution_normal_data_t *d = (_ccs_distribution_normal_data_t *)data;
	const ccs_numeric_type_t  data_type   = d->common_data.data_types[0];
	const ccs_scale_type_t scale_type     = d->scale_type;
	const ccs_numeric_t    quantization   = d->quantization;
	const ccs_float_t      mu             = d->mu;
	const ccs_float_t      sigma          = d->sigma;
	const int              quantize       = d->quantize;
	gsl_rng *grng;
	CCS_VALIDATE(ccs_rng_get_gsl_rng(rng, &grng));
	if (data_type == CCS_NUM_FLOAT)
		return _ccs_distribution_normal_strided_samples_float(grng, scale_type,
		                                                      quantization.f, mu,
		                                                      sigma, quantize,
		                                                      num_values, stride,
		                                                      (ccs_float_t*) values);
	else
		return _ccs_distribution_normal_strided_samples_int(grng, scale_type,
		                                                    quantization.i, mu,
		                                                    sigma, quantize,
		                                                    num_values, stride, values);
}

static ccs_result_t
_ccs_distribution_normal_soa_samples(_ccs_distribution_data_t  *data,
                                     ccs_rng_t                  rng,
                                     size_t                     num_values,
                                     ccs_numeric_t            **values) {
	if (*values)
		return _ccs_distribution_normal_samples(data, rng, num_values, *values);
	return CCS_SUCCESS;
}

extern ccs_result_t
ccs_create_normal_distribution(ccs_numeric_type_t  data_type,
                               ccs_float_t         mu,
                               ccs_float_t         sigma,
                               ccs_scale_type_t    scale_type,
                               ccs_numeric_t       quantization,
                               ccs_distribution_t *distribution_ret) {
	CCS_CHECK_PTR(distribution_ret);
	if (data_type != CCS_NUM_FLOAT && data_type != CCS_NUM_INTEGER)
		return -CCS_INVALID_TYPE;
	if (scale_type != CCS_LINEAR && scale_type != CCS_LOGARITHMIC)
		return -CCS_INVALID_SCALE;
	if (data_type == CCS_NUM_INTEGER && quantization.i < 0 )
		return -CCS_INVALID_VALUE;
	if (data_type == CCS_NUM_FLOAT && quantization.f < 0.0 )
		return -CCS_INVALID_VALUE;
	uintptr_t mem = (uintptr_t)calloc(1, sizeof(struct _ccs_distribution_s) + sizeof(_ccs_distribution_normal_data_t) + sizeof(ccs_numeric_type_t));

	if (!mem)
		return -CCS_OUT_OF_MEMORY;
	ccs_distribution_t distrib = (ccs_distribution_t)mem;
	_ccs_object_init(&(distrib->obj), CCS_DISTRIBUTION, (_ccs_object_ops_t *)&_ccs_distribution_normal_ops);
        _ccs_distribution_normal_data_t * distrib_data = (_ccs_distribution_normal_data_t *)(mem + sizeof(struct _ccs_distribution_s));
	distrib_data->common_data.data_types    = (ccs_numeric_type_t *)(mem + sizeof(struct _ccs_distribution_s) + sizeof(_ccs_distribution_normal_data_t));
	distrib_data->common_data.type          = CCS_NORMAL;
	distrib_data->common_data.dimension     = 1;
	distrib_data->common_data.data_types[0] = data_type;
	distrib_data->scale_type                = scale_type;
	distrib_data->quantization              = quantization;
	distrib_data->mu                        = mu;
	distrib_data->sigma                     = sigma;
	if (data_type == CCS_NUM_FLOAT) {
		if (quantization.f != 0.0)
			distrib_data->quantize = 1;
	} else {
		if (quantization.i != 0)
			distrib_data->quantize = 1;
	}
	distrib->data = (_ccs_distribution_data_t *)distrib_data;
	*distribution_ret = distrib;
	return CCS_SUCCESS;
}

extern ccs_result_t
ccs_normal_distribution_get_parameters(ccs_distribution_t  distribution,
                                       ccs_float_t        *mu_ret,
                                       ccs_float_t        *sigma_ret,
                                       ccs_scale_type_t   *scale_type_ret,
                                       ccs_numeric_t      *quantization_ret) {
	CCS_CHECK_DISTRIBUTION(distribution, CCS_NORMAL);
	if (!mu_ret && !sigma_ret && !scale_type_ret && !quantization_ret)
		return -CCS_INVALID_VALUE;
	_ccs_distribution_normal_data_t * data = (_ccs_distribution_normal_data_t *)distribution->data;

	if (mu_ret)
		*mu_ret = data->mu;
	if (sigma_ret)
		*sigma_ret = data->sigma;
	if (scale_type_ret)
		*scale_type_ret = data->scale_type;
	if (quantization_ret)
		*quantization_ret = data->quantization;
	return CCS_SUCCESS;
}


/*
 * Copyright (c) 1997-1999 Massachusetts Institute of Technology
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

/* This file was automatically generated --- DO NOT EDIT */
/* Generated on Mon Mar  8 17:44:42 EST 1999 */

#include <fftw-int.h>
#include <fftw.h>

/* Generated by: ./genfft -magic-alignment-check -magic-twiddle-load-all -magic-variables 4 -magic-loopi -notwiddle 15 */

/*
 * This function contains 156 FP additions, 56 FP multiplications,
 * (or, 128 additions, 28 multiplications, 28 fused multiply/add),
 * 62 stack variables, and 60 memory accesses
 */
static const fftw_real K587785252 = FFTW_KONST(+0.587785252292473129168705954639072768597652438);
static const fftw_real K951056516 = FFTW_KONST(+0.951056516295153572116439333379382143405698634);
static const fftw_real K250000000 = FFTW_KONST(+0.250000000000000000000000000000000000000000000);
static const fftw_real K559016994 = FFTW_KONST(+0.559016994374947424102293417182819058860154590);
static const fftw_real K500000000 = FFTW_KONST(+0.500000000000000000000000000000000000000000000);
static const fftw_real K866025403 = FFTW_KONST(+0.866025403784438646763723170752936183471402627);

/*
 * Generator Id's :
 * $Id: exprdag.ml,v 1.36 1999/02/19 17:22:11 athena Exp $
 * $Id: fft.ml,v 1.41 1999/02/19 17:22:13 athena Exp $
 * $Id: to_c.ml,v 1.24 1999/02/19 17:22:17 athena Exp $
 */

void fftw_no_twiddle_15(const fftw_complex *input, fftw_complex *output, int istride, int ostride)
{
     fftw_real tmp5;
     fftw_real tmp33;
     fftw_real tmp57;
     fftw_real tmp145;
     fftw_real tmp100;
     fftw_real tmp124;
     fftw_real tmp21;
     fftw_real tmp26;
     fftw_real tmp27;
     fftw_real tmp49;
     fftw_real tmp54;
     fftw_real tmp55;
     fftw_real tmp136;
     fftw_real tmp137;
     fftw_real tmp147;
     fftw_real tmp61;
     fftw_real tmp62;
     fftw_real tmp63;
     fftw_real tmp112;
     fftw_real tmp113;
     fftw_real tmp126;
     fftw_real tmp83;
     fftw_real tmp88;
     fftw_real tmp94;
     fftw_real tmp10;
     fftw_real tmp15;
     fftw_real tmp16;
     fftw_real tmp38;
     fftw_real tmp43;
     fftw_real tmp44;
     fftw_real tmp139;
     fftw_real tmp140;
     fftw_real tmp146;
     fftw_real tmp58;
     fftw_real tmp59;
     fftw_real tmp60;
     fftw_real tmp115;
     fftw_real tmp116;
     fftw_real tmp125;
     fftw_real tmp72;
     fftw_real tmp77;
     fftw_real tmp93;
     ASSERT_ALIGNED_DOUBLE();
     {
	  fftw_real tmp1;
	  fftw_real tmp97;
	  fftw_real tmp4;
	  fftw_real tmp96;
	  fftw_real tmp32;
	  fftw_real tmp98;
	  fftw_real tmp29;
	  fftw_real tmp99;
	  ASSERT_ALIGNED_DOUBLE();
	  tmp1 = c_re(input[0]);
	  tmp97 = c_im(input[0]);
	  {
	       fftw_real tmp2;
	       fftw_real tmp3;
	       fftw_real tmp30;
	       fftw_real tmp31;
	       ASSERT_ALIGNED_DOUBLE();
	       tmp2 = c_re(input[5 * istride]);
	       tmp3 = c_re(input[10 * istride]);
	       tmp4 = tmp2 + tmp3;
	       tmp96 = K866025403 * (tmp3 - tmp2);
	       tmp30 = c_im(input[5 * istride]);
	       tmp31 = c_im(input[10 * istride]);
	       tmp32 = K866025403 * (tmp30 - tmp31);
	       tmp98 = tmp30 + tmp31;
	  }
	  tmp5 = tmp1 + tmp4;
	  tmp29 = tmp1 - (K500000000 * tmp4);
	  tmp33 = tmp29 - tmp32;
	  tmp57 = tmp29 + tmp32;
	  tmp145 = tmp97 + tmp98;
	  tmp99 = tmp97 - (K500000000 * tmp98);
	  tmp100 = tmp96 + tmp99;
	  tmp124 = tmp99 - tmp96;
     }
     {
	  fftw_real tmp17;
	  fftw_real tmp20;
	  fftw_real tmp45;
	  fftw_real tmp79;
	  fftw_real tmp80;
	  fftw_real tmp81;
	  fftw_real tmp48;
	  fftw_real tmp82;
	  fftw_real tmp22;
	  fftw_real tmp25;
	  fftw_real tmp50;
	  fftw_real tmp84;
	  fftw_real tmp85;
	  fftw_real tmp86;
	  fftw_real tmp53;
	  fftw_real tmp87;
	  ASSERT_ALIGNED_DOUBLE();
	  {
	       fftw_real tmp18;
	       fftw_real tmp19;
	       fftw_real tmp46;
	       fftw_real tmp47;
	       ASSERT_ALIGNED_DOUBLE();
	       tmp17 = c_re(input[6 * istride]);
	       tmp18 = c_re(input[11 * istride]);
	       tmp19 = c_re(input[istride]);
	       tmp20 = tmp18 + tmp19;
	       tmp45 = tmp17 - (K500000000 * tmp20);
	       tmp79 = K866025403 * (tmp19 - tmp18);
	       tmp80 = c_im(input[6 * istride]);
	       tmp46 = c_im(input[11 * istride]);
	       tmp47 = c_im(input[istride]);
	       tmp81 = tmp46 + tmp47;
	       tmp48 = K866025403 * (tmp46 - tmp47);
	       tmp82 = tmp80 - (K500000000 * tmp81);
	  }
	  {
	       fftw_real tmp23;
	       fftw_real tmp24;
	       fftw_real tmp51;
	       fftw_real tmp52;
	       ASSERT_ALIGNED_DOUBLE();
	       tmp22 = c_re(input[9 * istride]);
	       tmp23 = c_re(input[14 * istride]);
	       tmp24 = c_re(input[4 * istride]);
	       tmp25 = tmp23 + tmp24;
	       tmp50 = tmp22 - (K500000000 * tmp25);
	       tmp84 = K866025403 * (tmp24 - tmp23);
	       tmp85 = c_im(input[9 * istride]);
	       tmp51 = c_im(input[14 * istride]);
	       tmp52 = c_im(input[4 * istride]);
	       tmp86 = tmp51 + tmp52;
	       tmp53 = K866025403 * (tmp51 - tmp52);
	       tmp87 = tmp85 - (K500000000 * tmp86);
	  }
	  tmp21 = tmp17 + tmp20;
	  tmp26 = tmp22 + tmp25;
	  tmp27 = tmp21 + tmp26;
	  tmp49 = tmp45 - tmp48;
	  tmp54 = tmp50 - tmp53;
	  tmp55 = tmp49 + tmp54;
	  tmp136 = tmp80 + tmp81;
	  tmp137 = tmp85 + tmp86;
	  tmp147 = tmp136 + tmp137;
	  tmp61 = tmp45 + tmp48;
	  tmp62 = tmp50 + tmp53;
	  tmp63 = tmp61 + tmp62;
	  tmp112 = tmp82 - tmp79;
	  tmp113 = tmp87 - tmp84;
	  tmp126 = tmp112 + tmp113;
	  tmp83 = tmp79 + tmp82;
	  tmp88 = tmp84 + tmp87;
	  tmp94 = tmp83 + tmp88;
     }
     {
	  fftw_real tmp6;
	  fftw_real tmp9;
	  fftw_real tmp34;
	  fftw_real tmp68;
	  fftw_real tmp69;
	  fftw_real tmp70;
	  fftw_real tmp37;
	  fftw_real tmp71;
	  fftw_real tmp11;
	  fftw_real tmp14;
	  fftw_real tmp39;
	  fftw_real tmp73;
	  fftw_real tmp74;
	  fftw_real tmp75;
	  fftw_real tmp42;
	  fftw_real tmp76;
	  ASSERT_ALIGNED_DOUBLE();
	  {
	       fftw_real tmp7;
	       fftw_real tmp8;
	       fftw_real tmp35;
	       fftw_real tmp36;
	       ASSERT_ALIGNED_DOUBLE();
	       tmp6 = c_re(input[3 * istride]);
	       tmp7 = c_re(input[8 * istride]);
	       tmp8 = c_re(input[13 * istride]);
	       tmp9 = tmp7 + tmp8;
	       tmp34 = tmp6 - (K500000000 * tmp9);
	       tmp68 = K866025403 * (tmp8 - tmp7);
	       tmp69 = c_im(input[3 * istride]);
	       tmp35 = c_im(input[8 * istride]);
	       tmp36 = c_im(input[13 * istride]);
	       tmp70 = tmp35 + tmp36;
	       tmp37 = K866025403 * (tmp35 - tmp36);
	       tmp71 = tmp69 - (K500000000 * tmp70);
	  }
	  {
	       fftw_real tmp12;
	       fftw_real tmp13;
	       fftw_real tmp40;
	       fftw_real tmp41;
	       ASSERT_ALIGNED_DOUBLE();
	       tmp11 = c_re(input[12 * istride]);
	       tmp12 = c_re(input[2 * istride]);
	       tmp13 = c_re(input[7 * istride]);
	       tmp14 = tmp12 + tmp13;
	       tmp39 = tmp11 - (K500000000 * tmp14);
	       tmp73 = K866025403 * (tmp13 - tmp12);
	       tmp74 = c_im(input[12 * istride]);
	       tmp40 = c_im(input[2 * istride]);
	       tmp41 = c_im(input[7 * istride]);
	       tmp75 = tmp40 + tmp41;
	       tmp42 = K866025403 * (tmp40 - tmp41);
	       tmp76 = tmp74 - (K500000000 * tmp75);
	  }
	  tmp10 = tmp6 + tmp9;
	  tmp15 = tmp11 + tmp14;
	  tmp16 = tmp10 + tmp15;
	  tmp38 = tmp34 - tmp37;
	  tmp43 = tmp39 - tmp42;
	  tmp44 = tmp38 + tmp43;
	  tmp139 = tmp69 + tmp70;
	  tmp140 = tmp74 + tmp75;
	  tmp146 = tmp139 + tmp140;
	  tmp58 = tmp34 + tmp37;
	  tmp59 = tmp39 + tmp42;
	  tmp60 = tmp58 + tmp59;
	  tmp115 = tmp71 - tmp68;
	  tmp116 = tmp76 - tmp73;
	  tmp125 = tmp115 + tmp116;
	  tmp72 = tmp68 + tmp71;
	  tmp77 = tmp73 + tmp76;
	  tmp93 = tmp72 + tmp77;
     }
     {
	  fftw_real tmp134;
	  fftw_real tmp28;
	  fftw_real tmp133;
	  fftw_real tmp142;
	  fftw_real tmp144;
	  fftw_real tmp138;
	  fftw_real tmp141;
	  fftw_real tmp143;
	  fftw_real tmp135;
	  ASSERT_ALIGNED_DOUBLE();
	  tmp134 = K559016994 * (tmp16 - tmp27);
	  tmp28 = tmp16 + tmp27;
	  tmp133 = tmp5 - (K250000000 * tmp28);
	  tmp138 = tmp136 - tmp137;
	  tmp141 = tmp139 - tmp140;
	  tmp142 = (K951056516 * tmp138) - (K587785252 * tmp141);
	  tmp144 = (K951056516 * tmp141) + (K587785252 * tmp138);
	  c_re(output[0]) = tmp5 + tmp28;
	  tmp143 = tmp134 + tmp133;
	  c_re(output[9 * ostride]) = tmp143 - tmp144;
	  c_re(output[6 * ostride]) = tmp143 + tmp144;
	  tmp135 = tmp133 - tmp134;
	  c_re(output[12 * ostride]) = tmp135 - tmp142;
	  c_re(output[3 * ostride]) = tmp135 + tmp142;
     }
     {
	  fftw_real tmp110;
	  fftw_real tmp56;
	  fftw_real tmp109;
	  fftw_real tmp118;
	  fftw_real tmp120;
	  fftw_real tmp114;
	  fftw_real tmp117;
	  fftw_real tmp119;
	  fftw_real tmp111;
	  ASSERT_ALIGNED_DOUBLE();
	  tmp110 = K559016994 * (tmp44 - tmp55);
	  tmp56 = tmp44 + tmp55;
	  tmp109 = tmp33 - (K250000000 * tmp56);
	  tmp114 = tmp112 - tmp113;
	  tmp117 = tmp115 - tmp116;
	  tmp118 = (K951056516 * tmp114) - (K587785252 * tmp117);
	  tmp120 = (K951056516 * tmp117) + (K587785252 * tmp114);
	  c_re(output[5 * ostride]) = tmp33 + tmp56;
	  tmp119 = tmp110 + tmp109;
	  c_re(output[14 * ostride]) = tmp119 - tmp120;
	  c_re(output[11 * ostride]) = tmp119 + tmp120;
	  tmp111 = tmp109 - tmp110;
	  c_re(output[2 * ostride]) = tmp111 - tmp118;
	  c_re(output[8 * ostride]) = tmp111 + tmp118;
     }
     {
	  fftw_real tmp150;
	  fftw_real tmp148;
	  fftw_real tmp149;
	  fftw_real tmp154;
	  fftw_real tmp156;
	  fftw_real tmp152;
	  fftw_real tmp153;
	  fftw_real tmp155;
	  fftw_real tmp151;
	  ASSERT_ALIGNED_DOUBLE();
	  tmp150 = K559016994 * (tmp146 - tmp147);
	  tmp148 = tmp146 + tmp147;
	  tmp149 = tmp145 - (K250000000 * tmp148);
	  tmp152 = tmp21 - tmp26;
	  tmp153 = tmp10 - tmp15;
	  tmp154 = (K951056516 * tmp152) - (K587785252 * tmp153);
	  tmp156 = (K951056516 * tmp153) + (K587785252 * tmp152);
	  c_im(output[0]) = tmp145 + tmp148;
	  tmp155 = tmp150 + tmp149;
	  c_im(output[6 * ostride]) = tmp155 - tmp156;
	  c_im(output[9 * ostride]) = tmp156 + tmp155;
	  tmp151 = tmp149 - tmp150;
	  c_im(output[3 * ostride]) = tmp151 - tmp154;
	  c_im(output[12 * ostride]) = tmp154 + tmp151;
     }
     {
	  fftw_real tmp129;
	  fftw_real tmp127;
	  fftw_real tmp128;
	  fftw_real tmp123;
	  fftw_real tmp132;
	  fftw_real tmp121;
	  fftw_real tmp122;
	  fftw_real tmp131;
	  fftw_real tmp130;
	  ASSERT_ALIGNED_DOUBLE();
	  tmp129 = K559016994 * (tmp125 - tmp126);
	  tmp127 = tmp125 + tmp126;
	  tmp128 = tmp124 - (K250000000 * tmp127);
	  tmp121 = tmp49 - tmp54;
	  tmp122 = tmp38 - tmp43;
	  tmp123 = (K951056516 * tmp121) - (K587785252 * tmp122);
	  tmp132 = (K951056516 * tmp122) + (K587785252 * tmp121);
	  c_im(output[5 * ostride]) = tmp124 + tmp127;
	  tmp131 = tmp129 + tmp128;
	  c_im(output[11 * ostride]) = tmp131 - tmp132;
	  c_im(output[14 * ostride]) = tmp132 + tmp131;
	  tmp130 = tmp128 - tmp129;
	  c_im(output[2 * ostride]) = tmp123 + tmp130;
	  c_im(output[8 * ostride]) = tmp130 - tmp123;
     }
     {
	  fftw_real tmp95;
	  fftw_real tmp101;
	  fftw_real tmp102;
	  fftw_real tmp106;
	  fftw_real tmp107;
	  fftw_real tmp104;
	  fftw_real tmp105;
	  fftw_real tmp108;
	  fftw_real tmp103;
	  ASSERT_ALIGNED_DOUBLE();
	  tmp95 = K559016994 * (tmp93 - tmp94);
	  tmp101 = tmp93 + tmp94;
	  tmp102 = tmp100 - (K250000000 * tmp101);
	  tmp104 = tmp58 - tmp59;
	  tmp105 = tmp61 - tmp62;
	  tmp106 = (K951056516 * tmp104) + (K587785252 * tmp105);
	  tmp107 = (K951056516 * tmp105) - (K587785252 * tmp104);
	  c_im(output[10 * ostride]) = tmp100 + tmp101;
	  tmp108 = tmp102 - tmp95;
	  c_im(output[7 * ostride]) = tmp107 + tmp108;
	  c_im(output[13 * ostride]) = tmp108 - tmp107;
	  tmp103 = tmp95 + tmp102;
	  c_im(output[ostride]) = tmp103 - tmp106;
	  c_im(output[4 * ostride]) = tmp106 + tmp103;
     }
     {
	  fftw_real tmp65;
	  fftw_real tmp64;
	  fftw_real tmp66;
	  fftw_real tmp90;
	  fftw_real tmp92;
	  fftw_real tmp78;
	  fftw_real tmp89;
	  fftw_real tmp91;
	  fftw_real tmp67;
	  ASSERT_ALIGNED_DOUBLE();
	  tmp65 = K559016994 * (tmp60 - tmp63);
	  tmp64 = tmp60 + tmp63;
	  tmp66 = tmp57 - (K250000000 * tmp64);
	  tmp78 = tmp72 - tmp77;
	  tmp89 = tmp83 - tmp88;
	  tmp90 = (K951056516 * tmp78) + (K587785252 * tmp89);
	  tmp92 = (K951056516 * tmp89) - (K587785252 * tmp78);
	  c_re(output[10 * ostride]) = tmp57 + tmp64;
	  tmp91 = tmp66 - tmp65;
	  c_re(output[7 * ostride]) = tmp91 - tmp92;
	  c_re(output[13 * ostride]) = tmp91 + tmp92;
	  tmp67 = tmp65 + tmp66;
	  c_re(output[4 * ostride]) = tmp67 - tmp90;
	  c_re(output[ostride]) = tmp67 + tmp90;
     }
}

fftw_codelet_desc fftw_no_twiddle_15_desc =
{
     "fftw_no_twiddle_15",
     (void (*)()) fftw_no_twiddle_15,
     15,
     FFTW_FORWARD,
     FFTW_NOTW,
     241,
     0,
     (const int *) 0,
};


/**
 * @file batchf_zgemm_stride.c
 *
 *  @brief BBLAS batchf_zgemm_stride double _Complex routine.
 *
 *  BBLAS is a software package provided by Univ. of Manchester,
 *  Univ. of Tennessee.
 *
 * @version 1.0.0
 * @author  Samuel  D. Relton
 * @author  Pedro   V. Lara
 * @author  Mawussi Zounon
 * @date    2016-04-06
 *
 **/

#ifndef DOXYGEN_SHOULD_SKIP_THIS
/**
 * Code generation
 * @precisions normal z -> c d s
 **/
#endif

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

#define COMPLEX

/**
    Purpose
    -------
    <b>batchf_zgemm_stride</b> is a batch version of zgemm for fixed size.
	It performs the matrix-matrix operations

	arrayC[i] = alpha[i]*op( arrayA[i] )*op( arrayB[i] ) + beta[i]*arrayC[i],

	where op( X ) is one of

	op( X ) = X      or
	op( X ) = X**T   or
	op( X ) = X**H,

	alpha[i] and beta[i] are scalars, and arrayA[i], arrayB[i] and C are matrices, with
	op( arrayA[i] ) an m by k matrix, op( arrayB[i] ) a k by n matrix and arrayC[i]
	an m by n matrix.

	Fixed Batch Operations
	-----------------------------------

	- all parameters that are arrays must have length at least batch_count.
	- all parameters that are arrays must have all values set.

	the values of transA, transB, M, N, K, alpha, beta, lda, ldb, and ldc
    are used for all computations.

	Parameters
	----------
	@param[in]
	transA   <tt>enum BBLAS_TRANS</tt>.
	         transA specifies the form of op( arrayA[i] ) to be used in
			 the matrix multiplication as follows:
      -     = BblasNoTrans:    op( arrayA[i] ) = arrayA[i].
      -     = BblasTrans:      op( arrayA[i] ) = arrayA[i]**T.
      -     = BblasConjTrans:  op( arrayA[i] ) = arrayA[i]**H.

	@param[in]
	transB   <tt>enum BBLAS_TRANS</tt>.
	         On entry, transB specifies the form of op( arrayB[i] ) to be used in
             the matrix multiplication as follows:
      -     = BblasNoTrans:    op( arrayB[i] ) = arrayB[i].
      -     = BblasTrans:      op( arrayB[i] ) = arrayB[i]**T.
      -     = BblasConjTrans:  op( arrayB[i] ) = arrayB[i]**H.

	@param[in]
	M       <tt>int</tt>.
            M specifies the number of rows of the matrix
            op( arrayA[i] ) and of the matrix arrayC[i].
            M must be greater than zero.

	@param[in]
	N       <tt>int</tt>.
            N specifies the number of columns of the matrix
            op( arrayB[i] ) and the number of columns of the matrix arrayC[i].
 		    N must be greater than zero.

    @param[in]
    K      <tt>int</tt>.
	       K specifies the number of columns of the matrix
           op( arrayA[i] ) and the number of rows of the matrix op( arrayB[i] ).
		   K must be greater than zero.

    @param[in]
    alpha   <tt>complex_16</tt>.

    @param[in]
    arrayA  Array of pointers.
            Each element arrayA[i] is a pointer to a COMPLEX_16 matrix of
 		    dimension lda by Ka, where Ka is K when transA = BblasNoTrans,
 		    and is M otherwise.
            When using transA = BblasNoTrans the leading M by K
            part of arrayA[i] must contain the matrix elements, otherwise
            the leading K by M part of arrayA[i] must contain the
            matrix elements.

    @param[in]
    lda     <tt>int</tt>.
            lda specifies the first dimension of arrayA[i] as declared
            in the calling (sub) program. When transA = BblasNoTrans then
            lda must be at least max( 1, M ), otherwise lda must be at
            least max( 1, K ).

    @param[in]
    strideA <tt>int</tt>.
            strideA specifies the storage spacing between A matrices.
                    strideA must be at least M by K.

    @param[in]
    arrayB  Array of pointers.
  		    Each element arrayB[i] is a pointer to a COMPLEX_16 matrix of
  		    dimension ldb by Kb, where Kb is N when transB = BblasNoTrans,
  		    and is K otherwise.
            When using transB = BblasNoTrans the leading K by N
            part of arrayB[i] must contain the matrix elements, otherwise
            the leading N by K part of arrayB[i] must contain the
            matrix elements.

    @param[in]
    ldb     <tt>int</tt>.
  		    ldb specifies the first dimension of arrayB[i] as declared
            in the calling (sub) program. When transB = BblasNoTrans then
            ldb must be at least max( 1, K ), otherwise ldb must be at
            least max( 1, N ).

    @param[in]
    strideB <tt>int</tt>.
            strideB specifies the storage spacing between A matrices.
            strideB must be at least K by N.

    @param[in]
    beta    <tt>complex_16</tt>.
            When beta is set to zero arrayC[i] need not be set on input.

    @param[in,out]
    arrayC  Array of pointers.
	        Each element arrayC[i] is a pointer to a COMPLEX_16 matrix of
      	    dimension ldc by N.
  	        Before entry, the leading M by N part of the arrayC[i] must
            contain a matrix C, except when beta is zero, in which
            case C need not be set on entry.
            On exit, the matrix arrayC[i] is overwritten by the M by N matrix
            ( alpha*op( arrayA[i] )*op( arrayB[i] ) + beta*arrayC[i] ).

    @param[in]
    ldc     <tt>int</tt>.
  	        Each element ldc specifies the first dimension of arrayC[i] as declared
  	        in the calling (sub) program. The value ldc must be at least
  	        max( 1, M )

    @param[in]
    strideC <tt>int</tt>.
            strideC specifies the storage spacing between A matrices.
            strideC must be at least M by N.

    @param[in]
    batch_count  <tt>int</tt>
                 The number of matrices to operate on.

    @param[in,out]
    info    <tt>int</tt>.
            info is the error return code of the batch.
  	        The error codes can be found in bblas_macros.h.
 **/
void batchf_zgemm_stride(
    const enum BBLAS_TRANS transA, const enum BBLAS_TRANS transB,
    const int M,  const int N, const int K,
    const BBLAS_Complex64_t alpha,
    const BBLAS_Complex64_t *arrayA, const int lda, const int strideA,
    const BBLAS_Complex64_t *arrayB, const int ldb, const int strideB,
    const BBLAS_Complex64_t beta,
    BBLAS_Complex64_t *arrayC, const int ldc, const int strideC,
    const int batch_count, int *info)
{
    /* Local variables */
    int batch_iter;
    int LDA,  LDB;
    char func_name[15] = "batchf_zgemm_op";

	/* Check input arguments */
    if (batch_count < 0)
    {
	    xerbla_batch(func_name, BBLAS_ERR_BATCH_COUNT, -1);
    }
    if ((transA != BblasNoTrans) &&
        (transA != BblasTrans) &&
        (transA != BblasConjTrans))
    {
        xerbla_batch(func_name, BBLAS_ERR_TRANSA, -1);
        *info  = BBLAS_ERR_TRANSA;
        return;
    }
    if ((transB != BblasNoTrans) &&
        (transB != BblasTrans) &&
        (transB != BblasConjTrans))
    {
        xerbla_batch(func_name, BBLAS_ERR_TRANSB, -1);
        *info = BBLAS_ERR_TRANSB;
        return;
    }
    if ( transA == BblasNoTrans )
    {
        LDA = M;
    } else
    {
        LDA = K;
    }
    if ( transB == BblasNoTrans )
    {
        LDB = K;
    } else
    {
        LDB = N;
    }
    if (M < 0)
    {
        xerbla_batch(func_name, BBLAS_ERR_M, -1);
        *info = BBLAS_ERR_M;
        return;
    }
    if (N < 0) {
        xerbla_batch(func_name, BBLAS_ERR_N, -1);
        *info = BBLAS_ERR_N;
        return;
    }
    if (K < 0) {
        xerbla_batch(func_name, BBLAS_ERR_K, -1);
        *info = BBLAS_ERR_K;
        return;
    }
    if (lda < max(1, LDA)) {
        xerbla_batch(func_name, BBLAS_ERR_LDA, -1);
        *info =  BBLAS_ERR_LDA;
        return;
    }
    if (ldb < max(1, LDB)) {
        xerbla_batch(func_name, BBLAS_ERR_LDB, -1);
        *info = BBLAS_ERR_LDB;
        return;
    }
    if (ldc < max(1, M)) {
        xerbla_batch(func_name, BBLAS_ERR_LDC, -1);
        *info = BBLAS_ERR_LDC;
        return;
    }
    if (strideA < M*K) {
        xerbla_batch(func_name, BBLAS_ERR_STRIDEA, -1);
        *info =  BBLAS_ERR_STRIDEA;
        return;
    }
    if (strideB < K*N) {
        xerbla_batch(func_name, BBLAS_ERR_STRIDEB, -1);
        *info =  BBLAS_ERR_STRIDEB;
        return;
    }
    if (strideC < M*N) {
        xerbla_batch(func_name, BBLAS_ERR_STRIDEC, -1);
        *info =  BBLAS_ERR_STRIDEC;
        return;
    }

    /* Skip subproblems where nothing needs to be done */
    if (M == 0 || N == 0     ||
	    ((alpha == (BBLAS_Complex64_t)0.0 || K == 0) &&
	    beta == (BBLAS_Complex64_t)1.0 ))
    {
        *info =  BBLAS_SUCCESS;
        return;
    }
    for (batch_iter = 0; batch_iter < batch_count; batch_iter++)
    {
        /* Call to cblas_zgemm */
        cblas_zgemm(
        BblasColMajor,
        transA,
        transB,
        M,
        N,
        K,
        CBLAS_SADDR(alpha),
        //&arrayA[batch_iter*M*K],
        &arrayA[batch_iter*strideA],
        lda,
        //&arrayB[batch_iter*K*N],
        &arrayB[batch_iter*strideB],
        ldb,
        CBLAS_SADDR(beta),
        //&arrayC[batch_iter*M*N],
        &arrayC[batch_iter*strideC],
        ldc);
    }
    /* Successful */
    *info = BBLAS_SUCCESS;
}
#undef COMPLEX


/**
 * Cascade simulation based on pair production and bremsstrahlung interactions
 *
 * copyright  (C) 2007
 * the icecube collaboration
 *
 * $Id: I3CascadeSimulation.h 35916 2007-08-23 09:34:12Z bvoigt $
 *
 * @version $Revision: 35916 $
 * @date $LastChangedDate: 2007-08-23 03:34:12 -0600 (Thu, 23 Aug 2007) $
 * @author Bernhard Voigt <bernhard.voigt@desy.de>   Last changed by: $LastChangedBy: bvoigt $
 */

#ifndef I3_CASCADE_SIMULATION_H_INCLUDED
#define I3_CASCADE_SIMULATION_H_INCLUDED

//Standard C/C++ includes
#include <vector>
#include <stack>

// Gnu Scientific Library
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_integration.h>

// local includes
#include "cmc/I3CascadeMCCommon.h"
#include "cmc/I3CascadeDevelopment.h"
#include "cmc/I3CascadeParametrization.h"
#include "cmc/I3MetropolisHastings.h"

// icetray includes
#include "phys-services/I3RandomService.h"

/**
 * @brief Local definition of a Particle struct
 *
 * The particle struct holds energy and postion informations
 * of particles in the simulation.
 */
struct Particle {
  static const short ELECTRON = 1;
  static const short PHOTON = 0;
  double energy;
  double x;
  short type;
  /**
   * Particle with energy, at position x (1-dimensional), of
   * type type (1 is electron, 0 is photon)
   *
   * @param energy
   * @param x
   * @param type
   */
  Particle(double energy, double x, short type) : energy(energy), x(x), type(type) { }
};

/**
 * @brief Local definition of beta distribution
 *
 * The struct defines a pdf function, ie. the beta distribtuion
 * not normed, though.
 * It provides a rvs method to draw random samples from
 * the beta distribution using gsl_rng
 *
 * The parameters used for the beta distribibution (alpha and beta)
 * are defined.
 *
 * Functions are passed to I3MetropolisHastings and need to be static.
 */
struct beta_dist {
  // define the logger name, use the same as for the simulation class
  SET_LOGGER("I3CascadeSimulation");

  // parameter to beta function
  static double alpha;
  static double beta;

  /**
   * Beta Function \f$ x^{\alpha-1} (1-x)^{\beta-1}\f$
   *
   * @param x
   */
  static double pdf(const double& x) {
    log_trace("alpha %.2f, beta %.2f", alpha, beta);
    return pow(x, alpha-1) * pow(1-x, beta-1);
  }

  /**
   * Returns a random variate drawn from the beta distribution
   *
   * Uses the gsl_ran_beta method
   *
   * @param random gsl_rng instance
   */
  static double rvs(const gsl_rng* random) {
    log_trace("alpha %.2f, beta %.2f", alpha, beta);
    return gsl_ran_beta(random, alpha, beta);
  }
};

/**
 *@brief This class implements a Cascade development simulation including
 *       the LPM effect
 *
 * Description of the simulation can be found here:
 * http://wiki.icecube.wisc.edu/index.php/Cascade_Simulation
 *
 * Cross section formulars used are defined in I3CascadeSimulationCrossSections
 *
 *
 * @author Bernhard Voigt
 */
class I3CascadeSimulation : public I3CascadeDevelopment, public I3CascadeMCCommon::Sampler {

  // define the logger name
  SET_LOGGER("I3CascadeSimulation");

 public:

  /**
   * @brief Constructor
   *
   * @param random random number service from icetray frame
   */
  I3CascadeSimulation(I3RandomServicePtr random);

  /**
   * @brief Destructor
   *
   * Frees the gsl objects
   */
  ~I3CascadeSimulation();

  virtual void SetRandomNumberGenerator(I3RandomServicePtr);

  /**
   * Simulates a cascade of the given energy
   *
   * @param energy
   */
  void Simulate(I3Particle::ParticleType type, double energy);

  /**
   * Sets the energy threshold down to which particles are tracked (Default is 50 TeV)
   *
   * This shouldn't be lower than 1 GeV, since this is the limit for the Bremsstrahlung
   * interaction
   *
   * @param thresholdEnergy
   */
  void SetThreshold(double thresholdEnergy) {
    threshold_ = thresholdEnergy;
  }

  /**
   * Returns the threshold energy down to which particles are tracked
   *
   * @return threshold energy
   */
  double GetThreshold() {
    return threshold_;
  }

  /**
   * @brief default energy threshold down to which particles are tracked
   */
  static const double DEFAULT_THRESHOLD;

 private:

  I3CascadeSimulation(const I3CascadeSimulation&);

  /**
   * Cacluates the pair productio mean free path for the given energy
   *
   * @param energy
   */
  double PairProductionMeanFreePath(double energy);

  /**
   * Cacluates the bremsstrahlung radiation length (cut off energy is defined as 1 GeV)
   *
   * @param energy
   */
  double BremsstrahlungMeanFreePath(double energy);

  /**
   * Samples the bremsstrahlung cross section to get a fractional energy
   * of a secondary particle produced in an interaction with a incident particle
   * of the given energy
   *
   * @see I3MetropolisHastings::Sample
   *
   * @param energy
   */
  double SampleBremsstrahlungCrossSection(double energy);

  /**
   * Samples the pair production cross section to get a fractional energy
   * of a secondary particle produced in an interaction with a incident particle
   * of the given energy
   *
   * @see I3MetropolisHastings::Sample
   *
   * @param energy
   */
  double SamplePairProductionCrossSection(double energy);

  /**
   * Inits the simulation
   *
   * Interpolation of the mean free paths is performed
   * The metropolis hastings random samplers are initialized
   */
  void Init();

  /**
   * Inits the metropolis hastings random samplers
   */
  void InitMetropolisHastings();

  /**
   * Draw and discard samples from the MH samplers
   * to make them independent of initial conditions
   */
  void BurnIn();

  /**
   * Calcuation and interpolation of the pair production mean free path
   * in an energy range specified by lower and upper (in log10)
   * The interpolation used the given number of points.
   * Default energy range is from 100 GeV to 10 EeV
   *
   * Uses the integration and interpolation routines from gsl
   *
   * @param lower
   * @param upper
   * @param points
   */
  void InterpolPairProductionMeanFreePath(double lower=2., double upper=13.0, int points=100);

  /**
   * Calcuation and interpolation of the bremsstrahlung radiation length
   * in an energy range specified by lower and upper (in log10)
   * The interpolation used the given number of points.
   * Default energy range is from 100 GeV to 10 EeV
   *
   * Uses the integration and interpolation routines from gsl
   *
   * The integration of the cross section is done down to a lower edge of
   * I3CascadeSimulation::BREMSSTRAHLUNGCUTOFF
   * this is due to the infrared limit, where the cross section raises to infinite values.
   *
   * @param lower
   * @param upper
   * @param points
   */
  void InterpolBremsstrahlungMeanFreePath(double lower=2., double upper=13.0, int points=100);

  /**
   * Returns the mean free path of a particle
   *
   * @param particle
   */
  double MeanFreePath(Particle particle);

  /**
   * Propagates a particle
   *
   * Draws the next interaction point
   * Calls the interaction method to produce secondaries
   *
   * @param particle
   */
  void Propagate(Particle particle);

  /**
   * Creates secondaries produced in an interaction of the given particle
   *
   * @param particle
   */
  void Interaction(Particle particle);

  /**
   * Produces a secondary gamma
   */
  void BremsstrahlungInteraction(Particle electron);

  /**
   * Produces a electron, positron pair
   */
  void PairProductionInteraction(Particle photon);

  /**
   * Deposits the particle
   *
   * Calculates the energy loss profile of the given particle
   * and adds it to the total energy loss profile of this simulation
   */
  void Deposit(Particle particle);

  /**
   * @brief stack holding particles that have to be propagated
   */
  std::stack<Particle> particles_;

  /**
   * @brief counter how many particles have been created
   */
  unsigned int particlesCreated_;

  /**
   * @brief counter how many particles have been deleted
   */
  unsigned int particlesDeleted_;

  /**
   * @brief struct holding beta distribution parameters, provides function
   * evaluation and random numbers
   */
  beta_dist betaDist_;

  /**
   * @brief random number sampler for low energy bremsstrahlung
   */
  I3MetropolisHastings lowBremsSampler_;

  /**
   * @brief random number sampler for high energy bremsstrahlung
   */
  I3MetropolisHastings highBremsSampler_;

  /**
   * @brief random number sampler for low energy pair production
   */
  I3MetropolisHastings lowPairSampler_;

  /**
   * @brief random number sampler for high energy pair production
   */
  I3MetropolisHastings highPairSampler_;

  /**
   * @brief parametrization of energy loss profile for low energy particles
   */
  I3CascadeParametrization parametrization_;

  /**
   * @brief gsl spline accelerator object used for spline evaluation (bremsstrahlung)
   */
  gsl_interp_accel* interpolAccBrems_;

  /**
   * @brief gsl spline - interpolation of Bresmstrahlung mean free path
   */
  gsl_spline* splineBrems_;

  /**
   * @brief gsl spline accelerator object used for spline evaluation (pair production)
   */
  gsl_interp_accel* interpolAccPair_;

  /**
   * @brief gsl spline - interpolation of Pairproduction mean free path
   */
  gsl_spline* splinePair_;

  /**
   * @brief threshold down to which particles are tracked, default is 50 TeV
   */
  double threshold_;

  /**
   * @brief repeat burn-in for each event
   *
   * This ensures that the events are truly independent and can be reproduced
   * given the state of the random number generator.
   */
  bool perEventBurnIn_;

  /**
   * @brief lowest energy of bremsstrahlung photons
   */
  static const double BREMSSTRAHLUNGCUTOFF;

};
#endif


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <inttypes.h>
#include <math.h>
#include <assert.h>
#include <nlopt.h>
// The "procedural" interface to Reduce/CSL.
#include "C_call_CSL.h"

// activate by setting to 1 or 2
int DEBUG = 0;

// The optimization "object":
static nlopt_opt opt       = NULL;
// The result computed by NLOPT_optimize:
static double *result;
// The local optimization sub-algorithm/object, for algorithms that use it:
static nlopt_opt local_opt = NULL;

// Nlopt return codes, see nlopt_result in nlopt.h.  Used only in few
// places. nlopt_main.red does error checking at the Reduce level.
char *nlopt_result_mess(nlopt_result r) {
  char *s;
  switch (r) {
  case -1: s = "nlopt failure!";  break;
  case -2: s = "invalid arguments or unsupported constraints (perhaps for this algorithm)!";  break;
  case -3: s = "out of memory!";  break;
  case -4: s = "progress limited by roundoff";  break;
  case -5: s = "forced stop";  break;
  case 1:  s = "nlopt success";  break;
  case 2:  s = "stop value reached";  break;
  case 3:  s=  "f-tolerance reached";  break;
  case 4:  s = "x-tolerance reached";  break;
  case 5:  s = "max evaluations reached";  break;
  case 6:  s = "max time reached";  break;
  };
  return s;
}

#define ERRORCHECK(who,r) if (r != NLOPT_SUCCESS) \
    {fprintf(stderr, "*%s: %s\n", who, nlopt_result_mess(r));}

// =======================================================================
// Basics
// =======================================================================

void NLOPT_create(int32_t anum, int32_t n) {
  opt = nlopt_create(anum, n);
  if (opt == NULL)
    fprintf(stderr, "Couldn't create '%s'!\n", nlopt_algorithm_name(anum));
    // nlopt_algorithm_name is built-in to the NLopt library
}

void NLOPT_destroy() {
  nlopt_destroy(opt);
}

int32_t NLOPT_get_algorithm() {
  // The only reason for this 'NLOPT_' is to convert from nlopt_algorithm to 'int32_t'!
  // Isn't there a better way?
  int32_t a = nlopt_get_algorithm(opt);
  return a;
}

int32_t NLOPT_get_dimension() {
  // The only reason for this 'NLOPT_' is to convert from "unsigned" to 'int32_t'!
  int32_t d = nlopt_get_dimension(opt);
  return d;
}

// =======================================================================
// Setting upper and lower bounds (hyper-rectangle)
// =======================================================================

int32_t NLOPT_set_lower_bounds(const double *lb) {
  int32_t r = nlopt_set_lower_bounds(opt, lb);
  return r;
}
int32_t NLOPT_set_upper_bounds(const double *ub) {
  int32_t r = nlopt_set_upper_bounds(opt, ub);
  return r;
}

// acts like a Icon generator
double NLOPT_get_lower_bound(int32_t i, int32_t n) {
  static double *slb = NULL;
  if (i == 0) {
    // Contortion 1:
    slb = (double *) malloc(n*sizeof(double));
    int32_t r = nlopt_get_lower_bounds(opt, slb);
    ERRORCHECK("NLOPT_get_lower_bound",r);
  }
  // Contortion 2:
  double lbi = slb[i];
  if (i == n-1) free(slb);
  return lbi;
}

// acts like a Icon generator
double NLOPT_get_upper_bound(int32_t i, int32_t n) {
  static double *sub = NULL;
  if (i == 0) {
    // Contortion 1:
    sub = (double *) malloc(n*sizeof(double));
    int32_t r = nlopt_get_upper_bounds(opt, sub);
    ERRORCHECK("NLOPT_get_upper_bound",r);
  }
  // Contortion 2:
  double ubi = sub[i];
  if (i == n-1) free(sub);
  return ubi;
}

// =======================================================================
// Stopping criteria
// =======================================================================

int32_t NLOPT_set_xtol_rel(double t) {
  return nlopt_set_xtol_rel(opt, t);
}
int32_t NLOPT_set_ftol_rel(double t) {
  return nlopt_set_ftol_rel(opt, t);
}
int32_t NLOPT_set_ftol_abs(double t) {
  return nlopt_set_ftol_abs(opt, t);
}
int32_t NLOPT_set_maxeval(int32_t m) {
  return nlopt_set_maxeval(opt, m);
}
int32_t NLOPT_set_maxtime(double m) {
  return nlopt_set_maxtime(opt, m);
}
int32_t NLOPT_set_stopval(double v) {
  return nlopt_set_stopval(opt, v);
}

double NLOPT_get_xtol_rel() {
  return nlopt_get_xtol_rel(opt);
}
double NLOPT_get_ftol_rel() {
  return nlopt_get_ftol_rel(opt);
}
double NLOPT_get_ftol_abs() {
  return nlopt_get_ftol_abs(opt);
}
int32_t NLOPT_get_maxeval() {
  return nlopt_get_maxeval(opt);
}
double NLOPT_get_maxtime() {
  return nlopt_get_maxtime(opt);
}

// ===========================================================================
// Functions *internal* to the interface for dealing with arrays of doubles
// To understand what's going on see the comments in nlopt_set_lower_bounds()
// in nlopt_main.red.
// ===========================================================================
double *newDoubleArray(int32_t len) {
  double *a;
  a = (double *) malloc(len*sizeof(double));
  return a;
}
void setDoubleArray(double a[], int32_t i, double x) {
  a[i] = x;
}
void freeDoubleArray(double a[]) {
  free(a);
}
// This internal function turns a C array of doubles into a Lisp list and
// leaves it at the top of the stack.  The "stack" is used by the "procedural"
// interface to Reduce, see file "C_call_CSL.h".
void C_darray_to_Lisp_list(int32_t n, const double x[]) {
  // Push the x[i] on the stack.
  for (int i = 0; i < n; i++) PROC_push_floating(x[i]);
  // Make a list out of the x[i]; it is left at the top of the stack.
  PROC_make_function_call("list", n);
  // Now having put 'list on the front, get rid of it.
  // (This perhaps shows that that ACN should really extend the procedural
  // interface to make this slightly nicer. But with luck this will suffice
  // for now.)
  PROC_lisp_eval();
}

// ================================================================
// Setting the optimization objective
// ================================================================
// This is the most complex and difficult part of the interface so far.

// This function is internal to the interface (static), and is of type nlopt_func.
// It is modelled after f77_func_wrap in api/f77api.c.  It is used as a wrapper
// for scalar functions, which can represent either the optimization objective,
// or a general equality or inequality constraint.
// The PROC_ code is borrowed from cuba_integrand in redcuba.c of the 'cuba' package.
static double red_func_wrap(unsigned n, const double *x, double *grad, void *f_data) {
  double z;
  char *fn = (char *) f_data;
  if (DEBUG > 1) {
    fprintf(stderr, "red_func_wrap: %s", fn);
    if (grad) fprintf(stderr, " [G]");
    fprintf(stderr, "\n");
  }
  // Convert the function name (string) to a symbol, push it on the stack, and
  // wrap it in a quote:
  PROC_push_symbol(fn);
  PROC_make_function_call("quote", 1);
  // Turn the array 'x' into a Lisp list and leave it at the top of the "stack".
  C_darray_to_Lisp_list(n,x);
  // Wrap it in a quote:
  PROC_make_function_call("quote", 1);
  // Evaluate a call to the Reduce algebraic function 'fn' that takes an n-list
  // of reals 'x', and delivers either a single real, f(x), or, if 'grad' is not
  // NULL, the flattened (n+1)-list of reals {f(x),grad(x)}.
  int32_t r = PROC_make_function_call("call_alg_fn_list2list", 2);
  PROC_lisp_eval();
  if (DEBUG > 1) {
    fprintf(stderr, "  %s(", fn);
    for (int i=0; i<n; i++) {
      fprintf(stderr, "%.4g", x[i]);
      if (i < n-1) fprintf(stderr, ",");
    }
  };
  if (grad) {
    // If 'grad' is not NULL, the Reduce function 'fn' is expected to return a
    // flattened list {f(x),grad(x)}, where grad(x) is an n-list which should
    // be assigned to 'grad'. (Also see the function red_mfunc_wrap.)
    PROC_handle y = PROC_get_value();
    z = PROC_floating_value(PROC_first(y));
    if (DEBUG > 1) {
      fprintf(stderr, ") = %.5g\n", z);
      fprintf(stderr, "  grad = {");
    };
    y = PROC_rest(y);
    for (int i = 0; i < n; i++) {
      grad[i] = PROC_floating_value(PROC_first(y));
      y = PROC_rest(y);
      if (DEBUG > 1) {
	fprintf(stderr, "%.4g", grad[i]);
	if (i < n-1) fprintf(stderr, ",");
      }
    }
    if (DEBUG > 1) fprintf(stderr, "}\n");
  }
  else {  // grad is NULL, result is just f(x)
    z = PROC_floating_value(PROC_get_value());
    if (DEBUG > 1) fprintf(stderr, ") = %.5g\n", z);
  }
  return z;
}

// Modelled after F77_nlo_set_min_objective in api/f77funcs_.h.
int32_t NLOPT_set_min_objective(char *fname) {
  // In the call to nlopt_set_min_objective:
  //   1. The wrapper red_func_wrap is the nlopt_func argument,
  //   2. The name of the Reduce function defining the objective is the *f_data argument,
  //   3. We can't just pass fname to to this argument, we need the string rfun on the heap!
  char *rfun = malloc(strlen(fname)*sizeof(char)+1);
  strcpy(rfun,fname);
  return nlopt_set_min_objective(opt, red_func_wrap, rfun);
}

int32_t NLOPT_set_max_objective(char *fname) {
  char *rfun = malloc(strlen(fname)*sizeof(char)+1);
  strcpy(rfun,fname);
  return nlopt_set_max_objective(opt, red_func_wrap, rfun);
}

int32_t NLOPT_optimize(double *x0, int32_t n) {
  double *x, f;
  x = (double *) malloc(n*sizeof(double));
  for (int j=0; j<n; j++) x[j] = x0[j];
  if (DEBUG) fprintf(stderr, "NLOPT_optimize:\n");
  int32_t r = nlopt_optimize(opt, x, &f);
  // assign results to the static 'result' array:
  result = (double *) malloc((n+1)*sizeof(double));
  result[0] = f;
  for (int j=0; j<n; j++) result[j+1] = x[j];
  free(x);
  if (DEBUG) {
    fprintf(stderr, "  f = %f\n", result[0]);
    fprintf(stderr, "  result =");
    for (int j=0; j<n; j++)
      fprintf(stderr, " %.5g", result[j+1]);
    fprintf(stderr, "\n");
  }
  return r;
}

double get_result(int32_t i) {
  return result[i];
}

void free_result() {
  free(result);
}

// ================================================================
// Setting the local optimization sub-algorithm
// ================================================================

int32_t NLOPT_set_local_optimizer(int32_t anum) {
  local_opt = nlopt_create(anum, nlopt_get_dimension(opt));
  return nlopt_set_local_optimizer(opt, local_opt);
}

// =============================================================================
// Defining scalar constraints
// =============================================================================

// Note: in the next two functions
// 1. The wrapper function red_func_wrap is the nlopt_func argument,
// 2. The name of the Reduce function defining the constraint is the *f_data argument,
// 3. We can't just pass c_fname to to this argument, we need the string cfun on the heap!

int32_t NLOPT_add_eq_constraint(char *c_fname, double tol) {
  char *cfun = malloc(strlen(c_fname)*sizeof(char)+1);
  assert(cfun != NULL);
  strcpy(cfun,c_fname);
  if (DEBUG) fprintf(stderr, "added '%s' equality constraint, %.3g\n", cfun, tol);
  return nlopt_add_equality_constraint(opt, red_func_wrap, cfun, tol);
}
int32_t NLOPT_add_ineq_constraint(char *c_fname, double tol) {
  char *cfun = malloc(strlen(c_fname)*sizeof(char)+1);
  assert(cfun != NULL);
  strcpy(cfun,c_fname);
  if (DEBUG) fprintf(stderr, "added '%s' inequality constraint, %.3g\n", cfun, tol);
  return nlopt_add_inequality_constraint(opt, red_func_wrap, cfun, tol);
}

// =============================================================================
// Defining vector-valued constraints
// =============================================================================

// This function is internal to the interface (static), and is of type nlopt_mfunc.
// It is modelled after f77_mfunc_wrap in api/f77api.c.  It is used as a wrapper for
// functions that, given an n-vector 'x', return an m-vector in 'result'.
static void red_mfunc_wrap(unsigned m, double *result, unsigned n, const double *x,
			   double *grad, void *f_data) {
  char *fn = (char *) f_data;
  if (DEBUG > 1) {
    fprintf(stderr, "red_mfunc_wrap: %s", fn);
    if (grad) fprintf(stderr, " [G]");
    fprintf(stderr, "\n");
  }
  // Convert the function name (string) to a symbol, push it on the stack, and
  // wrap it in a quote:
  PROC_push_symbol(fn);
  PROC_make_function_call("quote", 1);
  // Turn the array x into a Lisp list and leave it at the top of the "stack".
  C_darray_to_Lisp_list(n,x);
  // Wrap it in a quote:
  PROC_make_function_call("quote", 1);
  // Evaluate a call to the Reduce m-dimensional algebraic function fn: R^n -> R^m
  int32_t r = PROC_make_function_call("call_alg_fn_list2list", 2);
  PROC_lisp_eval();
  if (DEBUG > 1) {
    fprintf(stderr, "  %s(", fn);
    for (int i=0; i<n; i++) {
      fprintf(stderr, "%.4g", x[i]);
      if (i < n-1) fprintf(stderr, ",");
    }
    fprintf(stderr, ") = {");
  }
  // If 'grad' is NULL, the result is a an m-list, which we assign to the C
  // array 'result' element-by-element.  If 'grad' is non-NULL, then it points
  // to an array of length m*n, and the result of 'fn' also contains a list of
  // length m*n, the gradients of the constraint functions with respect to x,
  // and 'grad' should be set to this list.  It is assumed that ∂c_i/∂x_j is
  // stored in grad[i*n + j].
  // Irrespective of 'grad', we first get the first m elements of the list and
  // assign them to 'result'.
  PROC_handle y = PROC_get_value();
  for (int i = 0; i < m; i++) {
    result[i] = PROC_floating_value(PROC_first(y));
    y = PROC_rest(y);
    if (DEBUG > 1) {
      fprintf(stderr, "%.4g", result[i]);
      if (i < m-1)  fprintf(stderr, ",");
    }
  }
  if (DEBUG > 1) fprintf(stderr, "}\n");
  if (grad) {
    // Now handle the last m*n elements of what 'fn' has returned
    if (DEBUG > 1) fprintf(stderr, "  grad = {");
    for (int i = 0; i < m*n; i++) {
      grad[i] = PROC_floating_value(PROC_first(y));
      y = PROC_rest(y);
      if (DEBUG > 1) {
	fprintf(stderr, "%.4g", grad[i]);
	if (i < m*n-1) fprintf(stderr, ",");
      }
    }
    if (DEBUG > 1) fprintf(stderr, "}\n");
  }
}

// Note: look at the notes for the functions for scalar constraints
int32_t NLOPT_add_eq_mconstraint(char *c_fname, int32_t m, double *tol) {
  char *cfun = malloc(strlen(c_fname)*sizeof(char)+1);
  assert(cfun != NULL);
  strcpy(cfun,c_fname);
  if (DEBUG) fprintf(stderr, "added '%s' equality m-constraint\n", cfun);
  return nlopt_add_equality_mconstraint(opt, m, red_mfunc_wrap, cfun, tol);
}
int32_t NLOPT_add_ineq_mconstraint(char *c_fname, int32_t m, double *tol) {
  char *cfun = malloc(strlen(c_fname)*sizeof(char)+1);
  assert(cfun != NULL);
  strcpy(cfun,c_fname);
  if (DEBUG) fprintf(stderr, "added '%s' inequality m-constraint\n", cfun);
  return nlopt_add_inequality_mconstraint(opt, m, red_mfunc_wrap, cfun, tol);
}

// =============================================================================
// Removing constraints
// =============================================================================

int32_t NLOPT_remove_eq_constraints() {
  return nlopt_remove_equality_constraints(opt);
}
int32_t NLOPT_remove_ineq_constraints() {
  return nlopt_remove_inequality_constraints(opt);
}

// =============================================================================
// Step size for algorithms without derivatives
// =============================================================================

int32_t NLOPT_set_initial_step(const double *dx) {
  return nlopt_set_initial_step(opt, dx);
}
int32_t NLOPT_set_initial_step1(double dx) {
  return nlopt_set_initial_step1(opt, dx);
}

// ============================================================================
// Stochastic search algorithms
// ============================================================================

int32_t NLOPT_set_population(int p) {
  return nlopt_set_population(opt, (unsigned) p);
}

void NLOPT_srand(int64_t seed) {
  nlopt_srand((unsigned long) seed);
}


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "parmt_invert.h"
#ifdef PARMT_USE_INTEL
#include <mkl_lapacke.h>
#include <mkl_cblas.h>
#else
#include <lapacke.h>
#include <cblas.h>
#endif
#include "parmt_utils.h"
#include "iscl/array/array.h"
#include "iscl/linalg/linalg.h"
#include "iscl/memory/memory.h"

/*!
 * @brief Inverts for a moment tensor at each location in the grid-search.
 *
 * @param[in] data     parMT data structure with observations and
 *                     corresponding green's functions at all locations
 *                     for all waveforms.
 * @param[in] ldm      Leading dimension of mts.  This must be at least 6.
 * @param[in] lcDev    If true then apply the deviatoric constraint.
 * @param[in] lcDev    Otherwise invert for all moment tensors terms.
 *
 * @param[out] mts     Moment tensors tabulated at each location.  This has
 *                     dimension [data.nlocs x ldm].
 * @param[out] phi     L2 objective function for each moment tensor.  This has
 *                     dimension [data.nlocs].
 *
 * @result 0 indicates success.
 *
 */
int parmt_invertLocationSearch(struct parmtData_struct data,
                               const int ldm,
                               const bool lcDev,
                               double *__restrict__ mts,
                               double *__restrict__ phi)
{
    double *G, *gxxAll, *gyyAll, *gzzAll, *gxyAll, *gxzAll, *gyzAll,
           *obsAll, *wts;
    //double m6[6], phiLoc;
    int *dataPtr, i1, ierr, ierr1, ierr2, iloc, iobs, k, mrows;
    ierr = 0;
    if (ldm < 6 || mts == NULL || phi == NULL)
    {
        if (ldm < 6){fprintf(stderr, "%s: ldm must be at least 6\n", __func__);}
        if (mts == NULL){fprintf(stderr, "%s: mts is NULL\n", __func__);}
        if (phi == NULL){fprintf(stderr, "%s: phi is NULL\n", __func__);}
        return -1;
    }
    G = NULL;
    wts = NULL;
    gxxAll = NULL;
    gyyAll = NULL;
    gzzAll = NULL;
    gxyAll = NULL;
    gxzAll = NULL;
    gyzAll = NULL;
    // 6 observations required for full MT, 5 for deviatoric constraint
    if (data.nobs < 6 - (int) lcDev)
    {
        fprintf(stderr, "%s: Insufficient number of observations \n",
                __func__);
        return -1;
    }
    // Figure out the workspace
    dataPtr = memory_calloc32i(data.nobs+1);
    for (iobs=0; iobs<data.nobs; iobs++)
    {
        dataPtr[iobs+1] = dataPtr[iobs] + data.data[iobs].npts;
    }
    if (dataPtr[data.nobs] < 6 - (int) lcDev)
    {
        fprintf(stderr, "%s: Insufficient number of points to invert with\n",
                __func__);
        goto ERROR;
    }
    obsAll = memory_calloc64f(dataPtr[data.nobs]);
    gxxAll = memory_calloc64f(dataPtr[data.nobs]);
    gyyAll = memory_calloc64f(dataPtr[data.nobs]);
    gzzAll = memory_calloc64f(dataPtr[data.nobs]);
    gxyAll = memory_calloc64f(dataPtr[data.nobs]);
    gxzAll = memory_calloc64f(dataPtr[data.nobs]);
    gyzAll = memory_calloc64f(dataPtr[data.nobs]);
    // Loop on locations
    for (iloc=0; iloc<data.nlocs; iloc++)
    {
        // Loop on observations
        ierr1 = 0;
        ierr2 = 0;
        for (iobs=0; iobs<data.nobs; iobs++)
        {
            k = iobs*data.nlocs + iloc;;
            G = parmt_utils_sacGrns2GrnsMat(data.sacGxx[k], data.sacGyy[k],
                                            data.sacGzz[k], data.sacGxy[k],
                                            data.sacGxz[k], data.sacGyz[k],
                                            &mrows, &ierr1);
            memory_free64f(&G);
            if (ierr1 != 0)
            {
                fprintf(stderr, "%s: Failed to make greens functions matrix\n",
                        __func__);
                ierr2 = ierr2 + 1;
            }
            if (mrows != dataPtr[iobs+1] - dataPtr[iobs])
            {
                fprintf(stderr, "%s: Internal error on mrows and npts\n",
                        __func__);
                ierr2 = ierr2 + 1;
                goto NEXT_OBS;
            }
            // Copy the observations and Green's functions
            i1 = dataPtr[iobs];
            cblas_dcopy(mrows, &G[0*mrows], 1, &gxxAll[i1], 1);
            cblas_dcopy(mrows, &G[1*mrows], 1, &gyyAll[i1], 1);
            cblas_dcopy(mrows, &G[2*mrows], 1, &gzzAll[i1], 1);
            cblas_dcopy(mrows, &G[3*mrows], 1, &gxyAll[i1], 1);
            cblas_dcopy(mrows, &G[4*mrows], 1, &gxzAll[i1], 1);
            cblas_dcopy(mrows, &G[5*mrows], 1, &gyzAll[i1], 1);
            cblas_dcopy(mrows, data.data[iobs].data, 1, &obsAll[i1], 1);
NEXT_OBS:;
            memory_free64f(&G);

        } // loop on observations
        // Invert it
        if (ierr2 != 0)
        {
            ierr1 = parmt_invertMT64f(data.nobs,
                                      lcDev, dataPtr,
                                      wts,
                                      gxxAll, gyyAll, gzzAll,
                                      gxyAll, gxzAll, gyzAll,
                                      obsAll, &phi[iloc], &mts[iloc*ldm]);
            //&phiLoc, m6);
            if (ierr1 != 0)
            {
                fprintf(stderr, "%s: Error inverting location %d\n",
                        __func__, iloc);
                ierr = ierr + 1;
            }
        }
        else
        {
            ierr = ierr + 1;
        }
    }
ERROR:;
    memory_free32i(&dataPtr);
    memory_free64f(&gxxAll);
    memory_free64f(&gyyAll);
    memory_free64f(&gzzAll);
    memory_free64f(&gxyAll);
    memory_free64f(&gxzAll);
    memory_free64f(&gyzAll);
    return 0;
}
//============================================================================//
/*!
 * @brief
 *
 * @param[in] nobs     number of waveforms (observations)
 * @param[in] lcDev    if true then force the solution to be purely deviatoric.
 *                     otherwise all moment tensor terms will be inverted for.
 * @param[in] dataPtr  points from iobs'th observation to start index of
 *                     observation [nobs+1]
 * @param[in] wts      if not NULL then these are the data weights for each
 *                     observations [nobs].
 * @param[in] gxxAll   all Gxx greens functions [dataPtr[nobs]]
 * @param[in] gyyAll   all Gyy greens functions [dataPtr[nobs]]
 * @param[in] gzzAll   all Gzz greens functions [dataPtr[nobs]]
 * @param[in] gxyAll   all Gxy greens functions [dataPtr[nobs]]
 * @param[in] gxzAll   all Gxz greens functions [dataPtr[nobs]]
 * @param[in] gyzAll   all Gyz greens functions [dataPtr[nobs]]
 * @param[in] obsAll   observations [dataPtr[nobs]]
 *
 * @param[out] phi     L2 difference between observed and estimate seismograms.
 * @param[out] m       on successful contains the moment tensor elements packed
 *                     \f$\{ m_{xx}, m_{yy}, m_{zz},
 *                           m_{xy}, m_{xz}, m_{yz} \}\f$.
 *
 * @result 0 indicates success
 *
 * @author Ben Baker
 *
 * @copyright ISTI distributed under Apache 2
 *
 */
int parmt_invertMT64f(const int nobs,
                      const bool lcDev,
                      const int *dataPtr,
                      const double *__restrict__ wts,
                      const double *__restrict__ gxxAll,
                      const double *__restrict__ gyyAll,
                      const double *__restrict__ gzzAll,
                      const double *__restrict__ gxyAll,
                      const double *__restrict__ gxzAll,
                      const double *__restrict__ gyzAll,
                      const double *__restrict__ obsAll,
                      double *phi, double *__restrict__ m)
{
    double *G, *b, *est, m6[6];
    int i1, i2, ierr, iobs, ldg, mrows, ncols, nploc;
    *phi = 0.0;
    ierr = 0;
    // Create the forward modeling matrix
    if (nobs < 5 || dataPtr == NULL ||
        gxxAll == NULL || gyyAll == NULL || gzzAll == NULL ||
        gxyAll == NULL || gxzAll == NULL || gyzAll == NULL ||
        m == NULL)
    {
        if (nobs < 5)
        {
            fprintf(stderr, "%s: At least 6 observations required\n", __func__);
        }
        if (dataPtr == NULL)
        {
            fprintf(stderr, "%s: Error dataPtr is NULL\n", __func__);
        }
        if (gxxAll == NULL)
        {
            fprintf(stderr, "%s: Error gxxAll is NULL\n", __func__);
        }
        if (gyyAll == NULL)
        {
            fprintf(stderr, "%s: Error gyyAll is NULL\n", __func__);
        }
        if (gzzAll == NULL)
        {
            fprintf(stderr, "%s: Error gzzAll is NULL\n", __func__);
        }
        if (gxyAll == NULL)
        {
            fprintf(stderr, "%s: Error gxyAll is NULL\n", __func__);
        }
        if (gxzAll == NULL)
        {
            fprintf(stderr, "%s: Error gxzAll is NULL\n", __func__);
        }
        if (gyzAll == NULL)
        {
            fprintf(stderr, "%s: Error gyzAll is NULL\n", __func__);
        }
        if (m == NULL){fprintf(stderr, "%s: Error m is NULL\n", __func__);}
        return -1;
    }
    memset(m, 0, 6*sizeof(double));
    ncols = 6;
    if (lcDev){ncols = 5;}
    // Get the data size
    mrows = 0;
    for (iobs=0; iobs<nobs; iobs++)
    {
        mrows = mrows + dataPtr[iobs-1] - dataPtr[iobs];
    }
    if (mrows < ncols)
    {
        fprintf(stderr, "%s: Error problem is undetermined\n", __func__);
        return -1;
    }
    ldg = mrows;
    G = memory_calloc64f(ldg*ncols);
    b = memory_calloc64f(mrows);
    est = memory_calloc64f(mrows);
    // Set the forward modeling matrix
    if (wts == NULL)
    {
        if (lcDev)
        {
            cblas_dcopy(mrows, gxxAll, 1, &G[0*ldg], 1);
            cblas_daxpy(mrows, -1.0, gzzAll, 1, &G[0*ldg], 1); // mxx - mzz
            cblas_dcopy(mrows, gyyAll, 1, &G[1*ldg], 1);
            cblas_daxpy(mrows, -1.0, gzzAll, 1, &G[1*ldg], 1); // myy - mzz
            cblas_dcopy(mrows, gxyAll, 1, &G[2*ldg], 1);
            cblas_dcopy(mrows, gxzAll, 1, &G[3*ldg], 1);
            cblas_dcopy(mrows, gyzAll, 1, &G[4*ldg], 1);
        }
        else
        {
            cblas_dcopy(mrows, gxxAll, 1, &G[0*ldg], 1);
            cblas_dcopy(mrows, gyyAll, 1, &G[1*ldg], 1);
            cblas_dcopy(mrows, gyyAll, 1, &G[2*ldg], 1);
            cblas_dcopy(mrows, gxyAll, 1, &G[3*ldg], 1);
            cblas_dcopy(mrows, gxzAll, 1, &G[4*ldg], 1);
            cblas_dcopy(mrows, gyzAll, 1, &G[5*ldg], 1);
        }
        cblas_dcopy(mrows, obsAll, 1, b, 1);
    }
    else
    {
        if (lcDev)
        {
            for (iobs=0; iobs<nobs; iobs++)
            {
                i1 = dataPtr[iobs];
                i2 = dataPtr[iobs];
                nploc = i2 - i1;
                cblas_daxpy(nploc, wts[iobs], &gxxAll[i1], 1, &G[0*ldg+i1], 1);
                cblas_daxpy(nploc,-wts[iobs], &gzzAll[i1], 1, &G[0*ldg+i1], 1);
                cblas_daxpy(nploc, wts[iobs], &gyyAll[i1], 1, &G[1*ldg+i1], 1);
                cblas_daxpy(nploc,-wts[iobs], &gzzAll[i1], 1, &G[1*ldg+i1], 1);
                cblas_daxpy(nploc, wts[iobs], &gxyAll[i1], 1, &G[2*ldg+i1], 1);
                cblas_daxpy(nploc, wts[iobs], &gxzAll[i1], 1, &G[3*ldg+i1], 1);
                cblas_daxpy(nploc, wts[iobs], &gyzAll[i1], 1, &G[4*ldg+i1], 1);
                cblas_daxpy(nploc, wts[iobs], &obsAll[i1], 1, &b[i1], 1);
            }
        }
        else
        {
            for (iobs=0; iobs<nobs; iobs++)
            {
                i1 = dataPtr[iobs];
                i2 = dataPtr[iobs];
                nploc = i2 - i1;
                cblas_daxpy(nploc, wts[iobs], &gxxAll[i1], 1, &G[0*ldg+i1], 1);
                cblas_daxpy(nploc, wts[iobs], &gyyAll[i1], 1, &G[1*ldg+i1], 1);
                cblas_daxpy(nploc, wts[iobs], &gzzAll[i1], 1, &G[2*ldg+i1], 1);
                cblas_daxpy(nploc, wts[iobs], &gxyAll[i1], 1, &G[3*ldg+i1], 1);
                cblas_daxpy(nploc, wts[iobs], &gxzAll[i1], 1, &G[4*ldg+i1], 1);
                cblas_daxpy(nploc, wts[iobs], &gyzAll[i1], 1, &G[5*ldg+i1], 1);
                cblas_daxpy(nploc, wts[iobs], &obsAll[i1], 1, &b[i1], 1);
            }
        }
    }
    // Solve \tilde{G} m = \tilde{b}
    ierr = linalg_lstsq_qr64f_work(LAPACK_COL_MAJOR,
                                   mrows, ncols, 1,
                                   false, G, b,
                                   m6, NULL);
    if (ierr != 0)
    {
        fprintf(stderr, "%s: Error solving Gm=u\n", __func__);
        ierr = 1;
        goto END;
    }
    if (lcDev)
    {
        m[0] = m6[0];
        m[1] = m6[1];
        m[2] =-(m6[0] + m6[1]);
        m[3] = m6[2];
        m[4] = m6[3];
        m[5] = m6[4];
    }
    else
    {
        cblas_dcopy(6, m6, 1, m, 1);
    }
    // Apply u = \tilde{G} m
    cblas_dgemv(CblasColMajor, CblasNoTrans, mrows, ncols,
                1.0, G, ncols, m6, 1, 0.0, est, 1);
    // Compute the least-squares residual
    *phi = array_normDiff64f(mrows, est, b, TWO_NORM, 2.0, &ierr);
END:;
    // Release memory
    memory_free64f(&G);
    memory_free64f(&b);
    memory_free64f(&est);
    return ierr;
}