// Boost.Geometry

// Copyright (c) 2018 Adeel Ahmad, Islamabad, Pakistan.

// Contributed and/or modified by Adeel Ahmad, as part of Google Summer of Code 2018 program.

// This file was modified by Oracle on 2019.
// Modifications copyright (c) 2019 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

// This file is converted from GeographicLib, https://geographiclib.sourceforge.io
// GeographicLib is originally written by Charles Karney.

// Author: Charles Karney (2008-2017)

// Last updated version of GeographicLib: 1.49

// Original copyright notice:

// Copyright (c) Charles Karney (2008-2017) <charles@karney.com> and licensed
// under the MIT/X11 License. For more information, see
// https://geographiclib.sourceforge.io

#ifndef BOOST_GEOMETRY_FORMULAS_KARNEY_INVERSE_HPP
#define BOOST_GEOMETRY_FORMULAS_KARNEY_INVERSE_HPP

#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/hypot.hpp>

#include <boost/geometry/util/condition.hpp>
#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/series_expansion.hpp>
#include <boost/geometry/util/normalize_spheroidal_coordinates.hpp>

#include <boost/geometry/formulas/flattening.hpp>
#include <boost/geometry/formulas/result_inverse.hpp>

namespace boost { namespace geometry { namespace math {

// TODO: Moved temporarily because of C++11 is used

/*!
\brief The exact difference of two angles reduced to (-180deg, 180deg].
*/
template<typename T>
inline T difference_angle(T const& x, T const& y, T& e)
{
    T t, d = math::sum_error(std::remainder(-x, T(360)), std::remainder(y, T(360)), t);

    normalize_azimuth<degree, T>(d);

    // Here y - x = d + t (mod 360), exactly, where d is in (-180,180] and
    // abs(t) <= eps (eps = 2^-45 for doubles).  The only case where the
    // addition of t takes the result outside the range (-180,180] is d = 180
    // and t > 0.  The case, d = -180 + eps, t = -eps, can't happen, since
    // sum_error would have returned the exact result in such a case (i.e., given t = 0).
    return math::sum_error(d == 180 && t > 0 ? -180 : d, t, e);
}

}}} // namespace boost::geometry::math

namespace boost { namespace geometry { namespace formula
{

namespace se = series_expansion;

/*!
\brief The solution of the inverse problem of geodesics on latlong coordinates,
       after Karney (2011).
\author See
- Charles F.F Karney, Algorithms for geodesics, 2011
https://arxiv.org/pdf/1109.4448.pdf
*/
template <
    typename CT,
    bool EnableDistance,
    bool EnableAzimuth,
    bool EnableReverseAzimuth = false,
    bool EnableReducedLength = false,
    bool EnableGeodesicScale = false,
    size_t SeriesOrder = 8
>
class karney_inverse
{
    static const bool CalcQuantities = EnableReducedLength || EnableGeodesicScale;
    static const bool CalcAzimuths = EnableAzimuth || EnableReverseAzimuth || CalcQuantities;
    static const bool CalcFwdAzimuth = EnableAzimuth || CalcQuantities;
    static const bool CalcRevAzimuth = EnableReverseAzimuth || CalcQuantities;

public:
    typedef result_inverse<CT> result_type;

    template <typename T1, typename T2, typename Spheroid>
    static inline result_type apply(T1 const& lo1,
                                    T1 const& la1,
                                    T2 const& lo2,
                                    T2 const& la2,
                                    Spheroid const& spheroid)
    {
        static CT const c0 = 0;
        static CT const c0_001 = 0.001;
        static CT const c0_1 = 0.1;
        static CT const c1 = 1;
        static CT const c2 = 2;
        static CT const c3 = 3;
        static CT const c8 = 8;
        static CT const c16 = 16;
        static CT const c90 = 90;
        static CT const c180 = 180;
        static CT const c200 = 200;
        static CT const pi = math::pi<CT>();
        static CT const d2r = math::d2r<CT>();
        static CT const r2d = math::r2d<CT>();

        result_type result;

        CT lat1 = la1;
        CT lat2 = la2;

        CT lon1 = lo1;
        CT lon2 = lo2;

        CT const a = CT(get_radius<0>(spheroid));
        CT const b = CT(get_radius<2>(spheroid));
        CT const f = formula::flattening<CT>(spheroid);
        CT const one_minus_f = c1 - f;
        CT const two_minus_f = c2 - f;

        CT const tol0 = std::numeric_limits<CT>::epsilon();
        CT const tol1 = c200 * tol0;
        CT const tol2 = sqrt(tol0);

        // Check on bisection interval.
        CT const tol_bisection = tol0 * tol2;

        CT const etol2 = c0_1 * tol2 /
            sqrt((std::max)(c0_001, std::abs(f)) * (std::min)(c1, c1 - f / c2) / c2);

        CT tiny = std::sqrt((std::numeric_limits<CT>::min)());

        CT const n = f / two_minus_f;
        CT const e2 = f * two_minus_f;
        CT const ep2 = e2 / math::sqr(one_minus_f);

        // Compute the longitudinal difference.
        CT lon12_error;
        CT lon12 = math::difference_angle(lon1, lon2, lon12_error);

        int lon12_sign = lon12 >= 0 ? 1 : -1;

        // Make points close to the meridian to lie on it.
        lon12 = lon12_sign * lon12;
        lon12_error = (c180 - lon12) - lon12_sign * lon12_error;

        // Convert to radians.
        CT lam12 = lon12 * d2r;
        CT sin_lam12;
        CT cos_lam12;

        if (lon12 > c90)
        {
            math::sin_cos_degrees(lon12_error, sin_lam12, cos_lam12);
            cos_lam12 *= -c1;
        }
        else
        {
            math::sin_cos_degrees(lon12, sin_lam12, cos_lam12);
        }

        // Make points close to the equator to lie on it.
        lat1 = math::round_angle(std::abs(lat1) > c90 ? c90 : lat1);
        lat2 = math::round_angle(std::abs(lat2) > c90 ? c90 : lat2);

        // Arrange points in a canonical form, as explained in
        // paper, Algorithms for geodesics, Eq. (44):
        //
        //     0 <= lon12 <= 180
        //     -90 <= lat1 <= 0
        //     lat1 <= lat2 <= -lat1
        int swap_point = std::abs(lat1) < std::abs(lat2) ? -1 : 1;

        if (swap_point < 0)
        {
            lon12_sign *= -1;
            swap(lat1, lat2);
        }

        // Enforce lat1 to be <= 0.
        int lat_sign = lat1 < 0 ? 1 : -1;
        lat1 *= lat_sign;
        lat2 *= lat_sign;

        CT sin_beta1, cos_beta1;
        math::sin_cos_degrees(lat1, sin_beta1, cos_beta1);
        sin_beta1 *= one_minus_f;

        math::normalize_unit_vector<CT>(sin_beta1, cos_beta1);
        cos_beta1 = (std::max)(tiny, cos_beta1);

        CT sin_beta2, cos_beta2;
        math::sin_cos_degrees(lat2, sin_beta2, cos_beta2);
        sin_beta2 *= one_minus_f;

        math::normalize_unit_vector<CT>(sin_beta2, cos_beta2);
        cos_beta2 = (std::max)(tiny, cos_beta2);

        // If cos_beta1 < -sin_beta1, then cos_beta2 - cos_beta1 is a
        // sensitive measure of the |beta1| - |beta2|. Alternatively,
        // (cos_beta1 >= -sin_beta1), abs(sin_beta2) + sin_beta1 is
        // a better measure.
        // Sometimes these quantities vanish and in that case we
        // force beta2 = +/- bet1a exactly.
        if (cos_beta1 < -sin_beta1)
        {
            if (cos_beta1 == cos_beta2)
            {
                sin_beta2 = sin_beta2 < 0 ? sin_beta1 : -sin_beta1;
            }
        }
        else
        {
            if (std::abs(sin_beta2) == -sin_beta1)
            {
                cos_beta2 = cos_beta1;
            }
        }

        CT const dn1 = sqrt(c1 + ep2 * math::sqr(sin_beta1));
        CT const dn2 = sqrt(c1 + ep2 * math::sqr(sin_beta2));

        CT sigma12;
        CT m12x, s12x, M21;

        // Index zero element of coeffs_C1 is unused.
        se::coeffs_C1<SeriesOrder, CT> const coeffs_C1(n);

        bool meridian = lat1 == -90 || sin_lam12 == 0;

        CT cos_alpha1, sin_alpha1;
        CT cos_alpha2, sin_alpha2;

        if (meridian)
        {
            // Endpoints lie on a single full meridian.

            // Point to the target latitude.
            cos_alpha1 = cos_lam12;
            sin_alpha1 = sin_lam12;

            // Heading north at the target.
            cos_alpha2 = c1;
            sin_alpha2 = c0;

            CT sin_sigma1 = sin_beta1;
            CT cos_sigma1 = cos_alpha1 * cos_beta1;

            CT sin_sigma2 = sin_beta2;
            CT cos_sigma2 = cos_alpha2 * cos_beta2;

            CT sigma12 = std::atan2((std::max)(c0, cos_sigma1 * sin_sigma2 - sin_sigma1 * cos_sigma2),
                                                   cos_sigma1 * cos_sigma2 + sin_sigma1 * sin_sigma2);

            CT dummy;
            meridian_length(n, ep2, sigma12, sin_sigma1, cos_sigma1, dn1,
                                             sin_sigma2, cos_sigma2, dn2,
                                             cos_beta1, cos_beta2, s12x,
                                             m12x, dummy, result.geodesic_scale,
                                             M21, coeffs_C1);

            if (sigma12 < c1 || m12x >= c0)
            {
                if (sigma12 < c3 * tiny)
                {
                    sigma12  = m12x = s12x = c0;
                }

                m12x *= b;
                s12x *= b;
            }
            else
            {
                // m12 < 0, i.e., prolate and too close to anti-podal.
                meridian = false;
            }
        }

        CT omega12;

        if (!meridian && sin_beta1 == c0 &&
            (f <= c0 || lon12_error >= f * c180))
        {
            // Points lie on the equator.
            cos_alpha1 = cos_alpha2 = c0;
            sin_alpha1 = sin_alpha2 = c1;

            s12x = a * lam12;
            sigma12 = omega12 = lam12 / one_minus_f;
            m12x = b * sin(sigma12);

            if (BOOST_GEOMETRY_CONDITION(EnableGeodesicScale))
            {
                result.geodesic_scale = cos(sigma12);
            }
        }
        else if (!meridian)
        {
            // If point1 and point2 belong within a hemisphere bounded by a
            // meridian and geodesic is neither meridional nor equatorial.

            // Find the starting point for Newton's method.
            CT dnm;
            sigma12 = newton_start(sin_beta1, cos_beta1, dn1,
                                   sin_beta2, cos_beta2, dn2,
                                   lam12, sin_lam12, cos_lam12,
                                   sin_alpha1, cos_alpha1,
                                   sin_alpha2, cos_alpha2,
                                   dnm, coeffs_C1, ep2,
                                   tol1, tol2, etol2,
                                   n, f);

            if (sigma12 >= c0)
            {
                // Short lines case (newton_start sets sin_alpha2, cos_alpha2, dnm).
                s12x = sigma12 * b * dnm;
                m12x = math::sqr(dnm) * b * sin(sigma12 / dnm);
                if (BOOST_GEOMETRY_CONDITION(EnableGeodesicScale))
                {
                    result.geodesic_scale = cos(sigma12 / dnm);
                }

                // Convert to radians.
                omega12 = lam12 / (one_minus_f * dnm);
            }
            else
            {
                // Apply the Newton's method.
                CT sin_sigma1 = c0, cos_sigma1 = c0;
                CT sin_sigma2 = c0, cos_sigma2 = c0;
                CT eps = c0, diff_omega12 = c0;

                // Bracketing range.
                CT sin_alpha1a = tiny, cos_alpha1a = c1;
                CT sin_alpha1b = tiny, cos_alpha1b = -c1;

                size_t iteration = 0;
                size_t max_iterations = 20 + std::numeric_limits<size_t>::digits + 10;

                for (bool tripn = false, tripb = false;
                     iteration < max_iterations;
                     ++iteration)
                {
                    CT dv;
                    CT v = lambda12(sin_beta1, cos_beta1, dn1,
                                    sin_beta2, cos_beta2, dn2,
                                    sin_alpha1, cos_alpha1,
                                    sin_lam12, cos_lam12,
                                    sin_alpha2, cos_alpha2,
                                    sigma12,
                                    sin_sigma1, cos_sigma1,
                                    sin_sigma2, cos_sigma2,
                                    eps, diff_omega12,
                                    iteration < max_iterations,
                                    dv, f, n, ep2, tiny, coeffs_C1);

                    // Reversed test to allow escape with NaNs.
                    if (tripb || !(std::abs(v) >= (tripn ? c8 : c1) * tol0))
                        break;

                    // Update bracketing values.
                    if (v > c0 && (iteration > max_iterations ||
                        cos_alpha1 / sin_alpha1 > cos_alpha1b / sin_alpha1b))
                    {
                        sin_alpha1b = sin_alpha1;
                        cos_alpha1b = cos_alpha1;
                    }
                    else if (v < c0 && (iteration > max_iterations ||
                             cos_alpha1 / sin_alpha1 < cos_alpha1a / sin_alpha1a))
                    {
                        sin_alpha1a = sin_alpha1;
                        cos_alpha1a = cos_alpha1;
                    }

                    if (iteration < max_iterations && dv > c0)
                    {
                        CT diff_alpha1 = -v / dv;

                        CT sin_diff_alpha1 = sin(diff_alpha1);
                        CT cos_diff_alpha1 = cos(diff_alpha1);

                        CT nsin_alpha1 = sin_alpha1 * cos_diff_alpha1 +
                            cos_alpha1 * sin_diff_alpha1;

                        if (nsin_alpha1 > c0 && std::abs(diff_alpha1) < pi)
                        {
                            cos_alpha1 = cos_alpha1 * cos_diff_alpha1 - sin_alpha1 * sin_diff_alpha1;
                            sin_alpha1 = nsin_alpha1;
                            math::normalize_unit_vector<CT>(sin_alpha1, cos_alpha1);

                            // In some regimes we don't get quadratic convergence because
                            // slope -> 0. So use convergence conditions based on epsilon
                            // instead of sqrt(epsilon).
                            tripn = std::abs(v) <= c16 * tol0;
                            continue;
                        }
                    }

                    // Either dv was not positive or updated value was outside legal
                    // range. Use the midpoint of the bracket as the next estimate.
                    // This mechanism is not needed for the WGS84 ellipsoid, but it does
                    // catch problems with more eeccentric ellipsoids. Its efficacy is
                    // such for the WGS84 test set with the starting guess set to alp1 =
                    // 90deg:
                    // the WGS84 test set: mean = 5.21, sd = 3.93, max = 24
                    // WGS84 and random input: mean = 4.74, sd = 0.99
                    sin_alpha1 = (sin_alpha1a + sin_alpha1b) / c2;
                    cos_alpha1 = (cos_alpha1a + cos_alpha1b) / c2;
                    math::normalize_unit_vector<CT>(sin_alpha1, cos_alpha1);
                    tripn = false;
                    tripb = (std::abs(sin_alpha1a - sin_alpha1) + (cos_alpha1a - cos_alpha1) < tol_bisection ||
                             std::abs(sin_alpha1 - sin_alpha1b) + (cos_alpha1 - cos_alpha1b) < tol_bisection);
                }

                CT dummy;
                se::coeffs_C1<SeriesOrder, CT> const coeffs_C1_eps(eps);
                // Ensure that the reduced length and geodesic scale are computed in
                // a "canonical" way, with the I2 integral.
                meridian_length(eps, ep2, sigma12, sin_sigma1, cos_sigma1, dn1,
                                                   sin_sigma2, cos_sigma2, dn2,
                                                   cos_beta1, cos_beta2, s12x,
                                                   m12x, dummy, result.geodesic_scale,
                                                   M21, coeffs_C1_eps);

                m12x *= b;
                s12x *= b;
            }
        }

        if (swap_point < 0)
        {
            swap(sin_alpha1, sin_alpha2);
            swap(cos_alpha1, cos_alpha2);
            swap(result.geodesic_scale, M21);
        }

        sin_alpha1 *= swap_point * lon12_sign;
        cos_alpha1 *= swap_point * lat_sign;

        sin_alpha2 *= swap_point * lon12_sign;
        cos_alpha2 *= swap_point * lat_sign;

        if (BOOST_GEOMETRY_CONDITION(EnableReducedLength))
        {
            result.reduced_length = m12x;
        }

        if (BOOST_GEOMETRY_CONDITION(CalcAzimuths))
        {
            if (BOOST_GEOMETRY_CONDITION(CalcFwdAzimuth))
            {
                result.azimuth = atan2(sin_alpha1, cos_alpha1) * r2d;
            }

            if (BOOST_GEOMETRY_CONDITION(CalcRevAzimuth))
            {
                result.reverse_azimuth = atan2(sin_alpha2, cos_alpha2) * r2d;
            }
        }

        if (BOOST_GEOMETRY_CONDITION(EnableDistance))
        {
            result.distance = s12x;
        }

        return result;
    }

