#ifndef PROBLEM_HPP
#define PROBLEM_HPP

#include <polyfem/ElementBases.hpp>
#include <polyfem/LocalBoundary.hpp>
#include <polyfem/Mesh.hpp>

#include <polyfem/Common.hpp>

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

namespace polyfem
{
	class Problem
	{
	public:
		Problem(const std::string &name);
		virtual void init(const Mesh &mesh) { }

		virtual void rhs(const std::string &formulation, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const = 0;
		virtual bool is_rhs_zero() const = 0;

		virtual void bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const = 0;
		virtual void velocity_bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const { }
		virtual void acceleration_bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const { }

		virtual void neumann_bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const { }
		virtual void neumann_velocity_bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const { }
		virtual void neumann_acceleration_bc(const Mesh &mesh, const Eigen::MatrixXi &global_ids, const Eigen::MatrixXd &uv, const Eigen::MatrixXd &pts, const double t, Eigen::MatrixXd &val) const { }

		virtual void exact(const Eigen::MatrixXd &pts, Eigen::MatrixXd &val) const { };
		virtual void exact_grad(const Eigen::MatrixXd &pts, Eigen::MatrixXd &val) const { };

		inline const std::string &name() const { return name_; }

		virtual bool has_exact_sol() const = 0;
		virtual bool is_scalar() const = 0;
		// virtual bool is_mixed() const { return false; }
		virtual bool is_linear_in_time() const { return true; }

		virtual bool is_time_dependent() const { return false; }
		virtual void initial_solution(const Eigen::MatrixXd &pts, Eigen::MatrixXd &val) const { }
		virtual void initial_velocity(const Eigen::MatrixXd &pts, Eigen::MatrixXd &val) const { }
		virtual void initial_acceleration(const Eigen::MatrixXd &pts, Eigen::MatrixXd &val) const { }

		virtual void set_parameters(const json &params) { }

		void setup_bc(const Mesh &mesh, const std::vector< ElementBases > &bases, std::vector< LocalBoundary > &local_boundary, std::vector< int > &boundary_nodes, std::vector< LocalBoundary > &local_neumann_boundary);

		// std::vector<int> &boundary_ids() { return boundary_ids_; }
		// const std::vector<int> &boundary_ids() const { return boundary_ids_; }

		virtual bool is_dimention_dirichet(const int tag, const int dim) const { return true; }

		//here for efficiency reasons
		virtual bool all_dimentions_dirichelt() const { return true; }

		virtual ~Problem() { }
	protected:
		std::vector<int> boundary_ids_;
		std::vector<int> neumann_boundary_ids_;

	private:
		std::string name_;
	};

	class ProblemFactory
	{
	public:
		static const ProblemFactory &factory();

		std::shared_ptr<Problem> get_problem(const std::string &problem) const;
		inline const std::vector<std::string> &get_problem_names() const { return problem_names_; }

	private:
		ProblemFactory();
		std::map<std::string, std::shared_ptr<Problem>> problems_;
		std::vector<std::string> problem_names_;
	};
}

#endif //PROBLEM_HPP


#pragma once

#include <variant>
#include <immer/vector.hpp>
#include <optional>
#include <string>

#include <boost/hana.hpp>

#include "calc/fwd.hpp"

namespace calc::buttons {
    namespace hana = boost::hana;

    namespace traits {
        struct rhs_bound { number_t rhs; };
        struct stateful { };
        struct mutable_rhs { };
        struct holdable { };

        template <class Tag>
        constexpr auto has_trait = hana::partial(
            hana::trait<std::is_base_of>,
            hana::type_c<Tag>
        );

        constexpr auto is_rhs_bound    = has_trait<rhs_bound>;
        constexpr auto is_stateful     = has_trait<stateful>;
        constexpr auto has_mutable_rhs = has_trait<mutable_rhs>;
        constexpr auto is_holdable     = has_trait<holdable>;
    }

    struct add:      traits::rhs_bound, traits::mutable_rhs { };
    struct sub:      traits::rhs_bound, traits::mutable_rhs { };
    struct mul:      traits::rhs_bound, traits::mutable_rhs { };
    struct div:      traits::rhs_bound, traits::mutable_rhs { };
    struct power:    traits::rhs_bound, traits::mutable_rhs { };
    struct append:   traits::rhs_bound, traits::mutable_rhs { };
    struct meta_add: traits::rhs_bound                      { };

    struct rotate_left  { };
    struct rotate_right { };
    struct erase_last   { };
    struct opposite     { };
    struct reverse      { };
    struct sum          { };
    struct mirror       { };
    struct inv10        { };

    struct replace: traits::stateful {
        replace() = default;
        replace(std::string from, std::string to): from(from), to(to) { }

        std::string from, to;
    };

    struct store: traits::stateful, traits::holdable {
        std::optional<number_t> stored;
    };

    using button_v = std::variant<
        add, sub, mul, div,
        power, append, meta_add,
        rotate_left, rotate_right,
        erase_last, opposite,
        reverse, sum,
        mirror, inv10,
        replace, store
    >;

    using buttons_t = immer::vector<button_v>;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/include/functions/dist.hpp>
#include <nt2/include/functions/splat.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/sdk/simd/io.hpp>

#include <nt2/include/constants/three.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/valmax.hpp>
#include <nt2/include/constants/valmin.hpp>
#include <nt2/include/constants/ten.hpp>
#include <nt2/include/constants/mten.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>

#include <boost/dispatch/functor/meta/call.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>

NT2_TEST_CASE_TPL ( dist_real,  BOOST_SIMD_SIMD_REAL_TYPES)
{
  using nt2::dist;
  using nt2::tag::dist_;
  using boost::simd::native;

  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;

  NT2_TEST_TYPE_IS( typename boost::dispatch::meta::call<dist_(vT,vT)>::type
                  , vT
                  );

#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_EQUAL(dist(nt2::Inf<vT>() , nt2::Inf<vT>()) , nt2::Nan<vT>());
  NT2_TEST_EQUAL(dist(nt2::Minf<vT>(), nt2::Minf<vT>()), nt2::Nan<vT>());
  NT2_TEST_EQUAL(dist(nt2::Nan<vT>() , nt2::Nan<vT>()) , nt2::Nan<vT>());
#endif

  NT2_TEST_EQUAL(dist(nt2::Zero<vT>(), nt2::Zero<vT>()), nt2::Zero<vT>());
  NT2_TEST_EQUAL(dist(nt2::Mone<vT>(), nt2::One<vT>()), nt2::Two<vT>());
  NT2_TEST_EQUAL(dist(nt2::One<vT>(), nt2::Three<vT>()), nt2::Two<vT>());
}

NT2_TEST_CASE_TPL ( dist_integer_ui,  BOOST_SIMD_SIMD_UNSIGNED_TYPES)
{
  using nt2::dist;
  using nt2::tag::dist_;
  using boost::simd::native;

  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;

  NT2_TEST_TYPE_IS( typename boost::dispatch::meta::call<dist_(vT,vT)>::type
                  , vT
                  );

  NT2_TEST_EQUAL(dist(nt2::Zero<vT>(), nt2::Zero<vT>()), nt2::Zero<vT>());
  NT2_TEST_EQUAL(dist(nt2::One<vT>(), nt2::Three<vT>()), nt2::Two<vT>());
  NT2_TEST_EQUAL(dist(nt2::Valmax<vT>(), nt2::Zero<vT>()), nt2::Valmax<vT>());
  NT2_TEST_EQUAL(dist(nt2::Zero<vT>(), nt2::Valmax<vT>()), nt2::Valmax<vT>());
}

NT2_TEST_CASE_TPL ( dist_integer_si,  BOOST_SIMD_SIMD_INTEGRAL_SIGNED_TYPES)
{
  using nt2::dist;
  using nt2::tag::dist_;
  using boost::simd::native;

  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;

  NT2_TEST_TYPE_IS( typename boost::dispatch::meta::call<dist_(vT,vT)>::type
                  , vT
                  );

  NT2_TEST_EQUAL(dist(nt2::Zero<vT>(), nt2::Zero<vT>()), nt2::Zero<vT>());
  NT2_TEST_EQUAL(dist(nt2::Mone<vT>(), nt2::One<vT>()), nt2::Two<vT>());
  NT2_TEST_EQUAL(dist(nt2::One<vT>(), nt2::Three<vT>()), nt2::Two<vT>());
  NT2_TEST_EQUAL(dist(nt2::Valmax<vT>(), nt2::Zero<vT>()), nt2::Valmax<vT>());
  NT2_TEST_EQUAL(dist(nt2::Zero<vT>(), nt2::Valmax<vT>()), nt2::Valmax<vT>());

  NT2_TEST_EQUAL(dist(nt2::Ten<vT>(), nt2::Mten<vT>()), nt2::splat<vT>(20));
  NT2_TEST_EQUAL(dist(nt2::Mten<vT>(), nt2::Ten<vT>()), nt2::splat<vT>(20));
}


/* @file  point.hpp
 * @date  <date>
 * @brief <Descriptions>
 *
 * Copyright (C) 2020 NIIC EDA
 * All rights reserved.
 *
 * This software may be modified and distributed under the terms
 * of the BSD license.  See the LICENSE file for details.
 */

#ifndef EDI_GEO_POINT_HPP_
#define EDI_GEO_POINT_HPP_

#include <boost/geometry.hpp>

#include "util/util.h"

EDI_BEGIN_NAMESPACE

namespace geo {

/// @brief Point in cartesian coordinate system
template <typename CoordinateType, std::size_t DimensionCount>
class BasePoint : public std::array<CoordinateType, DimensionCount> {
  public:
    using BaseType = std::array<CoordinateType, DimensionCount>;

    /// @brief default constructor
    BasePoint() : BaseType() {
        this->fill(std::numeric_limits<CoordinateType>::max());
    }

    /// @brief constructor
    template <class... Args>
    BasePoint(CoordinateType const& first, Args... args)
        : BaseType({first, args...}) {}

    /// @brief copy constructor
    BasePoint(BasePoint const& rhs) : BaseType(rhs) {}

    /// @brief move constructor
    BasePoint(BasePoint&& rhs) : BaseType(std::move(rhs)) {}

    /// @brief copy assignment
    BasePoint& operator=(BasePoint const& rhs) {
        if (this != &rhs) {
            this->BaseType::operator=(rhs);
        }
        return *this;
    }

    /// @brief copy assignment
    BasePoint& operator=(BasePoint&& rhs) {
        if (this != &rhs) {
            this->BaseType::operator=(std::move(BaseType(rhs)));
        }
        return *this;
    }

    /// @brief getter for data
    template <std::size_t Dimension>
    CoordinateType const& get() const {
        static_assert(Dimension < DimensionCount,
                      "Dimension < DimensionCount failed");
        return (*this)[Dimension];
    }
    /// @brief setter for data
    template <std::size_t Dimension>
    void set(CoordinateType const& v) {
        static_assert(Dimension < DimensionCount,
                      "Dimension < DimensionCount failed");
        (*this)[Dimension] = v;
    }
    /// @brief setter for data
    template <class... Args>
    void set(CoordinateType const& first, Args... args) {
        *this = {first, args...};
    }

    /// @brief summarize memory usage of the object in bytes
    std::size_t memory() const { return sizeof(*this); }

  protected:
    /// @brief overload output stream
    friend OStreamBase& operator<<(OStreamBase& os, BasePoint const& rhs) {
        os << DataTypeName(className(rhs));
        os << DataBegin("(");
        auto delimiter = DataDelimiter("");
        for (std::size_t i = 0; i < DimensionCount; ++i) {
            os << delimiter << rhs[i];
            delimiter = DataDelimiter();
        }
        os << DataEnd(")");
        return os;
    }
    /// @brief overload input stream
    friend IStreamBase& operator>>(IStreamBase& is, BasePoint& rhs) {
        is >> DataTypeName(className(rhs));
        is >> DataBegin("(");
        auto delimiter = DataDelimiter("");
        for (std::size_t i = 0; i < DimensionCount; ++i) {
            is >> delimiter >> rhs[i];
            delimiter = DataDelimiter();
        }
        is >> DataEnd(")");
        return is;
    }
};

/// @brief Point class for specialization
template <typename CoordinateType, std::size_t DimensionCount>
class Point;

/// @brief Specialization for 2D point
template <typename CoordinateType>
class Point<CoordinateType, 2> : public BasePoint<CoordinateType, 2> {
  public:
    using BaseType = BasePoint<CoordinateType, 2>;

