#include <utility/io/vector.hpp>
#include <utility/random_vector.hpp>

#include <burst/algorithm/counting_sort.hpp>
#include <burst/execution/parallel_policy.hpp>

#include <doctest/doctest.h>

#include <boost/algorithm/cxx11/all_of.hpp>
#include <boost/iterator/indirect_iterator.hpp>
#include <boost/range/adaptor/indirected.hpp>
#include <boost/range/iterator_range.hpp>

#include <forward_list>
#include <iterator>
#include <limits>
#include <memory>
#include <mutex>
#include <thread>
#include <unordered_set>
#include <vector>

TEST_SUITE("counting_sort")
{
    TEST_CASE("Сортировка отсортированного диапазона не изменяет его")
    {
        std::vector<std::uint8_t> values{0, 1, 2, 3, 4};

        SUBCASE("при последовательной сортировке")
        {
            std::vector<std::uint8_t> sorted_values(values.size());
            burst::counting_sort_copy(values.begin(), values.end(), sorted_values.begin());

            CHECK(std::is_sorted(sorted_values.begin(), sorted_values.end()));
        }
        SUBCASE("при параллельной сортировке")
        {
            std::vector<std::uint8_t> sorted_values(values.size());
            burst::counting_sort_copy
            (
                burst::par(4),
                values.begin(),
                values.end(),
                sorted_values.begin()
            );

            CHECK(std::is_sorted(sorted_values.begin(), sorted_values.end()));
        }
    }

    TEST_CASE("Результат сортировки убывающего диапазона — возрастающий диапазон")
    {
        std::vector<std::size_t> values{0x0104, 0x0203, 0x0302, 0x0401};

        std::vector<std::size_t> expected{0x0401, 0x0302, 0x0203, 0x0104};
        SUBCASE("при последовательной сортировке")
        {
            std::vector<std::size_t> sorted_values(values.size());
            burst::counting_sort_copy(values.begin(), values.end(), sorted_values.begin(),
                [] (const std::size_t & integer) -> std::uint8_t
                {
                    return integer & 0xff;
                });

            CHECK(sorted_values == expected);
        }
        SUBCASE("при параллельной сортировке")
        {
            std::vector<std::size_t> sorted_values(values.size());
            burst::counting_sort_copy
            (
                burst::par(2),
                values.begin(),
                values.end(),
                sorted_values.begin(),
                [] (const std::size_t & integer) -> std::uint8_t
                {
                    return integer & 0xff;
                }
            );

            CHECK(sorted_values == expected);
        }
    }

    TEST_CASE("Умеет сортировать побитово")
    {
        std::vector<std::uint8_t> numbers{0, 5, 3, 7, 1, 2, 4, 6};

        // Порядок внутри чётных и нечётных групп сохраняется, потому что сортировка устойчива.
        std::vector<std::uint8_t> even_goes_first{0, 2, 4, 6, 5, 3, 7, 1};

        SUBCASE("при последовательной сортировке")
        {
            std::vector<std::uint8_t> sorted_numbers(numbers.size());
            burst::counting_sort_copy(numbers.begin(), numbers.end(), sorted_numbers.begin(),
                [] (const std::uint8_t & number) -> bool
                {
                    return number & 0x01;
                });

            CHECK(sorted_numbers == even_goes_first);
        }
        SUBCASE("при параллельной сортировке")
        {
            std::vector<std::uint8_t> sorted_numbers(numbers.size());
            burst::counting_sort_copy
            (
                burst::par(4),
                numbers.begin(),
                numbers.end(),
                sorted_numbers.begin(),
                [] (const std::uint8_t & number) -> bool
                {
                    return number & 0x01;
                }
            );

            CHECK(sorted_numbers == even_goes_first);
        }
    }

    TEST_CASE("Сортировка хаоса приводит к порядку")
    {
        std::vector<std::uint8_t> values{0x12, 0xfd, 0x00, 0x15, 0x66};

        std::vector<std::uint8_t> expected{0x00, 0x12, 0x15, 0x66, 0xfd};
        SUBCASE("при последовательной сортировке")
        {
            std::vector<std::uint8_t> sorted_values(values.size());
            burst::counting_sort_copy(values.begin(), values.end(), sorted_values.begin());

            CHECK(sorted_values == expected);
        }
        SUBCASE("при параллельной сортировке")
        {
            std::vector<std::uint8_t> sorted_values(values.size());
            burst::counting_sort_copy
            (
                burst::par(3),
                values.begin(),
                values.end(),
                sorted_values.begin()
            );

            CHECK(sorted_values == expected);
        }
    }

    TEST_CASE("Отрицательные значения предшествуют неотрицательным в результате сортировки")
    {
        std::vector<std::int8_t> values{0, -1, 1, -2, 2};

        std::vector<std::int8_t> expected{-2, -1, 0, 1, 2};
        SUBCASE("при последовательной сортировке")
        {
            std::vector<std::int8_t> sorted_values(values.size());
            burst::counting_sort_copy(values.begin(), values.end(), sorted_values.begin());

            CHECK(sorted_values == expected);
        }
        SUBCASE("при параллельной сортировке")
        {
            std::vector<std::int8_t> sorted_values(values.size());
            burst::counting_sort_copy
            (
                burst::par(2),
                values.begin(),
                values.end(),
                sorted_values.begin()
            );

            CHECK(sorted_values == expected);
        }
    }

    TEST_CASE("Экстремальные значения элементов сортируются корректно")
    {
        std::vector<std::int8_t> values
        {
            0,
            std::numeric_limits<std::int8_t>::min(),
            std::numeric_limits<std::int8_t>::max()
        };

        std::vector<std::int8_t> expected
        {
            std::numeric_limits<std::int8_t>::min(),
            0,
            std::numeric_limits<std::int8_t>::max()
        };
        SUBCASE("при последовательной сортировке")
        {
            std::vector<std::int8_t> sorted_values(values.size());
            burst::counting_sort_copy(values.begin(), values.end(), sorted_values.begin());

            CHECK(sorted_values == expected);
        }
        SUBCASE("при параллельной сортировке")
        {
            std::vector<std::int8_t> sorted_values(values.size());
            burst::counting_sort_copy
            (
                burst::par(2),
                values.begin(),
                values.end(),
                sorted_values.begin()
            );

            CHECK(sorted_values == expected);
        }
    }

    TEST_CASE("Алгоритм сортировки устойчив")
    {
        std::vector<std::uint32_t> values
        {
            0x3301,
            0x3300,
            0x2201,
            0x2200,
            0x1101,
            0x1100
        };

        std::vector<std::uint32_t> expected
        {
            0x3300,
            0x2200,
            0x1100,
            0x3301,
            0x2201,
            0x1101
        };
        SUBCASE("при последовательной сортировке")
        {
            std::vector<std::uint32_t> sorted_values(values.size());
            burst::counting_sort_copy(values.begin(), values.end(), sorted_values.begin(),
                [] (const std::uint32_t & integer) -> std::uint8_t
                {
                    return integer & 0xff;
                });

            CHECK(sorted_values == expected);
        }
        SUBCASE("при параллельной сортировке")
        {
            std::vector<std::uint32_t> sorted_values(values.size());
            burst::counting_sort_copy
            (
                burst::par(3),
                values.begin(),
                values.end(),
                sorted_values.begin(),
                [] (const std::uint32_t & integer) -> std::uint8_t
                {
                    return integer & 0xff;
                }
            );

            CHECK(sorted_values == expected);
        }
    }

    TEST_CASE("Возвращает итератор после последнего отсортированного элемента в результирующем "
        "диапазоне")
    {
        std::vector<std::uint8_t> initial{5, 4, 3, 2, 1};

        SUBCASE("при последовательной сортировке")
        {
            std::vector<std::uint8_t> sorted(initial.size() * 2);
            auto sorted_end =
                burst::counting_sort_copy(initial.begin(), initial.end(), sorted.begin());

            CHECK(sorted_end == sorted.begin() + std::distance(initial.begin(), initial.end()));
        }
        SUBCASE("при параллельной сортировке")
        {
            std::vector<std::uint8_t> sorted(initial.size() * 2);
            auto sorted_end =
                burst::counting_sort_copy
                (
                    burst::par(2),
                    initial.begin(),
                    initial.end(),
                    sorted.begin()
                );

            CHECK(sorted_end == sorted.begin() + std::distance(initial.begin(), initial.end()));
        }
    }

    TEST_CASE("Исходный диапазон не изменяется")
    {
        SUBCASE("при последовательной сортировке")
        {
            std::vector<std::uint8_t> numbers{5, 4, 3, 2, 1};
            const auto numbers_copy = numbers;

            std::vector<std::uint8_t> sorted_numbers(numbers.size());
            burst::counting_sort_copy(numbers.begin(), numbers.end(), sorted_numbers.begin());

            CHECK(numbers == numbers_copy);
        }
        SUBCASE("при параллельной сортировке")
        {
            std::vector<std::uint8_t> numbers{5, 4, 3, 2, 1};
            const auto numbers_copy = numbers;

            std::vector<std::uint8_t> sorted_numbers(numbers.size());
            burst::counting_sort_copy
            (
                burst::par(3),
                numbers.begin(),
                numbers.end(),
                sorted_numbers.begin()
            );

            CHECK(numbers == numbers_copy);
        }
    }

    TEST_CASE("Может сортировать некопируемые элементы")
    {
        std::vector<std::unique_ptr<std::int8_t>> unsorted;
        unsorted.emplace_back(std::make_unique<std::int8_t>(3));
        unsorted.emplace_back(std::make_unique<std::int8_t>(5));
        unsorted.emplace_back(std::make_unique<std::int8_t>(-1));
        unsorted.emplace_back(std::make_unique<std::int8_t>(20));
        unsorted.emplace_back(std::make_unique<std::int8_t>(0));

        auto expected = {-1, 0, 3, 5, 20};
        SUBCASE("при последовательной сортировке")
        {
            std::vector<std::unique_ptr<std::int8_t>> sorted(unsorted.size());
            burst::counting_sort_move(unsorted.begin(), unsorted.end(), sorted.begin(),
                [] (const auto & p)
                {
                    return *p;
                });

            CHECK(boost::algorithm::all_of(unsorted, [] (const auto & p) { return p == nullptr; }));

            CHECK
            (
                boost::make_iterator_range(boost::make_indirect_iterator(std::begin(sorted)), boost::make_indirect_iterator(std::end(sorted))) ==
                expected
            );
        }
        SUBCASE("при параллельной сортировке")
        {
            std::vector<std::unique_ptr<std::int8_t>> sorted(unsorted.size());
            burst::counting_sort_move
            (
                burst::par(4),
                unsorted.begin(),
                unsorted.end(),
                sorted.begin(),
                [] (const auto & p) {return *p;}
            );

            CHECK(boost::algorithm::all_of(unsorted, [] (const auto & p) { return p == nullptr; }));

            CHECK
            (
                boost::make_iterator_range(boost::make_indirect_iterator(std::begin(sorted)), boost::make_indirect_iterator(std::end(sorted))) ==
                expected
            );
        }
    }

    TEST_CASE("Для сортировки достаточно однонаправленного итератора")
    {
        auto unsorted = std::vector<std::int8_t>{4, 3, 2, 1};
        auto forward_list = std::forward_list<std::int8_t>(unsorted.begin(), unsorted.end());

        std::vector<std::int8_t> sorted(unsorted.size(), 0);
        burst::counting_sort_copy(forward_list.begin(), forward_list.end(), sorted.begin());

        auto expected = std::vector<std::int8_t>{1, 2, 3, 4};
        CHECK(sorted == expected);
    }

    TEST_CASE("Копирующий вариант допускает диапазоны на вход")
    {
        std::vector<std::uint8_t> values{5, 4, 3, 2, 1};

        SUBCASE("при последовательной сортировке")
        {
            std::vector<std::uint8_t> sorted_values(values.size());
            burst::counting_sort_copy(values, sorted_values.begin());

            std::vector<std::uint8_t> expected{1, 2, 3, 4, 5};
            CHECK(sorted_values == expected);
        }
        SUBCASE("при параллельной сортировке")
        {
            std::vector<std::uint8_t> sorted_values(values.size());
            burst::counting_sort_copy(burst::par(2), values, sorted_values.begin());

            std::vector<std::uint8_t> expected{1, 2, 3, 4, 5};
            CHECK(sorted_values == expected);
        }
    }

    TEST_CASE("Переносящий вариант допускает диапазоны на вход")
    {
        std::vector<std::unique_ptr<std::int8_t>> unsorted;
        unsorted.emplace_back(std::make_unique<std::int8_t>(17));
        unsorted.emplace_back(std::make_unique<std::int8_t>(5));
        unsorted.emplace_back(std::make_unique<std::int8_t>(27));
        unsorted.emplace_back(std::make_unique<std::int8_t>(-50));
        unsorted.emplace_back(std::make_unique<std::int8_t>(-1));
        unsorted.emplace_back(std::make_unique<std::int8_t>(127));
        unsorted.emplace_back(std::make_unique<std::int8_t>(0));

        auto expected = {-50, -1, 0, 5, 17, 27, 127};
        SUBCASE("при последовательной сортировке")
        {
            std::vector<std::unique_ptr<std::int8_t>> sorted(unsorted.size());
            burst::counting_sort_move(unsorted, sorted.begin(),
                [] (const auto & p)
                {
                    return *p;
                });

            CHECK((sorted | boost::adaptors::indirected) == expected);
        }
        SUBCASE("при параллельной сортировке")
        {
            std::vector<std::unique_ptr<std::int8_t>> sorted(unsorted.size());
            burst::counting_sort_move(burst::par(4), unsorted, sorted.begin(),
                [] (const auto & p) {
                    return *p;
                });

            CHECK((sorted | boost::adaptors::indirected) == expected);
        }
    }

    TEST_CASE("Сортировка подсчётом может быть распараллелена")
    {
        std::vector<std::int8_t> unsorted{100, -42, 99, 120, 0};

        std::vector<std::int8_t> sorted(unsorted.size());
        burst::counting_sort_copy(burst::par(2), unsorted, sorted.begin());

        std::vector<std::int8_t> expected{-42, 0, 99, 100, 120};
        CHECK(sorted == expected);
    }

    TEST_CASE_TEMPLATE("Непосредственно сортировка выполняется в пуле потоков, в вызывающем потоке "
        "обращений к разрядам нет", integer_type, std::int8_t, std::int16_t)
    {
        auto unsorted = utility::random_vector<integer_type>(1000, 0, 100);

        std::unordered_set<std::thread::id> thread_ids;
        std::mutex m;

        std::vector<integer_type> sorted(unsorted.size());
        constexpr auto thread_count = 3ul;
        burst::counting_sort_copy(burst::par(thread_count), unsorted, sorted.begin(),
            [& thread_ids, & m] (auto x)
            {
                std::unique_lock<std::mutex> l(m);
                thread_ids.insert(std::this_thread::get_id());
                return x;
            });

        CHECK(thread_ids.size() <= thread_count);
        CHECK(thread_ids.find(std::this_thread::get_id()) == thread_ids.end());
    }

    TEST_CASE("Если заявлено меньше двух потоков, то алгоритм откатывается на однопоточный вариант")
    {
        std::vector<std::uint8_t> unsorted{0, 5, 1, 6, 2, 111};

        std::unordered_set<std::thread::id> thread_ids;
        std::mutex m;

        std::vector<std::uint8_t> sorted(unsorted.size());
        burst::counting_sort_copy(burst::par(1), unsorted.begin(), unsorted.end(), sorted.begin(),
            [& thread_ids, & m] (auto x)
            {
                std::unique_lock<std::mutex> l(m);
                thread_ids.insert(std::this_thread::get_id());
                return x;
            });

        CHECK(thread_ids.size() == 1);
        CHECK(thread_ids.find(std::this_thread::get_id()) != thread_ids.end());
    }
}


