// Copyright © 2016-2021 Thomas Nagler and Thibault Vatter
//
// This file is part of the vinecopulib library and licensed under the terms of
// the MIT license. For a copy, see the LICENSE file in the root directory of
// vinecopulib or https://vinecopulib.github.io/vinecopulib/.

#include <boost/math/special_functions/log1p.hpp>
#include <vinecopulib/misc/tools_eigen.hpp>

namespace vinecopulib {
inline ClaytonBicop::ClaytonBicop()
{
  family_ = BicopFamily::clayton;
  parameters_ = Eigen::VectorXd(1);
  parameters_lower_bounds_ = Eigen::VectorXd(1);
  parameters_upper_bounds_ = Eigen::VectorXd(1);
  parameters_ << 1e-10;
  parameters_lower_bounds_ << 1e-10;
  parameters_upper_bounds_ << 28;
}

inline double
ClaytonBicop::generator(const double& u)
{
  double theta = double(this->parameters_(0));
  return (std::pow(u, -theta) - 1) / theta;
}

inline double
ClaytonBicop::generator_inv(const double& u)
{
  double theta = double(this->parameters_(0));
  return std::pow(1 + theta * u, -1 / theta);
}

inline double
ClaytonBicop::generator_derivative(const double& u)
{
  return (-1) * std::pow(u, -1 - this->parameters_(0));
}

// inline double ClaytonBicop::generator_derivative2(const double &u)
//{
//    double theta = double(this->parameters_(0));
//    return (1 + theta) * std::pow(u, -2 - theta);
//}

inline Eigen::VectorXd
ClaytonBicop::pdf_raw(const Eigen::MatrixXd& u)
{
  double theta = static_cast<double>(parameters_(0));
  // avoid numerical issues when copula is too close to independence
  if (theta < 1e-10) {
    auto f = [](const double&, const double&) { return 1.0; };
    return tools_eigen::binaryExpr_or_nan(u, f);
  }

  auto f = [theta](const double& u1, const double& u2) {
    double temp = boost::math::log1p(theta) - (1.0 + theta) * std::log(u1 * u2);
    temp = temp - (2.0 + 1.0 / (theta)) *
                    std::log(std::pow(u1, -theta) + std::pow(u2, -theta) - 1.0);
    return std::exp(temp);
  };
  return tools_eigen::binaryExpr_or_nan(u, f);
}

inline Eigen::VectorXd
ClaytonBicop::hinv1_raw(const Eigen::MatrixXd& u)
{
  double theta = double(this->parameters_(0));
  Eigen::VectorXd hinv = u.col(0).array().pow(theta + 1.0);
  if (theta < 75) {
    hinv = u.col(1).cwiseProduct(hinv);
    hinv = hinv.array().pow(-theta / (theta + 1.0));
    Eigen::VectorXd x = u.col(0);
    x = x.array().pow(-theta);
    hinv = hinv - x + Eigen::VectorXd::Ones(x.size());
    hinv = hinv.array().pow(-1 / theta);
  } else {
    hinv = hinv1_num(u);
  }
  return hinv;
}

inline Eigen::MatrixXd
ClaytonBicop::tau_to_parameters(const double& tau)
{
  Eigen::VectorXd parameters(1);
  parameters(0) = 2 * std::fabs(tau) / (1 - std::fabs(tau));
  return parameters.cwiseMax(parameters_lower_bounds_)
    .cwiseMin(parameters_upper_bounds_);
}

inline double
ClaytonBicop::parameters_to_tau(const Eigen::MatrixXd& parameters)
{
  return parameters(0) / (2 + std::fabs(parameters(0)));
}

inline Eigen::VectorXd
ClaytonBicop::get_start_parameters(const double tau)
{
  Eigen::VectorXd par = tau_to_parameters(tau);
  par = par.cwiseMax(parameters_lower_bounds_);
  par = par.cwiseMin(parameters_upper_bounds_);
  return par;
}
}


//
// Implements iLQR (on a traditional chain) for nonlinear dynamics and cost.
//
// Arun Venkatraman (arunvenk@cs.cmu.edu)
// December 2016
//

#pragma once

#include <ilqr/ilqr_taylor_expansions.hh>

#include <Eigen/Dense>

#include <tuple>
#include <vector>

namespace ilqr
{

class iLQR
{
public:
    // Stores linearization points x, u and the Taylor expansions of the dynamics and cost.
    using TaylorExpansion = std::tuple<Eigen::VectorXd, Eigen::VectorXd, ilqr::Dynamics, ilqr::Cost>;

    iLQR(const DynamicsFunc &dynamics, const CostFunc &cost,
            const std::vector<Eigen::VectorXd> &Xs,
            const std::vector<Eigen::VectorXd> &Us);

    void backwards_pass();

    void forward_pass(std::vector<double> &costs,
            std::vector<Eigen::VectorXd> &states,
            std::vector<Eigen::VectorXd> &controls,
            bool update_linearizations
            );

    std::vector<Eigen::VectorXd> states();
    std::vector<Eigen::VectorXd> controls();

private:
    int state_dim_ = -1;
    int control_dim_  = -1;
    int T_ = -1; // time horizon

    DynamicsFunc true_dynamics_;
    CostFunc true_cost_;

    // Taylor series expansion points and expanded dynamics, cost.
    std::vector<TaylorExpansion> expansions_;

    // Feedback control gains.
    std::vector<Eigen::MatrixXd> Ks_;
    std::vector<Eigen::VectorXd> ks_;
};

} // namespace lqr


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

#include "MPC_last.h"
//#include <cppad/cppad.hpp>
#include <cppad/ipopt/solve.hpp>
#include <Eigen/Core>

// The program use fragments of code from
// https://github.com/udacity/CarND-MPC-Quizzes

using CppAD::AD;

// =========================================
// FG_eval class definition implementation.
// =========================================
class FG_eval
{
    public:
        // Fitted polynomial coefficients
        Eigen::VectorXd coeffs;

        double _dt, _ref_cte, _ref_etheta, _ref_vel;
        double  _w_cte, _w_etheta, _w_vel, _w_angvel, _w_accel, _w_angvel_d, _w_accel_d;
        int _mpc_steps, _x_start, _y_start, _theta_start, _v_start, _cte_start, _etheta_start, _angvel_start, _a_start;

        AD<double> cost_cte, cost_etheta, cost_vel;
        // Constructor
        FG_eval(Eigen::VectorXd coeffs)
        {
            this->coeffs = coeffs;

            // Set default value
            _dt = 0.1;  // in sec
            _ref_cte   = 0;
            _ref_etheta  = 0;
            _ref_vel   = 0.5; // m/s
            _w_cte     = 100;
            _w_etheta    = 100;
            _w_vel     = 1;
            _w_angvel   = 100;
            _w_accel   = 50;
            _w_angvel_d = 0;
            _w_accel_d = 0;

            _mpc_steps   = 40;
            _x_start     = 0;
            _y_start     = _x_start + _mpc_steps;
            _theta_start   = _y_start + _mpc_steps;
            _v_start     = _theta_start + _mpc_steps;
            _cte_start   = _v_start + _mpc_steps;
            _etheta_start  = _cte_start + _mpc_steps;
            _angvel_start = _etheta_start + _mpc_steps;
            _a_start     = _angvel_start + _mpc_steps - 1;
        }

        // Load parameters for constraints
        void LoadParams(const std::map<string, double> &params)
        {
            _dt = params.find("DT") != params.end() ? params.at("DT") : _dt;
            _mpc_steps = params.find("STEPS") != params.end()    ? params.at("STEPS") : _mpc_steps;
            _ref_cte   = params.find("REF_CTE") != params.end()  ? params.at("REF_CTE") : _ref_cte;
            _ref_etheta  = params.find("REF_ETHETA") != params.end() ? params.at("REF_ETHETA") : _ref_etheta;
            _ref_vel   = params.find("REF_V") != params.end()    ? params.at("REF_V") : _ref_vel;

            _w_cte   = params.find("W_CTE") != params.end()   ? params.at("W_CTE") : _w_cte;
            _w_etheta  = params.find("W_EPSI") != params.end()  ? params.at("W_EPSI") : _w_etheta;
            _w_vel   = params.find("W_V") != params.end()     ? params.at("W_V") : _w_vel;
            _w_angvel = params.find("W_ANGVEL") != params.end() ? params.at("W_ANGVEL") : _w_angvel;
            _w_accel = params.find("W_A") != params.end()     ? params.at("W_A") : _w_accel;
            _w_angvel_d = params.find("W_DANGVEL") != params.end() ? params.at("W_DANGVEL") : _w_angvel_d;
            _w_accel_d = params.find("W_DA") != params.end()     ? params.at("W_DA") : _w_accel_d;

            _x_start     = 0;
            _y_start     = _x_start + _mpc_steps;
            _theta_start   = _y_start + _mpc_steps;
            _v_start     = _theta_start + _mpc_steps;
            _cte_start   = _v_start + _mpc_steps;
            _etheta_start  = _cte_start + _mpc_steps;
            _angvel_start = _etheta_start + _mpc_steps;
            _a_start     = _angvel_start + _mpc_steps - 1;

            //cout << "\n!! FG_eval Obj parameters updated !! " << _mpc_steps << endl;
        }

        // MPC implementation (cost func & constraints)
        typedef CPPAD_TESTVECTOR(AD<double>) ADvector;
        // fg: function that evaluates the objective and constraints using the syntax
        void operator()(ADvector& fg, const ADvector& vars)
        {
            // fg[0] for cost function
            fg[0] = 0;
            cost_cte =  0;
            cost_etheta = 0;
            cost_vel = 0;

            /*
            for (int i = 0; i < _mpc_steps; i++)
            {
                cout << i << endl;
                cout << "_x_start" << vars[_x_start + i] <<endl;
                cout << "_y_start" << vars[_y_start + i] <<endl;
                cout << "_theta_start" << vars[_theta_start + i] <<endl;
                cout << "_v_start" << vars[_v_start + i] <<endl;
                cout << "_cte_start" << vars[_cte_start + i] <<endl;
                cout << "_etheta_start" << vars[_etheta_start + i] <<endl;
            }*/

            for (int i = 0; i < _mpc_steps; i++)
            {
              fg[0] += _w_cte * CppAD::pow(vars[_cte_start + i] - _ref_cte, 2); // cross deviation error
              fg[0] += _w_etheta * CppAD::pow(vars[_etheta_start + i] - _ref_etheta, 2); // heading error
              fg[0] += _w_vel * CppAD::pow(vars[_v_start + i] - _ref_vel, 2); // speed error

              cost_cte +=  _w_cte * CppAD::pow(vars[_cte_start + i] - _ref_cte, 2);
              cost_etheta +=  (_w_etheta * CppAD::pow(vars[_etheta_start + i] - _ref_etheta, 2));
              cost_vel +=  (_w_vel * CppAD::pow(vars[_v_start + i] - _ref_vel, 2));
            }
            cout << "-----------------------------------------------" <<endl;
            cout << "cost_cte, etheta, velocity: " << cost_cte << ", " << cost_etheta  << ", " << cost_vel << endl;

            // Minimize the use of actuators.
            for (int i = 0; i < _mpc_steps - 1; i++) {
              fg[0] += _w_angvel * CppAD::pow(vars[_angvel_start + i], 2);
              fg[0] += _w_accel * CppAD::pow(vars[_a_start + i], 2);
            }
            cout << "cost of actuators: " << fg[0] << endl;

            // Minimize the value gap between sequential actuations.
            for (int i = 0; i < _mpc_steps - 2; i++) {
              fg[0] += _w_angvel_d * CppAD::pow(vars[_angvel_start + i + 1] - vars[_angvel_start + i], 2);
              fg[0] += _w_accel_d * CppAD::pow(vars[_a_start + i + 1] - vars[_a_start + i], 2);
            }
            cout << "cost of gap: " << fg[0] << endl;

            // fg[x] for constraints
            // Initial constraints
            fg[1 + _x_start] = vars[_x_start];
            fg[1 + _y_start] = vars[_y_start];
            fg[1 + _theta_start] = vars[_theta_start];
            fg[1 + _v_start] = vars[_v_start];
            fg[1 + _cte_start] = vars[_cte_start];
            fg[1 + _etheta_start] = vars[_etheta_start];

            // Add system dynamic model constraint
            for (int i = 0; i < _mpc_steps - 1; i++)
            {
                // The state at time t+1 .
                AD<double> x1 = vars[_x_start + i + 1];
                AD<double> y1 = vars[_y_start + i + 1];
                AD<double> theta1 = vars[_theta_start + i + 1];
                AD<double> v1 = vars[_v_start + i + 1];
                AD<double> cte1 = vars[_cte_start + i + 1];
                AD<double> etheta1 = vars[_etheta_start + i + 1];

                // The state at time t.
                AD<double> x0 = vars[_x_start + i];
                AD<double> y0 = vars[_y_start + i];
                AD<double> theta0 = vars[_theta_start + i];
                AD<double> v0 = vars[_v_start + i];
                AD<double> cte0 = vars[_cte_start + i];
                AD<double> etheta0 = vars[_etheta_start + i];

                // Only consider the actuation at time t.
                //AD<double> angvel0 = vars[_angvel_start + i];
                AD<double> w0 = vars[_angvel_start + i];
                AD<double> a0 = vars[_a_start + i];

                //AD<double> f0 = coeffs[0] + coeffs[1] * x0 + coeffs[2] * CppAD::pow(x0, 2) + coeffs[3] * CppAD::pow(x0, 3);
                AD<double> f0 = 0.0;
                for (int i = 0; i < coeffs.size(); i++)
                {
                    f0 += coeffs[i] * CppAD::pow(x0, i); //f(0) = y
                }

                //AD<double> trj_grad0 = CppAD::atan(coeffs[1] + 2 * coeffs[2] * x0 + 3 * coeffs[3] * CppAD::pow(x0, 2));
                AD<double> trj_grad0 = 0.0;
                for (int i = 1; i < coeffs.size(); i++)
                {
                    trj_grad0 += i*coeffs[i] * CppAD::pow(x0, i-1); // f'(x0) = f(1)/1
                }
                trj_grad0 = CppAD::atan(trj_grad0);

                // Here's `x` to get you started.
                // The idea here is to constraint this value to be 0.
                //
                // NOTE: The use of `AD<double>` and use of `CppAD`!
                // This is also CppAD can compute derivatives and pass
                // these to the solver.
                // TODO: Setup the rest of the model constraints
                fg[2 + _x_start + i] = x1 - (x0 + v0 * CppAD::cos(theta0) * _dt);
                fg[2 + _y_start + i] = y1 - (y0 + v0 * CppAD::sin(theta0) * _dt);
                fg[2 + _theta_start + i] = theta1 - (theta0 +  w0 * _dt);
                fg[2 + _v_start + i] = v1 - (v0 + a0 * _dt);

                fg[2 + _cte_start + i] = cte1 - ((f0 - y0) + (v0 * CppAD::sin(etheta0) * _dt));
                //fg[2 + _etheta_start + i] = etheta1 - ((theta0 - trj_grad0) + w0 * _dt);//theta0-trj_grad0)->etheta : it can have more curvature prediction, but its gradient can be only adjust positive plan.
                fg[2 + _etheta_start + i] = etheta1 - (etheta0 + w0 * _dt);
            }
        }
};

