/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2000 - 2019 by the deal.II authors
 *
 * This file is modification of the version in the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level directory of deal.II.
 *
 * ---------------------------------------------------------------------
 *
 * Author: Wolfgang Bangerth, University of Heidelberg, 2000
 * Modified version.
 */
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/dofs/dof_handler.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/grid/grid_generator.h>
#include <deal.II/grid/grid_out.h>
#include <deal.II/grid/grid_refinement.h>
#include <deal.II/grid/tria.h>
#include <deal.II/lac/affine_constraints.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/vector.h>
#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/error_estimator.h>
#include <deal.II/numerics/vector_tools.h>
#include <fstream>

#include <bench_base.hpp>

DEFINE_uint32(
    num_refine_cycles, 1,
    "Number of refinement cycles for the adaptive refinement within deal.ii");
DEFINE_uint32(init_refine_level, 4,
              "Initial level for the refinement of the mesh.");
DEFINE_bool(dealii_orig, false, "Solve with dealii iterative CG");
DEFINE_bool(vis_sol, false, "Print the solution for visualization");

#define CHECK_HERE std::cout << "Here " << __LINE__ << std::endl;

using namespace dealii;
template <int dim, typename ValueType = double, typename IndexType = int,
          typename MixedValueType = double>
class BenchDealiiLaplace : public BenchBase<ValueType, IndexType> {
public:
    BenchDealiiLaplace();
    void run();
    void run(MPI_Comm mpi_communicator);

private:
    void setup_system();
    void assemble_system();
    void solve();
    void solve(MPI_Comm mpi_communicator);
    void refine_grid();
    void output_results(const unsigned int cycle) const;

    Triangulation<dim> triangulation;
    FE_Q<dim> fe;
    DoFHandler<dim> dof_handler;
    AffineConstraints<double> constraints;
    SparseMatrix<double> system_matrix;
    SparsityPattern sparsity_pattern;
    Vector<double> solution;
    Vector<double> system_rhs;
};

template <int dim>
double coefficient(const Point<dim> &p)
{
    if (p.square() < 0.5 * 0.5)
        return 20;
    else
        return 1;
}

template <int dim, typename ValueType, typename IndexType,
          typename MixedValueType>
BenchDealiiLaplace<dim, ValueType, IndexType,
                   MixedValueType>::BenchDealiiLaplace()
    : fe(2), dof_handler(triangulation)
{}

template <int dim, typename ValueType, typename IndexType,
          typename MixedValueType>
void BenchDealiiLaplace<dim, ValueType, IndexType,
                        MixedValueType>::setup_system()
{
    dof_handler.distribute_dofs(fe);
    solution.reinit(dof_handler.n_dofs());
    system_rhs.reinit(dof_handler.n_dofs());
    constraints.clear();
    DoFTools::make_hanging_node_constraints(dof_handler, constraints);
    VectorTools::interpolate_boundary_values(
        dof_handler, 0, Functions::ZeroFunction<dim>(), constraints);
    constraints.close();
    DynamicSparsityPattern dsp(dof_handler.n_dofs());
    DoFTools::make_sparsity_pattern(dof_handler, dsp, constraints,
                                    /*keep_constrained_dofs = */ false);
    sparsity_pattern.copy_from(dsp);
    system_matrix.reinit(sparsity_pattern);
}

template <int dim, typename ValueType, typename IndexType,
          typename MixedValueType>
void BenchDealiiLaplace<dim, ValueType, IndexType,
                        MixedValueType>::assemble_system()
{
    const 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_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);
    for (const auto &cell : dof_handler.active_cell_iterators()) {
        cell_matrix = 0;
        cell_rhs = 0;
        fe_values.reinit(cell);
        for (unsigned int q_index = 0; q_index < n_q_points; ++q_index) {
            const double current_coefficient =
                coefficient<dim>(fe_values.quadrature_point(q_index));
            for (unsigned int i = 0; i < dofs_per_cell; ++i) {
                for (unsigned int j = 0; j < dofs_per_cell; ++j)
                    cell_matrix(i, j) +=
                        (current_coefficient *               // a(x_q)
                         fe_values.shape_grad(i, q_index) *  // grad phi_i(x_q)
                         fe_values.shape_grad(j, q_index) *  // grad phi_j(x_q)
                         fe_values.JxW(q_index));            // dx
                cell_rhs(i) +=
                    (1.0 *                                // f(x)
                     fe_values.shape_value(i, q_index) *  // phi_i(x_q)
                     fe_values.JxW(q_index));             // dx
            }
        }
        cell->get_dof_indices(local_dof_indices);
        constraints.distribute_local_to_global(cell_matrix, cell_rhs,
                                               local_dof_indices, system_matrix,
                                               system_rhs);
    }
}

template <int dim, typename ValueType, typename IndexType,
          typename MixedValueType>
void BenchDealiiLaplace<dim, ValueType, IndexType, MixedValueType>::solve()
{
    SolverControl solver_control(1000, 1e-12);
    SolverCG<> solver(solver_control);
    PreconditionSSOR<> preconditioner;
    preconditioner.initialize(system_matrix, 1.2);
    auto start_time = std::chrono::steady_clock::now();
    solver.solve(system_matrix, solution, system_rhs, preconditioner);
    auto elapsed_time = std::chrono::duration<double>(
        std::chrono::steady_clock::now() - start_time);
    std::cout << "Time for solve only: " << elapsed_time.count() << std::endl;
    constraints.distribute(solution);
}

template <int dim, typename ValueType, typename IndexType,
          typename MixedValueType>
void BenchDealiiLaplace<dim, ValueType, IndexType, MixedValueType>::solve(
    MPI_Comm mpi_communicator)
{
    schwz::Metadata<ValueType, IndexType> metadata;
    schwz::Settings settings(FLAGS_executor);

    // Set solver metadata from command line args.
    metadata.mpi_communicator = mpi_communicator;
    MPI_Comm_rank(metadata.mpi_communicator, &metadata.my_rank);
    MPI_Comm_size(metadata.mpi_communicator, &metadata.comm_size);
    metadata.tolerance = FLAGS_set_tol;
    metadata.max_iters = FLAGS_num_iters;
    metadata.num_subdomains = metadata.comm_size;
    metadata.num_threads = FLAGS_num_threads;
    metadata.oned_laplacian_size = FLAGS_set_1d_laplacian_size;

    // Generic settings
    settings.write_debug_out = FLAGS_enable_debug_write;
    settings.write_perm_data = FLAGS_write_perm_data;
    settings.write_iters_and_residuals = FLAGS_write_iters_and_residuals;
    settings.print_matrices = FLAGS_print_matrices;
    settings.shifted_iter = FLAGS_shifted_iter;

    // Set solver settings from command line args.
    // Comm settings
    settings.comm_settings.enable_onesided = FLAGS_enable_onesided;
    if (FLAGS_remote_comm_type == "put") {
        settings.comm_settings.enable_put = true;
        settings.comm_settings.enable_get = false;
    } else if (FLAGS_remote_comm_type == "get") {
        settings.comm_settings.enable_put = false;
        settings.comm_settings.enable_get = true;
    }
    settings.comm_settings.enable_one_by_one = FLAGS_enable_one_by_one;
    settings.comm_settings.stage_through_host = FLAGS_stage_through_host;
    settings.comm_settings.enable_overlap = FLAGS_enable_comm_overlap;
    if (FLAGS_flush_type == "flush-all") {
        settings.comm_settings.enable_flush_all = true;
    } else if (FLAGS_flush_type == "flush-local") {
        settings.comm_settings.enable_flush_all = false;
        settings.comm_settings.enable_flush_local = true;
    }
    if (FLAGS_lock_type == "lock-all") {
        settings.comm_settings.enable_lock_all = true;
    } else if (FLAGS_lock_type == "lock-local") {
        settings.comm_settings.enable_lock_all = false;
        settings.comm_settings.enable_lock_local = true;
    }

    // Convergence settings
    settings.convergence_settings.put_all_local_residual_norms =
        FLAGS_enable_put_all_local_residual_norms;
    settings.convergence_settings.enable_global_check_iter_offset =
        FLAGS_enable_global_check_iter_offset;
    settings.convergence_settings.enable_global_check =
        FLAGS_enable_global_check;
    if (FLAGS_global_convergence_type == "centralized-tree") {
        settings.convergence_settings.enable_global_simple_tree = true;
    } else if (FLAGS_global_convergence_type == "decentralized") {
        settings.convergence_settings.enable_decentralized_leader_election =
            true;
        settings.convergence_settings.enable_accumulate =
            FLAGS_enable_decentralized_accumulate;
    }

    // General solver settings
    metadata.local_solver_tolerance = FLAGS_local_tol;
    metadata.local_precond = FLAGS_local_precond;
    metadata.local_max_iters = FLAGS_local_max_iters;
    metadata.updated_max_iters = FLAGS_updated_max_iters;
    settings.non_symmetric_matrix = FLAGS_non_symmetric_matrix;
    settings.restart_iter = FLAGS_restart_iter;
    settings.enable_logging = FLAGS_enable_logging;
    metadata.precond_max_block_size = FLAGS_precond_max_block_size;
    settings.matrix_filename = FLAGS_matrix_filename;
    settings.explicit_laplacian = FLAGS_explicit_laplacian;
    settings.enable_random_rhs = FLAGS_enable_random_rhs;
    settings.use_mixed_precision = FLAGS_use_mixed_precision;
    settings.overlap = FLAGS_overlap;
    settings.naturally_ordered_factor = FLAGS_factor_ordering_natural;
    settings.reorder = FLAGS_local_reordering;
    settings.factorization = FLAGS_local_factorization;
    if (FLAGS_partition == "metis") {
        settings.partition =
            schwz::Settings::partition_settings::partition_metis;
        settings.metis_objtype = FLAGS_metis_objtype;
    } else if (FLAGS_partition == "regular") {
        settings.partition =
            schwz::Settings::partition_settings::partition_regular;
    } else if (FLAGS_partition == "regular2d") {
        settings.partition =
            schwz::Settings::partition_settings::partition_regular2d;
    }
    if (FLAGS_local_solver == "iterative-ginkgo") {
        settings.local_solver =
            schwz::Settings::local_solver_settings::iterative_solver_ginkgo;
    } else if (FLAGS_local_solver == "direct-cholmod") {
        settings.local_solver =
            schwz::Settings::local_solver_settings::direct_solver_cholmod;
    } else if (FLAGS_local_solver == "direct-umfpack") {
        settings.local_solver =
            schwz::Settings::local_solver_settings::direct_solver_umfpack;
    } else if (FLAGS_local_solver == "direct-ginkgo") {
        settings.local_solver =
            schwz::Settings::local_solver_settings::direct_solver_ginkgo;
    }
    settings.debug_print = FLAGS_debug;
    MixedValueType dummy = 0.0;
    int gsize = 0;
    if (metadata.my_rank == 0) {
        metadata.global_size = system_matrix.m();
        std::cout << " Running on the " << FLAGS_executor << " executor on "
                  << metadata.num_subdomains << " ranks with "
                  << FLAGS_num_threads << " threads" << std::endl;
        std::cout << " MixedValueType: " << typeid(dummy).name() << std::endl;
        std::cout << " Problem Size: " << metadata.global_size
                  << " Number of non-zeros: "
                  << system_matrix.n_nonzero_elements() << std::endl;
        gsize = metadata.global_size;
    }
    MPI_Bcast(&gsize, 1, MPI_INT, 0, MPI_COMM_WORLD);
    metadata.global_size = gsize;
    if (FLAGS_print_config) {
        if (metadata.my_rank == 0) {
            this->print_config();
        }
    }
    using vec_vtype = gko::matrix::Dense<ValueType>;
    std::shared_ptr<vec_vtype> solution_vector;
    schwz::SolverRAS<ValueType, IndexType, MixedValueType> solver(settings,
                                                                  metadata);
    solver.initialize(system_matrix, system_rhs);
    auto start_time = std::chrono::steady_clock::now();
    solver.run(solution_vector);
    auto elapsed_time = std::chrono::duration<double>(
        std::chrono::steady_clock::now() - start_time);
    if (metadata.my_rank == 0) {
        std::cout << "Time for solve only: " << elapsed_time.count()
                  << std::endl;
    }
    if (FLAGS_timings_file != "null") {
        std::string rank_string = std::to_string(metadata.my_rank);
        if (metadata.my_rank < 10) {
            rank_string = "0" + std::to_string(metadata.my_rank);
        }
        std::string filename = FLAGS_timings_file + "_" + rank_string + ".csv";
        this->write_timings(metadata.time_struct, filename,
                            settings.comm_settings.enable_onesided);
    }
    if (FLAGS_write_comm_data) {
        std::string rank_string = std::to_string(metadata.my_rank);
        if (metadata.my_rank < 10) {
            rank_string = "0" + std::to_string(metadata.my_rank);
        }
        std::string filename_send = "num_send_" + rank_string + ".csv";
        std::string filename_recv = "num_recv_" + rank_string + ".csv";
        this->write_comm_data(metadata.num_subdomains, metadata.my_rank,
                              metadata.comm_data_struct, filename_send,
                              filename_recv);
    }

    if (metadata.my_rank == 0) {
        std::copy(solution_vector->get_values(),
                  solution_vector->get_values() + metadata.global_size,
                  solution.begin());
        constraints.distribute(solution);
    }
}

