// Copyright (c) 2013-2017 Anton Kozhevnikov, Thomas Schulthess
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without modification, are permitted provided that
// the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the
//    following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions
//    and the following disclaimer in the documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
// WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
// ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
// OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

/** \file sht.h
 *
 *  \brief Contains declaration and particular implementation of sirius::SHT class.
 */

#ifndef __SHT_H__
#define __SHT_H__

#include <math.h>
#include <stddef.h>
#include <gsl/gsl_sf_coupling.h>
#include <gsl/gsl_sf_legendre.h>
#include <string.h>
#include <vector>
#include "typedefs.h"
#include "utils.h"
#include "linalg.hpp"
#include "LebedevLaikov.h"

namespace sirius
{

/// Spherical harmonics transformation.
class SHT // TODO: better name
{
    private:

        int lmax_;

        int lmmax_;

        int num_points_;

        mdarray<double, 2> coord_;

        mdarray<double, 2> tp_;

        std::vector<double> w_;

        /// backward transformation from Ylm to spherical coordinates
        mdarray<double_complex, 2> ylm_backward_;

        /// forward transformation from spherical coordinates to Ylm
        mdarray<double_complex, 2> ylm_forward_;

        /// backward transformation from Rlm to spherical coordinates
        mdarray<double, 2> rlm_backward_;

        /// forward transformation from spherical coordinates to Rlm
        mdarray<double, 2> rlm_forward_;

        int mesh_type_;

    public:

        /// Default constructor.
        SHT(int lmax__)
            : lmax_(lmax__), mesh_type_(0)
        {
            lmmax_ = (lmax_ + 1) * (lmax_ + 1);

            if (mesh_type_ == 0) num_points_ = Lebedev_Laikov_npoint(2 * lmax_);
            if (mesh_type_ == 1) num_points_ = lmmax_;

            std::vector<double> x(num_points_);
            std::vector<double> y(num_points_);
            std::vector<double> z(num_points_);

            coord_ = mdarray<double, 2>(3, num_points_);

            tp_ = mdarray<double, 2>(2, num_points_);

            w_.resize(num_points_);

            if (mesh_type_ == 0) Lebedev_Laikov_sphere(num_points_, &x[0], &y[0], &z[0], &w_[0]);
            if (mesh_type_ == 1) uniform_coverage();

            ylm_backward_ = mdarray<double_complex, 2>(lmmax_, num_points_);

            ylm_forward_ = mdarray<double_complex, 2>(num_points_, lmmax_);

            rlm_backward_ = mdarray<double, 2>(lmmax_, num_points_);

            rlm_forward_ = mdarray<double, 2>(num_points_, lmmax_);

            for (int itp = 0; itp < num_points_; itp++)
            {
                if (mesh_type_ == 0)
                {
                    coord_(0, itp) = x[itp];
                    coord_(1, itp) = y[itp];
                    coord_(2, itp) = z[itp];

                    auto vs = spherical_coordinates(vector3d<double>(x[itp], y[itp], z[itp]));
                    spherical_harmonics(lmax_, vs[1], vs[2], &ylm_backward_(0, itp));
                    spherical_harmonics(lmax_, vs[1], vs[2], &rlm_backward_(0, itp));
                    for (int lm = 0; lm < lmmax_; lm++)
                    {
                        ylm_forward_(itp, lm) = conj(ylm_backward_(lm, itp)) * w_[itp] * fourpi;
                        rlm_forward_(itp, lm) = rlm_backward_(lm, itp) * w_[itp] * fourpi;
                    }
                }
                if (mesh_type_ == 1)
                {
                    double t = tp_(0, itp);
                    double p = tp_(1, itp);

                    coord_(0, itp) = sin(t) * cos(p);
                    coord_(1, itp) = sin(t) * sin(p);
                    coord_(2, itp) = cos(t);

                    spherical_harmonics(lmax_, t, p, &ylm_backward_(0, itp));
                    spherical_harmonics(lmax_, t, p, &rlm_backward_(0, itp));

                    for (int lm = 0; lm < lmmax_; lm++)
                    {
                        ylm_forward_(lm, itp) = ylm_backward_(lm, itp);
                        rlm_forward_(lm, itp) = rlm_backward_(lm, itp);
                    }
                }
            }

            if (mesh_type_ == 1)
            {
                linalg<CPU>::geinv(lmmax_, ylm_forward_);
                linalg<CPU>::geinv(lmmax_, rlm_forward_);
            }

            #if (__VERIFICATION > 0)
            {
                double dr = 0;
                double dy = 0;

                for (int lm = 0; lm < lmmax_; lm++)
                {
                    for (int lm1 = 0; lm1 < lmmax_; lm1++)
                    {
                        double t = 0;
                        double_complex zt(0, 0);
                        for (int itp = 0; itp < num_points_; itp++)
                        {
                            zt += ylm_forward_(itp, lm) * ylm_backward_(lm1, itp);
                            t += rlm_forward_(itp, lm) * rlm_backward_(lm1, itp);
                        }

                        if (lm == lm1)
                        {
                            zt -= 1.0;
                            t -= 1.0;
                        }
                        dr += std::abs(t);
                        dy += std::abs(zt);
                    }
                }
                dr = dr / lmmax_ / lmmax_;
                dy = dy / lmmax_ / lmmax_;

                if (dr > 1e-15 || dy > 1e-15)
                {
                    std::stringstream s;
                    s << "spherical mesh error is too big" << std::endl
                      << "  real spherical integration error " << dr << std::endl
                      << "  complex spherical integration error " << dy;
                    WARNING(s.str())
                }

                std::vector<double> flm(lmmax_);
                std::vector<double> ftp(num_points_);
                for (int lm = 0; lm < lmmax_; lm++)
                {
                    std::memset(&flm[0], 0, lmmax_ * sizeof(double));
                    flm[lm] = 1.0;
                    backward_transform(lmmax_, &flm[0], 1, lmmax_, &ftp[0]);
                    forward_transform(&ftp[0], 1, lmmax_, lmmax_, &flm[0]);
                    flm[lm] -= 1.0;

                    double t = 0.0;
                    for (int lm1 = 0; lm1 < lmmax_; lm1++) t += std::abs(flm[lm1]);

                    t /= lmmax_;

                    if (t > 1e-15)
                    {
                        std::stringstream s;
                        s << "test of backward / forward real SHT failed" << std::endl
                          << "  total error " << t;
                        WARNING(s.str());
                    }
                }
            }
            #endif
        }

        /// Perform a backward transformation from spherical harmonics to spherical coordinates.
        /** \f[
         *      f(\theta, \phi, r) = \sum_{\ell m} f_{\ell m}(r) Y_{\ell m}(\theta, \phi)
         *  \f]
         *
         *  \param [in] ld Size of leading dimension of flm.
         *  \param [in] flm Raw pointer to \f$ f_{\ell m}(r) \f$.
         *  \param [in] nr Number of radial points.
         *  \param [in] lmmax Maximum number of lm- harmonics to take into sum.
         *  \param [out] ftp Raw pointer to \f$ f(\theta, \phi, r) \f$.
         */
        template <typename T>
        void backward_transform(int ld, T const* flm, int nr, int lmmax, T* ftp);

        /// Perform a forward transformation from spherical coordinates to spherical harmonics.
        /** \f[
         *      f_{\ell m}(r) = \iint  f(\theta, \phi, r) Y_{\ell m}^{*}(\theta, \phi) \sin \theta d\phi d\theta =
         *        \sum_{i} f(\theta_i, \phi_i, r) Y_{\ell m}^{*}(\theta_i, \phi_i) w_i
         *  \f]
         *
         *  \param [in] ftp Raw pointer to \f$ f(\theta, \phi, r) \f$.
         *  \param [in] nr Number of radial points.
         *  \param [in] lmmax Maximum number of lm- coefficients to generate.
         *  \param [in] ld Size of leading dimension of flm.
         *  \param [out] flm Raw pointer to \f$ f_{\ell m}(r) \f$.
         */
        template <typename T>
        void forward_transform(T const* ftp, int nr, int lmmax, int ld, T* flm);

        /// Convert form Rlm to Ylm representation.
        static void convert(int lmax__, double const* f_rlm__, double_complex* f_ylm__);

        /// Convert from Ylm to Rlm representation.
        static void convert(int lmax__, double_complex const* f_ylm__, double* f_rlm__);

        //void rlm_forward_iterative_transform(double *ftp__, int lmmax, int ncol, double* flm)
        //{
        //    Timer t("sirius::SHT::rlm_forward_iterative_transform");
        //
        //    assert(lmmax <= lmmax_);

        //    mdarray<double, 2> ftp(ftp__, num_points_, ncol);
        //    mdarray<double, 2> ftp1(num_points_, ncol);
        //
        //    blas<cpu>::gemm(1, 0, lmmax, ncol, num_points_, 1.0, &rlm_forward_(0, 0), num_points_, &ftp(0, 0), num_points_, 0.0,
        //                    flm, lmmax);
        //
        //    for (int i = 0; i < 2; i++)
        //    {
        //        rlm_backward_transform(flm, lmmax, ncol, &ftp1(0, 0));
        //        double tdiff = 0.0;
        //        for (int ir = 0; ir < ncol; ir++)
        //        {
        //            for (int itp = 0; itp < num_points_; itp++)
        //            {
        //                ftp1(itp, ir) = ftp(itp, ir) - ftp1(itp, ir);
        //                //tdiff += fabs(ftp1(itp, ir));
        //            }
        //        }
        //
        //        for (int itp = 0; itp < num_points_; itp++)
        //        {
        //            tdiff += fabs(ftp1(itp, ncol - 1));
        //        }
        //        std::cout << "iter : " << i << " avg. MT diff = " << tdiff / num_points_ << std::endl;
        //        blas<cpu>::gemm(1, 0, lmmax, ncol, num_points_, 1.0, &rlm_forward_(0, 0), num_points_, &ftp1(0, 0), num_points_, 1.0,
        //                        flm, lmmax);
        //    }
        //}