#define BOOST_TEST_MODULE mperf Test
#include <boost/test/included/unit_test.hpp>

#include <cmath>
#include <chrono>
#include <thread>

#include "../parameters.hpp"
#include "../avaliation.hpp"

BOOST_AUTO_TEST_CASE( context_test )
{
  char* argv[] = { "20", "10000" };

  struct mperf::application_context context(2, argv);

  BOOST_TEST( context.avaliations_limit == 20 );
  BOOST_TEST( context.interation_limit == 10000 );
}

BOOST_AUTO_TEST_CASE( histance_test )
{
  int step = 5;
  long iteration_limit = 10000;
  long elements_amount = std::lroundl(std::pow(2, step));

  avaliation_histance histance(step, iteration_limit);

  BOOST_TEST( histance.step() == step );
  BOOST_TEST( histance.iteration_limit() == iteration_limit );
  BOOST_TEST( histance.elements_amount() == elements_amount );
}

BOOST_AUTO_TEST_CASE( result_test )
{
  using namespace std::literals;

  int step = 5;
  long iteration_limit = 10000;
  long elements_amount = std::lroundl(std::pow(2, step));

  std::chrono::milliseconds expected_average = 2000ms;

  avaliation_result result(iteration_limit, elements_amount);

  result.start();
  std::this_thread::sleep_for(expected_average);
  result.stop();

  std::chrono::duration<double, std::milli> expected = expected_average;

  BOOST_TEST( (int) expected.count() == (int) result.average());
}


