/**
* Copyright 2019, ftdlyc <yclu.cn@gmail.com>
* Licensed under the MIT license.
*/

#include <algorithm>
#include <functional>
#include <vector>

#include <ceres/ceres.h>
#include <ceres/loss_function.h>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/SVD>

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

#include "libcalib/calibration.h"
#include "libcalib/ceres_type.h"

namespace calib {

void init_camera_params(const std::vector<std::vector<cv::Point2d>>& world_points,
                        const std::vector<std::vector<cv::Point2d>>& image_points,
                        std::vector<double>& K,
                        std::vector<double>& D,
                        std::vector<std::vector<double>>& Rwc,
                        std::vector<std::vector<double>>& Twc) {
  int num_images = world_points.size();
  int num_points = 0;
  for(int i = 0; i < num_images; ++i) {
    num_points += world_points[i].size();
  }

  std::vector<cv::Mat> homo_vec;
  Eigen::MatrixXd mat_v = Eigen::MatrixXd::Zero(2 * num_images, 6);
  for(int i = 0; i < num_images; ++i) {
    cv::Mat homo = cv::findHomography(world_points[i], image_points[i], cv::RANSAC);
    auto homo_at = [&homo](int x, int y) { return homo.at<double>(y - 1, x - 1); };
    mat_v.block(2 * i, 0, 1, 6) << homo_at(1, 1) * homo_at(2, 1),
        homo_at(1, 1) * homo_at(2, 2) + homo_at(1, 2) * homo_at(2, 1),
        homo_at(1, 2) * homo_at(2, 2),
        homo_at(1, 3) * homo_at(2, 1) + homo_at(1, 1) * homo_at(2, 3),
        homo_at(1, 3) * homo_at(2, 2) + homo_at(1, 2) * homo_at(2, 3),
        homo_at(1, 3) * homo_at(2, 3);
    mat_v.block(2 * i + 1, 0, 1, 6) << homo_at(1, 1) * homo_at(1, 1) - homo_at(2, 1) * homo_at(2, 1),
        homo_at(1, 1) * homo_at(1, 2) + homo_at(1, 2) * homo_at(1, 1) -
            homo_at(2, 1) * homo_at(2, 2) - homo_at(2, 2) * homo_at(2, 1),
        homo_at(1, 2) * homo_at(1, 2) - homo_at(2, 2) * homo_at(2, 2),
        homo_at(1, 3) * homo_at(1, 1) + homo_at(1, 1) * homo_at(1, 3) -
            homo_at(2, 3) * homo_at(2, 1) - homo_at(2, 1) * homo_at(2, 3),
        homo_at(1, 3) * homo_at(1, 2) + homo_at(1, 2) * homo_at(1, 3) -
            homo_at(2, 3) * homo_at(2, 2) - homo_at(2, 2) * homo_at(2, 3),
        homo_at(1, 3) * homo_at(1, 3) - homo_at(2, 3) * homo_at(2, 3);
    homo_vec.emplace_back(homo);
  }

  Eigen::JacobiSVD<Eigen::MatrixXd> svd(mat_v.transpose() * mat_v, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix<double, 6, 6> mat_b = svd.matrixV().cast<double>();
  double b11                        = mat_b(0, 5);
  double b12                        = mat_b(1, 5);
  double b22                        = mat_b(2, 5);
  double b13                        = mat_b(3, 5);
  double b23                        = mat_b(4, 5);
  double b33                        = mat_b(5, 5);
  double cy                         = (b12 * b13 - b11 * b23) / (b11 * b22 - b12 * b12);
  double lambda                     = b33 - (b13 * b13 + cy * (b12 * b13 - b11 * b23)) / b11;
  double fx                         = sqrt(lambda / b11);
  double fy                         = sqrt(lambda * b11 / (b11 * b22 - b12 * b12));
  double radio                      = -b12 * fx * fx * fy / lambda;
  double cx                         = radio * cy / fx - b13 * fx * fx / lambda;
  K[0]                              = fx;
  K[1]                              = fy;
  K[2]                              = 0.;
  K[3]                              = cx;
  K[4]                              = cy;
  Eigen::Matrix3d K_eigen;
  K_eigen << K[0], K[2], K[3],
      0, K[1], K[4],
      0, 0, 1;

  for(int i = 0; i < num_images; ++i) {
    Eigen::Matrix3d rotation_eigen;
    Eigen::Matrix<double, 3, 1> translation_eigen;
    Eigen::Matrix3d homo_eigen;
    cv::cv2eigen(homo_vec[i], homo_eigen);
    double s              = (K_eigen.inverse() * homo_eigen.col(0)).norm();
    rotation_eigen.col(0) = K_eigen.inverse() * homo_eigen.col(0) / s;
    rotation_eigen.col(1) = K_eigen.inverse() * homo_eigen.col(1) / s;
    rotation_eigen.col(2) = rotation_eigen.col(0).cross(rotation_eigen.col(1));
    translation_eigen     = K_eigen.inverse() * homo_eigen.col(2) / s;

    cv::Mat rotation;
    cv::Mat angle_axis;
    cv::eigen2cv(rotation_eigen, rotation);
    cv::Rodrigues(rotation, angle_axis);

    Rwc[i][0] = angle_axis.at<double>(0);
    Rwc[i][1] = angle_axis.at<double>(1);
    Rwc[i][2] = angle_axis.at<double>(2);
    Twc[i][0] = translation_eigen(0, 0);
    Twc[i][1] = translation_eigen(1, 0);
    Twc[i][2] = translation_eigen(2, 0);
  }

  D[0] = 0.;
  D[1] = 0.;
  D[2] = 0.;
  D[3] = 0.;
  D[4] = 0.;
}

double optimize_params(const std::vector<std::vector<cv::Point2d>>& world_points,
                       const std::vector<std::vector<cv::Point2d>>& image_points,
                       std::vector<double>& K,
                       std::vector<double>& D,
                       std::vector<std::vector<double>>& Rwc,
                       std::vector<std::vector<double>>& Twc) {
  std::vector<double> intrinsics = {K[0], K[1], K[3], K[4], D[0], D[1], D[2], D[3], D[4]};
  std::vector<std::vector<double>> se3(world_points.size(), std::vector<double>(6));
  for(int i = 0; i < world_points.size(); ++i) {
    se3[i][0] = Twc[i][0];
    se3[i][1] = Twc[i][1];
    se3[i][2] = Twc[i][2];
    se3[i][3] = Rwc[i][0];
    se3[i][4] = Rwc[i][1];
    se3[i][5] = Rwc[i][2];
  }

  ceres::Problem problem;
  ceres::HuberLoss* loss_fuction = new ceres::HuberLoss(4.5);
  for(int i = 0; i < world_points.size(); ++i) {
    for(int j = 0; j < world_points[i].size(); ++j) {
      problem.AddResidualBlock(new ProjectCostFunction(world_points[i][j], image_points[i][j]), nullptr, intrinsics.data(), se3[i].data());
    }
    problem.SetParameterization(se3[i].data(), new SE3Parameterization());
  }

  ceres::Solver::Options options;
  options.linear_solver_type           = ceres::DENSE_QR;
  options.minimizer_progress_to_stdout = false;
  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);

  K = {intrinsics[0], intrinsics[1], 0., intrinsics[2], intrinsics[3]};
  D = {intrinsics[4], intrinsics[5], intrinsics[6], intrinsics[7], intrinsics[8]};
  for(int i = 0; i < world_points.size(); ++i) {
    Twc[i][0] = se3[i][0];
    Twc[i][1] = se3[i][1];
    Twc[i][2] = se3[i][2];
    Rwc[i][0] = se3[i][3];
    Rwc[i][1] = se3[i][4];
    Rwc[i][2] = se3[i][5];
  }

  double err = sqrt(2 * summary.final_cost / summary.num_residual_blocks);
  return err;
}

double calibrate_camera(const std::vector<std::vector<cv::Point2d>>& world_points,
                        const std::vector<std::vector<cv::Point2d>>& image_points,
                        std::vector<double>& K,
                        std::vector<double>& D,
                        std::vector<std::vector<double>>& Rwc,
                        std::vector<std::vector<double>>& Twc) {
  init_camera_params(world_points, image_points, K, D, Rwc, Twc);
  double err = optimize_params(world_points, image_points, K, D, Rwc, Twc);
  return err;
}

void init_stereo_params(std::vector<std::vector<double>>& Rwc1,
                        std::vector<std::vector<double>>& Rwc2,
                        std::vector<std::vector<double>>& Twc1,
                        std::vector<std::vector<double>>& Twc2,
                        std::vector<double>& R,
                        std::vector<double>& T) {
  auto num_images       = static_cast<int>(Rwc1.size());
  Eigen::MatrixXd mat_a = Eigen::MatrixXd::Zero(9 * num_images, 9);
  Eigen::MatrixXd mat_b = Eigen::MatrixXd::Zero(9 * num_images, 1);
  for(int i = 0; i < num_images; ++i) {
    Eigen::Matrix3d r1_t, r2;
    ceres::AngleAxisToRotationMatrix(Rwc1[i].data(), r1_t.data());
    ceres::AngleAxisToRotationMatrix(Rwc2[i].data(), r2.data());
    r1_t.transposeInPlace();
    mat_a.block(9 * i, 0, 3, 3)     = r1_t;
    mat_a.block(9 * i + 3, 3, 3, 3) = r1_t;
    mat_a.block(9 * i + 6, 6, 3, 3) = r1_t;
    mat_b.block(9 * i, 0, 9, 1) << r2(0, 0), r2(0, 1), r2(0, 2),
        r2(1, 0), r2(1, 1), r2(1, 2),
        r2(2, 0), r2(2, 1), r2(2, 2);
  }
  Eigen::MatrixXd r = (mat_a.transpose() * mat_a).inverse() * mat_a.transpose() * mat_b;
  r.resize(3, 3);
  r.transposeInPlace();

  T[0] = T[1] = T[2] = 0.;
  for(int i = 0; i < num_images; ++i) {
    Eigen::Matrix<double, 3, 1> t, t1(Twc1[i].data()), t2(Twc2[i].data());
    t = t2 - r * t1;
    T[0] += t(0, 0);
    T[1] += t(1, 0);
    T[2] += t(2, 0);
  }
  T[0] /= num_images;
  T[1] /= num_images;
  T[2] /= num_images;
  ceres::RotationMatrixToAngleAxis(r.data(), R.data());
}

double optimize_stereo_params(const std::vector<std::vector<cv::Point2d>>& world_points,
                              const std::vector<std::vector<cv::Point2d>>& image_points_1,
                              const std::vector<std::vector<cv::Point2d>>& image_points_2,
                              std::vector<double>& K1,
                              std::vector<double>& K2,
                              std::vector<double>& D1,
                              std::vector<double>& D2,
                              std::vector<std::vector<double>>& Rwc1,
                              std::vector<std::vector<double>>& Rwc2,
                              std::vector<std::vector<double>>& Twc1,
                              std::vector<std::vector<double>>& Twc2,
                              std::vector<double>& R,
                              std::vector<double>& T) {
  std::vector<double> intrinsics_1 = {K1[0], K1[1], K1[3], K1[4], D1[0], D1[1], D1[2], D1[3], D1[4]};
  std::vector<double> intrinsics_2 = {K2[0], K2[1], K2[3], K2[4], D2[0], D2[1], D2[2], D2[3], D2[4]};
  std::vector<std::vector<double>> se3_1(world_points.size(), std::vector<double>(6));
  std::vector<double> se3 = {T[0], T[1], T[2], R[0], R[1], R[2]};
  for(int i = 0; i < world_points.size(); ++i) {
    se3_1[i][0] = Twc1[i][0];
    se3_1[i][1] = Twc1[i][1];
    se3_1[i][2] = Twc1[i][2];
    se3_1[i][3] = Rwc1[i][0];
    se3_1[i][4] = Rwc1[i][1];
    se3_1[i][5] = Rwc1[i][2];
  }

  ceres::Problem problem;
  ceres::HuberLoss* loss_fuction = new ceres::HuberLoss(4.5);
  for(int i = 0; i < world_points.size(); ++i) {
    for(int j = 0; j < world_points[i].size(); ++j) {
      problem.AddResidualBlock(new StereoCostFunction(world_points[i][j], image_points_1[i][j], image_points_2[i][j]), nullptr,
                               intrinsics_1.data(), intrinsics_2.data(), se3_1[i].data(), se3.data());
    }
    problem.SetParameterization(se3_1[i].data(), new SE3Parameterization());
  }
  problem.SetParameterization(se3.data(), new SE3Parameterization());

  ceres::Solver::Options options;
  options.linear_solver_type           = ceres::DENSE_QR;
  options.trust_region_strategy_type   = ceres::DOGLEG;
  options.minimizer_progress_to_stdout = false;
  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);

