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

#include <cmath>
#include <fstream>
#include <iostream>
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <boost/property_tree/ini_parser.hpp>
#include <boost/property_tree/ptree.hpp>
#include <string>
#include <vector>
#include <yaml-cpp/yaml.h>

#include "SurgSim/DataStructures/Vertices.h"
#include "SurgSim/Framework/Log.h"
#include "SurgSim/Math/CardinalSplines.h"
#include "SurgSim/Math/Vector.h"

/// Function to generate points on a circle with radius 'radius'.
/// \note The points always start from the origin and go on the circle to 'angle'.
/// \param subdivisions Number of interpolated points on the circle.
/// \param radius Radius of the circle on which points are generated.
/// \param angle How much (of the circle) should points occupy, in radian.
/// \return A list of points on the circle.
std::vector<SurgSim::Math::Vector3d> generateNeedle(size_t subdivisions, double radius, double angle)
{
	SURGSIM_ASSERT(0 < angle && angle < 2.0 * M_PI) << __FUNCTION__ << " 'angle' must be in the range (0, 2PI)";
	SURGSIM_ASSERT(2 <= subdivisions) << __FUNCTION__ << " 'subdivisions' must be at least 2";

	double increment = angle / static_cast<double>(subdivisions);
	double controlAngle = 0.0;

	std::vector<SurgSim::Math::Vector3d> vertices(subdivisions+1);
	for (size_t i = 0; i < subdivisions; ++i)
	{
		vertices[i] = radius * SurgSim::Math::Vector3d(std::cos(controlAngle) - 1.0, std::sin(controlAngle), 0.0);
		controlAngle += increment;
	}
	vertices[subdivisions] =
		radius * SurgSim::Math::Vector3d(std::cos(controlAngle) - 1.0, std::sin(controlAngle), 0.0);

	return vertices;
}

/// Function to generate a suture, starting from the origin and wraps around 'axis' in circles.
/// A circle (formed by the suture when wraps 'axis') is defined by four markers.
/// Each marker is rotated 45 degrees along 'axis' from previous one.
/// 'subdivisions' controls how many points to interpolate between each pair of markers.
/// In the returned list, the origin will always be the first point.
/// \param subdivisions Number of points between makers.
/// \param radius Radius of the circle the suture forms.
/// \param length Length of the suture.
/// \param circles Number of circles the suture forms wrapping around 'axis'.
/// \param axis The axis around which the suture wraps.
/// \return A list of points representing the suture.
std::vector<SurgSim::Math::Vector3d> generateSuture(size_t subdivisions, double radius, double length, size_t circles,
										const SurgSim::Math::Vector3d& axis = SurgSim::Math::Vector3d(1.0, 0.0, 0.0))
{
	// Increment on X-axis.
	double increment = length / circles / 4.0;

	SurgSim::Math::Vector3d direction = axis;
	direction.normalize();
	auto rotateXToAxis = SurgSim::Math::makeRotationQuaternion(
											 -std::acos(direction.dot(SurgSim::Math::Vector3d(1.0, 0.0, 0.0))),
													  direction.cross(SurgSim::Math::Vector3d(1.0, 0.0, 0.0)));

	std::vector<SurgSim::Math::Vector3d> controlPoints;
	controlPoints.push_back(SurgSim::Math::Vector3d(0.0, 0.0, 0.0) - increment * direction);
	controlPoints.push_back(SurgSim::Math::Vector3d(0.0, 0.0, 0.0));

	// Used to control the rotation between markers.
	SurgSim::Math::Quaterniond rotation =
								SurgSim::Math::makeRotationQuaternion(M_PI_2, SurgSim::Math::Vector3d(1.0, 0.0, 0.0));
	auto point = SurgSim::Math::Vector3d(increment, radius, 0.0);
	for (size_t i = 0; i < circles * 4; ++i)
	{
		controlPoints.push_back(rotateXToAxis * point);
		point = rotation * point;
		point.x() += increment;
	}

	std::vector<SurgSim::Math::Vector3d> result;
	SurgSim::Math::CardinalSplines::interpolate(subdivisions, controlPoints, &result);

	return result;
}

/// Save a list of needle and a list of suture as one ply file.
/// Points in 'needle' will be output first, as the order they appear in 'needle' and
/// points in 'suture' will be output second, as the order they appear in 'suture'.
/// The caller of this function needs to make sure
/// the needle and the suture connects (smoothly) at needle[last] and suture[begin].
/// \param fileName Name of the ply file.
/// \param needle List of points representing the needle.
/// \param suture List of points representing the suture.
/// \param asPhysics Boolean to decide whether or not to save physics properties.
/// \param needleMassDensity Mass density of needle.
/// \param needlepoissonRatio Poisson ratio of needle.
/// \param needleYoungModulus Young modulus of needle.
/// \param sutureMassDensity Mass density of suture.
/// \param suturepoissonRatio Poisson ratio of suture.
/// \param sutureYoungModulus Young modulus of suture.
/// \param radius Radius used for fem1d beam.
void saveNeedleSuturePly(const std::string& fileName,
						 const std::vector<SurgSim::Math::Vector3d>& needle,
						 const std::vector<SurgSim::Math::Vector3d>& suture,
						 bool asPhysics,
						 double needleMassDensity, double needlePoissonRatio, double needleYoungModulus,
						 double sutureMassDensity, double suturePoissonRatio, double sutureYoungModulus,
						 double radius = 0.0001)
{
	std::ofstream out(fileName);

	size_t numOfPoints = needle.size() + suture.size();
	if (out.is_open())
	{
		out << "ply" << std::endl;
		out << "format ascii 1.0" << std::endl;
		out << "comment Created by OpenSurgSim, www.opensurgsim.org" << std::endl;
		out << "element vertex " << numOfPoints << std::endl;
		out << "property float x\nproperty float y\nproperty float z" << std::endl;
		if (asPhysics)
		{
			out << "element 1d_element " << numOfPoints - 1 << std::endl;
			out << "property list uint uint vertex_indices" << std::endl;
			out << "property double mass_density" << std::endl;
			out << "property double poisson_ratio" << std::endl;
			out << "property double young_modulus" << std::endl;
		}
		out << "element radius 1" << std::endl;
		out << "property double value" << std::endl;
		out << "element boundary_condition 0" << std::endl;
		out << "property uint vertex_index" << std::endl;
		out << "end_header" << std::endl;
		for (const auto& vertex : needle)
		{
			out << vertex[0] << " " << vertex[1] << " " << vertex[2] << std::endl;
		}
		for (const auto& vertex : suture)
		{
			out << vertex[0] << " " << vertex[1] << " " << vertex[2] << std::endl;
		}

		if (asPhysics)
		{
			int index = 0;
			for (const auto& vertex : needle)
			{
				out << "2 " << index << " " << index + 1 << " " <<
					needleMassDensity << " " << needlePoissonRatio << " " << needleYoungModulus << std::endl;
				++index;
			}
			for (const auto& vertex : suture)
			{
				out << "2 " << index << " " << index + 1 << " " <<
					sutureMassDensity << " " << suturePoissonRatio << " " << sutureYoungModulus << std::endl;
				++index;
			}
		}

		out << radius << std::endl;

		if (out.bad())
		{
			SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getDefaultLogger()) << __FUNCTION__
				<< "There was a problem writing " << fileName;
		}

		out.close();
	}
	else
	{
		SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getDefaultLogger()) << __FUNCTION__
			<< "Could not open " << fileName << " for writing.";
	}
};

// Utility to generate a ply file for needle suture.
// Command line input are supported.
int main(int argc, char* argv[])
{
	namespace po = boost::program_options;

	po::options_description commandLine("Allowed options");
	commandLine.add_options()("help", "produce help message")
	("filename", po::value<std::string>()->default_value("needlesuture.ply"),
									"File name to save the generated needle suture.")
	("needleSubdivisions", po::value<int>()->default_value(10),
									"Number of interpolated points for the needle (default 10)")
	("needleRadius", po::value<double>()->default_value(0.019), "Radius of the needle (in m) (default 0.019)")
	("needleAngle", po::value<double>()->default_value(135),
									"How big is the arc (common values: 90, 135, 180, 225) in degrees (default 135)")
	("sutureSubdivisions", po::value<int>()->default_value(20),
									"Number of interpolated points for each pair of markers on the suture (default 20)")
	("sutureRadius", po::value<double>()->default_value(0.01), "Radius of the suture (default 0.01")
	("sutureLength", po::value<double>()->default_value(0.45), "Length of th suture (in m) (default 0.45)")
	("sutureCircles", po::value<int>()->default_value(2), "Number of circles formed by suture going around the axis")
	("savePhysicsProperty", po::value<bool>()->default_value(true),
									"Output physics properties for the needle and suture in ply file")
	("physicsProperty", po::value<std::string>(),
									"File name for physics properties. Note that command line values have priority: "
									"program will use value(s) from command line if specified.")
	("needleMassDensity", po::value<double>(), "Mass density for the needle (default 7500 Kg.m-3)")
	("needlePoissonRatio", po::value<double>(), "Poisson ratio for the needle (default 0.305)")
	("needleYoungModulus", po::value<double>(), "Young Modulus for the needle (default (1.8e11 Pa)")
	("sutureMassDensity", po::value<double>(), "Mass density for the suture (default 900 Kg.m-3)")
	("suturePoissonRatio", po::value<double>(), "Poisson ratio for the suture (default 0.45)")
	("sutureYoungModulus", po::value<double>(), "Young Modulus for the suture (default 1.75e9 Pa)");

	po::variables_map variables;
	try
	{
		po::store(po::parse_command_line(argc, argv, commandLine), variables);
	}
	catch (po::error& e)
	{
		std::cerr << "ERROR: " << e.what() << std::endl << std::endl;
		std::cerr << commandLine << std::endl;
		return 1;
	}

	if (variables.count("help"))
	{
		std::cout << commandLine << "\n";
		return 1;
	}

	double needleMassDensity = 7500, needlePoissonRatio = 0.305, needleYoungModulus = 180 * 1e9;
	double sutureMassDensity = 900,  suturePoissonRatio = 0.45,  sutureYoungModulus = 1.75 * 1e9;
	if (variables.count("physicsProperty"))
	{
		boost::filesystem::path fileName("Data/" + variables["physicsProperty"].as<std::string>());
		boost::filesystem::path filePath = boost::filesystem::complete(fileName);
		std::cout << "File path: " << filePath << std::endl;
		if (!boost::filesystem::exists(filePath))
		{
			std::cout << "Can't find my file!" << std::endl;
			return -1;
		}

		boost::property_tree::ptree parameters;
		boost::property_tree::ini_parser::read_ini(filePath.string(), parameters);
		needleMassDensity  = parameters.get<double>("PhysicsProperty.needleMassDensity");
		needlePoissonRatio = parameters.get<double>("PhysicsProperty.needlePoissonRatio");
		needleYoungModulus = parameters.get<double>("PhysicsProperty.needleYoungModulus");
		sutureMassDensity  = parameters.get<double>("PhysicsProperty.sutureMassDensity");
		suturePoissonRatio = parameters.get<double>("PhysicsProperty.suturePoissonRatio");
		sutureYoungModulus = parameters.get<double>("PhysicsProperty.sutureYoungModulus");
	}

	// Command line values for physics properties will take precedence if specified.
	if (variables.count("needleMassDensity"))  needleMassDensity  = variables["needleMassDensity"].as<double>();
	if (variables.count("needlePoissonRatio")) needlePoissonRatio = variables["needlePoissonRatio"].as<double>();
	if (variables.count("needleYoungModulus")) needleYoungModulus = variables["needleYoungModulus"].as<double>();
	if (variables.count("sutureMassDensity"))  sutureMassDensity  = variables["sutureMassDensity"].as<double>();
	if (variables.count("suturePoissonRatio")) suturePoissonRatio = variables["suturePoissonRatio"].as<double>();
	if (variables.count("sutureYoungModulus")) sutureYoungModulus = variables["sutureYoungModulus"].as<double>();

	auto needlePoints = generateNeedle(variables["needleSubdivisions"].as<int>(),
									   variables["needleRadius"].as<double>(),
									   variables["needleAngle"].as<double>() / 180 * M_PI);

	auto suturePoints = generateSuture(variables["sutureSubdivisions"].as<int>(),
									   variables["sutureRadius"].as<double>(),
									   variables["sutureLength"].as<double>(),
									   variables["sutureCircles"].as<int>(),
									   needlePoints[0] - needlePoints[1]);

	// Points generated are starting from the origin, when constructing the needle suture,
	// the list of needle points need to be reversed and the origin is removed to avoid duplication.
	std::vector<SurgSim::Math::Vector3d> needlePointsReversed(needlePoints.rbegin(), needlePoints.rend()-1);

	saveNeedleSuturePly(variables["filename"].as<std::string>(), needlePointsReversed, suturePoints,
						variables["savePhysicsProperty"].as<bool>(),
						needleMassDensity, needlePoissonRatio, needleYoungModulus,
						sutureMassDensity, suturePoissonRatio, sutureYoungModulus);
	return 0;
}


