#include <controller_interface/controller.h>
#include <hardware_interface/joint_command_interface.h>
#include <control_toolbox/pid.h>
#include <realtime_tools/realtime_buffer.h>

#include <pluginlib/class_list_macros.h>
#include <std_msgs/Float64MultiArray.h>
#include <angles/angles.h>

#include <urdf/model.h>

#include <kdl/tree.hpp>
#include <kdl/kdl.hpp>
#include <kdl/chain.hpp>
#include <kdl/chaindynparam.hpp>
#include <kdl_parser/kdl_parser.hpp>

#include <boost/scoped_ptr.hpp>

#include "arm_controllers/ControllerJointState.h"

#define PI 3.141592
#define D2R PI/180.0
#define R2D 180.0/PI

namespace arm_controllers{

	class test: public controller_interface::Controller<hardware_interface::EffortJointInterface>
	{
		public:
		~test()
		{
			command_sub_.shutdown();
		}

		bool init(hardware_interface::EffortJointInterface* hw, ros::NodeHandle &n)
  		{
			loop_count_ = 0;
			// List of controlled joints
    		if (!n.getParam("joints", joint_names_))
			{
				ROS_ERROR("Could not find joint name");
				return false;
    		}
			n_joints_ = joint_names_.size();

			if(n_joints_ == 0)
			{
				ROS_ERROR("List of joint names is empty.");
				return false;
			}

			// urdf
			urdf::Model urdf;
			if (!urdf.initParam("robot_description"))
			{
				ROS_ERROR("Failed to parse urdf file");
            	return false;
			}

			// joint handle
			for(int i=0; i<n_joints_; i++)
			{
				try
				{
					joints_.push_back(hw->getHandle(joint_names_[i]));
				}
				catch (const hardware_interface::HardwareInterfaceException& e)
				{
					ROS_ERROR_STREAM("Exception thrown: " << e.what());
					return false;
				}

				urdf::JointConstSharedPtr joint_urdf = urdf.getJoint(joint_names_[i]);
				if (!joint_urdf)
				{
					ROS_ERROR("Could not find joint '%s' in urdf", joint_names_[i].c_str());
                	return false;
				}
				joint_urdfs_.push_back(joint_urdf);
			}

			// kdl parser
			if (!kdl_parser::treeFromUrdfModel(urdf, kdl_tree_)){
				ROS_ERROR("Failed to construct kdl tree");
				return false;
			}

			// kdl chain
			std::string root_name, tip_name;
			if (!n.getParam("root_link", root_name))
			{
				ROS_ERROR("Could not find root link name");
				return false;
    		}
			if (!n.getParam("tip_link", tip_name))
			{
				ROS_ERROR("Could not find tip link name");
				return false;
    		}
			if(!kdl_tree_.getChain(root_name, tip_name, kdl_chain_))
			{
				ROS_ERROR_STREAM("Failed to get KDL chain from tree: ");
				ROS_ERROR_STREAM("  "<<root_name<<" --> "<<tip_name);
				ROS_ERROR_STREAM("  Tree has "<<kdl_tree_.getNrOfJoints()<<" joints");
				ROS_ERROR_STREAM("  Tree has "<<kdl_tree_.getNrOfSegments()<<" segments");
				ROS_ERROR_STREAM("  The segments are:");

				KDL::SegmentMap segment_map = kdl_tree_.getSegments();
            	KDL::SegmentMap::iterator it;

            	for( it=segment_map.begin(); it != segment_map.end(); it++ )
              		ROS_ERROR_STREAM( "    "<<(*it).first);

            	return false;
			}

			gravity_ = KDL::Vector::Zero();
			gravity_(2) = -9.81;
			G_.resize(n_joints_);

			// inverse dynamics solver
			id_solver_.reset( new KDL::ChainDynParam(kdl_chain_, gravity_) );

			// command and state
			tau_cmd_ = Eigen::VectorXd::Zero(n_joints_);
			tau_fric_ = Eigen::VectorXd::Zero(n_joints_);
			q_cmd_.data = Eigen::VectorXd::Zero(n_joints_);
			qdot_cmd_.data = Eigen::VectorXd::Zero(n_joints_);
			qddot_cmd_.data = Eigen::VectorXd::Zero(n_joints_);

			q_.data = Eigen::VectorXd::Zero(n_joints_);
			qdot_.data = Eigen::VectorXd::Zero(n_joints_);

			q_error_ = Eigen::VectorXd::Zero(n_joints_);
			q_error_dot_ = Eigen::VectorXd::Zero(n_joints_);

			// pids
			pids_.resize(n_joints_);
			for (size_t i=0; i<n_joints_; i++)
			{
				// Load PID Controller using gains set on parameter server
				if (!pids_[i].init(ros::NodeHandle(n, "gains/" + joint_names_[i] + "/pid")))
				{
					ROS_ERROR_STREAM("Failed to load PID parameters from " << joint_names_[i] + "/pid");
					return false;
				}
			}

			// command subscriber
			commands_buffer_.writeFromNonRT(std::vector<double>(n_joints_, 0.0));
			command_sub_ = n.subscribe<std_msgs::Float64MultiArray>("command", 1, &test::commandCB, this);

			// Start realtime state publisher
			controller_state_pub_.reset(
				new realtime_tools::RealtimePublisher<arm_controllers::ControllerJointState>(n, "state", 1));

			controller_state_pub_->msg_.header.stamp = ros::Time::now();
			for (size_t i=0; i<n_joints_; i++)
			{
				controller_state_pub_->msg_.name.push_back(joint_names_[i]);
				controller_state_pub_->msg_.command.push_back(0.0);
				controller_state_pub_->msg_.command_dot.push_back(0.0);
				controller_state_pub_->msg_.state.push_back(0.0);
				controller_state_pub_->msg_.state_dot.push_back(0.0);
				controller_state_pub_->msg_.error.push_back(0.0);
				controller_state_pub_->msg_.error_dot.push_back(0.0);
				controller_state_pub_->msg_.effort_command.push_back(0.0);
				controller_state_pub_->msg_.effort_feedforward.push_back(0.0);
				controller_state_pub_->msg_.effort_feedback.push_back(0.0);
			}

   			return true;
  		}

		void starting(const ros::Time& time)
		{
			// get joint positions
			for(size_t i=0; i<n_joints_; i++)
			{
				q_(i) = joints_[i].getPosition();
				qdot_(i) = joints_[i].getVelocity();
			}

			ROS_INFO("Starting Gravity Compensation Controller");
		}

		void commandCB(const std_msgs::Float64MultiArrayConstPtr& msg)
		{
			if(msg->data.size()!=n_joints_)
			{
			ROS_ERROR_STREAM("Dimension of command (" << msg->data.size() << ") does not match number of joints (" << n_joints_ << ")! Not executing!");
			return;
			}
			commands_buffer_.writeFromNonRT(msg->data);
		}

  		void update(const ros::Time& time, const ros::Duration& period)
  		{
			std::vector<double> & commands = *commands_buffer_.readFromRT();
			double dt = period.toSec();
			double q_cmd_old;

			// get joint states
			static double t = 0;
			for (size_t i=0; i<n_joints_; i++)
			{
				q_cmd_old = q_cmd_(i);

				//q_cmd_(i) = commands[i];
				q_cmd_(i) = 45*D2R*sin(PI/2*t);

				enforceJointLimits(q_cmd_(i), i);
				// qdot_cmd_(i) = ( q_cmd_(i) - q_cmd_old )/dt;
				qdot_cmd_(i) = 45*D2R*PI/2*cos(PI/2*t);

				q_(i) = joints_[i].getPosition();
				qdot_(i) = joints_[i].getVelocity();

		        // Compute position error
				if (joint_urdfs_[i]->type == urdf::Joint::REVOLUTE)
				{
					angles::shortest_angular_distance_with_limits(
						q_(i),
						q_cmd_(i),
						joint_urdfs_[i]->limits->lower,
						joint_urdfs_[i]->limits->upper,
						q_error_(i));
				}
				else if (joint_urdfs_[i]->type == urdf::Joint::CONTINUOUS)
				{
					q_error_(i) = angles::shortest_angular_distance(q_(i), q_cmd_(i));
				}
				else // prismatic
				{
					q_error_(i) = q_cmd_(i) - q_(i);
				}
				q_error_dot_(i) = qdot_cmd_(i) - qdot_(i);

				// friction compensation, to do: implement friction observer
				tau_fric_(i) = 1*qdot_(i) + 1*KDL::sign(qdot_(i));
			}

			t += dt;

			// compute gravity torque
			id_solver_->JntToGravity(q_, G_);

			// torque command
			for(int i=0; i<n_joints_; i++)
			{
				//
				tau_cmd_(i) = G_(i) + tau_fric_(i);
				controller_state_pub_->msg_.effort_feedforward[i] = tau_cmd_(i);
				tau_cmd_(i) += pids_[i].computeCommand(q_error_(i), q_error_dot_(i), period);

				// effort saturation
				if (tau_cmd_(i) >= joint_urdfs_[i]->limits->effort)
					tau_cmd_(i) = joint_urdfs_[i]->limits->effort;

				if (tau_cmd_(i) <= -joint_urdfs_[i]->limits->effort)
					tau_cmd_(i) = -joint_urdfs_[i]->limits->effort;

				joints_[i].setCommand( tau_cmd_(i) );
			}

			// publish
			if (loop_count_ % 10 == 0)
			{
				if (controller_state_pub_->trylock())
				{
					controller_state_pub_->msg_.header.stamp = time;
					for(int i=0; i<n_joints_; i++)
					{
						controller_state_pub_->msg_.command[i] = R2D*q_cmd_(i);
						controller_state_pub_->msg_.command_dot[i] = R2D*qdot_cmd_(i);
						controller_state_pub_->msg_.state[i] = R2D*q_(i);
						controller_state_pub_->msg_.state_dot[i] = R2D*qdot_(i);
						controller_state_pub_->msg_.error[i] = R2D*q_error_(i);
						controller_state_pub_->msg_.error_dot[i] = R2D*q_error_dot_(i);
						controller_state_pub_->msg_.effort_command[i] = tau_cmd_(i);

						controller_state_pub_->msg_.effort_feedback[i] = tau_cmd_(i) - G_(i);
					}
					controller_state_pub_->unlockAndPublish();
				}
			}
  		}

  		void stopping(const ros::Time& time) { }

		void enforceJointLimits(double &command, unsigned int index)
		{
			// Check that this joint has applicable limits
			if (joint_urdfs_[index]->type == urdf::Joint::REVOLUTE || joint_urdfs_[index]->type == urdf::Joint::PRISMATIC)
			{
				if( command > joint_urdfs_[index]->limits->upper ) // above upper limnit
				{
					command = joint_urdfs_[index]->limits->upper;
				}
				else if( command < joint_urdfs_[index]->limits->lower ) // below lower limit
				{
					command = joint_urdfs_[index]->limits->lower;
				}
			}
		}
	private:
		int loop_count_;

		// joint handles
		unsigned int n_joints_;
		std::vector<std::string> joint_names_;
  		std::vector<hardware_interface::JointHandle> joints_;
		std::vector<urdf::JointConstSharedPtr> joint_urdfs_;

		// kdl
		KDL::Tree 	kdl_tree_;
		KDL::Chain	kdl_chain_;
		boost::scoped_ptr<KDL::ChainDynParam> id_solver_;	// inverse dynamics solver
		KDL::JntArray G_;									// gravity torque vector
		KDL::Vector gravity_;

		// cmd, state
		realtime_tools::RealtimeBuffer<std::vector<double> > commands_buffer_;
		KDL::JntArray q_cmd_, qdot_cmd_, qddot_cmd_;
		KDL::JntArray q_, qdot_;

		Eigen::VectorXd tau_cmd_, tau_fric_;
		Eigen::VectorXd q_error_, q_error_dot_;

		// gain
		std::vector<control_toolbox::Pid> pids_;       /**< Internal PID controllers. */

		// topic
		ros::Subscriber command_sub_;
		boost::scoped_ptr<
			realtime_tools::RealtimePublisher<
				arm_controllers::ControllerJointState> > controller_state_pub_;
	};

}

PLUGINLIB_EXPORT_CLASS(arm_controllers::test, controller_interface::ControllerBase)


/**********************************************************************************************************************
This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich.
Licensed under the BSD-2 license (see LICENSE file in main directory)
**********************************************************************************************************************/

#include <memory>
#include <array>

#include <Eigen/Dense>
#include <gtest/gtest.h>

#include <ct/rbd/rbd.h>

#include "../../models/testIrb4600/RobCoGenTestIrb4600.h"
#include "ct/rbd/robot/Dynamics.h"
#include "ct/rbd/robot/Kinematics.h"

using namespace ct::rbd;

TEST(DynamicsTestIrb4600, forward_dynamics_test)
{
    std::shared_ptr<TestIrb4600::Kinematics> kyn(new TestIrb4600::Kinematics);

    TestIrb4600::Dynamics testdynamics(kyn);

    using control_vector_t = typename TestIrb4600::Dynamics::control_vector_t;
    using ForceVector_t = typename TestIrb4600::Dynamics::ForceVector_t;
    using JointState_t = typename TestIrb4600::Dynamics::JointState_t;
    using JointAcceleration_t = typename TestIrb4600::Dynamics::JointAcceleration_t;
    using ExtLinkForces_t = typename TestIrb4600::Dynamics::ExtLinkForces_t;

    JointState_t irb_state;
    irb_state.setZero();

    control_vector_t torque_u = control_vector_t::Zero();
    ExtLinkForces_t ext_forces;
    ext_forces = ForceVector_t::Zero();

    JointAcceleration_t irb_xd;

    testdynamics.FixBaseForwardDynamics(irb_state, torque_u, ext_forces, irb_xd);

    JointAcceleration_t qdd;
    testdynamics.FixBaseID(irb_state, qdd, ext_forces, torque_u);
}