  K1 = {intrinsics_1[0], intrinsics_1[1], 0., intrinsics_1[2], intrinsics_1[3]};
  D1 = {intrinsics_1[4], intrinsics_1[5], intrinsics_1[6], intrinsics_1[7], intrinsics_1[8]};
  K2 = {intrinsics_2[0], intrinsics_2[1], 0., intrinsics_2[2], intrinsics_2[3]};
  D2 = {intrinsics_2[4], intrinsics_2[5], intrinsics_2[6], intrinsics_2[7], intrinsics_2[8]};
  R  = {se3[3], se3[4], se3[5]};
  T  = {se3[0], se3[1], se3[2]};

  double q[4], q1[4], q2[4];
  ceres::AngleAxisToQuaternion(R.data(), q);
  for(int i = 0; i < world_points.size(); ++i) {
    Twc1[i][0] = se3_1[i][0];
    Twc1[i][1] = se3_1[i][1];
    Twc1[i][2] = se3_1[i][2];
    Rwc1[i][0] = se3_1[i][3];
    Rwc1[i][1] = se3_1[i][4];
    Rwc1[i][2] = se3_1[i][5];

    ceres::AngleAxisToQuaternion(Rwc1[i].data(), q1);
    ceres::QuaternionProduct(q, q1, q2);
    ceres::QuaternionToAngleAxis(q2, Rwc2[i].data());
    ceres::AngleAxisRotatePoint(R.data(), Twc1[i].data(), Twc2[i].data());
    Twc2[i][0] += T[0];
    Twc2[i][1] += T[1];
    Twc2[i][2] += T[2];
  }

  double err = sqrt(2 * summary.final_cost / summary.num_residual_blocks / 2);
  return err;
}

double calibrate_stereo_camera(const std::vector<std::vector<cv::Point2d>>& world_points,
                               const std::vector<std::vector<cv::Point2d>>& image_points_1,
                               const std::vector<std::vector<cv::Point2d>>& image_points_2,
                               std::vector<double>& K1,
                               std::vector<double>& K2,
                               std::vector<double>& D1,
                               std::vector<double>& D2,
                               std::vector<std::vector<double>>& Rwc1,
                               std::vector<std::vector<double>>& Rwc2,
                               std::vector<std::vector<double>>& Twc1,
                               std::vector<std::vector<double>>& Twc2,
                               std::vector<double>& R,
                               std::vector<double>& T) {
  init_stereo_params(Rwc1, Rwc2, Twc1, Twc2, R, T);
  double err = optimize_stereo_params(world_points, image_points_1, image_points_2, K1, K2, D1, D2, Rwc1, Rwc2, Twc1, Twc2, R, T);
  return err;
}

} // namespace calib


/*    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
 *      lambertTargeterIzzo.h/.cpp source files, tudat revision 455/466.
 *      PyKEP kepler toolbox, Dario Izzo, ESA Advanced Concepts Team.
 *
 *    Notes
 *      This is a new implementation of the lambertTargeterIzzo class, for better adaptability and
 *      extension towards subclasses and future improvements/additions. Therefore, it replaces the
 *      lambertTargeterIzzo class while still providing the same functionality.
 *
 */

#include <cmath>

#include <boost/math/special_functions.hpp> // For asinh and acosh
#include <boost/exception/all.hpp> // For exceptions in sanity checks
#include <Eigen/Dense> // for cross product issues (can someone explain why, exactly?)

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

#include "Tudat/Astrodynamics/MissionSegments/zeroRevolutionLambertTargeterIzzo.h"
#include "Tudat/Mathematics/BasicMathematics/convergenceException.h"