// ====================================
// MPC class definition implementation.
// ====================================
MPC::MPC()
{
    // Set default value
    _mpc_steps = 20;
    _max_angvel = 3.0; // Maximal angvel radian (~30 deg)
    _max_throttle = 1.0; // Maximal throttle accel
    _bound_value  = 1.0e3; // Bound value for other variables

    _x_start     = 0;
    _y_start     = _x_start + _mpc_steps;
    _theta_start   = _y_start + _mpc_steps;
    _v_start     = _theta_start + _mpc_steps;
    _cte_start   = _v_start + _mpc_steps;
    _etheta_start  = _cte_start + _mpc_steps;
    _angvel_start = _etheta_start + _mpc_steps;
    _a_start     = _angvel_start + _mpc_steps - 1;

}

void MPC::LoadParams(const std::map<string, double> &params)
{
    _params = params;
    //Init parameters for MPC object
    _mpc_steps = _params.find("STEPS") != _params.end() ? _params.at("STEPS") : _mpc_steps;
    _max_angvel = _params.find("ANGVEL") != _params.end() ? _params.at("ANGVEL") : _max_angvel;
    _max_throttle = _params.find("MAXTHR") != _params.end() ? _params.at("MAXTHR") : _max_throttle;
    _bound_value  = _params.find("BOUND") != _params.end()  ? _params.at("BOUND") : _bound_value;

    _x_start     = 0;
    _y_start     = _x_start + _mpc_steps;
    _theta_start   = _y_start + _mpc_steps;
    _v_start     = _theta_start + _mpc_steps;
    _cte_start   = _v_start + _mpc_steps;
    _etheta_start  = _cte_start + _mpc_steps;
    _angvel_start = _etheta_start + _mpc_steps;
    _a_start     = _angvel_start + _mpc_steps - 1;

    cout << "\n!! MPC Obj parameters updated !! " << endl;
}

vector<double> MPC::Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs)
{
    bool ok = true;
    size_t i;
    typedef CPPAD_TESTVECTOR(double) Dvector;
    const double x = state[0];
    const double y = state[1];
    const double theta = state[2];
    const double v = state[3];
    const double cte = state[4];
    const double etheta = state[5];

    // Set the number of model variables (includes both states and inputs).
    // For example: If the state is a 4 element vector, the actuators is a 2
    // element vector and there are 10 timesteps. The number of variables is:
    // 4 * 10 + 2 * 9
    size_t n_vars = _mpc_steps * 6 + (_mpc_steps - 1) * 2;

    // Set the number of constraints
    size_t n_constraints = _mpc_steps * 6;

    // Initial value of the independent variables.
    // SHOULD BE 0 besides initial state.
    Dvector vars(n_vars);
    for (int i = 0; i < n_vars; i++)
    {
        vars[i] = 0;
    }

    // Set the initial variable values
    vars[_x_start] = x;
    vars[_y_start] = y;
    vars[_theta_start] = theta;
    vars[_v_start] = v;
    vars[_cte_start] = cte;
    vars[_etheta_start] = etheta;

    // Set lower and upper limits for variables.
    Dvector vars_lowerbound(n_vars);
    Dvector vars_upperbound(n_vars);

    // Set all non-actuators upper and lowerlimits
    // to the max negative and positive values.
    for (int i = 0; i < _angvel_start; i++)
    {
        vars_lowerbound[i] = -_bound_value;
        vars_upperbound[i] = _bound_value;
    }
    // The upper and lower limits of angvel are set to -25 and 25
    // degrees (values in radians).
    for (int i = _angvel_start; i < _a_start; i++)
    {
        vars_lowerbound[i] = -_max_angvel;
        vars_upperbound[i] = _max_angvel;
    }
    // Acceleration/decceleration upper and lower limits
    for (int i = _a_start; i < n_vars; i++)
    {
        vars_lowerbound[i] = -_max_throttle;
        vars_upperbound[i] = _max_throttle;
    }

    // Lower and upper limits for the constraints
    // Should be 0 besides initial state.
    Dvector constraints_lowerbound(n_constraints);
    Dvector constraints_upperbound(n_constraints);
    for (int i = 0; i < n_constraints; i++)
    {
        constraints_lowerbound[i] = 0;
        constraints_upperbound[i] = 0;
    }
    constraints_lowerbound[_x_start] = x;
    constraints_lowerbound[_y_start] = y;
    constraints_lowerbound[_theta_start] = theta;
    constraints_lowerbound[_v_start] = v;
    constraints_lowerbound[_cte_start] = cte;
    constraints_lowerbound[_etheta_start] = etheta;
    constraints_upperbound[_x_start] = x;
    constraints_upperbound[_y_start] = y;
    constraints_upperbound[_theta_start] = theta;
    constraints_upperbound[_v_start] = v;
    constraints_upperbound[_cte_start] = cte;
    constraints_upperbound[_etheta_start] = etheta;

    // object that computes objective and constraints
    FG_eval fg_eval(coeffs);
    fg_eval.LoadParams(_params);

    // options for IPOPT solver
    std::string options;
    // Uncomment this if you'd like more print information
    options += "Integer print_level  0\n";
    // NOTE: Setting sparse to true allows the solver to take advantage
    // of sparse routines, this makes the computation MUCH FASTER. If you
    // can uncomment 1 of these and see if it makes a difference or not but
    // if you uncomment both the computation time should go up in orders of
    // magnitude.
    options += "Sparse  true        forward\n";
    options += "Sparse  true        reverse\n";
    // NOTE: Currently the solver has a maximum time limit of 0.5 seconds.
    // Change this as you see fit.
    options += "Numeric max_cpu_time          0.5\n";

    // place to return solution
    CppAD::ipopt::solve_result<Dvector> solution;

    // solve the problem
    CppAD::ipopt::solve<Dvector, FG_eval>(
      options, vars, vars_lowerbound, vars_upperbound, constraints_lowerbound,
      constraints_upperbound, fg_eval, solution);

    // Check some of the solution values
    ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success;

    // Cost
    auto cost = solution.obj_value;
    std::cout << "------------ Total Cost(solution): " << cost << "------------" << std::endl;
    cout << "-----------------------------------------------" <<endl;

    this->mpc_x = {};
    this->mpc_y = {};
    this->mpc_theta = {};
    for (int i = 0; i < _mpc_steps; i++)
    {
        this->mpc_x.push_back(solution.x[_x_start + i]);
        this->mpc_y.push_back(solution.x[_y_start + i]);
        this->mpc_theta.push_back(solution.x[_theta_start + i]);
    }

    vector<double> result;
    result.push_back(solution.x[_angvel_start]);
    result.push_back(solution.x[_a_start]);
    return result;
}


/***************************************************************************
 *   Copyright (c) 2011 Konstantinos Poulios <logari81@gmail.com>          *
 *                                                                         *
 *   This file is part of the FreeCAD CAx development system.              *
 *                                                                         *
 *   This library is free software; you can redistribute it and/or         *
 *   modify it under the terms of the GNU Library General Public           *
 *   License as published by the Free Software Foundation; either          *
 *   version 2 of the License, or (at your option) any later version.      *
 *                                                                         *
 *   This library  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         *
 *   GNU Library General Public License for more details.                  *
 *                                                                         *
 *   You should have received a copy of the GNU Library General Public     *
 *   License along with this library; see the file COPYING.LIB. If not,    *
 *   write to the Free Software Foundation, Inc., 59 Temple Place,         *
 *   Suite 330, Boston, MA  02111-1307, USA                                *
 *                                                                         *
 ***************************************************************************/

#ifdef _MSC_VER
#pragma warning(disable : 4244)
#endif

#include <iostream>
#include <Eigen/QR>

using namespace Eigen;

// minimizes ( 0.5 * x^T * H * x + g^T * x ) under the condition ( A*x + c = 0 )
// it returns the solution in x, the row-space of A in Y, and the null space of A in Z
int qp_eq(MatrixXd &H, VectorXd &g, MatrixXd &A, VectorXd &c,
          VectorXd &x, MatrixXd &Y, MatrixXd &Z)
{
    FullPivHouseholderQR<MatrixXd> qrAT(A.transpose());
    MatrixXd Q = qrAT.matrixQ ();

    size_t params_num = qrAT.rows();
    size_t constr_num = qrAT.cols();
    size_t rank = qrAT.rank();

    if (rank != constr_num || constr_num > params_num)
        return -1;

    // A^T = Q*R*P^T = Q1*R1*P^T
    // Q = [Q1,Q2], R=[R1;0]
    // Y = Q1 * inv(R^T) * P^T
    // Z = Q2
    Y = qrAT.matrixQR().topRows(constr_num)
                       .triangularView<Upper>()
                       .transpose()
                       .solve<OnTheRight>(Q.leftCols(rank))
        * qrAT.colsPermutation().transpose();
    if (params_num == rank)
        x = - Y * c;
    else {
        Z = Q.rightCols(params_num-rank);

        MatrixXd ZTHZ = Z.transpose() * H * Z;
        VectorXd rhs = Z.transpose() * (H * Y * c - g);

        VectorXd y = ZTHZ.colPivHouseholderQr().solve(rhs);

        x = - Y * c + Z * y;
    }

    return 0;
}


#include <NTL/quad_float.h>
#include <NTL/RR.h>

#include <cfloat>

NTL_START_IMPL

#if (NTL_BITS_PER_LONG >= NTL_DOUBLE_PRECISION)

quad_float to_quad_float(long n)
{
   double xhi, xlo;

   xhi = TrueDouble(n);

   // Because we are assuming 2's compliment integer
   // arithmetic, the following prevents long(xhi) from overflowing.

   if (n > 0)
      xlo = TrueDouble(n+long(-xhi));
   else
      xlo = TrueDouble(n-long(xhi));

   // renormalize...just to be safe

   quad_float z;
   quad_float_normalize(z, xhi, xlo);
   return z;
}

quad_float to_quad_float(unsigned long n)
{
   double xhi, xlo, t;

   const double bnd = double(1L << (NTL_BITS_PER_LONG-2))*4.0;

   xhi = TrueDouble(n);

   if (xhi >= bnd)
      t = xhi - bnd;
   else
      t = xhi;

   // we use the "to_long" function here to be as portable as possible.
   long llo = to_long(n - (unsigned long)(t));
   xlo = TrueDouble(llo);

   quad_float z;
   quad_float_normalize(z, xhi, xlo);
   return z;
}
#endif

