/*
* Node integrating differential equations of a rocket's flight to simulate position, velocity, quaternion, angular rate and mass  (14 states) in real time
* Also manage the finite state machine to coordinate other nodes
*
* Inputs:
*   - Commands from the GUI to start the simulation:      \commands
*   - 3D force and torque from the rocket engine:         \control_pub
*   - 3D force and torque from the aerodynamics.py node:  \rocket_aero
*   - 3D force and torque from the disturbance.py node:   \disturbance_pub

* Parameters:
*   - Rocket model: 		/config/rocket_parameters.yaml
*   - Environment model: 	/config/environment_parameters.yaml
*   - Sensor error model: /config/perturbations_parameters.yaml
*
* Outputs:
*   - Finite state machine and time since start of simulation : \fsm_pub
*   - Rocket full state (14 states) at 100 Hz:                  \rocket_state
*   - Simulated IMU and barometer sensor data:                  \sensor_pub
*
*/

#include "ros/ros.h"

#include "real_time_simulator/FSM.h"
#include "real_time_simulator/State.h"
#include "real_time_simulator/Sensor.h"

#include "real_time_simulator/Control.h"

#include "std_msgs/String.h"
#include <boost/numeric/odeint/stepper/runge_kutta_dopri5.hpp>

#include <Eigen/Geometry>
#include <unsupported/Eigen/EulerAngles>

#include <chrono>
#include <random>

#include "rocket.hpp"

using namespace std;
using namespace boost::numeric::odeint;
using namespace Eigen;

class IntegratorNode {
private:
    typedef runge_kutta_dopri5<double> stepper_type;
    using stepper_type2 = runge_kutta_dopri5<Rocket::state, double, Rocket::state, double, vector_space_algebra>;
    stepper_type2 stepper;

    Rocket::state X;
    Rocket::state xout;

    Rocket rocket;

    //last updated rocket force & torque
    Rocket::control rocket_control;
    //last updated aerodynamic force & torque
    Rocket::control aero_control;
    //last updated perturbations force & torque
    Rocket::control perturbation_control;
    //last requested fsm
    real_time_simulator::FSM current_fsm;
    double time_zero;
    double rail_length;

    // subscribers
    ros::Subscriber command_sub;
    ros::Subscriber rocket_control_sub;
    ros::Subscriber rocket_aero_sub;
    ros::Subscriber rocket_perturbation_sub;

    // publishers
    ros::Publisher rocket_state_pub;
    ros::Publisher rocket_sensor_pub;
    ros::Publisher fsm_pub;
public:
    float integration_period = 10e-3;

    IntegratorNode(ros::NodeHandle &nh) {
        // Initialize publishers and subscribers
        initTopics(nh);

        /* ---------- Variable initialization  ---------- */
        // Initialize rocket class with useful parameters
        rocket.init(nh);

        // Initialize fsm
        current_fsm.time_now = 0;
        current_fsm.state_machine = "Idle";

        nh.param<double>("/environment/rail_length", rail_length, 0);

        // Initialize external forces
        aero_control << 0, 0,
                0, 0,
                0, 0;

        rocket_control << 0, 0,
                0, 0,
                rocket.maxThrust[2], 0;

        //Get initial orientation and convert in Radians
        float roll = 0, zenith = 0, azimuth = 0.0;
        nh.getParam("/environment/rocket_roll", roll);
        nh.getParam("/environment/rail_zenith", zenith);
        nh.getParam("/environment/rail_azimuth", azimuth);

        roll *= M_PI / 180;
        zenith *= M_PI / 180;
        azimuth *= M_PI / 180;

        typedef EulerSystem<EULER_Z, EULER_Y, EULER_Z> Rail_system;
        typedef EulerAngles<double, Rail_system> angle_type;

        angle_type init_angle(azimuth, zenith, roll);

        Quaterniond q(init_angle);

        // Init state X
        X << 0, 0, 0, 0, 0, 0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, rocket.propellant_mass;
        X.segment(6, 4) = q.coeffs();

        // Init sensors
        rocket.sensor_acc = (q.inverse())._transformVector(
                Eigen::Vector3d::UnitZ() * 3.986e14 / pow(6371e3 + rocket.h0, 2));
        rocket.sensor_gyro << 0.0, 0.0, 0.0;
        rocket.sensor_baro = 0;

        xout = X;
    }

    void initTopics(ros::NodeHandle &nh) {
        // Subscribe to commands
        command_sub = nh.subscribe("commands", 10, &IntegratorNode::processCommand, this);
        // Subscribe to control message from control node
        rocket_control_sub = nh.subscribe("control_measured", 100,
                                          &IntegratorNode::rocketControlCallback, this);
        // Subscribe to aero message
        rocket_aero_sub = nh.subscribe("rocket_aero", 100, &IntegratorNode::rocketAeroCallback, this);
        // Subscribe to perturbations message
        rocket_perturbation_sub = nh.subscribe("disturbance_pub", 100,
                                               &IntegratorNode::rocketPerturbationCallback, this);

        // Create state publisher
        rocket_state_pub = nh.advertise<real_time_simulator::State>("rocket_state", 10);
        // Create fake sensors publisher
        rocket_sensor_pub = nh.advertise<real_time_simulator::Sensor>("simu_sensor_pub", 10);
        // Create timer publisher and associated thread (100Hz)
        fsm_pub = nh.advertise<real_time_simulator::FSM>("fsm_pub", 10);
    }

    void step() {
        // State machine ------------------------------------------
        if (current_fsm.state_machine.compare("Idle") == 0) {

        } else {
            // Update current time
            current_fsm.time_now = ros::Time::now().toSec() - time_zero;

            if (current_fsm.state_machine.compare("Rail") == 0) {
                auto dynamics_rail = [this](const Rocket::state &x, Rocket::state &xdot, const double &t) -> void {
                    rocket.dynamics_rail(x, xdot, rocket_control, aero_control, t);
                };
                stepper.do_step(dynamics_rail, X, 0, xout, 0 + integration_period);

                // End of rail -> Launch state
                if (X(2) > rail_length) {
                    current_fsm.state_machine = "Launch";
                }
            } else if (current_fsm.state_machine.compare("Launch") == 0) {
                auto dynamics_flight = [this](const Rocket::state &x, Rocket::state &xdot, const double &t) -> void {
                    rocket.dynamics_flight(x, xdot, rocket_control, aero_control, perturbation_control, t);
                };
                stepper.do_step(dynamics_flight, X, 0, xout, 0 + integration_period);

                // End of burn -> no more thrust
                if (X(13) < 0) {
                    current_fsm.state_machine = "Coast";
                }
            } else if (current_fsm.state_machine.compare("Coast") == 0) {
                rocket_control << 0, 0,
                        0, 0,
                        0, 0;
                auto dynamics_flight = [this](const Rocket::state &x, Rocket::state &xdot, const double &t) -> void {
                    rocket.dynamics_flight(x, xdot, rocket_control, aero_control, perturbation_control, t);
                };
                stepper.do_step(dynamics_flight, X, 0, xout, 0 + integration_period);
            }

            X = xout;

            rocket.update_CM(X(13));
        }

        // Parse state and publish it on the /fast_rocket_state topic
        real_time_simulator::State current_state;

        current_state.pose.position.x = X(0);
        current_state.pose.position.y = X(1);
        current_state.pose.position.z = X(2);

        current_state.twist.linear.x = X(3);
        current_state.twist.linear.y = X(4);
        current_state.twist.linear.z = X(5);

        current_state.pose.orientation.x = X(6);
        current_state.pose.orientation.y = X(7);
        current_state.pose.orientation.z = X(8);
        current_state.pose.orientation.w = X(9);

        current_state.twist.angular.x = X(10);
        current_state.twist.angular.y = X(11);
        current_state.twist.angular.z = X(12);

        current_state.propeller_mass = X(13);

        rocket_state_pub.publish(current_state);

        // Publish time + state machine
        fsm_pub.publish(current_fsm);

        //std::cout << "Fast integration time: " << 1000*(ros::Time::now().toSec()-time_now) << "ms \n";
    }

// Callback function to store last received control
    void rocketControlCallback(const real_time_simulator::Control::ConstPtr &control_law) {
        rocket_control << control_law->force.x, control_law->torque.x,
                control_law->force.y, control_law->torque.y,
                control_law->force.z, control_law->torque.z;
    }

// Callback function to store last received aero force and torque
    void rocketAeroCallback(const real_time_simulator::Control::ConstPtr &rocket_aero) {
        aero_control << rocket_aero->force.x, rocket_aero->torque.x,
                rocket_aero->force.y, rocket_aero->torque.y,
                rocket_aero->force.z, rocket_aero->torque.z;
    }

// Callback function to store last received aero force and torque
    void rocketPerturbationCallback(const real_time_simulator::Control::ConstPtr &perturbation) {
        perturbation_control << perturbation->force.x, perturbation->torque.x,
                perturbation->force.y, perturbation->torque.y,
                perturbation->force.z, perturbation->torque.z;
    }

    void processCommand(const std_msgs::String &command) {
        if (command.data.compare("Coast") == 0) {
            current_fsm.state_machine = "Coast";
        } else {
            //received launch command
            time_zero = ros::Time::now().toSec();
            if (rail_length == 0) current_fsm.state_machine = "Launch";
            else current_fsm.state_machine = "Rail";
        }
    }

    void send_fake_sensor() {
        // construct a trivial random generator engine from a time-based seed:
        unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
        std::default_random_engine generator(seed);

        std::normal_distribution<double> acc_noise(rocket.acc_bias, rocket.acc_noise);
        std::normal_distribution<double> gyro_noise(rocket.gyro_bias, rocket.gyro_noise);
        std::normal_distribution<double> baro_noise(rocket.baro_bias, rocket.baro_noise);

        real_time_simulator::Sensor sensor_msg;

        sensor_msg.IMU_acc.x = rocket.sensor_acc(0) + acc_noise(generator);
        sensor_msg.IMU_acc.y = rocket.sensor_acc(1) + acc_noise(generator);
        sensor_msg.IMU_acc.z = rocket.sensor_acc(2) + acc_noise(generator);

        sensor_msg.IMU_gyro.x = rocket.sensor_gyro(0) + gyro_noise(generator);
        sensor_msg.IMU_gyro.y = rocket.sensor_gyro(1) + gyro_noise(generator);
        sensor_msg.IMU_gyro.z = rocket.sensor_gyro(2) + gyro_noise(generator);

        sensor_msg.baro_height = rocket.sensor_baro + baro_noise(generator);

        rocket_sensor_pub.publish(sensor_msg);
    }
};

int main(int argc, char **argv) {
    /* ---------- ROS intitialization ---------- */
    // Init ROS fast integrator node
    ros::init(argc, argv, "integrator");
    ros::NodeHandle nh;

    IntegratorNode integrator_node(nh);

    // Thread to integrate state. Duration defines interval time in seconds
    ros::Timer integrator_thread = nh.createTimer(
            ros::Duration(integrator_node.integration_period), [&](const ros::TimerEvent &) {
                integrator_node.step();
            });

    double sensor_period;
    nh.getParam("/perturbation/sensor_period", sensor_period);
    ros::Timer sensor_thread = nh.createTimer(ros::Duration(sensor_period), [&](const ros::TimerEvent &) {
        integrator_node.send_fake_sensor();
    });

    // Automatic callback of service and publisher from here
    ros::spin();
}


/*
 Copyright (C) 2019 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.
*/

#include <boost/make_shared.hpp>
#include <boost/test/unit_test.hpp>
#include <ored/marketdata/marketimpl.hpp>
#include <ored/portfolio/builders/swap.hpp>
#include <ored/portfolio/enginedata.hpp>
#include <ored/portfolio/portfolio.hpp>
#include <ored/portfolio/swap.hpp>
#include <ored/utilities/indexparser.hpp>
#include <ored/utilities/log.hpp>
#include <oret/toplevelfixture.hpp>
#include <ql/cashflows/iborcoupon.hpp>
#include <ql/indexes/inflation/ukrpi.hpp>
#include <ql/termstructures/yield/discountcurve.hpp>
#include <ql/time/calendars/unitedkingdom.hpp>
#include <ql/time/daycounters/actual365fixed.hpp>
#include <ql/time/daycounters/actualactual.hpp>
#include <qle/time/yearcounter.hpp>

