#include <iostream>
#include <armadillo>
#include <mkl.h>

#include "class_data.hpp"
#include "class_approximation.hpp"
#include "class_partition.hpp"
#include "evaluate_covariance.hpp"
#include "constants.hpp"
#include <string.h>

void Approximation::posterior_inference()
{
    gettimeofday(&timeNow, NULL);
    cout<<"===========================> Calculating posterior quantities starts. Elapsed time: "<<(double)timeNow.tv_sec-(double)timeBegin.tv_sec+((double)timeNow.tv_usec-(double)timeBegin.tv_usec)/1000000.0<<" seconds.\n";

    //Allocate memory for KCholTimesCurrentw (vector size: partition->nKnots)
    KCholTimesCurrentw = new vec [partition->nRegionsInTotal];

    //Allocate memory for KCholTimesCurrentA (cube size: first dimension: partition->nKnots, second dimesnion: partition->nKnots, third dimension or the number of slices: (partition->nRegionsInTotal-1)*partition->nRegionsInTotal/2)
    KCholTimesCurrentA = new cube [partition->nRegionsInTotal-partition->nRegionsAtEachLevel[NUM_LEVELS_M-1]];

    //Allocate memory for temporary variables
    vec w;
    mat A;
    double *tempMemory = new double [partition->nKnots*partition->nKnots];

    w.set_size(partition->nKnots);
    A.set_size(partition->nKnots,partition->nKnots);

    //Temporary variables if load ATilde from disk
    int size=(NUM_LEVELS_M-1)*NUM_LEVELS_M/2;
    unsigned long offset = partition->nRegionsInTotal-partition->nRegionsAtEachLevel[NUM_LEVELS_M-1];

    //From the second finest to the coarsest level
    for(int currentLevel = NUM_LEVELS_M-2; currentLevel > -1 ; currentLevel--)
    {
        for(unsigned long iRegion = REGION_START[currentLevel]; iRegion < REGION_END[currentLevel] + 1; iRegion++)
        {
            unsigned long indexReginAtThisLevel = iRegion - REGION_START[currentLevel];

            //Deal with this region
            if(SAVE_TO_DISK_FLAG) load_ATilde_from_disk(iRegion*NUM_PARTITIONS_J+1, iRegion*NUM_PARTITIONS_J+NUM_PARTITIONS_J, currentLevel == NUM_LEVELS_M-2, partition->nKnotsAtFinestLevel, offset, partition->nKnots, size);

            aggregate_w_and_A(w, A, tempMemory, currentLevel, (currentLevel+1)*(currentLevel+2)/2-1, iRegion*NUM_PARTITIONS_J+1, iRegion*NUM_PARTITIONS_J+NUM_PARTITIONS_J, iRegion);

            if(CALCULATION_MODE != "prediction")
                loglikelihood += -2*sum(log(RChol[iRegion].diag()));

            //Compute the posterior conditional variance-covariance matrix
            RChol[iRegion] = chol( RChol[iRegion]*RChol[iRegion].t()+A,"lower");

            //Compute KCholTimesCurrentw that is the Cholesky factor of the inverse of the conditional posterior variance-covariance matrix times w
            KCholTimesCurrentw[iRegion].set_size(partition->nKnots);
            KCholTimesCurrentw[iRegion] = solve(trimatl(RChol[iRegion]),w);

            //Compute KCholTimesCurrentA that is the Cholesky factor of the inverse of the conditional posterior variance-covariance matrix times A
            KCholTimesCurrentA[iRegion].set_size(partition->nKnots,partition->nKnots,currentLevel);

            //Get wTilde and ATilde
            if(currentLevel != 0)
                get_wTilde_and_ATilde_in_posterior(w,A, tempMemory, partition->nKnots, KCholTimesCurrentA[iRegion], KCholTimesCurrentw[iRegion], iRegion, currentLevel);

            if(CALCULATION_MODE != "prediction")
                KCholTimesCurrentA[iRegion].reset();

            //Free wTilde and ATilde of the children to save memory

            free_wTilde_and_ATilde(iRegion*NUM_PARTITIONS_J+1, iRegion*NUM_PARTITIONS_J+NUM_PARTITIONS_J);

            if(CALCULATION_MODE != "prediction")
            {
                loglikelihood += 2*sum(log(RChol[iRegion].diag()))-dot(KCholTimesCurrentw[iRegion],KCholTimesCurrentw[iRegion]);
                KCholTimesCurrentw[iRegion].reset();
            }

            if(SAVE_TO_DISK_FLAG)
            {
                //save ATilde of the current region to disk
                std::string fileName=TMP_DIRECTORY+"/"+std::to_string(iRegion)+".bin";
                ATilde[iRegion].save(fileName,arma_binary);
                ATilde[iRegion].reset();
            }

        }

        timeval timeNow;
        gettimeofday(&timeNow, NULL);
        cout<<"Posterior: Level "<<currentLevel+1<<" is complete. Elapsed time: "<<(double)timeNow.tv_sec-(double)timeBegin.tv_sec+((double)timeNow.tv_usec-(double)timeBegin.tv_usec)/1000000.0<<" seconds.\n";

    }

    if(CALCULATION_MODE!="prediction")
        cout<<"The obtained loglikelihood is: "<<-0.5*(loglikelihood+NUM_OBSERVATIONS*log(2*3.14159265359))<<endl;

    //Deallocate temporary memory
    w.reset();
    A.reset();
    delete[] tempMemory;

    gettimeofday(&timeNow, NULL);
    cout<<"===========================>  Calculating posterior quantities is complete. Elapsed time: "<<(double)timeNow.tv_sec-(double)timeBegin.tv_sec+((double)timeNow.tv_usec-(double)timeBegin.tv_usec)/1000000.0<<" seconds.\n\n";
}


#include <complex>

#include <Eigen/Dense>
#include <fftw3.h>

#include <fstream>
#include <iostream>
#include <memory>
#include <vector>
#include <string_view>
#include <numeric>

#include "yaml-cpp/yaml.h"

#include "bond.h"
#include "constants.h"
#include "file_io.h"
#include "velocity_verlet.h"
#include "energies.h"

class FFTWrapper {
    private:
        Eigen::VectorXd fft_input;
        Eigen::VectorXcd fft_result;
        fftw_plan plan;

    public:
    Eigen::VectorXcd calculate(const std::vector<double>& input) {
        if (input.size() != fft_input.size()) {
            throw std::runtime_error("The input array provided to FFTWrapper must be the same size as specified in the constructor. Got " + std::to_string(input.size()) + " vs " + std::to_string(fft_input.size()));
        }
        std::copy(std::begin(input), std::end(input), fft_input.data());
        fftw_execute(plan);
        return fft_result;
    }

        FFTWrapper(int input_size_) : fft_input(input_size_), fft_result((input_size_ / 2) + 1), plan(fftw_plan_dft_r2c_1d(fft_input.size(), fft_input.data(), reinterpret_cast<fftw_complex*>(fft_result.data()), FFTW_DESTROY_INPUT & FFTW_PATIENT)) {};

    ~FFTWrapper() {
        fftw_destroy_plan(plan);
    }

};

namespace Eigen {
    auto begin(VectorXcd vec) -> decltype(vec.data()) {
        return vec.data();
    }

    auto end(VectorXcd vec) -> decltype(vec.data()) {
        return vec.data() + vec.size();
    }
}

const std::string DEFAULT_CONFIG_FILE{"config.yaml"};

template <typename BondIter>
bool check_bond_sampling(BondIter bonds_begin, BondIter bonds_end, const Eigen::MatrixXd& masses, const double timestep, const float min_samples=10) {
    std::vector<double> bond_periods;
    std::transform(bonds_begin, bonds_end, std::back_inserter(bond_periods), [masses](auto&& bond){return bond->period(masses);});
    const double min_bond_period = *std::min_element(bond_periods.begin(), bond_periods.end());
    return (timestep * min_samples < min_bond_period);
}

std::vector<std::complex<double>> calculate_excitement_fft(const std::vector<double>& excitements) {

    // Set up empty input and output vectors for FFTW to mangle.
    std::vector<double> fft_input(excitements.size());
    auto out_size = (excitements.size() / 2) + 1;
    std::vector<std::complex<double>> fft_results(out_size);

    // We'll want to move this plan out of the function later so we can reuse it.
    auto plan = fftw_plan_dft_r2c_1d(fft_input.size(), fft_input.data(), reinterpret_cast<fftw_complex*>(fft_results.data()), FFTW_ESTIMATE);
    std::copy(excitements.begin(), excitements.end(), fft_input.begin());
    fftw_execute(plan);
    fftw_destroy_plan(plan);
    return fft_results;
}

int main(int argc, char** argv) {
    std::string config_filename;
    if (argc > 1) {
        config_filename = argv[1];
    } else {
        config_filename = DEFAULT_CONFIG_FILE;
    }
    YAML::Node config;
    try {
         config = YAML::LoadFile(config_filename);
    } catch (const YAML::BadFile& ex) {
         std::cerr << "Could not open " << config_filename << ". Is it present in this directory?\n";
         throw ex;
    }
    const std::string positionfile = config["positionfile"].as<std::string>();
    const std::string bondfile = config["bondfile"].as<std::string>();
    const std::string outputfile = config["outputfile"]["filename"].as<std::string>();
    const int output_frequency = config["outputfile"]["frequency"].as<int>();
    const int console_frequency = config["outputfile"]["consolefrequency"].as<int>();
    const double timestep = config["simulation"]["timestep"].as<double>();
    const int number_steps = config["simulation"]["steps"].as<int>();

    const int fft_to_ignore = config["fft"]["ignore_steps"].as<int>();
    const int fft_frequency = config["fft"]["frequency"].as<int>();
    const std::string fft_filename = config["fft"]["filename"].as<std::string>();

    constexpr auto unit_type = UnitType::ARBITRARY;
    auto [positions, atom_names] = load_positions(positionfile, unit_type, 3);
    auto bonds = load_bonds(bondfile, positions, atom_names);

    Eigen::VectorXd masses = load_masses(atom_names, unit_type);
    if (!check_bond_sampling(bonds.begin(), bonds.end(), masses, timestep, 10)) {
        std::cerr << "Shortest bond period is sampled less than ten times per timestep.\n";
    }

    Eigen::MatrixXd velocities = Eigen::MatrixXd::Zero(positions.rows(), positions.cols());

    for (const auto& excitement : config["excitements"]) {
        const auto atom_a_name = excitement["atoms"][0].as<std::string>();
        const int atom_a_id = atom_name_to_id(atom_names.begin(), atom_names.end(), atom_a_name);

        const auto atom_b_name = excitement["atoms"][1].as<std::string>();
        const int atom_b_id = atom_name_to_id(atom_names.begin(), atom_names.end(), atom_b_name);

        // Now find the correct bond.
        auto correct_bond = std::find_if(bonds.begin(), bonds.end(), [atom_a_id, atom_b_id](const auto& bond){
            return (bond->atoms[0] == atom_a_id) && (bond->atoms[1] == atom_b_id);
        });
        excite_bond(*correct_bond, positions, masses, excitement["factor"].as<double>());

        std::cout << "Exciting a bond between " << atom_a_name << " and " << atom_b_name << "\n";
    }

    // We need to set the initial accelerations to be correct.
    Eigen::MatrixXd accelerations = calculate_accelerations(bonds, positions, masses);

    std::ofstream output_file { outputfile };
    std::ofstream bond_excitement_file { "bonds.csv" };

    bond_excitement_file << "Time, ";
    for (const auto& bond : bonds) {
        bond_excitement_file << atom_names[bond->atoms[0]] << "->" << atom_names[bond->atoms[1]]
                             << ",";
    }
    bond_excitement_file << "\n";

    auto fft_wrapper = FFTWrapper(fft_frequency);

    std::vector<std::vector<double>> excitements_vec(bonds.size());

    std::vector<Eigen::VectorXcd> total_freq_vector(bonds.size());
    for (int i = 0; i < bonds.size(); ++i) {
        total_freq_vector[i] = Eigen::VectorXcd::Zero((fft_frequency / 2) + 1);
        excitements_vec[i] = std::vector<double>(fft_frequency);
    }

    int fft_samples_taken = 0;

    for (int step = 0; step < number_steps; ++step) {
        velocity_verlet(positions, velocities, accelerations, masses, timestep, bonds);

        if (step % console_frequency == 0 ) {;
            const auto potential_energy = calculate_potential_energy(bonds, positions);
            const auto kinetic_energy = calculate_kinetic_energy(velocities, masses);
            std::cout << "Step " << step << ", time = " << step * timestep << ", PE = " << potential_energy << ", KE = "
<< kinetic_energy << " TE = " << potential_energy + kinetic_energy  << "\n";
        }
        if (step % output_frequency == 0) {
            write_xyz(outputfile, positions, velocities, accelerations, step, atom_names);
            bond_excitement_file << step * timestep << ", ";
            for (const auto& bond : bonds) {
                bond_excitement_file << bond->get_excitement_factor(positions)<< ",";
            }
            bond_excitement_file << "\n";
        }

        if ((step - fft_to_ignore) % fft_frequency == 0 && step - fft_to_ignore > 0) {
            for (int j = 0; j < bonds.size(); ++j) {
                Eigen::VectorXcd ret_vec = fft_wrapper.calculate(excitements_vec[j]);
                total_freq_vector[j] += ret_vec;
            }
            fft_samples_taken++;
        }

        // Do this after we've output lest we write over the first element.
        if (step > fft_to_ignore) {
            for (int j = 0; j < bonds.size(); ++j) {
                excitements_vec[j][step % fft_frequency] = bonds[j]->get_excitement_factor(positions) - 1.0;
            }
        }
    }

    // Some final outputting.
    for (auto& sub_vec: total_freq_vector) {
        sub_vec /= fft_samples_taken;
    }
    std::ofstream fft_file{fft_filename};
    for (int i = 0; i < total_freq_vector[0].size(); ++i) {
        for (int bond = 0; bond < bonds.size(); ++bond) {
            const auto item = total_freq_vector[bond][i];
            if (item.imag() < 0.0) {
                fft_file << item.real() << item.imag() << "j";
            } else {
                fft_file << item.real() << "+" << item.imag() << "j";
            }
            if (bond != bonds.size() - 1) {
                fft_file << ", ";
            }
        }
        fft_file << "\n";
    }
}


