#include <iostream>
#include <string>

#include <boost/program_options.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/preprocessor/seq/for_each.hpp>

#include <amgcl/backend/builtin.hpp>
#include <amgcl/value_type/static_matrix.hpp>
#include <amgcl/make_solver.hpp>
#include <amgcl/amg.hpp>
#include <amgcl/solver/runtime.hpp>
#include <amgcl/coarsening/runtime.hpp>
#include <amgcl/relaxation/runtime.hpp>
#include <amgcl/relaxation/as_preconditioner.hpp>
#include <amgcl/preconditioner/cpr.hpp>
#include <amgcl/adapter/crs_tuple.hpp>
#include <amgcl/adapter/block_matrix.hpp>
#include <amgcl/io/mm.hpp>
#include <amgcl/io/binary.hpp>
#include <amgcl/profiler.hpp>

namespace amgcl { profiler<> prof; }
using amgcl::prof;
using amgcl::precondition;

//---------------------------------------------------------------------------
template <class Matrix>
void solve_cpr(const Matrix &K, const std::vector<double> &rhs, boost::property_tree::ptree &prm)
{
    auto t1 = prof.scoped_tic("CPR");

    typedef amgcl::backend::builtin<double> Backend;

    typedef
        amgcl::amg<Backend, amgcl::runtime::coarsening::wrapper, amgcl::runtime::relaxation::wrapper>
        PPrecond;

    typedef
        amgcl::relaxation::as_preconditioner<Backend, amgcl::runtime::relaxation::wrapper>
        SPrecond;

    prof.tic("setup");
    amgcl::make_solver<
        amgcl::preconditioner::cpr<PPrecond, SPrecond>,
        amgcl::runtime::solver::wrapper<Backend>
        > solve(K, prm);
    prof.toc("setup");

    std::cout << solve.precond() << std::endl;

    std::vector<double> x(rhs.size(), 0.0);

    size_t iters;
    double error;

    prof.tic("setup");
    std::tie(iters, error) = solve(rhs, x);
    prof.toc("setup");

    std::cout << "Iterations: " << iters << std::endl
              << "Error:      " << error << std::endl;
}

//---------------------------------------------------------------------------
template <int B, class Matrix>
void solve_block_cpr(const Matrix &K, const std::vector<double> &rhs, boost::property_tree::ptree &prm)
{
    auto t1 = prof.scoped_tic("CPR");

    typedef amgcl::static_matrix<double, B, B> val_type;
    typedef amgcl::static_matrix<double, B, 1> rhs_type;
    typedef amgcl::backend::builtin<val_type>  SBackend;
    typedef amgcl::backend::builtin<double>    PBackend;

    typedef
        amgcl::amg<
            PBackend,
            amgcl::runtime::coarsening::wrapper,
            amgcl::runtime::relaxation::wrapper>
        PPrecond;

    typedef
        amgcl::relaxation::as_preconditioner<
            SBackend,
            amgcl::runtime::relaxation::wrapper
            >
        SPrecond;

    prof.tic("setup");
    amgcl::make_solver<
        amgcl::preconditioner::cpr<PPrecond, SPrecond>,
        amgcl::runtime::solver::wrapper<SBackend>
        > solve(amgcl::adapter::block_matrix<val_type>(K), prm);
    prof.toc("setup");

    std::cout << solve.precond() << std::endl;

    std::vector<rhs_type> x(rhs.size(), amgcl::math::zero<rhs_type>());

    auto rhs_ptr = reinterpret_cast<const rhs_type*>(rhs.data());
    size_t n = amgcl::backend::rows(K) / B;

    size_t iters;
    double error;

    prof.tic("solve");
    std::tie(iters, error) = solve(amgcl::make_iterator_range(rhs_ptr, rhs_ptr + n), x);
    prof.toc("solve");

    std::cout << "Iterations: " << iters << std::endl
              << "Error:      " << error << std::endl;
}

//---------------------------------------------------------------------------
int main(int argc, char *argv[]) {
    using std::string;
    using std::vector;
    using amgcl::prof;
    using amgcl::precondition;

    namespace po = boost::program_options;
    namespace io = amgcl::io;

    po::options_description desc("Options");

    desc.add_options()
        ("help,h", "show help")
        (
         "binary,B",
         po::bool_switch()->default_value(false),
         "When specified, treat input files as binary instead of as MatrixMarket. "
         "It is assumed the files were converted to binary format with mm2bin utility. "
        )
        (
         "matrix,A",
         po::value<string>()->required(),
         "The system matrix in MatrixMarket format"
        )
        (
         "rhs,f",
         po::value<string>(),
         "The right-hand side in MatrixMarket format"
        )
        (
         "runtime-block-size,b",
         po::value<int>(),
         "The block size of the system matrix set at runtime"
        )
        (
         "static-block-size,c",
         po::value<int>()->default_value(1),
         "The block size of the system matrix set at compiletime"
        )
        (
         "params,P",
         po::value<string>(),
         "parameter file in json format"
        )
        (
         "prm,p",
         po::value< vector<string> >()->multitoken(),
         "Parameters specified as name=value pairs. "
         "May be provided multiple times. Examples:\n"
         "  -p solver.tol=1e-3\n"
         "  -p precond.coarse_enough=300"
        )
        ;

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);

    if (vm.count("help")) {
        std::cout << desc << std::endl;
        return 0;
    }

    po::notify(vm);

    boost::property_tree::ptree prm;
    if (vm.count("params")) read_json(vm["params"].as<string>(), prm);

    if (vm.count("prm")) {
        for(const string &v : vm["prm"].as<vector<string> >()) {
            amgcl::put(prm, v);
        }
    }

    int cb = vm["static-block-size"].as<int>();

    if (vm.count("runtime-block-size"))
        prm.put("precond.block_size", vm["runtime-block-size"].as<int>());
    else
        prm.put("precond.block_size", cb);

    size_t rows;
    vector<ptrdiff_t> ptr, col;
    vector<double> val, rhs;
    std::vector<char> pm;

    {
        auto t = prof.scoped_tic("reading");

        string Afile  = vm["matrix"].as<string>();
        bool   binary = vm["binary"].as<bool>();

        if (binary) {
            io::read_crs(Afile, rows, ptr, col, val);
        } else {
            size_t cols;
            std::tie(rows, cols) = io::mm_reader(Afile)(ptr, col, val);
            precondition(rows == cols, "Non-square system matrix");
        }

        if (vm.count("rhs")) {
            string bfile = vm["rhs"].as<string>();

            size_t n, m;

            if (binary) {
                io::read_dense(bfile, n, m, rhs);
            } else {
                std::tie(n, m) = io::mm_reader(bfile)(rhs);
            }

            precondition(n == rows && m == 1, "The RHS vector has wrong size");
        } else {
            rhs.resize(rows, 1.0);
        }
    }

#define CALL_BLOCK_SOLVER(z, data, B)                                          \
    case B:                                                                    \
        solve_block_cpr<B>(std::tie(rows, ptr, col, val), rhs, prm);           \
        break;

    switch(cb) {
        case 1:
            solve_cpr(std::tie(rows, ptr, col, val), rhs, prm);
            break;

        BOOST_PP_SEQ_FOR_EACH(CALL_BLOCK_SOLVER, ~, AMGCL_BLOCK_SIZES)

        default:
            precondition(false, "Unsupported block size");
            break;
    }

    std::cout << prof << std::endl;
}


/*
   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; either version 2 of the License, or
   (at your option) any later version.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.

   Product name: redemption, a FLOSS RDP proxy
   Copyright (C) Wallix 2016
   Author(s): Christophe Grosjean

*/

#include "utils/crypto/ssl_mod_exp_direct.hpp"

#include <cassert>
#include <cstddef>

#include "cxx/diagnostic.hpp"

#include <boost/multiprecision/cpp_int.hpp>

/**
 * \pre  \a out_len >= \a modulus_size
 * \return  the length of the big-endian number placed at out. ~size_t{} if error
 */
writable_bytes_view mod_exp_direct(
    writable_bytes_view out,
    bytes_view inr,
    bytes_view modulus,
    bytes_view exponent
) {
    assert(out.size() >= modulus.size());

    using int_type = boost::multiprecision::cpp_int;

    auto b256_to_bigint = [](bytes_view s) {
        int_type i;
        boost::multiprecision::import_bits(i, s.begin(), s.end());
        return i;
    };

    int_type base = b256_to_bigint(inr);
    int_type exp = b256_to_bigint(exponent);
    int_type m = b256_to_bigint(modulus);

    REDEMPTION_DIAGNOSTIC_PUSH()
    REDEMPTION_DIAGNOSTIC_GCC_IGNORE("-Wzero-as-null-pointer-constant")
    int_type r = boost::multiprecision::powm(base, exp, m);
    REDEMPTION_DIAGNOSTIC_POP()

    auto it = boost::multiprecision::export_bits(r, out.data(), 8);
    auto r_len = static_cast<size_t>(it - out.data());
    *it = 0;
    if (r_len == 1 && out[0] == 0) {
        r_len = 0;
    }
    return out.first(r_len);
}


/*! \file freefallsolveeom.cpp
    \brief 空気抵抗のある自由落下系に対して運動方程式を解くクラスの実装

    Copyright © 2018 @dc1394 All Rights Reserved.
    This software is released under the BSD 2-Clause License.
*/
#include "freefallsolveeom.h"
#include <cmath>                                // for std::cos, std::floor, std::log10
#include <cstdio>                               // for std::fclose, std::fflush, std::fopen, std::fwrite
#include <optional>                             // for std::make_optional
#include <queue>                                // for std::queue
#include <string>                               // for std::to_string
#include <tuple>                                // for std::get, std::make_tuple
#include <utility>                              // for std::make_pair
#include <boost/assert.hpp>                     // for BOOST_ASSERT
#include <boost/math/constants/constants.hpp>   // for boost::math::constants::pi
#include <boost/math/tools/minima.hpp>          // for boost::math::tools::brent_find_minima
#include <boost/math/tools/roots.hpp>           // for boost::math::tools::bisect

namespace freefallsolveeom {
    // #region コンストラクタ・デストラクタ

    FreefallSolveEom::FreefallSolveEom(double dt, double tintervalgraphplot, double eps, double m, double r, double h0, double v0, Ode_Solver_type ode_solver_type) :
        acc_(gsl_interp_accel_alloc(), gsl_interp_accel_deleter),
        dt_(dt),
        eps_(eps),
        fp_(nullptr, std::fclose),
        h0_(h0),
        imax_(static_cast<std::int32_t>(tintervalgraphplot / dt)),
        l2divm2northlatitude45_(sqr(sqr(FreefallSolveEom::R0 + h0) * 2.0 * pi<double>() / (24.0 * 60.0 * 60.0)) * std::cos(pi<double>() * 0.25)),
        m_(m),
        ode_solver_type_(ode_solver_type),
        r_(r),
        spherevolume_(4.0 / 3.0 * boost::math::constants::pi<double>() * r * r * r),
        spline_pressure_(nullptr, gsl_spline_deleter),
        spline_temperature_(nullptr, gsl_spline_deleter),
        tintervalgraphplot_(tintervalgraphplot),
        tintervaloutputcsv_(std::nullopt),
        islargertintervaloutputcsv_(std::nullopt),
        v0_(v0),
		x_({ R0 + h0, v0 })
    {
        initialize();
    }