        /// Transform Cartesian coordinates [x,y,z] to spherical coordinates [r,theta,phi]
        static vector3d<double> spherical_coordinates(vector3d<double> vc);

        /// Generate complex spherical harmonics Ylm
        static void spherical_harmonics(int lmax, double theta, double phi, double_complex* ylm);

        /// Generate real spherical harmonics Rlm
        /** Mathematica code:
         *  \verbatim
         *  R[l_, m_, th_, ph_] :=
         *   If[m > 0, std::sqrt[2]*ComplexExpand[Re[SphericalHarmonicY[l, m, th, ph]]],
         *   If[m < 0, std::sqrt[2]*ComplexExpand[Im[SphericalHarmonicY[l, m, th, ph]]],
         *   If[m == 0, ComplexExpand[Re[SphericalHarmonicY[l, 0, th, ph]]]]]]
         *  \endverbatim
         */
        static void spherical_harmonics(int lmax, double theta, double phi, double* rlm);

        /// Compute element of the transformation matrix from complex to real spherical harmonics.
        /** Real spherical harmonic can be written as a linear combination of complex harmonics:
         *
         *    \f[
         *        R_{\ell m}(\theta, \phi) = \sum_{m'} a^{\ell}_{m' m}Y_{\ell m'}(\theta, \phi)
         *    \f]
         *    where
         *    \f[
         *        a^{\ell}_{m' m} = \langle Y_{\ell m'} | R_{\ell m} \rangle
         *    \f]
         *
         *    Transformation from real to complex spherical harmonics is conjugate transpose:
         *
         *    \f[
         *        Y_{\ell m}(\theta, \phi) = \sum_{m'} a^{\ell*}_{m m'}R_{\ell m'}(\theta, \phi)
         *    \f]
         *
         *    Mathematica code:
         *    \verbatim
         *    b[m1_, m2_] :=
         *     If[m1 == 0, 1,
         *     If[m1 < 0 && m2 < 0, -I/std::sqrt[2],
         *     If[m1 > 0 && m2 < 0, (-1)^m1*I/std::sqrt[2],
         *     If[m1 < 0 && m2 > 0, (-1)^m2/std::sqrt[2],
         *     If[m1 > 0 && m2 > 0, 1/std::sqrt[2]]]]]]
         *
         *    a[m1_, m2_] := If[Abs[m1] == Abs[m2], b[m1, m2], 0]
         *
         *    R[l_, m_, t_, p_] := Sum[a[m1, m]*SphericalHarmonicY[l, m1, t, p], {m1, -l, l}]
         *    \endverbatim
         */
        static inline double_complex ylm_dot_rlm(int l, int m1, int m2)
        {
            const double isqrt2 = 0.70710678118654752440;

            assert(l >= 0 && std::abs(m1) <= l && std::abs(m2) <= l);

            if (!((m1 == m2) || (m1 == -m2))) {
                return double_complex(0, 0);
            }

            if (m1 == 0) {
                return double_complex(1, 0);
            }

            if (m1 < 0) {
                if (m2 < 0) {
                    return -double_complex(0, isqrt2);
                } else {
                    return std::pow(-1.0, m2) * double_complex(isqrt2, 0);
                }
            } else {
                if (m2 < 0) {
                    return pow(-1.0, m1) * double_complex(0, isqrt2);
                } else {
                    return double_complex(isqrt2, 0);
                }
            }
        }

        static inline double_complex rlm_dot_ylm(int l, int m1, int m2)
        {
            return std::conj(ylm_dot_rlm(l, m2, m1));
        }

        /// Gaunt coefficent of three complex spherical harmonics.
        /**
         *  \f[
         *    \langle Y_{\ell_1 m_1} | Y_{\ell_2 m_2} | Y_{\ell_3 m_3} \rangle
         *  \f]
         */
        static double gaunt_ylm(int l1, int l2, int l3, int m1, int m2, int m3);

        /// Gaunt coefficent of three real spherical harmonics.
        /**
         *  \f[
         *    \langle R_{\ell_1 m_1} | R_{\ell_2 m_2} | R_{\ell_3 m_3} \rangle
         *  \f]
         */
        static double gaunt_rlm(int l1, int l2, int l3, int m1, int m2, int m3);

        /// Gaunt coefficent of two complex and one real spherical harmonics.
        /**
         *  \f[
         *    \langle Y_{\ell_1 m_1} | R_{\ell_2 m_2} | Y_{\ell_3 m_3} \rangle
         *  \f]
         */
        static double_complex gaunt_hybrid(int l1, int l2, int l3, int m1, int m2, int m3);

        void uniform_coverage();

        /// Return Clebsch-Gordan coefficient.
        /** Clebsch-Gordan coefficients arise when two angular momenta are combined into a
         *  total angular momentum.
         */
        static inline double clebsch_gordan(int l1, int l2, int l3, int m1, int m2, int m3)
        {
            assert(l1 >= 0);
            assert(l2 >= 0);
            assert(l3 >= 0);
            assert(m1 >= -l1 && m1 <= l1);
            assert(m2 >= -l2 && m2 <= l2);
            assert(m3 >= -l3 && m3 <= l3);

            return pow(-1, l1 - l2 + m3) * sqrt(double(2 * l3 + 1)) *
                   gsl_sf_coupling_3j(2 * l1, 2 * l2, 2 * l3, 2 * m1, 2 * m2, -2 * m3);
        }

        inline double_complex ylm_backward(int lm,  int itp)
        {
            return ylm_backward_(lm, itp);
        }

        inline double rlm_backward(int lm,  int itp)
        {
            return rlm_backward_(lm, itp);
        }

        inline double coord(int x, int itp)
        {
            return coord_(x, itp);
        }

        inline int num_points()
        {
            return num_points_;
        }

        inline int lmax()
        {
            return lmax_;
        }

        inline int lmmax()
        {
            return lmmax_;
        }

        static void wigner_d_matrix(int l, double beta, mdarray<double, 2>& d_mtrx__)
        {
            long double cos_b2 = std::cos((long double)beta / 2.0L);
            long double sin_b2 = std::sin((long double)beta / 2.0L);

            for (int m1 = -l; m1 <= l; m1++)
            {
                for (int m2 = -l; m2 <= l; m2++)
                {
                    long double d = 0;
                    for (int j = 0; j <= std::min(l + m1, l - m2); j++)
                    {
                        if ((l - m2 - j) >= 0 && (l + m1 - j) >= 0 && (j + m2 - m1) >= 0)
                        {
                            long double g = (std::sqrt(Utils::factorial(l + m1)) / Utils::factorial(l - m2 - j)) *
                                            (std::sqrt(Utils::factorial(l - m1)) / Utils::factorial(l + m1 - j)) *
                                            (std::sqrt(Utils::factorial(l - m2)) / Utils::factorial(j + m2 - m1)) *
                                            (std::sqrt(Utils::factorial(l + m2)) / Utils::factorial(j));
                            d += g * std::pow(-1, j) * std::pow(cos_b2, 2 * l + m1 - m2 - 2 * j) * std::pow(sin_b2, 2 * j + m2 - m1);
                        }
                    }
                    d_mtrx__(m1 + l, m2 + l) = (double)d;
                }
            }
        }

        static void rotation_matrix_l(int l, vector3d<double> euler_angles, int proper_rotation,
                                      double_complex* rot_mtrx__, int ld)
        {
            mdarray<double_complex, 2> rot_mtrx(rot_mtrx__, ld, 2 * l + 1);

            mdarray<double, 2> d_mtrx(2 * l + 1, 2 * l + 1);
            wigner_d_matrix(l, euler_angles[1], d_mtrx);

            for (int m1 = -l; m1 <= l; m1++)
            {
                for (int m2 = -l; m2 <= l; m2++)
                {
                    rot_mtrx(m1 + l, m2 + l) = std::exp(double_complex(0, -euler_angles[0] * m1 - euler_angles[2] * m2)) *
                                               d_mtrx(m1 + l, m2 + l) * std::pow(proper_rotation, l);
                }
            }
        }

        static void rotation_matrix_l(int l, vector3d<double> euler_angles, int proper_rotation,
                                      double* rot_mtrx__, int ld)
        {
            mdarray<double, 2> rot_mtrx_rlm(rot_mtrx__, ld, 2 * l + 1);
            mdarray<double_complex, 2> rot_mtrx_ylm(2 * l + 1, 2 * l + 1);

            mdarray<double, 2> d_mtrx(2 * l + 1, 2 * l + 1);
            wigner_d_matrix(l, euler_angles[1], d_mtrx);

            for (int m1 = -l; m1 <= l; m1++)
            {
                for (int m2 = -l; m2 <= l; m2++)
                {
                    rot_mtrx_ylm(m1 + l, m2 + l) = std::exp(double_complex(0, -euler_angles[0] * m1 - euler_angles[2] * m2)) *
                                                   d_mtrx(m1 + l, m2 + l) * std::pow(proper_rotation, l);
                }
            }
            for (int m1 = -l; m1 <= l; m1++)
            {
                auto i13 = (m1 == 0) ? std::vector<int>({0}) : std::vector<int>({-m1, m1});

                for (int m2 = -l; m2 <= l; m2++)
                {
                    auto i24 = (m2 == 0) ? std::vector<int>({0}) : std::vector<int>({-m2, m2});

                    for (int m3: i13)
                    {
                        for (int m4: i24)
                        {
                            rot_mtrx_rlm(m1 + l, m2 + l) += std::real(rlm_dot_ylm(l, m1, m3) *
                                                                      rot_mtrx_ylm(m3 + l, m4 + l) *
                                                                      ylm_dot_rlm(l, m4, m2));
                        }
                    }
                }
            }
        }