using namespace QuantLib;
using namespace boost::unit_test_framework;
using namespace ore::data;
using std::vector;

// Inflation CPI Swap paying fixed * CPI(t)/baseCPI * N
// with last XNL N * (CPI(T)/baseCPI - 1)

namespace {

class TestMarket : public MarketImpl {
public:
    TestMarket() {
        // valuation date
        asof_ = Date(28, August, 2018);

        // build vectors with dates and discount factors for GBP
        vector<Date> datesGBP = {asof_,
                                 asof_ + 6 * Months,
                                 asof_ + 7 * Months,
                                 asof_ + 8 * Months,
                                 asof_ + 9 * Months,
                                 asof_ + 10 * Months,
                                 asof_ + 11 * Months,
                                 asof_ + 12 * Months,
                                 asof_ + 13 * Months,
                                 asof_ + 14 * Months,
                                 asof_ + 15 * Months,
                                 asof_ + 16 * Months,
                                 asof_ + 17 * Months,
                                 asof_ + 18 * Months,
                                 asof_ + 19 * Months,
                                 asof_ + 20 * Months,
                                 asof_ + 21 * Months,
                                 asof_ + 22 * Months,
                                 asof_ + 23 * Months,
                                 asof_ + 2 * Years,
                                 asof_ + 3 * Years,
                                 asof_ + 4 * Years,
                                 asof_ + 5 * Years,
                                 asof_ + 6 * Years,
                                 asof_ + 7 * Years,
                                 asof_ + 8 * Years,
                                 asof_ + 9 * Years,
                                 asof_ + 10 * Years,
                                 asof_ + 15 * Years,
                                 asof_ + 20 * Years};

        vector<DiscountFactor> dfsGBP = {1,      0.9955, 0.9953, 0.9947, 0.9941, 0.9933, 0.9924, 0.9914,
                                         0.9908, 0.9901, 0.9895, 0.9888, 0.9881, 0.9874, 0.9868, 0.9862,
                                         0.9855, 0.9849, 0.9842, 0.9836, 0.9743, 0.9634, 0.9510, 0.9361,
                                         0.9192, 0.9011, 0.8822, 0.8637, 0.7792, 0.7079};

        // build GBP discount curve
        yieldCurves_[make_tuple(Market::defaultConfiguration, YieldCurveType::Discount, "GBP")] =
            intDiscCurve(datesGBP, dfsGBP, ActualActual(), UnitedKingdom());

        // build GBP Libor inde
        Handle<IborIndex> hGBP = Handle<IborIndex>(
            parseIborIndex("GBP-LIBOR-6M", intDiscCurve(datesGBP, dfsGBP, ActualActual(), UnitedKingdom())));
        iborIndices_[make_pair(Market::defaultConfiguration, "GBP-LIBOR-6M")] = hGBP;

    }

private:
    Handle<YieldTermStructure> intDiscCurve(vector<Date> dates, vector<DiscountFactor> dfs, DayCounter dc,
                                            Calendar cal) {
        boost::shared_ptr<YieldTermStructure> idc(
            new QuantLib::InterpolatedDiscountCurve<LogLinear>(dates, dfs, dc, cal));
        return Handle<YieldTermStructure>(idc);
    }
};
} // namespace

BOOST_FIXTURE_TEST_SUITE(OREDataTestSuite, ore::test::TopLevelFixture)

BOOST_AUTO_TEST_SUITE(ZeroCouponSwapTests)

