#ifndef BOOST_GEOMETRY_PROJECTIONS_AEQD_HPP
#define BOOST_GEOMETRY_PROJECTIONS_AEQD_HPP

// Boost.Geometry - extensions-gis-projections (based on PROJ4)
// This file is automatically generated. DO NOT EDIT.

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017, 2018.
// Modifications copyright (c) 2017-2018, 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 PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 4.9.1

// Original copyright notice:

// Purpose:  Implementation of the aeqd (Azimuthal Equidistant) projection.
// Author:   Gerald Evenden
// Copyright (c) 1995, Gerald Evenden

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#include <boost/config.hpp>
#include <boost/geometry/util/math.hpp>
#include <boost/math/special_functions/hypot.hpp>

#include <boost/geometry/srs/projections/impl/base_static.hpp>
#include <boost/geometry/srs/projections/impl/base_dynamic.hpp>
#include <boost/geometry/srs/projections/impl/projects.hpp>
#include <boost/geometry/srs/projections/impl/factory_entry.hpp>
#include <boost/geometry/srs/projections/impl/aasincos.hpp>
#include <boost/geometry/srs/projections/impl/pj_mlfn.hpp>

#include <boost/geometry/srs/projections/par4.hpp>

#include <boost/type_traits/is_same.hpp>

namespace boost { namespace geometry
{

namespace srs { namespace par4
{
    struct aeqd {};
    //struct aeqd_guam {};

}} //namespace srs::par4

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace aeqd
    {

            static const double EPS10 = 1.e-10;
            static const double TOL = 1.e-14;
            static const int N_POLE = 0;
            static const int S_POLE = 1;
            static const int EQUIT = 2;
            static const int OBLIQ = 3;

            template <typename T>
            struct par_aeqd
            {
                T    sinph0;
                T    cosph0;
                T    en[EN_SIZE];
                T    M1;
                T    N1;
                T    Mp;
                T    He;
                T    G;
                int        mode;
            };

            template <typename T, typename Par, typename ProjParm>
            inline void e_forward(T& lp_lon, T& lp_lat, T& xy_x, T& xy_y, Par const& par, ProjParm const& proj_parm)
            {
                T  coslam, cosphi, sinphi, rho, s, H, H2, c, Az, t, ct, st, cA, sA;

                coslam = cos(lp_lon);
                cosphi = cos(lp_lat);
                sinphi = sin(lp_lat);
                switch (proj_parm.mode) {
                case N_POLE:
                    coslam = - coslam;
                    BOOST_FALLTHROUGH;
                case S_POLE:
                    xy_x = (rho = fabs(proj_parm.Mp - pj_mlfn(lp_lat, sinphi, cosphi, proj_parm.en))) *
                        sin(lp_lon);
                    xy_y = rho * coslam;
                    break;
                case EQUIT:
                case OBLIQ:
                    if (fabs(lp_lon) < EPS10 && fabs(lp_lat - par.phi0) < EPS10) {
                        xy_x = xy_y = 0.;
                        break;
                    }
                    t = atan2(par.one_es * sinphi + par.es * proj_parm.N1 * proj_parm.sinph0 *
                        sqrt(1. - par.es * sinphi * sinphi), cosphi);
                    ct = cos(t); st = sin(t);
                    Az = atan2(sin(lp_lon) * ct, proj_parm.cosph0 * st - proj_parm.sinph0 * coslam * ct);
                    cA = cos(Az); sA = sin(Az);
                    s = aasin(fabs(sA) < TOL ?
                        (proj_parm.cosph0 * st - proj_parm.sinph0 * coslam * ct) / cA :
                        sin(lp_lon) * ct / sA );
                    H = proj_parm.He * cA;
                    H2 = H * H;
                    c = proj_parm.N1 * s * (1. + s * s * (- H2 * (1. - H2)/6. +
                        s * ( proj_parm.G * H * (1. - 2. * H2 * H2) / 8. +
                        s * ((H2 * (4. - 7. * H2) - 3. * proj_parm.G * proj_parm.G * (1. - 7. * H2)) /
                        120. - s * proj_parm.G * H / 48.))));
                    xy_x = c * sA;
                    xy_y = c * cA;
                    break;
                }
            }

            template <typename T, typename Par, typename ProjParm>
            inline void e_inverse(T& xy_x, T& xy_y, T& lp_lon, T& lp_lat, Par const& par, ProjParm const& proj_parm)
            {
                static const T HALFPI = detail::HALFPI<T>();

                T c, Az, cosAz, A, B, D, E, F, psi, t;

                if ((c = boost::math::hypot(xy_x, xy_y)) < EPS10) {
                    lp_lat = par.phi0;
                    lp_lon = 0.;
                        return;
                }
                if (proj_parm.mode == OBLIQ || proj_parm.mode == EQUIT) {
                    cosAz = cos(Az = atan2(xy_x, xy_y));
                    t = proj_parm.cosph0 * cosAz;
                    B = par.es * t / par.one_es;
                    A = - B * t;
                    B *= 3. * (1. - A) * proj_parm.sinph0;
                    D = c / proj_parm.N1;
                    E = D * (1. - D * D * (A * (1. + A) / 6. + B * (1. + 3.*A) * D / 24.));
                    F = 1. - E * E * (A / 2. + B * E / 6.);
                    psi = aasin(proj_parm.sinph0 * cos(E) + t * sin(E));
                    lp_lon = aasin(sin(Az) * sin(E) / cos(psi));
                    if ((t = fabs(psi)) < EPS10)
                        lp_lat = 0.;
                    else if (fabs(t - HALFPI) < 0.)
                        lp_lat = HALFPI;
                    else
                        lp_lat = atan((1. - par.es * F * proj_parm.sinph0 / sin(psi)) * tan(psi) /
                            par.one_es);
                } else { /* Polar */
                    lp_lat = pj_inv_mlfn(proj_parm.mode == N_POLE ? proj_parm.Mp - c : proj_parm.Mp + c,
                        par.es, proj_parm.en);
                    lp_lon = atan2(xy_x, proj_parm.mode == N_POLE ? -xy_y : xy_y);
                }
            }

            template <typename T, typename Par, typename ProjParm>
            inline void e_guam_fwd(T& lp_lon, T& lp_lat, T& xy_x, T& xy_y, Par const& par, ProjParm const& proj_parm)
            {
                T cosphi, sinphi, t;

                cosphi = cos(lp_lat);
                sinphi = sin(lp_lat);
                t = 1. / sqrt(1. - par.es * sinphi * sinphi);
                xy_x = lp_lon * cosphi * t;
                xy_y = pj_mlfn(lp_lat, sinphi, cosphi, proj_parm.en) - proj_parm.M1 +
                    .5 * lp_lon * lp_lon * cosphi * sinphi * t;
            }

            template <typename T, typename Par, typename ProjParm>
            inline void e_guam_inv(T& xy_x, T& xy_y, T& lp_lon, T& lp_lat, Par const& par, ProjParm const& proj_parm)
            {
                T x2, t;
                int i;

                x2 = 0.5 * xy_x * xy_x;
                lp_lat = par.phi0;
                for (i = 0; i < 3; ++i) {
                    t = par.e * sin(lp_lat);
                    lp_lat = pj_inv_mlfn(proj_parm.M1 + xy_y -
                        x2 * tan(lp_lat) * (t = sqrt(1. - t * t)), par.es, proj_parm.en);
                }
                lp_lon = xy_x * t / cos(lp_lat);
            }

            template <typename T, typename Par, typename ProjParm>
            inline void s_forward(T& lp_lon, T& lp_lat, T& xy_x, T& xy_y, Par const& /*par*/, ProjParm const& proj_parm)
            {
                static const T HALFPI = detail::HALFPI<T>();

                T coslam, cosphi, sinphi;

                sinphi = sin(lp_lat);
                cosphi = cos(lp_lat);
                coslam = cos(lp_lon);
                switch (proj_parm.mode) {
                case EQUIT:
                    xy_y = cosphi * coslam;
                    goto oblcon;
                case OBLIQ:
                    xy_y = proj_parm.sinph0 * sinphi + proj_parm.cosph0 * cosphi * coslam;
            oblcon:
                    if (fabs(fabs(xy_y) - 1.) < TOL)
                        if (xy_y < 0.)
                            BOOST_THROW_EXCEPTION( projection_exception(-20) );
                        else
                            xy_x = xy_y = 0.;
                    else {
                        xy_y = acos(xy_y);
                        xy_y /= sin(xy_y);
                        xy_x = xy_y * cosphi * sin(lp_lon);
                        xy_y *= (proj_parm.mode == EQUIT) ? sinphi :
                                proj_parm.cosph0 * sinphi - proj_parm.sinph0 * cosphi * coslam;
                    }
                    break;
                case N_POLE:
                    lp_lat = -lp_lat;
                    coslam = -coslam;
                    BOOST_FALLTHROUGH;
                case S_POLE:
                    if (fabs(lp_lat - HALFPI) < EPS10)
                        BOOST_THROW_EXCEPTION( projection_exception(-20) );
                    xy_x = (xy_y = (HALFPI + lp_lat)) * sin(lp_lon);
                    xy_y *= coslam;
                    break;
                }
            }

            template <typename T, typename Par, typename ProjParm>
            inline void s_inverse(T& xy_x, T& xy_y, T& lp_lon, T& lp_lat, Par const& par, ProjParm const& proj_parm)
            {
                static const T ONEPI = detail::ONEPI<T>();
                static const T HALFPI = detail::HALFPI<T>();

                T cosc, c_rh, sinc;

                if ((c_rh = boost::math::hypot(xy_x, xy_y)) > ONEPI) {
                    if (c_rh - EPS10 > ONEPI)
                        BOOST_THROW_EXCEPTION( projection_exception(-20) );
                    c_rh = ONEPI;
                } else if (c_rh < EPS10) {
                    lp_lat = par.phi0;
                    lp_lon = 0.;
                        return;
                }
                if (proj_parm.mode == OBLIQ || proj_parm.mode == EQUIT) {
                    sinc = sin(c_rh);
                    cosc = cos(c_rh);
                    if (proj_parm.mode == EQUIT) {
                                    lp_lat = aasin(xy_y * sinc / c_rh);
                        xy_x *= sinc;
                        xy_y = cosc * c_rh;
                    } else {
                        lp_lat = aasin(cosc * proj_parm.sinph0 + xy_y * sinc * proj_parm.cosph0 /
                            c_rh);
                        xy_y = (cosc - proj_parm.sinph0 * sin(lp_lat)) * c_rh;
                        xy_x *= sinc * proj_parm.cosph0;
                    }
                    lp_lon = atan2(xy_x, xy_y);
                } else if (proj_parm.mode == N_POLE) {
                    lp_lat = HALFPI - c_rh;
                    lp_lon = atan2(xy_x, -xy_y);
                } else {
                    lp_lat = c_rh - HALFPI;
                    lp_lon = atan2(xy_x, xy_y);
                }
            }

            // Azimuthal Equidistant
            template <typename Parameters, typename T>
            inline void setup_aeqd(Parameters& par, par_aeqd<T>& proj_parm, bool is_sphere, bool is_guam)
            {
                static const T HALFPI = detail::HALFPI<T>();

                par.phi0 = pj_param(par.params, "rlat_0").f;
                if (fabs(fabs(par.phi0) - HALFPI) < EPS10) {
                    proj_parm.mode = par.phi0 < 0. ? S_POLE : N_POLE;
                    proj_parm.sinph0 = par.phi0 < 0. ? -1. : 1.;
                    proj_parm.cosph0 = 0.;
                } else if (fabs(par.phi0) < EPS10) {
                    proj_parm.mode = EQUIT;
                    proj_parm.sinph0 = 0.;
                    proj_parm.cosph0 = 1.;
                } else {
                    proj_parm.mode = OBLIQ;
                    proj_parm.sinph0 = sin(par.phi0);
                    proj_parm.cosph0 = cos(par.phi0);
                }
                if (is_sphere) {
                } else {
                    if (!pj_enfn(par.es, proj_parm.en))
                        BOOST_THROW_EXCEPTION( projection_exception(0) );
                    if (is_guam) {
                        proj_parm.M1 = pj_mlfn(par.phi0, proj_parm.sinph0, proj_parm.cosph0, proj_parm.en);
                    } else {
                        switch (proj_parm.mode) {
                        case N_POLE:
                            proj_parm.Mp = pj_mlfn<T>(HALFPI, 1., 0., proj_parm.en);
                            break;
                        case S_POLE:
                            proj_parm.Mp = pj_mlfn<T>(-HALFPI, -1., 0., proj_parm.en);
                            break;
                        case EQUIT:
                        case OBLIQ:
                            proj_parm.N1 = 1. / sqrt(1. - par.es * proj_parm.sinph0 * proj_parm.sinph0);
                            proj_parm.G = proj_parm.sinph0 * (proj_parm.He = par.e / sqrt(par.one_es));
                            proj_parm.He *= proj_parm.cosph0;
                            break;
                        }
                    }
                }
            }

            // template class, using CRTP to implement forward/inverse
            template <typename CalculationType, typename Parameters>
            struct base_aeqd_e : public base_t_fi<base_aeqd_e<CalculationType, Parameters>,
                     CalculationType, Parameters>
            {

                typedef CalculationType geographic_type;
                typedef CalculationType cartesian_type;

                par_aeqd<CalculationType> m_proj_parm;

                inline base_aeqd_e(const Parameters& par)
                    : base_t_fi<base_aeqd_e<CalculationType, Parameters>,
                     CalculationType, Parameters>(*this, par) {}

                // FORWARD(e_forward)  elliptical
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                    e_forward(lp_lon, lp_lat, xy_x, xy_y, this->m_par, this->m_proj_parm);
                }

                // INVERSE(e_inverse)  elliptical
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(cartesian_type& xy_x, cartesian_type& xy_y, geographic_type& lp_lon, geographic_type& lp_lat) const
                {
                    e_inverse(xy_x, xy_y, lp_lon, lp_lat, this->m_par, this->m_proj_parm);
                }

                static inline std::string get_name()
                {
                    return "aeqd_e";
                }

            };

