// Copyright (c) Jeremy Siek 2001, Marc Wintermantel 2002
//
// Permission to use, copy, modify, distribute and sell this software
// and its documentation for any purpose is hereby granted without fee,
// provided that the above copyright notice appears in all copies and
// that both that copyright notice and this permission notice appear
// in supporting documentation.  Silicon Graphics makes no
// representations about the suitability of this software for any
// purpose.  It is provided "as is" without express or implied warranty.

#ifndef BOOST_GRAPH_BANDWIDTH_HPP
#define BOOST_GRAPH_BANDWIDTH_HPP

#include <boost/graph/graph_traits.hpp>
#include <boost/detail/numeric_traits.hpp>

namespace boost {

  template <typename Graph, typename VertexIndexMap>
  typename graph_traits<Graph>::vertices_size_type
  ith_bandwidth(typename graph_traits<Graph>::vertex_descriptor i,
                const Graph& g,
                VertexIndexMap index)
  {
    typedef typename graph_traits<Graph>::vertices_size_type size_type;
    size_type b = 0;
    typename graph_traits<Graph>::out_edge_iterator e, end;
    for (tie(e, end) = out_edges(i, g); e != end; ++e) {
      int f_i = get(index, i);
      int f_j = get(index, target(*e, g));
      using namespace std; // to call abs() unqualified
      if(f_i > f_j)
      b = std::max(b, size_type(f_i - f_j));
    }
    return b;
  }

  template <typename Graph>
  typename graph_traits<Graph>::vertices_size_type
  ith_bandwidth(typename graph_traits<Graph>::vertex_descriptor i,
                const Graph& g)
  {
    return ith_bandwidth(i, g, get(vertex_index, g));
  }

  template <typename Graph, typename VertexIndexMap>
  typename graph_traits<Graph>::vertices_size_type
  bandwidth(const Graph& g, VertexIndexMap index)
  {
    typename graph_traits<Graph>::vertices_size_type b = 0;
    typename graph_traits<Graph>::vertex_iterator i, end;
    for (tie(i, end) = vertices(g); i != end; ++i)
        b = std::max(b, ith_bandwidth(*i, g, index));
    return b;
  }

  template <typename Graph>
  typename graph_traits<Graph>::vertices_size_type
  bandwidth(const Graph& g)
  {
    return bandwidth(g, get(vertex_index, g));
  }

  template <typename Graph, typename VertexIndexMap>
  typename graph_traits<Graph>::vertices_size_type
  edgesum(const Graph& g, VertexIndexMap index_map)
  {
    typedef typename graph_traits<Graph>::vertices_size_type size_type;
    typedef typename detail::numeric_traits<size_type>::difference_type diff_t;
    size_type sum = 0;
    typename graph_traits<Graph>::edge_iterator i, end;
    for (tie(i, end) = edges(g); i != end; ++i) {
      diff_t f_u = get(index_map, source(*i, g));
      diff_t f_v = get(index_map, target(*i, g));
      using namespace std; // to call abs() unqualified
      sum += abs(f_u - f_v);
    }
    return sum;
  }

} // namespace boost

#endif // BOOST_GRAPH_BANDWIDTH_HPP


#include <algorithm>
#include <map>
#include <vector>

#include <boost/test/unit_test.hpp>

#include <kr/utility/random.h>

BOOST_AUTO_TEST_SUITE(RandomTest)

BOOST_AUTO_TEST_CASE(RandInt)
{
    using kr::utility::random;
    const int average = 20;
    for (std::uint32_t seed : {0, 1, 2, 37, 4096})
    {
        random r{seed};
        for (auto range : {1, 2, 8, 12, 100})
        {
            std::vector<int> counts(range, 0);
            for (int i = 0; i < range * average; ++i)
            {
                auto value = r.randint(0, range - 1);
                ++counts.at(value);
            }
            int max_variance = static_cast<int>(std::sqrt(range) * 2 + 4);
            for (int i = 0; i < range; ++i)
            {
                BOOST_CHECK_GE(counts[i], std::max(1, average - max_variance));
                BOOST_CHECK_LE(counts[i], average + max_variance + 1);
            }
        }
    }
}

BOOST_AUTO_TEST_CASE(Uniform)
{
    using kr::utility::random;
    std::map<float, float> ranges = {
        {0.0, 1.2},
        {3.0, 100.1},
        {-42, 42},
        {-110.0, 0.0}};
    for (std::uint32_t seed : {0, 1, 2, 37, 4096})
    {
        random r{seed};
        for (auto [min, max] : ranges)
        {
            auto value = r.uniform(min, max);
            BOOST_CHECK_GE(value, min);
            BOOST_CHECK_LE(value, max);
        }
    }
}

BOOST_AUTO_TEST_SUITE_END()


//
// Created by Sergej Krivonos on 25.02.18.
//
#define BOOST_TEST_MODULE System test
#define BOOST_THREAD_PROVIDES_EXECUTORS
#define BOOST_THREAD_PROVIDES_FUTURE_CONTINUATION
#define BOOST_THREAD_USES_MOVE
#include <boost/test/unit_test.hpp>

#include "System.h"
#include "VarHost.h"
#include "e.h"
#include "i.h"
#include "pi.h"

#include <array>
#include <chrono>
#include <future>
#include <iostream>     // cout, endl
#include <fstream>      // fstream
#include <thread>
#ifdef _WIN32
#include <Windows.h>
#endif

#include <boost/lexical_cast.hpp>
#include <boost/thread/shared_mutex.hpp>
#include <boost/thread/thread_pool.hpp>
#include <boost/thread/executor.hpp>
#include <boost/thread/executors/basic_thread_pool.hpp>
#include <boost/tokenizer.hpp>

template<typename TimeT = std::chrono::milliseconds>
struct measure
{
    template<typename F, typename ...Args>
    static typename TimeT::rep execution(F&& func, Args&&... args)
    {
        auto start = std::chrono::steady_clock::now();
        std::forward<decltype(func)>(func)(std::forward<Args>(args)...);
        auto duration = std::chrono::duration_cast<TimeT>(std::chrono::steady_clock::now() - start);
        return duration.count();
    }
};

using namespace omnn::math;
using namespace boost::unit_test;
using namespace std;

BOOST_AUTO_TEST_CASE(System_tests)
{
    {
        System sys;
        Variable a,b;
        Valuable t;
        t.SetView(Valuable::View::Equation);
        t = a - 8 - b;
        sys << t;
        sys << a + b - 100;
        auto s = sys.Solve(a);
        auto haveOneSolution = s.size()==1;
        BOOST_TEST(haveOneSolution);
        if(haveOneSolution)
        {
            auto _ = *s.begin();
            BOOST_TEST(_ == 54);
        }
    }

    {
        System s;
        Variable a,b;
        s << a - 8 - b;
        s << a + b - 21;
        auto _ = s.Solve(a);
        BOOST_TEST(_.size()==1);
    }
}

BOOST_AUTO_TEST_CASE(sq_System_test
                     , *disabled()
                     )
{
    System s;
    Variable a,b,x;
	s << a - b - 3
		<< a + b - x
		<< a * b * 4 - (49 - 9)
		<< x * x - 49
        << x*b + 9 +2*a*b + 3*b - 49
        ;

    auto _ = s.Solve(x);
    BOOST_TEST(_.size()==1);
	auto sqx = *_.begin();
	BOOST_TEST(sqx == 7);
}

BOOST_AUTO_TEST_CASE(ComplexSystem_test, *disabled()) // TODO :
{
    // https://github.com/ohhmm/openmind/issues/8
    // In a farm there are 100 animals consisting of cows,goats,and buffalos.each goat gives 250g of milk,each Buffalo gives 6kg of milk and if each cow gives 4kg of milk.if in total 100 animals produce 40 kg of milk how many animals of each type are present?
    // https://www.quora.com/Can-this-math-problem-be-solved
    System s;
    Variable c,g,b;
    s   << c+g+b-100
        << g*250+b*6000+c*4000-40000;

    // c,g,b are integers, see https://math.stackexchange.com/a/1598552/118612
    // TODO : try (x mod 1) or cos(2*PI*x) instead
    using namespace constant;
    s   << (e^(2*pi*i*c))-1
        << (e^(2*pi*i*g))-1
        << (e^(2*pi*i*b))-1
    ;
    auto _ = s.Solve(c);
//    auto cc = s.SolveSingleInteger(c);
//    auto gc = s.SolveSingleInteger(g);
//    auto bc = s.SolveSingleInteger(b);
//    BOOST_TEST(values);
}

BOOST_AUTO_TEST_CASE(hello_sudoku_world
                     ,*disabled()
                    )
{
    constexpr unsigned Sz = 9;
    int data[Sz][Sz] = {
        {0, 9, 8, 0, 4, 0, 0, 0, 0},
        {4, 2, 0, 0, 9, 0, 0, 8, 0},
        {0, 0, 0, 3, 0, 1, 0, 0, 0},
        {6, 3, 9, 0, 0, 8, 7, 0, 0},
        {2, 0, 4, 9, 0, 7, 3, 0, 8},
        {0, 0, 7, 5, 0, 0, 9, 2, 6},
        {0, 0, 0, 4, 0, 3, 0, 0, 0},
        {0, 6, 0, 0, 1, 0, 0, 4, 9},
        {0, 0, 0, 0, 5, 0, 8, 3, 0}
    };
    Variable value[Sz][Sz];

    auto f = 1*2*3*4*5*6*7*8*9;
    System world;
    for (auto x=Sz; x--;) {
        Valuable sumx, sumy, mulx(1), muly(1);
        for (auto y=Sz; y--; ) {
            sumx += value[x][y];
            sumy += value[y][x];
            mulx *= value[x][y];
            muly *= value[y][x];
            if (data[x][y]) {
                world << value[x][y].Equals(data[x][y]);
            }
        }
        world << sumx.Equals(45) << sumy.Equals(45)
            << mulx.Equals(f) << muly.Equals(f);
    }

    for (auto x=Sz; x--;) {
        for (auto y=Sz; y--; ) {
            auto& i = value[x][y];
            auto s = world.Solve(i);
            if (s.size()!=1) {
                IMPLEMENT
            }else{
                std::cout << *s.begin() << ' ';
            }
        }
        std::cout << std::endl;
    }
}

BOOST_AUTO_TEST_CASE(Sudoku_simplest_test
                     ,*disabled()
                     ) // solve sudoku through a system of equations though
{
    Valuable s;
    Variable x,y,v;
    constexpr unsigned Sz = 9;
    int data[Sz][Sz] = {
        {0, 9, 8, 0, 4, 0, 0, 0, 0},
        {4, 2, 0, 0, 9, 0, 0, 8, 0},
        {0, 0, 0, 3, 0, 1, 0, 0, 0},
        {6, 3, 9, 0, 0, 8, 7, 0, 0},
        {2, 0, 4, 9, 0, 7, 3, 0, 8},
        {0, 0, 7, 5, 0, 0, 9, 2, 6},
        {0, 0, 0, 4, 0, 3, 0, 0, 0},
        {0, 6, 0, 0, 1, 0, 0, 4, 9},
        {0, 0, 0, 0, 5, 0, 8, 3, 0}
    };
    Variable value[Sz][Sz];

    Valuable::optimizations = {};

    auto at = [&](const Valuable& xx, const Valuable& yy, const Valuable& vv){
        return x.Equals(xx).LogicAnd(y.Equals(yy)).LogicAnd(v.Equals(vv));
    };

    // define known data
    for(auto rowIdx = Sz; rowIdx--;){
        for(auto colIdx = Sz; colIdx--;){
            auto& i = data[rowIdx][colIdx];
            if(i)
                s += at(colIdx,rowIdx,i).sq();
        }
    }

    // define the universe
    // for simplicity check only row/col/square sum equalities to 45
    for(int xx=0; xx<=8; ++xx){
        auto sumInRow = -45_v;
        auto sumInCol = -45_v;
        auto sumInSq = -45_v;
        for(int yy=0; yy<=8; ++yy){
//            if (data[yy][xx]) {
//                <#statements#>
//            }
//            auto a = at(xx,yy,value)(value);
            sumInRow += at(xx,yy,value[xx][yy])(value[xx][yy]);
            sumInCol += at(yy,xx,value[xx][yy])(value[xx][yy]);
            auto sqn = xx;
            auto sqx = sqn%3;
            auto sqy = (sqn - sqx) / 3;
            auto insqn = yy;
            auto insqx = insqn % 3;
            auto insqy = (insqn - insqx) / 3;
            auto sqxx = sqx*3+insqx;
            auto sqyy = sqy*3+insqy;
            sumInSq += at(sqxx,sqyy,value[sqxx][sqyy])(value[sqxx][sqyy]);
        }
        s += sumInRow ^ 2;
        s += sumInCol ^ 2;
        s += sumInSq ^ 2;
    }

//    Valuable::optimizations = true;
//    std::cout << measure<>::execution([&](){
//        s.optimize();
//    }) << std::endl;
//    Valuable::optimizations = {};

    //    std::atomic<int> MaxTasks = boost::thread::hardware_concurrency();
    std::deque<std::future<void>> tasks;
    //    auto ChooseNextTask = [&tasks](){
    //
    //    };
    //    auto AddPoolTask = [](std::function<
    // solving
    auto sysMutex = std::make_shared<boost::shared_mutex>();
    for(auto rowIdx = Sz; rowIdx--;){
        for(auto colIdx = Sz; colIdx--;){
            auto& i = data[rowIdx][colIdx];
            if(!i){
                auto co = s;
                auto wasopt = Valuable::optimizations;
                Valuable::optimizations = true;
                co.eval({{x, colIdx},{y, rowIdx}});
                std::cout << co.str() << std::endl;
                co.optimize();
                Valuable::optimizations = wasopt;
                auto is = co.IntSolutions(v);
                if(is.size()==1 && is.begin()->IsInt()){
                    i = static_cast<int>(*is.begin());
                    s += at(colIdx, rowIdx, i);
                } else {
                    IMPLEMENT
                }
//                tasks.push_back(
//                                std::async([colIdx, rowIdx, sysMutex,
//                                            &s, &at, &data](){
//                    Variable find;
//                    sysMutex->lock_shared();
//                    decltype(s) localSystem = s;
//                    sysMutex->unlock_shared();
//                    localSystem << at(colIdx,rowIdx, find);
//                    auto solutions = localSystem.Solve(find);
//                    if (solutions.size()) {
//                        if (solutions.size()==1) {
//                            auto solution = *solutions.begin();
//                            if (!solution.IsInt()) {
//                                IMPLEMENT
//                            }
//                            else
//                            {
//                                data[rowIdx][colIdx] = static_cast<int>(solution);
//                                auto item = at(colIdx,rowIdx,solution);
//                                sysMutex->lock();
//                                s << item;
//                                sysMutex->unlock();
//                            }
//                        }
//                        else
//                        {
//                            // intersect into possible solutions array for this x,y
//                            auto values = 1_v;
//                            for(auto& solution: solutions){
//                                values.logic_or(at(colIdx,rowIdx,solution));
//                            }
//                            sysMutex->lock();
//                            s<<values;
//                            sysMutex->unlock();
//                        }
//                    }
//                }));
            }
        }
    }
    do {
        tasks.pop_front();
    } while (tasks.size());

    ofstream o(TEST_SRC_DIR"sudoku.txt", fstream::out);
    for(auto rowIdx = Sz; rowIdx--;){
        for(auto colIdx = Sz; colIdx--;){
            auto& i = data[colIdx][rowIdx];
            BOOST_TEST(i != 0);
            o << setw(2) << i;
        }
        o << endl;
    }
    o.close();
}