BOOST_AUTO_TEST_CASE(testZeroCouponSwapPrice) {

    BOOST_TEST_MESSAGE("Testing Zero Coupon Swap Price...");

    // build market
    Date today(28, August, 2018);
    Settings::instance().evaluationDate() = today;
    boost::shared_ptr<TestMarket> market = boost::make_shared<TestMarket>();
    Date marketDate = market->asofDate();
    BOOST_CHECK_EQUAL(today, marketDate);
    Settings::instance().evaluationDate() = marketDate;

    // Test if GBP discount curve is empty
    Handle<YieldTermStructure> dts = market->discountCurve("GBP");
    QL_REQUIRE(!dts.empty(), "GBP discount curve not found");
    BOOST_CHECK_CLOSE(market->discountCurve("GBP")->discount(today + 1 * Years), 0.9914, 0.0001);

    // Test if GBP Libor curve is empty
    Handle<IborIndex> iis = market->iborIndex("GBP-LIBOR-6M");
    QL_REQUIRE(!iis.empty(), "GBP LIBOR 6M ibor Index not found");
    BOOST_TEST_MESSAGE(
        "CPISwap: Projected Libor fixing: " << market->iborIndex("GBP-LIBOR-6M")->forecastFixing(today + 1 * Years));

    // envelope
    Envelope env("CP");

    // Start/End date
    Size years = 5;
    Date startDate = today;
    Date endDate = today + years * Years;

    // date 2 string
    std::ostringstream oss;
    oss << io::iso_date(startDate);
    string start(oss.str());
    oss.str("");
    oss.clear();
    oss << io::iso_date(endDate);
    string end(oss.str());

    // Leg variables
    vector<Real> notional(1, 1000000);
    string paymentConvention = "MF";
    Real rate = 0.02;
    vector<Real> rates;
    rates.push_back(rate);

    // Schedule
    string conv = "MF";
    string rule = "Zero";
    ScheduleData scheduleData(ScheduleRules(start, end, "5y", "UK", conv, conv, rule));
    Schedule schedule = makeSchedule(scheduleData);
    BOOST_CHECK_EQUAL(schedule.dates().size(), 2);

    // GBP Libor Leg
    bool isPayerLibor = true;
    string indexLibor = "GBP-LIBOR-6M";
    LegData leg(boost::make_shared<ZeroCouponFixedLegData>(rates), isPayerLibor, "GBP",
                     scheduleData, "Year", notional, vector<string>(), paymentConvention);

    // Build swap trades
    vector<LegData> legData;
    legData.push_back(leg);
    boost::shared_ptr<Trade> swap(new ore::data::Swap(env, legData));

    // engine data and factory
    boost::shared_ptr<EngineData> engineData = boost::make_shared<EngineData>();
    engineData->model("Swap") = "DiscountedCashflows";
    engineData->engine("Swap") = "DiscountingSwapEngine";
    boost::shared_ptr<EngineFactory> engineFactory = boost::make_shared<EngineFactory>(engineData, market);
    engineFactory->registerBuilder(boost::make_shared<SwapEngineBuilder>());

    // build swaps and portfolio
    boost::shared_ptr<Portfolio> portfolio(new Portfolio());
    swap->id() = "Swap";

    portfolio->add(swap);
    portfolio->build(engineFactory);

    Date maturity = schedule.endDate();
    Date fixedPayDate = schedule.calendar().adjust(maturity, parseBusinessDayConvention(paymentConvention));
    Real df = market->discountCurve("GBP")->discount(fixedPayDate);
    Real expectedNPV = -1 * notional.back() * (pow(1.0 + rate, years) - 1.0) * df;
    BOOST_CHECK_CLOSE(swap->instrument()->NPV(), expectedNPV, 1E-8); // this is 1E-10 rel diff
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


#pragma once
#include <Eigen/Core>
#include <Eigen/SVD>
#include <vector>
#include <unordered_set>
#include <numeric>
#include <iterator>
#include <queue>
#include <iostream>

template <int _Dims>
class Optimize {
public:
    typedef Eigen::Array<float,_Dims,1> vars_type;
    typedef std::function<float(vars_type)> funk_type;

    Optimize(funk_type funk, vars_type min_bounds = -vars_type::Ones(), vars_type max_bounds = vars_type::Ones());
    ~Optimize() {};

    void setNumObservations(int num_observations) {num_observations_ = num_observations;};
    void setMaxDepth(int max_depth) {max_depth_ = max_depth;};
    void setThreshold(float threshold) {threshold_ = threshold;};
    void setMaxIterations(int max_iterations) {max_iterations_=max_iterations;};

    Optimize<_Dims>::vars_type run();

private:

    static int powint(int x, int p)//https://stackoverflow.com/a/1505791
    {
        if (p == 0) return 1;
        if (p == 1) return x;
        int tmp = powint(x, p/2);
        if (p%2 == 0) return tmp * tmp;
        else return x * tmp * tmp;
    }

    struct Split {
        int dimension;
        int left_index;
        int right_index;
        bool left_right; //Keep left or right side of split
        Split(int dim, int left_idx, int right_idx, bool lr) : dimension(dim), left_index(left_idx), right_index(right_idx), left_right(lr){};
    };

    struct Partition {
        std::vector<Split> splits;
        std::vector<int> elements;

        bool getClass(const Eigen::Matrix<float,-1,_Dims+1> & data, float threshold){
            int low = 0;
            for (auto ind : elements) {
                if (data(ind,data.cols()-1) < threshold)
                    low++;
            }
            return ((float) low / (float) elements.size())<=0.5;
        };

        void getBounds(const Eigen::Matrix<float,-1,_Dims+1> & data, vars_type & min, vars_type & max){
            for (auto split : splits){
                float x_left = data(split.left_index,split.dimension);
                float x_right = data(split.right_index,split.dimension);
                float x_mid = (x_left+x_right)/2;
                if (split.left_right) {
                    if (min(split.dimension) < x_mid)
                        min(split.dimension) = x_mid;
                } else {
                    if (max(split.dimension) > x_mid)
                        max(split.dimension) = x_mid;
                }
            }
        };
    };

    funk_type funk_;
    vars_type min_bounds_;
    vars_type max_bounds_;

    int num_observations_ = 125;
    float threshold_ = 3;
    int max_depth_ = 10;
    int max_iterations_ = 10;
    float tolerance_ = 1e-6;

    Eigen::Matrix<float,-1,_Dims+1> data_;
    Eigen::Matrix<float,_Dims,_Dims> householder_ = Eigen::Matrix<float,_Dims,_Dims>::Identity();

    //Sample randomly from bounds
    Eigen::Matrix<float,-1,_Dims+1> randSample(int n);
    Eigen::Matrix<float,-1,_Dims+1> randSampleLow(int n, const std::vector<vars_type> & mins, const std::vector<vars_type> & maxs);

    //Generates observations from random samples
    void generateData() {
        data_ = randSample(num_observations_);
    };
    void generateDataLow(const std::vector<vars_type> & mins, const std::vector<vars_type> & maxs) {
        data_ = randSampleLow(num_observations_, mins, maxs);
    };

    //Sorts along each dimension and stores permutation
    void sortData();
    std::array<std::vector<int>,_Dims> permutations_;

    float calculateGini(const std::vector<bool> & classified, int start, int end);
    int findSplit(const Partition & main_part, Partition & left_part, Partition & right_part);

    bool withinUnion(const vars_type & x, const std::vector<vars_type> & mins, const std::vector<vars_type> & maxs);

};

// Implementation
template <int _Dims>
Optimize<_Dims>::Optimize(Optimize::funk_type funk, Optimize::vars_type min_bounds, Optimize::vars_type max_bounds) :
    funk_(funk),
    min_bounds_(min_bounds),
    max_bounds_(max_bounds)
{
}

template <int _Dims>
typename Optimize<_Dims>::vars_type Optimize<_Dims>::run()
{
    //place box
    generateData();

    //partition box
    std::vector<int> v(num_observations_);
    std::iota(v.begin(),v.end(),0);

    vars_type current_min = vars_type::Zero();
    float current_min_f = std::numeric_limits<float>::max();

    for (int k = 0; k < max_iterations_; k++){
        //PCA to find dominant direction of hyper-ellipse
        Eigen::JacobiSVD<Eigen::Matrix<float,-1,_Dims>> svd(data_.block(0,0,num_observations_,_Dims), Eigen::ComputeThinU | Eigen::ComputeThinV);
        svd.computeV();
        vars_type dominant = svd.matrixV().template block<1,_Dims>(0,0).transpose();
        //Get Householder transformation matrix
        vars_type e1 = vars_type::Zero();
        e1(0) = 1;
        vars_type u = e1 - dominant;
        u.matrix().normalize();
        householder_ = Eigen::Matrix<float,_Dims,_Dims>::Identity() - 2*u.matrix() * u.matrix().transpose();

        //Apply Householder transformation
        bool no_transform = false;
        if (!std::isnan(householder_.norm()) && !std::isnan(data_.block(0,0,num_observations_,_Dims).norm())){
            data_.block(0,0,num_observations_,_Dims) = data_.block(0,0,num_observations_,_Dims).matrix() * householder_.transpose();
        } else {
            no_transform = true;
        }
        sortData();

        std::queue<Partition> partition_queue;
        std::vector<Partition> terminal;

        Partition initial;
        initial.elements = v;
        partition_queue.push(initial);

        int depth = 0;
        while (depth < max_depth_){
            std::queue<Partition> temp_queue;
            while (!partition_queue.empty()){
                auto part = partition_queue.front();
                partition_queue.pop();

                //Check stopping conditions
                if (part.elements.size() <= 1){
                    //terminal
                    terminal.push_back(part);
                    continue;
                }

                Partition left_part, right_part;
                int split_ret = this->findSplit(part, left_part, right_part);

                if (!split_ret) {
                    terminal.push_back(part);
                } else if (depth == max_depth_-1){
                    terminal.push_back(left_part);
                    terminal.push_back(right_part);
                } else {
                    temp_queue.push(left_part);
                    temp_queue.push(right_part);
                }
            }
            partition_queue = temp_queue;
            depth++;
        }

        std::vector<vars_type> minimums;
        std::vector<vars_type> maximums;
        //Find bounding box in new coordinates
        //We have 2^_Dims corners
        vars_type new_min = householder_.transpose() * min_bounds_.matrix();
        vars_type new_max = householder_.transpose() * max_bounds_.matrix();
        for(int i = 0; i < powint(2,_Dims); i++) {
            vars_type corner;
            for (int j = 0; j < _Dims; j++){
                if (i & powint(2,j))
                    corner(j) = max_bounds_(j);
                else
                    corner(j) = min_bounds_(j);
            }
            vars_type corner_transformed = householder_.transpose() * corner.matrix();
            for (int d = 0; d < _Dims; d++){
                if (corner_transformed(d) < new_min(d))
                    new_min(d) = corner_transformed(d);
                if (corner_transformed(d) > new_max(d))
                    new_max(d) = corner_transformed(d);
            }
        }
        for (auto t : terminal) {
            if (!t.getClass(data_,threshold_)){ //classified as low
                vars_type min = new_min;
                vars_type max = new_max;
                t.getBounds(data_, min, max);
                minimums.push_back(min);
                maximums.push_back(max);
            }
        }

        //Randomly sample in union of low partitions
        generateDataLow(minimums,maximums);

        //Apply inverse Householder transformation
        if (!no_transform){
            data_.block(0,0,num_observations_,_Dims) = data_.block(0,0,num_observations_,_Dims).matrix() * householder_;
        }

        for (int i = 0; i < num_observations_; i++){
            //Calculate f for new batch of points
            data_(i,_Dims) = funk_(data_.template block<1,_Dims>(i,0));
            //Check if f is new minimum
            if (data_(i,_Dims) < current_min_f){
                current_min = data_.template block<1,_Dims>(i,0);
                current_min_f = data_(i,_Dims);
            }
        }
    }
    return current_min;
}

template <int _Dims>
Eigen::Matrix<float,-1,_Dims+1> Optimize<_Dims>::randSample(int n)
{
    Eigen::Matrix<float,-1,_Dims+1> samples;
    samples.resize(n,_Dims+1);
    for (int i = 0; i < n; i++){
        vars_type x = vars_type::Random();//between -1 and 1
        x += 1;
        x /= 2;
        x *= max_bounds_-min_bounds_;
        x += min_bounds_;
        samples.template block<1,_Dims>(i,0) = x;
        samples(i,_Dims) = funk_(x);
    }
    return samples;
}

template <int _Dims>
Eigen::Matrix<float,-1,_Dims+1> Optimize<_Dims>::randSampleLow(int n, const std::vector<vars_type> & mins, const std::vector<vars_type> & maxs)
{
    Eigen::Matrix<float,-1,_Dims+1> samples;
    samples.resize(n,_Dims+1);

    //Create a new bounding box on which to sample
    // vars_type new_min_bounds = mins[0];
    // vars_type new_max_bounds = maxs[0];
    vars_type new_min_bounds = mins[0];
    vars_type new_max_bounds = maxs[0];

    for (int i = 1; i < mins.size(); i++){
        for (int d = 0; d < _Dims; d++){
            if (mins[i](d) < new_min_bounds(d))
                new_min_bounds(d) = mins[i](d);
            if (maxs[i](d) > new_max_bounds(d))
                new_max_bounds(d) = maxs[i](d);
        }
    }

    for (int i = 0; i < n; i++){
        vars_type x = vars_type::Random();//between -1 and 1
        x += 1;
        x /= 2;
        x *= new_max_bounds-new_min_bounds;
        x += new_min_bounds;
        while (!withinUnion(x, mins, maxs)){
            x = vars_type::Random();//between -1 and 1
            x += 1;
            x /= 2;
            x *= new_max_bounds-new_min_bounds;
            x += new_min_bounds;
        }
        samples.template block<1,_Dims>(i,0)= x;
    }
    return samples;
}

template <int _Dims>
float Optimize<_Dims>::calculateGini(const std::vector<bool> & classified, int start, int end)
{
    int num_low_int=0;
    for (int i = start; i < end; i++){
        if (!classified[i])
            num_low_int++;
    }
    float num_low = num_low_int;
    float num_total = end-start;

    return 2*num_low/num_total * (1 - num_low/num_total);
}

template <int _Dims>
int Optimize<_Dims>::findSplit(const Optimize<_Dims>::Partition & main_part, Optimize<_Dims>::Partition & left_part, Optimize<_Dims>::Partition & right_part)
{
    auto indices = main_part.elements;

    std::vector<int> v(data_.rows());
    std::iota(v.begin(),v.end(),0);

    //classify indices
    std::unordered_set<int> low;
    std::unordered_set<int> high;

    for (int i : indices){
        if (data_(i,_Dims) < threshold_)
            low.insert(i);
        else
            high.insert(i);
    }

    //partition
    float delta = 0;
    for (int dim = 0; dim < _Dims; dim++){
        auto p = permutations_[dim];
        //keep only indices in current partition
        for (auto it = p.begin(); it != p.end(); ){
            if (low.find(*it)==low.end() && high.find(*it)==high.end())
                it = p.erase(it);
            else
                ++it;
        }
        //sort indices in non-decreasing order along current dimension
        std::vector<int> sorted_inds(indices.size());
        std::transform(p.begin(), p.end(), sorted_inds.begin(), [&](int i){ return v[i]; });
         //get class at each index
        std::vector<bool> classified;
        for (int ind : sorted_inds){
            if (low.find(ind) == low.end())
                classified.push_back(true); //high
            else
                classified.push_back(false); //low
        }
        //find best split
        for (int i = 0; i < classified.size() - 1; i++){
            float gini_left = calculateGini(classified, 0, i+1);
            float gini_right = calculateGini(classified, i+1, classified.size());
            float gini_all = calculateGini(classified, 0, classified.size());

            float size_left = i+1;
            float size_right = classified.size() - i - 1;
            float size_all = classified.size();

            float d = gini_all - gini_left*size_left/size_all - gini_right*size_right/size_all;

            if (d > delta){
                delta = d;
                left_part = main_part;
                right_part = main_part;

                left_part.elements =std::vector<int>(sorted_inds.begin(),std::next(sorted_inds.begin(),i+1));
                right_part.elements = std::vector<int>(std::next(sorted_inds.begin(),i+1),sorted_inds.end());

                left_part.splits.push_back(Split(dim,sorted_inds[i],sorted_inds[i+1],false));
                right_part.splits.push_back(Split(dim,sorted_inds[i],sorted_inds[i+1],true));
            }
        }
    }
    //Return 0 if stopping condition has been met
    if (delta < tolerance_){
        return 0;
    }
    return 1;
}

template <int _Dims>
void Optimize<_Dims>::sortData()
{
    //Each vector in permutations_ should contain the sorted indices along a dimension
    for (int dim = 0; dim < _Dims; dim++){
        std::vector<float> xi(data_.rows());
        for (int i = 0; i < data_.rows(); i++)
            xi[i] = data_(i,dim);
        //sort in non-decreasing order
        std::vector<int> p(xi.size());
        std::iota(p.begin(), p.end(), 0);
        std::sort(p.begin(), p.end(), [&](std::size_t i, std::size_t j){ return xi[i] < xi[j]; });
        //store permutation
        permutations_[dim] = p;
    }
}

template <int _Dims>
bool Optimize<_Dims>::withinUnion(const vars_type & x, const std::vector<vars_type> & mins, const std::vector<vars_type> & maxs)
{
    for (int i = 0; i < mins.size(); i++){
        if ((x-mins[i] < 0).count() == 0 && (maxs[i]-x < 0).count() == 0){
            return true;
        }
    }
    return false;
}


#include "modules/neuralNetwork/layer/linear/linear.hpp"
#include "modules/neuralNetwork/neuralNetwork.hpp"

#ifdef _KORALI_USE_CUDNN
  #include "auxiliar/cudaUtils.hpp"
#endif

#ifdef _KORALI_USE_ONEDNN
  #include "auxiliar/dnnUtils.hpp"
using namespace dnnl;
#endif

#include <Eigen/Dense>
using namespace Eigen;

namespace korali
{
namespace neuralNetwork
{
namespace layer
{
;

void Linear::initialize()
{
  // Checking Layer size
  if (_outputChannels == 0) KORALI_LOG_ERROR("Node count for layer (%lu) should be larger than zero.\n", _index);

  // Checking position
  if (_index == 0) KORALI_LOG_ERROR("Feed Forward layers cannot be the starting layer of the NN\n");
  if (_index == _nn->_layers.size() - 1) KORALI_LOG_ERROR("Feed Forward layers cannot be the last layer of the NN\n");
}

std::vector<float> Linear::generateInitialHyperparameters()
{
  std::vector<float> hyperparameters;

  // If this is not the initial layer, calculate hyperparameters for weight and bias operation
  if (_prevLayer != nullptr)
  {
    // Setting value for this layer's xavier constant
    float xavierConstant = std::sqrt(6.0f) / std::sqrt(_outputChannels + _prevLayer->_outputChannels);

    // Adding layer's weights hyperparameter values
    for (size_t i = 0; i < _outputChannels; i++)
      for (size_t j = 0; j < _prevLayer->_outputChannels; j++)
        hyperparameters.push_back(_weightScaling * xavierConstant * _nn->_uniformGenerator->getRandomNumber());

    // Adding layer's bias hyperparameter values
    for (size_t i = 0; i < _outputChannels; i++)
      hyperparameters.push_back(0.0f);
  }

  return hyperparameters;
}

void Linear::createHyperparameterMemory()
{
  // Checking Layer sizes
  ssize_t OC = _outputChannels;
  ssize_t IC = _prevLayer->_outputChannels;

  // Setting hyperparameter count
  _hyperparameterCount = IC * OC + OC;

  if (_nn->_engine == "Korali")
  {
    _weightValues = (float *)malloc(IC * OC * sizeof(float));
    _biasValues = (float *)malloc(OC * sizeof(float));
  }

#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    memory::dims weightDims = {OC, IC};
    auto weightMemDesc = memory::desc(weightDims, memory::data_type::f32, memory::format_tag::ab);
    _weightsMem = memory(weightMemDesc, _nn->_dnnlEngine);

    auto biasMemDesc = memory::desc({OC}, memory::data_type::f32, memory::format_tag::a);
    _biasMem = memory(biasMemDesc, _nn->_dnnlEngine);
  }
#endif

#ifdef _KORALI_USE_CUDNN
  if (_nn->_engine == "CuDNN")
  {
    cudnnErrCheck(cudnnCreateFilterDescriptor(&_weightsFilterDesc));
    cudnnErrCheck(cudnnSetFilter4dDescriptor(_weightsFilterDesc, CUDNN_DATA_FLOAT, CUDNN_TENSOR_NCHW, OC, IC, 1, 1));
    cudaErrCheck(cudaMalloc((void **)&_weightsFilter, IC * OC * sizeof(float)));

    cudnnErrCheck(cudnnCreateTensorDescriptor(&_biasTensorDesc));
    cudnnErrCheck(cudnnSetTensor4dDescriptor(_biasTensorDesc, CUDNN_TENSOR_NCHW, CUDNN_DATA_FLOAT, 1, OC, 1, 1));
    cudaErrCheck(cudaMalloc((void **)&_biasTensor, OC * sizeof(float)));
  }
#endif
}

void Linear::copyHyperparameterPointers(Layer *dstLayer)
{
  Linear *dstPtr = dynamic_cast<Linear *>(dstLayer);
  dstPtr->_hyperparameterCount = _hyperparameterCount;

  if (_nn->_engine == "Korali")
  {
    dstPtr->_weightValues = _weightValues;
    dstPtr->_biasValues = _biasValues;
  }

#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    dstPtr->_weightsMem = _weightsMem;
    dstPtr->_biasMem = _biasMem;
  }
#endif

#ifdef _KORALI_USE_CUDNN
  if (_nn->_engine == "CuDNN")
  {
    dstPtr->_weightsFilterDesc = _weightsFilterDesc;
    dstPtr->_weightsFilter = _weightsFilter;
    dstPtr->_biasTensorDesc = _biasTensorDesc;
    dstPtr->_biasTensor = _biasTensor;
  }
#endif
}

void Linear::createForwardPipeline()
{
  // Calling base layer function
  Layer::createForwardPipeline();

#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    // We create the inner product (Wx + b) operation
    auto inner_product_d = inner_product_forward::desc(_propKind, _prevLayer->_outputMem[0].get_desc(), _weightsMem.get_desc(), _biasMem.get_desc(), _outputMem[0].get_desc());

    // Create inner product primitive descriptor.
    dnnl::primitive_attr forwardPrimitiveAttributes;
    _forwardInnerProductPrimitiveDesc = inner_product_forward::primitive_desc(inner_product_d, forwardPrimitiveAttributes, _nn->_dnnlEngine);

    // Create the weights+bias primitive.
    _forwardInnerProductPrimitive = inner_product_forward(_forwardInnerProductPrimitiveDesc);
  }
#endif

#ifdef _KORALI_USE_CUDNN
  if (_nn->_engine == "CuDNN")
  {
    // Creating convolution operator
    cudnnErrCheck(cudnnCreateConvolutionDescriptor(&_convolutionDesc));
    cudnnErrCheck(cudnnSetConvolution2dDescriptor(_convolutionDesc, 0, 0, 1, 1, 1, 1, CUDNN_CONVOLUTION, CUDNN_DATA_FLOAT));
    cudnnErrCheck(cudnnGetConvolutionForwardWorkspaceSize(_nn->_cuDNNHandle, _prevLayer->_outputTensorDesc, _weightsFilterDesc, _convolutionDesc, _outputTensorDesc, CUDNN_CONVOLUTION_FWD_ALGO_IMPLICIT_GEMM, &_convolutionWorkspaceSize));

    _convolutionWorkspace.resize(_nn->_timestepCount);
    for (size_t t = 0; t < _nn->_timestepCount; t++)
      cudaErrCheck(cudaMalloc((void **)&_convolutionWorkspace[t], _convolutionWorkspaceSize * sizeof(float)));
  }
#endif
}

void Linear::createBackwardPipeline()
{
  /*********************************************************************************
   *  Initializing memory objects and primitives for BACKWARD propagation
   *********************************************************************************/

  // Checking Layer sizes
  ssize_t OC = _outputChannels;
  ssize_t IC = _prevLayer->_outputChannels;

  // Calling base layer function
  Layer::createBackwardPipeline();

  if (_nn->_engine == "Korali")
  {
    _weightGradient = (float *)malloc(IC * OC * sizeof(float));
    _biasGradient = (float *)malloc(OC * sizeof(float));
  }

// Creating backward propagation primitives
#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    _weightsGradientMem = memory(_weightsMem.get_desc(), _nn->_dnnlEngine);
    _biasGradientMem = memory(_biasMem.get_desc(), _nn->_dnnlEngine);

    auto backwardDataDesc = inner_product_backward_data::desc(
      _prevLayer->_outputGradientMem[0].get_desc(),
      _weightsMem.get_desc(),
      _outputGradientMem[0].get_desc());

    // Create the primitive.
    auto backwardDataPrimitiveDesc = inner_product_backward_data::primitive_desc(backwardDataDesc, _nn->_dnnlEngine, _forwardInnerProductPrimitiveDesc);
    _backwardDataPrimitive = inner_product_backward_data(backwardDataPrimitiveDesc);

    auto backwardWeightsDesc = inner_product_backward_weights::desc(
      _prevLayer->_outputMem[0].get_desc(),
      _weightsMem.get_desc(),
      _biasMem.get_desc(),
      _outputGradientMem[0].get_desc());

    // Create the primitive.
    auto backwardWeightsPrimitiveDesc = inner_product_backward_weights::primitive_desc(backwardWeightsDesc, _nn->_dnnlEngine, _forwardInnerProductPrimitiveDesc);
    _backwardWeightsPrimitive = inner_product_backward_weights(backwardWeightsPrimitiveDesc);
  }
#endif

#ifdef _KORALI_USE_CUDNN
  if (_nn->_engine == "CuDNN")
  {
    cudaErrCheck(cudaMalloc((void **)&_weightsGradientFilter, IC * OC * sizeof(float)));
    cudaErrCheck(cudaMalloc((void **)&_biasGradientTensor, OC * sizeof(float)));
  }
#endif
}