template <int dim, typename ValueType, typename IndexType,
          typename MixedValueType>
void BenchDealiiLaplace<dim, ValueType, IndexType,
                        MixedValueType>::refine_grid()
{
    Vector<float> estimated_error_per_cell(triangulation.n_active_cells());
    KellyErrorEstimator<dim>::estimate(
        dof_handler, QGauss<dim - 1>(fe.degree + 1),
        std::map<types::boundary_id, const Function<dim> *>(), solution,
        estimated_error_per_cell);
    GridRefinement::refine_and_coarsen_fixed_number(
        triangulation, estimated_error_per_cell, 0.3, 0.03);
    triangulation.execute_coarsening_and_refinement();
}

template <int dim, typename ValueType, typename IndexType,
          typename MixedValueType>
void BenchDealiiLaplace<dim, ValueType, IndexType, MixedValueType>::
    output_results(const unsigned int cycle) const
{
    {
        GridOut grid_out;
        std::ofstream output("grid-" + std::to_string(cycle) + ".gnuplot");
        GridOutFlags::Gnuplot gnuplot_flags(false, 5);
        grid_out.set_flags(gnuplot_flags);
        MappingQGeneric<dim> mapping(3);
        grid_out.write_gnuplot(triangulation, output, &mapping);
    }
    {
        DataOut<dim> data_out;
        data_out.attach_dof_handler(dof_handler);
        data_out.add_data_vector(solution, "solution");
        data_out.build_patches();
        std::ofstream output("solution-" + std::to_string(cycle) + ".vtu");
        data_out.write_vtu(output);
    }
}

template <int dim, typename ValueType, typename IndexType,
          typename MixedValueType>
void BenchDealiiLaplace<dim, ValueType, IndexType, MixedValueType>::run()
{
    int num_cycles = FLAGS_num_refine_cycles;

    for (unsigned int cycle = 0; cycle < num_cycles; ++cycle) {
        std::cout << "Cycle " << cycle << ':' << std::endl;

        if (cycle == 0) {
            GridGenerator::hyper_cube(triangulation);
            triangulation.refine_global(FLAGS_init_refine_level);
        } else
            refine_grid();
        std::cout << "   Number of active cells:       "
                  << triangulation.n_active_cells() << std::endl;
        setup_system();
        std::cout << "   Number of degrees of freedom: " << dof_handler.n_dofs()
                  << std::endl;
        assemble_system();
        this->solve();
        if (FLAGS_vis_sol) {
            output_results(cycle);
        }
    }
}

template <int dim, typename ValueType, typename IndexType,
          typename MixedValueType>
void BenchDealiiLaplace<dim, ValueType, IndexType, MixedValueType>::run(
    MPI_Comm mpi_communicator)
{
    int num_cycles = FLAGS_num_refine_cycles;
    int mpi_size, mpi_rank;
    MPI_Comm_size(mpi_communicator, &mpi_size);
    MPI_Comm_rank(mpi_communicator, &mpi_rank);

    for (unsigned int cycle = 0; cycle < num_cycles; ++cycle) {
        if (mpi_rank == 0) {
            std::cout << "Cycle " << cycle << ':' << std::endl;

            if (cycle == 0) {
                GridGenerator::hyper_cube(triangulation);
                triangulation.refine_global(FLAGS_init_refine_level);
            } else
                refine_grid();
            std::cout << "   Number of active cells:       "
                      << triangulation.n_active_cells() << std::endl;
            setup_system();
            std::cout << "   Number of degrees of freedom: "
                      << dof_handler.n_dofs() << std::endl;
            assemble_system();
        }
        this->solve(MPI_COMM_WORLD);
        if (mpi_rank == 0) {
            if (FLAGS_vis_sol) {
                output_results(cycle);
            }
        }
    }
}

int main(int argc, char **argv)
{
    try {
        initialize_argument_parsing(&argc, &argv);
        BenchDealiiLaplace<3, double, int, float> laplace_problem;
        if (FLAGS_num_threads > 1) {
            int req_thread_support = MPI_THREAD_MULTIPLE;
            int prov_thread_support = MPI_THREAD_MULTIPLE;

            MPI_Init_thread(&argc, &argv, req_thread_support,
                            &prov_thread_support);
            if (prov_thread_support != req_thread_support) {
                std::cout << "Required thread support is " << req_thread_support
                          << " but provided thread support is only "
                          << prov_thread_support << std::endl;
            }
        } else {
            MPI_Init(&argc, &argv);
        }

        int rank = 0;
        MPI_Comm_rank(MPI_COMM_WORLD, &rank);
        if (FLAGS_dealii_orig) {
            if (rank == 0) {
                auto start_time = std::chrono::steady_clock::now();
                laplace_problem.run();
                auto elapsed_time = std::chrono::duration<double>(
                    std::chrono::steady_clock::now() - start_time);
                std::cout << "Total Time for setup+solve: "
                          << elapsed_time.count() << std::endl;
            }
        } else {
            auto start_time = std::chrono::steady_clock::now();
            laplace_problem.run(MPI_COMM_WORLD);
            auto elapsed_time = std::chrono::duration<double>(
                std::chrono::steady_clock::now() - start_time);
            if (rank == 0) {
                std::cout << "Total Time for setup+solve: "
                          << elapsed_time.count() << std::endl;
            }
        }
        MPI_Finalize();
    } catch (std::exception &exc) {
        std::cerr << std::endl
                  << std::endl
                  << "----------------------------------------------------"
                  << std::endl;
        std::cerr << "Exception on processing: " << std::endl
                  << exc.what() << std::endl
                  << "Aborting!" << std::endl
                  << "----------------------------------------------------"
                  << std::endl;
        return 1;
    } catch (...) {
        std::cerr << std::endl
                  << std::endl
                  << "----------------------------------------------------"
                  << std::endl;
        std::cerr << "Unknown exception!" << std::endl
                  << "Aborting!" << std::endl
                  << "----------------------------------------------------"
                  << std::endl;
        return 1;
    }
    return 0;
}


//  Copyright (c) 2017 Zahra Khatami
//
// Train your data, then record them in an output file stated in "retrieving_weights_multi_classes_into_text_file"

#include <limits>
#include <math.h>
#include <iostream>
#include <stdlib.h>
#include <time.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Eigen>
#include <Eigen/LU>

using namespace Eigen;

#define MAX_FLOAT (std::numeric_limits<float>::max())
#define MIN_FLOAT (std::numeric_limits<float>::min())

class multinomial_logistic_regression_model {

	std::size_t number_of_experiments;
	std::size_t number_of_features;
	std::size_t number_of_classes;
	float threshold; 							//the convergence for estimating the final weights
	float eta;
	MatrixXf experimental_results; 				//the experimental values of the features of the training data
	MatrixXf experimental_results_trans;		//transpose of experimental_results
	MatrixXf execution_times;					//execution time for each class for each experiment
	MatrixXf weightsm; 							//weights of our learning network : F * K
	MatrixXf weightsm_trans;					//transpose of weights : K * F
	MatrixXf new_weightsm;						//updated weights after each step : F * K
	int* real_output;							//real output of each experimental results
	MatrixXf targets_multi_class;				//binary real output of each experimental results : N * K
	MatrixXf outputsm;							//outputs of the training data : N * K
	MatrixXf gradient;							//gradient of E : F * K
	MatrixXf sum_w_experimental_results;		//used for computing output : N
	int* predicted_output_multi_class;			//predicted class of each experimental results
	float* averages;							//parameters for normalization
	float* averages_2;							//parameters for normalization
	float* var;									//parameters for normalization

	void normalizing_weights_multi_class();
	void convert_target_to_binary(int* target_src, MatrixXf& targets_dst);
	int eye_kj(std::size_t k, std::size_t j);
	void computing_all_output();
	void computing_all_gradient();
	void learning_weights_multi_classes();
	void new_values_for_weightsm();
	float computing_new_least_squared_err_multi_class();
	void updating_values_of_weights_multi_class();
	void printing_weights_multi_class();
	void estimating_output_multiclass();
	void printing_computed_values(std::size_t row, std::size_t col, MatrixXf& mat);

public:
	multinomial_logistic_regression_model(std::size_t number_of_expr, std::size_t number_of_ftrs, std::size_t number_of_cls,
											float th, float** expr_results, int* target_expr, float** exec_time) {
		number_of_experiments = number_of_expr;
		number_of_features = number_of_ftrs;
		number_of_classes = number_of_cls;
		threshold = th;
		eta = 0.01;

		sum_w_experimental_results = MatrixXf::Random(number_of_experiments, 1);
		weightsm = MatrixXf::Random(number_of_features, number_of_classes);
		weightsm_trans = MatrixXf::Random(number_of_classes, number_of_features);
		new_weightsm = MatrixXf::Random(number_of_features, number_of_classes);
		gradient = MatrixXf::Random(number_of_features, number_of_classes);
		experimental_results = MatrixXf(number_of_experiments, number_of_features);
		experimental_results_trans = MatrixXf::Random(number_of_features, number_of_experiments);
		execution_times = MatrixXf::Random(number_of_experiments, number_of_classes);
		targets_multi_class = MatrixXf::Random(number_of_experiments, number_of_classes);
		outputsm = MatrixXf::Random(number_of_experiments, number_of_classes);
		predicted_output_multi_class = new int[number_of_experiments];
		real_output = new int[number_of_experiments];

		//variance and average of each features value for normalization
		averages = new float[number_of_features];
		averages_2 = new float[number_of_features];
		var = new float[number_of_features];

		//initializing weights
		for(std::size_t f = 0; f < number_of_features; f++) {
			for(std::size_t k = 0; k < number_of_classes; k++) {
				weightsm(f, k) = 0.1;
			}
		}

		for(std::size_t i = 0; i < number_of_experiments; i++) {

			//initializing experimental_results
			for(std::size_t f = 0; f < number_of_features; f++) {
				experimental_results(i, f) = expr_results[i][f];
			}

			//initializing execution_times
			for(std::size_t c = 0; c < number_of_classes; c++) {
				execution_times(i, c) = exec_time[i][c];
			}

			//initializing real outputs
			real_output[i] = target_expr[i];
		}

		//initializing targets_multi_class
		convert_target_to_binary(target_expr, targets_multi_class);
		outputsm = targets_multi_class;
	}

	void learning_multi_classes();
	void retrieving_weights_multi_classes_into_text_file();
	void printing_predicted_output_multi_class();
	void finalizing_step();
};

//it prints computed values : for testing
void multinomial_logistic_regression_model::printing_computed_values(std::size_t row, std::size_t col, MatrixXf& mat) {
	if(row != 0 && col != 0) {
		for(std::size_t r = 0; r < row; r++) {
			for(std::size_t c = 0; c < col; c++) {
				printf("%f, ", mat(r, c));
			}
			std::cout<<std::endl;
		}
	}
	else if(row == 0 && col != 0){
		for(std::size_t c = 0; c < col; c++) {
			printf("%f, ", mat(0, c));
		}
	}
	else {
		for(std::size_t r = 0; r < row; r++) {
			printf("%f, ", mat(r, 0));
		}
	}
	std::cout<<std::endl;
}

void multinomial_logistic_regression_model::convert_target_to_binary(int* target_src, MatrixXf& targets_multi_class) {
	for(std::size_t n = 0; n < number_of_experiments; n++) {
		for(std::size_t k = 0; k < number_of_classes; k++) {
			if(target_src[n] == k) {
				targets_multi_class(n, k) = 1.0;
			}
			else {
				targets_multi_class(n, k) = 0.0;
			}
		}
	}
}