    /// @brief default constructor
    Point() : BaseType() {}
    /// @brief constructor
    Point(CoordinateType const& v0, CoordinateType const& v1)
        : BaseType(v0, v1) {}
    /// @brief copy constructor
    Point(Point const& rhs) : BaseType(rhs) {}
    /// @brief move constructor
    Point(Point&& rhs) : BaseType(std::move(rhs)) {}
    /// @brief copy assignment
    Point& operator=(Point const& rhs) {
        if (this != &rhs) {
            this->BaseType::operator=(rhs);
        }
        return *this;
    }
    /// @brief move assignment
    Point& operator=(Point&& rhs) {
        if (this != &rhs) {
            this->BaseType::operator=(std::move(rhs));
        }
        return *this;
    }

    /// @brief getter for x
    CoordinateType const& getX() const {
        return this->BaseType::template get<0>();
    }
    /// @brief getter for x
    CoordinateType const& x() const { return this->getX(); }
    /// @brief getter for y
    CoordinateType const& getY() const {
        return this->BaseType::template get<1>();
    }
    /// @brief getter for y
    CoordinateType const& y() const { return this->getY(); }
    /// @brief getter general
    template <std::size_t Dimension>
    CoordinateType const& get() const {
        return this->BaseType::template get<Dimension>();
    }
    /// @brief setter for x
    void setX(CoordinateType const& v) { this->BaseType::template set<0>(v); }
    /// @brief setter for y
    void setY(CoordinateType const& v) { this->BaseType::template set<1>(v); }
    /// @brief setter general
    template <std::size_t Dimension>
    void set(CoordinateType const& v) {
        this->BaseType::template set<Dimension>(v);
    }
    /// @brief setter for x and y
    void set(CoordinateType const& v0, CoordinateType const& v1) {
        this->BaseType::template set(v0, v1);
    }

    /// @brief summarize memory usage of the object in bytes
    std::size_t memory() const { return this->BaseType::memory(); }

  protected:
    /// @brief overload output stream
    friend OStreamBase& operator<<(OStreamBase& os, Point const& rhs) {
        os << DataTypeName(className(rhs)) << DataBegin("(");
        Point::BaseType const& base = rhs;
        os << base;
        os << DataEnd(")");
        return os;
    }
    /// @brief overload input stream
    friend IStreamBase& operator>>(IStreamBase& is, Point& rhs) {
        is >> DataTypeName(className(rhs)) >> DataBegin("(");
        Point::BaseType& base = rhs;
        is >> base;
        is >> DataEnd(")");
        return is;
    }
};

/// @brief Specialization for 3D point
template <typename CoordinateType>
class Point<CoordinateType, 3> : public BasePoint<CoordinateType, 3> {
  public:
    using BaseType = BasePoint<CoordinateType, 3>;

    /// @brief default constructor
    Point() : BaseType() {}
    /// @brief constructor
    Point(CoordinateType const& v0, CoordinateType const& v1,
          CoordinateType const& v2)
        : BaseType(v0, v1, v2) {}
    /// @brief copy constructor
    Point(Point const& rhs) : BaseType(rhs) {}
    /// @brief move constructor
    Point(Point&& rhs) : BaseType(std::move(rhs)) {}
    /// @brief copy assignment
    Point& operator=(Point const& rhs) {
        if (this != &rhs) {
            this->BaseType::operator=(rhs);
        }
        return *this;
    }
    /// @brief move assignment
    Point& operator=(Point&& rhs) {
        if (this != &rhs) {
            this->BaseType::operator=(std::move(rhs));
        }
        return *this;
    }

    /// @brief getter for x
    CoordinateType const& getX() const {
        return this->BaseType::template get<0>();
    }
    /// @brief getter for x
    CoordinateType const& x() const { return this->getX(); }
    /// @brief getter for y
    CoordinateType const& getY() const {
        return this->BaseType::template get<1>();
    }
    /// @brief getter for y
    CoordinateType const& y() const { return this->getY(); }
    /// @brief getter for z
    CoordinateType const& getZ() const {
        return this->BaseType::template get<2>();
    }
    /// @brief getter for z
    CoordinateType const& z() const { return this->getZ(); }
    /// @brief getter general
    template <std::size_t Dimension>
    CoordinateType const& get() const {
        return this->BaseType::template get<Dimension>();
    }
    /// @brief setter for x
    void setX(CoordinateType const& v) { this->BaseType::template set<0>(v); }
    /// @brief setter for y
    void setY(CoordinateType const& v) { this->BaseType::template set<1>(v); }
    /// @brief setter for z
    void setZ(CoordinateType const& v) { this->BaseType::template set<2>(v); }
    /// @brief setter general
    template <std::size_t Dimension>
    void set(CoordinateType const& v) {
        this->BaseType::template set<Dimension>(v);
    }
    /// @brief setter for x, y, z
    void set(CoordinateType const& v0, CoordinateType const& v1,
             CoordinateType const& v2) {
        this->BaseType::template set(v0, v1, v2);
    }

    /// @brief summarize memory usage of the object in bytes
    std::size_t memory() const { return this->BaseType::memory(); }

  protected:
    /// @brief overload output stream
    friend OStreamBase& operator<<(OStreamBase& os, Point const& rhs) {
        os << DataTypeName(className(rhs)) << DataBegin("(");
        Point::BaseType const& base = rhs;
        os << base;
        os << DataEnd(")");
        return os;
    }
    /// @brief overload input stream
    friend IStreamBase& operator>>(IStreamBase& is, Point& rhs) {
        is >> DataTypeName(className(rhs)) >> DataBegin("(");
        Point::BaseType& base = rhs;
        is >> base;
        is >> DataEnd(")");
        return is;
    }
};  // namespace geo

}  // namespace geo

EDI_END_NAMESPACE

namespace boost {
namespace geometry {
namespace traits {

//////// for point ////////
template <typename CoordinateType, std::size_t DimensionCount>
struct tag<EDI_NAMESPACE::geo::Point<CoordinateType, DimensionCount>> {
    typedef point_tag type;
};

template <typename CoordinateType, std::size_t DimensionCount>
struct point_type<EDI_NAMESPACE::geo::Point<CoordinateType, DimensionCount>> {
    typedef typename EDI_NAMESPACE::geo::Point<CoordinateType, DimensionCount>
        type;
};

template <typename CoordinateType, std::size_t DimensionCount>
struct coordinate_type<
    EDI_NAMESPACE::geo::Point<CoordinateType, DimensionCount>> {
    typedef CoordinateType type;
};

template <typename CoordinateType, std::size_t DimensionCount>
struct coordinate_system<
    EDI_NAMESPACE::geo::Point<CoordinateType, DimensionCount>> {
    typedef boost::geometry::cs::cartesian type;
};

template <typename CoordinateType, std::size_t DimensionCount>
struct dimension<EDI_NAMESPACE::geo::Point<CoordinateType, DimensionCount>>
    : boost::mpl::int_<DimensionCount> {};

template <typename CoordinateType, std::size_t DimensionCount,
          std::size_t Dimension>
struct access<EDI_NAMESPACE::geo::Point<CoordinateType, DimensionCount>,
              Dimension> {
    static inline CoordinateType get(
        EDI_NAMESPACE::geo::Point<CoordinateType, DimensionCount> const& p) {
        return p.template get<Dimension>();
    }

    static inline void set(
        EDI_NAMESPACE::geo::Point<CoordinateType, DimensionCount>& p,
        CoordinateType const& value) {
        p.template set<Dimension>(value);
    }
};

}  // namespace traits
}  // namespace geometry
}  // namespace boost

#endif


#include <cassert>
#include <iostream>
#include <boost/timer.hpp>

const unsigned rep= 500000;

template <typename T>
class vector
{
    void check_size(int that_size) const { assert(my_size == that_size); }
    void check_index(int i) const { assert(i >= 0 && i < my_size); }
  public:
    explicit vector(int size)
      : my_size(size), data( new T[my_size] )
    {}

    vector()
      : my_size(0), data(0)
    {}

    vector( const vector& that )
      : my_size(that.my_size), data( new T[my_size] )
    {
	for (int i= 0; i < my_size; ++i)
	    data[i]= that.data[i];
    }

    ~vector() { if (data) delete [] data ; }

    vector& operator=( const vector& that )
    {
	check_size(that.my_size);
	for (int i= 0; i < my_size; ++i)
	    data[i]= that.data[i];
    }

    int size() const { return my_size ; }

    const T& operator[]( int i ) const
    {
	check_index(i);
	return data[i];
    }

    T& operator[]( int i )
    {
	check_index(i);
	return data[i] ;
    }

  private:
    int   my_size ;
    T*    data ;
};

template <typename T>
std::ostream& operator<<(std::ostream& os, const vector<T>& v)
{
  os << '[';
  for (int i= 0; i < v.size(); ++i) os << v[i] << ',';
  os << ']';
  return os ;
}

template <unsigned Offset, unsigned Max>
struct my_axpy_ftor
{
    template <typename Vector>
    void operator()(Vector& u, const Vector& v, const Vector& w, unsigned i)
    {
	u[i+Offset]= 3.0f * v[i+Offset] + w[i+Offset];
	my_axpy_ftor<Offset+1, Max>()(u, v, w, i);
    }
};

template <unsigned Max>
struct my_axpy_ftor<Max, Max>
{
    template <typename Vector>
    void operator()(Vector& u, const Vector& v, const Vector& w, unsigned i) {}
};

template <unsigned BSize, typename Vector>
void inline my_axpy(Vector& u, const Vector& v, const Vector& w)
{
    unsigned s= u.size(), sb= s / BSize * BSize;

    for (unsigned i= 0; i < sb; i+= BSize)
	my_axpy_ftor<0, BSize>()(u, v, w, i);

    for (unsigned i= sb; i < s; i++)
	u[i]= 3.0f * v[i] + w[i];
}

template <unsigned BSize, typename Vector>
void test(Vector& u, const Vector& v, const Vector& w)
{
    boost::timer unrolled;
    for (unsigned j= 0; j < rep; j++)
	my_axpy<BSize>(u, v, w);
    std::cout << "Unrolled<" << BSize << "> loop takes "
	      << unrolled.elapsed() / double(rep) * 1000000 << "µs\n";
    std::cout << u << '\n';
}

int main(int argc, char** argv)
{
    unsigned s= 1000;
    if (argc > 1) s= atoi(argv[1]);
    vector<float> u(s), v(s), w(s);

    for (unsigned i= 0; i < s; i++) {
	v[i]= float(i);
	w[i]= float(2*i + 15);
    }

    for (unsigned j= 0; j < 3; j++)
	for (unsigned i= 0; i < s; i++)
	    u[i]= 3.0f * v[i] + w[i];

    boost::timer unrolled;
    for (unsigned j= 0; j < rep; j++) {
	unsigned sb= s / 4 * 4;
	for (unsigned i= 0; i < sb; i+= 4) {
	    u[i]= 3.0f * v[i] + w[i];
	    u[i+1]= 3.0f * v[i+1] + w[i+1];
	    u[i+2]= 3.0f * v[i+2] + w[i+2];
	    u[i+3]= 3.0f * v[i+3] + w[i+3];
	}
	for (unsigned i= sb; i < s; i++)
	    u[i]= 3.0f * v[i] + w[i];
    }
    std::cout << "Unrolled loop takes " << unrolled.elapsed() / double(rep) * 1000000 << "µs\n";
    std::cout << u << '\n';

    boost::timer native;
    for (unsigned j= 0; j < rep; j++)
	for (unsigned i= 0; i < s; i++)
	    u[i]= 3.0f * v[i] + w[i];
    std::cout << "Native loop takes " << native.elapsed() / double(rep) * 1000000 << "µs\n";
    std::cout << u << '\n';

    test<1>(u, v, w);
    test<2>(u, v, w);
    test<4>(u, v, w);
    test<6>(u, v, w);
    test<8>(u, v, w);
    test<10>(u, v, w);
    test<12>(u, v, w);
    test<16>(u, v, w);
    test<32>(u, v, w);
    test<64>(u, v, w);

    return 0 ;
}


//
// Copyright (c) 2002 Kresimir Fresl
// Copyright (c) 2010 Rutger ter Borg
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef BOOST_NUMERIC_BINDINGS_UBLAS_BANDED_HPP
#define BOOST_NUMERIC_BINDINGS_UBLAS_BANDED_HPP

#include <boost/numeric/bindings/begin.hpp>
#include <boost/numeric/bindings/detail/adaptor.hpp>
#include <boost/numeric/bindings/detail/if_row_major.hpp>
#include <boost/numeric/bindings/end.hpp>
#include <boost/numeric/bindings/ublas/detail/basic_ublas_adaptor.hpp>
#include <boost/numeric/bindings/ublas/detail/convert_to.hpp>
#include <boost/numeric/bindings/ublas/matrix_expression.hpp>
#include <boost/numeric/bindings/value_type.hpp>
#include <boost/numeric/ublas/banded.hpp>

namespace boost {
namespace numeric {
namespace bindings {
namespace detail {

template< typename T, typename F, typename A, typename Id, typename Enable >
struct adaptor< ublas::banded_matrix< T, F, A >, Id, Enable > {