            // template class, using CRTP to implement forward/inverse
            template <typename CalculationType, typename Parameters>
            struct base_aeqd_e_guam : public base_t_fi<base_aeqd_e_guam<CalculationType, Parameters>,
                     CalculationType, Parameters>
            {

                typedef CalculationType geographic_type;
                typedef CalculationType cartesian_type;

                par_aeqd<CalculationType> m_proj_parm;

                inline base_aeqd_e_guam(const Parameters& par)
                    : base_t_fi<base_aeqd_e_guam<CalculationType, Parameters>,
                     CalculationType, Parameters>(*this, par) {}

                // FORWARD(e_guam_fwd)  Guam elliptical
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                    e_guam_fwd(lp_lon, lp_lat, xy_x, xy_y, this->m_par, this->m_proj_parm);
                }

                // INVERSE(e_guam_inv)  Guam elliptical
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(cartesian_type& xy_x, cartesian_type& xy_y, geographic_type& lp_lon, geographic_type& lp_lat) const
                {
                    e_guam_inv(xy_x, xy_y, lp_lon, lp_lat, this->m_par, this->m_proj_parm);
                }

                static inline std::string get_name()
                {
                    return "aeqd_e_guam";
                }

            };

            // template class, using CRTP to implement forward/inverse
            template <typename BGParameters, typename CalculationType, typename Parameters>
            struct base_aeqd_e_static : public base_t_fi<base_aeqd_e_static<BGParameters, CalculationType, Parameters>,
                     CalculationType, Parameters>
            {

                typedef CalculationType geographic_type;
                typedef CalculationType cartesian_type;

                par_aeqd<CalculationType> m_proj_parm;

                static const bool is_guam = ! boost::is_same
                    <
                        typename srs::par4::detail::tuples_find_if
                            <
                                BGParameters,
                                //srs::par4::detail::is_guam
                                srs::par4::detail::is_param<srs::par4::guam>::pred
                            >::type,
                        void
                    >::value;

                inline base_aeqd_e_static(const Parameters& par)
                    : base_t_fi<base_aeqd_e_static<BGParameters, CalculationType, Parameters>,
                     CalculationType, Parameters>(*this, par)
                {}

                // FORWARD(e_forward or e_guam_fwd)  elliptical
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                    if (is_guam)
                        e_guam_fwd(lp_lon, lp_lat, xy_x, xy_y, this->m_par, this->m_proj_parm);
                    else
                        e_forward(lp_lon, lp_lat, xy_x, xy_y, this->m_par, this->m_proj_parm);
                }

                // INVERSE(e_inverse or e_guam_inv)  elliptical
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(cartesian_type& xy_x, cartesian_type& xy_y, geographic_type& lp_lon, geographic_type& lp_lat) const
                {
                    if (is_guam)
                        e_guam_inv(xy_x, xy_y, lp_lon, lp_lat, this->m_par, this->m_proj_parm);
                    else
                        e_inverse(xy_x, xy_y, lp_lon, lp_lat, this->m_par, this->m_proj_parm);
                }

