//=======================================================================
// Copyright (c) 2013 Piotr Wygocki
//
// 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 knapsack_0_1.hpp
 * @brief
 * @author Piotr Wygocki
 * @version 1.0
 * @date 2013-09-30
 */
#ifndef PAAL_KNAPSACK_0_1_HPP
#define PAAL_KNAPSACK_0_1_HPP

#include "paal/utils/functors.hpp"
#include "paal/utils/knapsack_utils.hpp"
#include "paal/utils/less_pointees.hpp"
#include "paal/utils/irange.hpp"
#include "paal/dynamic/knapsack/fill_knapsack_dynamic_table.hpp"
#include "paal/dynamic/knapsack/knapsack_common.hpp"
#include "paal/greedy/knapsack_0_1_two_app.hpp"

#include <boost/range/adaptor/reversed.hpp>
#include <boost/function_output_iterator.hpp>
#include <boost/optional.hpp>

#include <vector>

namespace paal {

namespace detail {

/**
 * @brief For 0/1 knapsack dynamic algorithm for given element the table has to
 * be traversed from the highest to the lowest element
 */
struct Knapsack_0_1_get_position_range {
    template <typename T>
    auto operator()(T begin, T end)
        ->decltype(irange(begin, end) | boost::adaptors::reversed) {
        return irange(begin, end) | boost::adaptors::reversed;
    }
};

/**
 * @brief This class helps solving 0/1 knapsack problem.
 *        Function solve returns the optimal value
 *        Function Retrieve solution returns chosen elements
 *
 * @tparam Objects
 * @tparam ObjectSizeFunctor
 * @tparam ObjectValueFunctor
 */
template <typename Objects, typename ObjectSizeFunctor,
          typename ObjectValueFunctor, typename Comparator>
class Knapsack_0_1 {
    using base = knapsack_base<Objects, ObjectSizeFunctor, ObjectValueFunctor>;
    using SizeType = typename base::SizeType;
    using ValueType = typename base::ValueType;
    using ObjectType = typename base::ObjectType;
    using ObjectRef = typename base::ObjectRef;
    using ReturnType = typename base::return_type;
    using ValueOrNull = boost::optional<ValueType>;
    static_assert(std::is_integral<SizeType>::value,
                  "Size type must be integral");
    using ValueOrNullVector = std::vector<ValueOrNull>;

  public:

    Knapsack_0_1(ObjectSizeFunctor size, ObjectValueFunctor value,
                 Comparator compare = Comparator())
        : m_size(size), m_value(value), m_comparator(compare) {}

    /**
     * @brief  Function solves dynamic programming problem
     * @returns the optimal value
     */
    template <typename GetBestElement>
    ReturnType solve(Objects objects, SizeType capacity,
                     GetBestElement getBest) {
        m_object_on_size.resize(capacity + 1);
        fill_table(m_object_on_size, objects, capacity);
        auto maxValue = getBest(m_object_on_size.begin(),
                                m_object_on_size.end(), m_comparator);

        if (maxValue != m_object_on_size.end()) {
            return ReturnType(**maxValue, maxValue - m_object_on_size.begin());
        } else {
            return ReturnType(ValueType{}, SizeType{});
        }
    }

    //@brief here we find actual solution
    // that is, the chosen objects
    // this is done by simple divide and conquer strategy
    template <typename OutputIterator>
    void retrieve_solution(ValueType maxValue, SizeType size, Objects objects,
                           OutputIterator & out) const {
        m_object_on_size.resize(size + 1);
        m_object_on_size_rec.resize(size + 1);
        retrieve_solution_rec(maxValue, size, std::begin(objects),
                              std::end(objects), out);
    }

  private:
    template <typename OutputIterator, typename ObjectsIter>
    void retrieve_solution_rec(ValueType maxValue, SizeType capacity,
                               ObjectsIter oBegin, ObjectsIter oEnd,
                               OutputIterator & out) const {
        if (maxValue == ValueType()) {
            return;
        }

        auto objNr = std::distance(oBegin, oEnd);
        assert(objNr);

        // boundary case only one object left
        if (objNr == 1) {
            assert(m_value(*oBegin) == maxValue);
            *out = *oBegin;
            ++out;
            return;
        }

        // main case, at least 2 objects left
        auto midle = oBegin + objNr / 2;
        fill_table(m_object_on_size, boost::make_iterator_range(oBegin, midle),
                   capacity);
        fill_table(m_object_on_size_rec,
                   boost::make_iterator_range(midle, oEnd), capacity);

        SizeType capacityLeftPartInOptimalSolution{};
        for (auto capacityLeftPart : irange(capacity + 1)) {
            auto left = m_object_on_size[capacityLeftPart];
            auto right = m_object_on_size_rec[capacity - capacityLeftPart];
            if (left && right) {
                if (*left + *right == maxValue) {
                    capacityLeftPartInOptimalSolution = capacityLeftPart;
                    break;
                }
            }
        }
        auto left = m_object_on_size[capacityLeftPartInOptimalSolution];
        auto right =
            m_object_on_size_rec[capacity - capacityLeftPartInOptimalSolution];
        assert(left && right && *left + *right == maxValue);

        retrieve_solution_rec(*left, capacityLeftPartInOptimalSolution, oBegin,
                              midle, out);
        retrieve_solution_rec(
            *right, capacity - capacityLeftPartInOptimalSolution, midle, oEnd,
            out);
    }

    template <typename ObjectsRange>
    void fill_table(ValueOrNullVector &values, ObjectsRange &&objects,
                    SizeType capacity) const {
        fill_knapsack_dynamic_table(
            values.begin(), values.begin() + capacity + 1,
            std::forward<ObjectsRange>(objects), m_size,
            [&](ValueOrNull val,
                typename boost::range_iterator<ObjectsRange>::type obj) {
            return *val + m_value(*obj);
        },
            [&](ValueOrNull left, ValueOrNull right) {
            return m_comparator(*left, *right);
        },
            [](ValueOrNull & val) {
            val = ValueType{};
        },
            Knapsack_0_1_get_position_range{});
    }

    ObjectSizeFunctor m_size;
    ObjectValueFunctor m_value;
    Comparator m_comparator;
    mutable ValueOrNullVector m_object_on_size;
    mutable ValueOrNullVector m_object_on_size_rec;
};

template <typename Objects, typename ObjectSizeFunctor,
          typename ObjectValueFunctor, typename Comparator>
Knapsack_0_1<Objects, ObjectSizeFunctor, ObjectValueFunctor, Comparator>
make_knapsack_0_1(ObjectSizeFunctor size, ObjectValueFunctor value,
                  Comparator comp) {
    return Knapsack_0_1<Objects, ObjectSizeFunctor, ObjectValueFunctor,
                        Comparator>(size, value, comp);
}

template <typename Knapsack, typename IndexType, typename ValueType,
          typename Objects, typename OutputIterator>
void retrieve_solution(const Knapsack &knapsack, ValueType maxValue,
                       IndexType size, Objects &&objects, OutputIterator & out,
                       retrieve_solution_tag) {
    knapsack.retrieve_solution(maxValue, size, objects, out);
}

template <typename Knapsack, typename IndexType, typename ValueType,
          typename Objects, typename OutputIterator>
void retrieve_solution(const Knapsack &knapsack, ValueType maxValue,
                       IndexType size, Objects &&objects, OutputIterator & out,
                       no_retrieve_solution_tag) {}

/**
 * @brief Solution to Knapsack 0/1 problem
 *  overload for integral Size case
 */
template <typename KnapsackData, typename retrieve_solution_tag>
auto knapsack(KnapsackData knap_data, zero_one_tag, integral_size_tag,
         retrieve_solution_tag retrieve_solutionTag) {
    using Value = typename KnapsackData::value;

    auto knapsack = make_knapsack_0_1<typename KnapsackData::objects>(
        knap_data.get_size(), knap_data.get_value(), utils::less{});
    auto value_size =
        knapsack.solve(knap_data.get_objects(), knap_data.get_capacity(),
                       get_max_element_on_capacity_indexed_collection<Value>());
    retrieve_solution(knapsack, value_size.first, value_size.second,
                      knap_data.get_objects(), knap_data.get_output_iter(),
                      retrieve_solutionTag);
    return value_size;
}

/**
 * @brief Solution to Knapsack 0/1 problem
 *  overload for integral Value case
 */
template <typename KnapsackData, typename retrieve_solution_tag>
auto knapsack(KnapsackData knap_data, zero_one_tag, integral_value_tag,
         retrieve_solution_tag retrieve_solutionTag) {
    using Value = typename KnapsackData::value;
    using Size = typename KnapsackData::size;

    auto knapsack = make_knapsack_0_1<typename KnapsackData::objects>(
        knap_data.get_value(), knap_data.get_size(), utils::greater{});
    auto maxValue = get_value_bound(knap_data, zero_one_tag{}, upper_tag{});
    auto value_size = knapsack.solve(
        knap_data.get_objects(), maxValue,
        get_max_element_on_value_indexed_collection<boost::optional<Size>,
                                                    Value>(
            boost::optional<Size>(knap_data.get_capacity() + 1)));
    retrieve_solution(knapsack, value_size.first, value_size.second,
                      knap_data.get_objects(), knap_data.get_output_iter(),
                      retrieve_solutionTag);
    return std::make_pair(value_size.second, value_size.first);
}

} // detail

/**
 * @brief Solution to Knapsack 0/1 problem
 *
 * @tparam Objects
 * @tparam OutputIterator
 * @tparam ObjectSizeFunctor
 * @tparam ObjectValueFunctor
 * @param oBegin given objects
 * @param oEnd
 * @param out the result is returned using output iterator
 * @param size functor that for given object returns its size
 * @param value functor that for given object returns its value
 */
template <typename Objects, typename OutputIterator, typename ObjectSizeFunctor,
          typename ObjectValueFunctor = utils::return_one_functor>
auto knapsack_0_1(Objects &&objects,
             detail::FunctorOnRangePValue<ObjectSizeFunctor, Objects>
                 capacity, // capacity is of size type
             OutputIterator out, ObjectSizeFunctor size,
             ObjectValueFunctor value = ObjectValueFunctor{}) {

    return detail::knapsack_check_integrality(
        detail::make_knapsack_data(std::forward<Objects>(objects), capacity,
                                   size, value, out),
        detail::zero_one_tag{});
}

/**
 * @brief Solution to Knapsack 0/1 problem, without retrieving the objects in
* the solution
 *
 * @tparam Objects
 * @tparam OutputIterator
 * @tparam ObjectSizeFunctor
 * @tparam ObjectValueFunctor
 * @param oBegin given objects
 * @param oEnd
 * @param size functor that for given object returns its size
 * @param value functor that for given object returns its value
 */
template <typename Objects, typename ObjectSizeFunctor,
          typename ObjectValueFunctor = utils::return_one_functor>
auto knapsack_0_1_no_output(Objects &&objects,
                       detail::FunctorOnRangePValue<ObjectSizeFunctor, Objects>
                           capacity, // capacity is of size type
                       ObjectSizeFunctor size,
                       ObjectValueFunctor value = ObjectValueFunctor{}) {
    auto out = boost::make_function_output_iterator(utils::skip_functor{});
    return detail::knapsack_check_integrality(
        detail::make_knapsack_data(
            std::forward<Objects>(objects), capacity, size, value, out),
        detail::zero_one_tag{}, detail::no_retrieve_solution_tag{});
}

} // paal

#endif // PAAL_KNAPSACK_0_1_HPP


#include "benchmark.h"

#include <Eigen/Core>
#include <chrono>
#include <iostream>
#include <cstdlib>

