//---------------------------------------------------------------------------//
// Copyright (c) 2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2021 Nikita Kaskov <nbering@nil.foundation>
//
// MIT License
//
// 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.
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_MATH_EXPRESSION_PARSER_DEF_HPP
#define CRYPTO3_MATH_EXPRESSION_PARSER_DEF_HPP

#include <cmath>
#include <iostream>
#include <limits>
#include <string>

#include <boost/math/constants/constants.hpp>
#include <boost/spirit/home/x3.hpp>

#include <nil/crypto3/math/expressions/ast.hpp>
#include <nil/crypto3/math/expressions/ast_adapted.hpp>
#include <nil/crypto3/math/expressions/math.hpp>
#include <nil/crypto3/math/expressions/parser.hpp>

namespace nil {
    namespace crypto3 {
        namespace math {
            namespace expressions {
                namespace detail {

                    namespace x3 = boost::spirit::x3;

                    namespace parser {

                        // LOOKUP

                        struct constant_ : x3::symbols<double> {
                            constant_() {
                                // clang-format off
                                add
                                    ("e"      , boost::math::constants::e<double>())
                                    ("epsilon", std::numeric_limits<double>::epsilon())
                                    ("phi"    , boost::math::constants::phi<double>())
                                    ("pi"     , boost::math::constants::pi<double>())
                                    ;
                                // clang-format on
                            }
                        } constant;

                        struct ufunc_ : x3::symbols<double (*)(double)> {
                            ufunc_() {
                                // clang-format off
                                add
                                    ("abs"   , static_cast<double (*)(double)>(&std::abs))
                                    ("acos"  , static_cast<double (*)(double)>(&std::acos))
                                    ("acosh" , static_cast<double (*)(double)>(&std::acosh))
                                    ("asin"  , static_cast<double (*)(double)>(&std::asin))
                                    ("asinh" , static_cast<double (*)(double)>(&std::asinh))
                                    ("atan"  , static_cast<double (*)(double)>(&std::atan))
                                    ("atanh" , static_cast<double (*)(double)>(&std::atanh))
                                    ("cbrt"  , static_cast<double (*)(double)>(&std::cbrt))
                                    ("ceil"  , static_cast<double (*)(double)>(&std::ceil))
                                    ("cos"   , static_cast<double (*)(double)>(&std::cos))
                                    ("cosh"  , static_cast<double (*)(double)>(&std::cosh))
                                    ("deg"   , static_cast<double (*)(double)>(&math::deg))
                                    ("erf"   , static_cast<double (*)(double)>(&std::erf))
                                    ("erfc"  , static_cast<double (*)(double)>(&std::erfc))
                                    ("exp"   , static_cast<double (*)(double)>(&std::exp))
                                    ("exp2"  , static_cast<double (*)(double)>(&std::exp2))
                                    ("floor" , static_cast<double (*)(double)>(&std::floor))
                                    ("isinf" , static_cast<double (*)(double)>(&math::isinf))
                                    ("isnan" , static_cast<double (*)(double)>(&math::isnan))
                                    ("log"   , static_cast<double (*)(double)>(&std::log))
                                    ("log2"  , static_cast<double (*)(double)>(&std::log2))
                                    ("log10" , static_cast<double (*)(double)>(&std::log10))
                                    ("rad"   , static_cast<double (*)(double)>(&math::rad))
                                    ("round" , static_cast<double (*)(double)>(&std::round))
                                    ("sgn"   , static_cast<double (*)(double)>(&math::sgn))
                                    ("sin"   , static_cast<double (*)(double)>(&std::sin))
                                    ("sinh"  , static_cast<double (*)(double)>(&std::sinh))
                                    ("sqrt"  , static_cast<double (*)(double)>(&std::sqrt))
                                    ("tan"   , static_cast<double (*)(double)>(&std::tan))
                                    ("tanh"  , static_cast<double (*)(double)>(&std::tanh))
                                    ("tgamma", static_cast<double (*)(double)>(&std::tgamma))
                                    ;
                                // clang-format on
                            }
                        } ufunc;

                        struct bfunc_ : x3::symbols<double (*)(double, double)> {
                            bfunc_() {
                                // clang-format off
                                add
                                    ("atan2", static_cast<double (*)(double, double)>(&std::atan2))
                                    ("max"  , static_cast<double (*)(double, double)>(&std::fmax))
                                    ("min"  , static_cast<double (*)(double, double)>(&std::fmin))
                                    ("pow"  , static_cast<double (*)(double, double)>(&std::pow))
                                    ;
                                // clang-format on
                            }
                        } bfunc;

                        struct unary_op_ : x3::symbols<double (*)(double)> {
                            unary_op_() {
                                // clang-format off
                                add
                                    ("+", static_cast<double (*)(double)>(&math::plus))
                                    ("-", static_cast<double (*)(double)>(&math::minus))
                                    ("!", static_cast<double (*)(double)>(&math::unary_not))
                                    ;
                                // clang-format on
                            }
                        } unary_op;

                        struct additive_op_ : x3::symbols<double (*)(double, double)> {
                            additive_op_() {
                                // clang-format off
                                add
                                    ("+", static_cast<double (*)(double, double)>(&math::plus))
                                    ("-", static_cast<double (*)(double, double)>(&math::minus))
                                    ;
                                // clang-format on
                            }
                        } additive_op;

                        struct multiplicative_op_ : x3::symbols<double (*)(double, double)> {
                            multiplicative_op_() {
                                // clang-format off
                                add
                                    ("*", static_cast<double (*)(double, double)>(&math::multiplies))
                                    ("/", static_cast<double (*)(double, double)>(&math::divides))
                                    ("%", static_cast<double (*)(double, double)>(&std::fmod))
                                    ;
                                // clang-format on
                            }
                        } multiplicative_op;

                        struct logical_op_ : x3::symbols<double (*)(double, double)> {
                            logical_op_() {
                                // clang-format off
                                add
                                    ("&&", static_cast<double (*)(double, double)>(&math::logical_and))
                                    ("||", static_cast<double (*)(double, double)>(&math::logical_or))
                                    ;
                                // clang-format on
                            }
                        } logical_op;

                        struct relational_op_ : x3::symbols<double (*)(double, double)> {
                            relational_op_() {
                                // clang-format off
                                add
                                    ("<" , static_cast<double (*)(double, double)>(&math::less))
                                    ("<=", static_cast<double (*)(double, double)>(&math::less_equals))
                                    (">" , static_cast<double (*)(double, double)>(&math::greater))
                                    (">=", static_cast<double (*)(double, double)>(&math::greater_equals))
                                    ;
                                // clang-format on
                            }
                        } relational_op;

                        struct equality_op_ : x3::symbols<double (*)(double, double)> {
                            equality_op_() {
                                // clang-format off
                                add
                                    ("==", static_cast<double (*)(double, double)>(&math::equals))
                                    ("!=", static_cast<double (*)(double, double)>(&math::not_equals))
                                    ;
                                // clang-format on
                            }
                        } equality_op;

                        struct power_ : x3::symbols<double (*)(double, double)> {
                            power_() {
                                // clang-format off
                                add
                                    ("**", static_cast<double (*)(double, double)>(&std::pow))
                                    ;
                                // clang-format on
                            }
                        } power;

                        // ADL markers

                        struct expression_class;
                        struct logical_class;
                        struct equality_class;
                        struct relational_class;
                        struct additive_class;
                        struct multiplicative_class;
                        struct factor_class;
                        struct primary_class;
                        struct unary_class;
                        struct binary_class;
                        struct variable_class;

                        // clang-format off

                        // Rule declarations

                        auto const expression     = x3::rule<expression_class    , ast::expression>{"expression"};
                        auto const logical        = x3::rule<logical_class       , ast::expression>{"logical"};
                        auto const equality       = x3::rule<equality_class      , ast::expression>{"equality"};
                        auto const relational     = x3::rule<relational_class    , ast::expression>{"relational"};
                        auto const additive       = x3::rule<additive_class      , ast::expression>{"additive"};
                        auto const multiplicative = x3::rule<multiplicative_class, ast::expression>{"multiplicative"};
                        auto const factor         = x3::rule<factor_class        , ast::expression>{"factor"};
                        auto const primary        = x3::rule<primary_class       , ast::operand   >{"primary"};
                        auto const unary          = x3::rule<unary_class         , ast::unary_op  >{"unary"};
                        auto const binary         = x3::rule<binary_class        , ast::binary_op >{"binary"};
                        auto const variable       = x3::rule<variable_class      , std::string    >{"variable"};

                        // Rule defintions

                        auto const expression_def =
                            logical
                            ;

                        auto const logical_def =
                            equality >> *(logical_op > equality)
                            ;

                        auto const equality_def =
                            relational >> *(equality_op > relational)
                            ;

                        auto const relational_def =
                            additive >> *(relational_op > additive)
                            ;

                        auto const additive_def =
                            multiplicative >> *(additive_op > multiplicative)
                            ;

                        auto const multiplicative_def =
                            factor >> *(multiplicative_op > factor)
                            ;

                        auto const factor_def =
                            primary >> *( power > factor )
                            ;

                        auto const unary_def =
                            ufunc > '(' > expression > ')'
                            ;

                        auto const binary_def =
                            bfunc > '(' > expression > ',' > expression > ')'
                            ;

                        auto const variable_def =
                            x3::raw[x3::lexeme[x3::alpha >> *(x3::alnum | '_')]]
                            ;

                        auto const primary_def =
                              x3::double_
                            | ('(' > expression > ')')
                            | (unary_op > primary)
                            | binary
                            | unary
                            | constant
                            | variable
                            ;

                        BOOST_SPIRIT_DEFINE(
                            expression,
                            logical,
                            equality,
                            relational,
                            additive,
                            multiplicative,
                            factor,
                            primary,
                            unary,
                            binary,
                            variable
                        )

                        // clang-format on

                        struct expression_class {
                            template <typename Iterator, typename Exception, typename Context>
                            x3::error_handler_result on_error(Iterator &, Iterator const &last,
                                                              Exception const &x, Context const &) {
                                std::cout << "Expected " << x.which() << " at \""
                                          << std::string{x.where(), last} << "\"" << std::endl;
                                return x3::error_handler_result::fail;
                            }
                        };

                        using expression_type = x3::rule<expression_class, ast::expression>;

                        BOOST_SPIRIT_DECLARE(expression_type)

                    } // namespace parser

                    parser::expression_type grammar() { return parser::expression; }

                }    // namespace detail
            }    // namespace expressions
        }    // namespace math
    }        // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_MATH_EXPRESSION_PARSER_DEF_HPP


#include "problemes.h"
#include "arithmetique.h"

#include <fstream>

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

typedef unsigned long long nombre;
typedef std::vector<nombre> vecteur;

typedef boost::rational<nombre> fraction;

namespace {
    bool infini(nombre n, nombre k) {
        k /= arithmetique::PGCD(n, k);
        while (k % 2 == 0)
            k /= 2;

        while (k % 5 == 0)
            k /= 5;

        return k == 1;
    }