        static void rotation_matrix(int lmax, vector3d<double> euler_angles, int proper_rotation,
                                    mdarray<double_complex, 2>& rotm)
        {
            rotm.zero();

            for (int l = 0; l <= lmax; l++)
            {
                rotation_matrix_l(l, euler_angles, proper_rotation, &rotm(l * l, l * l), rotm.ld());
            }
        }

        static void rotation_matrix(int lmax, vector3d<double> euler_angles, int proper_rotation,
                                    mdarray<double, 2>& rotm)
        {
            rotm.zero();

            for (int l = 0; l <= lmax; l++)
            {
                rotation_matrix_l(l, euler_angles, proper_rotation, &rotm(l * l, l * l), rotm.ld());
            }
        }

        /// Compute derivative of real-spherical harmonic with respect to theta angle.
        static double dRlm_dtheta(int lm, double theta, double phi)
        {
            switch (lm) {
                case 0: return 0;

                case 1: return -(std::sqrt(3/pi)*std::cos(theta)*std::sin(phi))/2.;

                case 2: return -(std::sqrt(3/pi)*std::sin(theta))/2.;

                case 3: return -(std::sqrt(3/pi)*std::cos(phi)*std::cos(theta))/2.;

                case 4: return -(std::sqrt(15/pi)*std::cos(phi)*std::cos(theta)*std::sin(phi)*std::sin(theta));

                case 5: return -(std::sqrt(15/pi)*std::cos(2*theta)*std::sin(phi))/2.;

                case 6: return (-3*std::sqrt(5/pi)*std::cos(theta)*std::sin(theta))/2.;

                case 7: return -(std::sqrt(15/pi)*std::cos(phi)*std::cos(2*theta))/2.;

                case 8: return (std::sqrt(15/pi)*std::cos(2*phi)*std::sin(2*theta))/4.;

                case 9: return (-3*std::sqrt(35/(2.*pi))*std::cos(theta)*std::sin(3*phi)*std::pow(std::sin(theta),2))/4.;

                case 10: return (std::sqrt(105/pi)*std::sin(2*phi)*(std::sin(theta) - 3*std::sin(3*theta)))/16.;

                case 11: return (std::sqrt(21/(2.*pi))*std::cos(theta)*(7 - 15*std::cos(2*theta))*std::sin(phi))/8.;

                case 12: return (-3*std::sqrt(7/pi)*(3 + 5*std::cos(2*theta))*std::sin(theta))/8.;

                case 13: return (std::sqrt(21/(2.*pi))*std::cos(phi)*std::cos(theta)*(7 - 15*std::cos(2*theta)))/8.;

                case 14: return (std::sqrt(105/pi)*std::cos(2*phi)*(1 + 3*std::cos(2*theta))*std::sin(theta))/8.;

                case 15: return (-3*std::sqrt(35/(2.*pi))*std::cos(3*phi)*std::cos(theta)*std::pow(std::sin(theta),2))/4.;

                case 16: return (-3*std::sqrt(35/pi)*std::cos(theta)*std::sin(4*phi)*std::pow(std::sin(theta),3))/4.;

                case 17: return (-3*std::sqrt(35/(2.*pi))*(1 + 2*std::cos(2*theta))*std::sin(3*phi)*std::pow(std::sin(theta),2))/4.;

                case 18: return (3*std::sqrt(5/pi)*(1 - 7*std::cos(2*theta))*std::sin(2*phi)*std::sin(2*theta))/8.;

                case 19: return (-3*std::sqrt(5/(2.*pi))*(std::cos(2*theta) + 7*std::cos(4*theta))*std::sin(phi))/8.;

                case 20: return (15*std::cos(theta)*(3 - 7*std::pow(std::cos(theta),2))*std::sin(theta))/(4.*std::sqrt(pi));

                case 21: return (-3*std::sqrt(5/(2.*pi))*std::cos(phi)*(std::cos(2*theta) + 7*std::cos(4*theta)))/8.;

                case 22: return (3*std::sqrt(5/pi)*std::cos(2*phi)*(-2*std::sin(2*theta) + 7*std::sin(4*theta)))/16.;

                case 23: return (-3*std::sqrt(35/(2.*pi))*std::cos(3*phi)*(1 + 2*std::cos(2*theta))*std::pow(std::sin(theta),2))/4.;

                case 24: return (3*std::sqrt(35/pi)*std::cos(4*phi)*std::cos(theta)*std::pow(std::sin(theta),3))/4.;

                default: {
                    TERMINATE_NOT_IMPLEMENTED
                }
            }
            return 0; // make compiler happy
        }