BOOST_AUTO_TEST_CASE(Sudoku_system_test
                     ,*disabled()
) // solve sudoku through a system of equations though
{
    System s;
    Variable x,y,v;
    constexpr unsigned Sz = 9;
    int data[Sz][Sz] = {
        {0, 9, 8, 0, 4, 0, 0, 0, 0},
        {4, 2, 0, 0, 9, 0, 0, 8, 0},
        {0, 0, 0, 3, 0, 1, 0, 0, 0},
        {6, 3, 9, 0, 0, 8, 7, 0, 0},
        {2, 0, 4, 9, 0, 7, 3, 0, 8},
        {0, 0, 7, 5, 0, 0, 9, 2, 6},
        {0, 0, 0, 4, 0, 3, 0, 0, 0},
        {0, 6, 0, 0, 1, 0, 0, 4, 9},
        {0, 0, 0, 0, 5, 0, 8, 3, 0}
    };

    // at
    auto at = [&](auto& xx, auto& yy, auto& vv){
        return x.Equals(xx).LogicAnd(y.Equals(yy)).LogicAnd(v.Equals(vv));
    };

    // define known data
    for(auto rowIdx = Sz; rowIdx--;){
        for(auto colIdx = Sz; colIdx--;){
            auto& i = data[rowIdx][colIdx];
            if(i)
                s << at(colIdx,rowIdx,i);
        }
    }

    // define the universe
    // for simplicity check only row/col/square sum equalities to 45
    Variable value;
    for(int xx=0; xx<=8; ++xx){
        auto sumInRow = -45_v;
        auto sumInCol = -45_v;
        auto sumInSq = -45_v;
        for(int yy=0; yy<=8; ++yy){
            //            sumInRow += at(xx,yy,value)(value);
            //            sumInCol += at(yy,xx,value)(value);
            auto sqn = xx;
            auto sqx = sqn%3;
            auto sqy = (sqn - sqx) / 3;
            auto insqn = yy;
            auto insqx = insqn % 3;
            auto insqy = (insqn - insqx) / 3;
            //            sumInSq += at(sqx*3+insqx,sqy*3+insqy,value)(value);
        }
        s << sumInRow << sumInCol << sumInSq;
    }

    //    std::atomic<int> MaxTasks = boost::thread::hardware_concurrency();
    std::deque<std::future<void>> tasks;
    //    auto ChooseNextTask = [&tasks](){
    //
    //    };
    //    auto AddPoolTask = [](std::function<
    // solving
    auto sysMutex = std::make_shared<boost::shared_mutex>();
    for(auto rowIdx = Sz; rowIdx--;){
        for(auto colIdx = Sz; colIdx--;){
            auto& i = data[rowIdx][colIdx];
            if(!i){
                tasks.push_back(
                                std::async([colIdx, rowIdx, sysMutex,
                                            &s, &at, &data](){
                    Variable find;
                    sysMutex->lock_shared();
                    decltype(s) localSystem = s;
                    sysMutex->unlock_shared();
                    localSystem << at(colIdx,rowIdx, find);
                    auto solutions = localSystem.Solve(find);
                    if (solutions.size()) {
                        if (solutions.size()==1) {
                            auto solution = *solutions.begin();
                            if (!solution.IsInt()) {
                                IMPLEMENT
                            }
                            else
                            {
                                data[rowIdx][colIdx] = static_cast<int>(solution);
                                auto item = at(colIdx,rowIdx,solution);
                                sysMutex->lock();
                                s << item;
                                sysMutex->unlock();
                            }
                        }
                        else
                        {
                            // intersect into possible solutions array for this x,y
                            auto values = 1_v;
                            for(auto& solution: solutions){
                                values.logic_or(at(colIdx,rowIdx,solution));
                            }
                            sysMutex->lock();
                            s<<values;
                            sysMutex->unlock();
                        }
                    }
                }));
            }
        }
    }
    do {
        tasks.pop_front();
    } while (tasks.size());
}

BOOST_AUTO_TEST_CASE(Sudoku_test
                     ,*disabled()
                     ) // solve sudoku through a system of equations though
{
    System s;
    Variable x,y,v;
    std::array<std::array<int, 9>, 9> data {{
        {0, 9, 8, 0, 4, 0, 0, 0, 0},
        {4, 2, 0, 0, 9, 0, 0, 8, 0},
        {0, 0, 0, 3, 0, 1, 0, 0, 0},
        {6, 3, 9, 0, 0, 8, 7, 0, 0},
        {2, 0, 4, 9, 0, 7, 3, 0, 8},
        {0, 0, 7, 5, 0, 0, 9, 2, 6},
        {0, 0, 0, 4, 0, 3, 0, 0, 0},
        {0, 6, 0, 0, 1, 0, 0, 4, 9},
        {0, 0, 0, 0, 5, 0, 8, 3, 0}
    }};

    // at
    auto at = [&](auto& xx, auto& yy, auto& vv){
        return x.Equals(xx).LogicAnd(y.Equals(yy)).LogicAnd(v.Equals(vv));
    };

    // define known data
    for(auto rowIdx = data.size(); rowIdx--;){
        for(auto colIdx = data[rowIdx].size(); colIdx--;){
            auto& i = data[rowIdx][colIdx];
            if(i)
                s << at(colIdx,rowIdx,i);
        }
    }

    // define possible values
    auto abet = [](const Variable& x) {
        auto v = 1_v;
        for(int i=1; i<10; ++i){
            v.logic_or(x.Equals(i));
        }
        return v;
    };

    // define the universe
     // each item
    for(int xx=0; xx<=8; ++xx){
        for(int yy=0; yy<=8; ++yy){
            for(int val=0; val<=8; ++val)
            {
                auto valsExceptVal = abet(v)/v.Equals(val);
                auto valAt = at(x,y,val);

                // inequality in row
                for(auto leastCorowers = xx + 1; leastCorowers <= 8; ++leastCorowers){
                    auto siblingSameVal = at(leastCorowers,y,val);
                    auto siblingExceptVal = 1_v;
                    for(int exceptVal = 0; exceptVal<=8; ++exceptVal){
                        if(exceptVal!=val)
                            siblingExceptVal.logic_or(at(leastCorowers,y,exceptVal));
                    }
                    s << valAt.Ifz(siblingExceptVal,siblingExceptVal.LogicOr(siblingSameVal));
                }

                // inequality in column
                for(auto leastCocolumners = yy + 1; leastCocolumners <= 8; ++leastCocolumners){
                    auto siblingSameVal = at(leastCocolumners,y,val);
                    auto siblingExceptVal = 1_v;
                    for(int exceptVal = 0; exceptVal<=8; ++exceptVal){
                        if(exceptVal!=val)
                            siblingExceptVal.logic_or(at(leastCocolumners,y,exceptVal));
                    }
                    s << valAt.Ifz(siblingExceptVal,siblingExceptVal.LogicOr(siblingSameVal));
                }

                // inequality in square
                auto sqxStart = xx-(xx%3);
                for(int sqx = sqxStart; sqx < sqxStart+3; ++sqx){
                    auto sqyStart = yy-(yy%3);
                    for(int sqy = sqyStart; sqy < sqyStart+3; ++sqy){
                        if(!(sqx==xx && sqy==yy)){
                            auto siblingSameVal = at(sqx,sqy,val);
                            auto siblingExceptVal = 1_v;
                            for(int exceptVal = 0; exceptVal<=8; ++exceptVal){
                                if(exceptVal!=val)
                                    siblingExceptVal.logic_or(at(sqx,sqy,exceptVal));
                            }
                            s << valAt.Ifz(siblingExceptVal,siblingExceptVal.LogicOr(siblingSameVal));
                        }
                    }
                }
            }
        }
    }

    // x,y,v are integers, see https://math.stackexchange.com/a/1598552/118612
    //    using namespace constant;
    //    s   << (e^(2*pi*i*x))-1
    //        << (e^(2*pi*i*y))-1
    //        << (e^(2*pi*i*v))-1
    //    ;

    // inequality in squares
// todo: try general using coordinatesMultiplicityTo3
//    Variable gaugeX, gaugeY;
//    auto coordinatesMultiplicityTo3 = logicAnd(gaugeX % 3, gaugeY % 3);
//    auto notEqual = [](auto& e1, auto&e2){
//IMPLEMENT
//    };
//    s << logicAnd(coordinatesMultiplicityTo3,
//            logicOr({
//                logicAnd(equality(gaugeX,x), equality(gaugeY,y))
//            })
//            )
     // inequality in squares
//first attept
//    for(int sqareStartX = 0; sqareStartX < 9; sqareStartX += 3){
//        for(int sqareStartY = 0; sqareStartY < 9; sqareStartY += 3){
//            for(int i=1; i<=9; ++i) {
//                auto valsExcept = [&](auto& va, auto& exceptValue){
//                    Valuable allItems=1_v;
//                    for(int xi = sqareStartX; xi < sqareStartX+3; ++xi){
//                        for(int yj = sqareStartY; yj < sqareStartY+3; ++yj){
//                            for(int ii = 1; ii<=9; ++ii)
//                            {
//                                if(ii!=i){
//                                    allItems = allItems.LogicOr(at(xi,yj,));
//                                } else {
//
//                                }
//                            }
//                            auto item = equals(x,xi)
//                                    .LogicAnd(equals(y,yj))
//                                    .LogicAnd(equals(v,i));
//
//                        }
//                    }
//                    return abet(va) / va.Equals(exceptValue);
//                };
//                auto valsExceptI = valsExcept(v,i);
//                s << (x.Equals(sqareStartX).LogicAnd(y.Equals(sqareStartY)))
//                    .LogicAnd();
//            }
//        }
//    }

    // solving
    for(auto rowIdx = data.size(); rowIdx--;){
        for(auto colIdx = data[rowIdx].size(); colIdx--;){
            auto& i = data[rowIdx][colIdx];
            if(!i){
                auto sysMutex = std::make_shared<boost::shared_mutex>();
                std::function<void()> addThisTask = [colIdx,rowIdx,sysMutex,&s,&at,addThisTask](){
                    std::async([colIdx,rowIdx, sysMutex, &s, &at, addThisTask](){
                        Variable find;
                        sysMutex->lock_shared();
                        decltype(s) localSystem = s;
                        sysMutex->unlock_shared();
                        localSystem << at(colIdx,rowIdx, find);
                        auto solutions = localSystem.Solve(find);
                        if (solutions.size()) {
                            if (solutions.size()==1) {
                                auto solution = *solutions.begin();
                                if (!solution.IsInt()) {
                                    IMPLEMENT
                                }
                                else
                                {
                                    auto item = at(colIdx,rowIdx,solution);
                                    sysMutex->lock();
                                    s << item;
                                    sysMutex->unlock();
                                }
                            }
                            else
                            {
                                // todo : intersect into possible solutions array for this x,y
                                for(auto& solution: solutions){

                                }
                            }
                        }
                    });
                };
            }
        }
    }

//    auto _ = s.Solve(c);
//    auto cc = s.SolveSingleInteger(c);
//    auto gc = s.SolveSingleInteger(g);
//    auto bc = s.SolveSingleInteger(b);
//    BOOST_TEST(values);
}