// Copyright (c) 2018-2019 FRC Team 3512. All Rights Reserved.

#pragma once

#include <Eigen/Core>
#include <frc/controller/StateSpaceLoop.h>

#include "Constants.hpp"
#include "control/ClimberCoeffs.hpp"
#include "control/TrapezoidalMotionProfile.hpp"
#include "logging/CsvLogger.hpp"

namespace frc3512 {

class ClimberController {
public:
    // State tolerances in meters and meters/sec respectively.
    static constexpr double kPositionTolerance = 0.05;
    static constexpr double kVelocityTolerance = 2.0;

    ClimberController();

    ClimberController(const ClimberController&) = delete;
    ClimberController& operator=(const ClimberController&) = delete;

    void Enable();
    void Disable();

    void SetGoal(double goal);

    /**
     * Sets the references.
     *
     * @param position Position of the carriage in meters.
     * @param velocity Velocity of the carriage in meters per second.
     */
    void SetReferences(units::meter_t position,
                       units::meters_per_second_t velocity);

    bool AtGoal() const;

    bool AtReferences() const;

    bool ErrorExceeded() const;

    /**
     * Sets the current encoder measurement.
     *
     * @param measuredPosition Position of the carriage in meters.
     */
    void SetMeasuredPosition(double measuredPosition);

    /**
     * Returns the control loop calculated voltage.
     */
    double ControllerVoltage();

    /**
     * Returns the estimated position.
     */
    double EstimatedPosition() const;

    /**
     * Returns the estimated velocity.
     */
    double EstimatedVelocity() const;

    /**
     * Returns the error between the position reference and the position
     * estimate.
     */
    double PositionError() const;

    /**
     * Returns the error between the velocity reference and the velocity
     * estimate.
     */
    double VelocityError() const;

    /**
     * Returns the current reference set by the profile
     */
    double PositionReference();

    /**
     * Executes the control loop for a cycle.
     */
    void Update(void);

    /**
     * Resets any internal state.
     */
    void Reset(void);

private:
    // The current sensor measurement.
    Eigen::Matrix<double, 1, 1> m_Y;
    TrapezoidalMotionProfile::State m_goal;

    TrapezoidalMotionProfile::Constraints constraints{kClimberMaxV,
                                                      kClimberMaxA};
    TrapezoidalMotionProfile m_positionProfile{constraints, {0_m, 0_mps}};

    TrapezoidalMotionProfile::State m_profiledReference;

    // The control loop.
    frc::StateSpaceLoop<2, 1, 1> m_loop{MakeClimberLoop()};

    bool m_atReferences = false;
    bool m_errorExceeded = false;

    CsvLogger climberLogger{"/home/lvuser/Climber.csv",
                            "Time,EstPos,PosRef,Voltage"};
};

}  // namespace frc3512


/*
 Copyright (C) 2017 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

/*! \file qle/termstructures/aposurface.hpp
    \brief Average future price option surface derived from future option surface
    \ingroup termstructures
 */

#ifndef quantext_apo_surface_hpp
#define quantext_apo_surface_hpp

#include <boost/optional.hpp>
#include <ql/patterns/lazyobject.hpp>
#include <ql/quotes/simplequote.hpp>
#include <ql/termstructures/volatility/equityfx/blackvoltermstructure.hpp>
#include <qle/indexes/commodityindex.hpp>
#include <qle/pricingengines/commodityapoengine.hpp>
#include <qle/termstructures/blackvariancesurfacemoneyness.hpp>
#include <qle/termstructures/pricetermstructure.hpp>
#include <qle/time/futureexpirycalculator.hpp>

namespace QuantExt {

//! Average future price option (APO) surface derived from a future option surface
class ApoFutureSurface : public QuantLib::LazyObject, public QuantLib::BlackVolatilityTermStructure {

public:
    ApoFutureSurface(const QuantLib::Date& referenceDate, const std::vector<QuantLib::Real>& moneynessLevels,
                     const boost::shared_ptr<CommoditySpotIndex>& index,
                     const QuantLib::Handle<PriceTermStructure>& pts,
                     const QuantLib::Handle<QuantLib::YieldTermStructure>& yts,
                     const boost::shared_ptr<FutureExpiryCalculator>& expCalc,
                     const QuantLib::Handle<QuantLib::BlackVolTermStructure>& baseVts,
                     const boost::shared_ptr<FutureExpiryCalculator>& baseExpCalc, QuantLib::Real beta = 0.0,
                     bool flatStrikeExtrapolation = true,
                     const boost::optional<QuantLib::Period>& maxTenor = boost::none);

    //! \name TermStructure interface
    //@{
    QuantLib::Date maxDate() const override;
    //@}

    //! \name VolatilityTermStructure interface
    //@{
    QuantLib::Real minStrike() const override;
    QuantLib::Real maxStrike() const override;

    //! \name Visitability
    //@{
    void accept(QuantLib::AcyclicVisitor& v) override;
    //@}

    //! \name Observer interface
    //@{
    void update() override;
    //@}

    //! \name LazyObject interface
    //@{
    void performCalculations() const override;
    //@}

    //! \name Inspectors
    //@{
    const boost::shared_ptr<BlackVarianceSurfaceMoneyness>& vts() const;
    //@}

protected:
    //! \name BlackVolatilityTermStructure
    //@{
    QuantLib::Volatility blackVolImpl(QuantLib::Time t, QuantLib::Real strike) const override;
    //@}

private:
    boost::shared_ptr<CommoditySpotIndex> index_;
    boost::shared_ptr<FutureExpiryCalculator> baseExpCalc_;

    //! The APO schedule dates.
    std::vector<QuantLib::Date> apoDates_;

    //! This will keep a handle on the APO vol quotes that are calculated.
    std::vector<std::vector<boost::shared_ptr<QuantLib::SimpleQuote> > > vols_;

    //! The surface that is created to do the work.
    boost::shared_ptr<BlackVarianceSurfaceMoneyness> vts_;

    //! The engine for valuing the APOs
    boost::shared_ptr<CommodityAveragePriceOptionBaseEngine> apoEngine_;
};

} // namespace QuantExt

#endif


/*
 Copyright 2009-2012 Karsten Ahnert
 Copyright 2009-2012 Mario Mulansky

 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)
 */

/* example showing how odeint can be used with std::list */

#include <iostream>
#include <cmath>
#include <list>

#include <boost/numeric/odeint.hpp>

//[ list_bindings
typedef std::list< double > state_type;

namespace boost { namespace numeric { namespace odeint {

template< >
struct is_resizeable< state_type >
{ // declare resizeability
    typedef boost::true_type type;
    const static bool value = type::value;
};

template< >
struct same_size_impl< state_type , state_type >
{ // define how to check size
    static bool same_size( const state_type &v1 ,
                           const state_type &v2 )
    {
        return v1.size() == v2.size();
    }
};

template< >
struct resize_impl< state_type , state_type >
{ // define how to resize
    static void resize( state_type &v1 ,
                        const state_type &v2 )
    {
        v1.resize( v2.size() );
    }
};

} } }
//]

void lattice( const state_type &x , state_type &dxdt , const double /* t */ )
{
    state_type::const_iterator x_begin = x.begin();
    state_type::const_iterator x_end = x.end();
    state_type::iterator dxdt_begin = dxdt.begin();

    x_end--; // stop one before last
    while( x_begin != x_end )
    {
        *(dxdt_begin++) = std::sin( *(x_begin) - *(x_begin++) );
    }
    *dxdt_begin = sin( *x_begin - *(x.begin()) ); // periodic boundary
}

using namespace boost::numeric::odeint;

int main()
{
    const int N = 32;
    state_type x;
    for( int i=0 ; i<N ; ++i )
        x.push_back( 1.0*i/N );

    integrate_const( runge_kutta4< state_type >() , lattice , x , 0.0 , 10.0 , 0.1 );
}


//==================================================================================================
/*!
  @file
  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_ERFC_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ERFC_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!
  @ingroup group-euler
    Function object implementing erfc capabilities

  Computes the complementary error function

  @par Semantic:

  For every parameter of floating type T

  @code
  T r = erfc(x);
  @endcode

  is similar to:

  @code
  T0 r = oneminus(erf(x));
  @endcode

  @par Decorators

  std_ for floating entries

  @see erf, erfcx, oneminus

  **/
  const boost::dispatch::functor<tag::erfc_> erfc = {};
} }
#endif

#include <boost/simd/function/scalar/erfc.hpp>
#include <boost/simd/function/simd/erfc.hpp>

#endif


#include <celero/Celero.h>

#include <random>
#include <limits>

#include <glm/glm.hpp>
#include <glm/gtx/quaternion.hpp>
#include <Eigen/Dense>
#include <Eigen/StdVector>

///
/// This is the main(int argc, char** argv) for the entire celero program.
/// You can write your own, or use this macro to insert the standard one into the project.
///
CELERO_MAIN

constexpr int g_samples = 30;
constexpr int g_iterations = 200000;

std::mt19937 rng;

constexpr int number_of_tests = 1;

class GLM_Vec3Fixture : public celero::TestFixture
{
public:
	GLM_Vec3Fixture()
	{
		rng.seed(0);
	}

	std::vector<glm::vec3> vecs;

	std::vector<celero::TestFixture::ExperimentValue> getExperimentValues() const override
	{
		std::vector<celero::TestFixture::ExperimentValue> problemSpace;
		const int totalNumberOfTests = number_of_tests;

		//for (int i = 0; i < totalNumberOfTests; i++)
		//	problemSpace.emplace_back(int64_t(pow(2, i)));
		problemSpace.emplace_back(10);

		return problemSpace;
	}

	/// Before each run, build a vector of random integers.
	void setUp(celero::TestFixture::ExperimentValue const& experimentValue) override
	{
		vecs.reserve(experimentValue.Value);

		auto random = std::bind(std::uniform_real_distribution<float>(std::numeric_limits<float>::min(), std::numeric_limits<float>::max()), rng);

		for (decltype(experimentValue.Value) i = 0; i < experimentValue.Value; ++i)
			vecs.emplace_back(random(), random(), random());
	}
};

class GLM_QuaternionFixture : public celero::TestFixture
{
public:
	GLM_QuaternionFixture()
	{
		rng.seed(0);
	}

	std::vector<glm::quat> quats;

	std::vector<celero::TestFixture::ExperimentValue> getExperimentValues() const override
	{
		std::vector<celero::TestFixture::ExperimentValue> problemSpace;
		const int totalNumberOfTests = number_of_tests;

		//for (int i = 0; i < totalNumberOfTests; i++)
		//	problemSpace.emplace_back(int64_t(pow(2, i)));
		problemSpace.emplace_back(10);

		return problemSpace;
	}

	/// Before each run, build a vector of random integers.
	void setUp(celero::TestFixture::ExperimentValue const& experimentValue) override
	{
		quats.reserve(experimentValue.Value);

		auto random = std::bind(std::uniform_real_distribution<float>(std::numeric_limits<float>::min(), std::numeric_limits<float>::max()), rng);

		for (decltype(experimentValue.Value) i = 0; i < experimentValue.Value; ++i)
			quats.emplace_back(random(), random(), random(), random());
	}
};

class EigenVec3Fixture : public celero::TestFixture
{
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

	EigenVec3Fixture()
	{
		rng.seed(0);
	}

	std::vector<Eigen::Vector3f> vecs;

	std::vector<celero::TestFixture::ExperimentValue> getExperimentValues() const override
	{
		std::vector<celero::TestFixture::ExperimentValue> problemSpace;
		const int totalNumberOfTests = number_of_tests;

		//for (int i = 0; i < totalNumberOfTests; i++)
		//	problemSpace.emplace_back(int64_t(pow(2, i)));
		problemSpace.emplace_back(10);

		return problemSpace;
	}