int main(int argc, char** argv)
{
    testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}


// Typical includes
#include <iostream>
#include <iomanip>
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <cstdint>
#include <iterator>

// Other includes
#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>
#include <boost/archive/iterators/base64_from_binary.hpp>
#include <boost/archive/iterators/binary_from_base64.hpp>
#include <boost/archive/iterators/transform_width.hpp>
#include <boost/archive/iterators/ostream_iterator.hpp>
#include <boost/archive/iterators/remove_whitespace.hpp>
#include <stdint.h>

// Project includes
#include "LZJD.h"
#include "MurmurHash3.h"

using namespace std;
namespace po = boost::program_options;
namespace fs = boost::filesystem;
namespace bi = boost::archive::iterators;

//Used for handling output to file or STD out
ostream* out_target = NULL;

uint64_t digest_size = 1024;
bool compare;
bool gen_compare;
int32_t threshold;
uint32_t threads;
string alt_output;

static void readAllBytes(char const* filename, vector<char>& result)
{
    ifstream ifs(filename, ios::binary|ios::ate);
    ifstream::pos_type pos = ifs.tellg();

    result.clear();//empty out
    result.resize(pos); //make sure we have enough space
    ifs.seekg(0, ios::beg);
    ifs.read(&result[0], pos);
}

static pair<vector<string>,vector<vector<int32_t>>> readDigest(string path)
{
    vector<string> s_out;
    vector<vector<int32_t>> i_out;
    string line;
    ifstream inFile(path);
    if (inFile.is_open())
    {
        while (getline(inFile, line))
        {
            auto first_colon = line.find(":", 0);
            auto second_colon = line.find(":", first_colon+1);
            string path = line.substr(first_colon+1, second_colon-first_colon-1);
            string base64ints = line.substr(second_colon+1, line.size()-second_colon);
//            cout << path << "  " <<  base64ints;
//            cout << endl;
              // Remove the padding characters
            auto size = base64ints.size();
            while (size > 0 && base64ints[size - 1] == '=')
                size--;
            base64ints = base64ints.substr(0, size);

            //TODO this is not 100% kosher, but C++ is a pain.

            typedef
                bi::transform_width<
                    bi::binary_from_base64<bi::remove_whitespace<string::const_iterator>>,
                        8, 6
                >
                base64_dec;

            vector<uint8_t> int_parts;

            copy(
                 base64_dec(base64ints.cbegin()),
                 base64_dec(base64ints.cend()),
                 std::back_inserter(int_parts)
                 );

            vector<int32_t> decoded_ints(int_parts.size()/4);
            for (int i = 0; i < int_parts.size(); i+=4)
            {
                //big endian extraction of the right value
                int32_t dec_i = (int_parts[i+0]<<24) | (int_parts[i+1]<<16) | (int_parts[i+2]<<8) | (int_parts[i+3]<<0);
                decoded_ints[i/4] = dec_i;
//                cout << dec_i << ", ";
            }
//            cout << ": " << int_parts.size() << " " <<  base64ints.size() << " " << endl;
//            cout << decoded_ints.size() << endl;
            //out.push_back(make_pair(path, decoded_ints));
            i_out.push_back(decoded_ints);
            s_out.push_back(path);

        }
        inFile.close();
        return make_pair(s_out, i_out);
    }
    else
    {
        //throw an error
        return make_pair(s_out, i_out);
    }
}

void compareDigests(pair<vector<string>,vector<vector<int32_t>>>& A, pair<vector<string>,vector<vector<int32_t>>>& B)
{
    vector<string> A_s = A.first;
    vector<string> B_s = B.first;
    vector<vector<int32_t>> A_i = A.second;
    vector<vector<int32_t>> B_i = B.second;

    for (size_t i = 0; i < A_i.size(); i++)
    {
        string hAiN = A_s[i];
        vector<int32_t> hAiH = A_i[i];

        size_t j_start;
        if (&A == &B)
            j_start = i + 1; //don't self compare / repeat comparisons
        else
            j_start = 0;

        for (size_t j = j_start; j < B_i.size(); j++)
        {
            int32_t sim = similarity(hAiH, B_i[j]);
            if (sim >= threshold)
            {
                *out_target << hAiN << "|" << B_s[j] << "|";
                //make the similarity output formated as 00X, 0XX, or 100, depending on value
                if (sim < 100)
                {
                    *out_target << "0";
                    if (sim < 10)
                        *out_target << "0";
                }
                *out_target << sim << "\n";
            }
        }
    }
}

int main(int argc, char* argv[])
{
    bool deep;

	po::options_description desc("LZJD Options");
	desc.add_options()
		("help,h", "produce help message")
        ("deep,r", po::bool_switch(&deep)->default_value(false), "generate SDBFs from directories and files")
        ("compare,c", po::bool_switch(&compare)->default_value(false), "compare SDBFs in file, or two SDBF files")
        ("gen-compare,g", po::bool_switch(&gen_compare)->default_value(false), "compare all pairs in source data")
		("threads,p", po::value<uint32_t>(&threads)->default_value(1), "restrict compute threads to N threads")
        ("threshold,t", po::value<int32_t>(&threshold)->default_value(20), "only show results >=threshold")
        ("output,o", po::value<string>(&alt_output)->default_value(""), "send output to files")
	;

    //give me extra arguments https://stackoverflow.com/questions/10178525/how-to-handle-unsolicited-parameters-in-boostprogram-options
    vector<string> additionalParameters;

    po::variables_map vm;
    try
    {
        po::parsed_options parsed = po::command_line_parser(argc, argv).
            options(desc).allow_unregistered().run();
        po::store(parsed, vm);
        additionalParameters = po::collect_unrecognized(parsed.options,
                                                        po::include_positional);
        po::notify(vm);
	} catch(exception &e) {
		// cerr << e.what() << endl; // Useful for debugging boost::program_options
		cerr << "Invalid options provided." << endl << desc << endl;
		return EXIT_FAILURE;
	}

	if (vm.count("help")) {
		cout << desc << endl;
		return EXIT_FAILURE;
	}

    if(alt_output != "")
        out_target = new ofstream(alt_output);
    else
        out_target = &cout;

    if(compare)
    {
        if(additionalParameters.size() > 2 || additionalParameters.size() == 0)
        {
            cout << "Compare option (-c --compare) requires either one or two inputs, which are paths to digest files. Instead, " << additionalParameters.size() << " inputs were given." << endl;
            return EXIT_FAILURE;
        }

        pair<vector<string>,vector<vector<int32_t>>> digest1 = readDigest(additionalParameters[0]);
        if(additionalParameters.size() == 2)
        {
            pair<vector<string>,vector<vector<int32_t>>> digest2 = readDigest(additionalParameters[1]);
            compareDigests(digest1, digest2);
        }
        else
            compareDigests(digest1, digest1);

        if(out_target != &cout)
        {
            ((ofstream*)out_target)->close();
            delete out_target;
        }
        return EXIT_SUCCESS;
    }

    vector<fs::path> toProcess;
    for (auto input_path : additionalParameters)
    {
        fs::path p(input_path);
        if(!fs::exists(p))
            continue;
        if(fs::is_directory(p))
        {
            if(deep)
            {
                for (fs::recursive_directory_iterator end, dir(p); dir != end; ++dir)
                {
                    if(!fs::is_directory(*dir))
                        toProcess.push_back(*dir);
                }
            }
        }
        else
            toProcess.push_back(p);
    }

    //reused temp space
    vector<char> all_bytes;
    //store digsets IF we do gen-compare
    vector<string> digestNewName;
    vector<vector<int32_t>> digesstNewInts;
    for(fs::path p : toProcess)
    {
        readAllBytes(p.c_str(), all_bytes);

        vector<int32_t> di = digest(digest_size, all_bytes);

        if(gen_compare)
        {
            digesstNewInts.push_back(di);
            digestNewName.push_back(p.generic_string());
        }
        else
        {
            *out_target << "lzjd:" << p.generic_string() << ":";
            out_target->flush();
            //See http://www.boost.org/doc/libs/1_48_0/libs/serialization/doc/dataflow.html for base64 in boost
            std::stringstream os;
            typedef
                bi::base64_from_binary<    // convert binary values to base64 characters
                    bi::transform_width<   // retrieve 6 bit integers from a sequence of 32 bit ints
                        vector<int32_t>::const_iterator,
                        6,
                        32
                    >
                >
                base64_text;

            copy(
                 base64_text(di.cbegin()),
                 base64_text(di.cend()),
                 ostream_iterator<char>(os)
                 );

            *out_target << os.str();
            //add padding to Base64 output
            switch(di.size()*4 % 3)
            {
                case 1:
                    *out_target << "=";
                case 2:
                    *out_target << "=";
                case 0:
                    /*nothing happens case*/;
            }
            *out_target << "\n";
        }
    }

    if(gen_compare)
    {
        pair<vector<string>,vector<vector<int32_t>>> digestNew = make_pair(digestNewName, digesstNewInts);
        compareDigests(digestNew, digestNew);
    }

    if(out_target != &cout)
    {
        ((ofstream*)out_target)->close();
        delete out_target;
    }
	return EXIT_SUCCESS;
}


/*
 * GridTools
 *
 * Copyright (c) 2014-2019, ETH Zurich
 * All rights reserved.
 *
 * Please, refer to the LICENSE file in the root directory.
 * SPDX-License-Identifier: BSD-3-Clause
 */

#include <gridtools/stencil_composition/caches/cache_metafunctions.hpp>
#include <gridtools/stencil_composition/caches/extract_extent_caches.hpp>

#include <boost/fusion/include/pair.hpp>

#include <gtest/gtest.h>

#include <gridtools/common/defs.hpp>
#include <gridtools/stencil_composition/stencil_composition.hpp>
#include <gridtools/tools/backend_select.hpp>

using namespace gridtools;
using namespace execute;

typedef storage_traits<backend::x86>::storage_info_t<0, 2> storage_info_ij_t;
typedef storage_traits<backend::x86>::data_store_t<float_type, storage_info_ij_t> storage_type;

typedef arg<0, storage_type> p_in;
typedef arg<2, storage_type> p_out;
typedef arg<1, storage_type> p_buff;
typedef arg<3, storage_type> p_notin;

struct functor2 {
    typedef accessor<0, intent::in, extent<0, 0, 0, 0, -1, 0>> in;
    typedef accessor<1, intent::inout, extent<0, 0, 0, 0, 0, 1>> out;
    typedef make_param_list<in, out> param_list;

    template <typename Evaluation>
    GT_FUNCTION static void apply(Evaluation &eval);
};

typedef detail::cache_impl<cache_type::ij, p_in, cache_io_policy::fill> cache1_t;
typedef detail::cache_impl<cache_type::ij, p_buff, cache_io_policy::fill> cache2_t;
typedef detail::cache_impl<cache_type::k, p_out, cache_io_policy::local> cache3_t;
typedef detail::cache_impl<cache_type::k, p_notin, cache_io_policy::local> cache4_t;
typedef std::tuple<cache1_t, cache2_t, cache3_t, cache4_t> caches_t;

TEST(cache_metafunctions, get_ij_cache_storage_tuple) {
    using testee_t = get_ij_cache_storage_tuple<caches_t, extent<-2, 2, -3, 2>, 32, 4>::type;

    using expected_t = std::tuple<boost::fusion::pair<p_in, ij_cache_storage<float_type, 36, 9, 2, 3>>,
        boost::fusion::pair<p_buff, ij_cache_storage<float_type, 36, 9, 2, 3>>>;

    static_assert(std::is_same<testee_t, expected_t>::value, "");
}

using esf1k_t = decltype(make_stage<functor2>(p_in(), p_notin()));
using esf2k_t = decltype(make_stage<functor2>(p_notin(), p_out()));

using esfk_sequence_t = meta::list<esf1k_t, esf2k_t>;

static_assert(std::is_same<extract_k_extent_for_cache<p_out, esfk_sequence_t>, extent<0, 0, 0, 0, 0, 1>>(), "");

static_assert(std::is_same<extract_k_extent_for_cache<p_notin, esfk_sequence_t>, extent<0, 0, 0, 0, -1, 1>>(), "");

TEST(cache_metafunctions, get_k_cache_storage_tuple) {

    using testee_t = typename get_k_cache_storage_tuple<caches_t, esfk_sequence_t>::type;

    using expected_t = std::tuple<boost::fusion::pair<p_out, k_cache_storage<p_out, float_type, 0, 1>>,
        boost::fusion::pair<p_notin, k_cache_storage<p_notin, float_type, -1, 1>>>;

    static_assert(std::is_same<testee_t, expected_t>::value, "");
}


//
// Created by kevin on 3/6/17.
//

#ifndef CAFFE_CONV_LAYER_HPP
#define CAFFE_CONV_LAYER_HPP

#include <string>
#include <utility>
#include <vector>

#include <boost/unordered_map.hpp>

#include "caffe/blob.hpp"
#include "caffe/common.hpp"
#include "caffe/layer.hpp"
#include "caffe/proto/caffe.pb.h"
#include "caffe/layers/base_conv_layer.hpp"

namespace caffe {