    // The ublas banded row_major format corresponds to the LAPACK band format.
    // LAPACK is column_major; so we flip the data order reported by uBLAS.
    typedef typename copy_const< Id, T >::type value_type;
    // typedef typename if_row_major<
    //             typename convert_to< tag::data_order, F >::type,
    //             tag::column_major,
    //             tag::row_major
    //         >::type data_order;
    typedef typename convert_to< tag::data_order, F >::type data_order;
    typedef mpl::map<
        mpl::pair< tag::value_type, value_type >,
        mpl::pair< tag::entity, tag::matrix >,
        mpl::pair< tag::size_type<1>, std::ptrdiff_t >,
        mpl::pair< tag::size_type<2>, std::ptrdiff_t >,
        mpl::pair< tag::matrix_type, tag::band >,
        mpl::pair< tag::data_structure, tag::band_array >,
        mpl::pair< tag::data_order, data_order >,
        mpl::pair< tag::bandwidth_type<1>, std::ptrdiff_t >,
        mpl::pair< tag::bandwidth_type<2>, std::ptrdiff_t >,
        mpl::pair< tag::stride_type<1>,
            typename if_row_major< data_order, std::ptrdiff_t, tag::contiguous >::type >,
        mpl::pair< tag::stride_type<2>,
            typename if_row_major< data_order, tag::contiguous, std::ptrdiff_t >::type >
    > property_map;

    static std::ptrdiff_t size1( const Id& id ) {
        return id.size1();
    }

    static std::ptrdiff_t size2( const Id& id ) {
        return id.size2();
    }

    static value_type* begin_value( Id& id ) {
        return bindings::begin_value( id.data() );
    }

    static value_type* end_value( Id& id ) {
        return bindings::end_value( id.data() );
    }

    // A.k.a. left half-bandwidth
    static std::ptrdiff_t bandwidth1( const Id& id ) {
        return id.lower();
    }

    // A.k.a. right half-bandwidth
    static std::ptrdiff_t bandwidth2( const Id& id ) {
        return id.upper();
    }

    // These strides are over the band array structure; not over
    // the band matrix representation of this structure
    static std::ptrdiff_t stride1( const Id& id ) {
        return id.lower() + id.upper() + 1;
    }

    static std::ptrdiff_t stride2( const Id& id ) {
        return id.lower() + id.upper() + 1;
    }

};

template< typename T, typename Id, typename Enable >
struct adaptor< ublas::banded_adaptor< T >, Id, Enable >:
    basic_ublas_adaptor<
        T,
        Id,
        mpl::pair< tag::matrix_type, tag::band >,
        mpl::pair< tag::bandwidth_type<1>, std::ptrdiff_t >,
        mpl::pair< tag::bandwidth_type<2>, std::ptrdiff_t >
    > {

    // A.k.a. left half-bandwidth
    static std::ptrdiff_t bandwidth1( const Id& id ) {
        return id.lower();
    }

    // A.k.a. right half-bandwidth
    static std::ptrdiff_t bandwidth2( const Id& id ) {
        return id.upper();
    }

};

} // detail
} // bindings
} // numeric
} // boost

#endif


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

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

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISTANCE_POLYGON_TO_SEGMENT_OR_BOX_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISTANCE_POLYGON_TO_SEGMENT_OR_BOX_HPP

#include <boost/range.hpp>

#include <boost/geometry/core/exterior_ring.hpp>
#include <boost/geometry/core/interior_rings.hpp>
#include <boost/geometry/core/point_type.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/strategies/distance.hpp>
#include <boost/geometry/strategies/distance_comparable_to_regular.hpp>
#include <boost/geometry/strategies/tags.hpp>

#include <boost/geometry/algorithms/assign.hpp>
#include <boost/geometry/algorithms/within.hpp>
#include <boost/geometry/algorithms/intersects.hpp>

#include <boost/geometry/util/math.hpp>

#include <boost/geometry/algorithms/dispatch/distance.hpp>

#include <boost/geometry/algorithms/detail/distance/point_to_geometry.hpp>
#include <boost/geometry/algorithms/detail/distance/range_to_segment_or_box.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace distance
{

template <typename Polygon, typename SegmentOrBox, typename Strategy>
class polygon_to_segment_or_box
{
private:
    typedef typename strategy::distance::services::comparable_type
       <
           Strategy
       >::type comparable_strategy;

    typedef typename strategy::distance::services::return_type
        <
            comparable_strategy,
            typename point_type<Polygon>::type,
            typename point_type<SegmentOrBox>::type
        >::type comparable_return_type;

public:
    typedef typename strategy::distance::services::return_type
        <
            Strategy,
            typename point_type<Polygon>::type,
            typename point_type<SegmentOrBox>::type
        >::type return_type;

    static inline return_type apply(Polygon const& polygon,
                                    SegmentOrBox const& segment_or_box,
                                    Strategy const& strategy)
    {
        typedef typename geometry::ring_type<Polygon>::type e_ring;
        typedef typename geometry::interior_type<Polygon>::type i_rings;
        typedef typename range_value<i_rings>::type i_ring;

        if ( geometry::intersects(polygon, segment_or_box) )
        {
            return 0;
        }

        e_ring const& ext_ring = geometry::exterior_ring<Polygon>(polygon);
        i_rings const& int_rings = geometry::interior_rings<Polygon>(polygon);

        comparable_strategy cstrategy =
            strategy::distance::services::get_comparable
                <
                    Strategy
                >::apply(strategy);

        comparable_return_type cd_min = range_to_segment_or_box
            <
                e_ring, SegmentOrBox, comparable_strategy
            >::apply(ext_ring, segment_or_box, cstrategy, false);

        typedef typename boost::range_iterator<i_rings const>::type iterator_type;
        for (iterator_type it = boost::begin(int_rings);
             it != boost::end(int_rings); ++it)
        {
            comparable_return_type cd = range_to_segment_or_box
                <
                    i_ring, SegmentOrBox, comparable_strategy
                >::apply(*it, segment_or_box, cstrategy, false);

            if ( cd < cd_min )
            {
                cd_min = cd;
            }
        }

        return strategy::distance::services::comparable_to_regular
            <
                comparable_strategy,
                Strategy,
                Polygon,
                SegmentOrBox
            >::apply(cd_min);
    }
};

}} // namespace detail::distance
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DETAIL
namespace dispatch
{

template <typename Polygon, typename Segment, typename Strategy>
struct distance
    <
        Polygon, Segment, Strategy, polygon_tag, segment_tag,
        strategy_tag_distance_point_segment, false
    >
    : detail::distance::polygon_to_segment_or_box<Polygon, Segment, Strategy>
{};

template <typename Polygon, typename Box, typename Strategy>
struct distance
    <
        Polygon, Box, Strategy, polygon_tag, box_tag,
        strategy_tag_distance_point_segment, false
    >
    : detail::distance::polygon_to_segment_or_box<Polygon, Box, Strategy>
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISTANCE_POLYGON_TO_SEGMENT_OR_BOX_HPP


// File: geometry.hpp
// Project: core
// Created Date: 14/04/2021
// Author: Shun Suzuki
// -----
// Last Modified: 22/09/2021
// Modified By: Shun Suzuki (suzuki@hapis.k.u-tokyo.ac.jp)
// -----
// Copyright (c) 2021 Hapis Lab. All rights reserved.
//

#pragma once

#if _MSC_VER
#pragma warning(push)
#pragma warning(disable : 6031 6255 6294 26450 26451 26454 26495 26812)
#endif
#if defined(__GNUC__) && !defined(__llvm__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
#endif
#include <Eigen/Dense>
#if _MSC_VER
#pragma warning(pop)
#endif
#if defined(__GNUC__) && !defined(__llvm__)
#pragma GCC diagnostic pop
#endif

#include <map>
#include <memory>
#include <utility>
#include <vector>

#include "hardware_defined.hpp"

namespace autd {
namespace core {

class Geometry;
using GeometryPtr = std::unique_ptr<Geometry>;

using Vector3 = Eigen::Matrix<double, 3, 1>;
using Vector4 = Eigen::Matrix<double, 4, 1>;
using Matrix4X4 = Eigen::Matrix<double, 4, 4>;
using Quaternion = Eigen::Quaternion<double>;

/**
 * \brief Device contains an AUTD device geometry.
 */
struct Device {
  Device(const Vector3& position, const Quaternion& quaternion)
      : x_direction(quaternion * Vector3(1, 0, 0)),
        y_direction(quaternion * Vector3(0, 1, 0)),
        z_direction(quaternion * Vector3(0, 0, 1)),
        global_trans_positions(std::make_unique<Vector3[]>(NUM_TRANS_IN_UNIT)) {
    const Eigen::Transform<double, 3, Eigen::Affine> transform_matrix = Eigen::Translation<double, 3>(position) * quaternion;
    auto index = 0;
    for (size_t y = 0; y < NUM_TRANS_Y; y++)
      for (size_t x = 0; x < NUM_TRANS_X; x++) {
        if (is_missing_transducer(x, y)) continue;
        const Vector4 local_pos = Vector4(static_cast<double>(x) * TRANS_SPACING_MM, static_cast<double>(y) * TRANS_SPACING_MM, 0, 1);
        const Vector4 global_pos = transform_matrix * local_pos;
        global_trans_positions[index++] = Vector3(global_pos[0], global_pos[1], global_pos[2]);
      }
    g2l = transform_matrix.inverse();
  }

  Device(const Vector3& position, const Vector3& euler_angles)
      : Device(position, Eigen::AngleAxis<double>(euler_angles.x(), Vector3::UnitZ()) * Eigen::AngleAxis<double>(euler_angles.y(), Vector3::UnitY()) *
                             Eigen::AngleAxis<double>(euler_angles.z(), Vector3::UnitZ())) {}

  Vector3 x_direction;
  Vector3 y_direction;
  Vector3 z_direction;
  std::unique_ptr<Vector3[]> global_trans_positions;
  Eigen::Transform<double, 3, Eigen::Affine> g2l;
};

/**
 * @brief Geometry of all devices
 */
class Geometry {
 public:
  Geometry() : _wavelength(8.5), _attenuation(0) {}
  ~Geometry() = default;
  Geometry(const Geometry& v) noexcept = default;
  Geometry& operator=(const Geometry& obj) = default;
  Geometry(Geometry&& obj) = default;
  Geometry& operator=(Geometry&& obj) = default;

  /**
   * @brief  Add new device with position and rotation. Note that the transform is done with order: Translate -> Rotate
   * @param position Position of transducer #0, which is the one at the lower-left corner.
   * (The lower-left corner is the one with the two missing transducers.)
   * @param euler_angles ZYZ convention euler angle of the device
   * @param group Grouping ID of the device
   * @return an id of added device
   */
  size_t add_device(const Vector3& position, const Vector3& euler_angles, const size_t group = 0) {
    const auto device_id = this->_devices.size();
    this->_devices.emplace_back(position, euler_angles);
    this->_group_map[device_id] = group;
    return device_id;
  }

  /**
   * @brief Same as add_device(const Vector3&, const Vector3&, const size_t), but using quaternion rather than zyz euler angles.
   * @param position Position of transducer #0, which is the one at the lower-left corner.
   * @param quaternion rotation quaternion of the device.
   * @param group Grouping ID
   * @return an id of added device
   */
  size_t add_device(const Vector3& position, const Quaternion& quaternion, const size_t group = 0) {
    const auto device_id = this->_devices.size();
    this->_devices.emplace_back(position, quaternion);
    this->_group_map[device_id] = group;
    return device_id;
  }

  /**
   * @brief Delete device
   * @param idx Index of the device to delete
   * @return an index of deleted device
   */
  size_t del_device(const size_t idx) {
    this->_devices.erase(this->_devices.begin() + idx);
    return idx;
  }

  /**
   * @brief Clear all devices
   */
  void clear_devices() { std::vector<Device>().swap(this->_devices); }

  /**
   * @brief ultrasound wavelength
   */
  double& wavelength() noexcept { return this->_wavelength; }

  /**
   * @brief attenuation coefficient
   */
  double& attenuation_coefficient() noexcept { return this->_attenuation; }

  /**
   * @brief Number of devices
   */
  [[nodiscard]] size_t num_devices() const noexcept { return this->_devices.size(); }

  /**
   * @brief Number of transducers
   */
  [[nodiscard]] size_t num_transducers() const noexcept { return this->num_devices() * NUM_TRANS_IN_UNIT; }