void benchmark_Eigen(
    const std::vector<std::string>& parameterNames,
    const Json::Array& parameters,
    const std::vector<std::string>& returnNames,
    Json::Array& returnValues)
{
    //number of runs for time measures
    const int runs = 10;
	const int subruns = 2;

    //test if the config is valid
    assert(parameterNames.size() == 2);
    assert(parameterNames[0] == "Vector-Size");
    assert(parameterNames[1] == "Num-Combinations");
    assert(returnNames.size() == 1);
    assert(returnNames[0] == "Time");

    int numConfigs = parameters.Size();
    for (int config = 0; config < numConfigs; ++config)
    {
        //Input
        int vectorSize = parameters[config][0].AsInt32();
        int numCombinations = parameters[config][1].AsInt32();
        double totalTime = 0;
        std::cout << "  VectorSize: " << vectorSize << ", Num-Combinations: " << numCombinations << std::flush;

        //Create matrices
        std::vector<Eigen::VectorXf> vectors(numCombinations);
        std::vector<float> factors(numCombinations);
        for (int i = 0; i < numCombinations; ++i) {
            vectors[i] = Eigen::VectorXf(vectorSize);
            vectors[i].setRandom();
            factors[i] = std::rand() / (float)(RAND_MAX);
        }

        //Run it multiple times
        for (int run = 0; run < runs; ++run)
        {
            //Main logic
            auto start = std::chrono::steady_clock::now();

			for (int subrun = 0; subrun < subruns; ++subrun) {
				switch (numCombinations)
				{
				case 1: (vectors[0] * factors[0]).eval(); break;
				case 2: (vectors[0] * factors[0] + vectors[1] * factors[1]).eval(); break;
				case 3: (vectors[0] * factors[0] + vectors[1] * factors[1] + vectors[2] * factors[2]).eval(); break;
				case 4: (vectors[0] * factors[0] + vectors[1] * factors[1] + vectors[2] * factors[2] + vectors[3] * factors[3]).eval(); break;
				case 5: (vectors[0] * factors[0] + vectors[1] * factors[1] + vectors[2] * factors[2] + vectors[3] * factors[3] + vectors[4] * factors[4]).eval(); break;
				case 6: (vectors[0] * factors[0] + vectors[1] * factors[1] + vectors[2] * factors[2] + vectors[3] * factors[3] + vectors[4] * factors[4] + vectors[5] * factors[5]).eval(); break;
				case 7: (vectors[0] * factors[0] + vectors[1] * factors[1] + vectors[2] * factors[2] + vectors[3] * factors[3] + vectors[4] * factors[4] + vectors[5] * factors[5] + vectors[6] * factors[6]).eval(); break;
				case 8: (vectors[0] * factors[0] + vectors[1] * factors[1] + vectors[2] * factors[2] + vectors[3] * factors[3] + vectors[4] * factors[4] + vectors[5] * factors[5] + vectors[6] * factors[6] + vectors[7] * factors[7]).eval(); break;
				case 9: (vectors[0] * factors[0] + vectors[1] * factors[1] + vectors[2] * factors[2] + vectors[3] * factors[3] + vectors[4] * factors[4] + vectors[5] * factors[5] + vectors[6] * factors[6] + vectors[7] * factors[7] + vectors[8] * factors[8]).eval(); break;
				case 10: (vectors[0] * factors[0] + vectors[1] * factors[1] + vectors[2] * factors[2] + vectors[3] * factors[3] + vectors[4] * factors[4] + vectors[5] * factors[5] + vectors[6] * factors[6] + vectors[7] * factors[7] + vectors[8] * factors[8] + vectors[9] * factors[9]).eval(); break;
				}
			}

            auto finish = std::chrono::steady_clock::now();
            double elapsed = std::chrono::duration_cast<
                std::chrono::duration<double> >(finish - start).count() * 1000 / subruns;
            totalTime += elapsed;
        }

        //Result
        Json::Array result;
        double finalTime = totalTime / runs;
        result.PushBack(finalTime);
        returnValues.PushBack(result);
        std::cout << " -> " << finalTime << "ms" << std::endl;
    }
}


#ifndef STAN_MATH_TORSTEN_PKMODEL_POLYEXP_HPP
#define STAN_MATH_TORSTEN_PKMODEL_POLYEXP_HPP

#include <boost/math/tools/promotion.hpp>
#include <math.h>
#include <iostream>
#include <limits>
#include <vector>

namespace torsten {
// DEV
// Code is not optimized. Instead of using FOR loops for a and alpha,
// we coud use matrix operations. On the other hand, seeing most models
// have a relatively low number of compartments, efficiency gain would
// probably be very small.

/**
 *	PolyExp calculates portions of analytical solutions to certain ODEs.
 *	It is called by the pred and predSS functions for the linear one and two compartment
 *	models.
 *
 *	In particular, it calculates the sums of exponentials of the form:
 *		sum(i=1 to n) a[i]*exp(-alpha[i]*t)
 *	or the convolution of that with a step function.
 *
 *	First, update dose.
 *	Three cases:
 *		a. bolus dose
 *		b. no bolus dose, unsteady state
 *		c. no bolus dose, steady state
 *
 *	Second, update rate.
 * 	Two cases:
 *		a. truncated infusion
 *		b. continuous infusion
 *
 * @tparam T_x type of scalar for independent variable (often time)
 * @tparam T_dose type of scalar for dose
 * @tparam T_rate type of scalar for rate
 * @tparam T_xinf type of scalar for duration of infusion time
 * @tparam T_tau type of scalar for dosing interval (for multiple dosing case)
 * @tparam T_a type of scalar for scale relative to unit bolus
 * @tparam T_alpha type of scalar for unit
 * @param[in] x independent variable (often time)
 * @param[in] dose
 * @param[in] rate
 * @param[in] xinf duration of infusion time
 * @param[in] tau dosing interval (for multiple dosing case)
 * @param[in] ss steady state approximation (0:no, 1:yes)
 * @param[in] a relative unit to bolus
 * @param[in] alpha unit
 * @param[in] number of terms in polyexponential
 * @return sum of exponentials or convolution of sum with a step function
 *
 */
template<typename T_x, typename T_dose, typename T_rate, typename T_xinf,
  typename T_tau, typename T_a, typename T_alpha>
typename boost::math::tools::promote_args<T_x, T_dose, T_rate,
  typename boost::math::tools::promote_args<T_xinf, T_tau, T_a,
  T_alpha>::type>::type
PolyExp(const T_x& x,
        const T_dose& dose,
        const T_rate& rate,
        const T_xinf& xinf,
        const T_tau& tau,
        const bool& ss,
        const std::vector<T_a>& a,
        const std::vector<T_alpha>& alpha,
        const int& n) {
  using std::vector;
  using boost::math::tools::promote_args;

  typedef typename promote_args<T_x, T_dose, T_rate,
    typename promote_args<T_xinf, T_tau, T_a, T_alpha>::type>::type scalar;

  scalar result = 0, bolusResult, dx, nlntv;
  double inf = std::numeric_limits<double>::max();  // "infinity"

  assert((alpha.size() >= (size_t) n) && (a.size() >= (size_t) n));

  // UPDATE DOSE
  if (dose > 0) {  // bolus dose
    if ((tau <= 0) && (x >= 0)) {
      for (int i = 0; i < n; i++) result += a[i] * exp(-alpha[i] * x);
    } else {
      if (!ss) {
        nlntv = x / tau + 1;
        dx = x - trunc(x / tau) * tau;
        for (int i = 0; i < n; i++)
          result += a[i] * exp(-alpha[i] * x) *
            (1 - exp(-nlntv * alpha[i] * tau))
            / (1 - exp(-alpha[i] * tau));
      } else {
        dx = x - trunc(x / tau) * tau;
        for (int i = 0; i < n; i++)
          result += a[i] * exp(-alpha[i] * x) / (1 - exp(-alpha[i] * tau));
        }
    }
  }
  bolusResult = dose * result;

  // UPDATE RATE
  result = 0;
  if ((rate > 0) && (xinf < inf)) {  // truncated infusion
    if (tau <= 0) {
      if (x >= 0) {
        if (x <= xinf) {
           for (int i = 0; i < n; i++) {
            result += a[i] * (1 - exp(-alpha[i] * x)) / alpha[i];
           }
        } else {
          for (int i = 0; i < n; i++)
            result += a[i] * (1 - exp(-alpha[i] * xinf))
              * exp(-alpha[i] * (x - xinf)) / alpha[i];
        }
      }
    } else if (!ss) {
      assert(xinf <= tau);  // and "other case later", says Bill
      dx = x - trunc(x / tau) * tau;
      nlntv = trunc(x / tau) + 1;
      if (dx <= xinf) {
        for (int i = 0; i < n; i++) {
          if (n > 1)
            result += a[i] * (1 - exp(-alpha[i] * xinf)) * exp(-alpha[i] *
              (dx - xinf + tau)) * (1 - exp(-(nlntv - 1) * alpha[i] * tau)) /
              (1 - exp(-alpha[i] * tau)) / alpha[i];
          result += a[i] * (1 - exp(-alpha[i] * dx)) / alpha[i];
        }
      } else {
        for (int i = 0; i < n; i++)
          result += a[i] * (1 - exp(-alpha[i] * xinf)) * exp(-alpha[i]
            * (dx - xinf)) * (1 - exp(-nlntv * alpha[i] * tau))
            / (1 - exp(-alpha[i] * tau)) / alpha[i];
      }
    } else {
      assert(xinf <= tau);
      dx = x - trunc(x / tau) * tau;
      nlntv = trunc(x / tau) + 1;
      if (dx <= xinf) {
        for (int i = 0; i < n; i++)
          result += a[i] * (1 - exp(-alpha[i] * xinf)) * exp(-alpha[i]
            * (dx - xinf + tau)) / (1 - exp(-alpha[i] * tau)) / alpha[i] + a[i]
            * (1 - exp(-alpha[i] * dx)) / alpha[i];
      } else {
        for (int i = 0; i < n; i++)
          result += a[i] * (1 - exp(-alpha[i] * xinf))
            * exp(-alpha[i] * (dx - xinf))
            / (1 - exp(-alpha[i] * tau)) / alpha[i];
      }
    }
  } else {  // continuous infusion (xinf = inf). tau is ignored.
    if (!ss) {
      if (x >= 0) {
        for (int i = 0; i < n; i++)
          result += a[i] * (1 - exp(-alpha[i] * x)) / alpha[i];
      }
    } else {
      for (int i = 0; i < n; i++)
        result += a[i] / alpha[i];
    }
  }

  return bolusResult + rate * result;
}

}

#endif


//
// Created by huangqinjin on 7/18/17.
//
#include <glw/Widget.hpp>
#include <GL/glew.h>
#include <Eigen/Geometry>
#include <iostream>

using namespace glw;
using namespace Eigen;

class MainWindow : public Widget
{
    const bool orth;
    Affine3f T = Affine3f::Identity();

public:
    explicit MainWindow(bool orth) : orth(orth) {}

private:
    void initialize() override
    {
        glClearColor(0, 0, 0, 0);
        glShadeModel(GL_SMOOTH);
        glEnable(GL_DEPTH_TEST);

        glMatrixMode(GL_PROJECTION);
        glLoadIdentity();
        if(orth) glOrtho(-1, 1, -1, 1, 1, 3 /* -1 */);
        else glFrustum(-1.0, 1.0, -1.0, 1.0, 1, 100);
        glMatrixMode(GL_MODELVIEW);

        T = Translation3f(0, 0, -2.f);

        std::cout << "initialize" << std::endl;
    }

    void paintEvent(PaintEvent* e) override
    {
        // Clear the screen.
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        glLoadMatrixf(T.data());
        glTranslatef(-0.5f, -0.5f, -0.5f);

        GLfloat data[8][3] = {
                {0, 0, 0},
                {1, 0, 0},
                {1, 1, 0},
                {0, 1, 0},
                {0, 1, 1},
                {0, 0, 1},
                {1, 0, 1},
                {1, 1, 1},
        };

        glBegin(GL_QUADS);
        glColor3fv(data[0]); glVertex3fv(data[0]);
        glColor3fv(data[1]); glVertex3fv(data[1]);
        glColor3fv(data[2]); glVertex3fv(data[2]);
        glColor3fv(data[3]); glVertex3fv(data[3]);

        glColor3fv(data[2]); glVertex3fv(data[2]);
        glColor3fv(data[3]); glVertex3fv(data[3]);
        glColor3fv(data[4]); glVertex3fv(data[4]);
        glColor3fv(data[7]); glVertex3fv(data[7]);

        glColor3fv(data[4]); glVertex3fv(data[4]);
        glColor3fv(data[7]); glVertex3fv(data[7]);
        glColor3fv(data[6]); glVertex3fv(data[6]);
        glColor3fv(data[5]); glVertex3fv(data[5]);

        glColor3fv(data[6]); glVertex3fv(data[6]);
        glColor3fv(data[5]); glVertex3fv(data[5]);
        glColor3fv(data[0]); glVertex3fv(data[0]);
        glColor3fv(data[1]); glVertex3fv(data[1]);

        glColor3fv(data[0]); glVertex3fv(data[0]);
        glColor3fv(data[3]); glVertex3fv(data[3]);
        glColor3fv(data[4]); glVertex3fv(data[4]);
        glColor3fv(data[5]); glVertex3fv(data[5]);

        glColor3fv(data[1]); glVertex3fv(data[1]);
        glColor3fv(data[2]); glVertex3fv(data[2]);
        glColor3fv(data[7]); glVertex3fv(data[7]);
        glColor3fv(data[6]); glVertex3fv(data[6]);
        glEnd();

        // Draw the scene:
//        glBegin(GL_TRIANGLES);
//        glColor3f(1.0f, 0.0f, 0.0f);
//        glVertex3f(-1, -1, 0.0);
//
//        glColor3f(0.0f, 1.0f, 0.0f);
//        glVertex3f(1, 0.0, 0.0);
//
//        glColor3f(0.0f, 0.0f, 1.0f);
//        glVertex3f(0.0, 1, 0.0);
//        glEnd();
//
//        repaint(1);
        std::cout << "paint " << e->dt << std::endl;
    }

    void keyEvent(KeyEvent* e) override
    {
        std::cout << "key " << (char)e->key << ' ' << (int)e->action << std::endl;
    }

    void mouseEvent(MouseEvent* e) override
    {
        std::cout << "mouse " << (int)e->button << ' ' << (int)e->action << std::endl;
        if(e->button == MouseButton::MIDDLE && e->action == KeyAction::RELEASE)
        {
            initialize();
            repaint();
        }
    }

    void wheelEvent(WheelEvent* e) override
    {
        std::cout << "wheel " << e->dx << std::endl;

        if(orth) T.linear() *= (1 + e->dx * 0.1f);
        else T.translation() += Vector3f(0, 0, -e->dx * 0.1f);
        repaint();
    }