    template <typename CoeffsC1>
    static inline void meridian_length(CT const& epsilon, CT const& ep2, CT const& sigma12,
                                       CT const& sin_sigma1, CT const& cos_sigma1, CT const& dn1,
                                       CT const& sin_sigma2, CT const& cos_sigma2, CT const& dn2,
                                       CT const& cos_beta1, CT const& cos_beta2,
                                       CT& s12x, CT& m12x, CT& m0,
                                       CT& M12, CT& M21,
                                       CoeffsC1 const& coeffs_C1)
    {
        static CT const c1 = 1;

        CT A12x = 0, J12 = 0;
        CT expansion_A1, expansion_A2;

        // Evaluate the coefficients for C2.
        se::coeffs_C2<SeriesOrder, CT> coeffs_C2(epsilon);

        if (BOOST_GEOMETRY_CONDITION(EnableDistance) ||
            BOOST_GEOMETRY_CONDITION(EnableReducedLength) ||
            BOOST_GEOMETRY_CONDITION(EnableGeodesicScale))
        {
            // Find the coefficients for A1 by computing the
            // series expansion using Horner scehme.
            expansion_A1 = se::evaluate_A1<SeriesOrder>(epsilon);

            if (BOOST_GEOMETRY_CONDITION(EnableReducedLength) ||
                BOOST_GEOMETRY_CONDITION(EnableGeodesicScale))
            {
                // Find the coefficients for A2 by computing the
                // series expansion using Horner scehme.
                expansion_A2 = se::evaluate_A2<SeriesOrder>(epsilon);

                A12x = expansion_A1 - expansion_A2;
                expansion_A2 += c1;
            }
            expansion_A1 += c1;
        }

        if (BOOST_GEOMETRY_CONDITION(EnableDistance))
        {
            CT B1 = se::sin_cos_series(sin_sigma2, cos_sigma2, coeffs_C1)
                  - se::sin_cos_series(sin_sigma1, cos_sigma1, coeffs_C1);

            s12x = expansion_A1 * (sigma12 + B1);

            if (BOOST_GEOMETRY_CONDITION(EnableReducedLength) ||
                BOOST_GEOMETRY_CONDITION(EnableGeodesicScale))
            {
                CT B2 = se::sin_cos_series(sin_sigma2, cos_sigma2, coeffs_C2)
                      - se::sin_cos_series(sin_sigma1, cos_sigma1, coeffs_C2);

                J12 = A12x * sigma12 + (expansion_A1 * B1 - expansion_A2 * B2);
            }
        }
        else if (BOOST_GEOMETRY_CONDITION(EnableReducedLength) ||
                 BOOST_GEOMETRY_CONDITION(EnableGeodesicScale))
        {
            for (size_t i = 1; i <= SeriesOrder; ++i)
            {
                coeffs_C2[i] = expansion_A1 * coeffs_C1[i] -
                               expansion_A2 * coeffs_C2[i];
            }

            J12 = A12x * sigma12 +
                   (se::sin_cos_series(sin_sigma2, cos_sigma2, coeffs_C2)
                  - se::sin_cos_series(sin_sigma1, cos_sigma1, coeffs_C2));
        }

        if (BOOST_GEOMETRY_CONDITION(EnableReducedLength))
        {
            m0 = A12x;

            m12x = dn2 * (cos_sigma1 * sin_sigma2) -
                   dn1 * (sin_sigma1 * cos_sigma2) -
                   cos_sigma1 * cos_sigma2 * J12;
        }

        if (BOOST_GEOMETRY_CONDITION(EnableGeodesicScale))
        {
            CT cos_sigma12 = cos_sigma1 * cos_sigma2 + sin_sigma1 * sin_sigma2;
            CT t = ep2 * (cos_beta1 - cos_beta2) *
                         (cos_beta1 + cos_beta2) / (dn1 + dn2);

            M12 = cos_sigma12 + (t * sin_sigma2 - cos_sigma2 * J12) * sin_sigma1 / dn1;
            M21 = cos_sigma12 - (t * sin_sigma1 - cos_sigma1 * J12) * sin_sigma2 / dn2;
        }
    }

    /*
     Return a starting point for Newton's method in sin_alpha1 and
     cos_alpha1 (function value is -1). If Newton's method
     doesn't need to be used, return also sin_alpha2 and
     cos_alpha2 and function value is sig12.
    */
    template <typename CoeffsC1>
    static inline CT newton_start(CT const& sin_beta1, CT const& cos_beta1, CT const& dn1,
                                  CT const& sin_beta2, CT const& cos_beta2, CT dn2,
                                  CT const& lam12, CT const& sin_lam12, CT const& cos_lam12,
                                  CT& sin_alpha1, CT& cos_alpha1,
                                  CT& sin_alpha2, CT& cos_alpha2,
                                  CT& dnm, CoeffsC1 const& coeffs_C1, CT const& ep2,
                                  CT const& tol1, CT const& tol2, CT const& etol2, CT const& n, CT const& f)
    {
        static CT const c0 = 0;
        static CT const c0_01 = 0.01;
        static CT const c0_1 = 0.1;
        static CT const c0_5 = 0.5;
        static CT const c1 = 1;
        static CT const c2 = 2;
        static CT const c6 = 6;
        static CT const c1000 = 1000;
        static CT const pi = math::pi<CT>();

        CT const one_minus_f = c1 - f;
        CT const x_thresh = c1000 * tol2;

        // Return a starting point for Newton's method in sin_alpha1
        // and cos_alpha1 (function value is -1). If Newton's method
        // doesn't need to be used, return also sin_alpha2 and
        // cos_alpha2 and function value is sig12.
        CT sig12 = -c1;

        // bet12 = bet2 - bet1 in [0, pi); beta12a = bet2 + bet1 in (-pi, 0]
        CT sin_beta12 = sin_beta2 * cos_beta1 - cos_beta2 * sin_beta1;
        CT cos_beta12 = cos_beta2 * cos_beta1 + sin_beta2 * sin_beta1;

        CT sin_beta12a = sin_beta2 * cos_beta1 + cos_beta2 * sin_beta1;

        bool shortline = cos_beta12 >= c0 && sin_beta12 < c0_5 &&
            cos_beta2 * lam12 < c0_5;

        CT sin_omega12, cos_omega12;

        if (shortline)
        {
            CT sin_beta_m2 = math::sqr(sin_beta1 + sin_beta2);

            sin_beta_m2 /= sin_beta_m2 + math::sqr(cos_beta1 + cos_beta2);
            dnm = math::sqrt(c1 + ep2 * sin_beta_m2);

            CT omega12 = lam12 / (one_minus_f * dnm);

            sin_omega12 = sin(omega12);
            cos_omega12 = cos(omega12);
        }
        else
        {
            sin_omega12 = sin_lam12;
            cos_omega12 = cos_lam12;
        }

        sin_alpha1 = cos_beta2 * sin_omega12;
        cos_alpha1 = cos_omega12 >= c0 ?
            sin_beta12 + cos_beta2 * sin_beta1 * math::sqr(sin_omega12) / (c1 + cos_omega12) :
            sin_beta12a - cos_beta2 * sin_beta1 * math::sqr(sin_omega12) / (c1 - cos_omega12);

        CT sin_sigma12 = boost::math::hypot(sin_alpha1, cos_alpha1);
        CT cos_sigma12 = sin_beta1 * sin_beta2 + cos_beta1 * cos_beta2 * cos_omega12;

        if (shortline && sin_sigma12 < etol2)
        {
            sin_alpha2 = cos_beta1 * sin_omega12;
            cos_alpha2 = sin_beta12 - cos_beta1 * sin_beta2 *
                (cos_omega12 >= c0 ? math::sqr(sin_omega12) /
                (c1 + cos_omega12) : c1 - cos_omega12);

            math::normalize_unit_vector<CT>(sin_alpha2, cos_alpha2);
            // Set return value.
            sig12 = atan2(sin_sigma12, cos_sigma12);
        }
        // Skip astroid calculation if too eccentric.
        else if (std::abs(n) > c0_1 ||
                 cos_sigma12 >= c0 ||
                 sin_sigma12 >= c6 * std::abs(n) * pi *
                 math::sqr(cos_beta1))
        {
            // Nothing to do, zeroth order spherical approximation will do.
        }
        else
        {
            // Scale lam12 and bet2 to x, y coordinate system where antipodal
            // point is at origin and singular point is at y = 0, x = -1.
            CT lambda_scale, beta_scale;

            CT y;
            volatile CT x;

            CT lam12x = atan2(-sin_lam12, -cos_lam12);
            if (f >= c0)
            {
                CT k2 = math::sqr(sin_beta1) * ep2;
                CT eps = k2 / (c2 * (c1 + sqrt(c1 + k2)) + k2);

                se::coeffs_A3<SeriesOrder, CT> const coeffs_A3(n);

                CT const A3 = math::horner_evaluate(eps, coeffs_A3.begin(), coeffs_A3.end());

                lambda_scale = f * cos_beta1 * A3 * pi;
                beta_scale = lambda_scale * cos_beta1;

                x = lam12x / lambda_scale;
                y = sin_beta12a / beta_scale;
            }
            else
            {
                CT cos_beta12a = cos_beta2 * cos_beta1 - sin_beta2 * sin_beta1;
                CT beta12a = atan2(sin_beta12a, cos_beta12a);

                CT m12b, m0, dummy;
                meridian_length(n, ep2, pi + beta12a,
                                sin_beta1, -cos_beta1, dn1,
                                sin_beta2, cos_beta2, dn2,
                                cos_beta1, cos_beta2, dummy,
                                m12b, m0, dummy, dummy, coeffs_C1);

                x = -c1 + m12b / (cos_beta1 * cos_beta2 * m0 * pi);
                beta_scale = x < -c0_01
                           ? sin_beta12a / x
                           : -f * math::sqr(cos_beta1) * pi;
                lambda_scale = beta_scale / cos_beta1;

                y = lam12x / lambda_scale;
            }

            if (y > -tol1 && x > -c1 - x_thresh)
            {
                // Strip near cut.
                if (f >= c0)
                {
                    sin_alpha1 = (std::min)(c1, -CT(x));
                    cos_alpha1 = - math::sqrt(c1 - math::sqr(sin_alpha1));
                }
                else
                {
                    cos_alpha1 = (std::max)(CT(x > -tol1 ? c0 : -c1), CT(x));
                    sin_alpha1 = math::sqrt(c1 - math::sqr(cos_alpha1));
                }
            }
            else
            {
                // Solve the astroid problem.
                CT k = astroid(CT(x), y);

                CT omega12a = lambda_scale * (f >= c0 ? -x * k /
                    (c1 + k) : -y * (c1 + k) / k);

                sin_omega12 = sin(omega12a);
                cos_omega12 = -cos(omega12a);

                // Update spherical estimate of alpha1 using omgega12 instead of lam12.
                sin_alpha1 = cos_beta2 * sin_omega12;
                cos_alpha1 = sin_beta12a - cos_beta2 * sin_beta1 *
                    math::sqr(sin_omega12) / (c1 - cos_omega12);
            }
        }

        // Sanity check on starting guess. Backwards check allows NaN through.
        if (!(sin_alpha1 <= c0))
        {
            math::normalize_unit_vector<CT>(sin_alpha1, cos_alpha1);
        }
        else
        {
            sin_alpha1 = c1;
            cos_alpha1 = c0;
        }

        return sig12;
    }