NTL_CHEAP_THREAD_LOCAL
long quad_float::oprec = 10;

void quad_float::SetOutputPrecision(long p)
{
   if (p < 1) p = 1;

   if (NTL_OVERFLOW(p, 1, 0))
      ResourceError("quad_float: output precision too big");

   oprec = p;
}

void power(quad_float& z, const quad_float& a, long e)
{
   quad_float res, u;
   unsigned long k;

   if (e < 0)
      k = -((unsigned long) e);
   else
      k = e;

   res = 1.0;
   u = a;

   while (k) {
      if (k & 1)
         res = res * u;

      k = k >> 1;
      if (k)
         u = u * u;
   }

   if (e < 0)
      z = 1.0/res;
   else
      z = res;
}

void power2(quad_float& z, long e)
{
   z.hi = _ntl_ldexp(1.0, e);
   z.lo = 0;
}

long to_long(const quad_float& x)
{
   double fhi, flo;

   fhi = floor(x.hi);

   if (fhi == x.hi)
      flo = floor(x.lo);
   else
      flo = 0;

   // the following code helps to prevent unnecessary integer overflow,
   // and guarantees that to_long(to_quad_float(a)) == a, for all long a,
   // provided long's are not too wide.

   if (fhi > 0)
      return long(flo) - long(-fhi);
   else
      return long(fhi) + long(flo);
}

// This version of ZZ to quad_float coversion relies on the
// precise rounding rules implemented by the ZZ to double conversion.

void conv(quad_float& z, const ZZ& a)
{
   double xhi, xlo;

   conv(xhi, a);

   if (!IsFinite(&xhi)) {
      z.hi = xhi;
      z.lo = 0;
      return;
   }

   NTL_ZZRegister(t);

   conv(t, xhi);
   sub(t, a, t);

   conv(xlo, t);

   quad_float_normalize(z, xhi, xlo);
}

void conv(ZZ& z, const quad_float& x)
{
   NTL_ZZRegister(t1);
   NTL_ZZRegister(t2);
   NTL_ZZRegister(t3);

   double fhi, flo;

   fhi = floor(x.hi);

   if (fhi == x.hi) {
      flo = floor(x.lo);

      conv(t1, fhi);
      conv(t2, flo);

      add(z, t1, t2);
   }
   else
      conv(z, fhi);
}

ostream& operator<<(ostream& s, const quad_float& a)
{
   quad_float aa = a;

   if (!IsFinite(&aa)) {
      s << "NaN";
      return s;
   }

   RRPush push;
   RROutputPush opush;

   RR::SetPrecision(long(3.33*quad_float::oprec) + 10);
   RR::SetOutputPrecision(quad_float::oprec);

   NTL_TLS_LOCAL(RR, t);

   conv(t, a);
   s << t;

   return s;
}

istream& operator>>(istream& s, quad_float& x)
{
   RRPush push;
   RR::SetPrecision(4*NTL_DOUBLE_PRECISION);

   NTL_TLS_LOCAL(RR, t);
   NTL_INPUT_CHECK_RET(s, s >> t);
   conv(x, t);

   return s;
}

void random(quad_float& x)
{
   RRPush push;
   RR::SetPrecision(4*NTL_DOUBLE_PRECISION);

   NTL_TLS_LOCAL(RR, t);
   random(t);
   conv(x, t);
}

quad_float random_quad_float()
{
   quad_float x;
   random(x);
   return x;
}

long IsFinite(quad_float *x)
{
   return IsFinite(&x->hi) && IsFinite(&x->lo);
}

quad_float floor(const quad_float& x)
{
   double fhi = floor(x.hi);

   if (fhi != x.hi)
      return quad_float(fhi, 0.0);
   else {
      double flo = floor(x.lo);
      quad_float z;
      quad_float_normalize(z, fhi, flo);
      return z;
   }
}

quad_float ceil(const quad_float& x) {
  return -floor(-x);
}

quad_float trunc(const quad_float& x) {
  if (x>=0.0) return floor(x); else return -floor(-x);
}

long compare(const quad_float& x, const quad_float& y)
{
   if (x.hi > y.hi)
      return 1;
   else if (x.hi < y.hi)
      return -1;
   else if (x.lo > y.lo)
      return 1;
   else if (x.lo < y.lo)
      return -1;
   else
      return 0;
}

quad_float fabs(const quad_float& x)
{ if (x.hi>=0.0) return x; else return -x; }

quad_float ldexp(const quad_float& x, long exp) { // x*2^exp
   double xhi, xlo;
   quad_float z;

   xhi = _ntl_ldexp(x.hi, exp);
   xlo = _ntl_ldexp(x.lo, exp);

   quad_float_normalize(z, xhi, xlo);
   return z;
}

quad_float exp(const quad_float& x) { // New version 97 Aug 05
/*
!  Calculate a quadruple-precision exponential
!  Method:
!   x    x.log2(e)    nint[x.log2(e)] + frac[x.log2(e)]
!  e  = 2          = 2
!
!                     iy    fy
!                  = 2   . 2
!  Then
!   fy    y.loge(2)
!  2   = e
!
!  Now y.loge(2) will be less than 0.3466 in absolute value.
!  This is halved and a Pade aproximation is used to approximate e^x over
!  the region (-0.1733, +0.1733).   This approximation is then squared.
*/
  if (x.hi<DBL_MIN_10_EXP*2.302585092994045684017991)
    return to_quad_float(0.0);
  if (x.hi>DBL_MAX_10_EXP*2.302585092994045684017991) {
    ResourceError("exp(quad_float): overflow");
  }

  static const quad_float Log2 = to_quad_float("0.6931471805599453094172321214581765680755");
  // GLOBAL (assumes C++11 thread-safe init)

  quad_float y,temp,ysq,sum1,sum2;
  long iy;
  y=x/Log2;
  temp = floor(y+0.5);
  iy = to_long(temp);
  y=(y-temp)*Log2;
  y=ldexp(y,-1L);
  ysq=y*y;
  sum1=y*((((ysq+3960.0)*ysq+2162160.0)*ysq+302702400.0)*ysq+8821612800.0);
  sum2=(((90.0*ysq+110880.0)*ysq+30270240.0)*ysq+2075673600.0)*ysq+17643225600.0;
/*
!                     sum2 + sum1         2.sum1
! Now approximation = ----------- = 1 + ----------- = 1 + 2.temp
!                     sum2 - sum1       sum2 - sum1
!
! Then (1 + 2.temp)^2 = 4.temp.(1 + temp) + 1
*/
  temp=sum1/(sum2-sum1);
  y=temp*(temp+1);
  y=ldexp(y,2L);
  return ldexp(y+1,iy);
}

quad_float log(const quad_float& t) { // Newton method. See Bailey, MPFUN
  if (t.hi <= 0.0) {
    ArithmeticError("log(quad_float): argument must be positive");
  }

  quad_float s = to_quad_float(log(t.hi));
  // NOTE: in case log yields excess precision, this assumes
  // that to_quad_float removes it

  quad_float e = exp(s);
  return s+(t-e)/e;  // Newton step
}

quad_float sqrt(const quad_float& y)
{
  if (y.hi < 0.0)
    ArithmeticError("quad_float: square root of negative number");
  if (y.hi == 0.0) return quad_float(0.0,0.0);

  double c = TrueDouble(sqrt(y.hi));
  // NOTE: we call TrueDouble, just in case sqrt yields excess precision

  quad_float yy = y;
  quad_float_in_place_sqrt(yy, c);
  return yy;
}

long operator> (const quad_float& x, const quad_float& y) {
   return (x.hi> y.hi) || (x.hi==y.hi && x.lo> y.lo); }
long operator>=(const quad_float& x, const quad_float& y) {
   return (x.hi>y.hi) || (x.hi==y.hi && x.lo>=y.lo); }
long operator< (const quad_float& x, const quad_float& y) {
   return (x.hi< y.hi) || (x.hi==y.hi && x.lo< y.lo); }
long operator<=(const quad_float& x, const quad_float& y) {
   return (x.hi<y.hi) || (x.hi==y.hi && x.lo<=y.lo); }
long operator==(const quad_float& x, const quad_float& y)
   { return x.hi==y.hi && x.lo==y.lo; }
long operator!=(const quad_float& x, const quad_float& y)
   { return x.hi!=y.hi || x.lo!=y.lo; }

NTL_END_IMPL


#pragma once
#include <Eigen/Sparse>

namespace edp
{
namespace internal
{
	inline uint32_t ipow(uint32_t base, uint32_t exp)
	{
		uint32_t result = 1U;
		while (exp)
		{
			if (exp & 1U)
				result *= base;
			exp >>= 1U;
			base *= base;
		}

		return result;
	}
}

template<typename T>
struct TwoSiteTerm
{
	std::pair<uint32_t, uint32_t> sites;
	Eigen::SparseMatrix<T> m;

	TwoSiteTerm(const std::pair<uint32_t, uint32_t>& p1, const Eigen::SparseMatrix<T>& p2)
		: sites(p1), m(p2)
	{
	}
	TwoSiteTerm(std::pair<uint32_t, uint32_t>&& p1, Eigen::SparseMatrix<T>&& p2)
		: sites(p1), m(p2)
	{
	}
};

template<typename T>
struct OneSiteTerm
{
	uint32_t site;
	Eigen::SparseMatrix<T> m;

	OneSiteTerm(uint32_t p1, const Eigen::SparseMatrix<T>& p2)
		: site(p1), m(p2)
	{
	}

	OneSiteTerm(uint32_t p1, Eigen::SparseMatrix<T>&& p2)
		: site(p1), m(p2)
	{
	}
};

template<typename T>
class LocalHamiltonian
{
private:
	uint32_t numSites_;
	uint64_t d_; //local Hibert dimension

	std::vector<TwoSiteTerm<T> > twoSiteTerms_;
	std::vector<OneSiteTerm<T> > oneSiteTerms_;

	uint32_t swapBaseD(uint32_t idx, uint32_t pos, uint32_t val) const
	{
		uint32_t b = internal::ipow(d_, pos);
		uint32_t upper = (idx / (b*d_))*d_ + val;
		return upper*b + (idx % b);
	}
public:
	LocalHamiltonian(uint32_t numSites, uint32_t d)
		: numSites_{numSites}, d_{d}
	{
	}
	LocalHamiltonian(const LocalHamiltonian&) = default;
	LocalHamiltonian(LocalHamiltonian&&) = default;

	LocalHamiltonian& operator=(const LocalHamiltonian&) = default;
	LocalHamiltonian& operator=(LocalHamiltonian&&) = default;

	void clearTerms()
	{
		std::vector<TwoSiteTerm<T> >().swap(twoSiteTerms_);
		std::vector<OneSiteTerm<T> >().swap(oneSiteTerms_);
	}
	uint32_t getNumSites() const
	{
		return numSites_;
	}

	std::map<uint32_t, T> getCol(uint32_t n) const;
	inline std::map<uint32_t, T> operator()(uint32_t n) const
	{
		return getCol(n);
	}

	void addTwoSiteTerm(const std::pair<int,int>& site, Eigen::SparseMatrix<T> m)
	{
		m.makeCompressed();
		twoSiteTerms_.emplace_back(site, std::move(m));
	}
	void addOneSiteTerm(uint32_t site, Eigen::SparseMatrix<T> m)
	{
		m.makeCompressed();
		oneSiteTerms_.emplace_back(site, std::move(m));
	}
};

}

template<typename T>
std::map<uint32_t, T> edp::LocalHamiltonian<T>::getCol(uint32_t n) const
{
	using internal::ipow;
	using Eigen::SparseMatrix;

	std::map<uint32_t, T> m;

	for(auto& twoSiteTerm : twoSiteTerms_)
	{
		auto a = (n/ipow(d_,twoSiteTerm.sites.first))%d_;
		auto b = (n/ipow(d_,twoSiteTerm.sites.second))%d_;
		//auto col = twoSiteTerm.m.col(a*d_ + b);
		auto col = b*d_ + a;
		for(typename SparseMatrix<T>::InnerIterator it(twoSiteTerm.m, col); it; ++it)
		{
			uint32_t r = it.row();
			uint32_t t = n;
			t = swapBaseD(t, twoSiteTerm.sites.first, r%d_);
			t = swapBaseD(t, twoSiteTerm.sites.second, r/d_);
			m[t] += it.value();
		}
	}
	for(auto& oneSiteTerm: oneSiteTerms_)
	{
		uint32_t a = (n/ipow(d_,oneSiteTerm.site))%d_;
		//auto col = oneSiteTerm.m.col(a);
		auto col = a;

		for(typename SparseMatrix<T>::InnerIterator it(oneSiteTerm.m, col); it; ++it)
		{
			uint32_t r = it.row();
			uint32_t t = n;
			t = swapBaseD(t, oneSiteTerm.site, r);
			m[t] += it.value();
		}
	}
	return m;
}


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      Montebruck O, Gill E. Satellite Orbits, Corrected Third Printing, Springer, 2005.
 *      Bate R. Fundamentals of Astrodynamics, Courier Dover Publications, 1971.
 *
 */