#include <Data/DATA.h>
#include <Driver/SIMULATION.h>
#include <Equation/EQUATION.h>
#include <Equation/NONLINEAR_EQUATION.h>
#include <Equation/TRUST_REGION.h>
#include <Force/FORCE.h>
#include <Parsing/PARSER_REGISTRY.h>
#include <Utilities/EIGEN_HELPERS.h>
#include <Utilities/LOG.h>
#include <Utilities/MATH.h>
#include <Utilities/RANDOM.h>
#include <Eigen/Eigenvalues>
#include <iomanip>
using namespace Mechanics;
///////////////////////////////////////////////////////////////////////
template<class TV> TRUST_REGION<TV>::
TRUST_REGION()
{
    precision=1e-6;
    contract_factor=.25;
    expand_factor=2.5;
    contract_threshold=.4;
    expand_threshold=.8;
    expand_threshold_rad=.8;
    trust_iterations=20;
    tol=1e-8;
}
///////////////////////////////////////////////////////////////////////
template<class TV> void TRUST_REGION<TV>::
Linearize(SIMULATION<TV>& simulation,const T dt,const T time)
{
    DATA<TV>& data=simulation.data;
    FORCE<TV>& force=simulation.force;

    // zero velocities
    equation->Initialize(data,force);
    current_velocities.resize(equation->Velocity_DOF(),1);current_velocities.setZero();
    equation->Unpack_Velocities(data,current_velocities);

    // zero forces
    force.Pack_Forces(solve_forces);
    solve_forces.setZero();
    force.Unpack_Forces(solve_forces);

    // store positions
    data.Pack_Positions(positions);

    equation->Linearize(data,force,dt,time,true);
    equation->RHS(rhs);
    force.Pack_Forces(solve_forces);
}
///////////////////////////////////////////////////////////////////////
template<class TV> void TRUST_REGION<TV>::
Linearize_Around(SIMULATION<TV>& simulation,const T dt,const T time,const Vector& solve_vector)
{
    DATA<TV>& data=simulation.data;
    FORCE<TV>& force=simulation.force;
    // sk is solve_vector
    int velocity_dof=equation->Velocity_DOF();
    Vector solve_velocities=solve_vector.block(0,0,velocity_dof,1);
    solve_forces.Set(solve_vector.block(velocity_dof,0,solve_vector.rows()-velocity_dof,1));
    data.Unpack_Positions(positions);
    force.Increment_Forces(solve_forces,1);

    candidate_velocities=current_velocities+solve_velocities;
    equation->Unpack_Velocities(data,candidate_velocities);
    data.Step();
    equation->Linearize(data,force,dt,time,false);
    force.Increment_Forces(solve_forces,-1);
}
///////////////////////////////////////////////////////////////////////
template<class TV> void TRUST_REGION<TV>::
Increment_X(SIMULATION<TV>& simulation)
{
    // sk is solve_vector
    int velocity_dof=equation->Velocity_DOF();
    Vector solve_velocities=sk.block(0,0,velocity_dof,1);
    solve_forces.Set(sk.block(velocity_dof,0,sk.rows()-velocity_dof,1));
    simulation.force.Increment_Forces(solve_forces,1);
    simulation.force.Pack_Forces(solve_forces);
    current_velocities+=solve_velocities;
    rhs=try_rhs;

    // store errors
    T one_over_maxabs=1;
    if(rhs.rows()>0){
        T maxabs=rhs.array().abs().maxCoeff();
        if(!maxabs){maxabs=1;}
        one_over_maxabs=1/maxabs;}
    equation->Store_Errors(simulation.data,rhs.block(0,0,velocity_dof,1)*one_over_maxabs);
    solve_forces.Set(rhs.block(velocity_dof,0,rhs.rows()-velocity_dof,1)*one_over_maxabs);
    simulation.force.Store_Errors(solve_forces);
}
///////////////////////////////////////////////////////////////////////
template<class TV> void TRUST_REGION<TV>::
Step(SIMULATION<TV>& simulation,const T dt,const T time)
{
    static int call_count=0;
    static int total_steps=0;
    // loosely based on trustOptim implementation
    status=CONTINUE;

    iteration=0;
    max_iterations=100;
    radius=1;
    min_radius=1e-9;
    preconditioner_refresh_frequency=1;

    Linearize(simulation,dt,time);
    f=equation->Evaluate();
    equation->Gradient(gk);
    norm_gk=gk.norm();
    Update_Hessian(false);
    Update_Preconditioner(false);

    static int failed_radius=0;
    do{
        iteration++;
        LOG::cout<<std::endl<<"BEGINNING STEP "<<iteration<<std::endl;
        status=Update_One_Step(simulation,dt,time);
        LOG::cout<<"norm_gk: "<<norm_gk<<" norm_gk/sqrt(nvars): "<<norm_gk/sqrt(T(nvars))<<std::endl;
        //if(norm_gk/sqrt(T(nvars))<=precision && f<=precision){status=SUCCESS;}
        if(norm_gk/sqrt(T(nvars))<=precision){status=SUCCESS;}
        if(iteration>=max_iterations){status=EMAXITER;}
        if(radius<=min_radius){ // trust region collapse
            status=ETOLG;
            failed_radius++;
        }

        // update Hessian
        if(status==MOVED || status==EXPAND){
            //Update_Hessian(status!=EXPAND);
            Update_Hessian(false);
            //Check_Derivative(simulation,dt,time);
            if(simulation.force.Equations_Changed() || iteration%preconditioner_refresh_frequency==0){Update_Preconditioner(false);}
            status=CONTINUE;}
        if(simulation.substeps){
            std::string frame_name="Frame "+std::to_string(simulation.current_frame)+" substep "+std::to_string(iteration)+" real "+std::to_string(int(status==CONTINUE))+ " f "+std::to_string(f);
            simulation.Write(frame_name);}
        if(status==CONTRACT){status=CONTINUE;}
    }while(status==CONTINUE);
    std::string frame_name="End frame "+std::to_string(simulation.current_frame)+" substep "+std::to_string(iteration)+" real "+std::to_string(int(status==CONTINUE))+ " f "+std::to_string(f);
    simulation.Write(frame_name);
    //Check_Derivative(simulation,dt,time);
    LOG::cout<<"SOLVE STEPS: "<<iteration<<" Failed due to radius: "<<failed_radius<<std::endl;
    call_count++;
    total_steps+=iteration;
    LOG::cout<<"Current average: "<<total_steps/(T)call_count<<std::endl;
}
///////////////////////////////////////////////////////////////////////
template<class TV> void TRUST_REGION<TV>::
Update_Preconditioner(bool identity)
{
    /*Matrix<T,Dynamic,Dynamic> dense(hessian);
    EigenSolver<Matrix<T,Dynamic,Dynamic>> es(dense,false);
    LOG::cout<<es.eigenvalues()<<std::endl;*/
    if(!identity){
        preconditioner.compute(hessian);}
    if(identity || preconditioner.info()!=ComputationInfo::Success){
        LOG::cout<<"Preconditioner computation failed; using identity"<<std::endl;
        SparseMatrix<T> BB(nvars,nvars);
        BB.setIdentity();
        preconditioner.compute(BB);}
    /*inverse_scale.resize(nvars);
    for(int i=0;i<nvars;i++){
    inverse_scale(i)=1/preconditioner.scalingS()(i);}*/
}
///////////////////////////////////////////////////////////////////////
template<class TV> void TRUST_REGION<TV>::
Update_Hessian(bool use_accurate_hessian)
{
    if(use_accurate_hessian){
        equation->Accurate_Hessian(hessian);}
    else{
        equation->Hessian(hessian);}
    equation->Jacobian(jacobian);
    nvars=hessian.rows();
}
///////////////////////////////////////////////////////////////////////
template<class M>
void Print_No_Angular(const M& m)
{
    for(int i=0;i<m.rows();i++){
        if(i%6>2){continue;}
        for(int j=0;j<m.cols();j++){
            if(j%6>2){continue;}
            LOG::cout<<std::setw(13)<<m(i,j)<<" ";
        }
        LOG::cout<<std::endl;
    }
}
template<class TV> typename TRUST_REGION<TV>::STATUS TRUST_REGION<TV>::
Update_One_Step(SIMULATION<TV>& simulation,const T dt,const T time)
{
    auto step_status=UNKNOWN;
    T try_f,step_quality,predicted_reduction;
    /*LOG::cout<<"Jacobian adjoint: "<<std::endl;
    Print_No_Angular(Matrix<T,Dynamic,Dynamic>(jacobian.adjoint()));
    LOG::cout<<"Error: "<<std::endl;
    LOG::cout<<"Un-adjusted hessian sk:"<<std::endl;*/
    //hessian=jacobian.adjoint()*jacobian;
    //Solve_Trust_Conjugate_Gradient(sk);
    //equation->Hessian(hessian);
    Solve_Trust_Conjugate_Gradient(sk);
    //Solve_Trust_MINRES(sk);
    T norm_sk_scaled=Norm(preconditioner,sk,wd);
    if(!std::isfinite(norm_sk_scaled)){step_status=FAILEDCG;}
    else{
        Linearize_Around(simulation,dt,time,sk);
        try_f=equation->Evaluate();
        if(std::isfinite(try_f)){
            T actual_reduction=f-try_f;
            T gs=gk.dot(sk);
            T sBs=sk.dot(hessian.template selfadjointView<Lower>()*sk);
            predicted_reduction=-(gs+sBs/2);
            //Jsk=jacobian*sk;
            //componentwise_prediction=gk.cwiseProduct(Jsk)+Jsk.cwiseProduct(Jsk)/2;
            equation->RHS(try_rhs);
            //LOG::cout<<"Try RHS: "<<std::endl<<try_rhs<<std::endl;
            //LOG::cout<<"Componentwise quality: "<<componentwise_prediction.sum()<<std::endl<<rhs.Diff(try_rhs).cwiseQuotient(componentwise_prediction)<<std::endl;
            //int index;
            //LOG::cout<<"Min value at index "<<index<<" is "<<componentwise_prediction.minCoeff(&index)<<std::endl;
            //LOG::cout<<"Max value at index "<<index<<" is "<<componentwise_prediction.maxCoeff(&index)<<std::endl;
            if(predicted_reduction<0){step_status=ENEGMOVE;}
            step_quality=actual_reduction/predicted_reduction;
            LOG::cout<<"Candidate error with value "<<try_f<<":"<<std::endl;
            /*Matrix<T,Dynamic,1> error;
            equation->RHS(error);
            LOG::cout<<"Current error: "<<std::endl;
            Print_No_Angular(error.transpose());*/
            LOG::cout<<"AP: "<<step_quality<<" old f: "<<f<<" try f: "<<try_f<<" ared: "<<actual_reduction<<" pred: "<<predicted_reduction<<" radius: "<<radius<<" gs: "<<gs<<" sBs: "<<sBs<<" norm_sk_scaled: "<<norm_sk_scaled<<std::endl;}
        else{step_status=FAILEDCG;}}
    if(step_status!=FAILEDCG && step_status!=ENEGMOVE){
        if(step_quality>contract_threshold){
            equation->Gradient(try_g);
            if(std::isfinite(try_g.norm())){
                f=try_f;
                Increment_X(simulation);
                gk=try_g;
                norm_gk=gk.norm();
                /*LOG::cout<<"Resolved error:"<<std::endl;
                Matrix<T,Dynamic,1> error;
                equation->RHS(error);
                Print_No_Angular(error.transpose());*/
                if(step_quality>expand_threshold){// && norm_sk_scaled>=expand_threshold_rad*radius){
                    step_status=EXPAND;}
                else{step_status=MOVED;}}
            else{step_status=FAILEDCG;}}
        else if(step_quality<0){step_status=NEGRATIO;}
        else{step_status=CONTRACT;}}

    LOG::cout<<"Step status: "<<step_status<<std::endl;
    switch(step_status){
        case NEGRATIO:{
            T gksk=gk.dot(sk);
            T gamma_bad=(1-contract_threshold)*gksk/((1-contract_threshold)*(f+gksk+contract_threshold*(f-predicted_reduction)-try_f));
            radius=std::min(contract_factor*norm_sk_scaled,std::max((T)0.0625,gamma_bad)*radius);
            step_status=CONTRACT;
            break;}
        case CONTRACT:
        case FAILEDCG:
        case ENEGMOVE:
            step_status=CONTRACT;
            //radius=norm_sk_scaled*contract_factor;
            radius*=contract_factor;
            break;
        case EXPAND:
            //radius=std::max(expand_factor*norm_sk_scaled,radius);
            radius*=expand_factor;
            break;
        default:
            break;
    };
    return step_status;
}
///////////////////////////////////////////////////////////////////////
template<class TV> typename TV::Scalar TRUST_REGION<TV>::
Norm(const Preconditioner& preconditioner,const Vector& v,Vector& scratch)
{
    if(preconditioner.permutationP().rows() == v.rows()){
        scratch=preconditioner.permutationP()*v;}
    else{scratch=v;}
    scratch=preconditioner.matrixL().adjoint().template triangularView<Upper>()*scratch;
    return scratch.norm();
}
///////////////////////////////////////////////////////////////////////
template<class TV> void TRUST_REGION<TV>::
Solve_Trust_MINRES(Vector& sol)
{
    std::array<T,2> alpha,beta,res;
    std::array<Vector,2> v,d;
    int n=hessian.rows();
    for(int i=0;i<2;i++){
        v[i].resize(n);
        v[i].setZero();
        d[i].resize(n);
        d[i].setZero();
    }
    T norm_A=0;
    T cond_A=1;
    T c=-1,s=0; // givens

    T gamma_min=1e99;
    std::array<T,2> delta1,delta2,ep,gamma1,gamma2;
    delta1[1]=0;
    Vector pk,tk,old_sol;
    sol.resize(n);sol.setZero();
    T tau=0;
    T epsilon=1e-8;
    beta[0]=0;
    T norm_rhs=gk.norm();
    beta[1]=norm_rhs;
    v[1]=-gk/norm_rhs;
    res[0]=norm_rhs;
    tau=norm_rhs;

    auto sign=[&](T x) {return (std::fabs(x)<epsilon?0:x/std::fabs(x));};

    int i;
    std::stringstream reason;
    for(i=0;i<trust_iterations;i++){
        int cur=(i+1)%2,next=i%2;

        pk=hessian*v[cur];
        alpha[cur]=v[cur].dot(pk);
        pk-=alpha[cur]*v[cur];
        v[next]=pk-beta[cur]*v[next];
        beta[next]=v[next].norm();
        if(fabs(beta[next])>epsilon){
            v[next]/=beta[next];}

        delta2[cur]=c*delta1[cur]+s*alpha[cur];
        gamma1[cur]=s*delta1[cur]-c*alpha[cur];

        ep[next]=s*beta[next];
        delta1[next]=-c*beta[next];

        T a=gamma1[cur],b=beta[next];
        if(fabs(b)<epsilon){
            s=0;
            gamma2[cur]=fabs(a);
            if(fabs(a)<epsilon){
                c=1;}
            else{
                c=sign(a);}
        }
        else if(fabs(a)<epsilon){
            c=0;
            s=sign(b);
            gamma2[cur]=fabs(b);
        }
        else if(fabs(b)>fabs(a)){
            T t=a/b;
            s=sign(b)/sqrt(1+sqr(t));
            c=s*t;
            gamma2[cur]=b/s;}
        else{
            T t=b/a;
            c=sign(a)/sqrt(1+sqr(t));
            s=c*t;
            gamma2[cur]=a/c;}

        tau=c*res[next];
        res[cur]=s*res[next];

        if(i==0){norm_A=sqrt(sqr(alpha[cur])+sqr(beta[next]));}
        else{
            T tnorm=sqrt(sqr(alpha[cur])+sqr(beta[next])+sqr(beta[cur]));
            norm_A=std::max(norm_A,tnorm);}

        if(fabs(gamma2[cur])>epsilon){
            d[cur]=(v[cur]-delta2[cur]*d[next]-ep[cur]*d[cur])/gamma2[cur];

            old_sol=sol;
            sol+=tau*d[cur];
            if(sol.norm()>=radius){
                LOG::cout<<"tau was originally "<<tau<<std::endl;
                LOG::cout<<"Direction "<<d[cur].transpose()<<std::endl;
                d[cur]*=tau;
                tau=Find_Tau(old_sol,d[cur]);
                LOG::cout<<"Chosen tau is "<<tau<<std::endl;
                sol=old_sol+tau*d[cur];
                reason<<"Intersect TR bound";
                break;
            }
            gamma_min=std::min(gamma_min,gamma2[cur]);
            cond_A=norm_A/gamma_min;
        }

        LOG::cout<<"residual: "<<res[i%2]<<std::endl;
        if(res[i%2]/norm_rhs<tol){
            reason<<"Reached tolerance";
            break;
        }
    }
    CG_stop_reason=reason.str();
    LOG::cout<<"MINRES reason: "<<CG_stop_reason<<" iterations: "<<i<<std::endl;
    LOG::cout<<"solutn: "<<sol.transpose()<<std::endl;
    LOG::cout<<"result: "<<(hessian*sol).transpose()<<std::endl;
    LOG::cout<<"actual: "<<-gk.transpose()<<std::endl;
}
///////////////////////////////////////////////////////////////////////
template<class TV> void TRUST_REGION<TV>::
Solve_Trust_Conjugate_Gradient(Vector& pk)
{
    T dot_ry,dot_ry_old,aj,tau,dBd,p_norm_gk;
    int j;
    zj.resize(hessian.rows());
    zj.setZero();
    rj=-gk;
    p_norm_gk=Norm(preconditioner,gk,wd);
    T local_tol=std::min((T).5,(T)sqrt(p_norm_gk))*p_norm_gk;
    LOG::cout<<"Local tol: "<<local_tol<<" tol: "<<tol<<std::endl;

    // Solve LL'y=r
    yj=preconditioner.solve(rj);
    dj=yj;

    std::stringstream reason;
    for(j=0;j<trust_iterations;j++){
        dBd=dj.dot(hessian.template selfadjointView<Lower>()*dj);
        if(dBd<=0){
            tau=Find_Tau(zj,dj);
            pk.noalias()=zj+tau*dj;
            num_CG_iterations=j+1;
            reason<<"Negative curvature: "<<dBd;
            break;}

        aj=rj.dot(yj)/dBd;
        zj_old=zj;
        zj.noalias()+=aj*dj;

        if(Norm(preconditioner,zj,wd)>=radius){
            // find tau>=0 s.t. p intersects trust region
            tau=Find_Tau(zj_old,dj);
            pk.noalias()=zj_old+tau*dj;
            num_CG_iterations=j+1;
            reason<<"Intersect TR bound";
            break;}

        dot_ry=rj.dot(yj);
        rj.noalias()-=aj*(hessian.template selfadjointView<Lower>()*dj).eval();

        if(Norm(preconditioner,rj,wd)/p_norm_gk<local_tol){
            pk=zj;
            num_CG_iterations=j+1;
            reason<<"Reached tolerance";
            break;}

        dot_ry_old=dot_ry;

        //updating yj
        yj=preconditioner.solve(rj);
        dot_ry=rj.dot(yj);
        dj*=dot_ry/dot_ry_old;
        dj.noalias()+=yj;}

    if(j>=trust_iterations){
        pk=zj;
        num_CG_iterations=j;
        reason<<"Exceeded max CG iterations";}

    CG_stop_reason=reason.str();
    LOG::cout<<"CG reason: "<<CG_stop_reason<<" iterations: "<<num_CG_iterations<<std::endl;
}
///////////////////////////////////////////////////////////////////////
template<class TV> void TRUST_REGION<TV>::
Multiply(const Preconditioner& X,const Vector& v,Vector& out)
{
    if(X.permutationP().rows() == v.rows()){
        out=X.permutationP()*v;}
    else{out=v;}
    out=X.matrixL().adjoint().template triangularView<Upper>()*out;
    out=X.matrixL().template triangularView<Lower>()*out;
    if(X.permutationP().rows() == v.rows()){
        out=X.permutationP().inverse()*out;}
}
///////////////////////////////////////////////////////////////////////
template<class TV> typename TV::Scalar TRUST_REGION<TV>::
Find_Tau(const Vector& z,const Vector& d)
{
    Multiply(preconditioner,d,wd);
    Multiply(preconditioner,z,wz);

    T pCd=z.dot(wd);
    T dCd=d.dot(wd);
    T pCp=z.dot(wz);
    return (-2*pCd+sqrt(4*pCd*pCd-4*dCd*(pCp-radius*radius)))/(2*dCd);
}
///////////////////////////////////////////////////////////////////////
template<class TV> void TRUST_REGION<TV>::
Check_Derivative(SIMULATION<TV>& simulation,const T dt,const T time)
{
    DATA<TV>& data=simulation.data;
    FORCE<TV>& force=simulation.force;

    T epsilon=1e-2;
    Matrix<T,Dynamic,1> variables=equation->Get_Unknowns(data,force);
    variables.setZero();
    if(!sk.rows()){sk.resize(variables.rows());sk.setZero();}
    Linearize_Around(simulation,dt,time,variables);

    data.random.Direction(variables);
    T f0=equation->Evaluate();
    Matrix<T,Dynamic,1> gradient;equation->Gradient(gradient);
    SparseMatrix<T> h;equation->Hessian(h);
    auto Evaluate_Step_Error = [&](T eps){
        Linearize_Around(simulation,dt,time,eps*variables);

        T f1=equation->Evaluate();
        T predicted_delta_f=gradient.dot(eps*variables)+(T).5*eps*eps*variables.transpose()*h*variables;
        T error=f1-f0-predicted_delta_f;
        LOG::cout<<"Error for "<<eps<<": "<<error<<std::endl;
        return error;
    };
    T last_error=Evaluate_Step_Error(epsilon);
    int divisors=8;
    for(int i=0;i<divisors;i++){
        epsilon/=2;
        T new_error=Evaluate_Step_Error(epsilon);
        LOG::cout<<"Ratio: "<<last_error/new_error<<std::endl;
        last_error=new_error;
    }
    //LOG::cout<<"Ratio: "<<Evaluate_Step_Error(epsilon)/Evaluate_Step_Error(epsilon/2)<<std::endl;
    Linearize_Around(simulation,dt,time,sk);
}
///////////////////////////////////////////////////////////////////////
GENERIC_TYPE_DEFINITION(TRUST_REGION)
DEFINE_AND_REGISTER_PARSER(TRUST_REGION,void)
{
    auto step=std::make_shared<TRUST_REGION<TV>>();
    step->equation=new NONLINEAR_EQUATION<TV>();
    Parse_Scalar(node["precision"],step->precision,step->precision);
    Parse_Scalar(node["contract_threshold"],step->contract_threshold,step->contract_threshold);
    Parse_Scalar(node["expand_threshold"],step->expand_threshold,step->expand_threshold);
    Parse_Scalar(node["contract_factor"],step->contract_factor,step->contract_factor);
    Parse_Scalar(node["expand_factor"],step->expand_factor,step->expand_factor);
    Parse_Scalar(node["trust_iterations"],step->trust_iterations,step->trust_iterations);
    Parse_Scalar(node["tol"],step->tol,step->tol);
    simulation.evolution.push_back(step);
    return 0;
}