// smooth: Lie Theory for Robotics
// https://github.com/pettni/smooth
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#ifndef SMOOTH__LIE_GROUP_HPP_
#define SMOOTH__LIE_GROUP_HPP_

#include <Eigen/Core>

#include <concepts>

#include "manifold.hpp"

/**
 * @file lie_group.hpp Internal and external LieGroup interfaces and free LieGroup functions.
 */

namespace smooth {

namespace traits {

/**
 * @brief Trait class for making a class a LieGroup instance via specialization.
 */
template<typename T>
struct lie;

}  // namespace traits

// clang-format off

/**
 * @brief Class-external Lie group interface defined through the traits::lie trait class.
 */
template<typename G>
concept LieGroup =
requires (Eigen::Index dof) {
  // Underlying scalar type
  typename traits::lie<G>::Scalar;
  // Default representation
  typename traits::lie<G>::PlainObject;
  // Compile-time degrees of freedom (tangent space dimension). Can be dynamic (equal to -1)
  {traits::lie<G>::Dof}->std::convertible_to<Eigen::Index>;
  // Commutativity
  {traits::lie<G>::IsCommutative}->std::convertible_to<bool>;
  // Return the identity element (dof = Dof for static size)
  {traits::lie<G>::Identity(dof)}->std::convertible_to<typename traits::lie<G>::PlainObject>;
  // Return a random element (dof = Dof for static size)
  {traits::lie<G>::Random(dof)}->std::convertible_to<typename traits::lie<G>::PlainObject>;
} &&
// GROUP INTERFACE
requires(const G & g1, const G & g2, typename traits::lie<G>::Scalar eps) {
  // Group adjoint
  {traits::lie<G>::Ad(g1)}->std::convertible_to<Eigen::Matrix<typename traits::lie<G>::Scalar, traits::lie<G>::Dof, traits::lie<G>::Dof>>;
  // Group composition
  {traits::lie<G>::composition(g1, g2)}->std::convertible_to<typename traits::lie<G>::PlainObject>;
  // Run-time degrees of freedom (tangent space dimension).
  {traits::lie<G>::dof(g1)}->std::convertible_to<Eigen::Index>;
  // Group inverse
  {traits::lie<G>::inverse(g1)}->std::convertible_to<typename traits::lie<G>::PlainObject>;
  // Check if two elements are (approximately) equal
  {traits::lie<G>::isApprox(g1, g2, eps)}->std::convertible_to<bool>;
  // Group logarithm (maps from group to algebra)
  {traits::lie<G>::log(g1)}->std::convertible_to<Eigen::Vector<typename traits::lie<G>::Scalar, traits::lie<G>::Dof>>;
} &&
// TANGENT INTERFACE
requires(const Eigen::Vector<typename traits::lie<G>::Scalar, traits::lie<G>::Dof> & a) {
  // Algebra adjoint
  {traits::lie<G>::ad(a)}->std::convertible_to<Eigen::Matrix<typename traits::lie<G>::Scalar, traits::lie<G>::Dof, traits::lie<G>::Dof>>;
  // Algebra exponential (maps from algebra to group)
  {traits::lie<G>::exp(a)}->std::convertible_to<typename traits::lie<G>::PlainObject>;
  // Right derivative of the exponential map
  {traits::lie<G>::dr_exp(a)}->std::convertible_to<Eigen::Matrix<typename traits::lie<G>::Scalar, traits::lie<G>::Dof, traits::lie<G>::Dof>>;
  // Right derivative of the exponential map inverse
  {traits::lie<G>::dr_expinv(a)}->std::convertible_to<Eigen::Matrix<typename traits::lie<G>::Scalar, traits::lie<G>::Dof, traits::lie<G>::Dof>>;
  // Second right derivative of the exponential map
  {traits::lie<G>::d2r_exp(a)}->std::convertible_to<Eigen::Matrix<typename traits::lie<G>::Scalar, traits::lie<G>::Dof, (traits::lie<G>::Dof > 0 ? traits::lie<G>::Dof * traits::lie<G>::Dof : -1)>>;
  // Second right derivative of the exponential map inverse
  {traits::lie<G>::d2r_expinv(a)}->std::convertible_to<Eigen::Matrix<typename traits::lie<G>::Scalar, traits::lie<G>::Dof, (traits::lie<G>::Dof > 0 ? traits::lie<G>::Dof * traits::lie<G>::Dof : -1)>>;
} && (
  // Cast to different scalar type
  !std::is_convertible_v<typename traits::lie<G>::Scalar, double> ||
  requires (const G & g) {
    {traits::lie<G>::template cast<double>(g)};
  }
) && (
  !std::is_convertible_v<typename traits::lie<G>::Scalar, float> ||
  requires (const G & g) {
    {traits::lie<G>::template cast<double>(g)};
  }
) &&
// PlainObject must be default-constructible
std::is_default_constructible_v<typename traits::lie<G>::PlainObject> &&
std::is_copy_constructible_v<typename traits::lie<G>::PlainObject> &&
// PlainObject must be assignable from G
std::is_assignable_v<typename traits::lie<G>::PlainObject &, G>;

////////////////////////////////////////////////
//// Lie group interface for NativeLieGroup ////
////////////////////////////////////////////////

/**
 * @brief Concept defining class with an internal Lie group interface.
 *
 * Concept satisfied if G has members that correspond to the LieGroup concept.
 */
template<typename G>
concept NativeLieGroup = requires
{
  typename G::Scalar;
  typename G::Tangent;
  typename G::PlainObject;
  {G::Dof}->std::convertible_to<Eigen::Index>;
  {G::IsCommutative}->std::convertible_to<bool>;
} &&
(!(G::Dof > 0) || requires {
  {G::Identity()}->std::convertible_to<typename G::PlainObject>;
  {G::Random()}->std::convertible_to<typename G::PlainObject>;
}) &&
(!(G::Dof == -1) || requires (Eigen::Index dof) {
  {G::Identity(dof)}->std::convertible_to<typename G::PlainObject>;
  {G::Random(dof)}->std::convertible_to<typename G::PlainObject>;
}) &&
(G::Tangent::SizeAtCompileTime == G::Dof) &&
requires(const G & g1, const G & g2, typename G::Scalar eps) {
  {g1.dof()}->std::convertible_to<Eigen::Index>;  // degrees of freedom at runtime
  {g1.Ad()}->std::convertible_to<Eigen::Matrix<typename G::Scalar, G::Dof, G::Dof>>;
  {g1 * g2}->std::convertible_to<typename G::PlainObject>;
  {g1.inverse()}->std::convertible_to<typename G::PlainObject>;
  {g1.isApprox(g2, eps)}->std::convertible_to<bool>;
  {g1.log()}->std::convertible_to<Eigen::Vector<typename G::Scalar, G::Dof>>;
} &&
requires(const Eigen::Vector<typename G::Scalar, G::Dof> & a) {
  {G::ad(a)}->std::convertible_to<Eigen::Matrix<typename G::Scalar, G::Dof, G::Dof>>;
  {G::exp(a)}->std::convertible_to<typename G::PlainObject>;
  {G::dr_exp(a)}->std::convertible_to<Eigen::Matrix<typename G::Scalar, G::Dof, G::Dof>>;
  {G::dr_expinv(a)}->std::convertible_to<Eigen::Matrix<typename G::Scalar, G::Dof, G::Dof>>;
  {G::d2r_exp(a)}->std::convertible_to<Eigen::Matrix<typename G::Scalar, G::Dof, G::Dof * G::Dof>>;
  {G::d2r_expinv(a)}->std::convertible_to<Eigen::Matrix<typename G::Scalar, G::Dof, G::Dof * G::Dof>>;
};

// clang-format on

namespace traits
{

/**
 * @brief LieGroup interface for NativeLieGroup
 */
template<NativeLieGroup G>
struct lie<G>
{
  // \cond
  using Scalar = typename G::Scalar;
  template<typename NewScalar>
  using CastT       = typename G::template CastT<NewScalar>;
  using PlainObject = typename G::PlainObject;

  static constexpr Eigen::Index Dof   = G::Dof;
  static constexpr bool IsCommutative = G::IsCommutative;

  // group interface

  static inline PlainObject Identity([[maybe_unused]] Eigen::Index dof)
  {
    if constexpr (G::Dof == -1) {
      return G::Identity(dof);
    } else {
      return G::Identity();
    }
  }
  static inline PlainObject Random([[maybe_unused]] Eigen::Index dof)
  {
    if constexpr (G::Dof == -1) {
      return G::Random(dof);
    } else {
      return G::Random();
    }
  }
  static inline typename G::TangentMap Ad(const G & g) { return g.Ad(); }
  template<NativeLieGroup Go>
  static inline PlainObject composition(const G & g1, const Go & g2)
  {
    return g1.operator*(g2);
  }
  static inline Eigen::Index dof(const G &) { return G::Dof; }
  static inline PlainObject inverse(const G & g) { return g.inverse(); }
  template<NativeLieGroup Go>
  static inline bool isApprox(const G & g, const Go & go, Scalar eps)
  {
    return g.isApprox(go, eps);
  }
  static inline typename G::Tangent log(const G & g) { return g.log(); }
  template<typename NewScalar>
  static inline CastT<NewScalar> cast(const G & g)
  {
    return g.template cast<NewScalar>();
  }

  // tangent interface