namespace tudat
{
namespace mission_segments
{

//! Get radial velocity at departure.
double ZeroRevolutionLambertTargeterIzzo::getRadialVelocityAtDeparture( )
// Based on lambertTargeterIzzo class.
{
    // If execute has not been called yet, execute.
    if ( !solved ) execute( );

    // Determine radial unit vector.
    const Eigen::Vector3d radialUnitVectorAtDeparture = cartesianPositionAtDeparture.normalized( );

    // Compute radial velocity at departure.
    return cartesianVelocityAtDeparture.dot( radialUnitVectorAtDeparture );
}

//! Get transverse velocity at departure.
double ZeroRevolutionLambertTargeterIzzo::getTransverseVelocityAtDeparture( )
// Based on lambertTargeterIzzo class.
{
    // If execute has not been called yet, execute.
    if ( !solved ) execute( );

    // Compute angular momemtum vector.
    const Eigen::Vector3d angularMomentumVector =
            cartesianPositionAtDeparture.cross( cartesianVelocityAtDeparture );

    // Compute normalized angular momentum vector.
    const Eigen::Vector3d angularMomentumUnitVector = angularMomentumVector.normalized( );

    // Determine radial unit vector.
    const Eigen::Vector3d radialUnitVectorAtDeparture = cartesianPositionAtDeparture.normalized( );

    // Compute tangential unit vector.
    Eigen::Vector3d tangentialUnitVectorAtDeparture =
            angularMomentumUnitVector.cross( radialUnitVectorAtDeparture );

    // Compute tangential velocity at departure.
    return cartesianVelocityAtDeparture.dot( tangentialUnitVectorAtDeparture );
}

//! Get radial velocity at arrival.
double ZeroRevolutionLambertTargeterIzzo::getRadialVelocityAtArrival( )
// Based on lambertTargeterIzzo class.
{
    // If execute has not been called yet, execute.
    if ( !solved ) execute( );

    // Determine radial unit vector.
    const Eigen::Vector3d radialUnitVectorAtArrival = cartesianPositionAtArrival.normalized( );

    // Compute radial velocity at arrival.
    return cartesianVelocityAtArrival.dot( radialUnitVectorAtArrival );
}

//! Get transverse velocity at arrival.
double ZeroRevolutionLambertTargeterIzzo::getTransverseVelocityAtArrival( )
// Based on lambertTargeterIzzo class.
{
    // If execute has not been called yet, execute.
    if ( !solved ) execute( );

    // Compute angular momemtum vector.
    const Eigen::Vector3d angularMomentumVector =
            cartesianPositionAtArrival.cross( cartesianVelocityAtArrival );

    // Compute normalized angular momentum vector.
    const Eigen::Vector3d angularMomentumUnitVector = angularMomentumVector.normalized( );

    // Determine radial unit vector.
    const Eigen::Vector3d radialUnitVectorAtArrival = cartesianPositionAtArrival.normalized( );

    // Compute tangential unit vector.
    Eigen::Vector3d tangentialUnitVectorAtArrival
            = angularMomentumUnitVector.cross( radialUnitVectorAtArrival );

    // Compute tangential velocity at departure.
    return cartesianVelocityAtArrival.dot( tangentialUnitVectorAtArrival );
}

//! Get semi-major axis.
double ZeroRevolutionLambertTargeterIzzo::getSemiMajorAxis( )
// Based on lambertTargeterIzzo class.
{
    // If execute has not been called yet, execute.
    if ( !solved ) execute( );

    // Compute specific orbital energy: eps = v^2/ - mu/r.
    const double specificOrbitalEnergy = cartesianVelocityAtDeparture.squaredNorm( ) / 2.0
            - gravitationalParameter / cartesianPositionAtDeparture.norm( );

    // Compute semi-major axis: a = -mu / 2*eps.
    return -gravitationalParameter / ( 2.0 * specificOrbitalEnergy );
}

// What about get retrograde flag, tolerance and max number of iterations? Might be useful to check.

//! Execute the solving procedure.
void ZeroRevolutionLambertTargeterIzzo::execute( )
{
    // Sanity checks
    sanityCheckTimeOfFlight( );
    sanityCheckGravitationalParameter( );

    // Transform dimensional parameters to dimensionless parameters if not done already (e.g. in
    // multirevolution case
    if( !transformed ) transformDimensions( );

    // Solve root (single rev)
    double xResult = ZeroRevolutionLambertTargeterIzzo::computeRootTimeOfFlight( );

    // Reconstruct Vs
    computeVelocities( xResult );

    solved = true;
}

//! Sanity check time of flight.
void ZeroRevolutionLambertTargeterIzzo::sanityCheckTimeOfFlight( )
{
    // If time of flight is negative, throw an exception
    if ( timeOfFlight < 0 )
    {
        // Throw exception.
        throw std::runtime_error(
                    "Time-of-flight specified in Lambert problem must be strictly positive. Specified time-of-flight in days." +
                                         std::to_string( timeOfFlight ) );
    }
    // Else, do nothing and continue.
}

//! Sanity check gravitational parameter.
void ZeroRevolutionLambertTargeterIzzo::sanityCheckGravitationalParameter( )
{
    // If gravitational parameter is negative, throw an exception
    if ( gravitationalParameter < 0 )
    {
        // Throw exception.
        throw std::runtime_error(
                    "Gravitational parameter specified in Lambert problem must be strictly positive. Specified gravitational parameter: " +
                    std::to_string( gravitationalParameter ) );
    }
    // Else, do nothing and continue.
}

//! Transform input to sub results in adimensional units.
void ZeroRevolutionLambertTargeterIzzo::transformDimensions( )
// Created using theory from PyKEP toolbox and LambertTargeterIzzo class
{
    // Compute normalizing values.
    const double distanceNormalizingValue = cartesianPositionAtDeparture.norm( );
    velocityNormalizingValue = std::sqrt( gravitationalParameter /
                                          distanceNormalizingValue );
    const double timeNormalizingValue = distanceNormalizingValue / velocityNormalizingValue;

    // Compute transfer geometry parameters in adimensional units.
    // Time of Flight.
    normalizedTimeOfFlight = timeOfFlight / timeNormalizingValue;

    // Cosine of transfer angle.
    const double cosineOfTransferAngle =
            cartesianPositionAtDeparture.dot( cartesianPositionAtArrival )
            / (distanceNormalizingValue * cartesianPositionAtArrival.norm( ) );

    // Normalized Cartesian position at arrival.
    normalizedRadiusAtArrival = cartesianPositionAtArrival.norm( ) / distanceNormalizingValue;

    // Chord.
    normalizedChord = std::sqrt( 1.0 + normalizedRadiusAtArrival
                                 * ( normalizedRadiusAtArrival
                                     - 2.0 * cosineOfTransferAngle ) );

    // Semi-perimeter.
    normalizedSemiPerimeter = ( 1.0 + normalizedRadiusAtArrival + normalizedChord ) / 2.0;

    // Assuming a prograde motion, determine whether the transfer corresponds to the long- or the
    // short-way solution: longway if x1*y2 - x2*y1 < 0.
    isLongway = false;
    if ( cartesianPositionAtDeparture.x( ) * cartesianPositionAtArrival.y( )
         - cartesianPositionAtDeparture.y( ) * cartesianPositionAtArrival.x( ) < 0.0 )
    {
        isLongway = true;
    }

    // If retrograde is true, switch longway flag.
    if ( isRetrograde )
    {
        isLongway = !isLongway;
    }

    // Semi-major axis of the minimum energy ellipse.
    normalizedMinimumEnergySemiMajorAxis = normalizedSemiPerimeter / 2.0;

    // Transfer angle.
    transferAngle = std::acos( cosineOfTransferAngle );
    if ( isLongway )
    {
        transferAngle = 2.0 * mathematical_constants::PI - transferAngle;
    }

    // Lambda parameter.
    lambdaParameter = std::sqrt( normalizedRadiusAtArrival )
            * std::cos( transferAngle / 2.0 ) / normalizedSemiPerimeter;

    // Set transformed flag to true, as the dimension transformation has been performed
    transformed = true;
}

//! Compute time-of-flight using Lagrange's equation.
double ZeroRevolutionLambertTargeterIzzo::computeTimeOfFlight( const double xParameter )
// Created using theory from PyKEP toolbox and LambertTargeterIzzo class
{
    // Determine semi-major axis.
    const double semiMajorAxis = normalizedMinimumEnergySemiMajorAxis
            / ( 1.0 - xParameter * xParameter );

    // If x < 1, the solution is an ellipse.
    if ( xParameter < 1.0 )
    {
        // Alpha parameter in Lagrange's equation (no explanation available).
        const double alphaParameter = 2.0 * std::acos( xParameter );

        // Beta parameter in Lagrange's equation (no explanation available).
        double betaParameter;
        // If long transfer arc
        if ( isLongway )
        {
            betaParameter = -2.0 * std::asin(
                        std::sqrt( ( normalizedSemiPerimeter - normalizedChord )
                                   / ( 2.0 * semiMajorAxis ) ) );
        }
        // Otherwise short transfer arc
        else
        {
            betaParameter = 2.0 * std::asin(
                        std::sqrt( ( normalizedSemiPerimeter - normalizedChord )
                                   / ( 2.0 * semiMajorAxis ) ) );
        }

        // Time-of-flight according to Lagrange including multiple revolutions.
        const double timeOfFlight = semiMajorAxis * std::sqrt( semiMajorAxis ) *
                ( ( alphaParameter - std::sin( alphaParameter ) )
                  - ( betaParameter - std::sin( betaParameter ) ) );

        return timeOfFlight;
    }
    // Otherwise it is a hyperbola.
    else
    {
        // Alpha parameter in Lagrange's equation (no explanation available).
        const double alphaParameter = 2.0 * boost::math::acosh( xParameter );

        // Beta parameter in Lagrange's equation (no explanation available).
        double betaParameter;
        // If long transfer arc
        if ( isLongway )
        {
            betaParameter = -2.0 * boost::math::asinh ( std::sqrt( ( normalizedSemiPerimeter
                                                                     - normalizedChord )
                                                                   / ( -2.0 * semiMajorAxis ) ) );
        }
        // Otherwise short transfer arc
        else
        {
            betaParameter = 2.0 * boost::math::asinh ( std::sqrt( ( normalizedSemiPerimeter
                                                                    - normalizedChord )
                                                                  / ( -2.0 * semiMajorAxis ) ) );
        }

        // Time-of-flight according to Lagrange.
        const double timeOfFlightLagrange = -semiMajorAxis * std::sqrt( -semiMajorAxis ) *
                ( ( std::sinh( alphaParameter ) - alphaParameter )
                  - ( std::sinh( betaParameter ) - betaParameter ) );

        return timeOfFlightLagrange;
    }
}

//! Solve the time of flight equation for x.
double ZeroRevolutionLambertTargeterIzzo::computeRootTimeOfFlight( )
// Created using theory from PyKEP toolbox and LambertTargeterIzzo class
{
    // Find root (secant method, currently hard coded)
    // Optimize log(t_spec).
    const double logarithmOfTheSpecifiedTimeOfFlight = std::log( normalizedTimeOfFlight );

    // Define initial guesses for abcissae (x) and ordinates (y).
    double x1 = std::log( 0.5 ), x2 = std::log( 1.5 );

    double y1 = std::log( computeTimeOfFlight( -0.5 ) ) - logarithmOfTheSpecifiedTimeOfFlight;

    double y2 = std::log( computeTimeOfFlight( 0.5 ) ) - logarithmOfTheSpecifiedTimeOfFlight;

    // Declare and initialize root-finding parameters.
    double rootFindingError = 1.0, xNew = 0.0, yNew = 0.0;
    int iterator = 0;

    // Root-finding loop.
    while ( ( rootFindingError > convergenceTolerance ) && (y1 != y2)
            && ( iterator < maximumNumberOfIterations ) )
    {
        // Update iterator.
        iterator++;

        // Compute new x-value.
        xNew = ( x1 * y2 - y1 * x2 ) / ( y2 - y1 );

        // Compute corresponding y-value.
        yNew = std::log( computeTimeOfFlight( std::exp( xNew ) - 1.0 ) )
                - logarithmOfTheSpecifiedTimeOfFlight;

        // Update abcissae and ordinates.
        x1 = x2;
        y1 = y2;
        x2 = xNew;
        y2 = yNew;

        // Compute root-finding error.
        rootFindingError = std::fabs( x1 - xNew );
    }

    // Verify that root-finder has converged.
    if ( iterator == maximumNumberOfIterations )
    {
        throw std::runtime_error(
                    "Multi-Revolution Lambert targeter failed to converge to a solution. Reached the maximum number of iterations: " +
                    std::to_string( maximumNumberOfIterations ) );
    }

    // Recovering x parameter and returning it.
    double xParameter = std::exp( xNew ) - 1.0;
    return xParameter;
}

//! Compute velocities at departure and arrival.
void ZeroRevolutionLambertTargeterIzzo::computeVelocities( const double xParameter )
// Created using theory from PyKEP toolbox and LambertTargeterIzzo class
{
    // Then it is possible to retrieve a sensible value from the x-parameter computed)
    // Determine semi-major axis of the conic.
    const double semiMajorAxis = normalizedMinimumEnergySemiMajorAxis
            / ( 1.0 - xParameter * xParameter );

    // Declare variables.
    double etaParameter, etaParameterSquared, psiParameter;

    // If x < 1, the solution is an ellipse.
    if ( xParameter < 1.0 )
    {
        // Alpha parameter in Lagrange's equation (no explanation available).
        const double alphaParameter = 2.0 * std::acos( xParameter );

        // Beta parameter in Lagrange's equation (no explanation available).
        double betaParameter = 2.0 * std::asin( std::sqrt( ( normalizedSemiPerimeter
                                                             - normalizedChord )
                                                           / ( 2.0 * semiMajorAxis ) ) );

        if ( isLongway )
        {
            betaParameter = -betaParameter;
        }

        // Psi parameter in Izzo's approach (no explanation available).
        psiParameter = ( alphaParameter - betaParameter ) / 2.0;

        // Eta parameter in Izzo's approach (no explanation available).
        etaParameterSquared = 2.0 * semiMajorAxis * std::sin( psiParameter )
                * std::sin( psiParameter ) / normalizedSemiPerimeter;
        etaParameter = std::sqrt( etaParameterSquared );
    }

    // Otherwise it is a hyperbola.
    else
    {
        // Alpha parameter in Lagrange's equation (no explanation available).
        const double alphaParameter = 2.0 * boost::math::acosh( xParameter );

        // Beta parameter in Lagrange's equation (no explanation available).
        double betaParameter = 2.0 * boost::math::asinh (
                    std::sqrt( ( normalizedSemiPerimeter - normalizedChord )
                               / ( -2.0 * semiMajorAxis ) ) );

        if ( isLongway )
        {
            betaParameter = -betaParameter;
        }

        // Psi parameter in Izzo's approach (no explanation available).
        psiParameter = (alphaParameter - betaParameter ) / 2.0;

        // Eta parameter in Izzo's approach (no explanation available).
        etaParameterSquared = -2.0 * semiMajorAxis * std::sinh( psiParameter )
                * std::sinh( psiParameter ) / normalizedSemiPerimeter;
        etaParameter = std::sqrt( etaParameterSquared );
    }

    // Determine semi-latus rectum, p.
    const double semiLatusRectum = ( normalizedRadiusAtArrival
                                     / ( normalizedMinimumEnergySemiMajorAxis
                                         * etaParameterSquared ) )
            * std::sin( transferAngle / 2.0 )
            * std::sin( transferAngle / 2.0 );

    // Velocity components at departure.
    const double radialVelocityAtDeparture =
            ( 1.0 / ( etaParameter * std::sqrt( normalizedMinimumEnergySemiMajorAxis ) ) )
            * ( 2.0 * lambdaParameter * normalizedMinimumEnergySemiMajorAxis
                - ( lambdaParameter + xParameter * etaParameter ) );

    const double transverseVelocityAtDeparture = std::sqrt( semiLatusRectum );

    // Velocity components at arrival.
    const double transverseVelocityAtArrival = transverseVelocityAtDeparture
            / normalizedRadiusAtArrival;
    const double radialVelocityAtArrival = ( transverseVelocityAtDeparture
                                             - transverseVelocityAtArrival )
            / std::tan( transferAngle / 2.0 )
            - radialVelocityAtDeparture;

    // Determining inertial vectors
    // Determine radial unit vectors.
    const Eigen::Vector3d radialUnitVectorAtDeparture = cartesianPositionAtDeparture.normalized( );
    const Eigen::Vector3d radialUnitVectorAtArrival = cartesianPositionAtArrival.normalized( );

    // Determine plane of motion.
    Eigen::Vector3d angularMomentumVector;

    if ( isLongway )
    {
        angularMomentumVector = radialUnitVectorAtArrival.cross( radialUnitVectorAtDeparture );
    }
    else
    {
        angularMomentumVector = radialUnitVectorAtDeparture.cross( radialUnitVectorAtArrival );
    }

    // Compute normalized angular momentum vector.
    const Eigen::Vector3d angularMomentumUnitVector = angularMomentumVector.normalized( );

    // Compute transverse unit vectors.
    const Eigen::Vector3d transverseUnitVectorAtDeparture =
            radialUnitVectorAtDeparture.cross( angularMomentumUnitVector );
    const Eigen::Vector3d transverseUnitVectorAtArrival =
            radialUnitVectorAtArrival.cross( angularMomentumUnitVector );

    // Reconstruct non-dimensional velocity vectors.
    cartesianVelocityAtDeparture << radialVelocityAtDeparture * radialUnitVectorAtDeparture
                                    - transverseVelocityAtDeparture * transverseUnitVectorAtDeparture;

    cartesianVelocityAtArrival << radialVelocityAtArrival * radialUnitVectorAtArrival
                                  - transverseVelocityAtArrival * transverseUnitVectorAtArrival;

    // Return to dimensions of initial problem definition.
    cartesianVelocityAtDeparture *= velocityNormalizingValue;
    cartesianVelocityAtArrival *= velocityNormalizingValue;
}

} // namespace mission_segments
} // namespace tudat


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_EXTENSIONS_GIS_GEOGRAPHIC_STRATEGIES_VINCENTY_HPP
#define BOOST_GEOMETRY_EXTENSIONS_GIS_GEOGRAPHIC_STRATEGIES_VINCENTY_HPP

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

#include <boost/geometry/strategies/distance.hpp>
#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>
#include <boost/geometry/util/promote_floating_point.hpp>
#include <boost/geometry/util/math.hpp>