    void cursorEvent(CursorEvent* e) override
    {
        if(KeyAction::PRESS == status(MouseButton::LEFT))
        {
            Vector3f axis(e->dp.y, e->dp.x, 0);
            T.linear() = AngleAxisf(axis.norm() * 0.01f, axis.normalized()) * T.linear();
        }
        else if(KeyAction::PRESS == status(MouseButton::RIGHT))
        {
            Vector3f dir(e->dp.x, -e->dp.y, 0);
            T.translation() += dir * 0.01f;
        }
        else
        {
            return;
        }
        repaint();
        std::cout << "cursor " << ' ' << (int)e->action << ' ' << e->dp.x << ' ' << e->dp.y << std::endl;
    }

    void moveEvent(MoveEvent* e) override
    {
        std::cout << "move " << e->dp.x << ' ' << e->dp.y << std::endl;
    }

    void resizeEvent(SizeEvent* e) override
    {
        auto sz = size();
        auto s = static_cast<GLsizei>((std::min)(sz.w, sz.h));
        glViewport(0, 0, s, s);

        std::cout << "resize " << e->dz.w << ' ' << e->dz.h << std::endl;
    }

    void closeEvent() override
    {
        close();
        std::cout << "close" << std::endl;
    }
};

int main(int argc, char* argv[])
{
    std::cout << "STDCPP_DEFAULT_NEW_ALIGNMENT = "
              #ifdef __STDCPP_DEFAULT_NEW_ALIGNMENT__
              << __STDCPP_DEFAULT_NEW_ALIGNMENT__
              #else
              << "NULL"
              #endif
              << "    EIGEN_MAX_STATIC_ALIGN_BYTES = "
              << EIGEN_MAX_STATIC_ALIGN_BYTES
              << std::endl;

    Application app(argc, argv);
    MainWindow w(argc <= 1);
    w.rename("MainWindow");
    w.resize({200, 200});
    w.show();
    return app.exec();
}


#ifndef PCL_TRACKING_IMPL_DISTANCE_COHERENCE_H_
#define PCL_TRACKING_IMPL_DISTANCE_COHERENCE_H_

#include <Eigen/Dense>
#include <pcl/tracking/distance_coherence.h>

namespace pcl
{
  namespace tracking
  {
    template <typename PointInT> double
    DistanceCoherence<PointInT>::computeCoherence (PointInT &source, PointInT &target)
    {
       Eigen::Vector4f p = source.getVector4fMap ();
       Eigen::Vector4f p_dash = target.getVector4fMap ();
       double d = (p - p_dash).norm ();
       return 1.0 / (1.0 + d * d * weight_);
    }
  }
}

#define PCL_INSTANTIATE_DistanceCoherence(T) template class PCL_EXPORTS pcl::tracking::DistanceCoherence<T>;

#endif


#include <Eigen/Dense>

int main() {
  auto res = Eigen::Matrix3d::Random().skew();
  auto res2 = Eigen::Quaterniond::Identity().integrate(Eigen::Vector3d::Ones());
}


#ifndef DIIS_HPP
#define DIIS_HPP

#include <armadillo>
#include <deque>

namespace willow {
namespace qcmol {

class DIIS {

public:
  DIIS() {}
  DIIS(const arma::mat &Sm, const arma::mat &SmInvh, const arma::mat &Fm)
      : Sm_(Sm), SmInvh_(SmInvh), FmOld_(Fm) {
    // o_started_ = true;
    err_cutoff_ = 0.05;
  }

  arma::mat getF(arma::mat &Fm, arma::mat &Dm) {
    // compute error matrix
    arma::mat FmNew(Fm.n_rows, Fm.n_cols, arma::fill::zeros);
    arma::mat errMat = Fm * Dm * Sm_ - Sm_ * Dm * Fm;
    // transform it to the orthonormal basis
    errMat = SmInvh_.t() * errMat * SmInvh_;

    arma::vec err = arma::vectorise(errMat);

    max_err_ = arma::max(err);
    auto tmp = fabs(arma::min(err));

    if (tmp > max_err_)
      max_err_ = tmp;

    //    if ( max_err_ > err_cutoff_ ) {
    if (false) {
      // Do simple averaging until DIIS starts
      // cout << "Simple Mixed " << endl;
      FmNew = 0.5 * Fm + 0.5 * FmOld_;
      FmOld_ = Fm;
    } else {
      // cout << "DIIS Mixed " << endl;

      if (FmList_.size() == ndiis_) {
        FmList_.pop_front();
        ErrList_.pop_front();
      }

      FmList_.push_back(Fm);
      ErrList_.push_back(err);

      int N = ErrList_.size();
      // Original, Pulay's DIIS
      arma::mat B(N + 1, N + 1, arma::fill::zeros);
      // RHS vector
      arma::vec A(N + 1, arma::fill::zeros);

      // Compute errors;
      for (auto i = 0; i < N; i++)
        for (auto j = 0; j <= i; j++) {
          B(i, j) = arma::dot(ErrList_[i], ErrList_[j]);
          B(j, i) = B(i, j);
        }

      // Fill in the rest of B
      for (auto i = 0; i < N; i++) {
        B(N, i) = -1.0;
        B(i, N) = -1.0;
      }
      // Fill in A
      A(N) = -1.0;

      // Solve B*X = A
      arma::vec X;
      bool succ = arma::solve(X, B, A);

      if (succ) {
        // Form weighted Fock Matrix
        FmNew.zeros();
        for (auto i = 0; i < FmList_.size(); i++)
          FmNew += X(i) * FmList_[i];
      } else {
        FmNew = 0.5 * Fm + 0.5 * FmOld_;
      }
    }

    return FmNew;
  }

private:
  // bool o_started_;
  const unsigned int ndiis_ = 15;
  double err_cutoff_;
  double max_err_;
  arma::mat Sm_;
  arma::mat SmInvh_; // Half-inverse Overlap Matrix
  arma::mat FmOld_;

  std::deque<arma::mat> FmList_;
  std::deque<arma::vec> ErrList_;
};

} // namespace qcmol
} // namespace willow

#endif


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2015 Peter Caspers

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file cclgmparametrization.hpp
    \brief cross currency lgm parametrization
*/

#ifndef quantlib_cclgm_parametrization_hpp
#define quantlib_cclgm_parametrization_hpp

#include <ql/math/integrals/integral.hpp>
#include <ql/math/integrals/simpsonintegral.hpp>
#include <ql/patterns/curiouslyrecurring.hpp>
#include <ql/experimental/models/lgmparametrization.hpp>
#include <ql/experimental/models/lgmfxparametrization.hpp>

#include <vector>

#include <boost/make_shared.hpp>
#include <boost/bind.hpp>

namespace QuantLib {

namespace detail {

template <class Impl, class ImplFx, class ImplLgm>
class CcLgmParametrization : public CuriouslyRecurringTemplate<Impl> {
  public:
    //! constructor
    CcLgmParametrization(
        const std::vector<boost::shared_ptr<
            detail::LgmFxParametrization<ImplFx> > > &fxParametrizations,
        const std::vector<boost::shared_ptr<
            detail::LgmParametrization<ImplLgm> > > &lgmParametrizations)
        : n_(fxParametrizations.size()),
          fxParametrizations_(fxParametrizations),
          lgmParametrizations_(lgmParametrizations) {

        QL_REQUIRE(
            fxParametrizations.size() + 1 == lgmParametrizations.size(),
            "number of fx parametrizations ("
                << fxParametrizations.size()
                << ") must be equal to the number of lgm parametrizations ("
                << lgmParametrizations.size() << ") minus one");

        // if no integrator is given, we provide a simple one as default
        integrator_ = boost::make_shared<SimpsonIntegral>(1E-10, 100);
    }

    void update() const;

    //! inspectors
    const Size n() const;

    const Real H_i(const Size i, const Real t) const;
    const Real zeta_i(const Size i, const Real t) const;
    const Real alpha_i_alpha_j(const Size i, const Size j, const Real t) const;
    const Real alpha_i_sigma_j(const Size i, const Size j, const Real t) const;
    const Real sigma_i_sigma_j(const Size i, const Size j, const Real t) const;
    const Real H_i_alpha_i_alpha_j(const Size i, const Size j,
                                   const Real t) const;
    const Real H_i_H_j_alpha_i_alpha_j(const Size i, const Size j,
                                       const Real t) const;
    const Real H_i_alpha_i_sigma_j(const Size i, const Size j,
                                   const Real t) const;

    const Real int_alpha_i_alpha_j(const Size i, const Size j, const Real a,
                                   const Real b) const;
    const Real int_alpha_i_sigma_j(const Size i, const Size j, const Real a,
                                   const Real b) const;
    const Real int_sigma_i_sigma_j(const Size i, const Size j, const Real a,
                                   const Real b) const;
    const Real int_H_i_alpha_i_alpha_j(const Size i, const Size j, const Real a,
                                       const Real b) const;
    const Real int_H_i_H_j_alpha_i_alpha_j(const Size i, const Size j,
                                           const Real a, const Real b) const;
    const Real int_H_i_alpha_i_sigma_j(const Size i, const Size j, const Real a,
                                       const Real b) const;
    const Real rho_alpha_alpha(const Size i, const Size j) const;
    const Real rho_alpha_sigma(const Size i, const Size j) const;
    const Real rho_sigma_sigma(const Size i, const Size j) const;

  protected:
    //! interface (required)

    const Real rho_alpha_alpha_impl(const Size i, const Size j) const;
    const Real rho_alpha_sigma_impl(const Size i, const Size j) const;
    const Real rho_sigma_sigma_impl(const Size i, const Size j) const;

    //! interface (optional, default implementation uses numerical integration)

    const void update_impl() const;
    const Real int_alpha_i_alpha_j_impl(const Size i, const Size j,
                                        const Real a, const Real b) const;
    const Real int_alpha_i_sigma_j_impl(const Size i, const Size j,
                                        const Real a, const Real b) const;
    // for i=j this is not used, but the native variance implementation
    // in LgmFxParametrization
    const Real int_sigma_i_sigma_j_impl(const Size i, const Size j,
                                        const Real a, const Real b) const;
    const Real int_H_i_alpha_i_alpha_j_impl(const Size i, const Size j,
                                            const Real a, const Real b) const;
    const Real int_H_i_H_j_alpha_i_alpha_j_impl(const Size i, const Size j,
                                                const Real a,
                                                const Real b) const;
    const Real int_H_i_alpha_i_sigma_j_impl(const Size i, const Size j,
                                            const Real a, const Real b) const;

    //! optionally changes the integrator
    void setIntegrator(const boost::shared_ptr<Integrator> &integrator) {
        integrator_ = integrator;
    }