    bool terminating(nombre n) {
        long double k0 = n / boost::math::constants::e<long double>();
        nombre k_max = static_cast<nombre>(std::ceil(k0));
        nombre k_min = static_cast<nombre>(std::floor(k0));

        long double p_max = puissance::puissance(n / std::ceil(k0), k_max);
        long double p_min = puissance::puissance(n / std::floor(k0), k_min);

        if (p_max > p_min)
            return infini(n, k_max);
        else
            return infini(n, k_min);
    }
}

ENREGISTRER_PROBLEME(183, "Maximum product of parts") {
    // Let N be a positive integer and let N be split into k equal parts, r = N/k, so that N = r + r + ... + r.
    // Let P be the product of these parts, P = r × r × ... × r = rk.
    //
    // For example, if 11 is split into five equal parts, 11 = 2.2 + 2.2 + 2.2 + 2.2 + 2.2,
    // then P = 2.2**5 = 51.53632.
    //
    // Let M(N) = Pmax for a given value of N.
    //
    // It turns out that the maximum for N = 11 is found by splitting eleven into four equal parts
    // which leads to Pmax = (11/4)4; that is, M(11) = 14641/256 = 57.19140625, which is a terminating decimal.
    //
    // However, for N = 8 the maximum is achieved by splitting it into three equal parts, so M(8) = 512/27,
    // which is a non-terminating decimal.
    //
    // Let D(N) = N if M(N) is a non-terminating decimal and D(N) = -N if M(N) is a terminating decimal.
    //
    // For example, ΣD(N) for 5 ≤ N ≤ 100 is 2438.
    //
    // Find ΣD(N) for 5 ≤ N ≤ 10000.
    nombre limite = 10000;
    // std::cout << std::boolalpha;
    // std::cout << "terminating(8) = " << terminating(8) << std::endl;
    // std::cout << "terminating(11) = " << terminating(11) << std::endl;

    nombre resultat_positif = 0;
    nombre resultat_negatif = 0;
    for (nombre n = 5; n < limite + 1; ++n) {
        if (terminating(n))
            resultat_negatif += n;
        else
            resultat_positif += n;
    }

    return std::to_string(resultat_positif - resultat_negatif);
}


#include <iostream>
#include <Eigen/Dense>
#include <iostream>
#include "svm.h"
using namespace Eigen;
using namespace std;

void generate_data(MatrixXd& X, VectorXd& Y, int n){
    double k = 0.6;
    double b = -15.7;

    double x,y;
    for(int i = 0;i<n;i++){
        x = rand()*10.0;
        y = rand()*10.0;
        X(i,0) = x;
        X(i,1) = y;
        Y(i) = (x*k + b)>y ? 1 : 0;
    }

}

int main() {

    int n_train = 10000;
    int n_test = 100;
    MatrixXd train_x(n_train,2);
    VectorXd train_y(n_train);
    generate_data(train_x,train_y,n_train);

    MatrixXd test_x(n_test,2);
    VectorXd test_y(n_test);
    generate_data(test_x,test_y,n_test);

    //GaussianKernel kernel(0.5);
    LinearKernel kernel;
    SVM svm(1.0,&kernel);
    svm.fit(train_x,train_y);
    double e_rate = svm.score(test_x,test_y);
    std::cout<< e_rate << std::endl;

    return 0;
}


/*
Metaheuristic Optimization Using Population-Based Simulated Annealing.

Copyright (c) 2021 Gabriele Gilardi

Features
--------
- The code has been written in C++ using the Eigen library (ver. 3.3.9) and
  tested using g++ 8.1.0 (MinGW-W64).
- Variables can be real, integer, or mixed real/integer.
- Variables can be constrained to a specific interval or value setting the
  lower and the upper boundaries.
- Neighboroud search is performed using a normal distribution along randomly
  chosen dimensions.
- A nonlinear decreasing schedule is used for the temperature and for the
  standard deviation in the neighboroud search.
- Search space can be normalized to improve convergency.
- An arbitrary number of parameters can be passed (in a tuple) to the function
  to minimize.
- Solver parameters and results are passed using structures.
- Usage: test_Eigen.exe <example>.

Main Parameters
---------------
example
    Name of the example to run (Parabola, Alpine, Tripod, and Ackley.)
func
    Function to minimize. The position of all agents is passed to the function
    at the same time.
LB, UB
    Lower and upper boundaries of the search space.
nPop >=1, epochs >= 1
    Number of agents (population) and number of iterations.
T0 > 0
    Initial temperature.
0 < alphaT < 1
    Temperature reduction rate.
0 < alphaS <= 1
    Standard deviation (neighboroud search) reduction rate.
nMove > 0
    Number of neighbours of a state evaluated at each epoch.
0 < prob < 1
    Probability the dimension of a state is changed.
sigma0 > 0
    Initial standard deviation used to search the neighboroud of a state.
IntVar
    List of indexes specifying which variable should be treated as integer.
    If all variables are real set <IntVar> = <NULL>. Indexes are specified
    in the range 0 to <nVar-1>. It cannot be used when the search space is
    normalized.
normalize = true, false
    Specifies if the search space should be normalized. If <true>, parameter
    <sigma0> is applied to the normalized search space.
args
    Tuple containing any parameter that needs to be passed to the function. If
    no parameters are passed set <args> = <NULL>.
nVar
    Number of variables.
nIntVar >= 0
    Number of integer variables.
X0
    Global minimum point (used only to compare with the numerical solution).
seed
    Seeding value for the random number generator.

Examples
--------
There are four examples: Parabola, Alpine, Tripod, and Ackley.

- Parabola, Alpine, and Ackley can have an arbitrary number of dimensions,
  while Tripod has only two dimensions.

- Parabola, Tripod, and Ackley are examples where parameters (respectively,
  array X0, scalars kx and ky, and array X0) are passed using args.

- The global minimum for Parabola and Ackley is at X0; the global minimum for
  Alpine is at zero; the global minimum for Tripod is at [0,-ky] with local
  minimum at [-kx,+ky] and [+kx,+ky].

References
----------
- Simulated annealing @ https://en.wikipedia.org/wiki/Simulated_annealing
- Kirkpatrick et al., 1983, "Optimization by Simulated Annealing", JSTOR
  @ https://www.jstor.org/stable/1690046
- Jamil and Yang, 2013, "A Literature Survey of Benchmark Functions For Global
  Optimization Problems", arXiv @ https://arxiv.org/abs/1308.4008
- Eigen template library for linear algebra @ https://eigen.tuxfamily.org/
*/

#include <random>
#include <Eigen/Dense>
#include <iostream>

using namespace std;
using namespace Eigen;

/* Structure used to pass the parameters (with default values) */
struct Parameters {
    int nPop = 20;
    int epochs = 1000;
    int nMove = 100;
    double T0 = 0.1;
    double alphaT = 0.99;
    double sigma0 = 0.1;
    double alphaS = 0.98;
    double prob = 0.5;
    bool normalize = false;
    ArrayXi IntVar;
    ArrayXXd args;
    int seed = 1234567890;
};

/* Structure used to return the results */
struct Results {
    double best_cost;
    ArrayXXd best_pos;
    ArrayXd F;
    double T;
    ArrayXXd sigma;
};

/* Simulated annealing function prototype */
Results sa(ArrayXd (*func)(ArrayXXd, ArrayXXd), ArrayXXd LB, ArrayXXd UB,
           Parameters p);

// Parabola: F(X) = sum((X - X0)^2)
// Xmin = X0
ArrayXd Parabola(ArrayXXd X, ArrayXXd args)
{
    int nPop;
    ArrayXd f;
    ArrayXXd dX;

    nPop = X.rows();
    dX = X - args.replicate(nPop, 1);
    f = (dX * dX).rowwise().sum();

    return f;
}

// Ackley: F(X)= + 20 + exp(1) - exp(sum(cos(2*pi*(X-X0))/n)
//               - 20*exp(-0.2*sqrt(sum((X-X0)^2)/n))
// Xmin = X0
ArrayXd Ackley(ArrayXXd X, ArrayXXd args)
{
    const double pi = 3.14159265358979323846;
    int nPop, nVar;
    ArrayXd f;
    ArrayXXd dX;

    nPop = X.rows();
    nVar = X.cols();
    dX = X - args.replicate(nPop, 1);
    f = + 20.0 + exp(1.0)
        - exp((cos(2.0 * pi * dX)).rowwise().sum() / nVar)
        - 20.0 * exp(-0.2 * sqrt((dX * dX).rowwise().sum() / nVar));

    return f;
}

// Tripod:
// F(x,y)= p(y)*(1 + p(x)) + abs(x + kx*p(y)*(1 - 2*p(x)))
//         + abs(y + ky*(1 - 2*p(y)))
// p(x) = 1 if x >= 0, p(x) = 0 if x < 0; p(y) = 1 if y >= 0, p(y) = 0 if y < 0
// Global minimum at [0,-ky], local minimum at [-kx,ky] and [kx,ky]; kx, ky > 0
ArrayXd Tripod(ArrayXXd X, ArrayXXd args)
{
    double kx = args(0, 0), ky = args(0, 1);
    ArrayXd f, x, y, px, py;

    x = X.col(0);
    y = X.col(1);
    px = (x >= 0.0).cast<double>();
    py = (y >= 0.0).cast<double>();

    f = py * (1.0 + px) + abs(x + kx * py * (1.0 - 2.0 * px)) +
        abs(y + ky * (1.0 - 2.0 * py));

    return f;
}

// Alpine: F(X) = sum(abs(X*sin(X) + 0.1*X))
// Xmin = 0
ArrayXd Alpine(ArrayXXd X, ArrayXXd args)
{
    ArrayXd f;

    f = (abs(X * sin(X) + 0.1 * X)).rowwise().sum();

    return f;
}

/* Main function */
int main(int argc, char **argv)
{
    Parameters p;
    Results res;
    string example;
    int nVar;
    double sum, err;

    /*Eigen declarations*/
    ArrayXXd UB, LB, X0;
    ArrayXd (*func)(ArrayXXd, ArrayXXd);

    /* Read example to run */
    if (argc != 2) {
        printf("\nUsage: test_Eigen <example>\n");
        exit(EXIT_FAILURE);
    }
    example = argv[1];

    // Parabola: F(X) = sum((X - X0)^2)
    // Xmin = X0
    if (example == "Parabola") {
        func = Parabola;
        nVar = 20;

        UB.setConstant(1, nVar, 500.0);     // Upper and lower boundaries
        LB = -UB;

        X0.setZero(1, nVar);                // Global minimum
        for (int i=0; i<nVar; i++) {
            X0(0, i) = 1.1 * double(i);
        }

        p.args = X0;                        // Arguments
    }

    // Ackley: F(X)= + 20 + exp(1) - exp(sum(cos(2*pi*(X-X0))/n)
    //               - 20*exp(-0.2*sqrt(sum((X-X0)^2)/n))
    // Xmin = X0
    else if (example == "Ackley") {
        func = Ackley;
        nVar = 10;
        p.nPop = 50;

        UB.setConstant(1, nVar, 50.0);      // Upper and lower boundaries
        LB = -UB;

        X0.setConstant(1, nVar, 1.6789);    // Global minimum

        p.args = X0;                        // Arguments
    }

    // Tripod:
    // F(x,y)= p(y)*(1 + p(x)) + abs(x + kx*p(y)*(1 - 2*p(x)))
    //         + abs(y + ky*(1 - 2*p(y)))
    // p(x) = 1 if x >= 0, p(x) = 0 if x < 0; p(y) = 1 if y >= 0, p(y) = 0 if y < 0
    // Global minimum at [0,-ky], local minimum at [-kx,ky] and [kx,ky]; kx, ky > 0
    else if (example == "Tripod") {
        func = Tripod;
        nVar = 2;               // The equation works only with two dimensions
        double kx = 20.0;
        double ky = 40.0;

        UB.setConstant(1, nVar, 100.0);     // Upper and lower boundaries
        LB = -UB;

        X0.setZero(1, nVar);                // Global minimum
        X0(0, 1) = -ky;

        p.args.setZero(1, nVar);            // Arguments
        p.args(0, 0) = kx;
        p.args(0, 1) = ky;
    }

    // Alpine: F(X) = sum(abs(X*sin(X) + 0.1*X))
    // Xmin = 0
    // Note: the solution is VERY sensitive to the parameter values and the
    //       random generated numbers
    else if (example == "Alpine") {
        func = Alpine;
        nVar = 10;
        p.sigma0 = 0.2;
        p.alphaS = 1.0;

        UB.setConstant(1, nVar, 10.0);      // Upper and lower boundaries
        LB = -UB;

        X0.setZero(1, nVar);                // Global minimum
    }

    else {
        printf("\nFunction not found.\n");
        exit(EXIT_FAILURE);
    }

    /* Solve */
    res = sa(func, LB, UB, p);

    /* Print results */
    printf("\nBest position:");
    for (int j=0; j<nVar; j++) {
        printf("\n %g", res.best_pos(0, j));
    }
    printf("\n\nCost: %g", res.best_cost);
    printf("\nFinal T: %g", res.T);
    sum = res.sigma.sum();
    printf("\nFinal sigma (avr): %g", sum / double(nVar));
    err = ((res.best_pos - X0) * (res.best_pos - X0)).sum();
    printf("\nError: %g\n", sqrt(err));

    return 0;
}


/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

/**
 * \file     beta_distribution.cpp
 * \author   Collin Johnson
 *
 * Definition of BetaDistribution.
 */

#include "math/beta_distribution.h"
#include <boost/math/special_functions/beta.hpp>
#include <cassert>
#include <iostream>

namespace vulcan
{
namespace math
{

double beta_normalizer(double alpha, double beta)
{
    return 1.0 / boost::math::beta(alpha, beta);
}

BetaDistribution::BetaDistribution(double alpha, double beta)
: alpha_(alpha)
, beta_(beta)
, normalizer_(beta_normalizer(alpha_, beta_))
{
    assert(alpha_ >= 0.0);
    assert(beta_ >= 0.0);
}

double BetaDistribution::sample(void) const
{
    std::cout << "STUB: BetaDistribution::sample(void)\n";
    return 0.0;
}

double BetaDistribution::likelihood(double value) const
{
    if ((value <= 0.0) || (value >= 1.0)) {
        return 0.0;
    }

    return std::pow(value, alpha_ - 1.0) * std::pow(1.0 - value, beta_ - 1.0) * normalizer_;
}

bool BetaDistribution::save(std::ostream& out) const
{
    out << alpha_ << ' ' << beta_ << '\n';
    return out.good();
}

bool BetaDistribution::load(std::istream& in)
{
    in >> alpha_ >> beta_;

    assert(alpha_ >= 0.0);
    assert(beta_ >= 0.0);

    normalizer_ = beta_normalizer(alpha_, beta_);

    return in.good();
}

}   // namespace math
}   // namespace vulcan


/*!
 * @file diffusion_problem_basis.hpp
 * @brief Contains implementation of multiscale basis functions.
 * @author Konrad Simon
 * @date August 2019
 */

#ifndef INCLUDE_DIFFUSION_PROBLEM_BASIS_HPP_
#define INCLUDE_DIFFUSION_PROBLEM_BASIS_HPP_

// Deal.ii
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/logstream.h>

#include <deal.II/lac/vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/affine_constraints.h>

#include <deal.II/grid/tria.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/grid/grid_generator.h>

#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/dofs/dof_tools.h>

#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>

#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/data_out.h>

// STL
#include <cmath>
#include <fstream>
#include <iostream>

// My Headers
#include "matrix_coeff.hpp"
#include "right_hand_side.hpp"
#include "neumann_bc.hpp"
#include "dirichlet_bc.hpp"
#include "basis_q1.hpp"

/*!
 * @namespace DiffusionProblem
 * @brief Contains implementation of the main object
 * and all functions to solve a time-independent
 * Dirichlet-Neumann problem on a unit square.
 */
namespace DiffusionProblem
{
using namespace dealii;

/*!
 * @class DiffusionProblemBasis
 * @brief Main class to solve for time-independent
 * multiscale basis functions (Dirichlet problem) on a
 * given coarse quadrilateral cell without oversampling.
 */
template <int dim>
class DiffusionProblemBasis
{
public:
	DiffusionProblemBasis ();
	void run ();

