//          Copyright(C) Rishabh Arora 2017.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <vector>
#include <unordered_map>
#include <algorithm>
#include <boost/numeric/ublas/vector.hpp>
#include <variant>
using namespace boost::numeric::ublas;

// supports 3 data types for now
#define cumulative_type std::variant<vector<int>, vector<double>, vector<std::string> >

enum allowed_types {
	INT,
	DOUBLE,
	STRING,
};

class data_frame {
public:

	/// \default constructor: no params
	data_frame() {
		row_ = col_ = 0;
	}
	/// \params :
	///		number of columns
	data_frame(const size_t col) {
		col_ = col;
		header_.resize(col_);
		row_ = 0;
	}

	data_frame(std::vector<std::string> header, std::vector<cumulative_type> data) {
		// assuming equal sized vectors for now
		// set conditions for col and row
		for(size_t i = 0; i < header.size(); i++) {
			header_.push_back(header[i]);
			data_[header[i]] = data[i];
		}
	}

	template <class T>
	const vector<T>& column(std::string header) {
		return std::get< vector < T > >(data_[header]);
	}

	// not working :( : Same thing worked for string version. Fix it fast!!!!
	// const cumulative_type operator[] (const std::string header) const {
	// 	return (const cumulative_type&) data_[header];
	// }
	cumulative_type& operator[] (const std::string header) {
		if (data_.find(header) != data_.end()) {
			return (cumulative_type&) data_[header];
		}
		data_[header] = cumulative_type();
		return (cumulative_type&)data_[header];
	}

private:
	size_t col_, row_;
	std::unordered_map < std::string, cumulative_type > data_;
	std::vector< std::string > header_;
};

int main() {
	vector<int> x(2);
	vector<double> y(2);
	x(0) = 1;
	x(1) = 2;
	y(0) = 1.1;
	y(1) = 2.1;

	// cumulative_type X = x, Y = y;
	// data_frame df({"x", "y"}, {X, Y});
	// df["z"] = y;
	// cumulative_type A = df["z"];
	// vector<double> a = std::get<vector<double> >(A);
	// for(int i = 0; i < (int)a.size(); i++) std::cout << a(i) << ' ';
	const cumulative_type &t = x;
	std::cout << t.index() << std::endl;
	std::variant<int, char> a = 1, b = 2, c;
	c = a + b;
	return 0;
}