        ///  Compute derivative of real-spherical harmonic with respect to phi angle and divide by sin(theta).
        static double dRlm_dphi_sin_theta(int lm, double theta, double phi)
        {
            switch (lm) {
                case 0: return 0;

                case 1: return -(std::sqrt(3/pi)*std::cos(phi))/2.;

                case 2: return 0;

                case 3: return (std::sqrt(3/pi)*std::sin(phi))/2.;

                case 4: return -(std::sqrt(15/pi)*std::cos(2*phi)*std::sin(theta))/2.;

                case 5: return -(std::sqrt(15/pi)*std::cos(phi)*std::cos(theta))/2.;

                case 6: return 0;

                case 7: return (std::sqrt(15/pi)*std::cos(theta)*std::sin(phi))/2.;

                case 8: return -(std::sqrt(15/pi)*std::cos(phi)*std::sin(phi)*std::sin(theta));

                case 9: return (-3*std::sqrt(35/(2.*pi))*std::cos(3*phi)*std::pow(std::sin(theta),2))/4.;

                case 10: return -(std::sqrt(105/pi)*std::cos(2*phi)*std::sin(2*theta))/4.;

                case 11: return -(std::sqrt(21/(2.*pi))*std::cos(phi)*(3 + 5*std::cos(2*theta)))/8.;

                case 12: return 0;

                case 13: return (std::sqrt(21/(2.*pi))*(3 + 5*std::cos(2*theta))*std::sin(phi))/8.;

                case 14: return -(std::sqrt(105/pi)*std::cos(phi)*std::cos(theta)*std::sin(phi)*std::sin(theta));

                case 15: return (3*std::sqrt(35/(2.*pi))*std::sin(3*phi)*std::pow(std::sin(theta),2))/4.;

                case 16: return (-3*std::sqrt(35/pi)*std::cos(4*phi)*std::pow(std::sin(theta),3))/4.;

                case 17: return (-9*std::sqrt(35/(2.*pi))*std::cos(3*phi)*std::cos(theta)*std::pow(std::sin(theta),2))/4.;

                case 18: return (-3*std::sqrt(5/pi)*std::cos(2*phi)*(3*std::sin(theta) + 7*std::sin(3*theta)))/16.;

                case 19: return (-3*std::sqrt(5/(2.*pi))*std::cos(phi)*(9*std::cos(theta) + 7*std::cos(3*theta)))/16.;

                case 20: return 0;

                case 21: return (3*std::sqrt(5/(2.*pi))*std::cos(theta)*(1 + 7*std::cos(2*theta))*std::sin(phi))/8.;

                case 22: return (-3*std::sqrt(5/pi)*std::sin(2*phi)*(3*std::sin(theta) + 7*std::sin(3*theta)))/16.;

                case 23: return (9*std::sqrt(35/(2.*pi))*std::cos(theta)*std::sin(3*phi)*std::pow(std::sin(theta),2))/4.;

                case 24: return (-3*std::sqrt(35/pi)*std::sin(4*phi)*std::pow(std::sin(theta),3))/4.;

                default: {
                    TERMINATE_NOT_IMPLEMENTED
                }
            }
            return 0; // make compiler happy
        }
};

template <>
inline void SHT::backward_transform<double>(int ld, double const* flm, int nr, int lmmax, double* ftp)
{
    assert(lmmax <= lmmax_);
    assert(ld >= lmmax);
    linalg<CPU>::gemm(1, 0, num_points_, nr, lmmax, &rlm_backward_(0, 0), lmmax_, flm, ld, ftp, num_points_);
}

template <>
inline void SHT::backward_transform<double_complex>(int ld, double_complex const* flm, int nr, int lmmax, double_complex* ftp)
{
    assert(lmmax <= lmmax_);
    assert(ld >= lmmax);
    linalg<CPU>::gemm(1, 0, num_points_, nr, lmmax, &ylm_backward_(0, 0), lmmax_, flm, ld, ftp, num_points_);
}

template <>
inline void SHT::forward_transform<double>(double const* ftp, int nr, int lmmax, int ld, double* flm)
{
    assert(lmmax <= lmmax_);
    assert(ld >= lmmax);
    linalg<CPU>::gemm(1, 0, lmmax, nr, num_points_, &rlm_forward_(0, 0), num_points_, ftp, num_points_, flm, ld);
}

template <>
inline void SHT::forward_transform<double_complex>(double_complex const* ftp, int nr, int lmmax, int ld, double_complex* flm)
{
    assert(lmmax <= lmmax_);
    assert(ld >= lmmax);
    linalg<CPU>::gemm(1, 0, lmmax, nr, num_points_, &ylm_forward_(0, 0), num_points_, ftp, num_points_, flm, ld);
}

inline double SHT::gaunt_ylm(int l1, int l2, int l3, int m1, int m2, int m3)
{
    assert(l1 >= 0);
    assert(l2 >= 0);
    assert(l3 >= 0);
    assert(m1 >= -l1 && m1 <= l1);
    assert(m2 >= -l2 && m2 <= l2);
    assert(m3 >= -l3 && m3 <= l3);

    return std::pow(-1.0, std::abs(m1)) * std::sqrt(double(2 * l1 + 1) * double(2 * l2 + 1) * double(2 * l3 + 1) / fourpi) *
           gsl_sf_coupling_3j(2 * l1, 2 * l2, 2 * l3, 0, 0, 0) *
           gsl_sf_coupling_3j(2 * l1, 2 * l2, 2 * l3, -2 * m1, 2 * m2, 2 * m3);
}

inline double SHT::gaunt_rlm(int l1, int l2, int l3, int m1, int m2, int m3)
{
    assert(l1 >= 0);
    assert(l2 >= 0);
    assert(l3 >= 0);
    assert(m1 >= -l1 && m1 <= l1);
    assert(m2 >= -l2 && m2 <= l2);
    assert(m3 >= -l3 && m3 <= l3);

    double d = 0;
    for (int k1 = -l1; k1 <= l1; k1++)
    {
        for (int k2 = -l2; k2 <= l2; k2++)
        {
            for (int k3 = -l3; k3 <= l3; k3++)
            {
                d += std::real(std::conj(SHT::ylm_dot_rlm(l1, k1, m1)) *
                                         SHT::ylm_dot_rlm(l2, k2, m2) *
                                         SHT::ylm_dot_rlm(l3, k3, m3)) * SHT::gaunt_ylm(l1, l2, l3, k1, k2, k3);
            }
        }
    }
    return d;
}

inline double_complex SHT::gaunt_hybrid(int l1, int l2, int l3, int m1, int m2, int m3)
{
    assert(l1 >= 0);
    assert(l2 >= 0);
    assert(l3 >= 0);
    assert(m1 >= -l1 && m1 <= l1);
    assert(m2 >= -l2 && m2 <= l2);
    assert(m3 >= -l3 && m3 <= l3);

    if (m2 == 0)
    {
        return double_complex(gaunt_ylm(l1, l2, l3, m1, m2, m3), 0.0);
    }
    else
    {
        return (ylm_dot_rlm(l2, m2, m2) * gaunt_ylm(l1, l2, l3, m1, m2, m3) +
                ylm_dot_rlm(l2, -m2, m2) * gaunt_ylm(l1, l2, l3, m1, -m2, m3));
    }
}

inline vector3d<double> SHT::spherical_coordinates(vector3d<double> vc)
{
    vector3d<double> vs;

    double eps = 1e-12;

    vs[0] = vc.length();

    if (vs[0] <= eps)
    {
        vs[1] = 0.0;
        vs[2] = 0.0;
    }
    else
    {
        vs[1] = std::acos(vc[2] / vs[0]); // theta = cos^{-1}(z/r)

        if (std::abs(vc[0]) > eps || std::abs(vc[1]) > eps)
        {
            vs[2] = std::atan2(vc[1], vc[0]); // phi = tan^{-1}(y/x)
            if (vs[2] < 0.0) vs[2] += twopi;
        }
        else
        {
            vs[2] = 0.0;
        }
    }

    return vs;
}

inline void SHT::spherical_harmonics(int lmax, double theta, double phi, double_complex* ylm)
{
    double x = std::cos(theta);
    std::vector<double> result_array(lmax + 1);

    for (int l = 0; l <= lmax; l++)
    {
        for (int m = 0; m <= l; m++)
        {
            double_complex z = std::exp(double_complex(0.0, m * phi));
            ylm[Utils::lm_by_l_m(l, m)] = gsl_sf_legendre_sphPlm(l, m, x) * z;
            if (m % 2)
            {
                ylm[Utils::lm_by_l_m(l, -m)] = -std::conj(ylm[Utils::lm_by_l_m(l, m)]);
            }
            else
            {
                ylm[Utils::lm_by_l_m(l, -m)] = std::conj(ylm[Utils::lm_by_l_m(l, m)]);
            }
        }
    }
}

inline void SHT::spherical_harmonics(int lmax, double theta, double phi, double* rlm)
{
    int lmmax = (lmax + 1) * (lmax + 1);
    std::vector<double_complex> ylm(lmmax);
    spherical_harmonics(lmax, theta, phi, &ylm[0]);

    double t = std::sqrt(2.0);

    rlm[0] = y00;

    for (int l = 1; l <= lmax; l++)
    {
        for (int m = -l; m < 0; m++)
            rlm[Utils::lm_by_l_m(l, m)] = t * ylm[Utils::lm_by_l_m(l, m)].imag();

        rlm[Utils::lm_by_l_m(l, 0)] = ylm[Utils::lm_by_l_m(l, 0)].real();

        for (int m = 1; m <= l; m++)
            rlm[Utils::lm_by_l_m(l, m)] = t * ylm[Utils::lm_by_l_m(l, m)].real();
    }
}

inline void SHT::uniform_coverage()
{
    tp_(0, 0) = pi;
    tp_(1, 0) = 0;

    for (int k = 1; k < num_points_ - 1; k++)
    {
        double hk = -1.0 + double(2 * k) / double(num_points_ - 1);
        tp_(0, k) = acos(hk);
        double t = tp_(1, k - 1) + 3.80925122745582 / sqrt(double(num_points_)) / sqrt(1 - hk * hk);
        tp_(1, k) = fmod(t, twopi);
    }

    tp_(0, num_points_ - 1) = 0;
    tp_(1, num_points_ - 1) = 0;
}

inline void SHT::convert(int lmax__, double const* f_rlm__, double_complex* f_ylm__)
{
    int lm = 0;
    for (int l = 0; l <= lmax__; l++)
    {
        for (int m = -l; m <= l; m++)
        {
            if (m == 0)
            {
                f_ylm__[lm] = f_rlm__[lm];
            }
            else
            {
                int lm1 = Utils::lm_by_l_m(l, -m);
                f_ylm__[lm] = ylm_dot_rlm(l, m, m) * f_rlm__[lm] + ylm_dot_rlm(l, m, -m) * f_rlm__[lm1];
            }
            lm++;
        }
    }
}

inline void SHT::convert(int lmax__, double_complex const* f_ylm__, double* f_rlm__)
{
    int lm = 0;
    for (int l = 0; l <= lmax__; l++)
    {
        for (int m = -l; m <= l; m++)
        {
            if (m == 0)
            {
                f_rlm__[lm] = std::real(f_ylm__[lm]);
            }
            else
            {
                int lm1 = Utils::lm_by_l_m(l, -m);
                f_rlm__[lm] = std::real(rlm_dot_ylm(l, m, m) * f_ylm__[lm] + rlm_dot_ylm(l, m, -m) * f_ylm__[lm1]);
            }
            lm++;
        }
    }
}

};

#endif // __SHT_H__


/* multifit_nlinear/svd.c
 *
 * Copyright (C) 2016 Patrick Alken
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

/*
 * This module handles the solution of the linear least squares
 * system:
 *
 * [     J      ] dx = - [ f ]
 * [ sqrt(mu)*D ]        [ 0 ]
 *
 * using an SVD approach. The system above is transformed to "standard form"
 * via:
 *
 * J~ = J D^{-1}
 * dx~ = D dx
 *
 * so that
 *
 * [     J~     ] dx~ = - [ f ]
 * [ sqrt(mu)*I ]         [ 0 ]
 *
 * can be solved with a standard SVD method, and then dx is recovered
 * from dx~ via: dx = D^{-1} dx~
 */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_multifit_nlinear.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_blas.h>

typedef struct
{
  size_t n;                  /* number of residuals */
  size_t p;                  /* number of parameters */
  gsl_matrix *U;             /* U factor of J, n-by-p */
  gsl_matrix *V;             /* V factor of J, p-by-p */
  gsl_vector *S;             /* singular values, size p */
  gsl_vector *workp;         /* workspace, length p */
  double mu;                 /* LM parameter */
} svd_state_t;

static int svd_init(const void * vtrust_state, void * vstate);
static int svd_presolve(const double mu, const void * vtrust_state, void * vstate);
static int svd_solve(const gsl_vector * f, gsl_vector *x,
                     const void * vtrust_state, void *vstate);
static int svd_rcond(double * rcond, void * vstate);

static void *
svd_alloc (const size_t n, const size_t p)
{
  svd_state_t *state;

  (void)n;

  state = calloc(1, sizeof(svd_state_t));
  if (state == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate svd state", GSL_ENOMEM);
    }

  state->U = gsl_matrix_alloc(n, p);
  if (state->U == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for U", GSL_ENOMEM);
    }

  state->V = gsl_matrix_alloc(p, p);
  if (state->V == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for V", GSL_ENOMEM);
    }

  state->S = gsl_vector_alloc(p);
  if (state->S == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for S",
                      GSL_ENOMEM);
    }

  state->workp = gsl_vector_alloc(p);
  if (state->workp == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for workp",
                      GSL_ENOMEM);
    }

  state->mu = 0.0;
  state->n = n;
  state->p = p;

  return state;
}