#include <boost/geometry/extensions/gis/geographic/detail/ellipsoid.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace distance
{

/*!
\brief Distance calculation formulae on latlong coordinates, after Vincenty, 1975
\ingroup distance
\tparam Point1 \tparam_first_point
\tparam Point2 \tparam_second_point
\tparam CalculationType \tparam_calculation
\author See http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
\author Adapted from various implementations to get it close to the original document
    - http://www.movable-type.co.uk/scripts/LatLongVincenty.html
    - http://exogen.case.edu/projects/geopy/source/geopy.distance.html
    - http://futureboy.homeip.net/fsp/colorize.fsp?fileName=navigation.frink

*/
template
<
    typename Point1,
    typename Point2 = Point1,
    typename CalculationType = void
>
class vincenty
{
public :
    typedef typename promote_floating_point
        <
            typename select_most_precise
                <
                    typename select_calculation_type
                        <
                            Point1,
                            Point2,
                            CalculationType
                        >::type,
                    double // to avoid bad results in float
                >::type
        >::type calculation_type;

    inline vincenty()
    {}

    explicit inline vincenty(geometry::detail::ellipsoid<calculation_type> const& e)
        : m_ellipsoid(e)
    {}

    inline calculation_type apply(Point1 const& p1, Point2 const& p2) const
    {
        return calculate(get_as_radian<0>(p1), get_as_radian<1>(p1),
                        get_as_radian<0>(p2), get_as_radian<1>(p2));
    }

    inline geometry::detail::ellipsoid<calculation_type> ellipsoid() const
    {
        return m_ellipsoid;
    }

private :
    geometry::detail::ellipsoid<calculation_type> m_ellipsoid;

    inline calculation_type calculate(calculation_type const& lon1,
                calculation_type const& lat1,
                calculation_type const& lon2,
                calculation_type const& lat2) const
    {
        calculation_type const c2 = 2;
        calculation_type const pi = geometry::math::pi<calculation_type>();
        calculation_type const two_pi = c2 * pi;

        // lambda: difference in longitude on an auxiliary sphere
        calculation_type L = lon2 - lon1;
        calculation_type lambda = L;

        if (L < -pi) L += two_pi;
        if (L > pi) L -= two_pi;

        if (math::equals(lat1, lat2) && math::equals(lon1, lon2))
        {
            return calculation_type(0);
        }

        // U: reduced latitude, defined by tan U = (1-f) tan phi
        calculation_type const c1 = 1;
        calculation_type const one_min_f = c1 - m_ellipsoid.f();

        calculation_type const U1 = atan(one_min_f * tan(lat1)); // above (1)
        calculation_type const U2 = atan(one_min_f * tan(lat2)); // above (1)

        calculation_type const cos_U1 = cos(U1);
        calculation_type const cos_U2 = cos(U2);
        calculation_type const sin_U1 = sin(U1);
        calculation_type const sin_U2 = sin(U2);

        // alpha: azimuth of the geodesic at the equator
        calculation_type cos2_alpha;
        calculation_type sin_alpha;

        // sigma: angular distance p1,p2 on the sphere
        // sigma1: angular distance on the sphere from the equator to p1
        // sigma_m: angular distance on the sphere from the equator to the midpoint of the line
        calculation_type sigma;
        calculation_type sin_sigma;
        calculation_type cos2_sigma_m;

        calculation_type previous_lambda;

        calculation_type const c3 = 3;
        calculation_type const c4 = 4;
        calculation_type const c6 = 6;
        calculation_type const c16 = 16;

        calculation_type const c_e_12 = 1e-12;

        do
        {
            previous_lambda = lambda; // (13)
            calculation_type sin_lambda = sin(lambda);
            calculation_type cos_lambda = cos(lambda);
            sin_sigma = sqrt(math::sqr(cos_U2 * sin_lambda) + math::sqr(cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda)); // (14)
            calculation_type cos_sigma = sin_U1 * sin_U2 + cos_U1 * cos_U2 * cos_lambda; // (15)
            sin_alpha = cos_U1 * cos_U2 * sin_lambda / sin_sigma; // (17)
            cos2_alpha = c1 - math::sqr(sin_alpha);
            cos2_sigma_m = math::equals(cos2_alpha, 0) ? 0 : cos_sigma - c2 * sin_U1 * sin_U2 / cos2_alpha; // (18)

            calculation_type C = m_ellipsoid.f()/c16 * cos2_alpha * (c4 + m_ellipsoid.f() * (c4 - c3 * cos2_alpha)); // (10)
            sigma = atan2(sin_sigma, cos_sigma); // (16)
            lambda = L + (c1 - C) * m_ellipsoid.f() * sin_alpha *
                (sigma + C * sin_sigma * ( cos2_sigma_m + C * cos_sigma * (-c1 + c2 * math::sqr(cos2_sigma_m)))); // (11)

        } while (geometry::math::abs(previous_lambda - lambda) > c_e_12
                && geometry::math::abs(lambda) < pi);

        calculation_type sqr_u = cos2_alpha * (math::sqr(m_ellipsoid.a()) - math::sqr(m_ellipsoid.b())) / math::sqr(m_ellipsoid.b()); // above (1)

        // Oops getting hard here
        // (again, problem is that ttmath cannot divide by doubles, which is OK)
        calculation_type const c47 = 47;
        calculation_type const c74 = 74;
        calculation_type const c128 = 128;
        calculation_type const c256 = 256;
        calculation_type const c175 = 175;
        calculation_type const c320 = 320;
        calculation_type const c768 = 768;
        calculation_type const c1024 = 1024;
        calculation_type const c4096 = 4096;
        calculation_type const c16384 = 16384;

        calculation_type A = c1 + sqr_u/c16384 * (c4096 + sqr_u * (-c768 + sqr_u * (c320 - c175 * sqr_u))); // (3)
        calculation_type B = sqr_u/c1024 * (c256 + sqr_u * ( -c128 + sqr_u * (c74 - c47 * sqr_u))); // (4)
        calculation_type delta_sigma = B * sin_sigma * ( cos2_sigma_m + (B/c4) * (cos(sigma)* (-c1 + c2 * cos2_sigma_m)
                - (B/c6) * cos2_sigma_m * (-c3 + c4 * math::sqr(sin_sigma)) * (-c3 + c4 * cos2_sigma_m))); // (6)

        return m_ellipsoid.b() * A * (sigma - delta_sigma); // (19)
    }
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <typename Point1, typename Point2>
struct tag<strategy::distance::vincenty<Point1, Point2> >
{
    typedef strategy_tag_distance_point_point type;
};

template <typename Point1, typename Point2>
struct return_type<strategy::distance::vincenty<Point1, Point2> >
{
    typedef typename strategy::distance::vincenty<Point1, Point2>::calculation_type type;
};

template <typename Point1, typename Point2, typename P1, typename P2>
struct similar_type<vincenty<Point1, Point2>, P1, P2>
{
    typedef vincenty<P1, P2> type;
};

template <typename Point1, typename Point2, typename P1, typename P2>
struct get_similar<vincenty<Point1, Point2>, P1, P2>
{
    static inline vincenty<P1, P2> apply(vincenty<Point1, Point2> const& input)
    {
        return vincenty<P1, P2>(input.ellipsoid());
    }
};

template <typename Point1, typename Point2>
struct comparable_type<vincenty<Point1, Point2> >
{
    typedef vincenty<Point1, Point2> type;
};

template <typename Point1, typename Point2>
struct get_comparable<vincenty<Point1, Point2> >
{
    static inline vincenty<Point1, Point2> apply(vincenty<Point1, Point2> const& input)
    {
        return input;
    }
};

template <typename Point1, typename Point2>
struct result_from_distance<vincenty<Point1, Point2> >
{
    template <typename T>
    static inline typename return_type<vincenty<Point1, Point2> >::type apply(vincenty<Point1, Point2> const& , T const& value)
    {
        return value;
    }
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

// We might add a vincenty-like strategy also for point-segment distance, but to calculate the projected point is not trivial

}} // namespace strategy::distance

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_GIS_GEOGRAPHIC_STRATEGIES_VINCENTY_HPP


//==================================================================================================
/*!
  @file

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

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

#ifndef BOOST_SIMD_NO_INFINITIES
#include <boost/simd/constant/inf.hpp>
#endif
#include <boost/simd/arch/common/detail/generic/sinhc_kernel.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/log_2.hpp>
#include <boost/simd/constant/maxlog.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/function/scalar/abs.hpp>
#include <boost/simd/function/scalar/average.hpp>
#include <boost/simd/function/scalar/exp.hpp>
#include <boost/simd/function/scalar/if_else.hpp>
#include <boost/simd/function/scalar/rec.hpp>
#include <boost/simd/function/scalar/sqr.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( sinhc_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {

    BOOST_FORCEINLINE A0 operator() (A0 a0) const BOOST_NOEXCEPT
    {
      //////////////////////////////////////////////////////////////////////////////
      // if x = abs(a0) is less than 1 sinhc is computed using a polynomial(float)
      // respectively rational(double) approx inspired from cephes sinh approx.
      // else according x < Threshold e =  exp(x) or exp(x/2) is respectively
      // computed
      // * in the first case sinh is ((e-rec(e))/2)/x
      // * in the second     sinh is (e/2/x)*e (avoiding undue overflow)
      // Threshold is Maxlog - Log_2 defined in Maxshlog
      //////////////////////////////////////////////////////////////////////////////
      A0 x = bs::abs(a0);
      #ifndef BOOST_SIMD_NO_INFINITIES
      if (x == Inf<A0>()) return x;
      #endif
      if( x < One<A0>())
      {
        return detail::sinhc_kernel<A0>::compute(sqr(x));
      }
      else
      {
        auto test1 = (x >  Maxlog<A0>()-Log_2<A0>());
        A0 fac = if_else(test1, Half<A0>(), One<A0>());
        A0 tmp = exp(x*fac);
        A0 tmp1 = (Half<A0>()*tmp)/x;
        return if_else(test1, tmp1*tmp, average(tmp, -rec(tmp))/x);
      }
     }
  };
} } }

#endif


//
// Created by Hamza El-Kebir on 5/8/21.
//

#ifndef LODESTAR_ORDINARYDIFFERENTIALEQUATION_HPP
#define LODESTAR_ORDINARYDIFFERENTIALEQUATION_HPP

#ifdef LS_USE_GINAC

#include <Eigen/Dense>
#include "ginac/ginac.h"

#include "Lodestar/systems/StateSpace.hpp"

#include <string>
#include <deque>

namespace ls {
    namespace symbolic {
        class OrdinaryDifferentialEquation {
        public:
            OrdinaryDifferentialEquation() :
                    functions_(GiNaC::lst{GiNaC::ex(0)}),
                    states_(GiNaC::lst{GiNaC::symbol("x")}),
                    inputs_(GiNaC::lst{GiNaC::symbol("u")}),
                    time_(GiNaC::symbol("t"))
            {
                makeSymbolMap();
            }

            OrdinaryDifferentialEquation(const GiNaC::lst &functions,
                                         const GiNaC::lst &states,
                                         const GiNaC::lst &inputs) :
                    functions_(functions),
                    states_(states),
                    inputs_(inputs),
                    time_(GiNaC::symbol("t"))
            {
                makeSymbolMap();
            }

            OrdinaryDifferentialEquation(const GiNaC::lst &functions,
                                         const GiNaC::lst &states,
                                         const GiNaC::lst &inputs,
                                         const GiNaC::symbol &time) :
                    functions_(functions),
                    states_(states),
                    inputs_(inputs),
                    time_(time)
            {
                makeSymbolMap();
            }

            GiNaC::exmap generateExpressionMap() const;

            GiNaC::exmap generateExpressionMap(
                    const std::vector<GiNaC::relational> &relationals) const;

            GiNaC::exmap
            generateExpressionMap(const std::vector<double> &states,
                                  const std::vector<double> &inputs) const;

            GiNaC::exmap generateExpressionMap(double t,
                                               const std::vector<double> &states,
                                               const std::vector<double> &inputs) const;

            GiNaC::symbol getSymbol(const std::string &symbolName) const;

            GiNaC::symbol getStateSymbol(unsigned int i) const;

            GiNaC::symbol getInputSymbol(unsigned int i) const;

            GiNaC::symbol getTimeSymbol() const;

            const GiNaC::lst &getFunctions() const;

            void setFunctions(const GiNaC::lst &functions);

            const GiNaC::lst &getStates() const;

            void setStates(const GiNaC::lst &states);

            const GiNaC::lst &getInputs() const;

            void setInputs(const GiNaC::lst &inputs);

            Eigen::MatrixXd evalf(const GiNaC::exmap &m) const;

            Eigen::MatrixXd evalf(const std::vector<double> &states,
                                  const std::vector<double> &inputs) const;

            Eigen::MatrixXd
            evalf(double t, const std::vector<double> &states,
                  const std::vector<double> &inputs) const;

            GiNaC::matrix generateJacobian(const GiNaC::lst &variables) const;

            GiNaC::matrix generateJacobianStates() const;

            std::string generateJacobianStatesCppFunc(const std::string &functionName, const bool dynamicType = false) const;

            std::string generateJacobianStatesArrayInputCppFunc(const std::string &functionName, const bool dynamicType = false) const;

            GiNaC::matrix generateJacobianInputs() const;

            std::string generateJacobianInputsCppFunc(const std::string &functionName, const bool dynamicType = false) const;

            std::string generateJacobianInputsArrayInputCppFunc(const std::string &functionName, const bool dynamicType = false) const;

            Eigen::MatrixXd generateJacobianMatrix(const GiNaC::lst &variables,
                                                   const GiNaC::exmap &exmap) const;

            Eigen::MatrixXd
            generateJacobianMatrix(const GiNaC::matrix &jacobian,
                                   const GiNaC::exmap &exmap) const;

            Eigen::MatrixXd
            generateJacobianMatrixStates(const GiNaC::exmap &exmap) const;

            Eigen::MatrixXd
            generateJacobianMatrixInputs(const GiNaC::exmap &exmap) const;

            std::string generateMatrixCppFunc(const GiNaC::matrix &ginacMatrix, const std::string &functionName, const bool dynamicType = false) const;

            std::string generateMatrixArrayInputCppFunc(const GiNaC::matrix &ginacMatrix, const std::string &functionName, const bool dynamicType = false) const;

            systems::StateSpace<> linearize(const GiNaC::exmap &exmap) const;

            systems::StateSpace<> linearize(const std::vector<double> &states,
                                          const std::vector<double> &inputs) const;

            systems::StateSpace<> linearize(double t,
                                          const std::vector<double> &states,
                                          const std::vector<double> &inputs) const;

            systems::StateSpace<> linearize(const GiNaC::matrix &jacobianStates,
                                          const GiNaC::matrix &jacobianInputs,
                                          const GiNaC::exmap &exmap) const;

            systems::StateSpace<> linearize(const GiNaC::matrix &jacobianStates,
                                          const GiNaC::matrix &jacobianInputs,
                                          const std::vector<double> &states,
                                          const std::vector<double> &inputs) const;

            systems::StateSpace<> linearize(const GiNaC::matrix &jacobianStates,
                                          const GiNaC::matrix &jacobianInputs,
                                          double t,
                                          const std::vector<double> &states,
                                          const std::vector<double> &inputs) const;

            static Eigen::MatrixXd matrixToMatrixXd(const GiNaC::matrix &mat);

            static Eigen::MatrixXd matrixToMatrixXd(const GiNaC::ex &ex);

        protected:
            GiNaC::lst functions_;
            GiNaC::symbol time_;
            GiNaC::lst states_;
            GiNaC::lst inputs_;

            std::map<std::string, GiNaC::symbol> symbolMap_;

            void makeSymbolMap();

            static void replaceString(std::string &str, const std::string &source, const std::string &dest);
            static void replaceStringAll(std::string &str, const std::string &source, const std::string &dest);
            static std::string stripWhiteSpace(std::string &str);
        };
    }
}

#endif

#endif //LODESTAR_ORDINARYDIFFERENTIALEQUATION_HPP


/**
 * \copyright
 * Copyright (c) 2012-2017, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 *
 */

#include "MohrCoulomb.h"

#include "BaseLib/Error.h"
#include "MathLib/MathTools.h"

#ifndef Q_MOC_RUN // to avoid Qt4 bug, https://bugreports.qt.io/browse/QTBUG-22829
#include <boost/math/special_functions/sign.hpp>
#endif

namespace MaterialLib
{
namespace Fracture
{

namespace
{

struct MaterialPropertyValues
{
    double Kn = 0.0;
    double Ks = 0.0;
    double phi = 0.0; // friction angle
    double psi = 0.0; // dilation angle
    double c = 0.0;

    template <typename MaterialProperties>
    MaterialPropertyValues(
            MaterialProperties const& mp,
            double const t,
            ProcessLib::SpatialPosition const& x)
    {
        Kn = mp.normal_stiffness(t,x)[0];
        Ks = mp.shear_stiffness(t,x)[0];
        phi = MathLib::to_radians(mp.friction_angle(t,x)[0]);
        psi = MathLib::to_radians(mp.dilatancy_angle(t,x)[0]);
        c = mp.cohesion(t,x)[0];
    }
};

} // no namespace

template <int DisplacementDim>
void MohrCoulomb<DisplacementDim>::computeConstitutiveRelation(
        double const t,
        ProcessLib::SpatialPosition const& x,
        Eigen::Ref<Eigen::VectorXd const> w_prev,
        Eigen::Ref<Eigen::VectorXd const> w,
        Eigen::Ref<Eigen::VectorXd const> sigma_prev,
        Eigen::Ref<Eigen::VectorXd> sigma,
        Eigen::Ref<Eigen::MatrixXd> Kep,
        typename FractureModelBase<DisplacementDim>::MaterialStateVariables&
        material_state_variables)
{
    if (DisplacementDim == 3)
    {
        OGS_FATAL("MohrCoulomb fracture model does not support 3D case.");
        return;
    }
    material_state_variables.reset();

    MaterialPropertyValues const mat(_mp, t, x);
    Eigen::VectorXd const dw = w - w_prev;

    Eigen::MatrixXd Ke(2,2);
    Ke.setZero();
    Ke(0,0) = mat.Ks;
    Ke(1,1) = mat.Kn;

    sigma.noalias() = sigma_prev + Ke * dw;

    // if opening
    if (sigma[1] > 0)
    {
        Kep.setZero();
        sigma.setZero();
        material_state_variables.setTensileStress(true);
        return;
    }

    // check shear yield function (Fs)
    double const Fs = std::abs(sigma[0]) + sigma[1] * std::tan(mat.phi) - mat.c;
    material_state_variables.setShearYieldFunctionValue(Fs);
    if (Fs < .0)
    {
        Kep = Ke;
        return;
    }

    Eigen::VectorXd dFs_dS(2);
    dFs_dS[0] = boost::math::sign(sigma[0]);
    dFs_dS[1] = std::tan(mat.phi);

    // plastic potential function: Qs = |tau| + Sn * tan da
    Eigen::VectorXd dQs_dS(2);
    dQs_dS[0] = boost::math::sign(sigma[0]);
    dQs_dS[1] = std::tan(mat.psi);

    // plastic multiplier
    Eigen::RowVectorXd const A = dFs_dS.transpose() * Ke / (dFs_dS.transpose() * Ke * dQs_dS);
    double const d_eta = A * dw;

    // plastic part of the dispalcement
    Eigen::VectorXd const dwp = dQs_dS * d_eta;

    // correct stress
    sigma.noalias() = sigma_prev + Ke * (dw - dwp);

    // Kep
    Kep = Ke - Ke * dQs_dS * A;
}

template class MohrCoulomb<2>;
template class MohrCoulomb<3>;

}   // namespace Fracture
}  // namespace MaterialLib


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

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

 /*!

    @ingroup group-trigonometric
    Function object implementing rem_pio2_medium capabilities

    Computes the remainder modulo \f$\pi/2\f$ with medium algorithm,
    and the angle quadrant between 0 and 3.

    @par Semantic:

    For every parameter of floating type T

    @code
    T r, rc;
    as_integer<T> n;
    rem_pio2_cephes(x, n, r);
    @endcode

    is similar to:

    @code
    as_integer<T> n = idivround2even(x, Pio_2<T>());
    T r =  remainder(x, Pio_2<T>());
    @endcode

    @par Note:

    @c rem_pio2_medium compute the remainder modulo \f$\pi/2\f$ with medium algorithm,
    and the angle quadrant between 0 and 3.
    This is a medium_ version version accurate if the input is in:
     \f$[-2^6\pi,2^6\pi]\f$ for float,
     \f$[-2^{18}\pi,2^{18}\pi]\f$ for double.
    \par
    The reduction of the argument modulo \f$\pi/2\f$ is generally
    the most difficult part of trigonometric evaluations.
    The accurate algorithm is over costly and implies the knowledge
    of a few hundred \f$pi\f$ decimals
    some simpler algorithms as this one
    can be used, but the precision is only insured on smaller intervals.

    @see rem_pio2, rem_pio2_straight,rem_2pi,  rem_pio2_cephes,

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

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

#endif


//
//  multivector.hpp
//
//  Created by r. on 09/05/14
//

#ifndef round1_multivector_hpp
#define round1_multivector_hpp

// Standard libraries
#include <algorithm>    // std::random_shuffle
#include <numeric> // std::iota
#include <iostream>
#include <vector>
#include <queue>
#include <string>
#include <sstream>
#include <list>
#include <set>
#include <limits>

// Boost MPI
#include <boost/mpi.hpp>
#include <boost/serialization/vector.hpp>

// Boost ublas
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/vector_sparse.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/io.hpp>

//
#include "../include/stopwatch.hpp"

namespace mypara
{

	namespace ublas = boost::numeric::ublas;
    namespace mpi = boost::mpi;

	class multivector : public ublas::matrix<double, ublas::column_major>
	{
	public:
		typedef ublas::matrix<double, ublas::column_major> parent_type;
		typedef parent_type::value_type value_type;
		typedef parent_type::size_type size_type;

		typedef ublas::matrix_column<parent_type> matrix_column;
		typedef ublas::matrix_column<const parent_type> const_matrix_column;

		typedef ublas::matrix<value_type, ublas::column_major> dense_matrix;
	private:
		// MPI
		mpi::communicator world;
	private:
		// Total number of verticals to distribute (horizontal size)
		unsigned int total;
		// For each process, begin and end of its portion
		std::vector<unsigned int> p2gja;
		std::vector<unsigned int> p2gjb;
	public:
		const mpi::communicator& getcm() const { return world; }
	public:
        // Width of the whole multivector
		unsigned int getot() const { return total; }
        // Rank in the communicator
		unsigned int getrk() const { return world.rank(); }
        // Begin of portion (global)
		unsigned int getja() const { return p2gja[getrk()]; }
        // End of portion (global)
		unsigned int getjb() const { return p2gjb[getrk()]; }
        // Size of portion
		unsigned int getsz() const { return (getjb() - getja()); }
	public:
		unsigned int gj2pr(unsigned int j) const
        // Finds the owner process of global column j
		{
			for (auto p = 0; p < world.size(); ++p)
				if ((p2gja[p] <= j) && (j < p2gjb[p]))
					return p;
			return world.size();
		}

		bool ismyj(unsigned int j) const { return ((getja() <= j) && (j < getjb())); }
	public:
		multivector(const mpi::communicator& comm)
		: world(comm), total(0), parent_type(0,0)
		{
		}

		multivector(size_type size1, unsigned int total, const mpi::communicator& comm)
		: world(comm), total(total), parent_type(0,0)
		// Pre: the operation is requested by all processes in the communicator comm
		{
			stopwatch::Time time(stopwatch::watches, "multivector::multivector");

//			world.barrier();

			//cout << "ID: " << world.rank() << ": new multivector" << endl;

			unsigned int wsize = world.size();

			// Compute everybody's portion
			{
				unsigned int share = total; // Number of vectors still to be distributed
				unsigned int pleft = wsize; // Number of processes still unemployed
				while (pleft)
				{
					p2gjb.push_back(share);
					share -= (share / pleft);
					p2gja.push_back(share);
					pleft--;
				}
				assert(share == 0);
				assert(pleft == 0);
			}

			assert(p2gja.size() == wsize);
			assert(p2gjb.size() == wsize);

			parent_type& mymat = *this;
			mymat.resize(size1, getsz());
			mymat.clear();
		}
	public:
		multivector& operator=(const multivector&) = default;
	public:
		matrix_column
		colio_local(unsigned int j)
		// Provides access to global column #j
		// Pre: the column is owned by the local process
		{
			assert(ismyj(j));
			unsigned int loclj = (j - getja());
			parent_type& x = *this;
			ublas::matrix_column< parent_type > mc(x, loclj);
			return mc;
		}

		void
		setcolumn(unsigned int j, const ublas::vector<value_type>& colmn)
		// Replaces global column #j
		// Pre: the column is owned by the local process
		{
			matrix_column mc = this->colio_local(j);
			mc = colmn;
		}

        ublas::vector<value_type>
        getcolumn(unsigned int j) const
		// Gets a copy of global column #j
		// Pre: the column is owned by the local process
        {
			assert(ismyj(j));
			unsigned int loclj = (j - getja());
			const parent_type& x = *this;
            ublas::vector<value_type> v = column(x, loclj);
			return v;
        }

		dense_matrix
		getwhole() const
        // Combines all portions to one dense matrix available locally
		// Pre: requested by all processes in the communicator of u
		{
			getcm().barrier();

			std::vector<parent_type> parts;
			mpi::all_gather(getcm(), (parent_type&)(*this), parts);
			dense_matrix whole(size1(), getot());
			for (auto p = 0; p < parts.size(); ++p)
			{
				ublas::range vrang(0, parts[p].size1());
				ublas::range hrang(p2gja[p], p2gjb[p]);
				ublas::matrix_range<dense_matrix> subma(whole, vrang, hrang);
				subma = parts[p];
			}
			return whole;
		}
	public:
		bool
		shrink_communicator()
		// If the local has no capacity, it is removed from the communicator
		// Returns true iff the local has any capacity
		{
			multivector& me = *this;

			unsigned int isfat = ((me.size2() == 0) ? 0 : 1);
			mpi::communicator small = me.getcm().split(isfat);

			embrace_communicator(small);

			return (isfat != 0);
		}

		void
		embrace_communicator(const mpi::communicator& newcm)
		{
			multivector& me = *this;

			unsigned int newja = me.getja();
			unsigned int newjb = me.getjb();

			assert(me.size2() == (newjb - newja));

			me.world = newcm;

			me.p2gja.resize(0);
			me.p2gjb.resize(0);
			mpi::all_gather(me.world, newja, me.p2gja);
			mpi::all_gather(me.world, newjb, me.p2gjb);
		}

	private:
		void
		keepj(unsigned int ja, unsigned int jb)
		{
            stopwatch::Time time(stopwatch::watches, "mypara::multivector::keepj()");

			multivector& me = *this;
            assert((0 <= ja) && (ja <= jb) && (jb <= me.getot()));

            unsigned int np = me.getcm().size();
            assert((np == me.p2gja.size()) && (np == me.p2gjb.size()));

            for (unsigned int p = 0; p != np; ++p)
            {
                // Old range of process p
                unsigned int oldja = me.p2gja[p];
                unsigned int oldjb = me.p2gjb[p];

                // New range of process p
                unsigned int newja, newjb;
                {
                    newja = std::min(std::max(oldja, ja), oldjb);
                    newjb = std::max(std::min(oldjb, jb), oldja);
                    assert(newja <= newjb);
                    assert((oldja <= newja) && (newjb <= oldjb));
                }

                // If I am process p, then cut my data to the new range
                if (p == me.getcm().rank()) {
                    ublas::range hrang(newja - oldja, newjb - oldja);
                    ublas::range vrang(0, me.size1());
                    ((parent_type)me) = ublas::matrix_range<parent_type>(me, vrang, hrang);
                }

                // Compute p2gja and p2gjb of process p
                {
                    assert(newja <= newjb);
                    if (newja != newjb) {
                        assert((ja <= newja) && (newja <= newjb) && (newjb <= jb));
                        newja -= ja;
                        newjb -= ja;
                    } else {
                        newja = newjb = 0;
                    }

                    me.p2gja[p] = newja;
                    me.p2gjb[p] = newjb;
                }
            }

            me.total = (jb - ja);
		}

	public:
		void
		split(multivector& A, multivector& B, unsigned int j0) const
		// First j0 columns of *this go to a, the others to b
		// Pre: *this, a and b all have the same communicator
		{
            stopwatch::Time time(stopwatch::watches, "mypara::multivector::split()");

			const multivector& me = *this;
			assert(j0 <= me.getot());

			//
			B.p2gja = A.p2gja = me.p2gja;
			B.p2gjb = A.p2gjb = me.p2gjb;
            B.total = A.total = me.total;

            // Clean data of a and b, but make consistent
			A.resize(0, A.getjb() - A.getja());
			B.resize(0, B.getjb() - B.getja());

            //
			A.keepj(0, j0);
			B.keepj(j0, me.getot());

			// Copy data
			A.resize(me.size1(), A.getjb() - A.getja());
			B.resize(me.size1(), B.getjb() - B.getja());

			// This could be done more efficiently:

			for (unsigned int j = A.getja(); j != A.getjb(); ++j) {
				A.setcolumn(j, me.getcolumn(j));
			}

			for (unsigned int j = B.getja(); j != B.getjb(); ++j) {
				B.setcolumn(j, me.getcolumn(j0 + j));
			}
		}

		void
		merge(multivector& a, multivector& b)
		// Pre: a and b are the (possibly modified) result of split
		// Note: a, b, and this need not share the communicator
		{
			stopwatch::Time time(stopwatch::watches, "mypara::multivector::merge()");

			multivector& me = *this;
			assert(me.getot() == (a.getot() + b.getot()));
			assert((me.size1() == a.size1()) && (me.size1() == b.size1()));

			for (unsigned int j = a.getja(); j != a.getjb(); ++j)
			{
				me.setcolumn(j, a.getcolumn(j));
			}

			unsigned int j0 = a.getot();
			for (unsigned int j = b.getja(); j != b.getjb(); ++j)
			{
				me.setcolumn(j0 + j, b.getcolumn(j));
			}
		}

	};

	// Operators

	double
	inner_prod_max(const multivector& a, const multivector& b)
	// Pre: a and b share the mpi communicator
	// Pre: the operation is requested by all processes in the communicator of a / b
	{
		stopwatch::Time time(stopwatch::watches, "multivector::inner_prod");

		std::vector<double> local_vec;
		{
			assert(a.getja() == b.getja());
			assert(a.getjb() == b.getjb());

			for (unsigned int k = 0; k != a.getsz(); ++k)
				local_vec.push_back(ublas::inner_prod(ublas::column(a, k), ublas::column(b, k)));
		}

		// negative infinity
		double local = std::numeric_limits<double>::lowest();

		if (local_vec.size())
			local = *std::max_element(local_vec.begin(), local_vec.end());

		double global = 0;
		{
			mpi::all_reduce(a.getcm(), local, global, mpi::maximum<double>());
		}

		return global;
	}

	double
	inner_prod(const multivector& a, const multivector& b)
	// Pre: a and b share the mpi communicator
	// Pre: the operation is requested by all processes in the communicator of a / b
	{
        stopwatch::Time time(stopwatch::watches, "multivector::inner_prod");

		double local = 0;
		{
			assert(a.getja() == b.getja());
			assert(a.getjb() == b.getjb());

			for (unsigned int k = 0; k != a.getsz(); ++k)
			{
				local += ublas::inner_prod(ublas::column(a, k), ublas::column(b, k));
			}
		}

		double global = 0;
		{
			mpi::all_reduce(a.getcm(), local, global, std::plus<double>());
		}

		return global;
	}

    // RIGHT MULTIPLY

    void
    prod_ref(const multivector& u, const multivector::dense_matrix& m, multivector& v)
    // This is the reference implementation for the following:
    // Right matrix multiply u * m, assumes no structure in m
    // Compare with function prod(u, m, jplan, v)
    // Pre: the operation is requested by all processes in the communicator of u and v
    // Pre: u and v have the same communicator
    {
        stopwatch::Time time(stopwatch::watches, "multivector::prod(ref)");

        typedef multivector::value_type value_type;
        typedef multivector::const_matrix_column const_matrix_column;
        typedef ublas::vector<value_type> vector;

        for (unsigned int j = 0; j != v.getot(); ++j)
        {
            namespace ublas = boost::numeric::ublas;
            typedef ublas::vector<value_type> Vec;

            // From the j-th column of the right matrix m, get local subvector
            ublas::vector<value_type> submv(u.getsz());
            {
                ublas::range verrg(u.getja(), u.getjb());
                const_matrix_column colmj(m, j);
                submv = ublas::vector_range<const_matrix_column>(colmj, verrg);
            }

            // Am I receiving data and/or sending data?
            bool itodo = (v.ismyj(j) || (0 != ublas::norm_inf(submv)));

            // A. Who has something to contribute?
            // Note: cannot interchange order with B
            mpi::communicator activ = u.getcm().split(itodo ? 1 : 0);

            // B. Do I have something to contribute? If not, skip.
            // Note: cannot interchange order with A
            if (!itodo) continue;

            // Find the rank of the process in the activ communicator
            // that holds the j-th column of the result
            unsigned int trank;
            {
                unsigned int local = (v.ismyj(j) ? activ.rank() : 0);
                mpi::all_reduce(activ, local, trank, std::plus<unsigned int>());
            }

            // Local contribution to j-th column of the result
            Vec myvec(u.size1());
            myvec = ublas::prod(u, submv);

            // Am I to receive the product vector?
            if (activ.rank() == trank) {
                Vec resul(myvec.size());
                mpi::reduce(activ, myvec, resul, std::plus<Vec>(), trank);
                v.setcolumn(j, resul);
            } else {
                mpi::reduce(activ, myvec, std::plus<Vec>(), trank);
            }
        }

        v.getcm().barrier();
    }

	struct Jplan
	{
	public:
		enum Strategy { nnz = 1, jforward = 2, jreverse = 4 };
	private:
		Strategy strategy;
	public:
		typedef double value_type;
		typedef ublas::compressed_vector<value_type> sparse_vector;
		typedef ublas::compressed_matrix<value_type, ublas::column_major> sparse_matrix;
	public:
		std::vector<sparse_vector> M_local;
	public:
		typedef ublas::vector<unsigned int> GROUP;
		typedef ublas::vector<unsigned int> TARGT;
	public:
		std::vector<GROUP> group_plan;
		std::vector<TARGT> targt_plan;
	private:
		bool ready;
	public:
		bool is_ready() const { return ready; }
	public:
		Jplan() : ready(false), strategy(Strategy::nnz) { }

		Jplan& operator()(const Strategy& s)
		{
			strategy = s;
			return (*this);
		}

		void
		compute(const multivector& u, const sparse_matrix& m, const multivector& v)
		// Pre: u and v have the same communicator
		{
			stopwatch::Time time(stopwatch::watches, "Jplan::compute");

			assert(u.getot() == m.size1());
			assert(v.getot() == m.size2());

			// Horizontal size of the result multivector v
			unsigned int width = (unsigned int)(m.size2());

			// Clear
			{
				M_local.clear();
				group_plan.clear();
				targt_plan.clear();
			}

			std::vector<sparse_vector> M;

			// Step 1.
			// Convert the right multiplication matrix to a vector of vectors
			for (unsigned int j = 0; j != width; ++j)
			{
				sparse_vector v(ublas::column(m, j));
				M.push_back(v);

				// From the j-th column of the right matrix m, get local subvector
				sparse_vector submv(u.getsz());
				{
					ublas::range range(u.getja(), u.getjb());
					submv = ublas::vector_range<sparse_vector>(v, range);
				}
				assert(submv.size() == u.getsz());
				M_local.push_back(submv);
			}

			// Step 2.
			// Create sets of disjoint communication groups
			{
				std::list<unsigned int> jleft;
				{
					std::vector<unsigned int> order(M.size());

					// Default strategy: Strategy::jforward
					std::iota(order.begin(), order.end(), 0.);

					if (strategy == Strategy::nnz)
					{
						// Sort with decreasing nnz value
						struct R {
							typedef std::vector<sparse_vector> VOV;
							const VOV& M;
							R(const VOV& M) : M(M) { }
							bool operator()(unsigned int i, unsigned int j) const {
								return (M[i].nnz() > M[j].nnz());
							}
						};
						std::sort(order.begin(), order.end(), R(M));
					}

					if (strategy == Strategy::jreverse) {
						std::sort(order.rbegin(), order.rend());
					}

					for (unsigned int j = 0; j != width; ++j)
						jleft.push_back(order[j]);
				}

				// While there are target columns to process
				while (jleft.size())
				{
					GROUP group(u.getcm().size());
					// group[p] is the number of the communication group
					// where process p is involved
					// It is zero if the process has nothing to communicate

					TARGT targt(u.getcm().size());
					// targt[p] is the target column in the result
					// for the communication group masks[p]

					group.clear();
					targt.clear();

					unsigned int grpno = 0;
					auto pj = jleft.begin();
					while (pj != jleft.end())
					{
						// jmask[p] is one if process p is involved, and zero else
						ublas::vector<unsigned int> jmask(group.size());
						jmask.clear();

						unsigned int j = *pj;

						// conflict iff jmask has nontrivial overlap with group
						bool conflict = false;

						// Target index
						{
							unsigned int p = v.gj2pr(j);
							conflict = conflict || (group[p] != 0);
							jmask[p] = 1;
						}

						// Source indices
						{
							typedef ublas::compressed_vector<double> sparse_vector;
							const sparse_vector& vj = M[j];
							// (Does a non-const iterator work here?)
							for (sparse_vector::const_iterator i = vj.begin(); i != vj.end(); ++i)
							{
								unsigned int p = u.gj2pr((unsigned int)i.index());
								conflict = conflict || (group[p] != 0);
								jmask[p] = 1;
								if (conflict) break;
							}
						}

						if (conflict) {
							// Bad luck, try another column j
							pj++;
						} else {
							grpno++;
							group += (grpno * jmask);
							targt += (j * jmask);

							// The target column j has been scheduled for processing
							// Remove from queue and proceed to next column
							pj = jleft.erase(pj);
						}
					}

					group_plan.push_back(group);
					targt_plan.push_back(targt);
				}
			}

			this->ready = true;
		}
	}; // class Jplan

	void
	prod(const multivector& u, const Jplan::sparse_matrix& m, Jplan& jplan, multivector& v)
    // Right matrix multiply, u * m
	// Good for matrices m where each row is sparse
	// Pre: the operation is requested by all processes in the communicator of u and v
	// Pre: u and v have the same communicator
	// TODO: implement using MPI groups
	{
        stopwatch::Time time(stopwatch::watches, "multivector::prod(jplan)");

		typedef multivector::value_type value_type;
		//		typedef multivector::dense_matrix dense_matrix;
		typedef multivector::const_matrix_column const_matrix_column;
        typedef ublas::vector<value_type> vector;

		if (!jplan.is_ready()) jplan.compute(u, m, v);
		assert(jplan.is_ready());

		for (unsigned int k = 0; k != jplan.group_plan.size(); ++k)
		{
			auto& group = jplan.group_plan[k];
			auto& targt = jplan.targt_plan[k];

			//std::cout << "group: " << group << std::endl;
			//std::cout << "targt: " << targt << std::endl;

			// Partition the MPI communicator into disjoint groups

			unsigned int r = u.getcm().rank();
			unsigned int color = group[r];
			//std::cout << masks << std::endl;
			mpi::communicator activ = u.getcm().split(color);

			//std::cout << "Proc: " << r << "; color: " << color << "; targt: " << targt[r] << std::endl;

			// Am I in one of the communicating groups?
			if (!color) continue;

			// The truly parallel part:
			// Compute the partial matrix matrix product

			unsigned int j = targt[r];

			// From the j-th column of the right matrix m, get local subvector
			const Jplan::sparse_vector& submv = jplan.M_local[j];

			// Find the rank of the process in the activ communicator
			// that holds the j-th column of the result
			unsigned int jdest;
			{
				unsigned int local = (v.ismyj(j) ? activ.rank() : 0);
				mpi::all_reduce(activ, local, jdest, std::plus<unsigned int>());
			}

			//std::cout << "j: " << j << "; trank: " << trank << std::endl;

			// Local contribution to j-th column of the result
			typedef ublas::vector<value_type> Vec;
			Vec myvec(u.size1()); myvec.clear();
			myvec = ublas::prod(u, submv);

			// Am I to receive the product vector?
			if (activ.rank() == jdest) {
				Vec resul(myvec.size());
				mpi::reduce(activ, myvec, resul, std::plus<Vec>(), jdest);
				v.setcolumn(j, resul);
			} else {
				mpi::reduce(activ, myvec, std::plus<Vec>(), jdest);
			}
		}

        #ifndef NDEBUG
        /// Check correctness
        {
            multivector v_ref = v;
            mypara::prod_ref(u, m, v_ref);
            assert(v.size1() == v_ref.size1());
            assert(v.size2() == v_ref.size2());
            assert(ublas::norm_inf(v - v_ref) <= 1e-6);
        }//*/
        #endif
	}

	multivector
	prod(const multivector& u, const multivector::dense_matrix& m, Jplan& jplan)
    // Right matrix multiply. See void prod(u, m, jplan, v)
	{
		assert(u.getot() == m.size1());

		multivector v(u.size1(), (unsigned int)(m.size2()), u.getcm());
		mypara::prod(u, m, jplan, v);

		return v;
	}

	void
	prod(const multivector& u, const ublas::compressed_matrix<double, ublas::column_major>& m, multivector& v)
	// Asynchronous implementation of:
    // Right matrix multiply u * m
	// Compare with function prod(u, m, jplan, v)
    // Pre: m is sparse (send buffers created)
	// Pre: the operation is requested by all processes in the communicator of u and v
	// Pre: u and v have the same communicator
	{
        stopwatch::Time time(stopwatch::watches, "multivector::prod(asynch)");

        v.clear();

		// Type of m
		typedef ublas::compressed_matrix<double, ublas::column_major> matrix;
		// Column of m
		typedef ublas::matrix_column<const matrix> cmc;
		// Vector type of column of m
		typedef ublas::compressed_vector<matrix::value_type> sparse_vector;

		// Step 1.
		// Convert the right multiplication matrix to a vector of vectors
		std::vector<sparse_vector> M, M_local;
		{
			stopwatch::Time time(stopwatch::watches, "multivector::prod(asynch).Mlocal");

			for (unsigned int j = 0; j != m.size2(); ++j)
			{
				sparse_vector vcolj(ublas::column(m, j));

				// From the j-th column of the right matrix m, get local subvector
				sparse_vector submv(u.getsz());
				{
					ublas::range range(u.getja(), u.getjb());
					submv = ublas::vector_range<sparse_vector>(vcolj, range);
					assert(submv.size() == u.getsz());
				}

				M.push_back(vcolj);
				M_local.push_back(submv);
			}
		}

		// Vector for a column of u or v
		typedef ublas::vector<multivector::value_type> Vec;

        //
        struct Packet {
            int dest, src, tag;
            Vec myvec;
            //
            Packet(int dest, int tag, const Vec& myvec, int src) : dest(dest), tag(tag), myvec(myvec), src(src)
            { }
			//
			std::string str() const {
                std::stringstream s;
                s << "dest: " << dest << ", "
                  << "tag: " << tag << ", "
                  << "vecsize: " << myvec.size() << ", "
                  << "src: " << src;
                return s.str();
			}
        };

        // Queue in everything that has to be sent
        std::list<Packet> sendbuf;
        std::vector<mpi::request> sendreq;
		{
			stopwatch::Time time(stopwatch::watches, "multivector::prod(asynch).sendbuf");

			// Prepare for send: iterate over target columns
			for (unsigned int j = 0; j != v.getot(); ++j)
			{
				// Is the target local?
				if (v.ismyj(j)) continue;

				// Local subvector from the j-th column of the right matrix m
				const sparse_vector& submv = M_local[j];

				// Do I have data to send?
				if (0 == submv.nnz()) continue;

				// Local contribution to j-th column of the result
				Vec myvec = ublas::prod(u, submv);
				assert(myvec.size() == v.size1());

				// Destination rank
				const unsigned int trank = v.gj2pr(j);
				// Source rank
				const unsigned int srank = u.getcm().rank();

				assert(trank != srank);
				const int tag = j;
				// Put data into send buffer
				sendbuf.push_back(Packet(trank, tag, myvec, srank));

				// Submit MPI send request
				const Packet& packet = sendbuf.back();
				//std::cout << "Sending packet " << packet.str() << std::endl;
				sendreq.push_back(u.getcm().isend(packet.dest, packet.tag, packet.myvec));
			}
		}

        // Queue in everything that has to be received
        std::list<Packet> recvbuf;
        std::vector<mpi::request> recvreq;
		{
			stopwatch::Time time(stopwatch::watches, "multivector::prod(asynch).recvbuf");

			for (unsigned int j = v.getja(); j != v.getjb(); ++j)
			{
				// j-th column of m
				sparse_vector vcolj = M[j];

				// Identify senders
				std::set<unsigned int> ranks;
				for (sparse_vector::const_iterator i = vcolj.begin(); i != vcolj.end(); ++i)
				{
					// Is the source local?
					if (u.ismyj((unsigned int)i.index())) continue;

					unsigned int srank = u.gj2pr((unsigned int)i.index());
					ranks.insert(srank);
				}

				for (auto i = ranks.begin(); i != ranks.end(); ++i)
				{
					const unsigned int srank = (*i);
					const int tag = j;

					// Receive buffer
                    // The target vector will be automatically resized by boost::mpi
					recvbuf.push_back(Packet(v.getcm().rank(), tag, Vec(0), srank));

					// Submit MPI receive request
					Packet& packet = recvbuf.back();
					//std::cout << "Recving packet " << packet.str() << std::endl;
					recvreq.push_back(u.getcm().irecv(packet.src, packet.tag, packet.myvec));
				}
			}
		}

        // Do what is entirely local
		{
			stopwatch::Time time(stopwatch::watches, "multivector::prod(asynch).local");

			for (unsigned int j = v.getja(); j != v.getjb(); ++j)
			{
				// Local contribution to j-th column of the result
				v.setcolumn(j, ublas::prod(u, M_local[j]));
			}
		}

		// Receive all data
		{
			stopwatch::Time time(stopwatch::watches, "multivector::prod(asynch).recvall");

			mpi::wait_all(recvreq.begin(), recvreq.end());
			//std::cout << "#" << v.getcm().rank() << ": recieved" << std::endl;

			for (std::list<Packet>::const_iterator i = recvbuf.begin(); i != recvbuf.end(); ++i)
			{
				unsigned int j = i->tag;
				assert(v.ismyj(j));
				assert(i->myvec.size() == v.size1());
				v.colio_local(j) += i->myvec;
			}
		}

		// Wait for all sent data to be received (before destroying the send buffer)
		{
			stopwatch::Time time(stopwatch::watches, "multivector::prod(asynch).sendall");

			mpi::wait_all(sendreq.begin(), sendreq.end());
			//std::cout << "#" << v.getcm().rank() << ": sent" << std::endl;
		}
//		v.getcm().barrier();

        #ifndef NDEBUG
        /// Check correctness
        {
            multivector v_ref = v;
            mypara::prod_ref(u, m, v_ref);
            assert(v.size1() == v_ref.size1());
            assert(v.size2() == v_ref.size2());
            assert(ublas::norm_inf(v - v_ref) <= 1e-6);
			std::cout << "Prod check ok (use -DNDEBUG to disable)" << std::endl;
        }//*/
        #endif
	}

	multivector
    prod(const multivector& u, const ublas::compressed_matrix<double, ublas::column_major>& m)
    // Right matrix multiply. See void prod(u, m, v)
	{
		assert(u.getot() == m.size1());

		multivector v(u.size1(), (unsigned int)(m.size2()), u.getcm());
		mypara::prod(u, m, v);

		return v;
	}

    // LEFT MULTIPLY

	multivector
	prod(const ublas::compressed_matrix<double, ublas::row_major>& m, const multivector& u)
    // Left matrix multiply
	{
        stopwatch::Time time(stopwatch::watches, "multivector::prod(left)");

//		u.getcm().barrier();

		assert(m.size2() == u.size1());

		multivector v(m.size1(), u.getot(), u.getcm());
		{
			//ublas::compressed_matrix<multivector::value_type, ublas::column_major> m1(m);
			//ublas::matrix<multivector::value_type, ublas::row_major> u1(u);
			stopwatch::Time time(stopwatch::watches, "multivector::prod(left)/ublas::prod");
			((multivector::parent_type&)v) = boost::numeric::ublas::prod(m, u);
		}

		return v;
	}
}