#include <Eigen/Core>
#include <cmath>

#include "Tudat/Astrodynamics/BasicAstrodynamics/missionGeometry.h"
#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/stateVectorIndices.h"

namespace tudat
{

namespace mission_geometry
{

//! Compute whether an orbit is retrograde based on inclination.
bool isOrbitRetrograde( const double inclination )
{
    bool isRetrograde = false;

    // Check which range inclination is in and return value accordingly.
    if ( inclination < 0.0 || inclination > mathematical_constants::PI )
    {
        throw std::runtime_error(
                    "The inclination is in the wrong range when determining retrogradeness" );
    }
    else if ( inclination <= mathematical_constants::PI / 2.0 )
    {
        isRetrograde = false;
    }
    else if ( inclination > mathematical_constants::PI / 2.0 )
    {
        isRetrograde = true;
    }

    return isRetrograde;
}

//! Compute whether an orbit is retrograde based on Keplerian state.
bool isOrbitRetrograde( const Eigen::Vector6d& keplerElements )
{
    // Get inclination from vector and call overloaded function.
    return isOrbitRetrograde(
                keplerElements(
                    orbital_element_conversions::inclinationIndex ) );
}

//! Compute the shadow function.
double computeShadowFunction( const Eigen::Vector3d& occultedBodyPosition,
                              const double occultedBodyRadius,
                              const Eigen::Vector3d& occultingBodyPosition,
                              const double occultingBodyRadius,
                              const Eigen::Vector3d& satellitePosition )
{
    // Calculate coordinates of the spacecraft with respect to the occulting body.
    const Eigen::Vector3d satellitePositionRelativeToOccultingBody = satellitePosition
            - occultingBodyPosition;

    // Calculate apparent radius of occulted body.
    const double occultedBodyApparentRadius
            = std::asin( occultedBodyRadius
                         / ( occultedBodyPosition - satellitePosition ).norm( ) );

    // Calculate apparent radius of occulting body.
    const double occultingBodyApparentRadius =
            std::asin( occultingBodyRadius / satellitePositionRelativeToOccultingBody.norm( ) );

    // Calculate apparent separation of the center of both bodies.
    const double apparentSeparationPartOne = -satellitePositionRelativeToOccultingBody.transpose( )
            * ( occultedBodyPosition - satellitePosition );
    const double apparentSeparationPartTwo = satellitePositionRelativeToOccultingBody.norm( )
            * ( occultedBodyPosition - satellitePosition ).norm( );
    const double apparentSeparation = std::acos( apparentSeparationPartOne
                                                 / apparentSeparationPartTwo );

    // Set initial value for the shadow function.
    double shadowFunction = 1.0;

    // Check if partial occultation takes place
    if ( std::fabs( occultedBodyApparentRadius - occultingBodyApparentRadius ) < apparentSeparation
         && apparentSeparation < occultedBodyApparentRadius + occultingBodyApparentRadius )
    {
        // Pre-compute values for optimal computations.
        const double apparentSeparationSquared = apparentSeparation * apparentSeparation;
        const double occultedBodyApparentRadiusSquared = occultedBodyApparentRadius
                * occultedBodyApparentRadius;
        const double occultingBodyApparentRadiusSquared = occultingBodyApparentRadius
                * occultingBodyApparentRadius;

        // Partial occultation takes place, calculate the occulted area.
        const double occultedAreaPartOne
                = ( apparentSeparationSquared + occultedBodyApparentRadiusSquared
                    - occultingBodyApparentRadiusSquared ) / ( 2.0 * apparentSeparation );
        const double occultedAreaPartTwo = std::sqrt( occultedBodyApparentRadiusSquared
                                                      - occultedAreaPartOne * occultedAreaPartOne );
        const double occultedArea = occultedBodyApparentRadiusSquared
                * std::acos( occultedAreaPartOne / occultedBodyApparentRadius )
                + occultingBodyApparentRadiusSquared
                * std::acos( ( apparentSeparation - occultedAreaPartOne )
                             / occultingBodyApparentRadius )
                - apparentSeparation * occultedAreaPartTwo;
        shadowFunction = 1.0 - occultedArea / ( mathematical_constants::PI *
                                                occultedBodyApparentRadiusSquared );
    }

    else
    {
        // Full or no occultation takes place.
        // Check for type of occultation.
        if ( apparentSeparation < occultingBodyApparentRadius - occultedBodyApparentRadius &&
             occultedBodyApparentRadius < occultingBodyApparentRadius )
        {
            // Total occultation.
            shadowFunction = 0.0;
        }

        else if ( apparentSeparation < occultedBodyApparentRadius - occultingBodyApparentRadius &&
                  occultedBodyApparentRadius > occultingBodyApparentRadius )
        {
            // Maximum partial occultation.
            shadowFunction = 0.0;
        }

        else if ( occultedBodyApparentRadius + occultingBodyApparentRadius <= apparentSeparation )
        {
            // No occultation
            shadowFunction = 1.0;
        }
    }

    // Return the shadow function
    return shadowFunction;
}

double computeSphereOfInfluence( const double distanceToCentralBody,
                                 const double ratioOfOrbitingToCentralBodyMass )
{
    // Return the radius of the sphere of influence.
    return distanceToCentralBody * std::pow( ratioOfOrbitingToCentralBodyMass, 0.4 );
}

//! Compute the sphere of influence.
double computeSphereOfInfluence( const double distanceToCentralBody,
                                 const double massOrbitingBody,
                                 const double massCentralBody )
{
    // Return the radius of the sphere of influence.
    return computeSphereOfInfluence( distanceToCentralBody, massOrbitingBody / massCentralBody );
}

} // namespace mission_geometry

} // namespace tudat


//=======================================================================
// Copyright 2007 Aaron Windsor - Ben Sisson
//
// 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 <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/ref.hpp>
#include <vector>

#include <boost/graph/make_biconnected_planar.hpp>
#include <boost/graph/make_maximal_planar.hpp>
#include <boost/graph/planar_face_traversal.hpp>
#include <boost/graph/boyer_myrvold_planar_test.hpp>
#include <boost/graph/planar_canonical_ordering.hpp>
#include <boost/graph/chrobak_payne_drawing.hpp>
#include <boost/graph/is_straight_line_drawing.hpp>

using namespace boost;

//a class to hold the coordinates of the straight line embedding
struct coord_t{
  std::size_t x;
  std::size_t y;
};

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

  typedef adjacency_list
    < vecS,
      vecS,
      undirectedS,
      property<vertex_index_t, int>,
      property<edge_index_t, int>
    >
    graph;

  graph g(7);
  add_edge(0,1,g);
  add_edge(1,2,g);
  add_edge(2,3,g);
  add_edge(3,0,g);
  add_edge(0,4,g);
  add_edge(1,5,g);
  add_edge(2,6,g);
  add_edge(3,7,g);
  add_edge(4,5,g);
  add_edge(5,6,g);
  add_edge(6,7,g);
  add_edge(7,4,g);

  //Initialize the interior edge index
  property_map<graph, edge_index_t>::type e_index = get(edge_index, g);
  graph_traits<graph>::edges_size_type edge_count = 0;
  graph_traits<graph>::edge_iterator ei, ei_end;
  for(boost::tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
    put(e_index, *ei, edge_count++);

  //Test for planarity; compute the planar embedding as a side-effect
  typedef std::vector< graph_traits<graph>::edge_descriptor > vec_t;
  std::vector<vec_t> embedding(num_vertices(g));
  boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g,
                                   boyer_myrvold_params::embedding =
                                       &embedding[0]
                                   );

  make_biconnected_planar(g, &embedding[0]);

  // Re-initialize the edge index, since we just added a few edges
  edge_count = 0;
  for(boost::tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
    put(e_index, *ei, edge_count++);

  //Test for planarity again; compute the planar embedding as a side-effect
  boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g,
                                   boyer_myrvold_params::embedding =
                                       &embedding[0]
                                   );

  make_maximal_planar(g, &embedding[0]);

  // Re-initialize the edge index, since we just added a few edges
  edge_count = 0;
  for(boost::tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
    put(e_index, *ei, edge_count++);

  boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g,
                                   boyer_myrvold_params::embedding =
                                       &embedding[0]
                                   );

  // Find a canonical ordering
  std::vector<graph_traits<graph>::vertex_descriptor> ordering;
  planar_canonical_ordering(g, &embedding[0], std::back_inserter(ordering));

  //Set up a property map to hold the mapping from vertices to coord_t's
  typedef std::vector< coord_t > straight_line_drawing_storage_t;
  typedef boost::iterator_property_map
    < straight_line_drawing_storage_t::iterator,
      property_map<graph, vertex_index_t>::type
    >
    straight_line_drawing_t;

  straight_line_drawing_storage_t straight_line_drawing_storage
    (num_vertices(g));
  straight_line_drawing_t straight_line_drawing
    (straight_line_drawing_storage.begin(),
     get(vertex_index,g)
     );

  // Compute the straight line drawing
  chrobak_payne_straight_line_drawing(g,
                                      embedding,
                                      ordering.begin(),
                                      ordering.end(),
                                      straight_line_drawing
                                      );

  std::cout << "The straight line drawing is: " << std::endl;
  graph_traits<graph>::vertex_iterator vi, vi_end;
  for(boost::tie(vi,vi_end) = vertices(g); vi != vi_end; ++vi)
    {
      coord_t coord(get(straight_line_drawing,*vi));
      std::cout << *vi << " -> (" << coord.x << ", " << coord.y << ")"
                << std::endl;
    }

  // Verify that the drawing is actually a plane drawing
  if (is_straight_line_drawing(g, straight_line_drawing))
    std::cout << "Is a plane drawing." << std::endl;
  else
    std::cout << "Is not a plane drawing." << std::endl;

  return 0;
}


/** A simple cartesian space controller.
    It receives a destination, a duration and publishes the trajectory
    in cartesian space at 200Hz
    Note: the robot is required to be calibrated to the zero configuration
    as used in the kdl chain model.
**/
#include "ros/ros.h"
#include <boost/lexical_cast.hpp>
#include <boost/thread/thread.hpp>
#include <trajectory_msgs/JointTrajectory.h>
#include "bosch_arm_srvs/GetJointAngles.h"
#include <kdl/chain.hpp>
#include <kdl/chainfksolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/frames_io.hpp>
#include <stdio.h>
#include <iostream>
#include <vector>
using namespace Eigen;