  /**
   * @brief Convert device ID into group ID
   */
  [[nodiscard]] size_t group_id_for_device_idx(const size_t device_idx) const { return this->_group_map.at(device_idx); }

  /**
   * @brief Position of a transducer specified by id
   */
  [[nodiscard]] const Vector3& position(const size_t global_transducer_idx) const {
    const auto local_trans_id = global_transducer_idx % NUM_TRANS_IN_UNIT;
    return position(device_idx_for_trans_idx(global_transducer_idx), local_trans_id);
  }

  /**
   * @brief Position of a transducer specified by id
   */
  [[nodiscard]] const Vector3& position(const size_t device_idx, const size_t local_transducer_idx) const {
    return this->_devices[device_idx].global_trans_positions[local_transducer_idx];
  }

  /**
   * @brief Convert a global position to a local position
   */
  [[nodiscard]] Vector3 to_local_position(const size_t device_idx, const Vector3& global_position) const {
    const Vector4 homo = Vector4(global_position[0], global_position[1], global_position[2], 1);
    const Vector4 local_position = this->_devices[device_idx].g2l * homo;
    return Vector3(local_position[0], local_position[1], local_position[2]);
  }

  /**
   * @brief Normalized direction of a device
   */
  [[nodiscard]] const Vector3& direction(const size_t device_idx) const { return this->_devices[device_idx].z_direction; }

  /**
   * @brief Normalized long-axis direction of a device
   */
  [[nodiscard]] const Vector3& x_direction(const size_t device_idx) const { return this->_devices[device_idx].x_direction; }

  /**
   * @brief Normalized short-axis direction of a device
   */
  [[nodiscard]] const Vector3& y_direction(const size_t device_idx) const { return this->_devices[device_idx].y_direction; }

  /**
   * @brief Same as the direction()
   */
  [[nodiscard]] const Vector3& z_direction(const size_t device_idx) const { return this->_devices[device_idx].z_direction; }

  /**
   * @brief Convert transducer index into device index
   */
  [[nodiscard]] static size_t device_idx_for_trans_idx(const size_t transducer_idx) { return transducer_idx / NUM_TRANS_IN_UNIT; }

 private:
  std::vector<Device> _devices;
  std::map<size_t, size_t> _group_map;
  double _wavelength;
  double _attenuation;
};
}  // namespace core
}  // namespace autd


#ifndef __PREM_hpp_
#define __PREM_hpp_

#include <Eigen/Geometry>
#include <random>

#include "Physical_Parameters.hpp"

//PREM Functions
	extern int PREM_Layer(double r);

//Mean Free Path PreFactor
	extern double g_PREM[2];
	extern double g_Factor(int layer);
	extern void Initialize_PREM(double mChi,double sigma0);
	extern void Update_PREM(double mChi,double sigma0,double velocity);

//tExit Function: Calculates when the particle exits the current layer
	extern double tExit(Eigen::Vector3d& position, Eigen::Vector3d& vel);

//Free Path Vector
	extern Eigen::Vector3d FreePathVector(double mX,double sigma,Eigen::Vector3d& position,Eigen::Vector3d& vel,std::mt19937& PRNG);
//Which nucleus does the DM scatter on?
	extern int ScatterNucleus(Eigen::Vector3d& position,std::mt19937& PRNG);

#endif


#pragma once

#include <mtao/types.hpp>
#include <Eigen/Sparse>
#include "mandoline/cutedge.hpp"

namespace mandoline {
struct InterpolatedEdge {

    // the interpolated coordinates
    mtao::Vec2d ts;
    int parent_eid = -1;