#endif


//  (C) Copyright John Maddock 2006.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_TOOLS_SOLVE_HPP
#define BOOST_MATH_TOOLS_SOLVE_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/config.hpp>
#include <boost/math/tools/assert.hpp>

#ifdef _MSC_VER
#pragma warning(push)
#pragma warning(disable:4996 4267 4244)
#endif

#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>

#ifdef _MSC_VER
#pragma warning(pop)
#endif

namespace boost{ namespace math{ namespace tools{

//
// Find x such that Ax = b
//
// Caution: this uses undocumented, and untested ublas code,
// however short of writing our own LU-decomposition code
// it's the only game in town.
//
template <class T>
boost::numeric::ublas::vector<T> solve(
          const boost::numeric::ublas::matrix<T>& A_,
          const boost::numeric::ublas::vector<T>& b_)
{
   //BOOST_MATH_ASSERT(A_.size() == b_.size());

   boost::numeric::ublas::matrix<T> A(A_);
   boost::numeric::ublas::vector<T> b(b_);
   boost::numeric::ublas::permutation_matrix<> piv(b.size());
   lu_factorize(A, piv);
   lu_substitute(A, piv, b);
   //
   // iterate to reduce error:
   //
   boost::numeric::ublas::vector<T> delta(b.size());
   for(unsigned k = 0; k < 1; ++k)
   {
      noalias(delta) = prod(A_, b);
      delta -= b_;
      lu_substitute(A, piv, delta);
      b -= delta;

      T max_error = 0;

      for(unsigned i = 0; i < delta.size(); ++i)
      {
         T err = fabs(delta[i] / b[i]);
         if(err > max_error)
            max_error = err;
      }
      //std::cout << "Max change in LU error correction: " << max_error << std::endl;
   }

   return b;
}

}}} // namespaces