static void
svd_free(void *vstate)
{
  svd_state_t *state = (svd_state_t *) vstate;

  if (state->U)
    gsl_matrix_free(state->U);

  if (state->V)
    gsl_matrix_free(state->V);

  if (state->S)
    gsl_vector_free(state->S);

  if (state->workp)
    gsl_vector_free(state->workp);

  free(state);
}

/* compute svd of J */
static int
svd_init(const void * vtrust_state, void * vstate)
{
  int status;
  const gsl_multifit_nlinear_trust_state *trust_state =
    (const gsl_multifit_nlinear_trust_state *) vtrust_state;
  svd_state_t *state = (svd_state_t *) vstate;
  size_t i;

  gsl_matrix_set_zero(state->U);

  /* compute U = J D^{-1} */
  for (i = 0; i < state->p; ++i)
    {
      gsl_vector_const_view Ji = gsl_matrix_const_column(trust_state->J, i);
      gsl_vector_view ui = gsl_matrix_column(state->U, i);
      double di = gsl_vector_get(trust_state->diag, i);

      gsl_blas_daxpy(1.0 / di, &Ji.vector, &ui.vector);
    }

  status = gsl_linalg_SV_decomp(state->U, state->V, state->S, state->workp);

  return status;
}

static int
svd_presolve(const double mu, const void * vtrust_state, void * vstate)
{
  svd_state_t *state = (svd_state_t *) vstate;

  state->mu = mu;

  (void)vtrust_state;

  return GSL_SUCCESS;
}

static int
svd_solve(const gsl_vector * f, gsl_vector *x,
          const void * vtrust_state, void *vstate)
{
  int status = GSL_SUCCESS;
  const gsl_multifit_nlinear_trust_state *trust_state =
    (const gsl_multifit_nlinear_trust_state *) vtrust_state;
  svd_state_t *state = (svd_state_t *) vstate;
  const size_t p = state->p;
  const double tol = GSL_DBL_EPSILON;
  const double s0 = gsl_vector_get(state->S, 0);
  size_t j;

  /* compute workp = - U^T f */
  gsl_blas_dgemv(CblasTrans, -1.0, state->U, f, 0.0, state->workp);

  /*
   * compute:
   *
   * workp = sum_i s_i / (s_i^2 + mu) (-u_i^T f)
   */

  if (state->mu == 0.0)
    {
      /*
       * compute Gauss-Newton direction by solving
       * J x = -f
       */

      for (j = 0; j < p; ++j)
        {
          double sj = gsl_vector_get(state->S, j);
          double *ptr = gsl_vector_ptr(state->workp, j);
          double alpha;

          if (sj <= tol * s0)
            alpha = 0.0;
          else
            alpha = 1.0 / sj;

          *ptr *= alpha;
        }
    }
  else
    {
      /*
       * solve:
       *
       * [  J D^{-1}  ] (D x) = -[ f ]
       * [ sqrt(mu) I ]          [ 0 ]
       *
       * using SVD factorization of J D^{-1}
       */

      for (j = 0; j < p; ++j)
        {
          double sj = gsl_vector_get(state->S, j);
          double *ptr = gsl_vector_ptr(state->workp, j);

          *ptr *= sj / (sj*sj + state->mu);
        }
    }

  /* compute: x = V * workp */
  gsl_blas_dgemv(CblasNoTrans, 1.0, state->V, state->workp, 0.0, x);

  /* compute D^{-1} x */
  gsl_vector_div(x, trust_state->diag);

  return status;
}

static int
svd_rcond(double * rcond, void * vstate)
{
  int status = GSL_SUCCESS;
  svd_state_t *state = (svd_state_t *) vstate;
  double smax = gsl_vector_get(state->S, 0);
  double smin = gsl_vector_get(state->S, state->p - 1);

  *rcond = smin / smax;

  return status;
}

static const gsl_multifit_nlinear_solver svd_type =
{
  "svd",
  svd_alloc,
  svd_init,
  svd_presolve,
  svd_solve,
  svd_rcond,
  svd_free
};

const gsl_multifit_nlinear_solver *gsl_multifit_nlinear_solver_svd = &svd_type;


#ifndef LA_WRAPPER
#define LA_WRAPPER

// #include <gsl/gsl_cblas.h>
#include <cblas.h>
#include <complex>
#include <lapack_header.h>

namespace blas_wrapper
{

//! @param n : Anzahl Elemente.
//! @param x : Quellvektor x.
//! @param incx : Speicher Abstand zwischen Elemente in Vector x.
//! @param y : Zielvektor (y= alpha*x+y).
//! @param incy: Speicher Abstand zwischen Elemente in Vector y.

static void
copy(int n, const float *x, int incx, float *y, int incy)
{
    cblas_scopy(n, x, incx, y, incy);
}

static void
copy(int n, const double *x, int incx, double *y, int incy)
{
    cblas_dcopy(n, x, incx, y, incy);
}

static void
copy(int n, const std::complex<double> *x, int incx, std::complex<double> *y, int incy)
{
    cblas_zcopy(n, reinterpret_cast<const double*>(x), incx, reinterpret_cast<double*>(y), incy);
}

static void
copy(int n, const std::complex<float> *x, int incx, std::complex<float> *y, int incy)
{
    cblas_ccopy(n, reinterpret_cast<const float*>(x), incx, reinterpret_cast<float*>(y), incy);
}

static void
copy(int n, const int *x, int incx, int *y, int incy)
{
    for(int i=0; i<n; i++)
    {
        y[i*incy]=x[i*incx];
    }
}

//! @param n : Anzahl Elemente.
//! @param alpha: Skalar.
//! @param x : Quellvektor x (auch output).
//! @param incx : Speicher Abstand zwischen Elemente in Vector x.

static void
scal (int n, float alpha, float *x, int incx)
{
    cblas_sscal (n, alpha, x, incx);
}

static void
scal (int n, double alpha, double *x, int incx)
{
    cblas_dscal (n, alpha, x, incx);
}

static void
scal (int n, std::complex<float> alpha, std::complex<float> *x, int incx)
{
   cblas_cscal(n, reinterpret_cast<const float*>(&alpha), reinterpret_cast<float*>(x), incx);
}

static void
scal (int n, std::complex<double>  alpha, std::complex<double> *x, int incx)
{
   cblas_zscal(n, reinterpret_cast<const double*>(&alpha), reinterpret_cast<double*>(x), incx);
}

//! @param trans : gibt an ob A transponiert ist oder nicht. Sei trans = 'N' oder 'n' so ist op(A)= A, sei trans = 'T', 't','C' oder 'c' so ist op(A)= trans(A)
//! @param m : Anzahl Zeilen in Matrix A.
//! @param n : Anzahl Spalten in Matrix A.
//! @param alpha: Skalar fuer A.
//! @param A : Matrix A
//! @param lda : leading dimension von A.
//! @param x : Vektor mit der laenge von mindestens (1+(n-1)*abs(incx)) falls trans = 'N' oder 'n', sonst mindestens der laenge (1+(m-1)*abs(incx)).
//! @param incx : Speicher Abstand zwischen Elemente in Vector x.
//! @param beta : Skalar fuer Vektor y.
//! @param y : Vektor mit der laenge von mindestens (1+(n-1)*abs(incy)) falls trans = 'N' oder 'n', sonst mindestens der laenge (1+(m-1)*abs(incy)).
//! @param incy: Speicher Abstand zwischen Elemente in Vector y.

static void gemv (char trans, int m, int n, float alpha,
                  const float * const A, int lda,
                  const float * const x,  int incx, float beta,
                  float *y, int incy)
{
    CBLAS_TRANSPOSE tr = ( ( (trans == 't') || (trans == 'T') ) ? CblasTrans : CblasNoTrans );
    cblas_sgemv (CblasColMajor, tr, m, n, alpha, A, lda, x, incx, beta, y, incy);
}

static void gemv (char trans, int m, int n, double alpha,
                  const double * const A, int lda,
                  const double * const x,  int incx, double beta,
                  double *y, int incy)
{
    CBLAS_TRANSPOSE tr = ( ( (trans == 't') || (trans == 'T') ) ? CblasTrans : CblasNoTrans );
    cblas_dgemv (CblasColMajor, tr, m, n, alpha, A, lda, x, incx, beta, y, incy);
}

static void gemv (char trans, int m, int n, std::complex<float> & alpha,
                  const std::complex<float> * const A, int lda,
                  const std::complex<float> * const x,  int incx, std::complex<float> & beta,
                  std::complex<float> *y, int incy)
{
    CBLAS_TRANSPOSE tr = ( ( (trans == 't') || (trans == 'T') ) ? CblasTrans : CblasNoTrans );
    cblas_cgemv (CblasColMajor, tr, m, n, &alpha, A, lda, x, incx, &beta, y, incy);
}

static void gemv (char trans, int m, int n, std::complex<double> & alpha,
                  const std::complex<double> * const A, int lda,
                  const std::complex<double> * const x,  int incx, std::complex<double>  & beta,
                  std::complex<double> *y, int incy)
{
    CBLAS_TRANSPOSE tr = ( ( (trans == 't') || (trans == 'T') ) ? CblasTrans : CblasNoTrans );
    cblas_zgemv (CblasColMajor, tr, m, n, &alpha, A, lda, x, incx, &beta, y, incy);
}

//! @param transa : gibt an ob A transponiert ist oder nicht. Sei transa = 'N' oder 'n' so ist op(A)= A, sei transa = 'T' oder 't' so ist op(A)= trans(A), sei transa = 'C' oder 'c' so ist op(A)=adjoint(A)
//! @param transb : gibt an ob B transponiert ist oder nicht. Sei transb = 'N' oder 'n' so ist op(B)= A, sei transb = 'T' oder 't' so ist op(B)= trans(B), sei transb = 'C' oder 'c' so ist op(B)=adjoint(B)
//! @param m : Anzahl Zeilen in Matrix A und Matrix C.
//! @param n : Anzahl Spalten in Matrix B und Matrix C.
//! @param k : Anzahl Spalten in Matrix A und Zeilen in Matrix B.
//! @param alpha: Skalar fuer op(A)*op(B).
//! @param A : Matrix A
//! @param lda : leading dimension von A.
//! @param B : Matrix B.
//! @param ldb : leading dimension von B.
//! @param beta : Skalar fuer Matrix C.
//! @param C : Matrix C.
//! @param ldc : leading dimension von C.
static void gemm(char transa, char transb, int m, int n, int k, float alpha,
                 const float * const A, int lda, const float * const B, int ldb,
                 float beta, float * C, int ldc)
{
    CBLAS_TRANSPOSE tr_a = ( ( (transa == 't') || (transa == 'T') ) ? CblasTrans : ( (transa == 'c') || (transa == 'C') ) ? CblasTrans : CblasNoTrans );
    CBLAS_TRANSPOSE tr_b = ( ( (transb == 't') || (transb == 'T') ) ? CblasTrans : ( (transb == 'c') || (transb == 'C') ) ? CblasTrans : CblasNoTrans );

    cblas_sgemm(CblasColMajor,
                tr_a, tr_b,
                m, n, k,
                alpha,
                A, lda,
                B, ldb,
                beta,
                C, ldc);
}

static void gemm(char transa, char transb, int m, int n, int k, double alpha,
                 const double * const A, int lda, const double * const B, int ldb,
                 double beta, double * C, int ldc)
{
    CBLAS_TRANSPOSE tr_a = ( ( (transa == 't') || (transa == 'T') ) ? CblasTrans : ( (transa == 'c') || (transa == 'C') ) ? CblasTrans : CblasNoTrans );
    CBLAS_TRANSPOSE tr_b = ( ( (transb == 't') || (transb == 'T') ) ? CblasTrans : ( (transb == 'c') || (transb == 'C') ) ? CblasTrans : CblasNoTrans );

    cblas_dgemm(CblasColMajor, tr_a, tr_b, m, n, k, alpha,
                A, lda, B, ldb,
                beta, C, ldc);
}

static void gemm(char transa, char transb, int m, int n, int k, const std::complex<float> alpha,
                 const std::complex<float> * const A, int lda, const std::complex<float> * const B, int ldb,
                 const std::complex<float> beta, std::complex<float> * C, int ldc)
{
    CBLAS_TRANSPOSE tr_a = ( ( (transa == 't') || (transa == 'T') ) ? CblasTrans : ( (transa == 'c') || (transa == 'C') ) ? CblasConjTrans : CblasNoTrans );
    CBLAS_TRANSPOSE tr_b = ( ( (transb == 't') || (transb == 'T') ) ? CblasTrans : ( (transb == 'c') || (transb == 'C') ) ? CblasConjTrans : CblasNoTrans );

    cblas_cgemm(CblasColMajor,
                tr_a, tr_b,
                m, n, k,
                reinterpret_cast<const float*>(&alpha),
                reinterpret_cast<const float*>(A), lda,
                reinterpret_cast<const float*>(B), ldb,
                reinterpret_cast<const float*>(&beta),
                reinterpret_cast<float*>(C), ldc);
}

static void gemm(char transa, char transb, int m, int n, int k, const std::complex<double> alpha,
                 const std::complex<double> * const A, int lda, const std::complex<double> * const B, int ldb,
                 const std::complex<double> beta, std::complex<double> * C, int ldc)
{
    CBLAS_TRANSPOSE tr_a = ( ( (transa == 't') || (transa == 'T') ) ? CblasTrans : ( (transa == 'c') || (transa == 'C') ) ? CblasConjTrans : CblasNoTrans );
    CBLAS_TRANSPOSE tr_b = ( ( (transb == 't') || (transb == 'T') ) ? CblasTrans : ( (transb == 'c') || (transb == 'C') ) ? CblasConjTrans : CblasNoTrans );

    cblas_zgemm(CblasColMajor,
                tr_a, tr_b,
                m, n, k,
                reinterpret_cast<const double*>(&alpha),
                reinterpret_cast<const double*>(A), lda,
                reinterpret_cast<const double*>(B), ldb,
                reinterpret_cast<const double*>(&beta),
                reinterpret_cast<double*>(C), ldc);
}

} // END NAMESPACE blas_wrapper