    FreefallSolveEom::FreefallSolveEom(double dt, double tintervalgraphplot, double tintervaloutputcsv, std::string const & csvfilename, double eps, double m, double r, double h0, double v0, Ode_Solver_type ode_solver_type) :
        acc_(gsl_interp_accel_alloc(), gsl_interp_accel_deleter),
        dt_(dt),
        eps_(eps),
        fp_(std::unique_ptr< FILE, decltype(&std::fclose) >(std::fopen(csvfilename.c_str(), "w"), std::fclose)),
        h0_(h0),
        imax_(static_cast<std::int32_t>(tintervalgraphplot / dt)),
        l2divm2northlatitude45_(sqr(sqr(FreefallSolveEom::R0 + h0) * 2.0 * pi<double>() / (24.0 * 60.0 * 60.0)) * std::cos(pi<double>() * 0.25)),
        m_(m),
        ode_solver_type_(ode_solver_type),
        outputtocsvdigits_(std::to_string(tintervaloutputcsv > 0.1 ? 1 : static_cast<std::int32_t>(std::ceil(-std::log10(tintervaloutputcsv))))),
        r_(r),
        spherevolume_(4.0 / 3.0 * boost::math::constants::pi<double>() * r * r * r),
        spline_pressure_(nullptr, gsl_spline_deleter),
        spline_temperature_(nullptr, gsl_spline_deleter),
        tintervalgraphplot_(tintervalgraphplot),
        tintervaloutputcsv_(std::make_optional(tintervaloutputcsv)),
        islargertintervaloutputcsv_(std::make_optional(tintervalgraphplot <= tintervaloutputcsv)),
        v0_(v0),
        x_({ R0 + h0, v0 })
    {
        initialize();
    }

    // #endregion コンストラクタ・デストラクタ

    // #region publicメンバ関数

    std::tuple< double, double, double, FreefallSolveEom::hmaxtype, FreefallSolveEom::vmaxtype, FreefallSolveEom::tandvtype, FreefallSolveEom::tandvandbooltype > FreefallSolveEom::operator()()
    {
        FreefallSolveEom::state_type result{};
        switch (ode_solver_type_) {
        case Ode_Solver_type::ADAMS_BASHFORTH_MOULTON:
            result = solveeom_run(adams_bashforth_moulton< 2, FreefallSolveEom::state_type >());
            break;

        case Ode_Solver_type::BULIRSCH_STOER:
            result = solveeom_run(bulirsch_stoer< FreefallSolveEom::state_type >(eps_, eps_));
            break;

        case Ode_Solver_type::CONTROLLED_RUNGE_KUTTA:
            result = solveeom_run(make_controlled(eps_, eps_, error_stepper_type()));
            break;

        default:
            BOOST_ASSERT(!"Ode_Solver_typeがあり得ない値になっている！");
            break;
        }

        auto const t = iscalculationfinished_ ? tend_ : t_;
        auto const h = result[0] - FreefallSolveEom::R0;
        auto const v = result[1];

        FreefallSolveEom::hmaxtype stateofhmax = std::nullopt;
        if (hmaxoftandh_)
        {
            stateofhmax = hmaxoftandh_;
        }
        else if (v0_ <= 0.0)
        {
            stateofhmax = std::make_optional(std::make_pair(0.0, h0_));
        }

        FreefallSolveEom::vmaxtype stateofvmax = std::nullopt;
        if (vmaxoftandhandv_ && std::fabs(std::get<2>(*vmaxoftandhandv_)) >= std::fabs(v0_))
        {
            stateofvmax = vmaxoftandhandv_;
        }
        else if (vmaxoftandhandv_ || (iscalculationfinished_ && std::fabs(v) < std::fabs(v0_)))
        {
            stateofvmax = std::make_optional(std::make_tuple(0.0, h0_, v0_));
        }
        else if (iscalculationfinished_ && std::fabs(v) >= std::fabs(v0_))
        {
            stateofvmax = std::make_optional(std::make_tuple(t, h, v));
        }

        return std::make_tuple(t, h, v, stateofhmax, stateofvmax, stateescapeofkarmanline_, stateescapeofexosphere_);
    }

    // #endregion publicメンバ関数

    // #region privateメンバ関数

    double FreefallSolveEom::get_P(double r) const
    {
        // ジオポテンシャル高度を取得
        auto const H = getGeopotentialFromAltitude(r);

        // 「標準大気 ー 各高度における空気の温度・圧力・密度・音速・粘性係数・動粘性係数の計算式」
        // https://pigeon-poppo.com/standard-atmosphere/ より
        if (H <= 11000.0)
        {
            return 101325.0 * std::pow(288.15 / FreefallSolveEom::get_T(r), -5.256);
        }
        else if (H <= 20000.0)
        {
            return 22632.064 * std::exp(-0.1577 * (FreefallSolveEom::MTOKM * H - 11.0));
        }
        else if (H <= 32000.0)
        {
            return 5474.889 * std::pow(216.65 / FreefallSolveEom::get_T(r), 34.163);
        }
        else if (H <= 47000.0)
        {
            return 868.019 * std::pow(228.65 / FreefallSolveEom::get_T(r), 12.201);
        }
        else if (H <= 51000.0)
        {
            return 110.906 * std::exp(-0.1262 * (FreefallSolveEom::MTOKM * H - 47.0));
        }
        else if (H <= 71000.0)
        {
            return 66.939 * std::pow(270.65 / FreefallSolveEom::get_T(r), -12.201);
        }
        else if (H <= 84852.0)
        {
            return 3.956 * std::pow(214.65 / FreefallSolveEom::get_T(r), -17.082);
        }
        else if (r - FreefallSolveEom::R0 <= 1000000.0)
        {
            return gsl_spline_eval(spline_pressure_.get(), r - FreefallSolveEom::R0, acc_.get());
        }
        else
        {
            return 0.0;
        }
    }

    double FreefallSolveEom::get_T(double r) const
    {
        // ジオポテンシャル高度を取得
        auto const H = getGeopotentialFromAltitude(r);

        // 「標準大気 ー 各高度における空気の温度・圧力・密度・音速・粘性係数・動粘性係数の計算式」
        // https://pigeon-poppo.com/standard-atmosphere/ より
        if (H <= 11000.0)
        {
            return FreefallSolveEom::CELSIUSTOABSOLUTETEMPERATURE + 15.0 - 6.5 * FreefallSolveEom::MTOKM * H;
        }
        else if (H <= 20000.0)
        {
            return FreefallSolveEom::CELSIUSTOABSOLUTETEMPERATURE - 56.5;
        }
        else if (H <= 32000.0)
        {
            return FreefallSolveEom::CELSIUSTOABSOLUTETEMPERATURE - 76.5 + FreefallSolveEom::MTOKM * H;
        }
        else if (H <= 47000.0)
        {
            return FreefallSolveEom::CELSIUSTOABSOLUTETEMPERATURE - 134.1 + 2.8 * FreefallSolveEom::MTOKM * H;
        }
        else if (H <= 51000.0)
        {
            return FreefallSolveEom::CELSIUSTOABSOLUTETEMPERATURE - 2.5;
        }
        else if (H <= 71000.0)
        {
            return FreefallSolveEom::CELSIUSTOABSOLUTETEMPERATURE + 140.3 - 2.8 * FreefallSolveEom::MTOKM * H;
        }
        else if (H <= 84852.0)
        {
            return FreefallSolveEom::CELSIUSTOABSOLUTETEMPERATURE + 83.5 - 2.0 * FreefallSolveEom::MTOKM * H;
        }
        else if (r - FreefallSolveEom::R0 <= 1000000.0)
        {
            return gsl_spline_eval(spline_temperature_.get(), r - FreefallSolveEom::R0, acc_.get());
        }
        else
        {
            return 1000.0;
        }
    }

    void FreefallSolveEom::initialize()
    {
        // 高度80km以上での気圧と温度のデータ
        // 国立天文台編『理科年表』丸善出版（2012）p.331より引用
        p_data_ = { 1.0524,    0.37338,   0.18359,   0.15381,   3.2011E-2, 7.1042E-3,
                    2.5382E-3, 1.2505E-3, 7.2028E-4, 3.0395E-4, 1.5271E-4, 8.4736E-5,
                    2.4767E-5, 8.7704E-6, 3.4498E-6, 1.4518E-6, 6.4468E-7, 3.0236E-7,
                    1.5137E-7, 8.2130E-8, 4.8865E-8, 3.1908E-8, 2.2599E-8, 1.7036E-8,
                    1.3415E-8, 1.0873E-8, 7.5138E-9 };

        t_data_ = { 198.639, 186.87, 186.87, 186.87, 195.08, 240.00, 360.00, 469.27,
                    559.63,  696.29, 790.07, 854.56, 941.33, 976.01, 990.06, 995.83,
                    998.22,  999.24, 999.67, 999.85, 999.93, 999.97, 999.99, 999.99,
                    1000.0,  1000.0, 1000.0 };

        z_mesh_ = { 80000.0,  86000.0,  90000.0,  91000.0,  100000.0, 110000.0,
                    120000.0, 130000.0, 140000.0, 160000.0, 180000.0, 200000.0,
                    250000.0, 300000.0, 350000.0, 400000.0, 450000.0, 500000.0,
                    550000.0, 600000.0, 650000.0, 700000.0, 750000.0, 800000.0,
                    850000.0, 900000.0, 1000000.0 };

        BOOST_ASSERT(p_data_.size() == z_mesh_.size());
        BOOST_ASSERT(t_data_.size() == z_mesh_.size());

        spline_pressure_.reset(gsl_spline_alloc(gsl_interp_cspline, z_mesh_.size()));
        spline_temperature_.reset(gsl_spline_alloc(gsl_interp_cspline, z_mesh_.size()));

        gsl_spline_init(spline_pressure_.get(), z_mesh_.data(), p_data_.data(), z_mesh_.size());
        gsl_spline_init(spline_temperature_.get(), z_mesh_.data(), t_data_.data(), z_mesh_.size());
    }