BOOST_AUTO_TEST_CASE(kaggle_test, *disabled())
{
    using namespace boost;
    using namespace boost::executors;
    basic_thread_pool tp;

    //TypedVarHost<std::string> vh;

    std::atomic<int> cntLines = 0;
    std::map<int, std::string> ids;
    std::string line;
    {
    ifstream in(TEST_SRC_DIR"train.csv", ifstream::in);
    if (!in.is_open())
    {
        cout << "cannot open file\n";
        return;
    }

    std::vector<Variable> v;
    System sys;
    std::mutex systemMutex;
    using namespace boost;

    if (!in.eof()) {
        in >> line; // headers
        tokenizer<escaped_list_separator<char> > tk(
            line, escaped_list_separator<char>('\\', ',', '\"'));
        for (tokenizer<escaped_list_separator<char> >::iterator i(tk.begin());
            i != tk.end(); ++i)
            v.push_back(Variable());

#ifdef _WIN32
        EnableMenuItem(
            GetSystemMenu(GetConsoleWindow(), {}),
            SC_CLOSE,
            MF_GRAYED);
#endif // _WIN32
    }

    if (!in.eof())
        ++cntLines;

    int skip = 0;
    {
        ifstream system(TEST_SRC_DIR"sys.csv", fstream::in);
        if (system.is_open())
        {
            while (!system.eof()) {
                constexpr auto Sz = 1 << 18;
                char s[Sz];
                system.getline(s, Sz);
                line = s;
                if (!line.empty())
                {
                    ++skip;
                    auto c = memchr(s, ';', Sz);
                    *(char*)c = 0;
                    ids[skip] = s;
                }
            }
            system.close();
        }
        else
        {
            cout << "cannot open file, creating new";
            ofstream(TEST_SRC_DIR"sys.csv", fstream::out).close();
            return;
        }
    }

    std::atomic<int> completedLines = skip;
    while (!in.eof()) {
        in >> line;
        if (!in.eof())
            ++cntLines;
        if (completedLines + 1 >= cntLines)
            continue;
        tp.submit([&, line]()
        {
            auto sum = std::make_shared<Sum>();
            sum->SetView(Valuable::View::Equation);
            auto sumMutex = std::make_shared<std::mutex>();

            auto cntWords = std::make_shared<std::atomic<int>>(0);
            tokenizer<escaped_list_separator<char> > tk(
                line, escaped_list_separator<char>('\\', ',', '\"'));
            int vi = 0;
            for (tokenizer<escaped_list_separator<char> >::iterator i(tk.begin());
                i != tk.end(); ++i)
            {
                tp.submit([&, sum, sumMutex, cntWords, w = *i, vi](){
                    auto tid = std::this_thread::get_id();
                    std::cout << tid << " start " << vi << std::endl;
                    const Variable& va = v[vi];
                    {
                        Valuable val;
                        if (vi) {
                            val = boost::lexical_cast<double>(w);
                        }
                        else {
                            long long l;
                            std::stringstream ss(w);
                            ss >> hex >> l;
                            val = l;
                        }
                        val = std::move((va - val).sq());

                        std::lock_guard g(*sumMutex);
                        sum->Add(val);
                    }
                    if (cntWords->fetch_add(1) + 1 == v.size())
                    {
                        sum->optimize();

                        {
                            std::lock_guard g(systemMutex);
                            sys << *sum;
                            ofstream system(TEST_SRC_DIR"sys.csv", fstream::app);
                            system << completedLines << ';' << *sum << endl;
                        }
                        std::cout << tid << " line complete " << completedLines << std::endl;
                        if (completedLines.fetch_add(1) + 1 == cntLines)
                        {
                            // save sys and start processing test lines file
                            std::cout << tid << " sys complete " << std::endl;
                        }
                    }
                    std::cout << tid << " complete " << vi << std::endl;
                });
                ++vi;
            }
        });
    }

    in.close();
    }
    tp.join();

    {
        ifstream in(TEST_SRC_DIR"train.csv", ifstream::in);
        ifstream system(TEST_SRC_DIR"sys.csv", fstream::in);
        ifstream test(TEST_SRC_DIR"test.csv", ifstream::in);

        if (!in.is_open() || !system.is_open())
        {
            cout << "cannot open file\n";
            return;
        }
        auto h = VarHost::make<std::string>();
        while (!system.eof()) {
            constexpr auto Sz = 1 << 18;
            char s[Sz];
            system.getline(s, Sz);
            line = std::string(s);
            if (!line.empty())
            {
                std::size_t sc = line.find(';');
                line = line.substr(sc + 1, line.length() - sc);
                Valuable v(line, h);
                BOOST_TEST(v.str() == line);
            }
        }
    }

    tp.join();
}


// solving A * X = B
// using driver function gesv()

//#define BOOST_NO_FUNCTION_TEMPLATE_ORDERING

#include <cstddef>
#include <iostream>
#include <boost/numeric/bindings/lapack/driver/gesv.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/std/vector.hpp>
#include <boost/numeric/ublas/io.hpp>

namespace ublas = boost::numeric::ublas;
namespace lapack = boost::numeric::bindings::lapack;
namespace bindings = boost::numeric::bindings;

using std::size_t;
using std::cout;
using std::endl;

#ifndef F_ROW_MAJOR
typedef ublas::matrix<double, ublas::column_major> m_t;
#else
typedef ublas::matrix<double, ublas::row_major> m_t;
#endif

int main() {

  cout << endl;
  size_t n = 3, nrhs = 1;

  m_t a (n, n);   // system matrix
  a(0,0) = 1.; a(0,1) = 1.; a(0,2) = 1.;
  a(1,0) = 2.; a(1,1) = 3.; a(1,2) = 1.;
  a(2,0) = 1.; a(2,1) = -1.; a(2,2) = -1.;

// see leading comments for `gesv()' in clapack.hpp
#ifndef F_ROW_MAJOR
  m_t b (n, nrhs);  // right-hand side matrix
  b(0,0) = 4.; b(1,0) = 9.; b(2,0) = -2.;
#else
  m_t b (nrhs, n);
  b(0,0) = 4.; b(0,1) = 9.; b(0,2) = -2.;
#endif

  cout << "A: " << a << endl;
  cout << "B: " << b << endl;

  std::vector< int > pivot( bindings::size1( a ) );
  lapack::gesv (a, pivot, b);
  cout << "X: " << b << endl;

  cout << endl;
}


#pragma once

#include <Eigen/Dense>

class GaussQuadrature
{
public:

    using Integrand = std::function<double(Eigen::Vector3d const&)>;
    using Domain = Eigen::AlignedBox3d;

    static double integrate(Integrand integrand, Domain const& domain, unsigned int p);
};


//  (C) Copyright Nick Thompson 2019.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_STATISTICS_LJUNG_BOX_HPP
#define BOOST_MATH_STATISTICS_LJUNG_BOX_HPP

#include <cmath>
#include <iterator>
#include <utility>
#include <boost/math/distributions/chi_squared.hpp>
#include <boost/math/statistics/univariate_statistics.hpp>

namespace boost::math::statistics {

template<class RandomAccessIterator>
auto ljung_box(RandomAccessIterator begin, RandomAccessIterator end, int64_t lags = -1, int64_t fit_dof = 0) {
    using Real = typename std::iterator_traits<RandomAccessIterator>::value_type;
    int64_t n = std::distance(begin, end);
    if (lags >= n) {
      throw std::domain_error("Number of lags must be < number of elements in array.");
    }

    if (lags == -1) {
      // This is the same default as Mathematica; it seems sensible enough . . .
      lags = static_cast<int64_t>(std::ceil(std::log(Real(n))));
    }

    if (lags <= 0) {
      throw std::domain_error("Must have at least one lag.");
    }

    auto mu = boost::math::statistics::mean(begin, end);

    std::vector<Real> r(lags + 1, Real(0));
    for (size_t i = 0; i < r.size(); ++i) {
      for (auto it = begin + i; it != end; ++it) {
        Real ak = *(it) - mu;
        Real akml = *(it-i) - mu;
        r[i] += ak*akml;
      }
    }

    Real Q = 0;

    for (size_t k = 1; k < r.size(); ++k) {
      Q += r[k]*r[k]/(r[0]*r[0]*(n-k));
    }
    Q *= n*(n+2);

    typedef boost::math::policies::policy<
          boost::math::policies::promote_float<false>,
          boost::math::policies::promote_double<false> >
          no_promote_policy;

    auto chi = boost::math::chi_squared_distribution<Real, no_promote_policy>(Real(lags - fit_dof));

    Real pvalue = 1 - boost::math::cdf(chi, Q);
    return std::make_pair(Q, pvalue);
}

template<class RandomAccessContainer>
auto ljung_box(RandomAccessContainer const & v, int64_t lags = -1, int64_t fit_dof = 0) {
    return ljung_box(v.begin(), v.end(), lags, fit_dof);
}

}
#endif


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "../util/AlgorithmUtils.hpp"
#include "../util/FluidEigenMappings.hpp"
#include "../../data/FluidIndex.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Core>

namespace fluid {
namespace algorithm {

class RatioMask
{

  using ArrayXXd = Eigen::ArrayXXd;

public:
  void init(RealMatrixView denominator)
  {
    using namespace _impl;
    using namespace Eigen;
    mMultiplier = (1 / asEigen<Array>(denominator).max(epsilon));
  }

  void process(const ComplexMatrixView& mixture, RealMatrixView targetMag,
               index exponent, ComplexMatrixView result)
  {
    using namespace _impl;
    using namespace Eigen;
    assert(mixture.cols() == targetMag.cols());
    assert(mixture.rows() == targetMag.rows());
    ArrayXXcd tmp =
        asEigen<Array>(mixture) *
        (asEigen<Array>(targetMag).pow(exponent) * mMultiplier.pow(exponent))
            .min(1.0);
    result = asFluid(tmp);
  }

private:
  ArrayXXd mMultiplier;
};

} // namespace algorithm
} // namespace fluid


#include <iostream>
#include <armadillo>

#include "utils/ImageUtils.h"
#include "utils/Benchmark.h"
#include "models/neural-network-model/discriminator/ArtDiscriminatorModel.h"
#include "models/neural-network-model/generator/ArtGeneratorModel.h"

#define LOG(x) std::cout << x << std::endl;

#define IMAGE_SIZE 64
#define INPUT_LENGTH IMAGE_SIZE * IMAGE_SIZE * 3
#define GENERATOR_HIDDEN_LAYER_NODE_COUNT 128
#define DISCRIMINATOR_HIDDEN_LAYER_NODE_COUNT 64

using namespace arma;

void testDiscriminator()
{
	ArtDiscriminatorModel* model = new ArtDiscriminatorModel(INPUT_LENGTH);
	model->addHiddenLayer(DISCRIMINATOR_HIDDEN_LAYER_NODE_COUNT);
	model->addHiddenLayer(DISCRIMINATOR_HIDDEN_LAYER_NODE_COUNT);
	model->init();

	ArtDiscriminatorModelResult* result = model->discriminateArt(randu<vec>(INPUT_LENGTH));

	delete result;
	delete model;
}

void testGenerator()
{
	ArtGeneratorModel* model = new ArtGeneratorModel(INPUT_LENGTH);
	model->addHiddenLayer(GENERATOR_HIDDEN_LAYER_NODE_COUNT);
	model->addHiddenLayer(GENERATOR_HIDDEN_LAYER_NODE_COUNT);
	model->init();

	vec result = model->calculate(randu<vec>(INPUT_LENGTH));

	delete model;
}

int main()
{
	Benchmark::run(testDiscriminator, 5, "Discriminator Test");
	Benchmark::run(testGenerator, 5, "Generator Test");

	system("PAUSE");

	return 0;
}


#include <cstdlib>
#include <iostream>
#include <complex>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/bindings/lower.hpp>
#include <boost/numeric/bindings/upper.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/blas/level3.hpp>
#include <boost/numeric/bindings/conj.hpp>
#include "print.hpp"
#include "random.hpp"

namespace ublas=boost::numeric::ublas;
namespace blas=boost::numeric::bindings::blas;