    std::map<std::array<int, 2>, double> sparse_matrix_entries(const std::array<int,2> &indices, const mtao::ColVecs2i &E) const;
    template <int D>
    std::map<std::array<int, 2>, double> sparse_matrix_entries(const CutEdge<D> &edge, const mtao::ColVecs2i &E) const {
        assert(edge.is_mesh_edge());
        return sparse_matrix_entries(edge.indices, E);
    }
    std::map<std::array<int, 2>, double> sparse_matrix_entries(const CutMeshEdge<2> &edge, const mtao::ColVecs2i &E) const {
        return sparse_matrix_entries(edge.indices, E);
    }
    double volume() const;
};
}// namespace mandoline


#include "SRKSpinTracker.h"
#include <iostream>
#include <algorithm>
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#include <boost/numeric/odeint.hpp>

#include "SRKODEState.h"

using namespace boost::numeric::odeint;
using namespace std;

//typedef runge_kutta_cash_karp54< SRKMotionState > error_stepper_type;
typedef runge_kutta_dopri5<SRKODEState, SRKSpinFloat> error_stepper_type;

typedef controlled_runge_kutta<error_stepper_type> controlled_stepper_type;

//#define SRKSPINTRACKERDEBUG 1

SRKSpinTracker::SRKSpinTracker()
{
	eps_abs = 1.e-7;
	eps_rel = 1.e-7;
	initialStepSize = 0.01;
	stepsTaken = 0;
	constStepper = false;
}
SRKSpinTracker::SRKSpinTracker(SRKGlobalField* theField):
	SRKSpinTracker()
{
	theEquationOfMotion.setGlobalField(theField);
}

SRKSpinTracker::~SRKSpinTracker()
{

}

void SRKSpinTracker::trackSpin(SRKODEState& theState, double timeToTrack, std::vector<SRKODEState>* stepRecord, std::vector<double>* stepTimes)
{
	runge_kutta4<SRKODEState, SRKSpinFloat> stepper;
	controlled_stepper_type controlled_stepper(default_error_checker<SRKSpinFloat, range_algebra, default_operations>(SRKSpinFloat(eps_abs), SRKSpinFloat(eps_rel), SRKSpinFloat(1.0), SRKSpinFloat(1.0)));

#ifdef SRKSPINTRACKERDEBUG
	cout <<"PRESPIN TRACKING";
	printMotionState(theState);
#endif

	if(stepRecord != nullptr)
	{
		if(constStepper)
		{
			integrate_const(stepper, theEquationOfMotion, theState, SRKSpinFloat(0.0), SRKSpinFloat(timeToTrack), SRKSpinFloat(initialStepSize), pushBackStateAndTime(stepRecord, stepTimes));
		}
		else
		{
			integrate_adaptive(controlled_stepper, theEquationOfMotion, theState, theState[8], theState[8] + SRKSpinFloat(timeToTrack), SRKSpinFloat(initialStepSize), pushBackStateAndTime(stepRecord, stepTimes));
		}

	}
	else
	{
		if(constStepper)
		{
			integrate_const(stepper, theEquationOfMotion, theState, SRKSpinFloat(0.0), SRKSpinFloat(timeToTrack), SRKSpinFloat(initialStepSize));
		}
		else
		{
			integrate_adaptive(controlled_stepper, theEquationOfMotion, theState, theState[8], theState[8] + SRKSpinFloat(timeToTrack), SRKSpinFloat(initialStepSize));
		}
	}
	theState[8] += timeToTrack;

#ifdef SRKSPINTRACKERDEBUG
	cout <<"PostSpin TRACKING";
	printMotionState(theState);
#endif

}

void SRKSpinTracker::trackSpinAltA(SRKODEState& theState, double timeToTrack, std::vector<SRKODEState>* stepRecord, std::vector<double>* stepTimes)
{
	SRKSpinFloat timeToTrackConv(timeToTrack);
	error_stepper_type theStepper;
	SRKSpinFloat dt = SRKSpinFloat(initialStepSize);
	SRKSpinFloat t0 = theState[8];  //Initial time simulation started at
	SRKSpinFloat deltaPhi;
	SRKSpinFloat val;
	SRKODEState previousState(9);
	SRKODEState stateError(9);
	bool potentialLastStep = false;
	for (;;)
	{

		previousState = theState;
		theStepper.do_step(theEquationOfMotion, theState, theState[8], dt, stateError);
		deltaPhi = theState[6] - previousState[6];
		if(deltaPhi > SRKSpinFloat(.785)) //if it rotates greater than pi slow it down!
		{
			dt = dt * SRKSpinFloat(.5) / deltaPhi;
			theState = previousState;
			potentialLastStep = false;
		}
		else
		{
			val = abs(stateError[6]);
//			val/=(eps_abs+eps_rel*abs(deltaPhi));
			val /= (SRKSpinFloat(eps_rel) * abs(deltaPhi));
			if(val > SRKSpinFloat(1.)) //too much error, slow down and repeat
			{
//				SRKSpinFloat a=(SRKSpinFloat(0.9)*SRKSpinFloat(pow(val,SRKSpinFloat(-1.)/(SRKSpinFloat(theStepper.error_order_value) - SRKSpinFloat(1.)))));
				SRKSpinFloat a = SRKSpinFloat(0);
				SRKSpinFloat b = SRKSpinFloat(0.2);
				dt = dt * max(a, b);
				theState = previousState;
				potentialLastStep = false;
			}
			else if(val < SRKSpinFloat(0.5))
			{
				theState[8] += dt;
				stepsTaken++;
				if(stepRecord != nullptr && stepTimes != nullptr)
				{
					stepRecord->push_back(theState);
					stepTimes->push_back(static_cast<double>(theState[8]));
				}
				if(potentialLastStep) //If potential last step worked we break!
				{
					break;
				}

//				SRKSpinFloat a=SRKSpinFloat(0.9)*SRKSpinFloat(pow(val,SRKSpinFloat(-1.)/SRKSpinFloat(theStepper.order_value )));
				SRKSpinFloat a = SRKSpinFloat(0);
				SRKSpinFloat b = SRKSpinFloat(5);
				dt = dt * max(a, b);

			}
			else
			{
				theState[8] += dt;
				stepsTaken++;
				if(stepRecord != nullptr && stepTimes != nullptr)
				{
					stepRecord->push_back(theState);
					stepTimes->push_back(static_cast<double>(theState[8]));
				}
				if(potentialLastStep) //If potential last step worked we break!
				{
					break;
				}
			}

		}

		if(theState[8] + dt > t0 + timeToTrackConv)
		{
			dt = t0 + timeToTrackConv - theState[8];
			potentialLastStep = true;
		}

	}

}


/*
 * This file is a part of the TChecker project.
 *
 * See files AUTHORS and LICENSE for copyright details.
 *
 */

#include <boost/dynamic_bitset.hpp>

#include "tchecker/dbm/refdbm.hh"
#include "tchecker/refzg/semantics.hh"

namespace tchecker {

namespace refzg {

/* Semantics functions */

/*!
  \brief Compute initial zone with reference clocks w.r.t. standard semantics
  \note see tchecker::refzg::standard_semantics_t::initial
*/
tchecker::state_status_t standard_initial(tchecker::dbm::db_t * rdbm, tchecker::reference_clock_variables_t const & r,
                                          boost::dynamic_bitset<> const & delay_allowed,
                                          tchecker::clock_constraint_container_t const & invariant, tchecker::integer_t spread)
{
  tchecker::refdbm::zero(rdbm, r);

  if (tchecker::refdbm::bound_spread(rdbm, r, spread) == tchecker::dbm::EMPTY)
    return tchecker::STATE_CLOCKS_EMPTY_SPREAD;

  if (tchecker::refdbm::constrain(rdbm, r, invariant) == tchecker::dbm::EMPTY)
    return tchecker::STATE_CLOCKS_SRC_INVARIANT_VIOLATED;

  return tchecker::STATE_OK;
}

/*!
  \brief Compute next zone with reference clocks w.r.t. standard semantics
  \note see tchecker::refzg::standard_semantics_t::next
*/
tchecker::state_status_t
standard_next(tchecker::dbm::db_t * rdbm, tchecker::reference_clock_variables_t const & r,
              boost::dynamic_bitset<> const & src_delay_allowed, tchecker::clock_constraint_container_t const & src_invariant,
              boost::dynamic_bitset<> const & sync_ref_clocks, tchecker::clock_constraint_container_t const & guard,
              tchecker::clock_reset_container_t const & clkreset, boost::dynamic_bitset<> const & tgt_delay_allowed,
              tchecker::clock_constraint_container_t const & tgt_invariant, tchecker::integer_t spread)
{
  if (src_delay_allowed.any()) {
    tchecker::refdbm::asynchronous_open_up(rdbm, r, src_delay_allowed);

    if (tchecker::refdbm::constrain(rdbm, r, src_invariant) == tchecker::dbm::EMPTY)
      return tchecker::STATE_CLOCKS_SRC_INVARIANT_VIOLATED; // should never occur
  }

  if (tchecker::refdbm::bound_spread(rdbm, r, spread) == tchecker::dbm::EMPTY)
    return tchecker::STATE_CLOCKS_EMPTY_SPREAD;

  if (tchecker::refdbm::synchronize(rdbm, r, sync_ref_clocks) == tchecker::dbm::EMPTY)
    return tchecker::STATE_CLOCKS_EMPTY_SYNC;

  if (tchecker::refdbm::constrain(rdbm, r, guard) == tchecker::dbm::EMPTY)
    return tchecker::STATE_CLOCKS_GUARD_VIOLATED;

  tchecker::refdbm::reset(rdbm, r, clkreset);

  if (tchecker::refdbm::constrain(rdbm, r, tgt_invariant) == tchecker::dbm::EMPTY)
    return tchecker::STATE_CLOCKS_TGT_INVARIANT_VIOLATED;

  return tchecker::STATE_OK;
}

/*!
  \brief Compute initial zone with reference clocks w.r.t. elapsed semantics
  \note see tchecker::refzg::elapsed_semantics_t::initial
*/
tchecker::state_status_t elapsed_initial(tchecker::dbm::db_t * rdbm, tchecker::reference_clock_variables_t const & r,
                                         boost::dynamic_bitset<> const & delay_allowed,
                                         tchecker::clock_constraint_container_t const & invariant, tchecker::integer_t spread)
{
  tchecker::refdbm::zero(rdbm, r);

  if (tchecker::refdbm::constrain(rdbm, r, invariant) == tchecker::dbm::EMPTY)
    return tchecker::STATE_CLOCKS_SRC_INVARIANT_VIOLATED;

  if (delay_allowed.any()) {
    tchecker::refdbm::asynchronous_open_up(rdbm, r, delay_allowed);

    if (tchecker::refdbm::constrain(rdbm, r, invariant) == tchecker::dbm::EMPTY)
      return tchecker::STATE_CLOCKS_SRC_INVARIANT_VIOLATED;
  }

  if (tchecker::refdbm::bound_spread(rdbm, r, spread) == tchecker::dbm::EMPTY)
    return tchecker::STATE_CLOCKS_EMPTY_SPREAD;

  return tchecker::STATE_OK;
}

/*!
  \brief Compute next zone with reference clocks w.r.t. elapsed semantics
  \note see tchecker::refzg::elapsed_semantics_t::next
*/
tchecker::state_status_t
elapsed_next(tchecker::dbm::db_t * rdbm, tchecker::reference_clock_variables_t const & r,
             boost::dynamic_bitset<> const & src_delay_allowed, tchecker::clock_constraint_container_t const & src_invariant,
             boost::dynamic_bitset<> const & sync_ref_clocks, tchecker::clock_constraint_container_t const & guard,
             tchecker::clock_reset_container_t const & clkreset, boost::dynamic_bitset<> const & tgt_delay_allowed,
             tchecker::clock_constraint_container_t const & tgt_invariant, tchecker::integer_t spread)
{
  if (tchecker::refdbm::constrain(rdbm, r, src_invariant) == tchecker::dbm::EMPTY)
    return tchecker::STATE_CLOCKS_SRC_INVARIANT_VIOLATED;

  if (tchecker::refdbm::synchronize(rdbm, r, sync_ref_clocks) == tchecker::dbm::EMPTY)
    return tchecker::STATE_CLOCKS_EMPTY_SYNC;

  if (tchecker::refdbm::constrain(rdbm, r, guard) == tchecker::dbm::EMPTY)
    return tchecker::STATE_CLOCKS_GUARD_VIOLATED;

  tchecker::refdbm::reset(rdbm, r, clkreset);

  if (tchecker::refdbm::constrain(rdbm, r, tgt_invariant) == tchecker::dbm::EMPTY)
    return tchecker::STATE_CLOCKS_TGT_INVARIANT_VIOLATED;

  if (tgt_delay_allowed.any()) {
    tchecker::refdbm::asynchronous_open_up(rdbm, r, tgt_delay_allowed);

    if (tchecker::refdbm::constrain(rdbm, r, tgt_invariant) == tchecker::dbm::EMPTY)
      return tchecker::STATE_CLOCKS_TGT_INVARIANT_VIOLATED;
  }

  if (tchecker::refdbm::bound_spread(rdbm, r, spread) == tchecker::dbm::EMPTY)
    return tchecker::STATE_CLOCKS_EMPTY_SPREAD;

  return tchecker::STATE_OK;
}

/* standard_semantics_t */

tchecker::state_status_t standard_semantics_t::initial(tchecker::dbm::db_t * rdbm,
                                                       tchecker::reference_clock_variables_t const & r,
                                                       boost::dynamic_bitset<> const & delay_allowed,
                                                       tchecker::clock_constraint_container_t const & invariant,
                                                       tchecker::integer_t spread)
{
  return tchecker::refzg::standard_initial(rdbm, r, delay_allowed, invariant, spread);
}

tchecker::state_status_t standard_semantics_t::next(
    tchecker::dbm::db_t * rdbm, tchecker::reference_clock_variables_t const & r,
    boost::dynamic_bitset<> const & src_delay_allowed, tchecker::clock_constraint_container_t const & src_invariant,
    boost::dynamic_bitset<> const & sync_ref_clocks, tchecker::clock_constraint_container_t const & guard,
    tchecker::clock_reset_container_t const & clkreset, boost::dynamic_bitset<> const & tgt_delay_allowed,
    tchecker::clock_constraint_container_t const & tgt_invariant, tchecker::integer_t spread)
{
  return tchecker::refzg::standard_next(rdbm, r, src_delay_allowed, src_invariant, sync_ref_clocks, guard, clkreset,
                                        tgt_delay_allowed, tgt_invariant, spread);
}

/* elapsed_semantics_t */

tchecker::state_status_t elapsed_semantics_t::initial(tchecker::dbm::db_t * rdbm,
                                                      tchecker::reference_clock_variables_t const & r,
                                                      boost::dynamic_bitset<> const & delay_allowed,
                                                      tchecker::clock_constraint_container_t const & invariant,
                                                      tchecker::integer_t spread)
{
  return tchecker::refzg::elapsed_initial(rdbm, r, delay_allowed, invariant, spread);
}

tchecker::state_status_t
elapsed_semantics_t::next(tchecker::dbm::db_t * rdbm, tchecker::reference_clock_variables_t const & r,
                          boost::dynamic_bitset<> const & src_delay_allowed,
                          tchecker::clock_constraint_container_t const & src_invariant,
                          boost::dynamic_bitset<> const & sync_ref_clocks, tchecker::clock_constraint_container_t const & guard,
                          tchecker::clock_reset_container_t const & clkreset, boost::dynamic_bitset<> const & tgt_delay_allowed,
                          tchecker::clock_constraint_container_t const & tgt_invariant, tchecker::integer_t spread)
{
  return tchecker::refzg::elapsed_next(rdbm, r, src_delay_allowed, src_invariant, sync_ref_clocks, guard, clkreset,
                                       tgt_delay_allowed, tgt_invariant, spread);
}

/* synchronizable_standard_semantics_t */

tchecker::state_status_t synchronizable_standard_semantics_t::initial(tchecker::dbm::db_t * rdbm,
                                                                      tchecker::reference_clock_variables_t const & r,
                                                                      boost::dynamic_bitset<> const & delay_allowed,
                                                                      tchecker::clock_constraint_container_t const & invariant,
                                                                      tchecker::integer_t spread)
{
  tchecker::state_status_t status = tchecker::refzg::standard_initial(rdbm, r, delay_allowed, invariant, spread);
  if (status != tchecker::STATE_OK)
    return status;

  if (!tchecker::refdbm::is_synchronizable(rdbm, r))
    return tchecker::STATE_ZONE_EMPTY_SYNC;

  return tchecker::STATE_OK;
}

tchecker::state_status_t synchronizable_standard_semantics_t::next(
    tchecker::dbm::db_t * rdbm, tchecker::reference_clock_variables_t const & r,
    boost::dynamic_bitset<> const & src_delay_allowed, tchecker::clock_constraint_container_t const & src_invariant,
    boost::dynamic_bitset<> const & sync_ref_clocks, tchecker::clock_constraint_container_t const & guard,
    tchecker::clock_reset_container_t const & clkreset, boost::dynamic_bitset<> const & tgt_delay_allowed,
    tchecker::clock_constraint_container_t const & tgt_invariant, tchecker::integer_t spread)
{
  tchecker::state_status_t status = tchecker::refzg::standard_next(rdbm, r, src_delay_allowed, src_invariant, sync_ref_clocks,
                                                                   guard, clkreset, tgt_delay_allowed, tgt_invariant, spread);
  if (status != tchecker::STATE_OK)
    return status;

  if (!tchecker::refdbm::is_synchronizable(rdbm, r))
    return tchecker::STATE_ZONE_EMPTY_SYNC;

  return tchecker::STATE_OK;
}

/* synchronizable_elapsed_semantics_t */

tchecker::state_status_t synchronizable_elapsed_semantics_t::initial(tchecker::dbm::db_t * rdbm,
                                                                     tchecker::reference_clock_variables_t const & r,
                                                                     boost::dynamic_bitset<> const & delay_allowed,
                                                                     tchecker::clock_constraint_container_t const & invariant,
                                                                     tchecker::integer_t spread)
{
  tchecker::state_status_t status = tchecker::refzg::elapsed_initial(rdbm, r, delay_allowed, invariant, spread);
  if (status != tchecker::STATE_OK)
    return status;

  if (!tchecker::refdbm::is_synchronizable(rdbm, r))
    return tchecker::STATE_ZONE_EMPTY_SYNC;

  return tchecker::STATE_OK;
}

tchecker::state_status_t synchronizable_elapsed_semantics_t::next(
    tchecker::dbm::db_t * rdbm, tchecker::reference_clock_variables_t const & r,
    boost::dynamic_bitset<> const & src_delay_allowed, tchecker::clock_constraint_container_t const & src_invariant,
    boost::dynamic_bitset<> const & sync_ref_clocks, tchecker::clock_constraint_container_t const & guard,
    tchecker::clock_reset_container_t const & clkreset, boost::dynamic_bitset<> const & tgt_delay_allowed,
    tchecker::clock_constraint_container_t const & tgt_invariant, tchecker::integer_t spread)
{
  tchecker::state_status_t status = tchecker::refzg::elapsed_next(rdbm, r, src_delay_allowed, src_invariant, sync_ref_clocks,
                                                                  guard, clkreset, tgt_delay_allowed, tgt_invariant, spread);
  if (status != tchecker::STATE_OK)
    return status;

  if (!tchecker::refdbm::is_synchronizable(rdbm, r))
    return tchecker::STATE_ZONE_EMPTY_SYNC;

  return tchecker::STATE_OK;
}

/* factory */

tchecker::refzg::semantics_t * semantics_factory(enum tchecker::refzg::semantics_type_t semantics)
{
  switch (semantics) {
  case tchecker::refzg::STANDARD_SEMANTICS:
    return new tchecker::refzg::standard_semantics_t{};
  case tchecker::refzg::ELAPSED_SEMANTICS:
    return new tchecker::refzg::elapsed_semantics_t{};
  case tchecker::refzg::SYNC_STANDARD_SEMANTICS:
    return new tchecker::refzg::synchronizable_standard_semantics_t{};
  case tchecker::refzg::SYNC_ELAPSED_SEMANTICS:
    return new tchecker::refzg::synchronizable_elapsed_semantics_t{};
  default:
    throw std::invalid_argument("Unknown semantics over zones with reference clocks");
  }
}

} // end of namespace refzg

} // end of namespace tchecker


/*
 * Copyright (c) 2011 Adrian Michel
 * http://www.amichel.com
 *
 * Permission to use, copy, modify, distribute and sell this
 * software and its documentation for any purpose is hereby
 * granted without fee, provided that both the above copyright
 * notice and this permission notice appear in all copies and in
 * the supporting documentation.
 *
 * This library is distributed in the hope that it will be
 * useful. However, Adrian Michel makes no representations about
 * the suitability of this software for any purpose.  It is
 * provided "as is" without any express or implied warranty.
 *
 * Should you find this library useful, please email
 * info@amichel.com with a link or other reference
 * to your work.
*/

#ifndef DE_MUTATION_STRATEGY_HPP_INCLUDED
#define DE_MUTATION_STRATEGY_HPP_INCLUDED

// MS compatible compilers support #pragma once

#if defined(_MSC_VER) && (_MSC_VER >= 1020)
#pragma once
#endif

#include "population.hpp"

#define URN_DEPTH 5

#include <boost/tuple/tuple.hpp>

namespace de
{

/**
 * Parameters used by mutation strategies
 * weight factor, crossover factor and dither factor which is
 * calculated from the previous two
 *
 * @author adrian (12/1/2011)
 */
class mutation_strategy_arguments
{
private:
	const double m_weight;
	const double m_crossover;
	const double m_dither;

public:
	/**
	 * constructs a mutation_strategy_arguments object.
	 *
	 * Besides the weight and crossover factors, which are supplied
	 * by the calling code, this object holds a dither factor, which
	 * is calculated upon construction, and is used by some mutation
	 * strtegies.
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param weight weight factor which is a double between 0-2 as
	 *  			 defined by the differential evolution algorithm
	 * @param crossover crossover factor which is a double between
	 *  				0-1 as defined by the differential evolution
	 *  				algorithm
	 */
	mutation_strategy_arguments( double weight, double crossover )
	: m_weight( weight ), m_crossover( crossover ), m_dither(  weight + genrand() * ( 1.0 - weight ) )
	{
		// todo: test or assert that weight and crossover are within bounds (0-1, 0-2 or something)
	}