	template<typename Dtype>
	class ConvolutionLayer : public BaseConvolutionLayer<Dtype> {
	public:
		/**
		 * @param param provides ConvolutionParameter convolution_param,
		 *    with ConvolutionLayer options:
		 *  - num_output. The number of filters.
		 *  - kernel_size / kernel_h / kernel_w. The filter dimensions, given by
		 *  kernel_size for square filters or kernel_h and kernel_w for rectangular
		 *  filters.
		 *  - stride / stride_h / stride_w (\b optional, default 1). The filter
		 *  stride, given by stride_size for equal dimensions or stride_h and stride_w
		 *  for different strides. By default the convolution is dense with stride 1.
		 *  - pad / pad_h / pad_w (\b optional, default 0). The zero-padding for
		 *  convolution, given by pad for equal dimensions or pad_h and pad_w for
		 *  different padding. Input padding is computed implicitly instead of
		 *  actually padding.
		 *  - group (\b optional, default 1). The number of filter groups. Group
		 *  convolution is a method for reducing parameterization by selectively
		 *  connecting input and output channels. The input and output channel dimensions must be divisible
		 *  by the number of groups. For group @f$ \geq 1 @f$, the
		 *  convolutional filters' input and output channels are separated s.t. each
		 *  group takes 1 / group of the input channels and makes 1 / group of the
		 *  output channels. Concretely 4 input channels, 8 output channels, and
		 *  2 groups separate input channels 1-2 and output channels 1-4 into the
		 *  first group and input channels 3-4 and output channels 5-8 into the second
		 *  group.
		 *  - bias_term (\b optional, default true). Whether to have a bias.
		 *  - engine: convolution has CAFFE (matrix multiplication) and CUDNN (library
		 *    kernels + stream parallelism) engines.
		 */
		explicit ConvolutionLayer(const LayerParameter &param)
				: BaseConvolutionLayer<Dtype>(param) {}

		virtual inline const char *type() const { return "Convolution"; }

	protected:
		virtual void Forward_cpu(const vector<Blob < Dtype> *

		>& bottom,
		const vector<Blob < Dtype>*>& top);

		virtual void Forward_gpu(const vector<Blob < Dtype> *

		>& bottom,
		const vector<Blob < Dtype>*>& top);

		virtual void Backward_cpu(const vector<Blob < Dtype> *

		>& top,
		const vector<bool> &propagate_down,
		const vector<Blob < Dtype>*>& bottom);

		virtual void Backward_gpu(const vector<Blob < Dtype> *

		>& top,
		const vector<bool> &propagate_down,
		const vector<Blob < Dtype>*>& bottom);

		virtual inline bool reverse_dimensions() { return false; }

		virtual void compute_output_shape();
	};
}

#endif //CAFFE_CONV_LAYER_HPP


/**
 * @file   DL_MPL.cpp
 * @author Wei Li
 * @date   Oct 2019
 */
#include <algorithm>
#include <vector>
#include <boost/timer/timer.hpp>
#include"DL_MPL.h"

SIMPLEMPL_BEGIN_NAMESPACE

std::vector<std::list<Edge_Simple> >  Read_Graph_File(std::string filename, int & vertex_numbers, int & edge_numbers)
{
	std::ifstream filein(filename.c_str());
	// std::cout << "filename : " << filename << std::endl;
	filein >> vertex_numbers >> edge_numbers; // >> mask_numbers;
	/*
	std::cout << "vertex_numbers : " << vertex_numbers << std::endl;
	std::cout << "edge_numbers : " << edge_numbers << std::endl;
	std::cout << "mask_numbers : " << mask_numbers << std::endl;
	*/

	int source, target;
	std::vector<std::list<Edge_Simple> > edge_list;
	edge_list.resize(vertex_numbers + 1);
	for (int i = 1; i <= edge_numbers; i++)
	{
		filein >> source >> target;
		edge_list[source].push_back(Edge_Simple{ target, i });
		edge_list[target].push_back(Edge_Simple{ source, i });
	}
	filein.close();
	return edge_list;
}

std::vector<std::list<Edge_Simple> >  Read_Stitch_Graph_File(std::string filename, int & vertex_numbers, int & edge_numbers)
{
	std::ifstream filein(filename.c_str());
	int total_vertex_numbers;
	// std::cout << "filename : " << filename << std::endl;
	filein >> total_vertex_numbers; // >> mask_numbers;
	/*
	std::cout << "vertex_numbers : " << vertex_numbers << std::endl;
	std::cout << "edge_numbers : " << edge_numbers << std::endl;
	std::cout << "mask_numbers : " << mask_numbers << std::endl;
	*/

	int source, target;
	double weight;
	std::vector<Vertex*> node_list;
	node_list.resize(total_vertex_numbers);
	vertex_numbers = 0;
	edge_numbers = 0;
	while (filein >> source >> target >> weight){
		//if node source and node target is not created. New one/
		if(!node_list[source]){
			Vertex* source_vertex = new Vertex;
			assert(source_vertex->Conflicts.empty());
			node_list[source] = source_vertex;
			vertex_numbers ++;
		}
		if(!node_list[target]){
			Vertex* target_vertex = new Vertex;
			node_list[target] = target_vertex;
			vertex_numbers ++;
		}

		//if two nodes are stitch relationships and both of them are parent
		if(weight < 0){
			Vertex* parent_vertex = new Vertex;
			assert(parent_vertex->Childs.empty());
			parent_vertex->parentOf(node_list[source]);
			parent_vertex->parentOf(node_list[target]);
			vertex_numbers -= 1;
			continue;
		}
		node_list[source]->Conflicts.insert(node_list[target]);
	}
	filein.close();

	//need to store node list, which does not contain child node
	std::set<Vertex*> node_wo_stitch_list;
	int index = 1;
	for(std::vector<Vertex*>::iterator it = node_list.begin(); it != node_list.end(); ++it) {
		//if the node in parent node, means it has no stitch relations
		if((*it)->Is_Parent){
			node_wo_stitch_list.insert((*it));
			(*it)->updateConflicts();
			(*it)->No = index;
			index ++;
		}
		//else, add its parent node if it has not been added into node_wo_stitch_list
		else{
			assert((*it)->parent->Is_Parent);
			if(node_wo_stitch_list.find((*it)->parent) == node_wo_stitch_list.end()){
				node_wo_stitch_list.insert((*it)->parent);
				(*it)->parent->updateConflicts();
				(*it)->parent->No = index;
				index ++;
			}

		}
	}
    mplAssert(node_wo_stitch_list.size() == (unsigned int)vertex_numbers);

	std::vector<std::list<Edge_Simple> >  edge_list;
	edge_list.resize(vertex_numbers + 1);
	for(std::set<Vertex*>::iterator it = node_wo_stitch_list.begin(); it != node_wo_stitch_list.end(); ++it) {
		assert((*it)->No != 0);
		for(std::set<Vertex*>::iterator itconflict = (*it)->Conflicts_in_LG.begin(); itconflict != (*it)->Conflicts_in_LG.end(); ++itconflict) {
			if((*itconflict)->Is_Parent){
				assert((*itconflict)->No != 0);
				edge_numbers++;
				edge_list[(*it)->No].push_back(Edge_Simple{ (*itconflict)->No, edge_numbers });
				edge_list[(*itconflict)->No].push_back(Edge_Simple{ (*it)->No, edge_numbers });
			}
			else{
				assert((*itconflict)->parent->Is_Parent);
				//avoid repeat (conflicts of stitch realation nodes may represent same conflict)
				edge_numbers++;
				edge_list[(*it)->No].push_back(Edge_Simple{ (*itconflict)->parent->No, edge_numbers });
				edge_list[(*itconflict)->parent->No].push_back(Edge_Simple{ (*it)->No, edge_numbers });
			}
		}
		}
	std::cout<<"EDGE list generated with size: "<<edge_numbers<<std::endl;
	return edge_list;
}

std::vector<int> BFS_Order_no_stitch_first(std::vector<std::list<Edge_Simple> >  & edge_list,DancingLink & dl ){
	std::vector<int> result_vector;
	std::queue<int> intermediate_queue;
	std::set<int> nonexistent;
	std::vector<int> node_degree;
	node_degree.assign(edge_list.size(),0); //edge_list.size() == vertex_number + 1
	//int first_vertex = find_max_degree_node(edge_list);
	calcualte_degree_of_each_node(edge_list,node_degree);
	int max_degree = find_max_degree(edge_list);
	std::vector<int> rows_num;
	rows_num.assign(edge_list.size(),0);
	int max_rows = 0;
	int min_rows = INT_MAX;
	for(auto i = 1; i<(int)edge_list.size();i++){
		rows_num[i] = dl.Col_Header_Table[i].Children_Number;
		if(max_rows < dl.Col_Header_Table[i].Children_Number){max_rows = dl.Col_Header_Table[i].Children_Number;}
		if(min_rows > dl.Col_Header_Table[i].Children_Number){min_rows = dl.Col_Header_Table[i].Children_Number;}
	}
	// std::cout<<"rows_num ";
	// for (auto i = rows_num.begin(); i != rows_num.end(); ++i)
	// 	std::cout << *i << ' ';
	// std::cout<<std::endl;
	//we calculate the case of each node as the root and return the smallest calculation costs
	int smallest_cost = INT_MAX;
	int smallest_cost_node = -1;
	std::vector<int> tmp_result_vector;
	for(auto root_node = 1; root_node < (int)edge_list.size(); root_node++){
		tmp_result_vector.clear();
		nonexistent.clear();
		intermediate_queue.push(root_node);
		tmp_result_vector.push_back(root_node);
		while (!intermediate_queue.empty())
		{
			int next = intermediate_queue.front();
			intermediate_queue.pop();
			if (nonexistent.find(next) == nonexistent.end())
			{
				nonexistent.insert(next);
				tmp_result_vector.push_back(next);
				// for (auto i = edge_list[next].begin(); i != edge_list[next].end(); i++)
				// 	intermediate_queue.push(i->target);
				//push the nodes to the queue by small-degree firstly order
				for(int row_num = min_rows; row_num <= max_rows; row_num++){
					for(int d = 1; d<= max_degree;d++){
						for (auto i = edge_list[next].begin(); i != edge_list[next].end(); i++){
							if(node_degree[i->target] == d && rows_num[i->target] == row_num){
								intermediate_queue.push(i->target);
							}
						}
					}
			}

				continue;
			}
			else
				continue;
		}
		//calculate the cost of each node : calculation function is \sum rows_num[node] * (node_num - index) (largest rows_num, we want to put it into the final location)
		int total_cost = 0;
		for(auto index = 1; index <edge_list.size();index++){
			total_cost += rows_num[tmp_result_vector[index]] * (edge_list.size() - index);
		}
		if(total_cost < smallest_cost){
			result_vector= tmp_result_vector;
			smallest_cost = total_cost;
			smallest_cost_node = root_node;
		}
		// std::cout<<"total_cost " <<total_cost<<std::endl;
		// std::cout<<"TMP RESULT IS"<<std::endl;
		// for (auto i = tmp_result_vector.begin(); i != tmp_result_vector.end(); ++i)
    	// 	std::cout << *i << ' ';
		// std::cout<<std::endl;
	}
	return result_vector;
}
std::vector<int> BFS_Order(std::vector<std::list<Edge_Simple> >  & edge_list)
{
	std::vector<int> result_vector;
	std::queue<int> intermediate_queue;
	std::set<int> nonexistent;
	std::vector<int> node_degree;
	node_degree.assign(edge_list.size(),0); //edge_list.size() == vertex_number + 1
	//int first_vertex = find_max_degree_node(edge_list);
	int first_vertex = calcualte_degree_of_each_node(edge_list,node_degree);
	intermediate_queue.push(first_vertex);
	result_vector.push_back(first_vertex);

	int max_degree = find_max_degree(edge_list);
	while (!intermediate_queue.empty())
	{
		int next = intermediate_queue.front();
		intermediate_queue.pop();
		if (nonexistent.find(next) == nonexistent.end())
		{
			nonexistent.insert(next);
			result_vector.push_back(next);
			// for (auto i = edge_list[next].begin(); i != edge_list[next].end(); i++)
			// 	intermediate_queue.push(i->target);
			//push the nodes to the queue by small-degree firstly order
			for(int d = 1; d<= max_degree;d++){
				for (auto i = edge_list[next].begin(); i != edge_list[next].end(); i++){
					if(node_degree[i->target] == d){
						intermediate_queue.push(i->target);
					}
				}
			}

			continue;
		}
		else
			continue;
	}
	return result_vector;
}