void Linear::forwardData(const size_t t)
{
  size_t N = _batchSize;
  size_t IC = _prevLayer->_outputChannels;
  size_t OC = _outputChannels;

  if (_nn->_engine == "Korali")
  {
    // Performing Wx computation
    Map<MatrixXf> matA(_weightValues, IC, OC);
    Map<MatrixXf> matB(_prevLayer->_outputValues, IC, N);
    Map<MatrixXf> matC(_outputValues, OC, N);

    matC = matA.transpose() * matB;

    // Adding Bias
    for (size_t i = 0; i < N; i++)
      for (size_t j = 0; j < OC; j++)
        _outputValues[i * OC + j] += _biasValues[j];
  }

#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    // Arguments to the inner product operation
    std::unordered_map<int, dnnl::memory> forwardInnerProductArgs;
    forwardInnerProductArgs[DNNL_ARG_SRC] = _prevLayer->_outputMem[t];
    forwardInnerProductArgs[DNNL_ARG_WEIGHTS] = _weightsMem;
    forwardInnerProductArgs[DNNL_ARG_BIAS] = _biasMem;
    forwardInnerProductArgs[DNNL_ARG_DST] = _outputMem[t];

    _forwardInnerProductPrimitive.execute(_nn->_dnnlStream, forwardInnerProductArgs);
  }
#endif

#ifdef _KORALI_USE_CUDNN
  if (_nn->_engine == "CuDNN")
  {
    float alpha1 = 1.0f;
    float alpha2 = 0.0f;
    cudnnErrCheck(cudnnConvolutionForward(
      _nn->_cuDNNHandle,
      &alpha1,
      _prevLayer->_outputTensorDesc,
      _prevLayer->_outputTensor[t],
      _weightsFilterDesc,
      _weightsFilter,
      _convolutionDesc,
      CUDNN_CONVOLUTION_FWD_ALGO_IMPLICIT_GEMM,
      _convolutionWorkspace[t],
      _convolutionWorkspaceSize,
      &alpha2,
      _outputTensorDesc,
      _outputTensor[t]));

    float alpha = 1.0f;
    float beta = 1.0f;
    cudnnAddTensor(_nn->_cuDNNHandle, &alpha, _biasTensorDesc, _biasTensor, &beta, _outputTensorDesc, _outputTensor[t]);
  }
#endif
}

void Linear::backwardData(const size_t t)
{
  int N = _batchSize;
  int IC = _prevLayer->_outputChannels;
  int OC = _outputChannels;

  if (_nn->_mode == "Inference")
    KORALI_LOG_ERROR("Requesting Layer backward data propagation but NN was configured for inference only.\n");

  if (_nn->_engine == "Korali")
  {
    // Backward propagating Wx+b operation
    Map<MatrixXf> matA(_weightValues, IC, OC);
    Map<MatrixXf> matB(_outputGradient, OC, N);
    Map<MatrixXf> matC(_prevLayer->_outputGradient, IC, N);

    matC = matA * matB;
  }

#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    _backwardDataArgs[DNNL_ARG_DIFF_DST] = _outputGradientMem[t];             // Input
    _backwardDataArgs[DNNL_ARG_WEIGHTS] = _weightsMem;                        // Input
    _backwardDataArgs[DNNL_ARG_DIFF_SRC] = _prevLayer->_outputGradientMem[t]; // Output

    _backwardDataPrimitive.execute(_nn->_dnnlStream, _backwardDataArgs);
  }
#endif

#ifdef _KORALI_USE_CUDNN
  if (_nn->_engine == "CuDNN")
  {
    float alpha = 1.0f;
    float beta = 0.0f;
    cudnnErrCheck(cudnnConvolutionBackwardData(
      _nn->_cuDNNHandle,
      &alpha,
      _weightsFilterDesc,
      _weightsFilter,
      _outputTensorDesc,
      _outputGradientTensor[t],
      _convolutionDesc,
      CUDNN_CONVOLUTION_BWD_DATA_ALGO_0,
      _convolutionWorkspace[t],
      _convolutionWorkspaceSize,
      &beta,
      _prevLayer->_outputTensorDesc,
      _prevLayer->_outputGradientTensor[t]));
  }
#endif
}

void Linear::backwardHyperparameters(size_t t)
{
  const size_t N = _batchSize;
  const size_t IC = _prevLayer->_outputChannels;
  const size_t OC = _outputChannels;

  if (_nn->_mode == "Inference")
    KORALI_LOG_ERROR("Requesting Layer hyperparameter gradient propagation but NN was configured for inference only.\n");

  if (_nn->_engine == "Korali")
  {
    // Performing Weight gradient calculation
    Map<MatrixXf> matA(_prevLayer->_outputValues, IC, N);
    Map<MatrixXf> matB(_outputGradient, OC, N);
    Map<MatrixXf> matC(_weightGradient, IC, OC);

    matC = matA * matB.transpose();

    // Setting the bias values to all minibatch inputs
    for (size_t j = 0; j < OC; j++) _biasGradient[j] = _outputGradient[0 * OC + j];
    for (size_t i = 1; i < N; i++)
      for (size_t j = 0; j < OC; j++) _biasGradient[j] += _outputGradient[i * OC + j];
  }

#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    // Arguments for the backward propagation of the gradient wrt Weights and Biases
    std::unordered_map<int, dnnl::memory> backwardWeightsArgs;
    backwardWeightsArgs[DNNL_ARG_SRC] = _prevLayer->_outputMem[t];    // Input
    backwardWeightsArgs[DNNL_ARG_DIFF_DST] = _outputGradientMem[t];   // Input
    backwardWeightsArgs[DNNL_ARG_DIFF_WEIGHTS] = _weightsGradientMem; // Output
    backwardWeightsArgs[DNNL_ARG_DIFF_BIAS] = _biasGradientMem;       // Output

    _backwardWeightsPrimitive.execute(_nn->_dnnlStream, backwardWeightsArgs);
  }
#endif