  private:
    const Size n_;
    std::vector<boost::shared_ptr<detail::LgmFxParametrization<ImplFx> > >
        fxParametrizations_;
    std::vector<boost::shared_ptr<detail::LgmParametrization<ImplLgm> > >
        lgmParametrizations_;
    boost::shared_ptr<Integrator> integrator_;
};

// inline

template <class Impl, class ImplFx, class ImplLgm>
void CcLgmParametrization<Impl, ImplFx, ImplLgm>::update() const {
    this->impl().update_impl();
}

template <class Impl, class ImplFx, class ImplLgm>
const Size CcLgmParametrization<Impl, ImplFx, ImplLgm>::n() const {
    return n_;
}

template <class Impl, class ImplFx, class ImplLgm>
const Real
CcLgmParametrization<Impl, ImplFx, ImplLgm>::H_i(const Size i,
                                                 const Real t) const {
    return lgmParametrizations_[i]->H(t);
}

template <class Impl, class ImplFx, class ImplLgm>
const Real
CcLgmParametrization<Impl, ImplFx, ImplLgm>::zeta_i(const Size i,
                                                    const Real t) const {
    return lgmParametrizations_[i]->zeta(t);
}

template <class Impl, class ImplFx, class ImplLgm>
const Real CcLgmParametrization<Impl, ImplFx, ImplLgm>::alpha_i_alpha_j(
    const Size i, const Size j, const Real t) const {
    return lgmParametrizations_[i]->alpha(t) *
           lgmParametrizations_[j]->alpha(t) * rho_alpha_alpha(i, j);
}

template <class Impl, class ImplFx, class ImplLgm>
inline const Real CcLgmParametrization<Impl, ImplFx, ImplLgm>::alpha_i_sigma_j(
    const Size i, const Size j, const Real t) const {
    return lgmParametrizations_[i]->alpha(t) *
           fxParametrizations_[j]->sigma(t) * rho_alpha_sigma(i, j);
}

template <class Impl, class ImplFx, class ImplLgm>
inline const Real CcLgmParametrization<Impl, ImplFx, ImplLgm>::sigma_i_sigma_j(
    const Size i, const Size j, const Real t) const {
    return fxParametrizations_[i]->sigma(t) * fxParametrizations_[j]->sigma(t) *
           rho_sigma_sigma(i, j);
}

template <class Impl, class ImplFx, class ImplLgm>
inline const Real
CcLgmParametrization<Impl, ImplFx, ImplLgm>::H_i_alpha_i_alpha_j(
    const Size i, const Size j, const Real t) const {
    return lgmParametrizations_[i]->H(t) * lgmParametrizations_[i]->alpha(t) *
           lgmParametrizations_[j]->alpha(t) * rho_alpha_alpha(i, j);
}

template <class Impl, class ImplFx, class ImplLgm>
inline const Real
CcLgmParametrization<Impl, ImplFx, ImplLgm>::H_i_H_j_alpha_i_alpha_j(
    const Size i, const Size j, const Real t) const {
    return lgmParametrizations_[i]->H(t) * lgmParametrizations_[j]->H(t) *
           lgmParametrizations_[i]->alpha(t) *
           lgmParametrizations_[j]->alpha(t) * rho_alpha_alpha(i, j);
}

template <class Impl, class ImplFx, class ImplLgm>
inline const Real
CcLgmParametrization<Impl, ImplFx, ImplLgm>::H_i_alpha_i_sigma_j(
    const Size i, const Size j, const Real t) const {
    return lgmParametrizations_[i]->H(t) * lgmParametrizations_[i]->alpha(t) *
           fxParametrizations_[j]->sigma(t) * rho_alpha_sigma(i, j);
}

template <class Impl, class ImplFx, class ImplLgm>
inline const Real
CcLgmParametrization<Impl, ImplFx, ImplLgm>::int_alpha_i_alpha_j(
    const Size i, const Size j, const Real a, const Real b) const {
    return this->impl().int_alpha_i_alpha_j_impl(i, j, a, b);
}

template <class Impl, class ImplFx, class ImplLgm>
inline const Real
CcLgmParametrization<Impl, ImplFx, ImplLgm>::int_alpha_i_sigma_j(
    const Size i, const Size j, const Real a, const Real b) const {
    return this->impl().int_alpha_i_sigma_j_impl(i, j, a, b);
}

template <class Impl, class ImplFx, class ImplLgm>
inline const Real
CcLgmParametrization<Impl, ImplFx, ImplLgm>::int_sigma_i_sigma_j(
    const Size i, const Size j, const Real a, const Real b) const {
    // use the "native" implementation if possible
    if(i==j)
        return fxParametrizations_[i]->variance(b) -
               fxParametrizations_[i]->variance(a);
    return this->impl().int_sigma_i_sigma_j_impl(i, j, a, b);
}

template <class Impl, class ImplFx, class ImplLgm>
inline const Real
CcLgmParametrization<Impl, ImplFx, ImplLgm>::int_H_i_alpha_i_alpha_j(
    const Size i, const Size j, const Real a, const Real b) const {
    return this->impl().int_H_i_alpha_i_alpha_j_impl(i, j, a, b);
}

template <class Impl, class ImplFx, class ImplLgm>
inline const Real
CcLgmParametrization<Impl, ImplFx, ImplLgm>::int_H_i_H_j_alpha_i_alpha_j(
    const Size i, const Size j, const Real a, const Real b) const {
    return this->impl().int_H_i_H_j_alpha_i_alpha_j_impl(i, j, a, b);
}

template <class Impl, class ImplFx, class ImplLgm>
inline const Real
CcLgmParametrization<Impl, ImplFx, ImplLgm>::int_H_i_alpha_i_sigma_j(
    const Size i, const Size j, const Real a, const Real b) const {
    return this->impl().int_H_i_alpha_i_sigma_j_impl(i, j, a, b);
}

template <class Impl, class ImplFx, class ImplLgm>
inline const Real CcLgmParametrization<Impl, ImplFx, ImplLgm>::rho_alpha_alpha(
    const Size i, const Size j) const {
    return this->impl().rho_alpha_alpha_impl(i, j);
}
template <class Impl, class ImplFx, class ImplLgm>
inline const Real CcLgmParametrization<Impl, ImplFx, ImplLgm>::rho_alpha_sigma(
    const Size i, const Size j) const {
    return this->impl().rho_alpha_sigma_impl(i, j);
}
template <class Impl, class ImplFx, class ImplLgm>
inline const Real CcLgmParametrization<Impl, ImplFx, ImplLgm>::rho_sigma_sigma(
    const Size i, const Size j) const {
    return this->impl().rho_sigma_sigma_impl(i, j);
}

// default implementation

template <class Impl, class ImplFx, class ImplLgm>
const Real CcLgmParametrization<Impl, ImplFx, ImplLgm>::rho_alpha_alpha_impl(
    const Size i, const Size j) const {
    QL_FAIL("correlation alpha-alpha not implemented");
}
template <class Impl, class ImplFx, class ImplLgm>
const Real CcLgmParametrization<Impl, ImplFx, ImplLgm>::rho_alpha_sigma_impl(
    const Size i, const Size j) const {
    QL_FAIL("correlation alpha-sigma not implemented");
}
template <class Impl, class ImplFx, class ImplLgm>
const Real CcLgmParametrization<Impl, ImplFx, ImplLgm>::rho_sigma_sigma_impl(
    const Size i, const Size j) const {
    QL_FAIL("correlation sigma-sigma not implemented");
}

template <class Impl, class ImplFx, class ImplLgm>
const void CcLgmParametrization<Impl, ImplFx, ImplLgm>::update_impl() const {
    for (Size i = 0; i < fxParametrizations_.size(); ++i) {
        fxParametrizations_[i]->update();
    }
    for (Size i = 0; i < lgmParametrizations_.size(); ++i) {
        lgmParametrizations_[i]->update();
    }
}

template <class Impl, class ImplFx, class ImplLgm>
inline const Real
CcLgmParametrization<Impl, ImplFx, ImplLgm>::int_alpha_i_alpha_j_impl(
    const Size i, const Size j, const Real a, const Real b) const {
    return integrator_->operator()(
        boost::bind(
            &CcLgmParametrization<Impl, ImplFx, ImplLgm>::alpha_i_alpha_j, this,
            i, j, _1),
        a, b);
}

template <class Impl, class ImplFx, class ImplLgm>
inline const Real
CcLgmParametrization<Impl, ImplFx, ImplLgm>::int_alpha_i_sigma_j_impl(
    const Size i, const Size j, const Real a, const Real b) const {

    return integrator_->operator()(
        boost::bind(
            &CcLgmParametrization<Impl, ImplFx, ImplLgm>::alpha_i_sigma_j, this,
            i, j, _1),
        a, b);
}

template <class Impl, class ImplFx, class ImplLgm>
inline const Real
CcLgmParametrization<Impl, ImplFx, ImplLgm>::int_sigma_i_sigma_j_impl(
    const Size i, const Size j, const Real a, const Real b) const {

    return integrator_->operator()(
        boost::bind(
            &CcLgmParametrization<Impl, ImplFx, ImplLgm>::sigma_i_sigma_j, this,
            i, j, _1),
        a, b);
}

template <class Impl, class ImplFx, class ImplLgm>
inline const Real
CcLgmParametrization<Impl, ImplFx, ImplLgm>::int_H_i_alpha_i_alpha_j_impl(
    const Size i, const Size j, const Real a, const Real b) const {
    return integrator_->operator()(
        boost::bind(
            &CcLgmParametrization<Impl, ImplFx, ImplLgm>::H_i_alpha_i_alpha_j,
            this, i, j, _1),
        a, b);
}

template <class Impl, class ImplFx, class ImplLgm>
inline const Real
CcLgmParametrization<Impl, ImplFx, ImplLgm>::int_H_i_H_j_alpha_i_alpha_j_impl(
    const Size i, const Size j, const Real a, const Real b) const {
    return integrator_->operator()(
        boost::bind(&CcLgmParametrization<Impl, ImplFx,
                                          ImplLgm>::H_i_H_j_alpha_i_alpha_j,
                    this, i, j, _1),
        a, b);
}

template <class Impl, class ImplFx, class ImplLgm>
inline const Real
CcLgmParametrization<Impl, ImplFx, ImplLgm>::int_H_i_alpha_i_sigma_j_impl(
    const Size i, const Size j, const Real a, const Real b) const {
    return integrator_->operator()(
        boost::bind(
            &CcLgmParametrization<Impl, ImplFx, ImplLgm>::H_i_alpha_i_sigma_j,
            this, i, j, _1),
        a, b);
}

} // namespace detail

} // namespace QuantLib

#endif


/////////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::distributions::location_scale::random.hpp             //
//                                                                             //
//  (C) Copyright 2009 Erwann Rogard                                           //
//  Use, modification and distribution are subject to 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_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_LOCATION_SCALE_RANDOM_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_LOCATION_SCALE_RANDOM_HPP_ER_2009
#include <boost/statistics/detail/distribution_toolkit/distributions/location_scale/location_scale.hpp>
#include <boost/random/location_scale.hpp>
#include <boost/statistics/detail/distribution_common/meta/random/distribution.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace distribution{
namespace meta{

    template<typename Z>
    struct random_distribution<
        toolkit::location_scale_distribution<Z>
    >{
        typedef toolkit::location_scale_distribution<Z> dist_;
        typedef typename meta::random_distribution<Z>::type z_;
        typedef boost::random::location_scale_distribution<z_> type;

        static type call(const dist_& d){
            return type(
                d.mu(),
                d.sigma(),
                make_random_distribution(d.z())
            );
        }
    };

}// meta
}// distribution
}// detail
}// statistics
}// boost

#endif


#include "MatLabVariable.h"
#include <array>
//#include <boost/locale.hpp>
//#include <boost/format.hpp>

//#include "../../../Exception.h"
//#include "../../../StringUtilities.h"

#define var const auto

using namespace Eigen;
namespace Jde::IO::MatLab
{
#pragma region Constructors
	MatLabVariable::MatLabVariable( string_view name, const Eigen::MatrixXd& matrix ):
		_name{name},
		_matrixType(Matrix),
		_matrix(&matrix)
	{}

	MatLabVariable::MatLabVariable( string_view name, const Eigen::VectorXd& vector ):
		_name{name},
		_matrixType(Vector),
		_vector(&vector)
	{}
	MatLabVariable::MatLabVariable( string_view name, const std::shared_ptr<Eigen::SparseMatrix<double>> matrix ):
		_name{name},
		_matrixType(Sparse),
		_sparse(matrix)
	{}

	MatLabVariable::MatLabVariable( string_view name, matio_classes classType, matio_types data_type, int rank, size_t *dims, void *data, int opt ):
		_pVariable( Mat_VarCreate(string(name).c_str(), classType, data_type, rank, dims, data, opt) ),
		_name( name ),
		_matrixType(Data)
	{}

	MatLabVariable::MatLabVariable( matvar_t* variable ):
		_pVariable(variable),
		_name( variable->name )
	{}

	MatLabVariable::MatLabVariable( const MatLabVariable& copyFrom ):
		_pVariable( copyFrom._pVariable),
		_name(copyFrom._name),
		_matrixType(copyFrom._matrixType)
	{
		if( _matrixType==Matrix )
			_matrix = copyFrom._matrix;
		else if( _matrixType==Vector )
			_vector= copyFrom._vector;
		else if( _matrixType==Sparse )
			_sparse = copyFrom._sparse;
	}