std::vector<int> BFS_Order_max_first(std::vector<std::list<Edge_Simple> >  & edge_list)
{
	std::vector<int> result_vector;
	std::queue<int> intermediate_queue;
	std::set<int> nonexistent;
	std::vector<int> node_degree;
	node_degree.assign(edge_list.size(),0); //edge_list.size() == vertex_number + 1
	int first_vertex = find_max_degree_node(edge_list);
	//int first_vertex = calcualte_degree_of_each_node(edge_list,node_degree);
	intermediate_queue.push(first_vertex);
	result_vector.push_back(first_vertex);
	//int first_vertex = find_max_degree_node(edge_list);
	calcualte_degree_of_each_node(edge_list,node_degree);
	int max_degree = find_max_degree(edge_list);
	while (!intermediate_queue.empty())
	{
		int next = intermediate_queue.front();
		intermediate_queue.pop();
		if (nonexistent.find(next) == nonexistent.end())
		{
			nonexistent.insert(next);
			result_vector.push_back(next);
			for (auto i = edge_list[next].begin(); i != edge_list[next].end(); i++)
				intermediate_queue.push(i->target);
			//push the nodes to the queue by max-degree firstly order

			// for(int d = max_degree; d>= 1;d--){
			// 	for (auto i = edge_list[next].begin(); i != edge_list[next].end(); i++){
			// 		if(node_degree[i->target] == d){
			// 			intermediate_queue.push(i->target);
			// 		}
			// 	}
			// }

			// for(int d = 1; d<= max_degree;d++){
			// 	for (auto i = edge_list[next].begin(); i != edge_list[next].end(); i++){
			// 		if(node_degree[i->target] == d){
			// 			intermediate_queue.push(i->target);
			// 		}
			// 	}
			// }
			continue;
		}
		else
			continue;
	}
	return result_vector;
}
int  find_max_degree_node(std::vector<std::list<Edge_Simple> >  & edge_list){
	int max_degree = 0;
	int vertex_node = 1;
	for ( int it = 1; it < edge_list.size(); ++it){
		if(edge_list[it].size()> max_degree){
			max_degree = edge_list[it].size();
			vertex_node = it;
		}
	}
	return vertex_node;
}

int  find_max_degree(std::vector<std::list<Edge_Simple> >  & edge_list){
	int max_degree = 0;
	int vertex_node = 1;
	for ( int it = 1; it < edge_list.size(); ++it){
		if(edge_list[it].size()> max_degree){
			max_degree = edge_list[it].size();
			vertex_node = it;
		}
	}
	return max_degree;
}

int calcualte_degree_of_each_node(std::vector<std::list<Edge_Simple> >  & edge_list, std::vector<int> & node_degree){
	int min_degree = std::numeric_limits<int>::max();
	int vertex_node = 1;
	for ( int it = 1; it < edge_list.size(); ++it){
		if(edge_list[it].size()< min_degree){
			min_degree = edge_list[it].size();
			vertex_node = it;
		}
		node_degree[it] = edge_list[it].size();
	}
	return vertex_node;
}
std::vector<int> Simple_Order(int size)
{
	std::vector<int> result_vector;
	result_vector.reserve(size);
	result_vector.push_back(1);
	for (int i = 1; i < size; i++)
	{
		result_vector.push_back(i);
	}
	return result_vector;
}

int Sorting_Queue(DancingLink & dl)
{
	int col_no = std::numeric_limits<int>::max();
	int max = std::numeric_limits<int>::max();
	for (auto c = dl.DL_Header.Right; c != &dl.DL_Header; c = c->Right)
	{
		if (c->Children_Number < max)
		{
			max = c->Children_Number;
			col_no = c->Col;
		}
	}
	return col_no;
}

void Convert_to_Exat_Cover(int & row_numbers, int & col_numbers, std::string infilename, std::string Exact_Cover_Filename, bool whether_BFS,
	int & vertex_numbers, int & edge_numbers, int & mask_numbers, std::vector<int> & MPLD_search_vector)
{
	std::ofstream fileout(Exact_Cover_Filename);

	std::vector<std::list<Edge_Simple> >  edge_list = Read_Stitch_Graph_File(infilename, vertex_numbers, edge_numbers); //, mask_numbers);

	if (whether_BFS)
		MPLD_search_vector = BFS_Order(edge_list);
	else
		MPLD_search_vector = Simple_Order(edge_list.size());
	row_numbers = vertex_numbers * mask_numbers + 1;
	col_numbers = edge_numbers * mask_numbers + vertex_numbers;
	fileout << row_numbers << std::endl;
	fileout << col_numbers << std::endl;
	int count = 0;
	int total_edge = 0;
	for (auto it = edge_list.begin(); it != edge_list.end(); it++)
	{
		if(it == edge_list.begin()){continue;}
		std::cout<<it->size()<<std::endl;
		total_edge += it->size();
 		int temp = (it->size() + 1) * mask_numbers;
		count += temp;
	}
	std::cout<<total_edge<<std::endl;
	assert(total_edge == 2 * edge_numbers );
	count += edge_numbers*mask_numbers;

	fileout << count << std::endl;
	for (unsigned int it = 1; it < edge_list.size(); ++it)
	{
		for (int i = 1; i <= mask_numbers; ++i)
		{
			fileout << (it - 1)*mask_numbers + i << " " << it << std::endl;
			for (auto j = edge_list[it].begin(); j != edge_list[it].end(); ++j)
			{
				fileout << (it - 1)*mask_numbers + i << " " << vertex_numbers + (j->No - 1)*mask_numbers + i << std::endl;
			}
		}
	}
	for (int i = 0; i < edge_numbers; ++i)
	{
		for (int j = 1; j <= mask_numbers; ++j)
			fileout << vertex_numbers * mask_numbers + 1 << " " << vertex_numbers + i * mask_numbers + j << std::endl;
	}
	fileout.close();
	return;
}

int Next_Column(std::vector<int> & MPLD_search_vector, int depth)
{
	return MPLD_search_vector[depth];
}

int Next_Column_stitch(DancingLink & dl,std::vector<int> & MPLD_search_vector)
{
	bool not_find_last_uncovered_col = true;
	int last_col = 0;
	for(unsigned int i = 0; i< MPLD_search_vector.size(); i++){
		Cell *col = &dl.Col_Header_Table[MPLD_search_vector[i]];
		//if the column(vertex) has been removed(selected)
		//if(col_cover_vector[MPLD_search_vector[i]])	continue;
		if(col->Left->Right != col) continue;
		else{
			mplAssert(col->InDLX);
			if(not_find_last_uncovered_col){
				last_col = MPLD_search_vector[i];
				not_find_last_uncovered_col = false;
			}
		}

		if(col-> Children_Number == 1){
			return MPLD_search_vector[i];
		}
	}
	assert(not_find_last_uncovered_col == false);
	return last_col;
}
bool Vertices_All_Covered(DancingLink & dl, int& vertex_numbers)
{
	if (dl.DL_Header.Right->Col > vertex_numbers)
		return true;
	else
		return false;
}

void store_intermediate_process(DancingLink & dl, int this_col, std::set<int> & row_set,
								std::vector<int> & Delete_the_Row_in_which_Col,
								std::vector<std::list<int> >  & Order_of_Row_Deleted_in_Col,std::vector<int>  &  conflict_col_table,std::vector<int>  &last_rows)
{
	for (auto row_it = row_set.begin(); row_it != row_set.end(); ++row_it) {
		Delete_the_Row_in_which_Col[*row_it] = this_col;
		for (auto row_ele = dl.Row_Header_Table[*row_it].Right; row_ele != &dl.Row_Header_Table[*row_it]; row_ele = row_ele->Right)
			{	//std::cout<<"row deleted is "<<*row_it<<"in column "<<this_col<<std::endl;
							Order_of_Row_Deleted_in_Col[row_ele->Col].push_back(*row_it);
			conflict_col_table[row_ele->Col] = this_col;
			last_rows[row_ele->Col] = (*row_it);}
	}
}

void efficient_store_intermediate_process(DancingLink & dl, int this_col, std::set<int> & row_set,
								std::vector<int> & Delete_the_Row_in_which_Col,
								std::vector<std::list<int> >  & Order_of_Row_Deleted_in_Col)
{
	for (auto row_it = row_set.begin(); row_it != row_set.end(); ++row_it) {
		Delete_the_Row_in_which_Col[*row_it] = this_col;
		for (auto row_ele = dl.Row_Header_Table[*row_it].Right; row_ele != &dl.Row_Header_Table[*row_it]; row_ele = row_ele->Right)
			{	//std::cout<<"row deleted is "<<*row_it<<"in column "<<this_col<<std::endl;
							Order_of_Row_Deleted_in_Col[row_ele->Col].push_back(*row_it);}
	}
}

void recover_intermediate_process(DancingLink & dl, int this_col, std::set<int>& row_set,
								std::vector<int> & Delete_the_Row_in_which_Col,
								std::vector<std::list<int> >  & Order_of_Row_Deleted_in_Col,std::vector<int>  &  conflict_col_table,std::vector<int>  &last_rows)
{
	(void)this_col;
	for (auto row_it = row_set.begin(); row_it != row_set.end(); ++row_it)
	{
		 Delete_the_Row_in_which_Col[*row_it] = 0;
		for (auto row_ele = dl.Row_Header_Table[*row_it].Right; row_ele != &dl.Row_Header_Table[*row_it]; row_ele = row_ele->Right)
			{Order_of_Row_Deleted_in_Col[row_ele->Col].pop_back();
			conflict_col_table[row_ele->Col] = Order_of_Row_Deleted_in_Col[row_ele->Col].back();
			last_rows[row_ele->Col] =  0;}
	}
}

void efficient_recover_intermediate_process(DancingLink & dl, int this_col, std::set<int>& row_set,
								std::vector<int> & Delete_the_Row_in_which_Col,
								std::vector<std::list<int> >  & Order_of_Row_Deleted_in_Col)
{
	(void)this_col;
	for (auto row_it = row_set.begin(); row_it != row_set.end(); ++row_it)
	{
		 Delete_the_Row_in_which_Col[*row_it] = 0;
		for (auto row_ele = dl.Row_Header_Table[*row_it].Right; row_ele != &dl.Row_Header_Table[*row_it]; row_ele = row_ele->Right)
			{Order_of_Row_Deleted_in_Col[row_ele->Col].pop_back();}
	}
}

bool MPLD_X_Solver(DancingLink & dl, std::vector<int8_t>& color_vector,std::vector<int> & result_vec, std::pair<int, int>  & conflict_pair,
			int vertex_numbers, int mask_numbers,
			std::vector<int> & Delete_the_Row_in_which_Col,
			std::vector<std::list<int> >  & Order_of_Row_Deleted_in_Col, int depth, std::vector<int> & MPLD_search_vector, const char* result_file,
			std::vector<bool>& col_cover_vector,std::vector<int>& row_select_vector,
			std::vector<int> & partial_conflict_col_table,
			std::vector<int>  &  conflict_col_table,std::vector<int>  &partial_last_rows,std::vector<int>  &last_rows)
{
	// If there is no columns left or all the verteices are covered, then the algorithm terminates.
	if (dl.DL_Header.Right == &dl.DL_Header || Vertices_All_Covered(dl, vertex_numbers))
	{
		//Decode_OpenMPL(vertex_numbers, mask_numbers,color_vector,result_vec, conflict_pair, result_file);
		return true;
	}
	/*
	if (dl.DL_Header.Right->Col > vertex_numbers )
	{
		this_col = Sorting_Queue(dl);
	}
	else
	{
	*/
	Cell *col;
	//TODO: this depth should be a bug cause sometimes it is not by order (need a bool vector to record the col cover information)
	int this_col = Next_Column_stitch(dl,MPLD_search_vector);

	col = &dl.Col_Header_Table[this_col];
	LR_remove(*col);
	col_cover_vector[this_col] = true;
	if (col->Children_Number == 0)
	{
		// int last_row = Order_of_Row_Deleted_in_Col[this_col].back();
		// int conflict_col = Delete_the_Row_in_which_Col[last_row];
		//std::cout<<"last row is "<<last_row<<std::endl;
		//mplAssert(conflict_col == conflict_col_table[this_col]);
		conflict_pair = std::make_pair(conflict_col_table[this_col], this_col);
		//Also,  record the partial results (rows) in the last detected conflict
		row_select_vector.assign(result_vec.begin(),result_vec.end());
		row_select_vector.push_back(last_rows[this_col]);
		//We should should push back the last row(coloring result of this conflict col)
		partial_last_rows.assign(last_rows.begin(), last_rows.end());
		partial_conflict_col_table.assign(conflict_col_table.begin(), conflict_col_table.end());
		// if (MPLD_X_Solver(dl, color_vector,result_vec, conflict_pair, vertex_numbers, mask_numbers, Delete_the_Row_in_which_Col, Order_of_Row_Deleted_in_Col, depth + 1, MPLD_search_vector, result_file))
		// 	return true;
	}
	for (Cell *j = col->Down; j != col; j = j->Down)
	{
		std::set<int> row_set;
		std::set<int> col_set;
		result_vec.push_back(j->Row);
		Select_All_Rows_Cols(dl, j->Row, row_set, col_set);
		store_intermediate_process(dl, this_col, row_set, Delete_the_Row_in_which_Col, Order_of_Row_Deleted_in_Col,conflict_col_table,last_rows);

		Remove_Rows_Cols(dl, row_set, col_set);
		if (MPLD_X_Solver(dl, color_vector,result_vec, conflict_pair, vertex_numbers, mask_numbers, Delete_the_Row_in_which_Col, Order_of_Row_Deleted_in_Col, depth + 1,
		MPLD_search_vector, result_file,col_cover_vector,row_select_vector,partial_conflict_col_table,conflict_col_table,partial_last_rows,last_rows))
			return true;

		result_vec.pop_back();
		recover_intermediate_process(dl, this_col, row_set, Delete_the_Row_in_which_Col, Order_of_Row_Deleted_in_Col,conflict_col_table,last_rows);
		Recover_Rows_Cols(dl, row_set, col_set);
	}
	LR_recover(*col);
	col_cover_vector[this_col] = false;

	return false;
}