//
// Created by cc on 2020/8/4.
//

#include <ros/ros.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>
#include <Eigen/Eigen>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <mavros_msgs/AttitudeTarget.h>
#include <math.h>
#include <dynamic_reconfigure/server.h>
#include <pva_tracker/PVA_TrackerConfig.h>
#include <nav_msgs/Odometry.h>

#define GRAVITATIONAL_ACC 9.81

using namespace Eigen;
using namespace std;

// Coefficients
Vector3d position_error_p;
Vector3d position_error_d;
Vector3d position_error_i;
Vector3d velocity_error_p;
Vector3d velocity_error_d;
Vector3d velocity_error_i;

double p_i_acc_error_limit;
double v_i_acc_error_limit;

// Global Variables
Vector3d planned_p;
Vector3d planned_v;
Vector3d planned_a;
double planned_yaw;
Vector3d current_p;
Vector3d current_v;
Quaterniond current_att;
ros::Publisher att_ctrl_pub, odom_sp_enu_pub;
double thrust_factor;

Eigen::Quaterniond euler2quaternion_eigen(float roll, float pitch, float yaw)
{
    Eigen::Quaterniond temp;
    temp.x() = sin(roll/2)*cos(pitch/2)*cos(yaw/2) - cos(roll/2)*sin(pitch/2)*sin(yaw/2);
    temp.y() = cos(roll/2)*sin(pitch/2)*cos(yaw/2) + sin(roll/2)*cos(pitch/2)*sin(yaw/2);
    temp.z() = cos(roll/2)*cos(pitch/2)*sin(yaw/2) - sin(roll/2)*sin(pitch/2)*cos(yaw/2);
    temp.w() = cos(roll/2)*cos(pitch/2)*cos(yaw/2) + sin(roll/2)*sin(pitch/2)*sin(yaw/2);

    return temp;
}

Eigen::Vector3d quaternion2euler_eigen(float x, float y, float z, float w)
{
    Eigen::Vector3d temp;//roll pitch yaw
    temp.x() = atan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y));

    temp.y() = asin(2.0 * (-z * x + w * y));
    temp.z() = atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z));
    return temp;
}

Vector3d vectorElementMultiply(Vector3d v1, Vector3d v2)
{
    Vector3d result;
    result << v1(0)*v2(0), v1(1)*v2(1), v1(2)*v2(2);
    return result;
}

void vector3dLimit(Vector3d &v, double limit)  ///limit should be positive
{
    if(limit > 0){
        for(int i=0; i<3; i++){
            v(i) = fabs(v(i)) > limit ? (v(i) > 0 ? limit : -limit) : v(i);
        }
    }
}

void pvaCallback(const trajectory_msgs::JointTrajectoryPoint::ConstPtr& msg)
{
    mavros_msgs::AttitudeTarget att_setpoint;

/*    /// NWU frame to ENU frame
    planned_p << -msg->positions[1], msg->positions[0], msg->positions[2];
    planned_yaw = msg->positions[3] + M_PI/2.0;
    planned_v << -msg->velocities[1], msg->velocities[0], msg->velocities[2];
    planned_a << -msg->accelerations[1], msg->accelerations[0], msg->accelerations[2];*/

    planned_p << msg->positions[0], msg->positions[1], msg->positions[2];
    planned_yaw = msg->positions[3];
    planned_v << msg->velocities[0], msg->velocities[1], msg->velocities[2];
    planned_a << msg->accelerations[0], msg->accelerations[1], msg->accelerations[2];

    /// Publish to record in rosbag
    nav_msgs::Odometry odom_sp_enu;
    odom_sp_enu.header.stamp = ros::Time::now();
    odom_sp_enu.pose.pose.position.x = planned_p(0);
    odom_sp_enu.pose.pose.position.y = planned_p(1);
    odom_sp_enu.pose.pose.position.z = planned_p(2);
    odom_sp_enu.twist.twist.linear.x = planned_v(0);
    odom_sp_enu.twist.twist.linear.y = planned_v(1);
    odom_sp_enu.twist.twist.linear.z = planned_v(2);
    odom_sp_enu_pub.publish(odom_sp_enu);

    /// Calculate desired thrust and attitude
    Vector3d p_error = planned_p - current_p;
    Vector3d v_error = planned_v - current_v;

    static Vector3d p_error_last;
    static Vector3d v_error_last;
    static Vector3d p_error_accumulate;
    static Vector3d v_error_accumulate;
    static bool if_init = true;

    if(if_init){
        if_init = false;
        p_error_last = p_error;
        v_error_last = v_error;
        p_error_accumulate = p_error;
        v_error_accumulate = v_error;
        return;
    }

    /**Core code**/
    Vector3d delt_p_error = p_error - p_error_last;
    Vector3d delt_v_error = v_error - v_error_last;

    p_error_accumulate += p_error;
    v_error_accumulate += v_error;
    vector3dLimit(p_error_accumulate, p_i_acc_error_limit);
    vector3dLimit(v_error_accumulate, v_i_acc_error_limit);

    Vector3d a_fb =   /// PID
            vectorElementMultiply(p_error, position_error_p) + vectorElementMultiply(v_error, velocity_error_p) +
            vectorElementMultiply(delt_p_error, position_error_d) + vectorElementMultiply(delt_v_error, velocity_error_d) +
            vectorElementMultiply(p_error_accumulate, position_error_i) + vectorElementMultiply(v_error_accumulate, velocity_error_i);

    // Set a maximum acceleration feedforward value given by position and velocity error.
    for(int i=0; i<3; i++){
        if(fabs(a_fb(i)) > 5.0) a_fb(i) = 5.0 * a_fb(i) / fabs(a_fb(i));
    }

    p_error_last = p_error;
    v_error_last = v_error;

    Vector3d z_w_norm(0, 0, 1.0);
    Vector3d a_des = a_fb + planned_a + GRAVITATIONAL_ACC * z_w_norm;

    Vector3d att_des_norm = a_des / a_des.norm();

    Quaterniond z_w_quat(0, 0, 0, 1.0);
    Quaterniond att_current_vector_quat = current_att * z_w_quat * current_att.inverse();
    Vector3d current_att_vector(att_current_vector_quat.x(), att_current_vector_quat.y(), att_current_vector_quat.z());

//    Quaterniond att_des_q = Quaterniond::FromTwoVectors(current_att_vector, att_des_norm);
    Quaterniond att_des_q = Quaterniond::FromTwoVectors(z_w_norm, att_des_norm);

    //add yaw
    Quaterniond yaw_quat(cos(planned_yaw/2.0), att_des_norm(0)*sin(planned_yaw/2.0),
                         att_des_norm(1)*sin(planned_yaw/2.0),att_des_norm(2)*sin(planned_yaw/2.0));
    att_des_q = yaw_quat * att_des_q;

    //Calculate thrust
    double thrust_des = a_des.norm() * thrust_factor;  //a_des.dot(att_current_vector) * THRUST_FACTOR

    /**End of Core code**/

    Vector3d euler_angle=quaternion2euler_eigen(att_des_q.x(),att_des_q.y(),att_des_q.z(),att_des_q.w());

    att_des_q=euler2quaternion_eigen(euler_angle(0),euler_angle(1),euler_angle(2));

    att_setpoint.header.stamp = ros::Time::now();
    att_setpoint.orientation.w = att_des_q.w();
    att_setpoint.orientation.x = att_des_q.x();
    att_setpoint.orientation.y = att_des_q.y();
    att_setpoint.orientation.z = att_des_q.z();
    att_setpoint.thrust = thrust_des;

    ROS_INFO_THROTTLE(1.0, "Attitude Quaternion Setpoint is w=%f, x=%f, y=%f, z=%f, thrust=%f", att_setpoint.orientation.w,
                      att_setpoint.orientation.x, att_setpoint.orientation.y, att_setpoint.orientation.z, att_setpoint.thrust);

    att_ctrl_pub.publish(att_setpoint);
}

void positionCallback(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    /// ENU frame
    current_p << msg->pose.position.x, msg->pose.position.y, msg->pose.position.z;
    current_att.w() = msg->pose.orientation.w;
    current_att.x() = msg->pose.orientation.x;
    current_att.y() = msg->pose.orientation.y;
    current_att.z() = msg->pose.orientation.z;
}

void velocityCallback(const geometry_msgs::TwistStamped::ConstPtr &msg)
{
    /// ENU frame
    current_v << msg->twist.linear.x, msg->twist.linear.y, msg->twist.linear.z;
}

void configureCallback(tracker::PVA_TrackerConfig &config, uint32_t level) {
    position_error_p << config.position_p_xy, config.position_p_xy, config.position_p_z;
    position_error_d << config.position_d_xy, config.position_d_xy, config.position_d_z;
    position_error_i << config.position_i_xy, config.position_i_xy, config.position_i_z;
    p_i_acc_error_limit = config.p_i_acc_error_limit;

    velocity_error_p << config.velocity_p_xy, config.velocity_p_xy, config.velocity_p_z;
    velocity_error_d << config.velocity_d_xy, config.velocity_d_xy, config.velocity_d_z;
    velocity_error_i << config.velocity_i_xy, config.velocity_i_xy, config.velocity_i_z;
    v_i_acc_error_limit = config.v_i_acc_error_limit;
    thrust_factor = config.hover_thrust_factor;
}

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

    dynamic_reconfigure::Server<tracker::PVA_TrackerConfig> server;
    dynamic_reconfigure::Server<tracker::PVA_TrackerConfig>::CallbackType f;
    f = boost::bind(&configureCallback, _1, _2);
    server.setCallback(f);

    ros::NodeHandle nh;

    ros::Subscriber pva_sub = nh.subscribe("/pva_setpoint", 1, pvaCallback);
    ros::Subscriber position_sub = nh.subscribe("/mavros/local_position/pose", 1, positionCallback);
    ros::Subscriber velocity_sub = nh.subscribe("/mavros/local_position/velocity_local", 1, velocityCallback);

    att_ctrl_pub = nh.advertise<mavros_msgs::AttitudeTarget>("/mavros/setpoint_raw/attitude", 1);
    odom_sp_enu_pub = nh.advertise<nav_msgs::Odometry>("/odom_sp_enu", 1);

    ros::spin();
    return 0;
}


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

namespace boost { namespace simd
{
  namespace tag
  {
    /*!
      @brief saturate generic tag

      Represents the saturate function in generic contexts.

      @par Models:
      Hierarchy
    **/
    struct saturate_ : ext::elementwise_<saturate_>
    {
      /// @brief parent Hierarchy
      typedef ext::elementwise_<saturate_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_saturate_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
    template<class Site, class... Ts>
    BOOST_FORCEINLINE generic_dispatcher<tag::saturate_, Site> dispatching_saturate_(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
    {
      return generic_dispatcher<tag::saturate_, Site>();
    }
    template<class... Args>
    struct impl_saturate_;
  }