	/// Before each run, build a vector of random integers.
	void setUp(celero::TestFixture::ExperimentValue const& experimentValue) override
	{
		vecs.reserve(experimentValue.Value);

		auto random = std::bind(std::uniform_real_distribution<float>(std::numeric_limits<float>::min(), std::numeric_limits<float>::max()), rng);

		for (decltype(experimentValue.Value) i = 0; i < experimentValue.Value; ++i)
			vecs.emplace_back(random(), random(), random());
	}
};

class EigenVec4Fixture : public celero::TestFixture
{
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

	EigenVec4Fixture()
	{
		rng.seed(0);
	}

	std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f>> vecs;

	std::vector<celero::TestFixture::ExperimentValue> getExperimentValues() const override
	{
		std::vector<celero::TestFixture::ExperimentValue> problemSpace;
		const int totalNumberOfTests = 4;

		//for (int i = 0; i < totalNumberOfTests; i++)
		//	problemSpace.emplace_back(int64_t(pow(2, i)));
		problemSpace.emplace_back(10);

		return problemSpace;
	}

	/// Before each run, build a vector of random integers.
	void setUp(celero::TestFixture::ExperimentValue const& experimentValue) override
	{
		vecs.reserve(experimentValue.Value);

		auto random = std::bind(std::uniform_real_distribution<float>(std::numeric_limits<float>::min(), std::numeric_limits<float>::max()), rng);

		for (decltype(experimentValue.Value) i = 0; i < experimentValue.Value; ++i)
			vecs.emplace_back(random(), random(), random(), random());
	}
};

class EigenQuaternionFixture : public celero::TestFixture
{
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

	EigenQuaternionFixture()
	{
		rng.seed(0);
	}

	std::vector<Eigen::Quaternionf, Eigen::aligned_allocator<Eigen::Quaternionf>> quats;

	std::vector<celero::TestFixture::ExperimentValue> getExperimentValues() const override
	{
		std::vector<celero::TestFixture::ExperimentValue> problemSpace;
		const int totalNumberOfTests = 1;

		//for (int i = 0; i < totalNumberOfTests; i++)
		//	problemSpace.emplace_back(int64_t(pow(2, i)));
		problemSpace.emplace_back(10);

		return problemSpace;
	}

	/// Before each run, build a vector of random integers.
	void setUp(celero::TestFixture::ExperimentValue const& experimentValue) override
	{
		quats.reserve(experimentValue.Value);

		auto random = std::bind(std::uniform_real_distribution<float>(std::numeric_limits<float>::min(), std::numeric_limits<float>::max()), rng);

		for (decltype(experimentValue.Value) i = 0; i < experimentValue.Value; ++i)
			quats.emplace_back(random(), random(), random(), random());
	}
};

BASELINE_F(Vector3_addition, GLM, GLM_Vec3Fixture, g_samples, g_iterations)
{
	celero::DoNotOptimizeAway(vecs[0] + vecs[1]);
}

BENCHMARK_F(Vector3_addition, Eigen, EigenVec3Fixture, g_samples, g_iterations)
{
	celero::DoNotOptimizeAway(vecs[0] + vecs[1]);
}

BASELINE_F(Vector3_addition_10, GLM, GLM_Vec3Fixture, g_samples, g_iterations)
{
	celero::DoNotOptimizeAway(vecs[0] + vecs[1] + vecs[2] + vecs[3] + vecs[3] + vecs[4] + vecs[5] + vecs[6] + vecs[7] + vecs[8] + vecs[9]);
}

BENCHMARK_F(Vector3_addition_10, Eigen, EigenVec3Fixture, g_samples, g_iterations)
{
	celero::DoNotOptimizeAway(vecs[0] + vecs[1] + vecs[2] + vecs[3] + vecs[3] + vecs[4] + vecs[5] + vecs[6] + vecs[7] + vecs[8] + vecs[9]);
}

BASELINE_F(Vector3_multiplication, GLM, GLM_Vec3Fixture, g_samples, g_iterations)
{
	celero::DoNotOptimizeAway(vecs[0] * vecs[1]);
}

BENCHMARK_F(Vector3_multiplication, Eigen, EigenVec3Fixture, g_samples, g_iterations)
{
	celero::DoNotOptimizeAway(vecs[0].cwiseProduct(vecs[1]));
}

BASELINE_F(Vector3_multiplication_10, GLM, GLM_Vec3Fixture, g_samples, g_iterations)
{
	celero::DoNotOptimizeAway(vecs[0] * vecs[1] * vecs[2] * vecs[3] * vecs[3] * vecs[4] * vecs[5] * vecs[6] * vecs[7] * vecs[8] * vecs[9]);
}

BENCHMARK_F(Vector3_multiplication_10, Eigen, EigenVec3Fixture, g_samples, g_iterations)
{
	celero::DoNotOptimizeAway(
		vecs[0].cwiseProduct(
			vecs[1].cwiseProduct(
				vecs[2].cwiseProduct(
					vecs[3].cwiseProduct(
						vecs[3].cwiseProduct(
							vecs[4].cwiseProduct(
								vecs[5].cwiseProduct(
									vecs[6].cwiseProduct(
										vecs[7].cwiseProduct(
											vecs[8].cwiseProduct(vecs[9])
											)
										)
									)
								)
							)
						)
					)
				)
			)
		);
}

BASELINE_F(Vector3_division, GLM, GLM_Vec3Fixture, g_samples, g_iterations)
{
	celero::DoNotOptimizeAway(vecs[0] / vecs[1]);
}

BENCHMARK_F(Vector3_division, Eigen, EigenVec3Fixture, g_samples, g_iterations)
{
	celero::DoNotOptimizeAway(vecs[0].cwiseQuotient(vecs[1]));
}

BASELINE_F(Vector3_division_10, GLM, GLM_Vec3Fixture, g_samples, g_iterations)
{
	celero::DoNotOptimizeAway(vecs[0] / vecs[1] / vecs[2] / vecs[3] / vecs[3] / vecs[4] / vecs[5] / vecs[6] / vecs[7] / vecs[8] / vecs[9]);
}

BENCHMARK_F(Vector3_division_10, Eigen, EigenVec3Fixture, g_samples, g_iterations)
{
	celero::DoNotOptimizeAway(
		vecs[0].cwiseQuotient(
			vecs[1].cwiseQuotient(
				vecs[2].cwiseQuotient(
					vecs[3].cwiseQuotient(
						vecs[3].cwiseQuotient(
							vecs[4].cwiseQuotient(
								vecs[5].cwiseQuotient(
									vecs[6].cwiseQuotient(
										vecs[7].cwiseQuotient(
											vecs[8].cwiseQuotient(vecs[9])
											)
										)
									)
								)
							)
						)
					)
				)
			)
		);
}

BASELINE_F(Vector3_dot, GLM, GLM_Vec3Fixture, g_samples, g_iterations)
{
	celero::DoNotOptimizeAway(glm::dot(vecs[0], vecs[1]));
}

BENCHMARK_F(Vector3_dot, Eigen, EigenVec3Fixture, g_samples, g_iterations)
{
	celero::DoNotOptimizeAway(vecs[0].dot(vecs[1]));
}

BASELINE_F(Vector3_cross, GLM, GLM_Vec3Fixture, g_samples, g_iterations)
{
	celero::DoNotOptimizeAway(glm::cross(vecs[0], vecs[1]));
}

BENCHMARK_F(Vector3_cross, Eigen, EigenVec3Fixture, g_samples, g_iterations)
{
	celero::DoNotOptimizeAway(vecs[0].cross(vecs[1]));
}

BENCHMARK_F(Vector3_cross, Eigen_vec4_cross3, EigenVec4Fixture, g_samples, g_iterations)
{
	celero::DoNotOptimizeAway(vecs[0].cross3(vecs[1]));
}

BASELINE_F(Vector3_cross_10, GLM, GLM_Vec3Fixture, g_samples, g_iterations)
{
	celero::DoNotOptimizeAway(
		glm::cross(vecs[0],
			glm::cross(vecs[1],
				glm::cross(vecs[2],
					glm::cross(vecs[3],
						glm::cross(vecs[4],
							glm::cross(vecs[5],
								glm::cross(vecs[16],
									glm::cross(vecs[7],
										glm::cross(vecs[8], vecs[9])
										)
									)
								)
							)
						)
					)
				)
			)
		);
}

BENCHMARK_F(Vector3_cross_10, Eigen, EigenVec3Fixture, g_samples, g_iterations)
{
	celero::DoNotOptimizeAway(
		vecs[0].cross(
			vecs[1].cross(
				vecs[2].cross(
					vecs[3].cross(
						vecs[3].cross(
							vecs[4].cross(
								vecs[5].cross(
									vecs[6].cross(
										vecs[7].cross(
											vecs[8].cross(vecs[9])
											)
										)
									)
								)
							)
						)
					)
				)
			)
		);
}

BENCHMARK_F(Vector3_cross_10, Eigen_vec4_cross3, EigenVec4Fixture, g_samples, g_iterations)
{
	celero::DoNotOptimizeAway(
		vecs[0].cross3(
			vecs[1].cross3(
				vecs[2].cross3(
					vecs[3].cross3(
						vecs[3].cross3(
							vecs[4].cross3(
								vecs[5].cross3(
									vecs[6].cross3(
										vecs[7].cross3(
											vecs[8].cross3(vecs[9])
											)
										)
									)
								)
							)
						)
					)
				)
			)
		);
}

BASELINE_F(Quaternion_mul, GLM, GLM_QuaternionFixture, g_samples, g_iterations)
{
	celero::DoNotOptimizeAway(quats[0] * quats[1]);
}

BENCHMARK_F(Quaternion_mul, Eigen, EigenQuaternionFixture, g_samples, g_iterations)
{
	celero::DoNotOptimizeAway(quats[0] * quats[1]);
}

BASELINE_F(Quaternion_mul_10, GLM, GLM_QuaternionFixture, g_samples, g_iterations)
{

	celero::DoNotOptimizeAway(quats[0] * quats[1] * quats[2] * quats[3] * quats[3] * quats[4] * quats[5] * quats[6] * quats[7] * quats[8] * quats[9]);
}

BENCHMARK_F(Quaternion_mul_10, Eigen, EigenQuaternionFixture, g_samples, g_iterations)
{

	celero::DoNotOptimizeAway(quats[0] * quats[1] * quats[2] * quats[3] * quats[3] * quats[4] * quats[5] * quats[6] * quats[7] * quats[8] * quats[9]);
}

#ifdef OPTIMIZE_AWAY_BENCHMARKS
BASELINE_F(Vector3_addition_opt, GLM, GLM_Vec3Fixture, g_samples, g_iterations)
{
	auto vec = vec1 + vec2;
}

BENCHMARK_F(Vector3_addition_opt, Eigen, EigenVec3Fixture, g_samples, g_iterations)
{
	auto vec = vec1 + vec2;
}

BASELINE_F(Vector3_cross_opt, GLM, GLM_Vec3Fixture, g_samples, g_iterations)
{
	auto vec = glm::cross(vec1, vec2);
}

BENCHMARK_F(Vector3_cross_opt, Eigen, EigenVec3Fixture, g_samples, g_iterations)
{
	auto vec = vec1.cross(vec2);
}

BENCHMARK_F(Vector3_cross_opt, Eigen_vec4_cross3, EigenVec4Fixture, g_samples, g_iterations)
{
	auto vec = vec1.cross3(vec2);
}

BASELINE_F(Quaternion_mul_opt, GLM, GLM_QuaternionFixture, g_samples, g_iterations)
{
	auto quat = quat1 * quat2;
}

BENCHMARK_F(Quaternion_mul_opt, Eigen, EigenQuaternionFixture, g_samples, g_iterations)
{
	auto quat = quat1 * quat2;
}
#endif


#include <iostream>
#include <boost/multiprecision/cpp_int.hpp>
using namespace std;
using namespace boost::multiprecision;

int main() {
	int n;
	cpp_int a, b, sum;
	cin >> n;
	for (int i = 0; i < n; i++) {
		cin >> a >> b;
		sum = a + b;
		string str = sum.str();
		if (str.length() > 80) cout << "overflow" << endl;
		else cout << a + b << endl;
	}
	return 0;
}


#ifndef IMAGE_LABEL_TRANSFORMATION_HPP
#define IMAGE_LABEL_TRANSFORMATION_HPP

#include <boost/array.hpp>

#include <opencv2/core/core.hpp>
#include <opencv2/opencv.hpp>

#include <vector>

#include "caffe/blob.hpp"
#include "caffe/common.hpp"
#include "caffe/layer.hpp"
#include "caffe/proto/caffe.pb.h"