bool Efficient_MPLD_X_Solver(DancingLink & dl,std::vector<int8_t>& color_vector, std::vector<int> & result_vec, std::pair<int, int>  & conflict_pair,
			int vertex_numbers, int mask_numbers,
			std::vector<int> & Delete_the_Row_in_which_Col,
			std::vector<std::list<int> >  & Order_of_Row_Deleted_in_Col, int depth, std::vector<int> & MPLD_search_vector, const char* result_file,
			std::vector<int> & partial_row_results, std::vector<int> & partial_col_results,std::vector<int> & col_results)
{
	if (dl.DL_Header.Right == &dl.DL_Header || Vertices_All_Covered(dl, vertex_numbers))
	{
		//Decode_OpenMPL(vertex_numbers, mask_numbers,color_vector,result_vec, conflict_pair, result_file);
		return true;
	}

	Cell *col;
	int this_col = Next_Column_stitch(dl,MPLD_search_vector);
#ifdef DEBUG_DANCINGLINKCOLORING
    mplPrint(kDEBUG, "BEGIN: this_col : %d depth : %d\n", this_col, depth);
#endif

	col = &dl.Col_Header_Table[this_col];
	LR_remove(*col);
	col_results.push_back(this_col);
	if (col->Children_Number == 0)
	{
		int last_row = Order_of_Row_Deleted_in_Col[this_col].back();
		int conflict_col = Delete_the_Row_in_which_Col[last_row];
		conflict_pair = std::make_pair(conflict_col, this_col);
		partial_row_results.assign(result_vec.begin(),result_vec.end());
		partial_row_results.push_back(last_row);
		partial_col_results.assign(col_results.begin(), col_results.end());
	}

	for (Cell *j = col->Down; j != col; j = j->Down)
	{
		std::set<int> row_set;
		std::set<int> col_set;
		result_vec.push_back(j->Row);
		Select_All_Rows_Cols(dl, j->Row, row_set, col_set);
		efficient_store_intermediate_process(dl, this_col, row_set, Delete_the_Row_in_which_Col, Order_of_Row_Deleted_in_Col);
		Remove_Rows_Cols(dl, row_set, col_set);
		if (Efficient_MPLD_X_Solver(dl,color_vector, result_vec, conflict_pair, vertex_numbers, mask_numbers, Delete_the_Row_in_which_Col, Order_of_Row_Deleted_in_Col, depth + 1,
		MPLD_search_vector, result_file,partial_row_results,partial_col_results,col_results))
			return true;

		result_vec.pop_back();
		efficient_recover_intermediate_process(dl, this_col, row_set, Delete_the_Row_in_which_Col, Order_of_Row_Deleted_in_Col);
		Recover_Rows_Cols(dl, row_set, col_set);
	}
	LR_recover(*col);
#ifdef DEBUG_DANCINGLINKCOLORING
    mplPrint(kDEBUG, "END: this_col : %d depth : %d\n", this_col, depth);
#endif
	col_results.pop_back();
	return false;
}

bool Efficient_MPLD_X_Solver_v2(DancingLink & dl, std::vector<int> & result_vec, std::pair<int, int>  & conflict_pair,
			int vertex_numbers, std::vector<int> & Delete_the_Row_in_which_Col,
			std::vector<std::list<int> >  & Order_of_Row_Deleted_in_Col, int depth, std::vector<int> & MPLD_search_vector,
			std::vector<int> & partial_row_results, std::vector<int> & partial_col_results,std::vector<int> & col_results,
			std::vector<std::vector<int>> &early_quit_count,bool & early_quit)
{
	if (dl.DL_Header.Right == &dl.DL_Header || Vertices_All_Covered(dl, vertex_numbers))
	{
		return true;
	}

	Cell *col;
	int this_col = Next_Column_stitch(dl,MPLD_search_vector);

	col = &dl.Col_Header_Table[this_col];
	LR_remove(*col);
	col_results.push_back(this_col);
	if (col->Children_Number == 0)
	{
		int last_row = Order_of_Row_Deleted_in_Col[this_col].back();
		int conflict_col = Delete_the_Row_in_which_Col[last_row];
		conflict_pair = std::make_pair(conflict_col, this_col);
		partial_row_results.assign(result_vec.begin(),result_vec.end());
		partial_row_results.push_back(last_row);
		partial_col_results.assign(col_results.begin(), col_results.end());
		early_quit_count[conflict_col][this_col] ++;
		early_quit_count[this_col][conflict_col] ++;
		if(early_quit_count[this_col][conflict_col] > 500 || early_quit_count[conflict_col][this_col]> 500){
			early_quit = true;
			LR_recover(*col);
			col_results.pop_back();
			return false;
		}
	}

	for (Cell *j = col->Down; j != col; j = j->Down)
	{
		std::set<int> row_set;
		std::set<int> col_set;
		result_vec.push_back(j->Row);
		Select_All_Rows_Cols(dl, j->Row, row_set, col_set);
		efficient_store_intermediate_process(dl, this_col, row_set, Delete_the_Row_in_which_Col, Order_of_Row_Deleted_in_Col);
		Remove_Rows_Cols(dl, row_set, col_set);
		if (Efficient_MPLD_X_Solver_v2(dl, result_vec, conflict_pair, vertex_numbers, Delete_the_Row_in_which_Col, Order_of_Row_Deleted_in_Col, depth + 1,
		MPLD_search_vector,partial_row_results,partial_col_results,col_results,early_quit_count,early_quit))
			return true;
		else{
			if(early_quit){
				result_vec.pop_back();
				efficient_recover_intermediate_process(dl, this_col, row_set, Delete_the_Row_in_which_Col, Order_of_Row_Deleted_in_Col);
				Recover_Rows_Cols(dl, row_set, col_set);
				LR_recover(*col);
				col_results.pop_back();
#ifdef DEBUG_DANCINGLINKCOLORING
                mplPrint(kDEBUG, "END by early stop: this_col : %d depth : %d\n", this_col, depth);
#endif
				return false;
			}
		}

		result_vec.pop_back();
		efficient_recover_intermediate_process(dl, this_col, row_set, Delete_the_Row_in_which_Col, Order_of_Row_Deleted_in_Col);
		Recover_Rows_Cols(dl, row_set, col_set);
	}
	LR_recover(*col);
	col_results.pop_back();
	return false;
}

/***
 * The core function to solve one dl.
 * Typically, this function may run MPLD_X_Solver  multiple times due to the existence of some exact conflicts
 * Return : selected row
 * **/
std::vector<int> core_solve_dl(DancingLink & dl, std::vector<std::list<Edge_Simple> > & edge_list,  int  row_numbers,  int  col_numbers,
 int  vertex_numbers, int mask_number){
	//the total conflict pairs
	// boost::timer::cpu_timer dancing_link_timer;
	// //dancing_link_timer.start();
	std::pair<int, int>  conflict_pair;
	//the conflict pair if MPLD_X_Solver has NO a conflict-free solution
	std::set<std::pair<int, int> >  final_conflict;
	//selected result rows if MPLD_X_Solver has a conflict-free solution
	std::vector<int> selected_rows;
	//the final selected cols;
	std::vector<int> selected_cols;
	//the partial selected rows(coloring results actually) if MPLD_X_Solver has NO a conflict-free solution
	std::vector<int> partial_selected_rows;
	//the partial selected cols(selected )  if MPLD_X_Solver has NO a conflict-free solution
	std::vector<int> partial_selected_cols;
	//the final selected rows
	std::vector<int> final_result;
	//middle information for recover partial results
	std::vector<int> Delete_the_Row_in_which_Col;
	std::vector<std::list<int> >  Order_of_Row_Deleted_in_Col;
	Order_of_Row_Deleted_in_Col.resize(col_numbers + 1);
	Delete_the_Row_in_which_Col.resize(row_numbers + 1);
	std::vector<int> MPLD_search_vector;
	//MPLD_search_vector = BFS_Order(edge_list);
	//MPLD_search_vector = Simple_Order(edge_list.size());
	MPLD_search_vector = BFS_Order_no_stitch_first(edge_list,dl);
	//MPLD_search_vector = BFS_Order_max_first(edge_list);
	// for(auto i = 0; i<MPLD_search_vector.size(); i++){
	// 	std::cout<<MPLD_search_vector[i]<<" ";
	// }
	// std::cout<<std::endl;
	int depth = 1;
	std::vector<std::vector<int>> early_quit_count;
	for(auto i = 0; i <= vertex_numbers; i++){
		std::vector<int> row_early_quite_count;
		row_early_quite_count.assign(vertex_numbers+1,0);
		early_quit_count.push_back(row_early_quite_count);
	}
	bool early_quit = false;
	bool result = Efficient_MPLD_X_Solver_v2(dl, selected_rows, conflict_pair, vertex_numbers, Delete_the_Row_in_which_Col, Order_of_Row_Deleted_in_Col, depth,
		MPLD_search_vector,partial_selected_rows,partial_selected_cols,selected_cols,early_quit_count, early_quit);
	int iteration = 1;
	//std::cout<< "IN MPL:::::::::::::::::::::first try solve " << dancing_link_timer.format(6)<<std::endl;
	//dancing_link_timer.start();
	while(result==false && partial_selected_rows.size()< vertex_numbers){
		mplAssert(partial_selected_rows.size() == partial_selected_cols.size());
		iteration++;
#ifdef DEBUG_DANCINGLINKCOLORING
        mplPrint(kDEBUG, "iteration %d\n", iteration);
#endif
		final_conflict.insert(conflict_pair);
		int col_edge = -1;
		for(std::list<Edge_Simple>::iterator conflict_edge = edge_list[conflict_pair.first].begin();conflict_edge != edge_list[conflict_pair.first].end();++conflict_edge){
			if((*conflict_edge).target == conflict_pair.second){
				col_edge  = (*conflict_edge).No;
				break;
			}
		}
		if(col_edge==-1){
			std::cout<<"bug found!"<<std::endl;
		}
		mplAssert(col_edge!=-1);
		mplAssert(selected_rows.size() == 0);
		for(int i = 1;i<=mask_number;i++){
			int this_col = vertex_numbers + (col_edge -1)*(mask_number) + i;
			//Cell *col = &dl.Col_Header_Table[this_col];
			Remove_Single_Col(dl,this_col);
		}
		//std::cout<< "IN MPL:::::::::::::::::::::remove conflict cols " << dancing_link_timer.format(6)<<std::endl;
		//dancing_link_timer.start();
		final_result.insert( final_result.end(), partial_selected_rows.begin(), partial_selected_rows.end() );
		//NOTE that the order between the two parts (remove edges and recover partial results are important! Cause some rows won't be removed this time due to the exact conflict edge is removed)
		for(int i = 0; i < partial_selected_rows.size(); i++){
			//if the row is selected ( in the partial result), recover the partial results
				std::set<int> row_set;
				std::set<int> col_set;
				Select_All_Rows_Cols(dl, partial_selected_rows[i], row_set, col_set);
				efficient_store_intermediate_process(dl, partial_selected_cols[i], row_set, Delete_the_Row_in_which_Col, Order_of_Row_Deleted_in_Col);
				Remove_Rows_Cols(dl, row_set, col_set);
		}
		//std::cout<< "IN MPL:::::::::::::::::::::recover partial results " << dancing_link_timer.format(6)<<std::endl;
		//dancing_link_timer.start();
		partial_selected_rows.clear();
		partial_selected_cols.swap(selected_cols);
		selected_cols.clear();
		depth = 1;
		for (auto &v: early_quit_count) {
			std::fill(v.begin(), v.end(), 0);
		}
		early_quit = false;
		result = Efficient_MPLD_X_Solver_v2(dl, selected_rows, conflict_pair, vertex_numbers, Delete_the_Row_in_which_Col, Order_of_Row_Deleted_in_Col, depth,
		MPLD_search_vector,partial_selected_rows,partial_selected_cols,selected_cols,early_quit_count, early_quit);
		//std::cout<< "IN MPL:::::::::::::::::::::second try solve " << dancing_link_timer.format(6)<<std::endl;
		//dancing_link_timer.start();
	}
	if(selected_rows.size() != 0){
		final_result.insert( final_result.end(), selected_rows.begin(), selected_rows.end() );
		mplAssert(final_result.size() == vertex_numbers);
	}
	else final_result.insert( final_result.end(), partial_selected_rows.begin(), partial_selected_rows.end() );
	return final_result;
}
/***
 * The function to decode and calculate the cost of the results of DL.
 * Args: selected rows
 * Return : selected row
 * **/

void decode_row_results(std::vector<int> & final_result, std::vector<int8_t>& color_vector, int vertex_number,
int  mask_number, std::vector<std::vector<std::pair<uint32_t,uint32_t>>>& decode_mat, std::vector<Vertex*> & node_list ){
	std::vector<int8_t> color_results_wo_stitch (vertex_number);
	for (auto i = final_result.begin(); i != final_result.end(); i++)
	{
		// if the selected row is in the range of parent node (no stitch)
		// std::cout << *i << std::endl;
		if((*i) < vertex_number * mask_number + 1){
		int No = ((*i) - 1) / mask_number + 1;
		int mask = (*i + 2) % mask_number;
		color_results_wo_stitch[No-1] = mask; }
		// std::cout<<"selected rows: "<<(*i)<<std::endl;
		//if the selected_rows represent some stitches rows
		if((*i) > (vertex_number * (uint32_t)(mask_number) + (uint32_t)1)){
			std::vector<std::pair<uint32_t,uint32_t>>& row_decoder = decode_mat[(*i)- vertex_number * mask_number -2];
			for(std::vector<std::pair<uint32_t,uint32_t>>::iterator it = row_decoder.begin(); it != row_decoder.end(); ++it) {
				color_vector[(*it).first] = (*it).second;
				// std::cout<<"Corresponding color "<<(*it).first<<" "<<(*it).second<<std::endl;
			}
		}
	}
	// std::cout<<"Finished"<<std::endl;

	for(std::vector<Vertex*>::iterator it = node_list.begin(); it != node_list.end(); ++it) {
		if(color_vector[(*it)->Stitch_No]!= -1){continue;}
		if((*it)->No == 0){
			mplAssert((*it)->Is_Parent == false);
			mplAssert((*it)->parent->Stitch_No == -1);
			mplAssert(color_results_wo_stitch[(*it)->parent->No -1]!= -1);
			color_vector[(*it)->Stitch_No] = color_results_wo_stitch[(*it)->parent->No -1];
		}
		else{
			mplAssert(color_results_wo_stitch[(*it)->No -1]!=-1);
			color_vector[(*it)->Stitch_No] = color_results_wo_stitch[(*it)->No -1];
		}
	}
	return;
}

