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

// interval instance types
#include "../test_type_lists.hpp"
#include "../test_value_maker.hpp"

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

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

#include "../test_interval_map_mixed.hpp"

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_map_mixed_infix_plus_overload_4_bicremental_types)
{            interval_map_mixed_infix_plus_overload_4_bicremental_types<bicremental_type_1, int>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_map_mixed_infix_pipe_overload_4_bicremental_types)
{            interval_map_mixed_infix_pipe_overload_4_bicremental_types<bicremental_type_2, double>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_map_mixed_infix_minus_overload_4_bicremental_types)
{            interval_map_mixed_infix_minus_overload_4_bicremental_types<bicremental_type_3, boost::rational<int> >(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_map_mixed_infix_et_overload_4_bicremental_types)
{            interval_map_mixed_infix_et_overload_4_bicremental_types<bicremental_type_4, int>(); }


#include <complex>

#include "common.hpp"

#include <armadillo>
#include "highfive_arma.hpp"
#include "highfive_aview.hpp"
#include "interface.hpp"

using namespace HighFive;

int main() {
    const std::string filename("aview.h5");
    File file(filename, File::ReadWrite | File::Create | File::Truncate);
    Interface iface(filename);

    const size_t d1 = 2, d2 = 3, d3 = 4, d4 = 5;
    const size_t dt = d1 * d2 * d3 * d4;
    arma::vec v1(dt, arma::fill::randn);
    v1.print("v1");
    libaview::array_view<double> av1(v1.memptr(), v1.n_elem);
    libaview::tens4d t1(av1, d1, d2, d3, d4);

    DataSpace dspace_t1 =  DataSpace::From(t1);
    std::cout << dspace_t1.getDimensions() << std::endl;
    std::cout << "dt: " << dt << std::endl;
    // std::cout << "1: " << t1.view(0).size() << std::endl;
    // std::cout << "2: " << t1.view(0, 0).size() << std::endl;
    // std::cout << "3: " << t1.view(0, 0, 0).size() << std::endl;
    std::cout << "d2: " << d2 << " " << t1.view(0).size() / t1.view(0, 0).size() << std::endl;
    std::cout << "d3: " << d3 << " " << t1.view(0, 0).size() / t1.view(0, 0, 0).size() << std::endl;
    std::cout << "d4: " << d4 << " " << t1.view(0, 0, 0).size() << std::endl;

    size_t d1c = 0;
    while (true) {
        try {
            t1.view(d1c);
            d1c++;
        } catch (const std::out_of_range &e) {
            break;
        }
    }
    std::cout << "d1: " << d1 << " " << d1c << std::endl;

    DataSet dset_t1 = file.createDataSet<double>("t1", dspace_t1);
    dset_t1.write(t1);

    arma::vec v2(dt);
    libaview::array_view<double> av2(v2.memptr(), v2.n_elem);
    libaview::tens4d t2(av2, d1, d2, d3, d4);
    DataSet dset_t2 = file.getDataSet("t1");
    // dset_t2.read(t2);
    iface.read("t1", t2);
    // v2.print("v2");

    std::vector<double> v3(dt);
    libaview::array_view<double> av3(v3.data(), v2.n_elem);
    // libaview::tens4d t3(av3, d1, d2, d3, d4);
    // need to allocate underlying memory first, otherwise runtime error (not segfault)
    // pass a pointer to get around dim restrictions
    dset_t2.read(v3.data());
    // v3.clear();
    // v3.resize(dt);

    // auto dset_t3 = file.createDataSet<double>("t3", DataSpace::From(av3));
    // dset_t3.write(av3);
    // std::cout << v3 << std::endl;
    iface.write("t3", av3);

    const size_t dt2 = d1 * d2 * d3;
    arma::cx_cube cz1(d1, d2, d3, arma::fill::randu);
    libaview::array_view<std::complex<double>> vz1(cz1.memptr(), cz1.n_elem);
    libaview::tens3z tz1(vz1, d1, d2, d3);

    // auto dset_z1_arma = file.createDataSet<std::complex<double>>("z1_arma", DataSpace::From(cz1));
    // auto dset_z1_aview = file.createDataSet<std::complex<double>>("z1_aview", DataSpace::From(tz1));
    // dset_z1_arma.write(cz1);
    // dset_z1_aview.write(tz1);
    // cz1.print("cz1");
    iface.write("z1_arma", cz1);
    iface.write("z1_aview", tz1);
    iface.write("z1_arma", tz1);

    return 0;
}


/* file.hpp
 * -*- coding: utf-8 -*-
 *
 *
 */

#ifndef FILE_HPP
#define FILE_HPP

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

#include <string>
#include <vector>
#include <sstream>
#include <glob.h>

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

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

#include "../src/Frame.hpp"
#include "../src/SecondaryStructureFile.hpp"
#include "../src/NormalModeMeanSquareFluctuationCalculator.hpp"

#include "string.hpp"
#include "type.hpp"

template <typename T>
std::vector<T> get_std_vector_from_csv(const std::string & csv_path,
                                       const std::string & column_name)
{
    io::CSVReader<1> csv_file(csv_path);
    csv_file.read_header(io::ignore_extra_column, column_name);
    std::vector<T> result;
    T value;

    while (csv_file.read_row(value)) {
        result.push_back(value);
    }
    return result;
}

Eigen::MatrixXd get_matrix_from_csv(std::string const & csv_filename);

Frame get_frame_from_csv(const std::string & csv_filename);

Frame get_frame_from_csv_not_hex(const std::string & csv_filename);

std::vector<Range> get_ranges_from_csv(const std::string & csv_path);

Eigen::VectorXd get_vector_from_csv(const std::string & csv_path, const std::string & column_name);

Eigen::MatrixXd get_sym_matrix_from_file(const std::string & matrix_file_path,
                                         int dim, char delim = ',', bool is_hex = false);

NormalModeMeanSquareFluctuationCalculator get_nmmsfc_for_testing(const std::string& pdb_path,
                                                                 const std::string& dssp_path,
                                                                 const std::string& test_prefix);

std::vector<std::string> glob(const std::string& pat);

#endif

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


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

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

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

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <ql/exercise.hpp>
#include <ql/pricingengines/blackformula.hpp>
#include <ql/time/calendars/nullcalendar.hpp>
#include <qle/models/fxeqoptionhelper.hpp>

#include <boost/make_shared.hpp>

namespace QuantExt {

FxEqOptionHelper::FxEqOptionHelper(const Period& maturity, const Calendar& calendar, const Real strike,
                                   const Handle<Quote> spot, const Handle<Quote> volatility,
                                   const Handle<YieldTermStructure>& domesticYield,
                                   const Handle<YieldTermStructure>& foreignYield,
                                   BlackCalibrationHelper::CalibrationErrorType errorType)
    : BlackCalibrationHelper(volatility, errorType), termStructure_(domesticYield), hasMaturity_(true),
      maturity_(maturity), calendar_(calendar), strike_(strike), spot_(spot), foreignYield_(foreignYield) {
    registerWith(spot_);
    registerWith(foreignYield_);
}

FxEqOptionHelper::FxEqOptionHelper(const Date& exerciseDate, const Real strike, const Handle<Quote> spot,
                                   const Handle<Quote> volatility, const Handle<YieldTermStructure>& domesticYield,
                                   const Handle<YieldTermStructure>& foreignYield,
                                   BlackCalibrationHelper::CalibrationErrorType errorType)
    : BlackCalibrationHelper(volatility, errorType), termStructure_(domesticYield), hasMaturity_(false),
      exerciseDate_(exerciseDate), strike_(strike), spot_(spot), foreignYield_(foreignYield) {
    registerWith(spot_);
    registerWith(foreignYield_);
}

void FxEqOptionHelper::performCalculations() const {
    if (hasMaturity_)
        exerciseDate_ = calendar_.advance(termStructure_->referenceDate(), maturity_);
    tau_ = termStructure_->timeFromReference(exerciseDate_);
    atm_ = spot_->value() * foreignYield_->discount(tau_) / termStructure_->discount(tau_);
    effStrike_ = strike_;
    if (effStrike_ == Null<Real>())
        effStrike_ = atm_;
    type_ = effStrike_ >= atm_ ? Option::Call : Option::Put;
    boost::shared_ptr<StrikedTypePayoff> payoff(new PlainVanillaPayoff(type_, effStrike_));
    boost::shared_ptr<Exercise> exercise = boost::make_shared<EuropeanExercise>(exerciseDate_);
    option_ = boost::shared_ptr<VanillaOption>(new VanillaOption(payoff, exercise));
    BlackCalibrationHelper::performCalculations();
}

Real FxEqOptionHelper::modelValue() const {
    calculate();
    option_->setPricingEngine(engine_);
    return option_->NPV();
}

Real FxEqOptionHelper::blackPrice(Real volatility) const {
    calculate();
    const Real stdDev = volatility * std::sqrt(tau_);
    return blackFormula(type_, effStrike_, atm_, stdDev, termStructure_->discount(tau_));
}

} // namespace QuantExt


//          Copyright Maksym V. Bilinets 2015 - 2019.
// 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 "tools/avl_tree_invariant.h"
#include "tools/trees_generator.h"

#include <dst/allocator/global_counter_allocator.h>
#include <dst/binary_tree/balanced_tree.h>

#include <boost/test/unit_test.hpp>

#include <cstddef> // std::size_t
#include <iostream>
#include <iterator> // std::advance
#include <queue>
#include <utility> // std::pair, std::make_pair

namespace dst_test
{
using avl_tree =
  dst::binary_tree::balanced_tree<int, dst::global_counter_allocator<int>>;

avl_tree init_avl_tree(const dst::binary_tree::initializer_tree<int>& init)
{
  avl_tree tree;

  std::queue<std::pair<dst::binary_tree::initializer_tree<int>::tree_iterator,
                       avl_tree::tree_iterator>>
    queue;

  queue.push(
    std::make_pair(init.root(), tree.insert_left(tree.nil(), *init.root())));

  while (!queue.empty())
  {
    const auto p = queue.front();
    queue.pop();

    const auto l = left(p.first);
    const auto r = right(p.first);

    if (!!l)
      queue.push(std::make_pair(l, tree.insert_left(p.second, *l)));

    if (!!r)
      queue.push(std::make_pair(r, tree.insert_right(p.second, *r)));
  }

  return tree;
}

BOOST_AUTO_TEST_SUITE(test_binary_tree_avl)

BOOST_AUTO_TEST_CASE(load_test_insert)
{
  const auto hight = 4;

  const auto fib_tree = generate_fibonacci_tree(hight);
  const auto trees = generate_balanced_trees(hight);

  BOOST_TEST(trees.size() == number_of_balanced_trees(hight));

  for (auto& t : trees)
  {
    BOOST_TEST(avl_tree::allocator_type::allocated() == 0);

    auto tree = init_avl_tree({t, hight + 1, fib_tree});

    BOOST_TEST(topologically_equal(t.root(), left(tree.root())));
    BOOST_TEST(avl_invariant_holds(tree));

    for (std::size_t i = 0; i < t.size(); ++i)
    {
      auto it1 = tree.begin();
      std::advance(it1, i);

      if (!left(it1.base()))
      {
        auto tree_copy = tree;

        auto it = tree_copy.begin();
        std::advance(it, i);

        tree_copy.insert_left(it.base(), 0);

        const auto invariant_holds = avl_invariant_holds(tree_copy);

        if (!invariant_holds)
        {
          auto it = tree.begin();
          std::advance(it, i);
          const auto x = it.base();

          std::cout << "Preceding buggy `insert_left` operation:" << std::endl;
          dst::binary_tree::write_graphviz(
            std::cout, tree.root(), &avl_tree::balance_factor, &x, &x + 1);

          BOOST_TEST(invariant_holds);

          return;
        }
      }

      if (!right(it1.base()))
      {
        auto tree_copy = tree;

        auto it = tree_copy.begin();
        std::advance(it, i);

        tree_copy.insert_right(it.base(), 0);

        const auto invariant_holds = avl_invariant_holds(tree_copy);

        if (!invariant_holds)
        {
          auto it = tree.begin();
          std::advance(it, i);
          const auto x = it.base();

          std::cout << "Preceding buggy `insert_right` operation:" << std::endl;
          dst::binary_tree::write_graphviz(
            std::cout, tree.root(), &avl_tree::balance_factor, &x, &x + 1);

          BOOST_TEST(invariant_holds);

          return;
        }
      }
    }
  }

  BOOST_TEST(avl_tree::allocator_type::allocated() == 0);
}

BOOST_AUTO_TEST_CASE(load_test_erase)
{
  const auto hight = 4;

  const auto fib_tree = generate_fibonacci_tree(hight);
  const auto trees = generate_balanced_trees(hight);

  BOOST_TEST(trees.size() == number_of_balanced_trees(hight));

  for (auto& t : trees)
  {
    BOOST_TEST(avl_tree::allocator_type::allocated() == 0);

    auto tree = init_avl_tree({t, hight + 1, fib_tree});

    BOOST_TEST(topologically_equal(t.root(), left(tree.root())));
    BOOST_TEST(avl_invariant_holds(tree));

    for (std::size_t i = 0; i < t.size(); ++i)
    {
      auto tree_copy = tree;

      auto it = tree_copy.begin();
      std::advance(it, i);

      if (!left(it.base()) || !right(it.base()))
        tree_copy.erase(it.base());
      else
        tree_copy.erase(it.base(), successor(it.base()));

      const auto invariant_holds = avl_invariant_holds(tree_copy);

      if (!invariant_holds)
      {
        auto it = tree.begin();
        std::advance(it, i);
        const auto x = it.base();

        std::cout << "Preceding buggy `erase` operation:" << std::endl;
        dst::binary_tree::write_graphviz(
          std::cout, tree.root(), &avl_tree::balance_factor, &x, &x + 1);

        BOOST_TEST(invariant_holds);

        return;
      }
    }
  }

  BOOST_TEST(avl_tree::allocator_type::allocated() == 0);
}

BOOST_AUTO_TEST_SUITE_END()
}


#include <mvp/Core/ConvexPolygon.h>

#include <boost/foreach.hpp>

namespace mvp {
namespace core {

ConvexPolygon::ConvexPolygon(VertexList pts) {
  // Algorithm is the 'giftwrap' algorithm http://www.cse.unsw.edu.au/~lambert/java/3d/giftwrap.html
  // Probably not the most efficient implementation...
  VW_ASSERT(pts.size() >= 3, vw::ArgumentErr() << "Need at least 3 points to construct a polygon!");

  vw::Vector2 end = pts[0];
  BOOST_FOREACH(vw::Vector2 const& v, pts) {
    if (end.y() < v.y()) {
      end = v;
    }
  }

  bool done = false;
  vw::Vector2 curr = end;

  while (!done) {
    bool found_next = false;
    BOOST_FOREACH(vw::Vector2 const& next, pts) {
      if (curr != next) {
        found_next = true;
        BOOST_FOREACH(vw::Vector2 const& pt, pts) {
          if (circulation_direction(curr, next, pt) > 0 && pt != next) {
            found_next = false;
            break;
          }
        }
        if (found_next) {
          curr = next;
          break;
        }
      }
    }

    VW_ASSERT(found_next, vw::LogicErr() << "Unable to construct convex hull");

    m_vertices.push_back(curr);
    if (curr == end) {
      done = true;
    }
  }
}

ConvexPolygon::ConvexPolygon(vw::BBox2 const& bbox) : m_vertices(4) {
  m_vertices[0] = bbox.max();
  m_vertices[1] = vw::Vector2(bbox.max().x(), bbox.min().y());
  m_vertices[2] = bbox.min();
  m_vertices[3] = vw::Vector2(bbox.min().x(), bbox.max().y());
}

vw::BBox2 ConvexPolygon::bounding_box() const {
  vw::BBox2 bbox;

  BOOST_FOREACH(vw::Vector2 const& v, m_vertices) {
    bbox.grow(v);
  }

  return bbox;
}

bool ConvexPolygon::contains(vw::Vector2 const& pt) const {
  // Use solution 3 from http://paulbourke.net/geometry/insidepoly/
  for (VertexList::const_iterator curr = m_vertices.begin(); curr != m_vertices.end(); curr++) {
    VertexList::const_iterator next = curr;
    if (++next == m_vertices.end()) {
      next = m_vertices.begin();
    }

    double circulation = circulation_direction(*curr, *next, pt);

    // Round to zero if close... (assume colinear)
    circulation = circulation * circulation < 1e-6 ? 0 : circulation;

    if (circulation > 0) {
      return false;
    }
  }

  return true;
}

bool ConvexPolygon::intersects(ConvexPolygon const& other) const {
  // Algorithm from http://www.gpwiki.org/index.php/Polygon_Collision
  // Straight up dumb check, no optimizations attempted...

  for (VertexList::const_iterator cursor = m_vertices.begin(); cursor != m_vertices.end(); cursor++) {
    VertexList::const_iterator next = cursor;
    if (++next == m_vertices.end()) {
      next = m_vertices.begin();
    }

    vw::Vector2 dir = *next - *cursor;
    vw::Vector2 perp_dir(-dir[1], dir[0]);

    double poly1_min = std::numeric_limits<double>::max();
    double poly1_max = std::numeric_limits<double>::min();
    BOOST_FOREACH(vw::Vector2 const& v, m_vertices) {
      double res = dot_prod(perp_dir, v);
      poly1_min = std::min(res, poly1_min);
      poly1_max = std::max(res, poly1_max);
    }

    double poly2_min = std::numeric_limits<double>::max();
    double poly2_max = std::numeric_limits<double>::min();
    BOOST_FOREACH(vw::Vector2 const& v, other.m_vertices) {
      double res = dot_prod(perp_dir, v);
      poly2_min = std::min(res, poly2_min);
      poly2_max = std::max(res, poly2_max);
    }

    if (poly1_min > poly2_max || poly1_max < poly2_min) {
      return false;
    }
  }

  return true;
}

}} // namespace core,mvp


// Copyright (c) Microsoft Corporation.
// Licensed under the MIT license.
//
// gDLS*: Generalized Pose-and-Scale Estimation Given Scale and Gravity Priors
//
// Victor Fragoso, Joseph DeGol, Gang Hua.
// Proc. of the IEEE/CVF Conf. on Computer Vision and Pattern Recognition 2020.
//
// Please contact the author of this library if you have any questions.
// Author: Victor Fragoso (victor.fragoso@microsoft.com)

#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <glog/logging.h>
#include "gtest/gtest.h"
#include "gdls_star/pinhole_camera.h"
#include "math/utils.h"

namespace msft {
namespace {

// Number of trials.
constexpr int kNumTrials = 32;
// Fixed focal length.
constexpr double kFocalLength = 10;

TEST(PinholeCameraTest, BasicPointProjection) {
  const Eigen::Vector2d principal_point(256, 256);
  const Eigen::Quaterniond rotation(
      Eigen::AngleAxisd(DegToRad(35.0),
                        Eigen::Vector3d::Random().normalized()));
  const Eigen::Vector3d translation = Eigen::Vector3d::Random();
  const PinholeCamera camera(kFocalLength,
                             principal_point,
                             rotation,
                             translation);
  const Eigen::Vector3d camera_position = rotation.conjugate() * -translation;
  Eigen::Vector2d pixel;
  for (int i = 0; i < kNumTrials; ++i) {
    const Eigen::Vector3d point_in_camera = Eigen::Vector3d::Random();
    const Eigen::Vector3d point_in_world =
        rotation.conjugate() * point_in_camera + camera_position;
    const double u =
        (kFocalLength * point_in_camera.x() / point_in_camera.z()) +
        principal_point.x();
    const double v =
        (kFocalLength * point_in_camera.y() / point_in_camera.z()) +
        principal_point.y();
    const Eigen::Vector2d expected_pixel(u, v);
    EXPECT_NEAR(camera.ProjectPoint(point_in_world, &pixel),
                point_in_camera.z(),
                1e-6);
    EXPECT_NEAR((expected_pixel - pixel).squaredNorm(), 0.0, 1e-6);
  }
}

TEST(PinholeCameraTest, BasicPixelToUnitRay) {
  const Eigen::Vector2d principal_point(256, 256);
  const Eigen::Quaterniond rotation(
      Eigen::AngleAxisd(DegToRad(35.0),
                        Eigen::Vector3d::Random().normalized()));
  const Eigen::Vector3d translation = Eigen::Vector3d::Random();
  const PinholeCamera camera(kFocalLength,
                             principal_point,
                             rotation,
                             translation);
  const Eigen::Vector3d camera_position = camera.GetPosition();
  for (int i = 0; i < kNumTrials; ++i) {
    // Point in front of camera.
    Eigen::Vector3d point_in_camera = Eigen::Vector3d::Random();
    point_in_camera.z() = std::abs(point_in_camera.z());
    const Eigen::Vector3d point_in_world =
        rotation.conjugate() * point_in_camera + camera_position;
    const Eigen::Vector3d unnormalized_ray = point_in_world - camera_position;
    const Eigen::Vector3d expected_ray = unnormalized_ray.normalized();
    Eigen::Vector2d pixel;
    camera.ProjectPoint(point_in_world, &pixel);
    const Eigen::Vector3d ray = camera.PixelToUnitRay(pixel);
    EXPECT_NEAR((ray - expected_ray).squaredNorm(), 0.0, 1e-6);
  }
}

TEST(PinholeCameraTest, CameraPosition) {
  const Eigen::Vector2d principal_point(256, 256);
  for (int i = 0; i < kNumTrials; ++i) {
    const Eigen::Quaterniond rotation(
        Eigen::AngleAxisd(DegToRad(35.0),
                          Eigen::Vector3d::Random().normalized()));
    const Eigen::Vector3d translation = Eigen::Vector3d::Random();
    const PinholeCamera camera(kFocalLength,
                               principal_point,
                               rotation,
                               translation);
    const Eigen::Vector3d expected_position =
        rotation.conjugate() * -translation;
    EXPECT_NEAR((expected_position - camera.GetPosition()).squaredNorm(),
                0.0,
                1e-6);
  }
}

}  // namespace
}  // namespace msft


#include <Eigen/Core>

#include <polyvec/curve-tracer/curve.hpp>
#include <polyvec/curve-tracer/curve_bezier.hpp>
#include <polyvec/utils/potrace.hpp>

NAMESPACE_BEGIN ( polyfit )
NAMESPACE_BEGIN ( PotraceUtil )

std::vector<Eigen::Matrix2Xd>
decode_potrace_curve ( potrace_curve_t* potrace_curve ) {
#if defined(POLYVEC_USE_POTRACE)

    int n = potrace_curve->n;
    int* tag = potrace_curve->tag;
    std::vector<Eigen::Matrix2Xd> ans;
    ans.reserve ( n );
    potrace_dpoint_t ( *c ) [3] = potrace_curve->c;
    Eigen::Matrix2Xd curve_points;

    for ( int i = 0; i < n; i++ ) {
        switch ( tag[i] ) {
        case POTRACE_CORNER: {
            curve_points.resize ( 2, 2 );
            {
                if ( i == 0 ) {
                    curve_points.col ( 0 ) << c[n - 1][2].x, c[n - 1][2].y;
                } else {
                    curve_points.col ( 0 ) << c[i - 1][2].x, c[i - 1][2].y;
                }

                curve_points.col ( 1 ) << c[i][1].x, c[i][1].y;
                ans.push_back ( curve_points );
            }
            {
                curve_points.col ( 0 ) << c[i][1].x, c[i][1].y;
                curve_points.col ( 1 ) << c[i][2].x, c[i][2].y;
                ans.push_back ( curve_points );
            }
            break;
        }

        case POTRACE_CURVETO: {
            curve_points.resize ( 2, 4 );

            if ( i == 0 ) {
                curve_points.col ( 0 ) << c[n - 1][2].x, c[n - 1][2].y;
            } else {
                curve_points.col ( 0 ) << c[i - 1][2].x, c[i - 1][2].y;
            }

            curve_points.col ( 1 ) << c[i][0].x, c[i][0].y;
            curve_points.col ( 2 ) << c[i][1].x, c[i][1].y;
            curve_points.col ( 3 ) << c[i][2].x, c[i][2].y;

            ans.push_back ( curve_points );
            break;
        }
        }
    }  // end of for over curves

    return ans;
#else
    assert_break ( 0 );
    return std::vector<Eigen::Matrix2Xd>();
#endif  //  defined(POLYVEC_USE_POTRACE)
}  // decode_potrace_curve

// Using the potrace function seemed to be harder than copy pasting the
// code for this case.
// copied from trace.c in potrace.
std::vector<Eigen::Matrix2Xd>
fit_polygon ( const Eigen::Matrix2Xd& polygon, const double alphamax ) {
    assert_break ( 0 && "I have not tested this yet" );
#if 0
#define sign(x) ((x) > 0 ? 1 : (x) < 0 ? -1 : 0)

    std::vector<Eigen::Matrix2Xd> curves_out ;

    auto mod = [] ( int i, int n ) {
        return ( i + n ) % n;
    };
    auto interval = [] ( double lambda, const Eigen::Vector2d& a,
    const Eigen::Vector2d& b ) {
        return a + lambda * ( b - a );
    };

    auto dorth_infty = [] ( const Eigen::Vector2d& p0, const Eigen::Vector2d& p2 ) {
        Eigen::Vector2d r;

        r.y() = sign ( p2.x() - p0.x() );
        r.x() = -sign ( p2.y() - p0.y() );

        return r;
    };

    auto ddenom = [dorth_infty] ( const Eigen::Vector2d& p0,
    const Eigen::Vector2d& p2 ) {
        Eigen::Vector2d r = dorth_infty ( p0, p2 );

        return r.y() * ( p2.x() - p0.x() ) - r.x() * ( p2.y() - p0.y() );
    };

    auto dpara = [] ( const Eigen::Vector2d& p0, const Eigen::Vector2d& p1,
    const Eigen::Vector2d& p2 ) {
        double x1, y1, x2, y2;

        x1 = p1.x() - p0.x();
        y1 = p1.y() - p0.y();
        x2 = p2.x() - p0.x();
        y2 = p2.y() - p0.y();

        return x1 * y2 - x2 * y1;
    };

    int m = ( int ) polygon.cols();

    int i, j, k;
    double dd, denom, alpha;
    Eigen::Vector2d p2, p3, p4;
    int make_corner, make_smooth;

    /* examine each vertex and find its best fit */
    for ( i = 0; i < m; i++ ) {
        j = mod ( i + 1, m );
        k = mod ( i + 2, m );
        p4 = interval ( 1 / 2.0, polygon.col ( k ), polygon.col ( j ) );

        denom = ddenom ( polygon.col ( i ), polygon.col ( k ) );

        if ( denom != 0.0 ) {
            dd = dpara ( polygon.col ( i ), polygon.col ( j ), polygon.col ( k ) ) / denom;
            dd = fabs ( dd );
            alpha = dd > 1 ? ( 1 - 1.0 / dd ) : 0;
            alpha = alpha / 0.75;
        } else {
            alpha = 4 / 3.0;
        }

        // curve->alpha0[j] = alpha; /* remember "original" value of alpha */

        make_corner = ( alpha >= alphamax );
        make_smooth = ( alpha < alphamax );

        // Shayan: this is the part that I have not yet figured out how to do.
        if ( make_corner ) { /* pointed corner */
            // curve->tag[j] = POTRACE_CORNER;
            // curve->c[j][1] = curve->vertex[j];
            // curve->c[j][2] = p4;
            curves_out.push_back ( Eigen::Matrix2Xd ( 2, 2 ) ) ;
            curves_out.push_back ( Eigen::Matrix2Xd ( 2, 2 ) ) ;
            curves_out.back().push_back (
        } else if ( make_smooth ) {
            if ( alpha < 0.55 ) {
                alpha = 0.55;
            } else if ( alpha > 1 ) {
                alpha = 1;
            }

            p2 = interval ( .5 + .5 * alpha, polygon.col ( i ), polygon.col ( j ) );
            p3 = interval ( .5 + .5 * alpha, polygon.col ( k ), polygon.col ( j ) );
            curve->tag[j] = POTRACE_CURVETO;
            curve->c[j][0] = p2;
            curve->c[j][1] = p3;
            curve->c[j][2] = p4;
        } else {
            assert ( 0 );
        }

        // curve->alpha[j] = alpha; /* store the "cropped" value of alpha */
        // curve->beta[j] = 0.5;
    }

    return;

#undef sign
#endif
}

std::vector<Eigen::Matrix2Xd>
run_pipeline ( const Eigen::Matrix2Xd& path,
               bool should_adjust_path,
               bool optimize_curve ) {
#if defined(POLYVEC_USE_POTRACE)

    //
    // Trace with potrace
    //

    // create the parameters
    param_unique_ptr potrace_params;
    potrace_params.reset ( potrace_param_default() );
    potrace_params->alphamax = 1.0;
    potrace_params->opticurve = optimize_curve;
    potrace_params->opttolerance = 1.0;

    // create the path
    path_unique_ptr potrace_path;
    potrace_path.reset ( potrace_path_create() );

    //
    // set the pixels
    //
    Eigen::Matrix2Xi path_int = path.cast<int>();
    potrace_path_manual ( potrace_path.get(), ( int ) path_int.cols(),
                          path_int.data() );

    //
    // Fit the polygon
    //
    potrace_ipolygon_automatic ( potrace_path.get(), potrace_params.get() );

    //
    // adjust the path
    //
    if ( should_adjust_path ) {
        potrace_dpolygon_automatic ( potrace_path.get(), potrace_params.get() );
    } else {
        potrace_dpolygon_naive ( potrace_path.get() );
    }

    // Now smooth out
    Eigen::VectorXi path_transitions ( path.cols() );
    path_transitions.setConstant ( POTRACE_TRANSITION_GUESS );
    potrace_smooth ( potrace_path.get(), potrace_params.get(),
                     ( int ) path_transitions.size(), path_transitions.data() );

    // Now get the curves
    return decode_potrace_curve ( &potrace_path->curve );

#else
    assert_break ( 0 );
    return std::vector<Eigen::Matrix2Xd>();
#endif

}  // end of run_pipeline()

NAMESPACE_END ( PotraceUtil )
NAMESPACE_END ( polyfit )


//-----------------------------------------------------------------------------
// Copyright (c) 2016-2018 Benjamin Buch
//
// https://github.com/bebuch/mitrax
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt)
//-----------------------------------------------------------------------------
#ifndef _mitrax__matrix__eigen__hpp_INCLUDED_
#define _mitrax__matrix__eigen__hpp_INCLUDED_

#include "eigen_fwd.hpp"

#include <Eigen/Core>

namespace mitrax::detail{

	template < typename T, col_t C, row_t R >
	class eigen_matrix_impl final: auto_dim_pair_t< C, R >{
	public:
		static_assert(!std::is_const_v< T >);
		static_assert(!std::is_reference_v< T >);

		/// \brief Type of the data that administrates the matrix
		using value_type = T;

		/// \brief Type with the make functions
		using maker_type = maker::eigen_t;

		/// \brief Type of the underlaying Eigen matrix
		using eigen_type = ::Eigen::Matrix< value_type,
			R == 0_R ? ::Eigen::Dynamic : static_cast< int >(R),
			C == 0_C ? ::Eigen::Dynamic : static_cast< int >(C) >;

		eigen_matrix_impl(default_constructor_key)
			// TODO: Initialize Elements???
			{}

		eigen_matrix_impl(eigen_matrix_impl&&) = default;

		eigen_matrix_impl(eigen_matrix_impl const&) = default;

		eigen_matrix_impl(
			col< C != 0_C, C > c, row< R != 0_R, R > r,
			eigen_type&& values
		):
			auto_dim_pair_t< C, R >(c, r),
			values_(std::move(values))
			{}

		eigen_matrix_impl& operator=(eigen_matrix_impl&&) = default;

		eigen_matrix_impl& operator=(eigen_matrix_impl const&) = default;

		using auto_dim_pair_t< C, R >::cols;
		using auto_dim_pair_t< C, R >::rows;

		value_type& operator()(c_t c, r_t r){
			return values_(size_t(r), size_t(c));
		}

		value_type const& operator()(c_t c, r_t r)const{
			return values_(size_t(r), size_t(c));
		}

		value_type* data(){
			return values_.data();
		}

		value_type const* data()const{
			return values_.data();
		}

		template < typename Iter >
		void reinit_iter(Iter iter){
			*this = maker_type().by_sequence
				(this->cols(), this->rows(), iter).impl();
		}

	private:
		eigen_type values_;
	};

	template < typename Iter, bool Cct, col_t C, bool Rct, row_t R >
	auto to_eigen_matrix_data(col< Cct, C > c, row< Rct, R > r, Iter iter){
		::Eigen::Matrix< iter_type_t< Iter >,
			Rct ? static_cast< int >(R) : ::Eigen::Dynamic,
			Cct ? static_cast< int >(C) : ::Eigen::Dynamic > data(
				static_cast< int >(size_t(r)), static_cast< int >(size_t(c))
			);

		auto const count = size_t(c) * size_t(r);
		for(size_t i = 0; i < count; ++i, ++iter){
			data(i / size_t(c), i % size_t(c)) = *iter;
		}

		return data;
	}

}

namespace mitrax::maker{

	template < typename Iter, bool Cct, col_t C, bool Rct, row_t R >
	eigen_matrix< iter_type_t< Iter >, Cct ? C : 0_C, Rct ? R : 0_R >
	eigen_t::by_sequence(col< Cct, C > c, row< Rct, R > r, Iter iter)const{
		return {init, c, r, detail::to_eigen_matrix_data(c, r, iter)};
	}

	constexpr auto eigen = eigen_t();

}

#endif


/*
 * a_star.hpp
 *
 *  Created on: Mar 25, 2016
 *      Author: cslab
 */

#ifndef WANDRIAN_INCLUDE_PLANS_BOUSTROPHEDON_ONLINE_A_STAR_HPP_
#define WANDRIAN_INCLUDE_PLANS_BOUSTROPHEDON_ONLINE_A_STAR_HPP_

#include <boost/graph/astar_search.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/random.hpp>
#include <boost/random.hpp>
#include <boost/graph/graphviz.hpp>
#include <sys/time.h>
#include <vector>
#include <list>
#include <math.h>

namespace wandrian {
namespace plans {
namespace boustrophedon_online {

struct location {
  double x, y; // lat, long
};

typedef float cost;

// euclidean distance heuristic
template<class Graph, class CostType, class LocMap>
class distance_heuristic: public boost::astar_heuristic<Graph, CostType> {

public:
  typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
  distance_heuristic(LocMap l, Vertex goal) :
      m_location(l), m_goal(goal) {
  }
  CostType operator()(Vertex u) {
    CostType dx = m_location[m_goal].x - m_location[u].x;
    CostType dy = m_location[m_goal].y - m_location[u].y;
    return std::sqrt(dx * dx + dy * dy);
  }

private:
  LocMap m_location;
  Vertex m_goal;
};

struct found_goal {
};
// exception for termination

// visitor that terminates when we find the goal
template<class Vertex>
class astar_goal_visitor: public boost::default_astar_visitor {

public:
  astar_goal_visitor(Vertex goal) :
      m_goal(goal) {
  }
  template<class Graph>
  void examine_vertex(Vertex u, Graph& g) {
    if (u == m_goal)
      throw found_goal();
  }

private:
  Vertex m_goal;
};

}
}
}

#endif /* WANDRIAN_INCLUDE_PLANS_BOUSTROPHEDON_ONLINE_A_STAR_HPP_ */


#include "pch_bcl.h"
#include "Armature.h"
#include "Animations.h"
#include <regex>
//#include <boost\assign.hpp>
#include <iostream>

using namespace DirectX;
using namespace Causality;

Bone::Bone()
	: LclScaling(1.0f), LclLength(1.0f), GblScaling(1.0f), GblLength(1.0f), LclTw(1.0f), GblTw(1.0f)
{
}

void Bone::UpdateGlobalData(const Bone & reference)
{
	XMVECTOR ParQ = XMLoadA(reference.GblRotation);
	XMVECTOR Q = XMQuaternionMultiply(XMLoadA(LclRotation), ParQ);
	XMVECTOR ParS = XMLoadA(reference.GblScaling);
	XMVECTOR S = ParS * XMLoadA(LclScaling);
	XMStoreA(GblRotation, Q);
	XMStoreA(GblScaling, S);

	//OriginPosition = reference.GblTranslation; // should be a constriant

	XMVECTOR V = XMLoadA(LclTranslation);

	LclLength = XMVectorGetX(XMVector3Length(V));

	V *= ParS;

	GblLength = XMVectorGetX(XMVector3Length(V));

	V = XMVector3Rotate(V, ParQ);//ParQ
	V = XMVectorAdd(V, XMLoadA(reference.GblTranslation));

	XMStoreA(GblTranslation, V);
}

// This will assuming LclTranslation is not changed
void Bone::UpdateLocalData(const Bone& reference)
{
	//OriginPosition = reference.GblTranslation;
	XMVECTOR InvParQ = reference.GblRotation;
	InvParQ = XMQuaternionInverse(InvParQ); // PqInv
	XMVECTOR Q = GblRotation;
	Q = XMQuaternionMultiply(Q, InvParQ);
	XMStoreA(LclRotation, Q);

	Q = (XMVECTOR)GblTranslation - (XMVECTOR)reference.GblTranslation;
	//Q = XMVector3Length(Q);
	//Q = XMVectorSelect(g_XMIdentityR1.v, Q, g_XMIdentityR1.v);

	GblLength = XMVectorGetX(XMVector3Length(Q));

	Q = XMVector3Rotate(Q, InvParQ);

	XMVECTOR S = XMLoadA(GblScaling);
	Q /= S;

	LclLength = XMVectorGetX(XMVector3Length(Q));

	LclTranslation = Q;

	S /= XMLoadA(reference.GblScaling);
	LclScaling = S;
}

void Bone::UpdateLocalDataByPositionOnly(const Bone & reference)
{
	XMVECTOR v0 = XMLoadA(this->GblTranslation) - XMLoadA(reference.GblTranslation);
	v0 = XMVector3InverseRotate(v0, reference.GblRotation);

	LclScaling = Vector3{ 1.0f, XMVectorGetX(XMVector3Length(v0)), 1.0f };

	// with Constraint No-X
	v0 = XMVector3Normalize(v0);
	XMFLOAT4A Sp;
	XMStoreFloat4A(&Sp, v0);
	float Roll = -std::asinf(Sp.x);
	float Pitch = std::atan2f(Sp.z, Sp.y);
	this->LclRotation = XMQuaternionRotationRollPitchYaw(Pitch, 0.0f, Roll);
	this->GblRotation = XMQuaternionMultiply(this->LclRotation, reference.GblRotation);
}

XMMATRIX Bone::TransformMatrix(const Bone & from, const Bone & to)
{
	using namespace DirectX;

	XMVECTOR VScaling = XMLoadA(to.GblScaling) / XMLoadA(from.GblScaling);
	XMVECTOR VRotationOrigin = XMVectorSelect(g_XMSelect1110.v, XMLoadA(from.GblTranslation), g_XMSelect1110.v);
	XMMATRIX MRotation = XMMatrixRotationQuaternion(XMQuaternionConjugate(XMLoadA(from.GblRotation)));
	XMVECTOR VTranslation = XMVectorSelect(g_XMSelect1110.v, XMLoadA(to.GblTranslation), g_XMSelect1110.v);

	VRotationOrigin = -VRotationOrigin;
	VRotationOrigin = XMVector3Transform(VRotationOrigin, MRotation);
	VRotationOrigin = VRotationOrigin * VScaling;
	XMMATRIX M = MRotation;
	M.r[3] = XMVectorSelect(g_XMIdentityR3.v, VRotationOrigin, g_XMSelect1110.v);

	MRotation = XMMatrixRotationQuaternion(XMLoadA(to.GblRotation));
	M = XMMatrixMultiply(M, MRotation);
	M.r[3] += VTranslation;

	//XMVECTOR toEnd = XMVector3Transform(XMLoadA(from.GblTranslation), M);

	//if (XMVector4Greater(XMVector3LengthSq(toEnd - XMLoadA(to.GblTranslation)), XMVectorReplicate(0.001)))
	//	std::cout << "NO!!!!" << std::endl;
	return M;
}

XMMATRIX Bone::RigidTransformMatrix(const Bone & from, const Bone & to)
{
	XMVECTOR rot = XMQuaternionInverse(from.GblRotation);
	rot = XMQuaternionMultiply(rot, to.GblRotation);
	XMVECTOR tra = XMLoadA(to.GblTranslation) - XMLoadA(from.GblTranslation);
	return XMMatrixRigidTransform(XMLoadA(from.GblTranslation), rot, tra);
}

XMDUALVECTOR Bone::RigidTransformDualQuaternion(const Bone & from, const Bone & to)
{
	XMVECTOR rot = XMQuaternionInverse(from.GblRotation);
	rot = XMQuaternionMultiply(rot, to.GblRotation);
	XMVECTOR tra = XMLoadA(to.GblTranslation) - XMLoadA(from.GblTranslation);
	return XMDualQuaternionRigidTransform(XMLoadA(from.GblTranslation), rot, tra);
}

ArmatureFrame::ArmatureFrame(size_t size)
	: BaseType(size)
{
}

ArmatureFrame::ArmatureFrame(const IArmature & armature)
{
	assert(this->size() == armature.size());
	auto df = armature.bind_frame();
	BaseType::assign(df.begin(), df.end());
}

//ArmatureFrame::ArmatureFrame(ArmatureFrameView frameView)
//	: BaseType(frameView.begin(), frameView.end())
//{
//}

ArmatureFrame::ArmatureFrame(const ArmatureFrameConstView &frameView)
	: BaseType(frameView.begin(), frameView.end())
{
}

ArmatureFrame::ArmatureFrame(const ArmatureFrame &) = default;

ArmatureFrame::ArmatureFrame(ArmatureFrame && rhs)
{
	*this = std::move(rhs);
}

ArmatureFrame& ArmatureFrame::operator=(const ArmatureFrame&) = default;
ArmatureFrame& ArmatureFrame::operator=(ArmatureFrame&& rhs)
{
	BaseType::_Assign_rv(std::move(rhs));
	return *this;
}
//ArmatureFrame& ArmatureFrame::operator=(ArmatureFrameView frameView)
//{
//	BaseType::assign(frameView.begin(), frameView.end());
//	return *this;
//}
ArmatureFrame& ArmatureFrame::operator=(const ArmatureFrameConstView &frameView)
{
	BaseType::assign(frameView.begin(), frameView.end());
	return *this;
}

namespace Causality
{
	void FrameRebuildGlobal(const IArmature& armature, ArmatureFrameView frame)
	{
		for (auto& joint : armature.joints())
		{
			auto& bone = frame[joint.ID];
			if (joint.is_root())
			{
				bone.GblRotation = bone.LclRotation;
				bone.GblScaling = bone.LclScaling;
				bone.GblTranslation = bone.LclTranslation;
				bone.LclLength = bone.GblLength = 1.0f; // Length of root doesnot have any meaning
			}
			else
			{
				//bone.OriginPosition = at(joint.ParentID).GblTranslation;

				bone.UpdateGlobalData(frame[joint.ParentID]);
			}
		}
	}

	void FrameRebuildLocal(const IArmature& armature, ArmatureFrameView frame)
	{
		for (auto& joint : armature.joints())
		{
			auto& bone = frame[joint.ID];
			if (joint.is_root())
			{
				bone.LclRotation = bone.GblRotation;
				bone.LclScaling = bone.GblScaling;
				bone.LclTranslation = bone.GblTranslation;
				bone.LclLength = bone.GblLength = 1.0f; // Length of root doesnot have any meaning
			}
			else
			{
				bone.UpdateLocalData(frame[joint.ParentID]);
			}
		}
	}

	void FrameLerpEst(ArmatureFrameView out, ArmatureFrameConstView lhs, ArmatureFrameConstView rhs, float t, const IArmature& armature, bool rebuild)
	{
		//assert((Armature == lhs.pArmature) && (lhs.pArmature == rhs.pArmature));
		XMVECTOR vt = XMVectorReplicate(t);
		for (size_t i = 0; i < armature.size(); i++)
		{
			XMVECTOR Q = DirectX::XMVectorLerpV(XMLoadA(lhs[i].LclRotation), XMLoadA(rhs[i].LclRotation), vt);
			Q = _DXMEXT XMVector4Normalize(Q);
			XMStoreA(out[i].LclRotation, Q);
			XMStoreA(out[i].LclScaling, DirectX::XMVectorLerpV(XMLoadA(lhs[i].LclScaling), XMLoadA(rhs[i].LclScaling), vt));
			XMStoreA(out[i].LclTranslation, DirectX::XMVectorLerpV(XMLoadA(lhs[i].LclTranslation), XMLoadA(rhs[i].LclTranslation), vt));
		}
		if (rebuild)
			FrameRebuildGlobal(armature, out);
	}

	void FrameLerp(ArmatureFrameView out, ArmatureFrameConstView lhs, ArmatureFrameConstView rhs, float t, const IArmature& armature, bool rebuild)
	{
		//assert((Armature == lhs.pArmature) && (lhs.pArmature == rhs.pArmature));
		XMVECTOR vt = XMVectorReplicate(t);
		for (size_t i = 0; i < armature.size(); i++)
		{
			XMStoreA(out[i].LclRotation, DirectX::XMQuaternionSlerpV(XMLoadA(lhs[i].LclRotation), XMLoadA(rhs[i].LclRotation), vt));
			XMStoreA(out[i].LclScaling, DirectX::XMVectorLerpV(XMLoadA(lhs[i].LclScaling), XMLoadA(rhs[i].LclScaling), vt));
			XMStoreA(out[i].LclTranslation, DirectX::XMVectorLerpV(XMLoadA(lhs[i].LclTranslation), XMLoadA(rhs[i].LclTranslation), vt));
		}
		if (rebuild)
			FrameRebuildGlobal(armature, out);
	}

	void FrameDifference(ArmatureFrameView out, ArmatureFrameConstView from, ArmatureFrameConstView to)
	{
		auto n = std::min(from.size(), to.size());
		assert(out.size() >= n);
		for (size_t i = 0; i < n; i++)
		{
			auto& lt = out[i];
			lt.LocalTransform() = from[i].LocalTransform();
			lt.LocalTransform().Inverse();
			lt.LocalTransform() *= to[i].LocalTransform();

			lt.GlobalTransform() = from[i].GlobalTransform();
			lt.GlobalTransform().Inverse();
			lt.GlobalTransform() *= to[i].GlobalTransform();
		}
	}

	void FrameDeform(ArmatureFrameView out, ArmatureFrameConstView from, ArmatureFrameConstView deformation)
	{
		auto n = std::min(from.size(), deformation.size());
		assert(out.size() >= n);
		for (size_t i = 0; i < n; i++)
		{
			auto& lt = out[i];
			lt.LocalTransform() = from[i].LocalTransform();
			lt.LocalTransform() *= deformation[i].LocalTransform();
		}
	}

	void FrameScaleEst(_Inout_ ArmatureFrameView frame, _In_ ArmatureFrameConstView  ref, float scale)
	{
		auto n = std::min(frame.size(), ref.size());
		XMVECTOR sv = XMVectorReplicate(scale);
		for (size_t i = 0; i < n; i++)
		{
			auto& t1 = frame[i].LocalTransform();
			auto& t0 = ref[i].LocalTransform();
			auto& out = t1;
			out.Scale = XMVectorLerpV(XMLoadA(t0.Scale), XMLoadA(t1.Scale), sv);
			out.Translation = XMVectorLerpV(XMLoadA(t0.Translation), XMLoadA(t1.Translation), sv);
			out.Rotation = XMQuaternionSlerpFastV(XMLoadA(t0.Rotation), XMLoadA(t1.Rotation), sv);
		}
	}

	void FrameScale(_Inout_ ArmatureFrameView frame, _In_ ArmatureFrameConstView  ref, float scale)
	{
		auto n = std::min(frame.size(), ref.size());
		XMVECTOR sv = XMVectorReplicate(scale);
		for (size_t i = 0; i < n; i++)
		{
			auto& lt = frame[i].LocalTransform();
			auto& rt = ref[i].LocalTransform();
			IsometricTransform::LerpV(lt, rt, lt, sv);
		}
	}

	void FrameBlend(ArmatureFrameView out, ArmatureFrameConstView lhs, ArmatureFrameConstView rhs, float* blend_weights, const IArmature& armature)
	{

	}

	void FrameTransformMatrix(XMFLOAT3X4* pOut, ArmatureFrameConstView from, ArmatureFrameConstView to, size_t numOut)
	{
		using namespace std;
		size_t n = min(from.size(), to.size());
		if (numOut > 0)
			n = min(n, numOut);
		for (int i = 0; i < n; ++i)
		{
			XMMATRIX mat = Bone::TransformMatrix(from[i], to[i]);
			mat = XMMatrixTranspose(mat);
			XMStoreFloat3x4(pOut + i, mat);
		}
	}

	void FrameTransformMatrix(XMFLOAT4X4* pOut, ArmatureFrameConstView from, ArmatureFrameConstView to, size_t numOut)
	{
		using namespace std;
		size_t n = min(from.size(), to.size());
		if (numOut > 0)
			n = min(n, numOut);
		for (int i = 0; i < n; ++i)
		{
			XMMATRIX mat = Bone::TransformMatrix(from[i], to[i]);
			//mat = XMMatrixTranspose(mat);
			XMStoreFloat4x4(pOut + i, mat);
		}
	}
	Joint::Joint()
	{
		JointBasicData::ID = nullid;
		JointBasicData::ParentID = nullid;
		MirrorJoint = nullptr;
	}
	Joint::Joint(int id)
	{
		JointBasicData::ID = id;
		JointBasicData::ParentID = nullid;
		MirrorJoint = nullptr;
	}
	Joint::Joint(const JointBasicData & data)
		: JointBasicData(data)
	{
		MirrorJoint = nullptr;
	}
	Joint::Joint(const Joint & rhs)
		: JointBasicData(rhs)
	{
		MirrorJoint = nullptr;
	}
	Joint::~Joint()
	{

	}
	inline int Joint::reindex(int baseid)
	{
		int id = baseid;
		for (auto& node : this->nodes())
		{
			node.ID = id++;
			auto parent = node.parent();
			node.ParentID = parent ? parent->ID : nullid;
		}
		return id;
	}
}

StaticArmature::StaticArmature(array_view<JointBasicData> data)
{
	size_t jointCount = data.size();
	m_joints.resize(jointCount);

	for (size_t i = 0; i < jointCount; i++)
	{
		m_joints[i].SetID(i);

		int parentID = data[i].ParentID;
		if (parentID != i &&parentID >= 0)
		{
			m_joints[parentID].append_children_back(&m_joints[i]);
		}
		else
		{
			m_rootIdx = i;
		}
	}

	CaculateTopologyOrder();
}

StaticArmature::StaticArmature()
{
}

StaticArmature::StaticArmature(std::istream & file)
{
	size_t jointCount;
	file >> jointCount;

	m_joints.resize(jointCount);
	m_defaultFrame.resize(jointCount);

	// Joint line format:
	// Hip(Name) -1(ParentID)
	// 1.5(Pitch) 2.0(Yaw) 0(Roll) 0.5(BoneLength)
	for (size_t idx = 0; idx < jointCount; idx++)
	{
		auto& joint = m_joints[idx];
		auto& bone = bind_frame()[idx];
		((JointBasicData&)joint).ID = idx;
		file >> ((JointBasicData&)joint).Name >> ((JointBasicData&)joint).ParentID;
		if (joint.ParentID != idx && joint.ParentID >= 0)
		{
			m_joints[joint.ParentID].append_children_back(&joint);
		}
		else
		{
			m_rootIdx = idx;
		}

		Vector4 vec;
		file >> vec.x >> vec.y >> vec.z >> vec.w;
		bone.LclRotation = XMQuaternionRotationRollPitchYawFromVector(vec);
		bone.LclScaling.y = vec.w;
	}

	CaculateTopologyOrder();
	FrameRebuildGlobal(*this, bind_frame());
}

StaticArmature::StaticArmature(size_t JointCount, int * Parents, const char* const* Names)
	: m_joints(JointCount)
{
	m_defaultFrame.resize(JointCount);
	for (size_t i = 0; i < JointCount; i++)
	{
		m_joints[i].SetID(i);
		m_joints[i].SetName(Names[i]);
		((JointBasicData&)m_joints[i]).ParentID = Parents[i];
		if (Parents[i] != i && Parents[i] >= 0)
		{
			m_joints[Parents[i]].append_children_back(&m_joints[i]);
		}
		else
		{
			m_rootIdx = i;
		}
	}
	CaculateTopologyOrder();
}

StaticArmature::~StaticArmature()
{
	// so that the destructor won't tries to delete multiple times
	for (auto& joint : m_joints)
	{
		joint.isolate();
	}
}

StaticArmature::StaticArmature(const IArmature & rhs)
{
	clone_from(rhs);
}

//StaticArmature::StaticArmature(const self_type & rhs)
//{
//	clone_from(rhs);
//}

StaticArmature::StaticArmature(self_type && rhs)
{
	*this = std::move(rhs);
}

StaticArmature& StaticArmature::operator=(const self_type & rhs)
{
	clone_from(rhs);
	return *this;
}

StaticArmature::self_type & StaticArmature::operator=(self_type && rhs)
{
	using std::move;
	m_rootIdx = rhs.m_rootIdx;
	m_joints = move(rhs.m_joints);
	m_order = move(rhs.m_order);
	m_defaultFrame = move(rhs.m_defaultFrame);
	return *this;
}

//void StaticArmature::clone_from(const self_type & rhs)
//{
//	this->m_joints.resize(rhs.m_joints.size());
//	for (int i = 0; i < m_joints.size(); i++)
//	{
//		this->m_joints[i] = rhs.m_joints[i];
//	}
//	this->m_order = rhs.m_order;
//	this->m_rootIdx = rhs.m_rootIdx;
//	this->m_defaultFrame = make_unique<ArmatureFrame>(*rhs.m_defaultFrame);
//
//	// Re-construct the tree structure
//	for (int i : this->m_order)
//	{
//		if (m_joints[i].ParentID >= 0 && m_joints[i].ParentID != i)
//		{
//			m_joints[m_joints[i].ParentID].append_children_back(&m_joints[i]);
//		}
//	}
//}

void StaticArmature::clone_from(const IArmature & rhs)
{
	for (auto& j : rhs.joints())
		m_order.push_back(j.ID);

	this->m_joints.resize(m_order.size());
	this->m_rootIdx = rhs.root()->ID;
	this->m_defaultFrame = rhs.bind_frame();

	// Copy Joint meta-data
	for (auto& j : rhs.joints())
		m_joints[j.ID] = j;

	// rebuild structure
	for (int i : this->m_order)
	{
		if (m_joints[i].ParentID >= 0 && m_joints[i].ParentID != i)
		{
			m_joints[m_joints[i].ParentID].append_children_back(&m_joints[i]);
		}
	}
}

//void GetBlendMatrices(_Out_ XMFLOAT4X4* pOut);

Joint * StaticArmature::at(int index) {
	return &m_joints[index];
}

Joint * StaticArmature::root()
{
	return &m_joints[m_rootIdx];
}

size_t StaticArmature::size() const
{
	return m_joints.size();
}

StaticArmature::frame_const_view StaticArmature::bind_frame() const
{
	return m_defaultFrame;
	// TODO: insert return statement here
}

void StaticArmature::set_default_frame(frame_type && frame) { m_defaultFrame = std::move(frame); }

void StaticArmature::CaculateTopologyOrder()
{
	m_order.reserve(size());
	for (auto& j : root()->nodes())
		m_order.push_back(j.ID);
}

// Lerp the local-rotation and scaling, "interpolate in Time"
//
//iterator_range<std::sregex_token_iterator> words_from_string(const std::string& str)
//{
//	using namespace std;
//	regex wordPattern("[_\\s]?[A-Za-z][a-z]*\\d*");
//	sregex_token_iterator wbegin(str.begin(), str.end(), wordPattern);
//	iterator_range<sregex_token_iterator> words(wbegin, sregex_token_iterator());
//	return words;
//}
//
//using namespace std;
//
//std::map<std::string, JointSemanticProperty>
//name2semantic = boost::assign::map_list_of
//(string("hand"), JointSemanticProperty(Semantic_Hand))
//(string("foreleg"), JointSemanticProperty(Semantic_Hand | Semantic_Foot))
//(string("arm"), JointSemanticProperty(Semantic_Hand))
//(string("claw"), JointSemanticProperty(Semantic_Hand))
//(string("wing"), JointSemanticProperty(Semantic_Hand | Semantic_Wing))
//(string("head"), JointSemanticProperty(Semantic_Head))
//(string("l"), JointSemanticProperty(Semantic_Left))
//(string("r"), JointSemanticProperty(Semantic_Right))
//(string("left"), JointSemanticProperty(Semantic_Left))
//(string("right"), JointSemanticProperty(Semantic_Right))
//(string("leg"), JointSemanticProperty(Semantic_Foot))
//(string("foot"), JointSemanticProperty(Semantic_Foot))
//(string("tail"), JointSemanticProperty(Semantic_Tail))
//(string("ear"), JointSemanticProperty(Semantic_Ear))
//(string("eye"), JointSemanticProperty(Semantic_Eye))
//(string("noise"), JointSemanticProperty(Semantic_Nouse));
//
//const JointSemanticProperty & Joint::AssignSemanticsBasedOnName()
//{
//	using namespace std;
//	using namespace boost::adaptors;
//
//	auto words = words_from_string(Name);
//	for (auto& word : words)
//	{
//		string word_str;
//		if (*word.first == '_' || *word.first == ' ')
//			word_str = std::string(word.first + 1, word.second);
//		else
//			word_str = std::string(word.first, word.second);
//
//		for (auto& c : word_str)
//		{
//			c = std::tolower(c);
//		}
//
//		this->Semantic |= name2semantic[word_str];
//	}
//	return this->Semantic;
//}

// check if two tree node is "structural-similar"
template <class Derived, bool ownnersip>
bool is_similar(_In_ const stdx::tree_node<Derived, ownnersip> *p, _In_ const stdx::tree_node<Derived, ownnersip> *q)
{
	typedef const stdx::tree_node<Derived, ownnersip> * pointer;
	pointer pc = p->first_child();
	pointer qc = q->first_child();

	if ((pc == nullptr) != (qc == nullptr))
		return false;
	else if (!(pc || qc))
		return true;

	while (pc != nullptr && qc != nullptr)
	{
		bool similar = is_similar(pc, qc);
		if (!similar) return false;
		pc = pc->next_sibling();
		qc = qc->next_sibling();
	}

	return pc == nullptr && qc == nullptr;
}

//bool is_mirror(const Joint* j0, const Joint* j1)
//{
//}

template <class Derived, bool ownnersip>
int child_index(_In_ const stdx::tree_node<Derived, ownnersip> *child)
{
	auto parent = child->parent();
	auto p = parent->first_child();
	int idx = 0;
	while (p != nullptr && p != child)
	{
		++idx;
		p = p->next_sibling();
	}
	return idx;
}

void Causality::BuildJointMirrorRelation(IArmature& armature)
{
	Joint* root = armature.root();
	ArmatureFrameConstView frame = armature.bind_frame();

	float epsilon = 1.00f;
	auto _children = root->descendants_breadth_first();
	std::vector<std::reference_wrapper<Joint>> children(_children.begin(), _children.end());
	std::vector<Joint*> &joints = reinterpret_cast<std::vector<Joint*> &>(children);

	for (int i = 0; i < children.size(); i++)
	{
		auto& bonei = frame[joints[i]->ID];
		auto& ti = bonei.GblTranslation;
		auto ji = joints[i];

		for (int j = i + 1; j < children.size(); j++)
		{
			auto& bonej = frame[joints[j]->ID];
			auto& tj = bonej.GblTranslation;

			auto jj = joints[j];

			auto pi = joints[i]->parent();
			auto pj = joints[j]->parent();

			if (((pi == pj && pi->MirrorJoint == nullptr) || pi->MirrorJoint == pj || pj->MirrorJoint == pi) && is_similar(ji, jj))
			{
				if (pi != pj && child_index(ji) != child_index(jj))
					continue;

				ji->MirrorJoint = jj;
				jj->MirrorJoint = ji;
			}
		}
	}

#ifdef _DEBUG
	for (int i = 0; i < children.size(); i++)
	{
		std::cout << joints[i]->Name;
		if (joints[i]->MirrorJoint)
			std::cout << " <==> " <<  joints[i]->MirrorJoint->Name;
		std::cout << std::endl;
	}
#endif
}

DynamicArmature::DynamicArmature(DynamicArmature && rhs) = default;

DynamicArmature::~DynamicArmature()
{
}

DynamicArmature::DynamicArmature(std::unique_ptr<joint_type>&& root, frame_type && defaultframe)
{
	m_root = move(root);
	m_defaultFrame = move(defaultframe);

	// build the reverse index
	for (auto& joint : m_root->nodes())
	{
		auto rib = m_index.try_emplace(joint.ID, &joint);
		assert(rib.second);
	}
}
DynamicArmature::joint_type * DynamicArmature::at(int index)
{
	auto itr = m_index.find(index);
	if (itr != m_index.end())
		return itr->second;
	else
		return nullptr;
}
DynamicArmature::joint_type * DynamicArmature::root()
{
	return m_root.get();
}
size_t DynamicArmature::size() const
{
	return m_index.size();
}
DynamicArmature::frame_const_view DynamicArmature::bind_frame() const { return m_defaultFrame; }
DynamicArmature::frame_type & DynamicArmature::bind_frame() { return m_defaultFrame; }
void DynamicArmature::set_default_frame(frame_type && frame) { m_defaultFrame = std::move(frame); }
//	void DynamicArmature::clone_from(const IArmature & rhs)
//	{
//	}
void DynamicArmature::clone_from(const joint_type & root)
{
	m_root.reset(root.clone());
	reindex();
}
//	void DynamicArmature::reroot(joint_type * pNewRoot)
//	{
//	}
//	void DynamicArmature::remove(unsigned int jointID)
//	{
//		auto itr = m_index.find(jointID);
//		assert(itr != m_index.end() && itr->second);
//		auto subtree = itr->second;
//		subtree->isolate();
//		for (auto& joint : subtree->nodes())
//			m_index.erase(joint.ID);
//		delete subtree;
//	}
//
//	void DynamicArmature::remove(joint_type * pJoint)
//	{
//		pJoint->isolate();
//		for (auto& joint : pJoint->nodes())
//		{
//			m_index.erase(joint.ID);
//		}
//		delete pJoint;
//	}
//	void DynamicArmature::append(joint_type * pTargetJoint, joint_type * pSrcJoint)
//	{
//		pTargetJoint->append_children_back(pSrcJoint);
//		reindex();
//	}
//	std::map<int, int> DynamicArmature::append(joint_type * pTargetJoint, DynamicArmature * pSkeleton, bool IsCoordinateRelative)
//	{
//		std::map<int, int> mapper;
//		if (pSkeleton == nullptr)
//			return mapper;
//
//		if (pTargetJoint != nullptr && !this->contains(pTargetJoint))
//		{
//			std::cout << "Error : pTargetJoint don't belong to the skeleton." << std::endl;
//			return mapper;
//		}
//
//		unsigned int Key = 0;
//
//		// Copy the sub-skeleton
//		Joint* pRoot = new Joint(*pSrcSubSkeleton);
//
//		if (this->empty())
//		{
//			this->m_root = pRoot;
//		}
//
//		// If don't give the append target , somehow, find one instead
//		bool DefaultInvalidFlag = false;
//		if (!pTargetJoint)
//		{
//			cout << "Warning : Don't get a specific binding bone , find it instead." << endl;
//			// We can't give a relative coordinate if we don't know it , right?
//			IsRelativeCoordinate = false;
//			pTargetJoint = this->FindClosestJoint(pRoot->Entity[Current].Position, Current);
//			DefaultInvalidFlag = true;
//		}
//
//		pTargetJoint->Children_Add(pRoot);
//
//		pRoot->for_all_in_sub_skeleton([&](Joint* pJoint) {
//			// Fix the ID
//			while (containtsKey(Key))
//				Key++;
//			mapper->insert(pair<unsigned int, unsigned int>(pJoint->ID, Key));
//			pJoint->ID = Key;
//			this->Index.insert(Kinematics::IndexedSkeleton::Index_Item(Key, pJoint));
//
//			// Deduce the hierarchical data if needed
//			if (!IsRelativeCoordinate) {
//				pJoint->Deduce_Hierarchical_from_Global(Current);
//				// If default skeleton is invalid , so , use current instead.
//				if (!DefaultInvalidFlag) {
//					pJoint->Deduce_Hierarchical_from_Global(Default);
//				}
//				else {
//					pJoint->Snap_Default_to_Current();
//				}
//
//
//			}
//		});
//
//		pTargetJoint->ReBuildSubSkeleton_Global_from_Hierarchical(Current);
//		pTargetJoint->ReBuildSubSkeleton_Global_from_Hierarchical(Default);
//		return mapper;
//		return std::map<int, int>();
//	}
std::map<int, int> DynamicArmature::reindex()
{
	std::map<int, int> mapper;
	int idx = 0;
	for (auto& joint : m_root->nodes())
	{
		mapper.try_emplace(joint.ID, idx);
		joint.ID = idx++;
		if (joint.parent())
			joint.ParentID = joint.parent()->ID;
	}
	return mapper;
}
//}


#include <tiny.h>
#include <geometry.h>  // needed for geometry shape types
#include <convex.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

#include <cmath>

typedef tiny::MathTypes<double>  M;
typedef M::quaternion_type       Q;
typedef M::vector3_type          V;
typedef M::real_type             T;
typedef M::coordsys_type         C;
typedef M::value_traits          VT;

BOOST_AUTO_TEST_SUITE(convex_growth_distance);

BOOST_AUTO_TEST_CASE(growth_distance_spheres)
{
  geometry::Sphere<V> const A;
  geometry::Sphere<V> const B;

  C X_A, X_B;
  V p_A, p_B;
  T growth_scale;
  size_t iterations;

  size_t const max_its = 100u;
  T      const epsilon = 0.001;

  //--- separated spheres -------------------------------------------------------
  X_A.T() = V::make(1.0,1.0,0.0);
  X_B.T() = V::make(4.0,1.0,0.0);

  bool succes = convex::growth_distance<M>(
                                           X_A
                                           , & A
                                           , X_B
                                           , & B
                                           , p_A
                                           , p_B
                                           , growth_scale
                                           , iterations
                                           , epsilon
                                           , max_its);

  BOOST_CHECK(succes);
  BOOST_CHECK_CLOSE( growth_scale, 1.50, 1.0 );
  BOOST_CHECK_CLOSE(p_A(0), 2.5, 1.0);
  BOOST_CHECK_CLOSE(p_A(1), 1.0, 0.01);
  BOOST_CHECK(p_A(2) == VT::zero());
  BOOST_CHECK_CLOSE(p_B(0), 2.5,  1.0);
  BOOST_CHECK_CLOSE(p_B(1), 1.0,  0.01);
  BOOST_CHECK(p_B(2) == VT::zero());

  //--- touching spheres -------------------------------------------------------
  X_A.T() = V::make(1.0,1.0,0.0);
  X_B.T() = V::make(3.0,1.0,0.0);

  succes = convex::growth_distance<M>(
                                      X_A
                                      , & A
                                      , X_B
                                      , & B
                                      , p_A
                                      , p_B
                                      , growth_scale
                                      , iterations
                                      , epsilon
                                      , max_its);

  BOOST_CHECK(succes);
  BOOST_CHECK_CLOSE( growth_scale, 1.00, 1.0 );
  BOOST_CHECK_CLOSE(p_A(0), 2.0, 1.0);
  BOOST_CHECK_CLOSE(p_A(1), 1.0, 0.01);
  BOOST_CHECK(p_A(2) == VT::zero());
  BOOST_CHECK_CLOSE(p_B(0), 2.0,  1.0);
  BOOST_CHECK_CLOSE(p_B(1), 1.0,  0.01);
  BOOST_CHECK(p_B(2) == VT::zero());

  //---- slight overlapping spheres a long x-axis ------------------------------
  X_A.T() = V::make(1.0, 1.0, 0.0);
  X_B.T() = V::make(2.5, 1.0, 0.0);

  succes = convex::growth_distance<M>(
                                      X_A
                                      , & A
                                      , X_B
                                      , & B
                                      , p_A
                                      , p_B
                                      , growth_scale
                                      , iterations
                                      , epsilon
                                      , max_its
                                      );

  BOOST_CHECK(succes);
  BOOST_CHECK_CLOSE( growth_scale, 0.75, 1.0 );
  BOOST_CHECK_CLOSE(p_A(0), 1.75, 1.0);
  BOOST_CHECK_CLOSE(p_A(1), 1.0,  0.01);
  BOOST_CHECK(p_A(2) == VT::zero());
  BOOST_CHECK_CLOSE(p_B(0), 1.75, 1.0);
  BOOST_CHECK_CLOSE(p_B(1), 1.0,  0.01);
  BOOST_CHECK(p_B(2) == VT::zero());

  //---- Increase overlap along x-axis -----------------------------------------
  X_B.T() = V::make(2,1,0);

  succes = convex::growth_distance<M>(
                                      X_A
                                      , & A
                                      , X_B
                                      , & B
                                      , p_A
                                      , p_B
                                      , growth_scale
                                      , iterations
                                      , epsilon
                                      , max_its
                                      );
  BOOST_CHECK(succes);
  BOOST_CHECK_CLOSE( growth_scale, 0.5, 1.0 );
  BOOST_CHECK_CLOSE(p_A(0), 1.5, 1.0);
  BOOST_CHECK_CLOSE(p_A(1), 1.0,  0.01);
  BOOST_CHECK(p_A(2) == VT::zero());
  BOOST_CHECK_CLOSE(p_B(0), 1.5, 1.0);
  BOOST_CHECK_CLOSE(p_B(1), 1.0,  0.01);
  BOOST_CHECK(p_B(2) == VT::zero());

}

BOOST_AUTO_TEST_CASE(growth_distance_capsule_and_sphere)
{
  convex::Capsule<M>   A;
  geometry::Sphere<V>  B;

  C X_A, X_B;
  V p_A, p_B;
  T growth_scale;
  size_t iterations;

  //--- Sphere touching capsule in one point in the middle of the --------------
  //--- "cylinder" face
  A.half_height() = 1.0;
  A.radius() = .50;
  B.radius() = .50;
  X_A.T() = V::make(.5,0,1);
  X_B.T() = V::make(1.5,0,1);

  T const epsilon = 0.01;

  bool succes = convex::growth_distance<M>(
                                           X_A
                                           , & A
                                           , X_B
                                           , & B
                                           , p_A
                                           , p_B
                                           , growth_scale
                                           , iterations
                                           , epsilon
                                           , 10u);

  BOOST_CHECK(succes);
  BOOST_CHECK_CLOSE( growth_scale, 1.0, 1.0 );

  BOOST_CHECK_CLOSE(p_A(0), 1.0, 1.0);
  BOOST_CHECK_SMALL(p_A(1), 0.001);
  BOOST_CHECK_CLOSE(p_A(2), 1.0, 0.01);

  BOOST_CHECK_CLOSE(p_B(0), 1.0, 1.0);
  BOOST_CHECK_SMALL(p_B(1), 0.001);
  BOOST_CHECK_CLOSE(p_B(2), 1.0, 0.01);

  //--- sphere is overlaping top-shere cap of capsule --------------------------
  X_B.T() = V::make(.5,0,2);

  succes = convex::growth_distance<M>(
                                      X_A
                                      , & A
                                      , X_B
                                      , & B
                                      , p_A
                                      , p_B
                                      , growth_scale
                                      , iterations
                                      , epsilon
                                      , 10u
                                      );
  BOOST_CHECK(succes);
  BOOST_CHECK_CLOSE( growth_scale, 0.5, 1.0 );

  BOOST_CHECK_CLOSE(p_A(0), 0.5, 0.01);
  BOOST_CHECK_CLOSE(p_A(1), 0.0, 0.01);
  BOOST_CHECK_CLOSE(p_A(2), 1.75, 1.0);

  BOOST_CHECK_CLOSE(p_B(0), 0.5, 0.01);
  BOOST_CHECK_CLOSE(p_B(1), 0.0, 0.01);
  BOOST_CHECK_CLOSE(p_B(2), 1.75, 1.0);

  //--- sphere is separated along y-axis at bottom-shere cap of capsule --------
  X_B.T() = V::make(.5,2,0);

  succes = convex::growth_distance<M>(
                                      X_A
                                      , & A
                                      , X_B
                                      , & B
                                      , p_A
                                      , p_B
                                      , growth_scale
                                      , iterations
                                      , 0.01
                                      , 10u
                                      );
  BOOST_CHECK(succes);
  BOOST_CHECK_CLOSE( growth_scale, 2.0, 1.0 );
  BOOST_CHECK_CLOSE( p_A(0), 0.5, 0.01);
  BOOST_CHECK_CLOSE( p_A(1), 1.0, 1.0);
  BOOST_CHECK_SMALL( p_A(2), 0.01);

  BOOST_CHECK_CLOSE( p_B(0), 0.5, 0.01);
  BOOST_CHECK_CLOSE( p_B(1), 1.0, 1.0);
  BOOST_CHECK_SMALL( p_B(2), 0.01);

}

BOOST_AUTO_TEST_CASE(growth_distance_cylinders)
{
  convex::Cylinder<M>  A;
  convex::Cylinder<M>  B;

  C X_A, X_B;
  V p_A, p_B;
  T growth_scale;
  size_t iterations;

  A.half_height() = 2.0f;
  A.radius() = 1.0f;
  B.half_height() = 1.0f;
  B.radius() = 1.0f;

  X_A.T() = V::make(1,0,2);
  X_B.T() = V::make(2,0,2);

  //--- Cylinder's overlapping along-x-axis
  bool succes = convex::growth_distance<M>(
                                         X_A
                                         , & A
                                         , X_B
                                         , & B
                                         , p_A
                                         , p_B
                                         , growth_scale
                                         , iterations
                                         , 0.01f
                                         , 10u);

  BOOST_CHECK(succes);
  BOOST_CHECK_CLOSE( growth_scale, 0.5, 1.0f );

  BOOST_CHECK_CLOSE(p_A(0), 1.5, 1.0);
  BOOST_CHECK_SMALL(p_A(1), 0.001);
  BOOST_CHECK_CLOSE(p_A(2), 2.0, 0.01);

  BOOST_CHECK_CLOSE(p_B(0), 1.5, 1.0);
  BOOST_CHECK_SMALL(p_B(1), 0.01);
  BOOST_CHECK_CLOSE(p_B(2), 2.0, 0.01);

  //--- Cylinders on-top of each other ----------------------------------------
  X_B.T() = V::make(1,0,4);

  succes = convex::growth_distance<M>(
                                    X_A
                                    , & A
                                    , X_B
                                    , & B
                                    , p_A
                                    , p_B
                                    , growth_scale
                                    , iterations
                                    , 0.01f
                                    , 10u
                                    );
  BOOST_CHECK(succes);
  BOOST_CHECK_CLOSE( growth_scale, 0.6666666, 1.0f );

  BOOST_CHECK_CLOSE(p_A(0), 1.0, 1.0);
  BOOST_CHECK_SMALL(p_A(1), 0.001);
  BOOST_CHECK_CLOSE(p_A(2), 3.3333333, 1.0);

  BOOST_CHECK_CLOSE(p_B(0), 1.0, 1.0);
  BOOST_CHECK_SMALL(p_B(1), 0.01);
  BOOST_CHECK_CLOSE(p_B(2), 3.3333333, 1.0);

  //--- overlapping along x-y direction ----------------------------------------

  B.radius() = 2.0f;
  B.half_height() = 2.0f;
  X_B.T() = V::make(3,0,4);

  succes = convex::growth_distance<M>(
                                    X_A
                                    , & A
                                    , X_B
                                    , & B
                                    , p_A
                                    , p_B
                                    , growth_scale
                                    , iterations
                                    , 0.01f
                                    , 10u
                                    );

  BOOST_CHECK(succes);
  BOOST_CHECK_CLOSE( growth_scale, 0.6666666, 1.0f );

  BOOST_CHECK_CLOSE(p_A(0), 1.666666, 1.0);
  BOOST_CHECK_SMALL(p_A(1), 0.001);
  BOOST_CHECK_CLOSE(p_A(2), 3.0, 0.01);

  BOOST_CHECK_CLOSE(p_B(0), 1.666666, 1.0);
  BOOST_CHECK_SMALL(p_B(1), 0.01);
  BOOST_CHECK_CLOSE(p_B(2), 3.0, 0.01);
}

BOOST_AUTO_TEST_CASE(growth_distance_box_sphere)
{
  geometry::Box<V>     A;
  geometry::Sphere<V>  B;

  C X_A, X_B;
  V p_A = V::zero();
  V p_B = V::zero();
  T growth_scale;

  size_t iterations;
  size_t const max_its  = 1000u;
  T      const epsilon  = 0.00001;

  A.half_extent()  = V::make( .5, .5, .5);
  B.radius()       = .5;

  X_A.T() = V::zero();
  X_A.Q() = Q::identity();

  X_B.T() = V::make(0.0,  1.0, 0.0);
  X_B.Q() = Q::identity();

  bool succes = convex::growth_distance<M>(
                                           X_A
                                           , & A
                                           , X_B
                                           , & B
                                           , p_A
                                           , p_B
                                           , growth_scale
                                           , iterations
                                           , epsilon
                                           , max_its
                                           );

  BOOST_CHECK(succes);

  BOOST_CHECK_CLOSE( growth_scale, 1.0, 5.0f );

  BOOST_CHECK_SMALL(p_A(0), 0.05);
  BOOST_CHECK_CLOSE(p_A(1), 0.5, 5.0);
  BOOST_CHECK_SMALL(p_A(2), 0.05);

  BOOST_CHECK_SMALL(p_B(0), 0.05);
  BOOST_CHECK_CLOSE(p_B(1), 0.5, 5.0);
  BOOST_CHECK_SMALL(p_B(2), 0.05);

  //--- Sphere and box are separated by large distance ------------------------
  X_B.T() = V::make(1,0,4);

  succes = convex::growth_distance<M>(
                                      X_A
                                      , & A
                                      , X_B
                                      , & B
                                      , p_A
                                      , p_B
                                      , growth_scale
                                      , iterations
                                      , 0.01f
                                      , 10u
                                      );

  BOOST_CHECK(succes);
  BOOST_CHECK_CLOSE( growth_scale, 4.0, 5.0 );

  BOOST_CHECK_CLOSE(p_A(0), 1.0, 25.0);   // 2015-12-11 Kenny: Really crapy results due to bad accuracy in inner GJK calls
  BOOST_CHECK_SMALL(p_A(1), 0.05);
  BOOST_CHECK_CLOSE(p_A(2), 2.0, 5.0);

  BOOST_CHECK_CLOSE(p_B(0), 1.0, 25.0);   // 2015-12-11 Kenny: Really crapy results due to bad accuracy in inner GJK calls
  BOOST_CHECK_SMALL(p_B(1), 0.05);
  BOOST_CHECK_CLOSE(p_B(2), 2.0, 5.0);

}

BOOST_AUTO_TEST_CASE(growth_distance_box_box_compile_test)
{
  geometry::Box<V>  A;
  geometry::Box<V>  B;

  C X_A, X_B;
  V p_A, p_B;
  T growth_scale;
  size_t iterations;

  A.half_extent() = V::make(1.0, 1.0f, 1.0f);
  B.half_extent() = V::make(1.0f,1.0f,1.0f);

  T delta = VT::numeric_cast(0.01);
  T epsilon = std::min(delta*A.get_scale(), delta*B.get_scale());
  X_A.Q() = Q(1,0,0,0);
  X_B.Q() = Q(1,0,0,0);

  X_A.T() = V::make(0,-1.0,0);
  X_B.T() = V::make(0,1,0);

  convex::growth_distance<M>(
                             X_A
                             , & A
                             , X_B
                             , & B
                             , p_A
                             , p_B
                             , growth_scale
                             , iterations
                             , epsilon
                             , 100u
                             );

  X_A.T() = V::make(0,-1.0,0);
  X_B.T() = V::make(0,2,0);

  convex::growth_distance<M>(
                             X_A
                             , & A
                             , X_B
                             , & B
                             , p_A
                             , p_B
                             , growth_scale
                             , iterations
                             , epsilon
                             , 100u
                             );

  X_A.T() = V::make(0,-1.0,0);
  X_B.T() = V::make(0,0.9,0);

  convex::growth_distance<M>(
                             X_A
                             , & A
                             , X_B
                             , & B
                             , p_A
                             , p_B
                             , growth_scale
                             , iterations
                             , epsilon
                             , 100u
                             );

}

BOOST_AUTO_TEST_SUITE_END();


#ifndef MANUALPHASE_HPP
#define MANUALPHASE_HPP

#include <set>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Dense>

namespace raisim
{
    constexpr int n_gait = 9;

    template<typename T>
    class ManualPhase
    {
    public:
        ManualPhase()
        {
            for (int i = 0; i < n_gait; ++i)
            {
                TIME[i] = UNIT[i] * unit_time_;
                STANCE_TIME[i] = DUTY_UNIT[i] * unit_time_ / (UNIT[i] / 2.0);
            }

            reset();
        }

        Eigen::Matrix<T, Eigen::Dynamic, 1> get_raw_phase()
        {
            phase_raw_.setZero();
            for (int i = 0; i < 4; ++i)
            {
                phase_raw_[i] = std::atan2(q_[2 * i + 1], q_[2 * i]);
            }
            return phase_raw_;
        }

        Eigen::Matrix<T, 4, 1> get_omega()
        {
            return omega_;
        }

        int get_hold_leg()
        {
            // not implemented in manual phase
            return -1;
        }

        bool get_transition_status()
        {
            // not implemented in manual phase
            return false;
        }

        inline void three_leg_mode(int leg)
        {
            // not implemented in manual phase
            ;
        }

        inline void change_gait(int gait_idx)
        {
            target_gait_idx_ = gait_idx;
        }

        inline void change_gait_()
        {
            if (target_gait_idx_ != gait_idx_ && std::fabs(phase_scalar_) <= dt_ / 2.0)
            {
                gait_idx_ = target_gait_idx_;
            }
        }

        inline int get_gait_index()
        {
            return gait_idx_;
        }

        inline T get_stance_time()
        {
            return STANCE_TIME[gait_idx_];
        }

        inline void reset()
        {
            unit_ = UNIT[0];
            time_ = TIME[0];
            duty_unity_ = DUTY_UNIT[0];
            stance_time_ = STANCE_TIME[0];

            q_ << 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0;
            q_dot_.setZero();

            phase_raw_ << - M_PI / 2.0, - M_PI / 2.0, - M_PI / 2.0, - M_PI / 2.0;
            phase_ = {0.0, 0.0, 0.0, 0.0};
            omega_ << - M_PI / unit_time_, - M_PI / unit_time_, - M_PI / unit_time_, - M_PI / unit_time_;

            phase_scalar_ = 0.0;
        }

        inline void step()
        {
            if (phase_scalar_ > TIME[gait_idx_])
            {
                phase_scalar_ = 0.0;
            }
            change_gait_();

            get_q_and_q_dot();

            phase_scalar_ += dt_;
        }

        inline void get_q_and_q_dot()
        {
            if (gait_idx_ == 0)
            {
                T theta = - phase_scalar_ / unit_time_ * M_PI + M_PI;
                for (int i = 0; i < 4; ++i)
                {
                    q_[2 * i] = std::cos(theta);
                    q_[2 * i + 1] = std::sin(theta);
                }
                omega_.setConstant(- M_PI / unit_time_);
            }

            if (gait_idx_ == 1)
            {
                T theta_fast = - phase_scalar_ / unit_time_ * M_PI + M_PI;
                T theta_slow = - phase_scalar_ / unit_time_ * M_PI / 3;

                if (phase_scalar_ / unit_time_ < 3)
                {
                    // 0, 3
                    q_[0] = std::cos(theta_slow);
                    q_[1] = std::sin(theta_slow);
                    q_[6] = std::cos(theta_slow);
                    q_[7] = std::sin(theta_slow);

                    omega_[0] = - M_PI / unit_time_ / 3;
                    omega_[3] = - M_PI / unit_time_ / 3;

                    // 1, 2
                    q_[2] = std::cos(theta_fast);
                    q_[3] = std::sin(theta_fast);
                    q_[4] = std::cos(theta_fast);
                    q_[5] = std::sin(theta_fast);

                    omega_[1] = - M_PI / unit_time_;
                    omega_[2] = - M_PI / unit_time_;
                }
                else
                {
                    theta_fast += M_PI;
                    theta_slow += M_PI;

                    // 0, 3
                    q_[0] = std::cos(theta_fast);
                    q_[1] = std::sin(theta_fast);
                    q_[6] = std::cos(theta_fast);
                    q_[7] = std::sin(theta_fast);

                    omega_[0] = - M_PI / unit_time_;
                    omega_[3] = - M_PI / unit_time_;

                    // 1, 2
                    q_[2] = std::cos(theta_slow);
                    q_[3] = std::sin(theta_slow);
                    q_[4] = std::cos(theta_slow);
                    q_[5] = std::sin(theta_slow);

                    omega_[1] = - M_PI / unit_time_ / 3;
                    omega_[2] = - M_PI / unit_time_ / 3;
                }
            }

            if (gait_idx_ == 2)
            {
                T theta_fast = - phase_scalar_ / unit_time_ * M_PI + M_PI;
                T theta_slow = - phase_scalar_ / unit_time_ * M_PI / 2 - M_PI / 2;

                // 0, 3
                q_[0] = std::cos(theta_slow);
                q_[1] = std::sin(theta_slow);
                q_[6] = std::cos(theta_slow);
                q_[7] = std::sin(theta_slow);

                omega_[0] = - M_PI / unit_time_ / 2;
                omega_[3] = - M_PI / unit_time_ / 2;

                // 1, 2
                q_[2] = std::cos(theta_fast);
                q_[3] = std::sin(theta_fast);
                q_[4] = std::cos(theta_fast);
                q_[5] = std::sin(theta_fast);

                omega_[1] = - M_PI / unit_time_;
                omega_[2] = - M_PI / unit_time_;
            }

            if (gait_idx_ == 3)
            {
                T theta_fast = - phase_scalar_ / unit_time_ * M_PI + M_PI;
                T theta_slow = - phase_scalar_ / unit_time_ * M_PI / 2 - M_PI;

                // 0, 3
                q_[0] = std::cos(theta_fast);
                q_[1] = std::sin(theta_fast);
                q_[6] = std::cos(theta_fast);
                q_[7] = std::sin(theta_fast);

                omega_[0] = - M_PI / unit_time_;
                omega_[3] = - M_PI / unit_time_;

                // 1, 2
                q_[2] = std::cos(theta_slow);
                q_[3] = std::sin(theta_slow);
                q_[4] = std::cos(theta_slow);
                q_[5] = std::sin(theta_slow);

                omega_[0] = - M_PI / unit_time_ / 2;
                omega_[3] = - M_PI / unit_time_ / 2;
            }

            if (gait_idx_ == 4)
            {
                T theta_fast = - phase_scalar_ / unit_time_ * M_PI + M_PI;
                T theta_slow = - phase_scalar_ / unit_time_ * M_PI / 3.0 + M_PI;

                // 0, 3
                q_[0] = std::cos(theta_fast);
                q_[1] = std::sin(theta_fast);
                q_[6] = std::cos(theta_fast);
                q_[7] = std::sin(theta_fast);

                omega_[0] = - M_PI / unit_time_;
                omega_[3] = - M_PI / unit_time_;

                // 1, 2
                q_[2] = std::cos(theta_slow);
                q_[3] = std::sin(theta_slow);
                q_[4] = std::cos(theta_slow);
                q_[5] = std::sin(theta_slow);

                omega_[0] = - M_PI / unit_time_ / 3;
                omega_[3] = - M_PI / unit_time_ / 3;
            }

            if (gait_idx_ == 5)
            {
                T theta_fast = - phase_scalar_ / unit_time_ * M_PI + M_PI;
                T theta_slow = - phase_scalar_ / unit_time_ * M_PI / 3.0 + M_PI * 2.0 / 3.0;

                // 0, 3
                q_[0] = std::cos(theta_slow);
                q_[1] = std::sin(theta_slow);
                q_[6] = std::cos(theta_slow);
                q_[7] = std::sin(theta_slow);

                omega_[0] = - M_PI / unit_time_ / 3;
                omega_[3] = - M_PI / unit_time_ / 3;

                // 1, 2
                q_[2] = std::cos(theta_fast);
                q_[3] = std::sin(theta_fast);
                q_[4] = std::cos(theta_fast);
                q_[5] = std::sin(theta_fast);

                omega_[1] = - M_PI / unit_time_;
                omega_[2] = - M_PI / unit_time_;
            }

            if (gait_idx_ == 6)
            {
                T theta_1 = - phase_scalar_ / unit_time_ * M_PI / 2.0 + M_PI;
                T theta_2 = - phase_scalar_ / unit_time_ * M_PI / 2.0 + M_PI / 2.0;

                // 0, 3
                q_[0] = std::cos(theta_1);
                q_[1] = std::sin(theta_1);
                q_[6] = std::cos(theta_1);
                q_[7] = std::sin(theta_1);

                omega_[0] = - M_PI / unit_time_ / 2;
                omega_[3] = - M_PI / unit_time_ / 2;

                // 1, 2
                q_[2] = std::cos(theta_2);
                q_[3] = std::sin(theta_2);
                q_[4] = std::cos(theta_2);
                q_[5] = std::sin(theta_2);

                omega_[0] = - M_PI / unit_time_ / 2;
                omega_[3] = - M_PI / unit_time_ / 2;
            }

            if (gait_idx_ == 7)
            {
                if (phase_scalar_ / unit_time_ < 2.0)
                {
                    T theta_1 = - phase_scalar_ / unit_time_ * M_PI + M_PI;

                    // 0, 3
                    q_[0] = std::cos(theta_1);
                    q_[1] = std::sin(theta_1);
                    q_[6] = std::cos(theta_1);
                    q_[7] = std::sin(theta_1);

                    omega_[0] = - M_PI / unit_time_;
                    omega_[3] = - M_PI / unit_time_;
                }
                else
                {
                    T theta_1 = - (phase_scalar_ / unit_time_ - 2.0) * M_PI / 2.0 + M_PI;

                    // 0, 3
                    q_[0] = std::cos(theta_1);
                    q_[1] = std::sin(theta_1);
                    q_[6] = std::cos(theta_1);
                    q_[7] = std::sin(theta_1);

                    omega_[0] = - M_PI / unit_time_ / 2.0;
                    omega_[3] = - M_PI / unit_time_ / 2.0;
                }

                if (phase_scalar_ / unit_time_ < 3.0 || phase_scalar_ / unit_time_ >= 5.0)
                {
                    T theta_2 = phase_scalar_ / unit_time_ < 3.0 ? - (phase_scalar_ / unit_time_ + 1.0) * M_PI / 2.0 + M_PI :  - (phase_scalar_ / unit_time_ - 5.0) * M_PI / 2.0 + M_PI;

                    // 1, 2
                    q_[2] = std::cos(theta_2);
                    q_[3] = std::sin(theta_2);
                    q_[4] = std::cos(theta_2);
                    q_[5] = std::sin(theta_2);

                    omega_[1] = - M_PI / unit_time_ / 2.0;
                    omega_[2] = - M_PI / unit_time_ / 2.0;
                }
                if (phase_scalar_ / unit_time_ >= 3.0 && phase_scalar_ / unit_time_ < 5.0)
                {
                    T theta_2 = - (phase_scalar_ / unit_time_ - 3.0) * M_PI + M_PI;

                    // 1, 2
                    q_[2] = std::cos(theta_2);
                    q_[3] = std::sin(theta_2);
                    q_[4] = std::cos(theta_2);
                    q_[5] = std::sin(theta_2);

                    omega_[1] = - M_PI / unit_time_;
                    omega_[2] = - M_PI / unit_time_;
                }
            }

            if (gait_idx_ == 8)
            {
                T theta_1 = phase_scalar_ / unit_time_ < 2.0 ?  - (phase_scalar_ / unit_time_) * M_PI + M_PI : - (phase_scalar_ / unit_time_ - 2.0) * M_PI / 2.0 + M_PI;
                T theta_2 = phase_scalar_ / unit_time_ < 4.0 ?  - (phase_scalar_ / unit_time_) * M_PI / 2.0 + M_PI : - (phase_scalar_ / unit_time_ - 4.0) * M_PI + M_PI;

                T omega_1 = phase_scalar_ / unit_time_ < 2.0 ? - M_PI / unit_time_ : - M_PI / unit_time_ / 2.0;
                T omega_2 = phase_scalar_ / unit_time_ < 4.0 ? - M_PI / unit_time_ / 2.0 : - M_PI / unit_time_;

                // 0, 3
                q_[0] = std::cos(theta_1);
                q_[1] = std::sin(theta_1);
                q_[6] = std::cos(theta_1);
                q_[7] = std::sin(theta_1);

                omega_[0] = omega_1;
                omega_[3] = omega_1;

                // 1, 2
                q_[2] = std::cos(theta_2);
                q_[3] = std::sin(theta_2);
                q_[4] = std::cos(theta_2);
                q_[5] = std::sin(theta_2);

                omega_[0] = omega_2;
                omega_[3] = omega_2;
            }
        }

        Eigen::Matrix<T, 8, 1> get_status()
        {
            return q_;
        }

        Eigen::Matrix<T, 8, 1> get_velocity()
        {
            // not implemented in manual phase
            return q_dot_;
        }

    private:
        int gait_idx_ = 0;
        int prev_gait_idx_ = 0;
        int target_gait_idx_ = 0;

        T phase_scalar_ = 0.0;
        T dt_ = 0.01;
        T unit_time_ = 0.15;

        std::array<T, n_gait> UNIT = {2.0, 6.0, 4.0, 4.0, 6.0, 6.0, 4.0, 6.0, 6.0};
        std::array<T, n_gait> DUTY_UNIT = {1.0, 2.0, 2.0, 2.0, 3.0, 3.0, 2.0, 3.0, 3.0};
        std::array<T, n_gait> TIME;
        std::array<T, n_gait> STANCE_TIME;

        T unit_, duty_unity_, time_, stance_time_;

        Eigen::Matrix<T, 8, 1> q_;
        Eigen::Matrix<T, 8, 1> q_dot_;

        Eigen::Matrix<T, 4, 1> phase_raw_; // -pi to +pi
        std::array<T, 4> phase_; // 0 to 2 * pi
        Eigen::Matrix<T, 4, 1> omega_;
    };
}

#endif


//=======================================================================
// Copyright (c)
//
// 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)
//=======================================================================
/**
 * @file n_queens_test.cpp
 * @brief
 * @author Piotr Wygocki
 * @version 1.0
 * @date 2014-01-04
 */

#include "test_utils/logger.hpp"

#include "paal/local_search/n_queens/n_queens_local_search.hpp"
#include "paal/data_structures/components/components_replace.hpp"

#include <boost/range/algorithm_ext/iota.hpp>

#include <iostream>

int main(int argc, char **argv) {
    std::string strategy = "FirstBetter";
    if (argc != 2 && argc != 3) {
        std::cout << argv[0] << " "
                  << "number_of_queens strategy=FirstBetter" << std::endl;
        return 1;
    } else if (argc == 3) {
        strategy = argv[2];
        assert(strategy == "FirstBetter" || strategy == "SteepestSlope");
    }
    const int number_of_queens = std::stoi(argv[1]);

    namespace ls = paal::local_search;
    typedef ls::n_queens_solution_adapter<std::vector<int>> Adapter;
    std::vector<int> queens(number_of_queens);
    boost::iota(queens, 0);

    ls::n_queens_local_search_components<> comps;
    int nr_of_iterations(0);
    auto countingGain = paal::utils::make_counting_functor_adaptor(
        comps.get<ls::Gain>(), nr_of_iterations);
    auto countingComps =
        paal::data_structures::replace<ls::Gain>(countingGain, comps);

    if (strategy == "FirstBetter") {
        ls::n_queens_solution_first_improving(queens, countingComps);
    } else {
        paal::utils::always_false nop;
        ls::n_queens_solution_local_search(
            queens, ls::best_improving_strategy{}, nop, nop, countingComps);
    }
    std::cout << Adapter(queens).obj_fun() << " " << nr_of_iterations
              << std::endl;

    return 0;
}