namespace lapack_wrapper
{

// @sect5{Lapack function wrappers}
// @sect6{Wrapper: SVD lapack-wrapper}
// Wraps the general matrix svd lapack function for the four types s, d, c and z.
// Lapack doc: http://www.netlib.org/lapack/explore-html/d8/d49/sgesvd_8f.html
inline static void gesvd(char * JOBU, char * JOBVT, const LAPACK_INT * M, const LAPACK_INT * N, float * A, const LAPACK_INT * LDA, float * S, float * U, const LAPACK_INT * LDU, float * VT, const LAPACK_INT * LDVT, float * WORK, const LAPACK_INT * LWORK, float * /*RWORK*/, LAPACK_INT * INFO)
{
    sgesvd_(JOBU, JOBVT, M, N, A, LDA, S, U, LDU, VT, LDVT, WORK, LWORK, INFO);
}

inline static void gesvd(char * JOBU, char * JOBVT, const LAPACK_INT * M, const LAPACK_INT * N, double * A, const LAPACK_INT * LDA, double * S, double * U, const LAPACK_INT * LDU, double * VT, const LAPACK_INT * LDVT, double * WORK, const LAPACK_INT * LWORK, double * /*RWORK*/, LAPACK_INT * INFO)
{
    dgesvd_(JOBU, JOBVT, M, N, A, LDA, S, U, LDU, VT, LDVT, WORK, LWORK, INFO);
}

inline static void gesvd(char * JOBU, char * JOBVT, const LAPACK_INT * M, const LAPACK_INT * N, std::complex<float> * A, const LAPACK_INT * LDA, float * S, std::complex<float> * U, const LAPACK_INT  * LDU, std::complex<float> * VT, const LAPACK_INT * LDVT, std::complex<float> * WORK, const LAPACK_INT * LWORK, float * RWORK, LAPACK_INT * INFO)
{
    cgesvd_(JOBU, JOBVT, M, N, A, LDA, S, U, LDU, VT, LDVT, WORK, LWORK, RWORK, INFO);
}

inline static void gesvd(char * JOBU, char * JOBVT, const LAPACK_INT * M, const LAPACK_INT * N, std::complex<double> * A, const LAPACK_INT * LDA, double * S, std::complex<double> * U, const LAPACK_INT * LDU, std::complex<double> * VT, const LAPACK_INT * LDVT, std::complex<double> * WORK, const LAPACK_INT * LWORK, double * RWORK, LAPACK_INT * INFO)
{
    zgesvd_(JOBU, JOBVT, M, N, A, LDA, S, U, LDU, VT, LDVT, WORK, LWORK, RWORK, INFO);
}

// @sect6{Wrapper: SVD divide and conquer lapack-wrapper}
// Wraps the general matrix sdd lapack function for the four types s, d, c and z.
// Lapack doc: http://www.netlib.org/lapack/explore-html/d1/d7e/group__double_g_esing_ga76f797b6a9e278ad7b21aae2b4a55d76.html#ga76f797b6a9e278ad7b21aae2b4a55d76
inline static void gesdd(char * JOBZ, const LAPACK_INT * M, const LAPACK_INT * N, float * A, const LAPACK_INT * LDA, float * S, float * U, const LAPACK_INT * LDU, float * VT, const LAPACK_INT * LDVT, float * WORK, const LAPACK_INT * LWORK, float * /*RWORK*/, LAPACK_INT * IWORK, LAPACK_INT * INFO)
{
    return sgesdd_(JOBZ, M, N, A, LDA, S, U, LDU, VT, LDVT, WORK, LWORK, IWORK, INFO);
}
inline static void gesdd(char * JOBZ, const LAPACK_INT * M, const LAPACK_INT * N, double * A, const LAPACK_INT * LDA, double * S, double * U, const LAPACK_INT * LDU, double * VT, const LAPACK_INT * LDVT, double * WORK, const LAPACK_INT * LWORK, double * /*RWORK*/, LAPACK_INT * IWORK, LAPACK_INT * INFO)
{
    return dgesdd_(JOBZ, M, N, A, LDA, S, U, LDU, VT, LDVT, WORK, LWORK, IWORK, INFO);
}
inline static void gesdd(char * JOBZ, const LAPACK_INT * M, const LAPACK_INT * N, std::complex<float> * A, const LAPACK_INT * LDA, float * S, std::complex<float> * U, const LAPACK_INT * LDU, std::complex<float> * VT, const LAPACK_INT * LDVT, std::complex<float> * WORK, const LAPACK_INT * LWORK, float * RWORK, LAPACK_INT * IWORK, LAPACK_INT * INFO)
{
    return cgesdd_(JOBZ, M, N, A, LDA, S, U, LDU, VT, LDVT, WORK, LWORK, RWORK, IWORK, INFO);
}
inline static void gesdd(char * JOBZ, const LAPACK_INT * M, const LAPACK_INT * N, std::complex<double> * A, const LAPACK_INT * LDA, double * S, std::complex<double> * U, const LAPACK_INT * LDU, std::complex<double> * VT, const LAPACK_INT * LDVT, std::complex<double> * WORK, const LAPACK_INT * LWORK, double * RWORK, LAPACK_INT * IWORK, LAPACK_INT * INFO)
{
    return zgesdd_(JOBZ, M, N, A, LDA, S, U, LDU, VT, LDVT, WORK, LWORK, RWORK, IWORK, INFO);
}

// @sect6{Wrapper: LUD lapack-wrapper}
// Wraps the general matrix LU decomposition lapack function for the four types s, d, c and z.
// Lapack doc: http://www.netlib.org/lapack/explore-html/de/de2/sgetrf_8f.html
inline static void getrf(const LAPACK_INT * M, const LAPACK_INT * N, float * A, const LAPACK_INT * LDA, LAPACK_INT * IPIV, LAPACK_INT * INFO)
{
    sgetrf_(M, N, A, LDA, IPIV, INFO);
}
inline static void getrf(const LAPACK_INT * M, const LAPACK_INT * N, double * A, const LAPACK_INT * LDA, LAPACK_INT * IPIV, LAPACK_INT * INFO)
{
    dgetrf_(M, N, A, LDA, IPIV, INFO);
}
inline static void getrf(const LAPACK_INT * M, const LAPACK_INT * N, std::complex<float> * A, const LAPACK_INT * LDA, LAPACK_INT * IPIV, LAPACK_INT * INFO)
{
    cgetrf_(M, N, A, LDA, IPIV, INFO);
}
inline static void getrf(const LAPACK_INT * M, const LAPACK_INT * N, std::complex<double> * A, const LAPACK_INT * LDA, LAPACK_INT * IPIV, LAPACK_INT * INFO)
{
    zgetrf_(M, N, A, LDA, IPIV, INFO);
}

// @sect6{Wrapper: LU-Inverse lapack-wrapper}
// Wraps the general matrix inversion lapack function for the two types s, d.
 // Lapack doc: http://www.netlib.org/lapack/explore-html/de/de2/sgetri_8f.html
inline static void getri (const LAPACK_INT * N, float * A, const LAPACK_INT * LDA, const LAPACK_INT * IPIV, float * WORK, const LAPACK_INT * LWORK, LAPACK_INT * INFO)
{
    sgetri_(N, A, LDA, IPIV, WORK, LWORK, INFO);
}
inline static void getri (const LAPACK_INT * N, double * A, const LAPACK_INT * LDA, const LAPACK_INT * IPIV, double * WORK, const LAPACK_INT * LWORK, LAPACK_INT * INFO)
{
    dgetri_(N, A, LDA, IPIV, WORK, LWORK, INFO);
}

// @sect6{Wrapper: QRF lapack-wrapper}
// Wraps the general matrix QR factorization lapack function for the four types s, d, c and z.
// Lapack doc: http://www.netlib.org/lapack/explore-html/df/d97/sgeqrf_8f.html
inline static void geqrf(const LAPACK_INT * M, const LAPACK_INT * N, float * A, const LAPACK_INT * LDA, float * TAU, float * WORK, const LAPACK_INT * LWORK, LAPACK_INT * INFO)
{
    sgeqrf_(M, N, A, LDA, TAU, WORK, LWORK, INFO);
}
inline static void geqrf(const LAPACK_INT * M, const LAPACK_INT * N, double * A, const LAPACK_INT * LDA, double * TAU, double * WORK, const LAPACK_INT * LWORK, LAPACK_INT * INFO)
{
    dgeqrf_(M, N, A, LDA, TAU, WORK, LWORK, INFO);
}
inline static void geqrf(const LAPACK_INT * M, const LAPACK_INT * N, std::complex<float> * A, const LAPACK_INT * LDA, std::complex<float> * TAU, std::complex<float> * WORK, const LAPACK_INT * LWORK, LAPACK_INT * INFO)
{
    cgeqrf_(M, N, A, LDA, TAU, WORK, LWORK, INFO);
}
inline static void geqrf(const LAPACK_INT * M, const LAPACK_INT * N, std::complex<double> * A, const LAPACK_INT * LDA, std::complex<double> * TAU, std::complex<double> * WORK, const LAPACK_INT * LWORK, LAPACK_INT * INFO)
{
    zgeqrf_(M, N, A, LDA, TAU, WORK, LWORK, INFO);
}

// @sect6{Wrapper: MQR lapack-wrapper}
// Wraps the lapack function of the product of elementary reflectors from the QR factorization for the four types s, d, c and z.
// Lapack doc: http://www.netlib.org/lapack/explore-html/d0/d98/sormqr_8f.html
inline static void xxmqr(char * SIDE, char * TRANS, const LAPACK_INT * M, const LAPACK_INT * N, const LAPACK_INT * K, float * A, const LAPACK_INT * LDA, float * TAU, float * C, const LAPACK_INT * LDC, float * WORK, const LAPACK_INT * LWORK, LAPACK_INT * INFO)
{
    sormqr_(SIDE, TRANS, M, N, K, A, LDA, TAU, C, LDC, WORK, LWORK, INFO);
}
inline static void xxmqr(char * SIDE, char * TRANS, const LAPACK_INT * M, const LAPACK_INT * N, const LAPACK_INT * K, double * A, const LAPACK_INT * LDA, double * TAU, double * C, const LAPACK_INT * LDC, double * WORK, const LAPACK_INT * LWORK, LAPACK_INT * INFO)
{
    dormqr_(SIDE, TRANS, M, N, K, A, LDA, TAU, C, LDC, WORK, LWORK, INFO);
}
inline static void xxmqr(char * SIDE, char * TRANS, const LAPACK_INT * M, const LAPACK_INT * N, const LAPACK_INT * K, std::complex<float> * A, const LAPACK_INT * LDA, std::complex<float> * TAU, std::complex<float> * C, const LAPACK_INT * LDC, std::complex<float> * WORK, const LAPACK_INT * LWORK, LAPACK_INT * INFO)
{
    cunmqr_(SIDE, TRANS, M, N, K, A, LDA, TAU, C, LDC, WORK, LWORK, INFO);
}
inline static void xxmqr(char * SIDE, char * TRANS, const LAPACK_INT * M, const LAPACK_INT * N, const LAPACK_INT * K, std::complex<double> * A, const LAPACK_INT * LDA, std::complex<double> * TAU, std::complex<double> * C, const LAPACK_INT * LDC, std::complex<double> * WORK, const LAPACK_INT * LWORK, LAPACK_INT * INFO)
{
    zunmqr_(SIDE, TRANS, M, N, K, A, LDA, TAU, C, LDC, WORK, LWORK, INFO);
}

// @sect6{Wrapper: EV lapack-wrapper}
// Wraps the lapack function for computing the eigenvalues and, optionally, the left and/or right eigenvectors for the four types s, d, c and z.
// Lapack doc: http://www.netlib.org/lapack/explore-html/d1/d74/group__eigen_s_y.html and http://www.netlib.org/lapack/explore-html/df/db2/cheev_8f.html
inline static void xxev(char * JOBZ, char * UPLO, const LAPACK_INT * N, float * A, const LAPACK_INT * LDA, float * W, float * WORK, const LAPACK_INT * LWORK, float * /*RWORK*/, LAPACK_INT * INFO)
{
    return ssyev_(JOBZ, UPLO, N, A, LDA, W, WORK, LWORK, INFO);
}
inline static void xxev(char * JOBZ, char * UPLO, const LAPACK_INT * N, double * A, const LAPACK_INT * LDA, double * W, double * WORK, const LAPACK_INT * LWORK, double * /*RWORK*/, LAPACK_INT * INFO)
{
    return dsyev_(JOBZ, UPLO, N, A, LDA, W, WORK, LWORK, INFO);
}
inline static void xxev(char * JOBZ, char * UPLO, const LAPACK_INT * N, std::complex<float> * A, const LAPACK_INT * LDA, float * W, std::complex<float> * WORK, const LAPACK_INT * LWORK, float * RWORK, LAPACK_INT * INFO)
{
    return cheev_(JOBZ, UPLO, N, A, LDA, W, WORK, LWORK, RWORK, INFO);
}
inline static void xxev(char * JOBZ, char * UPLO, const LAPACK_INT * N, std::complex<double> * A, const LAPACK_INT * LDA, double * W, std::complex<double> * WORK, const LAPACK_INT * LWORK, double * RWORK, LAPACK_INT * INFO)
{
    return zheev_(JOBZ, UPLO, N, A, LDA, W, WORK, LWORK, RWORK, INFO);
}

// @sect6{Wrapper: EV lapack-wrapper}
// Wraps the lapack function for computing the eigenvalues and, optionally, the left and/or right eigenvectors for the two real types s and d.
// Lapack doc: http://www.netlib.org/lapack/explore-html/d1/d74/group__eigen_s_y.html and http://www.netlib.org/lapack/explore-html/df/db2/cheev_8f.html
inline static void xtev(char * JOBZ, const LAPACK_INT * N, float * D, float * E, float * Z, const LAPACK_INT * LDZ, float * WORK, LAPACK_INT * INFO)
{
    return sstev_(JOBZ, N, D, E, Z, LDZ, WORK, INFO);
}
inline static void xtev(char * JOBZ, const LAPACK_INT * N, double * D, double * E, double * Z, const LAPACK_INT * LDZ, double * WORK, LAPACK_INT * INFO)
{
    dstev_(JOBZ, N, D, E, Z, LDZ, WORK, INFO);
}

// @sect6{Wrapper: Matrix norm lapack-wrapper}
// Wraps the lapack function for computing the eigenvalues and, optionally, the left and/or right eigenvectors for the two real types s and d.
// Lapack doc: http://www.netlib.org/lapack/explore-html/d1/d74/group__eigen_s_y.html and http://www.netlib.org/lapack/explore-html/df/db2/cheev_8f.html
inline static float lange(char * NORM, const LAPACK_INT * M, const LAPACK_INT * N, const float * A, const LAPACK_INT * LDA, float * WORK)
{
    return slange_( NORM, M, N, A, LDA, WORK );
}
inline static double lange(char * NORM, const LAPACK_INT * M, const LAPACK_INT * N, const double * A, const LAPACK_INT * LDA, double * WORK)
{
    return dlange_( NORM, M, N, A, LDA, WORK );
}
inline static float lange(char * NORM, const LAPACK_INT * M, const LAPACK_INT * N, const std::complex<float> * A, const LAPACK_INT * LDA, float * WORK)
{
    return clange_( NORM, M, N, A, LDA, WORK );
}
inline static double lange(char * NORM, const LAPACK_INT * M, const LAPACK_INT * N, const std::complex<double> * A, const LAPACK_INT * LDA, double * WORK)
{
    return zlange_( NORM, M, N, A, LDA, WORK );
}

} // END NAMESPACE LAPACK_WRAPPER