	MatLabVariable::~MatLabVariable()
	{
		//Logging::ProcessInfo::LogMemoryUsage( (boost::wformat("Before Variable '%1%' Free") % _name).str() );
		Mat_VarFree(_pVariable);
		_pVariable = nullptr;
		//Logging::ProcessInfo::LogMemoryUsage( (boost::wformat("After Variable '%1%' Free") % _name).str() );
	}
#pragma endregion
/*
	sp<Eigen::MatrixXd> MatLabVariable::ToMatrix()const
	{
		//MatLab::MatLabVariable variable = file[variableName];
		if( _pVariable==NULL )
			throw Exception( "Variable not initialized" );

		size_t stride = Mat_SizeOf( _pVariable->data_type );

		if( _pVariable->class_type!=MAT_C_DOUBLE )
			throw Exception( "class_type!=MAT_C_DOUBLE" );

		const size_t rowCount =  _pVariable->dims[0];
		const size_t columnCount =  _pVariable->dims[1];
		auto matrix = make_shared<Eigen::MatrixXd>( rowCount,columnCount );
		for( size_t i = 0; i < _pVariable->dims[0]; ++i )
		{
			for ( size_t j = 0; j < _pVariable->dims[1]; ++j )
			{
				size_t index = _pVariable->dims[0]*j+i;
				const char* data = static_cast<const char*>(_pVariable->data);
				double* value = (double*)( data+index*stride );
				(*matrix)(i,j) = *value;
			}
		}
		return matrix;
	}
	sp<Eigen::VectorXd> MatLabVariable::ToVector()const
	{
		if( _pVariable==NULL )
			throw Exception( "Variable not initialized" );

		size_t stride = Mat_SizeOf( _pVariable->data_type );

		if( _pVariable->class_type!=MAT_C_DOUBLE )
			throw Exception( "class_type!=MAT_C_DOUBLE" );

		const size_t rowCount =  _pVariable->dims[0];
		//const size_t columnCount =  _pVariable->dims[1];
		auto vector = make_shared<Eigen::VectorXd>(rowCount);
		for( size_t index = 0; index < rowCount; ++index )
		{
			const char* data = static_cast<const char*>(_pVariable->data);
			double* value = (double*)( data+index*stride );
			(*vector)(index) = *value;
		}
		return vector;
	}
*/
	double MatLabVariable::operator()(size_t rowIndex, size_t columnIndex) const
	{
		const char* data = static_cast<const char*>(_pVariable->data);
		auto index = _pVariable->dims[0]*columnIndex+rowIndex;
		auto stride = Mat_SizeOf( _pVariable->data_type );
		double* value = (double*)( data+index*stride );
		return *value;
	}
	void MatLabVariable::Write( mat_t* pFile )const
	{
		if( _matrixType!=Data )
		{
			if( _matrixType==Sparse )
			{
				Write( pFile, _name, *_sparse );
				return;
			}
			else
			{
				const size_t rowCount = _matrixType==Matrix ? _matrix->rows() : _vector->rows();
				const size_t columnCount = _matrixType==Matrix ? _matrix->cols() : 1;
				size_t* dims = static_cast<size_t*>( malloc( 2*sizeof(size_t)) );
				dims[0] = rowCount;
				dims[1] = columnCount;
				double* values = static_cast<double*>( malloc(rowCount*columnCount*sizeof(double)) );
				for( uint rowIndex=0; rowIndex<rowCount; ++rowIndex )
				{
					for( uint columnIndex=0; columnIndex<columnCount; ++columnIndex )
						values[rowIndex*columnCount+columnIndex] = _matrixType==Matrix ? (*_matrix)(rowIndex, columnIndex) : (*_vector)(rowIndex);
				}
				_pVariable = Mat_VarCreate( _name.c_str(), MAT_C_DOUBLE,MAT_T_DOUBLE,2,dims,values,0);
			}
		}
		Mat_VarWrite( pFile, _pVariable, MAT_COMPRESSION_ZLIB/*MAT_COMPRESSION_NONE*/ );
	}
#pragma region GetMatioType
	matio_types MatLabVariable::GetMatioType( const std::type_info& typeInfo )
	{
		matio_types dataType = matio_types::MAT_T_DOUBLE;
		if( typeInfo==typeid(float) )
			dataType = matio_types::MAT_T_SINGLE;
		else if( typeInfo==typeid(int) )
			dataType = matio_types::MAT_T_INT32;
		else if( typeInfo==typeid(size_t) )
			dataType = matio_types::MAT_T_UINT64;
		else if( typeInfo!=typeid(double) )
			THROW( Exception( "Datatype '{}' has not been implemented."sv, typeInfo.name()) );
		return dataType;
	}
	matio_classes MatLabVariable::GetMatioClass( const std::type_info& typeInfo )noexcept(false)
	{
		matio_classes classType = matio_classes::MAT_C_DOUBLE;
		if( typeInfo==typeid(float) )
			classType = matio_classes::MAT_C_SINGLE;
		else if( typeInfo==typeid(int) )
			classType = matio_classes::MAT_C_INT32;
		else if( typeInfo==typeid(size_t) )
			classType = matio_classes::MAT_C_UINT64;
		else if( typeInfo!=typeid(double) )
			THROW( Exception("class '{}' has not been implemented."sv, typeInfo.name()) );
		return classType;
	}
#pragma endregion
	const mat_sparse_t* MatLabVariable::GetSparsePointer()const noexcept(false)
	{
		if( _pVariable->class_type!=MAT_C_SPARSE )
			THROW( Exception( "class_type!=MAT_C_SPARSE"sv) );
		return static_cast<mat_sparse_t*>(_pVariable->data);
	}

	const std::vector<std::pair<size_t,size_t>> MatLabVariable::ColumnCounts()const noexcept(false)
	{
		std::vector<std::pair<size_t,size_t>> columnCounts( cols() );
		if( _pVariable->class_type==MAT_C_SPARSE )
		{
			const mat_sparse_t* sparse =  GetSparsePointer();
			size_t startIndex=0;
			for( mat_uint32_t jcIndex=1; jcIndex<sparse->njc; ++jcIndex )
			{
				const auto currentIndex = sparse->jc[jcIndex];
				std::pair<size_t,size_t> value = make_pair( currentIndex-startIndex,startIndex );
				columnCounts[jcIndex-1] = value;
				startIndex = value.first+value.second;
			}
		}
		else
		{
			for( uint columnIndex=0; columnIndex<cols(); ++columnIndex )
				columnCounts[columnIndex] = make_pair( rows(), rows()*columnIndex );
		}
		return columnCounts;
	}

	std::vector<string> WToColumnNames( matvar_t& variable )
	{
		const size_t rowCount = variable.dims[0];
		const size_t columnCount = variable.dims[1];
		std::vector<string> columnNames( rowCount );
		const char* pData2 = static_cast<char*>( variable.data );
		//const uint16_t* pData = static_cast<const uint16_t*>( variable.data );
		for( uint columnIndex=0, dataIndex=0; columnIndex<columnCount; ++columnIndex )
		{
		//	DBG( "columnIndex={}", columnIndex );
			for( uint rowIndex=0; rowIndex<rowCount; ++rowIndex, dataIndex+=2 )
			{
				auto& value = columnNames[rowIndex];
				var ch = pData2[dataIndex];
				value += static_cast<char>( ch );
			//	if( columnIndex<3 )
			//		DBG( "{}={}, {}", rowIndex, value, dataIndex );
			}
		}
		for( auto& columnName : columnNames )
			StringUtilities::RTrim( columnName );

		return columnNames;
	}
	std::vector<string> MatLabVariable::ToColumnNames()
	{
		if( _pVariable->data_type==MAT_T_UINT16 )
			return WToColumnNames( *_pVariable );
		const size_t rowCount = _pVariable->dims[0];
		const size_t columnCount = _pVariable->dims[1];
		std::vector<string> columnNames( rowCount );
		const char* pData = static_cast<const char*>( _pVariable->data );
		for( uint columnIndex=0, dataIndex=0; columnIndex<columnCount; ++columnIndex )
		{
			for( uint rowIndex=0; rowIndex<rowCount; ++rowIndex )
			{
				auto& value = columnNames[rowIndex];
				value += pData[dataIndex++];
			}
		}
		for( auto& columnName : columnNames )
			StringUtilities::RTrim( columnName );

		return columnNames;
	}
}


/*!
 * @file
 * Contains unit tests for the @ref Foldable typeclass.
 */

#include <boost/mpl11/foldable.hpp>

#include <boost/mpl11/core.hpp>
#include <boost/mpl11/detail/test/foldable.hpp>

struct Minimal;
template <typename ...xs>
struct minimal_ { using mpl_datatype = Minimal; };

template <typename ...xs>
struct minimal { using type = minimal_<xs...>; };

namespace boost { namespace mpl11 {
    template <>
    struct Foldable<Minimal> : instantiate<Foldable>::with<Minimal> {
        template <typename f, typename z, typename t>
        struct foldr_impl : z { };

        template <typename f, typename z, typename x, typename ...xs>
        struct foldr_impl<f, z, minimal_<x, xs...>>
            : f::type::template apply<
                x, foldr_impl<f, z, minimal_<xs...>>
            >
        { };

        template <typename f, typename z, typename t>
        struct foldl_impl : z { };

        template <typename f, typename z, typename x, typename ...xs>
        struct foldl_impl<f, z, minimal_<x, xs...>>
            : foldl_impl<
                f, typename f::type::template apply<z, x>, minimal_<xs...>
            >
        { };
    };

#if 0
    template <>
    struct Foldable<FoldMap> : instantiate<Foldable>::with<FoldMap> {
        template <typename MonoidType, typename f, typename t>
        struct fold_map_impl : zero<MonoidType> { };

        template <typename MonoidType, typename f,
            template <typename ...> class t, typename x, typename ...xs>
        struct fold_map_impl<MonoidType, f, t<x, xs...>>
            : plus<
                typename f::type::template apply<x>,
                fold_map_impl<MonoidType, f, t<xs...>>
            >
        { };
    };
#endif
}} // end namespace boost::mpl11

struct tests
    : boost::mpl11::detail::test_Foldable<minimal>
{ };

int main() { }


#pragma once

#ifdef USE_FIBONACCI_HEAP
#include <boost/heap/fibonacci_heap.hpp>
#endif

#include <boost/heap/d_ary_heap.hpp>
#include <unordered_map>
#include <unordered_set>

#include "neighbor.hpp"
#include "planresult.hpp"

namespace libMultiRobotPlanning {

/*!
  \example a_star.cpp Simple example using a 2D grid world and
  up/down/left/right
  actions
*/

/*! \brief A* Algorithm to find the shortest path

This class implements the A* algorithm. A* is an informed search algorithm
that finds the shortest path for a given map. It can use a heuristic that
needsto be admissible.

This class can either use a fibonacci heap, or a d-ary heap. The latter is the
default. Define "USE_FIBONACCI_HEAP" to use the fibonacci heap instead.

\tparam State Custom state for the search. Needs to be copy'able
\tparam Action Custom action for the search. Needs to be copy'able
\tparam Cost Custom Cost type (integer or floating point types)
\tparam Environment This class needs to provide the custom A* logic. In
    particular, it needs to support the following functions:
  - `Cost admissibleHeuristic(const State& s)`\n
    This function can return 0 if no suitable heuristic is available.

  - `bool isSolution(const State& s)`\n
    Return true if the given state is a goal state.

  - `void getNeighbors(const State& s, std::vector<Neighbor<State, Action,
   int> >& neighbors)`\n
    Fill the list of neighboring state for the given state s.

  - `void onExpandNode(const State& s, int fScore, int gScore)`\n
    This function is called on every expansion and can be used for statistical
purposes.

  - `void onDiscover(const State& s, int fScore, int gScore)`\n
    This function is called on every node discovery and can be used for
   statistical purposes.

    \tparam StateHasher A class to convert a state to a hash value. Default:
   std::hash<State>
*/
template <typename State, typename Action, typename Cost, typename Environment,
          typename StateHasher = std::hash<State> >
class AStar {
 public:
  AStar(Environment& environment) : m_env(environment) {}

  bool search(const State& startState,
              PlanResult<State, Action, Cost>& solution, Cost initialCost = 0) {
    solution.states.clear();
    solution.states.push_back(std::make_pair<>(startState, 0));
    solution.actions.clear();
    solution.cost = 0;

    openSet_t openSet;
    std::unordered_map<State, fibHeapHandle_t, StateHasher> stateToHeap;
    std::unordered_set<State, StateHasher> closedSet;
    std::unordered_map<State, std::tuple<State, Action, Cost, Cost>,
                       StateHasher>
        cameFrom;

    auto handle = openSet.push(
        Node(startState, m_env.admissibleHeuristic(startState), initialCost));
    stateToHeap.insert(std::make_pair<>(startState, handle));
    (*handle).handle = handle;

    std::vector<Neighbor<State, Action, Cost> > neighbors;
    neighbors.reserve(10);

    while (!openSet.empty()) {
      Node current = openSet.top();
      m_env.onExpandNode(current.state, current.fScore, current.gScore);

      if (m_env.isSolution(current.state)) {
        solution.states.clear();
        solution.actions.clear();
        auto iter = cameFrom.find(current.state);
        while (iter != cameFrom.end()) {
          solution.states.push_back(
              std::make_pair<>(iter->first, std::get<3>(iter->second)));
          solution.actions.push_back(std::make_pair<>(
              std::get<1>(iter->second), std::get<2>(iter->second)));
          iter = cameFrom.find(std::get<0>(iter->second));
        }
        solution.states.push_back(std::make_pair<>(startState, initialCost));
        std::reverse(solution.states.begin(), solution.states.end());
        std::reverse(solution.actions.begin(), solution.actions.end());
        // solution.cost = current.gScore;
        solution.cost = current.state.time;
        solution.fmin = current.fScore;

        return true;
      }

      openSet.pop();
      stateToHeap.erase(current.state);
      closedSet.insert(current.state);

      // traverse neighbors
      neighbors.clear();
      m_env.getNeighbors(current.state, neighbors);
      for (const Neighbor<State, Action, Cost>& neighbor : neighbors) {
        if (closedSet.find(neighbor.state) == closedSet.end()) {
          Cost tentative_gScore = current.gScore + neighbor.cost;
          // bool isSameDirection = false;
          if(cameFrom.count(current.state) != 0) {
            const auto& s_tuple = cameFrom.at(current.state);
            const auto& parent_state = std::get<0>(s_tuple);
            if(std::abs(parent_state.x - neighbor.state.x) == 2 || std::abs(parent_state.y - neighbor.state.y) == 2 ) {
              tentative_gScore -= 0.2; // if same direction, then g -1
              // isSameDirection = true;
            }
          }
          auto iter = stateToHeap.find(neighbor.state);
          if (iter == stateToHeap.end()) {  // Discover a new node
            Cost fScore =
                tentative_gScore + m_env.admissibleHeuristic(neighbor.state);
            // if (isSameDirection){
            //     ++fScore;
            // }
            auto handle =
                openSet.push(Node(neighbor.state, fScore, tentative_gScore));
            (*handle).handle = handle;
            stateToHeap.insert(std::make_pair<>(neighbor.state, handle));
            m_env.onDiscover(neighbor.state, fScore, tentative_gScore);
            // std::cout << "  this is a new node " << fScore << "," <<
            // tentative_gScore << std::endl;
          } else {
            auto handle = iter->second;
            // std::cout << "  this is an old node: " << tentative_gScore << ","
            // << (*handle).gScore << std::endl;
            // We found this node before with a better path
            if (tentative_gScore >= (*handle).gScore) {
              continue;
            }

            // update f and gScore
            Cost delta = (*handle).gScore - tentative_gScore;
            (*handle).gScore = tentative_gScore;
            (*handle).fScore -= delta;
            // if(isSameDirection) ++(*handle).fScore;
            openSet.increase(handle);
            m_env.onDiscover(neighbor.state, (*handle).fScore,
                             (*handle).gScore);
          }

          // Best path for this node so far
          // TODO: this is not the best way to update "cameFrom", but otherwise
          // default c'tors of State and Action are required
          cameFrom.erase(neighbor.state);
          cameFrom.insert(std::make_pair<>(
              neighbor.state,
              std::make_tuple<>(current.state, neighbor.action, neighbor.cost,
                                tentative_gScore)));
        }
      }
    }

    return false;
  }