//Ikj
int multinomial_logistic_regression_model::eye_kj(std::size_t k, std::size_t j) {
	if(k == j) {
		return 1;
	}
	return 0;
}

//computing outputs
void multinomial_logistic_regression_model::computing_all_output() {
	weightsm_trans = weightsm.transpose();
	//w^T * Q
	MatrixXf W_TQ_trans = MatrixXf::Random(number_of_classes, number_of_experiments);
	W_TQ_trans = weightsm_trans * experimental_results_trans;
	MatrixXf W_TQ = MatrixXf::Random(number_of_experiments, number_of_classes);
	W_TQ = W_TQ_trans.transpose();

	//sigma(exp(wQ))
	for(std::size_t n = 0; n < number_of_experiments; n++) {
		sum_w_experimental_results(n, 0) = 0.0;
		for(std::size_t k = 0; k < number_of_classes; k++) {
			sum_w_experimental_results(n, 0) += exp(W_TQ(n, k));
		}
	}

	//ynk
	for(std::size_t n = 0; n < number_of_experiments; n++) {
		for(std::size_t k = 0; k < number_of_classes; k++) {
			outputsm(n, k) = float(exp(W_TQ(n, k))/sum_w_experimental_results(n, 0));
		}
	}
}

//computing gradient
void multinomial_logistic_regression_model::computing_all_gradient(){
	//initializing
	gradient *= 0.0;
	for(std::size_t k = 0; k < number_of_classes; k++) {
		for(std::size_t n = 0; n < number_of_experiments; n++) {
			gradient.col(k) += (outputsm(n, k) - targets_multi_class(n, k)) * experimental_results_trans.col(n);
		}
	}
}

void multinomial_logistic_regression_model::new_values_for_weightsm() {
	new_weightsm = weightsm	- eta * gradient;
}

//computing leas squares err
float multinomial_logistic_regression_model::computing_new_least_squared_err_multi_class() {
	std::size_t num_err = 0;
	for(std::size_t n = 0; n < number_of_experiments; n++) {
		if(abs(execution_times(n, predicted_output_multi_class[n]) - execution_times(n, real_output[n])) > 0.2) {
			num_err++;
		}
	}
	float prec = float(num_err) / number_of_experiments;
	return prec;
}

//updating weights
void multinomial_logistic_regression_model::updating_values_of_weights_multi_class() {
	weightsm = new_weightsm;
}

void multinomial_logistic_regression_model::printing_weights_multi_class() {
	printing_computed_values(number_of_features, number_of_classes, weightsm);
	std::cout<<"\n --------------------\n";
}

//estimating class of each experimental results based on the computed weights
void multinomial_logistic_regression_model::estimating_output_multiclass() {
	for(std::size_t n = 0; n < number_of_experiments; n++) {
		float prob = MIN_FLOAT;
		for(std::size_t k = 0; k < number_of_classes; k++) {
			if(prob < outputsm(n, k)) {
				predicted_output_multi_class[n] = k;
				prob = outputsm(n, k);
			}
		}
	}
}

//updating weights till error meets the defined threshold
void multinomial_logistic_regression_model::learning_weights_multi_classes() {
	float least_squared_err = MAX_FLOAT;
	std::size_t itr = 1;

	//for some test, only for statring updating weights:
	computing_all_gradient();
	new_values_for_weightsm();
	updating_values_of_weights_multi_class();
	computing_all_output();
	estimating_output_multiclass();

	while(threshold < least_squared_err) {
		computing_all_gradient();
		new_values_for_weightsm();
		updating_values_of_weights_multi_class();
		computing_all_output();
		estimating_output_multiclass();
		least_squared_err = computing_new_least_squared_err_multi_class();
		std::cout<<"("<<itr<<")"<<"Least_squared_err =\t" << least_squared_err<<std::endl;
		printing_weights_multi_class();
		itr++;
	}
	std::cout<<"("<<itr<<") => "<<"Least_squared_err =\t" << least_squared_err<<std::endl;
}

void multinomial_logistic_regression_model::normalizing_weights_multi_class() {
	//initializing
	for(std::size_t i = 0; i < number_of_features; i++) {
		averages[i] = 0;
		averages_2[i] = 0;
		var[i] = 0;
	}

	//computing average and variance values for each feature
	for(std::size_t i = 0; i < number_of_experiments; i++) {
		for(std::size_t j = 0; j < number_of_features; j++) {
			averages[j] += experimental_results(i, j);
			averages_2[j] += (pow(experimental_results(i, j), 2.0));
		}
	}
	for(std::size_t i = 0; i < number_of_features; i++) {
		averages[i] = float(averages[i]/number_of_experiments);
		averages_2[i] = float(averages_2[i]/number_of_experiments);
		var[i] = sqrt(averages_2[i] - pow(averages[i], 2.0));
	}

	for(std::size_t n = 0; n < number_of_experiments; n++) {
		for(std::size_t f = 0; f < number_of_features; f++) {
			experimental_results(n, f) = float((experimental_results(n, f) - averages[f])/var[f]);
		}
	}
}

void multinomial_logistic_regression_model::learning_multi_classes() {
	normalizing_weights_multi_class();
	experimental_results_trans = experimental_results.transpose();
	learning_weights_multi_classes();
}

//retrieving information into the external file, which is going to be used at runtime
void multinomial_logistic_regression_model::retrieving_weights_multi_classes_into_text_file() {

  // for learning model on chunk_size training data
	std::ofstream outputFile("inputs/data_chunk.dat");

  // for learning model on prefetching distance training data:
  //std::ofstream outputFile("inputs/data_prefetch.dat");

	//normalization parameters (variance and average) in the first line
	for(std::size_t p = 0; p < number_of_features - 1; p++) {
		outputFile << var[p] << " " << averages[p] << " ";
	}
	outputFile << var[number_of_features - 1] << " " << averages[number_of_features - 1] << std::endl;

	for(std::size_t c = 0; c < number_of_classes; c++) {
		for(std::size_t f = 0; f < number_of_features - 1; f++) {
			outputFile << weightsm(f, c) << " ";
		}
		outputFile << weightsm(number_of_features - 1, c);
		if(c != number_of_classes - 1) {
			outputFile << std::endl;
		}
	}
}

void multinomial_logistic_regression_model::printing_predicted_output_multi_class(){
	std::size_t num_err = 0;
	for(std::size_t n = 0; n < number_of_experiments; n++) {
		if(abs(execution_times(n, predicted_output_multi_class[n]) - execution_times(n, real_output[n])) > 0.2){
			num_err++;
			std::cout << "\n [" << n << "] =\t" << predicted_output_multi_class[n] << "\t" << real_output[n];
		}
	}
	std::cout<<"\n number of error predicted is\t"<<num_err<<" out of "<<number_of_experiments<<std::endl;
}

void multinomial_logistic_regression_model::finalizing_step() {

	//releasing memory
	delete[] averages;
	delete[] averages_2;
	delete[] var;
	delete[] predicted_output_multi_class;
	delete[] real_output;
}


/**
 * @file numerical_functions.cpp
 */

#include "numerical_functions.h"
#include <Eigen/Dense>
#include <algorithm>
#include <numeric>
#include <limits>

using namespace himan;
using namespace numerical_functions;
using namespace Eigen;

template <typename T>
matrix<T> numerical_functions::Filter2D(const matrix<T>& A, const matrix<T>& B, bool useCuda)
{
#ifdef HAVE_CUDA
	if (useCuda)
	{
		return Filter2DGPU(A, B);
	}
#endif
	return Reduce2D<T>(A, B,
	                   [](T& val1, T& val2, const T& a, const T& b) {
		                   if (IsValid(a * b))
		                   {
			                   val1 += a * b;
			                   val2 += b;
		                   }
	                   },
	                   [](const T& val1, const T& val2) { return val2 == T(0) ? MissingValue<T>() : val1 / val2; },
	                   T(0), T(0));
}

template matrix<double> numerical_functions::Filter2D(const matrix<double>&, const matrix<double>& B, bool);
template matrix<float> numerical_functions::Filter2D(const matrix<float>&, const matrix<float>& B, bool);

template <typename T>
matrix<T> numerical_functions::Max2D(const matrix<T>& A, const matrix<T>& B, bool useCuda)
{
#ifdef HAVE_CUDA
	if (useCuda)
	{
		return Max2DGPU(A, B);
	}
#endif
	return Reduce2D<T>(A, B,
	                   [](T& val1, T& val2, const T& a, const T& b) {
		                   if (IsValid(a * b))
			                   val1 = !(a * b <= val1) ? a : val1;
	                   },
	                   [](const T& val1, const T& val2) { return val1; }, MissingValue<T>(), T(0));
}

template matrix<double> numerical_functions::Max2D(const matrix<double>&, const matrix<double>& B, bool);
template matrix<float> numerical_functions::Max2D(const matrix<float>&, const matrix<float>& B, bool);

template <typename T>
matrix<T> numerical_functions::Min2D(const matrix<T>& A, const matrix<T>& B, bool useCuda)
{
#ifdef HAVE_CUDA
	if (useCuda)
	{
		return Min2DGPU(A, B);
	}
#endif
	return Reduce2D<T>(A, B,
	                   [](T& val1, T& val2, const T& a, const T& b) {
		                   if (IsValid(a * b))
			                   val1 = !(a * b >= val1) ? a : val1;
	                   },
	                   [](const T& val1, const T& val2) { return val1; }, MissingValue<T>(), T(0));
}

template matrix<double> numerical_functions::Min2D(const matrix<double>&, const matrix<double>& B, bool);
template matrix<float> numerical_functions::Min2D(const matrix<float>&, const matrix<float>& B, bool);

template <typename T>
matrix<size_t> numerical_functions::IndexMax2D(const matrix<T>& A, const matrix<T>& B)
{
	return FindIndex2D(A, B,
			[](T& current_max, const T& a, const T& b) {
				return (a > current_max) & IsValid(b);
			}, std::numeric_limits<T>::lowest());
}

template matrix<size_t> numerical_functions::IndexMax2D(const matrix<float>& A, const matrix<float>& B);
template matrix<size_t> numerical_functions::IndexMax2D(const matrix<double>& A, const matrix<double>& B);

template <typename T>
std::pair<std::vector<T>, std::vector<T>> numerical_functions::LegGauss(size_t N, bool computeWeights)
{
	// Set up Eigenvalue problem
	//-------------------------------------------------------------------------------------------------------
	Matrix<T, Dynamic, Dynamic> J(N, N);

	Diagonal<Matrix<T, Dynamic, Dynamic>, 0> Jdiag0(J);
	Diagonal<Matrix<T, Dynamic, Dynamic>, 1> Jdiag1(J);

	for (size_t n = 0; n < N; ++n)
	{
		Jdiag0[n] = 0.0;
	}

	for (size_t n = 0; n < N - 1; ++n)
	{
		Jdiag1[n] = static_cast<T>(T(n + 1) * 1.0 / std::sqrt(2 * (n) + 1) * 1.0 / std::sqrt(2 * (n + 1) + 1));
	}
	//-------------------------------------------------------------------------------------------------------

	// Solve Eigenvalue problem
	//-------------------------------------------------------------------------------------------------------
	SelfAdjointEigenSolver<Matrix<T, Dynamic, Dynamic>> es(N);
	//-------------------------------------------------------------------------------------------------------

	es.computeFromTridiagonal(Jdiag0, Jdiag1, computeWeights ? ComputeEigenvectors : EigenvaluesOnly);

	// Extract Quadrature points and weights from eigenvalues and eigenvectors
	//-------------------------------------------------------------------------------------------------------
	std::vector<T> r(N);
	std::vector<T> w;

	Map<Matrix<T, Dynamic, Dynamic>> R(r.data(), N, 1);
	R = es.eigenvalues().real();

	if (computeWeights)
	{
		w.resize(N);
		Map<Array<T, Dynamic, Dynamic>> W(w.data(), 1, N);

		W = es.eigenvectors().real().row(0);
		W = W * W * 2;
	}
	//-------------------------------------------------------------------------------------------------------

	return std::make_pair(r, w);
}
template std::pair<std::vector<float>, std::vector<float>> numerical_functions::LegGauss(size_t, bool);
template std::pair<std::vector<double>, std::vector<double>> numerical_functions::LegGauss(size_t, bool);

