#include <iostream>
#include <fstream>
#include <boost/dynamic_bitset.hpp>
#include <bitset>
#include <random>
#include <time.h>
#include "src/dtl/bitmap/teb_wrapper.hpp"
#include "src/dtl/bitmap/teb_flat.hpp"
#include "src/dtl/bitmap/util/convert.hpp"

#include "src/dtl/bitmap/teb_types.hpp"
#include "src/dtl/bitmap/util/bitmap_fun.hpp"

dtl::teb_wrapper* teb_reconstruct;
dtl::teb_wrapper* teb_traverse;

using word_type = dtl::teb_word_type;
using bitmap_funcs = dtl::bitmap_fun<word_type>;
const size_t word_size = sizeof(word_type);

boost::dynamic_bitset<u_int32_t> generate_bitset(unsigned int bitset_size = 32, unsigned int inverse_bit_density = 4) {
    boost::dynamic_bitset<u_int32_t> initial_bitset(bitset_size);
    for (unsigned int i = 0; i < bitset_size; i++) {
        if (!(rand() % inverse_bit_density)) {
            initial_bitset.set(i);
        }
    }
    return initial_bitset;
}

boost::dynamic_bitset<u_int32_t> generate_bitset_bernoulli(unsigned int bitset_size = 32, std::size_t seed = 1u, double bit_density = 0.25) {
    boost::dynamic_bitset<u_int32_t> initial_bitset(bitset_size);
    std::default_random_engine generator;
    generator.seed(seed);
    std::bernoulli_distribution bernoulli(bit_density);
    for (unsigned int i = 0; i < bitset_size; i++) {
        if (bernoulli(generator)) {
            initial_bitset.set(i);
        }
    }
    return initial_bitset;
}

bool bitset_equals(boost::dynamic_bitset<u_int32_t> bitset1, boost::dynamic_bitset<u_int32_t> bitset2) {
    assert(bitset1.size() == bitset2.size());
    std::cout << std::endl;
    for (std::size_t i = 0; i < bitset1.size(); i++) {
        if (bitset1[i] != bitset2[i]) {
            std::cout << "\nBE: Unequal bit at position " << i << "\n";
            std::cout << "Bitset1: " << bitset1[i] << ", Bitset2: " << bitset2[i] << "\n\n";
            return false;
        } else {
            //std::cout << bitset1[i];
        }
    }
    std::cout << std::endl;
    return true;
}

bool bitmap_equals_teb(boost::dynamic_bitset<u_int32_t> bitset, dtl::teb_wrapper *teb) {
    assert(bitset.size() == teb->teb_->n_actual_);
    bool equal = true;
    for (std::size_t i = 0; i < bitset.size(); i++) {
        if (bitset[i] != teb->test(i)) {
            std::cout << "\nBeT: Unequal bit at position " << i << "\n";
            std::cout << "Bitset: " << bitset[i] << ", TEB: " << teb->test(i) << "\n\n";
            std::cout.flush();
            equal = false;
        }
    }
    return equal;
}

struct update_task {
    std::size_t pos;
    bool val;
};

namespace std {
    void swap(update_task &task1, update_task &task2) {
        std::size_t pos_temp = task1.pos;
        bool pos_val = task1.val;

        task1.pos = task2.pos;
        task1.val = task2.val;

        task2.pos = pos_temp;
        task2.val = pos_val;
    }
}

struct benchmark_task {
    boost::dynamic_bitset<u_int32_t> bitset;
    std::size_t prune_trigger;
    std::vector<update_task> mixed_updates;
    std::vector<update_task> all_zeroes;
    std::vector<update_task> all_ones;
    std::size_t rep_count;
    std::vector<update_task> run_breaking;
};

struct benchmark_result {
    u1 teb_equals_bitset;
    u1 decompressed_equals_bitset;
    std::size_t prune_count;
};

enum Update_Type {mixed, ones, zeroes};


/******************************************************************************
 * main.cpp: Main Process.
 *
 * Author: Carlos Eduardo de Andrade <andrade@ic.unicamp.br>
 *
 * (c) Copyright 2013 Institute of Computing, University of Campinas.
 *     All Rights Reserved.
 *
 *  Created on : Jun 24, 2011 by andrade
 *  Last update: Jul 02, 2020 by luishpmendes
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *****************************************************************************/

#include "brkga_mp_ipr.hpp"
#include "timer.hpp"
#include "combinatorial_auction.hpp"
#include "combinatorial_auction_decoder.hpp"

#include <iostream>
#include <fstream>
#include <iomanip>
#include <stdexcept>
#include <iterator>

#pragma GCC diagnostic ignored "-Weffc++"
#include <boost/lexical_cast.hpp>
#pragma GCC diagnostic pop

using namespace std;
using namespace BRKGA;

//------------------------[ Some control constants ]-------------------------//

// Controls stop criteria
enum StopRule{GENERATIONS = 'G', TARGET = 'T', IMPROVEMENT = 'I'};

// Initial population criteria
enum InitApproach{RAMDOM = 'R', LPRELAXATION = 'L'};

//-------------------------------[ Main ]------------------------------------//