class BoschArmKinematicModel
{
  public:
    double L0,L3,L4;
    Vector3f getTipPosition(Vector4f q)
    {
      Vector3f tip;
      double q1=q(0);
      double q2=q(1);
      double q3=q(2);
      double q4=q(3);
      tip(0)= - L4*(sin(q4)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3)) + cos(q2)*cos(q4)*sin(q1)) - L3*cos(q2)*sin(q1);
      tip(1)=  - L4*(cos(q4)*sin(q2) + cos(q2)*sin(q3)*sin(q4)) - L3*sin(q2);
      tip(2)=L0 - L4*(sin(q4)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3)) - cos(q1)*cos(q2)*cos(q4)) + L3*cos(q1)*cos(q2);
      return tip;
    }
    Matrix3f getJacobianLockJoint3(Vector4f qlock)
    {
      Matrix3f jacob;
      double q1=qlock(0);
      double q2=qlock(1);
      double q3=qlock(2);
      double q4=qlock(3);
      jacob(0,0)=L4*(sin(q4)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3)) - cos(q1)*cos(q2)*cos(q4)) - L3*cos(q1)*cos(q2);
      jacob(0,1)=L4*(cos(q4)*sin(q1)*sin(q2) + cos(q2)*sin(q1)*sin(q3)*sin(q4)) + L3*sin(q1)*sin(q2);
      jacob(0,2)=-L4*(cos(q4)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3)) - cos(q2)*sin(q1)*sin(q4));
      jacob(1,0)=0;
      jacob(1,1)=-L4*(cos(q2)*cos(q4) - sin(q2)*sin(q3)*sin(q4)) - L3*cos(q2);
      jacob(1,2)=L4*(sin(q2)*sin(q4) - cos(q2)*cos(q4)*sin(q3));
      jacob(2,0)=- L4*(sin(q4)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3)) + cos(q2)*cos(q4)*sin(q1)) - L3*cos(q2)*sin(q1);
      jacob(2,1)=- L4*(cos(q1)*cos(q4)*sin(q2) + cos(q1)*cos(q2)*sin(q3)*sin(q4)) - L3*cos(q1)*sin(q2);
      jacob(2,2)=-L4*(cos(q4)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3)) + cos(q1)*cos(q2)*sin(q4));
      return jacob;
    }

};
int main ( int argc, char **argv )
{
  ros::init ( argc, argv, "traj_gen" );
  ros::NodeHandle n;
  ros::Publisher traj_pub =  n.advertise<trajectory_msgs::JointTrajectory> ( "/traj_cmd",1 );
  //boost::thread t2 = boost::thread::thread ( boost::bind ( &pubTrajectory ) );
  ros::ServiceClient client = n.serviceClient<bosch_arm_srvs::GetJointAngles> ( "get_joint_angles" );
  bosch_arm_srvs::GetJointAngles srv;
  client.call ( srv );

  //read destination and duration
  KDL::Vector des;
  for ( int i=0;i<3;i++ )
    des[i]=boost::lexical_cast<double> ( argv[i+1] );
  double t=boost::lexical_cast<double> ( argv[4] );

  //TODO convert destination to joint angles

  Segment seg0=Segment ( Joint ( Joint::None ),
                         Frame ( Rotation::RPY ( M_PI/2,-M_PI/2,0 ),Vector ( 0,0,0.27 ) ) );
  Segment seg1=Segment ( Joint ( Joint::RotZ ),
                         Frame ( Rotation::RPY ( M_PI/2,-M_PI/2,0 ) ) );
  Segment seg2=Segment ( Joint ( Joint::RotZ ),
                         Frame ( Rotation::RPY ( M_PI/2,-M_PI/2,0 ) ) );
  Segment seg3=Segment ( Joint ( Joint::None ),
                         Frame ( Rotation::RPY ( M_PI/2,-M_PI/2,0 ),Vector ( 0,0,0.50 ) ) );
  Segment seg4=Segment ( Joint ( Joint::RotZ ),
                         Frame ( Vector ( 0.48,0,0 ) ) );
  Chain chain;
  chain.addSegment ( seg0 );
  chain.addSegment ( seg1 );
  chain.addSegment ( seg2 );
  chain.addSegment ( seg3 );
  chain.addSegment ( seg4 );
  ChainFkSolverPos_recursive fksolver = ChainFkSolverPos_recursive ( chain );

  //linear interpolation in joint space
  trajectory_msgs::JointTrajectory traj;
  traj.header.stamp=ros::Time::now();
  traj.header.frame_id="";
  traj.header.seq=0;
  traj.points.resize ( npts+1 );
  traj.joint_names.resize ( 4 );
  traj.joint_names[0]="joint1";
  traj.joint_names[1]="joint2";
  traj.joint_names[2]="joint3";
  traj.joint_names[3]="joint4";
  traj.points[i].positions.resize ( 4 );

  int npts=ceil ( t*200 );
  KDL::JntArray jointpositions = JntArray (3);
  KDL::Frame cartpos;
  double q3;
  bool kinematics_status;

  for ( int i=0;i<=npts;i++ )
  {
    //get the current joint position
    client.call ( srv );
    //compute forward kinematics.

    jointpositions (0) =srv.response.joint_angles[0];
    jointpositions (1) =srv.response.joint_angles[1];
    q3                 =srv.response.joint_angles[2];
    jointpositions (2) =srv.response.joint_angles[3];
    kinematics_status = fksolver.JntToCart ( jointpositions,cartpos );
    if ( kinematics_status>=0 )
    {
      std::cout << cartpos.p <<std::endl;
    }
    else
    {
      printf ( "%s \n","Error: could not calculate forward kinematics :(" );
    }
    double steps_to_go=npts-i;
    Vector step_length_cart= (des-cartpos.p)/steps_to_go;

    for ( int j=0;j<4;j++ )
      traj.points[i].positions[j]=srv.response.joint_angles[j]+i*dq[j];
    traj.points[i].time_from_start=ros::Duration ( i*t/npts );
  }
  //The first message is always lost
  for ( int i=0;i<2;i++ )
  {
    traj_pub.publish ( traj );
    sleep ( 1 );
  }
  ros::spin();
  return 0;
}


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      120926    E. Dekens         File created.
 *      121218    S. Billemont      Added output fuctions to display Legendre polynomial data,
 *                                  for debugging.
 *
 *    References
 *
 *    Notes
 *
 */

#include <sstream>
#include <stdexcept>

#include <boost/exception/all.hpp>
#include <boost/math/special_functions/factorials.hpp>

#include "Tudat/Mathematics/BasicMathematics/legendrePolynomials.h"
#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"