                static inline std::string get_name()
                {
                    return "aeqd_e_static";
                }

            };

            // template class, using CRTP to implement forward/inverse
            template <typename CalculationType, typename Parameters>
            struct base_aeqd_s : public base_t_fi<base_aeqd_s<CalculationType, Parameters>,
                     CalculationType, Parameters>
            {

                typedef CalculationType geographic_type;
                typedef CalculationType cartesian_type;

                par_aeqd<CalculationType> m_proj_parm;

                inline base_aeqd_s(const Parameters& par)
                    : base_t_fi<base_aeqd_s<CalculationType, Parameters>,
                     CalculationType, Parameters>(*this, par) {}

                // FORWARD(s_forward)  spherical
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                    s_forward(lp_lon, lp_lat, xy_x, xy_y, this->m_par, this->m_proj_parm);
                }

                // INVERSE(s_inverse)  spherical
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(cartesian_type& xy_x, cartesian_type& xy_y, geographic_type& lp_lon, geographic_type& lp_lat) const
                {
                    s_inverse(xy_x, xy_y, lp_lon, lp_lat, this->m_par, this->m_proj_parm);
                }

                static inline std::string get_name()
                {
                    return "aeqd_s";
                }

            };

    }} // namespace detail::aeqd
    #endif // doxygen

    /*!
        \brief Azimuthal Equidistant projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Azimuthal
         - Spheroid
         - Ellipsoid
        \par Projection parameters
         - lat_0: Latitude of origin (degrees)
         - guam (boolean)
        \par Example
        \image html ex_aeqd.gif
    */
    template <typename CalculationType, typename Parameters>
    struct aeqd_e : public detail::aeqd::base_aeqd_e<CalculationType, Parameters>
    {
        inline aeqd_e(const Parameters& par) : detail::aeqd::base_aeqd_e<CalculationType, Parameters>(par)
        {
            detail::aeqd::setup_aeqd(this->m_par, this->m_proj_parm, false, false);
        }
    };

    /*!
        \brief Azimuthal Equidistant projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Azimuthal
         - Spheroid
         - Ellipsoid
        \par Projection parameters
         - lat_0: Latitude of origin (degrees)
         - guam (boolean)
        \par Example
        \image html ex_aeqd.gif
    */
    template <typename CalculationType, typename Parameters>
    struct aeqd_e_guam : public detail::aeqd::base_aeqd_e_guam<CalculationType, Parameters>
    {
        inline aeqd_e_guam(const Parameters& par) : detail::aeqd::base_aeqd_e_guam<CalculationType, Parameters>(par)
        {
            detail::aeqd::setup_aeqd(this->m_par, this->m_proj_parm, false, true);
        }
    };

    /*!
        \brief Azimuthal Equidistant projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Azimuthal
         - Spheroid
         - Ellipsoid
        \par Projection parameters
         - lat_0: Latitude of origin (degrees)
         - guam (boolean)
        \par Example
        \image html ex_aeqd.gif
    */
    template <typename BGParameters, typename CalculationType, typename Parameters>
    struct aeqd_e_static : public detail::aeqd::base_aeqd_e_static<BGParameters, CalculationType, Parameters>
    {
        inline aeqd_e_static(const Parameters& par) : detail::aeqd::base_aeqd_e_static<BGParameters, CalculationType, Parameters>(par)
        {
            detail::aeqd::setup_aeqd(this->m_par, this->m_proj_parm,
                                     false,
                                     detail::aeqd::base_aeqd_e_static<BGParameters, CalculationType, Parameters>::is_guam);
        }
    };

    /*!
        \brief Azimuthal Equidistant projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Azimuthal
         - Spheroid
         - Ellipsoid
        \par Projection parameters
         - lat_0: Latitude of origin (degrees)
         - guam (boolean)
        \par Example
        \image html ex_aeqd.gif
    */
    template <typename CalculationType, typename Parameters>
    struct aeqd_s : public detail::aeqd::base_aeqd_s<CalculationType, Parameters>
    {
        inline aeqd_s(const Parameters& par) : detail::aeqd::base_aeqd_s<CalculationType, Parameters>(par)
        {
            detail::aeqd::setup_aeqd(this->m_par, this->m_proj_parm, true, false);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        template <typename BGP, typename CT, typename P>
        struct static_projection_type<srs::par4::aeqd, srs_sphere_tag, BGP, CT, P>
        {
            typedef aeqd_s<CT, P> type;
        };
        template <typename BGP, typename CT, typename P>
        struct static_projection_type<srs::par4::aeqd, srs_spheroid_tag, BGP, CT, P>
        {
            typedef aeqd_e_static<BGP, CT, P> type;
        };
        //BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::par4::aeqd, aeqd_s, aeqd_e_static)
        //BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::par4::aeqd_guam, aeqd_guam, aeqd_guam)

        // Factory entry(s)
        template <typename CalculationType, typename Parameters>
        class aeqd_entry : public detail::factory_entry<CalculationType, Parameters>
        {
            public :
                virtual base_v<CalculationType, Parameters>* create_new(const Parameters& par) const
                {
                    bool const guam = pj_param(par.params, "bguam").i != 0;

                    if (par.es && ! guam)
                        return new base_v_fi<aeqd_e<CalculationType, Parameters>, CalculationType, Parameters>(par);
                    else if (par.es && guam)
                        return new base_v_fi<aeqd_e_guam<CalculationType, Parameters>, CalculationType, Parameters>(par);
                    else
                        return new base_v_fi<aeqd_s<CalculationType, Parameters>, CalculationType, Parameters>(par);
                }
        };

        template <typename CalculationType, typename Parameters>
        inline void aeqd_init(detail::base_factory<CalculationType, Parameters>& factory)
        {
            factory.add_to_factory("aeqd", new aeqd_entry<CalculationType, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_AEQD_HPP


#include <iostream>
#include <algorithm>
#include <cstdio>
#include <cstdlib>
#include <fstream>
#include <sstream>
#include <cmath>
#include <string>
#include <iostream>
#include <vector>
#include <exception>
#include <GeographicLib/Geodesic.hpp>
#include <GeographicLib/Constants.hpp>
#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string.hpp>
#include "bilinearInterpolation.hpp"
#include "hypoStation.hpp"
#include "h5io.hpp"
#include "blackList.hpp"

struct TravelTimeInterpolator
{
    double getTime(const double offset, const double depth)
    {
        bilin.interpolate(1, &offset, &depth);
        auto ptr = bilin.getInterpolatedFunctionPointer();
        return ptr[0];
    }
    std::vector<double> getTimes(const std::vector<double> &offsets,
                                 const std::vector<double> &depths)
    {
        if (offsets.size() != depths.size())
        {
            throw std::invalid_argument("offsets.size() != depths.size()");
        }
        bilin.interpolate(offsets.size(), offsets.data(), depths.data());
        return bilin.getInterpolatedFunction();
    }
    BilinearInterpolation<double> bilin;
    std::string phaseLabel;
    std::vector<double> travelTimes;
    std::vector<double> depths;
    std::vector<double> offsets;
};

struct Receiver
{
    std::string network;
    std::string station;
    double latitude = 0;
    double longitude = 0;
    double depth = 0;
};

bool operator==(const Receiver &a, const Receiver &b)
{
    return (a.network == b.network && a.station == b.station);
}

std::vector<Receiver> getReceiverListFromCSV(const std::string &fileName,
                                             const bool haveHeader = true)
{
    std::ifstream infl(fileName);
    if (!infl){throw std::runtime_error("Could not open: " + fileName);}
    std::vector<Receiver> receivers;
    std::string line;
    std::vector<std::string> split;
    int lineNumber = 0;
    while (std::getline(infl, line))
    {
        lineNumber = lineNumber + 1;
        if (lineNumber == 1 && haveHeader){continue;}
        //UU,ALT,ENE,ENN,ENZ,01,40.59028,-111.6375,2635.0
        boost::split(split, line, boost::is_any_of(",\n\t"),
                     boost::token_compress_on);
        Receiver receiver;
        receiver.network = split[0];
        receiver.station = split[1];
        receiver.latitude = std::stod(split[6]);
        receiver.longitude = std::stod(split[7]);
        receiver.depth = std::stod(split[8]);
        bool lnew = true;
        for (const auto &r : receivers)
        {
            if (receiver == r)
            {
                lnew = false;
                break;
            }
        }
        if (lnew){receivers.push_back(receiver);}
    }
    infl.close();
    std::cout << "Number of receivers: " << receivers.size() << std::endl;
    return receivers;
}

std::vector<Receiver> getReceiverListFromNodalFile(const std::string &fileName)
{
    std::ifstream infl(fileName);
    if (!infl){throw std::runtime_error("Could not open: " + fileName);}
    std::vector<Receiver> receivers;
    std::vector<std::string> split;
    std::string line;
    while (std::getline(infl, line))
    {
        // -112.02516440 40.75396926 1272.3 001 001 5462 UU001
        boost::split(split, line, boost::is_any_of(" ,\n\t"),
                     boost::token_compress_on);
        Receiver receiver;
        receiver.network = "UU";
        receiver.station = split[3];
        receiver.latitude = std::stod(split[1]);
        receiver.longitude = std::stod(split[0]);
        receiver.depth = std::round(std::stod(split[2]));
//std::cout << receiver.network << "," << receiver.station << "," << receiver.latitude << "," << receiver.longitude << "," << receiver.depth << std::endl;
        if (isBlackListed(receiver.network, receiver.station)){continue;}
        bool lnew = true;
        for (const auto &r : receivers)
        {
            if (receiver == r)
            {
                lnew = false;
                break;
            }
        }
        if (lnew){receivers.push_back(receiver);}
    }
    infl.close();
    std::cout << "Number of nodes: " << receivers.size() << std::endl;
    return receivers;
}

/// @brief Creates a travel time table interpolation structure from the
///        travel time tables computed by GrowClust.
/// @param[in] fileName   The file name with the travel time table.
/// @param[in] phaseLabel The phase label (e.g., P or S).
/// @param[out] interp    This is the travel time with a travel time
///                       bilinear interpolation method.
void createGrowClustTravelTimeTable(const std::string &fileName,
                                    const std::string &phaseLabel,
                                    TravelTimeInterpolator *interp)
{
    std::ifstream infl(fileName);
    if (!infl)
    {
        throw std::runtime_error("Could not open: " + fileName);
    }
    int lineNumber = 0;
    std::string line;
    std::vector<std::string> split;
    int nDepths = 0;
    int nDistances = 0;
    std::vector<double> depths;
    std::vector<double> offsets;
    std::vector<double> travelTimes;
    while (std::getline(infl, line))
    {
        lineNumber = lineNumber + 1;
        if (lineNumber == 1){continue;} // Header
        // Remove leading blanks and parse
        boost::algorithm::trim_left(line);
        boost::split(split, line, boost::is_any_of("\n\t "),
                     boost::token_compress_on);
        // Read sizes
        if (lineNumber == 2)
        {
            nDistances = std::stoi(split[0]);
            nDepths = std::stoi(split[1]);
            offsets.reserve(nDistances);
            depths.resize(nDepths);
            travelTimes.resize(nDistances*nDepths, -1);
            continue;
        }
        // Read depths
        if (lineNumber == 3)
        {
            for (int id=0; id<nDepths; ++id)
            {
                depths[id] = std::stod(split[id]);
            }
            continue;
       }
       offsets.push_back(std::stod(split[0]));
       if (static_cast<int> (split.size() - 1) != nDepths)
       {
           throw std::runtime_error("Invalid size");
       }
       for (int i=0; i<nDepths; ++i)
       {
           auto indx = i*nDistances + offsets.size() - 1;
           travelTimes.at(indx) = std::stod(split[i+1]);
       }
    }
    infl.close();
    auto [tmin, tmax]
        = std::minmax_element(travelTimes.begin(), travelTimes.end());
    if (*tmin < 0){throw std::runtime_error("Failed to unpack ttimes");}
    // Create linearly interpolation
    interp->phaseLabel = phaseLabel;
    interp->offsets = offsets;
    interp->depths = depths;
    interp->travelTimes = travelTimes;
    interp->bilin.initialize(offsets.size(), offsets.data(),
                             depths.size(), depths.data(),
                             travelTimes.size(), travelTimes.data());
    // Check travel time interpolator works:
    //std::cout << travelTimes[10*nDistances+12] << " "  << interp->getTime(offsets[12], depths[10]) << std::endl;
}

/// @brief Create a grid in spherical coordinates.
void createGrid(const double lat0, const double lat1, const int nLat,
                const double lon0, const double lon1, const int nLon,
                const double z0,   const double z1,   const int nDep,
                std::vector<double> *lons,
                std::vector<double> *lats,
                std::vector<double> *depths)
{
    if (nDep < 2 || nLon < 2 || nLat < 2)
    {
        throw std::invalid_argument("need more than 2 grid points in x, y, z");
    }
    int nPts = nLat*nLon*nDep;
    std::cout << "Number of points in grid: " << nPts << std::endl;
    auto dLat = (lat1 - lat0)/(nLat - 1);
    auto dLon = (lon1 - lon0)/(nLon - 1);
    auto dDep = (z1 - z0)/(nDep - 1);
    lons->resize(nPts, 0);
    lats->resize(nPts, 0);
    depths->resize(nPts, -10);
    auto lonPtr = lons->data();
    auto latPtr = lats->data();
    auto depPtr = depths->data();
    // x
    #pragma omp simd collapse(3)
    for (int iLon=0; iLon<nLon; ++iLon)
    {
        // y
        for (int iLat=0; iLat<nLat; ++iLat)
        {
            // z
            for (int id=0; id<nDep; ++id)
            {
                auto indx = iLon*nDep*nLat + iLat*nDep + id;
                lonPtr[indx] = lon0 + iLon*dLon;
                latPtr[indx] = lat0 + iLat*dLat;
                depPtr[indx] = z0   + id*dDep;
            }
        }
    }
    auto dmin = std::min_element(depths->begin(), depths->end());
    if (*dmin <=-10){throw std::runtime_error("algorithm failure");}
}

/// @brief Creates the candidate source points in the grid search.
/// @param[out] lons     The longitudes in degrees.
/// @param[out] lats     The latitudes in degrees.
/// @param[out] depths   The depths in kilometers.
void createSourcePoints(std::vector<double> *lats,
                        std::vector<double> *lons,
                        std::vector<double> *depths)
{
    double kmPerDeg = 111.195;
    double deltaCoarse = 4; // 5 km spacing
    double lat0Coarse = 40.5;
    double lat1Coarse = 41.0;
    double lon0Coarse =-112.24;
    double lon1Coarse =-111.76;
    double z0Coarse = 1.5; // 1.5 km is sea-level
    double z1Coarse = 21.5;
    int nLatCoarse = static_cast<int> (std::round( (lat1Coarse - lat0Coarse)
                                                  /deltaCoarse*kmPerDeg) );
    int nLonCoarse = static_cast<int> (std::round( (lon1Coarse - lon0Coarse)
                                                  /deltaCoarse*kmPerDeg) );
    int nDepCoarse = static_cast<int> (std::round( (z1Coarse - z0Coarse)
                                                  /deltaCoarse ));

    double deltaFine = 1.25; // 1 km spacing
    double lon0Fine =-112.2;
    double lon1Fine =-111.8;
    double lat0Fine = 40.6;
    double lat1Fine = 40.85;
    double z0Fine = 0;
    double z1Fine = 18;
    int nLatFine = static_cast<int> (std::round( (lat1Fine - lat0Fine)
                                                /deltaFine*kmPerDeg) );
    int nLonFine = static_cast<int> (std::round( (lon1Fine - lon0Fine)
                                                /deltaFine*kmPerDeg) );
    int nDepFine = static_cast<int> (std::round( (z1Fine - z0Fine)
                                                /deltaFine ));

    std::vector<double> latsCoarse, lonsCoarse, depthsCoarse;
    createGrid(lat0Coarse, lat1Coarse, nLatCoarse,
               lon0Coarse, lon1Coarse, nLonCoarse,
               z0Coarse, z1Coarse, nDepCoarse,
               &lonsCoarse, &latsCoarse, &depthsCoarse);
    // Add in Bingham
    lonsCoarse.push_back(-112.1453);
    latsCoarse.push_back(40.5162);
    depthsCoarse.push_back(0);
    //std::cout << depthsCoarse.size() << std::endl;
    // Create the fine grid
    std::vector<double> latsFine, lonsFine, depthsFine;
    createGrid(lat0Fine, lat1Fine, nLatFine,
               lon0Fine, lon1Fine, nLonFine,
               z0Fine,   z1Fine, nDepFine,
               &lonsFine, &latsFine, &depthsFine);
    lonsCoarse.insert(std::end(lonsCoarse),
                      std::begin(lonsFine), std::end(lonsFine));
    latsCoarse.insert(std::end(latsCoarse),
                      std::begin(latsFine), std::end(latsFine));
    depthsCoarse.insert(std::end(depthsCoarse),
                        std::begin(depthsFine), std::end(depthsFine));
    //std::cout << depthsCoarse.size() << std::endl;
    *lons = lonsCoarse;
    *lats = latsCoarse;
    *depths = depthsCoarse;
}

// @brief Compute the source-to-receiver distances.
/// @param[in] receiverLatitude   The receiver's latitude in degrees.
/// @param[in] receiverLongitude  The receiver's longitude in degrees.
/// @param[in] sourceLatitudes    The source latitudes in degrees.
/// @param[in] sourceLongitudes   The source longitudes in degrees.
/// @param[out] distances         The source receiver distances in kilometers.
void computeDistances(const double receiverLatitude,
                      const double receiverLongitude,
                      const std::vector<double> &sourceLatitudes,
                      const std::vector<double> &sourceLongitudes,
                      std::vector<double> *distances)
{
    GeographicLib::Geodesic geodesic{GeographicLib::Constants::WGS84_a(),
                                     GeographicLib::Constants::WGS84_f()};
    distances->resize(sourceLatitudes.size());
    auto dPtr = distances->data();
    for (int i=0; i<static_cast<int> (sourceLatitudes.size()); ++i)
    {
        geodesic.Inverse(sourceLatitudes[i], sourceLongitudes[i],
                         receiverLatitude,   receiverLongitude,
                         dPtr[i]);
        dPtr[i] = dPtr[i]*1.e-3; // km
    }
}

int main()
{
    struct TravelTimeInterpolator pTimes;
    struct TravelTimeInterpolator sTimes;
/*
    auto receivers = getReceiverListFromCSV("../magna/magna_3c_stations.csv", true);
    auto nodes = getReceiverListFromNodalFile("../magna/Magna-Locs_2020-SN.txt");
    receivers.insert(std::end(receivers), std::begin(nodes), std::end(nodes));
*/
    HypoStation stations;
    stations.read("../magna/locate/magna.sta");
    std::cout << "Total number of receivers: " << stations.networks.size() << std::endl;
//    std::cout << "Total number of receivers: " << receivers.size() << std::endl;
    createGrowClustTravelTimeTable("../magna/TT.wasatch.pg", "P", &pTimes);
    createGrowClustTravelTimeTable("../magna/TT.wasatch.sg", "S", &sTimes);
    std::vector<double> lons, lats, depths, distances;
    createSourcePoints(&lats, &lons, &depths);
    H5IO h5io;
    h5io.openFileForWriting("../magna/travelTimeTables.h5");
    h5io.setGeometry(lats.size(), lats.data(), lons.data(), depths.data());
std::ofstream locFile("locations.txt");
std::ofstream staFile("stations.txt");

    for (int irec=0; irec<static_cast<int> (stations.networks.size()); ++irec)
    {
        computeDistances(stations.latitudes[irec], stations.longitudes[irec],
                         lats, lons, &distances);
        auto dmax = std::max_element(distances.begin(), distances.end());
        auto pTravelTimes = pTimes.getTimes(distances, depths);
        auto sTravelTimes = sTimes.getTimes(distances, depths);
        auto pmax = std::max_element(pTravelTimes.begin(), pTravelTimes.end());
        auto smax = std::max_element(sTravelTimes.begin(), sTravelTimes.end());
        if (irec == 0)
        {
           for (int j=0; j<static_cast<int> (lats.size()); ++j)
           {
               locFile << lons[j] << " " << lats[j] << " " << depths[j] << " " << distances[j]
                       << " " << pTravelTimes[j] << std::endl;
           }
        }
        staFile << stations.longitudes[irec] << " " << stations.latitudes[irec] << std::endl;
       std::cout << stations.stations[irec] << " " << stations.latitudes[irec] << " "
                 << stations.longitudes[irec]
                 << " " << *dmax << " " << *pmax << " " << *smax << std::endl;
        h5io.addTravelTimeTable(stations.networks[irec],
                                stations.stations[irec],
                                "P",
                                pTravelTimes.size(), pTravelTimes.data());
        h5io.addTravelTimeTable(stations.networks[irec],
                                stations.stations[irec],
                                "S",
                                sTravelTimes.size(), sTravelTimes.data());
    }
/*
    for (int irec=0; irec<static_cast<int> (receivers.size()); ++irec)
    {
        computeDistances(receivers[irec].latitude, receivers[irec].longitude,
                         lats, lons, &distances);
        auto dmax = std::max_element(distances.begin(), distances.end());
        auto pTravelTimes = pTimes.getTimes(distances, depths);
        auto sTravelTimes = sTimes.getTimes(distances, depths);
        auto pmax = std::max_element(pTravelTimes.begin(), pTravelTimes.end());
        auto smax = std::max_element(sTravelTimes.begin(), sTravelTimes.end());
if (irec == 0)
{
for (int j=0; j<lats.size(); ++j)
{
   locFile << lons[j] << " " << lats[j] << " " << distances[j] << " " << pTravelTimes[j] << std::endl;
}
}
staFile << receivers[irec].longitude << " " << receivers[irec].latitude << std::endl;
std::cout << receivers[irec].station << " " << receivers[irec].latitude << " " << receivers[irec].longitude << " " << *dmax << " " << *pmax << " " << *smax << std::endl;
        h5io.addTravelTimeTable(receivers[irec].network,
                                receivers[irec].station,
                                "P",
                                pTravelTimes.size(), pTravelTimes.data());
        h5io.addTravelTimeTable(receivers[irec].network,
                                receivers[irec].station,
                                "S",
                                sTravelTimes.size(), sTravelTimes.data());
    }
*/
}


// TODO:
// 1. init state from file
// 2. many algo runs
// 3. save metadata (score)
// 4. optimize moderator
#include <iostream>
#include <fstream>
#include <sstream>
#include <ctime>
#include <iomanip>
#include <unordered_map>
#include <vector>
#include <algorithm>
#include <math.h>
#include <random>
#include <boost/functional/hash.hpp>
#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>
#include <boost/tokenizer.hpp>
#include <boost/algorithm/string.hpp>

#include "schedule.hh"
#include "params.hh"
#include "utils.hh"
#include "scorer.hh"

using namespace std;
namespace po = boost::program_options;

void usage(const string& name, const string& errmsg) {
  if (!errmsg.empty()) {
    cout << "Error: " << errmsg << endl << endl;
  }
  cout << "USAGE:" << endl;
  cout << "    " << name << " rankings_file" << endl;
}

bool parseArgs(int argc, char** argv, Params& params) {
  po::options_description desc("Allowed options");
  desc.add_options()
    ("help,h", "Show help message and exit")
    ("ranking_file,r", po::value<string>(), "Rankings CSV file")
    ("results_dir", po::value<string>()->default_value("results"), "Directory for saving results")
    ("iterations,i", po::value<u64>()->default_value(100000), "Number of iterations")
    ("init_temp", po::value<double>()->default_value(10.0), "Initial temperature")
    ("final_temp", po::value<double>()->default_value(0.00001), "Final temperature")
    ("timeslots", po::value<int>()->default_value(18), "Number of timeslots")
    ("rooms", po::value<int>()->default_value(9), "Number of rooms")
    ("room_size", po::value<int>()->default_value(12), "Room capacity including speaker")
    ("person_id_col", po::value<string>()->default_value("person_id"), "Name of person_id column")
    ("abstract_id_col", po::value<string>()->default_value("abstract_id"), "Name of abstract_id column")
    ("score_col", po::value<string>()->default_value("rating"), "Name of score column")
    ("input_delimiter", po::value<string>()->default_value(","), "Delimiter character of input file")
    ("default_score", po::value<Score>()->default_value(0), "Value of empty score")
    ("max_score", po::value<Score>()->default_value(5), "Minimum value for single score")
    ("min_score", po::value<Score>()->default_value(0), "Maximum value for single score")
    ("score_delta", po::value<Score>()->default_value(1), "Delta added per score (to avoid 0 score)")
    ("participation_range", po::value<u32>()->default_value(2), "Allowed deviation from mean number of participations per person")
    ("max_presentations", po::value<u32>()->default_value(3), "Max number of presentations per abstract")
    ("seed", po::value<int>(), "Algorithm random seed (for debugging)")
    ("verbose,v", "Verbose mode (for debugging)")
    ;

  try {
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);
    if (vm.count("help")) {
      cout << desc << "\n";
      return false;
    }
    setVerboseMode(vm.count("verbose") > 0);
    params.nTimeslots = vm["timeslots"].as<int>();
    params.nRooms = vm["rooms"].as<int>();
    params.roomSize = vm["room_size"].as<int>();
    params.maxIterations = vm["iterations"].as<u64>();
    params.initTemp = vm["init_temp"].as<double>();
    params.finalTemp = vm["final_temp"].as<double>();
    params.resultsDir = vm["results_dir"].as<string>();
    params.personIdCol = vm["person_id_col"].as<string>();
    params.abstractIdCol = vm["abstract_id_col"].as<string>();
    params.scoreCol = vm["score_col"].as<string>();
    params.defaultScore = vm["default_score"].as<Score>();
    params.maxScore = vm["max_score"].as<Score>();
    params.minScore = vm["min_score"].as<Score>();
    params.scoreDelta = vm["score_delta"].as<Score>();
    params.participationRange = vm["participation_range"].as<u32>();
    params.maxPresentations = vm["max_presentations"].as<u32>();
    params.maxNormScore = params.scoreDelta + params.maxScore;
    params.minNormScore = params.scoreDelta + params.minScore;
    string strDelim = vm["input_delimiter"].as<string>();
    if (strDelim.size() != 1) {
      err() << "input_delimiter should be a single character. Got: " << strDelim.size() << endl;
      return false;
    }
    params.inputDelimiter = strDelim[0];
    if (vm.count("seed")) {
      params.seed = vm["seed"].as<int>();
    } else {
      std::random_device rd;
      params.seed = rd();
    }

    if (!readRankings(vm["ranking_file"].as<string>(), params))
      return false;

    params.avgParticipations = round(double(params.nTimeslots * params.nRooms * (params.roomSize - 1)) / params.nPeople);
    params.minParticipations = ceil(params.avgParticipations - params.participationRange);
    params.maxParticipations = floor(params.avgParticipations + params.participationRange);

  } catch(po::error& e) {
    cout << "Error parsing command line: " << e.what();
    return false;
  }

  return true;
}

Score maxPotentialScore(const Rankings& rankings) {
  Score sumScore = 0;
  for (auto const& x : rankings)
    sumScore += x;
  return sumScore;
}

class SimAnnealing {
public:
  SimAnnealing(Schedule& sched, const Params& params, Scorer& scorer) :
    m_sched(sched), m_scorer(scorer), m_params(params), m_iter(0),
    m_timeslotCapacity(m_params.nRooms * m_params.roomSize), m_bestSched(sched) {}

  bool run() {
    m_startTime = chrono::system_clock::now();
    Score maxScore = maxPotentialScore(m_params.rankings);
    m_bestScore = 0;
    dbg() << "maxScore: " << maxScore << endl;
    s32 nextOutputSec = 0;
    m_temperature = m_params.initTemp;
    for (m_iter = 0; m_iter < m_params.maxIterations; ++m_iter) {
      if (m_iter % 10000 == 0) {
        double tempRatio = m_params.finalTemp / m_params.initTemp;
        m_temperature = m_params.initTemp *
          (exp(std::log(tempRatio) * (double(m_iter) / m_params.maxIterations)));
        if (elapsedSecs(m_startTime) >= nextOutputSec) {
          if (!outputStatus(dbg()))
            return false;
          ++nextOutputSec;
        }
      }
      try {
        oneIteration();
        if (m_scorer.score() > m_bestScore) {
          if (!handleNewBest())
            return false;
          m_bestScore = m_scorer.score();
        }
      } catch(std::exception& e) {
        cout << "Error in iter " << m_iter << ": " << e.what();
        return false;
      }
    }
    outputStatus(info());
    return true;
  }

  void outputSchedSummary(ostream& s) {
    s << endl;
    for (s32 t = 0; t < m_params.nTimeslots; ++t) {
      Score tScore = 0;
      s << setw(2) << (t + 1) << " || ";
      for (s32 r = 0; r < m_params.nRooms; ++r) {
        Score rScore = m_scorer.calcRoomScore(t, r);
        s << setw(3) << m_sched.getAbstractID(t, r) << " " << setw(4) << rScore << " | ";
        tScore += rScore;
      }
      s << tScore << endl;
    }
  }

  void outputSchedStats(ostream& s, const Schedule& sched) {
    // nPeople per number of rated abstracts they got
    // Score quantiles
    vector<s32> personParticipations(m_params.nPeople, 0);
    vector<s32> ratedAbstractsGotPerPerson(m_params.nPeople, 0);
    vector<s32> abstractPresentations(m_params.nAbstracts, 0);
    vector<s32> ratingsGotPerAbstract(m_params.nAbstracts, 0);
    for (s32 t = 0; t < m_params.nTimeslots; ++t) {
      for (s32 r = 0; r < m_params.nRooms; ++r) {
        ID abstractID = sched.getAbstractID(t, r);
        ++abstractPresentations[abstractID];
        for (s32 s = 1; s < m_params.roomSize; ++s) {
          ID personID = sched.getID(t, r, s);
          ++personParticipations[personID];
          if (getRankingOrig(personID, abstractID, m_params) > 0) {
            ++ratedAbstractsGotPerPerson[personID];
            ++ratingsGotPerAbstract[abstractID];
          }
        }
      }
    }
    vector<s32> ratedAbstractsPerPerson(m_params.nPeople, 0);
    vector<s32> ratingsPerAbstract(m_params.nAbstracts, 0);
    for (ID personID = 0; personID < m_params.nPeople; ++personID) {
      for (ID abstractID = 0; abstractID < m_params.nAbstracts; ++abstractID) {
        if (getRankingOrig(personID, abstractID, m_params) > 0) {
          ++ratedAbstractsPerPerson[personID];
          ++ratingsPerAbstract[abstractID];
        }
      }
    }
    s32 unmatchableAbstracts = 0, unmatchablePeople = 0;
    vector<s32> ratedGotPercentPerPerson(102, 0);
    vector<s32> ratedGotPercentOfMaxPerPerson(102, 0);
    for (ID personID = 0; personID < m_params.nPeople; ++personID) {
      s32 percent = (ratedAbstractsPerPerson[personID] == 0) ? 101 : (
        (20 * ratedAbstractsGotPerPerson[personID]) / ratedAbstractsPerPerson[personID]) * 5;
      s32 percentOfMax = (ratedAbstractsPerPerson[personID] == 0) ? 101 : (
        (20 * ratedAbstractsGotPerPerson[personID]) /
        min(ratedAbstractsPerPerson[personID], s32(m_params.maxParticipations))) * 5;
      ++ratedGotPercentPerPerson[percent];
      ++ratedGotPercentOfMaxPerPerson[percentOfMax];
      unmatchablePeople += max(0, s32(m_params.minParticipations) - ratedAbstractsPerPerson[personID]);
    }
    vector<s32> ratedGotPercentPerAbstract(102, 0);
    for (ID abstractID = 0; abstractID < m_params.nAbstracts; ++abstractID) {
      s32 percent = (ratingsPerAbstract[abstractID] == 0) ? 101 : (
        (20 * ratingsGotPerAbstract[abstractID]) / ratingsPerAbstract[abstractID]) * 5;
      ++ratedGotPercentPerAbstract[percent];
      unmatchableAbstracts += max(0, m_params.roomSize - 1 - ratingsPerAbstract[abstractID]);
    }

    vector<s32> nPeoplePerNParticipations = vectorCount(personParticipations);
    vector<s32> nPeoplePerNRatedAbstractsGot = vectorCount(ratedAbstractsGotPerPerson);
    vector<s32> nPeoplePerNRatedAbstracts = vectorCount(ratedAbstractsPerPerson);
    vector<s32> nAbstractsPerNPresentations = vectorCount(abstractPresentations);
    vector<s32> nAbstractsPerNRatingsGot = vectorCount(ratingsGotPerAbstract);
    vector<s32> nAbstractsPerNRatings = vectorCount(ratingsPerAbstract);
    s32 totalParticipations = m_params.nTimeslots * m_params.nRooms * (m_params.roomSize-1);

    s << "nPeople:" << m_params.nPeople;
    s << " nAbstracts:" << m_params.nAbstracts;
    s << " nRooms:" << m_params.nRooms;
    s << " nTimeslots:" << m_params.nTimeslots;
    s << " Room size:" << m_params.roomSize << endl;
    s << "nAbstracts ratings:" << vectorSum(ratedAbstractsPerPerson) << endl;
    s << "nAbstracts rated and got:" << vectorSum(ratedAbstractsGotPerPerson) << endl;
    s << "Total participations (excl. presenters): " << totalParticipations << endl;
    s << "Total forced unrated participations (people with less than " << m_params.minParticipations
      << " rankings): " << unmatchablePeople << " (max matches: "
      << (totalParticipations - unmatchablePeople) << ")" << endl;
    s << "Total forced unrated participations (abstracts with less than " << (m_params.roomSize - 1)
      << " rankings): " << unmatchableAbstracts << " (max matches: "
      << (totalParticipations - unmatchableAbstracts) << ")" << endl;
    s << "nPeople per number of participations:";
    outputVectorCount(s, nPeoplePerNParticipations) << endl;
    s << "nPeople per abstracts rated:";
    outputVectorCount(s, nPeoplePerNRatedAbstracts) << endl;
    s << "nPeople per abstracts rated got:";
    outputVectorCount(s, nPeoplePerNRatedAbstractsGot) << endl;
    s << "nAbstracts per number of presentations:";
    outputVectorCount(s, nAbstractsPerNPresentations) << endl;
    s << "nAbstracts per times rated:";
    outputVectorCount(s, nAbstractsPerNRatings) << endl;
    s << "nAbstracts per times rated and got:";
    outputVectorCount(s, nAbstractsPerNRatingsGot) << endl;
    s << "nAbstracts per ratings got percent: N/A:" << ratedGotPercentPerAbstract[101];
    ratedGotPercentPerAbstract[101] = 0;
    outputVectorCount(s, ratedGotPercentPerAbstract, "%") << endl;
    s << "nPeople per ratings got percent: N/A:" << ratedGotPercentPerPerson[101];
    ratedGotPercentPerPerson[101] = 0;
    outputVectorCount(s, ratedGotPercentPerPerson, "%") << endl;
    s << "nPeople per ratings got of their max percent: N/A:" << ratedGotPercentOfMaxPerPerson[101];
    ratedGotPercentOfMaxPerPerson[101] = 0;
    outputVectorCount(s, ratedGotPercentOfMaxPerPerson, "%") << endl;
  }

  const Schedule& bestSchedule() {
    m_bestSched.setAllIDs(m_bestSchedule);
    return m_bestSched;
  }

  const Schedule& curSchedule() {
    return m_sched;
  }

protected:
  Schedule& m_sched;
  Scorer& m_scorer;
  const Params m_params;
  u64 m_iter;
  const s32 m_timeslotCapacity;
  double m_temperature;
  Score m_bestScore;
  vector<ID> m_bestSchedule;
  Schedule m_bestSched;
  time_point m_startTime;

  std::string inResultsDir(std::string name) {
    return (boost::filesystem::path(m_params.resultsDir) / name).c_str();
  }

  bool handleNewBest() {
    m_sched.getAllIDs(m_bestSchedule);
    return true;

  }

  bool saveBest() {
    if (m_bestSchedule.empty())
      return true;
    string schedPath = inResultsDir("best_schedule.csv");
    ofstream schedFile(schedPath);
    if (schedFile.bad() || schedFile.fail()) {
      err() << "Error opening file '" << schedPath << "': " << strerror(errno) << endl;
      return false;
    }
    m_sched.outputIDs(schedFile, m_bestSchedule);

    string metadataPath = inResultsDir("best_schedule.metadata");
    ofstream metadataFile(metadataPath);
    if (metadataFile.bad() || metadataFile.fail()) {
      err() << "Error opening file '" << metadataPath << "': " << strerror(errno) << endl;
      return false;
    }
    metadataFile << "Score: " << m_scorer.score() << endl;
    metadataFile << "Iter: " << m_iter << endl;
    metadataFile << "Temperature: " << m_temperature << endl;
    metadataFile << "Elapsed seconds: " << elapsedSecs(m_startTime) << endl;
    outputParams(m_params, metadataFile);
    outputSchedSummary(metadataFile);
    outputSchedStats(metadataFile, bestSchedule());
    return true;
  }

  bool oneIteration() {
    Score curScore = m_scorer.score();
    s32 t = randInt(m_params.nTimeslots);
    s32 i1 = randInt(m_params.nPeople), i2 = randInt(m_params.nPeople);
    if (i2 < i1)
      swap(i1, i2);
    if (i1 >= m_timeslotCapacity || i1 == i2)
      return false;
    s32 room1 = i1 / m_params.roomSize;
    s32 seat1 = i1 % m_params.roomSize;
    ID id1 = m_sched.getID(t, room1, seat1);
    if (i2 < m_timeslotCapacity) { // Change type 1: swap two seats in time slot
      s32 room2 = i2 / m_params.roomSize;
      s32 seat2 = i2 % m_params.roomSize;
      ID id2 = m_sched.getID(t, room2, seat2);
      if (room1 == room2 && seat1 > 0 && seat2 > 0)
        return false;
      if ((seat1 == 0 && id2 >= m_params.nAbstracts) ||
          (seat2 == 0 && id1 >= m_params.nAbstracts)) {
        return false;
      }
      m_scorer.prepareSwapChange(t, room1, seat1, t, room2, seat2);
      if (!swapIfLegal(t, room1, seat1, room2, seat2)) {
        ASSERT(m_scorer.score() == curScore);
        return false;
      }
      m_scorer.tryChange();
      // m_scorer.recalcScore();
      Score newScore = m_scorer.score();
      if (!shouldAcceptStep(curScore, newScore, m_temperature)) {
        m_sched.setIDUnsafe(t, room2, seat2, INVALID_ID);
        m_sched.setIDUnsafe(t, room1, seat1, id1);
        m_sched.setIDUnsafe(t, room2, seat2, id2);
        m_scorer.undoChange();
        // m_scorer.recalcScore();
        ASSERT(abs(m_scorer.score() - curScore) < (m_params.minNormScore / 1000));
      }
    }
    else {  // Change type 2: Swap seat with a free person in time slot
      ID id2 = m_sched.getRandomFreePerson(t);
      if (seat1 == 0 && id2 >= m_params.nAbstracts) {
        return false;
      }
      m_scorer.prepareSetChange(t, room1, seat1, id2);
      if (!m_sched.setIDIfLegal(t, room1, seat1, id2))
        return false;
      m_scorer.tryChange();
      Score newScore = m_scorer.score();
      if (!shouldAcceptStep(curScore, newScore, m_temperature)) {
        m_sched.setIDUnsafe(t, room1, seat1, id1);
        m_scorer.undoChange();
        // m_scorer.recalcScore();
        ASSERT(abs(m_scorer.score() - curScore) < (m_params.minNormScore / 1000));
      }
    }
    return true;
  }

  bool swapIfLegal(s32 timeslot, s32 room1, s32 seat1, s32 room2, s32 seat2) {
    ID id1 = m_sched.getID(timeslot, room1, seat1);
    ID id2 = m_sched.getID(timeslot, room2, seat2);
    if (!m_sched.setIDIfLegal(timeslot, room2, seat2, INVALID_ID))
      return false;
    if (!m_sched.setIDIfLegal(timeslot, room1, seat1, id2)) {
      m_sched.setIDUnsafe(timeslot, room2, seat2, id2);
      return false;
    }
    if (!m_sched.setIDIfLegal(timeslot, room2, seat2, id1)) {
      m_sched.setIDUnsafe(timeslot, room1, seat1, id1);
      m_sched.setIDUnsafe(timeslot, room2, seat2, id2);
      return false;
    }
    return true;
  }

  vector<s32> vectorCount(const vector<s32>& data) {
    vector<s32> count(*max_element(begin(data), end(data)) + 1, 0);
    for (s32 d : data) {
      ++count[d];
    }
    return count;
  }

  s32 vectorSum(const vector<s32>& data) {
    s32 sum = 0;
    for (s32 d : data)
      sum += d;
    return sum;
  }

  ostream& outputVectorCount(ostream& s, const vector<s32>& v, string countSuffix="") {
    for (s32 i = 0; i < v.size(); ++i) {
      if (v[i] > 0)
        s << " " << i << countSuffix << ":" << v[i];
    }
    return s;
  }

  bool outputStatus(ostream& s) {
    s << "Iter " << double(m_iter) << "/" << double(m_params.maxIterations)
      << " (" << setprecision(4)
      << left << (100.0 * m_iter / m_params.maxIterations) << right << "%) temperature: "
      << m_temperature << " score: " << m_scorer.score() << " (dbg:" << m_scorer.calcScore()
      << ") best so far:" << m_bestScore << endl;
    //outputSchedSummary(s << endl);
    ASSERT(abs(m_scorer.score() - m_scorer.calcScore()) < (m_params.minNormScore / 1000));
    return saveBest();
  }

  bool shouldAcceptStep(Score curScore, Score newScore, double temperature) {
    if (newScore >= curScore)
      return true;
    double normDelta = double(newScore - curScore);
    return randProb() < exp(normDelta / temperature);
  }
};

void findSchedule(const Params& params) {
    outputParams(params, info());
    dbg() << "Creating empty schedule" << endl;
    Schedule sched = Schedule(params);
    dbg() << "Initializing schedule" << endl;
    sched.initState();
    dbg() << "Initializing scorer" << endl;
    SumHappinessScorer scorer(sched, params);

    dbg() << "Initializing algorithm" << endl;
    SimAnnealing sa(sched, params, scorer);

    dbg() << "Stats:" << endl;
    auto& s = dbg();
    sa.outputSchedStats(s, sa.curSchedule());
    sa.outputSchedSummary(s);
    s << "Score:" << scorer.score() << endl;

    dbg() << "Optimizing schedule" << endl;
    sa.run();
//    auto& s = dbg();
    sa.outputSchedSummary(s);

    SumHappinessScorer scorer2(sched, params);
    MinHappinessBonusScorer minScorer(sched, params);

    sa.outputSchedStats(s, sa.bestSchedule());
    s << "Score:" << scorer.score() << endl;

    SumScorers sumScorers(scorer2, minScorer);
    SimAnnealing sa2(sched, params, sumScorers);
    sa2.run();
    sa.outputSchedSummary(s);
    MinHappinessBonusScorer scorer3(sched, params);
    sa.outputSchedStats(s, sa.bestSchedule());
    dbg() << "min person ID: " << minScorer.calcMinPersonScoreID() << endl;
    s << "Score:" << SumHappinessScorer(sched, params).score() << endl;
}

int main(int argc, char** argv) {
  Params params;
  if (!parseArgs(argc, argv, params))
    return 2;
  randSetSeed(params.seed);
  try {
    findSchedule(params);
  } catch (const std::exception& e) {
    err() << e.what() << '\n';
  }
  return 0;
}


// Copyright 2020 Oscar Higgott

// 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 <iostream>
#include "lemon_mwpm.h"
#include <lemon/list_graph.h>
#include <lemon/matching.h>
#include <lemon/connectivity.h>
#include <vector>
#include <string>
#include "matching_graph.h"
#include <stdexcept>
#include <set>
#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>
#include <boost/graph/adjacency_list.hpp>

typedef lemon::ListGraph UGraph;
typedef UGraph::EdgeMap<double> LengthMap;
typedef lemon::MaxWeightedPerfectMatching<UGraph,LengthMap> MWPM;

const char * BlossomFailureException::what() const throw() {
    return "The Lemon implementation of the blossom algorithm "
            "(lemon::MaxWeightedPerfectMatching) "
            "was unable to find a solution to the minimum-weight "
            "perfect matching problem.";
}

MatchingResult::MatchingResult() {}

MatchingResult::MatchingResult(py::array_t<std::uint8_t> correction, double weight)
    : correction(correction), weight(weight) {}

std::string arr_repr(py::array_t<std::uint8_t> arr) {
    std::stringstream ss;
    ss << "array([";
    bool first = true;
    for (auto i : arr) {
        if (first){
            first = false;
        } else {
            ss << ", ";
        }
        ss << i;
    }
    ss << "], dtype=uint8)";
    return ss.str();
}

std::string MatchingResult::repr() const {
    std::stringstream ss;
    ss << "pymatching._cpp_mwpm.MatchingResult(correction=";
    ss << arr_repr(correction);
    ss << ", weight=" << weight << ")";
    return ss.str();
}

class DefectGraph {
    public:
        DefectGraph(int num_nodes);
        void AddEdge(int i, int j, double weight);
        UGraph g;
        LengthMap length;
        int num_nodes;
};

DefectGraph::DefectGraph(int num_nodes) : num_nodes(num_nodes),
         length(g)
{
    for (int i=0; i<num_nodes; i++){
        UGraph::Node x;
        x = g.addNode();
    }
}

void DefectGraph::AddEdge(int i, int j, double weight){
    UGraph::Edge e = g.addEdge(g.nodeFromId(i), g.nodeFromId(j));
    length[e] = weight;
}

MatchingResult ExactMatching(
    MatchingGraph& graph,
    const py::array_t<int>& defects,
    bool return_weight
    ){
    MatchingResult matching_result;
    if (!graph.HasComputedAllPairsShortestPaths()){
        graph.ComputeAllPairsShortestPaths();
    }
    int num_nodes = graph.GetNumNodes();

    auto d = defects.unchecked<1>();
    std::set<int> defects_set;
    for (int i=0; i<d.shape(0); i++){
        if (d(i) >= num_nodes){
            throw std::invalid_argument(
            "Defect id must be less than the number of nodes in the matching graph"
            );
        }
        defects_set.insert(d(i));
    }
    graph.FlipBoundaryNodesIfNeeded(defects_set);

    std::vector<int> defects_vec(defects_set.begin(), defects_set.end());

    int num_defects = defects_vec.size();

    DefectGraph defect_graph(num_defects);

    for (py::size_t i = 0; i<num_defects; i++){
        for (py::size_t j=i+1; j<num_defects; j++){
            defect_graph.AddEdge(i, j, -1.0*graph.Distance(
                defects_vec[i], defects_vec[j]
                ));
        }
    };

    MWPM pm(defect_graph.g, defect_graph.length);
    bool success = pm.run();
    if (!success){
        throw BlossomFailureException();
    }

    int N = graph.GetNumQubits();
    auto correction = new std::vector<int>(N, 0);
    std::set<int> qids;
    for (py::size_t i = 0; i<num_defects; i++){
        int j = defect_graph.g.id(pm.mate(defect_graph.g.nodeFromId(i)));
        if (i<j){
            std::vector<int> path = graph.ShortestPath(
                defects_vec[i], defects_vec[j]
                );
            for (std::vector<int>::size_type k=0; k<path.size()-1; k++){
                qids = graph.QubitIDs(path[k], path[k+1]);
                for (auto qid : qids){
                    if ((qid != -1) && (qid >= 0) && (qid < N)){
                        (*correction)[qid] = ((*correction)[qid] + 1) % 2;
                    }
                }
            }
        }
    }

    auto capsule = py::capsule(correction, [](void *correction) { delete reinterpret_cast<std::vector<int>*>(correction); });
    auto corr = py::array_t<int>(correction->size(), correction->data(), capsule);

    if (return_weight) {
        matching_result.weight = -1*pm.matchingWeight();
    } else {
        matching_result.weight = -1.0;
    }

    matching_result.correction = corr;
    return matching_result;
}

MatchingResult LocalMatching(
    MatchingGraph& graph,
    const py::array_t<int>& defects,
    int num_neighbours,
    bool return_weight,
    int max_attempts
    ){
    if (num_neighbours <= 0){
        throw std::invalid_argument("num_neighbours must be greater than zero");
    }
    auto d = defects.unchecked<1>();
    std::set<int> defects_set;
    for (int i=0; i<d.shape(0); i++) {
        defects_set.insert(d(i));
    }
    int num_attempts = 0;
    while (true) {
        try{
            return LemonDecodeMatchNeighbourhood(
                graph,
                defects_set,
                num_neighbours,
                return_weight
            );
        } catch (BlossomFailureException& e) {
            num_attempts++;
            if (num_neighbours >= defects_set.size() || num_attempts >= max_attempts){
                throw;
            } else {
                num_neighbours *= 2;
            }
        }
    }
}

MatchingResult LemonDecodeMatchNeighbourhood(
    MatchingGraph& graph,
    std::set<int>& defects_set,
    int num_neighbours,
    bool return_weight
    ){
    MatchingResult matching_result;

    int num_nodes = graph.GetNumNodes();

    for (auto d : defects_set){
        if (d >= num_nodes){
            throw std::invalid_argument(
            "Defect id must be less than the number of nodes in the matching graph"
            );
        }
    }

    graph.FlipBoundaryNodesIfNeeded(defects_set);

    std::vector<int> defects_vec(defects_set.begin(), defects_set.end());
    int num_defects = defects_vec.size();
    std::vector<int> defect_id(num_nodes, -1);
    for (int i=0; i<num_defects; i++){
        defect_id[defects_vec[i]] = i;
    }
    num_neighbours = std::min(num_neighbours, num_defects-1);

    DefectGraph defect_graph(num_defects);

    std::vector<std::pair<int, double>> neighbours;
    int j;
    bool is_in;
    for (int i=0; i<num_defects; i++){
        neighbours = graph.GetNearestNeighbours(defects_vec[i], num_neighbours, defect_id);
        for (const auto &neighbour : neighbours){
            j = defect_id[neighbour.first];
            UGraph::Edge FoundEdge = lemon::findEdge(
                defect_graph.g,
                defect_graph.g.nodeFromId(i),
                defect_graph.g.nodeFromId(j));
            is_in = FoundEdge != lemon::INVALID;
            if (!is_in && i!=j){
                defect_graph.AddEdge(i, j, -1.0*neighbour.second);
            }
        }
    }

    MWPM pm(defect_graph.g, defect_graph.length);
    bool success = pm.run();
    if (!success){
        throw BlossomFailureException();
    }

    int N = graph.GetNumQubits();
    auto correction = new std::vector<int>(N, 0);

    std::set<int> remaining_defects;
    for (int i=0; i<num_defects; i++){
        remaining_defects.insert(i);
    }

    std::vector<int> path;
    int i;
    std::set<int> qids;
    while (remaining_defects.size() > 0){
        i = *remaining_defects.begin();
        remaining_defects.erase(remaining_defects.begin());
        j = defect_graph.g.id(pm.mate(defect_graph.g.nodeFromId(i)));
        remaining_defects.erase(j);
        path = graph.GetPath(defects_vec[i], defects_vec[j]);
        for (std::vector<int>::size_type k=0; k<path.size()-1; k++){
            qids = graph.QubitIDs(path[k], path[k+1]);
            for (auto qid : qids){
                if ((qid != -1) && (qid >= 0) && (qid < N)){
                    (*correction)[qid] = ((*correction)[qid] + 1) % 2;
                }
            }
        }
    }
    auto capsule = py::capsule(correction, [](void *correction) { delete reinterpret_cast<std::vector<int>*>(correction); });
    auto corr = py::array_t<int>(correction->size(), correction->data(), capsule);

    if (return_weight) {
        matching_result.weight = -1*pm.matchingWeight();
    } else {
        matching_result.weight = -1.0;
    }

    matching_result.correction = corr;
    return matching_result;
}


#ifndef N_BODY_TREE_HPP
#define N_BODY_TREE_HPP

#include "data.hpp"
#include "logging.hpp"
#include "overloaded.hpp"
#include "space.hpp"
#include <array>
#include <boost/mpi.hpp>
#include <boost/optional.hpp>
#include <boost/serialization/access.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/optional.hpp>
#include <boost/serialization/variant.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/variant.hpp>
#include <cmath>
#include <cstddef>
#include <iostream>
#include <memory>
#include <tuple>
#include <vector>

namespace n_body::data::tree {

namespace detail {

constexpr std::size_t children_number(std::size_t dimension) noexcept {
  // if exception occurs, process will be terminated
  return static_cast<std::size_t>(std::pow(2, dimension));
}

} // namespace detail

enum class NodeType {
  Inner = 0,
  Leaf = 1,
};

std::ostream &operator<<(std::ostream &os, NodeType node_type) {
  switch (node_type) {
  case NodeType::Inner:
    return os << "Inner";
  case NodeType::Leaf:
    return os << "Leaf";
  default:
    return os << "Invalid node type";
  }
}

template <typename T, std::size_t Dimension> struct BodyTreeInnerNode {
  inline static constexpr std::size_t CHILDREN_NUMBER =
      detail::children_number(Dimension);

  using space_type = Space<T, Dimension>;

  std::array<boost::optional<std::size_t>, CHILDREN_NUMBER> children;

  /* serialization */
  friend class boost::serialization::access;
  template <typename Archive>
  void serialize(Archive &ar, const unsigned int /* version */) {
    ar &BOOST_SERIALIZATION_NVP(children);
  }
};

template <typename T, std::size_t Dimension> struct BodyTreeLeafNode {
  using space_type = Space<T, Dimension>;

  std::size_t body;

private:
  /* serialization */
  friend class boost::serialization::access;
  template <typename Archive>
  void serialize(Archive &ar, const unsigned int /* version */) {
    ar &BOOST_SERIALIZATION_NVP(body);
  }
};

template <typename T, std::size_t Dimension> struct BodyTreeNode {
  using space_type = Space<T, Dimension>;
  using inner_node_type = BodyTreeInnerNode<T, Dimension>;
  using leaf_node_type = BodyTreeLeafNode<T, Dimension>;

  Space<T, Dimension> space;
  Scalar<T> mass;
  Vector<T, Dimension> center_of_mass;
  boost::variant<inner_node_type, leaf_node_type> variant_part;

  NodeType node_type() const {
    return static_cast<NodeType>(variant_part.which());
  }

private:
  /* serialization */
  friend class boost::serialization::access;
  template <typename Archive>
  void serialize(Archive &ar, const unsigned int /* version */) {
    ar &BOOST_SERIALIZATION_NVP(space);
    ar &BOOST_SERIALIZATION_NVP(mass);
    ar &BOOST_SERIALIZATION_NVP(center_of_mass);
    ar &BOOST_SERIALIZATION_NVP(variant_part);
  }
};

template <typename T, std::size_t Dimension> struct BodyTree {
  using node_type = BodyTreeNode<T, Dimension>;
  using inner_node_type = typename node_type::inner_node_type;
  using leaf_node_type = typename node_type::leaf_node_type;

  using bodies_type = Bodies<T, Dimension>;
  using space_type = Space<T, Dimension>;

  std::vector<node_type> tree;

  void push(const bodies_type &bodies, const space_type &root_space,
            std::size_t body) {

    if (this->tree.empty()) {
      this->push_new_leaf_node(bodies, body, root_space);
    } else {
      // push to the root subtree
      this->push(0, bodies, body);
    }
  }

  void push(std::size_t subtree, const bodies_type &bodies, std::size_t body) {
    switch (this->node(subtree).node_type()) {
    case NodeType::Inner: {
      logging::logger(logging::Level::Trace)
          << "push body " << body << " to "
          << "inner node " << subtree << std::endl;
      data::average_position_by_mass_in_place(
          this->node(subtree).center_of_mass, this->node(subtree).mass,
          bodies[body].position, bodies[body].mass);

      auto body_part = space::part_of_space(this->node(subtree).space,
                                            bodies[body].position);
      if (auto next = this->child_of_node(subtree, body_part)) {
        // just push into the node
        push(*next, bodies, body);
      } else {
        // just use the place
        // root_inner is invalidated by push
        this->child_of_node(subtree, body_part) = this->push_new_leaf_node(
            bodies, body,
            space::subspace(this->node(subtree).space, body_part));
      }
      break;
    }
    case NodeType::Leaf: {
      logging::logger(logging::Level::Trace)
          << "push body " << body << " to "
          << "leaf node " << subtree << std::endl;
      if (this->node(subtree).center_of_mass == bodies[body].position) {
        throw std::runtime_error("two points are at exactly same point");
      }

      // change the leaf to an inner node
      this->expand_leaf_to_inner(subtree);
      // try insert again
      this->push(subtree, bodies, body);
      break;
    }
    }
  }

  void expand_leaf_to_inner(std::size_t leaf) {

    auto part = space::part_of_space(this->node(leaf).space,
                                     this->node(leaf).center_of_mass);
    auto new_leaf_node = tree.size();

    this->tree.push_back(node_type{
        space::subspace(this->node(leaf).space, part),
        this->node(leaf).mass,
        this->node(leaf).center_of_mass,
        this->node(leaf).variant_part,
    }); // node is invalidated
    inner_node_type inner_node;
    inner_node.children[part] = new_leaf_node;
    this->variant_part_of_node(leaf) =
        inner_node; // finally change the leaf into inner

    logging::logger(logging::Level::Trace)
        << "expand leaf " << leaf << " to " << new_leaf_node << std::endl;
  }

  void merge_tree(const BodyTree<T, Dimension> &other,
                  const Bodies<T, Dimension> &bodies) {
    if (this->tree.empty()) {
      this->tree = other.tree;
    } else if (other.tree.empty()) {
      return;
    } else {
      this->merge_tree(0, other, 0, bodies);
    }
  }

  void merge_tree(std::size_t root, const BodyTree<T, Dimension> &other,
                  std::size_t other_root, const Bodies<T, Dimension> &bodies) {
    if (other.tree[other_root].node_type() == NodeType::Leaf) {
      // if merging a leaf node
      // just push the body into the place and return
      this->push(root, bodies, other.body_of_node(other_root));
    } else {
      // if merging a inner node
      if (this->tree[root].node_type() == NodeType::Inner) {
        // if current node type is inner
        data::average_position_by_mass_in_place(
            this->tree[root].center_of_mass, this->tree[root].mass,
            other.tree[other_root].center_of_mass, other.tree[other_root].mass);

        for (std::size_t i = 0; i < inner_node_type::CHILDREN_NUMBER; ++i) {
          if (auto other_child = other.child_of_node(other_root, i)) {
            // if and only if other node's child exists
            if (auto this_child = this->child_of_node(root, i)) {
              this->merge_tree(*this_child, other, *other_child, bodies);
            } else {
              this->child_of_node(root, i) =
                  this->copy_tree(other, *other_child);
            }
          }
        }
      } else {
        // if current node type is leaf
        auto body = this->body_of_node(root);
        copy_tree_in_place(root, other, other_root);
        this->push(root, bodies, body);
      }
    }
  }

  // copy subtree to the back of the tree vector
  std::size_t copy_tree(const BodyTree<T, Dimension> &other,
                        std::size_t other_root) {
    auto place = this->tree.size();
    this->tree.push_back(other.node(other_root)); // do copy
    if (other.node(other_root).node_type() == NodeType::Inner) {
      for (std::size_t i = 0; i < inner_node_type::CHILDREN_NUMBER; ++i) {
        if (auto child = other.child_of_node(other_root, i)) {
          this->child_of_node(place, i) = this->copy_tree(other, *child);
        }
      }
    }
    return place;
  }

  // copy subtree into the place of tree vector
  void copy_tree_in_place(std::size_t place,
                          const BodyTree<T, Dimension> &other,
                          std::size_t other_root) {
    this->node(place) = other.node(other_root); // do copy
    if (other.node(other_root).node_type() == NodeType::Inner) {
      for (std::size_t i = 0; i < inner_node_type::CHILDREN_NUMBER; ++i) {
        if (auto child = other.child_of_node(other_root, i)) {
          this->child_of_node(place, i) = this->copy_tree(other, *child);
        }
      }
    }
  }

  // access methods
  boost::variant<inner_node_type, leaf_node_type> &
  variant_part_of_node(std::size_t node) {
    return this->tree[node].variant_part;
  }

  const boost::variant<inner_node_type, leaf_node_type> &
  variant_part_of_node(std::size_t node) const {
    return this->tree[node].variant_part;
  }

  boost::optional<std::size_t> &child_of_node(std::size_t node,
                                              std::size_t part) {
    return boost::get<inner_node_type>(this->tree[node].variant_part)
        .children[part];
  }

  const boost::optional<std::size_t> &child_of_node(std::size_t node,
                                                    std::size_t part) const {
    return boost::get<inner_node_type>(this->tree[node].variant_part)
        .children[part];
  }

  std::size_t &body_of_node(std::size_t node) {
    return boost::get<leaf_node_type>(this->tree[node].variant_part).body;
  }

  std::size_t body_of_node(std::size_t node) const {
    return boost::get<leaf_node_type>(this->tree[node].variant_part).body;
  }

  node_type &node(std::size_t node) { return this->tree[node]; }

  const node_type &node(std::size_t node) const { return this->tree[node]; }

private:
  std::size_t push_new_leaf_node(const bodies_type &bodies, std::size_t body,
                                 const Space<T, Dimension> &space) {
    auto new_node = tree.size();
    this->tree.push_back(node_type{
        space,                 // space
        bodies[body].mass,     // mass
        bodies[body].position, // center of mass
        leaf_node_type{
            body, // body
        },
    });
    logging::logger(logging::Level::Trace)
        << "create new leaf node " << new_node << " for body " << body
        << std::endl;
    return new_node;
  }

private:
  /* serialization */
  friend class boost::serialization::access;
  template <typename Archive>
  void serialize(Archive &ar, const unsigned int version) {
    ar &BOOST_SERIALIZATION_NVP(tree);
  }
};

// the root space fo t1 and t2 must be same
template <typename T, std::size_t Dimension>
BodyTree<T, Dimension> merge_tree(const BodyTree<T, Dimension> &t1,
                                  const BodyTree<T, Dimension> &t2,
                                  const Bodies<T, Dimension> &bodies) {
  BodyTree<T, Dimension> result;
  if (t1.tree.empty()) {
    result.tree = t2.tree;
  } else if (t2.tree.empty()) {
    result.tree = t1.tree;
  } else {
    result.tree = t1.tree;
    result.merge_tree(0, t2, 0, bodies);
  }
  return result;
}

// the root space fo t1 and t2 must be same
template <typename T, std::size_t Dimension>
BodyTree<T, Dimension> build_tree(const boost::mpi::communicator &comm,
                                  const Space<T, Dimension> &root_space,
                                  const Bodies<T, Dimension> &bodies) {
  communication::Division division(comm, bodies.size());

  BodyTree<T, Dimension> tree;
  for (auto i = division.begin; i < division.end; ++i) {
    tree.push(bodies, root_space, i);
  }

  // merge local trees
  logging::logger(logging::Level::Trace)
      << "start merging local trees" << std::endl;
  boost::mpi::all_reduce(comm, boost::mpi::inplace(tree),
                         [&bodies](const auto &t1, const auto &t2) {
                           logging::logger(logging::Level::Trace)
                               << "merge tree " << &t1 << " and " << &t2
                               << std::endl;
                           return merge_tree(t1, t2, bodies);
                         });
  return tree;
}

} // namespace n_body::data::tree

#endif


#include <iostream>
#include <thread>
#include <mutex>
#include <condition_variable>
#include <atomic>

#include <solver.hpp>
#include <polynome_reader.hpp>
#include <args_parser.hpp>

// FloatType for computations can be specified when configuring the project via CMake
// boost::multiprecision::cpp_dec_float_100 is very accurate but relatively slow in comparison to double
#ifdef WITH_BOOST_MULTIPRECISION
#include <boost/multiprecision/cpp_dec_float.hpp>
using FloatType = boost::multiprecision::cpp_dec_float_100;

//template Sqrt should be instantiated because we don't want to lose accuracy when using std::sqrt(double)
template<>
boost::multiprecision::cpp_dec_float_100 SqrtImpl(const boost::multiprecision::cpp_dec_float_100 &value) {
    return boost::multiprecision::sqrt(value);
}

//this template instantiation should be faster than using intermediate stringstream
template<>
boost::multiprecision::cpp_dec_float_100 ReadInteger(const char *str) {
    return boost::multiprecision::cpp_dec_float_100(str);
}
#else
using FloatType = double;
#endif

// this class holds Producer and Consumer threads and some other entities which is needed for square solving
class SquareSolverService
{
public:
    SquareSolverService(PolynomeReader<FloatType, 3> &reader, bool silent = false, int batchSize = 32):
        reader(reader),
        solver(BuildSolver<FloatType>()),
        kSilent(silent),
        kBatchSize(batchSize)
    {
    }
    virtual ~SquareSolverService() = default;

    void Run() {
        std::thread consumer([&](){ this->ConsumerThread(); });
        ProducerThread();
        StopConsumer();
        consumer.join();
    }

private:
    PolynomeReader<FloatType, 3> &reader;

    const std::unique_ptr<EquationSolver<FloatType> > solver;
    const int kBatchSize = 32;
    const bool kSilent = false;

    std::mutex mtx;
    std::condition_variable cvar;

    //we don't want to lose too much time because of the overhead of inter-thread synchronization
    //that's why we send data in batches, which are not too small and not too big
    using BatchType = std::vector<Polynome<FloatType> >;
    using BatchPtrType = std::unique_ptr<BatchType>;

    std::atomic<bool> batchReady = false;
    BatchPtrType preparedBatch;
    BatchPtrType formingBatch;

    void SendBatch() {
        std::unique_lock<std::mutex> ul(mtx);
        preparedBatch = std::move(formingBatch);
        batchReady = true;
        ul.unlock();
        cvar.notify_one();

        //reservation at some extent increases performance here
        formingBatch = std::make_unique<BatchType>();
        formingBatch->reserve(kBatchSize);

        ul.lock();
        cvar.wait(ul, [&]() { return this->batchReady.load() == false; });
    }

    void StopConsumer() {
        //consumer thread can be stopped via sending empty pointer to batch
        std::unique_lock<std::mutex> ul(mtx);

        batchReady = true;
        ul.unlock();
        cvar.notify_one();
    }

    void ProducerThread() {
        formingBatch = std::make_unique<BatchType>();
        formingBatch->reserve(kBatchSize);

        Polynome<FloatType> poly;
        while (reader >> poly) {
            formingBatch->emplace_back(std::move(poly));

            if (formingBatch->size() >= kBatchSize) {
                SendBatch();
            }
        }

        //don't forget to process remaining equations
        if (!formingBatch->empty()) {
            SendBatch();
        }
    }

    void ConsumerThread() {
        while (true) {
            std::unique_lock<std::mutex> ul(mtx);
            cvar.wait(ul, [&]() { return this->batchReady.load(); });

            BatchPtrType batch = std::move(preparedBatch);
            batchReady = false;

            ul.unlock();
            cvar.notify_one();

            //sending empty batch pointer will stop the consumer
            if (!batch) break;

            for (const Polynome<FloatType> &poly: (*batch)) {
                auto res = solver->Solve(poly);
                //printing results in console can take too much time in comparison to all the algo
                //so it can be disabled
                if (!kSilent) {
                    std::cout << poly << " => " << res << std::endl;
                }
            }
        }
    }
};

int main(int argc, char **argv) {
    std::cout.precision(std::numeric_limits<FloatType>::digits10);

    //argc_offset is used to make clear from which point of arguments interesting data is placed
    int argc_offset;
    const Configuration config = ParseCmdArgs(argc, argv, argc_offset);

    clock_t requestStartTime = clock();

    //interactive mode allows to type coeffs in console in real time
    PolynomeReader<FloatType, 3> reader = config.interactive ? PolynomeReader<FloatType, 3>(std::cin) : PolynomeReader<FloatType, 3>(argc - argc_offset, argv + argc_offset);
    SquareSolverService service(reader, config.silent, config.interactive ? 1 : 32);
    service.Run();

    clock_t requestEndTime = clock();

    if (config.measurePerformance) {
        std::cout.precision(6);
        std::cout << "request processing time: " << (requestEndTime - requestStartTime) / double(CLOCKS_PER_SEC) << std::endl;
    }

    return 0;
}