template <typename T>
T numerical_functions::Mean(const std::vector<T>& data)
{
	if (data.size() == 0)
	{
		return himan::MissingValue<T>();
	}

	return std::accumulate(data.begin(), data.end(), 0.0f) / static_cast<T>(data.size());
}

template double numerical_functions::Mean(const std::vector<double>&);
template float numerical_functions::Mean(const std::vector<float>&);

template <typename T>
T numerical_functions::Variance(const std::vector<T>& data)
{
	if (data.size() == 0)
	{
		return himan::MissingValue<T>();
	}

	const auto mean = Mean(data);

	T sum = 0.0f;

	for (const auto& x : data)
	{
		const auto t = x - mean;
		sum += t * t;
	}

	return sum / static_cast<T>(data.size());
}

template double numerical_functions::Variance(const std::vector<double>&);
template float numerical_functions::Variance(const std::vector<float>&);


/**
 * UnscentedKalmanFilterX.hpp
 * @author koide
 * 16/02/01
 **/
#ifndef KKL_UNSCENTED_KALMAN_FILTER_X_HPP
#define KKL_UNSCENTED_KALMAN_FILTER_X_HPP

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

namespace kkl {
  namespace alg {

/**
 * @brief Unscented Kalman Filter class
 * @param T        scaler type
 * @param System   system class to be estimated
 */
template<typename T, class System>
class UnscentedKalmanFilterX {
  typedef Eigen::Matrix<T, Eigen::Dynamic, 1> VectorXt;
  typedef Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> MatrixXt;
public:
  /**
   * @brief constructor
   * @param system               system to be estimated
   * @param state_dim            state vector dimension
   * @param input_dim            input vector dimension
   * @param measurement_dim      measurement vector dimension
   * @param process_noise        process noise covariance (state_dim x state_dim)
   * @param measurement_noise    measurement noise covariance (measurement_dim x measuremend_dim)
   * @param mean                 initial mean
   * @param cov                  initial covariance
   */
  UnscentedKalmanFilterX(const System& system, int state_dim, int input_dim, int measurement_dim, const MatrixXt& process_noise, const MatrixXt& measurement_noise, const VectorXt& mean, const MatrixXt& cov)
    : state_dim(state_dim),
    input_dim(input_dim),
    measurement_dim(measurement_dim),
    N(state_dim),
    M(input_dim),
    K(measurement_dim),
    S(2 * state_dim + 1),
    mean(mean),
    cov(cov),
    system(system),
    process_noise(process_noise),
    measurement_noise(measurement_noise),
    lambda(1),
    normal_dist(0.0, 1.0)
  {
    weights.resize(S, 1);
    sigma_points.resize(S, N);
    ext_weights.resize(2 * (N + K) + 1, 1);
    ext_sigma_points.resize(2 * (N + K) + 1, N + K);
    expected_measurements.resize(2 * (N + K) + 1, K);

    // initialize weights for unscented filter
    weights[0] = lambda / (N + lambda);
    for (int i = 1; i < 2 * N + 1; i++) {
      weights[i] = 1 / (2 * (N + lambda));
    }

    // weights for extended state space which includes error variances
    ext_weights[0] = lambda / (N + K + lambda);
    for (int i = 1; i < 2 * (N + K) + 1; i++) {
      ext_weights[i] = 1 / (2 * (N + K + lambda));
    }
  }

  /**
   * @brief predict
   * @param control  input vector
   */
  void predict(const VectorXt& control) {
    // calculate sigma points
    ensurePositiveFinite(cov);
    computeSigmaPoints(mean, cov, sigma_points);
    for (int i = 0; i < S; i++) {
      sigma_points.row(i) = system.f(sigma_points.row(i), control);
    }

    const auto& R = process_noise;

    // unscented transform
    VectorXt mean_pred(mean.size());
    MatrixXt cov_pred(cov.rows(), cov.cols());

    mean_pred.setZero();
    cov_pred.setZero();
    for (int i = 0; i < S; i++) {
      mean_pred += weights[i] * sigma_points.row(i);
    }
    for (int i = 0; i < S; i++) {
      VectorXt diff = sigma_points.row(i).transpose() - mean;
      cov_pred += weights[i] * diff * diff.transpose();
    }
    cov_pred += R;

    mean = mean_pred;
    cov = cov_pred;
  }

  /**
   * @brief correct
   * @param measurement  measurement vector
   */
  void correct(const VectorXt& measurement) {
    // create extended state space which includes error variances
    VectorXt ext_mean_pred = VectorXt::Zero(N + K, 1);
    MatrixXt ext_cov_pred = MatrixXt::Zero(N + K, N + K);
    ext_mean_pred.topLeftCorner(N, 1) = VectorXt(mean);
    ext_cov_pred.topLeftCorner(N, N) = MatrixXt(cov);
    ext_cov_pred.bottomRightCorner(K, K) = measurement_noise;

    ensurePositiveFinite(ext_cov_pred);
    computeSigmaPoints(ext_mean_pred, ext_cov_pred, ext_sigma_points);

    // unscented transform
    expected_measurements.setZero();
    for (int i = 0; i < ext_sigma_points.rows(); i++) {
      expected_measurements.row(i) = system.h(ext_sigma_points.row(i).transpose().topLeftCorner(N, 1));
      expected_measurements.row(i) += VectorXt(ext_sigma_points.row(i).transpose().bottomRightCorner(K, 1));
    }

    VectorXt expected_measurement_mean = VectorXt::Zero(K);
    for (int i = 0; i < ext_sigma_points.rows(); i++) {
      expected_measurement_mean += ext_weights[i] * expected_measurements.row(i);
    }
    MatrixXt expected_measurement_cov = MatrixXt::Zero(K, K);
    for (int i = 0; i < ext_sigma_points.rows(); i++) {
      VectorXt diff = expected_measurements.row(i).transpose() - expected_measurement_mean;
      expected_measurement_cov += ext_weights[i] * diff * diff.transpose();
    }

    // calculated transformed covariance
    MatrixXt sigma = MatrixXt::Zero(N + K, K);
    for (int i = 0; i < ext_sigma_points.rows(); i++) {
      auto diffA = (ext_sigma_points.row(i).transpose() - ext_mean_pred);
      auto diffB = (expected_measurements.row(i).transpose() - expected_measurement_mean);
      sigma += ext_weights[i] * (diffA * diffB.transpose());
    }

    kalman_gain = sigma * expected_measurement_cov.inverse();
    const auto& K = kalman_gain;

    VectorXt ext_mean = ext_mean_pred + K * (measurement - expected_measurement_mean);
    MatrixXt ext_cov = ext_cov_pred - K * expected_measurement_cov * K.transpose();

    mean = ext_mean.topLeftCorner(N, 1);
    cov = ext_cov.topLeftCorner(N, N);
  }

  /*			getter			*/
  const VectorXt& getMean() const { return mean; }
  const MatrixXt& getCov() const { return cov; }
  const MatrixXt& getSigmaPoints() const { return sigma_points; }

  System& getSystem() { return system; }
  const System& getSystem() const { return system; }
  const MatrixXt& getProcessNoiseCov() const { return process_noise; }
  const MatrixXt& getMeasurementNoiseCov() const { return measurement_noise; }

  const MatrixXt& getKalmanGain() const { return kalman_gain; }

  /*			setter			*/
  UnscentedKalmanFilterX& setMean(const VectorXt& m) { mean = m;			return *this; }
  UnscentedKalmanFilterX& setCov(const MatrixXt& s) { cov = s;			return *this; }

  UnscentedKalmanFilterX& setProcessNoiseCov(const MatrixXt& p) { process_noise = p;			return *this; }
  UnscentedKalmanFilterX& setMeasurementNoiseCov(const MatrixXt& m) { measurement_noise = m;	return *this; }

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
  const int state_dim;
  const int input_dim;
  const int measurement_dim;

  const int N;
  const int M;
  const int K;
  const int S;

public:
  VectorXt mean;
  MatrixXt cov;

  System system;
  MatrixXt process_noise;		//
  MatrixXt measurement_noise;	//

  T lambda;
  VectorXt weights;

  MatrixXt sigma_points;

  VectorXt ext_weights;
  MatrixXt ext_sigma_points;
  MatrixXt expected_measurements;

private:
  /**
   * @brief compute sigma points
   * @param mean          mean
   * @param cov           covariance
   * @param sigma_points  calculated sigma points
   */
  void computeSigmaPoints(const VectorXt& mean, const MatrixXt& cov, MatrixXt& sigma_points) {
    const int n = mean.size();
    assert(cov.rows() == n && cov.cols() == n);

    Eigen::LLT<MatrixXt> llt;
    llt.compute((n + lambda) * cov);
    MatrixXt l = llt.matrixL();

    sigma_points.row(0) = mean;
    for (int i = 0; i < n; i++) {
      sigma_points.row(1 + i * 2) = mean + l.col(i);
      sigma_points.row(1 + i * 2 + 1) = mean - l.col(i);
    }
  }

  /**
   * @brief make covariance matrix positive finite
   * @param cov  covariance matrix
   */
  void ensurePositiveFinite(MatrixXt& cov) {
    return;
    const double eps = 1e-9;

    Eigen::EigenSolver<MatrixXt> solver(cov);
    MatrixXt D = solver.pseudoEigenvalueMatrix();
    MatrixXt V = solver.pseudoEigenvectors();
    for (int i = 0; i < D.rows(); i++) {
      if (D(i, i) < eps) {
        D(i, i) = eps;
      }
    }

    cov = V * D * V.inverse();
  }

public:
  MatrixXt kalman_gain;

  std::mt19937 mt;
  std::normal_distribution<T> normal_dist;
};

  }
}

#endif


#include <stdlib.h>
#include <assert.h>
#include <math.h>
#include <complex.h>
#include <time.h>
#include <NTL/ZZ.h>
#include <NTL/ZZX.h>
#include <NTL/mat_ZZ.h>
#include <gmp.h>

#include "Sampling.h"
#include "params.h"

using namespace std;
using namespace NTL;

//==============================================================================
// Takes in input a random value and samples from distribution D_{\sigma_2}^+,
// Samples an element in Z+ with probability proportionnal to 2^{-x^2}
//==============================================================================
unsigned int Sample0(unsigned long alea)
{
    if((alea&1UL)==0UL)
    {
        return 0;
    }
    unsigned int i;
    unsigned int k = 1;
    unsigned long mask=0;
    unsigned long aux;
    for(i=1; i<1000;)
    {
        aux = (alea&mask);
        alea = (alea>>k);
        if(aux)
        {
            return Sample0(alea);
        }
        else
        {
            if((alea&1UL)==0UL)
            {
                return i;
            }
        }
        i++;
        k += 2;
        mask = (mask<<2)|6UL;
    }
    cout << "ERROR" << endl;
    return 999999;
}

//==============================================================================
// Samples from distribution D_{k\sigma_2}^+, ie
// Samples an element in Z+ with probability proportionnal to 2^{-(x/k)^2}
//==============================================================================
unsigned int Sample1(const unsigned int k)
{
    unsigned int x, y, z;
    unsigned long alea = rand();

    x = Sample0(alea);
    y = rand()%k;
    z = k*x + y;
    RR_t w = y*( (z<<1) - y );
    RR_t borne =  LDRMX / exp( w*log_2/(k*k) );
    alea = rand();
    if(alea>borne)
    {
        return Sample1(k);
    }
    else
    {
        return z;
    }
    cout << "ERROR" << endl;
    return 999999;
}

//==============================================================================
// Samples from distribution D_{k\sigma_2}, ie
// Samples an element in Z with probability proportionnal to 2^{-(x/k)^2}
//==============================================================================
signed int Sample2(const unsigned int k)
{
    signed int signe;
    signed int x;
    unsigned long alea = rand();
    while(1)
    {
        x = Sample1(k);
        if( (x!=0) || ((alea&1)==1) )
        {
            alea >>= 1;
            signe = 1 - 2*(alea&1);
            x *= signe;
            return x;
        }
        alea >>= 1;
    }
}