  template<typename Derived>
  static inline typename G::TangentMap ad(const Eigen::MatrixBase<Derived> & a)
  {
    return G::ad(a);
  }
  template<typename Derived>
  static inline PlainObject exp(const Eigen::MatrixBase<Derived> & a)
  {
    return G::exp(a);
  }
  template<typename Derived>
  static inline typename G::TangentMap dr_exp(const Eigen::MatrixBase<Derived> & a)
  {
    return G::dr_exp(a);
  }
  template<typename Derived>
  static inline typename G::TangentMap dr_expinv(const Eigen::MatrixBase<Derived> & a)
  {
    return G::dr_expinv(a);
  }
  template<typename Derived>
  static inline typename G::Hessian d2r_exp(const Eigen::MatrixBase<Derived> & a)
  {
    return G::d2r_exp(a);
  }
  template<typename Derived>
  static inline typename G::Hessian d2r_expinv(const Eigen::MatrixBase<Derived> & a)
  {
    return G::d2r_expinv(a);
  }
  // \endcond
};

///////////////////////////////////////////////
//// Lie group interface for Eigen vectors ////
///////////////////////////////////////////////

/**
 * @brief LieGroup interface for RnType
 */
template<RnType G>
struct lie<G>
{
  // \cond
  static constexpr int Dof            = G::SizeAtCompileTime;
  static constexpr bool IsCommutative = true;

  using Scalar      = typename G::Scalar;
  using PlainObject = Eigen::Vector<Scalar, Dof>;
  template<typename NewScalar>
  using CastT = Eigen::Vector<NewScalar, Dof>;

  // group interface

  static inline PlainObject Identity(Eigen::Index dof) { return G::Zero(dof); }
  static inline PlainObject Random(Eigen::Index dof) { return G::Random(dof); }
  static inline Eigen::Matrix<Scalar, Dof, Dof> Ad(const G & g)
  {
    return Eigen::Matrix<Scalar, Dof, Dof>::Identity(g.size(), g.size());
  }
  template<typename Derived>
  static inline PlainObject composition(const G & g1, const Eigen::MatrixBase<Derived> & g2)
  {
    return g1 + g2;
  }
  static inline Eigen::Index dof(const G & g) { return g.size(); }
  static inline PlainObject inverse(const G & g) { return -g; }
  template<typename Derived>
  static inline bool isApprox(const G & g, const Eigen::MatrixBase<Derived> & g2, Scalar eps)
  {
    return g.isApprox(g2, eps);
  }
  static inline Eigen::Vector<Scalar, Dof> log(const G & g) { return g; }
  template<typename NewScalar>
  static inline Eigen::Vector<NewScalar, Dof> cast(const G & g)
  {
    return g.template cast<NewScalar>();
  }

  // tangent interface

  template<typename Derived>
  static inline Eigen::Matrix<Scalar, Dof, Dof> ad(const Eigen::MatrixBase<Derived> & a)
  {
    return Eigen::Matrix<Scalar, Dof, Dof>::Zero(a.size(), a.size());
  }
  template<typename Derived>
  static inline PlainObject exp(const Eigen::MatrixBase<Derived> & a)
  {
    return a;
  }
  template<typename Derived>
  static inline Eigen::Matrix<Scalar, Dof, Dof> dr_exp(const Eigen::MatrixBase<Derived> & a)
  {
    return Eigen::Matrix<Scalar, Dof, Dof>::Identity(a.size(), a.size());
  }
  template<typename Derived>
  static inline Eigen::Matrix<Scalar, Dof, Dof> dr_expinv(const Eigen::MatrixBase<Derived> & a)
  {
    return Eigen::Matrix<Scalar, Dof, Dof>::Identity(a.size(), a.size());
  }
  template<typename Derived>
  static inline Eigen::Matrix<Scalar, Dof, (Dof > 0 ? Dof * Dof : -1)>
  d2r_exp(const Eigen::MatrixBase<Derived> & a)
  {
    return Eigen::Matrix<Scalar, Dof, (Dof > 0 ? Dof * Dof : -1)>::Zero(
      a.size(), a.size() * a.size());
  }
  template<typename Derived>
  static inline Eigen::Matrix<Scalar, Dof, (Dof > 0 ? Dof * Dof : -1)>
  d2r_expinv(const Eigen::MatrixBase<Derived> & a)
  {
    return Eigen::Matrix<Scalar, Dof, (Dof > 0 ? Dof * Dof : -1)>::Zero(
      a.size(), a.size() * a.size());
  }
  // \endcond
};

///////////////////////////////////////////////////////////////
//// Lie group interface for built-in floating point types ////
///////////////////////////////////////////////////////////////

/**
 * @brief LieGroup interface for ScalarType
 */
template<ScalarType G>
struct lie<G>
{
  // \cond
  using Scalar      = G;
  using PlainObject = G;
  template<typename NewScalar>
  using CastT = NewScalar;

  static constexpr int Dof            = 1;
  static constexpr bool IsCommutative = true;

  // group interface

  static inline PlainObject Identity(Eigen::Index) { return G(0); }
  static inline PlainObject Random(Eigen::Index)
  {
    return G(Scalar(-1) + static_cast<Scalar>(rand()) / static_cast<Scalar>(RAND_MAX / 2));
  }
  static inline Eigen::Matrix<Scalar, 1, 1> Ad(G) { return Eigen::Matrix<Scalar, 1, 1>{1}; }
  static inline PlainObject composition(G g1, G g2) { return g1 + g2; }
  static inline Eigen::Index dof(G) { return 1; }
  static inline PlainObject inverse(G g) { return -g; }
  static inline bool isApprox(G g1, G g2, Scalar eps)
  {
    using std::abs;
    return abs<G>(g1 - g2) <= eps * abs<G>(g1);
  }
  static inline Eigen::Matrix<Scalar, 1, 1> log(G g) { return Eigen::Matrix<Scalar, 1, 1>{g}; }
  template<typename NewScalar>
  static inline NewScalar cast(G g)
  {
    return static_cast<NewScalar>(g);
  }

  // tangent interface

  template<typename Derived>
  static inline Eigen::Matrix<Scalar, 1, 1> ad(const Eigen::MatrixBase<Derived> &)
  {
    return Eigen::Matrix<Scalar, 1, 1>::Zero();
  }
  template<typename Derived>
  static inline PlainObject exp(const Eigen::MatrixBase<Derived> & a)
  {
    return a(0);
  }
  template<typename Derived>
  static inline Eigen::Matrix<Scalar, 1, 1> dr_exp(const Eigen::MatrixBase<Derived> &)
  {
    return Eigen::Matrix<Scalar, 1, 1>::Identity();
  }
  template<typename Derived>
  static inline Eigen::Matrix<Scalar, 1, 1> dr_expinv(const Eigen::MatrixBase<Derived> &)
  {
    return Eigen::Matrix<Scalar, 1, 1>::Identity();
  }
  template<typename Derived>
  static inline Eigen::Matrix<Scalar, 1, 1> d2r_exp(const Eigen::MatrixBase<Derived> &)
  {
    return Eigen::Matrix<Scalar, 1, 1>::Zero();
  }
  template<typename Derived>
  static inline Eigen::Matrix<Scalar, 1, 1> d2r_expinv(const Eigen::MatrixBase<Derived> &)
  {
    return Eigen::Matrix<Scalar, 1, 1>::Zero();
  }
  // \endcond
};

/**
 * @brief Manifold interface for LieGroup that are not already Manifold.
 */
template<LieGroup G>
  requires(!RnType<G> && !ScalarType<G>)
struct man<G>
{
  // \cond
  using Scalar      = typename traits::lie<G>::Scalar;
  using PlainObject = typename traits::lie<G>::PlainObject;
  template<typename NewScalar>
  using CastT = typename traits::lie<G>::template CastT<NewScalar>;

  static constexpr Eigen::Index Dof = traits::lie<G>::Dof;

  static inline PlainObject Default(Eigen::Index dof) { return traits::lie<G>::Identity(dof); }

  static inline Eigen::Index dof(const G & g) { return traits::lie<G>::dof(g); }

  template<typename NewScalar>
  static inline CastT<NewScalar> cast(const G & g)
  {
    return traits::lie<G>::template cast<NewScalar>(g);
  }

  template<typename Derived>
  static inline PlainObject rplus(const G & g, const Eigen::MatrixBase<Derived> & a)
  {
    return traits::lie<G>::composition(g, traits::lie<G>::exp(a));
  }

  template<LieGroup Go = G>
  static inline Eigen::Matrix<Scalar, Dof, 1> rminus(const G & g1, const Go & g2)
  {
    return traits::lie<G>::log(traits::lie<Go>::composition(traits::lie<Go>::inverse(g2), g1));
  }
  // \endcond
};

}  // namespace traits

////////////////////////////////////////////////////////
//// Free functions that dispatch to traits::lie<G> ////
////////////////////////////////////////////////////////

// Group interface

template<LieGroup G>
static constexpr bool IsCommutative = traits::lie<G>::IsCommutative;

/**
 * @brief Identity in Lie group
 *
 * @param dof degrees of freedom
 */
template<LieGroup G>
inline PlainObject<G> Identity(Eigen::Index dof)
{
  return traits::lie<G>::Identity(dof);
}

/**
 * @brief Identity in Lie group with static Dof
 */
template<LieGroup G>
  requires(Dof<G> > 0)
inline PlainObject<G> Identity() { return traits::lie<G>::Identity(Dof<G>); }

/**
 * @brief Random element in Lie group
 *
 * @param dof degrees of freedom
 */
template<LieGroup G>
inline PlainObject<G> Random(Eigen::Index dof)
{
  return traits::lie<G>::Random(dof);
}

/**
 * @brief Random element in Lie group with static Dof
 */
template<LieGroup G>
  requires(Dof<G> > 0)
inline PlainObject<G> Random() { return traits::lie<G>::Random(Dof<G>); }

/**
 * @brief Group adjoint \f$ Ad_g a \coloneq (G * \hat(a) * G^{-1})^{\wedge} \f$
 */
template<LieGroup G>
inline TangentMap<G> Ad(const G & g)
{
  return traits::lie<G>::Ad(g);
}

/**
 * @brief Group binary composition
 */
template<LieGroup G, typename Arg>
inline PlainObject<G> composition(const G & g, Arg && a)
{
  return traits::lie<G>::composition(g, std::forward<Arg>(a));
}

/**
 * @brief Group multinary composition
 */
template<LieGroup G, typename Arg, typename... Args>
inline PlainObject<G> composition(const G & g, Arg && a, Args &&... as)
{
  return composition(composition(g, std::forward<Arg>(a)), std::forward<Args>(as)...);
}

/**
 * @brief Group inverse
 */
template<LieGroup G>
inline PlainObject<G> inverse(const G & g)
{
  return traits::lie<G>::inverse(g);
}

/**
 * @brief Check if two group elements are approximately equal
 */
template<LieGroup G, typename Arg>
inline bool isApprox(
  const G & g,
  Arg && a,
  typename traits::lie<G>::Scalar eps =
    Eigen::NumTraits<typename traits::lie<G>::Scalar>::dummy_precision())
{
  return traits::lie<G>::isApprox(g, std::forward<Arg>(a), eps);
}

/**
 * @brief Group logarithm
 *
 * @see exp()
 */
template<LieGroup G>
inline Tangent<G> log(const G & g)
{
  return traits::lie<G>::log(g);
}

// Tangent interface

/**
 * @brief Lie algebra adjoint \f$ ad_a b = [a, b] \f$
 */
template<LieGroup G, typename Arg>
inline TangentMap<G> ad(Arg && a)
{
  return traits::lie<G>::ad(std::forward<Arg>(a));
}

/**
 * @brief Lie algebra exponential
 *
 * @see log()
 */
template<LieGroup G, typename Arg>
inline PlainObject<G> exp(Arg && a)
{
  return traits::lie<G>::exp(std::forward<Arg>(a));
}

/**
 * @brief Right Jacobian of exponential map
 */
template<LieGroup G, typename Arg>
inline TangentMap<G> dr_exp(Arg && a)
{
  return traits::lie<G>::dr_exp(std::forward<Arg>(a));
}

/**
 * @brief Right Jacobian of exponential map inverse
 */
template<LieGroup G, typename Arg>
inline TangentMap<G> dr_expinv(Arg && a)
{
  return traits::lie<G>::dr_expinv(std::forward<Arg>(a));
}

/**
 * @brief Right Hessian of exponential map
 */
template<LieGroup G, typename Arg>
inline Hessian<G> d2r_exp(Arg && a)
{
  return traits::lie<G>::d2r_exp(std::forward<Arg>(a));
}

/**
 * @brief Right Hessian of exponential map inverse
 */
template<LieGroup G, typename Arg>
inline Hessian<G> d2r_expinv(Arg && a)
{
  return traits::lie<G>::d2r_expinv(std::forward<Arg>(a));
}

// Convenience methods

/**
 * @brief Left-plus
 */
template<LieGroup G, typename Derived>
inline PlainObject<G> lplus(const G & g, const Eigen::MatrixBase<Derived> & a)
{
  return composition(::smooth::exp<G>(a), g);
}

/**
 * @brief Left-minus
 */
template<LieGroup G, LieGroup Go>
inline Tangent<G> lminus(const G & g1, const Go & g2)
{
  return log(composition(g1, inverse(g2)));
}

/**
 * @brief Left Jacobian of exponential map
 */
template<LieGroup G, typename Derived>
inline TangentMap<G> dl_exp(const Eigen::MatrixBase<Derived> & a)
{
  return dr_exp<G>(-a);
}

/**
 * @brief Left Jacobian of exponential map inverse
 */
template<LieGroup G, typename Derived>
inline TangentMap<G> dl_expinv(const Eigen::MatrixBase<Derived> & a)
{
  return dr_expinv<G>(-a);
}

/**
 * @brief Left Hessian of exponential map
 */
template<LieGroup G, typename Derived>
inline Hessian<G> d2l_exp(const Eigen::MatrixBase<Derived> & a)
{
  return -d2r_exp<G>(-a);
}

/**
 * @brief Left Hessian of exponential map inverse
 */
template<LieGroup G, typename Derived>
inline Hessian<G> d2l_expinv(const Eigen::MatrixBase<Derived> & a)
{
  return -d2r_expinv<G>(-a);
}

}  // namespace smooth