namespace caffe {

template<typename Dtype>

struct ImageLabelTransformationAugmentSelection;

/**
 * @brief Applies common transformations to the input data, such as
 * scaling, mirroring, substracting the image mean...
 */
template <typename Dtype>
class ImageLabelTransformationLayer : public Layer<Dtype> {
 public:
  typedef cv::Size2i Size2i;
  typedef cv::Size_<Dtype> Size2v;
  typedef cv::Point2i Point2i;
  typedef cv::Point_<Dtype> Point2v;
  typedef cv::Rect Rect;
  typedef cv::Rect_<Dtype> Rectv;
  typedef cv::Vec<Dtype, 3> Vec3v;
  typedef cv::Mat_<cv::Vec<Dtype, 1> > Mat1v;
  typedef cv::Mat_<Vec3v> Mat3v;

  explicit ImageLabelTransformationLayer(const LayerParameter& param);

  virtual ~ImageLabelTransformationLayer() {}

  virtual void LayerSetUp(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
  virtual void Reshape(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);

  virtual inline const char* type() const { return "SegmentationTransformation"; }
  virtual inline int ExactNumBottomBlobs() const { return 2; }
  virtual inline int ExactNumTopBlobs() const { return 2; }

 protected:
  virtual void Forward_cpu(
      const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);

  virtual void Backward_cpu(
      const vector<Blob<Dtype>*>& top,
      const vector<bool>& propagate_down,
      const vector<Blob<Dtype>*>& bottom) {}

  void transform(
    const Mat3v& img,
    const Mat3v& label,
    Mat3v* img_aug,
    Mat3v* label_aug
);

  /**
   * @return a Dtype from [0..1].
   */
  Dtype randDouble();

  bool augmentation_flip(
    const Mat3v& img_src,
    Mat3v* img_aug,
    const Mat3v& label_src,
    Mat3v* label_aug
  );
  float augmentation_rotate(
      const Mat3v& img_src,
      Mat3v* img_aug,
      const Mat3v& label_src,
      Mat3v* label_aug);
  float augmentation_scale(
    const Mat3v& img_src,
    Mat3v* img_dst,
    const Mat3v& label_src,
    Mat3v* label_dst
);
  void transform_scale(
    const Mat3v& img,
    Mat3v* img_temp,
    const Mat3v& label,
    Mat3v* label_temp,
    const cv::Size& size
  );
  void transform_scale_adaptive(
    const Mat3v& img,
    Mat3v* img_temp,
    const Mat3v& label,
    Mat3v* label_temp,
    const cv::Size& size
  );
  Point2i augmentation_crop(
    const Mat3v& img_src,
    Mat3v* img_dst,
    const Mat3v& label_src,
    Mat3v* label_dst
);

  void transform_crop(
    const Mat3v& img_src,
    Mat3v* img_dst,
    const Mat3v& label_src,
    Mat3v* label_dst,
    Rect inner,
    Size2i dst_size,
    Point2i outer
) const;

  float augmentation_hueRotation(
      const Mat3v& img,
      Mat3v* result);

  float augmentation_desaturation(
      const Mat3v& img,
      Mat3v* result);

  float augmentation_contrast(
      const Mat3v& img,
      Mat3v* result);

  void clip_pixels(Mat3v& img, const Dtype min_val, const Dtype max_val);
  void check_labels(Mat3v& label, const Dtype min_val, const Dtype max_val);

  Mat1v getTransformationMatrix(Rect region, Dtype rotation) const;
  Rect getBoundingRect(Rect region, Dtype rotation) const;
  void matToBlob(const Mat3v& source, Dtype* destination) const;
  void matsToBlob(const vector<Mat3v>& source, Blob<Dtype>* destination) const;

  void matToRoundBlob1(const Mat3v& source, Dtype* destination) const;
  void matsToRoundBlob1(const vector<Mat3v>& source, Blob<Dtype>* destination) const;

  vector<Mat3v> blobToMats(const Blob<Dtype>& image) const;

  Mat3v dataToMat(
      const Dtype* _data,
      Size2i dimensions) const;

  vector<Mat3v> blobToMats1(const Blob<Dtype>& image) const;

  Mat3v dataToMat1(
      const Dtype* _data,
      Size2i dimensions) const;

  void retrieveMeanImage(Size2i dimensions = Size2i());
  void retrieveMeanChannels();

  void meanSubtract(Mat3v* source) const;
  void pixelMeanSubtraction(Mat3v* source) const;
  void channelMeanSubtraction(Mat3v* source) const;

  ImageLabelTransformationParameter a_param_;
  TransformationParameter t_param_;

  Phase phase_;

  Mat3v data_mean_;
  boost::array<Dtype, 3> mean_values_;
  shared_ptr<Caffe::RNG> rng_;
};

}  // namespace caffe

#endif /* IMAGE_LABEL_TRANSFORMATION_HPP */


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

#ifndef SOLVERS_SUPERLUSOLVER_HPP
#define SOLVERS_SUPERLUSOLVER_HPP

#include <Eigen/Dense>

#include <superlu_dist/superlu_ddefs.h>

#include <solvers/Solver.hpp>
#include <solvers/SparseSystem.hpp>

namespace solvers {

enum class SuperLUReorder {
    None = colperm_t::NATURAL,
    MinimumDegree = colperm_t::MMD_AT_PLUS_A,
    ColAMD = colperm_t::COLAMD,
    METIS = colperm_t::METIS_AT_PLUS_A,
};

/**
 * \brief SuperLUSolver Direct solver based on SuperLU_DIST library.
 */
class SuperLUSolver : public Solver {
private:
    /**
     * \brief _options Parameters of the resolution algorithm.
     */
    superlu_dist_options_t _options{};

public:
    explicit SuperLUSolver(SuperLUReorder reorder = SuperLUReorder::METIS);