    /*
     Solve the astroid problem using the equation:
     κ4 + 2κ3 + (1 − x2 − y 2 )κ2 − 2y 2 κ − y 2 = 0.

     For details, please refer to Eq. (65) in,
     Geodesics on an ellipsoid of revolution, Charles F.F Karney,
     https://arxiv.org/abs/1102.1215
    */
    static inline CT astroid(CT const& x, CT const& y)
    {
        static CT const c0 = 0;
        static CT const c1 = 1;
        static CT const c2 = 2;
        static CT const c3 = 3;
        static CT const c4 = 4;
        static CT const c6 = 6;

        CT k;

        CT p = math::sqr(x);
        CT q = math::sqr(y);
        CT r = (p + q - c1) / c6;

        if (!(q == c0 && r <= c0))
        {
            // Avoid possible division by zero when r = 0 by multiplying
            // equations for s and t by r^3 and r, respectively.
            CT S = p * q / c4;
            CT r2 = math::sqr(r);
            CT r3 = r * r2;

            // The discriminant of the quadratic equation for T3. This is
            // zero on the evolute curve p^(1/3)+q^(1/3) = 1.
            CT discriminant = S * (S + c2 * r3);

            CT u = r;

            if (discriminant >= c0)
            {
                CT T3 = S + r3;

                // Pick the sign on the sqrt to maximize abs(T3). This minimizes
                // loss of precision due to cancellation. The result is unchanged
                // because of the way the T is used in definition of u.
                T3 += T3 < c0 ? -std::sqrt(discriminant) : std::sqrt(discriminant);

                CT T = std::cbrt(T3);

                // T can be zero; but then r2 / T -> 0.
                u += T + (T != c0 ? r2 / T : c0);
            }
            else
            {
                CT ang = std::atan2(std::sqrt(-discriminant), -(S + r3));

                // There are three possible cube roots. We choose the root which avoids
                // cancellation. Note that discriminant < 0 implies that r < 0.
                u += c2 * r * cos(ang / c3);
            }

            CT v = std::sqrt(math::sqr(u) + q);

            // Avoid loss of accuracy when u < 0.
            CT uv = u < c0 ? q / (v - u) : u + v;
            CT w = (uv - q) / (c2 * v);

            // Rearrange expression for k to avoid loss of accuracy due to
            // subtraction. Division by 0 not possible because uv > 0, w >= 0.
            k = uv / (std::sqrt(uv + math::sqr(w)) + w);
        }
        else // q == 0 && r <= 0
        {
            // y = 0 with |x| <= 1. Handle this case directly.
            // For y small, positive root is k = abs(y)/sqrt(1-x^2).
            k = c0;
        }
        return k;
    }

    template <typename CoeffsC1>
    static inline CT lambda12(CT const& sin_beta1, CT const& cos_beta1, CT const& dn1,
                              CT const& sin_beta2, CT const& cos_beta2, CT const& dn2,
                              CT const& sin_alpha1, CT cos_alpha1,
                              CT const& sin_lam120, CT const& cos_lam120,
                              CT& sin_alpha2, CT& cos_alpha2,
                              CT& sigma12,
                              CT& sin_sigma1, CT& cos_sigma1,
                              CT& sin_sigma2, CT& cos_sigma2,
                              CT& eps, CT& diff_omega12,
                              bool diffp, CT& diff_lam12,
                              CT const& f, CT const& n, CT const& ep2, CT const& tiny,
                              CoeffsC1 const& coeffs_C1)
    {
        static CT const c0 = 0;
        static CT const c1 = 1;
        static CT const c2 = 2;

        CT const one_minus_f = c1 - f;

        if (sin_beta1 == c0 && cos_alpha1 == c0)
        {
            // Break degeneracy of equatorial line.
            cos_alpha1 = -tiny;
        }

        CT sin_alpha0 = sin_alpha1 * cos_beta1;
        CT cos_alpha0 = boost::math::hypot(cos_alpha1, sin_alpha1 * sin_beta1);

        CT sin_omega1, cos_omega1;
        CT sin_omega2, cos_omega2;
        CT sin_omega12, cos_omega12;

        CT lam12;

        sin_sigma1 = sin_beta1;
        sin_omega1 = sin_alpha0 * sin_beta1;

        cos_sigma1 = cos_omega1 = cos_alpha1 * cos_beta1;

        math::normalize_unit_vector<CT>(sin_sigma1, cos_sigma1);

        // Enforce symmetries in the case abs(beta2) = -beta1.
        // Otherwise, this can yield singularities in the Newton iteration.

        // sin(alpha2) * cos(beta2) = sin(alpha0).
        sin_alpha2 = cos_beta2 != cos_beta1 ?
            sin_alpha0 / cos_beta2 : sin_alpha1;

        cos_alpha2 = cos_beta2 != cos_beta1 || std::abs(sin_beta2) != -sin_beta1 ?
            sqrt(math::sqr(cos_alpha1 * cos_beta1) +
                (cos_beta1 < -sin_beta1 ?
                    (cos_beta2 - cos_beta1) * (cos_beta1 + cos_beta2) :
                    (sin_beta1 - sin_beta2) * (sin_beta1 + sin_beta2))) / cos_beta2 :
            std::abs(cos_alpha1);

        sin_sigma2 = sin_beta2;
        sin_omega2 = sin_alpha0 * sin_beta2;

        cos_sigma2 = cos_omega2 =
            (cos_alpha2 * cos_beta2);

        // Break degeneracy of equatorial line.
        math::normalize_unit_vector<CT>(sin_sigma2, cos_sigma2);

        // sig12 = sig2 - sig1, limit to [0, pi].
        sigma12 = atan2((std::max)(c0, cos_sigma1 * sin_sigma2 - sin_sigma1 * cos_sigma2),
                                          cos_sigma1 * cos_sigma2 + sin_sigma1 * sin_sigma2);

        // omg12 = omg2 - omg1, limit to [0, pi].
        sin_omega12 = (std::max)(c0, cos_omega1 * sin_omega2 - sin_omega1 * cos_omega2);
        cos_omega12 = cos_omega1 * cos_omega2 + sin_omega1 * sin_omega2;

        // eta = omg12 - lam120.
        CT eta = atan2(sin_omega12 * cos_lam120 - cos_omega12 * sin_lam120,
                       cos_omega12 * cos_lam120 + sin_omega12 * sin_lam120);

        CT B312;
        CT k2 = math::sqr(cos_alpha0) * ep2;

        eps = k2 / (c2 * (c1 + std::sqrt(c1 + k2)) + k2);

        se::coeffs_C3<SeriesOrder, CT> const coeffs_C3(n, eps);

        B312 = se::sin_cos_series(sin_sigma2, cos_sigma2, coeffs_C3)
             - se::sin_cos_series(sin_sigma1, cos_sigma1, coeffs_C3);

        se::coeffs_A3<SeriesOrder, CT> const coeffs_A3(n);

        CT const A3 = math::horner_evaluate(eps, coeffs_A3.begin(), coeffs_A3.end());

        diff_omega12 = -f * A3 * sin_alpha0 * (sigma12 + B312);
        lam12 = eta + diff_omega12;

        if (diffp)
        {
            if (cos_alpha2 == c0)
            {
                diff_lam12 = - c2 * one_minus_f * dn1 / sin_beta1;
            }
            else
            {
                CT dummy;
                meridian_length(eps, ep2, sigma12, sin_sigma1, cos_sigma1, dn1,
                                                   sin_sigma2, cos_sigma2, dn2,
                                                   cos_beta1, cos_beta2, dummy,
                                                   diff_lam12, dummy, dummy,
                                                   dummy, coeffs_C1);

                diff_lam12 *= one_minus_f / (cos_alpha2 * cos_beta2);
            }
        }
        return lam12;
    }

};

}}} // namespace boost::geometry::formula

#endif // BOOST_GEOMETRY_FORMULAS_KARNEY_INVERSE_HPP


#include <stdio.h>
#include "Substrate.h"
#include "Mathematix.h"
#include "finiteDifferences.h"
//#include <boost/lambda/lambda.hpp>

#define DX 13.

#define EXPLICIT        0
#define IMPLICIT        1
#define CRANK_NICHOLSON 2
#define ADI             3

//#define DIFFUSION_METHODE           EXPLICIT
//#define DIFFUSION_METHODE           IMPLICIT
//#define DIFFUSION_METHODE           CRANK_NICHOLSON
#define DIFFUSION_METHODE           ADI
#define DIFFUSION_REACTION_SEPARAT FALSE
//#define DIFFUSION_REACTION_SEPARAT TRUE

//#define DT 0.01

float **A;
float *b;//[20];
float *x;//[20];
int ADI_direction = 1;

void initMatrix( VoronoiDiagram *voronoiDiagram/*, int imax, int iimax*/)
{
	int matrixDim = voronoiDiagram->xN[0];

	for( int dim=1; dim<DIMENSIONS; dim++)
		if(voronoiDiagram->xN[dim]>matrixDim)
			matrixDim = voronoiDiagram->xN[dim];

	A = newMatrix( matrixDim, matrixDim);

	b = (float*) malloc( matrixDim * sizeof(float));
	x = (float*) malloc( matrixDim * sizeof(float));
/*	for( int i=0; i<imax; i++)
		for( int ii=0; ii<iimax; ii++){
			A[i][ii] = voronoiDiagram->voronoiCells[i + ii*20];
		}
*/}
double factor = 1.;

/*void setupMatrixOxygenX( VoronoiDiagram *voronoiDiagram, int iset, double timeStep)
{
	double r = factor*Oxygen_Diffusion * timeStep/(DX*DX);

	for( int m=0; m<20; m++)
		for( int n=0; n<20; n++)
			A[m][n] = 0;

	for( int m=0; m<20; m++){
		A[m][m]   = 1;

		if( m>0){
			A[m][m-1] = -r;
			A[m][m]  += r;
		}
		if( m<19){
			A[m][m]  += r;
			A[m][m+1] = -r;
		}

		b[m] = voronoiDiagram->voronoiCells[iset + m*20]->oxygen - timeStep * voronoiDiagram->voronoiCells[iset + m*20]->oxygen * GiveMeTheOxygenRate(  voronoiDiagram->voronoiCells[iset + m*20]);
	}
}

void setupMatrixOxygenY( VoronoiDiagram *voronoiDiagram, int iiset, double timeStep)
{
	double r = factor*Oxygen_Diffusion * timeStep/(DX*DX);

	for( int m=0; m<20; m++)
		for( int n=0; n<20; n++)
			A[m][n] = 0;

	for( int m=0; m<20; m++){
		A[m][m]   = 1;

		if( m>0){
			A[m][m-1] = -r;
			A[m][m]  += r;
		}
		if( m<19){
			A[m][m]  += r;
			A[m][m+1] = -r;
		}

		b[m] = voronoiDiagram->voronoiCells[m + iiset*20]->oxygen - timeStep * voronoiDiagram->voronoiCells[m+iiset*20]->oxygen * GiveMeTheOxygenRate(  voronoiDiagram->voronoiCells[m+iiset*20]);
	}
}*/

void setupMatrix( VoronoiDiagram *voronoiDiagram, int base_index, double timeStep, char molecule, char direction)
{
	double r;
	//double consumption;

	if( molecule == 'o'){
		r = factor*Oxygen_Diffusion * timeStep/(DX*DX);
	}else{
		r = factor*Glucose_Diffusion * timeStep/(DX*DX);
	}

	for( int m=0; m<voronoiDiagram->xN[(int)direction]/*20*/; m++)
		for( int n=0; n<voronoiDiagram->xN[(int)direction]/*20*/; n++)
			A[m][n] = 0;

	for( int m=0; m<voronoiDiagram->xN[(int)direction]/*20*/; m++){
		if( m>0 && m<voronoiDiagram->xN[(int)direction]-1){
			A[m][m-1] = -r;
			A[m][m]   =  1 + 2*r;
			A[m][m+1] = -r;
		}
		else{
			A[m][m]   = 1;
		}

		int di = (int)pow(voronoiDiagram->xN[(int)direction],direction);
		int index = base_index + (int)(m*di);
		//fprintf(stderr, "m=%i, dir=%i,  %i + %i => %i (< %i)\n", m, direction, base_index, (int)(m*pow(20,direction)), index, (int)pow(20,3));

		if( molecule == 'o')
			b[m] = voronoiDiagram->voronoiCells[index]->oxygen
#if DIFFUSION_REACTION_SEPARAT == FALSE
			     - timeStep/DIMENSIONS * voronoiDiagram->voronoiCells[index]->oxygen * GiveMeTheOxygenRate(  voronoiDiagram->voronoiCells[index])
#endif
			    ;
		else
			b[m] = voronoiDiagram->voronoiCells[index]->glucose
#if DIFFUSION_REACTION_SEPARAT == FALSE
			     - timeStep/DIMENSIONS * voronoiDiagram->voronoiCells[index]->glucose * GiveMeTheGlucoseRate(  voronoiDiagram->voronoiCells[index])
#endif
			    ;
		/*for( int n=0; n<20; n++)
			fprintf( stderr, "%lf ", A[m][n]);
		fprintf( stderr, "| %lf\n", b[m]);*/
	}
}

void setupMatrixADI( VoronoiDiagram *voronoiDiagram, int base_index, double timeStep, char molecule, char direction)
{
	double r;
	//double consumption;

	if( molecule == 'o'){
		r = factor*Oxygen_Diffusion * timeStep/(DX*DX);
	}else{
		r = factor*Glucose_Diffusion * timeStep/(DX*DX);
	}

	for( int m=0; m<voronoiDiagram->xN[(int)direction]/*20*/; m++)
		for( int n=0; n<voronoiDiagram->xN[(int)direction]/*20*/; n++)
			A[m][n] = 0;

	for( int m=0; m<voronoiDiagram->xN[(int)direction]/*20*/; m++){
		if( m>0 && m<voronoiDiagram->xN[(int)direction]-1){
			A[m][m-1] = -r;
			A[m][m]   =  1 + 2*r;
			A[m][m+1] = -r;
		}
		else{
			A[m][m]   = 1;
		}
		/*if( m>0){
			A[m][m-1] = -r;
			A[m][m]  += r;
		}
		if( m<voronoiDiagram->xN[(int)direction]-1){
			A[m][m]  += r;
			A[m][m+1] = -r;
		}*/

		int di = (int)pow(voronoiDiagram->xN[(int)direction],direction);
		int index = base_index + (int)(m*di);
		//fprintf(stderr, "m=%i, dir=%i,  %i + %i => %i (< %i)\n", m, direction, base_index, (int)(m*pow(20,direction)), index, (int)pow(20,3));

		if( molecule == 'o')
			b[m] = voronoiDiagram->voronoiCells[index]->oxygen
#if DIFFUSION_REACTION_SEPARAT == FALSE
			     - timeStep/DIMENSIONS * voronoiDiagram->voronoiCells[index]->oxygen * GiveMeTheOxygenRate(  voronoiDiagram->voronoiCells[index])
#endif
			    ;
		else
			b[m] = voronoiDiagram->voronoiCells[index]->glucose
#if DIFFUSION_REACTION_SEPARAT == FALSE
			     - timeStep/DIMENSIONS * voronoiDiagram->voronoiCells[index]->glucose * GiveMeTheGlucoseRate(  voronoiDiagram->voronoiCells[index])
#endif
			    ;
		/*for( int n=0; n<20; n++)
			fprintf( stderr, "%lf ", A[m][n]);
		fprintf( stderr, "| %lf\n", b[m]);*/
	}
}