#endif  // SMOOTH__LIE_GROUP_HPP_


#ifndef PRECONDITIONED_CONJUGATE_GRADIENT_H
#define PRECONDITIONED_CONJUGATE_GRADIENT_H
#include <Eigen/Dense>
#include <mtao/logging/timer.hpp>
#include <iostream>

template <typename Matrix>
struct DenseLDLT
{
    typedef typename Matrix::Scalar Scalar;
    template <typename A, typename B, typename C>
    inline Scalar tripleVectorProduct(const A & a, const B & b, const C & c) const {
        return a.cwiseProduct(b).dot(c);
    }
    inline Scalar tripleProduct(const Matrix & a, uint i, uint j) const
    {
        return tripleVectorProduct(a.row(i).head(j),a.row(j).head(j),a.diagonal().head(j));
    }
    DenseLDLT(const Matrix & A)
    {
        LD=A.template triangularView<Eigen::Lower>();
        int i,j;
        for(i=0; i<A.rows(); ++i)
        {
            for(j=0; j<i; ++j)
                if(std::abs(LD(j,j))>0.0001)
                {
                    LD(i,j)-=tripleProduct(LD,i,j);
                    LD(i,j)/=LD(j,j);
                }
                else
                {
                    LD(i,j)=0;
                }
            LD(i,i) -= tripleProduct(LD,i,i);
        }

    }
    template <typename Vector>
    void solve(const Vector & b, Vector & x) const
    {
        x = LD.template triangularView<Eigen::UnitLower>().solve(b);
        x.noalias() = x.cwiseQuotient(LD.diagonal());//safe beacuse it's a dot
        LD.template triangularView<Eigen::UnitLower>().transpose().solveInPlace(x);
    }
    Matrix getA() const
    {
        Matrix A = LD.template triangularView<Eigen::UnitLower>().transpose();
        A = LD.diagonal().asDiagonal() * A;
        A = LD.template triangularView<Eigen::UnitLower>() * A;

        return A;
    }
private:
    Matrix LD;
};

template <typename Matrix, typename Vector>
struct SparseLDLT
{
    typedef typename Matrix::Scalar Scalar;
    SparseLDLT() {}
    SparseLDLT(const Matrix & A)
    {
        // L=tril(A);
        L=A.template triangularView<Eigen::StrictlyLower>();//Don't copy the diagonal
        for(int i=0; i<L.rows(); ++i)
        {
            if(L.coeff(i,i)!=0)
                L.coeffRef(i,i)=0;
        }
        Dinv=D=A.diagonal();

        // for k=1:size(L,2)
        for(int k=0; k<A.rows(); ++k)//k is the column that we're infecting the remaining columns with
        {//L(:,k)

            //Solidify the current column values
            //==================================
            if(D(k)==0) continue;
            if(Dinv(k)<0.25*D(k))//If D has shrunk too much since it started
                Dinv(k)=1/D(k);
            else
                Dinv(k)=1/Dinv(k);
            L.innerVector(k) *= Dinv(k);

            //Add k terms to all of the following columns
            //===========================================
            for(typename Matrix::InnerIterator it(L,k); it; ++it)// -L(i,k)*D(k)*L(j,k)
            {
                int j = it.row();//j>k
                if(j<=k) continue;
                Scalar missing=0;
                Scalar multiplier=it.value();//L(j,k)*D(k)

                typename Matrix::InnerIterator k_it(L,k);
                typename Matrix::InnerIterator j_it(L,j);
                //move down teh column of L(:,k) to collect missing elements in the match with A(:,j)
                //i=k_it.row()

                while (k_it && k_it.row()<j){//L(i,k)
                    while(j_it)//L(i,j) occasionally
                    {
                        if(j_it.row() < k_it.row())
                            ++j_it;
                        else if(j_it.row() == k_it.row())//L(i,k) are L(i,j) are nonzero
                            break;
                        else
                        {
                            missing += k_it.value();//L(i,k) will fill something not in L(i,j)
                            break;
                        }
                    }
                    ++k_it;
                }

                if(k_it && j_it.row() == j)
                {
                    Dinv(j) -= it.value() * multiplier;
                }

                typename Matrix::InnerIterator j_it2(L,j);
                while(k_it && j_it2)
                {
                    if(j_it2.row() < k_it.row())
                        ++j_it2;
                    else if(j_it2.row() == k_it.row())//L(i,k) and L(i,j) are both nonzero, -=L(i,k)*L(j,k)*D(k)
                    {
                        j_it2.valueRef() -= multiplier * k_it.value() ;//k_it.value()=L(i,k)
                        ++j_it2;
                        ++k_it;
                    }
                    else
                    {
                        missing+=k_it.value();
                        ++k_it;
                    }
                }

                while(k_it)
                {
                    missing+=k_it.value();
                    ++k_it;
                }
                Dinv(j)-=0.97*missing*multiplier;
            }
        }

        /*
           std::cout << L << std::endl;
           */

    }
    void solve(const Vector & b, Vector & x) const
    {
        x = L.template triangularView<Eigen::UnitLower>().solve(b);
        x.noalias() = x.cwiseProduct(Dinv);//safe beacuse it's a dot
        L.transpose().template triangularView<Eigen::UnitUpper>().solveInPlace(x);
    }
    Matrix getA()
    {
        Matrix
                A = L.template triangularView<Eigen::UnitLower>();
        A = A * D.asDiagonal();
        A = A * L.template triangularView<Eigen::UnitLower>().transpose();

        return A;
    }
private:
    Matrix L;
    Vector D,Dinv;
};

template <typename MatrixType, typename VectorType, typename Preconditioner>
struct PreconditionedConjugateGradient
{
    typedef MatrixType Matrix;
    typedef VectorType Vector;
    typedef typename Vector::Scalar Scalar;
    PreconditionedConjugateGradient(const Matrix & A): A(A)
    {
        mtao::logging::timer("Preconditioner",false);
        precond = Preconditioner(A);
    }
    PreconditionedConjugateGradient(PreconditionedConjugateGradient&&) = default;
    auto solve(const Vector& b) const {
        mtao::logging::timer("Solve",false);
        auto x = b.eval();
        x.setZero();
        Vector r = b-A*x;
        Vector z;
        precond.solve(r,z);
        Vector p = z;
        Vector Ap = A*p;
        Scalar rdz = r.dot(z);
        Scalar alpha, beta;
        auto error = [&]() { return r.template lpNorm<Eigen::Infinity>(); };

        uint iterations = 0;
        Scalar eps = error() * epsilon;
        while(++iterations < 10 &&
        //while(++iterations < b.rows() &&
                error() > eps)
        {
            alpha = (rdz)/(p.dot(Ap));
            x+=alpha * p;
            r-=alpha * Ap;
            precond.solve(r,z);
            beta=1/rdz;
            rdz = r.dot(z);
            beta*=rdz;
            p=z+beta*p;
            Ap=A*p;
        }
        return x;
    }
    Scalar error()
    {
    }
private:
    const Matrix A;
    Preconditioner precond;

    Scalar epsilon = 1e-5;

};

template <typename Matrix, typename Vector>
auto ldlt_pcg_solver(const Matrix & A, const Vector& b)
{
    return PreconditionedConjugateGradient<Matrix,Vector, SparseLDLT<Matrix, mtao::Vector<typename Vector::Scalar, Vector::RowsAtCompileTime>>>(A);
    //auto solver = IterativeLinearSolver<PreconditionedConjugateGradientCapsule<Matrix,Vector, Preconditioner> >(A.rows(), 1e-5);
}
template <typename Matrix, typename Vector>
auto ldlt_pcg_solve(const Matrix & A, const Vector & b)
{
    auto solver = ldlt_pcg_solver(A,b);
    //auto solver = IterativeLinearSolver<PreconditionedConjugateGradientCapsule<Matrix,Vector, Preconditioner> >(A.rows(), 1e-5);
    return solver.solve(b);
}

#endif


#include <armadillo>

int main() {

    const size_t dim = 3;

    arma::mat A(dim, dim, arma::fill::ones);
    A.print("A");
    arma::mat B = A;
    arma::vec b(dim);
    for (size_t i = 0; i < dim; i++)
        b(i) = (i + 2);
    b.print("b");

    // error: element-wise multiplication: incompatible matrix dimensions: 3x3 and 3x1
    // (A % b).print("A % b");

    // This seems silly but it's ok from a memory access standpoint.
    for (size_t j = 0; j < dim; j++)
        A.col(j) *= b(j);
    A.print("% (1)");

    // error: no match for ‘operator*=’ (operand types are ‘arma::subview_row<double>’ and ‘arma::vec {aka arma::Col<double>}’)
    // for (size_t i = 0; i < dim; i++)
    //     B.row(i) *= b;
    // error: element-wise multiplication: incompatible matrix dimensions: 1x3 and 3x1
    // for (size_t i = 0; i < dim; i++)
    //     B.row(i) = B.row(i) % b;
    // This is matrix multiplication!
    // B *= b;

    // order doesn't matter
    arma::mat C = B.each_row() % b.t();
    C.print("% (2)");

    return 0;
}