/*
 Copyright (C) 2017 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/
#include <boost/make_shared.hpp>
#include <ql/math/interpolations/bilinearinterpolation.hpp>
#include <ql/math/interpolations/linearinterpolation.hpp>
#include <ql/quotes/simplequote.hpp>
#include <ql/termstructures/yield/forwardcurve.hpp>
#include <ql/utilities/dataformatters.hpp>
#include <qle/termstructures/blackvariancesurfacemoneyness.hpp>

using namespace std;

namespace QuantExt {

BlackVarianceSurfaceMoneyness::BlackVarianceSurfaceMoneyness(
    const Calendar& cal, const Handle<Quote>& spot, const std::vector<Time>& times, const std::vector<Real>& moneyness,
    const std::vector<std::vector<Handle<Quote> > >& blackVolMatrix, const DayCounter& dayCounter, bool stickyStrike,
    bool flatExtrapMoneyness)
    : BlackVarianceTermStructure(0, cal, Following, dayCounter), stickyStrike_(stickyStrike), spot_(spot),
      times_(times), moneyness_(moneyness), flatExtrapMoneyness_(flatExtrapMoneyness), quotes_(blackVolMatrix) {
    init();
}

BlackVarianceSurfaceMoneyness::BlackVarianceSurfaceMoneyness(
    const Date& referenceDate, const Calendar& cal, const Handle<Quote>& spot, const std::vector<Time>& times,
    const std::vector<Real>& moneyness, const std::vector<std::vector<Handle<Quote> > >& blackVolMatrix,
    const DayCounter& dayCounter, bool stickyStrike, bool flatExtrapMoneyness)
    : BlackVarianceTermStructure(referenceDate, cal, Following, dayCounter), stickyStrike_(stickyStrike), spot_(spot),
      times_(times), moneyness_(moneyness), flatExtrapMoneyness_(flatExtrapMoneyness), quotes_(blackVolMatrix) {
    init();
}

void BlackVarianceSurfaceMoneyness::update() {
    TermStructure::update();
    LazyObject::update();
}

void BlackVarianceSurfaceMoneyness::performCalculations() const {
    for (Size j = 1; j < variances_.columns(); j++) {
        for (Size i = 0; i < variances_.rows(); i++) {
            Real vol = quotes_[i][j - 1]->value();
            variances_[i][j] = times_[j] * vol * vol;
        }
    }
    varianceSurface_.update();
}

void BlackVarianceSurfaceMoneyness::init() {

    QL_REQUIRE(times_.size() == quotes_.front().size(), "mismatch between times vector and vol matrix colums");
    QL_REQUIRE(moneyness_.size() == quotes_.size(), "mismatch between moneyness vector and vol matrix rows");

    QL_REQUIRE(times_[0] > 0, "The first time must be greater than 0.0.");

    if (stickyStrike_) {
        // we don't want to know if the spot has changed - we take a copy here
        spot_ = Handle<Quote>(boost::make_shared<SimpleQuote>(spot_->value()));
    } else {
        registerWith(spot_);
    }

    // Insert time 0.0 in times_ and initialise variances_ with 0.0.
    times_.insert(times_.begin(), 0.0);
    variances_ = Matrix(moneyness_.size(), times_.size(), 0.0);

    // Check times_ and register with quotes
    for (Size j = 1; j < times_.size(); j++) {

        QL_REQUIRE(times_[j] > times_[j - 1], "Times must be sorted and unique but found that the "
                                                  << io::ordinal(j) << " time, " << times_[j]
                                                  << ", is not greater than the " << io::ordinal(j - 1) << " time, "
                                                  << times_[j - 1] << ".");

        for (Size i = 0; i < moneyness_.size(); i++) {
            registerWith(quotes_[i][j - 1]);
        }
    }

    varianceSurface_ =
        Bilinear().interpolate(times_.begin(), times_.end(), moneyness_.begin(), moneyness_.end(), variances_);

    notifyObservers();
}

Real BlackVarianceSurfaceMoneyness::blackVarianceImpl(Time t, Real strike) const {

    calculate();

    if (t == 0.0)
        return 0.0;

    return blackVarianceMoneyness(t, moneyness(t, strike));
}

Real BlackVarianceSurfaceMoneyness::blackVarianceMoneyness(Time t, Real m) const {
    if (t <= times_.back())
        return varianceSurface_(t, m, true);
    else
        return varianceSurface_(times_.back(), m, true) * t / times_.back();
}

BlackVarianceSurfaceMoneynessSpot::BlackVarianceSurfaceMoneynessSpot(
    const Calendar& cal, const Handle<Quote>& spot, const std::vector<Time>& times, const std::vector<Real>& moneyness,
    const std::vector<std::vector<Handle<Quote> > >& blackVolMatrix, const DayCounter& dayCounter, bool stickyStrike,
    bool flatExtrapMoneyness)
    : BlackVarianceSurfaceMoneyness(cal, spot, times, moneyness, blackVolMatrix, dayCounter, stickyStrike,
                                    flatExtrapMoneyness) {}

BlackVarianceSurfaceMoneynessSpot::BlackVarianceSurfaceMoneynessSpot(
    const Date& referenceDate, const Calendar& cal, const Handle<Quote>& spot, const std::vector<Time>& times,
    const std::vector<Real>& moneyness, const std::vector<std::vector<Handle<Quote> > >& blackVolMatrix,
    const DayCounter& dayCounter, bool stickyStrike, bool flatExtrapMoneyness)
    : BlackVarianceSurfaceMoneyness(referenceDate, cal, spot, times, moneyness, blackVolMatrix, dayCounter,
                                    stickyStrike, flatExtrapMoneyness) {}

Real BlackVarianceSurfaceMoneynessSpot::moneyness(Time, Real strike) const {
    if (strike == Null<Real>() || strike == 0) {
        return 1.0;
    } else {
        Real moneyness = strike / spot_->value();
        if (flatExtrapMoneyness_) {
            if (moneyness < moneyness_.front()) {
                moneyness = moneyness_.front();
            } else if (moneyness > moneyness_.back()) {
                moneyness = moneyness_.back();
            }
        }
        return moneyness;
    }
}

BlackVarianceSurfaceMoneynessForward::BlackVarianceSurfaceMoneynessForward(
    const Calendar& cal, const Handle<Quote>& spot, const std::vector<Time>& times, const std::vector<Real>& moneyness,
    const std::vector<std::vector<Handle<Quote> > >& blackVolMatrix, const DayCounter& dayCounter,
    const Handle<YieldTermStructure>& forTS, const Handle<YieldTermStructure>& domTS, bool stickyStrike,
    bool flatExtrapMoneyness)
    : BlackVarianceSurfaceMoneyness(cal, spot, times, moneyness, blackVolMatrix, dayCounter, stickyStrike,
                                    flatExtrapMoneyness),
      forTS_(forTS), domTS_(domTS) {
    init();
}

BlackVarianceSurfaceMoneynessForward::BlackVarianceSurfaceMoneynessForward(
    const Date& referenceDate, const Calendar& cal, const Handle<Quote>& spot, const std::vector<Time>& times,
    const std::vector<Real>& moneyness, const std::vector<std::vector<Handle<Quote> > >& blackVolMatrix,
    const DayCounter& dayCounter, const Handle<YieldTermStructure>& forTS, const Handle<YieldTermStructure>& domTS,
    bool stickyStrike, bool flatExtrapMoneyness)
    : BlackVarianceSurfaceMoneyness(referenceDate, cal, spot, times, moneyness, blackVolMatrix, dayCounter,
                                    stickyStrike, flatExtrapMoneyness),
      forTS_(forTS), domTS_(domTS) {
    init();
}

void BlackVarianceSurfaceMoneynessForward::init() {

    if (!stickyStrike_) {
        QL_REQUIRE(!forTS_.empty(), "foreign discount curve required for atmf surface");
        QL_REQUIRE(!domTS_.empty(), "domestic discount curve required for atmf surface");
        registerWith(forTS_);
        registerWith(domTS_);
    } else {
        for (Size i = 0; i < times_.size(); i++) {
            Time t = times_[i];
            Real fwd = spot_->value() * forTS_->discount(t) / domTS_->discount(t);
            forwards_.push_back(fwd);
        }
        forwardCurve_ = Linear().interpolate(times_.begin(), times_.end(), forwards_.begin());
    }
}

Real BlackVarianceSurfaceMoneynessForward::moneyness(Time t, Real strike) const {
    Real fwd;
    Real reqMoneyness; // for flat extrapolation
    if (strike == Null<Real>() || strike == 0)
        return 1.0;
    else {
        if (stickyStrike_)
            fwd = forwardCurve_(t, true);
        else
            fwd = spot_->value() * forTS_->discount(t) / domTS_->discount(t);
        reqMoneyness = strike / fwd;
        if (flatExtrapMoneyness_) {
            if ((strike / fwd) < moneyness_.front()) {
                reqMoneyness = moneyness_.front();
            } else if ((strike / fwd) > moneyness_.back()) {
                reqMoneyness = moneyness_.back();
            }
        }
        return reqMoneyness;
    }
}

} // namespace QuantExt


// json parser
#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>
namespace pt = boost::property_tree;

#include "SinglePhononMemoryKernel.h"

SinglePhononMemoryKernel::SinglePhononMemoryKernel(double gamma, double gamma_phon, double omega_phon, double coupling)
  			: gamma(gamma), gamma_phon(gamma_phon), omega_phon(omega_phon), coupling(coupling){}

SinglePhononMemoryKernel::SinglePhononMemoryKernel(const std::string &input_file,
                                     const std::string &section) {
  // Create a root
  pt::ptree root;

  // Load the json file in this ptree
  pt::read_json(input_file, root);

  // check if type is right
  std::string type = root.get<std::string>(section + ".type");assert(type == "lorentz");
  assert(type == "single_phonon");
  // read parameters of the given section from the json file
  this->gamma = root.get<double>(section + ".gamma");
  this->gamma_phon = root.get<double>(section + ".gamma_phon");
  this->omega_phon = root.get<double>(section + ".omega_phon");
  this->coupling = root.get<double>(section + ".coupling");
}

SinglePhononMemoryKernel::SinglePhononMemoryKernel(const std::string &input_file) {
  // Create a root
  pt::ptree root;

  // Load the json file in this ptree
  pt::read_json(input_file, root);

  // check if type is right
  std::string type = root.get<std::string>("MemoryKernel.type");
  assert(type == "single_phonon");

  // read parameters from json file
  this->gamma = root.get<double>("MemoryKernel.gamma");
  this->gamma_phon = root.get<double>("MemoryKernel.gamma_phon");
  this->omega_phon = root.get<double>("MemoryKernel.omega_phon");
  this->coupling = root.get<double>("MemoryKernel.coupling");
}

// return mu(omega) for defined memory kernel
std::complex<double> SinglePhononMemoryKernel::calculate(double omega) const {
  // complex unit
  const std::complex<double> I(0E0, 1E0);

  return this->gamma + this->coupling*this->coupling*pow(this->omega_phon,4)
    /(I*omega*(this->omega_phon*this->omega_phon - omega*omega - I*this->gamma_phon*omega ));
}

void SinglePhononMemoryKernel::print_info(std::ostream &stream) const {
  stream << "# SinglePhonenMemoryKernel\n#\n"
      << "# gamma = " << gamma << "\n"
      << "# gamma_phon = " << gamma_phon << "\n"
      << "# omega_phon = " << omega_phon << "\n"
      << "# coupling = " << coupling << "\n";
}


#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>
#include <pcl/common/centroid.h>
#include <pcl_ros/transforms.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/common/common.h>
#include <boost/make_shared.hpp>
#include <tf_conversions/tf_eigen.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>

// parameters names
const std::string CLOUD_DESCRIPTIONS="point_cloud_descriptions";
const std::string FRAME_ID = "frame_id";

// topics
const std::string POINT_CLOUD_TOPIC = "generated_cloud";

// constants

class GeneratePointCloud
{
	typedef pcl::PointCloud<pcl::PointXYZ> Cloud;
	struct Description
	{
		tf::Vector3 size;
		tf::Transform transform;
		double resolution;
	};

	public:
		GeneratePointCloud()
		{
		}

		bool init()
		{
			XmlRpc::XmlRpcValue list;
			ros::NodeHandle ph("~");

			// loading frame id
			if(!ph.getParam(FRAME_ID,frame_id_))
			{
				ROS_ERROR_STREAM("failed to load frame id");
			}

			// parameter numeric_fiels
			std::map<std::string,double> numeric_fields;
			numeric_fields.insert(std::make_pair("x",0));
			numeric_fields.insert(std::make_pair("y",0));
			numeric_fields.insert(std::make_pair("z",0));
			numeric_fields.insert(std::make_pair("rx",0));
			numeric_fields.insert(std::make_pair("ry",0));
			numeric_fields.insert(std::make_pair("rz",0));
			numeric_fields.insert(std::make_pair("l",0));
			numeric_fields.insert(std::make_pair("w",0));
			numeric_fields.insert(std::make_pair("h",0));
			numeric_fields.insert(std::make_pair("resolution",0));

			// loading point cloud descriptions
			if(ph.getParam(CLOUD_DESCRIPTIONS,list))
			{
				if(list.getType() == XmlRpc::XmlRpcValue::TypeArray)
				{
					for(int j = 0; j < list.size(); j++)
					{

						XmlRpc::XmlRpcValue &entry = list[j];

						// parse numeric fields
						std::map<std::string,double>::iterator i;
						for(i = numeric_fields.begin() ; i != numeric_fields.end(); i++)
						{
							if(entry.hasMember(i->first))
							{
								double val = static_cast<double>(entry[i->first]);
								numeric_fields[i->first] = val;
							}
							else
							{
								ROS_ERROR_STREAM("Point Cloud description entry is missing field '"<<i->first<<"'");
								return false;
							}
						}

						// populating structure
						Description d;
						d.size = tf::Vector3(numeric_fields["l"],numeric_fields["w"],numeric_fields["h"]);
						d.transform = tf::Transform(
								tf::Quaternion(numeric_fields["ry"],numeric_fields["rx"],numeric_fields["rz"]),
								tf::Vector3(numeric_fields["x"],numeric_fields["y"],numeric_fields["z"]));
						d.resolution = numeric_fields["resolution"];

						cloud_descriptions_.push_back(d);
					}

				}
			}

			return true;
		}

		void run()
		{
			ros::NodeHandle nh;
			ros::Publisher cloud_publisher = nh.advertise<sensor_msgs::PointCloud2>(POINT_CLOUD_TOPIC,1);

			if(init())
			{
				genenerate_cloud();

				sensor_msgs::PointCloud2 msg;
				pcl::toROSMsg(full_cloud_,msg);

				ros::Duration loop_duration(0.4f);
				while(ros::ok())
				{
					msg.header.stamp = ros::Time::now() - loop_duration;
					cloud_publisher.publish(msg);
					loop_duration.sleep();

				}
			}

		}

	protected:

		void genenerate_cloud()
		{
			full_cloud_.clear();
			for(unsigned int i = 0; i < cloud_descriptions_.size() ; i++)
			{
				Cloud box;
				Description &desc = cloud_descriptions_[i];
				create_box(desc,box);

				// transforming box
				Eigen::Affine3d eigen3d;
				tf::transformTFToEigen(desc.transform,eigen3d);
				pcl::transformPointCloud(box,box,Eigen::Affine3f(eigen3d));

				// concatenating box
				full_cloud_ +=box;
			}

			full_cloud_.header.frame_id = frame_id_;
		}

		void create_rectangular_patch(tf::Vector3 start,tf::Vector3 end,double res,Cloud& patch)
		{
			int count_x = (end.x() - start.x())/res + 1;
			int count_y = (end.y() - start.y())/res +1;

			pcl::PointXYZ p;
			p.z = 0;
			patch.resize(count_x * count_y);
			int counter = 0;
			for(int i = 0; i < count_x ; i++)
			{
				p.x = start.x() + res*i;
				for(int j = 0 ; j < count_y;j++)
				{
					p.y = start.y() + res*j;
					patch.points[counter] = p;
					counter++;
				}
			}
		}

		void create_box(const Description& desc,Cloud& box_points)
		{
			// face points
			Cloud top,bottom,front,rear,left,right,temp;
			tf::Vector3 start,end;

			// transforms
			tf::Transform t;
			Eigen::Affine3d eigen3d;

			// ================================ create top and bottom patches ===============================
			start = tf::Vector3(-desc.size.x()/2,-desc.size.y()/2,0);
			end = tf::Vector3(desc.size.x()/2,desc.size.y()/2,0);
			create_rectangular_patch(start,end,desc.resolution,temp);

			// transform cloud to top
			t = tf::Transform(tf::Quaternion::getIdentity(),tf::Vector3(0,0,0.5f*desc.size.z()));
			tf::transformTFToEigen(t,eigen3d);
			pcl::transformPointCloud(temp,top,Eigen::Affine3f(eigen3d));

			// transform cloud to bottom
			t = tf::Transform(tf::Quaternion::getIdentity(),tf::Vector3(0,0,0.5f*-desc.size.z()));
			tf::transformTFToEigen(t,eigen3d);
			pcl::transformPointCloud(temp,bottom,Eigen::Affine3f(eigen3d));

			// concatenate
			box_points += top;
			box_points += bottom;

			// ================================ create front and rear patches ===============================
			start = tf::Vector3(-desc.size.x()/2,-desc.size.z()/2,0);
			end = tf::Vector3(desc.size.x()/2,desc.size.z()/2,0);
			create_rectangular_patch(start,end,desc.resolution,temp);

			// transform cloud to front
			t = tf::Transform(tf::Quaternion(tf::Vector3(1,0,0),M_PI_2)
				,tf::Vector3(0,0.5f*desc.size.y(),0));
			tf::transformTFToEigen(t,eigen3d);
			pcl::transformPointCloud(temp,front,Eigen::Affine3f(eigen3d));

			// transform cloud to rear
			t = tf::Transform(tf::Quaternion(tf::Vector3(1,0,0),M_PI_2)
							,tf::Vector3(0,0.5f*-desc.size.y(),0));
			tf::transformTFToEigen(t,eigen3d);
			pcl::transformPointCloud(temp,rear,Eigen::Affine3f(eigen3d));

			box_points += front;
			box_points += rear;

			// ================================ create left and right patches ===============================
			start = tf::Vector3(-desc.size.z()/2,-desc.size.y()/2,0);
			end = tf::Vector3(desc.size.z()/2,desc.size.y()/2,0);
			create_rectangular_patch(start,end,desc.resolution,temp);

			// transform cloud to left
			t = tf::Transform(tf::Quaternion(tf::Vector3(0,1,0),M_PI_2)
				,tf::Vector3(0.5f*desc.size.x(),0,0));
			tf::transformTFToEigen(t,eigen3d);
			pcl::transformPointCloud(temp,left,Eigen::Affine3f(eigen3d));

			// transform cloud to right
			t = tf::Transform(tf::Quaternion(tf::Vector3(0,1,0),M_PI_2)
				,tf::Vector3(0.5f*-desc.size.x(),0,0));
			tf::transformTFToEigen(t,eigen3d);
			pcl::transformPointCloud(temp,right,Eigen::Affine3f(eigen3d));

			box_points += left;
			box_points += right;

		}

	protected:

		std::vector<Description> cloud_descriptions_;
		std::string frame_id_;
		Cloud full_cloud_;
		float resolution_;

};

int main(int argc,char** argv)
{
	ros::init(argc,argv,"generate_point_cloud_node");

	GeneratePointCloud g;
	g.run();
	return 0;
}


#include "gtest/gtest.h"

#include "fixtures.hpp"
#include <boost/tuple/tuple.hpp>

using namespace wali::xfa;

#define NUM_ELEMENTS(array)  (sizeof(array)/sizeof((array)[0]))

static Xfa::Word const words[] = {
    Words().epsilon,
    Words().a,
    Words().aa,
    Words().aaa,
    Words().aaaa,
    Words().a5,
    Words().a6,
    Words().a7,
    Words().a8,
    Words().b,
    Words().bb,
    Words().ab,
    Words().ba
};

static const unsigned num_words = NUM_ELEMENTS(words);

// WARNING: the order of the rows and columns in this table must be
//          consistent with the order of 'words' and 'nwas' below.
#if 0
static bool inclusion_answers_by_language[][num_words] = {
    /*             eps    a      aa     aaa    aaaa   a^5    a^6    a^7    a^8    b      bb     ab     ba */
    /* {}     */ { false, false, false, false, false, false, false, false, false, false, false, false, false },
    /* {eps}  */ { true,  false, false, false, false, false, false, false, false, false, false, false, false },
    /* {a}    */ { false, true,  false, false, false, false, false, false, false, false, false, false, false },
    /* {a^n}  */ { false, true,  true,  true,  true,  true,  true,  true,  true,  false, false, false, false },
    /* {a^2n} */ { false, false, true,  false, true,  false, true,  false, true,  false, false, false, false },
    /* {ab}   */ { false, false, false, false, false, false, false, false, false, false, false, true,  false },
    /* {a,b}  */ { false, true,  false, false, false, false, false, false, false, true,  false, false, false }
};
#endif