#endif // LA_WRAPPER


// Author: Samuel D. Relton
#include <stdio.h>
#include <cblas.h>
#include "sgemm_batched.h"

float A1[] = {
3, 1, 3,
1, 5, 9,
2, 6, 5
};

float B1[] = {
3, 1, 3,
1, 5, 9,
2, 6, 5
};

float C1[] = {
0, 0, 0,
0, 0, 0,
0, 0, 0
};

float A2[] = {
3, 1, 3,
1, 5, 9,
2, 6, 5
};

float B2[] = {
3, 1, 3,
1, 5, 9,
2, 6, 5
};

float C2[] = {
0, 0, 0,
0, 0, 0,
0, 0, 0
};

float ALPHA[] = {1.0, 1.0};
float BETA[] = {0.0, 0.0};

int LDA[] = {3, 3};
int LDB[] = {3, 3};
int LDC[] = {3, 3};

bblas_trans_t  TRANSA[] = {CblasNoTrans, CblasNoTrans};
bblas_trans_t  TRANSB[] = {CblasNoTrans, CblasNoTrans};

int M[] = {3, 3};
int N[] = {3, 3};
int K[] = {3, 3};

int BATCHCOUNT = 2;
bblas_batch_type_t BATCH_TYPE = BBLASFixed;