//==============================================================================
// Samples from distribution D_{sigma}, ie
// Samples an element in Z with probability proportionnal to e^{-x^2/2*(sigma^2)}
//==============================================================================
signed int Sample3(const RR_t sigma128)
{
    signed int x;
    double alea, borne;

    const RR_t sigma = sigma128;
    const unsigned long k = ( (unsigned long) ceil( (RR_t) sigma/sigma_1 ) );
    while(1)

    {
        x = Sample2(k);
        alea = ((RR_t)rand()) / LDRMX;
        borne = exp( -x*x*( 1/(2*sigma*sigma) - 1/(2*k*k*sigma_1*sigma_1) )   );
        assert(borne<=1);
        if(alea<borne)
        {
            return x;
        }
    }
}

//==============================================================================
// Samples from distribution D_{c,sigma}, ie
// Samples an element in Z with probability proportionnal to e^{-(c-x)^2/2*(sigma^2)}
//==============================================================================
signed int Sample4(RR_t c, RR_t sigma)
{
    RR_t alea, borne;
    signed int x;
    unsigned int coin;

    const signed int intc = ( (signed int) floor(c) );
    const RR_t fracc = c-intc;
    coin = rand();
    const RR_t denom = 1/(2*sigma*sigma);

    while(1)
    {
        x = Sample3(sigma);
        x += (coin&1);
        if(abs(x)>8){cout << x << endl;}
        coin >>= 1;
        borne = exp(-(x-fracc)*(x-fracc)*denom)/ ( exp(-x*x*denom) + exp(-(x-1)*(x-1)*denom) );

        assert(borne<1);
        alea = ( (RR_t)rand() ) / LDRMX;
        if(alea<borne)
        {
            return (x+intc);
        }
    }
}


/* =========================================================================
   Copyright (c) 2010-2014, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/** \example sparse.cpp
*
*   This tutorial demonstrates the use of sparse matrices.
*   The primary operation for sparse matrices in ViennaCL is the sparse matrix-vector product.
*
*   We start with including the respective headers:
**/

// system headers
#include <iostream>

// ublas headers
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/operation_sparse.hpp>
#include <boost/numeric/ublas/io.hpp>

// Must be set if you want to use ViennaCL algorithms on ublas objects
#define VIENNACL_WITH_UBLAS 1

// ViennaCL includes
#include "viennacl/scalar.hpp"
#include "viennacl/vector.hpp"
#include "viennacl/compressed_matrix.hpp"
#include "viennacl/linalg/prod.hpp"

// Additional helper functions for this tutorial:
#include "Random.hpp"
#include "vector-io.hpp"

// Shortcut for writing 'ublas::' instead of 'boost::numeric::ublas::'
using namespace boost::numeric;

/**
*   We setup a sparse matrix in uBLAS and populate it with values.
*   Then, the respective ViennaCL sparse matrix is created and initialized with data from the uBLAS matrix.
*   After a direct manipulation of the ViennaCL matrix, matrix-vector products are computed with both matrices.
**/
int main()
{
  typedef float       ScalarType;

  std::size_t size = 5;

  /**
  * Set up some ublas objects
  **/
  ublas::vector<ScalarType> rhs = ublas::scalar_vector<ScalarType>(size, ScalarType(size));
  ublas::compressed_matrix<ScalarType> ublas_matrix(size, size);

  ublas_matrix(0,0) =  2.0f; ublas_matrix(0,1) = -1.0f;
  ublas_matrix(1,0) = -1.0f; ublas_matrix(1,1) =  2.0f; ublas_matrix(1,2) = -1.0f;
  ublas_matrix(2,1) = -1.0f; ublas_matrix(2,2) =  2.0f; ublas_matrix(2,3) = -1.0f;
  ublas_matrix(3,2) = -1.0f; ublas_matrix(3,3) =  2.0f; ublas_matrix(3,4) = -1.0f;
  ublas_matrix(4,3) = -1.0f; ublas_matrix(4,4) =  2.0f;

  std::cout << "ublas matrix: " << ublas_matrix << std::endl;

  /**
  * Set up some ViennaCL objects and initialize with data from uBLAS objects
  **/
  viennacl::vector<ScalarType> vcl_rhs(size);
  viennacl::compressed_matrix<ScalarType> vcl_compressed_matrix(size, size);

  viennacl::copy(rhs, vcl_rhs);
  viennacl::copy(ublas_matrix, vcl_compressed_matrix);

  // just get the data directly from the GPU and print it:
  ublas::compressed_matrix<ScalarType> temp(size, size);
  viennacl::copy(vcl_compressed_matrix, temp);
  std::cout << "ViennaCL: " << temp << std::endl;

  // now modify GPU data directly:
  std::cout << "Modifying vcl_compressed_matrix a bit: " << std::endl;
  vcl_compressed_matrix(0, 0) =  3.0f;
  vcl_compressed_matrix(2, 3) = -3.0f;
  vcl_compressed_matrix(4, 2) = -3.0f;  //this is a new nonzero entry
  vcl_compressed_matrix(4, 3) = -3.0f;

  // and print it again:
  viennacl::copy(vcl_compressed_matrix, temp);
  std::cout << "ViennaCL matrix copied to uBLAS matrix: " << temp << std::endl;

  /**
  *  Compute matrix-vector products and output the results (should match):
  **/
  std::cout << "ublas: " << ublas::prod(temp, rhs) << std::endl;
  std::cout << "ViennaCL: " << viennacl::linalg::prod(vcl_compressed_matrix, vcl_rhs) << std::endl;

  /**
  *  That's it. Print a success message and exit.
  **/
  std::cout << "!!!! TUTORIAL COMPLETED SUCCESSFULLY !!!!" << std::endl;

  return EXIT_SUCCESS;
}


#pragma once

#include <array>
#include <unordered_map>
#include <cmath>
#include <Eigen/Dense>

#include "rfobject.hpp"
#include "mmap.hpp"

namespace lamon
{
    template<typename _ArrayTy>
    auto gelu(_ArrayTy&& x) -> decltype(0.f * x * (0.f + (0.f * (x + 0.f * x * x * x)).tanh()))
    {
        const float c1 = std::sqrt(2 / 3.14159265), c2 = 0.044715;
        return 0.5f * x * (1.f + (c1 * (x + c2 * x * x * x)).tanh());
    }

    template<typename _ArrayTy>
    auto sigmoid(_ArrayTy&& x) -> decltype(((-x).exp() + 1).inverse())
    {
        return ((-x).exp() + 1).inverse();
    }

    template<typename _ArrayTy>
    auto logsoftmax(_ArrayTy&& x) -> decltype(x - 0.f)
    {
        auto max = x.maxCoeff();
        max += std::log((x - max).exp().sum());
        return x - max;
    }

    struct EmbeddingLookup
    {
        ConstMatrix<float> embs;
        EmbeddingLookup(const utils::Object& o)
            : embs{ o.template to_matrix<float>() }
        {
        }

        EmbeddingLookup(const ConstMatrix<float>& o)
            : embs{ o }
        {
        }

        size_t get_embedding_size() const { return embs.rows(); }
        size_t get_vocab_size() const { return embs.cols(); }

        auto operator[](size_t idx) const -> decltype(embs.col(idx))
        {
            return embs.col(idx);
        }
    };

    struct LayerNorm
    {
        ConstVector<float> beta;
        ConstVector<float> gamma;

        LayerNorm(const utils::ObjectCollection& objs, const std::string& keys)
            : beta{ objs[keys + "/beta:0"].template to_vector<float>() },
            gamma{ objs[keys + "/gamma:0"].template to_vector<float>() }
        {
        }

        template<typename _DestTy>
        void apply_inplace(_DestTy&& dest) const
        {
            int cnt = dest.rows();
            for (int i = 0; i < dest.cols(); ++i)
            {
                auto col = dest.col(i);
                float avg = col.sum() / cnt;
                float std = std::sqrt(((col.array() * col.array()).sum() / cnt - avg * avg) + 1e-12f);
                col = (col.array() - avg) / std * gamma.array() + beta.array();
            }
        }

        template<typename _DestTy, typename _SrcTy>
        void apply(_DestTy&& dest, _SrcTy&& src) const
        {
            int cnt = src.rows();
            for (int i = 0; i < src.cols(); ++i)
            {
                auto col = src.col(i);
                float avg = col.sum() / cnt;
                float std = std::sqrt(((col.array() * col.array()).sum() / cnt - avg * avg) + 1e-12f);
                dest.col(i) = (col.array() - avg) / std * gamma.array() + beta.array();
            }
        }
    };

    struct Dense
    {
        ConstMatrix<float> kernel;
        ConstVector<float> bias;

        Dense(const utils::ObjectCollection& objs, const std::string& keys)
            : kernel{ objs[keys + "/kernel:0"].template to_matrix<float>() },
            bias{ objs[keys + "/bias:0"].template to_vector<float>() }
        {
        }

        size_t input_size() const
        {
            return kernel.rows();
        }

        size_t output_size() const
        {
            return kernel.cols();
        }

        template<typename _EigenTy>
        auto operator()(_EigenTy&& x) const
            -> decltype((kernel.transpose()* x).colwise() + bias)
        {
            return (kernel.transpose() * x).colwise() + bias;
        }

        template<typename _Ty1, typename _Ty2>
        auto apply_concated(_Ty1&& x, _Ty2&& y) const
            -> decltype((kernel.topRows(x.rows()).transpose()* x
                + kernel.bottomRows(y.rows()).transpose() * y).colwise() + bias)
        {
            return (kernel.topRows(x.rows()).transpose() * x
                + kernel.bottomRows(y.rows()).transpose() * y).colwise() + bias;
        }

        template<typename _EigenTy>
        auto partial(_EigenTy&& x, size_t begin, size_t size) const
            -> decltype((kernel.middleCols(begin, size).transpose()* x).colwise() + bias.segment(begin, size))
        {
            return (kernel.middleCols(begin, size).transpose() * x).colwise() + bias.segment(begin, size);
        }

        template<typename _EigenTy>
        float partial(_EigenTy&& x, size_t idx) const
        {
            return (kernel.col(idx).transpose() * x)(0) + bias(idx);
        }
    };

    struct LSTMCell : public Dense
    {
        LSTMCell(const utils::ObjectCollection& objs, const std::string& keys)
            : Dense{ objs, keys }
        {
        }

        size_t h_size() const
        {
            return bias.size() / 4;
        }

        size_t input_size() const
        {
            return kernel.rows() - h_size();
        }

        template<typename _EigenTy1, typename _EigenTy2, typename _EigenTy3>
        _EigenTy3& operator()(_EigenTy1&& input, _EigenTy2& c_state, _EigenTy3& h_state) const
        {
            Eigen::VectorXf gates = ((kernel.topRows(input.rows()).transpose() * input) + kernel.bottomRows(h_state.rows()).transpose() * h_state).colwise() + bias;
            const size_t gate_size = h_size();
            auto input_gate = gates.middleRows(0, gate_size).array();
            auto new_input = gates.middleRows(gate_size, gate_size).array();
            auto forget_gate = gates.middleRows(gate_size * 2, gate_size).array();
            auto output_gate = gates.middleRows(gate_size * 3, gate_size).array();

            c_state = (c_state.array() * sigmoid(forget_gate + 1)) + (sigmoid(input_gate) * new_input.tanh());
            h_state = c_state.array().tanh() * sigmoid(output_gate);
            return h_state;
        }

        template<typename _EigenTy1, typename _EigenTy2>
        _EigenTy1& operator()(_EigenTy1& input_h, _EigenTy2& c_state) const
        {
            Eigen::VectorXf gates = (kernel.transpose() * input_h).colwise() + bias;
            const size_t gate_size = h_size();
            auto input_gate = gates.middleRows(0, gate_size).array();
            auto new_input = gates.middleRows(gate_size, gate_size).array();
            auto forget_gate = gates.middleRows(gate_size * 2, gate_size).array();
            auto output_gate = gates.middleRows(gate_size * 3, gate_size).array();

            c_state = (c_state.array() * sigmoid(forget_gate + 1)) + (sigmoid(input_gate) * new_input.tanh());
            input_h.bottomRows(gate_size) = c_state.array().tanh() * sigmoid(output_gate);
            return input_h;
        }
    };
}


#include <HElib/FHE.h>
#include <HElib/FHEContext.h>
#include <HElib/EncryptedArray.h>
#include <HElib/PAlgebra.h>
#include <NTL/ZZX.h>

#include "SMP/HElib.hpp"
long inner_product(NTL::zz_pX const& a,
                   NTL::zz_pX const& b)
{
    NTL::zz_p ip(0);
    long deg = NTL::deg(a);
    for (long i = 0; i <= deg; i++) {
        ip += (NTL::coeff(a, i) * NTL::coeff(b, deg - i));
    }
    return ip._zz_p__rep;
}