 private:
  struct Node {
    Node(const State& state, Cost fScore, Cost gScore)
        : state(state), fScore(fScore), gScore(gScore) {}

    bool operator<(const Node& other) const {
      // Sort order
      // 1. lowest fScore
      // 2. highest gScore

      // Our heap is a maximum heap, so we invert the comperator function here
      if (fScore != other.fScore) {
        return fScore > other.fScore;
      } else {
        return gScore < other.gScore;
      }
    }

    friend std::ostream& operator<<(std::ostream& os, const Node& node) {
      os << "state: " << node.state << " fScore: " << node.fScore
         << " gScore: " << node.gScore;
      return os;
    }

    State state;

    Cost fScore;
    Cost gScore;

#ifdef USE_FIBONACCI_HEAP
    typename boost::heap::fibonacci_heap<Node>::handle_type handle;
#else
    typename boost::heap::d_ary_heap<Node, boost::heap::arity<2>,
                                     boost::heap::mutable_<true> >::handle_type
        handle;
#endif
  };

#ifdef USE_FIBONACCI_HEAP
  typedef typename boost::heap::fibonacci_heap<Node> openSet_t;
  typedef typename openSet_t::handle_type fibHeapHandle_t;
#else
  typedef typename boost::heap::d_ary_heap<Node, boost::heap::arity<2>,
                                           boost::heap::mutable_<true> >
      openSet_t;
  typedef typename openSet_t::handle_type fibHeapHandle_t;
#endif

 private:
  Environment& m_env;
};

}  // namespace libMultiRobotPlanning


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_TOINTS_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_TOINTS_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/config.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/constant/valmin.hpp>
#include <boost/simd/function/simd/bitwise_cast.hpp>
#include <boost/simd/function/simd/if_else.hpp>
#include <boost/simd/function/simd/is_greater_equal.hpp>
#include <boost/simd/function/simd/is_less_equal.hpp>
#include <boost/simd/function/simd/saturate.hpp>
#include <boost/simd/function/simd/splat.hpp>
#include <boost/simd/function/simd/toint.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/dispatch/meta/scalar_of.hpp>

#ifndef BOOST_SIMD_NO_NANS
#include <boost/simd/function/simd/if_zero_else.hpp>
#include <boost/simd/function/simd/is_nan.hpp>
#endif

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD(toints_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::uint_<A0>, X>
                          )
   {
     using result = bd::as_integer_t<A0, signed>;
     BOOST_FORCEINLINE result operator()(A0 const& a0) const
      {
        return bitwise_cast<result>(saturate<result>(a0));
      }
   };

   BOOST_DISPATCH_OVERLOAD(toints_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::int_<A0>, X>
                          )
   {
     BOOST_FORCEINLINE A0 operator()(A0 const& a0) const
      {
        return a0;
      }
   };

   BOOST_DISPATCH_OVERLOAD(toints_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          )
   {
      using result = bd::as_integer_t<A0>;
      BOOST_FORCEINLINE result operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        using sr_t = bd::scalar_of_t<result>;
        const A0 Vax = splat<A0>(bs::Valmax<sr_t>());
        const A0 Vix = splat<A0>(bs::Valmin<sr_t>());
      #ifndef BOOST_SIMD_NO_NANS
        A0 aa0 = if_zero_else(is_nan(a0), a0);
        return if_else(bs::is_less_equal(aa0, Vix), Valmin<result>(),
                       if_else(bs::is_greater_equal(aa0, Vax), Valmax<result>(),
                               toint(aa0)
                              )
                      );
      #else
        return if_else(bs::is_less_equal(a0, Vix), Valmin<result>(),
                       if_else(bs::is_greater_equal(a0, Vax), Valmax<result>(),
                               toint(a0)
                              )
                      );
      #endif
      }
   };

} } }

#endif


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2018 Zhongshi Jiang <jiangzs@nyu.edu>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.

#include "scaf.h"

#include <Eigen/Dense>
#include <Eigen/IterativeLinearSolvers>
#include <Eigen/Sparse>
#include <Eigen/SparseCholesky>
#include <Eigen/SparseQR>
#include <igl/PI.h>
#include <igl/Timer.h>
#include <igl/boundary_loop.h>
#include <igl/cat.h>
#include <igl/doublearea.h>
#include <igl/flip_avoiding_line_search.h>
#include <igl/flipped_triangles.h>
#include <igl/grad.h>
#include <igl/harmonic.h>
#include <igl/local_basis.h>
#include <igl/map_vertices_to_circle.h>
#include <igl/polar_svd.h>
#include <igl/readOBJ.h>
#include <igl/slice.h>
#include <igl/slice_into.h>
#include <igl/slim.h>
#include <igl/triangle/triangulate.h>
#include "mapping_energy_with_jacobians.h"