int main(int argc, char *argv[]) {
  {
    typedef std::complex<double> complex;
    typedef ublas::matrix<complex, ublas::column_major> matrix;
    typedef matrix::size_type size_type;
    rand_normal<complex>::reset();
    size_type n=8;
    matrix A(n, n), B(n, n), C(n, n);
    for (size_type j=0; j<n; ++j)
      for (size_type i=0; i<n; ++i) {
	A(i, j)=rand_normal<complex>::get();
	B(i, j)=rand_normal<complex>::get();
      }
    for (size_type j=0; j<n; ++j) {
      for (size_type i=0; i<j; ++i) {
	C(i, j)=rand_normal<complex>::get();
	C(j, i)=std::conj(C(i, j));
      }
      C(j, j)=rand_normal<complex>::get().real();
    }
    complex alpha(rand_normal<complex>::get());
    double beta(rand_normal<complex>::get().real());
    {
      matrix C1(alpha*ublas::prod(A, ublas::trans(ublas::conj(B)))+
		std::conj(alpha)*ublas::prod(B, ublas::trans(ublas::conj(A)))+
		beta*C);
      for (size_type j=0; j<n; ++j)
	for (size_type i=j+1; i<n; ++i)
	  C1(i, j)=C(i, j);
      matrix C2(C);
      blas::her2k(alpha, A, B, beta, blas::upper(C2));
      std::cout << "testing boost::ublas containers\n"
		<< "using ublas (A right htrans, C upper):\n" << print_mat(C1) << '\n'
		<< "using blas (A right htrans, C upper):\n" << print_mat(C2) << '\n'
		<< '\n';
    }
    {
      matrix C1(alpha*ublas::prod(A, ublas::trans(ublas::conj(B)))+
		std::conj(alpha)*ublas::prod(B, ublas::trans(ublas::conj(A)))+
		beta*C);
      for (size_type j=0; j<n; ++j)
    	for (size_type i=0; i<j; ++i)
    	  C1(i, j)=C(i, j);
      matrix C2(C);
      blas::her2k(alpha, A, B, beta, blas::lower(C2));
      std::cout << "testing boost::ublas containers\n"
    		<< "using ublas (A right htrans, C lower):\n" << print_mat(C1) << '\n'
    		<< "using blas (A right htrans, C lower):\n" << print_mat(C2) << '\n'
    		<< '\n';
    }
    {
      matrix C1(alpha*ublas::prod(ublas::trans(ublas::conj(A)), B)+
		std::conj(alpha)*ublas::prod(ublas::trans(ublas::conj(B)), A)+
		beta*C);
      for (size_type j=0; j<n; ++j)
    	for (size_type i=j+1; i<n; ++i)
    	  C1(i, j)=C(i, j);
      matrix C2(C);
      blas::her2k(alpha, blas::conj(A), B, beta, blas::upper(C2));
      std::cout << "testing boost::ublas containers\n"
    		<< "using ublas (A left htrans, C upper):\n" << print_mat(C1) << '\n'
    		<< "using blas (A left htrans, C upper):\n" << print_mat(C2) << '\n'
    		<< '\n';
    }
    {
      matrix C1(alpha*ublas::prod(ublas::trans(ublas::conj(A)), B)+
		std::conj(alpha)*ublas::prod(ublas::trans(ublas::conj(B)), A)+
		beta*C);
      for (size_type j=0; j<n; ++j)
    	for (size_type i=0; i<j; ++i)
    	  C1(i, j)=C(i, j);
      matrix C2(C);
      blas::her2k(alpha, blas::conj(A), B, beta, blas::lower(C2));
      std::cout << "testing boost::ublas containers\n"
    		<< "using ublas (A left htrans, C lower):\n" << print_mat(C1) << '\n'
    		<< "using blas (A left htrans, C lower):\n" << print_mat(C2) << '\n'
    		<< '\n';
    }
  }
  return EXIT_SUCCESS;
}


#include "engine_impl.hpp"
#include "rocket.hpp"

#include <boost/units/cmath.hpp>

EngineImpl::EngineImpl(Rocket& rocket_, const Body& launch_body_)
	: m_body(launch_body_)
	, m_rocket(rocket_)
	, m_state({Engine::position_t{0, launch_body_.radius + m_rocket.totalLength(), 0},
			   Engine::velocity_t{0}})
	, m_initial_position({0, launch_body_.radius, 0})
{
}

const Engine::State& EngineImpl::state() const
{
	return m_state;
}

utils::units::time EngineImpl::currentTime() const
{
	return m_current_time;
}

utils::units::length EngineImpl::altitude() const
{
	const utils::units::position_vector relative_pos = m_initial_position - m_state.position;
	return (relative_pos).norm();
}

utils::units::force EngineImpl::gravityForce() const
{
	using boost::units::si::cubic_meters;
	using boost::units::si::kilograms;
	using boost::units::si::seconds;
	static auto G = 6.67384E-11 * cubic_meters / kilograms / seconds / seconds;
	const auto vector = m_state.position; // because center of attracting body on {0, 0, 0} for now
	return G * m_rocket.currentMass() * m_body.mass / vector.norm2();
}

utils::point3d<utils::units::acceleration> EngineImpl::currentAcceleration() const
{
	const auto value = (m_rocket.currentThrust() - gravityForce()) / m_rocket.currentMass();
	return {0, value, 0};
}

void EngineImpl::tick(utils::units::time dt)
{
	m_rocket.burn(dt);
	if(m_state.position == utils::units::position_vector{0 * boost::units::si::meters})
		return;
	const auto acceleration = currentAcceleration();
	m_state.velocity += dt * acceleration;
	m_state.position += m_state.velocity * dt;
	m_current_time += dt;
}


#pragma once
#include <cmath>
#include <limits>

#include <Eigen/Dense>

namespace pcs {

template <typename Derived>
Eigen::Matrix<typename Eigen::DenseBase<Derived>::Scalar, 3, 1> hsv_to_rgb(
    const Eigen::DenseBase<Derived>& hsv) {
  using T = typename Eigen::DenseBase<Derived>::Scalar;
  T hh, p, q, t, ff;
  long i;
  T h = hsv.x();
  T s = hsv.y();
  T v = hsv.z();

  Eigen::Matrix<T, 3, 1> rgb;

  if (s == 0) {
    rgb.x() = std::round(v * 255);
    rgb.y() = std::round(v * 255);
    rgb.z() = std::round(v * 255);
    return rgb;
  }

  hh = h;
  if (hh >= 360.0) {
    hh = 0.f;
  }
  hh /= 60.0;
  i = static_cast<long>(hh);
  ff = hh - i;
  p = v * (1.0 - s);
  q = v * (1.0 - (s * ff));
  t = v * (1.0 - (s * (1.0 - ff)));

  switch (i) {
    case 0:
      rgb.x() = std::round(v * 255);
      rgb.y() = std::round(t * 255);
      rgb.z() = std::round(p * 255);
      break;
    case 1:
      rgb.x() = std::round(q * 255);
      rgb.y() = std::round(v * 255);
      rgb.z() = std::round(p * 255);
      break;
    case 2:
      rgb.x() = std::round(p * 255);
      rgb.y() = std::round(v * 255);
      rgb.z() = std::round(t * 255);
      break;
    case 3:
      rgb.x() = std::round(p * 255);
      rgb.y() = std::round(q * 255);
      rgb.z() = std::round(v * 255);
      break;
    case 4:
      rgb.x() = std::round(t * 255);
      rgb.y() = std::round(p * 255);
      rgb.z() = std::round(v * 255);
      break;
    case 5:
    default:
      rgb.x() = std::round(v * 255);
      rgb.y() = std::round(p * 255);
      rgb.z() = std::round(q * 255);
      break;
  }

  return rgb;
}

template <typename Derived>
Eigen::Matrix<typename Eigen::DenseBase<Derived>::Scalar, 3, 1> rgb_to_hsv(
    const Eigen::DenseBase<Derived>& rgb) {
  using T = typename Eigen::DenseBase<Derived>::Scalar;
  T r = rgb.x() / 255.0;
  T g = rgb.y() / 255.0;
  T b = rgb.z() / 255.0;
  Eigen::Matrix<T, 3, 1> hsv;
  T vmin, diff;
  hsv.z() = vmin = r;
  if (hsv.z() < g) hsv.z() = g;
  if (hsv.z() < b) hsv.z() = b;
  if (vmin > g) vmin = g;
  if (vmin > b) vmin = b;

  diff = hsv.z() - vmin;
  hsv.y() = diff / (std::abs(hsv.z()) + std::numeric_limits<T>::epsilon());
  diff = 60.0 / (diff + std::numeric_limits<T>::epsilon());
  if (hsv.z() == r) {
    hsv.x() = (g - b) * diff;
  } else if (hsv.z() == g) {
    hsv.x() = (b - r) * diff + 120.0;
  } else {
    hsv.x() = (r - g) * diff + 240.0;
  }

  if (hsv.x() < 0) {
    hsv.x() += 360.0;
  }

  return hsv;
}

}  // namespace pcs


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

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_LOG_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_LOG_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-exponential
    This function object returns the natural logarithm of its argument.

    @par Header <boost/simd/function/log.hpp>

    @see log10, log2, log1p

    @par Note:

    - log(x) return Nan for negative entries (peculiarly Mzero
    for floating numbers).

    @par Decorators

      - std_ for floating entries calls @c std::log

    @par Example:

      @snippet log.cpp log

    @par Possible output:

      @snippet log.txt log

  **/
  IEEEValue log(IEEEValue const& x);
} }
#endif

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

#endif


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

// Copyright (c) 2007-2013 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_EXTENSIONS_STRATEGIES_BUFFER_END_ROUND_HPP
#define BOOST_GEOMETRY_EXTENSIONS_STRATEGIES_BUFFER_END_ROUND_HPP

// Buffer strategies

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/strategies/tags.hpp>
#include <boost/geometry/strategies/side.hpp>
#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/select_most_precise.hpp>

#include <boost/geometry/extensions/strategies/buffer_side.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace buffer
{

template
<
    typename PointIn,
    typename PointOut
>
class end_round
{
    typedef typename strategy::side::services::default_strategy<typename cs_tag<PointIn>::type>::type side;
    typedef typename coordinate_type<PointOut>::type coordinate_type;

    typedef typename geometry::select_most_precise
        <
            typename geometry::select_most_precise
                <
                    typename geometry::coordinate_type<PointIn>::type,
                    typename geometry::coordinate_type<PointOut>::type
                >::type,
            double
        >::type promoted_type;

    int m_steps_per_circle;

    template <typename RangeOut>
    inline void generate_points(PointIn const& point,
                promoted_type alpha,
                promoted_type const& buffer_distance,
                RangeOut& range_out) const
    {
        promoted_type const two = 2.0;
        promoted_type const two_pi = two * geometry::math::pi<promoted_type>();

        int point_buffer_count = m_steps_per_circle;

        promoted_type const diff = two_pi / promoted_type(point_buffer_count);

        // For half circle:
        point_buffer_count /= 2;
        point_buffer_count++;

        for (int i = 0; i < point_buffer_count; i++, alpha -= diff)
        {
            typename boost::range_value<RangeOut>::type p;
            set<0>(p, get<0>(point) + buffer_distance * cos(alpha));
            set<1>(p, get<1>(point) + buffer_distance * sin(alpha));
            range_out.push_back(p);
        }
    }

    // COPIED FROM OCCUPATION_INFO
    template <typename T, typename P1, typename P2>
    static inline T calculate_angle(P1 const& from_point, P2 const& to_point)
    {
        typedef P1 vector_type;
        vector_type v = from_point;
        geometry::subtract_point(v, to_point);
        return atan2(geometry::get<1>(v), geometry::get<0>(v));
    }

public :
    inline end_round(int steps_per_circle = 100)
        : m_steps_per_circle(steps_per_circle)
    {}

    template <typename RangeOut, typename DistanceStrategy>
    inline void apply(PointIn const& penultimate_point,
                PointIn const& perp_left_point,
                PointIn const& ultimate_point,
                PointIn const& perp_right_point,
                buffer_side_selector side,
                DistanceStrategy const& distance,
                RangeOut& range_out) const
    {
        promoted_type alpha = calculate_angle<promoted_type>(perp_left_point, ultimate_point);

        promoted_type const dist_left = distance.apply(penultimate_point, ultimate_point, buffer_side_left);
        promoted_type const dist_right = distance.apply(penultimate_point, ultimate_point, buffer_side_right);
        if (geometry::math::equals(dist_left, dist_right))
        {
            generate_points(ultimate_point, alpha, dist_left, range_out);
        }
        else
        {
            promoted_type const two = 2.0;
            promoted_type dist_half_diff = (dist_left - dist_right) / two;

            if (side == buffer_side_right)
            {
                dist_half_diff = -dist_half_diff;
            }

            PointIn shifted_point;
            set<0>(shifted_point, get<0>(ultimate_point) + dist_half_diff * cos(alpha));
            set<1>(shifted_point, get<1>(ultimate_point) + dist_half_diff * sin(alpha));
            generate_points(shifted_point, alpha, (dist_left + dist_right) / two, range_out);
        }
    }

    static inline piece_type get_piece_type()
    {
        return buffered_round_end;
    }
};

}} // namespace strategy::buffer

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_STRATEGIES_BUFFER_END_ROUND_HPP