#endif // BOOST_MATH_TOOLS_SOLVE_HPP


//=======================================================================
// Copyright 2008
// Author: Matyas W Egyhazy
//
// 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_GRAPH_METRIC_TSP_APPROX_HPP
#define BOOST_GRAPH_METRIC_TSP_APPROX_HPP

// metric_tsp_approx
// Generates an approximate tour solution for the traveling salesperson
// problem in polynomial time. The current algorithm guarantees a tour with a
// length at most as long as 2x optimal solution. The graph should have
// 'natural' (metric) weights such that the triangle inequality is maintained.
// Graphs must be fully interconnected.

// TODO:
// There are a couple of improvements that could be made.
// 1) Change implementation to lower uppper bound Christofides heuristic
// 2) Implement a less restrictive TSP heuristic (one that does not rely on
//    triangle inequality).
// 3) Determine if the algorithm can be implemented without creating a new
//    graph.

#include <vector>

#include <boost/shared_ptr.hpp>
#include <boost/concept_check.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_as_tree.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>
#include <boost/graph/lookup_edge.hpp>
#include <boost/throw_exception.hpp>

namespace boost
{
    // Define a concept for the concept-checking library.
    template <typename Visitor, typename Graph>
    struct TSPVertexVisitorConcept
    {
    private:
        Visitor vis_;
    public:
        typedef typename graph_traits<Graph>::vertex_descriptor Vertex;