#include <iostream>
#include <map>
#include <algorithm>
#include <set>
#include <vector>
namespace igl
{

namespace scaf
{
void update_scaffold(igl::SCAFData &s)
{
  s.mv_num = s.m_V.rows();
  s.mf_num = s.m_T.rows();

  s.v_num = s.w_uv.rows();
  s.sf_num = s.s_T.rows();

  s.sv_num = s.v_num - s.mv_num;
  s.f_num = s.sf_num + s.mf_num;

  s.s_M = Eigen::VectorXd::Constant(s.sf_num, s.scaffold_factor);
}

void adjusted_grad(Eigen::MatrixXd &V,
                   Eigen::MatrixXi &F,
                   double area_threshold,
                   Eigen::SparseMatrix<double> &Dx,
                   Eigen::SparseMatrix<double> &Dy,
                   Eigen::SparseMatrix<double> &Dz)
{
  Eigen::VectorXd M;
  igl::doublearea(V, F, M);
  std::vector<int> degen;
  for (int i = 0; i < M.size(); i++)
    if (M(i) < area_threshold)
      degen.push_back(i);

  Eigen::SparseMatrix<double> G;
  igl::grad(V, F, G);

  Dx = G.topRows(F.rows());
  Dy = G.block(F.rows(), 0, F.rows(), V.rows());
  Dz = G.bottomRows(F.rows());

  // handcraft uniform gradient for faces area falling below threshold.
  double sin60 = std::sin(M_PI / 3);
  double cos60 = std::cos(M_PI / 3);
  double deno = std::sqrt(sin60 * area_threshold);
  Eigen::MatrixXd standard_grad(3, 3);
  standard_grad << -sin60 / deno, sin60 / deno, 0,
      -cos60 / deno, -cos60 / deno, 1 / deno,
      0, 0, 0;

  for (auto k : degen)
    for (int j = 0; j < 3; j++)
    {
      Dx.coeffRef(k, F(k, j)) = standard_grad(0, j);
      Dy.coeffRef(k, F(k, j)) = standard_grad(1, j);
      Dz.coeffRef(k, F(k, j)) = standard_grad(2, j);
    }
}

void compute_scaffold_gradient_matrix(SCAFData &s,
                                      Eigen::SparseMatrix<double> &D1,
                                      Eigen::SparseMatrix<double> &D2)
{
  using namespace Eigen;
  Eigen::SparseMatrix<double> G;
  MatrixXi F_s = s.s_T;
  int vn = s.v_num;
  MatrixXd V = MatrixXd::Zero(vn, 3);
  V.leftCols(2) = s.w_uv;

  double min_bnd_edge_len = INFINITY;
  int acc_bnd = 0;
  for (int i = 0; i < s.bnd_sizes.size(); i++)
  {
    int current_size = s.bnd_sizes[i];

    for (int e = acc_bnd; e < acc_bnd + current_size - 1; e++)
    {
      min_bnd_edge_len = (std::min)(min_bnd_edge_len,
                                    (s.w_uv.row(s.internal_bnd(e)) -
                                     s.w_uv.row(s.internal_bnd(e + 1)))
                                        .squaredNorm());
    }
    min_bnd_edge_len = (std::min)(min_bnd_edge_len,
                                  (s.w_uv.row(s.internal_bnd(acc_bnd)) -
                                   s.w_uv.row(s.internal_bnd(acc_bnd + current_size - 1)))
                                      .squaredNorm());
    acc_bnd += current_size;
  }

  double area_threshold = min_bnd_edge_len / 4.0;
  Eigen::SparseMatrix<double> Dx, Dy, Dz;
  adjusted_grad(V, F_s, area_threshold, Dx, Dy, Dz);

  MatrixXd F1, F2, F3;
  igl::local_basis(V, F_s, F1, F2, F3);
  D1 = F1.col(0).asDiagonal() * Dx + F1.col(1).asDiagonal() * Dy +
       F1.col(2).asDiagonal() * Dz;
  D2 = F2.col(0).asDiagonal() * Dx + F2.col(1).asDiagonal() * Dy +
       F2.col(2).asDiagonal() * Dz;
}

void mesh_improve(igl::SCAFData &s)
{
  using namespace Eigen;
  MatrixXd m_uv = s.w_uv.topRows(s.mv_num);
  MatrixXd V_bnd;
  V_bnd.resize(s.internal_bnd.size(), 2);
  for (int i = 0; i < s.internal_bnd.size(); i++) // redoing step 1.
  {
    V_bnd.row(i) = m_uv.row(s.internal_bnd(i));
  }

  if (s.rect_frame_V.size() == 0)
  {
    Matrix2d ob; // = rect_corners;
    {
      VectorXd uv_max = m_uv.colwise().maxCoeff();
      VectorXd uv_min = m_uv.colwise().minCoeff();
      VectorXd uv_mid = (uv_max + uv_min) / 2.;

      Eigen::Array2d scaf_range(3, 3);
      ob.row(0) = uv_mid.array() + scaf_range * ((uv_min - uv_mid).array());
      ob.row(1) = uv_mid.array() + scaf_range * ((uv_max - uv_mid).array());
    }
    Vector2d rect_len;
    rect_len << ob(1, 0) - ob(0, 0), ob(1, 1) - ob(0, 1);
    int frame_points = 5;

    s.rect_frame_V.resize(4 * frame_points, 2);
    for (int i = 0; i < frame_points; i++)
    {
      // 0,0;0,1
      s.rect_frame_V.row(i) << ob(0, 0), ob(0, 1) + i * rect_len(1) / frame_points;
      // 0,0;1,1
      s.rect_frame_V.row(i + frame_points)
          << ob(0, 0) + i * rect_len(0) / frame_points,
          ob(1, 1);
      // 1,0;1,1
      s.rect_frame_V.row(i + 2 * frame_points) << ob(1, 0), ob(1, 1) - i * rect_len(1) / frame_points;
      // 1,0;0,1
      s.rect_frame_V.row(i + 3 * frame_points)
          << ob(1, 0) - i * rect_len(0) / frame_points,
          ob(0, 1);
      // 0,0;0,1
    }
    s.frame_ids = Eigen::VectorXi::LinSpaced(s.rect_frame_V.rows(), s.mv_num, s.mv_num + s.rect_frame_V.rows());
  }

  // Concatenate Vert and Edge
  MatrixXd V;
  MatrixXi E;
  igl::cat(1, V_bnd, s.rect_frame_V, V);
  E.resize(V.rows(), 2);
  for (int i = 0; i < E.rows(); i++)
    E.row(i) << i, i + 1;
  int acc_bs = 0;
  for (auto bs : s.bnd_sizes)
  {
    E(acc_bs + bs - 1, 1) = acc_bs;
    acc_bs += bs;
  }
  E(V.rows() - 1, 1) = acc_bs;
  assert(acc_bs == s.internal_bnd.size());

  MatrixXd H = MatrixXd::Zero(s.component_sizes.size(), 2);
  {
    int hole_f = 0;
    int hole_i = 0;
    for (auto cs : s.component_sizes)
    {
      for (int i = 0; i < 3; i++)
        H.row(hole_i) += m_uv.row(s.m_T(hole_f, i)); // redoing step 2
      hole_f += cs;
      hole_i++;
    }
  }
  H /= 3.;

  MatrixXd uv2;
  igl::triangle::triangulate(V, E, H, std::basic_string<char>("qYYQ"), uv2, s.s_T);
  auto bnd_n = s.internal_bnd.size();

  for (auto i = 0; i < s.s_T.rows(); i++)
    for (auto j = 0; j < s.s_T.cols(); j++)
    {
      auto &x = s.s_T(i, j);
      if (x < bnd_n)
        x = s.internal_bnd(x);
      else
        x += m_uv.rows() - bnd_n;
    }

  igl::cat(1, s.m_T, s.s_T, s.w_T);
  s.w_uv.conservativeResize(m_uv.rows() - bnd_n + uv2.rows(), 2);
  s.w_uv.bottomRows(uv2.rows() - bnd_n) = uv2.bottomRows(-bnd_n + uv2.rows());

  update_scaffold(s);

  // after_mesh_improve
  compute_scaffold_gradient_matrix(s, s.Dx_s, s.Dy_s);

  s.Dx_s.makeCompressed();
  s.Dy_s.makeCompressed();
  s.Dz_s.makeCompressed();
  s.Ri_s = MatrixXd::Zero(s.Dx_s.rows(), s.dim * s.dim);
  s.Ji_s.resize(s.Dx_s.rows(), s.dim * s.dim);
  s.W_s.resize(s.Dx_s.rows(), s.dim * s.dim);
}

void add_new_patch(igl::SCAFData &s, const Eigen::MatrixXd &V_ref,
                   const Eigen::MatrixXi &F_ref,
                   const Eigen::RowVectorXd &center,
                   const Eigen::MatrixXd &uv_init)
{
  using namespace std;
  using namespace Eigen;

  assert(uv_init.rows() != 0);
  Eigen::VectorXd M;
  igl::doublearea(V_ref, F_ref, M);
  s.mesh_measure += M.sum() / 2;

  Eigen::VectorXi bnd;
  Eigen::MatrixXd bnd_uv;

  std::vector<std::vector<int>> all_bnds;
  igl::boundary_loop(F_ref, all_bnds);
  int num_holes = all_bnds.size() - 1;

  s.component_sizes.push_back(F_ref.rows());

  MatrixXd m_uv = s.w_uv.topRows(s.mv_num);
  igl::cat(1, m_uv, uv_init, s.w_uv);

  s.m_M.conservativeResize(s.mf_num + M.size());
  s.m_M.bottomRows(M.size()) = M / 2;

  for (auto cur_bnd : all_bnds)
  {
    s.internal_bnd.conservativeResize(s.internal_bnd.size() + cur_bnd.size());
    s.internal_bnd.bottomRows(cur_bnd.size()) = Map<ArrayXi>(cur_bnd.data(), cur_bnd.size()) + s.mv_num;
    s.bnd_sizes.push_back(cur_bnd.size());
  }

  s.m_T.conservativeResize(s.mf_num + F_ref.rows(), 3);
  s.m_T.bottomRows(F_ref.rows()) = F_ref.array() + s.mv_num;
  s.mf_num += F_ref.rows();

  s.m_V.conservativeResize(s.mv_num + V_ref.rows(), 3);
  s.m_V.bottomRows(V_ref.rows()) = V_ref;
  s.mv_num += V_ref.rows();

  s.rect_frame_V = MatrixXd();

  mesh_improve(s);
}

void compute_jacobians(SCAFData &s, const Eigen::MatrixXd &V_new, bool whole)
{
  auto comp_J2 = [](const Eigen::MatrixXd &uv,
                    const Eigen::SparseMatrix<double> &Dx,
                    const Eigen::SparseMatrix<double> &Dy,
                    Eigen::MatrixXd &Ji) {
    // Ji=[D1*u,D2*u,D1*v,D2*v];
    Ji.resize(Dx.rows(), 4);
    Ji.col(0) = Dx * uv.col(0);
    Ji.col(1) = Dy * uv.col(0);
    Ji.col(2) = Dx * uv.col(1);
    Ji.col(3) = Dy * uv.col(1);
  };

  Eigen::MatrixXd m_V_new = V_new.topRows(s.mv_num);
  comp_J2(m_V_new, s.Dx_m, s.Dy_m, s.Ji_m);
  if (whole)
    comp_J2(V_new, s.Dx_s, s.Dy_s, s.Ji_s);
}

double compute_energy_from_jacobians(const Eigen::MatrixXd &Ji,
                                     const Eigen::VectorXd &areas,
                                     igl::MappingEnergyType energy_type)
{
  double energy = 0;
  if (energy_type == igl::MappingEnergyType::SYMMETRIC_DIRICHLET)
    energy = -4; // comply with paper description
  return energy + igl::mapping_energy_with_jacobians(Ji, areas, energy_type, 0);
}

double compute_soft_constraint_energy(const SCAFData &s)
{
  double e = 0;
  for (auto const &x : s.soft_cons)
    e += s.soft_const_p * (x.second - s.w_uv.row(x.first)).squaredNorm();

  return e;
}

double compute_energy(SCAFData &s, Eigen::MatrixXd &w_uv, bool whole)
{
  if (w_uv.rows() != s.v_num)
    assert(!whole);
  compute_jacobians(s, w_uv, whole);
  double energy = compute_energy_from_jacobians(s.Ji_m, s.m_M, s.slim_energy);

  if (whole)
    energy += compute_energy_from_jacobians(s.Ji_s, s.s_M, s.scaf_energy);
  energy += compute_soft_constraint_energy(s);
  return energy;
}

void buildAm(const Eigen::VectorXd &sqrt_M,
             const Eigen::SparseMatrix<double> &Dx,
             const Eigen::SparseMatrix<double> &Dy,
             const Eigen::MatrixXd &W,
             Eigen::SparseMatrix<double> &Am)
{
  std::vector<Eigen::Triplet<double>> IJV;
  Eigen::SparseMatrix<double> Dz;

  Eigen::SparseMatrix<double> MDx = sqrt_M.asDiagonal() * Dx;
  Eigen::SparseMatrix<double> MDy = sqrt_M.asDiagonal() * Dy;
  igl::slim_buildA(MDx, MDy, Dz, W, IJV);

  Am.setFromTriplets(IJV.begin(), IJV.end());
  Am.makeCompressed();
}

void buildRhs(const Eigen::VectorXd &sqrt_M,
              const Eigen::MatrixXd &W,
              const Eigen::MatrixXd &Ri,
              Eigen::VectorXd &f_rhs)
{
  const int dim = (W.cols() == 4) ? 2 : 3;
  const int f_n = W.rows();
  f_rhs.resize(dim * dim * f_n);

  for (int i = 0; i < f_n; i++)
  {
    auto sqrt_area = sqrt_M(i);
    f_rhs(i + 0 * f_n) = sqrt_area * (W(i, 0) * Ri(i, 0) + W(i, 1) * Ri(i, 1));
    f_rhs(i + 1 * f_n) = sqrt_area * (W(i, 0) * Ri(i, 2) + W(i, 1) * Ri(i, 3));
    f_rhs(i + 2 * f_n) = sqrt_area * (W(i, 2) * Ri(i, 0) + W(i, 3) * Ri(i, 1));
    f_rhs(i + 3 * f_n) = sqrt_area * (W(i, 2) * Ri(i, 2) + W(i, 3) * Ri(i, 3));
  }
}

void get_complement(const Eigen::VectorXi &bnd_ids, int v_n, Eigen::ArrayXi &unknown_ids)
{ // get the complement of bnd_ids.
  int assign = 0, i = 0;
  for (int get = 0; i < v_n && get < bnd_ids.size(); i++)
  {
    if (bnd_ids(get) == i)
      get++;
    else
      unknown_ids(assign++) = i;
  }
  while (i < v_n)
    unknown_ids(assign++) = i++;
  assert(assign + bnd_ids.size() == v_n);
}

void build_surface_linear_system(const SCAFData &s, Eigen::SparseMatrix<double> &L, Eigen::VectorXd &rhs)
{
  using namespace Eigen;
  using namespace std;

  const int v_n = s.v_num - (s.frame_ids.size());
  const int dim = s.dim;
  const int f_n = s.mf_num;

  // to get the  complete A
  Eigen::VectorXd sqrtM = s.m_M.array().sqrt();
  Eigen::SparseMatrix<double> A(dim * dim * f_n, dim * v_n);
  auto decoy_Dx_m = s.Dx_m;
  decoy_Dx_m.conservativeResize(s.W_m.rows(), v_n);
  auto decoy_Dy_m = s.Dy_m;
  decoy_Dy_m.conservativeResize(s.W_m.rows(), v_n);
  buildAm(sqrtM, decoy_Dx_m, decoy_Dy_m, s.W_m, A);

  const VectorXi &bnd_ids = s.fixed_ids;
  auto bnd_n = bnd_ids.size();
  if (bnd_n == 0)
  {

    Eigen::SparseMatrix<double> At = A.transpose();
    At.makeCompressed();

    Eigen::SparseMatrix<double> id_m(At.rows(), At.rows());
    id_m.setIdentity();

    L = At * A;

    Eigen::VectorXd frhs;
    buildRhs(sqrtM, s.W_m, s.Ri_m, frhs);
    rhs = At * frhs;
  }
  else
  {
    MatrixXd bnd_pos;
    igl::slice(s.w_uv, bnd_ids, 1, bnd_pos);
    ArrayXi known_ids(bnd_ids.size() * dim);
    ArrayXi unknown_ids((v_n - bnd_ids.rows()) * dim);
    get_complement(bnd_ids, v_n, unknown_ids);
    VectorXd known_pos(bnd_ids.size() * dim);
    for (int d = 0; d < dim; d++)
    {
      auto n_b = bnd_ids.rows();
      known_ids.segment(d * n_b, n_b) = bnd_ids.array() + d * v_n;
      known_pos.segment(d * n_b, n_b) = bnd_pos.col(d);
      unknown_ids.block(d * (v_n - n_b), 0, v_n - n_b, unknown_ids.cols()) =
          unknown_ids.topRows(v_n - n_b) + d * v_n;
    }

    Eigen::SparseMatrix<double> Au, Ae;
    igl::slice(A, unknown_ids, 2, Au);
    igl::slice(A, known_ids, 2, Ae);

    Eigen::SparseMatrix<double> Aut = Au.transpose();
    Aut.makeCompressed();

    L = Aut * Au;

    Eigen::VectorXd frhs;
    buildRhs(sqrtM, s.W_m, s.Ri_m, frhs);

    rhs = Aut * (frhs - Ae * known_pos);
  }

  // add soft constraints.
  for (auto const &x : s.soft_cons)
  {
    int v_idx = x.first;

    for (int d = 0; d < dim; d++)
    {
      rhs(d * (v_n) + v_idx) += s.soft_const_p * x.second(d); // rhs
      L.coeffRef(d * v_n + v_idx,
                 d * v_n + v_idx) += s.soft_const_p; // diagonal
    }
  }
}

void build_scaffold_linear_system(const SCAFData &s, Eigen::SparseMatrix<double> &L, Eigen::VectorXd &rhs)
{
  using namespace Eigen;

  const int f_n = s.W_s.rows();
  const int v_n = s.Dx_s.cols();
  const int dim = s.dim;

  Eigen::VectorXd sqrtM = s.s_M.array().sqrt();
  Eigen::SparseMatrix<double> A(dim * dim * f_n, dim * v_n);
  buildAm(sqrtM, s.Dx_s, s.Dy_s, s.W_s, A);

  VectorXi bnd_ids;
  igl::cat(1, s.fixed_ids, s.frame_ids, bnd_ids);

  auto bnd_n = bnd_ids.size();
  assert(bnd_n > 0);
  MatrixXd bnd_pos;
  igl::slice(s.w_uv, bnd_ids, 1, bnd_pos);

  ArrayXi known_ids(bnd_ids.size() * dim);
  ArrayXi unknown_ids((v_n - bnd_ids.rows()) * dim);

  get_complement(bnd_ids, v_n, unknown_ids);

  VectorXd known_pos(bnd_ids.size() * dim);
  for (int d = 0; d < dim; d++)
  {
    auto n_b = bnd_ids.rows();
    known_ids.segment(d * n_b, n_b) = bnd_ids.array() + d * v_n;
    known_pos.segment(d * n_b, n_b) = bnd_pos.col(d);
    unknown_ids.block(d * (v_n - n_b), 0, v_n - n_b, unknown_ids.cols()) =
        unknown_ids.topRows(v_n - n_b) + d * v_n;
  }
  Eigen::VectorXd sqrt_M = s.s_M.array().sqrt();

  // manual slicing for A(:, unknown/known)'
  Eigen::SparseMatrix<double> Au, Ae;
  igl::slice(A, unknown_ids, 2, Au);
  igl::slice(A, known_ids, 2, Ae);

  Eigen::SparseMatrix<double> Aut = Au.transpose();
  Aut.makeCompressed();

  L = Aut * Au;

  Eigen::VectorXd frhs;
  buildRhs(sqrtM, s.W_s, s.Ri_s, frhs);

  rhs = Aut * (frhs - Ae * known_pos);
}

void solve_weighted_arap(SCAFData &s, Eigen::MatrixXd &uv)
{
  using namespace Eigen;
  using namespace std;
  int dim = s.dim;
  igl::Timer timer;
  timer.start();

  VectorXi bnd_ids;
  igl::cat(1, s.fixed_ids, s.frame_ids, bnd_ids);
  const auto v_n = s.v_num;
  const auto bnd_n = bnd_ids.size();
  assert(bnd_n > 0);
  MatrixXd bnd_pos;
  igl::slice(s.w_uv, bnd_ids, 1, bnd_pos);

  ArrayXi known_ids(bnd_n * dim);
  ArrayXi unknown_ids((v_n - bnd_n) * dim);

  get_complement(bnd_ids, v_n, unknown_ids);

  VectorXd known_pos(bnd_ids.size() * dim);
  for (int d = 0; d < dim; d++)
  {
    auto n_b = bnd_ids.rows();
    known_ids.segment(d * n_b, n_b) = bnd_ids.array() + d * v_n;
    known_pos.segment(d * n_b, n_b) = bnd_pos.col(d);
    unknown_ids.block(d * (v_n - n_b), 0, v_n - n_b, unknown_ids.cols()) =
        unknown_ids.topRows(v_n - n_b) + d * v_n;
  }

  Eigen::SparseMatrix<double> L;
  Eigen::VectorXd rhs;

  // fixed frame solving:
  // x_e as the fixed frame, x_u for unknowns (mesh + unknown scaffold)
  // min ||(A_u*x_u + A_e*x_e) - b||^2
  // => A_u'*A_u*x_u  = Au'* (b - A_e*x_e) := Au'* b_u
  //
  // separate matrix build:
  // min ||A_m x_m - b_m||^2 + ||A_s x_all - b_s||^2 + soft + proximal
  // First change dimension of A_m to fit for x_all
  // (Not just at the end, since x_all is flattened along dimensions)
  // L = A_m'*A_m + A_s'*A_s + soft + proximal
  // rhs = A_m'* b_m + A_s' * b_s + soft + proximal
  //
  Eigen::SparseMatrix<double> L_m, L_s;
  Eigen::VectorXd rhs_m, rhs_s;
  build_surface_linear_system(s, L_m, rhs_m);  // complete Am, with soft
  build_scaffold_linear_system(s, L_s, rhs_s); // complete As, without proximal

  L = L_m + L_s;
  rhs = rhs_m + rhs_s;
  L.makeCompressed();

  Eigen::VectorXd unknown_Uc((v_n - s.frame_ids.size() - s.fixed_ids.size()) * dim), Uc(dim * v_n);

  SimplicialLDLT<Eigen::SparseMatrix<double>> solver;
  unknown_Uc = solver.compute(L).solve(rhs);
  igl::slice_into(unknown_Uc, unknown_ids.matrix(), 1, Uc);
  igl::slice_into(known_pos, known_ids.matrix(), 1, Uc);

  uv = Map<Matrix<double, -1, -1, Eigen::ColMajor>>(Uc.data(), v_n, dim);
}

double perform_iteration(SCAFData &s)
{
  Eigen::MatrixXd V_out = s.w_uv;
  compute_jacobians(s, V_out, true);
  igl::slim_update_weights_and_closest_rotations_with_jacobians(s.Ji_m, s.slim_energy, 0, s.W_m, s.Ri_m);
  igl::slim_update_weights_and_closest_rotations_with_jacobians(s.Ji_s, s.scaf_energy, 0, s.W_s, s.Ri_s);
  solve_weighted_arap(s, V_out);
  auto whole_E = [&s](Eigen::MatrixXd &uv) { return compute_energy(s, uv, true); };

  Eigen::MatrixXi w_T;
  if (s.m_T.cols() == s.s_T.cols())
    igl::cat(1, s.m_T, s.s_T, w_T);
  else
    w_T = s.s_T;
  return igl::flip_avoiding_line_search(w_T, s.w_uv, V_out,
                                        whole_E, -1) /
         s.mesh_measure;
}
}
}