	void output_global_solution_in_cell () const;

	const FullMatrix<double>& get_global_element_matrix () const;
	const Vector<double>& get_global_element_rhs () const;
	const std::string& get_filename_global ();

	void set_n_local_refinements (unsigned int n_refine_local);
	void set_cell_data (typename Triangulation<dim>::active_cell_iterator &coarse_cell,
						unsigned int n_cell);
	void set_basis_data ();
	void set_output_flag (bool flag);
	void set_global_weights (const std::vector<double> &global_weights);

private:
	void make_grid ();
	void setup_system ();
	void assemble_system ();
	void assemble_global_element_matrix ();
	void solve_iterative (unsigned int index_basis);
	void output_basis () const;

	void set_filename_global ();

	Triangulation<dim>   			triangulation;
	FE_Q<dim>            			fe;
	DoFHandler<dim>      			dof_handler;

	std::vector<AffineConstraints<double>> 		constraints_vector;
	std::vector<Point<dim>> 					corner_points;

	SparsityPattern      			sparsity_pattern;
	SparseMatrix<double> 			diffusion_matrix;
	SparseMatrix<double> 			system_matrix;

	std::string	filename_global;

	/*!
	 * Solution vector.
	 */
	std::vector<Vector<double>>		solution_vector;

	/*!
	 * Contains the right-hand side.
	 */
	Vector<double>       			global_rhs; // this is only for the global assembly (speed-up)

	/*!
	 * Contains all parts of the right-hand side needed to
	 * solve the linear system..
	 */
	Vector<double>					system_rhs;

	/*!
	 * Holds global multiscale element matrix.
	 */
	FullMatrix<double>   		global_element_matrix;
	bool is_built_global_element_matrix;

	/*!
	 * Holds global multiscale element right-hand side.
	 */
	Vector<double>   			global_element_rhs;

	/*!
	 * Weights of multiscale basis functions.
	 */
	std::vector<double> 		global_weights;
	bool is_set_global_weights;

	/*!
	 * Global solution
	 */
	Vector<double>			global_solution;

	/*!
	 * Number of local refinements.
	 */
	unsigned int n_refine_local;
	bool is_set_n_refine_local;

	/*!
	 * Global cell number.
	 */
	unsigned int global_cell_number;

	/*!
	 * Iterator to global cell..
	 */
	typename Triangulation<dim>::active_cell_iterator global_cell;
	bool is_set_global_cell;

	/*!
	 * Object carries set of local \f$Q_1\f$-basis functions.
	 */
	Coefficients::BasisQ1<dim> basis_q1;
	bool is_set_basis_data;