// Static blocked LU Decomposition

#include <stdio.h>
#include <hpx/hpx_init.hpp>
#include <hpx/include/threads.hpp>
#include <hpx/include/lcos.hpp>
#include <hpx/lcos/local/dataflow.hpp>
#include <hpx/util/unwrapped.hpp>

#include "lu-local.h"

#include <boost/assign.hpp>

using std::vector;
using hpx::util::unwrapped;
using hpx::lcos::shared_future;
using hpx::lcos::wait_all;
using hpx::async;
using hpx::lcos::local::dataflow;
using hpx::when_all;
using hpx::make_ready_future;

vector<double> A;
auto diag_op  = unwrapped( &ProcessDiagonalBlock );
auto row_op   = unwrapped( &ProcessBlockOnRow );
auto col_op   = unwrapped( &ProcessBlockOnColumn );
auto inner_op = unwrapped( &ProcessInnerBlock );

void init_df(vector<vector<vector<shared_future<block>>>> &dfArray, int numBlocks, int size) {
    vector<vector<block>> blockList;
    getBlockList(blockList, numBlocks, size);

    dfArray[0].resize(numBlocks);
    dfArray[1].resize(numBlocks);

    for(int i = 0; i < numBlocks; i++){
        dfArray[0][i].resize( numBlocks );
        dfArray[1][i].resize( numBlocks );
    }

    shared_future<int> fsize = make_ready_future(size);

    dfArray[0][0][0] = async( ProcessDiagonalBlock, size, blockList[0][0] );

    for(int i = 1; i < numBlocks; i++) {
        dfArray[0][0][i] = dataflow( row_op, fsize, make_ready_future( blockList[0][i] ), dfArray[0][0][0]);
    }
    for(int i = 1; i < numBlocks; i++) {
        dfArray[0][i][0] = dataflow( col_op, fsize, make_ready_future( blockList[i][0] ), dfArray[0][0][0]);
        for(int j = 1; j < numBlocks; j++) {
            dfArray[0][i][j] = dataflow( inner_op, fsize, make_ready_future( blockList[i][j] ),
                                                   dfArray[0][0][j], dfArray[0][i][0] );
        }
    }
}

void LU( int size, int numBlocks)
{
    vector<vector<vector<shared_future<block>>>> dfArray(2);
    shared_future<int> fsize = make_ready_future(size);

    init_df(dfArray, numBlocks, size);

    for(int i = 1; i < numBlocks; i++) {
        dfArray[i%2][i][i] = dataflow( diag_op, fsize, dfArray[(i-1)%2][i][i]);
        for(int j = i + 1; j < numBlocks; j++){
            dfArray[i%2][i][j] = dataflow( row_op , fsize,
                                           dfArray[(i-1)%2][i][j],
                                           dfArray[ i   %2][i][i] );
        }
        for(int j = i + 1; j < numBlocks; j++){
            dfArray[i%2][j][i] = dataflow( col_op, fsize,
                                           dfArray[(i-1)%2][j][i],
                                           dfArray[ i   %2][i][i] );

            for(int k = i + 1; k < numBlocks; k++) {
                dfArray[i%2][j][k] = dataflow( inner_op, fsize,
                                               dfArray[(i-1)%2][j][k],
                                               dfArray[ i   %2][i][k],
                                               dfArray[ i   %2][j][i] );
            }
        }
    }
    wait_all(dfArray[(numBlocks-1)%2][numBlocks-1][numBlocks-1]);
}

int hpx_main(int argc, char *argv[])
{
    vector<double> originalA;
    int size = 1000;
    int numBlocks = 10;
    unsigned long t1, t2;
    bool runCheck = false;

    if( argc > 1 )
        size = atoi(argv[1]);
    if( argc > 2 )
        numBlocks = atoi(argv[2]);
    if( argc > 3 )
        runCheck = true;
    printf("size = %d, numBlocks = %d\n", size, numBlocks);

    A.resize(size*size, 0);
    if(runCheck) {
        printf("Error checking enabled\n");
        InitMatrix3( size );
        originalA.reserve(size*size);
        for(int i = 0; i < size * size; i++) {
            originalA[i] = A[i];
        }
    } else {
        printf("fast initialization\n");
        fastInitMatrix(size);
    }
    printf("initialization complete\n");

    if(numBlocks == 1) {
        t1 = GetTickCount();
        ProcessDiagonalBlock( size, block(size, 0, size));
        t2 = GetTickCount();
    } else if( numBlocks > 1) {
        t1 = GetTickCount();
        LU( size, numBlocks);
        t2 = GetTickCount();
    } else {
        printf("Error: numBlocks must be greater than 0.\n");
        return hpx::finalize();
    }
    printf("Time for LU-decomposition in secs: %f \n", (t2-t1)/1000000.0);

    if(runCheck) {
        checkResult( originalA,  size );
    }
    return hpx::finalize();
}

int main(int argc, char *argv[])
{
    using namespace boost::assign;
    std::vector<std::string> cfg;
    cfg += "hpx.os_threads=" +
        boost::lexical_cast<std::string>(hpx::threads::hardware_concurrency());

    return hpx::init(argc, argv, cfg);
}


#include <chrono>
#include <filesystem>
#include <iostream>
#include <iterator>
#include <vector>

#include <Eigen/Sparse>
#include <unsupported/Eigen/SparseExtra>

#include <memory_xplatform.h>

#ifdef DEBUG
    #define D(x) (std::cerr << x << std::endl)
#else
    #define D(x) do{}while(0)
#endif

#define CSV_EOL "\n"

#if defined(_WIN32) || defined(__CYGWIN__)
    #define OUTPUT_FILE "windows-output.csv"
#elif defined(__linux__) || defined(unix) || defined(__unix__) || defined(__unix)
    #define OUTPUT_FILE "unix-output.csv"
#else
    #error Unknown environment!
#endif

typedef unsigned long long ull;
typedef Eigen::SparseMatrix<double> SpMat;

struct result {
    unsigned int size;
    ull memory_delta;
    std::chrono::duration<double> solve_time;
    double relative_error;
};

result analyze_matrix(std::string filename);
std::ostream& operator<<(std::ostream& stream, const result& r);

int main() {
    result _result;
    std::ofstream output(OUTPUT_FILE, std::ofstream::out);

    // Write headers
    output << "filename" << ","
        << "size" << ","
        << "memory_delta" << ","
        << "solve_time" << ","
        << "relative_error" << CSV_EOL;

    // Look for matrix in ../matlab/matrix_mtx folder
    std::string path = "../matlab/matrix_mtx";
    for (const auto& entry : std::filesystem::directory_iterator(path)) {
        if (entry.path().extension() == ".mtx") {
            if (! output.is_open()) {
                output.open(OUTPUT_FILE, std::ofstream::app);
            }

            std::cout << entry.path() << std::endl;
            _result = analyze_matrix(entry.path().string());

            D("Writing output...");
            output << entry.path().stem() << "," << _result << CSV_EOL;
            D("");

            output.close();
        }
    }

    return 0;
}

/**
 * Analyze a single matrix given it's filename
 * The matrix will be imported assuming it's in .mtx format.
 *
 * @param std::string filename
 * @return result
 */
result analyze_matrix(std::string filename) {
    result r;
    ull start_tot_virtual, start_proc_virtual, start_proc_physical, start_tot_physical,
        end_tot_virtual, end_proc_virtual, end_proc_physical, end_tot_physical;

    SpMat A; // Eigen::SparseMatrix<double>
    D("Loading matrix file: " << filename);
    Eigen::loadMarket(A, filename);

    // Debug memory usage to cout
    D("Memory Usage (proc/total):");
    start_proc_virtual = memory::process_current_virtual();
    start_tot_virtual = memory::total_virtual();
    D("> Virtual: " << start_proc_virtual << " / " << start_tot_virtual);
    start_proc_physical = memory::process_current_physical();
    start_tot_physical = memory::total_physical();
    D("> Physical: " << start_proc_physical << " / " << start_tot_physical);
    D("");

    D("Solve:");
    D("> Calculating b vector...");
    Eigen::VectorXd x_es = Eigen::VectorXd::Ones(A.rows());
    Eigen::VectorXd b(A.rows());
    b = A*x_es;

    D("> Applying CholeskySimplicial solver...");
    auto chol_start = std::chrono::high_resolution_clock::now();
    Eigen::SimplicialCholesky<SpMat> chol(A);
    Eigen::VectorXd x_ap = chol.solve(b);
    auto chol_finish = std::chrono::high_resolution_clock::now();
    D("");

    D("Memory Usage (proc/total):");
    end_proc_virtual = memory::process_current_virtual();
    end_tot_virtual = memory::total_virtual();
    D("> Virtual: " << end_proc_virtual << " / " << end_tot_virtual);
    end_proc_physical = memory::process_current_physical();
    end_tot_physical = memory::total_physical();
    D("> Physical: " << end_proc_physical << " / " << end_tot_physical);
    D("");

    r.size = A.rows();
    r.memory_delta = end_proc_physical - start_proc_physical;
    r.solve_time = chol_finish - chol_start;
    r.relative_error = (x_ap - x_es).norm() / x_es.norm();

    D("Results");
    D("> Solve time in seconds: " << r.solve_time.count());
    D("> Relative error: " << r.relative_error);
    D("");

    return r;
}

/**
 * Override the << operator to allow an easy print of
 * the results struct to stdout or output file.
 *
 * @params std::ostream& stream
 * @params const result& r
 * @return std::ostream&
 */
std::ostream& operator<<(std::ostream& stream, const result& r) {
    stream << r.size << ","
        << r.memory_delta << ","
        << r.solve_time.count() << ","
        << r.relative_error;

    return stream;
}


#include <assert.h>
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>

#include <algorithm>

#include <NTL/GF2X.h>
#include <NTL/GF2XFactoring.h>
#include <NTL/GF2E.h>
#include <NTL/GF2EX.h>
#include <NTL/GF2EXFactoring.h>

#include "gcm.h"

using namespace std;
using namespace NTL;

#define NELEMS(arr) ((sizeof arr) / (sizeof arr[0]))

void initfield(void)
{
    GF2X gf2e128mod;

    // builds the right poly for GMAC's GF(2^128) modulus
    BuildIrred(gf2e128mod, 128);
    GF2E::init(gf2e128mod);
}

void asserthex(const char *s)
{
    size_t i, len;

    len = strlen(s);
    assert(len % 2 == 0);
    for (i = 0; i < len; i += 1) {
        char c;

        c = s[i];
        assert((c >= '0' && c <= '9') ||
               (c >= 'a' && c <= 'f') ||
               (c >= 'A' && c <= 'F'));
    }
}

void hex2bytes(struct slice *s, const char *hex)
{
    size_t i;
    unsigned tmp;

    asserthex(hex);

    s->len = strlen(hex) / 2;
    s->data = (uint8_t *)malloc(s->len);

    for (i = 0; i < s->len; i += 1) {
        sscanf(hex + (2*i), "%02x", &tmp);
        s->data[i] = tmp;
    }
}