	/**
	 * returns the weight factor
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	double weight() const { return m_weight; }
	/**
	 * returns the crossover factor
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	double crossover() const { return m_crossover; }
	/**
	 * returns the dither factor
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	double dither() const { return m_dither; }
};

/**
 * A an abstract based class for mutation strategies
 *
 * A mutation strategy defines how varaibles values are adjusted
 * during the optimization process
 *
 * @author adrian (12/1/2011)
 */
class mutation_strategy
{
private:
	mutation_strategy_arguments m_args;
	size_t m_varCount;

protected:
	/**
	 * Used to generate a set of 4 random size_t numbers
	 * by the mutation strategy as indexes
	 *
	 * The numbers must all be different, and also different from an
	 * index supplied externally
	 *
	 * @author adrian (12/1/2011)
	 */
	class Urn
	{
		size_t m_urn[ URN_DEPTH ];

	public:
		/**
		 * Constructs an urn object
		 *
		 * @author adrian (12/4/2011)
		 *
		 * @param NP upper limit (exclusive) for the generated random
		 *  		 numbers
		 * @param avoid value to avoid when generating the random
		 *  			numbers
		 */
		Urn( size_t NP, size_t avoid )
		{
			do m_urn[ 0 ] = genintrand( 0, NP, true ) ; while( m_urn[ 0 ] == avoid ) ;
			do m_urn[ 1 ] = genintrand( 0, NP, true ) ; while( m_urn[ 1 ] == m_urn[ 0 ] || m_urn[ 1 ] == avoid );
			do m_urn[ 2 ] = genintrand( 0, NP, true ) ; while( m_urn[ 2 ] == m_urn[ 1 ] || m_urn[ 2 ] == m_urn[ 0 ] || m_urn[ 2  ] == avoid );
			do m_urn[ 3 ] = genintrand( 0, NP, true ) ; while( m_urn[ 3 ] == m_urn[ 2 ] || m_urn[ 3 ] == m_urn[ 1 ] || m_urn[ 3 ] == m_urn[ 0 ] || m_urn[ 3 ] == avoid );
		}

		/**
		 * returns one of the four generated random numbers
		 *
		 * @author adrian (12/4/2011)
		 *
		 * @param index the index of the random number to return, can be
		 *  			between 0-3
		 *
		 * @return size_t
		 */
		size_t operator[]( size_t index ) const { assert( index < 4 ); return m_urn[ index ]; }
	};

public:
	virtual ~mutation_strategy()
	{
	}

	/**
	 * constructs a mutation strategy
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param varCount number of variables
	 * @param args mutation strategy arguments
	 */
	mutation_strategy( size_t varCount, const mutation_strategy_arguments& args )
	: m_args( args ), m_varCount( varCount )
	{
	}

	/**
	 * type for the tuple returned by the operator() member.
	 */
	typedef boost::tuple< individual_ptr, de::DVectorPtr > mutation_info;

	/**
	 * performs the mutation
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param pop a reference to the current population
	 * @param bestIt the best individual of the previous generation
	 * @param i the current individual index
	 *
	 * @return mutation_info tuple containing the mutated individual
	 *  	   and a vector of doubles of the same size as the
	 *  	   number of variables, used as origin to generate new
	 *  	   values in case they exceed the limits imposed by the
	 *  	   corresponding constraints
	 */
	virtual mutation_info operator()( const population& pop, individual_ptr bestIt, size_t i ) = 0;

	/**
	 * returns the number of variables
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return size_t
	 */
	size_t varCount() const { return m_varCount; }

	/**
	 * returns the weight factor
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	double weight() const { return m_args.weight(); }

	/**
	 * returns the crossover factor
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	double crossover() const { return m_args.crossover(); }

	/**
	 * returns the dither factor
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	double dither() const { return m_args.dither(); }
};

typedef boost::shared_ptr< mutation_strategy > mutation_strategy_ptr;

/**
 * Mutation strategy #1
 *
 * @author adrian (12/1/2011)
 */
class mutation_strategy_1 : public mutation_strategy
{
public:
	/**
	 * constructs a mutation strategy # 1
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param varCount number of variables
	 * @param args mutation strategy arguments
	 */
	mutation_strategy_1( size_t varCount, const mutation_strategy_arguments& args )
	: mutation_strategy( varCount, args )
	{
	}

	/**
	 * performs the mutation
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param pop a reference to the current population
	 * @param bestIt the best individual of the previous generation
	 * @param i the current individual index
	 *
	 * @return mutation_info tuple containing the mutated individual
	 *  	   and a vector of doubles of the same size as the
	 *  	   number of variables, used as origin to generate new
	 *  	   values in case they exceed the limits imposed by the
	 *  	   corresponding constraints
	 */
	mutation_info operator()( const population& pop, individual_ptr bestIt, size_t i )
	{
		assert( bestIt );

		de::DVectorPtr origin( boost::make_shared< de::DVector >( varCount() ) );
		individual_ptr tmpInd( boost::make_shared< individual >( *pop[ i ]->vars() ) );
		Urn urn( pop.size(), i );

		// make sure j is within bounds
		size_t j = genintrand( 0, varCount(), true );
		size_t k = 0;

		do
		{
			(*tmpInd->vars())[ j ] = (*pop[ urn[ 0 ] ]->vars() )[ j ] + weight() * ( (*pop[ urn[ 1 ] ]->vars() )[ j ] - (*pop[ urn[ 2 ] ]->vars())[ j ] );

			j = ++j % varCount();
			++k;
		} while( genrand() < crossover() && k < varCount() );

		origin = pop[ urn[ 0 ] ]->vars();

		return mutation_info( tmpInd, origin );
	}

};

/**
 * mutation strategy # 2
 *
 * @author adrian (12/4/2011)
 */
class mutation_strategy_2 : public mutation_strategy
{
public:
	/**
	 * constructs a mutation strategy # 2
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param varCount number of variables
	 * @param args mutation strategy arguments
	 */
	mutation_strategy_2( size_t varCount, const mutation_strategy_arguments& args )
	: mutation_strategy( varCount, args )
	{
	}

	/**
	 * performs the mutation
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param pop a reference to the current population
	 * @param bestIt the best individual of the previous generation
	 * @param i the current individual index
	 *
	 * @return mutation_info tuple containing the mutated individual
	 *  	   and a vector of doubles of the same size as the
	 *  	   number of variables, used as origin to generate new
	 *  	   values in case they exceed the limits imposed by the
	 *  	   corresponding constraints
	 */
	mutation_info operator()( const population& pop, individual_ptr bestIt, size_t i )
	{
		assert( bestIt );

		de::DVectorPtr origin( boost::make_shared< de::DVector >( varCount() ) );
		individual_ptr tmpInd( boost::make_shared< individual >( *pop[ i ]->vars() ) );
		Urn urn( pop.size(), i );

		// make sure j is within bounds
		size_t j = genintrand( 0, varCount(), true );
		size_t k = 0;

		do
		{
			(*tmpInd->vars())[ j ] = (*tmpInd->vars())[ j ] +
				weight() * ( (*bestIt->vars() )[ j ] - (*tmpInd->vars())[ j ] ) +
				weight() * ( (*pop[ urn[ 1 ] ]->vars() )[ j ] - (*pop[ urn[ 2 ] ]->vars())[ j ] );

			j = ++j % varCount();
			++k;
		} while( genrand() < crossover() && k < varCount() );

		origin = pop[ urn[ 0 ] ]->vars();

		return mutation_info( tmpInd, origin );
	}

};

/**
 * mutation strategy # 3
 *
 * @author adrian (12/4/2011)
 */
class mutation_strategy_3 : public mutation_strategy
{
public:
	/**
	 * constructs a mutation strategy # 3
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param varCount number of variables
	 * @param args mutation strategy arguments
	 */
	mutation_strategy_3( size_t varCount, const mutation_strategy_arguments& args )
	: mutation_strategy( varCount, args )
	{
	}

	/**
	 * performs the mutation
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param pop a reference to the current population
	 * @param bestIt the best individual of the previous generation
	 * @param i the current individual index
	 *
	 * @return mutation_info tuple containing the mutated individual
	 *  	   and a vector of doubles of the same size as the
	 *  	   number of variables, used as origin to generate new
	 *  	   values in case they exceed the limits imposed by the
	 *  	   corresponding constraints
	 */
	mutation_info operator()( const population& pop, individual_ptr bestIt, size_t i )
	{
		assert( bestIt );

		de::DVectorPtr origin( boost::make_shared< de::DVector >( varCount() ) );
		individual_ptr tmpInd( boost::make_shared< individual >( *pop[ i ]->vars() ) );
		Urn urn( pop.size(), i );

		// make sure j is within bounds
		size_t j = genintrand( 0, varCount(), true );
		size_t k = 0;

		do
		{
			double jitter = (0.0001* genrand() + weight() );

			(*tmpInd->vars())[ j ] = (*bestIt->vars() )[ j ] + jitter * ( (*pop[ urn[ 1 ] ]->vars() )[ j ] - (*pop[ urn[ 2 ] ]->vars())[ j ] );

			j = ++j % varCount();
			++k;
		} while( genrand() < crossover() && k < varCount() );

		origin = pop[ urn[ 0 ] ]->vars();

		return mutation_info( tmpInd, origin );
	}
};

/**
 * mutation strategy # 4
 *
 * @author adrian (12/4/2011)
 */
class mutation_strategy_4 : public mutation_strategy
{
public:
	/**
	 * constructs a mutation strategy # 4
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param varCount number of variables
	 * @param args mutation strategy arguments
	 */
	mutation_strategy_4( size_t varCount, const mutation_strategy_arguments& args )
	: mutation_strategy( varCount, args )
	{
	}

	/**
	 * performs the mutation
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param pop a reference to the current population
	 * @param bestIt the best individual of the previous generation
	 * @param i the current individual index
	 *
	 * @return mutation_info tuple containing the mutated individual
	 *  	   and a vector of doubles of the same size as the
	 *  	   number of variables, used as origin to generate new
	 *  	   values in case they exceed the limits imposed by the
	 *  	   corresponding constraints
	 */
	mutation_info operator()( const population& pop, individual_ptr bestIt, size_t i )
	{
		assert( bestIt );

		de::DVectorPtr origin( boost::make_shared< de::DVector >( varCount() ) );
		individual_ptr tmpInd( boost::make_shared< individual >( *pop[ i ]->vars() ) );
		Urn urn( pop.size(), i );

		// make sure j is within bounds
		size_t j = genintrand( 0, varCount(), true );
		size_t k = 0;

		double factor( weight() + genrand() * ( 1.0 - weight() ) );

		do
		{
			double jitter = (0.0001* genrand() + weight() );

			(*tmpInd->vars())[ j ] = (*pop[ urn[ 0 ] ]->vars() )[ j ] +
				factor * ( (*pop[ urn[ 1 ] ]->vars() )[ j ] - (*pop[ urn[ 2 ] ]->vars())[ j ] );

			j = ++j % varCount();
			++k;
		} while( genrand() < crossover() && k < varCount() );

		origin = pop[ urn[ 0 ] ]->vars();

		return mutation_info( tmpInd, origin );
	}

};

/**
 * Mutation strategy # 5
 *
 * @author adrian (12/1/2011)
 */
class mutation_strategy_5 : public mutation_strategy
{

public:
	/**
	 * constructs a mutation strategy # 5
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param varCount number of variables
	 * @param args mutation strategy arguments
	 */
	mutation_strategy_5( size_t varCount, const mutation_strategy_arguments& args )
	: mutation_strategy( varCount, args )
	{
	}