	/*!
	 * Write basis functions as vtu.
	 */
	bool output_flag;
};

/*!
 * Default constructor.
 */
template <int dim>
DiffusionProblemBasis<dim>::DiffusionProblemBasis ()
:
fe (1),
dof_handler (triangulation),
constraints_vector (GeometryInfo<dim>::vertices_per_cell),
corner_points (GeometryInfo<dim>::vertices_per_cell),
filename_global (""),
solution_vector (GeometryInfo<dim>::vertices_per_cell),
global_element_matrix (fe.dofs_per_cell,
	fe.dofs_per_cell),
is_built_global_element_matrix (false),
global_element_rhs (fe.dofs_per_cell),
global_weights (fe.dofs_per_cell, 0),
is_set_global_weights (false),
n_refine_local (0),
is_set_n_refine_local (false),
global_cell_number (0),
global_cell (),
is_set_global_cell (false),
basis_q1 (),
is_set_basis_data (false),
output_flag (false)
{}

/*!
 * @brief Set up the grid with a certain number of refinements.
 *
 * Generate a triangulation of \f$[0,1]^{\rm{dim}}\f$ with edges/faces
 * numbered form \f$1,\dots,2\rm{dim}\f$.
 */
template <int dim>
void DiffusionProblemBasis<dim>::make_grid ()
{
	Assert (is_set_n_refine_local,
				ExcMessage ("Number of local refinements must be set first."));
	Assert (is_set_global_cell,
				ExcMessage ("Global cell data must be set first."));

	GridGenerator::general_cell(triangulation, corner_points, /* colorize faces */ false);

	triangulation.refine_global (n_refine_local);
}

/*!
 * @brief Setup sparsity pattern and system matrix.
 *
 * Compute sparsity pattern and reserve memory for the sparse system matrix
 * and a number of right-hand side vectors. Also build a constraint object
 * to take care of Dirichlet boundary conditions.
 */
template <int dim>
void DiffusionProblemBasis<dim>::setup_system ()
{
	dof_handler.distribute_dofs (fe);

	std::cout << "Global cell   "
			<< global_cell_number
			<< ":   "
			<< triangulation.n_active_cells() << " active fine cells --- "
			<< dof_handler.n_dofs() << " subgrid dof"
			<< std::endl;

	/*
	 * Set up Dirichlet boundary conditions and sparsity pattern.
	 */
	DynamicSparsityPattern dsp(dof_handler.n_dofs());

	for (unsigned int index_basis = 0;
			index_basis<GeometryInfo<dim>::vertices_per_cell;
			++index_basis)
	{
		basis_q1.set_index (index_basis);

		constraints_vector[index_basis].clear();
		DoFTools::make_hanging_node_constraints(dof_handler, constraints_vector[index_basis]);

		VectorTools::interpolate_boundary_values(dof_handler,
													/*boundary id*/ 0,
													basis_q1,
													constraints_vector[index_basis]);
		constraints_vector[index_basis].close();
	}

	DoFTools::make_sparsity_pattern (dof_handler,
									dsp,
									constraints_vector[0], // sparsity pattern is the same for each basis
									/*keep_constrained_dofs =*/ true); // for time stepping this is essential to be true
	sparsity_pattern.copy_from(dsp);

	system_matrix.reinit (sparsity_pattern);
	diffusion_matrix.reinit (sparsity_pattern);

	for (unsigned int index_basis = 0;
			index_basis<GeometryInfo<dim>::vertices_per_cell;
			++index_basis)
	{
		solution_vector[index_basis].reinit (dof_handler.n_dofs());
	}
	system_rhs.reinit (dof_handler.n_dofs());
	global_rhs.reinit (dof_handler.n_dofs());
}

/*!
 * @brief Assemble the system matrix and the static right hand side.
 *
 * Assembly routine to build the time-independent (static) part.
 * Neumann boundary conditions will be put on edges/faces
 * with odd number. Constraints are not applied here yet.
 */
template <int dim>
void DiffusionProblemBasis<dim>::assemble_system ()
{
	QGauss<dim>  quadrature_formula(fe.degree + 1);

	FEValues<dim> 	fe_values (fe, quadrature_formula,
								update_values    |  update_gradients |
								update_quadrature_points  |  update_JxW_values);

	const unsigned int   	dofs_per_cell = fe.dofs_per_cell;
	const unsigned int   	n_q_points    = quadrature_formula.size();

	FullMatrix<double>   cell_diffusion_matrix (dofs_per_cell, dofs_per_cell);
	FullMatrix<double>   cell_mass_matrix (dofs_per_cell, dofs_per_cell);
	Vector<double>       cell_rhs (dofs_per_cell);

	std::vector<types::global_dof_index> local_dof_indices (dofs_per_cell);

	/*
	 * Matrix coefficient and vector to store the values.
	 */
	const Coefficients::MatrixCoeff<dim> 		matrix_coeff;
	std::vector<Tensor<2,dim>> 	matrix_coeff_values(n_q_points);

	/*
	 * Right hand side and vector to store the values.
	 */
	const Coefficients::RightHandSide<dim> 	right_hand_side;
	std::vector<double>      	rhs_values(n_q_points);

	/*
	 * Integration over cells.
	 */
	for (const auto &cell: dof_handler.active_cell_iterators())
	{
		cell_diffusion_matrix = 0;
		cell_rhs = 0;

		fe_values.reinit (cell);

		// Now actually fill with values.
		matrix_coeff.value_list(fe_values.get_quadrature_points (),
						  	  	  matrix_coeff_values);
		right_hand_side.value_list(fe_values.get_quadrature_points(),
									   rhs_values);

		for (unsigned int q_index=0; q_index<n_q_points; ++q_index)
		{
			for (unsigned int i=0; i<dofs_per_cell; ++i)
			{
				for (unsigned int j=0; j<dofs_per_cell; ++j)
				{

					cell_diffusion_matrix(i,j) += fe_values.shape_grad(i,q_index) *
										 matrix_coeff_values[q_index] *
										 fe_values.shape_grad(j,q_index) *
										 fe_values.JxW(q_index);
				} // end ++j

				cell_rhs(i) += fe_values.shape_value(i,q_index) *
								   rhs_values[q_index] *
								   fe_values.JxW(q_index);
			} // end ++i
		} // end ++q_index

		// get global indices
		cell->get_dof_indices (local_dof_indices);
		/*
		 * Now add the cell matrix and rhs to the right spots
		 * in the global matrix and global rhs. Constraints will
		 * be taken care of later.
		 */
		for (unsigned int i = 0; i < dofs_per_cell; ++i)
		{
			for (unsigned int j = 0; j < dofs_per_cell; ++j)
			{
				diffusion_matrix.add(local_dof_indices[i],
							local_dof_indices[j],
							cell_diffusion_matrix(i, j));
			}
			global_rhs(local_dof_indices[i]) += cell_rhs(i);
		}
	} // end ++cell
}

/*!
 *
 */
template <int dim>
void
DiffusionProblemBasis<dim>::assemble_global_element_matrix ()
{
	// First, reset.
	global_element_matrix = 0;

	// Get lengths of tmp vectors for assembly
	const unsigned int dofs_per_cell = fe.n_dofs_per_cell();

	Vector<double>			tmp (dof_handler.n_dofs());

	// This assembles the local contribution to the global global matrix
	// with an algebraic trick. It uses the local system matrix stored in
	// the respective basis object.
	for (unsigned int i_test=0;
			i_test < dofs_per_cell;
			++i_test)
	{
		// set an alias name
		const Vector<double>& test_vec = solution_vector[i_test];

		for (unsigned int i_trial=0;
				i_trial<dofs_per_cell;
				++i_trial)
		{
			// set an alias name
			const Vector<double>& trial_vec = solution_vector[i_trial];

			// tmp = system_matrix*trial_vec
			diffusion_matrix.vmult(tmp, trial_vec);

			// global_element_diffusion_matrix = test_vec*tmp
			global_element_matrix(i_test,i_trial) += (test_vec * tmp);

			// reset
			tmp = 0;
		} // end for i_trial

		global_element_rhs(i_test) += test_vec * global_rhs;

	} // end for i_test

	is_built_global_element_matrix = true;
}

/*!
 * @brief Iterative solver.
 *
 * CG-based solver with SSOR-preconditioning.
 */
template <int dim>
void DiffusionProblemBasis<dim>::solve_iterative (unsigned int index_basis)
{
	SolverControl           solver_control (1000, 1e-12);
	SolverCG<>              solver (solver_control);

	PreconditionSSOR<> preconditioner;
	preconditioner.initialize(system_matrix, 1.2);

	solver.solve (system_matrix,
				solution_vector[index_basis],
				system_rhs,
				preconditioner);

	constraints_vector[index_basis].distribute (solution_vector[index_basis]);

	std::cout << "   "
			<< "(cell   "
			<< global_cell_number
			<< ") "
			<< "(basis   "
			<< index_basis
			<< ")   "
			<< solver_control.last_step()
			<< " fine CG iterations needed to obtain convergence."
			<< std::endl;
}

/*!
 * Return the multiscale element matrix produced
 * from local basis functions.
 */
template <int dim>
const FullMatrix<double>&
DiffusionProblemBasis<dim>::get_global_element_matrix () const
{
	return global_element_matrix;
}

/*!
 * Get the right hand-side that was locally assembled
 * to speed up the global assembly.
 */
template <int dim>
const Vector<double>&
DiffusionProblemBasis<dim>::get_global_element_rhs () const
{
	return global_element_rhs;
}

/*!
 * Return filename for local pvtu record.
 */
template <int dim>
const std::string&
DiffusionProblemBasis<dim>::get_filename_global ()
{
	return filename_global;
}

/*!
 * Set the number of local refinements.
 * @param n_refine_local
 */
template <int dim>
void
DiffusionProblemBasis<dim>::set_n_local_refinements (unsigned int n_refine)
{
	n_refine_local = n_refine;

	is_set_n_refine_local = true;
}

/*!
 * Set the global cell data.
 * @param coarse_cell
 * @param n_cell_global
 */
template <int dim>
void
DiffusionProblemBasis<dim>::set_cell_data (typename Triangulation<dim>::active_cell_iterator &coarse_cell,
										unsigned int n_cell_global)
{
	global_cell = coarse_cell;
	global_cell_number = n_cell_global;

	for (unsigned int vertex_n=0;
			 vertex_n<GeometryInfo<dim>::vertices_per_cell;
			 ++vertex_n)
	{
		corner_points[vertex_n] = global_cell->vertex(vertex_n);
	}

	is_set_global_cell = true;
}

/*!
 * Initialize the basis coefficients for the global cell.
 * @param coarse_cell
 */
template <int dim>
void
DiffusionProblemBasis<dim>::set_basis_data ()
{
	Assert (is_set_global_cell,
					ExcMessage ("Pointer to global cell must be set first."));

	basis_q1.set_coeff (global_cell);

	is_set_basis_data = true;
}

/*!
 * Set the output flag to write basis functions to disk as vtu.
 * @param flag
 */
template <int dim>
void
DiffusionProblemBasis<dim>::set_output_flag (bool flag)
{
	output_flag = flag;
}

/*!
 * @brief Set global weights.
 * @param weights
 *
 * The coarse weights of the global solution determine
 * the local multiscale solution. They must be computed
 * and then set locally to write an output.
 */
template <int dim>
void
DiffusionProblemBasis<dim>::set_global_weights (const std::vector<double> &weights)
{
	// Copy assignment of global weights
	global_weights = weights;

	// reinitialize the global solution on this cell
	global_solution.reinit (dof_handler.n_dofs());

	const unsigned int dofs_per_cell	= fe.n_dofs_per_cell();

	// Set global solution using the weights and the local basis.
	for (unsigned int index_basis=0;
			index_basis<dofs_per_cell;
			++index_basis)
	{
		// global_solution = 1*global_solution + global_weights[index_basis]*solution_vector[index_basis]
		global_solution.sadd (1, global_weights[index_basis], solution_vector[index_basis]);
	}

	is_set_global_weights = true;
}

/*!
 * Define the gloabl filename for pvtu-file in global output.
 */
template <int dim>
void
DiffusionProblemBasis<dim>::set_filename_global ()
{
	filename_global += (dim == 2 ?
			"solution-ms_fine-2d" :
			"solution-ms_fine-3d");

	filename_global += "_cell-" + Utilities::int_to_string(global_cell_number, 4) + ".vtu";
}

/*!
 * @brief Write basis results to disk.
 *
 * Write basis results to disk in vtu-format.
 */
template <int dim>
void
DiffusionProblemBasis<dim>::output_basis () const
{
	DataOut<dim> data_out;
	data_out.attach_dof_handler (dof_handler);
	for (unsigned int index_basis=0;
				index_basis<GeometryInfo<dim>::vertices_per_cell;
				++index_basis)
	{
		data_out.add_data_vector (solution_vector[index_basis], "basis_" + Utilities::int_to_string(index_basis, 1));
	}
	data_out.build_patches ();

	std::string filename = "basis";
	filename += "_cell-" + Utilities::int_to_string(global_cell_number, 4);
	filename += ".vtu";

	std::ofstream output (dim == 2 ?
					"2d-" + filename :
					"3d-" + filename);

	data_out.write_vtu (output);
}

/*!
 * Write out global solution in cell.
 */
template <int dim>
void
DiffusionProblemBasis<dim>::output_global_solution_in_cell () const
{
	Assert (is_set_global_weights,
				ExcMessage ("Global weights must be set first."));

	DataOut<dim> data_out;
	data_out.attach_dof_handler (dof_handler);
	data_out.add_data_vector (global_solution, "solution");
	data_out.build_patches ();

	std::ofstream output (filename_global.c_str());
	data_out.write_vtu (output);
}

/*!
 * @brief Run function of the object.
 *
 * Run the computation after object is built.
 */
template <int dim>
void DiffusionProblemBasis<dim>::run ()
{
	Assert (is_set_basis_data,
						ExcMessage ("All basis data must be set first."));

	make_grid ();

	setup_system ();

	assemble_system ();

	set_filename_global ();

	for (unsigned int index_basis=0;
			index_basis<GeometryInfo<dim>::vertices_per_cell;
			++index_basis)
	{
		// reset everything
		system_rhs.reinit(solution_vector[index_basis].size());
		system_matrix.reinit (sparsity_pattern);

		system_matrix.copy_from(diffusion_matrix);

		// Now take care of constraints
		constraints_vector[index_basis].condense(system_matrix, system_rhs);

		// Now solve
		solve_iterative (index_basis);
	}

	assemble_global_element_matrix ();

	if (output_flag)
		output_basis ();
}

} // end namespace DiffusionProblem

#endif /* INCLUDE_DIFFUSION_PROBLEM_BASIS_HPP_ */


#include <wav2midi/fft/fft.hpp>
#include <boost/math/constants/constants.hpp>

namespace wav2midi::fft {
    namespace {
        using namespace std::complex_literals;

        constexpr auto pi = boost::math::constants::pi<double>();

        // @see http://geisterchor.blogspot.jp/2015/05/cfft.html
        std::complex<double> twiddle_factor(std::size_t n, uint32_t jk) {
            return std::exp(-1.0i * (2 * pi * jk / n));
        }

        void fft_detail(std::vector<std::complex<double>> & x, std::size_t n) {
            if (n <= 1) return;

            std::vector<std::complex<double>> x_e(n/2);
            std::vector<std::complex<double>> x_o(n/2);

            for (auto j = 0u; j < n/2; ++j) {
                x_e[j] = (x[j] + x[j + n/2]);
                x_o[j] = (x[j] - x[j + n/2]) * twiddle_factor(n, j);
            }

            fft_detail(x_e, n/2);
            fft_detail(x_o, n/2);

            for (auto j = 0u; j < n/2; ++j) {
                x[2*j    ] = x_e[j];
                x[2*j + 1] = x_o[j];
            }
        }
    }

// public
    fft::fft(const std::vector<double> & samplings, window::window_t window) {
        const auto n = samplings.size();

        for (auto i = 0u; i < n; ++i) {
            auto x = double(i) / (n - 1);
            frequencies_.emplace_back(samplings[i] * window(x));
        }
    }