    template <typename Stepper>
    FreefallSolveEom::state_type FreefallSolveEom::solveeom_run(Stepper const & stepper)
    {
        using namespace boost::math::tools;

        if (isfirststep_)
        {
            if (fp_)
            {
                outputresulttocsv(0.0, x_);
            }

            // 0秒目ですでにカーマン・ラインを突破しているかどうか
            if (x_[0] >= FreefallSolveEom::KARMANLINE)
            {
                stateescapeofkarmanline_ = std::make_optional(std::make_pair(0.0, v0_));
            }

            // 0秒目ですでに外気圏を脱出しているかどうか
            if (x_[0] >= FreefallSolveEom::ALTITUDEOFEXOSPHERE)
            {
                // 外気圏を脱出した際に速度が第二宇宙速度以上だったかどうか
                if (x_[1] >= FreefallSolveEom::SECONDESCAPEVELOCITYOFEXOSPHERE)
                {
                    stateescapeofexosphere_ = std::make_optional(std::make_tuple(0.0, v0_, true));

                    // 計算打ち切り
                    iscalculationfinished_ = true;
                    tend_ = 0.0;
                }
                else
                {
                    stateescapeofexosphere_ = std::make_optional(std::make_tuple(0.0, v0_, false));
                }
            }

            isfirststep_ = false;
            return x_;
        }

        std::queue<FreefallSolveEom::state_type> history{};

        for (auto i = 1; i <= imax_; i++) {
            auto const ttmp = static_cast<double>(i) * dt_;

            if (history.size() < 2)
            {
                history.push(x_);
            }
            else
            {
                history.pop();
                history.push(x_);
            }

            integrate_eom(stepper, dt_, x_);

            if (vmaxoftandhandv_ && x_[1] < 0.0)
            {
                BOOST_ASSERT(std::get<2>(*vmaxoftandhandv_) < x_[1]);
            }

            auto const statebefore = history.back();

            // カーマン・ラインを突破した際の時間を探索
            if (!stateescapeofkarmanline_ && x_[0] >= FreefallSolveEom::KARMANLINE)
            {
                auto maxit = MAXITER;
                auto res = bisect(
                    [&stepper, &statebefore, this](double t)
                {
                    auto x = statebefore;
                    integrate_eom(stepper, t, x);
                    return x[0] - FreefallSolveEom::KARMANLINE;
                },
                    0.0,
                    dt_,
                    eps_tolerance<double>(FreefallSolveEom::DIGITS),
                    maxit);

                auto const tescapeofkarmanline = (res.first + res.second) * 0.5;
                auto xtmp(statebefore);
                integrate_eom(stepper, tescapeofkarmanline, xtmp);

                stateescapeofkarmanline_ = std::make_optional(std::make_pair(t_ + static_cast<double>(i - 1) * dt_ + tescapeofkarmanline, xtmp[1]));
            }

            // 外気圏を脱出した際の時間を探索
            if (!stateescapeofexosphere_ && x_[0] >= FreefallSolveEom::ALTITUDEOFEXOSPHERE)
            {
                auto maxit = MAXITER;
                auto res = bisect(
                    [&stepper, &statebefore, this](double t)
                {
                    auto x = statebefore;
                    integrate_eom(stepper, t, x);
                    return x[0] - FreefallSolveEom::ALTITUDEOFEXOSPHERE;
                },
                    0.0,
                    dt_,
                    eps_tolerance<double>(FreefallSolveEom::DIGITS),
                    maxit);

                auto const tescapeofexosphere = (res.first + res.second) * 0.5;
                auto xtmp(statebefore);
                integrate_eom(stepper, tescapeofexosphere, xtmp);

                // 外気圏を脱出した際に速度が第二宇宙速度以上だったかどうか
                if (xtmp[1] >= FreefallSolveEom::SECONDESCAPEVELOCITYOFEXOSPHERE)
                {
                    stateescapeofexosphere_ = std::make_optional(std::make_tuple(t_ + static_cast<double>(i - 1) * dt_ + tescapeofexosphere, xtmp[1], true));

                    // 計算打ち切り
                    iscalculationfinished_ = true;
                    tend_ = t_ + static_cast<double>(i - 1) * dt_ + tescapeofexosphere;

                    return xtmp;
                }
                else
                {
                    stateescapeofexosphere_ = std::make_optional(std::make_tuple(t_ + static_cast<double>(i - 1) * dt_ + tescapeofexosphere, xtmp[1], false));
                }
            }

            // 地面に衝突する時の速度とその際の時間を探索
            if (x_[0] < FreefallSolveEom::R0)
            {
                auto maxit = MAXITER;
                auto res = bisect(
                    [&stepper, &statebefore, this](double t)
                {
                    auto x = statebefore;
                    integrate_eom(stepper, t, x);
                    return x[0] - FreefallSolveEom::R0;
                },
                    0.0,
                    dt_,
                    eps_tolerance<double>(FreefallSolveEom::DIGITS),
                    maxit);

                auto const tendtmp = (res.first + res.second) * 0.5;
                auto xtmp(statebefore);
                integrate_eom(stepper, tendtmp, xtmp);

                tend_ = t_ + static_cast<double>(i - 1) * dt_ + tendtmp;

                if (fp_)
                {
                    outputresulttocsv(tend_, xtmp);
                    std::fflush(fp_.get());
                    fp_.reset();
                }

                iscalculationfinished_ = true;

                return xtmp;
            }

            // 最高到達高度とその際の時間の探索
            if (statebefore[1] * x_[1] <= 0.0)
            {
                auto maxit = MAXITER;
                auto const res = bisect(
                    [&stepper, &statebefore, this](double t)
                {
                    auto x = statebefore;
                    integrate_eom(stepper, t, x);
                    return x[1];
                },
                    0.0,
                    dt_,
                    eps_tolerance<double>(FreefallSolveEom::DIGITS),
                    maxit);

                auto const thmaxtmp = (res.first + res.second) * 0.5;
                auto xtmp(statebefore);
                integrate_eom(stepper, thmaxtmp, xtmp);

                hmaxoftandh_ = std::make_optional(std::make_pair(t_ + static_cast<double>(i - 1) * dt_ + thmaxtmp, xtmp[0] - FreefallSolveEom::R0));
            }

            // 最高速度とその際の時間と高度の探索
            if (!vmaxoftandhandv_ && x_[1] < 0.0 && x_[1] > statebefore[1])
            {
                auto maxit = MAXITER;
                auto state2before = history.front();
                auto const res = brent_find_minima(
                    [&stepper, &state2before, this](double t)
                {
                    auto x = state2before;
                    integrate_eom(stepper, t, x);
                    return x[1];
                },
                    0.0,
                    2.0 * dt_,
                    FreefallSolveEom::DIGITS,
                    maxit);

                auto xtmp(state2before);
                integrate_eom(stepper, res.first, xtmp);

                BOOST_ASSERT(xtmp[1] < state2before[1]);
                BOOST_ASSERT(xtmp[1] < statebefore[1]);
                BOOST_ASSERT(xtmp[1] < x_[1]);

                vmaxoftandhandv_ = std::make_optional(std::make_tuple(t_ + static_cast<double>(i - 2) * dt_ + res.first, xtmp[0] - FreefallSolveEom::R0, xtmp[1]));
            }

            if (tintervaloutputcsv_ &&
                !(*islargertintervaloutputcsv_) &&
                (std::fabs(ttmp / *tintervaloutputcsv_ - std::floor(ttmp / *tintervaloutputcsv_)) <= FreefallSolveEom::ZERODECISIONTOCSV || std::fabs(ttmp / *tintervaloutputcsv_ - std::ceil(ttmp / *tintervaloutputcsv_)) <= FreefallSolveEom::ZERODECISIONTOCSV))
            {
                outputresulttocsv(t_ + ttmp, x_);
            }
        }

        cnt_++;
        t_ = static_cast<double>(cnt_) * tintervalgraphplot_;

        if (tintervaloutputcsv_ &&
            *islargertintervaloutputcsv_ &&
            (std::fabs(t_ / *tintervaloutputcsv_ - std::floor(t_ / *tintervaloutputcsv_)) <= FreefallSolveEom::ZERODECISIONTOCSV || std::fabs(t_ / *tintervaloutputcsv_ - std::ceil(t_ / *tintervaloutputcsv_)) <= FreefallSolveEom::ZERODECISIONTOCSV))
        {
            outputresulttocsv(t_, x_);
        }

        return x_;
    }

    // #endregion privateメンバ関数

    // #region templateメンバ関数の実体化

    template FreefallSolveEom::state_type FreefallSolveEom::solveeom_run<adams_bashforth_moulton< 2, FreefallSolveEom::state_type > >(adams_bashforth_moulton< 2, state_type > const & stepper);
    template FreefallSolveEom::state_type FreefallSolveEom::solveeom_run<bulirsch_stoer < FreefallSolveEom::state_type > >(bulirsch_stoer < state_type > const & stepper);
    template FreefallSolveEom::state_type FreefallSolveEom::solveeom_run<FreefallSolveEom::error_stepper_type>(error_stepper_type const & stepper);

    // #endregion templateメンバ関数の実体化

    // #region staticメンバ変数

    const double FreefallSolveEom::SECONDESCAPEVELOCITYOFEXOSPHERE = std::sqrt(2.0 * FreefallSolveEom::G * FreefallSolveEom::M / FreefallSolveEom::ALTITUDEOFEXOSPHERE);

    // #endregion staticメンバ変数
}


////////////////////////////////////////////////////////////////////////////////
// Fields.hh
////////////////////////////////////////////////////////////////////////////////
/*! @file
//  Classes implementing vector/scalar/tensor fields. Each class also specifies
//  how the quantities are flattened into a single 1D array. Field samples are
//  stored as columns of a dim x |D| 2D array that is then flattened in column
//  major format. Here |D| is the size of the discrete domain.
//
//  This means, for symmetric tensor fields, there are two flattenings: first
//  each sample is flattened into a 6-vector (in 3D) using Voigt notation, then
//  each 6-vector is stored as a column in a 6 x |D| array, which is flattened
//  into a 6 |D| vector.
//
//  For vector fields, the resulting flattened vector looks like:
//      [v_0x, v_0y, v_0z, v_1x, ..., v_|D|z]
//  This vector can be obtained with the getFlattened() method.
*/
//  Author:  Julian Panetta (jpanetta), julian.panetta@gmail.com
//  Company:  New York University
//  Created:  02/13/2013 16:27:14
////////////////////////////////////////////////////////////////////////////////
#ifndef FIELDS_HH
#define FIELDS_HH
#include <Eigen/Dense>
#include <vector>
#include <string>
#include <cassert>
#include <algorithm>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <stdexcept>
#include <cmath>
#include <limits>

#include "Flattening.hh"
#include "SymmetricMatrix.hh"

#include "Algebra.hh"

typedef enum { FIELD_SCALAR, FIELD_VECTOR, FIELD_MATRIX} FieldType;
enum class DomainType { PER_ELEMENT = 0, PER_NODE = 1, ANY = 3, GUESS = 3, UNKNOWN = -1};

template<typename Real, size_t t_dim>
class VectorField : public VectorSpace<Real, VectorField<Real, t_dim>> {
public:
    typedef Eigen::Matrix<Real, Eigen::Dynamic, 1> FlattenedType;
    typedef Eigen::Matrix<Real, t_dim, Eigen::Dynamic> ArrayType;
    typedef typename ArrayType::ColXpr         ValueType;
    typedef typename ArrayType::ConstColXpr    ConstValueType;

    // Copy and move constructors/assignment
    VectorField(const VectorField  &b) = default;
    VectorField(      VectorField &&b) = default;
    VectorField &operator=(const VectorField  &b) = default;
    VectorField &operator=(      VectorField &&b) = default;

    // Flattened data constructor
    // Note: copies data
    VectorField(const FlattenedType &values) {
        size_t domainSize = values.rows() / t_dim;
        assert(t_dim * domainSize == (size_t) values.rows());
        m_values = Eigen::Map<const ArrayType>(values.data(), t_dim,
                                               domainSize);
    }

    // Flattened data constructor (std::vector version)
    template<typename Real2>
    VectorField(const std::vector<Real2> &values) {
        size_t domainSize = values.size() / t_dim;
        if (t_dim * domainSize != values.size())
            throw std::runtime_error("Invalid flattened field size (not an " + std::to_string(t_dim) + "D field)");
        m_values = Eigen::Map<const Eigen::Matrix<Real2, t_dim, Eigen::Dynamic> >
            (&values[0], t_dim, domainSize);
    }

    // Uninitialized allocation constructor
    explicit VectorField(size_t domainSize = 0)
        : m_values(t_dim, domainSize) { }

    ConstValueType operator()(size_t i) const {
        assert(i < (size_t) m_values.cols());
        return m_values.col(i);
    }

    ValueType operator()(size_t i) {
        assert(i < (size_t) m_values.cols());
        return m_values.col(i);
    }

    void clear() { m_values = ArrayType::Zero(dim(), domainSize()); }

    ////////////////////////////////////////////////////////////////////////////
    // VectorSpace requirements
    ////////////////////////////////////////////////////////////////////////////
    void Add(const VectorField &b) { m_values += b.m_values; }
    void Scale(Real scalar)        { m_values *= scalar; }

    // Normalize data so that the maximum column magnitude is 1.
    void maxColumnNormalize() { m_values /= maxMag(); }

    ////////////////////////////////////////////////////////////////////////////
    // Non-VectorSpace operations.
    ////////////////////////////////////////////////////////////////////////////
    Real maxMag() const {
        Real maxNorm = 0;
        for (size_t i = 0; i < domainSize(); ++i)
            maxNorm = std::max(maxNorm, m_values.col(i).norm());
        return maxNorm;
    }

    Real minMag() const {
        Real minNorm = std::numeric_limits<Real>::max();
        for (size_t i = 0; i < domainSize(); ++i)
            minNorm = std::min(minNorm, m_values.col(i).norm());
        return minNorm;
    }

    // Component wise abs.
    VectorField cwiseAbs() const { auto r = VectorField(*this); r.m_values = r.m_values.cwiseAbs(); return r; }

    // Set all coefficients to a constant
    void setConstant(Real val) { m_values.setConstant(val); }

    // Sum of squared norms of each vector.
    Real frobeniusNormSq() const {
        Real normSq = 0;
        for (size_t i = 0; i < domainSize(); ++i)
            normSq += m_values.col(i).squaredNorm();
        return normSq;
    }

    // Scalar-valued inner product between vector field: apply dot product
    // pointwise and sum.
    Real innerProduct(const VectorField &b) const {
        assert(domainSize() == b.domainSize());
        Real result = 0;
        for (size_t i = 0; i < domainSize(); ++i)
            result += m_values.col(i).dot(b.m_values.col(i));
        return result;
    }