	/**
	 * performs the mutation
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param pop a reference to the current population
	 * @param bestIt the best individual of the previous generation
	 * @param i the current individual index
	 *
	 * @return mutation_info tuple containing the mutated individual
	 *  	   and a vector of doubles of the same size as the
	 *  	   number of variables, used as origin to generate new
	 *  	   values in case they exceed the limits imposed by the
	 *  	   corresponding constraints
	 */
	mutation_info operator()( const population& pop, individual_ptr bestIt, size_t i )
	{
		assert( bestIt );

		de::DVectorPtr origin( boost::make_shared< de::DVector >( varCount() ) );
		individual_ptr tmpInd( boost::make_shared< individual >( *pop[ i ]->vars() ) );
		Urn urn( pop.size(), i );

		// make sure j is within bounds
		size_t j = genintrand( 0, varCount(), true );
		size_t k = 0;

		do
		{
			(*tmpInd->vars())[ j ] = (*pop[ urn[ 0 ] ]->vars() )[ j ] + dither() * ( (*pop[ urn[ 1 ] ]->vars() )[ j ] - (*pop[ urn[ 2 ] ]->vars() )[ j ] );

			j = ++j % varCount();
			++k;
		} while( genrand() < crossover() && k < varCount() );

		origin = pop[ urn[ 0 ] ]->vars();
		return mutation_info( tmpInd, origin );
	}

};

}

#endif //DE_MUTATION_STRATEGY_HPP_INCLUDED


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

#ifndef MEASUREMENT_CONVERSION__EIGEN_UTILS_HPP_
#define MEASUREMENT_CONVERSION__EIGEN_UTILS_HPP_

#include <Eigen/Geometry>

namespace autoware
{
namespace common
{
namespace state_estimation
{

///
/// @brief      This class describes a data storage order.
///
enum class DataStorageOrder
{
  kRowMajor,
  kColumnMajor
};

namespace detail
{
///
/// @brief      A functor to compute an index in the data array.
///
class Index
{
public:
  ///
  /// @brief      Create an index functor.
  ///
  /// @param[in]  start_index    The start index at which to start the count.
  /// @param[in]  stride         The stride of the data, i.e., step to the next row or column.
  /// @param[in]  storage_order  How the data should be stored in memory.
  ///
  Index(
    const std::int32_t start_index,
    const std::int32_t stride,
    const DataStorageOrder storage_order)
  : m_start_index{start_index}, m_stride{stride}, m_storage_order{storage_order} {}