        BOOST_CONCEPT_USAGE(TSPVertexVisitorConcept)
        {
            Visitor vis(vis_);  // require copy construction
            Graph g(1);
            Vertex v(*vertices(g).first);
            vis_.visit_vertex(v, g); // require visit_vertex
        }
    };

    // Tree visitor that keeps track of a preorder traversal of a tree
    // TODO: Consider migrating this to the graph_as_tree header.
    // TODO: Parameterize the underlying stores o it doesn't have to be a vector.
    template<typename Node, typename Tree> class PreorderTraverser
    {
    private:
        std::vector<Node>& path_;
    public:
        typedef typename std::vector<Node>::const_iterator const_iterator;

        PreorderTraverser(std::vector<Node>& p) : path_(p) {}

        void preorder(Node n, const Tree&)
        { path_.push_back(n); }

        void inorder(Node, const Tree&) const {}
        void postorder(Node, const Tree&) const {}

        const_iterator begin() const { return path_.begin(); }
        const_iterator end() const { return path_.end(); }
    };

    // Forward declarations
    template <typename> class tsp_tour_visitor;
    template <typename, typename, typename, typename> class tsp_tour_len_visitor;

    template<typename VertexListGraph, typename OutputIterator>
    void metric_tsp_approx_tour(VertexListGraph& g, OutputIterator o)
    {
        metric_tsp_approx_from_vertex(g, *vertices(g).first,
            get(edge_weight, g), get(vertex_index, g),
            tsp_tour_visitor<OutputIterator>(o));
    }