int main(int argc, char* argv[])
{
    // First read parameters from command line:
    const int num_args = 15;

    #ifndef TUNING
    if(argc < num_args) {
    #else
    if(argc < num_args + 15) {
    #endif
        cout << "usage: " << argv[0]
             << " <config-file> <seed> <stop-rule> <stop-arg> <decode-approach>"
             << " <initial-population> <init-percentage> <init-max-time> <init-cplex-time> <init-cplex-iters>"
             << " <max-time> <pricing-scheme> <instance-file> <instance-lp-file>"
            #ifdef TUNING
             << " <brkga_params.population_size> <elite_percentage> <mutants_percentage>"
                " <num_elite_parents> <total_parents> <bias>"
                " <num_independent_populations>"
                " <pr_minimum_distance> <pr_type> <pr_selection>"
                " <alpha_block_size> <pr_percentage> <exchange_interval>"
                " <num_exchange_individuals> <reset_interval>"
            #endif
             << "\nwhere: "
             << "\n - <config-file>: parameters of BRKGA algorithm"
             << "\n - <seed>: seed for random generator"
             << "\n - <stop-rule> <stop-arg>: stop rule and its arguments where"
             << "\n\t+ Generations <number_generations>: the algorithm runs until <number_generations>"
             << "\n\t+ Target <value of expected target>: runs until obtains the target value"
             << "\n\t+ Iterations <max generations without improvement>: runs until the solutions don't improve by max generations"
             << "\n - <decode-approach>: the approach used to make solutions:"
             << "\n\t+ Lazy: sort the bids in non-increasing order of they genes and in the following order"
             << "\n\t+ Greedy: sort the bids in non-increasing order of utility and choose the items in this order"
             << "\n\t+ Pirkul: sort the bids in non-increasing order of pseudo-utility(dual multipliers) and choose the items in this order"
             << "\n - <initial-population> <init-percentage> <init-iterations>: indicates how the algorithm must start the initial population"
             << "\n\t+ Random 0 0 0 0: generate all individuals randomly (the 0 has no effect)"
             << "\n\t+ LP <init-percentage> <init-max-time> <init-cplex-time> <init-cplex-iters>: use population-size <init-percentage> LP "
             << "\n\t  relaxations as start individuals (if not generate all, complete with random ones). Each call to Cplex is limited"
             << "\n\t  to <init-cplex-time>, that can uses <init-cplex-iters> tries to tight the model. This procedure are limited to"
             << "\n\t  <init-max-time> seconds. The algorithm uses <init-iterations> to tight the formulation and to generate each relaxation."
             << "\n - <max-time>: max running time (in seconds). If 0, the algorithm stops on chosen stop rule."
             << "\n - <pricing-scheme>: Princing scheme to the auction. Possible values are:"
             << "\n\t+ First: to first price scheme"
             << "\n\t+ Second: to second price scheme"
             << "\n - <instance-file>: instance file"
             << "\n - [instance-lp-file]: instance LP file (here, any format readable by Cplex). Mandatory only if initial population to be LP relaxations"
             << "\n - <path-relink-type>: defines the distance function and path-relink approach"
             << "\n\t+ Direct: uses the direct path-relink and the Hamming distance"
             << "\n\t+ Permutation: uses the permutation based path-relink and the Kendall Tau distance"
             << "\n - <mininum-distance>: mininum distance between solution to path-relink"
             << "\n\n ALL PARAMETERS ARE MANDATORY\n"
             << endl;
        return 64;  // BSD usage error code
    }

    // Loading parameters from command line
    const string config_file(argv[1]);      // config file
    unsigned long seed;                     // RNG seed
    char stop_rule;                         // stopping rule
    double stop_arg;                        // argument to stopping rule
    CombinatorialAuctionDecoder::DecoderApproach decode_approach; // true to greedy, false to lazy
    char init_population;                   // how initial population must be generated, ...
    double init_percentage;                 // ... how many individuals and...
    double init_max_time;                   // ... for how much time...
    double init_cplex_time;                 // ... using this time for each iteration...
    unsigned init_cplex_iters;              // ... how many iterations to be used
    double max_time;                        // max running time
    Auction::Pricing pricing;               // princing scheme
    const char* instance_file = argv[13];   // instance file
//    const char* lp_file = argv[14];         // LP file

    const unsigned MAX_THR = 1;

    try {
        seed = boost::lexical_cast<unsigned long>(argv[2]);
        stop_rule = StopRule(toupper(argv[3][0]));
        stop_arg = boost::lexical_cast<double>(argv[4]);
        decode_approach = CombinatorialAuctionDecoder::DecoderApproach(toupper(argv[5][0]));
        init_population = InitApproach(toupper(argv[6][0]));
        init_percentage = boost::lexical_cast<double>(argv[7]);
        init_max_time = boost::lexical_cast<double>(argv[8]);
        init_cplex_time = boost::lexical_cast<double>(argv[9]);
        init_cplex_iters = boost::lexical_cast<unsigned>(argv[10]);
        max_time = boost::lexical_cast<double>(argv[11]);
        pricing = Auction::Pricing(toupper(argv[12][0]));

        if((stop_rule != 'G' && stop_rule != 'T' && stop_rule != 'I') ||
           (decode_approach != CombinatorialAuctionDecoder::LAZY &&
            decode_approach != CombinatorialAuctionDecoder::GREEDY &&
            decode_approach != CombinatorialAuctionDecoder::PIRKUL) ||
           (init_population != 'R' && init_population != 'L') ||
           (pricing != Auction::FirstPrice && pricing != Auction::SecondPrice))

            throw exception();
    }
    catch(exception& e) {
        cout << "*** Bad arguments. Verify them!" << endl;
        return 64; // BSD usage error code
    }

    /////////////////////////////////////////
    // Load config file and show basic info.
    /////////////////////////////////////////

    auto params = BRKGA::readConfiguration(config_file);
    auto& brkga_params = params.first;
    auto& control_params = params.second;

    //-----------------------------------------//
    // Tuning
    //-----------------------------------------//
    #ifdef TUNING
    brkga_params.population_size = boost::lexical_cast<unsigned>(argv[num_args]);
    brkga_params.elite_percentage = boost::lexical_cast<double>(argv[num_args + 1]);
    brkga_params.mutants_percentage = boost::lexical_cast<double>(argv[num_args + 2]);
    brkga_params.num_elite_parents = boost::lexical_cast<unsigned>(argv[num_args + 3]);
    brkga_params.total_parents = boost::lexical_cast<unsigned>(argv[num_args + 4]);
    brkga_params.bias_type = toEnum<BiasFunctionType>(argv[num_args + 5]);
    brkga_params.num_independent_populations = boost::lexical_cast<unsigned>(argv[num_args + 6]);
    brkga_params.pr_minimum_distance = boost::lexical_cast<double>(argv[num_args + 7]);
    brkga_params.pr_type = toEnum<PathRelinking::Type>(argv[num_args + 8]);
    brkga_params.pr_selection = toEnum<PathRelinking::Selection>(argv[num_args + 9]);
    brkga_params.alpha_block_size = boost::lexical_cast<double>(argv[num_args + 10]);
    brkga_params.pr_percentage = boost::lexical_cast<double>(argv[num_args + 11]);
    control_params.exchange_interval = boost::lexical_cast<unsigned>(argv[num_args + 12]);
    control_params.num_exchange_individuals = boost::lexical_cast<unsigned>(argv[num_args + 13]);
    control_params.reset_interval = boost::lexical_cast<unsigned>(argv[num_args + 14]);
    #endif

    //-----------------------------------------//

    cout << "------------------------------------------------------"
         << "\n> Parameters\n"
         << "\nConfig file: " << config_file
         << "\n   + % of Elite: " << brkga_params.elite_percentage
         << "\n   + % of Mutants: " << brkga_params.mutants_percentage
         << "\n   + # of independent populations: " << brkga_params.num_independent_populations
         << "\n   + maximum size of each population: " << brkga_params.population_size
         << "\n   + # of threads: " << MAX_THR
         << "\n   + interval of chromossome exchange: " << control_params.exchange_interval
         << "\n   + # of elite chromossome of each population: " << control_params.num_exchange_individuals
         << "\n   + reset interval: " << control_params.reset_interval
         << "\nSeed: " << seed
         << "\nStop Rule: [" << stop_rule << "] "
         << (stop_rule == 'G' ? "Generations -> " :
             (stop_rule == 'T' ? "Target -> " : "Improvement -> "))
         << stop_arg
         << "\nDecode approach: " << (decode_approach == CombinatorialAuctionDecoder::LAZY? "Lazy" :
                                     (decode_approach == CombinatorialAuctionDecoder::GREEDY? "Greedy" : "Pirkul"))
         << "\nInitial Population approach: " << (init_population == 'R'? "Random" : "LP Relaxations ");

    if(init_population == 'L')
        cout << "\n   + % of individuals: " << init_percentage
             << "\n   + Iterations: " << init_cplex_iters
             << "\n   + Init. Max time: " << init_max_time
             << "\n   + Init. Cplex time: " << init_cplex_time
             << "\n   + Init. Cplex iterarions: " << init_cplex_iters;

    cout << "\nPath relink type: " << brkga_params.pr_type;
    cout << "\nMininum path relink distance: " << brkga_params.pr_minimum_distance
         << "\nMax Time: " << max_time
         << "\nPricing Scheme: " << (pricing == Auction::FirstPrice? "First Price" : "Second Price")
         << "\n------------------------------------------------------" << endl;

    // Begin of real fun :-)
    try {
        // Loading instance file
        CombinatorialAuction auction;
        if(!auction.loadProblem(instance_file))
            return 65;  // BSD file read error code

        // Decoder to auction
        CombinatorialAuctionDecoder decoder(&auction, pricing, decode_approach);

        unsigned population_size = 10 * auction.num_bids;
        if(population_size > brkga_params.population_size)
            population_size =
                (auction.num_bids > brkga_params.population_size)?
                    auction.num_bids :
                    brkga_params.population_size;

        // The BRKGA_IPR algorithm object.
        // BRKGA_MP_IPR<CombinatorialAuctionDecoder> algorithm(
        //         decoder, BRKGA::Sense::MAXIMIZE, seed,
        //         auction.num_bids, brkga_params.population_size, elite_percentage,
        //         mutants_percentage, true, num_elite_parents, total_parents,
        //         bias, num_independent_populations, MAX_THR);

        BRKGA::BRKGA_MP_IPR<CombinatorialAuctionDecoder> algorithm(
                decoder, BRKGA::Sense::MAXIMIZE, seed, auction.num_bids,
                brkga_params, MAX_THR);

        // Distance functor for path-relink.
        std::shared_ptr<DistanceFunctionBase> dist_func;
        if(brkga_params.pr_type == BRKGA::PathRelinking::Type::DIRECT)
            dist_func.reset(new BRKGA::HammingDistance(0.5));
        else
            dist_func.reset(new BRKGA::KendallTauDistance());

        // Hold best results
        double best_fitness = numeric_limits< double >::min();
        vector< double > best_chromosome;
        unsigned last_update_iteration = 0;
        double last_update_time = 0.0;
        unsigned update_offset = 0;
        unsigned large_offset = 0;
        double init_population_time = 0.0;
        double path_relink_time = 0.0;
        unsigned path_relink_improvements = 0;
        unsigned num_homogenities = 0;

        Timer timer;

        // Initialize properly and take elapsed time to do it
        algorithm.initialize();
        init_population_time = timer.elapsed();

        unsigned iteration = 1;
        bool run = true;

        cout << "\n\n>>>>> Evolving <<<<<" << endl;
        cout << setiosflags(ios::fixed) << setprecision(6);

        // Reset the timer and do the iterations
        timer.restart();
        while(run) {
            // Run one iteration
            algorithm.evolve();

            // Take the best solution
            double fitness = algorithm.getBestFitness();

            // Print some information
            #ifdef DEBUG
            cout << "Iteration " << iteration
                 << " / Best Current: " << fitness
                 << " / Best Overall: " << best_fitness << endl;
            #endif

            if((fitness - best_fitness) > 0.000001) {
                last_update_time = timer.elapsed();

                if(stop_rule != TARGET)
                    cout << "Improvement Iteration " << iteration
                         << " / Current Time: " << last_update_time
                         << " / Best Current: " << fitness
                         << " / Best Overall: " << best_fitness
                         << endl;

                best_fitness = fitness;
                best_chromosome = algorithm.getBestChromosome();
                update_offset = iteration - last_update_iteration;
                last_update_iteration = iteration;

                if(large_offset < update_offset)
                    large_offset = update_offset;
            }

            unsigned iter_without_improvement = iteration - last_update_iteration;

            // Path relink.
            if((control_params.exchange_interval > 0) && (iter_without_improvement > 0) &&
               (iter_without_improvement % control_params.exchange_interval == 0)) {

                std::size_t block_size = ceil(brkga_params.alpha_block_size * sqrt(brkga_params.population_size));
                if(block_size > algorithm.getChromosomeSize())
                    block_size = algorithm.getChromosomeSize() / 2;

                cout << "Path relink at " << iteration << " iteration. "
                     << "Block size: " << block_size << ". "
                     << "Type: " << brkga_params.pr_type << ": ";
                cout.flush();

                // auto now = timer.elapsed();
                // bool res = algorithm.pathRelink(
                //                 dist_func,
                //                 auction.num_bids * brkga_params.pr_minimum_distance,
                //                 brkga_params.pr_type, brkga_params.pr_selection, block_size,
                //                 long(max_time) - (timer.elapsed() / long(1e9)),
                //                 brkga_params.pr_percentage);
                // path_relink_time += timer.elapsed() - now;

                auto now = timer.elapsed();
                auto result = algorithm.pathRelink(
                    brkga_params.pr_type,
                    brkga_params.pr_selection,
                    dist_func,
                    brkga_params.pr_number_pairs,
                    brkga_params.pr_minimum_distance,
                    1, // block_size doesn't not matter for permutation.
                    long(max_time) - (timer.elapsed() / long(1e9)),
                    brkga_params.pr_percentage);

                const auto pr_time = timer.elapsed() - now;
                path_relink_time += timer.elapsed() - now;

                using BRKGA::PathRelinking::PathRelinkingResult;
                switch(result) {
                    case PathRelinkingResult::TOO_HOMOGENEOUS:
                        ++num_homogenities;
                        cout << "- Populations are too too homogeneous | "
                                "Elapsed time: " << pr_time << endl;
                        break;

                    case PathRelinkingResult::NO_IMPROVEMENT:
                        cout << "- No improvement found | "
                                "Elapsed time: " << pr_time << endl;
                        break;

                    case PathRelinkingResult::ELITE_IMPROVEMENT:
                        cout << "- Improvement on the elite set but not in the "
                                "best individual | Elapsed time: "
                             << pr_time << endl;
                        break;

                    case PathRelinkingResult::BEST_IMPROVEMENT:
                        fitness = algorithm.getBestFitness();

                        cout << "- Best individual improvement: "
                             << fitness
                             << " | Elapsed time: " << pr_time << endl;

                        if((fitness - best_fitness) > 1e-6) {
                            ++path_relink_improvements;
                            last_update_time = timer.elapsed();

                            if(stop_rule != TARGET)
                                cout << "Improvement Iteration " << iteration
                                    << " / Current Time: " << last_update_time
                                    << " / Best Current: " << fitness
                                    << " / Best Overall: " << best_fitness
                                    << endl;

                            best_fitness = fitness;
                            best_chromosome = algorithm.getBestChromosome();
                            update_offset = iteration - last_update_iteration;
                            last_update_iteration = iteration;

                            if(large_offset < update_offset)
                                large_offset = update_offset;
                        }
                        break;

                    default:;
                } // end switch
            }

            // Time to reset?
            if(control_params.reset_interval > 0 && iter_without_improvement > 0 &&
               iter_without_improvement % control_params.reset_interval == 0) {
                algorithm.reset();
                if(stop_rule != TARGET)
                    cout << ">> Reset chromosomes with random keys after "
                         << control_params.reset_interval
                         << " iterations without improvement." << endl;
            }

            // Time to stop?
            switch(stop_rule) {
                case GENERATIONS:
                    if(iteration == (unsigned)stop_arg)
                        run = false;
                    break;

                case IMPROVEMENT:
                    if(iter_without_improvement >= (unsigned)stop_arg)
                        run = false;
                    break;

                case TARGET:
                    if((stop_arg - best_fitness) < 0.000001)
                        run = false;
                    break;
            }

            if(timer.elapsed() > max_time) {
                run = false;
            }

            ++iteration;
        } // end of while
        #ifdef DEBUG
        cout << "\n\n> End of evolving"
             << "\n------------------------------------------------------" << endl;
        #endif

        // Take elapsed time to optmization
        double elapsed_time = timer.elapsed();

        if(stop_rule != TARGET) {
            // Print results
            list< unsigned > solution = decoder.getSolutionFromChromosome(best_chromosome);
            double value_by_auction = auction.getSolutionRevenue(solution, pricing);

            cout << "\nBest solution > value: " << value_by_auction << " | (bids): ";
            copy(solution.begin(), solution.end(), ostream_iterator< unsigned >(cout, " "));

            cout << "\n\nRevenue & Iterations & Last Update Iteration & Last Update Time & "
                 << "Update Offset & Large Offset & Init. Time (s) & Total Elap. Time (s) & "
                 << "PathRelinkingImprovements & PathRelinkingTime & Num_homogenities\n"
                 << setiosflags(ios::fixed) << setprecision(6)
                 << best_fitness << " & "
                 << setprecision(2)
                 << --iteration << " & "            // only to correct the iteration number
                 << last_update_iteration << " & "
                 << last_update_time << " & "
                 << update_offset << " & "
                 << large_offset << " & "
                 << init_population_time << " & "
                 << elapsed_time << " & "
                 << path_relink_improvements << " & "
                 << path_relink_time << " & "
                 << num_homogenities;

            #ifdef TUNING
            cout << "\n\n"
                 << (-1 * best_fitness);
            cout.flush();
            #endif
        }
        else {
            cout << setiosflags(ios::fixed) << setprecision(2)
                 << best_fitness << " & "
                 << setprecision(2)
                 << --iteration << " & "            // only to correct the iteration number
                 << init_population_time << " & "
                 << elapsed_time;
        }

        cout.flush();
        dist_func.reset();
    }
    catch(exception& e) {
        cout << "\n***********************************************************"
             << "\n****  Exception Occured: " << e.what()
             << "\n***********************************************************"
             << endl;
        return 70; // BSD software internal error code
    }
    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
//
#ifndef BOOST_GIL_CONCEPTS_POINT_HPP
#define BOOST_GIL_CONCEPTS_POINT_HPP

#include <boost/gil/concepts/basic.hpp>
#include <boost/gil/concepts/concept_check.hpp>

#include <cstddef>

#if defined(BOOST_CLANG)
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wunused-local-typedefs"
#endif

#if defined(BOOST_GCC) && (BOOST_GCC >= 40900)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#endif

namespace boost { namespace gil {

// Forward declarations
template <typename T>
class point;

template <std::size_t K, typename T>
T const& axis_value(point<T> const& p);

template <std::size_t K, typename T>
T& axis_value(point<T>& p);

/// \brief N-dimensional point concept
/// \code
/// concept PointNDConcept<typename T> : Regular<T>
/// {
///     // the type of a coordinate along each axis
///     template <size_t K>
///     struct axis; where Metafunction<axis>;
///
///     const size_t num_dimensions;
///
///     // accessor/modifier of the value of each axis.
///
///     template <size_t K>
///     typename axis<K>::type const& T::axis_value() const;
///
///     template <size_t K>
///     typename axis<K>::type& T::axis_value();
/// };
/// \endcode
/// \ingroup PointConcept
///
template <typename P>
struct PointNDConcept
{
    void constraints()
    {
        gil_function_requires<Regular<P>>();

        using value_type = typename P::value_type;
        ignore_unused_variable_warning(value_type{});

        static const std::size_t N = P::num_dimensions;
        ignore_unused_variable_warning(N);
        using FT = typename P::template axis<0>::coord_t;
        using LT = typename P::template axis<N - 1>::coord_t;
        FT ft = gil::axis_value<0>(point);
        axis_value<0>(point) = ft;
        LT lt = axis_value<N - 1>(point);
        axis_value<N - 1>(point) = lt;

        //value_type v=point[0];
        //ignore_unused_variable_warning(v);
    }
    P point;
};

/// \brief 2-dimensional point concept
/// \code
/// concept Point2DConcept<typename T> : PointNDConcept<T>
/// {
///     where num_dimensions == 2;
///     where SameType<axis<0>::type, axis<1>::type>;
///
///     typename value_type = axis<0>::type;
///
///     value_type const& operator[](T const&, size_t i);
///     value_type& operator[](T&, size_t i);
///
///     value_type x,y;
/// };
/// \endcode
/// \ingroup PointConcept
///
template <typename P>
struct Point2DConcept
{
    void constraints()
    {
        gil_function_requires<PointNDConcept<P>>();
        static_assert(P::num_dimensions == 2, "");
        point.x = point.y;
        point[0] = point[1];
    }
    P point;
};

}} // namespace boost::gil

#if defined(BOOST_CLANG)
#pragma clang diagnostic pop
#endif

#if defined(BOOST_GCC) && (BOOST_GCC >= 40900)
#pragma GCC diagnostic pop
#endif

#endif


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   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
//==============================================================================
#include <nt2/arithmetic/include/functions/rsqrt.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <nt2/sdk/functor/meta/call.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>

#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/zero.hpp>

NT2_TEST_CASE_TPL ( rsqrt,  NT2_SIMD_REAL_TYPES)
{
  using nt2::rsqrt;
  using nt2::tag::rsqrt_;
  using boost::simd::native;

  typedef NT2_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>             vT;

  NT2_TEST_TYPE_IS( typename nt2::meta::call<rsqrt_(vT)>::type
                  , vT
                  );

  // specific values tests
  NT2_TEST_ULP_EQUAL(rsqrt(nt2::Inf<vT>()), nt2::Zero<vT>(), 0);
  NT2_TEST_ULP_EQUAL(rsqrt(nt2::Minf<vT>()), nt2::Nan<vT>(), 0);
  NT2_TEST_ULP_EQUAL(rsqrt(nt2::Mone<vT>()), nt2::Nan<vT>(), 0);
  NT2_TEST_ULP_EQUAL(rsqrt(nt2::Nan<vT>()), nt2::Nan<vT>(), 0);
  NT2_TEST_ULP_EQUAL(rsqrt(nt2::One<vT>()), nt2::One<vT>(), 0);
  NT2_TEST_ULP_EQUAL(rsqrt(nt2::Zero<vT>()), nt2::Inf<vT>(), 0);
}


#pragma once

#include <armadillo>
#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>
#include <vector>

namespace py = pybind11;

using pyarray = py::array_t<double, py::array::c_style | py::array::forcecast>;

arma::mat pymat(pyarray &array) {
  unsigned nc = 1, nr = array.shape()[0];;
  if (array.ndim()==2) {
    nc = array.shape()[1];
  }
  arma::mat x(&array.data()[0], nc, nr); //, true);
  return x.t();
}

arma::cube pycube(pyarray &array) {
    unsigned ns = array.shape()[0], nr = array.shape()[1], nc=array.shape()[2];
    arma::cube x(&array.data()[0], nc,nr, ns);

    // arma::cube x(&array.data()[0], nr,nc, ns); does not result in a cube that corresponds to the
    // 3d input numpy array
    arma::cube d(nr,nc,ns);
    for (int i = 0; i < ns; i++) {d.slice(i) = x.slice(i).t();}
    //    TODO: can I do this more efficient?
    return d;
}

pyarray matpy(arma::mat &x) {
  ssize_t              ndim    = 2;
  std::vector<ssize_t> shape   = { (ssize_t)x.n_rows , (ssize_t)x.n_cols };
  std::vector<ssize_t> strides = { (ssize_t)(sizeof(double)) , (ssize_t)(sizeof(double)*x.n_rows) };
  // return 2-D NumPy array
  return py::array(py::buffer_info(
    &x[0],                                   /* data as contiguous array  */
    sizeof(double),                          /* size of one scalar        */
    py::format_descriptor<double>::format(), /* data type                 */
    ndim,                                    /* number of dimensions      */
    shape,                                   /* shape of the matrix       */
    strides                                  /* strides for each axis     */
  ));
}

pyarray cubepy(arma::cube &x) {
  ssize_t              ndim    = 3;
  std::vector<ssize_t> shape   = { (ssize_t)x.n_slices, (ssize_t)x.n_rows, (ssize_t)x.n_cols};
  std::vector<ssize_t> strides = { (ssize_t)(sizeof(double)*x.n_rows*x.n_cols), (ssize_t)(sizeof(double)) , (ssize_t)(sizeof(double)*x.n_cols)};
  // return 3-D NumPy array
  return py::array(py::buffer_info(
    &x[0],                                   /* data as contiguous array  */
    sizeof(double),                          /* size of one scalar        */
    py::format_descriptor<double>::format(), /* data type                 */
    ndim,                                    /* number of dimensions      */
    shape,                                   /* shape of the matrix       */
    strides                                  /* strides for each axis     */
  ));
}


#include "test_case_with_time_measurements.h"
#include "test_pattern_generation.h"

#include <boost/sort/spreadsort/spreadsort.hpp>
#include <boost/sort/sort.hpp>

#include "radix_sort_metal_cpp.h"
#include "bitonic_sort_metal_cpp.h"

template<class T>
class TestCaseRadixSort : public TestCaseWithTimeMeasurements {

  protected:
    const size_t        m_num_elements;
    T*                  m_array;
    bool                m_calculation_correct;
  public:

    TestCaseRadixSort( const size_t num_elements )
        :m_num_elements       ( num_elements )
        ,m_calculation_correct( false )
    {
        if constexpr ( is_same<int, T>::value ) {

            setDataElementType( INT );
        }
        if constexpr ( is_same<float, T>::value ) {

            setDataElementType( FLOAT );
        }
        else {
            assert(true);
        }

        setVector( num_elements );

        setVerificationType( TRUE_FALSE );
    }

    virtual ~TestCaseRadixSort(){;}

    void checkResult( const T* truth )
    {
        T* out = getArray();

        setTrueFalse( true );
        for ( size_t i = 0; i < m_num_elements; i++ ) {

//            cerr << "out[" << i << "]:" << out[i] << "\ttruth[" << i << "]:" << truth[i] << "\n";
            if ( out[i] != truth[i] ) {
                setTrueFalse( false );
            }
        }
    }

    virtual void setArray( T* const a ){ m_array = a; }
    virtual T* getArray(){ return m_array; }

    virtual void run() = 0;
};

template<class T>
class TestCaseRadixSort_baseline : public TestCaseRadixSort<T> {

  public:
    TestCaseRadixSort_baseline( const size_t num_elements )
        :TestCaseRadixSort<T>( num_elements )
    {
        this->setImplementationType( CPP_STDLIB );
    }

    virtual ~TestCaseRadixSort_baseline(){;}

    void run() {

        std::sort( this->m_array, this->m_array + this->m_num_elements );
    }
};

template<class T>
class TestCaseRadixSort_boost_spread_sort : public TestCaseRadixSort<T> {

  public:
    TestCaseRadixSort_boost_spread_sort( const size_t num_elements )
        :TestCaseRadixSort<T>( num_elements )
    {
        this->setImplementationType(BOOST_SPREAD_SORT);
    }

    virtual ~TestCaseRadixSort_boost_spread_sort(){;}

    void run(){

        if constexpr ( is_same<float, T>::value || is_same<double, T>::value ) {
            boost::sort::spreadsort::float_sort ( this->m_array, this->m_array + this->m_num_elements );
        }
        else {
            boost::sort::spreadsort::integer_sort( this->m_array, this->m_array + this->m_num_elements );
        }
    }
};

template<class T>
class TestCaseRadixSort_boost_sample_sort : public TestCaseRadixSort<T> {

  private:
    size_t m_num_threads;

  public:
    TestCaseRadixSort_boost_sample_sort( const size_t num_element, const size_t num_threads )
        :TestCaseRadixSort<T>( num_element )
        ,m_num_threads( num_threads )
    {
        this->setBoostSampleSort( num_threads );
    }

    virtual ~TestCaseRadixSort_boost_sample_sort(){;}

    void run() {

        boost::sort::block_indirect_sort( this->m_array, this->m_array + this->m_num_elements , m_num_threads );
    }
};

template<class T>
class TestCaseRadixSort_Metal : public TestCaseRadixSort<T> {

  private:
    RadixSortMetalCpp m_metal;

  public:
    TestCaseRadixSort_Metal(
        const size_t num_elements,
        const bool   coalesced_write,
        const bool   early_out,
        const int    num_iterations_per_commit,
        const size_t num_threads_per_threadgroup )
        :TestCaseRadixSort<T>( num_elements )
        ,m_metal( num_elements, is_same<float, T>::value, coalesced_write, early_out, num_iterations_per_commit, num_threads_per_threadgroup )
    {
        static_assert( std::is_same<int, T>::value || std::is_same<long, T>::value || std::is_same<float,T>::value );

        if ( early_out ) {
            this->setMetal( coalesced_write ? COALESCED_WRITE_EARLY_OUT : UNCOALESCED_WRITE_EARLY_OUT, 1, num_threads_per_threadgroup );
        }
        else if ( num_iterations_per_commit > 1 ) {
            this->setMetal( coalesced_write ? COALESCED_WRITE_IN_ONE_COMMIT : UNCOALESCED_WRITE_IN_ONE_COMMIT, 1, num_threads_per_threadgroup );
        }
        else {
            this->setMetal( coalesced_write ? COALESCED_WRITE : UNCOALESCED_WRITE, 1, num_threads_per_threadgroup );
        }
    }

    virtual ~TestCaseRadixSort_Metal(){;}

    void setArray( T* const x ) {
        m_metal.resetBufferFlag();
        memcpy( m_metal.getRawPointerIn(), x, this->m_num_elements*sizeof(T) );
    }

    T* getArray() {
        return (T*)m_metal.getRawPointerOut();
    }

    void run() {
        m_metal.performComputation();
    }
};

template<class T>
class TestCaseBitonicSort_Metal : public TestCaseRadixSort<T> {

  private:
    BitonicSortMetalCpp m_metal;

  public:
    TestCaseBitonicSort_Metal(
        const size_t num_elements,
        const size_t num_threads_per_threadgroup )
        :TestCaseRadixSort<T>( num_elements )
        ,m_metal( num_elements, is_same<float, T>::value, num_threads_per_threadgroup )
    {
        static_assert( std::is_same<int, T>::value || std::is_same<float,T>::value );

        this->setMetal( BITONIC_SORT, 1, num_threads_per_threadgroup );
    }

    virtual ~TestCaseBitonicSort_Metal(){;}

    void setArray( T* const x ) {
        memcpy( m_metal.getRawPointerInOut(), x, this->m_num_elements*sizeof(T) );
    }

    T* getArray() {
        return (T*)m_metal.getRawPointerInOut();
    }

    void run() {
        m_metal.performComputation();
    }
};

template <class T>
class TestExecutorRadixSort : public TestExecutor {

  protected:

    const int             m_num_elements;
    const bool            m_repeatable;
    default_random_engine m_e;
    T*                    m_array_original;
    T*                    m_array_sorted_baseline;
    T*                    m_array_sorted;

  public:

    TestExecutorRadixSort( ostream& os, const int num_elements, const int num_trials, const bool repeatable, const T min_val, const T max_val )
        :TestExecutor  ( os, num_trials )
        ,m_num_elements( num_elements )
        ,m_repeatable  ( repeatable )
        ,m_e( repeatable? 0 : chrono::system_clock::now().time_since_epoch().count() )
        ,m_array_original        ( new T[num_elements] )
        ,m_array_sorted_baseline ( new T[num_elements] )
        ,m_array_sorted          ( new T[num_elements] )
    {
        fillArrayWithRandomValues( m_e, m_array_original,  m_num_elements, min_val, max_val );
    }

    virtual ~TestExecutorRadixSort() {
        delete[] m_array_original;
        delete[] m_array_sorted_baseline;
        delete[] m_array_sorted;
    }

    void cleanupAfterBatchRuns ( const int test_case ) {
        auto t = dynamic_pointer_cast< TestCaseRadixSort <T> >( this->m_test_cases[ test_case ] );
        if ( test_case == 0 ) {
            memcpy( m_array_sorted_baseline , t->getArray(), sizeof(T)*m_num_elements );
        }
        t->checkResult(  m_array_sorted_baseline );
    }

    void prepareForRun ( const int test_case, const int num ) {

        auto t = dynamic_pointer_cast< TestCaseRadixSort<T> >( this->m_test_cases[ test_case ] );

        memcpy( m_array_sorted, m_array_original, sizeof(T) * this->m_num_elements );

        t->setArray( m_array_sorted );
    }
};

static const size_t NUM_TRIALS = 10;

#if TARGET_OS_OSX

size_t nums_elements[]{ 32, 128, 512, 2*1024, 8*1024, 32*1024, 128*1024, 512*1024, 2*1024*1024, 8*1024*1024, 32*1024*1024, 128*1024*1024 };
uint num_threads_per_threadgroup = 1024;
int num_iterations_per_commit = 16;
#else
size_t nums_elements[]{ 32, 128, 512, 2*1024, 8*1024, 32*1024, 128*1024, 512*1024, 2*1024*1024, 8*1024*1024 };
uint num_threads_per_threadgroup = 1024;
int num_iterations_per_commit = 16;
#endif

template<class T>
void testSuitePerType () {

    for( auto num_elements : nums_elements ) {

        TestExecutorRadixSort<T> e( cout, num_elements, NUM_TRIALS, false, INT_MIN, INT_MAX );

        e.addTestCase( make_shared< TestCaseRadixSort_baseline <T> > ( num_elements ) );
        e.addTestCase( make_shared< TestCaseRadixSort_boost_spread_sort  <T> > ( num_elements         ) );
        e.addTestCase( make_shared< TestCaseRadixSort_boost_sample_sort  <T> > ( num_elements,     1  ) );
        e.addTestCase( make_shared< TestCaseRadixSort_boost_sample_sort  <T> > ( num_elements,     4  ) );
        e.addTestCase( make_shared< TestCaseRadixSort_boost_sample_sort  <T> > ( num_elements,    16  ) );
        e.addTestCase( make_shared< TestCaseRadixSort_boost_sample_sort  <T> > ( num_elements,    64  ) );
        e.addTestCase( make_shared< TestCaseRadixSort_Metal              <T> > ( num_elements , false, true  , 1, num_threads_per_threadgroup ) );
        e.addTestCase( make_shared< TestCaseRadixSort_Metal              <T> > ( num_elements , true,  true  , 1, num_threads_per_threadgroup ) );
        e.addTestCase( make_shared< TestCaseRadixSort_Metal              <T> > ( num_elements , false, false , 1, num_threads_per_threadgroup ) );
        e.addTestCase( make_shared< TestCaseRadixSort_Metal              <T> > ( num_elements , true,  false , 1, num_threads_per_threadgroup ) );
        e.addTestCase( make_shared< TestCaseRadixSort_Metal              <T> > ( num_elements , false, false , num_iterations_per_commit, num_threads_per_threadgroup ) );
        e.addTestCase( make_shared< TestCaseRadixSort_Metal              <T> > ( num_elements , true,  false , num_iterations_per_commit, num_threads_per_threadgroup ) );
        e.addTestCase( make_shared< TestCaseBitonicSort_Metal            <T> > ( num_elements , num_threads_per_threadgroup ) );

        e.execute();
    }
}

#if TARGET_OS_OSX
int main(int argc, char* argv[]) {
#else
int run_test() {
#endif
    TestCaseWithTimeMeasurements::printHeader( cout );

    cerr << "\n\nTesting for type int.\n\n";

    testSuitePerType<int> ();

    cerr << "\n\nTesting for type float.\n\n";

    testSuitePerType<float> ();

    return 0;
}


//
// Created by yche on 12/20/17.
//
#include <cstdlib>
#include <chrono>

#include <iostream>

#include <boost/program_options.hpp>

#include "../../yche_refactor/bprw_yche.h"
#include "../../yche_refactor/simrank.h"
#include "../../util/random_pair_generator.h"
#include "../../playground/pretty_print.h"

using namespace std;
using namespace boost::program_options;

void test_bp(string data_name, double c, double epsilon, double delta, int pair_num, int round) {
    string path = get_edge_list_path(data_name);
    GraphYche g(path);
    size_t n = static_cast<size_t>(g.n);

    cout << n << endl;

#ifdef GROUND_TRUTH
    TruthSim ts(data_name, g, c, epsilon);
    auto max_err = 0.0;
#endif
    auto sample_pairs = read_sample_pairs(data_name, pair_num, round);
    auto clock_start = clock();
    auto start = std::chrono::high_resolution_clock::now();

#pragma omp parallel
    {
        auto bprw = BackPush(data_name, g, c, epsilon, delta);

#ifdef GROUND_TRUTH
#pragma omp for reduction(max:max_err) schedule(dynamic, 100)
#else
#pragma omp for schedule(dynamic, 100)
#endif
        for (auto pair_i = 0; pair_i < pair_num; pair_i++) {
            auto q = pair<uint32_t, uint32_t>(sample_pairs[pair_i].first, sample_pairs[pair_i].second);

#ifdef GROUND_TRUTH
            auto res = bprw.query_one2one(q);
            max_err = max(max_err, abs(ts.sim(q.first, q.second) - res));
            if (abs(ts.sim(q.first, q.second) - res) > 0.01) {
#pragma omp critical
                cout << sample_pairs[pair_i].first << ", " << sample_pairs[pair_i].second << ","
                     << ts.sim(q.first, q.second) << "," << res << endl;
            }
#else
            bprw.query_one2one(q);
#endif
        }
    }

    auto end = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> elapsed = end - start;

    auto clock_end = clock();
    cout << "total query cpu time:" << static_cast<double>(clock_end - clock_start) / CLOCKS_PER_SEC << "s" << endl;
#ifdef GROUND_TRUTH
    cout << "max err:" << max_err << endl;
#endif
    cout << format("total query cost: %s s") % elapsed.count() << endl; // record the pre-processing time
}

int main(int args, char *argv[]) {
    string data_name(argv[1]);
    double c = atof(argv[4]);
    double epsilon = atof(argv[5]);
    double delta = atof(argv[6]);

    int pair_num = atoi(argv[2]);
    int round_i = atoi(argv[3]);
    test_bp(data_name, c, epsilon, delta, pair_num, round_i);
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

using namespace std;

template <typename Matrix>
void test(Matrix& A, const char* name)
{
    cout << "\n" << name << "\n" << "A =\n" << A;

    cout << "Pointer to major (row) index array is " << A.address_major()
	 << ", pointer to minor (column) index array is " << A.address_minor()
	 << ", pointer to data array is " << A.address_data() << "\n";
}

int main(int, char**)
{
    mtl::compressed2D<double>                                 cr(2, 2);
    cr= 2.0;

    test(cr, "Compressed row major");
    return 0;
}


/*
 [auto_generated]
 boost/numeric/odeint/external/compute/compute_operations.hpp

 [begin_description]
 Operations of Boost.Compute zipped iterators. Is the counterpart of the compute_algebra.
 [end_description]

 Copyright 2009-2011 Karsten Ahnert
 Copyright 2009-2011 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)
 */

#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_COMPUTE_COMPUTE_OPERATIONS_HPP_DEFINED
#define BOOST_NUMERIC_ODEINT_EXTERNAL_COMPUTE_COMPUTE_OPERATIONS_HPP_DEFINED

#include <boost/preprocessor/repetition.hpp>
#include <boost/compute.hpp>

namespace boost {
namespace numeric {
namespace odeint {

struct compute_operations {

#define BOOST_ODEINT_COMPUTE_TEMPL_FAC(z, n, unused)                           \
    , class Fac ## n = BOOST_PP_CAT(Fac, BOOST_PP_DEC(n))

#define BOOST_ODEINT_COMPUTE_MEMB_FAC(z, n, unused)                            \
    const Fac ## n m_alpha ## n;

#define BOOST_ODEINT_COMPUTE_PRM_FAC(z, n, unused)                             \
    BOOST_PP_COMMA_IF(n) const Fac ## n alpha ## n

#define BOOST_ODEINT_COMPUTE_INIT_FAC(z, n, unused)                            \
    BOOST_PP_COMMA_IF(n) m_alpha ## n (alpha ## n)

#define BOOST_ODEINT_COMPUTE_PRM_STATE(z, n, unused)                           \
    BOOST_PP_COMMA_IF(n) StateType ## n &s ## n

#define BOOST_ODEINT_COMPUTE_BEGIN_STATE(z, n, unused)                         \
    BOOST_PP_COMMA_IF( BOOST_PP_DEC(n) ) s ## n.begin()

#define BOOST_ODEINT_COMPUTE_END_STATE(z, n, unused)                           \
    BOOST_PP_COMMA_IF( BOOST_PP_DEC(n) ) s ## n.end()

#define BOOST_ODEINT_COMPUTE_LAMBDA(z, n, unused)                              \
    BOOST_PP_EXPR_IF(n, +) m_alpha ## n * bc::lambda::get< n >(bc::_1)

#define BOOST_ODEINT_COMPUTE_OPERATIONS(z, n, unused)                          \
    template<                                                                  \
        class Fac0 = double                                                    \
        BOOST_PP_REPEAT_FROM_TO(1, n, BOOST_ODEINT_COMPUTE_TEMPL_FAC, ~)       \
        >                                                                      \
    struct scale_sum ## n {                                                    \
        BOOST_PP_REPEAT(n, BOOST_ODEINT_COMPUTE_MEMB_FAC, ~)                   \
        scale_sum ## n(                                                        \
                BOOST_PP_REPEAT(n, BOOST_ODEINT_COMPUTE_PRM_FAC, ~)            \
                )                                                              \
            : BOOST_PP_REPEAT(n, BOOST_ODEINT_COMPUTE_INIT_FAC, ~)             \
        { }                                                                    \
        template< BOOST_PP_ENUM_PARAMS(BOOST_PP_INC(n), class StateType) >     \
        void operator()(                                                       \
                BOOST_PP_REPEAT(                                               \
                    BOOST_PP_INC(n),                                           \
                    BOOST_ODEINT_COMPUTE_PRM_STATE, ~)                         \
                ) const                                                        \
        {                                                                      \
            namespace bc = boost::compute;                                     \
            bc::transform(                                                     \
                    bc::make_zip_iterator(                                     \
                        boost::make_tuple(                                     \
                            BOOST_PP_REPEAT_FROM_TO(                           \
                                1, BOOST_PP_INC(n),                            \
                                BOOST_ODEINT_COMPUTE_BEGIN_STATE, ~)           \
                            )                                                  \
                        ),                                                     \
                    bc::make_zip_iterator(                                     \
                        boost::make_tuple(                                     \
                            BOOST_PP_REPEAT_FROM_TO(                           \
                                1, BOOST_PP_INC(n),                            \
                                BOOST_ODEINT_COMPUTE_END_STATE, ~)             \
                            )                                                  \
                        ),                                                     \
                    s0.begin(),                                                \
                    BOOST_PP_REPEAT(n, BOOST_ODEINT_COMPUTE_LAMBDA, ~)         \
                    );                                                         \
        }                                                                      \
    };

BOOST_PP_REPEAT_FROM_TO(2, 8, BOOST_ODEINT_COMPUTE_OPERATIONS, ~)

#undef BOOST_ODEINT_COMPUTE_TEMPL_FAC
#undef BOOST_ODEINT_COMPUTE_MEMB_FAC
#undef BOOST_ODEINT_COMPUTE_PRM_FAC
#undef BOOST_ODEINT_COMPUTE_INIT_FAC
#undef BOOST_ODEINT_COMPUTE_PRM_STATE
#undef BOOST_ODEINT_COMPUTE_BEGIN_STATE
#undef BOOST_ODEINT_COMPUTE_END_STATE
#undef BOOST_ODEINT_COMPUTE_LAMBDA
#undef BOOST_ODEINT_COMPUTE_OPERATIONS

    template<class Fac1 = double, class Fac2 = Fac1>
    struct scale_sum_swap2 {
        const Fac1 m_alpha1;
        const Fac2 m_alpha2;

        scale_sum_swap2(const Fac1 alpha1, const Fac2 alpha2)
            : m_alpha1(alpha1), m_alpha2(alpha2) { }

        template<class State0, class State1, class State2>
        void operator()(State0 &s0, State1 &s1, State2 &s2) const {
            namespace bc = boost::compute;

            bc::command_queue &queue   = bc::system::default_queue();
            const bc::context &context = queue.get_context();

            const char source[] = BOOST_COMPUTE_STRINGIZE_SOURCE(
                    kernel void scale_sum_swap2(
                        F1 a1, F2 a2,
                        global T0 *x0, global T1 *x1, global T2 *x2,
                        )
                    {
                        uint i = get_global_id(0);
                        T0 tmp = x0[i];
                        x0[i]  = a1 * x1[i] + a2 * x2[i];
                        x1[i]  = tmp;
                    }
                    );

            std::stringstream options;
            options
                << " -DT0=" << bc::type_name<typename State0::value_type>()
                << " -DT1=" << bc::type_name<typename State1::value_type>()
                << " -DT2=" << bc::type_name<typename State2::value_type>()
                << " -DF1=" << bc::type_name<Fac1>()
                << " -DF2=" << bc::type_name<Fac2>();

            bc::program program =
                bc::program::build_with_source(source, context, options.str());

            bc::kernel kernel(program, "scale_sum_swap2");
            kernel.set_arg(0, m_alpha1);
            kernel.set_arg(1, m_alpha2);
            kernel.set_arg(2, s0.get_buffer());
            kernel.set_arg(3, s1.get_buffer());
            kernel.set_arg(4, s2.get_buffer());

            queue.enqueue_1d_range_kernel(kernel, 0, s0.size());

        }
    };

    template<class Fac1 = double>
    struct rel_error {
        const Fac1 m_eps_abs, m_eps_rel, m_a_x, m_a_dxdt;

        rel_error(const Fac1 eps_abs, const Fac1 eps_rel, const Fac1 a_x, const Fac1 a_dxdt)
            : m_eps_abs(eps_abs), m_eps_rel(eps_rel), m_a_x(a_x), m_a_dxdt(a_dxdt) { }

        template <class State0, class State1, class State2>
        void operator()(State0 &s0, State1 &s1, State2 &s2) const {
            namespace bc = boost::compute;
            using bc::_1;
            using bc::lambda::get;

            bc::for_each(
                    bc::make_zip_iterator(
                        boost::make_tuple(
                            s0.begin(),
                            s1.begin(),
                            s2.begin()
                            )
                        ),
                    bc::make_zip_iterator(
                        boost::make_tuple(
                            s0.end(),
                            s1.end(),
                            s2.end()
                            )
                        ),
                    get<0>(_1) = abs( get<0>(_1) ) /
                        (m_eps_abs + m_eps_rel * (m_a_x * abs(get<1>(_1) + m_a_dxdt * abs(get<2>(_1)))))
                    );
        }
    };
};

} // odeint
} // numeric
} // boost

#endif // BOOST_NUMERIC_ODEINT_EXTERNAL_COMPUTE_COMPUTE_OPERATIONS_HPP_DEFINED


// ======================================================================
// \title  FlatOutputAdapterComponentImpl.cpp
// \author decoy
// \brief  cpp file for FlatOutputAdapter component implementation class
//
// \copyright
// Copyright 2009-2015, by the California Institute of Technology.
// ALL RIGHTS RESERVED.  United States Government Sponsorship
// acknowledged.
//
// ======================================================================

#include <Gnc/Utils/FlatOutputAdapter/FlatOutputAdapterComponentImpl.hpp>
#include "Fw/Types/BasicTypes.hpp"
#include <Eigen/Geometry>

namespace Gnc {

  // ----------------------------------------------------------------------
  // Construction, initialization, and destruction
  // ----------------------------------------------------------------------

  FlatOutputAdapterComponentImpl ::
#if FW_OBJECT_NAMES == 1
    FlatOutputAdapterComponentImpl(
        const char *const compName
    ) :
      FlatOutputAdapterComponentBase(compName)
#else
    FlatOutputAdapterComponentImpl(void)
#endif
  {

  }

  void FlatOutputAdapterComponentImpl ::
    init(
        const NATIVE_INT_TYPE instance
    )
  {
    FlatOutputAdapterComponentBase::init(instance);
  }

  FlatOutputAdapterComponentImpl ::
    ~FlatOutputAdapterComponentImpl(void)
  {

  }

  // ----------------------------------------------------------------------
  // Handler implementations for user-defined typed input ports
  // ----------------------------------------------------------------------

  void FlatOutputAdapterComponentImpl ::
    se3Cmd_handler(
        const NATIVE_INT_TYPE portNum,
        ROS::mav_msgs::Se3FeedForward &Se3FeedForward
    )
  {
      using namespace Eigen;

      ROS::geometry_msgs::Quaternion quat1(Se3FeedForward.getattitude());

      Eigen::Quaterniond quat2(quat1.getw(),quat1.getx(),quat1.gety(),quat1.getz());

      quat2.normalize();

      Eigen::Vector3d euler = quat2.toRotationMatrix().eulerAngles(0,1,2);

      printf("after euler in the function\n");
      ROS::mav_msgs::FlatOutput flatoutput(Se3FeedForward.getheader(),
                                           Se3FeedForward.getposition(),
                                           Se3FeedForward.getvelocity(),
                                           Se3FeedForward.getacceleration(),
                                           ROS::geometry_msgs::Vector3(0.0, 0.0, 0.0), // jerk
                                           ROS::geometry_msgs::Vector3(0.0, 0.0, 0.0), // snap
                                           euler(2),
                                           0.0); //yawdot
      this->flatOutput_out(0,flatoutput);
  }

} // end namespace Gnc