void block2felem(GF2E &x, const struct slice *block)
{
    GF2X p;
    size_t i, j;

    for (i = 0; i < block->len; i += 1) {
        uint8_t b = block->data[i];
        for (j = 0; j < 8; j += 1) {
            SetCoeff(p, 8*i + j, b >> (7-j));
        }
    }

    conv(x, p);
}

void packuint64(uint8_t *buf, uint64_t x)
{
    size_t i;

    for (i = 0; i < 8; i += 1) {
        buf[i] = x >> (8*(7-i));
    }
}

void buildpoly(GF2EX &p, const struct slice *a,
               const struct slice *c, const struct slice *t)
{
    GF2E x;
    const struct slice *sp[] = {a, c};
    struct slice s, block;
    size_t i, j;
    uint8_t lenblock[blocklen];

    i = 0;
    for (j = 0; j < NELEMS(sp); j += 1) {
        s = *sp[j];
        while (s.len > 0) {
            block.data = s.data;
            block.len = min(blocklen, s.len);

            block2felem(x, &block);
            SetCoeff(p, i, x);

            i += 1;
            s.data += block.len;
            s.len -= block.len;
        }
    }

    block.data = &lenblock[0];
    block.len = blocklen;
    packuint64(block.data, a->len << 3);
    packuint64(block.data + 8, c->len << 3);

    block2felem(x, &block);
    SetCoeff(p, i, x);

    i += 1;

    assert(t->len == blocklen);

    block2felem(x, t);
    SetCoeff(p, i, x);

    reverse(p, p);
}

void felem2hex(char *out, const GF2E &x)
{
    GF2X p;
    size_t i;
    uint b;
    const char *hex = "0123456789abcdef";

    p = rep(x);

    for (i = 0; i < 32; i += 1) {
        size_t j;

        j = 4 * i;
        b = ((conv<uint>(coeff(p, j+0)) << 3) |
             (conv<uint>(coeff(p, j+1)) << 2) |
             (conv<uint>(coeff(p, j+2)) << 1) |
             (conv<uint>(coeff(p, j+3))));
        out[i] = hex[b];
    }

    out[32] = '\0';
}


/***
 * @Author: JoeyforJoy
 * @Date: 2021-05-06 21:55:55
 * @LastEditTime: 2021-10-23 00:13:45
 * @LastEditors: JoeyforJoy
 * @Description: A simple KD Tree implementation
 * @Usage:
 *      Eigen::MatrixXd cloud_eigen;
 *      cloud_eigen = ... ; // assign point cloud for cloud_eigen
 *      int leaf_size = 5; // the minimum number of points in the leaf nodes
 *      KDTree kdtree(cloud_eigen, leaf_size); // build kdtree
 *      // knn search
 *      std::vector<int> pts_idx; // indices of result points
 *      std::vector<double> pts_dist; // distance of result points
 *      int k = 5;
 *      kdtree.knnSearch(point, k, pts_idx, pts_dist);
 *      // radius search
 *      int radius = 5;
 *      kdtree.radiusSearch(point, radius, pts_idx, pts_dist);
 */

#ifndef KDTREE_HPP
#define KDTREE_HPP
#include <memory>
#include <numeric>
#include <Eigen/Core>

#include "resultSet.hpp"

namespace NNSearch {
    struct KDNode {
        typedef std::shared_ptr<KDNode> Ptr;
        public:
            int dim; // 0 for X, 1 for Y, 2 for X
            double value;
            KDNode::Ptr left;
            KDNode::Ptr right;
            bool isLeaf;
            std::vector<int> points_idx;
        public:
            KDNode(): dim(0), value(0), isLeaf(false), left(nullptr), right(nullptr){}
            KDNode(int dim_): dim(dim_), value(0), left(nullptr), right(nullptr), isLeaf(false) {}
    };

    class KDTree {
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        public:
            KDTree(): _root(nullptr), _leaf_size(0) {}
            // copy constructor
            KDTree(const KDTree& tree): _root(tree.getRoot()), _leaf_size(tree._leaf_size) {}
            // copy assign
            KDTree& operator=(const KDTree& tree) {_root = tree.getRoot(); _leaf_size = tree._leaf_size;}
            // build KDTree
            KDTree(Eigen::MatrixXd &points, int leaf_size): _points(points), _leaf_size(leaf_size) {
                std::vector<int> pts_idx(points.rows(), 0);
                std::iota(pts_idx.begin(), pts_idx.end(), 0);
                _root = _buildKDTree(pts_idx, 0, pts_idx.size(), 0);
            }

            ~KDTree() {}

            KDNode::Ptr getRoot() const {return _root;}
            int getLeafSize() const {return _leaf_size;}

            // knn search
            void knnSearch(const Eigen::Vector3d &point, int k, std::vector<int> &pts_idx, std::vector<double> &pts_dist) {
                // KNN search
                KNNResultSet result_set(k);
                _SearchIdxDist(_root, point, result_set);
                // unpack result
                result_set.unpackResultSet(pts_idx, pts_dist);
            }

            // radius search
            void radiusSearch(const Eigen::Vector3d &point, double radius, std::vector<int> &pts_idx, std::vector<double> &pts_dist) {
                // radius search
                RadiusResultSet result_set(radius);
                _SearchIdxDist(_root, point, result_set);
                // unpack result
                result_set.unpackResultSet(pts_idx, pts_dist);
            }

        private:
            void _SearchIdxDist(KDNode::Ptr root, const Eigen::Vector3d &point, AbstractResultSet &result_set) {
                if (!root) return;
                if (root->isLeaf) {
                    // process leaf
                    for (int idx: root->points_idx) {
                        double dist = (_points.row(idx) - point.transpose()).norm();
                        result_set.addOnePoint(idx, dist);
                    }
                } else {
                    if (point(root->dim) <= root->value) { // If the value is less than the root, search the left.
                        _SearchIdxDist(root->left, point, result_set);
                        // If the point is too far, don't search the right tree.
                        if (fabs(point(root->dim) - root->value) <= result_set.worst_dist()) {
                            _SearchIdxDist(root->right, point, result_set);
                        }
                    } else {
                        _SearchIdxDist(root->right, point, result_set);
                        // If the point is too far, don't search the left tree.
                        if (fabs(point(root->dim) - root->value) <= result_set.worst_dist()) {
                            _SearchIdxDist(root->left, point, result_set);
                        }
                    }
                }
            }

            // 构造 KD 树
            KDNode::Ptr _buildKDTree(std::vector<int> &pts_idx,
                                    int left, int right, int dim) {
                if (right - left <= 0) return nullptr;
                KDNode::Ptr root(new KDNode(dim));
                if (right - left <= _leaf_size ) {
                    root->isLeaf = true;
                    root->points_idx.assign(pts_idx.begin()+left, pts_idx.begin()+right);
                } else {
                    // current splitting axis
                    auto min_max_x = std::minmax_element(
                        pts_idx.begin() + left,
                        pts_idx.begin() + right,
                        [&](int lhs, int rhs) { return _points(lhs, 0) < _points(rhs, 0); });
                    auto min_max_y = std::minmax_element(
                        pts_idx.begin() + left,
                        pts_idx.begin() + right,
                        [&](int lhs, int rhs) { return _points(lhs, 1) < _points(rhs, 1); });
                    auto min_max_z = std::minmax_element(
                        pts_idx.begin() + left,
                        pts_idx.begin() + right,
                        [&](int lhs, int rhs) { return _points(lhs, 2) < _points(rhs, 2); });
                    auto dx = _points((*min_max_x.second),0) - _points((*min_max_x.first),0);
                    auto dy = _points((*min_max_x.second),1) - _points((*min_max_x.first),1);
                    auto dz = _points((*min_max_x.second),2) - _points((*min_max_x.first),2);
                    dim = (dx > dy ? (dx > dz ? 0 : 2) : (dy > dz ? 1 : 2));

                    // find middle o(NlogN)
                    std::nth_element (
                        pts_idx.begin() + left,
                        pts_idx.begin() + (right - left) / 2 + left,
                        pts_idx.begin() + right,
                        [&]
                        (int lhs, int rhs)
                        { return _points(lhs, dim) < _points(rhs, dim);});

                    // find the middle and split
                    int mid = (right - left) / 2 + left;

                    root->value = (_points(pts_idx[mid], dim) + _points(pts_idx[mid+1], dim) + 1e-3) /2 ;
                    root->left = _buildKDTree(pts_idx, left, mid, dim);
                    root->right = _buildKDTree(pts_idx, mid, right, dim);
                }
                return root;
            }

        private:
            Eigen::MatrixXd _points;
            KDNode::Ptr _root;
            int _leaf_size;
    };
}
#endif


#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/random.hpp>
#include <queue>
#include <vector>
#include <chrono>
#include <boost/graph/graphviz.hpp>
#include "boostHelper/squareboost.h"
#include "leda/ledaRandomBiMaxFlow.h"
using namespace boost;
struct NodeInfo;
struct EdgeInfo;
typedef boost::adjacency_list<vecS, vecS, bidirectionalS, NodeInfo, EdgeInfo> Graph;
typedef boost::graph_traits<Graph>::vertex_descriptor vertex_d;
typedef boost::graph_traits<Graph>::edge_descriptor edge_d;
vertex_d s, t;
Graph G_random;

std::queue<vertex_d> overflowQ;

// vertex_d A = add_vertex(G_random);
// vertex_d B = add_vertex(G_random);
// vertex_d C = add_vertex(G_random);
// vertex_d D = add_vertex(G_random);
// vertex_d E = add_vertex(G_random);
// vertex_d F = add_vertex(G_random);

// std::pair<edge_d, bool> AB = add_boost::edge(A, B, G_random);
// std::pair<edge_d, bool> AC = add_boost::edge(A, C, G_random);
// std::pair<edge_d, bool> BD = add_boost::edge(B, D, G_random);
// std::pair<edge_d, bool> CE = add_boost::edge(C, E, G_random);
// std::pair<edge_d, bool> DC = add_boost::edge(D, C, G_random);
// std::pair<edge_d, bool> EF = add_boost::edge(E, F, G_random);

// std::pair<edge_d, bool> BA = add_boost::edge(B, A, G_random);
// std::pair<edge_d, bool> CA = add_boost::edge(C, A, G_random);
// std::pair<edge_d, bool> DB = add_boost::edge(D, B, G_random);
// std::pair<edge_d, bool> EC = add_boost::edge(E, C, G_random);
// std::pair<edge_d, bool> CD = add_boost::edge(C, D, G_random);
// std::pair<edge_d, bool> FE = add_boost::edge(F, E, G_random);