void decode_row_results_wo_skeleton(std::vector<int> & final_result, std::vector<int8_t>& color_vector, int vertex_number,
int  mask_number, std::vector<std::vector<std::pair<uint32_t,uint32_t>>>& decode_mat, std::vector<Vertex*> & node_list ){
	std::vector<int8_t> color_results_wo_stitch (vertex_number);
	for (auto i = final_result.begin(); i != final_result.end(); i++)
	{
		// if the selected row is in the range of parent node (no stitch)
		// std::cout << *i << std::endl;
		if((*i) < vertex_number * mask_number + 1){
		int No = ((*i) - 1) / mask_number + 1;
		int mask = (*i + 2) % mask_number;
		color_results_wo_stitch[No-1] = mask; }

		//if the selected_rows represent some stitches rows
		if((*i) > (vertex_number * (uint32_t)(mask_number))){
			std::vector<std::pair<uint32_t,uint32_t>>& row_decoder = decode_mat[(*i)- vertex_number * mask_number -1];
			for(std::vector<std::pair<uint32_t,uint32_t>>::iterator it = row_decoder.begin(); it != row_decoder.end(); ++it) {
				color_vector[(*it).first] = (*it).second;
			}
		}
	}

	for(std::vector<Vertex*>::iterator it = node_list.begin(); it != node_list.end(); ++it) {
		if(color_vector[(*it)->Stitch_No]!= -1){continue;}
		if((*it)->No == 0){
			mplAssert((*it)->Is_Parent == false);
			mplAssert((*it)->parent->Stitch_No == -1);
			mplAssert(color_results_wo_stitch[(*it)->parent->No -1]!= -1);
			color_vector[(*it)->Stitch_No] = color_results_wo_stitch[(*it)->parent->No -1];
		}
		else{
			mplAssert(color_results_wo_stitch[(*it)->No -1]!=-1);
			color_vector[(*it)->Stitch_No] = color_results_wo_stitch[(*it)->No -1];
		}
	}
	return;
}
void Decode(int vertex_numbers, int mask_numbers, std::vector<int> result_vec, std::pair<int, int> conflict_pair, std::string filename)
{
	(void)conflict_pair;
	(void)filename;
	(void)vertex_numbers;
	std::map<int, int> Final_Color;
	std::ofstream fileout(filename);
	fileout << "Solution : " << std::endl;
	for (auto i = result_vec.begin(); i != result_vec.end(); i++)
	{
		// std::cout << *i << std::endl;
		int No = ((*i) - 1) / mask_numbers + 1;
		int mask = (*i + 2) % mask_numbers + 1;
		Final_Color.insert(std::make_pair(No, mask));
	}
	for (auto i = Final_Color.begin(); i != Final_Color.end(); i++)
	{
		fileout << "Vertex " << i->first << " \t : " << i->second << std::endl;
	}
	fileout << "=============================================" << std::endl;

	// if (conflict_pair!=nullptr)
	// {
	// 	fileout << "Conflicts: " << std::endl;
	// 	fileout << "No\t Source\t Target" << std::endl;
	// 	fileout << count++ << "\t " << conflict_pair->first << "\t " <<conflict_pair->second << std::endl;
	// 	fileout << "=============================================" << std::endl;
	// }

	fileout.close();
}

void Decode_OpenMPL(int vertex_numbers, int mask_numbers, std::vector<int8_t>& color_vector,std::vector<int> result_vec, std::pair<int, int> conflict_pair,  char* filename)
{
	(void)filename;
	(void)conflict_pair;
	for (auto i = result_vec.begin(); i != result_vec.end(); i++)
	{
		//std::cout << *i << std::endl;
		if((*i) < vertex_numbers * mask_numbers + 1){
		int No = ((*i) - 1) / mask_numbers + 1;
		int mask = (*i + 2) % mask_numbers;
		color_vector[No-1] = mask; }
	}

	// if (!conflict_pair.empty())
	// {
	// 	std::cout << "Conflicts: " << std::endl;
	// 	std::cout << "No\t Source\t Target" << std::endl;
	// 	int count = 1;
	// 	for (auto i = conflict_pair.begin(); i != conflict_pair.end(); i++)
	// 		{color_vector[i->second-1] = color_vector[i->first-1];
	// 		std::cout << count++ << "\t " << i->first << "\t " << i->second << std::endl;}
	// 	std::cout << "=============================================" << std::endl;
	// }
}

void MPLD_Solver(std::string Graph_Filename, std::string Exact_Cover_Filename, bool whether_BFS, int mask_numbers,  const char* result_file)
{
	(void)result_file;
	int vertex_numbers;
	int edge_numbers;
	int row_numbers;
	int col_numbers;
	std::vector<int8_t> color_vec;
	std::vector<int> result_vec;
	std::pair<int, int>  conflict_pair;
	std::vector<int> Delete_the_Row_in_which_Col;
	std::vector<std::list<int> >  Order_of_Row_Deleted_in_Col;
	std::vector<int> MPLD_search_vector;
	DancingLink dl;
	std::cout<<"Ready to generate cover matrix"<<std::endl;
	Convert_to_Exat_Cover(row_numbers, col_numbers, Graph_Filename, Exact_Cover_Filename, whether_BFS, vertex_numbers, edge_numbers, mask_numbers, MPLD_search_vector);
	Order_of_Row_Deleted_in_Col.resize(col_numbers + 1);
	Delete_the_Row_in_which_Col.resize(row_numbers + 1);
	DL_Load(dl, Exact_Cover_Filename);
	//int depth = 1;
	//MPLD_X_Solver(dl, color_vec,result_vec, conflict_pair, vertex_numbers, mask_numbers, Delete_the_Row_in_which_Col, Order_of_Row_Deleted_in_Col, depth, MPLD_search_vector, result_file,col_cover_vector);
}

SIMPLEMPL_END_NAMESPACE


<% from space import dtypes, spaces %>
#ifndef BEMPP_PYTHON_SPACE_VARIANT_HPP
#define BEMPP_PYTHON_SPACE_VARIANT_HPP

#include "bempp/common/shared_ptr.hpp"
#include "types.h"
#include <boost/variant.hpp>
#include <type_traits>
#include <type_traits>

namespace Bempp {

//! A shared pointer that can hold const and non-const versions
class SpaceVariants {
    typedef boost::variant<
% for ctype in dtypes.values():
        shared_ptr< Space<${ctype}> >,
        shared_ptr< const Space<${ctype}> > ${',' if not loop.last else ''}
% endfor
    > t_variant;

    struct GetGrid : public boost::static_visitor< shared_ptr<Grid const> > {
        template<class T> shared_ptr<Grid const>
            operator()( shared_ptr<T> const &_in) const { return _in->grid(); }
    };

    struct IsCompatible : public boost::static_visitor<bool> {
        template<class T0, class T1>
            typename std::enable_if<
                std::is_same<
                    typename std::remove_const<T0>::type,
                    typename std::remove_const<T1>::type
                > :: value, bool
            > :: type operator()(shared_ptr<T0> const &_this,
                shared_ptr<T1> const &_that) const {
            return _this->spaceIsCompatible(*_that);
        }
        template<class T0, class T1>
            typename std::enable_if<
                not std::is_same<
                    typename std::remove_const<T0>::type,
                    typename std::remove_const<T1>::type
                > :: value, bool
            > :: type operator()(shared_ptr<T0> const &_this,
                shared_ptr<T1> const &_that) const {
            return false;
        }
    };

    struct IsSame : public boost::static_visitor<bool> {
        template<class T0, class T1>
            typename std::enable_if<
                std::is_same<
                    typename std::remove_const<T0>::type,
                    typename std::remove_const<T1>::type
                > :: value, bool
            > :: type operator()(shared_ptr<T0> const &_this,
                shared_ptr<T1> const &_that) const {
            return _this.get() == _that.get();
        }
        template<class T0, class T1>
            typename std::enable_if<
                not std::is_same<
                    typename std::remove_const<T0>::type,
                    typename std::remove_const<T1>::type
                > :: value, bool
            > :: type operator()(shared_ptr<T0> const &_this,
                shared_ptr<T1> const &_that) const {
            return false;
        }
    };

    struct DType: public boost::static_visitor<std::string> {
% for pyname, ctype in dtypes.items():
        std::string operator()(
                shared_ptr<Space<${ctype}> const> const &_in) const {
            return "${pyname}";
        }
% endfor
    };

    public:
        SpaceVariants() {}

        template<class T>
            void set(shared_ptr<T> const &_in) { space_ = _in; }
        template<class T>
            void set(shared_ptr<T const> const &_in) { space_ = _in; }

        std::string dtype() const {
            return boost::apply_visitor(SpaceVariants::DType(), space_);
        }

        shared_ptr<Grid const> grid() const {
            return boost::apply_visitor(SpaceVariants::GetGrid(), space_);
        }

        bool isCompatible(SpaceVariants const &_other) const {
            return boost::apply_visitor(SpaceVariants::IsCompatible(),
                    space_, _other.space_);
        }
        bool isSame(SpaceVariants const &_other) const {
            return boost::apply_visitor(SpaceVariants::IsSame(),
                    space_, _other.space_);
        }

        t_variant & variants() { return space_; }
        t_variant const & variants() const { return space_; }
    private:
        t_variant space_;
};

}
#endif


/* Copyright © 2018 Apple Inc. All rights reserved.
 *
 * Use of this source code is governed by a BSD-3-clause license that can
 * be found in the LICENSE.txt file or at https://opensource.org/licenses/BSD-3-Clause
 */

#define BOOST_TEST_MODULE test_model_spec

#include <unity/toolkits/neural_net/model_spec.hpp>

#include <algorithm>
#include <cstdint>
#include <memory>

#include <boost/test/unit_test.hpp>
#include <unity/toolkits/coreml_export/mlmodel_include.hpp>
#include <util/test_macros.hpp>