/* I was unable to find good information on how the PLL dividers work
in the ECP5 PLL Design and Usage Guide, so I ran several frequencies
through Lattice's clarity designer, and was surprised with what I
found:
| Input | Output | refclk | feedback | output | fvco |
|    12 |     48 |      1 |        4 |     12 |  576 |
|    12 |     60 |      1 |        5 |     10 |  600 |
|    20 |     30 |      2 |        3 |     20 |  600 |
|    45 |     30 |      3 |        2 |     20 |  600 |
|   100 |    400 |      1 |        4 |      1 |  400 |
|   200 |    400 |      1 |        2 |      2 |  800 |
|    50 |    400 |      1 |        8 |      2 |  800 |
|    70 |     40 |      7 |        4 |     15 |  600 |
|    12 |     36 |      1 |        3 |     18 |  648 |
|    12 |     96 |      1 |        8 |      6 |  576 |
|    90 |     40 |      9 |        4 |     15 |  600 |
|    90 |     50 |      9 |        5 |     13 |  650 |
|    43 |     86 |      1 |        2 |      7 |  602 |

it appears that
f_pfd = f_in/refclk
f_vco = f_pfd * feedback * output
f_out = f_vco / output
 */

#define INPUT_MIN 8.0f
#define INPUT_MAX 400.0f
#define OUTPUT_MIN 10.0f
#define OUTPUT_MAX 400.0f
#define PFD_MIN 3.125f
#define PFD_MAX 400.0f
#define VCO_MIN 400.0f
#define VCO_MAX 800.0f
#include <iostream>
#include <limits>
#include <fstream>
#include <boost/program_options.hpp>
using namespace std;

enum class pll_mode{
  SIMPLE,
  HIGHRES
};

struct secondary_params{
  bool enabled;
  int div;
  int cphase;
  int fphase;

  float freq;
  float phase;
};

struct pll_params{
  pll_mode mode;
  int refclk_div;
  int feedback_div;
  int output_div;
  int primary_cphase;

  secondary_params secondary[3];

  float fout;
  float fvco;

  pll_params() :mode(pll_mode::SIMPLE) {
    for(int i=0;i<3;i++){
      secondary[i].enabled = false;
      primary_cphase = 9;
    }
  }
};

pll_params calc_pll_params(float input, float output);
pll_params calc_pll_params_highres(float input, float output);
void generate_secondary_output(pll_params &params, int channel, float frequency, float phase);
void write_pll_config(pll_params params, const char* name, ofstream& file);

int main(int argc, char** argv){
  namespace po = boost::program_options;
  po::options_description options("Allowed options");

  options.add_options()("help,h", "show help");
  options.add_options()("input,i", po::value<float>(), "Input frequency in MHz");
  options.add_options()("output,o", po::value<float>(), "Output frequency in MHz");
  options.add_options()("s1", po::value<float>(), "Secondary Output frequency in MHz");
  options.add_options()("p1", po::value<float>()->default_value(0), "Secondary Output phase in degrees");
  options.add_options()("s2", po::value<float>(), "Secondary Output(2) frequency in MHz");
  options.add_options()("p2", po::value<float>()->default_value(0), "Secondary Output(2) phase in degrees");
  options.add_options()("s3", po::value<float>(), "Secondary Output(3) frequency in MHz");
  options.add_options()("p3", po::value<float>()->default_value(0), "Secondary Output(3) phase in degrees");
  options.add_options()("file,f", po::value<string>(), "Output to file");
  options.add_options()("highres", "Use secondary PLL output for higher frequency resolution");

  po::variables_map vm;
  po::parsed_options parsed = po::command_line_parser(argc, argv).options(options).run();
  po::store(parsed, vm);
  po::notify(vm);

  if(vm.count("help")){
    cerr << "Project Trellis - Open Source Tools for ECP5 FPGAs" << endl;
    cerr << "ecppll: ECP5 PLL Configuration Calculator" << endl;
    cerr << endl;
    cerr << "This tool is experimental! Use at your own risk!" << endl;
    cerr << endl;
    cerr << "Copyright (C) 2018-2019 David Shah <david@symbioticeda.com>" << endl;
    cerr << endl;
    cerr << options << endl;
    return 1;
  }
  if(vm.count("input") != 1 || vm.count("output") != 1){
    cerr << "Error: missing input or output frequency!\n";
    return 1;
  }
  float inputf = vm["input"].as<float>();
  float outputf = vm["output"].as<float>();
  if(inputf < INPUT_MIN || inputf > INPUT_MAX){
    cerr << "Warning: Input frequency " << inputf << "MHz not in range (" << INPUT_MIN << "MHz, " << INPUT_MAX << "MHz)\n";
  }
  if(outputf < OUTPUT_MIN || outputf > OUTPUT_MAX){
    cerr << "Warning: Output frequency " << outputf << "MHz not in range (" << OUTPUT_MIN << "MHz, " << OUTPUT_MAX << "MHz)\n";
  }
  pll_params params;
  if(vm.count("highres")){
    if(vm.count("s1") > 0){
      cerr << "Cannot specify secondary frequency in highres mode\n";
    }
    params = calc_pll_params_highres(inputf, outputf);
  }
  else{
    params = calc_pll_params(inputf, outputf);
    if(vm.count("s1"))
      generate_secondary_output(params, 0, vm["s1"].as<float>(), vm["p1"].as<float>());
    if(vm.count("s2"))
      generate_secondary_output(params, 1, vm["s2"].as<float>(), vm["p2"].as<float>());
    if(vm.count("s3"))
      generate_secondary_output(params, 2, vm["s3"].as<float>(), vm["p3"].as<float>());

  }

  cout << "Pll parameters:" << endl;
  cout << "Refclk divisor: " << params.refclk_div << endl;
  cout << "Feedback divisor: " << params.feedback_div << endl;
  cout << "Output divisor: " << params.output_div << endl;
  if(params.secondary[0].enabled){
    cout << "Secondary divisor: " << params.secondary[0].div << endl;
    cout << "Secondary freq: " << params.secondary[0].freq << endl;
    cout << "Secondary phase shift: " << params.secondary[0].phase << endl;
  }
  if(params.secondary[1].enabled){
    cout << "Secondary(2) divisor: " << params.secondary[1].div << endl;
    cout << "Secondary(2) freq: " << params.secondary[1].freq << endl;
    cout << "Secondary(2) phase shift: " << params.secondary[1].phase << endl;
  }
  if(params.secondary[2].enabled){
    cout << "Secondary(3) divisor: " << params.secondary[2].div << endl;
    cout << "Secondary(3) freq: " << params.secondary[2].freq << endl;
    cout << "Secondary(3) phase shift: " << params.secondary[2].phase << endl;
  }
  cout << "VCO frequency: " << params.fvco << endl;
  cout << "Output frequency: " << params.fout << endl;
  if(vm.count("file")){
    ofstream f;

    f.open(vm["file"].as<string>().c_str());

    write_pll_config(params, "pll", f);

    f.close();
  }

}

pll_params calc_pll_params(float input, float output){
  float error = std::numeric_limits<float>::max();
  pll_params params;
  for(int input_div=1;input_div <= 128; input_div++){

    float fpfd = input / (float)input_div;
    if(fpfd < PFD_MIN || fpfd > PFD_MAX)
      continue;
    for(int feedback_div=1;feedback_div <= 80; feedback_div++){
      for(int output_div=1;output_div <= 128; output_div++){
	float fvco = fpfd * (float)feedback_div * (float) output_div;

	if(fvco < VCO_MIN || fvco > VCO_MAX)
	  continue;

	float fout = fvco / (float) output_div;
	if(fabsf(fout - output) < error ||
	   (fabsf(fout-output) == error && fabsf(fvco - 600) < fabsf(params.fvco - 600))){
	  error = fabsf(fout-output);
	  params.refclk_div = input_div;
	  params.feedback_div = feedback_div;
	  params.output_div = output_div;
	  params.fout = fout;
	  params.fvco = fvco;

	  // shift the primary by 180 degrees. Lattice seems to do this
	  float ns_phase = 1/(fout * 1e6) * 0.5;
	  params.primary_cphase = ns_phase * (fvco * 1e6);

	}

      }
    }
  }
  return params;
}

pll_params calc_pll_params_highres(float input, float output){
  float error = std::numeric_limits<float>::max();
  pll_params params;
  for(int input_div=1;input_div <= 128; input_div++){

    float fpfd = input / (float)input_div;
    if(fpfd < PFD_MIN || fpfd > PFD_MAX)
      continue;
    for(int feedback_div=1;feedback_div <= 80; feedback_div++){
      for(int output_div=1;output_div <= 128; output_div++){
	float fvco = fpfd * (float)feedback_div * (float) output_div;

	if(fvco < VCO_MIN || fvco > VCO_MAX)
	  continue;
	float ffeedback = fvco / (float) output_div;
	if(ffeedback < OUTPUT_MIN || ffeedback > OUTPUT_MAX)
	  continue;
	for(int secondary_div = 1; secondary_div <= 128; secondary_div++){
	  float fout = fvco / (float) secondary_div;
	  if(fabsf(fout - output) < error ||
	     (fabsf(fout-output) == error && fabsf(fvco - 600) < fabsf(params.fvco - 600))){
	    error = fabsf(fout-output);
	    params.mode = pll_mode::HIGHRES;
	    params.refclk_div = input_div;
	    params.feedback_div = feedback_div;
	    params.output_div = output_div;
	    params.secondary[0].div = secondary_div;
	    params.secondary[0].enabled = true;
	    params.secondary[0].freq = fout;
	    params.fout = fout;
	    params.fvco = fvco;

	  }
	}

      }
    }
  }
  return params;
}

void generate_secondary_output(pll_params &params, int channel, float frequency, float phase){
  int div = params.fvco/frequency;
  float freq = params.fvco/div;
  cout << "sdiv " << div << endl;

  float ns_shift = 1/(freq * 1e6) * phase /  360.0;
  float phase_count = ns_shift * (params.fvco * 1e6);
  int cphase = (int) phase_count;
  int fphase = (int) ((phase_count - cphase) * 8);

  float ns_actual = 1/(params.fvco * 1e6) * (cphase + fphase/8.0);
  float phase_shift = 360 * ns_actual/ (1/(freq * 1e6));

  params.secondary[channel].enabled = true;
  params.secondary[channel].div = div;
  params.secondary[channel].freq = freq;
  params.secondary[channel].phase = phase_shift;
  params.secondary[channel].cphase = cphase + params.primary_cphase;
  params.secondary[channel].fphase = fphase;

}

void write_pll_config(pll_params params, const char* name,  ofstream& file){
  file << "module " << name << "(input clki, \n";
  for(int i=0;i<3;i++){
    if(!(i==0 && params.mode == pll_mode::HIGHRES) && params.secondary[i].enabled){
      file << "    output clks" << i+1 <<",\n";
    }
  }
  file << "    output locked,\n";
  file << "    output clko\n";
  file << ");\n";
  file << "wire clkfb;\n";
  file << "wire clkos;\n";
  file << "wire clkop;\n";
  file << "(* ICP_CURRENT=\"12\" *) (* LPF_RESISTOR=\"8\" *) (* MFG_ENABLE_FILTEROPAMP=\"1\" *) (* MFG_GMCREF_SEL=\"2\" *)\n";
  file << "EHXPLLL #(\n";
  file << "        .PLLRST_ENA(\"DISABLED\"),\n";
  file << "        .INTFB_WAKE(\"DISABLED\"),\n";
  file << "        .STDBY_ENABLE(\"DISABLED\"),\n";
  file << "        .DPHASE_SOURCE(\"DISABLED\"),\n";
  file << "        .CLKOP_FPHASE(0),\n";
  file << "        .CLKOP_CPHASE(" << params.primary_cphase << "),\n";
  file << "        .OUTDIVIDER_MUXA(\"DIVA\"),\n";
  file << "        .CLKOP_ENABLE(\"ENABLED\"),\n";
  file << "        .CLKOP_DIV(" << params.output_div << "),\n";
  if(params.secondary[0].enabled){
    file << "        .CLKOS_ENABLE(\"ENABLED\"),\n";
    file << "        .CLKOS_DIV(" << params.secondary[0].div << "),\n";
    file << "        .CLKOS_CPHASE(" << params.secondary[0].cphase << "),\n";
    file << "        .CLKOS_FPHASE(" << params.secondary[0].fphase << "),\n";
  }
  if(params.secondary[1].enabled){
    file << "        .CLKOS2_ENABLE(\"ENABLED\"),\n";
    file << "        .CLKOS2_DIV(" << params.secondary[1].div << "),\n";
    file << "        .CLKOS2_CPHASE(" << params.secondary[1].cphase << "),\n";
    file << "        .CLKOS2_FPHASE(" << params.secondary[1].fphase << "),\n";
  }
  if(params.secondary[2].enabled){
    file << "        .CLKOS3_ENABLE(\"ENABLED\"),\n";
    file << "        .CLKOS3_DIV(" << params.secondary[2].div << "),\n";
    file << "        .CLKOS3_CPHASE(" << params.secondary[2].cphase << "),\n";
    file << "        .CLKOS3_FPHASE(" << params.secondary[2].fphase << "),\n";
  }
  file << "        .CLKFB_DIV(" << params.feedback_div << "),\n";
  file << "        .CLKI_DIV(" << params.refclk_div <<"),\n";
  file << "        .FEEDBK_PATH(\"INT_OP\")\n";
  file << "    ) pll_i (\n";
  file << "        .CLKI(clki),\n";
  file << "        .CLKFB(clkfb),\n";
  file << "        .CLKINTFB(clkfb),\n";
  file << "        .CLKOP(clkop),\n";
  if(params.secondary[0].enabled){
    if(params.mode == pll_mode::HIGHRES)
      file << "        .CLKOS(clkos),\n";
    else
      file << "        .CLKOS(clks1),\n";

  }
  if(params.secondary[1].enabled){
    file << "        .CLKOS2(clks2),\n";
  }
  if(params.secondary[2].enabled){
    file << "        .CLKOS3(clks3),\n";
  }
  file << "        .RST(1'b0),\n";
  file << "        .STDBY(1'b0),\n";
  file << "        .PHASESEL0(1'b0),\n";
  file << "        .PHASESEL1(1'b0),\n";
  file << "        .PHASEDIR(1'b0),\n";
  file << "        .PHASESTEP(1'b0),\n";
  file << "        .PLLWAKESYNC(1'b0),\n";
  file << "        .ENCLKOP(1'b0),\n";
  file << "        .LOCK(locked)\n";
  file << "	);\n";
  if(params.mode == pll_mode::SIMPLE){
    file << "assign clko = clkop;\n";
  }
  else {
    file << "assign clko = clkos;\n";
  }
  file << "endmodule\n";

}