  /*!

    Saturate a value of type @c T with the limit values of type @c S.

    @par Semantic:

    @code
    T r = saturate(x, as_<S>())
    @endcode

    is similar to:

    @code
    if (a0 > Inf<S>()) r =  T(Inf<S>());
    else if  (a0 <  Minf<S>()) r =  T(Minf<S>());
    else r = a0;
    @endcode

    @par Note:
    Note that the functor is only valid if conceptually the type S is included in type T
    (or the contrary but this case leads to identity)

    @param a0 Value to saturate
    @param a1 Type specifier of the saturation limit

    @return  The input value saturated on limit of the type specifier
  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::saturate_, saturate, 2)

  /*!
    Returns the saturated value of the first input in the template parameter type,
    but in the same type as the input.

    @par Semantic:

    @code
    T r = saturate<S>(x)
    @endcode

    is similar to:

    @code
    if (a0 > Inf<S>()) r =  T(Inf<S>());
    else if  (a0 <  Minf<S>()) r =  T(Minf<S>());
    else r = a0;
    @endcode

    @param a0

    @return a value of same type as the inputs
  **/
  template<class T,class A0> BOOST_FORCEINLINE
  typename boost::dispatch::meta::
                  call< tag::saturate_( A0 const&
                                      , boost::dispatch::meta::as_<T>
                                      )
                      >::type
  saturate(A0 const& a0)
  {
    typename boost::dispatch::make_functor<tag::saturate_, A0>::type callee;
    return callee(a0,boost::dispatch::meta::as_<T>());
  }
} }

#endif


#pragma optimize ("", off)

#include "straightener.h"

//#include <yixin_loader.h>
#include <Eigen/Core>
#include <igl/slim.h>
#include <igl/per_vertex_normals.h>
#include <igl/unproject_onto_mesh.h>
#include <igl/harmonic.h>
#include <igl/grad.h>
#include <igl/colormap.h>
#include <igl/facet_components.h>
#include <igl/vertex_components.h>

#include <glm/gtx/component_wise.hpp>
#include <modules/base/algorithm/meshutils.h>
#include <modules/opengl/rendering/meshdrawergl.h>
#include <inviwo/core/io/rawvolumeramloader.h>

//#include <utils/utils.h>
#include <modules/opengl/openglutils.h>
#include <modules/opengl/texture/textureutils.h>

#include <modules/segmentangling/util/defer.h>

//#include <utils/rasterizer.h>
#include <inviwo/core/io/rawvolumereader.h>

namespace {

glm::vec3 eigenToGLM(const Eigen::RowVector3d& rv) {
    return glm::vec3(rv[0], rv[1], rv[2]);
}

glm::vec4 eigenToGLMColor(const Eigen::RowVector3d& rv) {
    return glm::vec4(rv[0], rv[1], rv[2], 1.f);
}

//template <typename T>
//glm::tvec3<T::Scalar> eigenToGLM(const Eigen::PlainObjectBase<T>& rv) {
//    return glm::tvec3<T::Scalar>(
//        rv(0, 0),
//    );
//}

} // namespace

namespace inviwo {

int iterCount = 0;

std::string Straightener::selectionStateToString(SelectionState s) {
    switch (s) {
        case SelectionState::None:
            return "Segment finished";
        case SelectionState::Front:
            return "Select front";
        case SelectionState::Back:
            return "Select back";
        default:
            return "HUH!?";
    }
}

bool Straightener::isInputParamEmpty(const InputParams& i) const {
    return (i.frontVertexId == -1) && (i.backVertexId == -1);
}

void Straightener::slimThread() {
    using namespace std;
    using namespace Eigen;

    //m_ui_state.m_avg_draw_state_update_time = 0.0;
    //m_ui_state.m_avg_slim_time = 0.0;

    igl::SLIMData sData;

    cout << "INFO: SLIM Thread: Starting SLIM background thread." << endl;
    while (_isSlimRunning) {

        _constraintsLock.lock();
        if (_isConstraintsChanged) {
            LogInfo("Yey");
            VectorXi slim_b;
            MatrixXd slim_bc;
            _deformationConstraints.slim_constraints(slim_b, slim_bc, _debugOnlyEndAndTets);

            const double soft_const_p = 1e5;
            const MatrixXd TV_0 = _outerTetra.TV;
            sData.exp_factor = 5.0;
            slim_precompute(
                _outerTetra.TV,
                _outerTetra.TT,
                TV_0,
                sData,
                igl::SLIMData::EXP_CONFORMAL,
                slim_b,
                slim_bc,
                soft_const_p
            );

            _isDeformationConstraintsReady = true;
            _isConstraintsChanged = false;
        }
        _constraintsLock.unlock();

        if (!_isDeformationConstraintsReady) {
            this_thread::yield();
            continue;
        }
        _stopInteraction = true;

        LogInfo("Iteration: " << iterCount);
        ++iterCount;

        igl::slim_solve(sData, 1);

        _slimDataMutex.lock();
        //@TODO(abock): try me
        //_slimDataOutput = std::move(sData.V_o);
        _slimDataOutput = sData.V_o;
        _slimDataMutex.unlock();

        _isOutputMeshDirty = true;

        //auto slim_end_time = chrono::high_resolution_clock::now();
        //m_ui_state.update_slim_ema(chrono::duration<double, milli>(slim_end_time - slim_start_time).count());

        //auto ds_update_start_time = chrono::high_resolution_clock::now();

        //int buffer = (m_current_buf + 1) % 2;
        //DrawState& ds = m_ds[buffer];
        //ds.update_tet_mesh(sData.V_o, TT);
        //ds.update_isovalue(isovals, m_constraints.m_level_set_isovalues[m_ui_state.m_current_level_set]);
        //ds.update_skeleton(isovals, m_ui_state.m_num_skel_verts);
        //ds.update_constraint_points(sData.b, sData.bc);

        //auto ds_update_end_time = chrono::high_resolution_clock::now();
        //m_ui_state.update_draw_state_ema(chrono::duration<double, milli>(ds_update_end_time - ds_update_start_time).count());

        //m_double_buf_lock.lock();
        //m_current_buf = buffer;
        //m_draw_state_changed = true;
        //m_double_buf_lock.unlock();
    }
}

const ProcessorInfo Straightener::processorInfo_{
    "bock.straightener",  // Class identifier
    "Tetmesh Straightener",            // Display name
    "Volume Operation",            // Category
    CodeState::Experimental,             // Code state
    Tags::GL                       // Tags
};

Straightener::Straightener()
    : Processor()
    , _inport("volumeInport")
    , _outport("_outport")
    , _trianglesVertexInport("_trianglesVertexInport")
    , _trianglesTetIndexInport("_trianglesTetIndexInport")
    , _skeletonTetraVertexInport("_skeletonTetraVertexInport")
    , _skeletonTetraTetIndexInport("_skeletonTetraTetIndexInport")
    , _outerTetraVertexInport("_outerTetraVertexInport")
    , _outerTetraTetIndexInport("_outerTetraTetIndexInport")
    , _meshOutport("meshOutport")
    //, _imageOutport("imageOutput")
    , _frontSelectionMeshPort("frontSelectionMesh")
    , _backSelectionMeshPort("backSelectionMesh")
    , _debug {
        MeshOutport("debug_isoValues"),
        meshutil::sphere({ 0.f, 0.f, 0.f }, 0.f, glm::vec4(0.f)),
        MeshOutport("debug_skeletonVertices"),
        meshutil::sphere({ 0.f, 0.f, 0.f }, 0.f, glm::vec4(0.f))
    }
    //, _debugMeshOutput("debugOutput")
    , _debugOnlyEndAndTets("onlyEndsAndTets", "_debugOnlyEndAndTets", false)
    , _selectionStateString("selectionState", "SelectionState")
    , _nBones("_nBones", "Number of Bones", 25, 1, 100)
    , _camera("camera", "Camera")
    //, _filename("filename", "Mesh File")
    , _sphereRadius("_sphereRadius", "Sphere Radius", 0.00075f, 0.f, 0.015f)
    //, _reload("reload", "Reload")
    , _windowSize("_windowSize", "Window Size")
    //, _levelsetPlane{
    //    FloatVec3Property("levelset_normal", "Normal"),
    //    FloatVec3Property("levelset_position", "Position"),
    //}
    , _eventPositionUpdate("_eventPositionUpdate", "Mouse Position Tracker",
        [this](Event* e) { eventUpdateMousePos(e); },
        MouseButton::None, MouseState::Move
    )
    , _eventStartDiffusion("_eventStartDiffusion", "Start Diffusion",
        [this](Event*) { eventStartDiffusion(); },
        IvwKey::Space
    )
    , _eventSelectPoint("_eventSelectPoint", "keyboardPress",
        [this](Event*) { eventSelectPoint(); },
        IvwKey::Tab
    )
    , _eventReset("_eventReset", "keyboardPressReset",
        [this](Event*) { eventReset(); },
        IvwKey::F1
    )
    , _eventPreviousInputParameter("_eventPreviousInputParameter", "Previous input parameter",
        [this](Event*) { eventPreviousParameter(); },
        IvwKey::A
    )
    , _eventNextInputParameter("_eventNextInputParameter", "Next input parameter",
        [this](Event*) { eventNextParameter(); },
        IvwKey::D
    )
    , _rasterizeVolume("_rasterizeVolume", "Rasterize")
    //, _eventPreviousLevelSet("_eventPreviousLevelSet", "Previous level set",
    //    [this](Event*) { eventPreviousLevelset(); },
    //    IvwKey::W
    //)
    //, _eventNextLevelSet("_eventNextLevelSet", "Previous level set",
    //    [this](Event*) { eventNextLevelset(); },
    //    IvwKey::S
    //)
    , _inputParameters(1)
    , _currentInputParameter(_inputParameters.begin())
    , _frontSelectionMesh(std::make_shared<BasicMesh>())
    , _backSelectionMesh(std::make_shared<BasicMesh>())
    , _inputParameterSelection("_inputParameterSelection", "Input parameter Selection")
    , _isDebugging("_isDebugging", "Debugging")
    , _diffusionReadyString("_diffusionReadyString", "Diffusion Ready")
    , _statusString("_statusString", "Status string")
    , _statusColor("_statusColor", "Status color")
    , _instructionsString("_instructionsString", "Instructions string")
    , _skeletonMeshReadyString("_skeletonMeshReadyString", "Skeleton Mesh Ready String")
    , _outerMeshReadyString("_outerMeshReadyString", "Outer Mesh Ready String")
{
    // Ports
    addPort(_inport);
    addPort(_outport);
    addPort(_trianglesVertexInport);
    addPort(_trianglesTetIndexInport);
    addPort(_skeletonTetraVertexInport);
    addPort(_skeletonTetraTetIndexInport);
    addPort(_outerTetraVertexInport);
    addPort(_outerTetraTetIndexInport);
    addPort(_meshOutport);
    //addPort(_imageOutport);
    addPort(_frontSelectionMeshPort);
    addPort(_backSelectionMeshPort);
    addPort(_debug.isoValues);
    addPort(_debug.skeletonVertices);

    //// Useful properties
    //addProperty(_levelsetPlane.normal);
    //addProperty(_levelsetPlane.position);

    // Internal properties
    _windowSize.setReadOnly(true);
    addProperty(_windowSize);

    addProperty(_isDebugging);
    _sphereRadius.onChange([this]() { createDebugMeshes(); });
    addProperty(_sphereRadius);

    _rasterizeVolume.onChange([this]() {
        LogInfo("Rasterizing: Start rasterizing");
        Eigen::MatrixXd TV;
        {
            std::lock_guard<std::mutex> gData(_slimDataMutex);

            TV = _slimDataOutput;
        }

        LogInfo("Rasterizing: Finished copy");

        const size3_t dims = _inport.getData()->getDimensions();
        size_t maxDim = glm::compMax(_inport.getData()->getDimensions()) + 1;

        Eigen::MatrixXd resizedTV = TV * maxDim;

        Eigen::RowVector3d bb_min = resizedTV.colwise().minCoeff();
        Eigen::RowVector3d bb_max = resizedTV.colwise().maxCoeff();
        Eigen::RowVector3i outTexSize = (bb_max - bb_min).cast<int>();
        //rasterize_tet_mesh(ds.m_TV, ds.m_TT, TC, tex_size, out_tex_size, tex, out_tex);

        Eigen::VectorXd tex(dims.x * dims.y * dims.z);

        Eigen::MatrixXd TTex = (_outerTetra.TV * maxDim);
        TTex.col(0) /= dims.x;
        TTex.col(1) /= dims.y;
        TTex.col(2) /= dims.z;

        const VolumeRAM* v = _inport.getData()->getRepresentation<VolumeRAM>();

        LogInfo("Rasterizing: Inviwo -> Eigen");

        int zero_pad = 1;
        Eigen::RowVector3i texSize = Eigen::RowVector3i(dims.x + 2 * zero_pad, dims.y + 2 * zero_pad, dims.z + 2 * zero_pad);
        tex.resize(texSize[0] * texSize[1] * texSize[2]);

        int count = 0;
        int tex_count = 0;
        for (int z = 0; z < dims.z + 2 * zero_pad; z++) {
            for (int y = 0; y < dims.y + 2 * zero_pad; y++) {
                for (int x = 0; x < dims.x + 2 * zero_pad; x++) {
                    if (x < zero_pad || y < zero_pad || z < zero_pad || x > dims.x + zero_pad - 1 || y == dims.y + zero_pad - 1 || z == dims.z + zero_pad - 1) {
                        tex[count++] = 0.0;
                    }
                    else {
                        const unsigned char* data = static_cast<const unsigned char*>(v->getData());
                        tex[count++] = data[tex_count++];
                    }
                }
            }
        }

        LogInfo("Rasterizing: Rasterizing");

        Eigen::VectorXd outTex;
        rasterize_tet_mesh(TV, _outerTetra.TT, TTex, texSize, outTexSize, tex, outTex);

        write_texture(
            "temp.raw",
            outTex
        );

        size3_t s(outTexSize[0], outTexSize[1], outTexSize[2]);

        RawVolumeReader r;
        r.setParameters(DataUInt8::get(), s, false);
        std::shared_ptr<Volume> readVol = r.readData("temp.raw");

        _outport.setData(readVol);

        LogInfo(outTexSize);
    });
    addProperty(_rasterizeVolume);

    addProperty(_camera);
    addProperty(_nBones);

    addProperty(_debugOnlyEndAndTets);

    // Events
    addProperty(_eventPositionUpdate);
    addProperty(_eventStartDiffusion);
    addProperty(_eventSelectPoint);
    addProperty(_eventReset);
    addProperty(_eventPreviousInputParameter);
    addProperty(_eventNextInputParameter);

    // Output properties
    addProperty(_selectionStateString);
    addProperty(_inputParameterSelection);
    addProperty(_diffusionReadyString);
    addProperty(_statusString);
    addProperty(_statusColor);
    addProperty(_instructionsString);
    addProperty(_skeletonMeshReadyString);
    addProperty(_outerMeshReadyString);

    // Rest of ctor
    _isSlimRunning = true;
    _slimThread = std::thread(&Straightener::slimThread, this);

    _frontSelectionMeshPort.setData(_frontSelectionMesh);
    _backSelectionMeshPort.setData(_backSelectionMesh);
    //_debugMesh = meshutil::sphere({ 0.f, 0.f, 0.f }, 0.f, glm::vec4(0.f));

    updateSelectionStateString();
    updateInputParameterString();
    updateStatusString();
    updateMeshReadyStrings();
}

Straightener::~Straightener() {
    LogInfo("Stopping SLIM thread");
    _isSlimRunning = false;
    std::this_thread::sleep_for(std::chrono::seconds(2));
    _slimThread.join();
}

void Straightener::process() {
    if (_isFirstFrame) {
        // Update the text that is stored in the workspace

        updateSelectionStateString();
        updateInputParameterString();
        updateDiffusionReadyString();
        updateStatusString();
        updateMeshReadyStrings();

        _isFirstFrame = false;
    }

    updateInstructionsString();
    updateMeshReadyStrings();

    if (_trianglesVertexInport.isChanged() || _trianglesTetIndexInport.isChanged()) {
        std::shared_ptr<const Eigen::MatrixXd> v = _trianglesVertexInport.getData();
        std::shared_ptr<const Eigen::MatrixXi> t = _trianglesTetIndexInport.getData();

        if (v->rows() > 0 && t->rows() > 0) {
            // We were passed a surface mesh consisting of triangles
            _triangle.TV = *v;
            _triangle.TF = *t;

            size3_t dim = _inport.getData()->getDimensions();
            size_t maxDim = glm::compMax(_inport.getData()->getDimensions()) + 1;
            _triangle.TV /= maxDim;

            igl::per_vertex_normals(_triangle.TV, _triangle.TF, _triangle.TFn);
            igl::facet_components(_triangle.TF, _triangle.components);

            _outputSurfaceMesh = createMeshFromTriangles();
            _meshOutport.setData(_outputSurfaceMesh);

            //_currentMeshType = MeshType::Triangle;
        }
    }

    if (_skeletonTetraVertexInport.isChanged() || _skeletonTetraTetIndexInport.isChanged()) {
        std::shared_ptr<const Eigen::MatrixXd> v = _skeletonTetraVertexInport.getData();
        std::shared_ptr<const Eigen::MatrixXi> t = _skeletonTetraTetIndexInport.getData();

        if (v->rows() > 0 || t->rows() > 0) {
            // We were passed a tetrahedral mesh
            _skeletonTetra.TVOriginal = *v;
            _skeletonTetra.TT = *t;

            std::vector<std::array<int, 4>> tris_sorted;
            std::vector<std::array<int, 4>> tris;

            tet_mesh_faces(_skeletonTetra.TT, _skeletonTetra.TF);

            _skeletonTetra.TV = _skeletonTetra.TVOriginal;
            //edge_endpoints(TV, TT, TEV1, TEV2);

            size3_t dim = _inport.getData()->getDimensions();
            size_t maxDim = glm::compMax(_inport.getData()->getDimensions()) + 1;

            _skeletonTetra.TVOriginal /= maxDim;
            _skeletonTetra.TV /= maxDim;

            igl::per_vertex_normals(_skeletonTetra.TV, _skeletonTetra.TF, _skeletonTetra.TFn);

            igl::facet_components(_skeletonTetra.TT, _skeletonTetra.components);

            //_texCoords.resize(_TV.rows(), 3);
            //for (int i = 0; i < _TV.rows(); i++) {
            //    // Subtract 1 since we pad the grid with a zero cell all around
            //    _texCoords.row(i) = _TV.row(i) - f.m_bb_min - Eigen::RowVector3d::Ones();
            //}

            _meshLock.lock();
            _outputSurfaceMesh = createOutputSurfaceMesh(_skeletonTetra.TV);
            _meshOutport.setData(_outputSurfaceMesh);
            _meshLock.unlock();

            for (InputParams& i : _inputParameters) {
                i.frontVertexId = nearest_vertex(_skeletonTetra.TV, _triangle.TV.row(i.frontVertexId));
                i.backVertexId = nearest_vertex(_skeletonTetra.TV, _triangle.TV.row(i.backVertexId));
            }

            // We can start if the user has said so and we have an outer tetrahedral mesh
            if (_userRequestedSlim && _outerTetra.TV.rows() > 0) {
                doSlimSetupStuff();
            }
        }
    }

    if (_outerTetraVertexInport.isChanged() || _outerTetraTetIndexInport.isChanged()) {
        std::shared_ptr<const Eigen::MatrixXd> v = _outerTetraVertexInport.getData();
        std::shared_ptr<const Eigen::MatrixXi> t = _outerTetraTetIndexInport.getData();

        if (v->rows() > 0 || t->rows() > 0) {
            // We were passed a tetrahedral mesh
            _outerTetra.TV = *v;
            _outerTetra.TT = *t;

            tet_mesh_faces(_outerTetra.TT, _outerTetra.TF);

            size3_t dim = _inport.getData()->getDimensions();
            size_t maxDim = glm::compMax(_inport.getData()->getDimensions()) + 1;
            _outerTetra.TV /= maxDim;

            igl::per_vertex_normals(_outerTetra.TV, _outerTetra.TF, _outerTetra.TFn);

            _outputSurfaceMesh = createOuterSurfaceMesh(_outerTetra.TV);
            _meshOutport.setData(_outputSurfaceMesh);

            // We are ready to start if the user has said so and we have a skeleton mesh
            if (_userRequestedSlim && _skeletonTetra.TV.rows() > 0) {
                doSlimSetupStuff();
            }
        }
    }

    updateStatusString();

    if (_isOutputMeshDirty) {
        LogInfo("Update of outer mesh in progess");

        std::lock_guard<std::mutex>g(_slimDataMutex);
        _outputSurfaceMesh = createOuterSurfaceMesh(_slimDataOutput);
        _meshOutport.setData(_outputSurfaceMesh);

        invalidate(InvalidationLevel::InvalidOutput);
        _isOutputMeshDirty = false;
    }

    _debug.isoValues.setData(_debug.isoMesh);
    _debug.skeletonVertices.setData(_debug.skeletonMesh);
}

bool Straightener::isReadyToComputeDiffusion() const {
    // Check that all specified segments have start and end points
    for (const InputParams& i : _inputParameters) {
        if ((i.frontVertexId == -1) || (i.backVertexId == -1)) {
            return false;
        }
    }
    return true;
}

void Straightener::doSlimSetupStuff() {
    bool success = DeformationConstraints::validate_endpoint_pairs(
        pointsFromPoints(),
        _skeletonTetra.components
    );
    if (!success) {
        clearInputParameters();

        _userRequestedSlim = false;
        _constraintValidationFailed = true;
        return;
    }

    geodesic_distances(
        _skeletonTetra.TV,
        _skeletonTetra.TT,
        pointsFromPoints(),
        _isoValues
    );

    std::lock_guard<std::mutex> g(_constraintsLock);

    _deformationConstraints.update_bone_constraints(
        _outerTetra.TV,
        _outerTetra.TT,
        _skeletonTetra.TV,
        _skeletonTetra.TT,
        _isoValues,
        _skeletonTetra.components,
        { { _currentInputParameter->frontVertexId, _currentInputParameter->backVertexId } },
        _nBones
    );
    _isConstraintsChanged = true;
    createDebugMeshes();

}

std::shared_ptr<BasicMesh> createMesh(const Eigen::MatrixXd& TV, const Eigen::MatrixXi& TF,
    const Eigen::MatrixXd& TFn, size3_t volumeDim, bool useIsovalues, const Eigen::VectorXd& isoValues = Eigen::VectorXd())
{
    std::shared_ptr<BasicMesh> mesh = std::make_shared<BasicMesh>();
    mesh->setModelMatrix(mat4());
    auto indices = mesh->addIndexBuffer(DrawType::Triangles, ConnectivityType::None);

    Eigen::MatrixXd colors;
    if (useIsovalues) {
        igl::colormap(igl::ColorMapType::COLOR_MAP_TYPE_INFERNO, isoValues, false, colors);
    }

    //_isoValues.rows() == 0

    size_t maxDim = glm::compMax(volumeDim) + 1;
    for (int i = 0; i < TV.rows(); ++i) {
        glm::vec3 p = glm::vec3(TV(i, 0), TV(i, 1), TV(i, 2));
        glm::vec4 c;
        if (useIsovalues) {
            c = eigenToGLMColor(colors.row(i));
        }
        else {
            c = glm::vec4((p * float(maxDim)) / glm::vec3(volumeDim) + glm::vec3(1.f), 1.f);
        }

        mesh->addVertex(
            p, // pos
            glm::vec3(TFn(i, 0), TFn(i, 1), TFn(i, 2)), // normal
            glm::vec3(0.f), // texCoord
            c // color
        );
    }

    for (int i = 0; i < TF.rows(); ++i) {
        indices->add({
            static_cast<unsigned int>(TF(i, 0)),
            static_cast<unsigned int>(TF(i, 1)),
            static_cast<unsigned int>(TF(i, 2))
        });
    }

    return mesh;
}

std::shared_ptr<BasicMesh> Straightener::createOutputSurfaceMesh(const Eigen::MatrixXd& TV) {
    bool useIsoValues = _isoValues.rows() != 0;
    return createMesh(
        TV,
        _skeletonTetra.TF, _skeletonTetra.TFn,
        _inport.getData()->getDimensions(),
        useIsoValues, _isoValues
    );
}

std::shared_ptr<BasicMesh> Straightener::createOuterSurfaceMesh(const Eigen::MatrixXd& TV) {
    return createMesh(TV, _outerTetra.TF, _outerTetra.TFn, _inport.getData()->getDimensions(), false);
}

std::shared_ptr<BasicMesh> Straightener::createMeshFromTriangles() {
    std::shared_ptr<BasicMesh> mesh = std::make_shared<BasicMesh>();
    mesh->setModelMatrix(mat4());
    auto indices = mesh->addIndexBuffer(DrawType::Triangles, ConnectivityType::None);

    size_t maxDim = glm::compMax(_inport.getData()->getDimensions()) + 1;

    for (int i = 0; i < _triangle.TV.rows(); ++i) {
        glm::vec3 p = glm::vec3(_triangle.TV(i, 0), _triangle.TV(i, 1), _triangle.TV(i, 2));
        glm::vec4 c = glm::vec4((p * float(maxDim)) / glm::vec3(_inport.getData()->getDimensions()) + glm::vec3(1.f), 1.f);
        //glm::vec4(colors(i, 0), colors(i, 1), colors(i, 2), 1.f);

        mesh->addVertex(
            p, // pos
            glm::vec3(_triangle.TFn(i, 0), _triangle.TFn(i, 1), _triangle.TFn(i, 2)), // normal
            glm::vec3(0.f), // texCoord
            c // color
        );
    }

    for (int i = 0; i < _triangle.TF.rows(); ++i) {
        indices->add({
            static_cast<unsigned int>(_triangle.TF(i, 0)),
            static_cast<unsigned int>(_triangle.TF(i, 1)),
            static_cast<unsigned int>(_triangle.TF(i, 2))
        });
    }

    return mesh;
}

void Straightener::createDebugMeshes() {
    _debug.isoMesh = meshutil::sphere({ 0.f, 0.f, 0.f }, 0.f, glm::vec4(0.f));
    //_debugMesh->setModelMatrix(mat4());

    Eigen::MatrixXd colors;
    if (_isoValues.rows() != 0) {
        igl::colormap(igl::ColorMapType::COLOR_MAP_TYPE_INFERNO, _isoValues, false, colors);
    }

    for (int i = 0; i < _skeletonTetra.TV.rows(); ++i) {
        glm::vec3 p = eigenToGLM(_skeletonTetra.TV.row(i));
        glm::vec4 c = _isoValues.rows() == 0 ? glm::vec4(1.f) : eigenToGLMColor(colors.row(i));
        //glm::vec4(colors(i, 0), colors(i, 1), colors(i, 2), 1.f);

        std::shared_ptr<BasicMesh> m = meshutil::sphere(p, _sphereRadius, c);

        _debug.isoMesh->append(m.get());
    }

    _debug.skeletonMesh = meshutil::sphere({ 0.f, 0.f, 0.f }, 0.f, glm::vec4(0.f));
    LogInfo("nBoneConstraints: " << _deformationConstraints.m_bone_constraints_idx.size());

    for (int i : _deformationConstraints.m_bone_constraints_idx) {
        glm::vec3 p = eigenToGLM(_skeletonTetra.TV.row(i));
        glm::vec4 c(0.f, 0.95f, 0.f, 1.f);

        std::shared_ptr<BasicMesh> m = meshutil::sphere(p, 1.5f * _sphereRadius, c);

        _debug.skeletonMesh->append(m.get());
    }

    invalidate(InvalidationLevel::InvalidOutput);
}

//////////////////////////////////////////////////////////////////////////////////////////
//                                Event Handlers
//////////////////////////////////////////////////////////////////////////////////////////

void Straightener::eventUpdateMousePos(Event* e) {
    updateStatusString();
    updateMeshReadyStrings();

    if (_stopInteraction) {
        return;
    }

    MouseEvent* mouseEvent = static_cast<MouseEvent*>(e);

    glm::dvec2 pos = mouseEvent->pos();

    // @FRAGILE(abock): needs to be the same as in ::createMesh
    glm::mat4 m = glm::mat4();

    glm::mat4 v = _camera.get().getViewMatrix();
    glm::mat4 p = _camera.get().getProjectionMatrix();

    // column-major v row-major (GLM v Eigen)
    glm::mat4 mv = glm::transpose(v * m);
    //glm::mat4 mv = (v * m);

    Eigen::Matrix4f modelView;
    modelView << mv[0][0], mv[0][1], mv[0][2], mv[0][3],
        mv[1][0], mv[1][1], mv[1][2], mv[1][3],
        mv[2][0], mv[2][1], mv[2][2], mv[2][3],
        mv[3][0], mv[3][1], mv[3][2], mv[3][3];

    p = glm::transpose(p);
    Eigen::Matrix4f projMatrix;
    projMatrix << p[0][0], p[0][1], p[0][2], p[0][3],
        p[1][0], p[1][1], p[1][2], p[1][3],
        p[2][0], p[2][1], p[2][2], p[2][3],
        p[3][0], p[3][1], p[3][2], p[3][3];

    glm::size2_t viewportSize = glm::size2_t(_windowSize.get());
    //glm::size2_t viewportSize = _imageOutport.getData()->getDimensions();

    Eigen::Vector4f viewport;
    viewport << 0.f, 0.f, static_cast<float>(viewportSize.x), static_cast<float>(viewportSize.y);

    //LogInfo("Viewport: " << viewport);

    //LogInfo("Mouse pos: " << pos);

    int fid;
    Eigen::Vector3f bc;

    _meshLock.lock();

    _currentHoverVertexId = -1;
    if (igl::unproject_onto_mesh(Eigen::Vector2f(pos.x, pos.y),
        modelView, projMatrix,
        viewport,
        currentTV(), currentTF(), fid, bc))
    {
        int max;
        bc.maxCoeff(&max);
        // HUH!?!  currentTF or currentTV?
        _currentHoverVertexId = currentTF()(fid, max);
    }

    _meshLock.unlock();

    //LogInfo("Current vertex id: " << _currentHoverVertexId);
}

void Straightener::eventStartDiffusion() {
    updateStatusString();

    if (_stopInteraction) {
        return;
    }

    if (isReadyToComputeDiffusion()) {
        _userRequestedSlim = true;
        if (hasTetraMeshes()) {
            doSlimSetupStuff();
        }
    }
}

void Straightener::eventSelectPoint() {
    if (_stopInteraction) {
        return;
    }

    _constraintValidationFailed = false;
    _selectedInvalidConstraint = false;

    std::lock_guard<std::mutex> g(_meshLock);
    defer { updateDiffusionReadyString(); };

    if (_currentHoverVertexId == -1) {
        return;
    }

    switch (_currentInputParameter->state) {
        case SelectionState::None:
            break;
        case SelectionState::Front:
        {
            _currentInputParameter->frontVertexId = _currentHoverVertexId;
            _currentInputParameter->state = SelectionState::Back;

            LogInfo("Selected front: " << _currentInputParameter->frontVertexId);

            glm::vec3 p = glm::vec3(
                currentTV()(_currentInputParameter->frontVertexId, 0),
                currentTV()(_currentInputParameter->frontVertexId, 1),
                currentTV()(_currentInputParameter->frontVertexId, 2)
            );
            std::shared_ptr<BasicMesh> mesh = meshutil::sphere(p, 0.015f, glm::vec4(0.f, 0.75f, 0.f, 1.f));
            _frontSelectionMesh->append(mesh.get());

            _frontSelectionMeshPort.setData(_frontSelectionMesh);
            break;
        }
        case SelectionState::Back:
        {
            _currentInputParameter->backVertexId = _currentHoverVertexId;
            _currentInputParameter->state = SelectionState::None;

            LogInfo("Selected back: " << _currentInputParameter->backVertexId);

            glm::vec3 p = glm::vec3(
                currentTV()(_currentInputParameter->backVertexId, 0),
                currentTV()(_currentInputParameter->backVertexId, 1),
                currentTV()(_currentInputParameter->backVertexId, 2)
            );
            std::shared_ptr<const BasicMesh> mesh = meshutil::sphere(p, 0.015f, glm::vec4(0.75f, 0.f, 0.f, 1.f));
            _backSelectionMesh->append(mesh.get());

            _backSelectionMeshPort.setData(_backSelectionMesh);
            break;
        }
    }

    const Eigen::VectorXi& comp = hasTetraMeshes() ? _skeletonTetra.components : _triangle.components;

    bool success = DeformationConstraints::validate_endpoint_pairs(
        pointsFromPoints(),
        comp
    );
    if (!success) {
        clearInputParameters();

        _userRequestedSlim = false;
        _selectedInvalidConstraint = true;
        return;
    }

    _selectionStateString = selectionStateToString(_currentInputParameter->state);
}

void Straightener::clearInputParameters() {
    _inputParameters.clear();
    _inputParameters.resize(1);
    _currentInputParameter = std::prev(_inputParameters.end());

    _frontSelectionMesh = std::make_shared<BasicMesh>();
    _backSelectionMesh = std::make_shared<BasicMesh>();
    _frontSelectionMeshPort.setData(_frontSelectionMesh);
    _backSelectionMeshPort.setData(_backSelectionMesh);
}

void Straightener::eventReset() {
    defer { updateDiffusionReadyString(); };

    clearInputParameters();

    _isoValues.resize(0, 0);
    hasTetraMeshes() ? createOutputSurfaceMesh(_skeletonTetra.TV) : createMeshFromTriangles();
}

void Straightener::eventPreviousParameter() {
    if (_stopInteraction) {
        return;
    }
    defer {
        updateDiffusionReadyString();
        updateSelectionStateString();
    };

    LogInfo("Selecting previous input parameter set");

    if (_currentInputParameter != _inputParameters.begin()) {
        --_currentInputParameter;

        auto oldParameter = std::next(_currentInputParameter);
        if (isInputParamEmpty(*(oldParameter))) {
            _inputParameters.erase(oldParameter);
        }

        updateInputParameterString();
    }

}

void Straightener::eventNextParameter() {
    if (_stopInteraction) {
        return;
    }
    LogInfo("Selecting next input parameter set");

    ++_currentInputParameter;

    // If we are now at the end, we need to add a new input state and repoint
    if (_currentInputParameter == _inputParameters.end()) {
        // We are currently the last input parameter and have to add a new one
        _inputParameters.push_back(InputParams());
        _currentInputParameter = std::prev(_inputParameters.end());
    }

    updateInputParameterString();
    updateDiffusionReadyString();
    updateSelectionStateString();
}

//void Straightener::eventPreviousLevelset() {
    //if (_stopInteraction) {
    //    return;
    //}
    //LogInfo("Selecting previous levelset");