namespace turi {
namespace neural_net {
namespace {

using CoreML::Specification::NeuralNetwork;
using CoreML::Specification::NeuralNetworkLayer;

BOOST_AUTO_TEST_CASE(test_export_empty) {

  NeuralNetwork nn_model;
  model_spec nn_spec(nn_model);
  float_array_map params = nn_spec.export_params_view();
  TS_ASSERT(params.empty());
}

BOOST_AUTO_TEST_CASE(test_export_conv_params) {

  // Build a CoreML spec with just a single conv layer.
  NeuralNetwork nn_model;
  auto* conv_layer = nn_model.add_layers();
  conv_layer->set_name("conv_test");
  auto* conv_params = conv_layer->mutable_convolution();
  conv_params->set_outputchannels(2);  // N
  conv_params->set_kernelchannels(3);  // C
  conv_params->add_kernelsize(4);      // H
  conv_params->add_kernelsize(5);      // W
  const size_t size = 2*3*4*5;
  auto* weights = conv_params->mutable_weights();
  for (size_t i = 0; i < size; ++i) {
    weights->add_floatvalue(100.f + i);
  }

  // Extract the parameters from the spec.
  model_spec nn_spec(nn_model);
  float_array_map params = nn_spec.export_params_view();

  // The result should have just one float array.
  TS_ASSERT_EQUALS(params.size(), 1);
  auto float_array = params["conv_test_weight"];

  // Shape must be [N, C, H, W], which is [2, 3, 4, 5]
  TS_ASSERT_EQUALS(float_array.dim(), 4);
  for (size_t i = 0; i < 4; ++i) {
    TS_ASSERT_EQUALS(float_array.shape()[i], i + 2);
  }

  // Verify the data was extracted intact.
  TS_ASSERT_EQUALS(float_array.size(), size);
  for (size_t i = 0; i < size; ++i) {
    TS_ASSERT_EQUALS(float_array.data()[i], 100.f + i);
  }
}

BOOST_AUTO_TEST_CASE(test_export_conv_params_invalid) {

  NeuralNetwork nn_model;
  auto* conv_layer = nn_model.add_layers();
  conv_layer->set_name("conv_test");
  conv_layer->mutable_convolution();
  // The default ConvolutionLayerParams value is not valid.

  model_spec nn_spec(nn_model);
  TS_ASSERT_THROWS_ANYTHING(nn_spec.export_params_view());
}

BOOST_AUTO_TEST_CASE(test_export_batchnorm_params) {

  // Build a CoreML spec with just a single batchnorm layer.
  NeuralNetwork nn_model;
  auto* batchnorm_layer = nn_model.add_layers();
  batchnorm_layer->set_name("batchnorm_test");
  auto* batchnorm_params = batchnorm_layer->mutable_batchnorm();
  batchnorm_params->set_channels(1);
  batchnorm_params->mutable_gamma()->add_floatvalue(2.0f);
  batchnorm_params->mutable_beta()->add_floatvalue(3.0f);
  batchnorm_params->mutable_mean()->add_floatvalue(4.0f);
  batchnorm_params->mutable_variance()->add_floatvalue(5.0f);

  // Extract the parameters from the spec.
  model_spec nn_spec(nn_model);
  float_array_map params = nn_spec.export_params_view();

  // The result should have four float arrays.
  TS_ASSERT_EQUALS(params.size(), 4);

  TS_ASSERT_EQUALS(params["batchnorm_test_gamma"].dim(), 1);
  TS_ASSERT_EQUALS(params["batchnorm_test_gamma"].shape()[0], 1);
  TS_ASSERT_EQUALS(params["batchnorm_test_gamma"].data()[0], 2.0f);

  TS_ASSERT_EQUALS(params["batchnorm_test_beta"].dim(), 1);
  TS_ASSERT_EQUALS(params["batchnorm_test_beta"].shape()[0], 1);
  TS_ASSERT_EQUALS(params["batchnorm_test_beta"].data()[0], 3.0f);

  TS_ASSERT_EQUALS(params["batchnorm_test_running_mean"].dim(), 1);
  TS_ASSERT_EQUALS(params["batchnorm_test_running_mean"].shape()[0], 1);
  TS_ASSERT_EQUALS(params["batchnorm_test_running_mean"].data()[0], 4.0f);

  TS_ASSERT_EQUALS(params["batchnorm_test_running_var"].dim(), 1);
  TS_ASSERT_EQUALS(params["batchnorm_test_running_var"].shape()[0], 1);
  TS_ASSERT_EQUALS(params["batchnorm_test_running_var"].data()[0], 5.0f);
}

BOOST_AUTO_TEST_CASE(test_add_convolution) {

  // Add an arbitrary convolution layer to an empty model spec.
  model_spec nn_spec;
  int weights_size = 16*8*5*5;
  auto weight_init_fn = [weights_size](float* w, float* w_end) {
    TS_ASSERT_EQUALS(w_end - w, weights_size);
    for (int i = 0; i < weights_size; ++i) {
      w[i] = static_cast<float>(i);
    }
  };
  nn_spec.add_convolution("test_name", "test_input", 16, 8, 5, 5, 1, 1,
                          model_spec::padding_type::SAME, weight_init_fn);

  // Verify the resulting NeuralNetworkLayer value.
  TS_ASSERT_EQUALS(nn_spec.get_coreml_spec().layers_size(), 1);
  const NeuralNetworkLayer& layer = nn_spec.get_coreml_spec().layers(0);

  TS_ASSERT_EQUALS(layer.name(), "test_name");
  TS_ASSERT_EQUALS(layer.input_size(), 1);
  TS_ASSERT_EQUALS(layer.input(0), "test_input");
  TS_ASSERT_EQUALS(layer.output_size(), 1);
  TS_ASSERT_EQUALS(layer.output(0), "test_name");

  TS_ASSERT_EQUALS(layer.convolution().outputchannels(), 16);
  TS_ASSERT_EQUALS(layer.convolution().kernelchannels(), 8);
  TS_ASSERT_EQUALS(layer.convolution().ngroups(), 1);
  TS_ASSERT(layer.convolution().has_same());
  TS_ASSERT_EQUALS(layer.convolution().same().asymmetrymode(),
                   CoreML::Specification::SamePadding::TOP_LEFT_HEAVY);

  // These repeated fields must all have length 2.
  TS_ASSERT_EQUALS(layer.convolution().kernelsize_size(), 2);
  TS_ASSERT_EQUALS(layer.convolution().stride_size(), 2);
  TS_ASSERT_EQUALS(layer.convolution().dilationfactor_size(), 2);

  // Check each value of these repeated fields.
  TS_ASSERT_EQUALS(layer.convolution().kernelsize(0), 5);
  TS_ASSERT_EQUALS(layer.convolution().kernelsize(1), 5);
  TS_ASSERT_EQUALS(layer.convolution().stride(0), 1);
  TS_ASSERT_EQUALS(layer.convolution().stride(1), 1);
  TS_ASSERT_EQUALS(layer.convolution().dilationfactor(0), 1);
  TS_ASSERT_EQUALS(layer.convolution().dilationfactor(1), 1);

  // Verify the contents of the weights
  const auto& conv_weights = layer.convolution().weights().floatvalue();
  TS_ASSERT_EQUALS(conv_weights.size(), weights_size);
  for (int i = 0; i < weights_size; ++i) {
    TS_ASSERT_EQUALS(conv_weights[i], static_cast<float>(i));
  }

  // Update the weights, adding 7.f to each value.
  std::vector<float> updated_weights(conv_weights.begin(), conv_weights.end());
  std::for_each(updated_weights.begin(), updated_weights.end(),
                [](float& x) { x += 7.f; });
  nn_spec.update_params({
      {"test_name_weight",
       shared_float_array::wrap(updated_weights, {16, 8, 5, 5})}
  });

  // Verify the updated weights.
  TS_ASSERT_EQUALS(conv_weights.size(), weights_size);
  for (int i = 0; i < weights_size; ++i) {
    TS_ASSERT_EQUALS(conv_weights[i], static_cast<float>(i) + 7.f);
  }
}

BOOST_AUTO_TEST_CASE(test_add_batchnorm) {

  // Add an arbitrary batch layer to an empty model spec.
  model_spec nn_spec;
  nn_spec.add_batchnorm("test_name", "test_input", 16, 0.125f);

  // Verify the resulting NeuralNetworkLayer value.
  TS_ASSERT_EQUALS(nn_spec.get_coreml_spec().layers_size(), 1);
  const NeuralNetworkLayer& layer = nn_spec.get_coreml_spec().layers(0);

  TS_ASSERT_EQUALS(layer.name(), "test_name");
  TS_ASSERT_EQUALS(layer.input_size(), 1);
  TS_ASSERT_EQUALS(layer.input(0), "test_input");
  TS_ASSERT_EQUALS(layer.output_size(), 1);
  TS_ASSERT_EQUALS(layer.output(0), "test_name");

  TS_ASSERT_EQUALS(layer.batchnorm().channels(), 16);
  TS_ASSERT_EQUALS(layer.batchnorm().epsilon(), 0.125f);

  // These repeated fields must all have length 16.
  TS_ASSERT_EQUALS(layer.batchnorm().gamma().floatvalue_size(), 16);
  TS_ASSERT_EQUALS(layer.batchnorm().beta().floatvalue_size(), 16);
  TS_ASSERT_EQUALS(layer.batchnorm().mean().floatvalue_size(), 16);
  TS_ASSERT_EQUALS(layer.batchnorm().variance().floatvalue_size(), 16);

  // Check each value of these repeated fields.
  TS_ASSERT(std::all_of(layer.batchnorm().gamma().floatvalue().begin(),
                        layer.batchnorm().gamma().floatvalue().end(),
                        [](float f) { return f == 1.f; }));
  TS_ASSERT(std::all_of(layer.batchnorm().beta().floatvalue().begin(),
                        layer.batchnorm().beta().floatvalue().end(),
                        [](float f) { return f == 0.f; }));
  TS_ASSERT(std::all_of(layer.batchnorm().mean().floatvalue().begin(),
                        layer.batchnorm().mean().floatvalue().end(),
                        [](float f) { return f == 0.f; }));
  TS_ASSERT(std::all_of(layer.batchnorm().variance().floatvalue().begin(),
                        layer.batchnorm().variance().floatvalue().end(),
                        [](float f) { return f == 1.f; }));

  // Update the beta values to 0.5f.
  std::vector<float> updated_beta(16, 0.5f);
  nn_spec.update_params({
      {"test_name_beta", shared_float_array::wrap(updated_beta, {16})}
  });

  // Check the update beta values.
  TS_ASSERT(std::all_of(layer.batchnorm().beta().floatvalue().begin(),
                        layer.batchnorm().beta().floatvalue().end(),
                        [](float f) { return f == 0.5f; }));
}

BOOST_AUTO_TEST_CASE(test_add_leakyrelu) {

  // Add an arbitrary leaky ReLU layer to an empty model spec.
  model_spec nn_spec;
  nn_spec.add_leakyrelu("test_name", "test_input", 0.125f);

  // Verify the resulting NeuralNetworkLayer value.
  TS_ASSERT_EQUALS(nn_spec.get_coreml_spec().layers_size(), 1);
  const NeuralNetworkLayer& layer = nn_spec.get_coreml_spec().layers(0);

  TS_ASSERT_EQUALS(layer.name(), "test_name");
  TS_ASSERT_EQUALS(layer.input_size(), 1);
  TS_ASSERT_EQUALS(layer.input(0), "test_input");
  TS_ASSERT_EQUALS(layer.output_size(), 1);
  TS_ASSERT_EQUALS(layer.output(0), "test_name");

  TS_ASSERT_EQUALS(layer.activation().leakyrelu().alpha(), 0.125f);
}

}  // namespace
}  // namespace neural_net
}  // namespace turi


//==================================================================================================
/*!
  @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_SHUFFLE_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SHUFFLE_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-swar
    Function object implementing shuffle capabilities

    SIMD register shuffling

    Shuffle the elements of two SIMD registers following a compile-time
    permutation pattern passed as a @metafunction.

    @par Semantic:

    Let @c T be a SIMD register type of cardinal @c N, @c Perm be a
    binary @metafunction. For any SIMD register @c x and  @c y of type @c T,
    the following code:

    @code
    T r = shuffle<Perm>(x,y);
    @endcode

    is equivalent to

    @code
    T r = shuffle< mpl::apply<Perm, int_<0>, int_<N> >::type::value
                 , ...
                 , mpl::apply<Perm, int_<C-1>, int_<N> >::type::value
                 >(x,y);
    @endcode

    @usage{shuffle_perm1.cpp}

    @tparam Perm Permutation pattern @metafunction

  **/
  template<std::ptrdiff_t... Ps, typename T>  T shuffle(T const& a);
  template<std::ptrdiff_t... Ps, typename T>  T shuffle(T const& a,T const& b);

  template<typename Permutation, typename T>  T shuffle(T const& a);
  template<typename Permutation, typename T>  T shuffle(T const& a,T const& b);
} }
#endif

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

#endif


// Boost.Geometry

// Copyright (c) 2019, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#include "common.hpp"

#include <boost/geometry/algorithms/area.hpp>
#include <boost/geometry/algorithms/length.hpp>
#include <boost/geometry/algorithms/perimeter.hpp>

int test_main(int, char*[])
{
    geom g;

    bg::area(g.r, bg::strategy::area::cartesian<>());
    bg::area(g.r, bg::strategy::area::spherical<>());
    bg::area(g.r, bg::strategy::area::geographic<>());
    bg::area(g.po, bg::strategy::area::cartesian<>());
    bg::area(g.po, bg::strategy::area::spherical<>());
    bg::area(g.po, bg::strategy::area::geographic<>());
    bg::area(g.mpo, bg::strategy::area::cartesian<>());
    bg::area(g.mpo, bg::strategy::area::spherical<>());
    bg::area(g.mpo, bg::strategy::area::geographic<>());

    bg::length(g.s, bg::strategy::distance::pythagoras<>());
    bg::length(g.s, bg::strategy::distance::haversine<>());
    bg::length(g.s, bg::strategy::distance::geographic<>());
    bg::length(g.ls, bg::strategy::distance::pythagoras<>());
    bg::length(g.ls, bg::strategy::distance::haversine<>());
    bg::length(g.ls, bg::strategy::distance::geographic<>());
    bg::length(g.mls, bg::strategy::distance::pythagoras<>());
    bg::length(g.mls, bg::strategy::distance::haversine<>());
    bg::length(g.mls, bg::strategy::distance::geographic<>());

    bg::perimeter(g.r, bg::strategy::distance::pythagoras<>());
    bg::perimeter(g.r, bg::strategy::distance::haversine<>());
    bg::perimeter(g.r, bg::strategy::distance::geographic<>());
    bg::perimeter(g.po, bg::strategy::distance::pythagoras<>());
    bg::perimeter(g.po, bg::strategy::distance::haversine<>());
    bg::perimeter(g.po, bg::strategy::distance::geographic<>());
    bg::perimeter(g.mpo, bg::strategy::distance::pythagoras<>());
    bg::perimeter(g.mpo, bg::strategy::distance::haversine<>());
    bg::perimeter(g.mpo, bg::strategy::distance::geographic<>());

    return 0;
}


// Copyright 2020 the Autoware Foundation
//
// 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.
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.

#include <gtest/gtest.h>
#include <ndt/utils.hpp>
#include <Eigen/Core>
#include <random>
#include <vector>
#include <algorithm>
#include "test_ndt_utils.hpp"

using autoware::localization::ndt::try_stabilize_covariance;
using autoware::localization::ndt::transform_adapters::pose_to_transform;
using autoware::localization::ndt::transform_adapters::transform_to_pose;

PoseParams::PoseParams(
  double x, double y, double z, double ang_x, double ang_y, double ang_z)
{
  // euler angles XYZ
  pose << x, y, z, ang_x, ang_y, ang_z;
}

PoseParams::PoseParams(double translation_range, double rotation_range)
{
  std::uniform_real_distribution<double> uni_trans(-translation_range, translation_range);
  std::uniform_real_distribution<double> uni_rot(-rotation_range, rotation_range);
  std::default_random_engine re;
  for (auto i = 0U; i < 3U; ++i) {
    pose(i) = uni_trans(re);
  }
  for (auto i = 3U; i < 6U; ++i) {
    pose(i) = uni_rot(re);
  }
}