//
//  Copyright (c) 2000-2002
//  Joerg Walter, Mathias Koch
//
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//
//  The authors gratefully acknowledge the support of
//  GeNeSys mbH & Co. KG in producing this work.
//

#include <boost/numeric/ublas/hermitian.hpp>
#include <boost/numeric/ublas/io.hpp>

int main () {
    using namespace boost::numeric::ublas;
    matrix<std::complex<double> > m (3, 3);
    hermitian_adaptor<matrix<std::complex<double> >, lower> hal (m);
    for (unsigned i = 0; i < hal.size1 (); ++ i) {
        for (unsigned j = 0; j < i; ++ j)
            hal (i, j) = std::complex<double> (3 * i + j, 3 * i + j);
        hal (i, i) = std::complex<double> (4 * i, 0);
    }
    std::cout << hal << std::endl;
    hermitian_adaptor<matrix<std::complex<double> >, upper> hau (m);
    for (unsigned i = 0; i < hau.size1 (); ++ i) {
        hau (i, i) = std::complex<double> (4 * i, 0);
        for (unsigned j = i + 1; j < hau.size2 (); ++ j)
            hau (i, j) = std::complex<double> (3 * i + j, 3 * i + j);
    }
    std::cout << hau << std::endl;
}


#ifndef _NCTX_PY_PTHS_
#define _NCTX_PY_PTHS_
//~ #include <boost/graph/property_maps/constant_property_map.hpp>
//~ #include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/dijkstra_shortest_paths_no_color_map.hpp>

namespace nctx { namespace python {
  template<bool d>
  class SpDummy{};

  template <bool directed>
  inline py::list build_path( typename GraphContainer<directed>::vertex start,
                          typename GraphContainer<directed>::vertex goal,
                          PropertyMapValueHolder<size_t,directed>& predecessors,
                          size_t pred_undef){
    using GraphC = GraphContainer<directed>;
    using Vertex = typename GraphC::vertex;
    py::list path;
    // extract path
    Vertex current = goal;
    //~ std::cout << "extract path: " << start << " -> " << goal << std::endl;
    do {
        auto const pred = predecessors.get_elem(current);

        //~ std::cout << "extract path: " << current << " <- " << pred << " path length now " << py::len(path) << std::endl;

        if(pred == pred_undef || current == pred)
            break;

        current = pred;

        path.append(current);
    } while(current != start);

    if(py::len(path) > 1){
      path.reverse();
      path.append(goal);
    }
    return path;
  }

  template <bool directed>
  inline void wrap_pathalgs(){
    using PMap = PropertyMapValueHolder<size_t,directed>;
    using PMatrix = PropertyMapVectorHolder<size_t, directed>;
    using GraphC = GraphContainer<directed>;
    using Vertex = typename GraphC::vertex;
    using Edge = typename GraphC::edge;

    std::string apsp_doc = std::string("Obtain all-pairs-shortest-paths using Dijkstra's algorithm taking into account contextual constraints.\n\nThis is basically a wrapper around :py:func:`dijkstra_ctx <nctx.") + (directed ? std::string(""):std::string("un")) + std::string("directed.AlgPaths.dijkstra_ctx>`. Please see the mentioned function for more information regarding the contextual constraints.\n\nArgs:\n    g (Graph): The Graph object\n    decision_fct (function): The decision function enforcing contextual constraints. The signature of the function is ``(vertex index, vertex index, vertex index) -> Bool``\n     distances (PropertyMapVecULong): matrix in which distances will be written\n\nExample:\n    >>> distmap = PropertyMapVecULong(g, 'distances_ctx')\n    >>> AlgPaths.dijkstra_apsp_ctx(g,lambda _start,_current,_next: (True), distmap)\n\n\n");
    std::string apsp_ret_doc = std::string("Obtain all-pairs-shortest-paths using Dijkstra's algorithm taking into account contextual constraints.\n\nThis is basically a wrapper around :py:func:`dijkstra_ctx <nctx.") + (directed ? std::string(""):std::string("un")) + std::string("directed.AlgPaths.dijkstra_ctx>`. Please see the mentioned function for more information regarding the contextual constraints.\n\nArgs:\n    g (Graph): The Graph object\n    decision_fct (function): The decision function enforcing contextual constraints. The signature of the function is ``(vertex index, vertex index, vertex index) -> Bool``\n\nReturns:\n    PropertyMapVecULong: matrix of distances named apsp_constraint\n\nExample:\n    >>> distmap = AlgPaths.dijkstra_apsp_ctx(g,lambda _start,_current,_next: (True))\n\n\n");

    std::string fpath_doc = std::string("Obtain a shortest paths from a start vertex to a target vertex using Dijkstra's algorithm taking into account contextual constraints.\n\nThis is basically a wrapper around :py:func:`dijkstra_ctx <nctx.") + (directed ? std::string(""):std::string("un")) + std::string("directed.AlgPaths.dijkstra_ctx>`. Please see the mentioned function for more information regarding the contextual constraints.\n\nArgs:\n    g (Graph): The Graph object\n    decision_fct (function): The decision function enforcing contextual constraints. The signature of the function is ``(vertex index, vertex index, vertex index) -> Bool``\n     start (int): Start vertex\n    target (int): Target vertex\n\nReturns:\n    list: A list of vertex indices describing the shortest path from start to target.\n\nExample:\n    >>> start = 3\n    >>> target = 10\n    >>> path = AlgPaths.find_path_ctx(g,start,target,lambda _start,_current,_next: (True))\n\n\n");

    py::class_<SpDummy<directed>>("AlgPaths", py::no_init)
      .def("dijkstra_ctx", +[](GraphC& gc, Vertex start, py::object fct_decision, PMap& distances) {
        std::function<bool (Vertex, Vertex, Vertex)> lmbd_decision = lambda_wrapper_t<bool, Vertex, Vertex, Vertex>( fct_decision );
        const size_t dist_inf = (std::numeric_limits<size_t>::max)();
        auto g = gc.get_graph();
        auto weight_map = boost::make_constant_property<Edge>(1.0);
        auto dummy_pred = b::dummy_property_map();
        dijkstra_ctx_dispatch(g, start,
                              b::weight_map(weight_map)
                              .distance_map(distances.get_map()),
                              distances.get_map(),
                              weight_map,
                              dummy_pred,
                              0,
                              lmbd_decision,
                              dist_inf);
      },(py::arg("g"), py::arg("s"), py::arg("decision_fct"), py::arg("distances")), "Obtain shortest paths with dynamic contextual constraints.\n\n\n\nSingle-source shortest paths using Dijkstra's algorithm taking into account contextual constraints. Enforcement of constraints is the task of the given user-defined function.\n\nThe function enforcing contextual constraints is evaluated at each node during shortest path discovery. The function needs to evaluate to ``true`` or ``false`` allowing an edge to be visited or not. As parameters, the function needs to accept the starting node of path traversal, the current node, and the descending node in question. If the function returns False, the descending node is not being visited. Passing the start vertex is unnecessary here. However, the signature is the same as for the other functions for usability reasons.\n\nThe three nodes are passed as indices allowing for access of (external) attribute and other associated information.\n\nNote that the decision function is evaluated more than once during path traversal. That means, there should not happen any resource-intense computation inside this function. Also, it does not allow to keep track of the status of calculation, e.g. by calculating the visited edges or something similar.\n\nIf the decision function simply returns True all the time, the set of shortest paths is the unaltered set of shortest paths expected by the classical Dijkstra-implementation.\n\n\n\nArgs:\n    g The graph object\n    s The source vertex\n    decision_fct A function enforcing constraints. The signature of the function is ``(vertex index, vertex index, vertex index) -> bool``, i.e. the function expects three vertex IDs for the start, the current, and the next vertex. It must evaluate to ``bool``. Passing the start vertex is unnecessary here. However, the signature is the same as for the other functions for usability reasons.\n    distances (PropertyMapULong): map in which distances will be written\n\nExample:\n    >>> start = 3\n    >>> distmap = PropertyMapULong(g, 'distances_ctx')\n    >>> AlgPaths.dijkstra_ctx(g, start, lambda _start,_current,_next: (True), distmap)\n\n\n")

      .def("dijkstra_ctx", +[](GraphC& gc, Vertex start, py::object fct_decision) -> PMap {
        std::function<bool (Vertex, Vertex, Vertex)> lmbd_decision = lambda_wrapper_t<bool, Vertex, Vertex, Vertex>( fct_decision );
        const size_t dist_inf = (std::numeric_limits<size_t>::max)();
        auto g = gc.get_graph();
        PMap distances(gc, "dijkstra_constraint");
        auto weight_map = boost::make_constant_property<Edge>(1.0);
        auto dummy_pred = b::dummy_property_map();
        dijkstra_ctx_dispatch(g, start,
                              b::weight_map(weight_map)
                              .distance_map(distances.get_map()),
                              distances.get_map(),
                              weight_map,
                              dummy_pred,
                              0,
                              lmbd_decision,
                              dist_inf);
        return distances;
      },(py::arg("g"), py::arg("s"), py::arg("decision_fct")), "Obtain shortest paths with dynamic contextual constraints.\n\n\n\nSingle-source shortest paths using Dijkstra's algorithm taking into account contextual constraints. Enforcement of constraints is the task of the given user-defined function.\n\nThe function enforcing contextual constraints is evaluated at each node during shortest path discovery. The function needs to evaluate to ``true`` or ``false`` allowing an edge to be visited or not. As parameters, the function needs to accept the starting node of path traversal, the current node, and the descending node in question. If the function returns False, the descending node is not being visited. Passing the start vertex is unnecessary here. However, the signature is the same as for the other functions for usability reasons.\n\nThe three nodes are passed as indices allowing for access of (external) attribute and other associated information.\n\nNote that the decision function is evaluated more than once during path traversal. That means, there should not happen any resource-intense computation inside this function. Also, it does not allow to keep track of the status of calculation, e.g. by calculating the visited edges or something similar.\n\nIf the decision function simply returns True all the time, the set of shortest paths is the unaltered set of shortest paths expected by the classical Dijkstra-implementation.\n\n\n\nArgs:\n    g The graph object\n    s The source vertex\n    decision_fct A function enforcing constraints. The signature of the function is ``(vertex index, vertex index, vertex index) -> bool``, i.e. the function expects three vertex IDs for the start, the current, and the next vertex. It must evaluate to ``bool``. Passing the start vertex is unnecessary here. However, the signature is the same as for the other functions for usability reasons.\n\n\nReturns:\n    PropertyMapULong: map containing distances named dijkstra_constraint\n\nExample:\n    >>> start = 3\n    >>> distmap = AlgPaths.dijkstra_ctx(g, start, lambda _start,_current,_next: (True))\n\n\n")

      .def("dijkstra_apsp_ctx", +[](GraphC& gc, py::object fct_decision, PMatrix& dmap) {
        std::function<bool (Vertex, Vertex, Vertex)> lmbd_decision = lambda_wrapper_t<bool, Vertex, Vertex, Vertex>( fct_decision );
        const size_t dist_inf = (std::numeric_limits<size_t>::max)();
        auto g = gc.get_graph();
        auto weight_map = boost::make_constant_property<Edge>(1.0);

        for(auto start : make_iterator_range(vertices(g))){
          std::vector<size_t> distances(b::num_vertices(g), 0);
          auto distance_map = make_iterator_property_map(distances.begin(), b::get(b::vertex_index, g));
          auto dummy_pred = b::dummy_property_map();
          dijkstra_ctx_dispatch(g, start,
                                b::weight_map(weight_map)
                                .distance_map(distance_map),
                                distance_map,
                                weight_map,
                                dummy_pred,
                                0,
                                lmbd_decision,
                                dist_inf);

          for(auto u : make_iterator_range(b::vertices(g)))
            dmap.set_elem(start,u,distances[u]);
        }
      },(py::arg("g"), py::arg("decision_fct"), py::arg("distances")), apsp_doc.c_str())

      .def("dijkstra_apsp_ctx", +[](GraphC& gc, py::object fct_decision) -> PMatrix {
        std::function<bool (Vertex, Vertex, Vertex)> lmbd_decision = lambda_wrapper_t<bool, Vertex, Vertex, Vertex>( fct_decision );
        const size_t dist_inf = (std::numeric_limits<size_t>::max)();
        auto g = gc.get_graph();
        auto weight_map = boost::make_constant_property<Edge>(1.0);
        PMatrix dmap(gc, "apsp_constraint");
        auto dummy_pred = b::dummy_property_map();

        for(auto start : make_iterator_range(vertices(g))){
          std::vector<size_t> distances(b::num_vertices(g), 0);
          auto distance_map = make_iterator_property_map(distances.begin(), b::get(b::vertex_index, g));
          dijkstra_ctx_dispatch(g, start,
                                b::weight_map(weight_map)
                                .distance_map(distance_map),
                                distance_map,
                                weight_map ,
                                dummy_pred,
                                0,
                                lmbd_decision,
                                dist_inf);

          for(auto u : make_iterator_range(b::vertices(g)))
            dmap.set_elem(start,u,distances[u]);
        }
        return dmap;
      },(py::arg("g"), py::arg("decision_fct")), apsp_ret_doc.c_str())

      .def("find_path_ctx", +[](GraphC& gc, Vertex start, Vertex goal, py::object fct_decision) -> py::list {
        std::function<bool (Vertex, Vertex, Vertex)> lmbd_decision = lambda_wrapper_t<bool, Vertex, Vertex, Vertex>( fct_decision );
        const size_t dist_inf = (std::numeric_limits<size_t>::max)();
        const int pred_init = (std::numeric_limits<int>::min)();
        auto g = gc.get_graph();
        PMap distances(gc, "dijkstra_constraint");
        PMap predecessors(gc, "predecessors_constraint", pred_init);
        auto weight_map = boost::make_constant_property<Edge>(1.0);

        dijkstra_ctx_dispatch(g, start,
                                b::weight_map(weight_map)
                                .distance_map(distances.get_map())
                                .predecessor_map(predecessors.get_map()),
                                distances.get_map(),
                                weight_map,
                                predecessors.get_map(),
                                pred_init,
                                lmbd_decision,
                                dist_inf);

        // extract path
        return build_path(start, goal, predecessors, pred_init);
      },(py::arg("g"), py::arg("start"), py::arg("target"), py::arg("decision_fct")), fpath_doc.c_str())

      .def("dijkstra", +[](GraphC& gc, Vertex start, PMap& distances) {
        auto g = gc.get_graph();
        auto weight_map = boost::make_constant_property<Edge>(1.0);
        boost::dijkstra_shortest_paths_no_color_map(g, start,
                                b::weight_map(weight_map)
                                .distance_map(distances.get_map()));
      },(py::arg("g"), py::arg("start"), py::arg("distances")), "Conventional single-source shortest paths discovery using Dijkstra's algorithm.\n\nArgs:\n    g (Graph): The Graph object\n    start (int): Source vertex\n    distances (PropertyMapULong): PropertyMap in which distances will be written.\n")

      .def("dijkstra", +[](GraphC& gc, Vertex start) -> PMap {
        auto g = gc.get_graph();
        PMap distances(gc, "dijkstra");
        auto weight_map = boost::make_constant_property<Edge>(1.0);
        boost::dijkstra_shortest_paths_no_color_map(g, start,
                                b::weight_map(weight_map)
                                .distance_map(distances.get_map()));
        return distances;
      },(py::arg("g"), py::arg("start")), "Conventional single-source shortest paths discovery using Dijkstra's algorithm.\n\nArgs:\n    g (Graph): The Graph object\n    start (int): Source vertex\n\nReturns:\n    PropertyMapULong: PropertyMap with distances.\n")

      .def("find_path", +[](GraphC& gc, Vertex start, Vertex goal) -> py::list {
        auto g = gc.get_graph();
        const int pred_init = (std::numeric_limits<int>::min)();
        PMap distances(gc, "dijkstra");
        PMap predecessors(gc, "predecessors", pred_init);
        auto weight_map = boost::make_constant_property<Edge>(1.0);

        boost::dijkstra_shortest_paths_no_color_map(g, start,
                                b::weight_map(weight_map)
                                .distance_map(distances.get_map())
                                .predecessor_map(predecessors.get_map()));

        return build_path(start, goal, predecessors, pred_init);
      }, (py::arg("g"), py::arg("start"), py::arg("target")), "Conventional shortest path discovery using Dijkstra's algorithm.\n\nArgs:\n    g (Graph): The Graph object\n    start (int): Source vertex\n    target (int): Target vertex\n\nReturns:\n    list: A list of vertex indices describing the shortest path from start to target.\n");
  }

}} //nproc::python