    // Unweighted mean vector.
    Eigen::Matrix<Real, t_dim, 1> mean() const {
        Eigen::Matrix<Real, t_dim, 1> result;
        result.setZero();
        for (size_t i = 0; i < domainSize(); ++i)
            result += m_values.col(i);
        result *= (1.0 / domainSize());
        return result;
    }

    const ArrayType &data() const { return m_values; }
          ArrayType &data()       { return m_values; }

    size_t dim() const { return t_dim; }
    size_t N()   const { return dim(); }
    size_t domainSize() const { return m_values.cols(); }
    FieldType fieldType() const { return FIELD_VECTOR; }

    void resizeDomain(size_t dSize) {
        m_values.resize(Eigen::NoChange, dSize);
        clear();
    }

    // Flattened access
    size_t size() const { return dim() * domainSize(); }
    void resize(size_t i) { assert(i % dim() == 0); resizeDomain(i / dim()); }
          Real &operator[](size_t i)       { assert(i < size()); return m_values.data()[i]; }
    const Real &operator[](size_t i) const { assert(i < size()); return m_values.data()[i]; }

    template<typename Real2>
    void getFlattened(std::vector<Real2> &v) const {
        v.resize(size());
        for (size_t i = 0; i < size(); ++i)
            v[i] = operator[](i);
    }

    void print(std::ostream &os, const std::string &componentSeparator = "\t",
               const std::string &elementPrefix = "",
               const std::string &elementSuffix = "\n",
               const std::string &elementSeparator = "") const {
        for (size_t i = 0; i < domainSize(); ++i) {
            if (i) os << elementSeparator;
            ConstValueType v = (*this)(i);
            os << elementPrefix << v[0];
            for (size_t j = 1; j < t_dim; ++j) {
                os << componentSeparator << v[j];
            }
            os << elementSuffix;
        }
    }

    void dump(const std::string &path) const {
        std::ofstream of(path);
        if (!of.is_open())
            throw std::runtime_error(std::string("Couldn't open '") +
                        path + "' for writing.");
        of << std::scientific << std::setprecision(16);
        print(of);
    }

protected:
    /** Data storage */
    ArrayType m_values;
};

template<typename Real>
class ScalarField : public VectorField<Real, 1> {
public:
    using typename VectorField<Real, 1>::FlattenedType;
    typedef Real value_type;

    // ScalarField's value type should act both like a vector (to mimic
    // base class VectorField<Real, 1>) and like a scalar (via typecasts)
    class ValueType {
    public:
        ValueType(Real &val) : m_val(val) { }
              Real &operator[](size_t i)       { (void) (i); assert(i == 0); return m_val; }
        const Real &operator[](size_t i) const { (void) (i); assert(i == 0); return m_val; }
        operator Real&()      { return m_val; }
        operator Real() const { return m_val; }
        ValueType &operator=(Real val) { m_val = val; return *this; }
    private:
        Real &m_val;
    };
    class ConstValueType {
    public:
        ConstValueType(const Real &val) : m_val(val) { }
        Real  operator[](size_t i) const { (void) i; assert(i == 0); return m_val; }
        operator Real() const { return m_val; }
    private:
        const Real &m_val;
    };

    ScalarField(const FlattenedType &values)
        : VectorField<Real, 1>(values) { }
    explicit ScalarField(size_t domainSize = 0)
        : VectorField<Real, 1>(domainSize) { }
    template<typename Real2>
    ScalarField(const std::vector<Real2> &values)
        : VectorField<Real, 1>(values) { }
    // Allow construction from 1-dim vector.
    template<typename Real2>
    ScalarField(const VectorField<Real2, 1> &values)
        : VectorField<Real, 1>(values) { }

    FieldType fieldType() const { return FIELD_SCALAR; }

    Real squaredNorm() const { return m_values.squaredNorm(); }
    Real norm() const { return m_values.norm(); }
    Real  sum() const { return m_values.sum(); }
    Real  min() const { return m_values.minCoeff(); }
    Real  max() const { return m_values.maxCoeff(); }

    // Return the entry with maximum/minimum magnitude
    Real minMag() const { Real m = min(), M = max(); return (std::abs(m) < M) ? m : M; }
    Real maxMag() const { Real m = min(), M = max(); return (std::abs(m) > M) ? m : M; }

    // Component wise abs.
    ScalarField cwiseAbs() const { auto r = ScalarField(*this); r.m_values = r.m_values.cwiseAbs(); return r; }

    // operator() should return numbers rather than column vectors...
    ConstValueType operator()(size_t i) const {
        assert(i < (size_t) m_values.cols());
        return ConstValueType(m_values(0, i));
    }
    ValueType operator()(size_t i) {
        assert(i < (size_t) m_values.cols());
        return ValueType(m_values(0, i));
    }

    const Real *data() const { return m_values.data(); }
          Real *data()       { return m_values.data(); }
    template<size_t dim>
    VectorField<Real, dim> unflatten() const {
        return VectorField<Real, dim>(m_values);
    }

    const typename VectorField<Real, 1>::ArrayType &values() const { return m_values; }

    void minRelax(const ScalarField<Real> &b) { m_values = m_values.cwiseMin(b.m_values); }
    void maxRelax(const ScalarField<Real> &b) { m_values = m_values.cwiseMax(b.m_values); }
    void minRelax(Real b) { m_values = m_values.cwiseMin(b); }
    void maxRelax(Real b) { m_values = m_values.cwiseMax(b); }

private:
    using VectorField<Real, 1>::m_values;
};

// Handles both VectorField and ScalarField output.
template<typename Real, size_t N>
std::ostream &operator<<(std::ostream &os, const VectorField<Real, N> &vf) {
    for (size_t i = 0; i < vf.domainSize(); ++i) {
        for (size_t c = 0; c < N; ++c) {
            os << (c ? "\t" : "") << vf(i)[c];
        }
        os << std::endl;
    }

    return os;
}

// Symmetric matrix NxN fields need only store the upper triangle of the NxN
// matrix. This triangle is flattened into a 1D vector following Voigt notation.
//  [ 0 2 ]   [ 0 5 4 ]  ...  [ 0  N*(N+1)/2 -1  ]
//  [   1 ]   [   1 3 ]       [    1             ]
//            [     2 ]       [        2     ... ]
//                            [         ..   N+1 ]
//                            [           .. N   ]
//                            [              N-1 ]
// This is the typical stress/strain flattening that
// collects the diagonal xx, yy, ... entries at the beginning
// The total number of entries is sum_{i=1}^N i = (N * (N + 1)) / 2
// (because there are i entries in the ith column).
template<typename Real, size_t t_N>
class SymmetricMatrixField : public VectorSpace<Real, SymmetricMatrixField<Real, t_N>> {
public:
    typedef Eigen::Matrix<Real, Eigen::Dynamic, 1> FlattenedType;
    typedef Eigen::Matrix<Real, flatLen(t_N), Eigen::Dynamic> ArrayType;

    typedef SymmetricMatrixRef<t_N, typename ArrayType::ColXpr,
            const typename ArrayType::ColXpr> ValueType;
    typedef ConstSymmetricMatrixRef<t_N,
            typename ArrayType::ConstColXpr> ConstValueType;

    SymmetricMatrixField(const SymmetricMatrixField &b) : m_values(b.m_values) { }

    SymmetricMatrixField(size_t domainSize, const FlattenedType &values) {
        assert(dim() * domainSize == values.rows());
        m_values = Eigen::Map<const ArrayType>(values.data(), dim(),
                                               domainSize);
    }

    // Eigen ArrayType constructor
    SymmetricMatrixField(const ArrayType values) : m_values(values) { }

    SymmetricMatrixField(size_t domainSize = 0)
        : m_values(dim(), domainSize) { }

    constexpr size_t dim() const { return flatLen(t_N); }
    size_t N()   const { return t_N; }
    size_t domainSize() const { return m_values.cols(); }
    FieldType fieldType() const { return FIELD_MATRIX; }

    void clear() { m_values = ArrayType::Zero(dim(), domainSize()); }
    void resizeDomain(size_t dSize) {
        m_values.resize(Eigen::NoChange, dSize);
        clear();
    }

    ConstValueType operator()(size_t i) const {
        return ConstValueType(m_values.col(i));
    }

    ValueType operator()(size_t i) {
        return ValueType(m_values.col(i));
    }

    ////////////////////////////////////////////////////////////////////////////
    // VectorSpace requirements
    ////////////////////////////////////////////////////////////////////////////
    void Add(const SymmetricMatrixField &b) { m_values += b.m_values; }
    void Scale(Real scalar)                 { m_values *= scalar; }

    ////////////////////////////////////////////////////////////////////////////
    // Non-VectorSpace operations.
    ////////////////////////////////////////////////////////////////////////////
    // MHS on Nov 3 3015
    SymmetricMatrixField &operator/=(const ScalarField<Real> &scalars) {
        assert(scalars.domainSize() == size_t(m_values.cols()));
        for (size_t i = 0; i < scalars.domainSize(); ++i)
            m_values.col(i) /= scalars(i);
        return *this;
    }

    // Component wise abs.
    SymmetricMatrixField cwiseAbs() const { return SymmetricMatrixField(m_values.cwiseAbs()); }
    // Set all coefficients to a constant
    void setConstant(Real val) { m_values.setConstant(val); }

    SymmetricMatrixField &operator=(const SymmetricMatrixField &b) {
        if (this == &b) return *this;
        m_values = b.m_values;
        return *this;
    }

    const ArrayType &data() const { return m_values; }
          ArrayType &data()       { return m_values; }

    void dump(const std::string &path) const {
        std::ofstream of(path);
        if (!of.is_open())
            throw std::runtime_error(std::string("Couldn't open '") +
                        path + "' for writing.");
        of << std::scientific << std::setprecision(16);
        for (size_t i = 0; i < domainSize(); ++i) {
            ConstValueType v = (*this)(i);
            of << v[0];
            for (size_t j = 1; j < dim(); ++j) {
                of << '\t' << v[j];
            }
            of << std::endl;
        }
    }

    void load(const std::string &path) {
        std::ifstream is(path);
        if (!is.is_open())
            throw std::runtime_error(std::string("Couldn't open '") + path);

        std::string line;
        std::vector<Real> data;
        while (std::getline(is >> std::ws, line)) {
            std::vector<Real> v;
            std::istringstream iss(line);
            Real c;
            size_t i = 0;
            while (iss >> c) { data.push_back(c); ++i; }
            if (i != dim()) throw std::runtime_error("Read wrong number of components.");
        }
        assert(data.size() % dim() == 0);
        int domSize = data.size() / dim();
        m_values = Eigen::Map<const ArrayType>(&data[0], dim(), domSize);
    }

private:
    /** Data storage */
    ArrayType m_values;
};

template<typename Real, size_t N>
std::ostream &operator<<(std::ostream &os, const SymmetricMatrixField<Real, N> &smf)
{
    for (size_t i = 0; i < smf.domainSize(); ++i) {
        for (size_t c = 0; c < smf.dim(); ++c) {
            os << (c ? "\t" : "") << smf(i)[c];
        }
        os << std::endl;
    }
    return os;
}

// Simple field class that can change dimension but is less efficient/statically
// checked.
// Stores in flattened x0 y0 x1 y1 ... format
template<typename _Real>
class DynamicField {
public:
    DynamicField(size_t dimensions, size_t domSize) {
        resize(dimensions, domSize);
    }

    DynamicField(const DynamicField &b) {
        m_dim = b.m_dim;
        m_storage = b.m_storage;
    }

    template<size_t _N>
    DynamicField(const VectorField<_Real, _N> &vf) {
        resize(vf.dim(), vf.domainSize());
        for (size_t i = 0; i < vf.dim(); ++i)
            for (size_t j = 0; j < vf.domainSize(); ++j)
                (*this)(i, j) = vf(j)[i];
    }

    void resize(size_t domSize) { m_storage.resize(domSize * m_dim); }
    void resize(size_t dim, size_t domSize) { m_dim = dim; resize(domSize); }

    size_t domainSize() const {
        assert(m_storage.size() % m_dim == 0);
        return m_storage.size() / m_dim;
    }

    size_t dim() const { return m_dim; }