static std::string expected_intersection[][7] = {
    /*             {}     {eps}    {a}    {a^n}     {a^2n}    {ab}    {a,b} */
    /* {}     */ { "{}",  "{}",    "{}",  "{}",     "{}",     "{}",   "{}"    },
    /* {eps}  */ { "{}",  "{eps}", "{}",  "{}",     "{}",     "{}",   "{}"    },
    /* {a}    */ { "{}",  "{}",    "{a}", "{a}",    "{}",     "{}",   "{a}"   },
    /* {a^n}  */ { "{}",  "{}",    "{a}", "{a^n}",  "{a^2n}", "{}",   "{a}"   },
    /* {a^2n} */ { "{}",  "{}",    "{}",  "{a^2n}", "{a^2n}", "{}",   "{}"    },
    /* {ab}   */ { "{}",  "{}",    "{}",  "{}",     "{}",     "{ab}", "{}"    },
    /* {a,b}  */ { "{}",  "{}",    "{a}", "{a}",    "{}",     "{}",   "{a,b}" }
};

static bool expected_containment[][7] = {
    /*             {}     {eps}    {a}    {a^n}     {a^2n}    {ab}    {a,b} */
    /* {}     */ { true,  true,    true,  true,     true,     true,   true    },
    /* {eps}  */ { false, true,    false, false,    false,    false,  false   },
    /* {a}    */ { false, false,   true,  true,     false,    false,  true    },
    /* {a^n}  */ { false, false,   false, true,     false,    false,  false   },
    /* {a^2n} */ { false, false,   false, true,     true,     false,  false   },
    /* {ab}   */ { false, false,   false, false,    false,    true,   false   },
    /* {a,b}  */ { false, false,   false, false,    false,    false,  true    }
};

static std::string languages[] = {
    "{}", "{eps}", "{a}", "{a^n}", "{a^2n}", "{ab}", "{a,b}"
};

static std::string get_expected_intersection_language(std::string const & left, std::string const & right) {
    std::string const
        * start = &languages[0],
        * end = start + NUM_ELEMENTS(languages),
        * left_ptr = std::find(start, end, left),
        * right_ptr = std::find(start, end, right);

    assert(left_ptr != end);
    assert(right_ptr != end);

    return expected_intersection[left_ptr - start][right_ptr - start];
}

static bool get_expected_containment(std::string const & left, std::string const & right) {
    std::string const
        * start = &languages[0],
        * end = start + NUM_ELEMENTS(languages),
        * left_ptr = std::find(start, end, left),
        * right_ptr = std::find(start, end, right);

    assert(left_ptr != end);
    assert(right_ptr != end);

    return expected_containment[left_ptr - start][right_ptr - start];
}

static bool xfas_same_modulo_words(Xfa const & left, Xfa const & right) {
    for (unsigned word = 0; word < num_words; ++word) {
        bool in_left = left.isAcceptedWithNonzeroWeight(words[word]);
        bool in_right = right.isAcceptedWithNonzeroWeight(words[word]);
        if (in_left != in_right) {
            std::cerr << "***** XFAs differ at word #" << word << "\n"
                      << "*****     which is " << (in_left ? "" : "not ") << "in the expected answer\n"
                      << "*****     and is " << (in_right ? "" : "not ") << "in the actual answer\n";
            return false;
        }
    }
    return true;
}

namespace wali {
    namespace xfa {

        TEST(wali$xfa$Xfa$helper, epsilonClose)
        {
            XfaContext context;
            Relations lrelations(context.voc, "left");
            Relations rrelations(context.voc, "left");
            sem_elem_t zero = lrelations.false_;

            Xfa lxfa = EmptyByImpossibleDataTransition(zero, lrelations).xfa;
            Xfa rxfa = EmptyByImpossibleDataTransition(zero, rrelations).xfa;

            wali::wfa::WFA::EpsilonCloseCache lcache,rcache;

            lxfa.wfa().epsilonCloseCached(getKey("p"), lcache);
            lxfa.wfa().epsilonCloseCached(getKey("q"), lcache);
            rxfa.wfa().epsilonCloseCached(getKey("p"), rcache);
            rxfa.wfa().epsilonCloseCached(getKey("q"), rcache);
        }

        std::pair<std::map<std::string, std::vector<Xfa> >,
                  std::map<std::string, std::vector<Xfa> > >
        produce_xfas(sem_elem_t zero,
                     Relations const & left_relations,
                     Relations const & right_relations)
        {
            std::map<std::string, std::vector<Xfa>> left_xfas, right_xfas;

            left_xfas["{}"].push_back(TrivialEmptyXfa(zero).xfa);
            right_xfas["{}"].push_back(TrivialEmptyXfa(zero).xfa);
            left_xfas["{eps}"].push_back(AcceptEmptyStringOnly(zero).xfa);
            right_xfas["{eps}"].push_back(AcceptEmptyStringOnly(zero).xfa);
            left_xfas["{}"].push_back(EmptyByMissingTransition(zero).xfa);
            right_xfas["{}"].push_back(EmptyByMissingTransition(zero).xfa);

#define REGISTER_XFA(language, name) \
            left_xfas[language].push_back(name(zero, left_relations).xfa);   \
            right_xfas[language].push_back(name(zero, right_relations).xfa);

            REGISTER_XFA("{}",     EmptyByImpossibleDataTransition);
            REGISTER_XFA("{a}",    SingleSimpleTrans);
            REGISTER_XFA("{a^n}",  BoringCycle);
            REGISTER_XFA("{a^2n}", DataRestrictedCycle);
            REGISTER_XFA("{ab}",   SimpleAB);
            REGISTER_XFA("{ab}",   AEpsilonB);
            REGISTER_XFA("{}",     ImpossibleSequence);
            REGISTER_XFA("{a,b}",  AOrB);
            REGISTER_XFA("{a}",    AlternativePathImpossible);
            REGISTER_XFA("{}",     EmptyByImpossibleDataTransitionEpsilon);
            REGISTER_XFA("{eps}",  SingleSimpleTransEpsilon);
#undef REGISTER_XFA

            return std::make_pair(left_xfas, right_xfas);
        }

        TEST(wali$xfa$Xfa$intersect, Battery)
        {
            XfaContext context;
            Relations left_relations(context.voc, "left");
            Relations right_relations(context.voc, "right");

            sem_elem_t zero = left_relations.false_;
            ASSERT_TRUE(zero->equal(right_relations.false_.get_ptr()));

            std::map<std::string, std::vector<Xfa>> left_xfas, right_xfas;
            boost::tie(left_xfas, right_xfas) = produce_xfas(zero, left_relations, right_relations);

            // Sanity check: every language should be the same.
            for (auto left_map_iter = left_xfas.begin(); left_map_iter != left_xfas.end(); ++left_map_iter) {
                std::string const & left_language = left_map_iter->first;

                auto const & left_some_xfas = left_map_iter->second;
                auto const & right_some_xfas = right_xfas[left_language];

                // Now make sure all XFAs with the given left and right
                // languages, when intersected, give the
                // expected_representative.
                for (auto left_xfa = left_some_xfas.begin(); left_xfa != left_some_xfas.end(); ++left_xfa) {
                    for (auto right_xfa = right_some_xfas.begin(); right_xfa != right_some_xfas.end(); ++right_xfa) {
                        std::stringstream ss;
                        ss << "Current left language: " << left_language << "\n"
                           << "Current XFA in that left language: " << (left_xfa - left_some_xfas.begin()) << "\n"
                           << "Current XFA in that right language: " << (right_xfa - right_some_xfas.begin()) << "\n";
                        SCOPED_TRACE(ss.str());

                        ASSERT_TRUE(xfas_same_modulo_words(*left_xfa, *right_xfa));
                    }
                }
            }

            // Now intersect every XFA in left_xfas with every XFA in
            // right_xfas. Each "every" gets TWO loops. We'll interleave them
            // though.
            for (auto left_map_iter = left_xfas.begin(); left_map_iter != left_xfas.end(); ++left_map_iter) {
                for (auto right_map_iter = right_xfas.begin(); right_map_iter != right_xfas.end(); ++right_map_iter) {
                    std::string const & left_language = left_map_iter->first;
                    std::string const & right_language = right_map_iter->first;

                    auto const & left_some_xfas = left_map_iter->second;
                    auto const & right_some_xfas = right_map_iter->second;

                    std::string const & expected_language = get_expected_intersection_language(left_language, right_language);
                    Xfa const & expected_representative = *(left_xfas[expected_language].begin());

                    // Now make sure all XFAs with the given left and right
                    // languages, when intersected, give the
                    // expected_representative.
                    for (auto left_xfa = left_some_xfas.begin(); left_xfa != left_some_xfas.end(); ++left_xfa) {
                        for (auto right_xfa = right_some_xfas.begin(); right_xfa != right_some_xfas.end(); ++right_xfa) {
                            std::stringstream ss;
                            ss << "Current left language: " << left_language << "\n"
                               << "Current right language: " << right_language << "\n"
                               << "Current XFA in that left language: " << (left_xfa - left_some_xfas.begin()) << "\n"
                               << "Current XFA in that right language: " << (right_xfa - right_some_xfas.begin()) << "\n";
                            SCOPED_TRACE(ss.str());

                            Xfa const & intersection = left_xfa->intersect_conjoin(*right_xfa);
                            ASSERT_TRUE(xfas_same_modulo_words(expected_representative, intersection));
                        }
                    }
                }
            }

        }