    const std::vector<std::complex<double>> & fft::execute() {
        const auto n = frequencies_.size();
        fft_detail(frequencies_, n);
        return frequencies_;
    }
}


#include <iostream>
#include <armadillo>
#include <cmath>
#include <time.h>
#include <fstream>

#include "../code-fredrik/comp_eig.hh"

using namespace std;
using namespace arma;

void RHO_A_FILL(vec &rho, mat &A, int N,double rhoN); //rho, kind of like a linespace
                                                 //A, Tridiagonal matrix
void Maxoff(mat &A, int N,int &k, int &l, double &max);        //Finds the max element of a matrix

int main(){
    int values[] = { 5, 10, 15, 20, 25, 30, 35, 40, 45, 50, 55, 60, 65, 70, 75, 80, 85, 90, 95, 100 };
    fstream Outfile;
    Outfile.open("steps.dat",ios::out);
    Outfile << "N     epsilon    steps  step_time  error"<<endl;
    for(int iter = 0; iter < 20; iter++){
        int N = values[iter]; //matrix size; N x N
        double rhoN = 6;
        double eps = 1E-10;
        double max;
        int k,l;

        double total_error = 0;
        double total_time = 0;
        int total_iterations = 0;

        // run 10 times and average
        int zcount = 10;
        for(int z = 0; z < zcount; z++) {
          std::cout << N << std::endl;
          mat A(N,N,fill::zeros); //indexes go from (0) to (N-1)
          mat S(N,N,fill::eye);
          vec rho(N,fill::zeros);
          vec eigen(N);

          RHO_A_FILL(rho,A,N,rhoN);
          Maxoff(A,N,k,l,max);
          //A.print();
          int iterations = 0;

          // solve with Armadillo's eig_sym
          vec arma_eigenvalues;
          mat arma_eigenvectors;
          eig_sym(arma_eigenvalues, arma_eigenvectors, A);

          double tau, t, s, c, il, ik, kk, ll, s_ik, s_il;
          double start, finish;

          start = clock();                      //clock value before eigen solve
          while(max > eps){
              tau = (A(l,l)-A(k,k))/(2.*A(k,l));
              if(tau>0){
                  t = 1.0/(tau + sqrt(1.0 + tau*tau));
              } else{
                  t = -1.0/( -tau + sqrt(1.0 + tau*tau));
              }

              //cosine and sine
              c = 1./sqrt(1.+t*t);
              s = t*c;

              //Jacobi rotating A round theta in N-dim space
              for(int i = 0; i<N; i++){
                  if ((i != k) && (i !=l)){

                      ik = A(i,k)*c - A(i,l)*s;
                      il = A(i,l)*c + A(i,k)*s;
                      A(i,k) = ik;
                      A(i,l) = il;
                      A(k,i) = ik;
                      A(l,i) = il;
                  }
                  s_ik = S(i,k);
                  s_il = S(i,l);
                  S(i,k) = c*s_ik - s*s_il;
                  S(i,l) = c*s_il + s*s_ik;
              }

              kk = A(k,k)*c*c - 2.*A(k,l)*c*s + A(l,l)*s*s;
              ll = A(l,l)*c*c + 2.*A(k,l)*c*s + A(k,k)*s*s;
              A(k,k) = kk;
              A(l,l) = ll ;
              A(k,l) = 0;
              A(l,k) = 0;

              iterations++;
              Maxoff(A,N,k,l,max);

          } //end of while
          finish = clock();                    //clock value after eigen solve

          //[eigen] is now eigenvalues
          for(int i = 0; i<N;i++){
              eigen(i) = A(i,i);
          }

          // eigen.print();

          // compare eigenvectors from own method with armadillo
          total_error += comp_eig(eigen, S, arma_eigenvalues, arma_eigenvectors);

          total_time += (finish -start)/CLOCKS_PER_SEC/iterations;
          total_iterations += iterations;

          /*
          fstream outfile;
          outfile.open("eigenvectors.dat",ios::out);

          for (int i = 0; i<N; i++){
              for (int j = 0; j<N; j++){
                  if(j%N == 0){outfile<<endl;}
                  outfile << S(i,j)<<" ";
                  }
              }
          outfile.close();
          */
        }

        Outfile <<N <<" "<<eps<<" "<<((double)total_iterations / zcount)<<" "<<(total_time / zcount) <<" "<<(total_error / zcount)<<endl;

    //    Outfile.close();
    }
} //end of main

void RHO_A_FILL(vec &rho, mat &A, int N,double rhoN){
    double h = rhoN/(N);
    for(int i = 0; i < N;i++){
        rho(i) = i*h;
        A(i,i) = 2./(h*h)+(rho(i)*rho(i));
        if(i<N-1){
            A(i+1,i) = -1./(h*h);
            A(i,i+1) = -1./(h*h);
            }
        }
    }

void Maxoff(mat &A, int N,int &k, int &l, double &max){
    max = 0;
    for(int i = 0; i < N ; i++){
        for(int j = i+1; j < N ; j++){
            if ( A(i,j)*A(i,j) > max){
                max =A(i,j)*A(i,j);
                k = i;
                l = j;
                }
            }
        }

    }


/* Tags: Minimal Spanning Tree, DFS, 2nd MST

  Key idea: * we look for 2nd best MST
            * construct MST with Kruskal
              --> Leias solution (equivalent to her Prim algorithm in terms of weight)
            * compute max edge on pairwise paths in MST with DFS in O(n^2) (paths are unique in tree)
            * loop through all edges _not_ in MST and
              compute best differences in adding edge (u,v) and
              removing worst edge on MST path between u and v
            * WHY? 2nd best MST is attained by adding a single edge that previosuly wasnt in MST
                   and then deleting the largest edge in the cycle that is introduced.
            * Since the graph is fully connected here, it is more efficient to
              precompute pairwise max_edge_weights in O(n^2) than to do it for each
              edge in O(E * V)
*/

// STL includes
#include <iostream>
#include <vector>
#include <queue>
#include <stack>

// BGL includes
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/pending/disjoint_sets.hpp>

typedef std::size_t                                            Index;

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
  boost::no_property, boost::property<boost::edge_weight_t, int> >      weighted_graph;
typedef boost::property_map<weighted_graph, boost::edge_weight_t>::type weight_map;
typedef boost::graph_traits<weighted_graph>::edge_descriptor            edge_desc;
typedef boost::graph_traits<weighted_graph>::vertex_descriptor          vertex_desc;
// edge: (u,v,c)
typedef std::tuple<int,int,int> Edge;
typedef std::vector<Edge> EdgeV;

void testcase()
{
  int n, tatt;
  std::cin >> n >> tatt;
  EdgeV edges;
  edges.reserve(n*n);
  for(int i = 0; i < n - 1; i++) {
      for(int j = 0; j < n - i - 1; j++) {
            int c;
            std::cin >> c;
            edges.emplace_back(i, i + j + 1, c);
      }
  }

  std::sort(edges.begin(), edges.end(),
      [](const Edge& e1, const Edge& e2) -> bool {
        return std::get<2>(e1) < std::get<2>(e2);
            });

  // construct MST with Kruskal --> Leias solution (equivalent to her Prim algorithm)
  std::vector<std::vector<int>> edge_in_mst(n, std::vector<int>(n, 0)); // nonzero means included
  boost::disjoint_sets_with_storage<> uf(n);
  int mst_cost = 0;
  int n_components = n;
  std::vector<std::vector<int>> mst_edges(n, std::vector<int>(0));
  for (EdgeV::const_iterator e = edges.begin(); e != edges.end(); ++e) {
    Index i1 = std::get<0>(*e);
    Index i2 = std::get<1>(*e);
    Index c1 = uf.find_set(i1);
    Index c2 = uf.find_set(i2);
    int cost = std::get<2>(*e);
    if(c1 != c2) {
      mst_cost += cost;
      uf.link(c1, c2);
      edge_in_mst[i1][i2] = cost;
      edge_in_mst[i2][i1] = cost;
      mst_edges[i1].push_back(i2);
      mst_edges[i2].push_back(i1);
      if(--n_components == 1) break;
    }
  }

  // compute max edge on pairwise paths in MST with DFS
  std::vector<std::vector<int>> max_e_betw(n, std::vector<int>(n, 0));
  for(int i = 0; i < n; i++) {
      std::stack<std::pair<int,int>> Q; // pair of (node, max edge on path form i to node)
      Q.push({i,0});
      std::vector<bool> visited(n, false);
      while(!Q.empty()) {
        auto v = Q.top();
        int j = v.first; int c = v.second;
        Q.pop();
        if(!visited[j]) {
          visited[j] = true;
          max_e_betw[i][j] = c;
          for(auto k : mst_edges[j]) Q.push({k, std::max(c, edge_in_mst[j][k])});
        }
      }
  }

  // loop through all edges not in MST, compute best differences in adding edge (u,v) and
  // removing worst edge on MST path between u and v
  int min_diff = std::numeric_limits<int>::max();
  for (EdgeV::const_iterator e = edges.begin(); e != edges.end(); ++e) {
    Index i1 = std::get<0>(*e);
    Index i2 = std::get<1>(*e);
    int cost = std::get<2>(*e);
    if(edge_in_mst[i1][i2] == 0) {
        min_diff = std::min(min_diff, cost - max_e_betw[i1][i2]);
    }
  }

  std::cout << mst_cost + min_diff << std::endl;
}

int main() {
    std::ios_base::sync_with_stdio(false);

    int t;
    std::cin >> t;
    for (int i = 0; i < t; ++i)
        testcase();
}


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

#ifndef CARDANO_HPP
#define CARDANO_HPP

// Solve polynomial equations of degree 3/4
// by Cardano/Ferrari's methods

#include <kv/interval.hpp>
#include <kv/rdouble.hpp>
#include <kv/complex.hpp>
#include <boost/numeric/ublas/vector.hpp>

namespace kv {

namespace ub = boost::numeric::ublas;

template <class T> bool cardano(const ub::vector< complex< interval<T> > > &in, ub::vector< complex< interval<T> > >& out)
{
	complex< interval<T> > a, b, c, p, q, w, m, n, nt, nn, tmp;
	int i;
	bool flag;

	if (in.size() != 4) return false;
	if (zero_in(in(3).real()) && zero_in(in(3).imag())) return false;

	a = in(2) / in(3);
	b = in(1) / in(3);
	c = in(0) / in(3);

	p = b - pow(a, 2) / 3.;
	q = 2 * pow(a, 3) /27. - a * b / 3. + c;
	w = complex< interval<T> >(-1., sqrt(interval<T>(3.))) / 2.;

	tmp = sqrt(pow(q, 2)/4. + pow(p, 3) / 27.) ;
	m = pow(-q / 2. + tmp, 1. / interval<T>(3.));
	n = pow(-q / 2. - tmp, 1. / interval<T>(3.));

	nt = n;
	flag = false;

	for (i=0; i<3; i++) {
		tmp = m * nt + p/3.;
		if (zero_in(tmp.real()) && zero_in(tmp.imag())) {
			if (flag == false) {
				nn = nt;
				flag = true;
			} else {
				nn.real() = interval<T>::hull(nn.real(), nt.real());
				nn.imag() = interval<T>::hull(nn.imag(), nt.imag());
			}
		}
		nt = w * nt;
	}

	out.resize(3);
	out(0) = m + nn - a / 3.;
	out(1) = w * m + w * w * nn - a / 3.;
	out(2) = w * w * m + w * nn - a / 3.;

	return true;
}

template <class T> bool ferrari(const ub::vector< complex< interval<T> > > &in, ub::vector< complex< interval<T> > >& out)
{
	complex< interval<T> > a, b, c, d, p, q, r, tmp;
	ub::vector< complex< interval<T> > > ci, co;
	complex< interval<T> > l, m, n, nt, nn;
	int i;
	bool flag;

	if (in.size() != 5) return false;
	if (zero_in(in(4).real()) && zero_in(in(3).imag())) return false;

	a = in(3) / in(4);
	b = in(2) / in(4);
	c = in(1) / in(4);
	d = in(0) / in(4);

	p = b - pow(a, 2) * 3. / 8.;
	q = c - b * a / 2. + pow(a, 3) / 8.;
	r = d - c * a / 4. + b * pow(a, 2) / 16. - pow(a, 4) * 3. / 256.;

	ci.resize(4);
	ci(3) = 1.;
	ci(2) = -p / 2.;
	ci(1) = -r;
	ci(0) = r * p / 2. - pow(q, 2) / 8.;

	cardano(ci, co);

	l = co(0);
	m = sqrt(2. * l - p);
	n = sqrt(pow(l, 2) - r);

	nt = n;
	flag = false;

	for (i=0; i<2; i++) {
		tmp = 2. * m * nt + q;
		if (zero_in(tmp.real()) && zero_in(tmp.imag())) {
			if (flag == false) {
				nn = nt;
				flag = true;
			} else {
				nn.real() = interval<T>::hull(nn.real(), nt.real());
				nn.imag() = interval<T>::hull(nn.imag(), nt.imag());
			}
		}
		nt = -nt;
	}

	out.resize(4);
	out(0) = (m + sqrt(pow(m, 2) - 4. * (l - nn))) / 2. - a / 4.;
	out(1) = (m - sqrt(pow(m, 2) - 4. * (l - nn))) / 2. - a / 4.;
	out(2) = (-m + sqrt(pow(m, 2) - 4. * (l + nn))) / 2. - a / 4.;
	out(3) = (-m - sqrt(pow(m, 2) - 4. * (l + nn))) / 2. - a / 4.;

	return true;
}

} // namespace kv

#endif // CARDANO_HPP


//
// Copyright 2019 Olzhas Zhumabek <anonymous.from.applecity@gmail.com>
// Copyright 2021 Pranam Lashkari <plashkari628@gmail.com>
//
// Use, modification and distribution are 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)
//
#ifndef BOOST_GIL_IMAGE_PROCESSING_NUMERIC_HPP
#define BOOST_GIL_IMAGE_PROCESSING_NUMERIC_HPP

#include <boost/gil/image_processing/kernel.hpp>
#include <boost/gil/image_processing/convolve.hpp>
#include <boost/gil/image_view.hpp>
#include <boost/gil/typedefs.hpp>
#include <boost/gil/detail/math.hpp>
// fixes ambigious call to std::abs, https://stackoverflow.com/a/30084734/4593721
#include <cstdlib>
#include <cmath>

namespace boost { namespace gil {

/// \defgroup ImageProcessingMath
/// \brief Math operations for IP algorithms
///
/// This is mostly handful of mathemtical operations that are required by other
/// image processing algorithms
///
/// \brief Normalized cardinal sine
/// \ingroup ImageProcessingMath
///
/// normalized_sinc(x) = sin(pi * x) / (pi * x)
///
inline double normalized_sinc(double x)
{
    return std::sin(x * boost::gil::detail::pi) / (x * boost::gil::detail::pi);
}

/// \brief Lanczos response at point x
/// \ingroup ImageProcessingMath
///
/// Lanczos response is defined as:
/// x == 0: 1
/// -a < x && x < a: 0
/// otherwise: normalized_sinc(x) / normalized_sinc(x / a)
inline double lanczos(double x, std::ptrdiff_t a)
{
    // means == but <= avoids compiler warning
    if (0 <= x && x <= 0)
        return 1;

    if (static_cast<double>(-a) < x && x < static_cast<double>(a))
        return normalized_sinc(x) / normalized_sinc(x / static_cast<double>(a));

    return 0;
}

#if BOOST_WORKAROUND(BOOST_MSVC, >= 1400)
#pragma warning(push)
#pragma warning(disable:4244) // 'argument': conversion from 'const Channel' to 'BaseChannelValue', possible loss of data
#endif

inline void compute_tensor_entries(
    boost::gil::gray16s_view_t dx,
    boost::gil::gray16s_view_t dy,
    boost::gil::gray32f_view_t m11,
    boost::gil::gray32f_view_t m12_21,
    boost::gil::gray32f_view_t m22)
{
    for (std::ptrdiff_t y = 0; y < dx.height(); ++y) {
        for (std::ptrdiff_t x = 0; x < dx.width(); ++x) {
            auto dx_value = dx(x, y);
            auto dy_value = dy(x, y);
            m11(x, y) = dx_value * dx_value;
            m12_21(x, y) = dx_value * dy_value;
            m22(x, y) = dy_value * dy_value;
        }
    }
}

#if BOOST_WORKAROUND(BOOST_MSVC, >= 1400)
#pragma warning(pop)
#endif

/// \brief Generate mean kernel
/// \ingroup ImageProcessingMath
///
/// Fills supplied view with normalized mean
/// in which all entries will be equal to
/// \code 1 / (dst.size()) \endcode
template <typename T = float, typename Allocator = std::allocator<T>>
inline detail::kernel_2d<T, Allocator> generate_normalized_mean(std::size_t side_length)
{
    if (side_length % 2 != 1)
        throw std::invalid_argument("kernel dimensions should be odd and equal");
    const float entry = 1.0f / static_cast<float>(side_length * side_length);

    detail::kernel_2d<T, Allocator> result(side_length, side_length / 2, side_length / 2);
    for (auto& cell: result) {
        cell = entry;
    }

    return result;
}

/// \brief Generate kernel with all 1s
/// \ingroup ImageProcessingMath
///
/// Fills supplied view with 1s (ones)
template <typename T = float, typename Allocator = std::allocator<T>>
inline detail::kernel_2d<T, Allocator> generate_unnormalized_mean(std::size_t side_length)
{
    if (side_length % 2 != 1)
        throw std::invalid_argument("kernel dimensions should be odd and equal");

    detail::kernel_2d<T, Allocator> result(side_length, side_length / 2, side_length / 2);
    for (auto& cell: result) {
        cell = 1.0f;
    }

    return result;
}

/// \brief Generate Gaussian kernel
/// \ingroup ImageProcessingMath
///
/// Fills supplied view with values taken from Gaussian distribution. See
/// https://en.wikipedia.org/wiki/Gaussian_blur
template <typename T = float, typename Allocator = std::allocator<T>>
inline detail::kernel_2d<T, Allocator> generate_gaussian_kernel(std::size_t side_length, double sigma)
{
    if (side_length % 2 != 1)
        throw std::invalid_argument("kernel dimensions should be odd and equal");

    const double denominator = 2 * boost::gil::detail::pi * sigma * sigma;
    auto middle = side_length / 2;
    std::vector<T, Allocator> values(side_length * side_length);
    for (std::size_t y = 0; y < side_length; ++y)
    {
        for (std::size_t x = 0; x < side_length; ++x)
        {
            const auto delta_x = middle > x ? middle - x : x - middle;
            const auto delta_y = middle > y ? middle - y : y - middle;
            const double power = (delta_x * delta_x +  delta_y * delta_y) / (2 * sigma * sigma);
            const double nominator = std::exp(-power);
            const float value = static_cast<float>(nominator / denominator);
            values[y * side_length + x] = value;
        }
    }

    return detail::kernel_2d<T, Allocator>(values.begin(), values.size(), middle, middle);
}

/// \brief Generates Sobel operator in horizontal direction
/// \ingroup ImageProcessingMath
///
/// Generates a kernel which will represent Sobel operator in
/// horizontal direction of specified degree (no need to convolve multiple times
/// to obtain the desired degree).
/// https://www.researchgate.net/publication/239398674_An_Isotropic_3_3_Image_Gradient_Operator
template <typename T = float, typename Allocator = std::allocator<T>>
inline detail::kernel_2d<T, Allocator> generate_dx_sobel(unsigned int degree = 1)
{
    switch (degree)
    {
        case 0:
        {
            return detail::get_identity_kernel<T, Allocator>();
        }
        case 1:
        {
            detail::kernel_2d<T, Allocator> result(3, 1, 1);
            std::copy(detail::dx_sobel.begin(), detail::dx_sobel.end(), result.begin());
            return result;
        }
        default:
            throw std::logic_error("not supported yet");
    }

    //to not upset compiler
    throw std::runtime_error("unreachable statement");
}

/// \brief Generate Scharr operator in horizontal direction
/// \ingroup ImageProcessingMath
///
/// Generates a kernel which will represent Scharr operator in
/// horizontal direction of specified degree (no need to convolve multiple times
/// to obtain the desired degree).
/// https://www.researchgate.net/profile/Hanno_Scharr/publication/220955743_Optimal_Filters_for_Extended_Optical_Flow/links/004635151972eda98f000000/Optimal-Filters-for-Extended-Optical-Flow.pdf
template <typename T = float, typename Allocator = std::allocator<T>>
inline detail::kernel_2d<T, Allocator> generate_dx_scharr(unsigned int degree = 1)
{
    switch (degree)
    {
        case 0:
        {
            return detail::get_identity_kernel<T, Allocator>();
        }
        case 1:
        {
            detail::kernel_2d<T, Allocator> result(3, 1, 1);
            std::copy(detail::dx_scharr.begin(), detail::dx_scharr.end(), result.begin());
            return result;
        }
        default:
            throw std::logic_error("not supported yet");
    }

    //to not upset compiler
    throw std::runtime_error("unreachable statement");
}

/// \brief Generates Sobel operator in vertical direction
/// \ingroup ImageProcessingMath
///
/// Generates a kernel which will represent Sobel operator in
/// vertical direction of specified degree (no need to convolve multiple times
/// to obtain the desired degree).
/// https://www.researchgate.net/publication/239398674_An_Isotropic_3_3_Image_Gradient_Operator
template <typename T = float, typename Allocator = std::allocator<T>>
inline detail::kernel_2d<T, Allocator> generate_dy_sobel(unsigned int degree = 1)
{
    switch (degree)
    {
        case 0:
        {
            return detail::get_identity_kernel<T, Allocator>();
        }
        case 1:
        {
            detail::kernel_2d<T, Allocator> result(3, 1, 1);
            std::copy(detail::dy_sobel.begin(), detail::dy_sobel.end(), result.begin());
            return result;
        }
        default:
            throw std::logic_error("not supported yet");
    }

    //to not upset compiler
    throw std::runtime_error("unreachable statement");
}

/// \brief Generate Scharr operator in vertical direction
/// \ingroup ImageProcessingMath
///
/// Generates a kernel which will represent Scharr operator in
/// vertical direction of specified degree (no need to convolve multiple times
/// to obtain the desired degree).
/// https://www.researchgate.net/profile/Hanno_Scharr/publication/220955743_Optimal_Filters_for_Extended_Optical_Flow/links/004635151972eda98f000000/Optimal-Filters-for-Extended-Optical-Flow.pdf
template <typename T = float, typename Allocator = std::allocator<T>>
inline detail::kernel_2d<T, Allocator> generate_dy_scharr(unsigned int degree = 1)
{
    switch (degree)
    {
        case 0:
        {
            return detail::get_identity_kernel<T, Allocator>();
        }
        case 1:
        {
            detail::kernel_2d<T, Allocator> result(3, 1, 1);
            std::copy(detail::dy_scharr.begin(), detail::dy_scharr.end(), result.begin());
            return result;
        }
        default:
            throw std::logic_error("not supported yet");
    }

    //to not upset compiler
    throw std::runtime_error("unreachable statement");
}

/// \brief Compute xy gradient, and second order x and y gradients
/// \ingroup ImageProcessingMath
///
/// Hessian matrix is defined as a matrix of partial derivates
/// for 2d case, it is [[ddxx, dxdy], [dxdy, ddyy].
/// d stands for derivative, and x or y stand for direction.
/// For example, dx stands for derivative (gradient) in horizontal
/// direction, and ddxx means second order derivative in horizon direction
/// https://en.wikipedia.org/wiki/Hessian_matrix
template <typename GradientView, typename OutputView>
inline void compute_hessian_entries(
    GradientView dx,
    GradientView dy,
    OutputView ddxx,
    OutputView dxdy,
    OutputView ddyy)
{
    auto sobel_x = generate_dx_sobel();
    auto sobel_y = generate_dy_sobel();
    detail::convolve_2d(dx, sobel_x, ddxx);
    detail::convolve_2d(dx, sobel_y, dxdy);
    detail::convolve_2d(dy, sobel_y, ddyy);
}

}} // namespace boost::gil

#endif


#include <cmath>
#include <cstdlib>
#include <tuple>
#include <iostream>
#include <Eigen/Dense>

namespace LogisticRegression
{
	//Z(X₁,X₂)=1(θ₁X₁+θ₂X₂>800)
	//Z(X₁,X₂)=0(θ₁X₁+θ₂X₂<=800)
	//θ₁=10,θ₂=150,X₂=1
	std::tuple<Eigen::MatrixX2d, Eigen::VectorXd> RandomGenterateTrainSet(size_t counts)
	{
		Eigen::MatrixX2d train_input(counts, 2);
		Eigen::VectorXd train_output(counts);
		for (size_t i = 0; i < counts; i++)
		{
			double X₁ = std::rand() % 100 + 20, X₂ = 1;
			train_input(i, 0) = X₁;
			train_input(i, 1) = X₂;
			double θ₁ = 10, θ₂ = 150;
			double loss = /*std::rand() % 11 - 5;*/0;
			double hx = θ₁ * X₁ + θ₂ * X₂ + loss;
			train_output[i] = hx > 800 ? 1 : 0;
		}
		/*for (size_t i = 0; i < train_input.cols() - 1; i++)
		{
			auto&& col_i = train_input.col(i);
			col_i = (col_i.array() - col_i.sum() / train_input.rows() / 2) / (col_i.maxCoeff() - col_i.minCoeff());
		}*/
		return { train_input/*.normalized()*/, train_output };
	}