void setupMatrixCrankNicholson( VoronoiDiagram *voronoiDiagram, int base_index, double timeStep, char molecule, char direction)
{
	double r;
	//double consumption;

	if( molecule == 'o'){
		r = Oxygen_Diffusion * timeStep/(DX*DX);
	}else{
		r = Glucose_Diffusion * timeStep/(DX*DX);
	}

	for( int m=0; m<20; m++)
		for( int n=0; n<20; n++)
			A[m][n] = 0;

	for( int m=0; m<20; m++){
		//A[m][m]   = 2;

		if( m>0){
			A[m][m-1] = -r;
			A[m][m]  += 1+r;
		}
		if( m<19){
			A[m][m]  += 1+r;
			A[m][m+1] = -r;
		}

		int index, index_minus, index_plus;

		index = base_index + (int)(m*pow(20,direction));
		index_plus  = base_index + (int)((m+1)*pow(20,direction));
		index_minus = base_index + (int)((m-1)*pow(20,direction));

		double factor, factor_minus, factor_plus;
		factor       = 1-r;
		factor_plus  = 0;
		factor_minus = 0;

#if DIFFUSION_REACTION_SEPARAT == FALSE
		if( molecule == 'o'){
			factor       -= GiveMeTheOxygenRate(  voronoiDiagram->voronoiCells[index]);
			if( m<19)factor_plus  = 1 - GiveMeTheOxygenRate(  voronoiDiagram->voronoiCells[index_plus]);
			if( m>0) factor_minus = 1 - GiveMeTheOxygenRate(  voronoiDiagram->voronoiCells[index_minus]);
		}else{
			factor       -= GiveMeTheGlucoseRate(  voronoiDiagram->voronoiCells[index]);
			if( m<19)factor_plus  = 1 - GiveMeTheGlucoseRate(  voronoiDiagram->voronoiCells[index_plus]);
			if( m>0) factor_minus = 1 - GiveMeTheGlucoseRate(  voronoiDiagram->voronoiCells[index_minus]);
		}
#endif
		b[m] = 0.;
		if( molecule == 'o'){
			if( m>0){
				b[m] += timeStep/DIMENSIONS * factor * voronoiDiagram->voronoiCells[index]->oxygen + timeStep/DIMENSIONS * factor_minus * voronoiDiagram->voronoiCells[index_minus]->oxygen;
			}
			if( m<19){
				b[m] += timeStep/DIMENSIONS * factor * voronoiDiagram->voronoiCells[index]->oxygen + timeStep/DIMENSIONS * factor_plus * voronoiDiagram->voronoiCells[index_plus]->oxygen;
			}
		}else{
			if( m>0){
				b[m] += timeStep/DIMENSIONS * factor * voronoiDiagram->voronoiCells[index]->glucose + timeStep/DIMENSIONS * factor_minus * voronoiDiagram->voronoiCells[index_minus]->glucose;
			}
			if( m<19){
				b[m] += timeStep/DIMENSIONS * factor * voronoiDiagram->voronoiCells[index]->glucose +timeStep/DIMENSIONS *  factor_plus * voronoiDiagram->voronoiCells[index_plus]->glucose;
			}
		}
		/*for( int n=0; n<20; n++)
			fprintf( stderr, "%lf ", A[m][n]);
		fprintf( stderr, "| %lf\n", b[m]);*/
	}
}

double secondDerivationOxygen( VoronoiCell * cell, VoronoiDiagram *voronoiDiagram)
{
	double temp = 0.;

/*	temp = - ((double)cell->countNeighborCells) * cell->oxygen;

	for( int i=0; i<cell->countNeighborCells; i++)
		temp += cell->neighborCells[i]->oxygen;

	return temp/(DX*DX) * 4./(double)cell->countNeighborCells;
*/
	int i   = 0,
	    ii  = 0,
	    iii = 0;

	int index = cell->index;

#if DIMENSIONS > 2
	iii = index / (20*20);
	index = index - iii*20*20;
#endif
#if DIMENSIONS > 1
	ii = index / 20;
	index = index - ii*20;
#endif
	i = index;
	//fprintf( stderr, "%i: (%i,%i,%i) => %i\n", cell->index, i, ii, iii, i + 20*ii + 20*20*iii);

	if( i>0)
		temp += voronoiDiagram->voronoiCells[i-1 + 20*ii + 20*20*iii]->oxygen - cell->oxygen;
	if( i<19)
		temp += voronoiDiagram->voronoiCells[i+1 + 20*ii + 20*20*iii]->oxygen - cell->oxygen;
#if DIMENSIONS > 1
	if( ii>0)
		temp += voronoiDiagram->voronoiCells[i + 20*(ii-1) + 20*20*iii]->oxygen - cell->oxygen;
	if( ii<19)
		temp += voronoiDiagram->voronoiCells[i + 20*(ii+1) + 20*20*iii]->oxygen - cell->oxygen;
#endif
#if DIMENSIONS > 2
	if( iii>0)
		temp += voronoiDiagram->voronoiCells[i + 20*ii + 20*20*(iii-1)]->oxygen - cell->oxygen;
	if( iii<19)
		temp += voronoiDiagram->voronoiCells[i + 20*ii + 20*20*(iii+1)]->oxygen - cell->oxygen;
#endif

	return temp/(DX*DX);
}

double secondDerivationGlucose( VoronoiCell * cell, VoronoiDiagram *voronoiDiagram)
{
	double temp = 0.;

/*	temp = - ((double)cell->countNeighborCells) * cell->glucose;

	for( int i=0; i<cell->countNeighborCells; i++)
		temp += cell->neighborCells[i]->glucose;

	return temp/(DX*DX) * 4./(double)cell->countNeighborCells;
*/
	int i   = 0,
	    ii  = 0,
	    iii = 0;

	int index = cell->index;

#if DIMENSIONS > 2
	iii = index / (20*20);
	index = index - iii*20*20;
#endif
#if DIMENSIONS > 1
	ii = index / 20;
	index = index - ii*20;
#endif
	i = index;
	//fprintf( stderr, "%i: (%i,%i,%i) => %i\n", cell->index, i, ii, iii, i + 20*ii + 20*20*iii);

	if( i>0)
		temp += voronoiDiagram->voronoiCells[i-1 + 20*ii + 20*20*iii]->glucose - cell->glucose;
	if( i<19)
		temp += voronoiDiagram->voronoiCells[i+1 + 20*ii + 20*20*iii]->glucose - cell->glucose;
#if DIMENSIONS > 1
	if( ii>0)
		temp += voronoiDiagram->voronoiCells[i + 20*(ii-1) + 20*20*iii]->glucose - cell->glucose;
	if( ii<19)
		temp += voronoiDiagram->voronoiCells[i + 20*(ii+1) + 20*20*iii]->glucose - cell->glucose;
#endif
#if DIMENSIONS > 2
	if( iii>0)
		temp += voronoiDiagram->voronoiCells[i + 20*ii + 20*20*(iii-1)]->glucose - cell->glucose;
	if( iii<19)
		temp += voronoiDiagram->voronoiCells[i + 20*ii + 20*20*(iii+1)]->glucose - cell->glucose;
#endif

	return temp/(DX*DX);
}

/*#if (DIFFUSION_METHODE != ADI)

double UpdateSystem( VoronoiDiagram *voronoiDiagram, double timeStep, double timeDifference){

	double time;

	int direction = (int)((myRand()*DIMENSIONS)/2.);

	for( time = 0; time+timeStep <= timeDifference; time += timeStep){

#if DIFFUSION_METHODE == EXPLICIT

		//fprintf( stderr, "EXPLICIT\n");
		// Finite Differences: explicit method
		for( int i=0; i<voronoiDiagram->countVoronoiCells; i++){

			voronoiDiagram->voronoiCells[i]->doxygen  = timeStep * (Oxygen_Diffusion  * secondDerivationOxygen(  voronoiDiagram->voronoiCells[i], voronoiDiagram) // diffusion
	#if DIFFUSION_REACTION_SEPARAT == FALSE
			                                          - voronoiDiagram->voronoiCells[i]->oxygen  * GiveMeTheOxygenRate(  voronoiDiagram->voronoiCells[i]) // reaction
	#endif
			                                          );
			voronoiDiagram->voronoiCells[i]->dglucose = timeStep * (Glucose_Diffusion * secondDerivationGlucose( voronoiDiagram->voronoiCells[i], voronoiDiagram) // diffusion
	#if DIFFUSION_REACTION_SEPARAT == FALSE
			                                          - voronoiDiagram->voronoiCells[i]->glucose * GiveMeTheGlucoseRate( voronoiDiagram->voronoiCells[i]) // reaction
	#endif
			                                          );
		}

#endif

#if (DIFFUSION_METHODE == IMPLICIT) || (DIFFUSION_METHODE == CRANK_NICHOLSON) || (DIFFUSION_METHODE == ADI)
		// Finite Differences: alternating direction implicit (ADI)
		double weight = 1.;
		// diffusion in x-direction
		for( int iset=0; iset<20; iset++){
			//setupMatrixOxygenX( voronoiDiagram, iset, timeStep);
	#if   DIFFUSION_METHODE == CRANK_NICHOLSON
			setupMatrixCrankNicholson( voronoiDiagram, iset, timeStep, 'o', 0);
			//fprintf( stderr, "CRANK_NICHOLSON\n");
	#elif DIFFUSION_METHODE == ADI
			setupMatrixADI( voronoiDiagram, iset, timeStep, 'o', direction);
	#else
			//fprintf( stderr, "ADI\n");
			setupMatrix( voronoiDiagram, iset, 0.5*timeStep, 'o', 0);
	#endif
			solveLinearSystem( A, b, x, 20);
			for( int ii=0; ii<20; ii++)
				voronoiDiagram->voronoiCells[iset + ii*20]->doxygen = weight*(b[ii] - voronoiDiagram->voronoiCells[iset + ii*20]->oxygen);

	#if DIFFUSION_METHODE == CRANK_NICHOLSON
			setupMatrixCrankNicholson( voronoiDiagram, iset, timeStep, 'g', 0);
	#elif DIFFUSION_METHODE == ADI
			setupMatrixADI( voronoiDiagram, iset, timeStep, 'g', direction);
	#else
			setupMatrix( voronoiDiagram, iset, 0.5*timeStep, 'g', 0);
	#endif
			solveLinearSystem( A, b, x, 20);
			for( int ii=0; ii<20; ii++)
				voronoiDiagram->voronoiCells[iset + ii*20]->dglucose = weight*(b[ii] - voronoiDiagram->voronoiCells[iset + ii*20]->glucose);
		}
	#if DIFFUSION_METHODE == ADI
		for( int i=0; i<voronoiDiagram->countVoronoiCells; i++){
			if( voronoiDiagram->voronoiCells[i]->position[0]>voronoiDiagram->xMin[0]+1
			 && voronoiDiagram->voronoiCells[i]->position[0]<voronoiDiagram->xMax[0]-1
			 && voronoiDiagram->voronoiCells[i]->position[1]>voronoiDiagram->xMin[1]+1
			 && voronoiDiagram->voronoiCells[i]->position[1]<voronoiDiagram->xMax[1]-1
		#if DIMENSIONS > 2
			 && voronoiDiagram->voronoiCells[i]->position[2]>voronoiDiagram->xMin[2]+1
			 && voronoiDiagram->voronoiCells[i]->position[2]<voronoiDiagram->xMax[2]-1
		#endif
			){
				voronoiDiagram->voronoiCells[i]->oxygen  += voronoiDiagram->voronoiCells[i]->doxygen;
				if( voronoiDiagram->voronoiCells[i]->oxygen < 0.) voronoiDiagram->voronoiCells[i]->oxygen = 0.;
				voronoiDiagram->voronoiCells[i]->glucose += voronoiDiagram->voronoiCells[i]->dglucose;
				if( voronoiDiagram->voronoiCells[i]->glucose < 0.) voronoiDiagram->voronoiCells[i]->glucose = 0.;
			}
		}
		direction = (direction+1)%DIMENSIONS;
	#endif

		// diffusion in y-direction
		for( int iiset=0; iiset<20; iiset++){
//			setupMatrixOxygenY( voronoiDiagram, iiset, timeStep);
	#if DIFFUSION_METHODE == CRANK_NICHOLSON
			setupMatrixCrankNicholson( voronoiDiagram, iiset, timeStep, 'o', 1);
	#elif DIFFUSION_METHODE == ADI
			setupMatrixADI( voronoiDiagram, iiset, timeStep, 'o', direction);
	#else
			setupMatrix( voronoiDiagram, iiset, 0.5*timeStep, 'o', 1);
	#endif
			solveLinearSystem( A, b, x, 20);
			for( int i=0; i<20; i++){
				voronoiDiagram->voronoiCells[i+iiset*20]->doxygen += weight*(b[i] - voronoiDiagram->voronoiCells[i+iiset*20]->oxygen);
			}

	#if DIFFUSION_METHODE == CRANK_NICHOLSON
			setupMatrixCrankNicholson( voronoiDiagram, iiset, timeStep, 'g', 1);
	#elif DIFFUSION_METHODE == ADI
			setupMatrixADI( voronoiDiagram, iiset, timeStep, 'g', direction);
	#else
			setupMatrix( voronoiDiagram, iiset, 0.5*timeStep, 'g', 1);
	#endif
			solveLinearSystem( A, b, x, 20);
			for( int i=0; i<20; i++){
				voronoiDiagram->voronoiCells[i+iiset*20]->dglucose += weight*(b[i] - voronoiDiagram->voronoiCells[i+iiset*20]->glucose);
			}
		}

#endif

		// Update
		for( int i=0; i<voronoiDiagram->countVoronoiCells; i++){
			if( voronoiDiagram->voronoiCells[i]->position[0]>voronoiDiagram->xMin[0]+1
			 && voronoiDiagram->voronoiCells[i]->position[0]<voronoiDiagram->xMax[0]-1
			 && voronoiDiagram->voronoiCells[i]->position[1]>voronoiDiagram->xMin[1]+1
			 && voronoiDiagram->voronoiCells[i]->position[1]<voronoiDiagram->xMax[1]-1
#if DIMENSIONS > 2
			 && voronoiDiagram->voronoiCells[i]->position[2]>voronoiDiagram->xMin[2]+1
			 && voronoiDiagram->voronoiCells[i]->position[2]<voronoiDiagram->xMax[2]-1
#endif
			){
				voronoiDiagram->voronoiCells[i]->oxygen  += voronoiDiagram->voronoiCells[i]->doxygen
#if DIFFUSION_REACTION_SEPARAT == TRUE
				                                          - timeStep * voronoiDiagram->voronoiCells[i]->oxygen  * GiveMeTheOxygenRate(  voronoiDiagram->voronoiCells[i])
#endif
				                                          ;
				if( voronoiDiagram->voronoiCells[i]->oxygen < 0.) voronoiDiagram->voronoiCells[i]->oxygen = 0.;
				voronoiDiagram->voronoiCells[i]->glucose += voronoiDiagram->voronoiCells[i]->dglucose
#if DIFFUSION_REACTION_SEPARAT == TRUE
				                                          - timeStep * voronoiDiagram->voronoiCells[i]->glucose * GiveMeTheGlucoseRate( voronoiDiagram->voronoiCells[i])
#endif
				                                          ;
				if( voronoiDiagram->voronoiCells[i]->glucose < 0.) voronoiDiagram->voronoiCells[i]->glucose = 0.;
			}
		}
	}

	return timeDifference - time;
}

#else*/