namespace tudat
{
namespace basic_mathematics
{

//! Default constructor, initializes cache object with 0 maximum degree and order.
LegendreCache::LegendreCache( const bool useGeodesyNormalization )
{
    useGeodesyNormalization_  = useGeodesyNormalization;

    if( useGeodesyNormalization_ )
    {
        legendrePolynomialFunction_ = geodesyNormalizedLegendrePolynomialFunction;
    }
    else
    {
        legendrePolynomialFunction_ = regularLegendrePolynomialFunction;
    }

    resetMaximumDegreeAndOrder( 0, 0 );

}

//! Constructor
LegendreCache::LegendreCache( const int maximumDegree, const int maximumOrder, const bool useGeodesyNormalization  )
{
    useGeodesyNormalization_  = useGeodesyNormalization;

    if( useGeodesyNormalization_ )
    {
        legendrePolynomialFunction_ = geodesyNormalizedLegendrePolynomialFunction;
    }
    else
    {
        legendrePolynomialFunction_ = regularLegendrePolynomialFunction;
    }

    resetMaximumDegreeAndOrder( maximumDegree, maximumOrder );
}

//! Get Legendre polynomial from cache when possible, and from direct computation otherwise.
void LegendreCache::update( const double polynomialParameter  )
{
    currentPolynomialParameter_ = polynomialParameter;
    currentPolynomialParameterComplement_ = std::sqrt( 1.0 - polynomialParameter * polynomialParameter ); // cosine of latitude is always positive!

    LegendreCache& thisReference = *this;
    for( int i = 0; i <= maximumDegree_; i++ )
    {
        for( int j = 0; ( ( j <= i ) && ( j <= maximumOrder_ ) ) ; j++ )
        {
            legendreValues_[ i * ( maximumOrder_ + 1 ) + j ] = legendrePolynomialFunction_( i, j, thisReference );
        }
    }
}

//! Update maximum degree and order of cache
void LegendreCache::resetMaximumDegreeAndOrder( const int maximumDegree, const int maximumOrder )
{
    maximumDegree_ = maximumDegree;
    maximumOrder_ = maximumOrder;
    legendreValues_.resize( ( maximumDegree_ + 1 ) * ( maximumOrder_ + 1 ) );

    currentPolynomialParameter_ = TUDAT_NAN;
    currentPolynomialParameterComplement_ = TUDAT_NAN;
}

//! Get Legendre polynomial value from the cache.
double LegendreCache::getLegendrePolynomial(
        const int degree, const int order )
{
    if( degree > maximumDegree_ || order > maximumOrder_ )
    {
        std::cerr<<"Error when requesting legendre cache, maximum degree or order exceeded "<<
                   degree<<" "<<maximumDegree_<<" "<<order<<" "<<maximumOrder_<<std::endl;
        return TUDAT_NAN;
    }
    else if( order > degree )
    {
        return 0.0;
    }
    else
    {
        return legendreValues_[ degree * ( maximumOrder_ + 1  ) + order ];
    };
}

//! Compute unnormalized associated Legendre polynomial.
double computeLegendrePolynomial( const int degree,
                                  const int order,
                                  LegendreCache& legendreCache )
{
    if( legendreCache.getUseGeodesyNormalization( ) )
    {
        throw std::runtime_error( "Error when computing Legendre polynomial, input uses normalization" );
    }

    // If degree or order is negative...
    if ( degree < 0 || order < 0 )
    {
        // Set error message.
        std::stringstream errorMessage;
        errorMessage << "Error: the Legendre polynomial of = " << degree << " and order = "
                     << order << " is undefined." << std::endl;

        // Throw a run-time error.
        boost::throw_exception( boost::enable_error_info( std::runtime_error(
                                                              errorMessage.str( ) ) ) );
    }

    // Else if order is greater than degree...
    else if ( order > degree && degree >= 0 )
    {
        // Return zero.
        return 0.0;
    }

    // Else if order and degree are lower than 2...
    else if ( degree <= 1 && order <= 1 )
    {
        // Compute polynomial explicitly.
        return computeLegendrePolynomialExplicit( degree, order, legendreCache.getCurrentPolynomialParameter( ) );
    }

    // Else if degree and order are sectoral...
    else if ( degree == order )
    {
        // Obtain polynomial of degree one and order one.
        const double degreeOneOrderOnePolynomial = legendreCache.getLegendrePolynomial(
                    1, 1 );

        // Obtain prior sectoral polynomial.
        const double priorSectoralPolynomial = legendreCache.getLegendrePolynomial(
                    degree - 1, order - 1 );

        // Compute polynomial.
        return computeLegendrePolynomialDiagonal(
                    degree, degreeOneOrderOnePolynomial, priorSectoralPolynomial );
    }

    // Else degree and order are zonal/tessoral...
    else
    {
        // Obtain prior degree polynomial.
        const double oneDegreePriorPolynomial = legendreCache.getLegendrePolynomial(
                    degree - 1, order );

        // Obtain two degrees prior polynomial.
        const double twoDegreesPriorPolynomial = legendreCache.getLegendrePolynomial(
                    degree - 2, order );

        // Compute polynomial.
        return computeLegendrePolynomialVertical( degree,
                                                  order,
                                                  legendreCache.getCurrentPolynomialParameter( ),
                                                  oneDegreePriorPolynomial,
                                                  twoDegreesPriorPolynomial );
    }
}

double computeLegendrePolynomial( const int degree,
                                  const int order,
                                  const double legendreParameter )
{
    LegendreCache legendreCache( degree, order, 0 );
    legendreCache.update( legendreParameter );
    return computeLegendrePolynomial( degree, order, legendreCache );
}

//! Compute geodesy-normalized associated Legendre polynomial.
double computeGeodesyLegendrePolynomial( const int degree,
                                         const int order,
                                         LegendreCache& geodesyLegendreCache )
{

    if( !geodesyLegendreCache.getUseGeodesyNormalization( ) )
    {
        throw std::runtime_error( "Error when computing Legendre polynomial, input uses no normalization" );
    }

    // If degree or order is negative...
    if ( degree < 0 || order < 0 )
    {
        // Set error message.
        std::stringstream errorMessage;
        errorMessage << "Error: the Legendre polynomial of = " << degree << " and order = "
                     << order << " is undefined." << std::endl;

        // Throw a run-time error.
        boost::throw_exception( boost::enable_error_info( std::runtime_error(
                                                              errorMessage.str( ) ) ) );
    }

    // Else if order is greater than degree...
    else if ( order > degree && degree >= 0 )
    {
        // Return zero.
        return 0.0;
    }

    // Else if order and degree are lower than 2...
    else if ( degree <= 1 && order <= 1 )
    {
        // Compute polynomial explicitly.
        return computeGeodesyLegendrePolynomialExplicit( degree, order, geodesyLegendreCache.getCurrentPolynomialParameter( ) );
    }

    // Else if degree and order are sectoral...
    else if ( degree == order )
    {
        // Obtain polynomial of degree one and order one.
        double degreeOneOrderOnePolynomial = geodesyLegendreCache.getLegendrePolynomial(
                    1, 1 );

        // Obtain prior sectoral polynomial.
        double priorSectoralPolynomial = geodesyLegendreCache.getLegendrePolynomial(
                    degree - 1, order - 1 );

        // Compute polynomial.
        return computeGeodesyLegendrePolynomialDiagonal(
                    degree, degreeOneOrderOnePolynomial, priorSectoralPolynomial );
    }

    // Else degree and order are zonal/tessoral...
    else
    {
        // Obtain prior degree polynomial.
        double oneDegreePriorPolynomial = geodesyLegendreCache.getLegendrePolynomial(
                    degree - 1, order );

        // Obtain two degrees prior polynomial.
        double twoDegreesPriorPolynomial = geodesyLegendreCache.getLegendrePolynomial(
                    degree - 2, order );

        // Compute polynomial.
        return computeGeodesyLegendrePolynomialVertical( degree,
                                                         order,
                                                         geodesyLegendreCache.getCurrentPolynomialParameter( ),
                                                         oneDegreePriorPolynomial,
                                                         twoDegreesPriorPolynomial );
    }
}

double computeGeodesyLegendrePolynomial( const int degree,
                                         const int order,
                                         const double legendreParameter )
{
    LegendreCache legendreCache( degree, order, 1 );
    legendreCache.update( legendreParameter );
    return computeGeodesyLegendrePolynomial( degree, order, legendreCache );
}

//! Compute derivative of unnormalized Legendre polynomial.
double computeLegendrePolynomialDerivative( const int order,
                                            const double polynomialParameter,
                                            const double currentLegendrePolynomial,
                                            const double incrementedLegendrePolynomial )
{
    // Return polynomial derivative.
    return incrementedLegendrePolynomial
            / std::sqrt( 1.0 - polynomialParameter * polynomialParameter )
            - static_cast< double >( order ) * polynomialParameter
            / ( 1.0 - polynomialParameter * polynomialParameter )
            * currentLegendrePolynomial;
}

//! Compute derivative of geodesy-normalized Legendre polynomial.
double computeGeodesyLegendrePolynomialDerivative( const int degree,
                                                   const int order,
                                                   const double polynomialParameter,
                                                   const double currentLegendrePolynomial,
                                                   const double incrementedLegendrePolynomial )
{
    // Compute normalization correction factor.
    double normalizationCorrection = std::sqrt( ( static_cast< double >( degree )
                                                  + static_cast< double >( order ) + 1.0 )
                                                * ( static_cast< double >( degree - order ) ) );

    // If order is zero apply multiplication factor.
    if ( order == 0 )
    {
        normalizationCorrection *= std::sqrt( 0.5 );
    }

    // Return polynomial derivative.
    return normalizationCorrection * incrementedLegendrePolynomial
            / std::sqrt( 1.0 - polynomialParameter * polynomialParameter )
            - static_cast< double >( order ) * polynomialParameter
            / ( 1.0 - polynomialParameter * polynomialParameter )
            * currentLegendrePolynomial;
}

//! Compute low degree/order unnormalized Legendre polynomial explicitly.
double computeLegendrePolynomialExplicit( const int degree,
                                          const int order,
                                          const double polynomialParameter )
{
    // Check which order is required for Legendre polynomial.
    switch( degree )
    {
    case 0:
        switch( order )
        {
        case 0:
            return 1.0;
        default:
            std::cerr << "Error, explicit legendre polynomial not possible for "
                      << degree << " " << order << std::endl;
        }
        break;
    case 1:
        switch( order )
        {
        case 0:
            return polynomialParameter;
        case 1:
            return std::sqrt( 1 - polynomialParameter * polynomialParameter );
        default:
            std::cerr << "Error, explicit legendre polynomial not possible for "
                      << degree << " " << order << std::endl;
        }
        break;
    case 2:
        switch( order )
        {
        case 0:
            return 0.5 * ( 3.0 * polynomialParameter * polynomialParameter - 1.0 );
        case 1:
            return 3.0 * polynomialParameter
                    * std::sqrt( 1.0 - polynomialParameter * polynomialParameter );
        case 2:
            return 3.0 * ( 1.0 - polynomialParameter * polynomialParameter );
        default:
            std::cerr << "Error, explicit legendre polynomial not possible for "
                      << degree << " " << order << std::endl;
        }
        break;
    case 3:
        switch( order )
        {
        case 0:
            return 0.5 * polynomialParameter
                    * ( 5.0 * polynomialParameter * polynomialParameter - 3.0 );
        case 1:
            return 1.5 * ( 5.0 * polynomialParameter * polynomialParameter - 1.0 )
                    * std::sqrt( 1.0 - polynomialParameter * polynomialParameter );
        case 2:
            return 15.0 * polynomialParameter * ( 1.0 - polynomialParameter * polynomialParameter );
        case 3:
            return 15.0 * ( 1.0 - polynomialParameter * polynomialParameter )
                    * std::sqrt( 1.0 - polynomialParameter * polynomialParameter );
        default:
            std::cerr << "Error, a explicit legendre polynomial not possible for "
                      << degree << " " << order << std::endl;
        }
        break;
    case 4:
        switch( order )
        {
        case 0:
            return ( 35.0 * polynomialParameter * polynomialParameter
                     * polynomialParameter * polynomialParameter
                     - 30.0 * polynomialParameter * polynomialParameter + 3.0 ) / 8.0;
        case 1:
            return -2.5 * ( 7.0 * polynomialParameter * polynomialParameter * polynomialParameter
                            - 3.0 * polynomialParameter )
                    * std::sqrt( 1.0 - polynomialParameter * polynomialParameter );
        case 2:
            return 15.0 / 2.0 * ( - 1.0 + 7.0 * polynomialParameter * polynomialParameter )
                    * ( 1.0 - polynomialParameter * polynomialParameter );
        case 3:
            return -105.0 * polynomialParameter * ( 1.0 - polynomialParameter * polynomialParameter )
                    * std::sqrt( 1.0 - polynomialParameter * polynomialParameter );
        case 4:
            return 105.0 * ( 1.0 - polynomialParameter * polynomialParameter )
                    * ( 1.0 - polynomialParameter * polynomialParameter );

        default:
            std::cerr << "Error, a explicit legendre polynomial not possible for "
                      << degree << " " << order << std::endl;
        }
        break;
    default:
        std::cerr << "Error, explicit legendre polynomial not possible for "
                  << degree << " " << order << std::endl;
    }
    return TUDAT_NAN;
}

//! Compute low degree/order geodesy-normalized Legendre polynomials explicitly.
double computeGeodesyLegendrePolynomialExplicit( const int degree,
                                                 const int order,
                                                 const double polynomialParameter )
{
    // If 0,0 term is requested return Legendre polynomial value.
    if ( degree == 0 && order == 0 )
    {
        return 1.0;
    }

    // Else if 1,0 term is requested return polynomial value.
    else if ( degree == 1 && order == 0 )
    {
        return std::sqrt( 3.0 ) * polynomialParameter;
    }

    // Else if 1,1 term is requested return polynomial value.
    else if ( degree == 1 && order == 1 )
    {
        return std::sqrt( 3.0 - 3.0 * polynomialParameter * polynomialParameter );
    }

    // Else the requested term cannot be computed; throw a run-time error.
    else
    {
        // Set error message.
        std::stringstream errorMessage;
        errorMessage  <<  "Error: computation of Legendre polynomial of = "  <<  degree
                      <<  " and order = "  <<  order  <<  " is not supported."  <<  std::endl;

        // Throw a run-time error.
        boost::throw_exception( boost::enable_error_info( std::runtime_error(
                                                              errorMessage.str( ) ) ) );
    }
}

//! Compute unnormalized Legendre polynomial through sectoral recursion.
double computeLegendrePolynomialDiagonal( const int degree,
                                          const double degreeOneOrderOnePolynomial,
                                          const double priorSectoralPolynomial )
{
    // Return polynomial.
    return ( 2.0 * static_cast< double >( degree ) - 1.0 )
            * degreeOneOrderOnePolynomial * priorSectoralPolynomial;

}

//! Compute geodesy-normalized Legendre polynomial through sectoral recursion.
double computeGeodesyLegendrePolynomialDiagonal( const int degree,
                                                 const double degreeOneOrderOnePolynomial,
                                                 const double priorSectoralPolynomial )
{
    // Return polynomial.
    return std::sqrt( ( 2.0 * static_cast< double >( degree ) + 1.0 )
                      / ( 6.0 * static_cast< double >( degree ) ) )
            * degreeOneOrderOnePolynomial * priorSectoralPolynomial;
}

//! Compute unnormalized Legendre polynomial through degree recursion.
double computeLegendrePolynomialVertical( const int degree,
                                          const int order,
                                          const double polynomialParameter,
                                          const double oneDegreePriorPolynomial,
                                          const double twoDegreesPriorPolynomial )
{
    // Return polynomial.
    return ( ( 2.0 * static_cast< double >( degree ) - 1.0 ) * polynomialParameter
             * oneDegreePriorPolynomial - ( static_cast< double >( degree + order ) - 1.0 )
             * twoDegreesPriorPolynomial ) / ( static_cast< double >( degree - order ) );
}

//! Compute geodesy-normalized Legendre polynomial through degree recursion.
double computeGeodesyLegendrePolynomialVertical( const int degree,
                                                 const int order,
                                                 const double polynomialParameter,
                                                 const double oneDegreePriorPolynomial,
                                                 const double twoDegreesPriorPolynomial )
{
    // Return polynomial.
    return std::sqrt( ( 2.0 * static_cast< double >( degree ) + 1.0 )
                      / ( ( static_cast< double >( degree + order ) ) * ( static_cast< double >( degree - order ) ) ) )
            * ( std::sqrt( 2.0 * static_cast< double >( degree ) - 1.0 ) * polynomialParameter * oneDegreePriorPolynomial
                - std::sqrt( ( static_cast< double >( degree + order ) - 1.0 )
                             * ( static_cast< double >( degree - order ) - 1.0 )
                             / ( 2.0 * static_cast< double >( degree ) - 3.0 ) )
                * twoDegreesPriorPolynomial );
}

//! Function to calculate the normalization factor for Legendre polynomials to geodesy-normalized.
double calculateLegendreGeodesyNormalizationFactor( const int degree, const int order )
{

    double deltaFunction = 0.0;
    if( order == 0 )
    {
        deltaFunction = 1.0;
    }

    double factor = std::sqrt(
        boost::math::factorial< double >( static_cast< double >( degree + order ) )
        / ( ( 2.0 - deltaFunction ) * ( 2.0 * static_cast< double >( degree ) + 1.0 )
            * boost::math::factorial< double >( static_cast< double >( degree - order ) ) ) );
    return 1.0 / factor;
}

} // namespace basic_mathematics
} // namespace tudat


#include "tasktorrent/tasktorrent.hpp"
#include <Eigen/Core>
#include <Eigen/Cholesky>
#include <fstream>
#include <array>
#include <random>
#include <mutex>
#include <iostream>
#include <map>
#include <memory>

#include <mpi.h>

typedef array<int, 2> int2;
typedef array<int, 3> int3;
typedef array<int, 4> int4;
typedef array<int, 5> int5;
typedef array<int, 6> int6;
typedef array<int, 7> int7;

struct scoped_timer {
  private:
    std::atomic<long long int>* time_us_;
    ttor::timer time_init_;
  public:
    scoped_timer(std::atomic<long long int>* time_us) {
        time_us_ = time_us;
        time_init_= ttor::wctime();
    }
    ~scoped_timer() {
        ttor::timer time_end_ = ttor::wctime();
        *time_us_ += static_cast<long long int>(1e6 * ttor::elapsed(time_init_, time_end_));
    }
};

ttor::view<double> make_view(Eigen::MatrixXd* A) {
    return ttor::view<double>(A->data(), A->size());
}

Eigen::MatrixXd make_from_view(ttor::view<double> A, int nrows) {
    Eigen::MatrixXd Add = Eigen::MatrixXd::Zero(nrows, nrows);
    assert(nrows * nrows == A.size());
    memcpy(Add.data(), A.data(), sizeof(double) * A.size());
    return Add;
}

void copy_from_view(Eigen::MatrixXd* dest, const ttor::view<double> A) {
    assert(dest->size() == A.size());
    memcpy(dest->data(), A.data(), sizeof(double) * A.size());
}

void accumulate(Eigen::MatrixXd* dest, const Eigen::MatrixXd* src) {
    assert(dest->size() == src->size());
    #pragma omp parallel for
    for(int k = 0; k < dest->size(); k++) {
        (*dest)(k) += (*src)(k);
    }
}

std::string to_string(int2 ij) {
    return to_string(ij[0]) + "_" + to_string(ij[1]);
}

std::string to_string(int3 ijk) {
    return to_string(ijk[0]) + "_" + to_string(ijk[1]) + "_" + to_string(ijk[2]);
}

/**
 * Matrix is of size N (global)
 * Each rank works on Nr x Nr
 * Each thread works on Nt x Nt
 *
 * There are n sub blocks on a given rank
 */