int INFO[] = {0, 0};

float *arrayA[] = {A1, A2};
float *arrayB[] = {B1, B2};
float *arrayC[] = {C1, C2};

int main()
{
	int i, j, k;

	sgemm_batched(TRANSA, TRANSB, M, N, K, ALPHA, arrayA,
		      LDA, arrayB, LDB, BETA, arrayC, LDC, BATCHCOUNT,
		      BATCH_TYPE, INFO);

	for (k = 0; k < BATCHCOUNT; k++) {
		putchar('\n');
		for (i=0; i<3; ++i) {
			for (j=0; j<3; ++j) {
				printf("%5.1f", arrayC[k][i*3+j]);

			}
			putchar('\n');
		}
	}

    return 0;
}


#ifdef GSLODE1

////////////////////////////// ODE V1 //////////////////////////////////////////

#include <gsl/gsl_odeiv.h>

typedef struct {int n; int (*f)(double,int, const double*, int, double *); int (*j)(double,int, const double*, int, int, double*);} Tode;

int odefunc (double t, const double y[], double f[], void *params) {
    Tode * P = (Tode*) params;
    (P->f)(t,P->n,y,P->n,f);
    return GSL_SUCCESS;
}

int odejac (double t, const double y[], double *dfdy, double dfdt[], void *params) {
     Tode * P = ((Tode*) params);
     (P->j)(t,P->n,y,P->n,P->n,dfdy);
     int j;
     for (j=0; j< P->n; j++)
        dfdt[j] = 0.0;
     return GSL_SUCCESS;
}

int ode(int method, double h, double eps_abs, double eps_rel,
        int f(double, int, const double*, int, double*),
        int jac(double, int, const double*, int, int, double*),
        KRVEC(xi), KRVEC(ts), RMAT(sol)) {

    const gsl_odeiv_step_type * T;

    switch(method) {
        case 0 : {T = gsl_odeiv_step_rk2; break; }
        case 1 : {T = gsl_odeiv_step_rk4; break; }
        case 2 : {T = gsl_odeiv_step_rkf45; break; }
        case 3 : {T = gsl_odeiv_step_rkck; break; }
        case 4 : {T = gsl_odeiv_step_rk8pd; break; }
        case 5 : {T = gsl_odeiv_step_rk2imp; break; }
        case 6 : {T = gsl_odeiv_step_rk4imp; break; }
        case 7 : {T = gsl_odeiv_step_bsimp; break; }
        case 8 : { printf("Sorry: ODE rk1imp not available in this GSL version\n"); exit(0); }
        case 9 : { printf("Sorry: ODE msadams not available in this GSL version\n"); exit(0); }
        case 10: { printf("Sorry: ODE msbdf not available in this GSL version\n"); exit(0); }
        default: ERROR(BAD_CODE);
    }

    gsl_odeiv_step * s = gsl_odeiv_step_alloc (T, xin);
    gsl_odeiv_control * c = gsl_odeiv_control_y_new (eps_abs, eps_rel);
    gsl_odeiv_evolve * e = gsl_odeiv_evolve_alloc (xin);

    Tode P;
    P.f = f;
    P.j = jac;
    P.n = xin;

    gsl_odeiv_system sys = {odefunc, odejac, xin, &P};

    double t = tsp[0];

    double* y = (double*)calloc(xin,sizeof(double));
    int i,j;
    for(i=0; i< xin; i++) {
        y[i] = xip[i];
        solp[i] = xip[i];
    }

       for (i = 1; i < tsn ; i++)
         {
           double ti = tsp[i];
           while (t < ti)
             {
               gsl_odeiv_evolve_apply (e, c, s,
                                       &sys,
                                       &t, ti, &h,
                                       y);
               // if (h < hmin) h = hmin;
             }
           for(j=0; j<xin; j++) {
               solp[i*xin + j] = y[j];
           }
         }

    free(y);
    gsl_odeiv_evolve_free (e);
    gsl_odeiv_control_free (c);
    gsl_odeiv_step_free (s);
    return 0;
}

#else

///////////////////// ODE V2 ///////////////////////////////////////////////////

#include <gsl/gsl_odeiv2.h>

typedef struct {int n; int (*f)(double,int, const double*, int, double *); int (*j)(double,int, const double*, int, int, double*);} Tode;

int odefunc (double t, const double y[], double f[], void *params) {
    Tode * P = (Tode*) params;
    (P->f)(t,P->n,y,P->n,f);
    return GSL_SUCCESS;
}

int odejac (double t, const double y[], double *dfdy, double dfdt[], void *params) {
     Tode * P = ((Tode*) params);
     (P->j)(t,P->n,y,P->n,P->n,dfdy);
     int j;
     for (j=0; j< P->n; j++)
        dfdt[j] = 0.0;
     return GSL_SUCCESS;
}

int ode(int method, double h, double eps_abs, double eps_rel,
        int f(double, int, const double*, int, double*),
        int jac(double, int, const double*, int, int, double*),
        KRVEC(xi), KRVEC(ts), RMAT(sol)) {

    const gsl_odeiv2_step_type * T;

    switch(method) {
        case 0 : {T = gsl_odeiv2_step_rk2; break; }
        case 1 : {T = gsl_odeiv2_step_rk4; break; }
        case 2 : {T = gsl_odeiv2_step_rkf45; break; }
        case 3 : {T = gsl_odeiv2_step_rkck; break; }
        case 4 : {T = gsl_odeiv2_step_rk8pd; break; }
        case 5 : {T = gsl_odeiv2_step_rk2imp; break; }
        case 6 : {T = gsl_odeiv2_step_rk4imp; break; }
        case 7 : {T = gsl_odeiv2_step_bsimp; break; }
        case 8 : {T = gsl_odeiv2_step_rk1imp; break; }
        case 9 : {T = gsl_odeiv2_step_msadams; break; }
        case 10: {T = gsl_odeiv2_step_msbdf; break; }
        default: ERROR(BAD_CODE);
    }

    Tode P;
    P.f = f;
    P.j = jac;
    P.n = xin;

    gsl_odeiv2_system sys = {odefunc, odejac, xin, &P};

    gsl_odeiv2_driver * d =
         gsl_odeiv2_driver_alloc_y_new (&sys, T, h, eps_abs, eps_rel);

    double t = tsp[0];

    double* y = (double*)calloc(xin,sizeof(double));
    int i,j;
    int status;
    for(i=0; i< xin; i++) {
        y[i] = xip[i];
        solp[i] = xip[i];
    }

       for (i = 1; i < tsn ; i++)
         {
           double ti = tsp[i];

           status = gsl_odeiv2_driver_apply (d, &t, ti, y);

           if (status != GSL_SUCCESS) {
         	  printf ("error in ode, return value=%d\n", status);
         	  break;
        	}

//           printf ("%.5e %.5e %.5e\n", t, y[0], y[1]);

           for(j=0; j<xin; j++) {
               solp[i*xin + j] = y[j];
           }
         }

    free(y);
    gsl_odeiv2_driver_free (d);

    return status;
}

#endif