double UpdateSystem( VoronoiDiagram *voronoiDiagram, double timeStep, double timeDifference){

	double time;

	//int direction = (int)(myRand()*DIMENSIONS+0.5);
	int direction = ADI_direction; ADI_direction = (ADI_direction+1)%DIMENSIONS;

	for( time = 0; time+timeStep <= timeDifference; time += timeStep){

#if DIFFUSION_METHODE == EXPLICIT

		//fprintf( stderr, "EXPLICIT\n");
		// Finite Differences: explicit method
		for( int i=0; i<voronoiDiagram->countVoronoiCells; i++){

			voronoiDiagram->voronoiCells[i]->doxygen  = timeStep * (Oxygen_Diffusion  * secondDerivationOxygen(  voronoiDiagram->voronoiCells[i], voronoiDiagram) // diffusion
	#if DIFFUSION_REACTION_SEPARAT == FALSE
			                                          - voronoiDiagram->voronoiCells[i]->oxygen  * GiveMeTheOxygenRate(  voronoiDiagram->voronoiCells[i]) // reaction
	#endif
			                                          );
			voronoiDiagram->voronoiCells[i]->dglucose = timeStep * (Glucose_Diffusion * secondDerivationGlucose( voronoiDiagram->voronoiCells[i], voronoiDiagram) // diffusion
	#if DIFFUSION_REACTION_SEPARAT == FALSE
			                                          - voronoiDiagram->voronoiCells[i]->glucose * GiveMeTheGlucoseRate( voronoiDiagram->voronoiCells[i]) // reaction
	#endif
			                                          );
		}

#else

	for( int d=0; d<DIMENSIONS; d++){
	//fprintf(stderr, "dir=%i\n", direction);
		// Finite Differences: alternating direction implicit (ADI)
		double weight = 1.;

		//int i = 0;
		//int ii = 0;
		//int iii = 0;

		for( int iset=0; iset<voronoiDiagram->xN[(d==0?1:0)]/*20*/; iset++){
			for( int iiset=0; iiset<voronoiDiagram->xN[(d<=1?2:1)]/*20*/; iiset++){

				// base index
				int index=0;
				int index_count = 0;
				for( int dd=0; dd<DIMENSIONS; dd++){
					if( dd!=direction){
						if(index_count==0)
							index += (int)( iset*pow( voronoiDiagram->xN[(d==0?1:0)]/*20*/, dd));
						else
							index += (int)( iiset*pow( voronoiDiagram->xN[(d<=1?2:1)]/*20*/, dd));
						index_count++;
					}
				}

				//fprintf(stderr, "setupMatrixADI(o): %i <= (%i,%i)\n", index, iset, iiset);

	#if   DIFFUSION_METHODE == CRANK_NICHOLSON
				setupMatrixCrankNicholson( voronoiDiagram, index, timeStep, 'o', direction);
				//fprintf( stderr, "CRANK_NICHOLSON\n");
	#elif DIFFUSION_METHODE == ADI
				setupMatrixADI( voronoiDiagram, index, timeStep, 'o', direction);
	#elif DIFFUSION_METHODE == IMPLICIT
				//fprintf( stderr, "ADI\n");
				setupMatrix( voronoiDiagram, index, timeStep, 'o', direction);
	#endif
				//setupMatrixADI( voronoiDiagram, index, timeStep, 'o', direction);
				//fprintf(stderr, "solve\n");
				//solveLinearSystemTridiagonalMatrix( A, b, x, voronoiDiagram->xN[d]);
				solveLinearSystem( A, b, x, voronoiDiagram->xN[d]/*20*/);
				for( int i=0; i<voronoiDiagram->xN[d]/*20*/; i++)
	#if DIFFUSION_METHODE == IMPLICIT
					if( d!=0)
						voronoiDiagram->voronoiCells[(int)(index+i*pow( voronoiDiagram->xN[d]/*20*/, direction))]->doxygen += weight*(b[i] - voronoiDiagram->voronoiCells[(int)(index+i*pow( voronoiDiagram->xN[d]/*20*/, direction))]->oxygen);
					else
	#endif
						voronoiDiagram->voronoiCells[(int)(index+i*pow( voronoiDiagram->xN[d]/*20*/, direction))]->doxygen = weight*(b[i] - voronoiDiagram->voronoiCells[(int)(index+i*pow( voronoiDiagram->xN[d]/*20*/, direction))]->oxygen);

				//fprintf(stderr, "setupMatrixADI(g): %i <= (%i,%i)\n", index, iset, iiset);

	#if   DIFFUSION_METHODE == CRANK_NICHOLSON
				setupMatrixCrankNicholson( voronoiDiagram, index, timeStep, 'g', direction);
				//fprintf( stderr, "CRANK_NICHOLSON\n");
	#elif DIFFUSION_METHODE == ADI
				setupMatrixADI( voronoiDiagram, index, timeStep, 'g', direction);
	#elif DIFFUSION_METHODE == IMPLICIT
				//fprintf( stderr, "Implicit\n");
				setupMatrix( voronoiDiagram, index, timeStep, 'g', direction);
	#endif
				//setupMatrixADI( voronoiDiagram, index, timeStep, 'g', direction);
				solveLinearSystem( A, b, x, voronoiDiagram->xN[d]/*20*/);
				//solveLinearSystemTridiagonalMatrix( A, b, x, voronoiDiagram->xN[d]);
				for( int i=0; i<voronoiDiagram->xN[d]/*20*/; i++)
	#if DIFFUSION_METHODE == IMPLICIT
					if( d!=0)
						//add differences
						voronoiDiagram->voronoiCells[(int)(index+i*pow( voronoiDiagram->xN[d]/*20*/, direction))]->dglucose += weight*(b[i] - voronoiDiagram->voronoiCells[(int)(index+i*pow( voronoiDiagram->xN[d]/*20*/, direction))]->glucose);
					else
	#endif
						// set differences
						voronoiDiagram->voronoiCells[(int)(index+i*pow( voronoiDiagram->xN[d]/*20*/, direction))]->dglucose = weight*(b[i] - voronoiDiagram->voronoiCells[(int)(index+i*pow( voronoiDiagram->xN[d]/*20*/, direction))]->glucose);
			}
		}
#endif

#if DIFFUSION_METHODE == IMPLICIT
		if(d+1==DIMENSIONS)
#endif
		// update half time step
		{//fprintf(stderr, "update half time step\n");
		for( int i=0; i<voronoiDiagram->countVoronoiCells; i++){
			if( voronoiDiagram->voronoiCells[i]->position[0]>voronoiDiagram->xMin[0]+1
			 && voronoiDiagram->voronoiCells[i]->position[0]<voronoiDiagram->xMax[0]-1
			 && voronoiDiagram->voronoiCells[i]->position[1]>voronoiDiagram->xMin[1]+1
			 && voronoiDiagram->voronoiCells[i]->position[1]<voronoiDiagram->xMax[1]-1
		#if DIMENSIONS > 2
			 && voronoiDiagram->voronoiCells[i]->position[2]>voronoiDiagram->xMin[2]+1
			 && voronoiDiagram->voronoiCells[i]->position[2]<voronoiDiagram->xMax[2]-1
		#endif
			){
				voronoiDiagram->voronoiCells[i]->oxygen  += voronoiDiagram->voronoiCells[i]->doxygen;
				if( voronoiDiagram->voronoiCells[i]->oxygen < 0.) voronoiDiagram->voronoiCells[i]->oxygen = 0.;
				voronoiDiagram->voronoiCells[i]->glucose += voronoiDiagram->voronoiCells[i]->dglucose;
				if( voronoiDiagram->voronoiCells[i]->glucose < 0.) voronoiDiagram->voronoiCells[i]->glucose = 0.;
			}
		}}

		// change direction
		direction = (direction+1)%DIMENSIONS;
	}

#if DIFFUSION_REACTION_SEPARAT == TRUE
	// reaction
	for( int i=0; i<voronoiDiagram->countVoronoiCells; i++){
		if( voronoiDiagram->voronoiCells[i]->position[0]>voronoiDiagram->xMin[0]+1
		 && voronoiDiagram->voronoiCells[i]->position[0]<voronoiDiagram->xMax[0]-1
		 && voronoiDiagram->voronoiCells[i]->position[1]>voronoiDiagram->xMin[1]+1
		 && voronoiDiagram->voronoiCells[i]->position[1]<voronoiDiagram->xMax[1]-1
	#if DIMENSIONS > 2
		 && voronoiDiagram->voronoiCells[i]->position[2]>voronoiDiagram->xMin[2]+1
		 && voronoiDiagram->voronoiCells[i]->position[2]<voronoiDiagram->xMax[2]-1
	#endif
		){
			voronoiDiagram->voronoiCells[i]->oxygen  += - timeStep * voronoiDiagram->voronoiCells[i]->oxygen  * GiveMeTheOxygenRate(  voronoiDiagram->voronoiCells[i]);
			if( voronoiDiagram->voronoiCells[i]->oxygen < 0.) voronoiDiagram->voronoiCells[i]->oxygen = 0.;
			voronoiDiagram->voronoiCells[i]->glucose += - timeStep * voronoiDiagram->voronoiCells[i]->glucose * GiveMeTheGlucoseRate( voronoiDiagram->voronoiCells[i]);
			if( voronoiDiagram->voronoiCells[i]->glucose < 0.) voronoiDiagram->voronoiCells[i]->glucose = 0.;
		}
	}
#endif
#if DIFFUSION_METHODE != EXPLICIT
	}
#endif
	return timeDifference - time;
}

//#endif

//////////////////////////////////////////////////////////////////////////////////////

void initMatrixImplicit( VoronoiDiagram *voronoiDiagram)
{
	int matrixDim = 1;

	for( int d=0; d<DIMENSIONS; d++)
		matrixDim *= voronoiDiagram->xN[d];

	b = (float*) malloc( matrixDim * sizeof(float));

	x = (float*) malloc( matrixDim * sizeof(float));

	A = newMatrix( matrixDim, matrixDim);
}

void setupMatrixImplicit( VoronoiDiagram *voronoiDiagram, double timeStep, char molecule)
{
	int di   = 1;
	int dii  = voronoiDiagram->xN[0];
	int diii = voronoiDiagram->xN[0]*voronoiDiagram->xN[1];

	double r = Glucose_Diffusion * timeStep/(DX*DX);

	int N = voronoiDiagram->xN[0]*voronoiDiagram->xN[1]*voronoiDiagram->xN[2];

	for( int iii=0; iii<voronoiDiagram->xN[2]; iii++)
	for( int ii=0; ii<voronoiDiagram->xN[1]; ii++)
	for( int i=0; i<voronoiDiagram->xN[0]; i++)
	{
		// actual element
		int m = i*di + ii*dii + iii*diii;
		//fprintf( stderr, "%i ", m);

		// init matrix row
		for( int n=0; n<N; n++)
			A[m][n] = 0.;

		//matrix
		A[m][m] = 1;

		// x
		/*if( i>0){
			A[m][m] = r;
			A[m][m-di] = -r;
		}
		if( i<voronoiDiagram->xN[0]-1){
			A[m][m] = r;
			A[m][m+di] = -r;
		}

		// y
		if( ii>0){
			A[m][m] = r;
			A[m][m-dii] = -r;
		}
		if( ii<voronoiDiagram->xN[1]-1){
			A[m][m] = r;
			A[m][m+dii] = -r;
		}

		// z
		if( iii>0){
			A[m][m] = r;
			A[m][m-diii] = -r;
		}
		if( iii<voronoiDiagram->xN[2]-1){
			A[m][m] = r;
			A[m][m+diii] = -r;
		}*/
		if( i>0 && i<voronoiDiagram->xN[0]-1 && ii>0 && ii<voronoiDiagram->xN[1]-1 && iii>0 && iii<voronoiDiagram->xN[2]-1)
		{
			A[m][m] += 6*r;
			A[m][m-di] = -r;
			A[m][m+di] = -r;
			A[m][m-dii] = -r;
			A[m][m+dii] = -r;
			A[m][m-diii] = -r;
			A[m][m+diii] = -r;
		}

		// vector
		b[m] = voronoiDiagram->voronoiCells[m]->glucose * ( 1. - timeStep * GiveMeTheGlucoseRate(  voronoiDiagram->voronoiCells[m]));

	}
}

double UpdateSystemImplicit( VoronoiDiagram *voronoiDiagram, double timeStep, double timeDifference)
{

	double time;

	int N = voronoiDiagram->xN[0]*voronoiDiagram->xN[1]*voronoiDiagram->xN[2];
	//float **B = newMatrix( N, N);

	for( time = 0; time+timeStep <= timeDifference; time += timeStep){
		//fprintf( stderr, "%i. iteration:\nSetup Matrix\n", (int)(time / timeStep + 0.5));
		int passedTime = clock();
		fprintf( stderr, "Set Matrix... \n");
		setupMatrixImplicit( voronoiDiagram, timeStep, 'G');
		fprintf( stderr, "...finished ( %li clocks, %.3lf sec)\n", (clock() - passedTime), (float)(clock() - passedTime)/CLOCKS_PER_SEC);

		/*for(int m=0; m<N; m++){
			for(int n=0; n<N; n++)
				fprintf( stderr, "%6.0lf ", A[m][n]);
			fprintf( stderr, "\n");
		}*/
		/*for(int m=0; m<N; m++){
			for(int n=0; n<N; n++)
				fprintf( stdout, "%i %i %lf \n", m, n, (A[m][n]!=0.?1.:0.));
			//fprintf( stderr, "\n");
		}*/

		/*fprintf( stderr, "Solve Matrix\n");
		solveLinearSystemB( A, b, x, N, B);

		fprintf( stderr, "Actualize Values\n");
		for(int m=0; m<N; m++)
			voronoiDiagram->voronoiCells[m]->glucose = b[m];
		*/
		for(int m=0; m<N; m++)
			x[m] = voronoiDiagram->voronoiCells[m]->glucose;

		passedTime = clock();
		fprintf( stderr, "ConjugateGradient... \n");
		ConjugateGradient( A, b, x, N, 1);
		fprintf( stderr, "...finished ( %li clocks, %.3lf sec)\n", (clock() - passedTime), (float)(clock() - passedTime)/CLOCKS_PER_SEC);
		//fprintf( stderr, "...finished ( %lisec)\n", (clock() - passedTime)/CLOCKS_PER_SEC);

		for(int m=0; m<N; m++)
			voronoiDiagram->voronoiCells[m]->glucose = x[m];
	}

	return timeDifference - time;

}