long inner_product(NTL::ZZX const& a,
                   NTL::ZZX const& b,
				   long p)
{
    long deg = NTL::deg(a);
	NTL::ZZ ip(0);
	NTL::ZZ P(p);
    for (long i = 0; i <= deg; i++) {
        ip += NTL::MulMod(NTL::coeff(a, i), NTL::coeff(b, deg - i), P);
    }
    return NTL::to_long(ip) % p;
}

void random_poly(NTL::ZZX &poly, long coeff, long degree)
{
	poly.SetLength(degree);
	for (long i = 0; i < degree; i++)
		NTL::SetCoeff(poly, i, NTL::RandomBnd(coeff) + 1);
}

void test_with_normal_encode() {
    long m = 4096<<2;
    long p = 769;
    FHEcontext context(m, p, 1);
    buildModChain(context, 4);
    FHESecKey sk(context);
    sk.GenSecKey(64);

    const auto &factors = context.alMod.getFactorsOverZZ();
	//auto ea = context.ea;
    EncryptedArray *ea = new EncryptedArray(context, factors[0]);
    const long l = ea->size();
    const long d = ea->getDegree();
	for (long _i = 0; _i < 100; _i++) {
		NTL::ZZX B;
		random_poly(B, p, d);

		std::vector<NTL::ZZX> Vec_A(l);
		std::vector<long> inner_products(l);
		for (long i = 0; i < l; i++) {
			random_poly(Vec_A[i], p, d);
			inner_products[i] = inner_product(Vec_A[i], B, p);
		}

		Ctxt ctx(sk);
		ea->skEncrypt(ctx, sk, std::vector<NTL::ZZX>(l, B));
		NTL::ZZX encoded_A;

		ea->encode(encoded_A, Vec_A);
		ctx.multByConstant(encoded_A);
		std::vector<NTL::ZZX> results;
		ea->decrypt(ctx, sk, results);
		std::vector<long> computed;
		for (auto &s : results) {
			computed.push_back(NTL::to_long(NTL::coeff(s, d - 1)));
		}
		for (long i = 0; i < l; i++) {
			if (computed.at(i) != inner_products.at(i)) {
				std::cout << NTL::deg(Vec_A[i]) << "->";
				std::cout << "computed " << computed.at(i) << " but want " <<
					inner_products.at(i) << std::endl;
			}
		}
	}
}

void test_it() {
    long m = 4096 << 2;
    long p = 769;
    FHEcontext context(m, p, 1);
    buildModChain(context, 4);
    FHESecKey sk(context);
    sk.GenSecKey(64);

	auto ea = context.ea;
    const long l = ea->size();
    const long d = ea->getDegree();

	for (long _i = 0; _i <100; _i++) {
		NTL::zz_p::init(p);

		NTL::zz_pX b;
		NTL::ZZX B;
		NTL::random(b, d);
		NTL::conv(B, b);

		std::vector<NTL::zz_pX> vec_A(l);
		std::vector<long> inner_products(l);
		for (long i = 0; i < l; i++) {
			NTL::random(vec_A[i], d);
			NTL::SetCoeff(vec_A[i], d - 1, 1); // make sure full degree
			inner_products[i] = inner_product(vec_A[i], b);
		}

		NTL::ZZX encoded_A;
		rawEncode(encoded_A, vec_A, context);

		Ctxt ctx(sk);
		sk.Encrypt(ctx, B);
		ctx.multByConstant(encoded_A);

		NTL::ZZX decrypted;
		sk.Decrypt(decrypted, ctx);

		std::vector<NTL::zz_pX> results;
        rawDecode(results, decrypted, context);

		std::vector<long> computed;
		for (auto &s : results) {
			computed.push_back(NTL::coeff(s, d - 1)._zz_p__rep);
		}

		for (long i = 0; i < l; i++) {
			if (computed.at(i) != inner_products.at(i)) {
				std::cout << NTL::deg(vec_A[i]) << "->";
				std::cout << "computed " << computed.at(i) << " but want " <<
					inner_products.at(i) << std::endl;
			}
		}
	}
}

void test_double_pack() {
    long m = 4096;
    long p = 113;
    FHEcontext context(m, p, 1);
    buildModChain(context, 4);
    FHESecKey sk(context);
    sk.GenSecKey(64);

	auto ea = context.ea;
    const long l = ea->size();
    const long d = ea->getDegree();
	const auto &factors = context.alMod.getFactorsOverZZ();

	for (long _i = 0; _i < 1; _i++) {
		NTL::zz_p::init(p);

		std::vector<NTL::zz_pX> vec_A(l);
		std::vector<NTL::zz_pX> vec_B(l);
		for (long i = 0; i < l; i++) {
			NTL::random(vec_B[i], d);
			NTL::SetCoeff(vec_B[i], d - 1, 1); // make sure full degree
        }

		std::vector<long> inner_products(l);
		for (long i = 0; i < l; i++) {
			NTL::random(vec_A[i], d);
			NTL::SetCoeff(vec_A[i], d - 1, 1); // make sure full degree
			long sm = 0;
			for (const auto& vec_b : vec_B) {
				sm += inner_product(vec_A[i], vec_B[i]);
				sm %= p;
			}
			std::cout << sm << " ";
			inner_products[l] = sm;
		}
		std::cout << "\n" << std::endl;

		NTL::ZZX Vec_B;
		Vec_B.SetLength(l * d);
		auto itr = Vec_B.rep.begin();
		for (long i = 0; i < l; i++) {
			long factor = NTL::to_long(factors[i][0]);
			factor = NTL::PowerMod(factor, i, p); // alpha_i^(i+1) mod p
			auto inv = NTL::InvMod(factor, p);
			if (i & 1)
				inv *= -1;
			std::cout << factor << " " << inv << ",";
			for (const auto &b : vec_B[i].rep) {
				NTL::conv(*itr++, inv * b);
			}
		}
		std::cout << '\n';

		NTL::ZZX encoded_A;
		rawEncode(encoded_A, vec_A, context);

		Ctxt ctx(sk);
		sk.Encrypt(ctx, Vec_B);
		ctx.multByConstant(encoded_A);

		NTL::ZZX decrypted;
		sk.Decrypt(decrypted, ctx);

		std::vector<NTL::zz_pX> results;
        rawDecode(results, decrypted, context);

		std::vector<long> computed;
		for (auto &s : results) {
			computed.push_back(NTL::coeff(s, d - 1)._zz_p__rep);
			std::cout << NTL::coeff(s, d - 1) << " ";
		}
		std::cout << "\n";
		// for (long i = 0; i < l; i++) {
		// 	if (computed.at(i) != inner_products.at(i)) {
		// 		std::cout << NTL::deg(vec_A[i]) << "->";
		// 		std::cout << "computed " << computed.at(i) << " but want " <<
		// 			inner_products.at(i) << std::endl;
		// 	}
		// }
	}
}
int main() {
	test_double_pack();
	// auto st = std::clock();
    // test_it();
	// std::cout << (std::clock() - st) / (double)CLOCKS_PER_SEC << std::endl;
    //
	// st = std::clock();
	// test_with_normal_encode();
	// std::cout << (std::clock() - st) / (double)CLOCKS_PER_SEC << std::endl;
	return 0;
}


/********************************************************
  Stanford Driving Software
  Copyright (c) 2011 Stanford University
  All rights reserved.

  Redistribution and use in source and binary forms, with
  or without modification, are permitted provided that the
  following conditions are met:

* Redistributions of source code must retain the above
  copyright notice, this list of conditions and the
  following disclaimer.
* Redistributions in binary form must reproduce the above
  copyright notice, this list of conditions and the
  following disclaimer in the documentation and/or other
  materials provided with the distribution.
* The names of the contributors may not be used to endorse
  or promote products derived from this software
  without specific prior written permission.

  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
  CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
  WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
  WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
  PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
  INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
  PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
  OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
  DAMAGE.
 ********************************************************/

#include <iostream>
#include <Eigen/Dense>
#include <aw_geometry_3d.h>

using namespace Eigen;