#ifdef _KORALI_USE_CUDNN
  if (_nn->_engine == "CuDNN")
  {
    float alpha = 1.0f;
    float beta = 0.0f;

    cudnnErrCheck(cudnnConvolutionBackwardBias(
      _nn->_cuDNNHandle,
      &alpha,
      _outputTensorDesc,
      _outputGradientTensor[t],
      &beta,
      _biasTensorDesc,
      _biasGradientTensor));

    cudnnErrCheck(cudnnConvolutionBackwardFilter(
      _nn->_cuDNNHandle,
      &alpha,
      _prevLayer->_outputTensorDesc,
      _prevLayer->_outputTensor[t],
      _outputTensorDesc,
      _outputGradientTensor[t],
      _convolutionDesc,
      CUDNN_CONVOLUTION_BWD_FILTER_ALGO_0,
      _convolutionWorkspace[t],
      _convolutionWorkspaceSize,
      &beta,
      _weightsFilterDesc,
      _weightsGradientFilter));
  }
#endif
}

void Linear::setHyperparameters(float *hyperparameters)
{
  size_t IC = _prevLayer->_outputChannels;
  size_t OC = _outputChannels;

  if (_nn->_engine == "Korali")
  {
    memcpy(_weightValues, &hyperparameters[0], IC * OC * sizeof(float));
    memcpy(_biasValues, &hyperparameters[IC * OC], OC * sizeof(float));
  }

#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    write_to_dnnl_memory(&hyperparameters[0], _weightsMem);
    write_to_dnnl_memory(&hyperparameters[IC * OC], _biasMem);
  }
#endif

#ifdef _KORALI_USE_CUDNN
  if (_nn->_engine == "CuDNN")
  {
    cudaErrCheck(cudaMemcpy(_weightsFilter, &hyperparameters[0], IC * OC * sizeof(float), cudaMemcpyHostToDevice));
    cudaErrCheck(cudaMemcpy(_biasTensor, &hyperparameters[IC * OC], OC * sizeof(float), cudaMemcpyHostToDevice));
  }
#endif
}

void Linear::getHyperparameters(float *hyperparameters)
{
  size_t IC = _prevLayer->_outputChannels;
  size_t OC = _outputChannels;

  if (_nn->_engine == "Korali")
  {
    memcpy(&hyperparameters[0], _weightValues, IC * OC * sizeof(float));
    memcpy(&hyperparameters[IC * OC], _biasValues, OC * sizeof(float));
  }

#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    read_from_dnnl_memory(&hyperparameters[0], _weightsMem);
    read_from_dnnl_memory(&hyperparameters[IC * OC], _biasMem);
  }
#endif

#ifdef _KORALI_USE_CUDNN
  if (_nn->_engine == "CuDNN")
  {
    cudaErrCheck(cudaMemcpy(&hyperparameters[0], _weightsFilter, IC * OC * sizeof(float), cudaMemcpyDeviceToHost));
    cudaErrCheck(cudaMemcpy(&hyperparameters[IC * OC], _biasTensor, OC * sizeof(float), cudaMemcpyDeviceToHost));
  }
#endif
}

void Linear::getHyperparameterGradients(float *gradient)
{
  size_t IC = _prevLayer->_outputChannels;
  size_t OC = _outputChannels;

  if (_nn->_engine == "Korali")
  {
    memcpy(&gradient[0], _weightGradient, IC * OC * sizeof(float));
    memcpy(&gradient[IC * OC], _biasGradient, OC * sizeof(float));
  }

#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    read_from_dnnl_memory(&gradient[0], _weightsGradientMem);
    read_from_dnnl_memory(&gradient[IC * OC], _biasGradientMem);
  }
#endif

#ifdef _KORALI_USE_CUDNN
  if (_nn->_engine == "CuDNN")
  {
    cudaErrCheck(cudaMemcpy(&gradient[0], _weightsGradientFilter, IC * OC * sizeof(float), cudaMemcpyDeviceToHost));
    cudaErrCheck(cudaMemcpy(&gradient[IC * OC], _biasGradientTensor, OC * sizeof(float), cudaMemcpyDeviceToHost));
  }
#endif
}

void Linear::setConfiguration(knlohmann::json& js)
{
 if (isDefined(js, "Results"))  eraseValue(js, "Results");

 Layer::setConfiguration(js);
 _type = "layer/linear";
 if(isDefined(js, "Type")) eraseValue(js, "Type");
 if(isEmpty(js) == false) KORALI_LOG_ERROR(" + Unrecognized settings for Korali module: linear: \n%s\n", js.dump(2).c_str());
}

void Linear::getConfiguration(knlohmann::json& js)
{

 js["Type"] = _type;
 Layer::getConfiguration(js);
}

void Linear::applyModuleDefaults(knlohmann::json& js)
{

 std::string defaultString = "{}";
 knlohmann::json defaultJs = knlohmann::json::parse(defaultString);
 mergeJson(js, defaultJs);
 Layer::applyModuleDefaults(js);
}

void Linear::applyVariableDefaults()
{

 Layer::applyVariableDefaults();
}

;

} //layer
} //neuralNetwork
} //korali
;


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2013-2015 Rokko Developers https://github.com/t-sakashita/rokko
*
* 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 <rokko/solver.hpp>
#include <rokko/collective.hpp>
#include <rokko/utility/frank_matrix.hpp>

#define BOOST_TEST_MODULE test_gather
#ifndef BOOST_TEST_DYN_LINK
#include <boost/test/included/unit_test.hpp>
#else
#include <boost/test/unit_test.hpp>
#endif
#include <boost/random.hpp>

template<typename GRID_MAJOR, typename DIST_MAT_MAJOR, typename LOC_MAT_MAJOR>
bool run_test(MPI_Comm comm, int dim, GRID_MAJOR const& grid_major, DIST_MAT_MAJOR const&, LOC_MAT_MAJOR const&) {
  int size, rank;
  MPI_Comm_size(comm, &size);
  MPI_Comm_rank(comm, &rank);

  // same seed for all processes
  boost::mt19937 eng;
  boost::variate_generator<boost::mt19937&, boost::uniform_real<> >
    rng(eng, boost::uniform_real<>());

  rokko::parallel_dense_solver solver(rokko::parallel_dense_solver::default_solver());
  rokko::grid g(comm, grid_major);
  rokko::distributed_matrix<double, DIST_MAT_MAJOR> mat(dim, dim, g, solver);
  for (int i = 0; i < dim; ++i) {
    for (int j = 0; j < dim; ++j) {
      double d = rng();
      if (mat.is_gindex(i, j)) mat.set_global(i, j, d);
    }
  }
#ifndef NDEBUG
  mat.print();
#endif

  int success_local = 1;
  rokko::localized_matrix<double, LOC_MAT_MAJOR> lmat(dim, dim);
  for (int r = 0; r < size; ++r) {
    rokko::gather(mat, lmat, r);
#ifndef NDEBUG
    if (rank == r) {
      std::cout << lmat << std::endl;
      std::cout.flush();
    }
    MPI_Barrier(comm);
#endif
  }
  for (int i = 0; i < dim; ++i) {
    for (int j = 0; j < dim; ++j) {
      if (mat.is_gindex(i, j) && std::abs(mat.get_global(i, j) - lmat(i, j)) >  10e-12)
          success_local = 0;
    }
  }

  int success;
  MPI_Allreduce(&success_local, &success, 1, MPI_INT, MPI_PROD, comm);
  BOOST_CHECK_EQUAL(success, true);
  return success;
}

BOOST_AUTO_TEST_CASE(test_gather) {
  MPI_Init(&boost::unit_test::framework::master_test_suite().argc,
           &boost::unit_test::framework::master_test_suite().argv);
  MPI_Comm comm = MPI_COMM_WORLD;
  int rank;
  MPI_Comm_rank(comm, &rank);

  int dim = 100;
  if (boost::unit_test::framework::master_test_suite().argc > 1) {
    dim = boost::lexical_cast<int>(boost::unit_test::framework::master_test_suite().argv[1]);
  }
  if (rank == 0) std::cout << "dimension = " << dim << std::endl;

  if (rank == 0) std::cout << "test for grid = col, dist = col, loc = col\n";
  run_test(comm, dim, rokko::grid_col_major, rokko::matrix_col_major(), rokko::matrix_col_major());

  /*
  if (rank == 0) std::cout << "test for grid = col, dist = col, loc = row\n";
  run_test(comm, dim, rokko::grid_col_major, rokko::matrix_col_major(), rokko::matrix_row_major());

  if (rank == 0) std::cout << "test for grid = col, dist = row, loc = col\n";
  run_test(comm, dim, rokko::grid_col_major, rokko::matrix_row_major(), rokko::matrix_col_major());

  if (rank == 0) std::cout << "test for grid = col, dist = row, loc = row\n";
  run_test(comm, dim, rokko::grid_col_major, rokko::matrix_row_major(), rokko::matrix_row_major());
  */

  if (rank == 0) std::cout << "test for grid = row, dist = col, loc = col\n";
  run_test(comm, dim, rokko::grid_row_major, rokko::matrix_col_major(), rokko::matrix_col_major());

  /*
  if (rank == 0) std::cout << "test for grid = row, dist = col, loc = row\n";
  run_test(comm, dim, rokko::grid_row_major, rokko::matrix_col_major(), rokko::matrix_row_major());

  if (rank == 0) std::cout << "test for grid = row, dist = row, loc = col\n";
  run_test(comm, dim, rokko::grid_row_major, rokko::matrix_row_major(), rokko::matrix_col_major());

  if (rank == 0) std::cout << "test for grid = row, dist = row, loc = row\n";
  run_test(comm, dim, rokko::grid_row_major, rokko::matrix_row_major(), rokko::matrix_row_major());
  */

  MPI_Finalize();
}


#include "aabb.h"
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace Eigen;
using namespace std;

aabb::aabb(const vec& low_bd, const vec& up_bd){
  low_bd_ = low_bd;
  up_bd_ = up_bd;
}
aabb::aabb(const aabb& other){
  low_bd_ = other.low_bd_;
  up_bd_ = other.up_bd_;
}
void aabb::merge(const aabb& other){
  for(size_t i = 0; i < 3; ++i){
    if(low_bd_(i) > other.low_bd_(i))
      low_bd_(i) = other.low_bd_(i);
    if(up_bd_(i) < other.up_bd_(i))
      up_bd_(i) = other.up_bd_(i);
  }

  {//check
    for(size_t i = 0; i < 3; ++i){
      assert(low_bd_(i) <= other.low_bd_(i));
      assert(up_bd_(i) >= other.up_bd_(i));
    }
  }
}

tri_aabb::tri_aabb(const size_t& id, const vec& p1, const vec& p2, const vec& p3):id_(id){
  p_.col(0) = p1;
  p_.col(1) = p2;
  p_.col(2) = p3;
  id_ = id;
  low_bd_ = p1;
  up_bd_ = p1;
  for(size_t i = 0; i < 3; ++i){
    if(low_bd_(i) > p2(i))
      low_bd_(i) = p2(i);
    else if (up_bd_(i) < p2(i))
      up_bd_(i) = p2(i);

    if(low_bd_(i) > p3(i))
      low_bd_(i) = p3(i);
    else if (up_bd_(i) < p3(i))
      up_bd_(i) = p3(i);
  }

  center = (p1 + p2 + p3) / 3.0;

  vec one_edge = p2 - p1, other_edge = p3 - p1;
  normal_ = one_edge.cross(other_edge);
  // normal_(0) = (p2(1) - p1(1)) * (p3(2) - p1(2)) - (p2(2) - p1(2)) * (p3(1) - p1(1));
  // normal_(1) = (p2(0) - p1(0)) * (p3(2) - p1(2)) - (p2(2) - p1(2)) * (p3(0) - p1(0));
  // normal_(2) = (p2(0) - p1(0)) * (p3(1) - p1(1)) - (p2(1) - p1(2)) * (p3(0) - p1(0));

  double norm  = normal_.norm();
  if(norm > 1e-6)
    normal_ = normal_ / norm;
  else
    normal_ = vec::Zero();
  d_ = -normal_.dot(p1) ;

}
tri_aabb::tri_aabb(const size_t& id, const tri& plane, const tri& n){
  id_ = id;
  p_ = plane;
  n_ = n;
  vec p1 = plane.col(0), p2 = plane.col(1), p3 = plane.col(2);
  low_bd_ = p1;
  up_bd_ = p1;
  for(size_t i = 0; i < 3; ++i){
    if(low_bd_(i) > p2(i))
      low_bd_(i) = p2(i);
    if (up_bd_(i) < p2(i))
      up_bd_(i) = p2(i);

    if(low_bd_(i) > p3(i))
      low_bd_(i) = p3(i);
    if (up_bd_(i) < p3(i))
      up_bd_(i) = p3(i);
  }

  center = (p1 + p2 + p3) / 3.0;
  vec one_edge = p2 - p1, other_edge = p3 - p1;
  normal_ = one_edge.cross(other_edge);
  // normal_(0) = (p2(1) - p1(1)) * (p3(2) - p1(2)) - (p2(2) - p1(2)) * (p3(1) - p1(1));
  // normal_(1) = (p2(0) - p1(0)) * (p3(2) - p1(2)) - (p2(2) - p1(2)) * (p3(0) - p1(0));
  // normal_(2) = (p2(0) - p1(0)) * (p3(1) - p1(1)) - (p2(1) - p1(2)) * (p3(0) - p1(0));

  double norm  = normal_.norm();
  if(norm > 1e-6)
    normal_ = normal_ / norm;
  else
    normal_ = vec::Zero();
  d_ = -normal_.dot(p1) ;
}