void ConjugateGradient_old( float **A, float *b, float *x, int N, int iterations)
{
	float r[N];
	float r2[N];

	float p[N];
	float q[N];

	//float beta[N];
	//float alpha[N];
	float beta;
	float alpha;

	float temp[N];

	// initialization of residual vector: r_0
	matrixVectorProduct( A, x, temp, N);
	vectorDifference( b, temp, r, N);

	// initialization of p_0
	vectorCopy( r, p, N);

	for( int i=0; i<iterations; i++){
		// q_k
		matrixVectorProduct( A, p, q, N);

		alpha = dotProduct( r, r, N);

		if(i>0){
			beta = alpha / dotProduct( r2, r2, N);

			vectorScale( p, beta, p, N);
			vectorSum( r, p, p, N);
		}

		alpha /= dotProduct( p, q, N);

		// next p
		vectorScale( p, alpha, temp, N);
		vectorSum( p, temp, p, N);

		// next x
		vectorScale( x, alpha, temp, N);
		vectorSum( x, temp, x, N);
	}
}

void ConjugateGradient3( float **A, float *b, float *x, int N, int iterations)
{
	float r[N];
	float r2[N];

	float p[N];
	float q[N];

	//float beta[N];
	//float alpha[N];
	float beta;
	float alpha;

	float temp[N];

	// initialization of residual vector: r_0
	matrixVectorProduct( A, x, temp, N);
	vectorDifference( b, temp, r, N);

	// initialization of p_0
	vectorCopy( r, p, N);

	for( int i=0; i<iterations; i++){
		// q_k
		matrixVectorProduct( A, p, q, N);

		alpha = dotProduct( r, r, N);

		if(i>0){
			beta = alpha / dotProduct( r2, r2, N);

			vectorScale( p, beta, p, N);
			vectorSum( r, p, p, N);
		}

		alpha /= dotProduct( p, q, N);

		// next x
		vectorScale( p, alpha, temp, N);
		vectorSum( x, temp, x, N);

		// old r
		vectorCopy( r, r2, N);

		// next r
		vectorScale( q, -alpha, temp, N);
		vectorSum( r, temp, r, N);

		//float *pointer ;
	}
}
void ConjugateGradient( float **A, float *b, float *x, int N, int iterations)
{
	// vectors
	float r[N];
	float w[N];
	float z[N];

	// scalars
	float alpha;
	float beta;

	float temp[N];

	// initialization of residual vector: r
	matrixVectorProduct( A, x, temp, N);
	vectorDifference( b, temp, r, N);

	// w
	vectorScale( r, -1, w, N);

	// z
	matrixVectorProduct( A, w, z, N);

	// alpha
	alpha = dotProduct( r, w, N) / dotProduct( w, z, N);

	// beta
	beta = 0.;

	// x
	vectorScale( w, alpha, temp, N);
	vectorSum( x, temp, x, N);

	for( int i=0; i<iterations; i++){
		vectorScale( z, alpha, temp, N);
		vectorDifference( r, temp, r, N);

		if( sqrt( dotProduct( r, r, N)) < 1e-10)
			return;

		// B = (r'*z)/(w'*z);
		beta = dotProduct( r, z, N) / dotProduct( w, z, N);

		// w = -r + B*w;
		vectorScale( w, beta, w, N);
		vectorDifference( w, r, w, N);

		// z = A*w;
		matrixVectorProduct( A, w, z, N);

		// a = (r'*w)/(w'*z);
		alpha = dotProduct( r, w, N) / dotProduct( w, z, N);

		// x = x + a*w;
		vectorScale( w, alpha, temp, N);
		vectorSum( x, temp, x, N);
	}
}


#include "PPXTF.hpp"

#include "common.hpp"
#include <cstdio>
#include <string>
#include <map>
#include <fstream>
#include <utility>
#include <vector>
#include <algorithm>
#include <random>
#include <tuple>
#include <utility>
#include <cassert>
#include <iostream>
#include <cstdlib>

// eigen
#include <Eigen/Core>

// stats
#define STATS_DONT_USE_OPENMP
#define STATS_ENABLE_EIGEN_WRAPPERS
//#define STATS_ENABLE_STDVEC_WRAPPERS
//#define STATS_ENABLE_INTERNAL_VEC_FEATURES
#include <stats.hpp>

using namespace Eigen;
using namespace std;

constexpr int BUF_SIZE = 1000;

using p_t = pair<int, int>;
using r_t = vector<map<p_t, double> >;
using mat_t = MatrixXd;
using vec_mat_t = vector<mat_t>;

std::mt19937_64 engine;

const map<int, double> Count2Rating = {
  {1,1.0},
  {2,2.0},
  {3,3.0},
  {4,4.0},
  {5,5.0},
  {6,6.0},
  {7,7.0},
  {8,8.0},
  {9,9.0},
  {10,10.0}
};