void gemm(const int N, const int Nt, const int n_threads, std::string logfile, const int verb, const bool test)
{
    const int rank = ttor::comm_rank();
    const int n_ranks = ttor::comm_size();
    const int n_ranks_1d = static_cast<int>(round(pow(n_ranks, 1.0/3.0)));
    assert(n_ranks_1d * n_ranks_1d * n_ranks_1d == n_ranks);
    const int rank_i = rank % n_ranks_1d;
    const int rank_j = (rank / n_ranks_1d) % n_ranks_1d;
    const int rank_k = rank / (n_ranks_1d * n_ranks_1d);
    const int3 rank_ijk = {rank_i, rank_j, rank_k};
    const int Nr = N / n_ranks_1d;
    assert(Nr * n_ranks_1d == N);
    const int n = Nr / Nt;
    assert(Nt * n == Nr);
    printf("Hello rank %d with 3d-index (%d %d %d) / (%d %d %d) from host %s, N %d, Nr %d, Nt %d, n %d\n", rank, rank_i, rank_j, rank_k, n_ranks_1d, n_ranks_1d, n_ranks_1d,
        ttor::processor_name().c_str(), N, Nr, Nt, n);

    printf("rank,%d\n", rank);
    printf("rank_i,%d\n", rank_i);
    printf("rank_j,%d\n", rank_j);
    printf("rank_k,%d\n", rank_k);
    printf("ntot,%d\n", N);
    printf("nrank,%d\n", Nr);
    printf("ntile,%d\n", Nt);
    printf("nthreads,%d\n", n_threads);
    printf("logfile,%s\n", logfile.c_str());
    printf("verb,%d\n", verb);
    printf("test,%d\n", test);

    auto rank_ijk_to_rank = [n_ranks_1d](int rank_i, int rank_j, int rank_k) {
        return rank_k * n_ranks_1d * n_ranks_1d + rank_j * n_ranks_1d + rank_i;
    };
    assert(rank_ijk_to_rank(rank_i, rank_j, rank_k) == rank);

    /**
     * Record timings
     **/
    std::atomic<long long int> send_copy_us_t(0);
    std::atomic<long long int> send_am_us_t(0);
    std::atomic<long long int> bcst_copy_us_t(0);
    std::atomic<long long int> bcst_am_us_t(0);
    std::atomic<long long int> gemm_us_t(0);
    std::atomic<long long int> gemm_copy_us_t(0);
    std::atomic<long long int> gemm_am_us_t(0);
    std::atomic<long long int> accu_us_t(0);

    /**
     * Original and final matrices
     **/
    // n x n matrix of Nt x Nt matrices, so Nr x Nr total
    std::vector<std::vector<Eigen::MatrixXd>> A_ij(n, std::vector<Eigen::MatrixXd>(n, Eigen::MatrixXd::Zero(Nt, Nt)));
    std::vector<std::vector<Eigen::MatrixXd>> C_ij(n, std::vector<Eigen::MatrixXd>(n, Eigen::MatrixXd::Zero(Nt, Nt)));
    std::vector<std::vector<Eigen::MatrixXd>> B_ij(n, std::vector<Eigen::MatrixXd>(n, Eigen::MatrixXd::Zero(Nt, Nt)));

    auto val_global = [](int i, int j) { return static_cast<double>(1 + i + j); };
    if(rank_k == 0) {
        for(int i = 0; i < n; i++) {
            for(int j = 0; j < n; j++) {
                auto val = [&](int i_, int j_) { return val_global(rank_i * Nr + i * Nt + i_, rank_j * Nr + j * Nt + j_); };
                A_ij[i][j] = Eigen::MatrixXd::NullaryExpr(Nt, Nt, val);
                B_ij[i][j] = Eigen::MatrixXd::NullaryExpr(Nt, Nt, val);
            }
        }
    }

    /**
     * Workspace
     **/
    std::vector<std::vector<Eigen::MatrixXd>> A_ijk(n, std::vector<Eigen::MatrixXd>(n, Eigen::MatrixXd::Zero(Nt, Nt)));
    std::vector<std::vector<Eigen::MatrixXd>> C_ijk(n, std::vector<Eigen::MatrixXd>(n, Eigen::MatrixXd::Zero(Nt, Nt)));
    std::vector<std::vector<Eigen::MatrixXd>> B_ijk(n, std::vector<Eigen::MatrixXd>(n, Eigen::MatrixXd::Zero(Nt, Nt)));
    std::vector<std::unique_ptr<std::atomic<int>>> C_ijk_counts(n * n);
    for(int i = 0; i < n*n; i++) {
        C_ijk_counts[i] = std::make_unique<std::atomic<int>>();
        C_ijk_counts[i]->store(0);
    }

    // C_ijk_accu[sub_i][sub_j][from] stores the results for (sub_i, sub_j) to be accumulated, from rank from
    std::vector<std::vector<std::vector<Eigen::MatrixXd>>> C_ijk_accu(
        n, std::vector<std::vector<Eigen::MatrixXd>>(
        n, std::vector<Eigen::MatrixXd>(
        n_ranks_1d, Eigen::MatrixXd::Zero(Nt, Nt))));

    MPI_Barrier(MPI_COMM_WORLD);

    /**
     * Initialize the runtime structures
     **/
    ttor::Communicator comm(MPI_COMM_WORLD, verb);
    ttor::Threadpool tp(n_threads, &comm, verb, "Wk_Gemm_" + to_string(rank) + "_");

    // send is indexed by int2, which are the sub blocks
    ttor::Taskflow<int2> send_Aij(&tp, verb);  // (i,j,0) sends A_ij to (i,j,j) for all i,j
    ttor::Taskflow<int2> send_Bij(&tp, verb);  // (i,j,0) sends B_ij to (i,j,i) for all i,j
    // send is indexed by int2, which are the sub blocks
    ttor::Taskflow<int2> bcst_Aij(&tp, verb);  // (i,j,j) sends A_ij along j to all (i,*,j) for all i,j
    ttor::Taskflow<int2> bcst_Bij(&tp, verb);  // (i,j,i) sends B_ij along i to all (*,j,i) for all i,j
    // gemm is indexed by int3, which are the sub blocks
    ttor::Taskflow<int3> gemm_Cijk(&tp, verb); // (i,j,k) compute C_ijk = A_ik * B_kj, send for accumulation reduction on (i,j,0)
    ttor::Taskflow<int3> accu_Cij(&tp, verb);  // accumulate (i,j,from) into (i,j)

    ttor::Logger log(1000000);
    if(logfile.size() > 0) {
        tp.set_logger(&log);
        comm.set_logger(&log);
    }

    /**
     * Send
     **/

    auto send_Aij_am = comm.make_active_msg([&](ttor::view<double>& Aij, int& sub_i, int& sub_j) {
        scoped_timer t(&send_am_us_t);
        copy_from_view(&A_ijk[sub_i][sub_j], Aij);
        bcst_Aij.fulfill_promise({sub_i, sub_j});
    });

    auto send_Bij_am = comm.make_active_msg([&](ttor::view<double>& Bij, int& sub_i, int& sub_j) {
        scoped_timer t(&send_am_us_t);
        copy_from_view(&B_ijk[sub_i][sub_j], Bij);
        bcst_Bij.fulfill_promise({sub_i, sub_j});
    });

    // (i,j,0) sends A_ij to (i,j,j) for all i,j
    send_Aij.set_task([&](int2 sub_ij){
        assert(rank_k == 0);
        scoped_timer t(&send_copy_us_t);
        int sub_i = sub_ij[0];
        int sub_j = sub_ij[1];
        ttor::view<double> A_view = make_view(&A_ij[sub_i][sub_j]);
        int dest = rank_ijk_to_rank(rank_i, rank_j, rank_j);
        if(dest == rank) {
            A_ijk[sub_i][sub_j] = A_ij[sub_i][sub_j];
            bcst_Aij.fulfill_promise({sub_i, sub_j});
        } else {
            send_Aij_am->send(dest, A_view, sub_i, sub_j);
        }
    }).set_indegree([&](int2) {
        return 1;
    }).set_mapping([&](int2 sub_ij) {
        // return (sub_ij[0] + n * sub_ij[1]) % n_threads;
        // return 0;
        return sub_ij[0] % n_threads;
    }).set_priority([&](int2 sub_ij){
        return 1.0 * n + (n - sub_ij[1]);
    }).set_binding([&](int2 sub_ij){
        return false;
        // return true;
    }).set_name([&](int2 sub_ij) { return "send_A_" + to_string(sub_ij) + "_" + to_string(rank_ijk); });

    // (i,j,0) sends B_ij to (i,j,i) for all i,j
    send_Bij.set_task([&](int2 sub_ij){
        assert(rank_k == 0);
        scoped_timer t(&send_copy_us_t);
        int sub_i = sub_ij[0];
        int sub_j = sub_ij[1];
        ttor::view<double> B_view = make_view(&B_ij[sub_i][sub_j]);
        int dest = rank_ijk_to_rank(rank_i, rank_j, rank_i);
        if(dest == rank) {
            B_ijk[sub_i][sub_j] = B_ij[sub_i][sub_j];
            bcst_Bij.fulfill_promise({sub_i, sub_j});
        } else {
            send_Bij_am->send(dest, B_view, sub_i, sub_j);
        }
    }).set_indegree([&](int2) {
        return 1;
    }).set_mapping([&](int2 sub_ij) {
        // return (sub_ij[0] + n * sub_ij[1]) % n_threads;
        // return 0;
        return (sub_ij[1] % n_threads);
    }).set_priority([&](int2 sub_ij){
        return 1.0 * n + (n - sub_ij[0]);
    }).set_binding([&](int2 sub_ij){
        return false;
        //return true;
    }).set_name([&](int2 sub_ij) { return "send_B_" + to_string(sub_ij) + "_" + to_string(rank_ijk); });

    /**
     * Broadcast
     **/

    auto bcst_Aij_am = comm.make_active_msg([&](ttor::view<double>& Aij, int &sub_i, int &sub_j) {
        scoped_timer t(&bcst_am_us_t);
        copy_from_view(&A_ijk[sub_i][sub_j], Aij);
        for(int k = 0; k < n; k++)
            gemm_Cijk.fulfill_promise({sub_i, k, sub_j});
    });

    auto bcst_Bij_am = comm.make_active_msg([&](ttor::view<double>& Bij, int &sub_i, int &sub_j) {
        scoped_timer t(&bcst_am_us_t);
        copy_from_view(&B_ijk[sub_i][sub_j], Bij);
        for(int k = 0; k < n; k++)
            gemm_Cijk.fulfill_promise({k, sub_j, sub_i});
    });

    // (i,j,j) sends A_ij along j to all (i,*,j) for all i,j
    bcst_Aij.set_task([&](int2 sub_ij){
        scoped_timer t(&bcst_copy_us_t);
        assert(rank_j == rank_k);
        int sub_i = sub_ij[0];
        int sub_j = sub_ij[1];
        ttor::view<double> A_view = make_view(&A_ijk[sub_i][sub_j]);
        for(int k = 0; k < n_ranks_1d; k++) {
            int dest = rank_ijk_to_rank(rank_i, k, rank_j);
            if(dest == rank) {
                for(int l = 0; l < n; l++)
                    gemm_Cijk.fulfill_promise({sub_i, l, sub_j});
            } else {
                bcst_Aij_am->send(dest, A_view, sub_i, sub_j);
            }
        }
    }).set_indegree([&](int2) {
        return 1;
    }).set_mapping([&](int2 sub_ij) {
        // return (sub_ij[0] + n * sub_ij[1]) % n_threads;
        // return 0;
        return (sub_ij[0] % n_threads);
    }).set_priority([&](int2 sub_ij){
        return 1.0 * n + (n - sub_ij[1]);
    }).set_binding([&](int2 sub_ij){
        return false;
        //return true;
    }).set_name([&](int2 sub_ij) { return "bcast_A_" + to_string(sub_ij) + "_" + to_string(rank_ijk); });

    // (i,j,i) sends B_ij along i to all (*,j,i) for all i,j
    bcst_Bij.set_task([&](int2 sub_ij){
        scoped_timer t(&bcst_copy_us_t);
        assert(rank_i == rank_k);
        int sub_i = sub_ij[0];
        int sub_j = sub_ij[1];
        ttor::view<double> B_view = make_view(&B_ijk[sub_i][sub_j]);
        for(int k = 0; k < n_ranks_1d; k++) {
            int dest = rank_ijk_to_rank(k, rank_j, rank_i);
            if(dest == rank) {
                for(int l = 0; l < n; l++)
                    gemm_Cijk.fulfill_promise({l, sub_j, sub_i});
            } else {
                bcst_Bij_am->send(dest, B_view, sub_i, sub_j);
            }
        }
    }).set_indegree([&](int2) {
        return 1;
    }).set_mapping([&](int2 sub_ij) {
        // return (sub_ij[0] + n * sub_ij[1]) % n_threads;
        // return 0;
        return (sub_ij[1] % n_threads);
    }).set_priority([&](int2 sub_ij){
        return 1.0 * n + (n - sub_ij[0]);
    }).set_binding([&](int2 sub_ij){
        return false;
        // return true;
    }).set_name([&](int2 sub_ij) { return "bcast_B_" + to_string(sub_ij) + "_" + to_string(rank_ijk); });

    /**
     * GEMM
     **/

    auto gemm_Cijk_am = comm.make_active_msg([&](ttor::view<double>& Cijk, int &sub_i, int &sub_j, int& from) {
        scoped_timer t(&gemm_am_us_t);
        copy_from_view(&C_ijk_accu[sub_i][sub_j][from], Cijk);
        accu_Cij.fulfill_promise({sub_i, sub_j, from});
    });

    // (i,j,k) compute C_ijk = A_ik * B_kj
    gemm_Cijk.set_task([&](int3 sub_ijk){
        int sub_i = sub_ijk[0];
        int sub_j = sub_ijk[1];
        int sub_k = sub_ijk[2];
        {
            scoped_timer t(&gemm_us_t);
            C_ijk[sub_i][sub_j].noalias() += A_ijk[sub_i][sub_k] * B_ijk[sub_k][sub_j];
        }
        //(*(C_ijk_counts[sub_i * n + sub_j]))++;
        scoped_timer t(&gemm_copy_us_t);
        if(sub_ijk[2] < n-1) {
            gemm_Cijk.fulfill_promise({sub_i, sub_j, sub_k+1});
        } else {
        //if(C_ijk_counts[sub_i * n + sub_j]->load() == n) {
            auto C_ijk_view = make_view(&C_ijk[sub_i][sub_j]);
            int dest = rank_ijk_to_rank(rank_i, rank_j, 0);
            if(dest == rank) {
                C_ijk_accu[sub_i][sub_j][rank_k] = C_ijk[sub_i][sub_j];
                accu_Cij.fulfill_promise({sub_i, sub_j, rank_k});
            } else {
                int k = rank_k;
                gemm_Cijk_am->send(dest, C_ijk_view, sub_i, sub_j, k);
            }
        }
    }).set_indegree([&](int3 sub_ijk) {
        return sub_ijk[2] == 0 ? 2 : 3; // 2 A_ik and B_kj blocks, + previous gemm
        // return 2;
    }).set_mapping([&](int3 sub_ijk) {
        if(n_threads == 1) return 0;
        // else return max(1, (sub_ijk[0] + sub_ijk[1] * n) % n_threads);
        else return max(1, (sub_ijk[0] + sub_ijk[1] * n) % n_threads); // + sub_ijk[2] * n * n) % n_threads);
        // return (sub_ijk[0] + sub_ijk[1] * n + sub_ijk[2] * n * n) % n_threads;
    }).set_binding([&](int3 sub_ijk) {
        return false;
    }).set_priority([&](int3 sub_ijk){
        return 0.0 * n + (n - sub_ijk[2]);
    }).set_name([&](int3 sub_ijk) { return "gemm_C_" + to_string(sub_ijk) + "_" + to_string(rank_ijk); });

    // (i,j,k) compute C_ijk = A_ik * B_kj
    accu_Cij.set_task([&](int3 sub_ij_from){
        scoped_timer t(&accu_us_t);
        int sub_i = sub_ij_from[0];
        int sub_j = sub_ij_from[1];
        int from  = sub_ij_from[2];
        C_ij[sub_i][sub_j] += C_ijk_accu[sub_i][sub_j][from];
    }).set_indegree([&](int3) {
        return 1;
    }).set_mapping([&](int3 sub_ij_from) {
        if(n_threads == 1) return 0;
        else return max(1, (sub_ij_from[0] + n * sub_ij_from[1]) % n_threads);
        // return (sub_ij_from[0] + n * sub_ij_from[1]) % n_threads;
    }).set_binding([&](int3) {
        return true;
    }).set_priority([&](int3){
        return 0.0;
    }).set_name([&](int3 sub_ij_from) { return "accu_C_" + to_string(sub_ij_from) + "_" + to_string(rank_ijk); });

    printf("Starting 3D Gemm...\n");
    ttor::timer t0 = ttor::wctime();
    if(rank_k == 0) {
        for(int i = 0; i < n; i++) {
            for(int j = 0; j < n; j++) {
                send_Aij.fulfill_promise({i,j});
                send_Bij.fulfill_promise({i,j});
            }
        }
    }
    tp.join();
    ttor::timer t1 = ttor::wctime();
    double total_time = ttor::elapsed(t0, t1);
    double gemm_time = gemm_us_t.load() * 1e-6;
    double gemm_time_per_thread = gemm_time / n_threads;
    printf("Done\n");
    printf("total_time,%e\n", ttor::elapsed(t0, t1));
    printf("send_copy_us_t,%e\n",send_copy_us_t.load() * 1e-6);
    printf("send_am_us_t,%e\n",send_am_us_t.load() * 1e-6);
    printf("bcst_copy_us_t,%e\n",bcst_copy_us_t.load() * 1e-6);
    printf("bcst_am_us_t,%e\n",bcst_am_us_t.load() * 1e-6);
    printf("gemm_us_t,%e\n",gemm_us_t.load() * 1e-6);
    printf("gemm_copy_us_t,%e\n",gemm_copy_us_t.load() * 1e-6);
    printf("gemm_am_us_t,%e\n",gemm_am_us_t.load() * 1e-6);
    printf("accu_us_t,%e\n",accu_us_t.load() * 1e-6);
    // For easy CSV parsing
    printf("[rank]>>>>Ntot,Nrank,Ntile,rank,n_ranks,nthreads,tot_time,gemm_time,gemm_time_per_thread\n");
    printf("[%d]>>>>%d,%d,%d,%d,%d,%d,%e,%e,%e\n",rank,N,Nr,Nt,rank,n_ranks,n_threads,total_time,gemm_time,gemm_time_per_thread);

    if(logfile.size() > 0) {
        std::ofstream logstream;
        std::string filename = logfile + ".log." + to_string(rank);
        printf("Saving log to %s\n", filename.c_str());
        logstream.open(filename);
        logstream << log;
        logstream.close();
    }

    MPI_Barrier(MPI_COMM_WORLD);
    if(test && rank_k == 0) {
        // Send all to 0
        int n_received = 0;
        int n_expected = (rank == 0 ? n * n * n_ranks_1d * n_ranks_1d : 0);
        Eigen::MatrixXd C_test = Eigen::MatrixXd::Zero(N, N);
        ttor::Communicator comm(MPI_COMM_WORLD, verb);
        auto am = comm.make_active_msg([&](ttor::view<double>& A, int& rank_i_from, int& rank_j_from, int& sub_i, int& sub_j){
            C_test.block(rank_i_from * Nr + sub_i * Nt, rank_j_from * Nr + sub_j * Nt, Nt, Nt) = make_from_view(A, Nt);
            n_received++;
        });
        int rank_i_from = rank_i;
        int rank_j_from = rank_j;
        for(int sub_i = 0; sub_i < n; sub_i++) {
            for(int sub_j = 0; sub_j < n; sub_j++) {
                auto C_view = make_view(&C_ij[sub_i][sub_j]);
                am->send(0, C_view, rank_i_from, rank_j_from, sub_i, sub_j);
            }
        }
        while((!comm.is_done()) || (n_received < n_expected)) {
            comm.progress();
        }
        // Compute reference on 0
        if(rank == 0) {
            Eigen::MatrixXd A_ref = Eigen::MatrixXd::NullaryExpr(N, N, val_global);
            Eigen::MatrixXd B_ref = Eigen::MatrixXd::NullaryExpr(N, N, val_global);
            ttor::timer t0 = ttor::wctime();
            Eigen::MatrixXd C_ref = A_ref * B_ref;
            ttor::timer t1 = ttor::wctime();
            double error = (C_ref - C_test).norm() / C_ref.norm();
            printf("\n==> GEMM error %e\n\n", error);
            printf("Reference code took %e\n", ttor::elapsed(t0, t1));
            assert(error <= 1e-12);
        }
    }
}