IGL_INLINE void igl::scaf_precompute(
    const Eigen::MatrixXd &V,
    const Eigen::MatrixXi &F,
    const Eigen::MatrixXd &V_init,
    igl::SCAFData &data,
    igl::MappingEnergyType slim_energy,
    Eigen::VectorXi &b,
    Eigen::MatrixXd &bc,
    double soft_p)
{
  Eigen::MatrixXd CN;
  Eigen::MatrixXi FN;
  igl::scaf::add_new_patch(data, V, F, Eigen::RowVector2d(0, 0), V_init);
  data.soft_const_p = soft_p;
  for (int i = 0; i < b.rows(); i++)
    data.soft_cons[b(i)] = bc.row(i);
  data.slim_energy = slim_energy;

  auto &s = data;

  if (!data.has_pre_calc)
  {
    int v_n = s.mv_num + s.sv_num;
    int f_n = s.mf_num + s.sf_num;
    int dim = s.dim;
    Eigen::MatrixXd F1, F2, F3;
    igl::local_basis(s.m_V, s.m_T, F1, F2, F3);
    auto face_proj = [](Eigen::MatrixXd& F){
      std::vector<Eigen::Triplet<double> >IJV;
      int f_num = F.rows();
      for(int i=0; i<F.rows(); i++) {
        IJV.push_back(Eigen::Triplet<double>(i, i, F(i,0)));
        IJV.push_back(Eigen::Triplet<double>(i, i+f_num, F(i,1)));
        IJV.push_back(Eigen::Triplet<double>(i, i+2*f_num, F(i,2)));
      }
      Eigen::SparseMatrix<double> P(f_num, 3*f_num);
      P.setFromTriplets(IJV.begin(), IJV.end());
      return P;
    };
    Eigen::SparseMatrix<double> G;
    igl::grad(s.m_V, s.m_T, G);
    s.Dx_m = face_proj(F1) * G;
    s.Dy_m = face_proj(F2) * G;

    igl::scaf::compute_scaffold_gradient_matrix(s, s.Dx_s, s.Dy_s);

    s.Dx_m.makeCompressed();
    s.Dy_m.makeCompressed();
    s.Ri_m = Eigen::MatrixXd::Zero(s.Dx_m.rows(), dim * dim);
    s.Ji_m.resize(s.Dx_m.rows(), dim * dim);
    s.W_m.resize(s.Dx_m.rows(), dim * dim);

    s.Dx_s.makeCompressed();
    s.Dy_s.makeCompressed();
    s.Ri_s = Eigen::MatrixXd::Zero(s.Dx_s.rows(), dim * dim);
    s.Ji_s.resize(s.Dx_s.rows(), dim * dim);
    s.W_s.resize(s.Dx_s.rows(), dim * dim);

    data.has_pre_calc = true;
  }
}

IGL_INLINE Eigen::MatrixXd igl::scaf_solve(SCAFData &s, int iter_num)
{
  using namespace std;
  using namespace Eigen;
  s.energy = igl::scaf::compute_energy(s, s.w_uv, false) / s.mesh_measure;

  for (int it = 0; it < iter_num; it++)
  {
    s.total_energy = igl::scaf::compute_energy(s, s.w_uv, true) / s.mesh_measure;
    s.rect_frame_V = Eigen::MatrixXd();
    igl::scaf::mesh_improve(s);

    double new_weight = s.mesh_measure * s.energy / (s.sf_num * 100);
    s.scaffold_factor = new_weight;
    igl::scaf::update_scaffold(s);

    s.total_energy = igl::scaf::perform_iteration(s);

    s.energy =
        igl::scaf::compute_energy(s, s.w_uv, false) / s.mesh_measure;
  }

  return s.w_uv.topRows(s.mv_num);
}

#ifdef IGL_STATIC_LIBRARY
#endif


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

// Copyright (c) 2012-2019 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to 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_GEOMETRY_ALGORITHMS_DETAIL_BUFFER_LINE_LINE_INTERSECTION_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_BUFFER_LINE_LINE_INTERSECTION_HPP

#include <boost/geometry/arithmetic/infinite_line_functions.hpp>
#include <boost/geometry/algorithms/detail/make/make.hpp>
#include <boost/geometry/strategies/buffer.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace buffer
{

// TODO: it might once be changed this to proper strategy
struct line_line_intersection
{
    template <typename Point>
    static inline strategy::buffer::join_selector
    apply(Point const& pi, Point const& pj,
          Point const& qi, Point const& qj,
          Point& ip)
    {
        typedef typename coordinate_type<Point>::type ct;
        typedef model::infinite_line<ct> line_type;

        line_type const p = detail::make::make_infinite_line<ct>(pi, pj);
        line_type const q = detail::make::make_infinite_line<ct>(qi, qj);

        if (arithmetic::intersection_point(p, q, ip))
        {
            return strategy::buffer::join_convex;
        }

        // The lines do not intersect.
        // Distinguish between continuing lines (having a similar direction)
        // and spikes (having the opposite direction).
        return arithmetic::similar_direction(p, q)
            ? strategy::buffer::join_continue
            : strategy::buffer::join_spike
            ;
    }
};

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

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_BUFFER_LINE_LINE_INTERSECTION_HPP


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 3 of the License, or (at your option) any later version.
//
// Alternatively, you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of
// the License, or (at your option) any later version.
//
// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.

#define EIGEN_NO_ASSERTION_CHECKING
#include "main.h"
#include <Eigen/Cholesky>
#include <Eigen/LU>

#ifdef HAS_GSL
#include "gsl_helper.h"
#endif

template<typename MatrixType> void cholesky(const MatrixType& m)
{
  /* this test covers the following files:
     LLT.h LDLT.h
  */
  int rows = m.rows();
  int cols = m.cols();

  typedef typename MatrixType::Scalar Scalar;
  typedef typename NumTraits<Scalar>::Real RealScalar;
  typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, MatrixType::RowsAtCompileTime> SquareMatrixType;
  typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> VectorType;

  MatrixType a0 = MatrixType::Random(rows,cols);
  VectorType vecB = VectorType::Random(rows), vecX(rows);
  MatrixType matB = MatrixType::Random(rows,cols), matX(rows,cols);
  SquareMatrixType symm =  a0 * a0.adjoint();
  // let's make sure the matrix is not singular or near singular
  MatrixType a1 = MatrixType::Random(rows,cols);
  symm += a1 * a1.adjoint();

  #ifdef HAS_GSL
  if (ei_is_same_type<RealScalar,double>::ret)
  {
    typedef GslTraits<Scalar> Gsl;
    typename Gsl::Matrix gMatA=0, gSymm=0;
    typename Gsl::Vector gVecB=0, gVecX=0;
    convert<MatrixType>(symm, gSymm);
    convert<MatrixType>(symm, gMatA);
    convert<VectorType>(vecB, gVecB);
    convert<VectorType>(vecB, gVecX);
    Gsl::cholesky(gMatA);
    Gsl::cholesky_solve(gMatA, gVecB, gVecX);
    VectorType vecX(rows), _vecX, _vecB;
    convert(gVecX, _vecX);
    symm.llt().solve(vecB, &vecX);
    Gsl::prod(gSymm, gVecX, gVecB);
    convert(gVecB, _vecB);
    // test gsl itself !
    VERIFY_IS_APPROX(vecB, _vecB);
    VERIFY_IS_APPROX(vecX, _vecX);

    Gsl::free(gMatA);
    Gsl::free(gSymm);
    Gsl::free(gVecB);
    Gsl::free(gVecX);
  }
  #endif

  {
    LDLT<SquareMatrixType> ldlt(symm);
    VERIFY(ldlt.isPositiveDefinite());
    // in eigen3, LDLT is pivoting
    //VERIFY_IS_APPROX(symm, ldlt.matrixL() * ldlt.vectorD().asDiagonal() * ldlt.matrixL().adjoint());
    ldlt.solve(vecB, &vecX);
    VERIFY_IS_APPROX(symm * vecX, vecB);
    ldlt.solve(matB, &matX);
    VERIFY_IS_APPROX(symm * matX, matB);
  }

  {
    LLT<SquareMatrixType> chol(symm);
    VERIFY(chol.isPositiveDefinite());
    VERIFY_IS_APPROX(symm, chol.matrixL() * chol.matrixL().adjoint());
    chol.solve(vecB, &vecX);
    VERIFY_IS_APPROX(symm * vecX, vecB);
    chol.solve(matB, &matX);
    VERIFY_IS_APPROX(symm * matX, matB);
  }

#if 0 // cholesky is not rank-revealing anyway
  // test isPositiveDefinite on non definite matrix
  if (rows>4)
  {
    SquareMatrixType symm =  a0.block(0,0,rows,cols-4) * a0.block(0,0,rows,cols-4).adjoint();
    LLT<SquareMatrixType> chol(symm);
    VERIFY(!chol.isPositiveDefinite());
    LDLT<SquareMatrixType> cholnosqrt(symm);
    VERIFY(!cholnosqrt.isPositiveDefinite());
  }
#endif
}

void test_eigen2_cholesky()
{
  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_1( cholesky(Matrix<double,1,1>()) );
    CALL_SUBTEST_2( cholesky(Matrix2d()) );
    CALL_SUBTEST_3( cholesky(Matrix3f()) );
    CALL_SUBTEST_4( cholesky(Matrix4d()) );
    CALL_SUBTEST_5( cholesky(MatrixXcd(7,7)) );
    CALL_SUBTEST_6( cholesky(MatrixXf(17,17)) );
    CALL_SUBTEST_7( cholesky(MatrixXd(33,33)) );
  }
}