vector<int> seq(int n){
  vector<int> v(n);
  for(int i = 0; i < n; ++i){v[i] = i;}
  return v;
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// random

vector<int> random_index(int n){
  vector<int> res = seq(n);
  shuffle(res.begin(), res.end(), engine);
  return res;
}

// m \times n matrix
// numbers are uniformly random and in the [0,1] range
mat_t random_mat(int m, int n){
  return 0.5 * (mat_t::Random(m, n).array() + 1);
}

// Cholesky decomposition
inline mat_t chol(const mat_t& A){
  return A.llt().matrixL();
}

// https://en.wikipedia.org/wiki/Multivariate_normal_distribution#Drawing_values_from_the_distribution
// https://qiita.com/jhako/items/30f420033b5c126eedc1
// mu[K,1]
// Sigma[K,K]
// ret[K,1]
mat_t multivariate_normal(const mat_t& mu, const mat_t& Sigma){
  int K = Sigma.rows();
  assert(mu.rows() == K);
  const mat_t L = chol(Sigma); // L * L' = Sigma
  mat_t r = mat_t::Zero(K, 1);
  for(int i = 0; i < K; ++i){r(i, 0) = stats::rnorm(0, 1, engine);}
  return mu + L * r;
}

// https://www.math.wustl.edu/~sawyer/hmhandouts/Wishart.pdf
// scale[K, K]
// res[K, K]
mat_t wishart_rvs(int df, const mat_t& scale){
  int K = scale.rows();
  mat_t A = chol(scale); // A * A' = scale
  mat_t T = mat_t::Zero(K, K); // T * T' = B; B ~ W(I_d, d, n)
  for(int i = 1; i < K; ++i){ // T_{i,j} ~ N(0,1) for j < i
    for(int j = 0; j < i; ++j){
      T(i, j) = stats::rnorm(0 ,1, engine);
    }
  }
  for(int i = 0; i < K; ++i){ // T_{i,i} ~ \chi^2(df-i)
    T(i, i) = std::sqrt(stats::rchisq(df - i, engine));
  }
  mat_t AT = A * T;
  return AT * AT.transpose(); // A * B * A' ~ W(scale, d, n); AT * AT' = A * T * T' * A' = A * B * A'
}

void seeding(unsigned seed){
  //std::random_device seed_gen;
  //engine = std::mt19937_64(seed_gen());
  engine = std::mt19937_64(seed);
  srand(seed); // for eigen
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

double count2rating(int count){
  auto it = Count2Rating.lower_bound(count);
  if(it == Count2Rating.end()){
    return Count2Rating.rbegin()->second;
  }else{
    return it->second;
  }
}

inline void write_to_tensor(r_t& r1, r_t& r2, r_t& r3,
			    int i, int j, int k,
			    double value){
  r1[i][p_t(j,k)] = value;
  r2[j][p_t(i,k)] = value;
  r3[k][p_t(i,j)] = value;
}

void read_tensor(r_t& r1, r_t& r2, r_t& r3, const string& infile){
  ifstream ifs(infile);
  string line;
  getline(ifs, line); // header
  while(getline(ifs, line)){
    auto row = split(line);
    assert(row.size() >= 4);
    int i = atoi(row[0].c_str());
    int j = atoi(row[1].c_str());
    int k = atoi(row[2].c_str());
    double value = count2rating(atoi(row[3].c_str()));
    write_to_tensor(r1, r2, r3, i, j, k, value);
  }
}

/*
  ###################### Read a training transition tensor ######################
  # [input1]: N -- Number of users
  # [input2]: M -- Number of POIs
  # [output1]: RT1_observed ([user_index]{poi_index_from, poi_index_to: value})
  # [output2]: RT2_observed ([poi_index_from]{user_index, poi_index_to: value})
  # [output3]: RT3_observed ([poi_index_to]{user_index, poi_index_from: value})
 */
tuple<r_t, r_t, r_t> ReadTrainTransTensor(int N, int M, int ZeroNum, const string& infile){
  r_t RT1_observed(N), RT2_observed(M), RT3_observed(M);
  // Read a training tensor --> (RT1,RT2,RT3)_observed
  read_tensor(RT1_observed, RT2_observed, RT3_observed, infile);
  // Randomly assign ZeroNum zero-values --> (RT1,RT2,RT3)_observed
  if(ZeroNum > 0){
    int MM = M * M;
    for(int user_index = 0; user_index < N; ++user_index){
      auto rand_index = random_index(MM);
      int zero_num = 0;
      for(int i = 0; i < MM; ++i){
	int poi_index_from = rand_index[i] % M;
	int poi_index_to = rand_index[i] / M;
	auto& a = RT1_observed[user_index];
	if(a.find(p_t(poi_index_from,poi_index_to)) == a.end()){
	  write_to_tensor(RT1_observed, RT2_observed, RT3_observed, user_index, poi_index_from, poi_index_to, 0);
	  zero_num += 1;
	  if(zero_num == ZeroNum){break;}
	}
      }
    }
  }else if(ZeroNum == -1){
    for(int user_index = 0; user_index < N; ++user_index){
      for(int poi_index_from = 0; poi_index_from < M; ++poi_index_from){
	for(int poi_index_to = 0; poi_index_to < M; ++poi_index_to){
	  auto& a = RT1_observed[user_index];
	  auto p = p_t(poi_index_from,poi_index_to);
	  if(a.find(p) == a.end()){
	    write_to_tensor(RT1_observed, RT2_observed, RT3_observed, user_index, poi_index_from, poi_index_to, 0);
	  }
	}
      }
    }
  }
  return forward_as_tuple(RT1_observed, RT2_observed, RT3_observed);
}
/*
  ######################## Read a training visit tensor #########################
  # [input1]: N -- Number of users
  # [input2]: M -- Number of POIs
  # [input3]: T (Number of time slots)
  # [output1]: RV1_observed ([user_index]{poi_index_from, time_slot: value})
  # [output2]: RV2_observed ([poi_index_from]{user_index, time_slot: value})
  # [output3]: RV3_observed ([time_slot]{user_index, poi_index_from: value})
 */
tuple<r_t, r_t, r_t> ReadTrainVisitTensor(int N, int M, int T, int ZeroNum, const string& infile){
  r_t RV1_observed(N), RV2_observed(M), RV3_observed(T);
  // Read a training tensor --> (RV1,RV2,RV3)_observed
  read_tensor(RV1_observed, RV2_observed, RV3_observed, infile);
  // Randomly assign ZeroNum zero-values --> (RV1,RV2,RV3)_observed
  if(ZeroNum > 0){
    int MT = M * T;
    for(int user_index = 0; user_index < N; ++user_index){
      auto rand_index = random_index(MT);
      int zero_num = 0;
      for(int i = 0; i < MT; ++i){
	int poi_index_from = rand_index[i] % M;
	int time_slot = rand_index[i] / M;
	auto& a = RV1_observed[user_index];
	if(a.find(p_t(poi_index_from,time_slot)) == a.end()){
	  write_to_tensor(RV1_observed, RV2_observed, RV3_observed, user_index, poi_index_from, time_slot, 0);
	  zero_num += 1;
	  if(zero_num == ZeroNum){break;}
	}
      }
    }
  }else if(ZeroNum == -1){
    for(int user_index = 0; user_index < N; ++user_index){
      for(int poi_index_from = 0; poi_index_from < M; ++poi_index_from){
	for(int time_slot = 0; time_slot < T; ++time_slot){
	  auto& a = RV1_observed[user_index];
	  auto p = p_t(poi_index_from,time_slot);
	  if(a.find(p) == a.end()){
	    write_to_tensor(RV1_observed, RV2_observed, RV3_observed, user_index, poi_index_from, time_slot, 0);
	  }
	}
      }
    }
  }
  return forward_as_tuple(RV1_observed, RV2_observed, RV3_observed);
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// io

void puts_mat(FILE* fout, const mat_t& A){
  int m = A.rows();
  int n = A.cols();
  assert(n > 0);
  for(int i = 0; i < m; ++i){
    fprintf(fout, "%lf", A(i,0));
    for(int j = 1; j < n; ++j){
      fprintf(fout, ",%lf", A(i,j));
    }
    fprintf(fout, "\n");
  }
}

void savetxt(const string& outfile, const mat_t& A){
  FILE* fp = fopen(outfile.c_str(), "w");
  puts_mat(fp, A);
  fclose(fp);
}

void save_parameters(int K, int ItrNum, const string& prefix, const string& name, const vec_mat_t& A, const mat_t& mu_A, const mat_t& Lam_A, const string& X){
  char buf[BUF_SIZE];
  sprintf(buf, "%s_Itr%d_%s%s.csv", prefix.c_str(), ItrNum, name.c_str(), X.c_str());
  savetxt(buf, A[ItrNum]);
  sprintf(buf, "%s_Itr%d_mu_%s%s.csv", prefix.c_str(), ItrNum, name.c_str(), X.c_str());
  savetxt(buf, mu_A);
  sprintf(buf, "%s_Itr%d_Lam_%s%s.csv", prefix.c_str(), ItrNum, name.c_str(), X.c_str());
  savetxt(buf, Lam_A);
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// ppmft

tuple<mat_t, mat_t> sample_Lam_X_mu_X(const mat_t& X, int N, int nu0, const mat_t& W0, double beta0, double mu0, int K){
  // Sample Lam_X
  // S_bar = np.sum([np.outer(A[itr, n, :], A[itr, n, :]) for n in range(N)], axis=0) / N
  mat_t S_bar = X.transpose() * X / N; // [K, K]
  // a_bar = np.sum(A[itr], axis=0) / N
  mat_t x_bar = X.colwise().sum() / N; // [1, K]
  // nu_ast = nu0 + n
  int nu_ast = nu0 + N; // 1
  // W0_ast = inv(inv(W0) + N * S_bar + (beta0 * N / (beta0 + N)) * np.outer(mu0 - a_bar, mu0 - a_bar))
  mat_t t = mat_t::Constant(1, K, mu0) - x_bar; // [1, K]
  mat_t W0_ast = (W0.inverse() + N * S_bar + (beta0 * N / (beta0 + N)) * (t.transpose() * t)).inverse(); // [K, K]
  // Lam_A = wishart.rvs(df=nu_ast, scale=W0_ast)
  //mat_t Lam_X = stats::rwish(W0_ast, nu_ast); // [K, K]
  mat_t Lam_X = wishart_rvs(nu_ast, W0_ast); // [K, K]

  // Sample mu_X
  // mu0_ast = (beta0 * mu0 + N * a_bar) / (beta0 + N)
  mat_t mu0_ast = ((mat_t::Constant(1, K, beta0 * mu0) + N * x_bar) / (beta0 + N)).transpose(); // [K, 1]
  // mu_A = multivariate_normal(mu0_ast, inv((beta0 + N) * Lam_A))
  mat_t mu_X = multivariate_normal(mu0_ast, ((beta0 + N) * Lam_X).inverse()); // [K, 1]
  return forward_as_tuple(Lam_X, mu_X);
}

tuple<mat_t, mat_t> calc_XYXY_XYR(int K, const mat_t& B, const mat_t& C, const map<p_t, double>& rt1){
  mat_t BC_BC = mat_t::Zero(K, K);
  mat_t BC_R = mat_t::Zero(1, K);
  for(auto kv : rt1){
    int i = kv.first.first, j = kv.first.second;
    double value = kv.second;
    mat_t bc_ij = B.row(i).array() * C.row(j).array(); // hadamard product
    mat_t bc_bc = bc_ij.transpose() * bc_ij; // [K,K]
    mat_t bc_r = bc_ij * value; // [1,K]
    BC_BC = BC_BC + bc_bc;
    BC_R = BC_R + bc_r;
  }
  return forward_as_tuple(BC_BC, BC_R.transpose());
}


/*
 * Copyright (c) 2013-2017 Masahide Kashiwagi (kashi@waseda.jp)
 */

#ifndef JOINTRANGE_HPP
#define JOINTRANGE_HPP

#include <algorithm>
#include <boost/numeric/ublas/vector.hpp>
#include <kv/affine.hpp>
#include <kv/matplotlib.hpp>

namespace kv {

namespace ub = boost::numeric::ublas;

template<class T>
void jointrange(const affine<T>& x, const affine<T>& y, const matplotlib& g, const char *color = "blue")
{
	int n, i, j, s;
	T tmp;
	T dx, dy;
	ub::vector<T> vx, vy;
	ub::matrix<int> signcache;

	n = std::max(x.a.size(), y.a.size()) - 1;

#if AFFINE_SIMPLE >= 1
	vx.resize(n+3);
	vy.resize(n+3);
#else
	vx.resize(n+1);
	vy.resize(n+1);
#endif

	for (i=0; i<x.a.size(); i++) {
		vx(i) = x.a(i);
	}
	for (i=x.a.size(); i<=n; i++) {
		vx(i) = 0.;
	}
	for (i=0; i<y.a.size(); i++) {
		vy(i) = y.a(i);
	}
	for (i=y.a.size(); i<=n; i++) {
		vy(i) = 0.;
	}

#if AFFINE_SIMPLE >= 1
	vx(n+1) = x.er;
	vy(n+1) = 0.;
	vx(n+2) = 0.;
	vy(n+2) = y.er;
	n += 2;
#endif

	signcache.resize(n+1, n+1);
	for (i=1; i<=n; i++) {
		for (j=i+1; j<=n; j++) {
			tmp = vy(i) * vx(j) - vx(i) * vy(j);
			if (tmp >= 0.) s = 1;
			else s = -1;
			signcache(i, j) = s;
			signcache(j, i) = -s;
		}
	}

	for (i=1; i<=n; i++) {
		dx = 0.;
		dy = 0.;
		for (j=1; j<=n; j++) {
			if (j == i) continue;
			// tmp = vy(i) * vx(j) - vx(i) * vy(j);
			// if (tmp >= 0.) s = 1;
			// else s = -1;
			s = signcache(i, j);
			dx += s * vx(j);
			dy += s * vy(j);
		}
		g.line(vx(0)+dx+vx(i), vy(0)+dy+vy(i),
		       vx(0)+dx-vx(i), vy(0)+dy-vy(i), color);
		g.line(vx(0)-dx+vx(i), vy(0)-dy+vy(i),
		       vx(0)-dx-vx(i), vy(0)-dy-vy(i), color);
	}
}

} // namespace kv

#endif // JOINTRANGE_HPP


#include "StdAfx.h"
#include "OptApp.h"
#include <boost/filesystem.hpp>
#include <g2o/types/slam3d/edge_se3.h>
#include <g2o/core/block_solver.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/solvers/pcg/linear_solver_pcg.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include "vertigo/vertex_switchLinear.h"
#include "vertigo/edge_switchPrior.h"
#include "vertigo/edge_se3Switchable.h"

typedef g2o::BlockSolver< g2o::BlockSolverTraits<6, 3> >  SlamBlockSolver;
typedef g2o::LinearSolverCSparse<SlamBlockSolver::PoseMatrixType> SlamLinearCSparseSolver;
typedef g2o::LinearSolverPCG<SlamBlockSolver::PoseMatrixType> SlamLinearPCGSolver;
typedef std::tr1::unordered_map<int, g2o::HyperGraph::Vertex*>     VertexIDMap;
typedef std::pair<int, g2o::HyperGraph::Vertex*> VertexIDPair;
typedef std::set<g2o::HyperGraph::Edge*> EdgeSet;

COptApp::COptApp(void)
{
}

COptApp::~COptApp(void)
{
}

bool COptApp::Init()
{
	namespace fs = boost::filesystem;
	if ( fs::exists( fs::path( odometry_log_file_ ) ) ) {
		odometry_traj_.LoadFromFile( odometry_log_file_ );
		if ( fs::exists( fs::path( odometry_info_file_ ) ) ) {
			odometry_info_.LoadFromFile( odometry_info_file_ );
		}
	}
	if ( fs::exists( fs::path( loop_log_file_ ) ) ) {
		loop_traj_.LoadFromFile( loop_log_file_ );
		if ( fs::exists( fs::path( loop_info_file_ ) ) ) {
			loop_info_.LoadFromFile( loop_info_file_ );
		}
	}
	pose_traj_.data_.clear();
	pose_traj_.data_.push_back( FramedTransformation( 0, 0, 1, Eigen::Matrix4d::Identity() ) );
	for ( int i = 0; i < ( int )odometry_traj_.data_.size(); i++ ) {
		pose_traj_.data_.push_back( FramedTransformation( i + 1, i + 1, i + 2, pose_traj_.data_[ i ].transformation_ * odometry_traj_.data_[ i ].transformation_ ) );
	}
	return ( odometry_traj_.data_.size() > 0 );
}

void COptApp::OptimizeSwitchable()
{
	struct SwitchableEdge {
	public:
		VertexSwitchLinear * v_;
		EdgeSwitchPrior * ep_;
		EdgeSE3Switchable * e_;
		FramedTransformation * t_;
	};

    g2o::SparseOptimizer* optimizer;
	optimizer = new g2o::SparseOptimizer();
	optimizer->setVerbose(true);
	SlamBlockSolver * solver = NULL;
	SlamLinearCSparseSolver* linearSolver = new SlamLinearCSparseSolver();
	linearSolver->setBlockOrdering(false);
	solver = new SlamBlockSolver(linearSolver);
	g2o::OptimizationAlgorithmLevenberg* algo = new g2o::OptimizationAlgorithmLevenberg(solver);
	optimizer->setAlgorithm(algo);

	std::vector< SwitchableEdge > switch_edge;

	Eigen::Matrix< double, 6, 6 > default_information;
	default_information = Eigen::Matrix< double, 6, 6 >::Identity();

	for ( int i = 0; i < ( int )pose_traj_.data_.size(); i++ ) {
		g2o::VertexSE3 * v = new g2o::VertexSE3();
		v->setId( i );
		v->setEstimate( Eigen2G2O( pose_traj_.data_[ i ].transformation_ ) );
		if ( i == 0 ) {
			v->setFixed( true );
		}
		optimizer->addVertex( v );

		if ( i > 0 ) {
			g2o::EdgeSE3* g2o_edge = new g2o::EdgeSE3();
			g2o_edge->vertices()[0] = dynamic_cast<g2o::VertexSE3*>(optimizer->vertex( i - 1 ));
			g2o_edge->vertices()[1] = dynamic_cast<g2o::VertexSE3*>(optimizer->vertex( i ));
			g2o_edge->setMeasurement( g2o::internal::fromSE3Quat( Eigen2G2O( odometry_traj_.data_[ i - 1 ].transformation_ ) ) );
			if ( odometry_info_.data_.size() > 0 ) {
				g2o_edge->setInformation( odometry_info_.data_[ i - 1 ].information_ );
			} else {
				g2o_edge->setInformation( default_information );
			}
			optimizer->addEdge( g2o_edge );
		}
	}

	for ( int i = 0; i < ( int )loop_traj_.data_.size(); i++ ) {
		FramedTransformation & t = loop_traj_.data_[ i ];

		SwitchableEdge edge;
		edge.t_ = &t;

		edge.v_ = new VertexSwitchLinear();
		edge.v_->setId( optimizer->vertices().size() );
		edge.v_->setEstimate( 1.0 );
		optimizer->addVertex( edge.v_ );

		edge.ep_ = new EdgeSwitchPrior();
		edge.ep_->vertices()[0] = edge.v_;
		edge.ep_->setMeasurement( 1.0 );
		edge.ep_->setInformation( Eigen::Matrix<double,1,1>::Identity() * weight_ );
		optimizer->addEdge( edge.ep_ );

		edge.e_ = new EdgeSE3Switchable();
		edge.e_->vertices()[0] = dynamic_cast<g2o::VertexSE3*>(optimizer->vertex( t.id1_ ));
		edge.e_->vertices()[1] = dynamic_cast<g2o::VertexSE3*>(optimizer->vertex( t.id2_ ));
		edge.e_->vertices()[2] = edge.v_;
		edge.e_->setMeasurement( g2o::internal::fromSE3Quat( Eigen2G2O( t.transformation_ ) ) );
		if ( loop_info_.data_.size() > 0 ) {
			edge.e_->setInformation( loop_info_.data_[ i ].information_ );
		} else {
			edge.e_->setInformation( default_information );
		}
		optimizer->addEdge( edge.e_ );
		switch_edge.push_back( edge );
	}

	optimizer->initializeOptimization();
	optimizer->optimize( max_iteration_ );

	for ( int i = 0; i < ( int )pose_traj_.data_.size(); i++ ) {
		g2o::VertexSE3 * v = dynamic_cast< g2o::VertexSE3 * >( optimizer->vertex( i ) );
		pose_traj_.data_[ i ].transformation_ = G2O2Matrix4d( v->estimateAsSE3Quat() );
	}
	pose_traj_.SaveToFile( pose_log_file_ );

	loop_remain_traj_.data_.clear();
	for ( int i = 0; i < ( int )switch_edge.size(); i++ ) {
		SwitchableEdge & edge = switch_edge[ i ];
		if ( edge.v_->estimate() > 0.5 ) {
			loop_remain_traj_.data_.push_back( loop_traj_.data_[ i ] );
		}
	}
	loop_remain_traj_.SaveToFile( loop_remain_log_file_ );

	refine_traj_.data_.clear();
	for ( int i = 0; i < ( int )odometry_traj_.data_.size(); i++ ) {
		refine_traj_.data_.push_back( odometry_traj_.data_[ i ] );
	}
	for ( int i = 0; i < ( int )switch_edge.size(); i++ ) {
		SwitchableEdge & edge = switch_edge[ i ];
		if ( edge.v_->estimate() > 0.5 && loop_traj_.data_[ i ].id1_ + 1 < loop_traj_.data_[ i ].id2_ ) {
			refine_traj_.data_.push_back( loop_traj_.data_[ i ] );
		}
	}
	refine_traj_.SaveToFile( refine_log_file_ );
}

void COptApp::OptimizeEM()
{
	struct SwitchableEdge {
	public:
		double weight_;
		g2o::EdgeSE3 * e_;
		FramedTransformation * t_;
	};

	g2o::SparseOptimizer* optimizer;
	optimizer = new g2o::SparseOptimizer();
	optimizer->setVerbose(true);
	SlamBlockSolver * solver = NULL;
	SlamLinearCSparseSolver* linearSolver = new SlamLinearCSparseSolver();
	linearSolver->setBlockOrdering(false);
	solver = new SlamBlockSolver(linearSolver);
	g2o::OptimizationAlgorithmLevenberg* algo = new g2o::OptimizationAlgorithmLevenberg(solver);
	optimizer->setAlgorithm(algo);

	std::vector< SwitchableEdge > switch_edge;

	Eigen::Matrix< double, 6, 6 > default_information;
	default_information = Eigen::Matrix< double, 6, 6 >::Identity();

	for ( int i = 0; i < ( int )pose_traj_.data_.size(); i++ ) {
		g2o::VertexSE3 * v = new g2o::VertexSE3();
		v->setId( i );
		v->setEstimate( Eigen2G2O( pose_traj_.data_[ i ].transformation_ ) );
		if ( i == 0 ) {
			v->setFixed( true );
		}
		optimizer->addVertex( v );

		if ( i > 0 ) {
			g2o::EdgeSE3* g2o_edge = new g2o::EdgeSE3();
			g2o_edge->vertices()[0] = dynamic_cast<g2o::VertexSE3*>(optimizer->vertex( i - 1 ));
			g2o_edge->vertices()[1] = dynamic_cast<g2o::VertexSE3*>(optimizer->vertex( i ));
			g2o_edge->setMeasurement( g2o::internal::fromSE3Quat( Eigen2G2O( odometry_traj_.data_[ i - 1 ].transformation_ ) ) );
			if ( odometry_info_.data_.size() > 0 ) {
				g2o_edge->setInformation( odometry_info_.data_[ i - 1 ].information_ );
			} else {
				g2o_edge->setInformation( default_information );
			}
			optimizer->addEdge( g2o_edge );
		}
	}

	for ( int i = 0; i < ( int )loop_traj_.data_.size(); i++ ) {
		FramedTransformation & t = loop_traj_.data_[ i ];
		SwitchableEdge edge;
		edge.t_ = &t;
		edge.weight_ = 0.0;

		edge.e_ = new g2o::EdgeSE3();
		edge.e_->vertices()[0] = dynamic_cast<g2o::VertexSE3*>(optimizer->vertex( t.id1_ ));
		edge.e_->vertices()[1] = dynamic_cast<g2o::VertexSE3*>(optimizer->vertex( t.id2_ ));
		edge.e_->setMeasurement( g2o::internal::fromSE3Quat( Eigen2G2O( t.transformation_ ) ) );
		if ( loop_info_.data_.size() > 0 ) {
			edge.e_->setInformation( loop_info_.data_[ i ].information_ );
		} else {
			edge.e_->setInformation( default_information );
		}
		optimizer->addEdge( edge.e_ );
		switch_edge.push_back( edge );
	}

	for ( int itr = 0; itr < max_iteration_; itr++ ) {
		// E step
		for ( int i = 0; i < ( int )switch_edge.size(); i++ ) {
			SwitchableEdge & edge = switch_edge[ i ];
			if ( loop_info_.data_.size() > 0 ) {
				edge.e_->setInformation( loop_info_.data_[ i ].information_ );
			} else {
				edge.e_->setInformation( default_information );
			}
			edge.e_->computeError();
			edge.weight_ = ( weight_ * weight_ ) / ( weight_ * weight_ + switch_edge[ i ].e_->chi2() );

			if ( loop_info_.data_.size() > 0 ) {
				edge.e_->setInformation( loop_info_.data_[ i ].information_ * sqrt( edge.weight_ ) );
			} else {
				edge.e_->setInformation( default_information * sqrt( edge.weight_ ) );
			}
		}

		// M step
		optimizer->initializeOptimization();
		optimizer->optimize( 1 );
	}

	for ( int i = 0; i < ( int )pose_traj_.data_.size(); i++ ) {
		g2o::VertexSE3 * v = dynamic_cast< g2o::VertexSE3 * >( optimizer->vertex( i ) );
		pose_traj_.data_[ i ].transformation_ = G2O2Matrix4d( v->estimateAsSE3Quat() );
	}
	pose_traj_.SaveToFile( pose_log_file_ );

	loop_remain_traj_.data_.clear();
	for ( int i = 0; i < ( int )switch_edge.size(); i++ ) {
		SwitchableEdge & edge = switch_edge[ i ];
		if ( edge.weight_ > 0.25 ) {
			loop_remain_traj_.data_.push_back( loop_traj_.data_[ i ] );
		}
	}
	loop_remain_traj_.SaveToFile( loop_remain_log_file_ );
}


/*
 * Copyright 2009-2011 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

#include <votca/tools/akimaspline.h>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <votca/tools/linalg.h>
#include <iostream>

namespace votca { namespace tools {

using namespace std;

void AkimaSpline::Interpolate(ub::vector<double> &x, ub::vector<double> &y)
{
    if(x.size() != y.size())
        throw std::invalid_argument("error in AkimaSpline::Interpolate : sizes of vectors x and y do not match");

    // Akima splines require at least 4 points
    if(x.size()<4)
        throw std::invalid_argument("error in AkimaSpline::Interpolate : vectors x and y have to contain at least 4 points");

    const int N = x.size();

    // adjust the grid
    _r.resize(N);

    // copy the grid points into f
    _r = x;

    // initialize vectors p1,p2,p3,p4 and t
    p0 = ub::zero_vector<double>(N);
    p1 = ub::zero_vector<double>(N);
    p2 = ub::zero_vector<double>(N);
    p3 = ub::zero_vector<double>(N);
    t = ub::zero_vector<double>(N);

    double m1,m2,m3,m4;

    double temp, g0, g1, g2, x1, x2, y1, y2, x4, x5, y4, y5, m5;

    // boundary conditions
    // >> determine t(0), t(1) and t(N-2), t(N-1)
    switch(_boundaries) {
        case splineNormal:
            // Akima method: estimation of two more points on each side using a
            // degree two polyomial
            // resulting slopes t(0), t(1), t(N-2), t(N-1) are directly calculated

            // left side: t(0), t(1)
            temp = (x(1)-x(0))/(x(2)-x(0));
            temp = temp*temp;
            g0 = y(0);
            g1 = ( (y(1)-y(0)) - temp*(y(2)-y(0)) ) / ( (x(1)-x(0)) - temp*(x(2)-x(0)) );
            g2 = ( (y(2)-y(0)) - g1*(x(2)-x(0)) ) / ( (x(2)-x(0))*(x(2)-x(0)) );
            x1 = x(0) - (x(2)-x(0));
            x2 = x(1) - (x(2)-x(0));
            y1 = g0 + g1*(x1-x(0)) + g2*(x1-x(0))*(x1-x(0));
            y2 = g0 + g1*(x2-x(0)) + g2*(x2-x(0))*(x2-x(0));
            m1 = (y2-y1)/(x2-x1);
            m2 = (y(0)-y2)/(x(0)-x2);
            m3 = (y(1)-y(0))/(x(1)-x(0));
            m4 = (y(2)-y(1))/(x(2)-x(1));
            t(0) = getSlope(m1,m2,m3,m4);
            m5 = (y(3)-y(2))/(x(3)-x(2));
            t(1) = getSlope(m2,m3,m4,m5);

            // right side: t(N-2), t(N-1)
            temp = (x(N-2)-x(N-1))/(x(N-3)-x(N-1));
            temp = temp*temp;
            g0 = y(N-1);
            g1 = ( (y(N-2)-y(N-1)) - temp*(y(N-3)-y(N-1)) ) / ( (x(N-2)-x(N-1)) - temp*(x(N-3)-x(N-1)) );
            g2 = ( (y(N-3)-y(N-1)) - g1*(x(N-3)-x(N-1)) ) / ( (x(N-3)-x(N-1))*(x(N-3)-x(N-1)) );
            x4 = x(N-2) + (x(N-1)-x(N-3));
            x5 = x(N-1) + (x(N-1)-x(N-3));
            y4 = g0 + g1*(x4-x(N-1)) + g2*(x4-x(N-1))*(x4-x(N-1));
            y5 = g0 + g1*(x5-x(N-1)) + g2*(x5-x(N-1))*(x5-x(N-1));
            m1 = (y(N-3)-y(N-4))/(x(N-3)-x(N-4));
            m2 = (y(N-2)-y(N-3))/(x(N-2)-x(N-3));
            m3 = (y(N-1)-y(N-2))/(x(N-1)-x(N-2));
            m4 = (y4-y(N-1))/(x4-x(N-1));
            m5 = (y5-y4)/(x5-x4);
            t(N-2) = getSlope(m1,m2,m3,m4);
            t(N-1) = getSlope(m2,m3,m4,m5);
            break;
        case splinePeriodic:
            // left: last two points determine the slopes t(0), t(1)
            m1 = (y(N-1)-y(N-2))/(x(N-1)-x(N-2));
            m2 = (y(0)-y(N-1))/(x(0)-x(N-1));
            m3 = (y(1)-y(0))/(x(1)-x(0));
            m4 = (y(2)-y(1))/(x(2)-x(1));
            m5 = (y(3)-y(2))/(x(3)-x(2));
            t(0) = getSlope(m1,m2,m3,m4);
            t(1) = getSlope(m2,m3,m4,m5);
            // right: first two points determine the slopes t(N-2), t(N-1)
            m1 = (y(N-3)-y(N-4))/(x(N-3)-x(N-4));
            m2 = (y(N-2)-y(N-3))/(x(N-2)-x(N-3));
            m3 = (y(N-1)-y(N-2))/(x(N-1)-x(N-2));
            m4 = (y(0)-y(N-1))/(x(0)-x(N-1));
            m5 = (y(1)-y(0))/(x(1)-x(0));
            t(N-2) = getSlope(m1,m2,m3,m4);
            t(N-1) = getSlope(m2,m3,m4,m5);
            break;
        case splineDerivativeZero:
	    throw std::runtime_error("erro in AkimaSpline::Interpolate: case splineDerivativeZero not implemented yet");
	    break;
    }

    // calculate t's for all inner points [2,N-3]
    for (int i=2; i<N-2; i++) {
        m1 = (y(i-1)-y(i-2))/(x(i-1)-x(i-2));
        m2 = (y(i)-y(i-1))/(x(i)-x(i-1));
        m3 = (y(i+1)-y(i))/(x(i+1)-x(i));
        m4 = (y(i+2)-y(i+1))/(x(i+2)-x(i+1));
        t(i) = getSlope(m1,m2,m3,m4);
    }

    // calculate p0,p1,p2,p3 for all intervals 0..(N-2), where interval
    // [x(i),x(i+1)] shall have number i (this means that the last interval
    // has number N-2)
    for (int i=0; i<N-1; i++) {
        p0(i) = y(i);
        p1(i) = t(i);
        p2(i) = ( 3.0*(y(i+1)-y(i))/(x(i+1)-x(i)) - 2.0*t(i) - t(i+1) ) / (x(i+1)-x(i));
        p3(i) = ( t(i) + t(i+1) - 2.0*(y(i+1)-y(i))/(x(i+1)-x(i)) ) / ( (x(i+1)-x(i))*(x(i+1)-x(i)) );
    }
}

void AkimaSpline::Fit(ub::vector<double> &x, ub::vector<double> &y)
{
    throw std::runtime_error("Akima fit not implemented.");
}

}}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2014 Klaus Spanderen

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 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 license for more details.
*/

/*! \file analyticpdfhestonengine.cpp
    \brief Analytic engine for arbitrary European payoffs under the Heston model
*/

#include <ql/math/functional.hpp>
#include <ql/math/integrals/gausslobattointegral.hpp>
#include <ql/experimental/exoticoptions/analyticpdfhestonengine.hpp>

#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#endif

#include <boost/bind.hpp>

#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic pop
#endif

#include <cmath>
#include <complex>

namespace QuantLib {

    namespace {
        struct HestonParams {
            Real v0, kappa, theta, sigma, rho;
        };

        HestonParams getHestonParams(
            const boost::shared_ptr<HestonModel>& model) {
            const HestonParams p = { model->v0(),    model->kappa(),
                                     model->theta(), model->sigma(),
                                     model->rho() };
            return p;
        }

        std::complex<Real> gamma(const HestonParams& p, Real p_x) {
            return std::complex<Real>(p.kappa, p.rho*p.sigma*p_x);
        }

        std::complex<Real> omega(const HestonParams& p, Real p_x) {
           const std::complex<Real> g = gamma(p, p_x);
           return std::sqrt(g*g
                  + p.sigma*p.sigma*std::complex<Real>(p_x*p_x, -p_x));
        }

        class CpxPv_Helper
            : public std::unary_function<Real, Real > {
          public:
            CpxPv_Helper(const HestonParams& p, Real x, Time t)
              : p_(p), t_(t), x_(x),
                c_inf_(std::min(10.0, std::max(0.0001,
                      std::sqrt(1.0-square<Real>()(p_.rho))/p_.sigma))
                      *(p_.v0 + p_.kappa*p_.theta*t))  {}

            Real operator()(Real x) const {
                return std::real(transformPhi(x));
            }

            Real p0(Real p_x) const {
                if (p_x < QL_EPSILON) {
                    return 0.0;
                }

                const Real u_x = std::max(QL_EPSILON, -std::log(p_x)/c_inf_);
                return std::real(phi(u_x)
                        *std::exp(std::complex<Real>(0.0, -2*u_x*x_))
                        /((p_x*c_inf_)*std::complex<Real>(0.0, u_x)));
            }

          private:
            std::complex<Real> transformPhi(Real x) const {
                if (x < QL_EPSILON) {
                    return std::complex<Real>(0.0, 0.0);
                }

                const Real u_x = -std::log(x)/c_inf_;
                return phi(u_x)/(x*c_inf_);
            }

            std::complex<Real> phi(Real p_x) const {
                const Real sigma2 = p_.sigma*p_.sigma;
                const std::complex<Real> g = gamma(p_, p_x);
                const std::complex<Real> o = omega(p_, p_x);
                const std::complex<Real> gamma = (g-o)/(g+o);

                return 2.0*std::exp(std::complex<Real>(0.0, p_x*x_)
                        - p_.v0*std::complex<Real>(p_x*p_x, -p_x)
                          /(g+o*(1.0+std::exp(-o*t_))/(1.0-std::exp(-o*t_)))
                         +p_.kappa*p_.theta/sigma2*(
                           (g-o)*t_ - 2.0*std::log((1.0-gamma*std::exp(-o*t_))
                                                               /(1.0-gamma))));
            }

            const HestonParams& p_;
            const Time t_;
            const Real x_, c_inf_;
        };
    }

    AnalyticPDFHestonEngine::AnalyticPDFHestonEngine(
        const boost::shared_ptr<HestonModel>& model,
        Real gaussLobattoEps,
        Size gaussLobattoIntegrationOrder)
    : gaussLobattoIntegrationOrder_(gaussLobattoIntegrationOrder),
      gaussLobattoEps_(gaussLobattoEps),
      model_(model) {  }

    void AnalyticPDFHestonEngine::calculate() const {
        // this is an European option pricer
        QL_REQUIRE(arguments_.exercise->type() == Exercise::European,
                   "not an European option");

        const boost::shared_ptr<HestonProcess>& process = model_->process();

        const Time t = process->time(arguments_.exercise->lastDate());

        const Real xMax = 8.0 * std::sqrt(process->theta()*t
            + (process->v0() - process->theta())
                *(1-std::exp(-process->kappa()*t))/process->kappa());

        results_.value = GaussLobattoIntegral(
            gaussLobattoIntegrationOrder_, gaussLobattoEps_)(
            boost::bind(&AnalyticPDFHestonEngine::weightedPayoff, this,_1, t),
                         -xMax, xMax);
    }

    Real AnalyticPDFHestonEngine::Pv(Real x_t, Time t) const {
        return GaussLobattoIntegral(
            gaussLobattoIntegrationOrder_, 0.1*gaussLobattoEps_)(
                CpxPv_Helper(getHestonParams(model_), x_t, t),
                0.0, 1.0)/M_TWOPI;
    }

    Real AnalyticPDFHestonEngine::cdf(Real s, Time t) const {
        const boost::shared_ptr<HestonProcess>& process = model_->process();
        const DiscountFactor d=  process->riskFreeRate()->discount(t)
                               / process->dividendYield()->discount(t);

        const Real s_t = process->s0()->value()/d;
        const Real x = std::log(s_t/s);

        return GaussLobattoIntegral(
            gaussLobattoIntegrationOrder_, gaussLobattoEps_)(
                boost::bind(&CpxPv_Helper::p0,
                    CpxPv_Helper(getHestonParams(model_), x, t), _1),
                0.0, 1.0)/M_TWOPI + 0.5;
    }

    Real AnalyticPDFHestonEngine::weightedPayoff(Real x_t, Time t) const {
        const boost::shared_ptr<HestonProcess>& process = model_->process();

        const Real s_0 = process->s0()->value();
        const DiscountFactor rD = process->riskFreeRate()->discount(t);
        const DiscountFactor dD = process->dividendYield()->discount(t);

        const Real s_t = s_0*std::exp(x_t)*dD/rD;
        const Real payoff = (*arguments_.payoff)(s_t);

        return (payoff != 0.0) ? payoff*Pv(x_t, t)*rD : 0.0;
    }
}