    Eigen::VectorXd solve(const SparseSystem& system,
                          double& duration) const override;
};

}    // namespace solvers

#endif    // SOLVERS_SUPERLUSOLVER_HPP


/**
 * Copyright (c) 2015-2019 Przemysław Dobrowolski
 *
 * This file is part of the Configuration Space Library (libcs2), a library
 * for creating configuration spaces of various motion planning problems.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */
#include "meshing.h"
#include <boost/scoped_array.hpp>
#include <cmath>

namespace meshing
{
namespace // anonymous
{
typedef QPair<int, int> Coord2i;

int clampCoord(int x, int m)
{
    return (x + m) % m;
}

QVector3D projectSpin(struct cs2_spin3f_s &spin)
{
    return QVector3D(static_cast<float>(spin.s12 / (1.0 - spin.s0)),
                     static_cast<float>(spin.s23 / (1.0 - spin.s0)),
                     static_cast<float>(spin.s31 / (1.0 - spin.s0)));
}

double projectedDistance(const struct cs2_spin3f_s *a, const struct cs2_spin3f_s *b)
{
    struct cs2_vec3f_s pa, pb, pc;

    pa.x = a->s12 / (1.0 - a->s0);
    pa.y = a->s23 / (1.0 - a->s0);
    pa.z = a->s31 / (1.0 - a->s0);

    pb.x = b->s12 / (1.0 - b->s0);
    pb.y = b->s23 / (1.0 - b->s0);
    pb.z = b->s31 / (1.0 - b->s0);

    cs2_vec3f_sub(&pc, &pa, &pb);

    return cs2_vec3f_len(&pc);
}

void meshSurfaceAdaptiveStep(TriangleListPtr trianglesFront, TriangleListPtr trianglesBack, struct cs2_predgparam3f_s *param, double targetRadius, int component, double minU, double maxU, double minV, double maxV, int maxSubdivisions, int subdivision)
{
    struct cs2_spin3f_s sp00, sp01, sp10, sp11;

    cs2_predgparam3f_eval(&sp00, param, minU, minV, component);
    cs2_predgparam3f_eval(&sp01, param, minU, maxV, component);
    cs2_predgparam3f_eval(&sp10, param, maxU, minV, component);
    cs2_predgparam3f_eval(&sp11, param, maxU, maxV, component);

    if (subdivision == maxSubdivisions || (projectedDistance(&sp00, &sp01) <= targetRadius && projectedDistance(&sp00, &sp10) <= targetRadius && projectedDistance(&sp00, &sp11) <= targetRadius &&
                                           projectedDistance(&sp01, &sp10) <= targetRadius && projectedDistance(&sp01, &sp11) <= targetRadius && projectedDistance(&sp10, &sp11) <= targetRadius))
    {
        QVector3D v00 = projectSpin(sp00);
        QVector3D v01 = projectSpin(sp01);
        QVector3D v10 = projectSpin(sp10);
        QVector3D v11 = projectSpin(sp11);

        QVector3D nu = QVector3D::crossProduct(v00 - v11, v01 - v11).normalized();
        QVector3D nl = QVector3D::crossProduct(v10 - v11, v00 - v11).normalized();

        Triangle tu(v00, v01, v11,
                    nu, nu, nu,
                    nu, nu, nu);

        Triangle tl(v00, v11, v10,
                    nl, nl, nl,
                    nl, nl, nl);

        if (trianglesFront)
        {
            trianglesFront->push_back(tu);
            trianglesFront->push_back(tl);
        }

        if (trianglesBack)
        {
            trianglesBack->push_back(tu.flipped());
            trianglesBack->push_back(tl.flipped());
        }
    }
    else
    {
        meshSurfaceAdaptiveStep(trianglesFront, trianglesBack, param, targetRadius, component, minU, minU + 0.5 * (maxU - minU), minV, minV + 0.5 * (maxV - minV), maxSubdivisions, subdivision + 1);
        meshSurfaceAdaptiveStep(trianglesFront, trianglesBack, param, targetRadius, component, minU, minU + 0.5 * (maxU - minU), minV + 0.5 * (maxV - minV), maxV, maxSubdivisions, subdivision + 1);
        meshSurfaceAdaptiveStep(trianglesFront, trianglesBack, param, targetRadius, component, minU + 0.5 * (maxU - minU), maxU, minV, minV + 0.5 * (maxV - minV), maxSubdivisions, subdivision + 1);
        meshSurfaceAdaptiveStep(trianglesFront, trianglesBack, param, targetRadius, component, minU + 0.5 * (maxU - minU), maxU, minV + 0.5 * (maxV - minV), maxV, maxSubdivisions, subdivision + 1);
    }
}

void meshSurfaceSimplePatch(TriangleListPtr trianglesFront, TriangleListPtr trianglesBack, int c, Coord2i controls[4][4], int evalCacheSize, QVector3D *evalCache)
{
    // ----
    // |\ |
    // | \|
    // ----
    //
    // [UV]
    //
    // U V -
    // |
    //
    // 00 u 01 u 02 u 03
    //   l    l    l
    // 10 u 11 u 12 u 13
    //   l    l    l
    // 20 u 21 u 22 u 23
    //   l    l    l
    // 30   31   32   33
    QVector3D vp[4][4]; // (u, v)
    QVector3D vn[3][3][2]; // (u, v, [u, l])
    QVector3D vs[2][2]; // central (u, v)

    for (int u = 0; u < 4; ++u)
        for (int v = 0; v < 4; ++v)
            vp[u][v] = evalCache[evalCacheSize * evalCacheSize * c + evalCacheSize * controls[u][v].first + controls[u][v].second];

    for (int u = 0; u < 3; ++u)
        for (int v = 0; v < 3; ++v)
            vn[u][v][0] = QVector3D::crossProduct(vp[u][v] - vp[u + 1][v + 1], vp[u][v + 1] - vp[u + 1][v + 1]).normalized();

    for (int u = 0; u < 3; ++u)
        for (int v = 0; v < 3; ++v)
            vn[u][v][1] = QVector3D::crossProduct(vp[u + 1][v] - vp[u + 1][v + 1], vp[u][v] - vp[u + 1][v + 1]).normalized();

    vs[0][0] = (vn[1][1][0] + vn[1][1][1] + vn[0][1][1] + vn[1][0][0] + vn[0][0][0] + vn[0][0][1]).normalized();
    vs[0][1] = (vn[1][2][0] + vn[1][2][1] + vn[0][2][1] + vn[1][1][0] + vn[0][1][0] + vn[0][1][1]).normalized();
    vs[1][0] = (vn[2][1][0] + vn[2][1][1] + vn[1][1][1] + vn[2][0][0] + vn[1][0][0] + vn[1][0][1]).normalized();
    vs[1][1] = (vn[2][2][0] + vn[2][2][1] + vn[1][2][1] + vn[2][1][0] + vn[1][1][0] + vn[1][1][1]).normalized();

    Triangle tu(vp[1][1], vp[1][2], vp[2][2],
                vn[1][1][0], vn[1][1][0], vn[1][1][0],
                vs[0][0], vs[0][1], vs[1][1]);

    Triangle tl(vp[1][1], vp[2][2], vp[2][1],
                vn[1][1][1], vn[1][1][1], vn[1][1][1],
                vs[0][0], vs[1][1], vs[1][0]);

    if (trianglesFront)
    {
        trianglesFront->push_back(tu);
        trianglesFront->push_back(tl);
    }

    if (trianglesBack)
    {
        trianglesBack->push_back(tu.flipped());
        trianglesBack->push_back(tl.flipped());
    }
}

} // namespace anonymous

void meshCurveSimple(LineListPtr lines, cs2_predgparam3f_s *param, double radius)
{
    int numOfComponents = cs2_predgparamtype3f_domain_components(param->t);
    struct cs2_spin3f_s sp0, sp1;
    int c;
    double pu;

    for (c = 0; c < numOfComponents; ++c)
    {
        for (pu = 0.0; pu < 1.0; pu += radius)
        {
            cs2_predgparam3f_eval(&sp0, param, pu, 0, c);
            cs2_predgparam3f_eval(&sp1, param, fmod(pu + radius, 1.0), 0, c);

            QVector3D v0 = projectSpin(sp0);
            QVector3D v1 = projectSpin(sp1);

            Line l(v0, v1);

            lines->push_back(l);
        }
    }
}

void meshSurfaceAdaptive(TriangleListPtr trianglesFront, TriangleListPtr trianglesBack, struct cs2_predgparam3f_s *param, double initialRadius, double targetRadius, int maxSubdivisions)
{
    int componentCount = cs2_predgparamtype3f_domain_components(param->t);

    for (int component = 0; component < componentCount; ++component)
        for (double u = 0; u <= 1 - initialRadius; u += initialRadius)
            for (double v = 0; v <= 1 - initialRadius; v += initialRadius)
                meshSurfaceAdaptiveStep(trianglesFront, trianglesBack, param, targetRadius, component, u, u + initialRadius, v, v + initialRadius, maxSubdivisions, 0);
}

void meshSurfaceSimple(TriangleListPtr trianglesFront, TriangleListPtr trianglesBack, struct cs2_predgparam3f_s *param, double radius)
{
    int numOfComponents = cs2_predgparamtype3f_domain_components(param->t);
    Coord2i controls[4][4]; /* [u][v] */

    // calculate eval cache size
    int evalCacheSize = 0;

    for (double cs = 0.0; cs < 1.0; cs += radius)
        ++evalCacheSize;

    ++evalCacheSize;

    // compute eval cache
    boost::scoped_array<QVector3D> evalCache(new QVector3D[evalCacheSize * evalCacheSize * numOfComponents]); // (u, v, c)
    struct cs2_spin3f_s spin;
    int u, v, c;
    double pu, pv;

    for (c = 0; c < numOfComponents; ++c)
    {
        for (u = 0, pu = 0.0; pu < 1.0; pu += radius, ++u)
        {
            for (v = 0, pv = 0.0; pv < 1.0; pv += radius, ++v)
            {
                cs2_predgparam3f_eval(&spin, param, pu, pv, c);
                evalCache[evalCacheSize * evalCacheSize * c + evalCacheSize * u + v] = projectSpin(spin);
            }
        }

        for (u = 0, pu = 0.0; pu < 1.0; pu += radius, ++u)
        {
            cs2_predgparam3f_eval(&spin, param, pu, 1.0, c);
            evalCache[evalCacheSize * evalCacheSize * c + evalCacheSize * u + (evalCacheSize - 1)] = projectSpin(spin);
        }

        for (v = 0, pv = 0.0; pv < 1.0; pv += radius, ++v)
        {
            cs2_predgparam3f_eval(&spin, param, 1.0, pv, c);
            evalCache[evalCacheSize * evalCacheSize * c + evalCacheSize * (evalCacheSize - 1) + v] = projectSpin(spin);
        }

        cs2_predgparam3f_eval(&spin, param, 1.0, 1.0, c);
        evalCache[evalCacheSize * evalCacheSize * c + evalCacheSize * (evalCacheSize - 1) + evalCacheSize - 1] = projectSpin(spin);
    }

    // perform meshing
    for (int c = 0; c < numOfComponents; ++c)
    {
        for (int pu = 0; pu < evalCacheSize; ++pu)
        {
            for (int pv = 0; pv < evalCacheSize; ++pv)
            {
                for (int u = 0; u < 4; ++u)
                    for (int v = 0; v < 4; ++v)
                        controls[u][v] = Coord2i(clampCoord(pu + u - 1, evalCacheSize), clampCoord(pv + v - 1, evalCacheSize));

                meshSurfaceSimplePatch(trianglesFront, trianglesBack, c, controls, evalCacheSize, evalCache.get());
            }
        }
    }
}

} // namespace meshing


#include "FHE/NTL-Subs.h"
#include "Math/Setup.h"

#include "Math/gfpvar.h"
#include "Math/gf2n.h"

#include "FHE/P2Data.h"
#include "FHE/QGroup.h"
#include "FHE/NoiseBounds.h"

#include "Tools/mkpath.h"

#include "FHEOffline/Proof.h"

#include <fstream>
using namespace std;

#ifdef USE_NTL
#include <NTL/ZZ.h>
#include <NTL/ZZX.h>
#include <NTL/GF2EXFactoring.h>
#include <NTL/GF2XFactoring.h>
NTL_CLIENT
#endif

#include "FHEOffline/DataSetup.h"

void generate_setup(int n, int lgp, int lg2, int sec, bool skip_2,
    int slack, bool round_up)
{
  DataSetup setup;

  // do the full setup for SHE data
  Parameters(n, lgp, sec, slack, round_up).generate_setup(setup.setup_p.params,
      setup.setup_p.FieldD);
  if (!skip_2)
    Parameters(n, lg2, sec, slack, round_up).generate_setup(
        setup.setup_2.params, setup.setup_2.FieldD);
}

bool same_word_length(int l1, int l2)
{
  return l1 / 64 == l2 / 64;
}

template <>
int generate_semi_setup(int plaintext_length, int sec,
    FHE_Params& params, FFT_Data& FTD, bool round_up)
{
  int m = 1024;
  int lgp = plaintext_length;
  bigint p;
  generate_prime(p, lgp, m);
  int lgp0, lgp1;
  FHE_Params tmp_params;
  while (true)
    {
      tmp_params = params;
      SemiHomomorphicNoiseBounds nb(p, phi_N(m), 1, sec,
          numBits(NonInteractiveProof::slack(sec, phi_N(m))), true, tmp_params);
      bigint p1 = 2 * p * m, p0 = p;
      while (nb.min_p0(params.n_mults() > 0, p1) > p0)
        {
          p0 *= 2;
        }
      if (phi_N(m) < nb.min_phi_m(2 + numBits(p0 * (params.n_mults() > 0 ? p1 : 1)),
          params.get_R()))
        {
          m *= 2;
          generate_prime(p, lgp, m);
        }
      else
        {
          lgp0 = numBits(p0) + 1;
          lgp1 = numBits(p1) + 1;
          break;
        }
    }

  params = tmp_params;
  int extra_slack = common_semi_setup(params, m, p, lgp0, lgp1, round_up);

  FTD.init(params.get_ring(), p);
  gfp::init_field(p);
  return extra_slack;
}

template <>
int generate_semi_setup(int plaintext_length, int sec,
    FHE_Params& params, P2Data& P2D, bool round_up)
{
  if (params.n_mults() > 0)
    throw runtime_error("only implemented for 0-level BGV");
  gf2n_short::init_field(plaintext_length);
  int m;
  char_2_dimension(m, plaintext_length);
  SemiHomomorphicNoiseBounds nb(2, phi_N(m), 1, sec,
      numBits(NonInteractiveProof::slack(sec, phi_N(m))), true, params);
  int lgp0 = numBits(nb.min_p0(false, 0));
  int extra_slack = common_semi_setup(params, m, 2, lgp0, -1, round_up);
  load_or_generate(P2D, params.get_ring());
  return extra_slack;
}

int common_semi_setup(FHE_Params& params, int m, bigint p, int lgp0, int lgp1, bool round_up)
{
  cout << "Need ciphertext modulus of length " << lgp0;
  if (params.n_mults() > 0)
    cout << "+" << lgp1;
  cout << " and " << phi_N(m) << " slots" << endl;

  int extra_slack = 0;
  if (round_up)
    {
      int i;
      for (i = 0; i <= 20; i++)
        {
          if (SemiHomomorphicNoiseBounds::min_phi_m(lgp0 + i, params) > phi_N(m))
            break;
          if (not same_word_length(lgp0, lgp0 + i))
            break;
        }
      extra_slack = i - 1;
      lgp0 += extra_slack;
      cout << "Rounding up to " << lgp0 << ", giving extra slack of "
          << extra_slack << " bits" << endl;
    }

  Ring R;
  ::init(R, m);
  bigint p0, p1 = 1;
  if (params.n_mults() > 0)
  {
    generate_moduli(p0, p1, m, p, lgp0, lgp1);
    params.set(R, {p0, p1});
  }
  else
  {
    generate_modulus(p0, m, p, lgp0);
    params.set(R, {p0});
  }
  return extra_slack;
}

int finalize_lengths(int& lg2p0, int& lg2p1, int n, int m, int* lg2pi,
    bool round_up, FHE_Params& params)
{
  if (n >= 2 and n <= 10)
    cout << "Difference to suggestion for p0: " << lg2p0 - lg2pi[n - 2]
        << ", for p1: " << lg2p1 - lg2pi[9 + n - 2] << endl;
  cout << "p0 needs " << int(ceil(1. * lg2p0 / 64)) << " words" << endl;
  cout << "p1 needs " << int(ceil(1. * lg2p1 / 64)) << " words" << endl;

  int extra_slack = 0;
  if (round_up)
    {
      int i = 0;
      for (i = 0; i < 10; i++)
        {
          if (phi_N(m) < NoiseBounds::min_phi_m(lg2p0 + lg2p1 + 2 * i, params))
            break;
          if (not same_word_length(lg2p0 + i, lg2p0))
            break;
          if (not same_word_length(lg2p1 + i, lg2p1))
            break;
        }
      i--;
      extra_slack = 2 * i;
      lg2p0 += i;
      lg2p1 += i;
      cout << "Rounding up to " << lg2p0 << "+" << lg2p1
          << ", giving extra slack of " << extra_slack << " bits" << endl;
    }

  cout << "Total length: " << lg2p0 + lg2p1 << endl;

  return extra_slack;
}

/******************************************************************************
 * Here onwards needs NTL
 ******************************************************************************/

/*
 * Subroutine for creating the FHE parameters
 */
int Parameters::SPDZ_Data_Setup_Char_p_Sub(int idx, int& m, bigint& p,
    FHE_Params& params)
{
  int n = n_parties;
  int lg2pi[5][2][9]
             = {  {  {130,132,132,132,132,132,132,132,132},
                     {104,104,104,106,106,108,108,110,110} },
                  {  {196,196,196,196,198,198,198,198,198},
                     {136,138,140,142,140,140,140,142,142} },
                  {  {325,325,325,325,330,330,330,330,330},
                     {205,205,205,210,205,205,205,205,210} },
                  {  {580,585,585,585,585,585,585,585,585},
                     {330,330,330,335,335,335,335,335,335} },
                  {  {1095,1095,1095,1095,1095,1095,1095,1095,1095},
                     {590,590,590,590,590,595,595,595,595} }
               };

  int lg2p0 = 0, lg2p1 = 0;
  if (n >= 2 and n <= 10)
    {
      lg2p0=lg2pi[idx][0][n-2];
      lg2p1=lg2pi[idx][1][n-2];
    }
  else if (sec == -1)
    throw runtime_error("no precomputed parameters available");

  while (sec != -1)
    {
      double phi_m_bound =
              NoiseBounds(p, phi_N(m), n, sec, slack, params).optimize(lg2p0, lg2p1);
      cout << "Trying primes of length " << lg2p0 << " and " << lg2p1 << endl;
      if (phi_N(m) < phi_m_bound)
        {
          int old_m = m;
          m = 2 << int(ceil(log2(phi_m_bound)));
          cout << "m = " << old_m << " too small, increasing it to " << m << endl;
          generate_prime(p, numBits(p), m);
        }
      else
        break;
    }

  init(R,m);
  int extra_slack = finalize_lengths(lg2p0, lg2p1, n, m, lg2pi[idx][0],
      round_up, params);
  generate_moduli(pr0, pr1, m, p, lg2p0, lg2p1);
  return extra_slack;
}

void generate_moduli(bigint& pr0, bigint& pr1, const int m, const bigint p,
        const int lg2p0, const int lg2p1)
{
  generate_modulus(pr0, m, p, lg2p0, "0");
  generate_modulus(pr1, m, p, lg2p1, "1", pr0);
}

void generate_modulus(bigint& pr, const int m, const bigint p, const int lg2pr,
    const string& i, const bigint& pr0)
{
  if (lg2pr==0) { throw invalid_params(); }

  bigint step=m;
  bigint twop=1<<(numBits(m)+1);
  bigint gc=gcd(step,twop);
  step=step*twop/gc;

  step *= p;
  pr = (bigint(1) << lg2pr) / step * step + 1;

  while (pr == pr0 || !probPrime(pr))
    {
      pr -= step;
      assert(numBits(pr) == lg2pr);
    }

  cout << "\t pr" << i << " = " << pr << "  :   " << numBits(pr) <<  endl;

  assert(pr % m == 1);
  assert(pr % p == 1);
  assert(numBits(pr) == lg2pr);

  cout << "Minimal MAX_MOD_SZ = " << int(ceil(1. * lg2pr / 64)) << endl;
}

/*
 * Create the char p FHE parameters
 */
template <>
void Parameters::SPDZ_Data_Setup(FHE_Params& params, FFT_Data& FTD)
{
  bigint p;
  int idx, m;
  SPDZ_Data_Setup_Primes(p, plaintext_length, idx, m);
  SPDZ_Data_Setup_Char_p_Sub(idx, m, p, params);

  Zp_Data Zp(p);
  gfp::init_field(p);
  FTD.init(R,Zp);
}

#ifdef USE_NTL
/* Compute Phi(N) */
int phi_N(int N)
{
  int phiN=1,p,e;
  PrimeSeq s;
  while (N!=1)
    { p=s.next();
      e=0;
      while ((N%p)==0) { N=N/p; e++; }
      if (e!=0)
        { phiN=phiN*(p-1)*power_long(p,e-1); }
    }
  return phiN;
}

/* Compute mobius function (naive method as n is small) */
int mobius(int n)
{
  int p,e,arity=0;
  PrimeSeq s;
  while (n!=1)
    { p=s.next();
      e=0;
      while ((n%p)==0) { n=n/p; e++; }
      if (e>1) { return 0; }
      if (e!=0) { arity^=1; }
    }
  if (arity==0) { return 1; }
  return -1;
}

/* Compute cyclotomic polynomial */
ZZX Cyclotomic(int N)
{
  ZZX Num,Den,G,F;
  NTL::set(Num); NTL::set(Den);
  int m,d;

  for (d=1; d<=N; d++)
    { if ((N%d)==0)
         { clear(G);
           SetCoeff(G,N/d,1); SetCoeff(G,0,-1);
           m=mobius(d);
           if (m==1)       { Num*=G; }
           else if (m==-1) { Den*=G; }
         }
    }
  F=Num/Den;
  return F;
}
#else
int phi_N(int N)
{
  if (((N - 1) & N) != 0)
    throw runtime_error("compile with NTL support");
  else if (N == 1)
    return 1;
  else
    return N / 2;
}
#endif

void init(Ring& Rg, int m, bool generate_poly)
{
  Rg.mm=m;
  Rg.phim=phi_N(Rg.mm);

  Rg.pi.resize(Rg.phim);    Rg.pi_inv.resize(Rg.mm);
  for (int i=0; i<Rg.mm; i++) { Rg.pi_inv[i]=-1; }

  if (((m - 1) & m) == 0 and not generate_poly)
    {
      // m is power of two
      // no need to generate poly
      int k = 0;
      for (int i = 1; i < Rg.mm; i++)
        {
          // easy GCD
          if (i % 2 == 1)
            {
              Rg.pi[k] = i;
              Rg.pi_inv[i] = k;
              k++;
            }
        }
    }
  else
    {
#ifdef USE_NTL
      int k=0;
      for (int i=1; i<Rg.mm; i++)
        { if (gcd(i,Rg.mm)==1)
          { Rg.pi[k]=i;
          Rg.pi_inv[i]=k;
          k++;
          }
        }

      ZZX P=Cyclotomic(Rg.mm);
      Rg.poly.resize(Rg.phim+1);
      for (int i=0; i<Rg.phim+1; i++)
        { Rg.poly[i]=to_int(coeff(P,i)); }
#else
      throw runtime_error("compile with NTL support");
#endif
    }
}

#ifdef USE_NTL
// Computes a(b) mod c
GF2X Subs_Mod(const GF2X& a,const GF2X& b,const GF2X& c)
{
  GF2X ans,pb,bb=b%c;
  ans=to_GF2X(coeff(a,0));
  pb=bb;
  for (int i=1; i<=deg(a); i++)
    { ans=ans+pb*coeff(a,i);
      pb=MulMod(pb,bb,c);
    }
  return ans;
}

// Computes a(x^pow) mod c  where x^m=1
GF2X Subs_PowX_Mod(const GF2X& a,int pow,int m,const GF2X& c)
{
  GF2X ans; ans.SetMaxLength(m);
  for (int i=0; i<=deg(a); i++)
    { int j=MulMod(i,pow,m);
      if (IsOne(coeff(a,i))) { SetCoeff(ans,j,1); }
    }
  ans=ans%c;
  return ans;
}

void init(P2Data& P2D,const Ring& Rg)
{
  GF2X G,F;
  SetCoeff(G,gf2n_short::degree(),1);
  SetCoeff(G,0,1);
  for (int i=0; i<gf2n_short::get_nterms(); i++)
    { SetCoeff(G,gf2n_short::get_t(i),1); }
  //cout << "G = " << G << endl;

  for (int i=0; i<=Rg.phi_m(); i++)
    { if (((Rg.Phi()[i])%2)!=0)
        { SetCoeff(F,i,1); }
    }
  //cout << "F = " << F << endl;

  // seed randomness to achieve same result for all players
  // randomness is used in SFCanZass and FindRoot
  SetSeed(ZZ(0));

  // Now factor F modulo 2
  vec_GF2X facts=SFCanZass(F);

  // Check all is compatible
  int d=deg(facts[0]);
  if (d%deg(G)!=0)
    { throw invalid_params(); }

  // Compute the quotient group
  QGroup QGrp;
  int Gord=-1,e=Rg.phi_m()/d; // e = # of plaintext slots, phi(m)/degree
  int seed=1;
  while (Gord!=e)
    { QGrp.assign(Rg.m(),seed);       // QGrp encodes the the quotient group Z_m^*/<2>
      Gord=QGrp.order();
      if (Gord!=e) { cout << "Group order wrong, need to repeat the Haf-Mc algorithm" << endl; seed++; }
    }
  //cout << " l = " << Gord << " , d = " << d << endl;
  if ((Gord*gf2n_short::degree())!=Rg.phi_m())
    { cout << "Plaintext type requires Gord*gf2n_short::degree ==  phi_m" << endl;
      throw not_implemented();
    }

  vector<GF2X> Fi(Gord);
  vector<GF2X> Rts(Gord);
  vector<GF2X> u(Gord);
  /*
     Find map from Type 0 (mod G) -> Type 1 (mod Fi)
     for the first of the Fi's only
  */
  Fi[0]=facts[0];
  GF2E::init(facts[0]);     // work with the extension field GF_2[X]/Fi[0]
  GF2EX Ga=to_GF2EX(G);     // represent G as a polynomial over the extension field
  Rts[0]=rep(FindRoot(Ga)); // Find a roof of G in this field

  cout << "Fixing field ordering and the maps (Need to count to " << Gord << " here)\n\t";
  GF2E::init(G);
  GF2X g;
  vector<int> used(facts.length());
  for (int i=0; i<facts.length(); i++) { used[i]=0; }
  used[0]=1;
  for (int i=0; i<Gord; i++)
    { cout << i << " " << flush;
      if (i!=0)
        { int hpow=QGrp.nth_element(i);
          Rts[i]=Subs_PowX_Mod(Rts[0],hpow,Rg.m(),F);
          bool flag=false;
          for (int j=0; j<facts.length(); j++)
            { if (used[j]==0)
                { g=Subs_PowX_Mod(facts[0],hpow,Rg.m(),facts[j]);
                  if (IsZero(g))
                    { g=Subs_Mod(G,Rts[i],facts[j]);
                      if (!IsZero(g))
                        { cout << "Something wrong - G" << endl;
                          throw invalid_params();
                        }
                      Fi[i]=facts[j];
                      used[j]=1;
                      flag=true;
                      break;
                    }
                }
            }
          if (flag==false)
           { cout << "Something gone wrong" << endl;
             throw invalid_params();
           }
          Rts[i]=Rts[i]%Fi[i];
        }

     // Now sort out the projection map (for CRT reconstruction)
     GF2X te=(F/Fi[i]);
     GF2X tei=InvMod(te%Fi[i],Fi[i]);
     u[i]=MulMod(te,tei,F); // u[i] = \prod_{j!=i} F[j]*(F[j]^{-1} mod F[i])
   }
  cout << endl;

  // Make the forward matrix
  //   This is a deg(F) x (deg(G)*Gord)  matrix which maps elements
  //   vectors in the SIMD representation into plaintext vectors

  imatrix A;
  A.resize(Rg.phi_m(), imatrix::value_type(Gord*gf2n_short::degree()));
  P2D.A.resize(A[0].size());
  for (auto& x : P2D.A)
    x.resize(A.size());
  for (int slot=0; slot<Gord; slot++)
    { for (int co=0; co<gf2n_short::degree(); co++)
        { // Work out how x^co in given slot maps to plaintext vector
          GF2X av;
          SetCoeff(av,co,1);
          // av is mod G, now map to mod Fi
          av=Subs_Mod(av,Rts[slot],Fi[slot]);
          // Now need to map using CRT to the plaintext vector
          av=MulMod(av,u[slot],F);
          //cout << slot << " " << co << " : " << av << endl;
          for (int k=0; k<Rg.phi_m(); k++)
	    {
              int i = slot*gf2n_short::degree()+co;
              if (IsOne(coeff(av,k)))
                { A[k][i]=1; }
              else
                { A[k][i]=0; }
	      P2D.A[i][k] = A[k][i];
	    }
       }
    }
  //cout << "Forward Matrix : " << endl; print(P2D.A);

  // Find pseudo inverse modulo 2
  pinv(P2D.Ai, A);
  P2D.Ai.resize(Gord*gf2n_short::degree());

  //cout << "Inverse Matrix : " << endl; print(P2D.Ai);

  P2D.slots=Gord;

}
#else
void init(P2Data&, const Ring&)
{
  throw runtime_error("need to compile with 'USE_NTL=1' in 'CONFIG'");
}
#endif

/*
 * Create the FHE parameters
 */
void char_2_dimension(int& m, int& lg2)
{
  switch (lg2)
    { case -1:
        m=17;
        lg2=8;
        break;
      case 40:
        m=13175;
        break;
      case -40:
        m=5797;
        lg2=40;
        break;
      default:
        throw runtime_error("field size not supported");
        break;
    }
}

template <>
void Parameters::SPDZ_Data_Setup(FHE_Params& params, P2Data& P2D)
{
  int n = n_parties;
  int lg2 = plaintext_length;

  int lg2pi[2][9]
             = {  {70,70,70,70,70,70,70,70,70},
                  {70,75,75,75,75,80,80,80,80}
               };

  cout << "Setting up parameters\n";
  if ((n<2 || n>10) and sec == -1) { throw invalid_params(); }

  int m,lg2p0,lg2p1,ex;

  char_2_dimension(m, lg2);

  if (sec == -1)
  {
    lg2p0=lg2pi[0][n-2];
    lg2p1=lg2pi[1][n-2];
  }
  else
  {
    NoiseBounds(2, phi_N(m), n, sec, slack, params).optimize(lg2p0, lg2p1);
    finalize_lengths(lg2p0, lg2p1, n, m, lg2pi[0], round_up, params);
  }

  if (NoiseBounds::min_phi_m(lg2p0 + lg2p1, params) > phi_N(m))
    throw runtime_error("number of slots too small");

  cout << "m = " << m << endl;
  init(R,m);

  if (lg2p0==0 || lg2p1==0) { throw invalid_params(); }

  // We want pr0=pr1=1 mod (m*twop) where twop is the smallest
  // power of two bigger than 2*m. This means we have m'th roots of
  // unity and twop'th roots of unity. This means FFT's are easier
  // to implement
  int lg2m=numBits(m);
  bigint step=m<<(lg2m+1);

  ex=lg2p0-2*lg2m;
  pr0=1; pr0=(pr0<<ex)*step+1;
  while (!probPrime(pr0)) { pr0=pr0+step; }
  cout << "\t pr0 = " << pr0 << "  :   " << numBits(pr0) << endl;

  ex=lg2p1-2*lg2m;
  pr1=1; pr1=(pr1<<ex)*step+1;
  while (!probPrime(pr1) || pr1==pr0) { pr1=pr1+step; }
  cout << "\t pr1 = " << pr1 << "  :   " << numBits(pr1) <<  endl;

  cout << "\t\tFollowing should be both 1" << endl;
  cout << "\t\tpr1 mod m = " << pr1%m << endl;
  cout << "\t\tpr1 mod 2^lg2m = " << pr1%(1<<lg2m) << endl;

  gf2n_short::init_field(lg2);
  load_or_generate(P2D, R);
}

void load_or_generate(P2Data& P2D, const Ring& R)
{
  try
  {
      P2D.load(R);
  }
  catch (...)
  {
      cout << "Loading failed" << endl;
      init(P2D,R);
      P2D.store(R);
  }
}

#ifdef USE_NTL
/*
 * Create FHE parameters for a general plaintext modulus p
 *   Basically this is for general large primes only
 */
void SPDZ_Data_Setup_Char_p_General(Ring& R, PPData& PPD, bigint& pr0,
        bigint& pr1, int n, int sec, bigint& p, FHE_Params& params)
{
  cout << "Setting up parameters" << endl;

  int lgp=numBits(p);
  int mm,idx;

  // mm is the minimum value of m we will accept
  if (lgp<48)
    { mm=100;  // Test case
      idx=0;
    }
  else if (lgp <96)
    { mm=8192;
      idx=1;
    }
  else if (lgp<192)
    { mm=16384;
      idx=2;
    }
  else if (lgp<384)
    { mm=16384;
      idx=3;
    }
  else if (lgp<768)
    { mm=32768;
      idx=4;
    }
  else
    { throw invalid_params(); }

  // Now find the small factors of p-1 and their exponents
  bigint t=p-1;
  vector<long> primes(100),exp(100);

  PrimeSeq s;
  long pr;
  pr=s.next();
  int len=0;
  while (pr<2*mm)
    { int e=0;
      while ((t%pr)==0)
	{ e++;
	  t=t/pr;
        }
      if (e!=0)
	{ primes[len]=pr;
          exp[len]=e;
	  if (len!=0) { cout << " * "; }
          cout << pr << "^" << e << flush;
          len++;
        }
      pr=s.next();
    }
  cout << endl;

  // We want to find the best m which divides pr-1, such that
  //    - 2*m > phi(m) > mm
  //    - m has the smallest number of factors
  vector<int> ee;
  ee.resize(len);
  for (int i=0; i<len; i++) { ee[i]=0; }
  int min_hwt=-1,m=-1,bphi_m=-1,bmx=-1;
  bool flag=true;
  while (flag)
    { int cand_m=1,hwt=0,mx=0;
      for (int i=0; i<len; i++)
        { //cout << ee[i] << " ";
          if (ee[i]!=0)
            { hwt++;
              for (int j=0; j<ee[i]; j++)
                { cand_m*=primes[i]; }
              if (ee[i]>mx) { mx=ee[i]; }
            }
        }
      // Put "if" here to stop searching for things which will never work
      if (cand_m>1 && cand_m<4*mm)
        { //cout << " : " << cand_m << " : " << hwt << flush;
          int phim=phi_N(cand_m);
          //cout << " : " << phim << " : " << mm << endl;
          if (phim>mm && phim<3*mm)
	    { if (m==-1 || hwt<min_hwt || (hwt==min_hwt && mx<bmx) || (hwt==min_hwt && mx==bmx && phim<bphi_m))
	        { m=cand_m;
                  min_hwt=hwt;
                  bphi_m=phim;
                  bmx=mx;
                  //cout << "\t OK" << endl;
                }
            }
        }
      else
        { //cout << endl;
        }
      int i=0;
      ee[i]=ee[i]+1;
      while (ee[i]>exp[i] && flag)
	{ ee[i]=0;
          i++;
          if (i==len) { flag=false; i=0; }
          else        { ee[i]=ee[i]+1;   }
	}
    }
  if (m==-1)
    { throw bad_value(); }
  cout << "Chosen value of m=" << m << "\t\t phi(m)=" << bphi_m << " : " << min_hwt << " : " << bmx << endl;

  Parameters parameters(n, lgp, sec);
  parameters.SPDZ_Data_Setup_Char_p_Sub(idx,m,p,params);
  int mx=0;
  for (int i=0; i<R.phi_m(); i++)
    { if (mx<R.Phi()[i]) { mx=R.Phi()[i]; } }
  cout << "Max Coeff = " << mx << endl;

  init(R, m, true);

  Zp_Data Zp(p);
  PPD.init(R,Zp);
  gfp::init_field(p);

  pr0 = parameters.pr0;
  pr1 = parameters.pr1;
}
#endif


// System includes
#include <iostream>
#include <math.h>
#include <time.h>
#include <cstdlib>
#include <string>
#include <vector>
#include <algorithm>
#include <sys/types.h>
#include <sys/stat.h>
#include <boost/filesystem.hpp>
#include <boost/lambda/bind.hpp>

// OpenNN includes
#include <opennn.h>
#include "genetic_pool.hpp"

geneticPool::geneticPool() {
	srand(time(NULL));
	this->mutationRate = 0.05;
	this->crossoverRate = 0.05;
}

geneticPool::~geneticPool() {

}

Vector<double> geneticPool::mutate(Vector<double> gen) {
	for(int i = 0; i < gen.size(); i++) {
		double val = 1.0*rand()/RAND_MAX;

		if(val < this->mutationRate) {
			gen[i] += 2.0*rand()/RAND_MAX - 1.0;
		}
	}

	return gen;
}

Vector<double> geneticPool::crossover(Vector<double> a, Vector<double> b) {
	for(int i = 0; i < a.size(); i++) {
		double val = 1.0*rand()/RAND_MAX;

		if(val < this->crossoverRate) {
			a[i] = b[i];
		}
	}

	return a;
}

Gene geneticPool::get_testgen() {
	int num = rand()%this->test_pool.size();
	return test_pool[num];
}

Gene geneticPool::get_testgen(int num) {
	return test_pool[num];
}

Gene geneticPool::get_traingen(int num) {
	return train_pool[num];
}

std::vector<Gene> geneticPool::get_traingroup() {
	return this->train_pool;
}

std::vector<Gene> geneticPool::get_testgroup() {
	return this->test_pool;
}

void geneticPool::add_traingen(Vector<double> gen) {
	Gene buff;
	buff.id = train_pool.size();
	buff.fitness = 0;
	buff.gen = gen;
	this->train_pool.push_back(buff);
}

void geneticPool::add_testgen(Vector<double> gen) {
	Gene buff;
	buff.id = train_pool.size();
	buff.fitness = 0;
	buff.gen = gen;
	this->test_pool.push_back(buff);
}

void geneticPool::set_traingen(std::vector< Vector<double> > genlist) {
	std::vector<Gene> lst;
	Gene buff;

	for(int i = 0;i < genlist.size(); i++) {
		buff.gen = genlist[i];
		lst.push_back(buff);
	}
	this->train_pool = lst;
}

void geneticPool::set_testgen(std::vector< Vector<double> > genlist) {
	std::vector<Gene> lst;
	Gene buff;

	for(int i = 0;i < genlist.size(); i++) {
		buff.gen = genlist[i];
		lst.push_back(buff);
	}
	this->test_pool = lst;
}

void geneticPool::set_traingen(std::vector<Gene> genlist) {
	this->train_pool = genlist;
}

void geneticPool::set_testgen(std::vector<Gene> genlist) {
	this->test_pool = genlist;
}

void geneticPool::set_trainfitness(int id, double fitness) {
	this->train_pool[id].fitness = fitness;
}

void geneticPool::set_testfitness(int id, double fitness) {
	this->test_pool[id].fitness = fitness;
}

void geneticPool::next_gen() {
	std::vector<Gene> new_generation;
	std::vector<Gene> new_tests;
	Vector<double> buffgene;
	Gene buff;
	std::vector<Gene> old_tests = this->test_pool;
	std::vector<Gene> old_train = this->train_pool;

	buff.fitness = -INF;

	// order the lists by the fitness
	std::sort(old_train.begin(), old_train.end(), this->sortbyfitness);
	std::sort(old_tests.begin(), old_tests.end(), this->sortbyfitness);
	int gen_size = this->train_pool.size();

	for(int i = 0; i < gen_size/2; i++) {
		buff.gen = old_train[i].gen;
		buff.id = i;
		buff.fitness = old_train[i].fitness;
		new_generation.push_back(buff);
		for(int j = 0; j < gen_size; j++) {
			if(buff.gen == this->train_pool[j].gen) {
				std::cout << "\nGEN USED: " << j << "\n\n";
			}
		}
	}

	for (int i = 0; i < gen_size/2; i++) {
		int val = rand()%(gen_size/2);

		buff.gen = this->crossover(old_train[i].gen, old_train[val].gen);
		buff.gen = this->mutate(buff.gen);
		buff.id = gen_size/2 + i;
		buff.fitness = -INF;
		new_generation.push_back(buff);
	}

	// make the old generation become substituted by the trainned one
	int j = 0, k = 0;

	for(int i = 0; i < gen_size; i++) {
		if(old_tests[j].fitness > old_train[k].fitness) {
			new_tests.push_back(old_tests[j]);
			j++;
		}
		else {
			new_tests.push_back(old_train[k]);
			k++;
		}
	}

	for(int i = 0; i < this->test_pool.size(); i++) {
		std::cout << this->test_pool[i].fitness << " ";
	}
	std::cout << "\n";

	for(int i = 0; i < this->train_pool.size(); i++) {
		std::cout << this->train_pool[i].fitness << " ";
	}
	std::cout << "\n\n";

	this->test_pool = new_tests;
	this->train_pool = new_generation;
}

bool geneticPool::exist_dir(std::string pathname) {
	struct stat info;

	if( stat( pathname.c_str(), &info ) != 0 ){
	    // printf( "cannot access %s\n", pathname );
	    return 0;
	}
	else if( info.st_mode & S_IFDIR ) {  // S_ISDIR() doesn't exist on my windows
	    // printf( "%s is a directory\n", pathname );
	    return 1;
	}
	else {
	    // printf( "%s is no directory\n", pathname );
	    return 0;
	}
}

void geneticPool::create_dir(std::string pathname) {
	std::string mk = "mkdir " + pathname;
	system(mk.c_str());
}

void geneticPool::save_folder(std::string folder, Vector<int> network_model) {

	std::string train_path = folder + "/train/";
	std::string test_path = folder + "/test/";

	if(!this->exist_dir(folder)) {
		this->create_dir(folder);
	}

	if(!this->exist_dir(train_path)) {
		this->create_dir(train_path);
	}

	if(!this->exist_dir(test_path)) {
		this->create_dir(test_path);
	}

	MultilayerPerceptron network(network_model);

	for(int i = 0; i < this->train_pool.size(); i++) {
		network.set_parameters(this->train_pool[i].gen);
		std::string path = train_path + std::to_string(i) + ".xml";

		tinyxml2::XMLDocument* doc = network.to_XML();
    	tinyxml2::XMLError eResult = doc->SaveFile(path.c_str());
	}

	for(int i = 0; i < this->test_pool.size(); i++) {
		network.set_parameters(this->test_pool[i].gen);
		std::string path = test_path + std::to_string(i) + ".xml";

		tinyxml2::XMLDocument* doc = network.to_XML();
    	tinyxml2::XMLError eResult = doc->SaveFile(path.c_str());
	}
}

void geneticPool::load_folder(std::string folder, Vector<int> network_model) {

	std::string train_path = folder + "/train/";
	std::string test_path = folder + "/test/";

	if(!this->exist_dir(folder)) {
		throw(1);
	}

	if(!this->exist_dir(train_path)) {
		throw(2);
	}

	if(!this->exist_dir(test_path)) {
		throw(3);
	}

	MultilayerPerceptron network(network_model);
	Gene buff_gen;

	int train_num = this->count_files(train_path);
	int test_num = this->count_files(test_path);

	for(int i = 0; i < train_num; i++) {
		std::string path = train_path + std::to_string(i) + ".xml";

		tinyxml2::XMLDocument n_doc;
    	n_doc.LoadFile(path.c_str());
    	network.from_XML(n_doc);

    	buff_gen.fitness = -INF;
    	buff_gen.gen = network.arrange_parameters();
    	this->train_pool.push_back(buff_gen);
	}

	for(int i = 0; i < test_num; i++) {
		std::string path = test_path + std::to_string(i) + ".xml";

		tinyxml2::XMLDocument n_doc;
    	n_doc.LoadFile(path.c_str());
    	network.from_XML(n_doc);

    	buff_gen.fitness = -INF;
    	buff_gen.gen = network.arrange_parameters();
    	this->test_pool.push_back(buff_gen);
	}
}

void geneticPool::load_folder(const char* folder, Vector<int> network_model) {
	std::string pathname(folder);
	this->load_folder(pathname, network_model);
}

void geneticPool::save_folder(const char* folder, Vector<int> network_model) {
	std::string pathname(folder);
	this->save_folder(pathname, network_model);
}

void geneticPool::load_folder(char* folder, Vector<int> network_model) {
	std::string pathname(folder);
	this->load_folder(pathname, network_model);
}

void geneticPool::save_folder(char* folder, Vector<int> network_model) {
	std::string pathname(folder);
	this->save_folder(pathname, network_model);
}

void geneticPool::set_mutationrate(double rate) {
	this->mutationRate = rate;
}

void geneticPool::set_crossoverrate(double rate) {
	this->crossoverRate = rate;
}

bool geneticPool::sortbyfitness(const Gene &a, const Gene &b) {
	return (a.fitness > b.fitness); // order descending
}

std::vector<Gene> geneticPool::random_gens(int gen_num, Vector<int> network_model) {
	MultilayerPerceptron network;
	Vector<double> gen;
	std::vector<Gene> lst;
	Gene buff;

	for(int i = 0; i < gen_num; i++) {
		network.set(network_model);
		gen = network.arrange_parameters();

		buff.gen = gen;
		buff.fitness = -INF;
		buff.id = 0;
		lst.push_back(buff);
	}

	return lst;
}

int geneticPool::count_files(std::string pathname) {
  using namespace boost::filesystem;
  using namespace boost::lambda;

  path the_path(pathname);

  int cnt = std::count_if(
      directory_iterator(the_path),
      directory_iterator(),
      static_cast<bool(*)(const path&)>(is_regular_file) );

  return cnt;
}


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TRIGONOMETRIC_CONSTANTS_MEDIUM_PI_HPP_INCLUDED
#define NT2_TRIGONOMETRIC_CONSTANTS_MEDIUM_PI_HPP_INCLUDED

#include <nt2/include/functor.hpp>
#include <boost/simd/constant/hierarchy.hpp>
#include <boost/simd/constant/register.hpp>
namespace nt2
{
  namespace tag
  {
   /*!
     @brief Medium_pi generic tag

     Represents the Medium_pi constant in generic contexts.

     @par Models:
        Hierarchy
   **/
    BOOST_SIMD_CONSTANT_REGISTER( Medium_pi, double
                                , 201, 0x43490fdb       //2^6/pi
                                , 0x412921fb54442d18ll  //2^{18}/pi
                                )
  }
  namespace ext
  {
   template<class Site, class... Ts>
   BOOST_FORCEINLINE generic_dispatcher<tag::Medium_pi, Site> dispatching_Medium_pi(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
   {
     return generic_dispatcher<tag::Medium_pi, Site>();
   }
   template<class... Args>
   struct impl_Medium_pi;
  }
  /*!
    Constant used in trigonometric reductions

    @par Semantic:

    For type T0:

    @code
    T0 r = Medium_pi<T0>();
    @endcode

    is similar to:

    @code
    if T is double
      r = Pi<T0>()*pow2(18);
    else if T is float
      r = Pi<T0>()*pow2(6);
    else
      r =  201
    @endcode

    @return a value of type T0
  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(tag::Medium_pi, Medium_pi);
}

#endif


#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_traits.hpp>

namespace configure {

	// Boost.Graph used to store dependencies
	typedef boost::adjacency_list<
		  boost::vecS
		, boost::vecS
		, boost::bidirectionalS
		, boost::property<
			  boost::vertex_color_t
			, boost::default_color_type
		>
	> Graph;

	// Graph traits
	typedef boost::graph_traits<Graph> GraphTraits;

	// Vertex type
	typedef GraphTraits::vertex_descriptor Vertex;

	// Edge type
	typedef GraphTraits::edge_descriptor Edge;

}


#include "TreeSection.h"

#include <boost/range/adaptors.hpp>
#include <cmath>

#include "Pattern.h"
#include "TreeLine.h"

TreeSection::TreeSection(int offset, int maxWidth, int start) {
  for (int i = start; i < ceil(maxWidth / 2) + 1; ++i) {
    int treeItemCount = i * 2 + 1;
    _lines.push_back(
        std::make_unique<TreeLine>(offset, maxWidth, treeItemCount));
  }
}

TreeSection::TreeSection(const TreeSection &other) {
  _lines.reserve(other._lines.size());
  for (const auto &it : other._lines) {
    _lines.push_back(it->clone());
  }
}

TreeSection &TreeSection::operator=(const TreeSection &other) { return *this; }

void TreeSection::print() {
  for (const auto &it : _lines) {
    it->print();
  }
}

int TreeSection::overall(const std::vector<std::unique_ptr<Symbol>> &items) {
  int sum = 0;
  for (const auto &it : _lines) {
    sum += it->overall(items);
  }
  return sum;
}

void TreeSection::decorate(int count) {
  int sum = overall(Pattern::tree());
  int tempCount = count;
  for (const auto &it : boost::adaptors::reverse(_lines)) {
    int linesSum = it->overall(Pattern::tree());
    float ratio = static_cast<float>(linesSum) / sum;
    int joyToSection = ceil(ratio * count);
    if (joyToSection > tempCount) {
      joyToSection = tempCount;
    }
    tempCount -= joyToSection;
    it->decorate(joyToSection);
  }
}

std::unique_ptr<Item> TreeSection::clone() {
  return std::make_unique<TreeSection>(*this);
}


// Boost.Geometry

// Copyright (c) 2021, Oracle and/or its affiliates.

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

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_STRATEGY_CARTESIAN_AREA_BOX_HPP
#define BOOST_GEOMETRY_STRATEGY_CARTESIAN_AREA_BOX_HPP

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/strategy/area.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace area
{

template
<
    typename CalculationType = void
>
class cartesian_box
{
public:
    template <typename Box>
    struct result_type
        : strategy::area::detail::result_type
            <
                Box,
                CalculationType
            >
    {};

    template <typename Box>
    static inline auto apply(Box const& box)
    {
        typedef typename result_type<Box>::type return_type;

        return return_type(get<max_corner, 0>(box) - get<min_corner, 0>(box))
             * return_type(get<max_corner, 1>(box) - get<min_corner, 1>(box));
    }
};

}} // namespace strategy::area

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGY_CARTESIAN_AREA_BOX_HPP


#pragma once

/************************************************************************/
/* namespace std                                                        */
/************************************************************************/
#include "Common.hpp"

/************************************************************************/
/* namespace pcl                                                        */
/************************************************************************/
#include <Eigen/Core>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/console/print.h>

#include <pcl/common/time.h>
#include <pcl/common/transforms.h>
#include <pcl/features/principal_curvatures.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/fpfh_omp.h>

#include <pcl/visualization/pcl_visualizer.h>

using Eigen::Matrix4f;

enum METHOD {
	ICP_CLASSIC,
	ICP_NOLINEAR,
	ICP_WITHNORMLS,
	RANSACPLUSICP,
	SAC_IA
};

enum FEATYPE {
	FPFH,
	PFH,
	VFH,
	SIFT
};

struct RegisterParameter {
	//Downsample
	float leaf = 0.01f;
	//RANSAC
	int MaximumIterationsRANSAC = 50000; // Number of RANSAC iterations
	int NumberOfSamples = 5; // Number of points to sample for generating/prerejecting a pose
	int CorrespondenceRandomness = 5; // Number of nearest features to use
	float SimilarityThreshold = 0.7f; // Polygonal edge length similarity threshold
	float MaxCorrespondence = 2.5f; // Inlier threshold
	float InlierFraction = 0.2f;
	//ICP
	double EuclideanEpsilon = 2e-8;
	int MaximumIterationsICP = 1000;
	//Method
	METHOD Method = ICP_NOLINEAR;
	FEATYPE FeaType;
};

/************************************************************************/
/* Typedef                                                              */
/************************************************************************/
typedef pcl::PointNormal PointNT;
typedef pcl::PointCloud<PointNT> PointCloudNT;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;
typedef pcl::FPFHSignature33 FeatureT;

typedef pcl::NormalEstimationOMP<PointNT, PointNT> NormalEstimationNT;
typedef pcl::FPFHEstimationOMP<PointNT, PointNT, FeatureT> FeatureEstimationT;
typedef pcl::PointCloud<FeatureT> FeatureCloudT;
typedef pcl::visualization::PointCloudColorHandlerCustom<PointNT> ColorHandlerNT;

/************************************************************************/
/* Output                                                               */
/************************************************************************/
using pcl::console::print_info;
using pcl::console::print_warn;
using pcl::console::print_error;
using pcl::console::print_value;
using pcl::console::print_debug;
using pcl::console::print_highlight;

/************************************************************************/
/* Load model and mesh                                                  */
/************************************************************************/
/**
*  @brief LoadModel: load .pcd file to program, either model or mesh
*  @param model_path   .pcd filepath
*  @param model        load PointCloudT/PointCloudNT to memory
*/
bool  LoadModel(const string model_path, PointCloudT::Ptr &model); //XYZ
bool  LoadModel(const string model_path, PointCloudNT::Ptr &model); //Normal

/************************************************************************/
/* Load grasping region point cloud                                     */
/************************************************************************/
bool LoadGraspPcd(const string model_path, PointCloudT::Ptr &grasp);
bool LoadGrasp(const string model_path, PointCloudT::Ptr &grasp);

/************************************************************************/
/* Downsample model point cloud                                         */
/************************************************************************/
void Downsample(PointCloudNT::Ptr &model, float leaf);

/************************************************************************/
/* Estimate model curvatures                                            */
/************************************************************************/
void EstimateCurvatures(PointCloudNT::Ptr &model, float radius);

/************************************************************************/
/* Estimate FPFH features                                               */
/************************************************************************/
void EstimateFPFH(PointCloudNT::Ptr &model, FeatureCloudT::Ptr &model_features, float leaf);

/************************************************************************/
/* Output Transformation Matrix                                         */
/************************************************************************/
void Print4x4Matrix(const Matrix4f & matrix);

/************************************************************************/
/* Registration with RANSAC and ICP                                     */
/************************************************************************/
/**
*  @brief Registration: register model and mesh with RANSAC+ICP, out Transformation matrix
*  @param model        input 3D points cloud
*  @param mesh         generated by Depth camera and convert to Point Cloud Normal Point
*  @param model_align  output aligned model, which used to reflect to 2D
*  @param para		   register parameter
*  @param showGraphic  show graphic result or not
*/
Matrix4f Registration(	PointCloudNT::Ptr &model,
						PointCloudNT::Ptr &mesh,
						PointCloudNT::Ptr &model_align,
						RegisterParameter &para,
						bool showGraphic = true	);

/**
*  @brief RegistrationNoShow: register model and mesh with RANSAC+ICP, out Transformation matrix, but not show 3D Points Cloud
*  @param model        input 3D points cloud
*  @param mesh         generated by Depth camera and convert to Point Cloud Normal Point
*  @param model_align  output aligned model, which used to reflect to 2D
*  @param para		   register parameter
*/
Matrix4f RegistrationNoShow(	PointCloudNT::Ptr &model,
								PointCloudNT::Ptr &mesh,
								PointCloudNT::Ptr &model_align,
								RegisterParameter &para );
/**
*  @brief RegistrationNoShow_ICP: register model and mesh with ICP, ICP-WITHNORMAL, ICP-NONLINEAR, out Transformation matrix
*  @param model        input 3D points cloud
*  @param mesh         generated by Depth camera and convert to Point Cloud Normal Point
*  @param model_align  output aligned model, which used to reflect to 2D
*  @param para		   register parameter
*/
Matrix4f RegistrationNoShow_ICP(	PointCloudNT::Ptr &model,
									PointCloudNT::Ptr &mesh,
									PointCloudNT::Ptr &model_align,
									RegisterParameter &para);


#include "prod_test.hpp"
#include <boost/numeric/ublas/matrix.hpp>

int main()
{

	///testing row major flaot prod
	bench_prod<float, ublas::basic_row_major<>, 10, 10> b1;

	///testing row major complex float prod
	bench_prod<std::complex<float>, ublas::basic_row_major<>, 10, 10> b2;

	///testing row major double prod
	bench_prod<double, ublas::basic_row_major<>, 10, 10> b3;

	///testing row major complex float prod
	bench_prod<std::complex<double>, ublas::basic_row_major<>, 10, 10> b4;

	///testing column major flaot prod
	bench_prod<float, ublas::basic_column_major<>, 10, 10> b5;

	///testing column major complex float prod
	bench_prod<std::complex<float>, ublas::basic_column_major<>, 10, 10> b6;

	///testing column major double prod
	bench_prod<double, ublas::basic_column_major<>, 10, 10> b7;

	///testing column major complex double prod
	bench_prod<std::complex<double>, ublas::basic_column_major<>, 10, 10> b8;

	std::cout << "Row major:" << std::endl;
	b1.run();
	b2.run();
	b3.run();
	b4.run();

	std::cout << "Column major:" << std::endl;
	b5.run();
	b6.run();
	b7.run();
	b8.run();

		return 0;

}


//
// Copyright 2005-2007 Adobe Systems Incorporated
//
// 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
//
#include <boost/gil.hpp>
#include <boost/gil/extension/io/jpeg.hpp>

#include <algorithm>
#include <fstream>

// Example file to demonstrate a way to compute histogram

using namespace boost::gil;

template <typename GrayView, typename R>
void gray_image_hist(const GrayView& img_view, R& hist) {
//    for_each_pixel(img_view,++lambda::var(hist)[lambda::_1]);
    for (typename GrayView::iterator it=img_view.begin(); it!=img_view.end(); ++it)
        ++hist[*it];
}

template <typename V, typename R>
void get_hist(const V& img_view, R& hist) {
    gray_image_hist(color_converted_view<gray8_pixel_t>(img_view), hist);
}

int main() {
    rgb8_image_t img;
    read_image("test.jpg", img, jpeg_tag());

    int histogram[256];
    std::fill(histogram,histogram + 256, 0);
    get_hist(const_view(img), histogram);

    std::fstream histo_file("out-histogram.txt", std::ios::out);
    for(std::size_t ii = 0; ii < 256; ++ii)
        histo_file << histogram[ii] << std::endl;
    histo_file.close();

    return 0;
}