namespace vlr {

//== CONSTANTS ========================================================
#define INSIDE  false
#define OUTSIDE true

// two vectors, a and b, starting from c
float dot(const Vector3f& a, const Vector3f& b) {
  return a.dot(b);
}

// two vectors, a and b
Vector3f cross(const Vector3f& a, const Vector3f& b) {
  return a.cross(b);
}

// two vectors, a and b, starting from c
Vector3f cross(const Vector3f& a, const Vector3f& b, const Vector3f& c) {
  float a0 = a(0) - c(0);
  float a1 = a(1) - c(1);
  float a2 = a(2) - c(2);
  float b0 = b(0) - c(0);
  float b1 = b(1) - c(1);
  float b2 = b(2) - c(2);
  return Vector3f(a1 * b2 - a2 * b1, a2 * b0 - a0 * b2, a0 * b1 - a1 * b0);
}

float dist2(const Vector3f& a, const Vector3f& b) {
  float x = a(0) - b(0);
  float y = a(1) - b(1);
  float z = a(2) - b(2);
  return x * x + y * y + z * z;
}

float dist(const Vector3f& a, const Vector3f& b) {
  return sqrtf(dist2(a, b));
}

// linear interpolation
Vector3f lerp(float t, const Vector3f& a, const Vector3f& b) {
  float v[3];
  float u = 1.0 - t;
  v[0] = u * a(0) + t * b(0);
  v[1] = u * a(1) + t * b(1);
  v[2] = u * a(2) + t * b(2);
  return Vector3f(v[0], v[1], v[2]);
}

// is the ball centered at b with radius r
// fully within the box centered at bc, with radius br?
bool ball_within_bounds(const Vector3f& b, float r, const Vector3f& bc, float br) {
  r -= br;
  if ((b(0) - bc(0) <= r) || (bc(0) - b(0) <= r) || (b(1) - bc(1) <= r) || (bc(1) - b(1) <= r) || (b(2) - bc(2) <= r) || (bc(2) - b(2) <= r)) return false;
  return true;
}

// is the ball centered at b with radius r
// fully within the box centered from min to max?
bool ball_within_bounds(const Vector3f& b, float r, const Vector3f& min, const Vector3f& max) {
  if ((b(0) - min(0) <= r) || (max(0) - b(0) <= r) || (b(1) - min(1) <= r) || (max(1) - b(1) <= r) || (b(2) - min(2) <= r) || (max(2) - b(2) <= r)) return false;
  return true;
}

// does the ball centered at b, with radius r,
// intersect the box centered at bc, with radius br?
bool bounds_overlap_ball(const Vector3f& b, float r, const Vector3f& bc, float br) {
  float sum = 0.0, tmp;
  if ((tmp = bc(0) - br - b(0)) > 0.0) {
    if (tmp > r) return false;
    sum += tmp * tmp;
  }
  else if ((tmp = b(0) - (bc(0) + br)) > 0.0) {
    if (tmp > r) return false;
    sum += tmp * tmp;
  }
  if ((tmp = bc(1) - br - b(1)) > 0.0) {
    if (tmp > r) return false;
    sum += tmp * tmp;
  }
  else if ((tmp = b(1) - (bc(1) + br)) > 0.0) {
    if (tmp > r) return false;
    sum += tmp * tmp;
  }
  if ((tmp = bc(2) - br - b(2)) > 0.0) {
    if (tmp > r) return false;
    sum += tmp * tmp;
  }
  else if ((tmp = b(2) - (bc(2) + br)) > 0.0) {
    if (tmp > r) return false;
    sum += tmp * tmp;
  }
  return (sum < r * r);
}

bool bounds_overlap_ball(const Vector3f& b, float r, const Vector3f& min, const Vector3f& max) {
  float sum = 0.0, tmp;
  if (b(0) < min(0)) {
    tmp = min(0) - b(0);
    if (tmp > r) return false;
    sum += tmp * tmp;
  }
  else if (b(0) > max(0)) {
    tmp = b(0) - max(0);
    if (tmp > r) return false;
    sum += tmp * tmp;
  }
  if (b(1) < min(1)) {
    tmp = min(1) - b(1);
    sum += tmp * tmp;
  }
  else if (b(1) > max(1)) {
    tmp = b(1) - max(1);
    sum += tmp * tmp;
  }
  r *= r;
  if (sum > r) return false;
  if (b(2) < min(2)) {
    tmp = min(2) - b(2);
    sum += tmp * tmp;
  }
  else if (b(2) > max(2)) {
    tmp = b(2) - max(2);
    sum += tmp * tmp;
  }
  return (sum < r);
}

// calculate barycentric coordinates of the point p
// (already on the triangle plane) with normal vector n
// and two edge vectors v1 and v2,
// starting from a common vertex t0
void bary_fast(const Vector3f& p, const Vector3f& n, const Vector3f& t0, const Vector3f& v1, const Vector3f& v2, float &b1, float &b2, float &b3) {
  // see bary above
  int i = 0;
  if (n(1) > n(0)) i = 1;
  if (n(2) > n(i)) {
    // ignore z
    float d = 1.0 / (v1(0) * v2(1) - v1(1) * v2(0));
    float x0 = (p(0) - t0(0));
    float x1 = (p(1) - t0(1));
    b1 = (x0 * v2(1) - x1 * v2(0)) * d;
    b2 = (v1(0) * x1 - v1(1) * x0) * d;
  }
  else if (i == 0) {
    // ignore x
    float d = 1.0 / (v1(1) * v2(2) - v1(2) * v2(1));
    float x0 = (p(1) - t0(1));
    float x1 = (p(2) - t0(2));
    b1 = (x0 * v2(2) - x1 * v2(1)) * d;
    b2 = (v1(1) * x1 - v1(2) * x0) * d;
  }
  else {
    // ignore y
    float d = 1.0 / (v1(2) * v2(0) - v1(0) * v2(2));
    float x0 = (p(2) - t0(2));
    float x1 = (p(0) - t0(0));
    b1 = (x0 * v2(0) - x1 * v2(2)) * d;
    b2 = (v1(2) * x1 - v1(0) * x0) * d;
  }
  b3 = 1.0 - b1 - b2;
}

bool closer_on_lineseg(const Vector3f& x, Vector3f& cp, const Vector3f& a, const Vector3f& b, float &d2) {
  Vector3f ba(b(0) - a(0), b(1) - a(1), b(2) - a(2));
  Vector3f xa(x(0) - a(0), x(1) - a(1), x(2) - a(2));

  float xa_ba = dot(xa, ba);
  // if the dot product is negative, the point is closest to a
  if (xa_ba < 0.0) {
    float nd = dist2(x, a);
    if (nd < d2) {
      cp = a;
      d2 = nd;
      return true;
    }
    return false;
  }

  // if the dot product is greater than squared segment length,
  // the point is closest to b
  float fact = xa_ba / ba.squaredNorm();
  if (fact >= 1.0) {
    float nd = dist2(x, b);
    if (nd < d2) {
      cp = b;
      d2 = nd;
      return true;
    }
    return false;
  }

  // take the squared dist x-a, squared dot of x-a to unit b-a,
  // use Pythagoras' rule
  float nd = xa.squaredNorm() - xa_ba * fact;
  if (nd < d2) {
    d2 = nd;
    cp(0) = a(0) + fact * ba(0);
    cp(1) = a(1) + fact * ba(1);
    cp(2) = a(2) + fact * ba(2);
    return true;
  }
  return false;
}

void distance_point_line(const Vector3f& x, const Vector3f& a, const Vector3f& b, float &d2, Vector3f& cp) {
  Vector3f ba(b(0) - a(0), b(1) - a(1), b(2) - a(2));
  Vector3f xa(x(0) - a(0), x(1) - a(1), x(2) - a(2));

  float xa_ba = dot(xa, ba);

  // if the dot product is negative, the point is closest to a
  if (xa_ba < 0.0) {
    d2 = dist2(x, a);
    cp = a;
    return;
  }

  // if the dot product is greater than squared segment length,
  // the point is closest to b
  float fact = xa_ba / ba.squaredNorm();
  if (fact >= 1.0) {
    d2 = dist2(x, b);
    cp = b;
    return;
  }

  // take the squared dist x-a, squared dot of x-a to unit b-a,
  // use Pythagoras' rule
  d2 = xa.squaredNorm() - xa_ba * fact;
  cp(0) = a(0) + fact * ba(0);
  cp(1) = a(1) + fact * ba(1);
  cp(2) = a(2) + fact * ba(2);
  return;
}

void distance_point_tri(const Vector3f& x, const Vector3f& t1, const Vector3f& t2, const Vector3f& t3, float &d2, Vector3f& cp) {
  // calculate the normal and distance from the plane
  Vector3f v1(t2(0) - t1(0), t2(1) - t1(1), t2(2) - t1(2));
  Vector3f v2(t3(0) - t1(0), t3(1) - t1(1), t3(2) - t1(2));
  Vector3f n = cross(v1, v2);
  float n_inv_mag2 = 1.0 / n.squaredNorm();
  float tmp = (x(0) - t1(0)) * n(0) + (x(1) - t1(1)) * n(1) + (x(2) - t1(2)) * n(2);
  float distp2 = tmp * tmp * n_inv_mag2;

  // calculate the barycentric coordinates of the point
  // (projected onto tri plane) with respect to v123
  float b1, b2, b3;
  float f = tmp * n_inv_mag2;
  Vector3f pp(x(0) - f * n(0), x(1) - f * n(1), x(2) - f * n(2));
  bary_fast(pp, n, t1, v1, v2, b1, b2, b3);

  // all non-negative, the point is within the triangle
  if (b1 >= 0.0 && b2 >= 0.0 && b3 >= 0.0) {
    d2 = distp2;
    cp = pp;
    return;
  }

  // look at the signs of the barycentric coordinates
  // if there are two negative signs, the positive
  // one tells the vertex that's closest
  // if there's one negative sign, the opposite edge
  // (with endpoints) is closest

  if (b1 < 0.0) {
    if (b2 < 0.0) {
      d2 = dist2(x, t3);
      cp = t3;
    }
    else if (b3 < 0.0) {
      d2 = dist2(x, t2);
      cp = t2;
    }
    else {
      distance_point_line(x, t2, t3, d2, cp);
    }
  }
  else if (b2 < 0.0) {
    if (b3 < 0.0) {
      d2 = dist2(x, t1);
      cp = t1;
    }
    else {
      distance_point_line(x, t1, t3, d2, cp);
    }
  }
  else {
    distance_point_line(x, t1, t2, d2, cp);
  }
  return;
}

bool closer_on_tri(const Vector3f& x, Vector3f& cp, const Vector3f& t1, const Vector3f& t2, const Vector3f& t3, float &d2) {
  // calculate the normal and distance from the plane
  Vector3f v1(t2(0) - t1(0), t2(1) - t1(1), t2(2) - t1(2));
  Vector3f v2(t3(0) - t1(0), t3(1) - t1(1), t3(2) - t1(2));
  Vector3f n = cross(v1, v2);
  float n_inv_mag2 = 1.0 / n.squaredNorm();
  float tmp = (x(0) - t1(0)) * n(0) + (x(1) - t1(1)) * n(1) + (x(2) - t1(2)) * n(2);
  float distp2 = tmp * tmp * n_inv_mag2;
  if (distp2 >= d2) return false;

  // calculate the barycentric coordinates of the point
  // (projected onto tri plane) with respect to v123
  float b1, b2, b3;
  float f = tmp * n_inv_mag2;
  Vector3f pp(x(0) - f * n(0), x(1) - f * n(1), x(2) - f * n(2));
  bary_fast(pp, n, t1, v1, v2, b1, b2, b3);

  // all non-negative, the point is within the triangle
  if (b1 >= 0.0 && b2 >= 0.0 && b3 >= 0.0) {
    d2 = distp2;
    cp = pp;
    return true;
  }

  // look at the signs of the barycentric coordinates
  // if there are two negative signs, the positive
  // one tells the vertex that's closest
  // if there's one negative sign, the opposite edge
  // (with endpoints) is closest

  if (b1 < 0.0) {
    if (b2 < 0.0) {
      float nd = dist2(x, t3);
      if (nd < d2) {
        d2 = nd;
        cp = t3;
        return true;
      }
      else {
        return false;
      }
    }
    else if (b3 < 0.0) {
      float nd = dist2(x, t2);
      if (nd < d2) {
        d2 = nd;
        cp = t2;
        return true;
      }
      else {
        return false;
      }
    }
    else return closer_on_lineseg(x, cp, t2, t3, d2);
  }
  else if (b2 < 0.0) {
    if (b3 < 0.0) {
      float nd = dist2(x, t1);
      if (nd < d2) {
        d2 = nd;
        cp = t1;
        return true;
      }
      else {
        return false;
      }
    }
    else return closer_on_lineseg(x, cp, t1, t3, d2);
  }
  else return closer_on_lineseg(x, cp, t1, t2, d2);
}

// calculate the intersection of a line going through p
// to direction dir with a plane spanned by t1,t2,t3
// (modified from Graphics Gems, p.299)
bool line_plane_X(const Vector3f& p, const Vector3f& dir, const Vector3f& t1, const Vector3f& t2, const Vector3f& t3, Vector3f& x, float &dist) {
  // note: normal doesn't need to be unit vector
  Vector3f nrm = cross(t1, t2, t3);
  float tmp = dot(nrm, dir);
  if (tmp == 0.0) {
    std::cerr << "Cannot intersect plane with a parallel line" << std::endl;
    return false;
  }
  // d  = -dot(nrm,t1)
  // t  = - (d + dot(p,nrm))/dot(dir,nrm)
  // is = p + dir * t
  x = dir;
  dist = (dot(nrm, t1) - dot(nrm, p)) / tmp;
  x *= dist;
  x += p;
  if (dist < 0.0) dist = -dist;
  return true;
}

bool line_plane_X(const Vector3f& p, const Vector3f& dir, const Vector3f& nrm, float d, Vector3f& x, float &dist) {
  float tmp = dot(nrm, dir);
  if (tmp == 0.0) {
    std::cerr << "Cannot intersect plane with a parallel line" << std::endl;
    return false;
  }
  x = dir;
  dist = -(d + dot(nrm, p)) / tmp;
  x *= dist;
  x += p;
  if (dist < 0.0) dist = -dist;
  return true;
}

// calculate barycentric coordinates of the point p
// on triangle t1 t2 t3
void bary(const Vector3f& p, const Vector3f& t1, const Vector3f& t2, const Vector3f& t3, float &b1, float &b2, float &b3) {
  // figure out the plane onto which to project the vertices
  // by calculating a cross product and finding its largest dimension
  // then use Cramer's rule to calculate two of the
  // barycentric coordinates
  // e.g., if the z coordinate is ignored, and v1 = t1-t3, v2 = t2-t3
  // b1 = det(gx(0)g v2(0); x(1) v2(1)) / det(v1(0) v2(0); v1(1) v2(1))
  // b2 = det(gv1(0)g x(0); v1(1) x(1)) / det(v1(0) v2(0); v1(1) v2(1))
  float v10 = t1(0) - t3(0);
  float v11 = t1(1) - t3(1);
  float v12 = t1(2) - t3(2);
  float v20 = t2(0) - t3(0);
  float v21 = t2(1) - t3(1);
  float v22 = t2(2) - t3(2);
  float c[2];
  c[0] = fabs(v11 * v22 - v12 * v21);
  c[1] = fabs(v12 * v20 - v10 * v22);
  int i = 0;
  if (c[1] > c[0]) i = 1;
  if (fabs(v10 * v21 - v11 * v20) > c[i]) {
    // ignore z
    float d = 1.0f / (v10 * v21 - v11 * v20);
    float x0 = (p(0) - t3(0));
    float x1 = (p(1) - t3(1));
    b1 = (x0 * v21 - x1 * v20) * d;
    b2 = (v10 * x1 - v11 * x0) * d;
  }
  else if (i == 0) {
    // ignore x
    float d = 1.0f / (v11 * v22 - v12 * v21);
    float x0 = (p(1) - t3(1));
    float x1 = (p(2) - t3(2));
    b1 = (x0 * v22 - x1 * v21) * d;
    b2 = (v11 * x1 - v12 * x0) * d;
  }
  else {
    // ignore y
    float d = 1.0f / (v12 * v20 - v10 * v22);
    float x0 = (p(2) - t3(2));
    float x1 = (p(0) - t3(0));
    b1 = (x0 * v20 - x1 * v22) * d;
    b2 = (v12 * x1 - v10 * x0) * d;
  }
  b3 = 1.0f - b1 - b2;
}

// calculate barycentric coordinates for the intersection of
// a line starting from p, going to direction dir, and the plane
// of the triangle t1 t2 t3
bool bary(const Vector3f& p, const Vector3f& dir, const Vector3f& t1, const Vector3f& t2, const Vector3f& t3, float &b1, float &b2, float &b3) {
  Vector3f x;
  float d;
  if (!line_plane_X(p, dir, t1, t2, t3, x, d)) return false;
  bary(x, t1, t2, t3, b1, b2, b3);

  return true;
}

// calculate the intersection of a line starting from p,
// going to direction dir, and the triangle t1 t2 t3
bool line_tri_X(const Vector3f& p, const Vector3f& dir, const Vector3f& t1, const Vector3f& t2, const Vector3f& t3, Vector3f& x, float& d) {
  float b1, b2, b3;
  Vector3f x_temp;
  float d_temp;
  if (!line_plane_X(p, dir, t1, t2, t3, x_temp, d_temp)) return false;

  bary(x_temp, t1, t2, t3, b1, b2, b3);
  // all non-negative, the point is within the triangle
  if (b1 >= 0.0 && b2 >= 0.0 && b3 >= 0.0) {
    x = x_temp;
    d = d_temp;
    return true;
  }
  return false;
}

} // namespace vlr