int main(int argc, char **argv)
{
    int req = MPI_THREAD_FUNNELED;
    int prov = -1;

    MPI_Init_thread(NULL, NULL, req, &prov);

    assert(prov == req);

    int N = 128;
    int Nt = 8;
    int n_threads = 1;
    int verb = 0;
    bool test = true;
    std::string logfile = "";

    if (argc >= 2)
    {
        N = atoi(argv[1]);
        assert(N > 0);
    }

    if (argc >= 3) {
        Nt = atoi(argv[2]);
        assert(Nt > 0);
        assert(Nt <= N);
    }

    if (argc >= 4) {
        n_threads = atoi(argv[3]);
        assert(n_threads > 0);
    }

    if (argc >= 5) {
        logfile = argv[4];
    }

    if (argc >= 6) {
        verb = atoi(argv[5]);
        assert(verb >= 0);
    }

    if (argc >= 7) {
        test = static_cast<bool>(atoi(argv[6]));
    }

    if(ttor::comm_rank() == 0) printf("Usage: ./3d_gemm N Nt n_threads logfile verb test\n");
    if(ttor::comm_rank() == 0) printf("Arguments: N (global matrix size) %d, Nt (smallest block size) %d, n_threads %d, logfile %s, verb %d, test %d\n", N, Nt, n_threads, logfile.c_str(), verb, test);

    gemm(N, Nt, n_threads, logfile, verb, test);

    MPI_Finalize();
}


///////////////////////////////////////////////////////////////////////////////
// importance_sampling::find_scale_to_finite_sum.hpp                         //
//                                                                           //
//  Copyright 2009 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_IMPORTANCE_SAMPLING_WEIGHTS_FIND_SCALE_TO_FINITE_SUM_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_IMPORTANCE_SAMPLING_WEIGHTS_FIND_SCALE_TO_FINITE_SUM_HPP_ER_2009
#include <cmath>
#include <numeric>
#include <stdexcept>
#include <boost/lambda/lambda.hpp>
#include <boost/range.hpp>
#include <boost/format.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/math/tools/precision.hpp>
#include <boost/numeric/conversion/bounds.hpp>
#include <boost/statistics/detail/importance_sampling/weights/maximal_finite_sums.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace importance_sampling{

    // Finds c such that the sum over sum{*i/c, i in [b,e)} < inf using the
    // bisection method.
    //
    // Warning: The value c is not insensitive to permutations of [b,e), due
    // to non-associativity in the fp system
    template<typename InIt>
    typename iterator_value<InIt>::type
    find_scale_to_finite_sum(
        InIt b,InIt e,
        typename iterator_value<InIt>::type low_init,
        typename iterator_value<InIt>::type high_init
    );

    // This version may be faster than that above with low = 1, high = highest
    template<typename InIt>
    typename iterator_value<InIt>::type
    find_scale_to_finite_sum(InIt b,InIt e);

    // Implementation //

    template<typename InIt>
    typename iterator_value<InIt>::type
    find_scale_to_finite_sum(
        InIt b,InIt e,
        typename iterator_value<InIt>::type low_init,
        typename iterator_value<InIt>::type high_init
    ){
        typedef typename iterator_value<InIt>::type val_;
        static val_ zero = static_cast<val_>(0);
        static val_ two = static_cast<val_>(2);
        static val_ eps = boost::math::tools::epsilon<val_>();

        BOOST_ASSERT(low_init < high_init);
        BOOST_ASSERT(low_init > zero);
        BOOST_ASSERT(high_init > zero);
        val_ low = low_init;
        val_ high = high_init;
        val_ mid = (low + high)/two;
        val_ delta, acc;

        do{
            delta = high - low;
            acc =
                std::accumulate(b,e,zero, lambda::_1 + ( lambda::_2 / mid) );
            if(boost::math::isinf(acc)){
                low = mid;
            }else{
                high = mid;
            }
            mid = (low+high)/two;
        }while(
            delta - (high - low)>eps
        );
        static
            const char* str = "%3% = find_scale_to_finite_sum(b,e,%1%,%2%) = inf";
        if(
            boost::math::isinf(
                std::accumulate(b,e,zero, lambda::_1 + ( lambda::_2 / high) )
            )
        ){
            format f(str); f%low_init%high_init%high;
            throw std::runtime_error(f.str());
        }
        return high;
    }

    template<typename InIt>
    typename iterator_value<InIt>::type
    find_scale_to_finite_sum(InIt b,InIt e){
        typedef typename iterator_value<InIt>::type val_;
        typedef numeric::bounds<val_>               bounds_;
        typedef std::vector<val_>                   vec_;
        static val_ zero = static_cast<val_>(0);
        static val_ one = static_cast<val_>(1);
        static val_ two = static_cast<val_>(2);
        static val_ highest = bounds_::highest();
        vec_ vec;
        maximal_finite_sums(b,e,std::back_inserter(vec));
        val_ low = one;
        val_ high = highest;
        val_ mid = find_scale_to_finite_sum(
            boost::begin(vec),
            boost::end(vec),
            low,high
        );
        low = mid;
        high = mid;
        while(
            !boost::math::isinf(
                std::accumulate(b,e,zero, lambda::_1 + ( lambda::_2 / low) )
            )
        ){
            low /= two;
            if(low<one){
                low = one;
                break;
            }
        }
        while(
            boost::math::isinf(
                std::accumulate(b,e,zero, lambda::_1 + ( lambda::_2 / high) )
            )
        ){
            high *= two;
            if(boost::math::isinf(high)){
                high = highest;
                break;
            }
        }
        return find_scale_to_finite_sum(b,e,low,high);
    }

}// importance_weights
}// detail
}// statistics
}// boost

#endif