    //if (_currentInputParameter->currentLevelset == _currentInputParameter->levelSetOrientations.begin()) {
    //    // We are at the first level set
    //    return;
    //}

    //--(_currentInputParameter->currentLevelset);

    //updateDiffusionReadyString();
//}

//void Straightener::eventNextLevelset() {
    //if (_stopInteraction) {
    //    return;
    //}
    //LogInfo("Selecting next levelset");

    //if (_currentInputParameter->currentLevelset + 1 == _currentInputParameter->levelSetOrientations.end()) {
    //    // We are at the last level set
    //    return;
    //}

    //++(_currentInputParameter->currentLevelset);

    //updateDiffusionReadyString();
//}

const Eigen::MatrixXd& Straightener::currentTV() const {
    return hasTetraMeshes() ? _skeletonTetra.TV : _triangle.TV;
}

const Eigen::MatrixXi& Straightener::currentTF() const {
    return hasTetraMeshes() ? _skeletonTetra.TF : _triangle.TF;
}

bool Straightener::hasTetraMeshes() const {
    return _skeletonTetra.TVOriginal.rows() > 0 && _outerTetra.TV.rows() > 0;
}

std::vector<std::array<int, 2>> Straightener::pointsFromPoints() const {
    std::vector<std::array<int, 2>> res;
    for (const InputParams& i : _inputParameters) {
        std::array<int, 2> v = { i.frontVertexId, i.backVertexId };
        res.push_back(v);
    }
    return res;
}

//////////////////////////////////////////////////////////////////////////////////////////
//                                  String update methods
//////////////////////////////////////////////////////////////////////////////////////////

void Straightener::updateSelectionStateString() {
    _selectionStateString = selectionStateToString(_currentInputParameter->state);
}

void Straightener::updateInputParameterString() {
    ptrdiff_t d = std::distance(_inputParameters.begin(), _currentInputParameter);
    _inputParameterSelection =
        "Editing " +
        // Adding one to make it more human readable 1-based indexing
        std::to_string(d + 1) +
        " of " +
        std::to_string(_inputParameters.size()) +
        " segments";
}

void Straightener::updateDiffusionReadyString() {
    _diffusionReadyString = isReadyToComputeDiffusion() ? "Ready" : "";
}

void Straightener::updateStatusString() {
    if (_constraintValidationFailed) {
        _statusString = "Error with constraints; please select fresh constraints";
        _statusColor = glm::vec4(0.65f, 0.1f, 0.1f, 1.f);
        return;
    }

    if (_selectedInvalidConstraint) {
        _statusString = "Error: Constraints need to be on separate components";
        _statusColor = glm::vec4(0.65f, 0.3f, 0.3f, 1.f);
        return;
    }

    if (hasTetraMeshes() && _userRequestedSlim) {
        _statusString = "Straightening";
        _statusColor = glm::vec4(0.f, 0.75f, 0.f, 1.f);
    }

    if (hasTetraMeshes() && isReadyToComputeDiffusion() && !_userRequestedSlim) {
        _statusString = "Waiting for start (SPACE)";
        _statusColor = glm::vec4(0.75f, 0.75f, 0.f, 1.f);
        return;
    }

    if (!hasTetraMeshes() && _userRequestedSlim) {
        _statusString = "Tetrahedralizing...";
        _statusColor = glm::vec4(0.75f, 0.75f, 0.f, 1.f);
        return;
    }

    _statusString = "";
}

void Straightener::updateInstructionsString() {
    bool b =
        (_trianglesVertexInport.getData()->rows() == 0) &&
        (_trianglesTetIndexInport.getData()->rows() == 0) &&
        (_skeletonTetraVertexInport.getData()->rows() == 0) &&
        (_skeletonTetraTetIndexInport.getData()->rows() == 0);

    if (b) {
        _instructionsString = "Press 'Load Dataset' to start";
    }
    else {
        _instructionsString = "";
    }
}

void Straightener::updateMeshReadyStrings() {
    if (_skeletonTetra.TVOriginal.rows() > 0) {
        _skeletonMeshReadyString = "Skeleton Mesh";
    }
    else {
        _skeletonMeshReadyString = "";
    }

    if (_outerTetra.TV.rows() > 0) {
        _outerMeshReadyString = "Outer Mesh";
    }
    else {
        _outerMeshReadyString = "";
    }
}

//////////////////////////////////////////////////////////////////////////////////////////
//                                  Inviiiiiiwo
//////////////////////////////////////////////////////////////////////////////////////////

const ProcessorInfo Straightener::getProcessorInfo() const {
    return processorInfo_;
}

}  // namespace

#pragma optimize ("", on)


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