    // Flattened access
          _Real &operator[](size_t i)       { return m_storage.at(i); }
    const _Real &operator[](size_t i) const { return m_storage.at(i); }

    _Real &operator()(size_t i, size_t j) {
        if (i >= dim() || j >= domainSize()) throw std::runtime_error("out of bounds access");
        return m_storage.at(j * dim() + i);
    }

    _Real  operator()(size_t i, size_t j) const {
        if (i >= dim() || j >= domainSize()) throw std::runtime_error("out of bounds access");
        return m_storage.at(j * dim() + i);
    }

    // Casts to Field types.
    operator ScalarField<_Real>() const {
        if (m_dim != 1) throw std::runtime_error("Illegal cast of vector field to scalar field.");
        return ScalarField<_Real>(m_storage);
    }
    template<size_t _dim>
    operator VectorField<_Real, _dim>() const {
        if (m_dim != _dim) throw std::runtime_error("Vector field cast dimension mismatch.");
        return VectorField<_Real, _dim>(m_storage);
    }
    template<size_t _dim>
    operator SymmetricMatrixField<_Real, _dim>() const {
        if (m_dim != _dim) throw std::runtime_error("Vector field cast dimension mismatch.");
        return SymmetricMatrixField<_Real, _dim>(m_storage);
    }

    friend std::ostream &operator<<(std::ostream &os, const DynamicField &f) {
        for (size_t j = 0; j < f.domainSize(); ++j) {
            for (size_t i = 0; i < f.dim(); ++i)
                os << (i ? "\t" : "") << f(i, j);
            os << std::endl;
        }
        return os;
    }

private:
    size_t m_dim;
    std::vector<_Real> m_storage;
};

#endif // FIELDS_HH


#include <assert.h>
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>

#include <NTL/GF2X.h>
#include <NTL/GF2XFactoring.h>
#include <NTL/GF2E.h>
#include <NTL/GF2EX.h>
#include <NTL/GF2EXFactoring.h>

#include "gcm.h"

using namespace std;
using namespace NTL;

int main(int argc, char **argv)
{
    struct slice a1, c1, t1, a2, c2, t2;
    struct slice *sp[] = {&a1, &c1, &t1, &a2, &c2, &t2};
    int i;
    GF2EX p, p1, p2;
    vec_pair_GF2EX_long factors;
    char out[33];

    argc -= 1;
    argv += 1;

    assert(argc == 6);

    for (i = 0; i < argc; i += 1) {
        hex2bytes(sp[i], argv[i]);
    }

    initfield();
    buildpoly(p1, &a1, &c1, &t1);
    buildpoly(p2, &a2, &c2, &t2);
    p = p1 + p2;
    MakeMonic(p);
    CanZass(factors, p);

    for (i = 0; i < factors.length(); i += 1) {
        if (deg(factors[i].a) == 1) {
            felem2hex(out, factors[i].a[0]);
            printf("%s\n", out);
        }
    }

    return 0;
}


#include <iostream>
#include <vector>
#include <math.h>

#include <boost/timer.hpp>

#include "NRGclasses.hpp"
#include "NRGfunctions.hpp"

#include "TwoChQS.hpp"

void TwoChQS_DiagHN(vector<double> Params,
		     CNRGbasisarray* pAbasis,CNRGbasisarray* pSingleSite,
		     CNRGmatrix* Qm1fNQ, CNRGarray* pAeig){

  CNRGmatrix HN(*pAbasis);
  double OldEl[2];

  double chi_N[2]={Params[0],Params[1]};
  double Lambda=Params[2];
  double auxEl;

  // Check time
  boost::timer t;

  // New approach
  HN.UpperTriangular=true;

  int icount=0;
  for (int ibl=0;ibl<pAbasis->NumBlocks();ibl++)
    {
      // HN is block diagonal (ibl,ibl) so we are setting the blocks
      HN.MatBlockMap.push_back(ibl);
      HN.MatBlockMap.push_back(ibl);

      // Each MatBlock is Nbl x Nbl in lenght
      HN.MatBlockBegEnd.push_back(icount);
      //icount+=pAbasis->GetBlockSize(ibl)*pAbasis->GetBlockSize(ibl);
      // New
      int sizebl=pAbasis->GetBlockSize(ibl);
      if (HN.UpperTriangular)
 	icount+=(sizebl*(sizebl+1))/2;
      else
	icount+=sizebl*sizebl;

      HN.MatBlockBegEnd.push_back(icount-1);

      cout << "  Setting up H_(N = " << pAbasis->Nshell << ") Block : " << ibl ;
      cout << " of " << pAbasis->NumBlocks();
      cout << "  size: " << pAbasis->GetBlockSize(ibl) << endl;
      t.restart();

      double Q1i=pAbasis->GetQNumber(ibl,0);
      double Si=pAbasis->GetQNumber(ibl,1);

      // Calculate matrix elements
      for (int ist=pAbasis->GetBlockLimit(ibl,0);
	   ist<=pAbasis->GetBlockLimit(ibl,1);ist++)
	{
	  int type_i=pAbasis->iType[ist];

// 	  for (int jst=pAbasis->GetBlockLimit(ibl,0);
//  	       jst<=pAbasis->GetBlockLimit(ibl,1);jst++)
//       NEW: calculate only half of the matrix elements
//
	  int j0;
	  // NEW THING! only calculates half of matrix els!
	  if (HN.UpperTriangular) j0=ist;
	  else j0=pAbasis->GetBlockLimit(ibl,0);
	  for (int jst=j0;
	           jst<=pAbasis->GetBlockLimit(ibl,1);jst++)
	    {
	      int type_j=pAbasis->iType[jst];

	      if (ist==jst)   // Diagonal terms
		{
		  auxEl=sqrt(Lambda)*(pAbasis->dEn[ist]);
		  HN.MatEl.push_back(auxEl);
		}
	      else 		  //Off-diagonal terms
		{
		  // Loop in channels
		  auxEl=0.0;
		  for (int ich=1;ich<=2;ich++)
		    {
		      OldEl[ich-1]=Qm1fNQ[ich-1].GetMatEl(pAbasis->StCameFrom[ist],pAbasis->StCameFrom[jst]);
		      int typep=type_i;
		      int type=type_j;

		      // if zero, try the h.c term
		      if (dEqual(fabs(OldEl[ich-1]),0.0))
			{
			  OldEl[ich-1]=Qm1fNQ[ich-1].GetMatEl(pAbasis->StCameFrom[jst],
						       pAbasis->StCameFrom[ist]);
			  typep=type_j;
			  type=type_i;
			}

		      // Will this work?
		      // if still zero, next.
		      //if (dEqual(fabs(OldEl[ich-1]),0.0)) break;

		      // Get SingleSite QNumbers
		      int pos=0;
		      int iblssp=pSingleSite->GetBlockFromSt(typep,pos);
		      int iblss=pSingleSite->GetBlockFromSt(type,pos);

		      double Qtildep=pSingleSite->GetQNumber(iblssp,0);
		      double Stildep=pSingleSite->GetQNumber(iblssp,1);
		      double Sztildep=pSingleSite->GetQNumber(iblssp,2);

		      double Qtilde=pSingleSite->GetQNumber(iblss,0);
		      double Stilde=pSingleSite->GetQNumber(iblss,1);
		      double Sztilde=pSingleSite->GetQNumber(iblss,2);

		      double Soldp=Si-Sztildep;
		      double Sold=Si-Sztilde;

		      // Check Fermi Sign
		      double FermiSign=1.0;
		      // will be -1 if only one of them is zero (A XOR B)
		      // Check this one.
		      // Fermi sign now depends on which type the thing is!!!
                      // In other words, it will be -1 if the state has only one
		      // electron in only one of the channels
		      // Single site states +-1,+-1/2!!
                      //
		      if ( (dEqual(Sztilde,0.5)||dEqual(Sztilde,-0.5)) ) FermiSign=-1.0;

		      double siteqnumsp[]={Qtildep,Stildep,Sztildep};
		      double siteqnums[]={Qtilde,Stilde,Sztilde};

		      //Loop in spins
		      for (int sigma=-1;sigma<=1;sigma+=2)
			{
			  double dSigma=0.5*(double)sigma;
			  double FullMatEl=0.0;
			  double auxCG[]={0.0,0.0,0.0,0.0};

			  // Loop in Szold
			  for (double Szold=Sold;Szold>=-Sold;Szold-=1.0)
			    {
			      double Szoldp=Szold-dSigma;
			      Sztilde=Si-Szold;
			      Sztildep=Sztilde+dSigma;
			      // Changing dSigma changes Sztildep
			      // Need an extra loop in the "p" block

			      // Site matrix element: finds the block
			      siteqnums[2]=Sztilde;
			      siteqnumsp[2]=Sztildep;
			      int siteblock=pSingleSite->GetBlockFromQNumbers(siteqnums);
			      int siteblockp=pSingleSite->GetBlockFromQNumbers(siteqnumsp);

// 			      if ((ist==302)&&( (jst==304)||(jst==305) ))
// 				cout << " ist = " << ist
// 				     << " jst = " << jst
// 				     << " ich = " << ich
// 				     << " sigma = " << sigma
// 				     << " Szold = " << Szold
// 				     << " Sztildep = " << Sztildep
// 				     << " Sztilde = " << Sztilde
// 				     << endl;

			      if ( (dLEqual(fabs(Sztildep),Stildep))&&
				   (dLEqual(fabs(Szold),Sold))&&
				   (dLEqual(fabs(Szoldp),Soldp)) )
				{
				  // CG coefs
				  auxCG[0]=CGordan(Sold,Szold,
						   Stilde,Sztilde,
						   Si,Si);

				  auxCG[1]=CGordan(Soldp,Szoldp,
						   Stildep,Sztildep,
						   Si,Si);

				  auxCG[2]=CGordan(Soldp,Szoldp,
						   0.5,dSigma,
						   Sold,Szold);

				  //Loop in site blocks!! Necessary!
				  for (int sitestatep=pSingleSite->GetBlockLimit(siteblockp,0);sitestatep<=pSingleSite->GetBlockLimit(siteblockp,1);sitestatep++)
				    {
				      for (int sitestate=pSingleSite->GetBlockLimit(siteblock,0);sitestate<=pSingleSite->GetBlockLimit(siteblock,1);sitestate++)
					{
					  double SpSm[2]={0.0,0.0};
					  SpSm[0]=TwoChQS_SpSm_table(sitestate,type);
					  SpSm[1]=TwoChQS_SpSm_table(sitestatep,typep);
					  auxCG[3]=TwoChQS_fd_table(ich,sigma,
								    sitestatep,sitestate);
//  					  if ((ist==302)&&( (jst==304)||(jst==305) ))
// 					    cout << " ist = " << ist
// 						 << " jst = " << jst
// 						 << " ich = " << ich
// 						 << " sigma = " << sigma
// 						 << " fd_table("
// 						 << sitestatep << ","
// 						 << sitestate << ") = "
// 						 << auxCG[3] << endl
// 						 << " SpSm1("
// 						 << sitestate << "," << type
// 						 << ") = " << SpSm[0]
// 						 << endl
// 						 << " SpSm2("
// 						 << sitestatep << "," << typep
// 						 << ") = " << SpSm[1]
// 						 << endl;

					  FullMatEl+=auxCG[0]*auxCG[1]*auxCG[2]*auxCG[3]*FermiSign*SpSm[0]*SpSm[1];

					}
				      // end loop in site block
				    }
				  // end loop in site blockp
				}
			      // END Calc coefs safeguard
			    }
			  // End loop in Szold

			auxEl+=chi_N[ich-1]*OldEl[ich-1]*FullMatEl;

// 			if ((ist==302)&&( (jst==304)||(jst==305) ))
// 			  cout << " ich = " << ich
// 			       << " OldEl = " << OldEl[ich-1]
// 			       << " FullMatEl = " << FullMatEl
// 			       << " auxEl = " << auxEl
// 			       << endl;

			}
		      // end loop in sigma
		    }
		  //end loop in channels

		  HN.MatEl.push_back(auxEl);
		  // NEW: add jst,ist as well
		  //HN.MatEl.push_back(auxEl);

		}
	      // END if ist=jst
	    }
	  //END loop in jst
	}
      // END loop in ist

      cout << " ...Done. Set-up Time: " << t.elapsed() << endl;
    }
  // END Loop in blocks (ibl)

  cout << "Updating Aeig " << endl;
  pAeig->ClearAll();

  // Syncronize with HN
  *pAeig=HN;

  // Set dEn,dEigVec
  pAeig->dEn.clear();
  pAeig->dEigVec.clear();

  for (int ibl=0;ibl<HN.NumBlocks();ibl++){
      //// NEW
    //       cout << " Regularizing block " << ibl << " of " << HN.NumBlocks()-1 << endl;
    //       HN.PutInRegularForm(ibl);
    //pAbasis->PrintBlockBasis(ibl);
    //HN.PrintMatBlock(ibl,ibl);
    cout << " Diagonalizing block " << ibl << " of " << HN.NumBlocks()-1 << endl;
    HN.DiagBlock(ibl,pAeig->dEn,pAeig->dEigVec);
    //pAeig->PrintBlockEn(ibl);
  }

  //uset this later
  pAeig->SetE0zero();

}
// end subroutine
////////////////////////
////////////////////////
////////////////////////