#endif


// Copyright (C) 2015 National ICT Australia (NICTA)
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.
// -------------------------------------------------------------------
//
// Written by Conrad Sanderson - http://conradsanderson.id.au

#include <armadillo>
#include "catch.hpp"

using namespace arma;

TEST_CASE("fn_all_1")
  {
  vec a(5, fill::zeros);
  vec b(5, fill::zeros);  b(0) = 1.0;
  vec c(5, fill::ones );

  REQUIRE( all(a) == false);
  REQUIRE( all(b) == false);
  REQUIRE( all(c) == true );

  REQUIRE( all(a(span::all)) == false);
  REQUIRE( all(b(span::all)) == false);
  REQUIRE( all(c(span::all)) == true );

  REQUIRE( all(  c -  c) == false);
  REQUIRE( all(2*c -2*c) == false);

  REQUIRE( all(c < 0.5) == false);
  REQUIRE( all(c > 0.5) == true );
  }

TEST_CASE("fn_all_2")
  {
  mat A(5, 6, fill::zeros);
  mat B(5, 6, fill::zeros);  B(0,0) = 1.0;
  mat C(5, 6, fill::ones );

  REQUIRE( all(vectorise(A)) == false);
  REQUIRE( all(vectorise(B)) == false);
  REQUIRE( all(vectorise(C)) == true );

  REQUIRE( all(vectorise(A(span::all,span::all))) == false);
  REQUIRE( all(vectorise(B(span::all,span::all))) == false);
  REQUIRE( all(vectorise(C(span::all,span::all))) == true );

  REQUIRE( all(vectorise(  C -  C)) == false);
  REQUIRE( all(vectorise(2*C -2*C)) == false);

  REQUIRE( all(vectorise(C) < 0.5) == false);
  REQUIRE( all(vectorise(C) > 0.5) == true );
  }

TEST_CASE("fn_all_3")
  {
  mat A(5, 6, fill::zeros);
  mat B(5, 6, fill::zeros);  B(0,0) = 1.0;
  mat C(5, 6, fill::ones );
  mat D(5, 6, fill::ones );  D(0,0) = 0.0;

  REQUIRE( accu( all(A)   == urowvec({0, 0, 0, 0, 0, 0}) ) == 6 );
  REQUIRE( accu( all(A,0) == urowvec({0, 0, 0, 0, 0, 0}) ) == 6 );
  REQUIRE( accu( all(A,1) == uvec   ({0, 0, 0, 0, 0}   ) ) == 5 );

  REQUIRE( accu( all(B)   == urowvec({0, 0, 0, 0, 0, 0}) ) == 6 );
  REQUIRE( accu( all(B,0) == urowvec({0, 0, 0, 0, 0, 0}) ) == 6 );
  REQUIRE( accu( all(B,1) == uvec   ({0, 0, 0, 0, 0}   ) ) == 5 );

  REQUIRE( accu( all(C)   == urowvec({1, 1, 1, 1, 1, 1}) ) == 6 );
  REQUIRE( accu( all(C,0) == urowvec({1, 1, 1, 1, 1, 1}) ) == 6 );
  REQUIRE( accu( all(C,1) == uvec   ({1, 1, 1, 1, 1}   ) ) == 5 );

  REQUIRE( accu( all(D)   == urowvec({0, 1, 1, 1, 1, 1}) ) == 6 );
  REQUIRE( accu( all(D,0) == urowvec({0, 1, 1, 1, 1, 1}) ) == 6 );
  REQUIRE( accu( all(D,1) == uvec   ({0, 1, 1, 1, 1}   ) ) == 5 );
  }


/*
 ISC License

 Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder

 Permission to use, copy, modify, and/or distribute this software for any
 purpose with or without fee is hereby granted, provided that the above
 copyright notice and this permission notice appear in all copies.

 THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.

 */

#include <stdio.h>
#include <stdlib.h>
#include "discretizeCheck.h"
#include <Eigen/Dense>
#include "utilities/avsEigenSupport.h"
#include "utilities/discretize.h"

uint64_t testDiscretize()
{
    uint64_t failures = 0;
    Discretize discretizor = Discretize(3);
    Eigen::Vector3d states;
    states << 0.1, 10.1, 11.1;
    Eigen::Vector3d LSBs;
    LSBs << 10., 10., 10.;
    discretizor.setLSB(LSBs);
    roundDirection_t roundThisWay = TO_ZERO;
    discretizor.setRoundDirection(roundThisWay);
    bool toCarryOrNotToCarry = false;
    discretizor.setCarryError(toCarryOrNotToCarry);
    Eigen::Vector3d expected;
    expected << 0, 10., 10.;

    states = discretizor.discretize(states);
    failures += states == expected ? 0 : 1;

    roundThisWay = FROM_ZERO;
    discretizor.setRoundDirection(roundThisWay);
    states << 0.1, 10.1, 11.1;
    expected << 10., 20., 20.;
    states = discretizor.discretize(states);
    failures += states == expected ? 0 : 1;

    roundThisWay = NEAR;
    discretizor.setRoundDirection(roundThisWay);
    states << 0.1, 10.1, 15.1;
    expected << 0, 10, 20;
    states = discretizor.discretize(states);
    failures += states == expected ? 0 : 1;

    discretizor = Discretize(3);
    discretizor.setLSB(LSBs);
    roundThisWay = TO_ZERO;
    discretizor.setRoundDirection(roundThisWay);
    toCarryOrNotToCarry = true;
    discretizor.setCarryError(toCarryOrNotToCarry);
    states << 0.1, 10.1, 15;
    expected << 0., 10., 10.;
    Eigen::Vector3d output;

    uint64_t numPts = 2;
    for(uint64_t i = 0; i < numPts; i++){
        output = discretizor.discretize(states);
        failures += output == expected ? 0 : 1;
        expected << 0, 10, 20;
    }

    return failures;

}


//
// Created by michel on 10-10-19.
//

#include <algorithm>
#include <boost/test/unit_test.hpp>
#include <chrono>
#include <iostream>
#include <simple-dsp/integration.h>

using FloatMultipliers = simpledsp::IntegrationMulipliers<float>;
using DoubleMultipliers = simpledsp::IntegrationMulipliers<double>;

#ifdef SIMPLE_DSP_INTEGRATION_LONG_TESTS_SKIP
static constexpr bool skipTests = true;
#else
static constexpr bool skipTests = false;
#endif
template <typename T>
static void impulseResponseSumTest(const char *testName, const char *message,
                                   size_t factor = 1, double epsilon = 1e-6) {
  T output = 0;
  T sum = 0;
  T maxSamples = simpledsp::IntegrationMulipliers<T>::maxSamples() / factor;
  size_t iterationCount = std::sqrt(maxSamples) * 4;
  size_t maxCount = simpledsp::minimum(static_cast<size_t>(maxSamples),
                                       std::numeric_limits<size_t>::max() - 2);

  simpledsp::IntegrationCoefficients<T> integrator(maxSamples);
  auto start = std::chrono::system_clock::now();
  auto maxDuration = std::chrono::seconds(2);

  size_t count = 1;
  integrator.integrate(1.0, output);
  sum = output;
  std::cout << "( " << testName << ": will run for a maximum of "
            << maxDuration.count() << " seconds )" << std::endl;
  for (size_t i = 0; i < iterationCount; i++) {
    for (size_t j = 0; j < iterationCount && count < maxCount; j++, count++) {
      integrator.integrate(0, output);
      sum += output;
    }
    auto now = std::chrono::system_clock::now();
    if (now - start > maxDuration) {
      break;
    }
  }

  T expected = 1 - pow(M_E, -1.0 * count / maxSamples);

  sum /= expected;

  bool result = fabs(1.0 - sum) < epsilon;
  BOOST_CHECK_MESSAGE(result, message);
  if (!result) {
    std::cout << testName << ": " << message << ": " << sum << " for " << count
              << "/" << maxSamples << " samples " << std::endl;
  }
}