  @copyright 2012-2015 NumScale SAS
  @copyright 2015 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_FUNCTION_SCALAR_FAST_RSQRT_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SCALAR_FAST_RSQRT_HPP_INCLUDED

#include <boost/simd/function/definition/fast_rsqrt.hpp>
#include <boost/simd/arch/common/scalar/function/fast_rsqrt.hpp>

#endif


//----------------------------------------------------------------------------
// Copyright (C) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the Server Side Public License, version 1,
// as published by the author.
//
// 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
// Server Side Public License for more details.
//
// You should have received a copy of the Server Side Public License
// along with this program. If not, see
// <https://github.com/NilFoundation/plugin/blob/master/LICENSE_1_0.txt>.
//----------------------------------------------------------------------------

#define BOOST_TEST_MODULE merkle_proof_test

#include <boost/test/data/monomorphic.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/unit_test.hpp>

#include <nil/crypto3/hash/blake2s.hpp>
#include <nil/crypto3/hash/pedersen.hpp>
#include <nil/crypto3/hash/poseidon.hpp>
#include <nil/crypto3/hash/sha2.hpp>

#include <nil/filecoin/storage/proofs/core/merkle/proof.hpp>

#include "./generate_tree.hpp"

using namespace nil::filecoin;

BOOST_AUTO_TEST_SUITE(merkle_proof_test_suite)

template<typename MerkleTreeType>
void merklepath() {
    std::size_t node_size = 32;
    std::size_t nodes = 64 * get_base_tree_count<MerkleTreeType>();

    auto rng = rand::thread_rng();
    auto data, tree;
    const std::tie(data, tree) = merkletree::generate_tree<MerkleTreeType>(rng, nodes, None);

    for (std::size_t i = 0; i < nodes; i++) {
        const auto proof = tree.gen_proof(i);

        assert(proof.verify(), "failed to validate");

        assert(proof.validate(i), "failed to validate valid merkle path");
        const auto data_slice = &data[i * node_size..(i + 1) * node_size].to_vec();
        assert(proof.validate_data(data_slice), "failed to validate valid data");
    }
}

BOOST_AUTO_TEST_CASE(merklepath_poseidon_2) {
    merklepath::<MerkleTreeWrapper<PoseidonHasher, DiskStore <PoseidonHasher::digest_type>, 2, 0, 0, >, > ();
}

BOOST_AUTO_TEST_CASE(merklepath_poseidon_4) {
    merklepath::<MerkleTreeWrapper<PoseidonHasher, DiskStore <PoseidonHasher::digest_type>, 4, 0, 0, >, > ();
}

BOOST_AUTO_TEST_CASE(merklepath_poseidon_8) {
    merklepath::<MerkleTreeWrapper<PoseidonHasher, DiskStore <PoseidonHasher::digest_type>, 8, 0, 0, >, > ();
}

BOOST_AUTO_TEST_CASE(merklepath_poseidon_8_2) {
    merklepath::<MerkleTreeWrapper<PoseidonHasher, DiskStore <PoseidonHasher::digest_type>, 8, 2, 0, >, > ();
}

BOOST_AUTO_TEST_CASE(merklepath_poseidon_8_4) {
    merklepath::<MerkleTreeWrapper<PoseidonHasher, DiskStore <PoseidonHasher::digest_type>, 8, 4, 0, >, > ();
}

BOOST_AUTO_TEST_CASE(merklepath_poseidon_8_4_2) {
    merklepath::<MerkleTreeWrapper<PoseidonHasher, DiskStore <PoseidonHasher::digest_type>, 8, 4, 2, >, > ();
}

BOOST_AUTO_TEST_CASE(merklepath_sha256_2) {
    merklepath::<MerkleTreeWrapper<Sha256Hasher, DiskStore <Sha256Hasher::digest_type>, 2, 0, 0, >, > ();
}

BOOST_AUTO_TEST_CASE(merklepath_sha256_4) {
    merklepath::<MerkleTreeWrapper<Sha256Hasher, DiskStore <Sha256Hasher::digest_type>, 4, 0, 0, >, > ();
}

BOOST_AUTO_TEST_CASE(merklepath_sha256_2_4) {
    merklepath::<MerkleTreeWrapper<Sha256Hasher, DiskStore <Sha256Hasher::digest_type>, 2, 4, 0, >, > ();
}

BOOST_AUTO_TEST_CASE(merklepath_sha256_top_2_4_2) {
    merklepath::<MerkleTreeWrapper<Sha256Hasher, DiskStore <Sha256Hasher::digest_type>, 2, 4, 2, >, > ();
}

BOOST_AUTO_TEST_CASE(merklepath_blake2s_2) {
    merklepath::<MerkleTreeWrapper<Blake2sHasher, DiskStore <Blake2sHasher::digest_type>, 2, 0, 0, >, > ();
}

BOOST_AUTO_TEST_CASE(merklepath_blake2s_4) {
    merklepath::<MerkleTreeWrapper<Blake2sHasher, DiskStore <Blake2sHasher::digest_type>, 4, 0, 0, >, > ();
}

BOOST_AUTO_TEST_CASE(merklepath_blake2s_8_4_2) {
    merklepath::<MerkleTreeWrapper<Blake2sHasher, DiskStore <Blake2sHasher::digest_type>, 8, 4, 2, >, > ();
}

BOOST_AUTO_TEST_SUITE_END()


#include <iostream>
#include "HexGridPrint.hpp"
#include <boost/lexical_cast.hpp>

HexGridPrint::HexGridPrint(int i_width,int i_height,
                           bool i_ULStart,bool i_LowShort):
  m_width(i_width),
  m_height(i_height),
  m_ULStart(i_ULStart),
  m_LowShort(i_LowShort),
  m_grid(2+5*i_width,1+4*i_height+(i_LowShort ? 0 : 2))
{
  int i,j;
  for (i = 0 ; i < i_width ; ++i)
  {
    int h = i_height;
    if (i_LowShort)
    {
      if (i % 2 == 0 && !i_ULStart) h--;
      if (i % 2 == 1 && i_ULStart) h--;
    }
    for (j = 0 ; j < h ; ++j)
    {
      DrawHex(i,j);
    }
  }
}

//       ___    +
//      /   \   |
//  ___/     \  |
// / 3 \     /  |
//*2   4\___/   |
//\1   5/   \   |
// \_0_/     \  |
//     \     /  |
//      \___/   +

int HexGridPrint::GetHexX(int i_x,int i_y)
{
  return i_x * 5;
}

int HexGridPrint::GetHexY(int i_x,int i_y)
{
  bool evencolumn = (i_x % 2 == 0);
  return 2 + 4 * i_y + ((evencolumn != m_ULStart) ? 2 : 0);
}

void HexGridPrint::DrawHex(int i_x,int i_y)
{
  int bx = GetHexX(i_x,i_y);
  int by = GetHexY(i_x,i_y);
  //  std::cout << "Drawing Hex (" << i_x << "," << i_y << ") at (";
  //  std::cout << bx << "," << by << ")" << std::endl;
  m_grid.AddString( "___",bx+2,by-2);
  m_grid.AddString("/   \\",bx+1,by-1);
  m_grid.AddString("/     \\",bx,by);
  m_grid.AddString("\\     /",bx,by+1);
  m_grid.AddString("\\___/",bx+1,by+2);

}

void HexGridPrint::AddItem(int i_x,int i_y,HexLoc i_loc,char i_datum)
{
  int bx = GetHexX(i_x,i_y);
  int by = GetHexY(i_x,i_y);
  int dx;
  int dy;

  switch (i_loc)
  {
  case DIR0:  dx = 3 ; dy =  2; break;
  case DIR1:  dx = 1 ; dy =  1; break;
  case DIR2:  dx = 1 ; dy =  0; break;
  case DIR3:  dx = 3 ; dy = -1; break;
  case DIR4:  dx = 5 ; dy =  0; break;
  case DIR5:  dx = 5 ; dy =  1; break;
  case STAT1: dx = 3 ; dy =  0; break;
  case STAT2: dx = 3 ; dy =  1; break;
  }

  m_grid.AddString(boost::lexical_cast<std::string>(i_datum),bx+dx,by+dy);
}

const std::string &HexGridPrint::Show() const
{
  return m_grid.Show();
}


// -*- mode: c++; indent-tabs-mode: nil; -*-
//
// Copyright (c) 2009-2013 Illumina, Inc.
//
// This software is provided under the terms and conditions of the
// Illumina Open Source Software License 1.
//
// You should have received a copy of the Illumina Open Source
// Software License 1 along with this program. If not, see
// <https://github.com/sequencing/licenses/>
//

/// \file

/// \author Chris Saunders
///
#ifndef __POSITION_SNP_CALL_PPROB_MONOGT_HH
#define __POSITION_SNP_CALL_PPROB_MONOGT_HH

#include "blt_common/snp_pos_info.hh"

#include "blt_util/monogt.hh"

#include <boost/utility.hpp>

#include <iosfwd>

struct monoploid_genotype : private boost::noncopyable {