	//        -z
	//hx=1/1+e
	//          i      i      i         i
	//-1/m * ∑y log(hx )+(1-y )log(1-hx )
	double LossFunction(const Eigen::MatrixXd& model, const Eigen::MatrixXd& train_input, const Eigen::MatrixXd& train_output)
	{
		auto z = (train_input * model).array();
		//translation relative position
		auto length = z.maxCoeff() / 2 + z.minCoeff() / 2;
		auto z_translation = z - length;
		auto hx = 1 / (1 + Eigen::exp(-1 * z_translation));
		//(0,1)
		constexpr double multiple = 0.999999;
		auto hx_limit = hx * multiple;
		//std::cout << hx_limit << "\n\n";
		//std::cout << train_output << "\n\n";
		return -1.0 / train_output.rows() * (train_output.array() * Eigen::log(hx_limit) + (1 - train_output.array()) * Eigen::log(1 - hx_limit)).sum();
	}

	//                            i      i     i
	//θ = θ - α * 1/m * ∑(h(x) - y(x) ) * x
	// j    j                                  j
	void GradientDescent(Eigen::MatrixXd& model, const Eigen::MatrixXd& train_input, const Eigen::MatrixXd& train_output, const Eigen::VectorXd& learning_rate, size_t batch_size)
	{
		for (size_t i = 0; i < batch_size; i++)
		{
			for (size_t train_index = 0; train_index < train_input.rows(); train_index++)
			{
				auto z = (train_input * model).array();
				//translation relative position
				auto length = z.maxCoeff() / 2 + z.minCoeff() / 2;
				auto z_translation = z - length;
				auto hx = 1 / (1 + Eigen::exp(-1 * z_translation));
				//(0,1)
				constexpr double multiple = 0.999999;
				auto hx_limit = hx * multiple;
				auto hx_sub_yx = hx_limit.matrix() - train_output;
				//std::cout << hx_sub_yx << "\n\n";
				Eigen::MatrixXd duplicate_line(model.cols(), train_input.cols());
				auto&& reference = duplicate_line << train_input.row(train_index);
				for (size_t line = 0; line < model.cols() - 1; line++)
				{
					reference, train_input.row(train_index);
				}
				Eigen::MatrixXd update = learning_rate.array() * (1.0 / train_input.rows() * hx_sub_yx.row(train_index) * duplicate_line).transpose().array();
				//std::cout << duplicate_line << "\n\n";
				//std::cout << update << "\n\n";
				std::cout << LossFunction(model, train_input, train_output) << "\n\n";
				model -= update;
				//std::cout << model << "\n\n";
			}
		}
	}
}

int main()
{
	std::cout << std::boolalpha;

	auto [train_input, train_output] = LogisticRegression::RandomGenterateTrainSet(100);
	//std::cout << train_input << std::endl;
	//std::cout << train_output << std::endl;

	Eigen::MatrixXd model = Eigen::Vector2d(1, 200);
	Eigen::Vector2d learning_rate(0.0001, 0.001);
	double limit = 0.05;
	size_t batch_size = 100;
	//std::cout << LogisticRegression::LossFunction(model, train_input, train_output) << std::endl;
	LogisticRegression::GradientDescent(model, train_input, train_output, learning_rate, batch_size);
	return 0;
}


#ifndef RBF_INTERPOLATION_HPP
#define RBF_INTERPOLATION_HPP

#include <vector>
#include <Eigen/Core>

namespace mathtoolbox
{
    enum class RbfType
    {
        Gaussian,         // f(r) = exp(-(epsilon * r)^2)
        ThinPlateSpline,  // f(r) = (r^2) * log(r)
        InverseQuadratic, // f(r) = (1 + (epsilon * r)^2)^(-1)
        Linear,           // f(r) = r
    };