int main()
{
	int n1_length = 0;
	bool logging;
	int a_vertices = 0, b_vertices = 0, m_edges = 0;
	std::cout << "Num of A vertices?\n";
	std::cin >> a_vertices;
	std::cout << "Num of B vertices?\n";
	std::cin >> b_vertices;
	std::cout << "Logging (1/0)\n";
	std::cin >> logging;
	// std::cout << "Num of M edges?\n";
	// std::cin >> m_edges;
	ledaRandomBiMaxFlow(G_random, s, t, a_vertices, b_vertices, m_edges, n1_length); //generate_square_bipartite(8, s, t, n1_length);
	// std::cout << "Returned";
	boost::property_map<Graph, int EdgeInfo::*>::type residual = get(&EdgeInfo::residual_flow, G_random);
	boost::property_map<Graph, int NodeInfo::*>::type v_id = get(&NodeInfo::id, G_random);
	boost::property_map<Graph, int NodeInfo::*>::type height = get(&NodeInfo::height, G_random);
	boost::property_map<Graph, int NodeInfo::*>::type excess = get(&NodeInfo::excess, G_random);
	// write_graphviz(std::cout, G_random, make_label_writer(v_id), make_label_writer(residual));
	auto end = std::chrono::system_clock::now(); //declaring timers
	auto start = std::chrono::system_clock::now();
	auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
	start = std::chrono::system_clock::now();

	graph_traits<Graph>::vertex_iterator vi, vi_end;
	graph_traits<Graph>::edge_iterator ei, ei_end;
	// -----------------------------Initialization----------------------------------
	int nums = 0;
	for (tie(vi, vi_end) = vertices(G_random); vi != vi_end; vi++)
	{ //initialise heights and excess
		v_id[*vi] = nums;
		height[*vi] = 1000000;
		excess[*vi] = 0;
		nums++;
	}
	// std::cout << "S is: " << v_id[s] << " and t is: " << v_id[t] << "\n";

	// residual[AB.first] = 5;
	// residual[AC.first] = 4;
	// residual[BD.first] = 4;
	// residual[DC.first] = 2;
	// residual[CE.first] = 4;
	// residual[EF.first] = 4;
	// residual[BA.first] = 0;
	// residual[CA.first] = 0;
	// residual[DB.first] = 0;
	// residual[CD.first] = 0;
	// residual[EC.first] = 0;
	// residual[FE.first] = 0;
	// std::cout << "inited heights and excess\n";
	height[t] = 0;
	graph_traits<Graph>::out_edge_iterator edgei, edgei_end;
	graph_traits<Graph>::out_edge_iterator edgej, edgej_end;
	graph_traits<Graph>::in_edge_iterator bi, bi_end;

	for (tie(edgei, edgei_end) = out_edges(s, G_random); edgei != edgei_end; edgei++) // max flow on outgoing edges of S and update excess of S's neighbours, add residual edge
	{
		// std::cout << "from S to : " << target(*edgei, G_random) << "\n The edge is: " << residual[*edgei] << "\n";
		excess[target(*edgei, G_random)] = residual[*edgei];
		// std::cout << "updated excess of node\n";
		if (excess[target(*edgei, G_random)] != 0 && target(*edgei, G_random) != t)
			overflowQ.push(target(*edgei, G_random));
		// std::cout << "prob added q\n";
		// std::pair<edge_d, bool> resEdge = boost::edge(target(*edgei, G_random), s, G_random);
		// std::cout << "made edge to back\n";
		residual[boost::edge(target(*edgei, G_random), s, G_random).first] = residual[*edgei];
		// std::cout << "updated rev cap\n";
		residual[*edgei] = 0;
		// std::cout << "zeroed cap\n";
	}
	// std::cout << "inited S things\n";
	std::queue<vertex_d> q;
	q.push(t);
	vertex_d temp;

	while (!q.empty())
	{
		temp = q.front();
		q.pop();
		int alt = 0;
		for (tie(bi, bi_end) = in_edges(temp, G_random); bi != bi_end; bi++)
		{
			// std::cout << "Reading Edge: " << source(*bi, G_random) << " ---> " << target(*bi, G_random);
			alt = height[temp] + 1;
			if (alt < height[source(*bi, G_random)])
			{
				height[source(*bi, G_random)] = alt;
				q.push(source(*bi, G_random));
			}
		}
	}
	// std::cout << "n1_length: " << n1_length << "\n";
	height[s] = 2 * n1_length + 1;

	//-----------------------------------------------------------------------------------------
	//--------------------------Algorithm------------------------------------------------------
	bool admissableEdge1 = false;
	bool admissableEdge2 = false;
	bool elementExistsInQ = false;

	int j_minimum_height = 10000, i_minimum_height = 10000;
	int minimum_flow = 0;
	int times = 0;
	vertex_d k, j, i;
	std::pair<edge_d, bool> newEdge;
	while (!overflowQ.empty())
	{
		// std::cout << "The q has: " << overflowQ.size() << "elements \n";
		vertex_d overflower = overflowQ.front();
		overflowQ.pop();
		// std::cout << "The excess node is: " << overflower << "\n and it's excess is: " << excess[overflower] << "\n";
		admissableEdge1 = false;
		if (overflower == s || overflower == t || excess[overflower] == 0)
			continue;

		for (tie(edgei, edgei_end) = out_edges(overflower, G_random); edgei != edgei_end; edgei++) // max flow on outgoing edges of S and update excess of S's neighbours, add residual edge
		{
			j = target(*edgei, G_random);
			i = overflower;
			// std::cout << "the edge goes from " << i << " To " << j << "and has a residual of: " << residual[*edgei] << "\n";

			if (height[i] > height[j] && residual[*edgei] > 0)
			{

				admissableEdge1 = true;
				// std::cout << "admissable i-j: " << i << "-" << j << "\n";
				admissableEdge2 = false;
				for (tie(edgej, edgej_end) = out_edges(j, G_random); edgej != edgej_end; edgej++)
				{
					if (j == s)
					{
						// std::cout << "found only S to push";
						minimum_flow = std::min(excess[i], residual[*edgei]);
						residual[*edgei] -= minimum_flow;
						// std::cout << "MINFLOW: " << minimum_flow << "\n";
						residual[boost::edge(s, i, G_random).first] += minimum_flow;
						// std::cout << "Pushing from " << i << "to s, flow: " << minimum_flow << "\n";
						excess[i] -= minimum_flow;
						excess[s] += minimum_flow;
						admissableEdge2 = true;
						continue;
					}
					if (height[j] > height[target(*edgej, G_random)] && residual[*edgej] > 0)
					{

						k = target(*edgej, G_random);
						// std::cout << "admissable j-k :" << j << "-" << k << "\n";
						admissableEdge2 = true;
						minimum_flow = std::min({excess[i], residual[*edgei], residual[*edgej]});
						// std::cout << "minimum flow is: " << minimum_flow << "\n";
						excess[i] -= minimum_flow;
						residual[boost::edge(j, i, G_random).first] += minimum_flow;
						residual[*edgei] -= minimum_flow;

						residual[boost::edge(k, j, G_random).first] += minimum_flow;
						residual[boost::edge(j, k, G_random).first] -= minimum_flow;
						excess[k] += minimum_flow;
						if (k != t && k != s)
						{
							overflowQ.push(k);
						}

						// std::cout << "pushed: " << minimum_flow << "\n";
						// excess[j] = 0;
					}
				}
				if (!admissableEdge2)
				{
					// std::cout << "Relabel J\n";

					for (tie(edgej, edgej_end) = out_edges(j, G_random); edgej != edgej_end; edgej++)
					{
						// std::cout << "MinHJ: " << j_minimum_height << " and targetJ: " << height[target(*edgej, G_random)] << "\n";
						if (residual[*edgej] == 0)
							continue;
						if (j_minimum_height > height[target(*edgej, G_random)])
						{
							j_minimum_height = height[target(*edgej, G_random)];
						}
					}
					// std::cout << "The j minimum height is: " << j_minimum_height << " so j's height becomes: " << j_minimum_height + 1 << "\n";
					height[j] = j_minimum_height + 1;
					j_minimum_height = 10000;
				}
			}
		}
		if (!admissableEdge1)
		{
			// std::cout << "Relabel I, the overflower: " << overflower << "\n";
			for (tie(edgei, edgei_end) = out_edges(i, G_random); edgei != edgei_end; edgei++)
			{
				// std::cout << "MinH: " << i_minimum_height << " and target: " << height[target(*edgei, G_random)] << "\n";
				if (residual[*edgei] == 0)
					continue;
				if (i_minimum_height > height[target(*edgei, G_random)])
				{
					i_minimum_height = height[target(*edgei, G_random)];
				}
			}
			// std::cout << "The i minimum height is: " << i_minimum_height << " so i's height becomes: " << i_minimum_height + 1 << "\n";
			height[i] = i_minimum_height + 1;
			i_minimum_height = 10000;
		}
		if (excess[overflower] > 0 && overflower != s && overflower != t)
		{
			overflowQ.push(overflower);
		}

		// //
		if (logging){
			std::cout << "--------------------------------\n";
			write_graphviz(std::cout, G_random, make_label_writer(excess), make_label_writer(residual));
		}
	}
	// std::cout << "Done";
	// write_graphviz(std::cout, G_random, make_label_writer(excess), make_label_writer(residual));

	end = std::chrono::system_clock::now();
	duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
	std::cout << "-------------BOOST-------------\n";
	std::cout << "Duration :" << duration.count() << "\n";
	std::cout << "S is: " << s << "and t is: " << t << "\n";
	std::cout << "Maximum Flow is: " << excess[t] << "\n";
	std::cout << "To visualise the graphs visit: https://dreampuf.github.io/GraphvizOnline \n and paste the dot code which looks like digraph G {...}\n";
	return 0;
}


/********************************************************************************
 * Author:   Gerrit Wellecke
 * Project:  Chimera states in populations of Kuramoto oscillators
 * Date:     March 2020
 *
 * Main function for the project on chimera states. As this project grew
 * gradually I only wrote one main for both systems (explicit and reduced). In
 * order to run the program properly only one of the two integration blocks
 * should be active at once, see below.
 * The integration functions were left here to simplify adaptations of code. All
 * functions that are "set in stone" are outsourced to other files.
 *
 * If no parallelisation is wanted the #pragma's in main can simply be commented
 * out.
 * Further the makefile might need to be adapted, depending on whether or not
 * parallelisation is wanted and what system architecture is used.
 *
 * OUTPUT is written to binary files. Each timestep of pop*osci oscillators is
 * written in sequence using 8-byte double precision floats.
 *******************************************************************************/
#include "functions.hpp"        // functions used in integration() and main()
#include "kuramoto.hpp"         // kuramoto model class
#include "ottantonsen.hpp"      // Ott-Antonsen reduction

#include <boost/numeric/odeint.hpp>
#include <omp.h>

#include <chrono>
#include <cmath>
#include <ctime>
#include <fstream>
#include <iostream>
#include <string>
#include <vector>

using namespace boost::numeric::odeint;

using state_type = std::vector<double>;

/* *************************************************************************** */