    template<typename VertexListGraph, typename WeightMap, typename OutputIterator>
    void metric_tsp_approx_tour(VertexListGraph& g, WeightMap w, OutputIterator o)
    {
        metric_tsp_approx_from_vertex(g, *vertices(g).first,
            w, tsp_tour_visitor<OutputIterator>(o));
    }

    template<typename VertexListGraph, typename OutputIterator>
    void metric_tsp_approx_tour_from_vertex(VertexListGraph& g,
        typename graph_traits<VertexListGraph>::vertex_descriptor start,
        OutputIterator o)
    {
        metric_tsp_approx_from_vertex(g, start, get(edge_weight, g),
            get(vertex_index, g), tsp_tour_visitor<OutputIterator>(o));
    }

    template<typename VertexListGraph, typename WeightMap,
        typename OutputIterator>
    void metric_tsp_approx_tour_from_vertex(VertexListGraph& g,
    typename graph_traits<VertexListGraph>::vertex_descriptor start,
        WeightMap w, OutputIterator o)
    {
        metric_tsp_approx_from_vertex(g, start, w, get(vertex_index, g),
            tsp_tour_visitor<OutputIterator>(o));
    }

    template<typename VertexListGraph, typename TSPVertexVisitor>
    void metric_tsp_approx(VertexListGraph& g, TSPVertexVisitor vis)
    {
        metric_tsp_approx_from_vertex(g, *vertices(g).first,
            get(edge_weight, g), get(vertex_index, g), vis);
    }

    template<typename VertexListGraph, typename Weightmap,
        typename VertexIndexMap, typename TSPVertexVisitor>
    void metric_tsp_approx(VertexListGraph& g, Weightmap w,
        TSPVertexVisitor vis)
    {
        metric_tsp_approx_from_vertex(g, *vertices(g).first, w,
            get(vertex_index, g), vis);
    }

    template<typename VertexListGraph, typename WeightMap,
        typename VertexIndexMap, typename TSPVertexVisitor>
    void metric_tsp_approx(VertexListGraph& g, WeightMap w, VertexIndexMap id,
        TSPVertexVisitor vis)
    {
        metric_tsp_approx_from_vertex(g, *vertices(g).first, w, id, vis);
    }

    template<typename VertexListGraph, typename WeightMap,
        typename TSPVertexVisitor>
    void metric_tsp_approx_from_vertex(VertexListGraph& g,
    typename graph_traits<VertexListGraph>::vertex_descriptor start,
        WeightMap w, TSPVertexVisitor vis)
    {
        metric_tsp_approx_from_vertex(g, start, w, get(vertex_index, g), vis);
    }

    template <
        typename VertexListGraph,
        typename WeightMap,
        typename VertexIndexMap,
        typename TSPVertexVisitor>
    void metric_tsp_approx_from_vertex(const VertexListGraph& g,
                                       typename graph_traits<VertexListGraph>::vertex_descriptor start,
                                       WeightMap weightmap,
                                       VertexIndexMap indexmap,
                                       TSPVertexVisitor vis)
    {
        using namespace boost;
        using namespace std;

        BOOST_CONCEPT_ASSERT((VertexListGraphConcept<VertexListGraph>));
        BOOST_CONCEPT_ASSERT((TSPVertexVisitorConcept<TSPVertexVisitor, VertexListGraph>));

        // Types related to the input graph (GVertex is a template parameter).
        typedef typename graph_traits<VertexListGraph>::vertex_descriptor GVertex;
        typedef typename graph_traits<VertexListGraph>::vertex_iterator GVItr;

        // We build a custom graph in this algorithm.
        typedef adjacency_list <vecS, vecS, directedS, no_property, no_property > MSTImpl;
        typedef graph_traits<MSTImpl>::vertex_descriptor Vertex;
        typedef graph_traits<MSTImpl>::vertex_iterator VItr;

        // And then re-cast it as a tree.
        typedef iterator_property_map<vector<Vertex>::iterator, property_map<MSTImpl, vertex_index_t>::type> ParentMap;
        typedef graph_as_tree<MSTImpl, ParentMap> Tree;
        typedef tree_traits<Tree>::node_descriptor Node;

        // A predecessor map.
        typedef vector<GVertex> PredMap;
        typedef iterator_property_map<typename PredMap::iterator, VertexIndexMap> PredPMap;

        PredMap preds(num_vertices(g));
        PredPMap pred_pmap(preds.begin(), indexmap);

        // Compute a spanning tree over the in put g.
        prim_minimum_spanning_tree(g, pred_pmap,
             root_vertex(start)
            .vertex_index_map(indexmap)
            .weight_map(weightmap));

        // Build a MST using the predecessor map from prim mst
        MSTImpl mst(num_vertices(g));
        std::size_t cnt = 0;
        pair<VItr, VItr> mst_verts(vertices(mst));
        for(typename PredMap::iterator vi(preds.begin()); vi != preds.end(); ++vi, ++cnt)
        {
            if(indexmap[*vi] != cnt) {
                add_edge(*next(mst_verts.first, indexmap[*vi]),
                         *next(mst_verts.first, cnt), mst);
            }
        }

        // Build a tree abstraction over the MST.
        vector<Vertex> parent(num_vertices(mst));
        Tree t(mst, *vertices(mst).first,
            make_iterator_property_map(parent.begin(),
            get(vertex_index, mst)));

        // Create tour using a preorder traversal of the mst
        vector<Node> tour;
        PreorderTraverser<Node, Tree> tvis(tour);
        traverse_tree(indexmap[start], t, tvis);

        pair<GVItr, GVItr> g_verts(vertices(g));
        for(PreorderTraverser<Node, Tree>::const_iterator curr(tvis.begin());
            curr != tvis.end(); ++curr)
        {
            // TODO: This is will be O(n^2) if vertex storage of g != vecS.
            GVertex v = *next(g_verts.first, get(vertex_index, mst)[*curr]);
            vis.visit_vertex(v, g);
        }

        // Connect back to the start of the tour
        vis.visit_vertex(start, g);
    }

    // Default tsp tour visitor that puts the tour in an OutputIterator
    template <typename OutItr>
    class tsp_tour_visitor
    {
        OutItr itr_;
    public:
        tsp_tour_visitor(OutItr itr)
            : itr_(itr)
        { }

        template <typename Vertex, typename Graph>
        void visit_vertex(Vertex v, const Graph&)
        {
            BOOST_CONCEPT_ASSERT((OutputIterator<OutItr, Vertex>));
            *itr_++ = v;
        }

    };

    // Tsp tour visitor that adds the total tour length.
    template<typename Graph, typename WeightMap, typename OutIter, typename Length>
    class tsp_tour_len_visitor
    {
        typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
        BOOST_CONCEPT_ASSERT((OutputIterator<OutIter, Vertex>));

        OutIter iter_;
        Length& tourlen_;
        WeightMap& wmap_;
        Vertex previous_;

        // Helper function for getting the null vertex.
        Vertex null()
        { return graph_traits<Graph>::null_vertex(); }

    public:
        tsp_tour_len_visitor(Graph const&, OutIter iter, Length& l, WeightMap map)
            : iter_(iter), tourlen_(l), wmap_(map), previous_(null())
        { }

        void visit_vertex(Vertex v, const Graph& g)
        {
            typedef typename graph_traits<Graph>::edge_descriptor Edge;

            // If it is not the start, then there is a
            // previous vertex
            if(previous_ != null())
            {
                // NOTE: For non-adjacency matrix graphs g, this bit of code
                // will be linear in the degree of previous_ or v. A better
                // solution would be to visit edges of the graph, but that
                // would require revisiting the core algorithm.
                Edge e;
                bool found;
                boost::tie(e, found) = lookup_edge(previous_, v, g);
                if(!found) {
                    BOOST_THROW_EXCEPTION(not_complete());
                }

                tourlen_ += wmap_[e];
            }

            previous_ = v;
            *iter_++ = v;
        }
    };

    // Object generator(s)
    template <typename OutIter>
    inline tsp_tour_visitor<OutIter>
    make_tsp_tour_visitor(OutIter iter)
    { return tsp_tour_visitor<OutIter>(iter); }

    template <typename Graph, typename WeightMap, typename OutIter, typename Length>
    inline tsp_tour_len_visitor<Graph, WeightMap, OutIter, Length>
    make_tsp_tour_len_visitor(Graph const& g, OutIter iter, Length& l, WeightMap map)
    { return tsp_tour_len_visitor<Graph, WeightMap, OutIter, Length>(g, iter, l, map); }

} //boost

#endif // BOOST_GRAPH_METRIC_TSP_APPROX_HPP