template <typename T>
static void stepResponseSumTest(const char *testName, const char *message) {
  T output = 0;
  T maxSamples = simpledsp::IntegrationMulipliers<T>::maxSamples();
  size_t iterationCount = std::sqrt(maxSamples) * 4;
  size_t maxCount = simpledsp::minimum(static_cast<size_t>(maxSamples),
                                       std::numeric_limits<size_t>::max() - 2);

  simpledsp::IntegrationCoefficients<T> integrator(maxSamples);
  auto start = std::chrono::system_clock::now();
  auto maxDuration = std::chrono::seconds(2);
  std::cout << "( " << testName << ": will run for a maximum of "
            << maxDuration.count() << " seconds )" << std::endl;

  size_t count = 1;
  integrator.integrate(1.0, output);
  for (size_t i = 0; i < iterationCount; i++) {
    for (size_t j = 0; j < iterationCount && count < maxCount; j++, count++) {
      integrator.integrate(1, output);
    }
    auto now = std::chrono::system_clock::now();
    if (now - start > maxDuration) {
      break;
    }
  }

  T expected = 1 - pow(M_E, -1.0 * count / maxSamples);

  output /= expected;

  bool result = (1.0 - output) < 0.001;
  BOOST_CHECK_MESSAGE(result, message);
  if (!result) {
    std::cout << testName << ": " << message << ": " << output << " for "
              << count << "/" << maxSamples << " samples " << std::endl;
  }
}

BOOST_AUTO_TEST_SUITE(iirIntegrationTest)

BOOST_AUTO_TEST_CASE(testAndReportSkippingLongTests) {
  if (skipTests) {
    std::cout << "Skipping tests with long duration because "
                 "SIMPLE_DSP_INTEGRATION_LONG_TESTS_SKIP is defined"
              << std::endl;
  }
}

BOOST_AUTO_TEST_CASE(testMaxSamplesFloatOkayBig) {
  BOOST_CHECK_MESSAGE(FloatMultipliers::maxSamples() > 1e6,
                      "Max samples for float integration not reasonable");
}

BOOST_AUTO_TEST_CASE(testFloatImpulseResponseSum) {
  if (skipTests) {
    return;
  }
  impulseResponseSumTest<float>(
      "Integration impulse response sum for single precision floats",
      "sum not within promille of unity", 8, 1e-3);
}

BOOST_AUTO_TEST_CASE(testDoubleImpulseResponseSum) {
  if (skipTests) {
    return;
  }
  impulseResponseSumTest<double>(
      "Integration impulse response sum for double precision floats",
      "sum not within promille of unity");
}

BOOST_AUTO_TEST_CASE(testFloatStepResponseSum) {
  if (skipTests) {
    return;
  }
  stepResponseSumTest<float>(
      "Integration impulse response sum for single precision floats",
      "step response not within promille of unity");
}

BOOST_AUTO_TEST_CASE(testDoubleStepResponseSum) {
  if (skipTests) {
    return;
  }
  stepResponseSumTest<double>(
      "Integration impulse response sum for double precision floats",
      "step response not within promille of unity");
}

BOOST_AUTO_TEST_CASE(testMaxSamplesDoubleOkayBig) {
  BOOST_CHECK_MESSAGE(DoubleMultipliers::maxSamples() > 1e9,
                      "Max samples for double integration not reasonable");
}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/numeric/ublas/io.hpp>

#include "geometry/rotation.h"
#include "utils/check.h"

namespace gca {

  labeled_polygon_3 apply(const rotation& r, const labeled_polygon_3& p) {
    vector<point> pts = apply(r, p.vertices());

    vector<vector<point>> holes;
    for (auto h : p.holes()) {
      holes.push_back(apply(r, h));
    }

    polygon_3 rotated = build_clean_polygon_3(pts, holes);

    rotated.correct_winding_order(times_3(r, p.normal()));

    point rnorm = rotated.normal();
    point pnorm = p.normal();
    point rtnorm = times_3(r, p.normal());

    // cout << "Original normal             = " << pnorm << endl;
    // cout << "Rotated normal              = " << rnorm << endl;
    // cout << "Rotation of original normal = " << rtnorm << endl;

    double theta = angle_between(rotated.normal(), rtnorm);

    DBG_ASSERT(within_eps(theta, 0.0, 0.1));

    return rotated;
  }

  void test_rotation(const point from_unit, const point to_unit, const rotation& r) {
    double d = determinant(r);
    if (!(within_eps(d, 1.0, 0.001))) {
      cout << "ERROR: determinant of rotation = " << d << endl;
      cout << "from unit normal = " << from_unit << endl;
      cout << "to unit normal = " << to_unit << endl;

      double theta = angle_between(from_unit, to_unit);
      cout << "theta = " << theta << endl;
      cout << r << endl;

      point rfu = times_3(r, from_unit);
      double res_angle = angle_between(rfu, to_unit);

      cout << "r * from = " << rfu << endl;
      cout << "resulting angle = " << res_angle << endl;

      // cout << "c = " << c << endl;
      // cout << "s = " << s << endl;
      // cout << "v = " << v << endl;

      // cout << "vx = " << endl;
      // cout << vx << endl;

      DBG_ASSERT(false);
    }

    if (!(within_eps(angle_between(times_3(r, from_unit), to_unit), 0.0, 0.1))) {
      cout << "ERROR: Incorrect rotation " << endl;
      cout << r << endl;

      cout << "from unit normal = " << from_unit << endl;
      cout << "to unit normal = " << to_unit << endl;

      cout << "r*" << from_unit << " = " << times_3(r, from_unit) << " != " << to_unit << endl;

      DBG_ASSERT(false);
    }

  }

  rotation rotate_unit_from_to_regular(const point from_unit, const point to_unit) {
    point v = cross(from_unit, to_unit);
    double s = v.len();
    double c = dot(from_unit, to_unit);

    boost::numeric::ublas::matrix<double> vx(3, 3);
    vx(0, 0) = 0;
    vx(0, 1) = -v.z;
    vx(0, 2) = v.y;

    vx(1, 0) = v.z;
    vx(1, 1) = 0;
    vx(1, 2) = -v.x;

    vx(2, 0) = -v.y;
    vx(2, 1) = v.x;
    vx(2, 2) = 0;

    const boost::numeric::ublas::matrix<double> id =
      boost::numeric::ublas::identity_matrix<double>(3);

    const boost::numeric::ublas::matrix<double> vxc = vx;

    const boost::numeric::ublas::matrix<double> vx2 = prod(vx, vx);

    const ublas::matrix<double> r =
      id + vxc + ((1.0 - c)/(s*s))*vx2;

    return r;
  }

  void check_is_unit_vector(const point v, const double tol) {
    if (!(within_eps(v.len(), 1.0, 0.00001))) {
      cout << "Error: Not a unit vector" << endl;
      cout << "v       = " << v << endl;
      cout << "v.len() = " << v.len() << endl;
      cout << "tol     = " << tol << endl;

      DBG_ASSERT(within_eps(v.len(), 1.0, tol));
    }
  }

  rotation rotate_unit_from_to(const point from_unit, const point to_unit) {

    //    DBG_ASSERT(within_eps(from_unit.len(), 1.0, 0.00001));
    //    DBG_ASSERT(within_eps(to_unit.len(),   1.0, 0.00001));

    double tol = 0.00001;

    check_is_unit_vector(from_unit, tol);
    check_is_unit_vector(to_unit, tol);

    double theta = angle_between(from_unit, to_unit);

    if (within_eps(theta, 0, 0.01)) {
      return boost::numeric::ublas::identity_matrix<double>(3);
    }

    if (within_eps(theta, 180, 0.01)) {
      return -1*boost::numeric::ublas::identity_matrix<double>(3);
    }

    const ublas::matrix<double> r =
      rotate_unit_from_to_regular(from_unit, to_unit);

    test_rotation(from_unit, to_unit, r);

    return r;

  }

  rotation rotate_from_to(const point from, const point to) {
    point from_unit = from.normalize();
    point to_unit = to.normalize();

    return rotate_unit_from_to(from_unit, to_unit);
  }

  triangular_mesh apply(const rotation& r, const triangular_mesh& m) {
    triangular_mesh rotated =
      m.apply([r](const point p)
	      { return times_3(r, p); });
    return rotated;
  }

  std::vector<point> apply(const rotation& r, const std::vector<point>& pts) {
    std::vector<point> rpts;
    for (auto p : pts) {
      rpts.push_back(times_3(r, p));
    }
    return rpts;
  }

  point apply(const rotation& r, const point p) {
    return times_3(r, p);
  }

  triangle apply(const rotation& r, const triangle tri) {
    return triangle(apply(r, tri.normal),
		    apply(r, tri.v1),
		    apply(r, tri.v2),
		    apply(r, tri.v3));
  }

  polyline apply(const rotation& r, const polyline& p) {
    vector<point> applied;
    for (auto pt : p) {
      applied.push_back(times_3(r, pt));
    }

    return polyline(applied);
  }

  polygon_3 apply_no_check(const rotation& r, const polygon_3& p) {
    vector<point> pts = apply(r, p.vertices());

    vector<vector<point>> holes;
    for (auto h : p.holes()) {
      holes.push_back(apply(r, h));
    }

    polygon_3 rotated = polygon_3(pts, holes, true); //, holes);

    rotated.correct_winding_order(times_3(r, p.normal()));

    point rnorm = rotated.normal();
    point pnorm = p.normal();
    point rtnorm = times_3(r, p.normal());

    // cout << "Original normal             = " << pnorm << endl;
    // cout << "Rotated normal              = " << rnorm << endl;
    // cout << "Rotation of original normal = " << rtnorm << endl;

    double theta = angle_between(rotated.normal(), rtnorm);

    DBG_ASSERT(within_eps(theta, 0.0, 0.1));

    return rotated;
  }

  std::vector<point>
  clean_for_conversion_to_polygon_3(const std::vector<point>& vertices) {
    auto outer_ring = vertices;

    outer_ring = clean_vertices(outer_ring);

    if (outer_ring.size() < 3) { return outer_ring; }

    delete_antennas_no_fail(outer_ring);

    return outer_ring;
  }

}


//  (C) Copyright John Maddock 2007.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/math/special_functions/spherical_harmonic.hpp>
#include <fstream>
#include <boost/math/tools/test_data.hpp>
#include <boost/random.hpp>
#include "mp_t.hpp"

using namespace boost::math::tools;
using namespace boost::math;
using namespace std;

float extern_val;
// confuse the compilers optimiser, and force a truncation to float precision:
float truncate_to_float(float const * pf)
{
   extern_val = *pf;
   return *pf;
}

template<class T>
boost::math::tuple<T, T, T, T, T, T> spherical_harmonic_data(T i)
{
   static boost::mt19937 r;

   int n = real_cast<int>(floor(i));
   boost::uniform_int<> ui(0, (std::min)(n, 40));
   int m = ui(r);

   boost::uniform_real<float> ur(-2*constants::pi<float>(), 2*constants::pi<float>());
   float _theta = ur(r);
   float _phi = ur(r);
   T theta = truncate_to_float(&_theta);
   T phi = truncate_to_float(&_phi);

   T r1 = spherical_harmonic_r(n, m, theta, phi);
   T r2 = spherical_harmonic_i(n, m, theta, phi);
   return boost::math::make_tuple(n, m, theta, phi, r1, r2);
}

int main(int argc, char*argv [])
{
   using namespace boost::math::tools;

   parameter_info<mp_t> arg1, arg2, arg3;
   test_data<mp_t> data;

   bool cont;
   std::string line;

   if(argc < 1)
      return 1;

   do{
      if(0 == get_user_parameter_info(arg1, "n"))
         return 1;
      arg1.type |= dummy_param;
      arg2.type |= dummy_param;
      arg3 = arg2;

      data.insert(&spherical_harmonic_data<mp_t>, arg1);

      std::cout << "Any more data [y/n]?";
      std::getline(std::cin, line);
      boost::algorithm::trim(line);
      cont = (line == "y");
   }while(cont);

   std::cout << "Enter name of test data file [default=spherical_harmonic.ipp]";
   std::getline(std::cin, line);
   boost::algorithm::trim(line);
   if(line == "")
      line = "spherical_harmonic.ipp";
   std::ofstream ofs(line.c_str());
   line.erase(line.find('.'));
   ofs << std::scientific << std::setprecision(40);
   write_code(ofs, data, line.c_str());

   return 0;
}