        TEST(wali$xfa$Xfa$language_contains, Battery)
        {
            XfaContext context;
            Relations & left_relations = context.left;
            Relations & right_relations = context.right;
            //std::cerr << "left_relations\n";
            //Relations left_relations(context.voc, "left");
            //std::cerr << "right_relations\n";
            //Relations right_relations(context.voc, "right");

            sem_elem_t zero = left_relations.false_;
            ASSERT_TRUE(zero->equal(right_relations.false_.get_ptr()));

            std::map<std::string, std::vector<Xfa>> left_xfas, right_xfas;
            boost::tie(left_xfas, right_xfas) = produce_xfas(zero, left_relations, right_relations);

            // Now intersect every XFA in left_xfas with every XFA in
            // right_xfas. Each "every" gets TWO loops. We'll interleave them
            // though.
            for (auto left_map_iter = left_xfas.begin(); left_map_iter != left_xfas.end(); ++left_map_iter) {
                for (auto right_map_iter = right_xfas.begin(); right_map_iter != right_xfas.end(); ++right_map_iter) {
                    std::string const & left_language = left_map_iter->first;
                    std::string const & right_language = right_map_iter->first;

                    auto const & left_some_xfas = left_map_iter->second;
                    auto const & right_some_xfas = right_map_iter->second;

                    bool expected = get_expected_containment(left_language, right_language);

                    // Now make sure all XFAs with the given left and right
                    // languages, when intersected, give the
                    // expected_representative.
                    for (auto left_xfa = left_some_xfas.begin(); left_xfa != left_some_xfas.end(); ++left_xfa) {
                        for (auto right_xfa = right_some_xfas.begin(); right_xfa != right_some_xfas.end(); ++right_xfa) {
                            std::stringstream ss;
                            ss << "Current left language: " << left_language << "\n"
                               << "Current right language: " << right_language << "\n"
                               << "Current XFA in that left language: " << (left_xfa - left_some_xfas.begin()) << "\n"
                               << "Current XFA in that right language: " << (right_xfa - right_some_xfas.begin()) << "\n";
                            SCOPED_TRACE(ss.str());

                            bool actual = language_contains(*left_xfa, *right_xfa, context.voc);
                            EXPECT_EQ(expected, actual);
                        }
                    }
                }
            }

        }

    }
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2022 Matt Borland. 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_MP_DETAIL_FUNCTIONS_TRUNC_HPP
#define BOOST_MP_DETAIL_FUNCTIONS_TRUNC_HPP

#include <cmath>
#include <limits>
#include <stdexcept>
#include <boost/multiprecision/detail/standalone_config.hpp>
#include <boost/multiprecision/detail/no_exceptions_support.hpp>

#ifdef BOOST_MP_MATH_AVAILABLE
#include <boost/math/special_functions/trunc.hpp>
#endif

namespace boost { namespace multiprecision { namespace detail {

namespace impl {

template <typename T>
inline T trunc BOOST_PREVENT_MACRO_SUBSTITUTION (const T arg)
{
    using std::floor;
    using std::ceil;

    return (arg > 0) ? floor(arg) : ceil(arg);
}

} // namespace impl

#ifdef BOOST_MP_MATH_AVAILABLE

template <typename T>
inline long long lltrunc BOOST_PREVENT_MACRO_SUBSTITUTION (const T arg)
{
    return boost::math::lltrunc(arg);
}

template <typename T>
inline int itrunc BOOST_PREVENT_MACRO_SUBSTITUTION (const T arg)
{
    return boost::math::itrunc(arg);
}

#else

template <typename T>
inline long long lltrunc BOOST_PREVENT_MACRO_SUBSTITUTION (const T arg)
{
    if (arg > LLONG_MAX)
    {
        BOOST_MP_THROW_EXCEPTION(std::domain_error("arg cannot be converted into a long long"));
    }

    return static_cast<long long>(boost::multiprecision::detail::impl::trunc(arg));
}

template <typename T>
inline int itrunc BOOST_PREVENT_MACRO_SUBSTITUTION (const T arg)
{
    if (arg > INT_MAX)
    {
        BOOST_MP_THROW_EXCEPTION(std::domain_error("arg cannot be converted into an int"));
    }

    return static_cast<int>(boost::multiprecision::detail::impl::trunc(arg));
}

#endif

}}} // Namespaces

#endif // BOOST_MP_DETAIL_FUNCTIONS_TRUNC_HPP


//
// Copyright 2016 Pixar
//
// Licensed under the Apache License, Version 2.0 (the "Apache License")
// with the following modification; you may not use this file except in
// compliance with the Apache License and the following modification to it:
// Section 6. Trademarks. is deleted and replaced with:
//
// 6. Trademarks. This License does not grant permission to use the trade
//    names, trademarks, service marks, or product names of the Licensor
//    and its affiliates, except as required to comply with Section 4(c) of
//    the License and to reproduce the content of the NOTICE file.
//
// You may obtain a copy of the Apache License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the Apache License with the above modification is
// distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
// KIND, either express or implied. See the Apache License for the specific
// language governing permissions and limitations under the Apache License.
//
#include <boost/python/def.hpp>

#include "pxr/pxr.h"
#include "pxr/base/tf/pyUtils.h"
#include "pxr/base/tf/pyContainerConversions.h"

#include "pxr/base/gf/math.h"

#include "pxr/base/gf/vec2i.h"
#include "pxr/base/gf/vec3i.h"
#include "pxr/base/gf/vec2f.h"
#include "pxr/base/gf/vec3f.h"
#include "pxr/base/gf/vec4f.h"
#include "pxr/base/gf/vec2d.h"
#include "pxr/base/gf/vec3d.h"
#include "pxr/base/gf/vec4d.h"

using namespace boost::python;
using std::vector;

PXR_NAMESPACE_USING_DIRECTIVE

void wrapMath()
{

    def("IsClose", (bool (*)(double, double, double))GfIsClose);
    def("RadiansToDegrees", GfRadiansToDegrees);
    def("DegreesToRadians", GfDegreesToRadians);

    def("Sqr", GfSqr<double>);
    def("Sqr", GfSqr<int>);

    def("Sqr", GfSqr<GfVec2i>);
    def("Sqr", GfSqr<GfVec3i>);
    def("Sqr", GfSqr<GfVec2f>);
    def("Sqr", GfSqr<GfVec3f>);
    def("Sqr", GfSqr<GfVec4f>);
    def("Sqr", GfSqr<GfVec2d>);
    def("Sqr", GfSqr<GfVec3d>);
    def("Sqr", GfSqr<GfVec4d>);

    def("Sgn", GfSgn<double>);
    def("Sgn", GfSgn<int>);

    def("Sqrt", (double (*)(double))GfSqrt);
    def("Sqrtf", (float (*)(float))GfSqrt,
        "Sqrtf(f) -> float\n\n"
        "f : float\n\n"
        "Use instead of Sqrt() to return the square root of f as a float instead of a double.");

    def("Exp", (double (*)(double))GfExp);
    def("Expf", (float (*)(float))GfExp,
        "Expf(f) -> float\n\n"
        "f : float\n\n"
        "Use instead of Exp() to return the exponent of f as a float instead of a double.");

    def("Log", (double (*)(double))GfLog);
    def("Logf", (float (*)(float))GfLog,
        "Logf(f) -> float\n\n"
        "f : float\n\n"
        "Use instead of Log() to return the logarithm of f as a float instead of a double.");

    def("Floor", (double (*)(double))GfFloor);
    def("Floorf", (float (*)(float))GfFloor,
        "Floorf(f) -> float\n\n"
        "f : float\n\n"
        "Use instead of Floor() to return the floor of f as a float instead of a double.");

    def("Ceil", (double (*)(double))GfCeil);
    def("Ceilf", (float (*)(float))GfCeil,
        "Ceilf(f) -> float\n\n"
        "f : float\n\n"
        "Use instead of Ceil() to return the ceiling of f as a float instead of a double.");

    def("Abs", (double (*)(double))GfAbs);
    def("Absf", (float (*)(float))GfAbs,
        "Absf(f) -> float\n\n"
        "f : float\n\n"
        "Use instead of Abs() to return the absolute value of f as a float instead of a double.");

    def("Round", (double (*)(double))GfRound);
    def("Roundf", (float (*)(float))GfRound,
        "Roundf(f) -> float\n\n"
        "f : float\n\n"
        "Use instead of Round() to return the rounded value of f as a float instead of a double.");

    def("Pow", (double (*)(double, double))GfPow);
    def("Powf", (float (*)(float, float))GfPow,
        "Powf(f) -> float\n\n"
        "f : float\n\n"
        "Use instead of Pow() to return the power of f as a float instead of a double.");

    def("Clamp", (double (*)(double, double, double))GfClamp);
    def("Clampf", (float (*)(float, float, float))GfClamp,
        "Clampf(f) -> float\n\n"
        "f : float\n\n"
        "Use instead of Clamp() to return the clamped value of f as a float instead of a double.");

    def("Mod", (double (*)(double, double))GfMod);
    def("Modf", (float (*)(float, float))GfMod,
        "Modf(f) -> float\n\n"
        "f : float\n\n"
        "Use instead of Mod() to return the modulus of f as a float instead of a double.");

    def("Lerp", GfLerp<double>);
    def("Lerpf", GfLerp<float>,
         "Lerpf(f) -> float\n\n"
        "f : float\n\n"
        "Use instead of Lerp() to return the linear interpolation of f as a float instead of a double.");

    def("Lerp", GfLerp<GfVec2i>);
    def("Lerp", GfLerp<GfVec3i>);
    def("Lerp", GfLerp<GfVec2f>);
    def("Lerp", GfLerp<GfVec3f>);
    def("Lerp", GfLerp<GfVec4f>);
    def("Lerp", GfLerp<GfVec2d>);
    def("Lerp", GfLerp<GfVec3d>);
    def("Lerp", GfLerp<GfVec4d>);

    def("Min", (double (*)(double, double)) GfMin<double>);
    def("Min", (double (*)(double, double, double)) GfMin<double>);
    def("Min", (double (*)(double, double, double, double)) GfMin<double>);
    def("Min", (double (*)(double, double, double, double, double))
        GfMin<double>);
    def("Min", (int (*)(int, int)) GfMin<int>);
    def("Min", (int (*)(int, int, int)) GfMin<int>);
    def("Min", (int (*)(int, int, int, int)) GfMin<int>);
    def("Min", (int (*)(int, int, int, int, int)) GfMin<int>);

    def("Max", (double (*)(double, double)) GfMax<double>);
    def("Max", (double (*)(double, double, double)) GfMax<double>);
    def("Max", (double (*)(double, double, double, double)) GfMax<double>);
    def("Max", (double (*)(double, double, double, double, double))
        GfMax<double>);
    def("Max", (int (*)(int, int)) GfMax<int>);
    def("Max", (int (*)(int, int, int)) GfMax<int>);
    def("Max", (int (*)(int, int, int, int)) GfMax<int>);
    def("Max", (int (*)(int, int, int, int, int)) GfMax<int>);

    def("Dot", (double (*)(double, double)) GfDot);

    TfPyContainerConversions::from_python_sequence< std::vector<int>, TfPyContainerConversions::variable_capacity_policy>();

    TfPyContainerConversions::from_python_sequence< std::vector<unsigned int>, TfPyContainerConversions::variable_capacity_policy>();

    TfPyContainerConversions::from_python_sequence< std::vector<bool>, TfPyContainerConversions::variable_capacity_policy>();

    TfPyContainerConversions::from_python_sequence< std::vector<double>, TfPyContainerConversions::variable_capacity_policy>();

    TfPyContainerConversions::from_python_sequence< std::vector< std::vector<int> >, TfPyContainerConversions::variable_capacity_policy>();

    TfPyContainerConversions::from_python_sequence< std::vector< std::vector<double> >, TfPyContainerConversions::variable_capacity_policy>();

}


#include "rbfcore.h"
#include "utility.h"
#include "Solver.h"
#include <armadillo>
#include <fstream>
#include <limits>
#include <iomanip>
#include <ctime>
#include <chrono>
#include<algorithm>
#include "ImplicitedSurfacing.h"
#include "OctotreeSurfacer.h"

typedef std::chrono::high_resolution_clock Clock;

void RBF_Core::BuildJ(RBF_Paras para) {

    isuse_sparse = para.isusesparse;
    sparse_para = para.sparse_para;
    Hermite_weight_smoothness = para.Hermite_weight_smoothness;
    Hermite_designcurve_weight = para.Hermite_designcurve_weight;
//    handcraft_sigma = para.handcraft_sigma;
//    wDir = para.wDir;
//    wOrt = para.wOrt;
//    wFlip = para.wFlip;
    curMethod = para.Method;

    Set_Actual_Hermite_LSCoef(para.Hermite_ls_weight);
    Set_Actual_User_LSCoef(para.user_lamnbda);
    isNewApprox = true;
    isnewformula = true;

    auto t1 = Clock::now();

    switch (curMethod) {
        case Hermite_UnitNormal:
            Set_Hermite_PredictNormal(pts);
            break;
    }
    auto t2 = Clock::now();
    cout << "Build Time: " << (setup_time = std::chrono::nanoseconds(t2 - t1).count() / 1e9) << endl << endl;
    if (0)BuildCoherentGraph();
}

void RBF_Core::AddPointsAndBuildJnew(vector<double> &newpts) {
    auto t1 = Clock::now();
    cout << "setting J" << endl;

    for (auto p:newpts) {
        pts.push_back(p);
    }

    arma::mat E, F, G;
    arma::mat B, D, A_hat_inv, C, newAinv;

    int k = newpts.size() / 3;
    int n = npt;
    double temp[9];

    {
        B.zeros(4 * n + 4, 4 * k);

        for (int i = 0; i < n; i++) {
            double *pi = pts.data() + i * 3;
            for (int j = 0; j < k; j++) {
                double *pj = newpts.data() + j * 3;
                B(i, j) = Kernal_Function_2p(pi, pj);

                Kernal_Gradient_Function_2p(pi, pj, temp);
                for (int l = 0; l < 3; l++) {
                    B(i, k + j * 3 + l) = temp[l];
                }

                Kernal_Gradient_Function_2p(pj, pi, temp);
                for (int l = 0; l < 3; l++) {
                    B(n + i * 3 + l, j) = temp[l];
                }

                Kernal_Hessian_Function_2p(pi, pj, temp);
                for (int p = 0; p < 3; p++) {
                    for (int q = 0; q < 3; q++) {
                        B(n + i * 3 + p, k + j * 3 + q) = -*(temp + p * 3 + q);
                    }
                }
            }
        }
        for (int i = 0; i < k; i++) {
            double *pi = pts.data() + i * 3;
            B(4 * n + 3, i) = 1;
            for (int j = 0; j < 3; j++) {
                B(4 * n + j, i) = pi[j];
                B(4 * n + j, i * 3 + j) = -1;
            }
        }
    }

    {
        cout << "k:" << k << endl;
        D.zeros(4 * k, 4 * k);
        for (int i = 0; i < k; i++) {
            double *pi = newpts.data() + i * 3;
            for (int j = 0; j < k; j++) {
                double *pj = newpts.data() + j * 3;

                D(i, j) = Kernal_Function_2p(pi, pj);
                Kernal_Gradient_Function_2p(pi, pj, temp);
                for (int l = 0; l < 3; l++) {
                    D(i, k + j * 3 + l) = temp[l];
                }

                Kernal_Gradient_Function_2p(pj, pi, temp);
                for (int l = 0; l < 3; l++) {
                    D(k + i * 3 + l, j) = temp[l];
                }

                Kernal_Hessian_Function_2p(pj, pi, temp);
                for (int p = 0; p < 3; p++) {
                    for (int q = 0; q < 3; q++) {
                        D(k + i * 3 + p, k + j * 3 + q) = -*(temp + p * 3 + q);
                    }
                }
            }
        }

        cout << D.n_cols << " " << D.n_rows << endl;
        for (int i = 0; i < 10; i++) {
            for (int j = 0; j < 10; j++) {
                cout << D(D.n_rows - 10 + i, D.n_rows - 10 + j) << "\t";
            }
            cout << endl;
        }

    }

    cout << det(D) << endl;

//    C = inv(D);
    {
        A_hat_inv.zeros(4 * (n + k) + 4, 4 * (n + k) + 4);
        arma::mat TEMP = inv(D - B.t() * Ainv * B);
        cout << det(TEMP) << endl;
        A_hat_inv.submat(4 * n + 4, 4 * n + 4, 4 * n + 4 + 4 * k - 1, 4 * n + 4 + 4 * k - 1)
                = TEMP;

        TEMP = (-Ainv) * B * TEMP;
        A_hat_inv.submat(0, 4 * n + 4, 4 * n + 4 - 1, 4 * n + 4 + 4 * k - 1)
                = TEMP;
        A_hat_inv.submat(4 * n + 4, 0, 4 * n + 4 + 4 * k - 1, 4 * n + 4 - 1)
                = TEMP.t();
        A_hat_inv.submat(0, 0, 4 * n + 4 - 1, 4 * n + 4 - 1)
                = Ainv - TEMP * B.t() * Ainv;
    }

    newAinv.zeros(4 * (n + k) + 4, 4 * (n + k) + 4);
    vector<vector<int> > submatrix_index = {
            {0,             0,             0,             0,             n,     n},//M00
            {0,             n,             0,             4 * n + 4,     n,     k},//M00
            {n,             n,             4 * n + 4,     4 * n + 4,     k,     k},//M00

            {0,             n + k,         0,             n,             n,     3 * n},//M01
            {0,             n + k + 3 * n, 0,             4 * n + 4 + k, n,     3 * k},//M01
            {n,             n + k,         4 * n + 4,     n,             k,     3 * n},//M01
            {n,             n + k + 3 * n, 4 * n + 4,     4 * n + 4 + k, k,     3 * k},//M01

            {0,             4 * (n + k),   0,             4 * n,         n,     4},//N0
            {n,             4 * (n + k),   4 * n + 4,     4 * n,         k,     4},//N0

            {n + k,         4 * (n + k),   n,             4 * n,         3 * n, 4},//N1
            {n + k + 3 * n, 4 * (n + k),   4 * n + 4 + k, 4 * n,         3 * k, 4},//N1

            {n + k,         n + k,         n,             n,             3 * n, 3 * n},//M11
            {n + k,         n + k + 3 * n, n,             4 * n + 4 + k, 3 * n, 3 * k},//M11
            {n + k + 3 * n, n + k + 3 * n, 4 * n + 4 + k, 4 * n + 4 + k, k,     k},//M11
    };
    for (const auto &v:submatrix_index) {
        newAinv.submat(v[0], v[1], v[0] + v[4] - 1, v[1] + v[5] - 1)
                = A_hat_inv.submat(v[2], v[3], v[2] + v[4] - 1, v[3] + v[5] - 1);
        newAinv.submat(v[1], v[0], v[1] + v[5] - 1, v[0] + v[4] - 1)
                = A_hat_inv.submat(v[3], v[2], v[3] + v[5] - 1, v[2] + v[4] - 1);
    }
    for (int i = 0; i < 10; i++) {
        for (int j = 0; j < 10; j++) {
            cout << setprecision(3) << newAinv(i, j) << " ";
        }
        cout << endl;
    }
    Ainv = newAinv;
    npt = pts.size() / 3;

    Minv = Ainv.submat(0, 0, npt * 4 - 1, npt * 4 - 1);
    Ninv = Ainv.submat(0, npt * 4, (npt) * 4 - 1, (npt + 1) * 4 - 1);

    J = Minv;
    J00 = J.submat(0, 0, npt - 1, npt - 1);
    J01 = J.submat(0, npt, npt - 1, npt * 4 - 1);
    J11 = J.submat(npt, npt, npt * 4 - 1, npt * 4 - 1);

    M.clear();
    N.clear();
    cout << "J11: " << J11.n_cols << endl;

    //Set_Hermite_DesignedCurve();

    Set_User_Lamnda_ToMatrix(User_Lamnbda_inject);

//		arma::vec eigval, ny;
//		arma::mat eigvec;
//		ny = eig_sym( eigval, eigvec, J);
//		cout<<ny<<endl;
    cout << "J: " << J.n_cols << endl;

    cout << "Add solve J total: " << (setK_time = std::chrono::nanoseconds(Clock::now() - t1).count() / 1e9) << endl;
    return;
}

void RBF_Core::InitAndOptNormal(RBF_Paras para) {

    auto t1 = Clock::now();
    curInitMethod = para.InitMethod;
    cout << "Init Method: " << mp_RBF_INITMETHOD[curInitMethod] << endl;
    switch (curInitMethod) {
        case Lamnbda_Search:
            Lambda_Search_GlobalEigen();
            break;

    }
    auto t2 = Clock::now();
    cout << "Init Time: " << (init_time = std::chrono::nanoseconds(t2 - t1).count() / 1e9) << endl << endl;

    mp_RBF_InitNormal[curMethod == HandCraft ? 0 : 1][curInitMethod] = initnormals;

    OptNormal(0);
}

void RBF_Core::OptNormal(int method) {

    cout << "OptNormal" << endl;
    auto t1 = Clock::now();

    switch (curMethod) {
        case Hermite_UnitNormal:
            Opt_Hermite_PredictNormal_UnitNormal();
            break;

    }
    auto t2 = Clock::now();
    cout << "Opt Time: " << (solve_time = std::chrono::nanoseconds(t2 - t1).count() / 1e9) << endl << endl;
    if (method == 0)
        mp_RBF_OptNormal[curMethod == HandCraft ? 0 : 1][curInitMethod] = newnormals;
}

void RBF_Core::Surfacing(int method, int n_voxels_1d) {

    n_evacalls = 0;

    if (method == 0) {
        Surfacer sf;
        surf_time = sf.Surfacing_Implicit(pts, n_voxels_1d, false, RBF_Core::Dist_Function);
        sf.WriteSurface(finalMesh_v, finalMesh_fv);
    } else if (method == 1) {
        //todo: octree surfacing
        double re_time;
        cout << "Implicit Surfacing: " << endl;

        auto t1 = Clock::now();

        OctotreeSurfacer surfacer(pts, [this](tuple<double, double, double> p) -> double {
            return RBF_Core::Dist_Function(get<0>(p), get<1>(p), get<2>(p));
        }, n_voxels_1d);
        surfacer.Surfacing();
        finalMesh_v = surfacer.vertices;
        finalMesh_fv = surfacer.triangleFaces;
        cout << "Implicit Surfacing Done." << endl;
        auto t2 = Clock::now();
        cout << "Total Surfacing time: " << (re_time = std::chrono::nanoseconds(t2 - t1).count() / 1e9) << endl;
        surf_time = re_time;
//        debug_output << "Node count:" << surfacer.face_node[0].size() << endl;
    }
    cout << "n_evacalls: " << n_evacalls << "   ave: " << surf_time / n_evacalls << endl;
//    debug_output << "Surfacing time:" << surf_time << endl;
//    debug_output << "Implicit function calls:" << n_evacalls << endl;
//    debug_output << "Vertices:" << finalMesh_v.size() / 3 << endl;
//    debug_output << "Faces:" << finalMesh_fv.size() / 3 << endl;

}

int RBF_Core::InjectData(vector<double> &pts, RBF_Paras para) {

    vector<int> labels;
    vector<double> normals, tangents;
    vector<uint> edges;

    InjectData(pts, labels, normals, tangents, edges, para);

}

int RBF_Core::InjectData(vector<double> &pts, vector<int> &labels, vector<double> &normals, vector<double> &tangents,
                         vector<uint> &edges, RBF_Paras para) {

    isuse_sparse = para.isusesparse;
    sparse_para = para.sparse_para;
    //isuse_sparse = false;
    this->pts = pts;
    this->labels = labels;
    this->normals = normals;
    this->tangents = tangents;
    this->edges = edges;
    npt = this->pts.size() / 3;
    curMethod = para.Method;
    curInitMethod = para.InitMethod;

    polyDeg = para.polyDeg;
    User_Lambda = para.user_lamnbda;
    rangevalue = para.rangevalue;
    maxvalue = 10000;

    cout << "number of points: " << pts.size() / 3 << endl;
    cout << "normals: " << this->normals.size() << endl;
    sol.Statue = 1;
    Init(para.Kernal);

    SetSigma(para.sigma);

    SetThis();

    return 1;
}

int RBF_Core::ThreeStep(vector<double> &pts, vector<int> &labels, vector<double> &normals, vector<double> &tangents,
                        vector<uint> &edges, RBF_Paras para) {

    InjectData(pts, labels, normals, tangents, edges, para);
    BuildJ(para);
    InitAndOptNormal(para);
    OptNormal(0);

    return 1;
}

int RBF_Core::AllStep(vector<double> &pts, vector<int> &labels, vector<double> &normals, vector<double> &tangents,
                      vector<uint> &edges, RBF_Paras para) {

    InjectData(pts, labels, normals, tangents, edges, para);
    BuildJ(para);
    InitAndOptNormal(para);
    OptNormal(0);
    Surfacing(0, 100);
    return 1;

}

void RBF_Core::BatchInitEnergyTest(vector<double> &pts, vector<int> &labels, vector<double> &normals,
                                   vector<double> &tangents, vector<uint> &edges, RBF_Paras para) {

    InjectData(pts, labels, normals, tangents, edges, para);
    BuildJ(para);
    para.ClusterVisualMethod = 0;//RBF_Init_EMPTY
    for (int i = 0; i < RBF_Init_EMPTY; ++i) {
        para.InitMethod = RBF_InitMethod(i);
        InitAndOptNormal(para);
        OptNormal(0);
        Record();
    }
    Print_Record_Init();
}

vector<double> *RBF_Core::ExportPts() {

    return &pts;

}

vector<double> *RBF_Core::ExportPtsNormal(int normal_type) {

    if (normal_type == 0)return &normals;
    else if (normal_type == 1)return &initnormals;
    else if (normal_type == 2)return &initnormals_uninorm;
    else if (normal_type == 3)return &newnormals;

    return NULL;
}

vector<double> *RBF_Core::ExportInitNormal(int kmethod, RBF_InitMethod init_type) {

    if (mp_RBF_InitNormal[kmethod].find(init_type) != mp_RBF_InitNormal[kmethod].end())
        return &(mp_RBF_InitNormal[kmethod][init_type]);
    else return NULL;
}

vector<double> *RBF_Core::ExportOptNormal(int kmethod, RBF_InitMethod init_type) {

    if (mp_RBF_OptNormal[kmethod].find(init_type) != mp_RBF_OptNormal[kmethod].end())
        return &(mp_RBF_OptNormal[kmethod][init_type]);
    else return NULL;
}

void RBF_Core::Print_Record_Init() {

    cout << "InitMethod" << string(30 - string("InitMethod").size(), ' ') << "InitEn\t\t FinalEn" << endl;
    cout << std::setprecision(8);
    {
        for (int i = 0; i < record_initmethod.size(); ++i) {
            cout << record_initmethod[i] << string(30 - record_initmethod[i].size(), ' ') << record_initenergy[i]
                 << "\t\t" << record_energy[i] << endl;
        }
    }

}


/*    Copyright (c) 2010-2018, 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.
 *
 */

#define BOOST_TEST_MAIN

#include <string>

#include <boost/make_shared.hpp>
#include <boost/test/unit_test.hpp>

#include "Tudat/Mathematics/BasicMathematics/linearAlgebra.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/orbitalElementConversions.h"

#include "Tudat/External/SpiceInterface/spiceInterface.h"
#include "Tudat/Mathematics/NumericalIntegrators/rungeKuttaCoefficients.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/accelerationModel.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/keplerPropagator.h"
#include "Tudat/SimulationSetup/tudatSimulationHeader.h"
#include "Tudat/SimulationSetup/PropagationSetup/dynamicsSimulator.h"

namespace tudat
{

namespace unit_tests
{

//! Using declarations.
using namespace interpolators;
using namespace numerical_integrators;
using namespace spice_interface;
using namespace ephemerides;
using namespace simulation_setup;
using namespace basic_astrodynamics;
using namespace orbital_element_conversions;
using namespace propagators;

BOOST_AUTO_TEST_SUITE( test_multi_arc_dynamics )

BOOST_AUTO_TEST_CASE( testKeplerMultiArcDynamics )
{
    //Load spice kernels.
    spice_interface::loadStandardSpiceKernels( );

    for( unsigned testCase = 0; testCase < 3; testCase++ )
    {
        std::vector< std::string > bodyNames;
        bodyNames.push_back( "Earth" );
        bodyNames.push_back( "Moon" );

        // Specify initial time
        double initialEphemerisTime = 1.0E7;
        double finalEphemerisTime = 2.0E7;
        double maximumTimeStep = 3600.0;
        double buffer = 5.0 * maximumTimeStep;

        // Create bodies needed in simulation
        std::map< std::string, std::shared_ptr< BodySettings > > bodySettings =
                getDefaultBodySettings( bodyNames, initialEphemerisTime - buffer, finalEphemerisTime + buffer );
        std::dynamic_pointer_cast< InterpolatedSpiceEphemerisSettings >( bodySettings[ "Moon" ]->ephemerisSettings )->
                resetFrameOrigin( "Earth" );
        bodySettings[ "Moon" ]->ephemerisSettings->resetMakeMultiArcEphemeris( true );
        bodySettings[ "Earth" ]->ephemerisSettings = std::make_shared< ConstantEphemerisSettings >(
                    Eigen::Vector6d::Zero( ) );

        NamedBodyMap bodyMap = createBodies( bodySettings );

        setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" );

        // Set accelerations between bodies that are to be taken into account.
        SelectedAccelerationMap accelerationMap;
        std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfMoon;
        accelerationsOfMoon[ "Earth" ].push_back( std::make_shared< AccelerationSettings >( central_gravity ) );
        accelerationMap[ "Moon" ] = accelerationsOfMoon;

        std::vector< std::string > bodiesToIntegrate, centralBodies;
        bodiesToIntegrate.push_back( "Moon" );
        centralBodies.push_back( "SSB" );

        std::vector< double > integrationArcStarts, integrationArcEnds;

        double integrationStartTime = initialEphemerisTime + 1.0E4;
        double integrationEndTime = finalEphemerisTime - 1.0E4;
        double arcDuration = 1.0E6;
        double arcOverlap = 1.0E4;

        double currentStartTime = integrationStartTime;
        double currentEndTime = integrationStartTime + arcDuration;

        do
        {
            integrationArcStarts.push_back( currentStartTime );
            integrationArcEnds.push_back( currentEndTime );

            currentStartTime = currentEndTime - arcOverlap;
            currentEndTime = currentStartTime + arcDuration;
        }
        while( currentEndTime < integrationEndTime );

        unsigned int numberOfIntegrationArcs = integrationArcStarts.size( );

        std::vector< Eigen::VectorXd > systemInitialStates;
        std::vector< Eigen::Vector6d > initialKeplerElements;

        systemInitialStates.resize( numberOfIntegrationArcs );
        initialKeplerElements.resize( numberOfIntegrationArcs );

        double earthGravitationalParameter =  bodyMap.at( "Earth" )->getGravityFieldModel( )->getGravitationalParameter( );
        for(  unsigned int j = 0; j < numberOfIntegrationArcs; j++ )
        {
            systemInitialStates[ j ]  = spice_interface::getBodyCartesianStateAtEpoch(
                        bodiesToIntegrate[ 0 ], "Earth", "ECLIPJ2000", "NONE", integrationArcStarts[ j ] );
            initialKeplerElements[ j ] = (
                        orbital_element_conversions::convertCartesianToKeplerianElements(
                            Eigen::Vector6d( systemInitialStates[ j ] ),
                            earthGravitationalParameter ) );
        }

        AccelerationMap accelerationModelMap = createAccelerationModelsMap(
                    bodyMap, accelerationMap, bodiesToIntegrate, centralBodies );

        std::vector< std::shared_ptr< SingleArcPropagatorSettings< double > > > arcPropagationSettingsList;
        for( unsigned int i = 0; i < numberOfIntegrationArcs; i++ )
        {
            arcPropagationSettingsList.push_back(
                        std::make_shared< TranslationalStatePropagatorSettings< double > >
                        ( centralBodies, accelerationModelMap, bodiesToIntegrate,
                          systemInitialStates.at( i ), integrationArcEnds.at( i ) ) );
        }

        // For case 0: test multi-arc estimation with same integration settings for each arc
        if( testCase == 0 )
        {
            std::shared_ptr< IntegratorSettings< > > integratorSettings =
                    std::make_shared< IntegratorSettings< > >
                    ( rungeKutta4, initialEphemerisTime, 120.0 );
            MultiArcDynamicsSimulator< > dynamicsSimulator(
                        bodyMap, integratorSettings, std::make_shared< MultiArcPropagatorSettings< double > >(
                            arcPropagationSettingsList ), integrationArcStarts );
        }
        // For case 1: test multi-arc estimation with different integration settings object for each arc
        else if( testCase == 1 )
        {
            std::vector< std::shared_ptr< IntegratorSettings< > > > integratorSettingsList;
            for( unsigned int i = 0; i < numberOfIntegrationArcs; i++ )
            {
                integratorSettingsList.push_back( std::make_shared< IntegratorSettings< > >
                                                  ( rungeKutta4, integrationArcStarts.at( i ), 120.0 ) );
            }
            MultiArcDynamicsSimulator< > dynamicsSimulator(
                        bodyMap, integratorSettingsList, std::make_shared< MultiArcPropagatorSettings< double > >(
                            arcPropagationSettingsList ) );
        }
        // For case 0: test multi-arc estimation with same integration settings for each arc, and arc initial state interpolated
        // from previous state
        else  if( testCase == 2 )
        {
            std::shared_ptr< IntegratorSettings< > > integratorSettings =
                    std::make_shared< IntegratorSettings< > >
                    ( rungeKutta4, initialEphemerisTime, 120.0 );
            MultiArcDynamicsSimulator< > dynamicsSimulator(
                        bodyMap, integratorSettings, std::make_shared< MultiArcPropagatorSettings< double > >(
                            arcPropagationSettingsList, true ), integrationArcStarts );
        }

        std::shared_ptr< Ephemeris > moonEphemeris = bodyMap.at( "Moon" )->getEphemeris( );

        double testStartTime, testEndTime;
        double testTimeStep = 10000.0;
        double timeBuffer = 1000.0;

        Eigen::Vector6d stateDifference;

        for( unsigned int i = 0; i < numberOfIntegrationArcs ; i++ )
        {
            if( i == 0 )
            {
                testStartTime = integrationArcStarts.at( i ) + timeBuffer;
            }
            else
            {
                testStartTime = integrationArcEnds.at( i - 1 ) + timeBuffer;
            }

            if( i == numberOfIntegrationArcs - 1 )
            {
                testEndTime = integrationArcEnds.at( i ) - timeBuffer;
            }
            else
            {
                testEndTime = integrationArcStarts.at( i + 1 ) - timeBuffer;
            }

            // Check if output corresponds to expected analytical solution
            if( testCase < 2 || i == 0 )
            {
                double currentTestTime = testStartTime;
                while( currentTestTime < testEndTime )
                {
                    stateDifference = ( moonEphemeris->getCartesianState( currentTestTime ) ) -
                            ( orbital_element_conversions::convertKeplerianToCartesianElements(
                                  propagateKeplerOrbit( initialKeplerElements.at( i ), currentTestTime - integrationArcStarts.at( i ),
                                                        earthGravitationalParameter ), earthGravitationalParameter ) );
                    for( int i = 0; i < 3; i++ )
                    {
                        BOOST_CHECK_SMALL( stateDifference( i ), 1.0E-4 );
                        BOOST_CHECK_SMALL( stateDifference( i + 3 ), 1.0E-10 );

                    }
                    currentTestTime += testTimeStep;
                }
            }
            // Check if arc information is properly passed to next arc
            else
            {
                double currentTestTime = testStartTime;
                while( currentTestTime < testEndTime )
                {
                    stateDifference = ( moonEphemeris->getCartesianState( currentTestTime ) ) -
                            ( orbital_element_conversions::convertKeplerianToCartesianElements(
                                  propagateKeplerOrbit( initialKeplerElements.at( 0 ), currentTestTime - integrationArcStarts.at( 0 ),
                                                        earthGravitationalParameter ), earthGravitationalParameter ) );
                    for( int i = 0; i < 3; i++ )
                    {
                        BOOST_CHECK_SMALL( stateDifference( i ), 1.0E-3);
                        BOOST_CHECK_SMALL( stateDifference( i + 3 ), 1.0E-9 );

                    }
                    currentTestTime += testTimeStep;
                }
            }
        }
    }
}

BOOST_AUTO_TEST_SUITE_END( )

}

}


/*-----------------------------------------------------------------------------+
Copyright (c) 2011-2011: Joachim Faulhaber
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
#define BOOST_TEST_MODULE icl::fix_icl_after_thread unit test
#include <libs/icl/test/disable_test_warnings.hpp>
#include "../unit_test_unwarned.hpp"

//#define BOOST_ICL_USE_STATIC_BOUNDED_INTERVALS
#include <boost/icl/interval_map.hpp>
#include <boost/icl/split_interval_map.hpp>
#include <boost/icl/separate_interval_set.hpp>
#include <boost/icl/split_interval_set.hpp>

using namespace std;
using namespace boost;
using namespace unit_test;
using namespace boost::icl;

BOOST_AUTO_TEST_CASE(ticket_5482)
{
    typedef interval_map<int,int,partial_absorber,std::less>    m1_t;
    typedef interval_map<int,int,partial_absorber,std::greater> m2_t;
    m1_t m1;
    m2_t m2;
    m1.insert(make_pair(m1_t::interval_type(1), 20));
    m1.insert(make_pair(m1_t::interval_type(2), 20));
    m1.insert(make_pair(m1_t::interval_type(3), 20));

    m2.insert(make_pair(m2_t::interval_type(1), 20));
    m2.insert(make_pair(m2_t::interval_type(2), 20));
    m2.insert(make_pair(m2_t::interval_type(3), 20));

    BOOST_CHECK_EQUAL(m1.iterative_size(), m2.iterative_size());
    BOOST_CHECK_EQUAL(m1.iterative_size(), 1);
    BOOST_CHECK_EQUAL(m2.iterative_size(), 1);
}

#include <boost/cstdint.hpp>
BOOST_AUTO_TEST_CASE(ticket_5559_Denis)
{
    //Submitted by Denis
    typedef boost::icl::interval_set<boost::uint32_t, std::greater> Set;
    const uint32_t ui32_max = (std::numeric_limits<uint32_t>::max)();

    Set q1( Set::interval_type::closed(ui32_max, 0) );
    Set q5( Set::interval_type::closed(0, 0) );

    BOOST_CHECK_EQUAL(q1, q1+q5);
}


#include <limits>
#include <random>
#include <iostream>
#include <boost/test/unit_test.hpp>
#include "algorithms/data_structures/array/fast_sum_of_matrix.hpp"

BOOST_AUTO_TEST_SUITE(TestFastSumOfMatrices)

BOOST_AUTO_TEST_CASE(small_matrix) {
    std::vector<std::vector<int>> matrix {
        {3, 31, -7}, {-46, -1,  -38}, {-3, 38, -40}, {16, 37, -26}
    };

    Algo::DS::Array::NumMatrix nm(matrix);

    BOOST_CHECK(-45 == nm.sumRegion(0, 2, 1, 2));
}

BOOST_AUTO_TEST_CASE(rand_tests) {
    std::random_device rd;
    std::mt19937 gen(rd());

    std::uniform_int_distribution<size_t> sizeDist(1, 10);
    std::uniform_int_distribution<int> valuesDist(-50, 50);

    for (size_t i = 0; i < 10; ++i) {
        std::vector<std::vector<int>> matrix(sizeDist(gen));
        const size_t colsNum = sizeDist(gen);

        for (size_t r = 0; r < matrix.size(); ++r) {
            matrix[r] = std::vector<int>(colsNum, 0);
            for (size_t c = 0; c < colsNum; ++c) {
                matrix[r][c] = valuesDist(gen);
            }
        }

        Algo::DS::Array::NumMatrix nm(matrix);

        std::uniform_int_distribution<int> rowsDist(
                    0, static_cast<int>(matrix.size() - 1));

        std::uniform_int_distribution<int> colsDist(
                    0, static_cast<int>(colsNum - 1));

        for (size_t j = 0; j < 100; ++j) {
            int r1 = rowsDist(gen);
            int c1 = colsDist(gen);

            std::uniform_int_distribution<int> secondRowDist(
                        r1, static_cast<int>(matrix.size() - 1));

            std::uniform_int_distribution<int> secondColDist(
                        c1, static_cast<int>(colsNum - 1));

            int r2 = secondRowDist(gen);
            int c2 = secondColDist(gen);

            int expected = 0;
            for (size_t rIter = static_cast<size_t>(r1);
                 rIter <= static_cast<size_t>(r2);
                 ++rIter)
            {
                for (size_t cIter = static_cast<size_t>(c1);
                     cIter <= static_cast<size_t>(c2);
                     ++cIter) {
                    expected += matrix[rIter][cIter];
                }

            }

            int result = nm.sumRegion(r1, c1, r2, c2);
            BOOST_CHECK(expected == result);
        }
    }
}

BOOST_AUTO_TEST_SUITE_END()


/*!
@file
Forward declares `boost::hana::symmetric_difference`.

@copyright Louis Dionne 2013-2017
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_HANA_FWD_SYMMETRIC_DIFFERENCE_HPP
#define BOOST_HANA_FWD_SYMMETRIC_DIFFERENCE_HPP

#include <boost/hana/config.hpp>
#include <boost/hana/core/when.hpp>

BOOST_HANA_NAMESPACE_BEGIN
    //! Returns the symmetric set-theoretic difference of two sets.
    //! @relates hana::set
    //!
    //! Given two sets `xs` and `ys`, `symmetric_difference(xs, ys)` is a new
    //! set containing all the elements of `xs` that are not contained in `ys`,
    //! and all the elements of `ys` that are not contained in `xs`. The
    //! symmetric difference of two sets satisfies the following:
    //! @code
    //!     symmetric_difference(xs, ys) == union_(difference(xs, ys), difference(ys, xs))
    //! @endcode
    //!
    //!
    //! @param xs, ys
    //! Two sets to compute the symmetric difference of.
    //!
    //!
    //! Example
    //! -------
    //! @include example/symmetric_difference.cpp
#ifdef BOOST_HANA_DOXYGEN_INVOKED
    constexpr auto symmetric_difference = [](auto&& xs, auto&& ys) {
        return tag-dispatched;
    };
#else
    template <typename S, typename = void>
    struct symmetric_difference_impl : symmetric_difference_impl<S, when<true>> { };

    struct symmetric_difference_t {
        template <typename Xs, typename Ys>
        constexpr auto operator()(Xs&& xs, Ys&& ys) const;
    };

    constexpr symmetric_difference_t symmetric_difference{};
#endif
BOOST_HANA_NAMESPACE_END

#endif // !BOOST_HANA_FWD_SYMMETRIC_DIFFERENCE_HPP


/**
 *  @file    FEProblem.hpp
 *  @brief   Defines basis functions.
 *  @author  Francois Roy
 *  @date    01/06/2020
 */
#ifndef FEPROBLEM_H
#define FEPROBLEM_H

#include <vector>
#include <cmath>
#include <math.h>
#include <Eigen/Core>
#include "spdlog/spdlog.h"
#include "FEMesh.hpp"

namespace numerical {
namespace fem {
/**
* Defines the finite element problem over a line.
*
* This class only define the 1D diffusion problem with Dirichlet/Neumann
* boundary conditions and heterogenous diffusion coefficient (for now).
*
* Support for hyperbolic equations (wave and convection diffusion) will be
* added later.
*/
template <typename T>
class FEProblem {  // TODO inherit from common/Problem
typedef Eigen::Matrix<T, Eigen::Dynamic, 1> Vec;
typedef std::vector<Eigen::Matrix<T, 3, 1>> Coord;
// function pointer to member functions
typedef  T (FEProblem<T>::*fctptr)(T x1, T x2, T time);
private:
  Vec m_t;
  std::vector<T> m_dx;
  T m_dt;
protected:
  FEMesh<T>* m_mesh;  // mesh
  // boundary types (0=Dirichlet, 1=Neumann) for left and right boundaries.
  int m_bc_types[2];
public:
  FEProblem():
    m_bc_types{0, 0} {
	// define mesh
	// m_mesh = new Mesh<T>();
  }
  virtual ~FEProblem(){
    // delete m_mesh;
  }
  /**
  *  Domain LHS (bilinear form)
  */
  virtual void dlhs(T e, T phi, T r, T s, T X, T x, T h){

  }
  /**
  *  Domain RHS (linear form)
  */
  virtual void drhs(T e, T phi, T r, T X, T x, T h){

  }
  /**
  *  Boundary RHS (bilinear form)
  */
  virtual void blhs(T e, T phi, T r, T s, T X, T x, T h){

  }
  /**
  *  Boundary LHS (linear form)
  */
  virtual void brhs(T e, T phi, T r, T X, T x, T h){

  }
  /**
  *  Get the reference solution at a specified mesh location.
  *
  * @param x The \f$x\f$-, \f$y\f$-, and \f$z\f$-coordinates of the mesh
  *    node.
  * @param t The discrete time.
  * @return The reference solution at a specified mesh location.
  */
  virtual T reference(const Eigen::Matrix<T, 3, 1>& x, T t){
    return 0.0;
  }
  /**
  * User defined function: right boundary.
  *
  * Right boundary value, i.e. for x = lengths[0][1].
  *
  * @param u The state variable.
  * @param t The discrete time.
  * @return The boundary value at a specified mesh location.
  */
  virtual T right(T u, T t){
    return 0.0;
  }
  /**
  * User defined function: left boundary.
  *
  * Left boundary value, i.e. for x = lengths[0][1].
  *
  * @param u The state variable.
  * @param t The discrete time.
  * @return The boundary value at a specified mesh location.
  */
  virtual T left(T u, T t){
    return 0.0;
  }
  /**
  * User defined function: source term.
  *
  * @param x The \f$x\f$-, \f$y\f$-, and \f$z\f$-coordinates of the mesh
  *    node.
  * @param t The discrete time.
  * @param u The state variable for nonlinear problems.
  * @return The source term at a specified mesh location.
  */
  virtual T source(const Eigen::Matrix<T, 3, 1>& x, T t, T u=0){
    return 0.0;
  }
};

}  // namespace fem
} // namespace numerical

#endif  // FEMPROBLEM_H


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/maybe.hpp>
using namespace boost::hana;

int main() {
    //! [main]
    BOOST_HANA_CONSTEXPR_LAMBDA auto f = [](auto x) {
        return x + 1; // next letter
    };

    BOOST_HANA_CONSTEXPR_ASSERT(ap(just(f), just('x')) == just(f('x')));
    BOOST_HANA_CONSTANT_ASSERT(ap(nothing, just('x')) == nothing);
    BOOST_HANA_CONSTANT_ASSERT(ap(just(f), nothing) == nothing);
    BOOST_HANA_CONSTANT_ASSERT(ap(nothing, nothing) == nothing);
    //! [main]
}


/* Averager.cpp
 * -*- coding: utf-8 -*-
 *
 */

#include <boost/test/unit_test.hpp>

// system includes =============================================================

#include <Eigen/Dense>

// local includes ==============================================================

#include "../src/Averager.hpp"

#include "utils/log.hpp"

BOOST_AUTO_TEST_SUITE(averager_int_test_suite)

BOOST_AUTO_TEST_CASE(one_int_gets_returned) {
    TEST_MESSAGE("one_int_gets_returned");

    Averager<int> uut;
    uut.add(42);
    BOOST_CHECK_EQUAL(uut.get(), 42);
}

BOOST_AUTO_TEST_CASE(multiple_ints_get_averaged) {
    TEST_MESSAGE("multiple_ints_get_averaged");

    Averager<int> uut;
    uut.add(42);
    uut.add(23);
    uut.add(1);
    BOOST_CHECK_EQUAL(uut.get(), 22);
}

BOOST_AUTO_TEST_CASE(multiple_ints_are_floored) {
    TEST_MESSAGE("multiple_ints_are_floored");

    Averager<int> uut;
    uut.add(42);
    uut.add(23);
    BOOST_CHECK_EQUAL(uut.get(), 32);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(averager_double_test_suite)

BOOST_AUTO_TEST_CASE(one_double_gets_returned) {
    TEST_MESSAGE("one_double_gets_returned");

    Averager<double> uut;
    uut.add(42);
    BOOST_CHECK_EQUAL(uut.get(), 42.0);
}

BOOST_AUTO_TEST_CASE(multiple_doubles_get_averaged) {
    TEST_MESSAGE("multiple_doubles_get_averaged");

    Averager<double> uut;
    uut.add(42);
    uut.add(23);
    BOOST_CHECK_EQUAL(uut.get(), 32.5);
}

BOOST_AUTO_TEST_CASE(get_copies_sum_correctly) {
    TEST_MESSAGE("get_copies_sum_correctly");

    Averager<double> uut;
    uut.add(42);
    BOOST_CHECK_EQUAL(uut.get(), 42);
    uut.add(23);
    BOOST_CHECK_EQUAL(uut.get(), 32.5);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(averager_matrix_test_suite)

BOOST_AUTO_TEST_CASE(one_matrix_gets_returned) {
    TEST_MESSAGE("one_matrix_gets_returned");

    Averager<Eigen::Matrix3i> uut;

    Eigen::Matrix3i diagonal(3, 3);
    for (unsigned i = 0; i < diagonal.rows(); ++i) {
        for (unsigned j = 0; j < diagonal.cols(); ++j) {
            if (i == j) {
                diagonal(i, j) = 1;
            } else {
                diagonal(i, j) = 0;
            }
        }
    }

    uut.add(diagonal);

    Eigen::Matrix3i result = uut.get();
    for (unsigned i = 0; i < diagonal.rows(); ++i) {
        for (unsigned j = 0; j < diagonal.cols(); ++j) {
            BOOST_CHECK_EQUAL(result(i, j), diagonal(i, j));
        }
    }
}

BOOST_AUTO_TEST_CASE(two_matrix_get_averaged) {
    TEST_MESSAGE("two_matrix_get_averaged");

    Averager<Eigen::Matrix3i> uut;

    Eigen::Matrix3i m1;
    m1(0, 0) = 1; m1(0, 1) = 0; m1(0, 2) = 0;
    m1(1, 0) = 0; m1(1, 1) = 1; m1(1, 2) = 0;
    m1(2, 0) = 0; m1(2, 1) = 0; m1(2, 2) = 1;

    Eigen::Matrix3i m2;
    m2(0, 0) = 1; m2(0, 1) = 1; m2(0, 2) = 2;
    m2(1, 0) = 0; m2(1, 1) = 3; m2(1, 2) = 0;
    m2(2, 0) = 2; m2(2, 1) = 1; m2(2, 2) = 1;

    Eigen::Matrix3i expected;
    expected(0, 0) = 1; expected(0, 1) = 0; expected(0, 2) = 1;
    expected(1, 0) = 0; expected(1, 1) = 2; expected(1, 2) = 0;
    expected(2, 0) = 1; expected(2, 1) = 0; expected(2, 2) = 1;

    uut.add(m1);
    uut.add(m2);

    Eigen::Matrix3i result = uut.get();

    for (unsigned i = 0; i < result.rows(); ++i) {
        for (unsigned j = 0; j < result.cols(); ++j) {
            BOOST_CHECK_EQUAL(result(i, j), expected(i, j));
        }
    }
}

BOOST_AUTO_TEST_SUITE_END()

// vim: tabstop=8 expandtab shiftwidth=4 softtabstop=4 fenc=utf-8


#pragma once

#include <polyfem/ElementBases.hpp>
#include <polyfem/Mesh2D.hpp>
#include <polyfem/LocalBoundary.hpp>
#include <polyfem/InterfaceData.hpp>
#include <polyfem/Navigation.hpp>

#include <polyfem/MeshNodes.hpp>

#include <Eigen/Dense>
#include <vector>

namespace polyfem
{
	class FEBasis2d
	{
	public:
		///
		/// @brief      Builds FE basis functions over the entire mesh (P1, P2 over triangles, Q1,
		///             Q2 over quads). Polygonal facets with > 4 vertices are dealt later on by the
		///             PolygonalBasis2d class.
		///
		/// @param[in]  mesh               The input planar mesh
		/// @param[in]  quadrature_order   The quadrature order
		/// @param[in]  discr_order        The order of the elements (1-4)
		/// @param[in]  serendipity        Uses serendipity bases or not (only for quads)
		/// @param[in]  has_polys          Does the mesh has polygons, if not the interface mapping is not necessary
		/// @param[in]  is_geom_bases      Flag to decide if build gemetric mapping or normal bases, used to decide if the ndoes are important
		/// @param[out] bases              List of basis functions per element
		/// @param[out] local_boundary     List of descriptor per element, indicating which edge of
		///                                the canonical elements lie on the boundary of the mesh
		/// @param[out] poly_edge_to_data  Data for edges at the interface with a polygon (used to
		///                                build the harmonics inside polygons)
		///
		/// @return     The number of basis functions created.
		///
		static int build_bases(
			const Mesh2D &mesh,
			const int quadrature_order,
			const int discr_order,
			const bool serendipity,
			const bool has_polys,
			const bool is_geom_bases,
			std::vector<ElementBases> &bases,
			std::vector<LocalBoundary> &local_boundary,
			std::map<int, InterfaceData> &poly_edge_to_data);

		///
		/// @brief      Builds FE basis functions over the entire mesh (P1, P2 over triangles, Q1,
		///             Q2 over quads). Polygonal facets with > 4 vertices are dealt later on by the
		///             PolygonalBasis2d class.
		///
		/// @param[in]  mesh               The input planar mesh
		/// @param[in]  quadrature_order   The quadrature order
		/// @param[in]  discr_order        The order for each element
		/// @param[in]  serendipity        Uses serendipity bases or not (only for quads)
		/// @param[in]  has_polys          Does the mesh has polygons, if not the interface mapping is not necessary
		/// @param[in]  is_geom_bases      Flag to decide if build gemetric mapping or normal bases, used to decide if the ndoes are important
		/// @param[out] bases              List of basis functions per element
		/// @param[out] local_boundary     List of descriptor per element, indicating which edge of
		///                                the canonical elements lie on the boundary of the mesh
		/// @param[out] poly_edge_to_data  Data for edges at the interface with a polygon (used to
		///                                build the harmonics inside polygons)
		///
		/// @return     The number of basis functions created.
		///
		static int build_bases(
			const Mesh2D &mesh,
			const int quadrature_order,
			const Eigen::VectorXi &discr_order,
			const bool serendipity,
			const bool has_polys,
			const bool is_geom_bases,
			std::vector<ElementBases> &bases,
			std::vector<LocalBoundary> &local_boundary,
			std::map<int, InterfaceData> &poly_edge_to_data);

		//return the local edge nodes for a tri or a quad of order p, index points to the edge
		static Eigen::VectorXi tri_edge_local_nodes(const int p, const Mesh2D &mesh, Navigation::Index index);
		static Eigen::VectorXi quad_edge_local_nodes(const int q, const Mesh2D &mesh, Navigation::Index index);
	};
} // namespace polyfem


// Copyright 2015-2017 Hans Dembinski
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/core/lightweight_test.hpp>
#include <boost/histogram/detail/limits.hpp>
#include <limits>

using namespace boost::histogram::detail;

int main() {

  BOOST_TEST_EQ(lowest<int>(), (std::numeric_limits<int>::min)());
  BOOST_TEST_EQ(lowest<float>(), -std::numeric_limits<float>::infinity());
  BOOST_TEST_EQ(lowest<double>(), -std::numeric_limits<double>::infinity());

  BOOST_TEST_EQ(highest<int>(), (std::numeric_limits<int>::max)());
  BOOST_TEST_EQ(highest<float>(), std::numeric_limits<float>::infinity());
  BOOST_TEST_EQ(highest<double>(), std::numeric_limits<double>::infinity());

  return boost::report_errors();
}


#pragma once

#include <boost/foreach.hpp>
#include <deque>
#include "modprop/compo/Interfaces.h"

namespace percepto
{

template <typename DataType>
class SumCost
: public Source<DataType>
{
public:

	typedef Source<DataType> SourceType;
	typedef Sink<DataType> SinkType;
	typedef DataType OutputType;

	SumCost() {}

	void AddSource( SourceType* s )
	{
		_sinks.emplace_back( this );
		s->RegisterConsumer( &_sinks.back() );
	}

	// TODO Come up with a better way to do this
	// Using a list allows remove, but makes random sampling hard
	void RemoveOldestSource()
	{
		if( _sinks.size() == 0 ) { return; }
		_sinks.pop_front();
	}

	virtual void Foreprop()
	{
		if( _sinks.size() == 0 ) { return; }
		BOOST_FOREACH( const SinkType& input, _sinks )
		{
			if( !input.IsValid() ) { return; }
		}

		OutputType out = _sinks[0].GetInput();
		for( unsigned int i = 1; i < _sinks.size(); ++i )
		{
			out += _sinks[i].GetInput();
		}
		SourceType::SetOutput( out );
		SourceType::Foreprop();
	}

	virtual void BackpropImplementation( const MatrixType& nextDodx )
	{
		if( _sinks.size() == 0 ) { return; }
		BOOST_FOREACH( SinkType& input, _sinks )
		{
			input.Backprop( nextDodx );
		}
	}

	// TODO Take a comparator as a template parameter
	OutputType ComputeMax() const
	{
		OutputType largest = -std::numeric_limits<OutputType>::infinity();
		BOOST_FOREACH( const SinkType& input, _sinks )
		{
			OutputType out = input.GetInput();
			if( out > largest ) { largest = out; }
		}
		return largest;
	}

protected:

	// Using a deque so that growing does not invalidate pointers and
	// we can efficiently remove items
	std::deque<SinkType> _sinks;

private:

	// Disallow copying b/c sinks can't copy correctly
	SumCost( const SumCost& other );

};

}


#include <binary_tree/binary_tree.h>

#define BOOST_TEST_MODULE Binary_Tree
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(Binary_Tree)

BOOST_AUTO_TEST_CASE(AVL_Tree)
{
    using KEY_TYPE = int;
    binary_tree::tree<KEY_TYPE>  avl_tree;
    constexpr KEY_TYPE FIRST = -1000;
    constexpr KEY_TYPE LAST  = 1000;
    BOOST_REQUIRE(FIRST < LAST);
    BOOST_REQUIRE_EQUAL(avl_tree.size(), 0);
    auto size = avl_tree.size();

    //Insert
    for(auto i = FIRST; i <= LAST; ++i) {
        BOOST_REQUIRE(avl_tree.count(i) == 0);
        BOOST_REQUIRE(avl_tree.insert(i));
        BOOST_REQUIRE(!avl_tree.insert(i));
        BOOST_REQUIRE(avl_tree.count(i) == 1);
        BOOST_REQUIRE_EQUAL(avl_tree.size(), ++size);
        avl_tree.check_height_test([](int hl, int hr){
            BOOST_REQUIRE(std::abs(hl-hr) <= 1);
        });
    }

    //Contains
    BOOST_REQUIRE(avl_tree.count(FIRST - 1) == 0);
    BOOST_REQUIRE(avl_tree.count(LAST  + 1) == 0);
    BOOST_REQUIRE(avl_tree.count(LAST*2) == 0);
    BOOST_REQUIRE(avl_tree.count(FIRST*2) == 0);
    for(auto i = FIRST; i <= LAST ; ++i) {
        BOOST_REQUIRE(avl_tree.count(i) == 1);
    }

    //Erase
    size = avl_tree.size();
    for(auto i = FIRST; i <= LAST; ++i) {
        BOOST_REQUIRE(avl_tree.count(i) == 1);
        BOOST_REQUIRE(avl_tree.erase(i));
        BOOST_REQUIRE(avl_tree.count(i) == 0);
        BOOST_REQUIRE(!avl_tree.erase(i));
        BOOST_REQUIRE_EQUAL(avl_tree.size(), --size);
        avl_tree.check_height_test([](int hl, int hr){
            BOOST_REQUIRE(std::abs(hl-hr) <= 1);
        });
    }
    BOOST_REQUIRE_EQUAL(avl_tree.size(), 0);
}

BOOST_AUTO_TEST_CASE(AVL_Tree_enum) {
    using KEY_TYPE = int;
    binary_tree::tree<KEY_TYPE>  avl_tree;
    constexpr KEY_TYPE FIRST = 0;
    constexpr KEY_TYPE LAST  = 10;
    BOOST_REQUIRE(FIRST < LAST);
    BOOST_REQUIRE_EQUAL(avl_tree.size(), 0);

    for (int i = FIRST; i <= LAST; ++i) {
        avl_tree.insert(i);
    }

   // avl_tree.dump_tree(std::cout);
    avl_tree.enumerate([](auto& v){
        std::cout << v << std::endl;
        return true;
    }, binary_tree::tree<KEY_TYPE>::EnumerationOrder::DESCENDING);
}

BOOST_AUTO_TEST_SUITE_END()


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

    @copyright 2016 NumScale SAS

  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_IS_FLINT_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_IS_FLINT_HPP_INCLUDED

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

  /*!

    @ingroup group-predicates
    This function object returns @ref True or @ref False according x is a flint or not.

    @par Header <boost/simd/function/is_flint.hpp>

    @par Note

    A flint is a 'floating integer' i.e. a floating number
    representing exactly an integer value.

    Be conscious that all sufficiently great floating points values
    are flint and even are even...

    @par Example:

      @snippet is_flint.cpp is_flint

    @par Possible output:

      @snippet is_flint.txt is_flint

  **/
  as_logical_t<Value> is_flint(Value const& x);
} }
#endif

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

#endif