// integration of explicit Kuramoto system
void integration(const double omega, const double alpha, const int pop, const int osci,
        std::vector< std::vector<double> > &K, const uint maxIteration, std::string simName,
        const std::vector<double> phase, const std::vector<double> variance, const double dt,
        const Distribution dist, const long seedGiven
        ) {
    // state vector with initial phases
    state_type x;
    x.resize(pop * osci);

    // set initial conditions and return seed of RNG
    long seed {setInitCond(x, pop, osci, variance, phase, dist, seedGiven)};

    // filename w/ given parameters
    std::string fileName {"data/" + simName};

    // fileIO
    std::ofstream output(fileName, std::ios::binary);
    std::ofstream ordParamOut("data/" + simName + ".order", std::ios::binary);

    // initialize stepper
    runge_kutta4<state_type> stepper;

    // initialize system
    Kuramoto kuramoto_func{omega, K, alpha, pop, osci};

    // write log-file
    kuramoto_func.writeLog(simName, maxIteration, dt, seed);

    // write initial time step
    output.write(reinterpret_cast<char*>(x.data()), x.size() * sizeof(double));

    // write initial order parameter
    std::vector<double> ordP {orderParam(x, pop, osci)};
    ordParamOut.write(reinterpret_cast<char*>(ordP.data()), ordP.size() * sizeof(double));

    // integration loop
    uint iteration {0};

    while (iteration < maxIteration) {
        // as t isn't explicitly considered it is just set 0 here
        stepper.do_step(kuramoto_func, x, 0., dt);

        // renormalize phases in state vector
        renorm(x);

        // write time step to file
        output.write(reinterpret_cast<char*>(x.data()), x.size() * sizeof(double));

        // compute order parameter -- write to separate file
        ordP = orderParam(x, pop, osci);
        ordParamOut.write(reinterpret_cast<char*>(ordP.data()), ordP.size() * sizeof(double));

        // iterate integration step
        iteration++;
    }

    output.close();
    ordParamOut.close();
}

// integration of reduced OA system (analogous to integration of Kuramoto sys.)
void integrationOA(const double omega, const double alpha, const int pop, const int osci,
        std::vector< std::vector<double> > &K, const uint maxIteration, std::string simName,
        const double dt, /* const std::string Kname, */ const long seed
        ) {
    // state vector with init. cond.
    state_type x;
    x.resize(3);

    // set initial conditions and get seed
    // uncomment line below if wanting to reproduce initial state of Kuramoto sys.

    // long seed {setInitCondOAfromKuramoto(x, pop, osci, Kname)};

    // if using above line, comment the line below
    setInitCondOA(x, seed);

    // filename w/ given parameters
    std::string fileName {"data/" + simName + ".order"};

    // fileIO
    std::ofstream output(fileName, std::ios::binary);

    // initialize stepper
    runge_kutta4<state_type> stepper;

    // initialize system
    OttAntonsen OA_func{omega, K, alpha, pop, osci};

    // write log-file
    OA_func.writeLog(simName, maxIteration, dt, seed);

    // write initial time step
    output.write(reinterpret_cast<char*>(x.data()), x.size() * sizeof(double));

    // integration loop
    uint iteration {0};

    while (iteration < maxIteration) {
        // as t isn't explicitly considered it is just set 0 here
        stepper.do_step(OA_func, x, 0., dt);

        // renormalize mean phase difference
        x[2] = fmod(x[2], 2*M_PI);
        if (x[2] < 0)
            x[2] += 2*M_PI;

        // write time step
        output.write(reinterpret_cast<char*>(x.data()), x.size() * sizeof(double));

        // iterate integration step
        iteration++;
    }

    output.close();
}

/* *************************************************************************** */

int main(void) {
    // system parameters as needed by Kuramoto and OttAntonsen
    double omega {0.};
    double beta {.01}; // for beta -> 0, P(Chimera) -> 1;
    double alpha {.5 * M_PI - beta};
    int pop {2};
    int osci {128};

    // parameters for initial conditions
    std::vector<double> phase{0., 0.}; // (0, 0) -- in phase
    std::vector<double> variance{.2, M_PI};
    Distribution distribution {Distribution::NORMAL}; // only for explicit case

    // parameters for integration
    uint maxIteration {500000};
    double dt{.1};

    // vectors to iterate over
    std::vector<double> Avec {.05, .15, .20, .40};
    std::vector<long> seedVec {4578, 130547, 1337, 4578};

    std::vector<std::string> Kname {"data/seed_4578_A_0.050000",
        "data/seed_130547_A_0.150000", "data/seed_1337_A_0.200000",
        "data/seed_4578_A_0.400000"};

    // Numerical solution of explicit Kuramoto system
    /*
#pragma omp parallel for
    for (int par = 0; par < static_cast<int>(Avec.size()); par++) {
        // set seed
        long seedGiven {seedVec[par]};

        // coupling matrix
        double A {Avec[par]};
        double mu {.5 + A/2.};
        double nu {.5 - A/2.};

        std::vector< std::vector<double> > K {{mu, nu}, {nu, mu}};

        // name for given integration
        // std::string simName {"seed_" + std::to_string(seedGiven) + "_A_" + std::to_string(A)};
        std::string simName{"OttAntonsen_1"};

        // just benchmarking fun
        auto start {std::chrono::high_resolution_clock::now()};

        // integration of Kuramoto system w/ above setup
        integration(omega, alpha, pop, osci, K, maxIteration, simName,
                    phase, variance, dt, distribution, seedGiven);

        // continue benchmarking fun
        auto stop {std::chrono::high_resolution_clock::now()};

        auto benchmark {std::chrono::duration_cast<std::chrono::microseconds>(stop - start)};

        std::cout.precision(6);
        // std::cout << "Thread " << omp_get_thread_num() << " has finished calculation "
                  // << par + 1 << '\n';
        std::cout << "Execution time: " << std::fixed << benchmark.count() * 1e-6 << "s\n";
    }
    */

    // Numerical solution of Ott-Antonsen reduction
    // set seed
    long seedGiven {std::time(nullptr)};

#pragma omp parallel for
    //for (int par = 0; par < static_cast<int>(Avec.size()); par++) {
    for (int par = 0; par <= 50 ; par++) {
        //seedGiven = seedVec[par];

        // coupling matrix
        //double A {Avec[par]};
        double A {.01 * par};
        double mu {.5 + A/2.};
        double nu {.5 - A/2.};

        std::vector< std::vector<double> > K {{mu, nu}, {nu, mu}};

        // name for given integration
        std::string simName{"OttAntonsen_A" + std::to_string(A)};

        // just benchmarking fun
        auto start {std::chrono::high_resolution_clock::now()};

        integrationOA(omega, alpha, pop, osci, K, maxIteration, simName,
                        dt, /* Kname[par], */ seedGiven);

        // continue benchmarking fun
        auto stop {std::chrono::high_resolution_clock::now()};

        auto benchmark {std::chrono::duration_cast<std::chrono::microseconds>(stop - start)};

        std::cout.precision(6);
        std::cout << "Execution time: " << std::fixed << benchmark.count() * 1e-6 << "s\n";
    }

    return 0;
}


#pragma once

#include <skynet/config.hpp>

#include <Eigen/Core>

namespace skynet{namespace statistics{
	namespace detail{

		pair<Eigen::MatrixXd, Eigen::MatrixXd>	get_missing_data_covariance_sub_matrixes(
			const Eigen::MatrixXd &mat, const std::vector<size_t> &missing_indexes)
		{
			ASSERT(mat.cols() == mat.rows(), "The rols is not equal to cols!.");

			std::vector<size_t> observed_indexes;
			for (int i = 0, j = 0; (i <mat.rows()) && (j < missing_indexes.size()); ++i){
				if (i != missing_indexes[j]){
					observed_indexes.push_back(i);
				}
				else{
					++j;
				}
			}

			Eigen::MatrixXd Sigma_oo(observed_indexes.size(), observed_indexes.size());
			for (int i = 0; i < observed_indexes.size(); ++i){
				for (int j = 0; j < observed_indexes.size(); ++j){
					Sigma_oo(i, j) = mat(observed_indexes[i], observed_indexes[j]);
				}
			}

			Eigen::MatrixXd Sigma_mo(missing_indexes.size(), observed_indexes.size());
			for (int i = 0; i < missing_indexes.size(); ++i){
				for (int j = 0; j < observed_indexes.size(); ++j){
					Sigma_mo(i,j) = mat(missing_indexes[i], observed_indexes[j]);
				}
			}

			return  make_pair(Sigma_oo, Sigma_mo);
		}

		pair<Eigen::VectorXd, Eigen::VectorXd> get_missing_data_means(
			const Eigen::MatrixXd &mat, const std::vector<size_t> &missing_indexes)
		{
			std::vector<size_t> observed_indexes;
			for (int i = 0, j = 0; (i <mat.rows()) && (j < missing_indexes.size()); ++i){
				if (i != missing_indexes[j]){
					observed_indexes.push_back(i);
				}
				else{
					++j;
				}
			}

			Eigen::VectorXd mean_o(observed_indexes.size());
			for (int i = 0; i < observed_indexes.size(); ++i){
				mean_o[i] = mat.row(observed_indexes[i]).mean();
			}

			Eigen::VectorXd mean_m(missing_indexes.size());
			for (int i = 0; i < missing_indexes.size(); ++i){
				mean_m[i] = mat.row(missing_indexes[i]).mean();
			}

			return make_pair(mean_o, mean_m);
		}

		template <typename V>
		Eigen::VectorXd get_observed_values(const V &vec, const std::vector<size_t> &missing_indexes){
			std::vector<size_t> observed_indexes;
			for (int i = 0, j = 0; (i <vec.size()) && (j < missing_indexes.size()); ++i){
				if (i != missing_indexes[j]){
					observed_indexes.push_back(i);
				}
				else{
					++j;
				}
			}

			Eigen::VectorXd observed_values(observed_indexes.size());
			for (int i = 0; i < observed_indexes.size(); ++i){
				observed_values[i] = vec[observed_indexes[i]];
			}

			return observed_values;
		}

	}

	class missing_data_EM{
	public:
		missing_data_EM(const  Eigen::MatrixXd &mat_data, const Eigen::Matrix<byte, -1, -1, 0,-1,-1> &mat_observed)
			: _mat_data(mat_data), _mat_observed(mat_observed)
		{
			ASSERT(mat_data.cols() == mat_observed.cols() && mat_data.rows() == mat_observed.rows(),
				"The size not match!");
			_missing_indexes_vec.resize(mat_data.cols());

			for (int col = 0; col < mat_observed.cols(); ++col){
				for (int row = 0; row < mat_observed.rows(); ++row){
					if (!mat_observed(row, col)){
						_missing_indexes_vec[col].push_back(row);
					}
				}
			}
		}

		Eigen::MatrixXd em(size_t  iter_num){
			for (int iter = 0; iter < iter_num; ++iter){
				refresh_covariance_matrix();
				for (size_t  sample_i = 0; sample_i < _mat_data.cols(); ++sample_i){
					auto missing_indexes = _missing_indexes_vec[sample_i];

					if (missing_indexes.empty()) continue;

					auto sub_matrixes =
						detail::get_missing_data_covariance_sub_matrixes(_mat_covariance, missing_indexes);
					auto means = detail::get_missing_data_means(_mat_data, missing_indexes);

					auto y_o = detail::get_observed_values(_mat_data.col(sample_i), missing_indexes);
					auto y_m = means.second +
						sub_matrixes.second * sub_matrixes.first.inverse() * (y_o - means.first);

					for (int i = 0; i < missing_indexes.size(); ++i){
						_mat_data.col(sample_i)[missing_indexes[i]] = y_m[i];
					}
				}
			}

			return _mat_data;
		}

	protected:

		void refresh_covariance_matrix(){
			_mat_covariance = _mat_data * _mat_data.transpose();
			_mat_covariance /= _mat_covariance.cols();
		}

	private:
		Eigen::MatrixXd								_mat_data;
		Eigen::Matrix<byte, -1, -1, 0, -1, -1>		_mat_observed;
		Eigen::MatrixXd								_mat_covariance;

		std::vector<std::vector<size_t>>	_missing_indexes_vec;
	};

}}