aabb merge_tri_aabbs(const std::vector<std::shared_ptr<tri_aabb>> tri_aabbs){
  if(tri_aabbs.empty())
    throw runtime_error("tri_aabbs is empty");

  bool bk =false;
  if(tri_aabbs.size() == 90)
    bk = true;
  size_t num = 0;
  aabb bd_box(tri_aabbs[0]->low_bd_, tri_aabbs[0]->up_bd_);
  for(auto& one_bdbox : tri_aabbs){
    bd_box.merge(*one_bdbox);
    ++num;
  }

  {//check
    for(size_t i = 0; i < tri_aabbs.size(); ++i){
      for(size_t j = 0; j < 3; ++j){
        assert(tri_aabbs[i]->low_bd_(j) >= bd_box.low_bd_(j));
        assert(tri_aabbs[i]->up_bd_(j) <= bd_box.up_bd_(j));

      }
    }

  }

  return bd_box;
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
Copyright (C) 2015 CompatibL

This file is part of QuantLib, a free-software/open-source library
for financial quantitative analysts and developers - http://quantlib.org/

QuantLib is free software: you can redistribute it and/or modify it
under the terms of the QuantLib license.  You should have received a
copy of the license along with this program; if not, please email
<quantlib-dev@lists.sf.net>. The license is also available online at
<http://quantlib.org/license.shtml>.

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

#ifndef cl_adjoint_bond_portfolio_impl_hpp
#define cl_adjoint_bond_portfolio_impl_hpp
#pragma once

#include "utilities.hpp"
#include "adjointtestutilities.hpp"
#include "adjointtestbase.hpp"
#include <boost/timer.hpp>
#include <iostream>
#include <ql/quantlib.hpp>

using namespace QuantLib;
using namespace boost::unit_test_framework;

#define OUTPUT_FOLDER_NAME "AdjointBondPortfolio"

namespace
{
    enum
    {
#if defined CL_GRAPH_GEN
        // Number of points for dependency plots.
        pointNo = 100,
        // Number of points for performance plot.
        iterNo = 1000,
        // Step for portfolio size for performance testing .
        step = 10,
#else
        // Number of points for dependency plots.
        pointNo = 10,
        // Number of points for performance plot.
        iterNo = 30,
        // Step for portfolio size for performance testing .
        step = 10,
#endif
        // Defines performance accuracy. Its value is a minimum number
        // of calling of O(1) complexity methods per one performance test.
        iterNumFactor = 0,
    };

    struct RateVariation
    {
        static std::deque<std::string > get_columns()
        {
            static std::deque<std::string > columns =
            {
                "Rate", ""
            };

            return columns;
        }

        template <typename stream_type>
        friend inline stream_type&
            operator << (stream_type& stm, RateVariation& v)
        {
                stm << v.inputRate_
                    << ";" << v.bondPortfolioPrice_ << std::endl;

                return stm;
            }

        Real inputRate_;
        Real bondPortfolioPrice_;
    };

    struct BondPortfolioData
    {
        BondPortfolioData() :
          calendar_(TARGET())
        , today_(calendar_.adjust(Date::todaysDate()))
        , frequency_(Semiannual)
        , bondDayCount_(Thirty360())
        , compounding_(Compounded)
        // Value of bond at maturity
        , faceAmount_(1000000.0)
        , issueMonth_(6)
        , length_(3)
        , settlementDays_(3)
        , coupon_(0.02)
        , accrualConvention_(Unadjusted)
        , paymentConvention_(ModifiedFollowing)
        , redemption_(100.0)
        {
        }

        // Method to create a bond object of a Quantlib FixedRateBond class.
        // It returns an instance of FixedRateBond class.
        FixedRateBond bondCreate()
        {
            Date issue = calendar_.advance(today_,
                                           issueMonth_, Months);

            //Maturity time for a bond.
            Date maturity = calendar_.advance(issue, length_, Years);
            Schedule sch(issue, maturity,
                         Period(frequency_), calendar_,
                         accrualConvention_, accrualConvention_,
                         DateGeneration::Backward, false);
            return FixedRateBond(settlementDays_, faceAmount_, sch,
                                  std::vector<Rate>(1, coupon_),
                                  bondDayCount_, paymentConvention_,
                                  redemption_, issue);
        }

        Real calculatePrice(Real rate)
        {
            return BondFunctions::cleanPrice(bondCreate(), rate,
                                                       bondDayCount_,
                                                       compounding_,
                                                       frequency_);
        }

        // Finite difference calculation of derivatives.
        //  - indep - vector of independent variables.
        //  - sfFinite - vector to store derivative values,
        // calculated by finite difference scheme.
        //  - h - step of approximation.
        void calculateCentralFinDiff(std::vector<cl::tape_double>& indep, std::vector<Real>& sfFinite, double h)
        {
            Size size = indep.size();
            sfFinite.resize(size);

            // Evaluate derivatives using step-forward and step-backward values using the formula:
            // Derivative = (stepforward - stepbackward) / (2 * h), h - step size.
            for (Size i = 0; i < size; i++)
            {
                // Calculate approximate derivative:
                sfFinite[i] = (calculatePrice(indep[i] + h) - calculatePrice(indep[i] - h)) / (2 * h);
            }
        }

        Calendar calendar_;
        Date today_;
        Real faceAmount_;
        Frequency frequency_;
        DayCounter bondDayCount_;
        Compounding compounding_;
        Integer issueMonth_;
        Integer length_;
        Natural settlementDays_;
        Real coupon_;
        BusinessDayConvention accrualConvention_;
        BusinessDayConvention paymentConvention_;
        Real redemption_;
    };

    struct TestData
        : public BondPortfolioData
    {
        struct Test
        : public cl::AdjointTest<Test>
        {
            Test(Size size, TestData* data)
            : size_(size)
            , data_(data)
            , rate_(size)
            , portfolioPrice_()
            {
                setLogger(&data_->outPerform_);

                if (size_ != 1)
                {
                    for (Size i = 0; i < size_; i++)
                    {
                        rate_[i] = 0.03 + 0.001*i;
                    }
                }
                else
                {
                    rate_[0] = 0.030;
                }
            }

            Size indepVarNumber() { return size_; }

            Size depVarNumber() { return 1; }

            Size minPerfIteration() { return iterNumFactor; }

            void recordTape()
            {
                cl::Independent(rate_);
                calculatePortfolioPrice();
                f_ = std::make_unique<cl::tape_function<double>>(rate_, portfolioPrice_);
            }

            // Calculates price of portfolio and each option.
            void calculatePortfolioPrice()
            {
                portfolioPrice_.resize(1, 0);
                for (Size i = 0; i < size_; i++)
                    portfolioPrice_[0]+=data_->calculatePrice(rate_[i]);
            }

            // Calculates derivatives using finite difference method.
            void calcAnalytical()
            {
                double h = 1.0e-3;  // shift for finite diff. method
                analyticalResults_.resize(size_);
                data_->calculateCentralFinDiff(rate_, analyticalResults_, h);
            }

            double relativeTol() const { return 1e-2; }

            double absTol() const { return 1e-5; }

            Size size_;
            TestData* data_;
            std::vector<cl::tape_double> rate_;
            std::vector<cl::tape_double> portfolioPrice_;
        };

        TestData()
            : BondPortfolioData()
            , outPerform_(OUTPUT_FOLDER_NAME "//",
            { { "filename", "AdjointPerformance" }
            , { "not_clear", "Not" }
            , { "title", "Bond portfolio value differentiation performance with respect to yield rates " }
            , { "xlabel", "Number of bonds in a portfolio" }
            , { "ylabel", "Time (s)" }
            , { "line_box_width", "-5" }
            , { "smooth", "50" }
            , { "cleanlog", "true" }
            })
            , outAdjoint_(OUTPUT_FOLDER_NAME "//",
            { { "filename", "Adjoint" }
            , { "not_clear", "Not" }
            , { "title", "Bond portfolio value adjoint differentiation performance with respect to yield rates" }
            , { "ylabel", "Time (s)" }
            , { "xlabel", "Number of yield rates" }
            , { "smooth", "50" }
            , { "cleanlog", "false" }
            })
            , outSize_(OUTPUT_FOLDER_NAME "//",
            { { "filename", "TapeSize" }
            , { "not_clear", "Not" }
            , { "title", "Tape size dependence on number of yield rates" }
            , { "ylabel", "Memory (MB)" }
            , { "smooth", "50" }
            , { "cleanlog", "false" }
            })
            , out_(OUTPUT_FOLDER_NAME "//output",
            { { "filename", "BondPriceOnRate" }
            , { "not_clear", "Not" }
            , { "title", "Bond price dependence on yield rate" }
            , { "ylabel", "Bond value" }
            , { "smooth", "50" }
            , { "cleanlog", "false" }
            })
            {
            }

        bool makeOutput()
        {
            bool ok = true;
            if (pointNo > 0)
            {
                ok &= recordDependencePlot();
            }
            ok &= cl::recordPerformance(*this, iterNo, step);
            return ok;
        }

        std::shared_ptr<Test> getTest(size_t size)
        {
            return std::make_shared<Test>(size, this);
        }

        // Makes plots for strike sensitivity dependence.
        bool recordDependencePlot()
        {
            std::vector<RateVariation> outData(pointNo);
            auto test = getTest(pointNo);

            for (Size i = 0; i < pointNo; i++)
            {
                outData[i] = { test->rate_[i], test->data_->calculatePrice(test->rate_[i]) };
            }
            out_ << outData;
            return true;
        }

        cl::tape_empty_test_output outPerform_;
        cl::tape_empty_test_output outAdjoint_;
        cl::tape_empty_test_output outSize_;
        cl::tape_empty_test_output out_;
    };

    typedef TestData::Test BondPortfolioTest;
}
#endif


#include "MexPackUnpack.h"
#include "mex.h"
#include <Eigen>
#include <iostream>

using namespace MexPackUnpackTypes;

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {

  // Create an unpacker with template parameters corresponding to what we expect from matlab
  // argument 1: Scalar double
  // argument 2: EDRM (Eigen map which is basically an Eigen matrix wrapping an existing pointer)

  MexUnpacker<double,  EDRM> my_unpack(nrhs, prhs);//We need the pointers to the inputs from matlab/octave

  try {

    auto [a, b] = my_unpack.unpackMex(); //  a is double, b is Eigen::Map<Eigen::MatrixXd> (which EDRM aliases)
                                         //  Note that in general one must be careful using auto with eigen
                                         //  objects due to its use of expression templates. However, in
                                         //  this case auto correctly deduces returned eigen objects types.

    Eigen::MatrixXd c=a*b; //Eigen matrices have arithmetic operations overloaded
    MexPacker<Eigen::MatrixXd> my_pack(nlhs, plhs); //Create a packing object
    my_pack.PackMex(c); //Return this object to matlab

// unpackMex can throw if the matlab objects passed at run time
// are not compatible with what is expected
  } catch (std::string s) {
    mexPrintf(s.data());
  }

}


///////////////////////////////////////////////////////////////////////////////
// cells_count.hpp                                                           //
//                                                                           //
//  Copyright 2010 Erwann Rogard. 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_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_CELLS_CELLS_COUNT_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_CELLS_CELLS_COUNT_HPP_ER_2010
#include <boost/mpl/for_each.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/detail/wrapper.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/factor/levels.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace contingency_table{
namespace cells_aux{

    template<typename A>
    struct count{

        typedef std::size_t size_;
        typedef void result_type;

        count(const A& acc,size_& n)
        :acc_( acc ),n_( n ){}

        template<typename Key>
        void operator()(const boost::mpl::detail::wrapper<Key>& u){
             this->n_ *= contingency_table::levels<Key>( this->acc_ ).size();
        }

        const A& acc_;
        size_& n_;
    };
}// cells_aux

  	template<typename Keys,typename AccSet>
    std::size_t cells_count(AccSet const& acc){
        namespace ns = statistics::detail;
        typedef boost::mpl::detail::wrapper<boost::mpl::_> op_;
        typedef cells_aux::count<AccSet> fun_;
        std::size_t n = 1;
        boost::mpl::for_each<Keys,op_>(
            fun_( acc, n )
        );
        return n;
    }

}// contingency_table
}// detail
}// statistics
}// boost