    monoploid_genotype() : is_snp(false), ref_gt(0), max_gt(0), max2_gt(0) {
        static const double p(1./static_cast<double>(MONOGT::SIZE));
        for (unsigned i(0); i<MONOGT::SIZE; ++i) pprob[i] = p;
    }

    bool is_snp;
    unsigned ref_gt;
    unsigned max_gt;
    unsigned max2_gt;
    double pprob[MONOGT::SIZE];
};

std::ostream& operator<<(std::ostream& os,monoploid_genotype& dp);

/// \brief call a snp @ pos by calculating the posterior probability
/// of all possible genotypes for a monoploid individual.
///
void
position_snp_call_pprob_monogt(const double theta,
                               const snp_pos_info& pi,
                               monoploid_genotype& dgt);

#endif


// Copyright 2019 Jan Niklas Hasse <jhasse@bixense.com>
// For conditions of distribution and use, see copyright notice in LICENSE.txt

#include "Fixture.hpp"

#include "../opengl.hpp"

#include <boost/math/special_functions/round.hpp>
#include <boost/range/adaptor/reversed.hpp>
#include <boost/test/unit_test.hpp>
#include <jngl.hpp>

Fixture::Fixture(const double scaleFactor) {
	jngl::setScaleFactor(scaleFactor);
	jngl::showWindow("unit test", boost::math::iround(320 * scaleFactor),
	                 boost::math::iround(70 * scaleFactor), false, { 32, 7 }, { 32, 7 });
	reset();
	emptyAsciiArt = getAsciiArt();
	BOOST_CHECK_EQUAL(emptyAsciiArt, R"(
▓▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▓
▒                              ▒
▒                              ▒
▒                              ▒
▒                              ▒
▒                              ▒
▓▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▒▓
)");
}

Fixture::~Fixture() {
	jngl::hideWindow();
}

std::string Fixture::getAsciiArt() const {
	const int w = jngl::getWindowWidth();
	const int h = jngl::getWindowHeight();
	std::vector<float> buffer(3 * w * h);
	glReadPixels(0, 0, w, h, GL_RGB, GL_FLOAT, buffer.data());

	// ASCII art should always have the same size, therefore let's take the scaleFactor into
	// account:
	auto reduceFactorAsFloat = static_cast<float>(10 * jngl::getScaleFactor());
	int reduceFactor = boost::math::iround(reduceFactorAsFloat);
	BOOST_CHECK_CLOSE(reduceFactorAsFloat, reduceFactor, 1e-6);

	assert(w % reduceFactor == 0);
	assert(h % reduceFactor == 0);
	size_t reducedW = w / reduceFactor;
	std::vector<std::vector<std::vector<float>>> reduced;
	size_t index = 0;
	for (int y = 0; y < h; y += reduceFactor) {
		reduced.emplace_back(reducedW, std::vector<float>(3, 0.0f));
		for (int i = 0; i < reduceFactor; ++i) {
			for (int x = 0; x < w; x += reduceFactor) {
				for (int j = 0; j < reduceFactor; ++j) {
					for (auto& cell : reduced.back().at(x / reduceFactor)) {
						assert(0.0f <= cell && cell <= 1.0f);
						cell += buffer[index] / float(reduceFactor * reduceFactor);
						++index;
					}
				}
			}
		}
	}
	BOOST_CHECK_EQUAL(index, buffer.size());
	assert(reduced.size() == size_t(h / reduceFactor));
	std::string out = "\n"; // Start with a newline for prettier output by Boost.Test
	for (const auto& row : boost::adaptors::reverse(reduced)) {
		assert(row.size() == reducedW);
		for (const auto& cell : row) {
			// ASCII:
			// const static std::vector<std::string> chars = { "@", "#", "%", "x", "o",
			//                                                 ";", ":", ",", ".", " " };

			// UTF-8:
			const static std::vector<std::string> chars = { "█", "▓", "▒", "░", " " };

			float gray = (cell.at(0) + cell.at(1) + cell.at(2)) / 3.0f;
			const size_t index = boost::math::iround(gray * float(chars.size() - 1));
			assert(index < chars.size());
			out += chars[index];
		}
		out += "\n";
	}
	assert(emptyAsciiArt != out); // We shouldn't call this function without drawing something first
	reset();
	return out;
}

void Fixture::reset() {
	jngl::swapBuffers();
	jngl::updateInput();

	const double frameSize = 5;
	const jngl::Vec2 screen(jngl::getScreenWidth(), jngl::getScreenHeight());
	jngl::drawRect(-screen.x / 2, screen.y / 2 - frameSize, screen.x, frameSize); // bottom
	jngl::drawRect(-screen.x / 2, -screen.y / 2, screen.x, frameSize);            // top
	jngl::drawRect(screen.x / 2 - frameSize, -screen.y / 2, frameSize, screen.y); // right
	jngl::drawRect(-screen.x / 2, -screen.y / 2, frameSize, screen.y);            // left
}