    class RbfInterpolation
    {
    public:
        RbfInterpolation(RbfType rbf_type = RbfType::ThinPlateSpline, double epsilon = 2.0);

        // API
        void   SetData(const Eigen::MatrixXd& X, const Eigen::VectorXd& y);
        void   ComputeWeights(bool use_regularization = false, double lambda = 0.001);
        double GetValue(const Eigen::VectorXd& x) const;

        // Getter methods
        const Eigen::VectorXd& GetY() const { return y; }
        const Eigen::MatrixXd& GetX() const { return X; }
        const Eigen::VectorXd& GetW() const { return w; }

    private:

        // Function type
        RbfType rbf_type;

        // A control parameter used in some kernel functions
        double epsilon;

        // Data points
        Eigen::MatrixXd X;
        Eigen::VectorXd y;

        // Weights
        Eigen::VectorXd w;

        // Returns f(r)
        double GetRbfValue(double r) const;

        // Returns f(||xj - xi||)
        double GetRbfValue(const Eigen::VectorXd& xi, const Eigen::VectorXd& xj) const;
    };
}

#endif // RBF_INTERPOLATION_HPP


/**
 * @file kalman_filter.cpp
 * @brief Kalman Filter.
 *
 */

#include <kalman_filter.hpp>
#include <Eigen/Dense>
#include <iostream>

using namespace filter;

KalmanFilter::KalmanFilter(
    const Eigen::MatrixXd& A,
    const Eigen::MatrixXd& C,
    const Eigen::MatrixXd& Q,
    const Eigen::MatrixXd& R,
    const Eigen::MatrixXd& P
    ):
    A(A), C(C), Q(Q), R(R), P0(P),
    m(C.rows()), n(A.rows()),
    I(n, n), x_hat(n), x_hat_new(n),
    initialized(false)
{
    // Initialize the identity transformation.
    I.setIdentity();
}

void KalmanFilter::init(double t0, const Eigen::VectorXd& x0)
{
    // Set parameters based on initial guess.
    x_hat = x0;
    P = P0;
    this->t0 = t0;
    initialized = true;
}

void KalmanFilter::init()
{
    // Set parameters to zero.
    x_hat.setZero();
    P = P0;
    t0 = 0;
    initialized = true;
}

Eigen::VectorXd KalmanFilter::compute(const Eigen::VectorXd& y, double dt)
{
    // Error handling.
    if(!this->initialized) throw std::runtime_error("Filter is not initialized!");

    // Set the new state.
    x_hat_new = A * x_hat;

    // Predict error covariance.
    P = A * P * A.transpose() + Q;

    // Update Kalman gain.
    K = P * C.transpose() * (C * P * C.transpose() + R).inverse();

    // Update estimate covariance.
    P = (I - K * C) * P;

    // Update state.
    x_hat_new += K * (y - C * x_hat_new);
    x_hat = x_hat_new;

    return x_hat;
}


/*=============================================================================
  Copyright (c) 2010-2016 Bolero MURAKAMI
  https://github.com/bolero-MURAKAMI/Sprig

  Distributed under 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)
=============================================================================*/
#ifndef SPRIG_CURVE_CATMULL_ROM_SPLINE_HPP
#define SPRIG_CURVE_CATMULL_ROM_SPLINE_HPP

#include <sprig/config/config.hpp>

#ifdef SPRIG_USING_PRAGMA_ONCE
#	pragma once
#endif	// #ifdef SPRIG_USING_PRAGMA_ONCE

#include <boost/geometry/core/access.hpp>
#include <sprig/detail/pow.hpp>

namespace sprig {
	//
	// catmull_rom_spline
	//
	template<typename T, typename Point>
	SPRIG_INLINE Point
	catmull_rom_spline(T const& t, Point const& p0, Point const& p1, Point const& p2, Point const& p3) {
		namespace bg = boost::geometry;
		using sprig::detail::pow2;
		using sprig::detail::pow3;
		return value_type(
			(-bg::get<0>(p0) + 3 * bg::get<0>(p1) - 3 * bg::get<0>(p2) + bg::get<0>(p3)) / 2 * pow3(t)
				+ (2 * bg::get<0>(p0) - 5 * bg::get<0>(p1) + 4 * bg::get<0>(p2) - bg::get<0>(p3) / 2 * pow2(t)
				+ (-bg::get<0>(p0) + bg::get<0>(p2)) / 2 * t
				+ bg::get<0>(p1)
				,
			(-bg::get<1>(p0) + 3 * bg::get<1>(p1) - 3 * bg::get<1>(p2) + bg::get<1>(p3)) / 2 * pow3(t)
				+ (2 * bg::get<1>(p0) - 5 * bg::get<1>(p1) + 4 * bg::get<1>(p2) - bg::get<1>(p3)) / 2 * pow2(t)
				+ (-bg::get<1>(p0) + bg::get<1>(p2)) / 2 * t
				+ bg::get<1>(p1)
			);
	}
	//
	// catmull_rom_spline_start
	//
	template<typename T, typename Point>
	SPRIG_INLINE Point
	catmull_rom_spline_start(T const& t, Point const& p0, Point const& p1, Point const& p2) {
		namespace bg = boost::geometry;
		using sprig::detail::pow2;
		return value_type(
			(bg::get<0>(p0) - 2 * bg::get<0>(p1) + bg::get<0>(p2)) / 2 * pow2(t)
				+ (-3 * bg::get<0>(p0) + 4 * bg::get<0>(p1) - bg::get<0>(p2)) / 2 * t
				+ bg::get<0>(p0)
				,
			(bg::get<1>(p0) - 2 * bg::get<1>(p1) + bg::get<1>(p2)) / 2 * pow2(t)
				+ (-3 * bg::get<1>(p0) + 4 * bg::get<1>(p1) - bg::get<1>(p2)) / 2 * t
				+ bg::get<1>(p0)
			);
	}
	//
	// catmull_rom_spline_end
	//
	template<typename T, typename Point>
	SPRIG_INLINE Point
	catmull_rom_spline_end(T const& t, Point const& p0, Point const& p1, Point const& p2) {
		namespace bg = boost::geometry;
		using sprig::detail::pow2;
		return value_type(
			(bg::get<0>(p0) - 2 * bg::get<0>(p1) + bg::get<0>(p2)) / 2 * pow2(t)
				+ (-bg::get<0>(p0) + bg::get<0>(p2)) / 2 * t
				+ bg::get<0>(p1)
				,
			(bg::get<1>(p0) - 2 * bg::get<1>(p1) + bg::get<1>(p2)) / 2 * pow2(t)
				+ (-bg::get<1>(p0) + bg::get<1>(p2)) / 2 * t
				+ bg::get<1>(p1)
			);
	}

	//
	// catmull_rom_spline_f
	//
	template<typename Point>
	struct catmull_rom_spline_f {
	public:
		typedef Point result_type;
	private:
		Point p0_;
		Point p1_;
		Point p2_;
		Point p3_;
	public:
		catmull_rom_spline_f(Point const& p0, Point const& p1, Point const& p2, Point const& p3)
			: p0_(p0), p1_(p1), p2_(p2), p3_(p3)
		{}
		template<typename T>
		Point operator()(T const& t) const {
			return sprig::catmull_rom_spline(t, p0_, p1_, p2_, p3_);
		}
	};
	//
	// catmull_rom_spline_start_f
	//
	template<typename Point>
	struct catmull_rom_spline_start_f {
	public:
		typedef Point result_type;
	private:
		Point p0_;
		Point p1_;
		Point p2_;
	public:
		catmull_rom_spline_start_f(Point const& p0, Point const& p1, Point const& p2)
			: p0_(p0), p1_(p1), p2_(p2)
		{}
		template<typename T>
		Point operator()(T const& t) const {
			return sprig::catmull_rom_spline_start(t, p0_, p1_, p2_);
		}
	};
	//
	// catmull_rom_spline_end_f
	//
	template<typename Point>
	struct catmull_rom_spline_end_f {
	public:
		typedef Point result_type;
	private:
		Point p0_;
		Point p1_;
		Point p2_;
	public:
		catmull_rom_spline_end_f(Point const& p0, Point const& p1, Point const& p2)
			: p0_(p0), p1_(p1), p2_(p2)
		{}
		template<typename T>
		Point operator()(T const& t) const {
			return sprig::catmull_rom_spline_end(t, p0_, p1_, p2_);
		}
	};

	//
	// make_catmull_rom_spline
	//
	template<typename Point>
	SPRIG_INLINE sprig::catmull_rom_spline_f<Point>
	make_catmull_rom_spline(Point const& p0, Point const& p1, Point const& p2, Point const& p3) {
		return sprig::catmull_rom_spline_f<Point>(p0, p1, p2, p3);
	}
	//
	// make_catmull_rom_spline_start
	//
	template<typename Point>
	SPRIG_INLINE sprig::catmull_rom_spline_start_f<Point>
	make_catmull_rom_spline_start(Point const& p0, Point const& p1, Point const& p2) {
		return sprig::catmull_rom_spline_start_f<Point>(p0, p1, p2);
	}
	//
	// make_catmull_rom_spline_end
	//
	template<typename Point>
	SPRIG_INLINE sprig::catmull_rom_spline_end_f<Point>
	make_catmull_rom_spline_end(Point const& p0, Point const& p1, Point const& p2) {
		return sprig::catmull_rom_spline_end_f<Point>(p0, p1, p2);
	}
}	// namespace sprig

#endif	// #ifndef SPRIG_CURVE_CATMULL_ROM_SPLINE_HPP


#ifndef __NED_CLASS__
#define __NED_CLASS__

#include "math.h"
#include <Eigen/Dense>

namespace geodesy_ned {
    static const double a = 6378137;
    static const double b = 6356752.3142;
    static const double esq = 6.69437999014 * 0.001;
    static const double e1sq = 6.73949674228 * 0.001;
    static const double f = 1 / 298.257223563;