Cov3x3Param::Cov3x3Param(Real e1_, Real e2_, Real e3_, bool valid_)
: e1{e1_}, e2{e2_}, e3{e3_}, valid{valid_}
{
  Eigen::Vector3d e_vals{e1, e2, e3};
  // Fixed diagonal matrix as the eigen vectors for simplicity.
  Eigen::Vector3d e_vec_scales{2, 7, 13};
  CovMatrix e_vecs = e_vec_scales.asDiagonal();
  cov = e_vecs * e_vals.asDiagonal() * e_vecs.inverse();
}

class TestTransformAdapters : public ::testing::TestWithParam<PoseParams> {};

class CovarianceStabilityTest : public ::testing::TestWithParam<Cov3x3Param> {};

TEST_P(TestTransformAdapters, pose_to_transform) {
  const EigenPose<Real> pose = GetParam().pose;
  RosTransform ros_transform;
  RosPose ros_pose;
  EigenTransform<Real> eig_transform;

  pose_to_transform(pose, ros_transform);
  pose_to_transform(pose, ros_pose);
  pose_to_transform(pose, eig_transform);

  compare(pose, ros_transform);
  compare(pose, ros_pose);
  compare(pose, eig_transform);
}

TEST_P(TestTransformAdapters, transform_to_pose) {
  const EigenPose<Real> pose = GetParam().pose;
  RosTransform ros_transform;
  RosPose ros_pose;

  make_transform(pose, ros_transform);
  make_transform(pose, ros_pose);

  {
    EigenPose<Real> pose_result;
    transform_to_pose(ros_transform, pose_result);
    compare(pose_result, ros_transform);
  }

  {
    EigenPose<Real> pose_result;
    transform_to_pose(ros_pose, pose_result);
    compare(pose_result, ros_pose);
  }
}

TEST_P(TestTransformAdapters, pose_to_transform_to_pose) {
  const EigenPose<Real> pose = GetParam().pose;
  RosTransform ros_transform;
  RosPose ros_pose;

  pose_to_transform(pose, ros_transform);
  pose_to_transform(pose, ros_pose);
  {
    EigenPose<Real> pose_result;
    transform_to_pose(ros_transform, pose_result);
    compare(pose, pose_result);
  }

  {
    EigenPose<Real> pose_result;
    transform_to_pose(ros_pose, pose_result);
    compare(pose, pose_result);
  }
}

INSTANTIATE_TEST_CASE_P(
  transform_param_tests, TestTransformAdapters,
  ::testing::Values(
    PoseParams{0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
    PoseParams{0.5, 0.9, 0.1, 1.0, -3.1, 0.05},
    PoseParams{2.5, -1.9, 0.1, -2.1, 0.1, 3.05},
    PoseParams{2.5, -1.9, 64, -2.1, 31, -1.2},
    PoseParams{0.001, 1.00009, -1.0, 0.0, 0.75, 0.13}
    // cppcheck-suppress syntaxError
  ), );

INSTANTIATE_TEST_CASE_P(
  fuzzed_tests, TestTransformAdapters,
  ::testing::Values(
    PoseParams{0.1, 50.0},
    PoseParams{100.0, 0.005},
    PoseParams{25.5, 4.5}
    // cppcheck-suppress syntaxError
  ), );

TEST_P(CovarianceStabilityTest, basic) {
  Cov3x3Param::CovMatrix covariance = GetParam().cov;
  std::vector<Real> e_vals{GetParam().e1, GetParam().e2, GetParam().e3};
  const auto scale = 0.01;
  std::vector<Real> filtered_evals;
  auto min_e_val = *std::max_element(e_vals.begin(), e_vals.end()) * scale;
  std::transform(
    e_vals.begin(), e_vals.end(), std::back_inserter(filtered_evals),
    [min_e_val](Real & e) {return e < min_e_val ? min_e_val : e;});

  const auto valid = GetParam().valid;

  EXPECT_EQ(try_stabilize_covariance(covariance, scale), valid);

  if (valid) {
    Eigen::SelfAdjointEigenSolver<Cov3x3Param::CovMatrix> solver;
    solver.compute(covariance);
    Eigen::Vector3d stable_e_vals = solver.eigenvalues();

    std::sort(filtered_evals.begin(), filtered_evals.end());
    std::sort(stable_e_vals.data(), stable_e_vals.data() + stable_e_vals.size());

    for (auto i = 0U; i < filtered_evals.size(); ++i) {
      EXPECT_FLOAT_EQ(filtered_evals[i], stable_e_vals(i));
    }
  }
}

INSTANTIATE_TEST_CASE_P(
  basic, CovarianceStabilityTest,
  ::testing::Values(
    Cov3x3Param(1e-2, 65.0, 24, true),
    Cov3x3Param(-12., 24., 65., false),
    Cov3x3Param(-12., -24., -65., false),
    Cov3x3Param(0.0, 0.0, 0.0, false),
    Cov3x3Param(1.0, 1.0, 0.0, false),
    Cov3x3Param(1e-2, 1e-3, 65, true),
    Cov3x3Param(1e-2, 1e-3, 1e-15, true)
    // cppcheck-suppress syntaxError
  ), );


// --------------------------------------------------------------------------
//                   OpenMS -- Open-Source Mass Spectrometry
// --------------------------------------------------------------------------
// Copyright The OpenMS Team -- Eberhard Karls University Tuebingen,
// ETH Zurich, and Freie Universitaet Berlin 2002-2015.
//
// This software is released under a three-clause BSD license:
//  * Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
//  * Neither the name of any author or any participating institution
//    may be used to endorse or promote products derived from this software
//    without specific prior written permission.
// For a full list of authors, refer to the file AUTHORS.
// --------------------------------------------------------------------------
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL ANY OF THE AUTHORS OR THE CONTRIBUTING
// INSTITUTIONS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
// OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
// OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
// ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// --------------------------------------------------------------------------
// $Maintainer: Hannes Roest $
// $Authors: Hannes Roest $
// --------------------------------------------------------------------------

#include <OpenMS/ANALYSIS/OPENSWATH/OPENSWATHALGO/ALGO/Scoring.h>
#include <OpenMS/ANALYSIS/OPENSWATH/OPENSWATHALGO/Macros.h>
#include <cmath>

#include <boost/numeric/conversion/cast.hpp>

namespace OpenSwath
{
  namespace Scoring
  {

    void normalize_sum(double x[], unsigned int n)
    {
      double sumx = std::accumulate(&x[0], &x[0] + n, 0.0);
      if (sumx == 0.0)
      {
        return;
      } // do not divide by zero
      for (unsigned int i = 0; i < n; i++)
      {
        x[i] = x[i] / sumx;
      }
    }

    double NormalizedManhattanDist(double x[], double y[], int n)
    {
      OPENSWATH_PRECONDITION(n > 0, "Need at least one element");

      double delta_ratio_sum = 0;
      normalize_sum(x, n);
      normalize_sum(y, n);
      for (int i = 0; i < n; i++)
      {
        delta_ratio_sum += std::fabs(x[i] - y[i]);
      }
      return delta_ratio_sum / n;
    }

    double RootMeanSquareDeviation(double x[], double y[], int n)
    {
      OPENSWATH_PRECONDITION(n > 0, "Need at least one element");

      double result = 0;
      for (int i = 0; i < n; i++)
      {

        result += (x[i] - y[i]) * (x[i] - y[i]);
      }
      return std::sqrt(result / n);
    }

    double SpectralAngle(double x[], double y[], int n)
    {
      OPENSWATH_PRECONDITION(n > 0, "Need at least one element");

      double dotprod = 0;
      double x_len = 0;
      double y_len = 0;
      for (int i = 0; i < n; i++)
      {
        dotprod += x[i] * y[i];
        x_len += x[i] * x[i];
        y_len += y[i] * y[i];
      }
      x_len = std::sqrt(x_len);
      y_len = std::sqrt(y_len);

      return std::acos(dotprod / (x_len * y_len));
    }

    XCorrArrayType::iterator xcorrArrayGetMaxPeak(XCorrArrayType& array)
    {
      OPENSWATH_PRECONDITION(array.size() > 0, "Cannot get highest apex from empty array.");

      XCorrArrayType::iterator max_it = array.begin();
      double max = array.begin()->second;
      for (XCorrArrayType::iterator it = array.begin(); it != array.end(); ++it)
      {
        if (it->second > max)
        {
          max = it->second;
          max_it = it;
        }
      }
      return max_it;
    }

    void standardize_data(std::vector<double>& data)
    {
      OPENSWATH_PRECONDITION(data.size() > 0, "Need non-empty array.");

      // subtract the mean and divide by the standard deviation
      double mean = std::accumulate(data.begin(), data.end(), 0.0) / (double) data.size();
      double sqsum = 0;
      for (std::vector<double>::iterator it = data.begin(); it != data.end(); ++it)
      {
        sqsum += (*it - mean) * (*it - mean);
      }
      double std = sqrt(sqsum / data.size()); // standard deviation

      for (std::size_t i = 0; i < data.size(); i++)
      {
        data[i] = (data[i] - mean) / std;
      }
    }

    XCorrArrayType normalizedCrossCorrelation(std::vector<double>& data1,
                                              std::vector<double>& data2, int maxdelay, int lag = 1)
    {
      OPENSWATH_PRECONDITION(data1.size() != 0 && data1.size() == data2.size(), "Both data vectors need to have the same length");

      // normalize the data
      standardize_data(data1);
      standardize_data(data2);
      std::map<int, double> result = calculateCrossCorrelation(data1, data2, maxdelay, lag);
      for (std::map<int, double>::iterator it = result.begin(); it != result.end(); ++it)
      {
        it->second = it->second / data1.size();
      }
      return result;
    }

    XCorrArrayType calculateCrossCorrelation(std::vector<double>& data1,
                                             std::vector<double>& data2, int maxdelay, int lag)
    {
      OPENSWATH_PRECONDITION(data1.size() != 0 && data1.size() == data2.size(), "Both data vectors need to have the same length");

      XCorrArrayType result;
      int datasize = boost::numeric_cast<int>(data1.size());
      int i, j, delay;

      for (delay = -maxdelay; delay <= maxdelay; delay = delay + lag)
      {
        double sxy = 0;
        for (i = 0; i < datasize; ++i)
        {
          j = i + delay;
          if (j < 0 || j >= datasize)
          {
            continue;
          }
          sxy += (data1[i]) * (data2[j]);
        }
        result[delay] = sxy;
      }
      return result;
    }

    XCorrArrayType calcxcorr_legacy_mquest_(std::vector<double>& data1,
                                            std::vector<double>& data2, bool normalize)
    {
      OPENSWATH_PRECONDITION(!data1.empty() && data1.size() == data2.size(), "Both data vectors need to have the same length");
      int maxdelay = boost::numeric_cast<int>(data1.size());
      int lag = 1;

      XCorrArrayType result;
      double mean1 = std::accumulate(data1.begin(), data1.end(), 0.) / (double)data1.size();
      double mean2 = std::accumulate(data2.begin(), data2.end(), 0.) / (double)data2.size();
      double denominator = 1.0;
      int datasize = boost::numeric_cast<int>(data1.size());
      int i, j, delay;

      // Normalized cross-correlation = subtract the mean and divide by the standard deviation
      if (normalize)
      {
        double sqsum1 = 0;
        double sqsum2 = 0;
        for (std::vector<double>::iterator it = data1.begin(); it != data1.end(); ++it)
        {
          sqsum1 += (*it - mean1) * (*it - mean1);
        }

        for (std::vector<double>::iterator it = data2.begin(); it != data2.end(); ++it)
        {
          sqsum2 += (*it - mean2) * (*it - mean2);
        }
        // sigma_1 * sigma_2 * n
        denominator = sqrt(sqsum1 * sqsum2);
      }

      for (delay = -maxdelay; delay <= maxdelay; delay = delay + lag)
      {
        double sxy = 0;
        for (i = 0; i < datasize; i++)
        {
          j = i + delay;
          if (j < 0 || j >= datasize)
          {
            continue;
          }
          if (normalize)
          {
            sxy += (data1[i] - mean1) * (data2[j] - mean2);
          }
          else
          {
            sxy += (data1[i]) * (data2[j]);
          }
        }

        if (denominator > 0)
        {
          result[delay] = sxy / denominator;
        }
        else
        {
          // e.g. if all datapoints are zero
          result[delay] = 0;
        }
      }
      return result;
    }

  } //end namespace Scoring
}


/******************************************************************
 *
 * uEcho for C
 *
 * Copyright (C) Satoshi Konno 2015
 *
 * This is licensed under BSD-style license, see file COPYING.
 *
 ******************************************************************/

#include <boost/test/unit_test.hpp>

#include <stdlib.h>

#include <uecho/util/bytes.h>

BOOST_AUTO_TEST_CASE(Bytes)
{
  byte* ibytes;
  int n;

  for (n = 0; n <= 0xFF; n++) {
    ibytes = uecho_bytes_fromint(n, 1);
    BOOST_CHECK_EQUAL(uecho_bytes_toint(ibytes, 1), n);
    free(ibytes);
  }

  for (n = 0; n <= 0xFFFF; n += (0xFFFF / 0xFF)) {
    ibytes = uecho_bytes_fromint(n, 2);
    BOOST_CHECK_EQUAL(uecho_bytes_toint(ibytes, 2), n);
    free(ibytes);
  }

  for (n = 0; n <= 0xFFFFFF; n += (0xFFFFFF / 0xFFFF)) {
    ibytes = uecho_bytes_fromint(n, 3);
    BOOST_CHECK_EQUAL(uecho_bytes_toint(ibytes, 3), n);
    free(ibytes);
  }
}