// Scrap code
			  // Sum in Sztilde
// 			  for (Sztilde=-Stilde;Sztilde<=Stilde;Sztilde+=1.0)
// 			    {
// 			      double Sztildep=Sztilde+dSigma;
// 			      double Szold=Si-Sztilde;
// 			      double Szoldp=Si-Sztildep;

// 			      // CG coefs
// 			      auxCG[0]=CGordan(Sold,Szold,
// 					       Stilde,Sztilde,Si,Si);
// 			      auxCG[1]=CGordan(Soldp,Szoldp,
// 					       Stildep,Sztildep,Si,Si);
// 			      auxCG[2]=CGordan(Soldp,Szoldp,
// 					       0.5,dSigma,Sold,Szold);
// 			      // Site matrix element
// 			      siteqnums[2]=Sztilde;
// 			      siteqnumsp[2]=Sztildep;
// 			      int sitestate=pSingleSite->GetBlockFromQNumbers(siteqnums);
// 			      int sitestatep=pSingleSite->GetBlockFromQNumbers(siteqnumsp);
// 			      auxCG[3]=TwoChQS_fd_table(ich,sigma,
// 							sitestatep,sitestate);

// 			      FullMatEl+=auxCG[0]*auxCG[1]*auxCG[2]*OldEl[ich-1]*auxCG[3]*FermiSign;

// 			    }
// 			  // END Loop in Sztilde


/*!
 * @file
 * @brief
 * @copyright alphya 2020-2021
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#ifndef NYARUGA_UTIL_CARTESIAN_PRODUCT_HPP
#define NYARUGA_UTIL_CARTESIAN_PRODUCT_HPP

#pragma once

// this code almost from
// https://stackoverflow.com/questions/16686942/how-to-iterate-over-two-stl-like-containers-cartesian-product

// ネストした for 文が書きやすくなります
// 速度はブレ幅が大きいですが、for 文をネストした場合より 2 倍以上遅くなることはないと思います
// パソコンの調子によって、1.5 倍くらいだったり、同じくらいだったりしました

// 例

/*
    array b = { false, true };
    array i = { 0, 1 };
    array s = { "Hello", "World" };

    // i, j, k は、それぞれの添え字に関してネストした for 文と
    // 同じような添え字を走る(直積)
    // 引数はイテレータが使える型や組み込みの配列なら OK
    // (使用する操作：std::end(), std::begin(), it++)
    for (auto&& [i,j,k]: cartesian_product(b,i,s))
    {
        std::cout << std::boolalpha << i << ", ";
        std::cout << j << ", ";
        std::cout << k << "\n";
    }

    //   for (int i = 0; i != 2; ++i)
    //   for (int j = 1; j != 3; ++j)
    //   for (int k = 2; k != 4; ++k)
   for (auto&& [i,j,k]: cartesian_product({0,2},{1,3},{2,4}))
    {
        std::cout << i << ", ";
        std::cout << j << ", ";
        std::cout << k << "\n";
    }

*/

#include <tuple>                        // make_tuple, tuple
#include <utility>                      // pair
#include <vector>                       // vector
#include <boost/coroutine2/coroutine.hpp>