    class Ned {
    public:

        Ned( const double lat,
             const double lon,
             const double height )
        {
            // Save NED origin
            _init_lat = deg2Rad( lat );
            _init_lon = deg2Rad( lon );
            _init_h = height;

            // Compute ECEF of NED origin
            geodetic2Ecef( lat, lon, height,
                        _init_ecef_x,
                        _init_ecef_y,
                        _init_ecef_z );

            // Compute ECEF to NED and NED to ECEF matrices
            double phiP = atan2( _init_ecef_z, sqrt( pow( _init_ecef_x, 2 ) +
                                                    pow( _init_ecef_y, 2 ) ) );

            _ecef_to_ned_matrix = __nRe__( phiP, _init_lon );
            _ned_to_ecef_matrix = __nRe__( _init_lat, _init_lon ).transpose();
        }

        void
        geodetic2Ecef( const double lat,
                    const double lon,
                    const double height,
                    double& x,
                    double& y,
                    double& z )
        {
            // Convert geodetic coordinates to ECEF.
            // http://code.google.com/p/pysatel/source/browse/trunk/coord.py?r=22
            double lat_rad = deg2Rad( lat );
            double lon_rad = deg2Rad( lon );
            double xi = sqrt(1 - esq * sin( lat_rad ) * sin( lat_rad ) );
            x = ( a / xi + height ) * cos( lat_rad ) * cos( lon_rad );
            y = ( a / xi + height ) * cos( lat_rad ) * sin( lon_rad );
            z = ( a / xi * ( 1 - esq ) + height ) * sin( lat_rad );
        }

        void
        ecef2Geodetic( const double x,
                    const double y,
                    const double z,
                    double& lat,
                    double& lon,
                    double& height )
        {
            // Convert ECEF coordinates to geodetic.
            // J. Zhu, "Conversion of Earth-centered Earth-fixed coordinates
            // to geodetic coordinates," IEEE Transactions on Aerospace and
            // Electronic Systems, vol. 30, pp. 957-961, 1994.

            double r = sqrt( x * x + y * y );
            double Esq = a * a - b * b;
            double F = 54 * b * b * z * z;
            double G = r * r + (1 - esq) * z * z - esq * Esq;
            double C = (esq * esq * F * r * r) / pow( G, 3 );
            double S = __cbrt__( 1 + C + sqrt( C * C + 2 * C ) );
            double P = F / (3 * pow( (S + 1 / S + 1), 2 ) * G * G);
            double Q = sqrt( 1 + 2 * esq * esq * P );
            double r_0 =  -(P * esq * r) / (1 + Q) + sqrt( 0.5 * a * a * (1 + 1.0 / Q) - P * (1 - esq) * z * z / (Q * (1 + Q)) - 0.5 * P * r * r);
            double U = sqrt( pow( (r - esq * r_0), 2 ) + z * z );
            double V = sqrt( pow( (r - esq * r_0), 2 ) + (1 - esq) * z * z );
            double Z_0 = b * b * z / (a * V);
            height = U * (1 - b * b / (a * V));
            lat = rad2Deg( atan( (z + e1sq * Z_0) / r ) );
            lon = rad2Deg( atan2( y, x ) );
        }

        void
        ecef2Ned( const double x,
                const double y,
                const double z,
                double& north,
                double& east,
                double& depth )
        {
            // Converts ECEF coordinate pos into local-tangent-plane ENU
            // coordinates relative to another ECEF coordinate ref. Returns a tuple
            // (East, North, Up).

            Eigen::Vector3d vect, ret;
            vect(0) = x - _init_ecef_x;
            vect(1) = y - _init_ecef_y;
            vect(2) = z - _init_ecef_z;
            ret = _ecef_to_ned_matrix * vect;
            north = ret(0);
            east = ret(1);
            depth = -ret(2);
        }

        void
        ned2Ecef( const double north,
                const double east,
                const double depth,
                double& x,
                double& y,
                double& z )
        {
            // NED (north/east/down) to ECEF coordinate system conversion.
            Eigen::Vector3d ned, ret;
            ned(0) = north;
            ned(1) = east;
            ned(2) = -depth;
            ret = _ned_to_ecef_matrix * ned;
            x = ret(0) + _init_ecef_x;
            y = ret(1) + _init_ecef_y;
            z = ret(2) + _init_ecef_z;
        }

        void
        geodetic2Ned( const double lat,
                    const double lon,
                    const double height,
                    double& north,
                    double& east,
                    double& depth )
        {
            // Geodetic position to a local NED system """
            double x, y, z;
            geodetic2Ecef( lat, lon, height,
                        x, y, z );
            ecef2Ned( x, y, z,
                    north, east, depth );
        }

        void
        ned2Geodetic( const double north,
                    const double east,
                    const double depth,
                    double& lat,
                    double& lon,
                    double& height )
        {
            // Local NED position to geodetic
            double x, y, z;
            ned2Ecef( north, east, depth,
                    x, y, z );
            ecef2Geodetic( x, y, z,
                        lat, lon, height );
        }

    private:
        double _init_lat;
        double _init_lon;
        double _init_h;
        double _init_ecef_x;
        double _init_ecef_y;
        double _init_ecef_z;
        Eigen::Matrix3d _ecef_to_ned_matrix;
        Eigen::Matrix3d _ned_to_ecef_matrix;

        double
        __cbrt__( const double x )
        {
            if( x >= 0.0 ) {
                return pow( x, 1.0/3.0 );
            }
            else {
                return -pow( fabs(x), 1.0/3.0 );
            }
        }

        Eigen::Matrix3d
        __nRe__( const double lat_rad,
                const double lon_rad )
        {
            double sLat = sin( lat_rad );
            double sLon = sin( lon_rad );
            double cLat = cos( lat_rad );
            double cLon = cos( lon_rad );

            Eigen::Matrix3d ret;
            ret(0, 0) = -sLat*cLon;     ret(0, 1) = -sLat*sLon;     ret(0, 2) = cLat;
            ret(1, 0) = -sLon;          ret(1, 1) = cLon;           ret(1, 2) = 0.0;
            ret(2, 0) = cLat*cLon;      ret(2, 1) = cLat*sLon;      ret(2, 2) = sLat;

            return ret;
        }

        double
        rad2Deg( const double radians )
        {
            return ( radians / M_PI ) * 180.0;
        }

        double
        deg2Rad( const double degrees )
        {
            return ( degrees / 180.0 ) * M_PI;
        }

    };
}; // namespace geodesy_ned
#endif // __NED_CLASS__