/* chi_squared_test.hpp header file
 *
 * Copyright Steven Watanabe 2010
 * 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)
 *
 * $Id$
 *
 */

#ifndef BOOST_RANDOM_TEST_CHI_SQUARED_TEST_HPP_INCLUDED
#define BOOST_RANDOM_TEST_CHI_SQUARED_TEST_HPP_INCLUDED

#include <vector>

#include <boost/math/special_functions/pow.hpp>
#include <boost/math/distributions/chi_squared.hpp>

// This only works for discrete distributions with fixed
// upper and lower bounds.

template<class IntType>
struct chi_squared_collector {

    static const IntType cutoff = 5;

    chi_squared_collector()
      : chi_squared(0),
        variables(0),
        prev_actual(0),
        prev_expected(0),
        current_actual(0),
        current_expected(0)
    {}

    void operator()(IntType actual, double expected) {
        current_actual += actual;
        current_expected += expected;

        if(current_expected >= cutoff) {
            if(prev_expected != 0) {
                update(prev_actual, prev_expected);
            }
            prev_actual = current_actual;
            prev_expected = current_expected;

            current_actual = 0;
            current_expected = 0;
        }
    }

    void update(IntType actual, double expected) {
        chi_squared += boost::math::pow<2>(actual - expected) / expected;
        ++variables;
    }

    double cdf() {
        if(prev_expected != 0) {
            update(prev_actual + current_actual, prev_expected + current_expected);
            prev_actual = 0;
            prev_expected = 0;
            current_actual = 0;
            current_expected = 0;
        }
        if(variables <= 1) {
            return 0;
        } else {
            return boost::math::cdf(boost::math::chi_squared(variables - 1), chi_squared);
        }
    }

    double chi_squared;
    std::size_t variables;

    IntType prev_actual;
    double prev_expected;

    IntType current_actual;
    double current_expected;
};

template<class IntType>
double chi_squared_test(const std::vector<IntType>& results, const std::vector<double>& probabilities, IntType iterations) {
    chi_squared_collector<IntType> calc;
    for(std::size_t i = 0; i < results.size(); ++i) {
        calc(results[i], iterations * probabilities[i]);
    }
    return calc.cdf();
}

#endif


// Copyright John Maddock 2012.
// 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_MATH_AIRY_HPP
#define BOOST_MATH_AIRY_HPP

#include <boost/math/special_functions/bessel.hpp>
#include <boost/math/special_functions/cbrt.hpp>

namespace boost{ namespace math{

namespace detail{

template <class T, class Policy>
T airy_ai_imp(T x, const Policy& pol)
{
   BOOST_MATH_STD_USING

   if(x < 0)
   {
      T p = (-x * sqrt(-x) * 2) / 3;
      T v = T(1) / 3;
      T j1 = boost::math::cyl_bessel_j(v, p, pol);
      T j2 = boost::math::cyl_bessel_j(-v, p, pol);
      T ai = sqrt(-x) * (j1 + j2) / 3;
      //T bi = sqrt(-x / 3) * (j2 - j1);
      return ai;
   }
   else if(fabs(x * x * x) / 6 < tools::epsilon<T>())
   {
      T tg = boost::math::tgamma(constants::twothirds<T>(), pol);
      T ai = 1 / (pow(T(3), constants::twothirds<T>()) * tg);
      //T bi = 1 / (sqrt(boost::math::cbrt(T(3))) * tg);
      return ai;
   }
   else
   {
      T p = 2 * x * sqrt(x) / 3;
      T v = T(1) / 3;
      //T j1 = boost::math::cyl_bessel_i(-v, p, pol);
      //T j2 = boost::math::cyl_bessel_i(v, p, pol);
      //
      // Note that although we can calculate ai from j1 and j2, the accuracy is horrible
      // as we're subtracting two very large values, so use the Bessel K relation instead:
      //
      T ai = cyl_bessel_k(v, p, pol) * sqrt(x / 3) / boost::math::constants::pi<T>();  //sqrt(x) * (j1 - j2) / 3;
      //T bi = sqrt(x / 3) * (j1 + j2);
      return ai;
   }
}

template <class T, class Policy>
T airy_bi_imp(T x, const Policy& pol)
{
   BOOST_MATH_STD_USING

   if(x < 0)
   {
      T p = (-x * sqrt(-x) * 2) / 3;
      T v = T(1) / 3;
      T j1 = boost::math::cyl_bessel_j(v, p, pol);
      T j2 = boost::math::cyl_bessel_j(-v, p, pol);
      //T ai = sqrt(-x) * (j1 + j2) / 3;
      T bi = sqrt(-x / 3) * (j2 - j1);
      return bi;
   }
   else if(fabs(x * x * x) / 6 < tools::epsilon<T>())
   {
      T tg = boost::math::tgamma(constants::twothirds<T>(), pol);
      //T ai = 1 / (pow(T(3), constants::twothirds<T>()) * tg);
      T bi = 1 / (sqrt(boost::math::cbrt(T(3))) * tg);
      return bi;
   }
   else
   {
      T p = 2 * x * sqrt(x) / 3;
      T v = T(1) / 3;
      T j1 = boost::math::cyl_bessel_i(-v, p, pol);
      T j2 = boost::math::cyl_bessel_i(v, p, pol);
      T bi = sqrt(x / 3) * (j1 + j2);
      return bi;
   }
}

template <class T, class Policy>
T airy_ai_prime_imp(T x, const Policy& pol)
{
   BOOST_MATH_STD_USING

   if(x < 0)
   {
      T p = (-x * sqrt(-x) * 2) / 3;
      T v = T(2) / 3;
      T j1 = boost::math::cyl_bessel_j(v, p, pol);
      T j2 = boost::math::cyl_bessel_j(-v, p, pol);
      T aip = -x * (j1 - j2) / 3;
      return aip;
   }
   else if(fabs(x * x) / 2 < tools::epsilon<T>())
   {
      T tg = boost::math::tgamma(constants::third<T>(), pol);
      T aip = 1 / (boost::math::cbrt(T(3)) * tg);
      return -aip;
   }
   else
   {
      T p = 2 * x * sqrt(x) / 3;
      T v = T(2) / 3;
      //T j1 = boost::math::cyl_bessel_i(-v, p, pol);
      //T j2 = boost::math::cyl_bessel_i(v, p, pol);
      //
      // Note that although we can calculate ai from j1 and j2, the accuracy is horrible
      // as we're subtracting two very large values, so use the Bessel K relation instead:
      //
      T aip = -cyl_bessel_k(v, p, pol) * x / (boost::math::constants::root_three<T>() * boost::math::constants::pi<T>());
      return aip;
   }
}

template <class T, class Policy>
T airy_bi_prime_imp(T x, const Policy& pol)
{
   BOOST_MATH_STD_USING

   if(x < 0)
   {
      T p = (-x * sqrt(-x) * 2) / 3;
      T v = T(2) / 3;
      T j1 = boost::math::cyl_bessel_j(v, p, pol);
      T j2 = boost::math::cyl_bessel_j(-v, p, pol);
      T aip = -x * (j1 + j2) / constants::root_three<T>();
      return aip;
   }
   else if(fabs(x * x) / 2 < tools::epsilon<T>())
   {
      T tg = boost::math::tgamma(constants::third<T>(), pol);
      T bip = sqrt(boost::math::cbrt(T(3))) / tg;
      return bip;
   }
   else
   {
      T p = 2 * x * sqrt(x) / 3;
      T v = T(2) / 3;
      T j1 = boost::math::cyl_bessel_i(-v, p, pol);
      T j2 = boost::math::cyl_bessel_i(v, p, pol);
      T aip = x * (j1 + j2) / boost::math::constants::root_three<T>();
      return aip;
   }
}

} // namespace detail

template <class T, class Policy>
inline typename tools::promote_args<T>::type airy_ai(T x, const Policy&)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<T>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   return policies::checked_narrowing_cast<result_type, Policy>(detail::airy_ai_imp<value_type>(static_cast<value_type>(x), forwarding_policy()), "boost::math::airy<%1%>(%1%)");
}

template <class T>
inline typename tools::promote_args<T>::type airy_ai(T x)
{
   return airy_ai(x, policies::policy<>());
}

template <class T, class Policy>
inline typename tools::promote_args<T>::type airy_bi(T x, const Policy&)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<T>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   return policies::checked_narrowing_cast<result_type, Policy>(detail::airy_bi_imp<value_type>(static_cast<value_type>(x), forwarding_policy()), "boost::math::airy<%1%>(%1%)");
}

template <class T>
inline typename tools::promote_args<T>::type airy_bi(T x)
{
   return airy_bi(x, policies::policy<>());
}

template <class T, class Policy>
inline typename tools::promote_args<T>::type airy_ai_prime(T x, const Policy&)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<T>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   return policies::checked_narrowing_cast<result_type, Policy>(detail::airy_ai_prime_imp<value_type>(static_cast<value_type>(x), forwarding_policy()), "boost::math::airy<%1%>(%1%)");
}

template <class T>
inline typename tools::promote_args<T>::type airy_ai_prime(T x)
{
   return airy_ai_prime(x, policies::policy<>());
}

template <class T, class Policy>
inline typename tools::promote_args<T>::type airy_bi_prime(T x, const Policy&)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<T>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;

   return policies::checked_narrowing_cast<result_type, Policy>(detail::airy_bi_prime_imp<value_type>(static_cast<value_type>(x), forwarding_policy()), "boost::math::airy<%1%>(%1%)");
}

template <class T>
inline typename tools::promote_args<T>::type airy_bi_prime(T x)
{
   return airy_bi_prime(x, policies::policy<>());
}

}} // namespaces

#endif // BOOST_MATH_AIRY_HPP


/**
 * \file SinusGeneratorFilter.cpp
 */

#include "SimpleSinusGeneratorFilter.h"

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

#include <cmath>
#include <cstdint>

namespace ATK
{
  template<class DataType_>
  SimpleSinusGeneratorFilter<DataType_>::SimpleSinusGeneratorFilter()
  :TypedBaseFilter<DataType_>(0, 1)
  {
  }

  template<class DataType_>
  void SimpleSinusGeneratorFilter<DataType_>::set_amplitude(DataType_ amplitude)
  {
    this->amplitude = amplitude;
  }

  template<class DataType_>
  void SimpleSinusGeneratorFilter<DataType_>::set_frequency(int frequency)
  {
    this->frequency = frequency;
  }

  template<class DataType_>
  void SimpleSinusGeneratorFilter<DataType_>::process_impl(gsl::index size) const
  {
    double real_increment = 2. / output_sampling_rate * frequency;

    for(gsl::index i = 0; i < size; ++i)
    {
      state += real_increment;
      outputs[0][i] = static_cast<DataType_>(amplitude * std::sin(state * boost::math::constants::pi<double>()));
    }
  }

  template class SimpleSinusGeneratorFilter<std::int16_t>;
  template class SimpleSinusGeneratorFilter<std::int32_t>;
  template class SimpleSinusGeneratorFilter<int64_t>;
  template class SimpleSinusGeneratorFilter<float>;
  template class SimpleSinusGeneratorFilter<double>;
}