namespace nyaruga {

namespace util {

/*

namespace impl1 { // コルーチン実装

namespace detail {

// the lambda is fully bound with one element from each of the ranges
template<class Op>
void insert_tuples(Op&& op) noexcept
{
    // evaluating the lambda will insert the currently bound tuple
    op();
}

// "peal off" the first range from the remaining tuple of ranges
template<class Op, class InputIterator1, class... InputIterator2>
void insert_tuples(Op&& op, std::pair<InputIterator1, InputIterator1> head, std::pair<InputIterator2, InputIterator2>... tail)
{
    // "peal off" the elements from the first of the remaining ranges
    // NOTE: the recursion will effectively generate the multiple nested for-loops
    for (auto it = head.first; it != head.second; ++it) {
        // bind the first free variable in the lambda, and
        // keep one free variable for each of the remaining ranges
        detail::insert_tuples(
            [&op,&it](InputIterator2... elems) { op(it, elems...); },
            tail...
        );
    }
}

}   // namespace detail

// convert a tuple of ranges to the range of tuples representing the Cartesian product
template<class... InputIterator>
void cartesian_product_impl(auto & sink, std::pair<InputIterator, InputIterator>&&... dimensions)
{
    detail::insert_tuples(
         [&sink](InputIterator... elems) { sink(std::make_tuple(*elems...)); },
         std::forward<decltype(dimensions)>(dimensions)...
    );
}

template<class...Args>
decltype(auto) cartesian_product(Args&&... args)
{
    using coro_type = boost::coroutines2::coroutine<
        typename std::tuple<std::remove_reference_t<decltype(*std::begin(std::declval<Args&>()))>...>
    >;

    return typename coro_type::pull_type(
    [...args=std::forward<Args>(args)](typename coro_type::push_type & sink)
    {
        cartesian_product_impl(sink, std::make_pair(std::begin(args), std::end(args))...);
    });
};
} //namespace impl1

*/

// from: https://stackoverflow.com/questions/16686942/how-to-iterate-over-two-stl-like-containers-cartesian-product
namespace impl2 { // ほぼ stack overflow から

namespace detail {

// the lambda is fully bound with one element from each of the ranges
template<class Op>
void insert_tuples(Op&& op)
{
        // evaluating the lambda will insert the currently bound tuple
        op();
}

// "peal off" the first range from the remaining tuple of ranges
template<class Op, class InputIterator1, class... InputIterator2>
void insert_tuples(Op&& op, std::pair<InputIterator1, InputIterator1> head, std::pair<InputIterator2, InputIterator2>... tail)
{
        // "peal off" the elements from the first of the remaining ranges
        // NOTE: the recursion will effectively generate the multiple nested for-loops
        for (auto it = head.first; it != head.second; ++it) {
                // bind the first free variable in the lambda, and
                // keep one free variable for each of the remaining ranges
                detail::insert_tuples(
                        [&op,&it](InputIterator2... elems) mutable { op(it, elems...); },
                        tail...
                );
        }
}

// convert a tuple of ranges to the range of tuples representing the Cartesian product
template<class OutputIterator, class... InputIterator>
void cartesian_product_impl(OutputIterator result, std::pair<InputIterator, InputIterator>... dimensions)
{
        insert_tuples(
                 [=](InputIterator... elems) mutable { *result++ = std::make_tuple(*elems...); },
                 dimensions...
        );
}

}       // namespace detail

template <typename...Args>
auto cartesian_product(Args&&...args)
{
    std::vector< typename std::tuple<std::remove_reference_t<decltype(*std::begin(std::declval<Args&>()))>...>  > result;
    detail::cartesian_product_impl(
            std::back_inserter(result),
            std::make_pair(std::begin(args), std::end(args))...
    );
    return result;
}

} // namespace impl2 (almost original)

using namespace impl2; // impl1 より倍速い場合があった 速度はパソコンの調子によって大きく左右される

} // namespace nyaruga::util

/* how to use

#include <string>
#include <iostream>
#include <array>
#include <stdio.h>
#include <chrono>

using std::array;

int main()
{
    array b = { false, true };
    array i = { 0, 1 };
    array s = { "Hello", "World" };

    using namespace std;
    chrono::system_clock::time_point start, end;

    start = chrono::system_clock::now();

    for (int f = 0; f < 1000; ++f)
    // now use a single flat loop over result to do your own thing
    for (auto&& [i,j,k]: cartesian_product(b,i,s)) {
        std::cout << std::boolalpha << i << ", ";
        std::cout << j << ", ";
        std::cout << k << "\n";
    }

        // 何かの処理

    end = chrono::system_clock::now();

    double time = static_cast<double>(chrono::duration_cast<chrono::microseconds>(end - start).count() / 1000.0);
    printf("time %lf[ms]\n", time);

}

*/

#include <initializer_list>

namespace nyaruga::util {

namespace detail_index {

template<class Op>
void insert_tuples(Op&& op)
{
        op();
}

void insert_tuples(auto&& op,auto head, auto... tail)
{
    for (auto i = *head.begin(); i < *(head.begin()+1); ++i) {
        insert_tuples(
            [&op,&i](typename decltype(tail)::value_type... elems) mutable { op(i, elems...); },
            tail...
        );
    }
}

void cartesian_product_impl(auto&& result, auto&&... dimensions)
{
    insert_tuples(
         [=](auto... elems) mutable { *result++ = std::make_tuple(elems...); },
         std::forward<decltype(dimensions)>(dimensions)...
    );
}

}       // namespace detail

template<typename... T>
auto cartesian_product(std::initializer_list<T>...args)
{
    std::vector<typename std::tuple<T...>> result;
    detail_index::cartesian_product_impl(std::back_inserter(result), args...);
    return result;
}

}

/* how to use
#include <string>
#include <iostream>
#include <array>
#include <stdio.h>
#include <chrono>

using std::array;

int main()
{

    using namespace std;
    chrono::system_clock::time_point start, end;

    start = chrono::system_clock::now();

    for (int f = 0; f < 1000; ++f)

   for (auto&& [i,j,k]: cartesian_product({0,2},{1,3},{2,4}))
//   equal to
//   for (int i = 0; i != 2; ++i)
//   for (int j = 1; j != 3; ++j)
//   for (int k = 2; k != 4; ++k)
    {
        std::cout << i << ", ";
        std::cout << j << ", ";
        std::cout << k << "\n";
    }

    end = chrono::system_clock::now();

    double time = static_cast<double>(chrono::duration_cast<chrono::microseconds>(end - start).count() / 1000.0);
    printf("time %lf[ms]\n", time);

}
*/

#endif // NYARUGA_UTPL_CARTESIAN_PRODUCT_HPP


#pragma once

#include <Eigen/Dense>

#include "perceive/foundation.hpp"
#include "perceive/utils/sdbm-hash.hpp"
#include "vector-2.hpp"
#include "vector-3.hpp"

namespace perceive
{
// An "axis-aligned" bounding box
#pragma pack(push, 1)
template<typename T> class AABBT
{
 public:
   CUSTOM_NEW_DELETE(AABBT)

   using value_type = T;

   T left{T(0.0)}, top{T(0.0)}, right{T(0.0)}, bottom{T(0.0)};

   AABBT() noexcept             = default;
   AABBT(const AABBT&) noexcept = default;
   AABBT(AABBT&&) noexcept      = default;
   ~AABBT()                     = default;
   AABBT& operator=(const AABBT&) noexcept = default;
   AABBT& operator=(AABBT&&) noexcept = default;

   AABBT(T in_left, T in_top, T in_right, T in_bottom)
   noexcept
       : left(in_left)
       , top(in_top)
       , right(in_right)
       , bottom(in_bottom)
   {}
   AABBT(Vector2T<T> lefttop, Vector2T<T> rightbottom)
   noexcept
       : left(lefttop.x)
       , top(lefttop.y)
       , right(rightbottom.x)
       , bottom(rightbottom.y)
   {}

   template<class Iterator> AABBT(Iterator first, Iterator last) noexcept
   {
      *this = minmax();
      union_with(first, last);
   }

   static AABBT nan() noexcept { return AABBT(T(NAN), T(NAN), T(NAN), T(NAN)); }
   static AABBT minmax() noexcept
   {
      auto mmax = std::numeric_limits<T>::max();
      auto mmin = std::numeric_limits<T>::lowest();
      return AABBT<T>(mmax, mmax, mmin, mmin);
   }

   bool is_finite() const noexcept
   {
      return true && std::isfinite(left) && std::isfinite(top)
             && std::isfinite(right) && std::isfinite(bottom);
   }

   // Union a point with the AABB
   void union_point(T x, T y) noexcept
   {
      if(std::isfinite(x)) {
         if(x < left) left = x;
         if(x > right) right = x;
      }
      if(std::isfinite(y)) {
         if(y < top) top = y;
         if(y > bottom) bottom = y;
      }
   }

   void union_point(const Vector2T<T>& p) noexcept { union_point(p.x, p.y); }
   void union_point(const T p[2]) noexcept { union_point(p[0], p[1]); }

   bool contains(T x, T y) const noexcept
   {
      if constexpr(std::is_floating_point<T>::value) {
         return is_close_between(x, left, right)
                and is_close_between(y, top, bottom);
      }
      return (x >= left) && (x <= right) && (y >= top) && (y <= bottom);
   }
   bool contains(const Vector2T<T>& p) const noexcept
   {
      return contains(p.x, p.y);
   }
   bool contains(const T p[2]) const noexcept { return contains(p[0], p[1]); }

   template<class Iterator>
   void union_with(Iterator first, Iterator last) noexcept
   {
      while(first != last) union_point(*first++);
   }

   T w() const noexcept
   {
      return right - left;
      if constexpr(std::is_integral_v<T>)
         return abs(right - left);
      else
         return fabs(right - left);
   }
   T h() const noexcept
   {
      return bottom - top;
      if constexpr(std::is_integral_v<T>)
         return abs(bottom - top);
      else
         return fabs(bottom - top);
   }
   T width() const noexcept { return w(); }
   T height() const noexcept { return h(); }

   T area() const noexcept { return w() * h(); }

   AABBT& grow(T inc) noexcept
   {
      left -= inc;
      top -= inc;
      right += inc;
      bottom += inc;
      return *this;
   }

   static AABBT intersection(const AABBT& a, const AABBT& b) noexcept
   {
      AABBT<T> c = a;

      c.left   = std::max(a.left, b.left);
      c.right  = std::min(a.right, b.right);
      c.top    = std::max(a.top, b.top);
      c.bottom = std::min(a.bottom, b.bottom);

      if(c.w() < 0) {
         c.left  = T(0.5 * double(c.left + c.right));
         c.right = c.left;
      }

      if(c.h() < 0) {
         c.top    = T(0.5 * double(c.top + c.bottom));
         c.bottom = c.top;
      }

      return c;
   }

   static T intersection_area(const AABBT& a, const AABBT& b) noexcept
   {
      AABBT<T> c = a;

      c.left   = std::max(a.left, b.left);
      c.right  = std::min(a.right, b.right);
      c.top    = std::max(a.top, b.top);
      c.bottom = std::min(a.bottom, b.bottom);

      T w = c.w();
      T h = c.h();

      return (w > 0.0 && h > 0.0) ? (w * h) : T(0.0);
   }

   // Raster order in images has a "flipped" y-axis
   AABBT reflect_raster() const noexcept
   {
      AABBT o = *this;
      std::swap(o.top, o.bottom);
      return o;
   }

   T intersection_area(const AABBT& o) const noexcept
   {
      return intersection_area(*this, o);
   }

   static T union_area(const AABBT& a, const AABBT& b) noexcept
   {
      return a.area() + b.area() - intersection_area(a, b);
   }
   T union_area(const AABBT& o) const noexcept { return union_area(*this, o); }

   Vector2T<T> centre() const noexcept
   {
      return Vector2T<T>(T(left + 0.5 * (right - left)),
                         T(top + 0.5 * (bottom - top)));
   }
   Vector2T<T> center() const noexcept { return centre(); }

   real aspect_ratio() const noexcept { return real(width()) / real(height()); }

   // Scale the AABB by some size
   AABBT& operator*=(T scalar) noexcept
   {
      Vector2T<T> c = centre();
      T w           = this->w() * scalar * 0.5;
      T h           = this->h() * scalar * 0.5;
      left          = c.x - w;
      right         = c.x + w;
      top           = c.y - h;
      bottom        = c.y + h;
      return *this;
   }

   AABBT& operator/=(T scalar) noexcept { return *this *= (1.0 / scalar); }
   AABBT operator*(T scalar) const noexcept
   {
      AABBT res(*this);
      res *= scalar;
      return res;
   }
   AABBT operator/(T scalar) const noexcept
   {
      AABBT res(*this);
      res /= scalar;
      return res;
   }

   Vector2T<T> left_top() const noexcept { return Vector2T<T>(left, top); }
   Vector2T<T> top_left() const noexcept { return Vector2T<T>(left, top); }
   Vector2T<T> right_bottom() const noexcept
   {
      return Vector2T<T>(right, bottom);
   }
   Vector2T<T> bottom_right() const noexcept
   {
      return Vector2T<T>(right, bottom);
   }

   Vector2T<T> corner(int index) const noexcept
   {
      switch(index) {
      case 0: return Vector2T<T>(left, bottom);
      case 1: return Vector2T<T>(left, top);
      case 2: return Vector2T<T>(right, top);
      case 3: return Vector2T<T>(right, bottom);
      }
      FATAL("kBAM!");
      return {};
   }

   std::array<Vector2T<T>, 4> to_array_polygon() const noexcept
   {
      std::array<Vector2T<T>, 4> X;
      X[0] = Vector2T<T>(left, top);
      X[1] = Vector2T<T>(right, top);
      X[2] = Vector2T<T>(right, bottom);
      X[3] = Vector2T<T>(left, bottom);
      return X;
   }

   vector<Vector2T<T>> to_polygon() const noexcept(false)
   {
      vector<Vector2T<T>> X(4);
      X[0] = Vector2T<T>(left, top);
      X[1] = Vector2T<T>(right, top);
      X[2] = Vector2T<T>(right, bottom);
      X[3] = Vector2T<T>(left, bottom);
      return X;
   }

   Vector3T<T> left_line() const noexcept
   {
      T l = this->left, t = this->top, r = this->right, b = this->bottom;
      return to_homgen_line(Vector3T<T>(l, t, 1), Vector3T<T>(l, b, 1));
   }

   Vector3T<T> right_line() const noexcept
   {
      T l = this->left, t = this->top, r = this->right, b = this->bottom;
      return to_homgen_line(Vector3T<T>(r, t, 1), Vector3T<T>(r, b, 1));
   }

   Vector3T<T> top_line() const noexcept
   {
      T l = this->left, t = this->top, r = this->right, b = this->bottom;
      return to_homgen_line(Vector3T<T>(l, t, 1), Vector3T<T>(r, t, 1));
   }

   Vector3T<T> bottom_line() const noexcept
   {
      T l = this->left, t = this->top, r = this->right, b = this->bottom;
      return to_homgen_line(Vector3T<T>(l, b, 1), Vector3T<T>(r, b, 1));
   }

   Vector3T<T> line(int ind) const noexcept
   {
      switch(ind) {
      case 0: return left_line();
      case 1: return top_line();
      case 2: return right_line();
      case 3: return bottom_line();
      }
      return left_line();
   }

   bool operator!=(const AABBT<T>& rhs) const noexcept
   {
      return !(*this == rhs);
   }
   bool operator==(const AABBT<T>& rhs) const noexcept
   {
      return is_close(left, rhs.left) and is_close(top, rhs.top)
             and is_close(right, rhs.right) and is_close(bottom, rhs.bottom);
   }

   AABBT& set_to(const T& l, const T& t, const T& r, const T& b) noexcept
   {
      left   = l;
      top    = t;
      right  = r;
      bottom = b;
      return *this;
   }
   AABBT& set_to(T a[4]) noexcept
   {
      set_to(a[0], a[1], a[2], a[3]);
      return *this;
   }

   T* copy_to(T a[4]) const noexcept
   {
      a[0] = left;
      a[1] = top;
      a[2] = right;
      a[3] = bottom;
      return a;
   }

   T* ptr() noexcept { return &left; }
   const T* ptr() const noexcept { return &left; }
   T& operator[](int idx) noexcept
   {
#ifdef DEBUG_BUILD
      assert(idx >= 0 && idx < 4);
#endif
      return ptr()[idx];
   }

   const T& operator[](int idx) const noexcept
   {
#ifdef DEBUG_BUILD
      assert(idx >= 0 && idx < 4);
#endif
      return ptr()[idx];
   }

   T& operator()(int idx) noexcept
   {
#ifdef DEBUG_BUILD
      assert(idx >= 0 && idx < 4);
#endif
      return ptr()[idx];
   }
   const T& operator()(int idx) const noexcept
   {
#ifdef DEBUG_BUILD
      assert(idx >= 0 && idx < 4);
#endif
      return ptr()[idx];
   }

   unsigned size() const noexcept { return 4; }

   std::string to_string(const char* fmt = "[{} {} {} {}]") const
   {
      return format(fmt, left, top, right, bottom);
   }

   std::string to_str() const { return to_string(); }

   size_t hash() const noexcept { return sdbm_hash(ptr(), sizeof(T) * size()); }

   friend std::string str(const AABBT<T>& aabb) { return aabb.to_string(); }
};
#pragma pack(pop)

template<typename T>
AABBT<T> intersection(const AABBT<T>& a, const AABBT<T>& b) noexcept
{
   return AABBT<T>::intersection(a, b);
}

template<typename T>
T intersection_area(const AABBT<T>& a, const AABBT<T>& b) noexcept
{
   return AABBT<T>::intersection_area(a, b);
}

template<typename T> T union_area(const AABBT<T>& a, const AABBT<T>& b) noexcept
{
   return AABBT<T>::union_area(a, b);
}

// Scalar multiples
template<typename T> AABBT<T> operator*(float a, const AABBT<T>& v) noexcept
{
   return v * a;
}
template<typename T> AABBT<T> operator/(float a, const AABBT<T>& v) noexcept
{
   return v / a;
}
template<typename T> AABBT<T> operator*(double a, const AABBT<T>& v) noexcept
{
   return v * a;
}
template<typename T> AABBT<T> operator/(double a, const AABBT<T>& v) noexcept
{
   return v / a;
}

// Intersection of homogeneous line ('line') with AABB
template<typename T>
std::pair<Vector2T<T>, Vector2T<T>>
intersect(const AABBT<T>& aabb, const Vector3T<T>& in_line) noexcept
{
   array<Vector3T<T>, 4> lines;
   array<Vector3T<T>, 4> isects;
   array<bool, 4> bounded;
   std::pair<Vector2T<T>, Vector2T<T>> ret
       = {Vector2T<T>::nan(), Vector2T<T>::nan()};

   const auto line = in_line.normalised_line();

   { // What are the lines of the AABB?, the order is [left, top, right, bot]
      int counter = 0;
      std::generate(
          begin(lines), end(lines), [&]() { return aabb.line(counter++); });
   }

   { // Where does 'line' intersect with each line of the AABB?
      std::transform(
          cbegin(lines), cend(lines), begin(isects), [&](const auto& ll) {
             return cross(line, ll).normalise_point();
          });
   }

   { // Are inserections on the border of the AABB?
     // Point 'X' is between lines 'l1' and 'l2'
      auto is_between = [&](const Vector3T<T>& X,
                            const Vector3T<T>& l1,
                            const Vector3T<T>& l2) -> bool {
         return X.is_finite() && (dot(X, l1) < 0.0) == (dot(X, l2) < 0.0);
      };

      bounded[0] = is_close_between(isects[0].y, aabb.top, aabb.bottom);
      bounded[1] = is_close_between(isects[1].x, aabb.left, aabb.right);
      bounded[2] = is_close_between(isects[2].y, aabb.top, aabb.bottom);
      bounded[3] = is_close_between(isects[3].x, aabb.left, aabb.right);
   }

   unsigned pos     = 0;
   auto push_result = [&](const Vector3T<T>& X) {
      if(pos < 2) {
         auto& p = (pos++ == 0) ? ret.first : ret.second;
         p(0)    = X(0);
         p(1)    = X(1);
      } else {
         const auto v  = Vector2T<T>(X(0), X(1));
         const auto q0 = (ret.first - ret.second).quadrance();
         const auto q1 = (ret.first - v).quadrance();
         if(q1 > q0) ret.second = v;
      }
   };

   for(size_t i = 0; i < 4; ++i)
      if(bounded[i]) push_result(isects[i]);

   if(false) {
      INFO("FEEDBACK");
      cout << format("AABB  = {}", str(aabb)) << endl;
      cout << format("line  = {}", str(line)) << endl;
      for(auto i = 0; i < 4; ++i) {
         cout << format(" {:c} ll = {}, isect = {{}, {}}",
                        (bounded[size_t(i)] ? '*' : ' '),
                        str(lines[size_t(i)]),
                        isects[size_t(i)].x,
                        isects[size_t(i)].y)
              << endl;
      }
      cout << format("out   = {{}, {}} -> {{}, {}}",
                     ret.first.x,
                     ret.first.y,
                     ret.second.x,
                     ret.second.y)
           << endl
           << endl;
   }

   return ret;
}

template<typename T>
std::pair<Vector2T<T>, Vector2T<T>> intersect(const AABBT<T>& aabb,
                                              const Vector2T<T>& A,
                                              const Vector2T<T>& B) noexcept
{
   const bool A_in = aabb.contains(A);
   const bool B_in = aabb.contains(B);

   if(A_in and B_in) return {A, B};

   const auto ll     = to_homgen_line(A, B);
   const auto [U, V] = intersect(aabb, ll);

   auto orthogonal_line_at = [&](const auto& U) -> auto
   {
      return Vector3T<T>(-ll.y, ll.x, ll.y * U.x - ll.x * U.y);
   };

   auto is_between_A_B = [&](const auto& U) -> bool {
      const auto tt = orthogonal_line_at(U);
      const auto da = tt.x * A.x + tt.y * A.y + tt.z;
      const auto db = tt.x * B.x + tt.y * B.y + tt.z;
      return std::signbit(da) != std::signbit(db);
   };

   auto subline
       = [&](const auto& X, const auto& Y, const auto& U, const auto& V) {
            return is_between_A_B(U) ? U : V;
         };

   if(A_in)
      return {A, subline(A, B, U, V)};
   else if(B_in)
      return {subline(B, A, U, V), B};
   if(is_between_A_B(U) and is_between_A_B(V)) return {U, V};
   return {Vector2T<T>::nan(), Vector2T<T>::nan()};
}

// The returned homography maps points in the 'src' AABB to the 'dst' AABB
template<typename T>
inline Eigen::Matrix3d rescale_homography(const AABBT<T>& src,
                                          const AABBT<T>& dst) noexcept
{
   Eigen::Matrix3d H = Eigen::Matrix3d::Identity();
   H(0, 0)           = dst.width() / src.width();
   H(1, 1)           = dst.height() / src.height();
   H(0, 2)           = dst.left - H(0, 0) * src.left;
   H(1, 2)           = dst.top - H(1, 1) * src.top;

   // auto tryit = [&] (Vector2 s, Vector2 d) {
   //     Vector3r x = H * Vector3r(s.x, s.y, 1.0);
   //     x /= x(2);
   //     cout << format("{} => {} ({}, {})",
   //                    s.to_string(), d.to_string(), x(0), x(1))
   //     << endl;
   // };

   // tryit(src.top_left(), dst.top_left());
   // tryit(src.right_bottom(), dst.right_bottom());

   return H;
}

template<typename T>
inline bool in_bounding_box(const Vector3T<T>& X,
                            const Vector3T<T>& A,
                            const Vector3T<T>& B)
{
   for(auto i = 0; i < 3; ++i)
      if(X(i) < A(i)) return false;
   for(auto i = 0; i < 3; ++i)
      if(X(i) > B(i)) return false;
   return true;
};

} // namespace perceive


#include <fstream>
#include <string>

#include <polycrypto/PolyCrypto.h>

#include <xutils/Utils.h>
#include <xutils/Log.h>

#include <boost/multiprecision/cpp_int.hpp>

using namespace boost::multiprecision;
using namespace std;
using namespace libpolycrypto;

/**
 * How do we reason about bandwidth in (t, n) DKG? (i.e., need t shares to reconstruct)
 *  - c   = the size of a poly commitment
 *  - s   = the size of a share
 *  - p   = the size of a proof for a share
 *  - f0c = the size of a Feldman commitment g^f(0) to f(0)
 *  - f0p = the size of the proof for g^f(0) (includes NIZKPoK for Kate et al)
 */

/**
 *  DOWNLOAD
 *  --------
 *  Each player i\in [n] needs to obtain from every other player j \ne i: a poly commitment + share + proof + f(0) comm + f(0) proof
 *   - per-player: (n-1)*(c + s + p + f0c + f0p) commitments + shares + proofs need to be downloaded
 *   - total: n * per-player
 */
template<class T>
T calcDownload(T n, T c, T s, T p, T f0c, T f0p) {
    T perPlayer = (n-1)*(c + s + p + f0c + f0p);

    return perPlayer;
}

/**
 *  UPLOAD
 *  ------
 *  Each player i needs to broadcast their poly commitment + f(0) comm + f(0) proof (same in eJF-DKG and in AMT DKG)
 *  Each player i sends n-1 shares to the n-1 other players (same in eJF-DKG and in AMT DKG)
 *  Each player i publishes the AMT (O(n) best case) or sends proofs individually (O(n log t) worst case)
 *  Assume n = 2^k for some k
 *
 *   - Best case per-player:  (c + f0c + f0p) + (n-1)*s + (2n-1)*c (for the AMT quotient commitments)
 *      - In eJF-DKG, last sum term would have been (n-1)*c (so we are only a bit more expensive)
 *
 *   - Worst case per-player: (c + f0c + f0p) + (n-1)*(s + (log(t-1) + 1)c)
 */
template<class T>
T calcUpload_WorstCase(T n, T c, T s, T p, T f0c, T f0p) {
    T perPlayer = c + f0c + f0p + (n-1)*(s + p);

    return perPlayer;
}

template<class T>
T calcUpload(T n, T c, T s, T p, T f0c, T f0p) {
    return calcUpload_WorstCase(n, c, s, p, f0c, f0p);
}

// NOTE: t is the reconstruction threshold so the poly degrees are t-1
template<class T>
std::tuple<T,T> feldman(T n, T t) {
    const T polyDeg = (t - 1);
    const T commSize = 32 * (polyDeg + 1); // need to send a group element for each coeff of the poly
    const T shareSize = 32;        // share is just a field element
    const T proofSize = 0;         // can verify share against commitment directly (not true for Pedersen though: needs 32-byte r(i))
    const T f0commSize = 0;        // g^f(0) is already part of the Feldman commitment to the first coeff c_0 = f(0)
    const T f0proofSize = 0;       // can verify f(0) directly against comm

    return std::make_tuple(
        calcDownload(n, commSize, shareSize, proofSize, f0commSize, f0proofSize),
        calcUpload(n, commSize, shareSize, proofSize, f0commSize, f0proofSize));
}

template<class T>
std::tuple<T,T> kate(T n) {
    const T commSize = 32;          // g^p(s) is one group element
    const T shareSize = 32;         // share is a field element
    const T proofSize = 32;         // proof is g^{p(s) - p(i) / (x - i)}: one group element
    const T f0commSize = 32;        // one group element
    // a Schnorr signature (e = H(g || g^k || g^x || proverID || bla), s = k + ex)
    // Reference: https://tools.ietf.org/html/rfc8235#page-8
    const T nizkPok = 64;           // basically a Schnorr signature
    const T f0proofSize = 32 + nizkPok; // g^f(0) proof: normal Kate proof + a NIZKPoK of f(0) w.r.t. g

    return std::make_tuple(
        calcDownload(n, commSize, shareSize, proofSize, f0commSize, f0proofSize),
        calcUpload(n, commSize, shareSize, proofSize, f0commSize, f0proofSize));
}

/**
 * This is the bandwidth if we send a constant-sized proof for g^f(0) and a log n-sized proof for f(i)
 */
template<class T>
std::tuple<T,T> amt(T n, T t) {
    // a tree of n = 2^i nodes has i + 1 nodes along any path and that's how many quotient commitments will be in our proof
    // when n is not a power of two, we round up using log2ceil
    const T commSize = 32;
    const T shareSize = 32;
    // we are only evaluating at n points this time, and proving g^f(0) using a normal Kate proof
    //const T numLevels = Utils::log2ceil(n) + 1;
    const T proofSize = (Utils::log2floor(t-1) + 1) * 32;
    const T f0commSize = 32;
    const T nizkPok = 64;           // see kate() description
    const T f0proofSize = 32 + nizkPok;    // proof for g^f(0) is now a normal constant-sized Kate proof (+ NIZKPoK)

    //loginfo << " * numLevels = " << numLevels << " (for n = " << n << ")" << endl;

    return std::make_tuple(
        calcDownload(n, commSize, shareSize, proofSize, f0commSize, f0proofSize),
        calcUpload(n, commSize, shareSize, proofSize, f0commSize, f0proofSize));
}

int main(int argc, char *argv[]) {
    libpolycrypto::initialize(nullptr, 0);

    if(argc < 3) {
        cout << "Usage: " << argv[0] << " <out-file> <max-f>" << endl;
        cout << endl;
        cout << "Estimates bandwidth for Feldman, Kate and AMT DKGs" << endl;
        cout << endl;
        cout << " <out-file>     writes numbers in this file" << endl;
        cout << " <max-f>        computes numbers for (k=f+1, n=2f+1) thresholds for f up to <max-f>" << endl;
        cout << endl;
        return 1;
    }

    string outFile(argv[1]);
    size_t max_f = static_cast<size_t>(std::stoi(argv[2]));

    ofstream fout(outFile);

    if(fout.fail()) {
        throw std::runtime_error("Could not write to output file");
    }

    using NumBytesType = int128_t;
    loginfo << "sizeof(NumBytesType): " << sizeof(NumBytesType) << endl;

    fout << "t,n,dkg,download_bw_bytes,download_bw_hum,upload_bw_bytes,upload_bw_hum,comm_bw_bytes,comm_bw_hum,total_bw_bytes,total_bw_hum" << endl;

    for(size_t p = 2; p <= max_f + 1; p *= 2) {
        size_t f = p - 1;
        size_t t = f + 1;
        size_t n = 2*f + 1;
        loginfo << t << " out of " << n  << " DKG" << endl;

        auto writeRow = [&fout, &n, &t](std::tuple<NumBytesType, NumBytesType> bytes, const char * scheme) {
            NumBytesType perPlayerDown = std::get<0>(bytes);
            NumBytesType perPlayerUp = std::get<1>(bytes);
            fout
                 << t << ","
                 << n << ","
                 << scheme << ","
                 << perPlayerDown << ","
                 << Utils::humanizeBytes(perPlayerDown) << ","
                 << perPlayerUp << ","
                 << Utils::humanizeBytes(perPlayerUp) << ","
                 << perPlayerDown + perPlayerUp << ","
                 << Utils::humanizeBytes(perPlayerDown + perPlayerUp) << ","
                 << n*(perPlayerDown + perPlayerUp) << ","
                 << Utils::humanizeBytes(n*(perPlayerDown + perPlayerUp))
                 << endl;
        };

        writeRow(feldman(n, t), "JF-DKG");
        writeRow(kate(n), "eJF-DKG");
        writeRow(amt(n, t), "AMT DKG");
    }

    loginfo << "All done!" << endl;

    return 0;
}