#endif


#include <deal.II/base/function.h>
#include <deal.II/base/function_parser.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/parameter_handler.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/timer.h>
#include <deal.II/base/table_handler.h>

#include <deal.II/lac/constraint_matrix.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/petsc_parallel_sparse_matrix.h>
#include <deal.II/lac/petsc_parallel_vector.h>
#include <deal.II/lac/petsc_solver.h>
#include <deal.II/lac/petsc_precondition.h>
#include <deal.II/lac/sparsity_tools.h>
#include <deal.II/lac/vector.h>

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

#include <deal.II/distributed/grid_refinement.h>
#include <deal.II/distributed/tria.h>

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

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

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

// #include <boost/program_options.hpp>

#include <mandy/function_tools.h>

#include <mandy/elastic_problem.h>
#include <mandy/piezoelectric_problem.h>

#include <fstream>
#include <iostream>

#include <algorithm>    // std::transform
#include <functional>   // std::plus

namespace aphex
{

  template <int dim>
  class Aphex
  {
  public:

    /**
     * Class constructor.
     */
    Aphex (const std::string &prm);

    /**
     * Class destructor.
     */
    ~Aphex ();

    /**
     * Run.
     */
    void run ();

  private:

    /**
     * MPI communicator.
     */
    MPI_Comm mpi_communicator;

    /**
     * A distributed grid on which all computations are done.
     */
    dealii::parallel::distributed::Triangulation<dim> triangulation;

    /**
     * Solution of the elastic problem.
     */
    dealii::PETScWrappers::MPI::Vector displacement;

    /**
     * Solution of the piezoelectric.
     */
    dealii::PETScWrappers::MPI::Vector piezoelectric_potential;

    /**
     * Parallel iostream.
     */
    dealii::ConditionalOStream pcout;

    /**
     * Stop clock.
     */
    dealii::TimerOutput timer;

  };

  template <int dim>
  Aphex<dim>::Aphex (const std::string &prm)
    :
    mpi_communicator (MPI_COMM_WORLD),
    triangulation (mpi_communicator,
		   typename dealii::Triangulation<dim>::MeshSmoothing
		   (dealii::Triangulation<dim>::smoothing_on_refinement |
		    dealii::Triangulation<dim>::smoothing_on_coarsening)),
    pcout (std::cout, (dealii::Utilities::MPI::this_mpi_process (mpi_communicator) == 0)),
    timer (mpi_communicator, pcout,
     	   dealii::TimerOutput::summary,
     	   dealii::TimerOutput::wall_times)
  {}

  template <int dim>
  Aphex<dim>::~Aphex ()
  {}

  template <int dim>
  void
  Aphex<dim>::run ()
  {

    try
      {

	dealii::GridGenerator::hyper_cube (triangulation, -10, 10);
	// triangulation.refine_global (parameters.get_integer ("Global mesh refinement steps"));
	triangulation.refine_global (2);

	{
	  dealii::TimerOutput::Scope time (timer, "material");
	  mandy::FunctionTools<3> material (triangulation, "material.prm");
	  material.run ();
	}

	{
	  dealii::TimerOutput::Scope time (timer, "elastic problem");
	  mandy::ElasticProblem<3> elastic_problem (triangulation, displacement,
						    mpi_communicator, "elastic.prm");
	  elastic_problem.run ();
	}

	{
	  dealii::TimerOutput::Scope time (timer, "piezoelectric problem");
	  mandy::PiezoelectricProblem<3> piezoelectric_problem (triangulation, displacement,
								mpi_communicator, "piezoelectric.prm");
	  piezoelectric_problem.run ();
	}

      }

    catch (std::exception &exc)
      {
	std::cerr << std::endl << std::endl
		  << "----------------------------------------------------"
		  << std::endl;
	std::cerr << "Exception on processing: " << std::endl
		  << exc.what() << std::endl
		  << "Aborting!" << std::endl
		  << "----------------------------------------------------"
		  << std::endl;
      }

    catch (...)
      {
	std::cerr << std::endl << std::endl
		  << "----------------------------------------------------"
		  << std::endl;
	std::cerr << "Unknown exception!" << std::endl
		  << "Aborting!" << std::endl
		  << "----------------------------------------------------"
		  << std::endl;
      }

  } // run ()

} // namespace aphex

/**
 * Main function: Initialise problem and run it.
 */
int main (int argc, char *argv[])
{

  // Initialise MPI
  dealii::Utilities::MPI::MPI_InitFinalize mpi_initialization (argc, argv, 1);

  try
    {
#ifdef INCLUDED_EXTERNAL_BOOST
      // Parse the commandline.
      boost::program_options::options_description description {"Options"};
      description.add_options ()
	("help", "Help screen")
	("prm", value<string> ()->default_value ("aphex.prm"), "Parameter file");

      boost::program_options::variables_map vmap;
      boost::program_options::store
	(boost::program_options::parse_command_line (argc, argv, description), vmap);

      if (vmap.count ("help"))
	std::cout << description
		  << std::endl;

      else if (vmap.count ("prm"))
	std::cout << "Parameter file: " << vmap["prm"].as<string>
		  << std::endl;

      // else if (...)
#endif

      std::vector<std::string> args (argv+1, argv+argc);

      AssertThrow (args.size ()>0, dealii::ExcMessage ("The number of input arguments must be greater than zero."));

      std::cout << std::endl << std::endl
		<< "----------------------------------------------------"
		<< std::endl
		<< "Caught arguments: ";
      for (unsigned int i=0; i<args.size (); ++i)
	std::cout << std::endl << "   " << args[i];
      std::cout  << std::endl
		 << "----------------------------------------------------"
		 << std::endl << std::endl;

      aphex::Aphex<3> aphex (args[1]);
      aphex.run ();
    }

  catch (std::exception &exc)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

      return 1;
    }

  catch (...)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }

  return 0;
}


#include <iostream>
#include <chrono>
#include <vector>
#include <numeric>
#include <cmath>
#include "time_lapse.h"
#include <fstream>
#include "file_loader.h"
#include <boost/filesystem.hpp>
#include <sstream>
#include <unordered_set>
#include <random>

extern "C" {
    #include "croaring/roaring.h"
}

using timer = measure<std::chrono::nanoseconds>;
using String = std::string;

template <class T>
using Vec2d = std::vector<std::vector<T>>;

namespace fs = boost::filesystem;

String get_cpu_name(){
    FILE* pipe = popen("cat /proc/cpuinfo | grep 'model name' | sed -n 1p | cut -d ':' -f 2 | awk '{$1=$1};1' | tr -d '\n'", "r");
    char buffer[128];
    String result;
    while(!feof(pipe)) {
        if(fgets(buffer, 128, pipe) != nullptr)
            result += buffer;
    }
    pclose(pipe);
    return result;
}

double avg(std::vector<uint16_t > const& v) {
    return 1.0 * std::accumulate(v.begin(), v.end(), 0LL) / v.size();
}

double stdev(std::vector<uint16_t > const& v, const double &avg){
    double accum = 0.0;
    std::for_each (std::begin(v), std::end(v), [&](const double d) {
        accum += (d - avg) * (d - avg);
    });
    auto denum = (v.size()-1) > 0 ? (v.size()-1) : 1;
    return std::sqrt(accum / denum);
}

long range(const uint16_t &min1, const uint16_t &max1, const uint16_t &min2, const uint16_t &max2){
    return std::max(max1, max2) - std::min(min1, min2);
}

void writeToFile(const String &dataset, const String &out_path){
    fs::path p{out_path};
    fs::path dir = p.parent_path();
    fs::create_directories(dir);

    std::ofstream file;
    file.open (out_path);
    file << dataset;
    file.close();
}

template<typename F>
String buildDataset(const Vec2d<uint16_t> &vals, const std::vector<String> &algoNames, F&& func){
    String dataset = "range,n1,average1,std1,n2,average2,std2";
    for(const auto &a : algoNames) {
        dataset += "," + a;
    }
    dataset += ",fastest_algo";
    dataset += "\n";

    //WriteTofile(append = false)
    std::cout <<  "computing stats..."  << std::endl;
    std::vector<double> avgs{};
    std::vector<double> stdevs{};
    std::vector<uint16_t > mins{};
    std::vector<uint16_t> maxs{};

    for(auto v : vals) {
        auto a = avg(v);
        avgs.push_back(a);
        stdevs.push_back(stdev(v, a));
        mins.push_back(*std::min_element(v.begin(), v.end()));
        maxs.push_back(*std::max_element(v.begin(), v.end()));
    }

    std::cout <<  "running algos.."  << std::endl;
    for(auto i = 0; i < vals.size(); i++){
        for(auto j = 0; j < vals.size(); j++){
            auto rng = range(mins[i], maxs[i], mins[j], maxs[j]);
            std::stringstream ss;
            auto times = fw(func)(vals[i], vals[j]);

            ss << rng << "," << vals[i].size() << "," << avgs[i] << "," << stdevs[i] << ","
                << vals[j].size() << "," << avgs[j] << "," << stdevs[j];
            for(auto t : times) {
                ss << "," << t;
            }
            auto it = std::find(times.begin(), times.end(), *std::min_element(times.begin(), times.end()));
            auto fastestAlgoIndex = std::distance(times.begin(), it);
            ss << "," << algoNames[fastestAlgoIndex];
            ss << "\n";
            dataset += ss.str();
        }
    }
    std::cout <<  "dataset done. \n"  << std::endl;
    return dataset;
}

std::vector<long> run_intersect_algos(const std::vector<uint16_t> &v1, const std::vector<uint16_t> &v2) {
    std::vector<long> times{};
    auto card = std::max(v1.size(), v2.size());
    auto *v3 = new uint16_t[card];

    auto time = timer::execution( intersect_skewed_uint16, v1.data(), v1.size(), v2.data(), v2.size(), v3);
    times.push_back(time);

    time = timer::execution( intersect_skewed_uint16, v2.data(), v2.size(), v1.data(), v1.size(), v3);
    times.push_back(time);

    time = timer::execution( intersect_uint16, v1.data(), v1.size(), v2.data(), v2.size(), v3);
    times.push_back(time);

    delete[] v3;
    return times;
}

std::vector<long> run_intersect_card_algos(const std::vector<uint16_t> &v1, const std::vector<uint16_t> &v2) {
    std::vector<long> times{};

    auto time = timer::execution( intersect_skewed_uint16_cardinality, v1.data(), v1.size(), v2.data(), v2.size());
    times.push_back(time);

    time = timer::execution( intersect_skewed_uint16_cardinality, v2.data(), v2.size(), v1.data(), v1.size());
    times.push_back(time);

    time = timer::execution( intersect_uint16_cardinality, v1.data(), v1.size(), v2.data(), v2.size());
    times.push_back(time);

    time = timer::execution( intersect_vector16_cardinality, v1.data(), v1.size(), v2.data(), v2.size());
    times.push_back(time);

    return times;
}