  ///
  /// @brief      Compute an index using the stored starting index, stride and storage order.
  ///
  /// @param[in]  row   Query row
  /// @param[in]  col   Query column
  ///
  /// @return     A 1D index in the underlying data array
  ///
  std::size_t operator()(const Eigen::Index row, const Eigen::Index col) const
  {
    switch (m_storage_order) {
      case DataStorageOrder::kRowMajor:
        return static_cast<std::size_t>(m_start_index + row * m_stride + col);
      case DataStorageOrder::kColumnMajor:
        return static_cast<std::size_t>(m_start_index + col * m_stride + row);
    }
    throw std::runtime_error("Unexpected storage order");
  }

private:
  std::int32_t m_start_index{};
  std::int32_t m_stride{};
  DataStorageOrder m_storage_order{};
};
}  // namespace detail

///
/// @brief      Downscale the isometry to a lower dimension if needed.
///
/// @param[in]  isometry              The isometry transform
///
/// @tparam     kStateDimensionality  Dimensionality of the space.
/// @tparam     FloatT                Type of scalar.
///
/// @return     Downscaled isometry.
///
template<std::int32_t kStateDimensionality, typename FloatT>
static constexpr Eigen::Transform<
  FloatT, kStateDimensionality, Eigen::TransformTraits::Isometry> downscale_isometry(
  const Eigen::Transform<FloatT, 3, Eigen::TransformTraits::Isometry> & isometry)
{
  static_assert(kStateDimensionality <= 3, "We only handle scaling the isometry down.");
  using Isometry = Eigen::Transform<
    FloatT, kStateDimensionality, Eigen::TransformTraits::Isometry>;
  Isometry result{Isometry::Identity()};
  result.linear() = isometry.rotation()
    .template block<kStateDimensionality, kStateDimensionality>(0, 0);
  result.translation() = isometry.translation().topRows(kStateDimensionality);
  return result;
}

///
/// @brief      Transform a given array to an Eigen matrix using the specified stride and a starting
///             index within this array.
///
/// @details    The function converts a given array into an Eigen matrix using the given starting
///             index and stride. The function will take the number of rows and columns from the
///             provided parameters and will iterate the given array as many times as needed to fill
///             all elements of the resulting matrix. It is assumed that there are no gaps between
///             the elements in a single row but that the columns are spaced `stride` apart.
///
/// @note       The storage order only refers to the storage order expected from the array, not from
///             the Eigen matrix. The Eigen matrices are expected to be column-major as per default.
///
/// @throws     std::runtime_error  if there is not enough elements in the array to perform a full
///                                 conversion of all the asked elements.
///
/// @param[in]  array          The given array (e.g. a covariance array from a message)
/// @param[in]  start_index    The start index of the first element in an array to be copied
/// @param[in]  stride         How big the step to the next row is in terms of indices in the array
/// @param[in]  storage_order  The storage order of the data in the input array (row-major for ROS)
///
/// @tparam     kRows          Number of rows in the resulting matrix
/// @tparam     kCols          Number of columns in the resulting matrix
/// @tparam     ScalarT        Scalar type (inferred)
/// @tparam     kSize          Size of the input array (inferred)
///
/// @return     An Eigen matrix that stores the requested data.
///
template<std::int32_t kRows, std::int32_t kCols, typename ScalarT, std::size_t kSize>
Eigen::Matrix<ScalarT, kRows, kCols> array_to_matrix(
  const std::array<ScalarT, kSize> & array,
  const std::int32_t start_index,
  const std::int32_t stride,
  const DataStorageOrder storage_order)
{
  using Mat = Eigen::Matrix<ScalarT, kRows, kCols>;
  const detail::Index index{start_index, stride, storage_order};
  Mat res{Mat::Zero()};
  const auto max_index = index(res.rows() - 1, res.cols() - 1);
  if (max_index >= array.size()) {
    throw std::runtime_error(
            "Trying to access out of bound memory at index " +
            std::to_string(max_index) + " of an array with size: " + std::to_string(array.size()));
  }

  for (auto col = 0; col < kCols; ++col) {
    for (auto row = 0; row < kRows; ++row) {
      res(row, col) = array[index(row, col)];
    }
  }
  return res;
}

///
/// @brief      Sets data in an array from a given Eigen matrix.
///
/// @note       The storage order only refers to the storage order expected from the array, not from
///             the Eigen matrix. The Eigen matrices are expected to be column-major as per default.
///
/// @param      array          The array for which the data is set
/// @param[in]  matrix         The matrix that has the data that is to be copied
/// @param[in]  start_index    The start index in the output data array
/// @param[in]  stride         The step that brings the index to the next row/column
/// @param[in]  storage_order  The storage order of the output data
///
/// @tparam     kRows          Number of rows in the input matrix, inferred by the compiler.
/// @tparam     kCols          Number of columns in the input matrix, inferred by the compiler.
/// @tparam     ScalarT        Type of underlying data elements, inferred by the compiler.
/// @tparam     kSize          Size of the input matrix, inferred by the compiler.
///
template<std::int32_t kRows, std::int32_t kCols, typename ScalarT, std::size_t kSize>
void set_from_matrix(
  std::array<ScalarT, kSize> & array,
  const Eigen::Matrix<ScalarT, kRows, kCols> & matrix,
  const std::int32_t start_index,
  const std::int32_t stride,
  const DataStorageOrder storage_order)
{
  const detail::Index index{start_index, stride, storage_order};
  const auto max_index = index(matrix.rows() - 1, matrix.cols() - 1);
  if (max_index >= array.size()) {
    throw std::runtime_error(
            "Trying to access out of bound memory at index " +
            std::to_string(max_index) + " of an array with size: " + std::to_string(array.size()));
  }
  for (auto col = 0; col < kCols; ++col) {
    for (auto row = 0; row < kRows; ++row) {
      array[index(row, col)] = matrix(row, col);
    }
  }
}

///
/// @brief      Get a slice of a given matrix.
///
/// @details    Given two sequences for rows and for columns generate a new matrix that contains
///             only rows and columns in those sequences.
///
/// @param[in]  matrix         The given matrix
/// @param[in]  sequence_rows  The sequence of rows to include in the slice
/// @param[in]  sequence_cols  The sequence of columns to include in the slice
///
/// @tparam     ScalarT        Type of underlying data
/// @tparam     kRows          Number of rows in a query matrix
/// @tparam     kCols          Number of columns in a query matrix
/// @tparam     kSeqRowsSize   Number of rows in the slice
/// @tparam     kSeqColsSize   Number of columns in the slice
///
/// @return     A matrix representing a slice of the original matrix.
///
template<
  typename ScalarT,
  std::int32_t kRows,
  std::int32_t kCols,
  std::size_t kSeqRowsSize,
  std::size_t kSeqColsSize>
Eigen::Matrix<
  ScalarT, static_cast<std::int32_t>(kSeqRowsSize), static_cast<std::int32_t>(kSeqColsSize)>
slice(
  const Eigen::Matrix<ScalarT, kRows, kCols> matrix,
  const std::array<Eigen::Index, kSeqRowsSize> & sequence_rows,
  const std::array<Eigen::Index, kSeqColsSize> & sequence_cols)
{
  using Mat = Eigen::Matrix<
    ScalarT, static_cast<std::int32_t>(kSeqRowsSize), static_cast<std::int32_t>(kSeqColsSize)>;
  Mat res{Mat::Zero()};
  for (auto col = 0UL; col < kSeqColsSize; ++col) {
    for (auto row = 0UL; row < kSeqRowsSize; ++row) {
      res(static_cast<Eigen::Index>(row), static_cast<Eigen::Index>(col)) =
        matrix(sequence_rows[row], sequence_cols[col]);
    }
  }
  return res;
}

}  // namespace state_estimation
}  // namespace common
}  // namespace autoware

#endif  // MEASUREMENT_CONVERSION__EIGEN_UTILS_HPP_


#define _FILE_OFFSET_BITS 64
#define _THREAD_SAFE

// #define BOOST_NO_CXX11_NULLPTR

#include <utility>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/visitors.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/graph_utility.hpp>

using namespace boost;

#include <stdio.h>
#include <stdint.h>

extern "C" {
#include "../compat.h"
#include "../graph500.h"
}

typedef adjacency_list<vecS, vecS, undirectedS> Graph;
static Graph *g;

int
create_graph_from_edgelist (int64_t *IJ_in, int64_t nedge)
{
  int err = 0;
  /*
  {
    printf("[ ");
    for( int64_t i = 0; i < nedge; i++ )
      {
	printf("<%" PRId64 ",%" PRId64 "> ", IJ_in[2*i], IJ_in[2*i+1]);
      }
    printf(" ]\n");
  }
  */
  // how many vertices do we have?
  int64_t num_vertices = 0;
  for( int64_t k = 0; k < nedge*2; k++ )
    {
      if( IJ_in[k] > num_vertices )
	{num_vertices = IJ_in[k];}
    }

  g = new Graph(num_vertices);
  for( int64_t k = 0; k < nedge; k++ )
    {
      add_edge(IJ_in[k*2], IJ_in[k*2+1], *g);
    }
  return err;
}

int
make_bfs_tree (int64_t *bfs_tree_out, int64_t *max_vtx_out,
	       int64_t srcvtx)
{
  int err = 0;

  *max_vtx_out = boost::num_vertices(*g) - 1;
  typedef Graph::vertex_descriptor Vertex;
  std::vector<Vertex> p(boost::num_vertices(*g));
  typedef std::vector<Vertex>::value_type* Piter;

  for( int64_t i = 0; i < boost::num_vertices(*g); i++ )
    {p[i] = -1;}
  p[srcvtx] = srcvtx;

  boost::breadth_first_search
    (*g, srcvtx,
     boost::visitor(boost::make_bfs_visitor
		    (boost::record_predecessors(&p[0],
						boost::on_tree_edge()) )));
  for( int64_t i = 0; i < boost::num_vertices(*g); i++ )
    {bfs_tree_out[i] = p[i];}
  /*
  printf("source %" PRId64 ": [ ", srcvtx);
  for( int64_t i = 0; i < boost::num_vertices(*g); i++ )
    {printf("%" PRId64 " ", p[i]);}
  printf("]\n");
  */
  return err;
}

void
destroy_graph (void)
{
}


/*
 * SingleValue.cpp
 * @brief domain constraint
 * @date Feb 13, 2012
 * @author Frank Dellaert
 */

#include <gtsam/base/Testable.h>
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <cpp/planning/SingleValueConstraint.h>

#include <boost/make_shared.hpp>
using namespace gtsam;
using namespace std;

namespace gtsam_planner {

/* ************************************************************************* */
void SingleValueConstraint::print(const string& s, const KeyFormatter& formatter) const {
  cout << s << "SingleValueConstraint on "
       << "j=" << formatter(keys_[0]) << " with value " << value_ << endl;
}

/* ************************************************************************* */
double SingleValueConstraint::operator()(const DiscreteValues& values) const {
  if (values.count(keys_[0]) != 0) {
      return (double)(values.at(keys_[0]) == value_);
  }
  return 0.0;
  // return (double)(values.at(keys_[0]) == value_);
}

/* ************************************************************************* */
DecisionTreeFactor SingleValueConstraint::toDecisionTreeFactor() const {
  DiscreteKeys keys;
  keys += DiscreteKey(keys_[0], cardinality_);
  vector<double> table;
  for (size_t i1 = 0; i1 < cardinality_; i1++) table.push_back(i1 == value_);
  DecisionTreeFactor converted(keys, table);
  return converted;
}

/* ************************************************************************* */
DecisionTreeFactor SingleValueConstraint::operator*(const DecisionTreeFactor& f) const {
  // TODO: can we do this more efficiently?
  return toDecisionTreeFactor() * f;
}

/* ************************************************************************* */
}  // namespace gtsam


#include <boost/test/unit_test.hpp>

#include "../test_utils.hh"
#include "../fixture.hh"
#include "../vector_utils.hh"

BOOST_FIXTURE_TEST_CASE(float_vector_copy, vector_fixture)
{
    // Given vector of five values and and vector with different id.
    std::vector<float> values = {4.234f, 3214.4243f, 290342.0f, 0.0f, 1.23456789f};
    auto vector1 = getScyllaVectorOf(test_const::float_vector_1_id, values);
    auto vector2 = getScyllaVector(test_const::float_vector_2_id);

    // When performing copy from first vector to another.
    scheduler->scopy(*vector1, *vector2);

    // Then the second vector has the values of first.
    std::optional<scylla_blas::vector_value<float>> difference = cmp_vector(*vector2, values);
    BOOST_CHECK(!difference.has_value());
    if (difference.has_value()) {
        BOOST_ERROR(fmt::format("Difference at position {0}, {1} - {2}",
                                difference->index,
                                difference->value,
                                values[difference->index - 1]));
    }
}

BOOST_FIXTURE_TEST_CASE(float_vector_copy_to_same, vector_fixture)
{
    // Given vector of five values and and vector with different id.
    std::vector<float> values = {4.234f, 3214.4243f, 290342.0f, 0.0f, 1.23456789f};
    auto vector1 = getScyllaVectorOf(test_const::float_vector_1_id, values);

    // When performing copy from first vector to another.
    scheduler->scopy(*vector1, *vector1);

    // Then the second vector has the values of first.
    std::optional<scylla_blas::vector_value<float>> difference = cmp_vector(*vector1, values);
    BOOST_CHECK(!difference.has_value());
    if (difference.has_value()) {
        BOOST_ERROR(fmt::format("Difference at position {0}, {1} - {2}",
                                difference->index,
                                difference->value,
                                values[difference->index - 1]));
    }
}

BOOST_FIXTURE_TEST_CASE(double_vector_copy, vector_fixture)
{
    // Given vector of six values and and vector with different id.
    std::vector<double> values = {5355.939194952, 4.234, 3214.4243, 290342.0, 0.0, 1.23456789};
    auto vector1 = getScyllaVectorOf(test_const::double_vector_1_id, values);
    auto vector2 = getScyllaDoubleVector(test_const::double_vector_2_id);

    // When performing copy from first vector to another.
    scheduler->dcopy(*vector1, *vector2);

    // Then the second vector has the values of first.
    std::optional<scylla_blas::vector_value<double>> difference = cmp_vector(*vector2, values);
    BOOST_CHECK(!difference.has_value());
    if (difference.has_value()) {
        BOOST_ERROR(fmt::format("Difference at position {0}, {1} - {2}",
                                difference->index,
                                difference->value,
                                values[difference->index - 1]));
    }
}

BOOST_FIXTURE_TEST_CASE(float_vector_alpha_sum_copy, vector_fixture)
{
    // Given two vectors with some values and alpha.
    std::vector<float> values1 = {4.0f, 3214.4243f, 290342.0f, 0.0f, 1.23456789f};
    std::vector<float> values2 = {4.5f, 324.4243f, 342.29f, 0.0f};
    auto vector1 = getScyllaVectorOf(test_const::float_vector_1_id, values1);
    auto vector2 = getScyllaVectorOf(test_const::float_vector_2_id, values2);
    float alpha = 1000.0f;

    // When performing saxpy from first vector to another.
    // saxpy - constant times a vector plus a vector.
    scheduler->saxpy(alpha, *vector1, *vector2);

    // Then the second vector has the result.
    while (values2.size() < values1.size()) {
        values2.push_back(0);
    }
    for (int i = 0; i < values1.size(); i++) {
        values2[i] += values1[i] * alpha;
    }

    print_vector(*vector2);
    std::optional<scylla_blas::vector_value<float>> difference = cmp_vector(*vector2, values2);
    BOOST_CHECK(!difference.has_value());
    if (difference.has_value()) {
        BOOST_ERROR(fmt::format("Difference at position {0}, {1} - {2}",
                                difference->index,
                                difference->value,
                                values2[difference->index - 1]));
    }
}

BOOST_FIXTURE_TEST_CASE(double_vector_alpha_sum_copy, vector_fixture)
{
    // Given two vectors with some values and alpha.
    std::vector<double> values1 = {4.4327805450, 3214.4243, 290342.0, 0.0, 1.23456789};
    std::vector<double> values2 = {4.5, 324.4243, 342.29, 0.0, 23442.252526, 1.2346981958};
    auto vector1 = getScyllaVectorOf(test_const::double_vector_1_id, values1);
    auto vector2 = getScyllaVectorOf(test_const::double_vector_2_id, values2);
    float alpha = 1000.0f;

    // When performing daxpy from first vector to another.
    // daxpy - constant times a vector plus a vector.
    scheduler->daxpy(alpha, *vector1, *vector2);

    // Then the second vector has the result.
    while (values2.size() < values1.size()) {
        values2.push_back(0);
    }
    for (int i = 0; i < values1.size(); i++) {
        values2[i] += values1[i] * alpha;
    }

    print_vector(*vector2);
    std::optional<scylla_blas::vector_value<double>> difference = cmp_vector(*vector2, values2);
    BOOST_CHECK(!difference.has_value());
    if (difference.has_value()) {
        BOOST_ERROR(fmt::format("Difference at position {0}, {1} - {2}",
                                difference->index,
                                difference->value,
                                values2[difference->index - 1]));
    }
}

BOOST_FIXTURE_TEST_CASE(double_vector_alpha_sum_copy_to_same, vector_fixture)
{
    // Given two vectors with some values and alpha.
    std::vector<double> values1 = {4.4327805450, 3214.4243, 290342.0, 0.0, 1.23456789};
    auto vector1 = getScyllaVectorOf(test_const::double_vector_1_id, values1);
    float alpha = 1000.0f;

    // When performing daxpy from first vector to another.
    // daxpy - constant times a vector plus a vector.
    scheduler->daxpy(alpha, *vector1, *vector1);

    // Then the second vector has the result.

    for (int i = 0; i < values1.size(); i++) {
        values1[i] += values1[i] * alpha;
    }

    print_vector(*vector1);
    std::optional<scylla_blas::vector_value<double>> difference = cmp_vector(*vector1, values1);
    BOOST_CHECK(!difference.has_value());
    if (difference.has_value()) {
        BOOST_ERROR(fmt::format("Difference at position {0}, {1} - {2}",
                                difference->index,
                                difference->value,
                                values1[difference->index - 1]));
    }
}


// Copyright (c) Omar Boukli-Hacene 2021. All Rights Reserved.
// Distributed under an MIT-style license that can be
// found in the LICENSE file.

// SPDX-License-Identifier: MIT

#include <boost/test/unit_test.hpp>

#include "tromino.h"

#include "unittest_helper.h"

BOOST_AUTO_TEST_SUITE(tromino_4x4_test_suite)

//
// X | - +
// - + | |
// | - + |
// + - - +
BOOST_AUTO_TEST_CASE(
    Given_trmn_solve_puzzle_WhenOrderIs4AndMarkIs0_0_ThenSolution) {
    constexpr int order = 4;
    constexpr trmn_position_t mark = {0, 0};

    // clang-format off
    const std::vector<ShimStep> expected = {
        {{0, 0}, {-1, -1}},
        {{2, 2}, {-1, -1}},
        {{2, 0}, {-1, 1}},
        {{0, 2}, {1, -1}},
        {{1, 1}, {-1, -1}},
    };
    // clang-format on

    std::vector<ShimStep> actual;

    trmn_solve_puzzle(order, mark, shim_add_tromino, &actual);

    BOOST_CHECK_EQUAL(actual.size(), 5);
    BOOST_CHECK_EQUAL_COLLECTIONS(
        actual.begin(), actual.end(), expected.begin(), expected.end());
}

BOOST_AUTO_TEST_CASE(
    Given_trmn_solve_puzzle_WhenOrderIs4AndMarkIs0_7_ThenSolution) {
    constexpr int order = 4;
    constexpr trmn_position_t mark = {0, 7};

    // clang-format off
    const std::vector<ShimStep> expected = {
        {{0, 2}, {-1, 1}},
        {{2, 0}, {-1, 1}},
        {{2, 2}, {-1, -1}},
        {{0, 0}, {1, 1}},
        {{1, 1}, {-1, 1}},
    };
    // clang-format on

    std::vector<ShimStep> actual;

    trmn_solve_puzzle(order, mark, shim_add_tromino, &actual);

    BOOST_CHECK_EQUAL(actual.size(), 5);
    BOOST_CHECK_EQUAL_COLLECTIONS(
        actual.begin(), actual.end(), expected.begin(), expected.end());
}

BOOST_AUTO_TEST_CASE(
    Given_trmn_solve_puzzle_WhenOrderIs4AndMarkIs7_0_ThenSolution) {
    constexpr int order = 4;
    constexpr trmn_position_t mark = {7, 0};

    // clang-format off
    const std::vector<ShimStep> expected = {
        {{2, 0}, {1, -1}},
        {{0, 2}, {1, -1}},
        {{0, 0}, {1, 1}},
        {{2, 2}, {-1, -1}},
        {{1, 1}, {1, -1}},
    };
    // clang-format on

    std::vector<ShimStep> actual;

    trmn_solve_puzzle(order, mark, shim_add_tromino, &actual);

    BOOST_CHECK_EQUAL(actual.size(), 5);
    BOOST_CHECK_EQUAL_COLLECTIONS(
        actual.begin(), actual.end(), expected.begin(), expected.end());
}

BOOST_AUTO_TEST_CASE(
    Given_trmn_solve_puzzle_WhenOrderIs4AndMarkIs7_7_ThenSolution) {
    constexpr int order = 4;
    constexpr trmn_position_t mark = {7, 7};

    // clang-format off
    const std::vector<ShimStep> expected = {
        {{2, 2}, {1, 1}},
        {{0, 0}, {1, 1}},
        {{0, 2}, {1, -1}},
        {{2, 0}, {-1, 1}},
        {{1, 1}, {1, 1}},
    };
    // clang-format on

    std::vector<ShimStep> actual;

    trmn_solve_puzzle(order, mark, shim_add_tromino, &actual);

    BOOST_CHECK_EQUAL(actual.size(), 5);
    BOOST_CHECK_EQUAL_COLLECTIONS(
        actual.begin(), actual.end(), expected.begin(), expected.end());
}

//
// + - - +
// | + - |
// | | | X
// + - + -
BOOST_AUTO_TEST_CASE(
    Given_trmn_solve_puzzle_WhenOrderIs4AndMarkIs3_2_ThenSolution) {
    constexpr int order = 4;
    constexpr trmn_position_t mark = {3, 2};

    // clang-format off
    std::vector<ShimStep> expected = {
        {{2, 2}, {1, -1}},
        {{0, 0}, {1, 1}},
        {{0, 2}, {1, -1}},
        {{2, 0}, {-1, 1}},
        {{1, 1}, {1, 1}},
    };
    // clang-format on

    std::vector<ShimStep> actual;

    trmn_solve_puzzle(order, mark, shim_add_tromino, &actual);

    BOOST_CHECK_EQUAL(actual.size(), 5);
    BOOST_CHECK_EQUAL_COLLECTIONS(
        actual.begin(), actual.end(), expected.begin(), expected.end());
}

BOOST_AUTO_TEST_SUITE_END()