Vec2d <uint16_t> load_data(const String &data_path){
    size_t *howmany;
    size_t count;
    std::vector<std::vector<uint16_t>> vals{};

    std::cout <<  "Loading data...\n"  << std::endl;
    uint32_t **values = read_all_integer_files(data_path.c_str(), ".txt", &howmany, &count);

    for(auto i = 0; i < count; i++) {
        std::vector<uint16_t> v{};
        for(auto j = 0; j < howmany[i]; j++) {
            if (values[i][j] > std::numeric_limits<uint16_t>::max()) {
                v.push_back((uint16_t) (values[i][j] >> 16));
            }
            v.push_back((uint16_t) (values[i][j] & 0xFFFF));
        }
        vals.push_back(v);
    }
    for(auto i = 0; i < count; i++){
        delete[] values[i];
    }
    delete[] values;
    delete[] howmany;
    return vals;
}

Vec2d <uint16_t> generate_random_data(int nb_set){
    std::random_device rd;     // only used once to initialise (seed) engine
    std::mt19937 rng(rd());    // random-number engine used (Mersenne-Twister in this case)
    std::uniform_int_distribution<uint16_t> uni(1,UINT16_MAX); // guaranteed unbiased
    std::vector<std::vector<uint16_t>> vals{};

    for(auto i = 0; i < nb_set; i++){
        std::unordered_set<uint16_t > myset = {};
        auto random_length = uni(rng);
        while(myset.size() < random_length){
            auto random_uint16 = uni(rng);
            myset.insert(random_uint16);
        }
        vals.emplace_back(myset.begin(), myset.end());
    }

    return vals;
}

void test(){

}

int main() {
//    auto iter = 5;
//    auto totIter = 10000;
//    std::vector<uint16_t> mins{};
//    for(auto j = 0; j < totIter; j++){
//        std::vector<uint16_t> times{};
//        for(auto i = 0; i < iter; i++) {
//            times.push_back(timer::execution(test));
//        }
//        mins.push_back(*std::min_element(times.begin(), times.end()));
//    }
//    std::cout << "average: " << avg(mins) << std::endl;
//    std::cout << "stdev: " << stdev(mins, avg(mins)) << std::endl;
//    std::cout << "max: " << *std::max_element(mins.begin(), mins.end()) << std::endl;
//    std::cout << "min: " << *std::min_element(mins.begin(), mins.end()) << std::endl;

    fs::directory_iterator it{fs::system_complete("../realdata")};
    Vec2d <uint16_t> masterData{};
    auto gpu_name = get_cpu_name();
    auto i = 0;
    //Yeah.. that's how boost wants us to iterate in order to support older versions.
    for(; it != fs::directory_iterator(); ++it){
        auto dir = *it;
        auto dataName = dir.path().filename().string();
        std::cout <<  "Opening " << dataName << "..."  << std::endl;
        auto data = load_data(dir.path().string());

        masterData.insert(std::end(masterData), std::begin(data), std::end(data));

//        //We will need other sets separated in order to test the masterset
//        if(dataName != "test_data")
//            masterData.insert(std::end(masterData), std::begin(data), std::end(data));
//        else{
//            fs::path out_path = fs::system_complete("../results/" + gpu_name + "/" + dataName);
//
//            //Intersect_dataset
//            auto filename = "/Intersect_dataset.csv";
//            std::cout << "building " << filename << std::endl;
//            auto dataset = buildDataset(data, {"skewed_1", "skewed_2", "non_skewed"}, run_intersect_algos);
//            writeToFile(dataset, out_path.string() +  filename);
//
//            //Intersect_card_dataset
//            filename = "/Intersect_card_dataset.csv";
//            std::cout << "building " << filename << std::endl;
//            dataset = buildDataset(data, {"skewed_1", "skewed_2", "non_skewed", "vector"}, run_intersect_card_algos);
//            writeToFile(dataset, out_path.string() + filename);
//        }
        i++;
    }

    auto out_path = fs::system_complete("../results/" + gpu_name);

    //Intersect_masterset
    auto filename = "/Intersect_masterset.csv";
    std::cout << "building " << filename << std::endl;
    auto dataset = buildDataset(masterData, {"skewed_1", "skewed_2", "non_skewed"}, run_intersect_algos);
    writeToFile(dataset, out_path.string() +  filename);

    //Intersect_card_masterset
    filename = "/Intersect_card_masterset.csv";
    std::cout << "building " << filename << std::endl;
    dataset = buildDataset(masterData, {"skewed_1", "skewed_2", "non_skewed", "vector"}, run_intersect_card_algos);
    writeToFile(dataset, out_path.string() + filename);

    //Intersect_testset
    filename = "/Intersect_testset.csv";
    std::cout << "building " << filename << std::endl;
    dataset = buildDataset(generate_random_data(400), {"skewed_1", "skewed_2", "non_skewed"}, run_intersect_algos);
    writeToFile(dataset, out_path.string() +  filename);

    //Intersect_card_testset
    filename = "/Intersect_card_testset.csv";
    std::cout << "building " << filename << std::endl;
    dataset = buildDataset(generate_random_data(400), {"skewed_1", "skewed_2", "non_skewed", "vector"}, run_intersect_card_algos);
    writeToFile(dataset, out_path.string() + filename);

    return 0;
}


#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <chrono>

using namespace std;
using namespace cv;

void feature_matching(const Mat& img1, const Mat& img2,
    std::vector<KeyPoint>& keypoints1,
    std::vector<KeyPoint>& keypoints2,
    std::vector< DMatch >& matches)
    {
        Mat descriptors_1, descriptors_2;
        Ptr<FeatureDetector> detector = ORB::create();
        Ptr<DescriptorExtractor> descriptor = ORB::create();

        // compute FAST corners
        detector->detect(img1, keypoints1);
        detector->detect(img2, keypoints2);

        // compute BRIEF descriptor
        descriptor->compute(img1, keypoints1, descriptors_1);
        descriptor->compute(img2, keypoints2, descriptors_2);

        Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create(4);
        /*
        FLANNBASED = 1,
        BRUTEFORCE = 2,
        BRUTEFORCE_L1 = 3,
        BRUTEFORCE_HAMMING = 4,
        BRUTEFORCE_HAMMINGLUT = 5,
        BRUTEFORCE_SL2 = 6 */

        vector<DMatch> allmatches;
        matcher->match(descriptors_1, descriptors_2, allmatches);

        // Filter all match points
        // Find maximum and minimum distance
        double min_dist = 1000000, max_dist = 0;
        for ( int i = 0; i < descriptors_1.rows; i++ )
        {
            double dist = allmatches[i].distance;
            if ( dist < min_dist ) min_dist = dist;
            if ( dist > max_dist ) max_dist = dist;
        }

        cout << "max distance: " << max_dist << endl;
        cout << "min distance: " << min_dist << endl;

        for ( int i = 0; i < descriptors_1.rows; i++ )
        {
            if ( allmatches[i].distance <= max ( 2*min_dist, 30.0 ) )
            {
                matches.push_back ( allmatches[i] );
            }
        }
    }

Point2d pixel2cam ( const Point2d& p, const Mat& K )
{
    return Point2d
           (
               ( p.x - K.at<double> ( 0,2 ) ) / K.at<double> ( 0,0 ),
               ( p.y - K.at<double> ( 1,2 ) ) / K.at<double> ( 1,1 )
           );
}

void bundleAdjustment(
    const vector<Point3f> points_3d,
    const vector<Point2f> points_2d,
    const Mat &K,
    Mat &R, Mat &t){
        typedef g2o::BlockSolver< g2o::BlockSolverTraits<6, 3>> Block;
        // typedef g2o::BlockSolver_6_3 Block;

        Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>();

        // Routine in pose estimation
        Block *solver_ptr = new Block(linearSolver);
        g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
        g2o::SparseOptimizer optimizer;
        optimizer.setAlgorithm (solver);
        optimizer.setVerbose(true);

        // Vertex SE(3)
        g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // Camera pose SE(3)
        Eigen::Matrix3d R_mat;

        R_mat <<
            R.at<double> ( 0,0 ), R.at<double> ( 0,1 ), R.at<double> ( 0,2 ),
            R.at<double> ( 1,0 ), R.at<double> ( 1,1 ), R.at<double> ( 1,2 ),
            R.at<double> ( 2,0 ), R.at<double> ( 2,1 ), R.at<double> ( 2,2 );

        pose->setId(0);
        pose->setEstimate(g2o::SE3Quat(
            R_mat,
            Eigen::Vector3d(t.at<double> ( 0,0 ), t.at<double> ( 1,0 ), t.at<double> ( 2,0 ))
        ));     // SE3Quat is 6 dimensional, first three rotation, last three translation.
                // Actually it's using Quaternion
        optimizer.addVertex(pose);

        int index = 1;
        // landmarks, use g2o::VertexSBAPointXYZ
        for (const Point3f p:points_3d){
            g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();
            point->setId(index++);
            point->setEstimate(Eigen::Vector3d(p.x, p.y, p.z));   // easy to set up, only need Eigen::Vector3d
            point->setMarginalized(true);
            optimizer.addVertex(point);
        }

        // parameter: camera intrinsics. Routine setup
        g2o::CameraParameters* camera = new g2o::CameraParameters (
            K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0
        );
        camera->setId ( 0 );
        optimizer.addParameter ( camera );

        index = 1;
        // Error terms. use edge g2o::EdgeProjectXYZ2UV from 3d to 2d. Routine
        for(const Point2f p:points_2d){
            g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
            edge->setId(index);
            edge->setVertex(0, dynamic_cast<g2o::VertexSBAPointXYZ*> (optimizer.vertex(index++)));
            edge->setVertex(1, pose);    // One side to pose, another side to each landmark
            edge->setMeasurement(Eigen::Vector2d(p.x, p.y));
            edge->setParameterId(0, 0);
            edge->setInformation(Eigen::Matrix2d::Identity());  //  infomation matrix, covariance matrix
            optimizer.addEdge(edge);
        }

        chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
        // Setup and initial optimization. Routine
        optimizer.setVerbose ( true );
        optimizer.initializeOptimization();
        optimizer.optimize ( 100 );
        chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
        chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> ( t2-t1 );
        cout<<"optimization costs time: "<<time_used.count() <<" seconds."<<endl;

        cout<<endl<<"after optimization:"<<endl;
        cout<<"T="<<endl<<Eigen::Isometry3d ( pose->estimate() ).matrix() <<endl;

    }

int main(int argc, char** argv){

     if ( argc != 5 )
    {
        cout<<"usage: pose_estimation_3d2d img1 img2 depth1 depth2"<<endl;
        return 1;
    }

    Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );
    Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR );

    vector<KeyPoint> keypoints_1, keypoints_2;
    vector<DMatch> matches;
    feature_matching ( img_1, img_2, keypoints_1, keypoints_2, matches );
    cout<<"Total "<<matches.size() <<" Matching Points" <<endl;

    Mat d1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED );
    Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
    vector<Point3f> pts_3d;
    vector<Point2f> pts_2d;
    for ( DMatch m:matches )
    {
        ushort d = d1.ptr<unsigned short> (int ( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ];
        if ( d == 0 )
            continue;
        float dd = d/5000.0;
        Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );
        pts_3d.push_back ( Point3f ( p1.x*dd, p1.y*dd, dd ) );  // Find feature point's 3D coordinates from Depth image
        pts_2d.push_back ( keypoints_2[m.trainIdx].pt );
    }

    cout<<"3d-2d pairs: "<<pts_3d.size() <<endl;

    Mat r, t;
    solvePnP ( pts_3d, pts_2d, K, Mat(), r, t, false );
    Mat R;
    cv::Rodrigues ( r, R ); // Rodrigues formula: Rotation vector to rotation matrix

    cout<<"R="<<endl<<R<<endl;
    cout<<"t="<<endl<<t<<endl;

    cout<<"calling bundle adjustment"<<endl;

    bundleAdjustment ( pts_3d, pts_2d, K, R, t );
}