#include <iostream>
#include <utility>
#include <algorithm>
#include <vector>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/transform_value_property_map.hpp>
#include <boost/property_map/function_property_map.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <future>
#include <torch/extension.h>
#define EPS 1e-20

class union_find{
public:
    union_find(int n){
        this->count = n;
        for(auto i=0; i<n; i++){
            parent[i] = i;
        }
    }

    void make_set(int x){
        this->parent[x] = x;
        count++;
    }
    int find(int x){
        int tmp = x;
        while(parent[tmp] != tmp){
            parent[tmp] = parent[parent[tmp]];
            tmp = parent[tmp];
        }
        return tmp;
    }
    // For now set parent[y] = x. Union by rank may come later. In that case we need to make sure the root is
    // always lower f value
    void link(int x, int y){
        parent[y] = x;
        count--;
    }
    int num_connected_component() const{
        return this->count;
    }
private:
    std::map<int, int> parent;
    int count;

};
using std::vector;
using torch::Tensor;
typedef std::pair<long, long> Edge;
typedef std::pair<int, Edge> Pers;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, boost::no_property, boost::property<boost::edge_weight_t, int>> Graph;
typedef boost::graph_traits<Graph>::vertex_descriptor VertexDesc;
typedef boost::graph_traits<Graph>::edge_descriptor EdgeDesc;
typedef boost::graph_traits<Graph>::vertex_iterator VertexIter;
typedef boost::graph_traits<Graph>::edge_iterator EdgeIter;
using PathType = vector<vector<VertexDesc>>;
using namespace torch::indexing;

PathType find_path(int start, VertexDesc goal, const Graph& _graph)  {
    vector<VertexDesc> p(num_vertices(_graph));
    vector<int> d(num_vertices(_graph));
    VertexDesc s = vertex(start, _graph);

    //auto idmap = boost::get(boost::vertex_index, _graph);

    //vector<VertexDesc> predecessors(boost::num_vertices(_graph), Graph::null_vertex());
    //vector<int> distances(boost::num_vertices(_graph));
    //boost::property_map<Graph, boost::edge_weight_t>::type weightmap = boost::get(boost::edge_weight, _graph);
    dijkstra_shortest_paths(_graph, s, boost::predecessor_map(&p[0]).distance_map(&d[0]));

    // extract path
    VertexDesc current = goal;
    PathType path { };
    //PathType edge_path;

    do {
        auto const pred = p.at(current);

        //std::cout << "extract path: " << current << " " << _graph[current].coord << " <- " << pred << std::endl;

        if(current == pred)
            break;
        path.push_back({current, pred});
        current = pred;

    } while(current != start);

    //std::reverse(path.begin(), path.end());

    return path;
}

bool mycmp(Edge a, Edge b){
    float a_val = std::max(a.first, a.second);
    float b_val = std::max(b.first, b.second);
    if (a_val < b_val)
        return true;
    else if(std::abs(a_val - b_val) < EPS)
        return a.second - b.second;
    return false;
}

bool vcmp(const vector<VertexDesc>& a, const vector<VertexDesc>& b){
    float a_val = std::max(a[0], a[1]);
    float b_val = std::max(b[0], b[1]);
    if (a_val < b_val)
        return true;
    else if(std::abs(a_val - b_val) < EPS)
        return a[1] - b[1];
    return false;
}

void print_pairs(vector<Pers> &ed) {
    for (auto e: ed) {
        std::cout << e.first << " (" << e.second.first << "," << e.second.second << ")" << std::endl;
    }
}

void print_path(const PathType& p){
    for(auto v : p){
        std::cout << "(" << v[0] << "," << v[1] << ")" << std::endl;
    }
}

struct CustomEdgeCompare {
    Tensor vert_fil;
    CustomEdgeCompare(const Tensor &vertex_filtration){
        this->vert_fil = vertex_filtration;
    }

    bool operator()(const vector<VertexDesc>& a, const vector<VertexDesc>& b) const {
        double a_val = std::max(vert_fil[a[0]].item<double>(), vert_fil[a[1]].item<double>());
        double b_val = std::max(vert_fil[b[0]].item<double>(), vert_fil[b[1]].item<double>());
        if (a_val < b_val)
            return true;
        else if(std::abs(a_val - b_val) < EPS)
            return (vert_fil[a[1]] - vert_fil[b[1]]).item<int>();
        return false;
    }
};
vector<Tensor> compute_pd0(const Tensor & vertex_filtration,
                           const vector<Tensor> & boundary_info){
    auto num_nodes = vertex_filtration.size(0);
    union_find uf = union_find(num_nodes);
    Tensor tensor_edges = boundary_info[0];
    Tensor edge_val = std::get<0>(torch::max(vertex_filtration.index({tensor_edges}), 1));
    Tensor sorted_edge_indices = edge_val.argsort(-1,false);
    const Tensor sorted_edges = tensor_edges.index({sorted_edge_indices});
    edge_val = edge_val.index({sorted_edge_indices});
    auto num_edges = sorted_edges.size(0);
    vector<Tensor> pd_0;

    for(auto i = 0; i < num_edges; i++){
        auto e = sorted_edges[i];
        auto e_val = edge_val[i];
        int u = e[0].item<int>();
        int v = e[1].item<int>();
        int root_u = uf.find(u);
        int root_v = uf.find(v);
        if(root_u == root_v){
            continue;
        }
        int root = root_u;
        int merged = root_v;
        if (vertex_filtration[root].item<double>() > vertex_filtration[merged].item<double>())
            std::swap(root, merged);
        else if (std::abs(vertex_filtration[root].item<double>() - vertex_filtration[merged].item<double>()) < EPS) {
            if (root > merged)
                std::swap(root, merged);
        }
        auto merged_val = vertex_filtration[merged];
        //std::cout << "M: " << merged_val.item<double>() << " E: " << e_val.item<double>()<< std::endl;
        Tensor pd_pair = torch::stack({merged_val, e_val});
        pd_0.emplace_back(pd_pair);
        uf.link(root, merged);
    }
    return pd_0;

}

vector<vector<Tensor>> extended_filt_persistence_single(const Tensor & vertex_filtration,
                                                        const vector<Tensor> & boundary_info){
    vector<vector<Tensor>> pd;
    auto num_nodes = vertex_filtration.size(0);
    union_find uf = union_find(num_nodes);
    Graph g;
    vector<size_t> pos_edge_index;
    vector<Tensor> pd_0_up = compute_pd0(vertex_filtration, boundary_info);
    vector<Tensor> pd_0_down, pd_1_rel, pd_1_ext;
    Tensor tensor_edges = boundary_info[0];
    Tensor edge_val = std::get<0>(torch::min(vertex_filtration.index({tensor_edges}), 1));
    Tensor sorted_edge_indices = edge_val.argsort(-1, true);
    const Tensor sorted_edges = tensor_edges.index({sorted_edge_indices});
    edge_val = edge_val.index({sorted_edge_indices});
    auto num_edges = sorted_edges.size(0);
    for(auto i = 0; i < num_edges; i++){
        auto e = sorted_edges[i];
        auto e_val = edge_val[i];
        int u = e[0].item<int>();
        int v = e[1].item<int>();
        int root_u = uf.find(u);
        int root_v = uf.find(v);
        if(root_u == root_v){
            pos_edge_index.push_back(i);
            continue;
        }
        boost::add_edge(u, v, 1, g);
        int root = root_u;
        int merged = root_v;
        if (vertex_filtration[root].item<double>() < vertex_filtration[merged].item<double>())
            std::swap(root, merged);
        else if (std::abs(vertex_filtration[root].item<double>() - vertex_filtration[merged].item<double>()) < EPS) {
            if (root < merged)
                std::swap(root, merged);
        }
        auto merged_val = vertex_filtration[merged];
        Tensor pd_pair = torch::stack({merged_val, e_val});
        pd_0_down.emplace_back(pd_pair);
        uf.link(root, merged);
    }
    //pd.push_back(pd_0);
    Tensor min_max = torch::stack({vertex_filtration.min(), vertex_filtration.max()});
    pd_1_rel.push_back(min_max);
    CustomEdgeCompare cmp = CustomEdgeCompare(vertex_filtration);
    for(auto ii : pos_edge_index){
        auto pos_edge = sorted_edges[ii];
        auto pos_edge_val = edge_val[ii];
        int u = pos_edge[0].item<int>();
        int v = pos_edge[1].item<int>();
        PathType p = find_path(u, v, g);
        //std::cout << "Edge: " << u << " " << v << std::endl;
        //print_path(p);
        auto result = *std::max_element(p.begin(), p.end(), cmp);
        boost::remove_edge(result[0], result[1], g);
        //std::cout << "Removed edge: " << result[0] << " " << result[1] << std::endl;
        boost::add_edge(u, v, 1, g);
        auto cut_edge_tensor = torch::from_blob(result.data(), {2}, torch::TensorOptions().dtype(at::kLong));
        auto cut_edge_val = vertex_filtration.index({cut_edge_tensor}).max();
        //std::cout << "CE " << cut_edge_val.item<double>() << " AE " << pos_edge_val.item<double>() << std::endl;
        auto pers_pair = torch::stack({cut_edge_val, pos_edge_val});
        pd_1_ext.push_back(pers_pair);
    }
    //pd.push_back(pd_1);
    pd.push_back(pd_0_up);
    pd.push_back(pd_0_down);
    pd.push_back(pd_1_rel);
    pd.push_back(pd_1_ext);
    return pd;

}
vector<vector<vector<Tensor>>> extended_filt_persistence_batch(const vector<std::tuple<Tensor, vector<Tensor>>> & batch){
auto futures = vector<std::future<vector<vector<Tensor>>>>();
for (auto & arg: batch){

futures.push_back(
        async(std::launch::async,[=]{
                  return extended_filt_persistence_single(
                          std::get<0>(arg),
                          std::get<1>(arg)
                  );
              }
)
);
}
auto ret = vector<vector<vector<Tensor>>>();
for (auto & fut: futures){
ret.push_back(
        fut.get()
);
}

return ret;
}

PYBIND11_MODULE(TORCH_EXTENSION_NAME, m) {
m.def("extended_persistence_batch", &extended_filt_persistence_batch, "A function to compute extended_persistence in batches as C-hofer");
m.def("extended_persistence_single", &extended_filt_persistence_single, "A function to compute extended_persistence with (v, [e]) format");
}


//
//! Copyright © 2021
//! Brandon Kohn
//
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//
#ifndef GEOMETRIX_CONVEX_POLYGON_POLYGON_INTERSECTION_HPP
#define GEOMETRIX_CONVEX_POLYGON_POLYGON_INTERSECTION_HPP
#pragma once

#include <geometrix/algorithm/intersection/segment_segment_intersection.hpp>
#include <geometrix/algorithm/orientation/point_segment_orientation.hpp>
#include <geometrix/primitive/point_sequence_traits.hpp>
#include <geometrix/primitive/polygon.hpp>
#include <geometrix/algorithm/point_in_polygon.hpp>
#include <geometrix/primitive/point.hpp>
#include <geometrix/arithmetic/arithmetic_promotion_policy.hpp>
#include <geometrix/utility/utilities.hpp>

#include <boost/container/flat_set.hpp>
#include <boost/container/small_vector.hpp>
#include <boost/container/new_allocator.hpp>
#include <boost/concept_check.hpp>

/////////////////////////////////////////////////////////////////////////////
//
// NAMESPACE
//
/////////////////////////////////////////////////////////////////////////////
namespace geometrix {
    enum class polygon_intersection_type
    {
        none = 0,
        face = 1,
        vertex = 2,
        overlapping = 3
    };

    //! From Computational Geometry in C, O'Rourke.
    template <typename Polygon1, typename Polygon2, typename Visitor, typename NumberComparisonPolicy>
    inline polygon_intersection_type convex_polygon_polygon_intersection(const Polygon1& pgon1, const Polygon2& pgon2, Visitor&& visitor, const NumberComparisonPolicy& cmp)
    {
        BOOST_CONCEPT_ASSERT((PointSequenceConcept<Polygon1>));
        BOOST_CONCEPT_ASSERT((PointSequenceConcept<Polygon2>));
        BOOST_CONCEPT_ASSERT((NumberComparisonPolicyConcept<NumberComparisonPolicy>));
        using access1 = point_sequence_traits<Polygon1>;
        using access2 = point_sequence_traits<Polygon2>;
        using length_t = typename select_arithmetic_type_from_sequences<typename access1::point_type, typename access2::point_type>::type;
        using area_t = decltype(std::declval<length_t>() * std::declval<length_t>());
        using point_t = point<length_t, 2>;
        using vector_t = vector<length_t, 2>;
        enum in_out_state
        {
            pgon1_in,
            pgon2_in,
            unknown
        };
		auto pointCmp = [&]( const point_t& lhs, const point_t& rhs )
		{
			return lexicographically_less_than( lhs, rhs, cmp );
		};
	#if BOOST_VERSION >= 107000
		using small_flat_set = boost::container::flat_set<point_t, decltype(pointCmp), boost::container::small_vector<point_t, 20, boost::container::new_allocator<point_t>>>;
		auto visited = small_flat_set(pointCmp);
    #else
		auto visited = boost::container::flat_set<point_t, decltype( pointCmp )>( pointCmp );
    #endif
		auto shouldVisit = [&]( const point_t& p )
		{
			auto it = visited.lower_bound( p );
			if( it != visited.end() && !( visited.key_comp()( p, *it ) ) )
				return;

			visited.insert( it, p );
			visitor( p );
		};
        auto get_point1 = [&](std::size_t i) { return access1::get_point(pgon1, i); };
        auto get_point2 = [&](std::size_t i) { return access2::get_point(pgon2, i); };
        auto advance = [](std::size_t& a, std::size_t& aa, std::size_t n) { ++aa; ++a %= n; };
        auto a = std::size_t {};
        auto b = std::size_t {};
        auto aa = std::size_t {};
        auto ba = std::size_t {};
        auto inflag = unknown;

        auto n = access1::size(pgon1);
        auto n2 = n * 2;
        auto m = access2::size(pgon2);
        auto m2 = m * 2;

        auto nVisited = 0;
        do
        {
            auto a1 = (a + n - 1) % n;
            auto b1 = (b + m - 1) % m;

            vector_t A = get_point1(a) - get_point1(a1);
            vector_t B = get_point2(b) - get_point2(b1);

            auto ABOrientation = get_orientation(A, B, cmp);
            auto aHB = get_orientation(get_point2(b1), get_point2(b), get_point1(a), cmp);
            auto bHA = get_orientation(get_point1(a1), get_point1(a), get_point2(b), cmp);

            point_t xpoints[2];
            auto iType = segment_segment_intersection(get_point1(a1), get_point1(a), get_point2(b1), get_point2(b), xpoints, cmp);
            if (iType == e_crossing || iType == e_endpoint)
            {
                shouldVisit(xpoints[0]);
                ++nVisited;

                if (inflag == unknown && nVisited == 1)
                    aa = ba = 0;
                if (aHB == oriented_left)
                    inflag = pgon1_in;
                else if (bHA == oriented_left)
                    inflag = pgon2_in;
            }
            else if (iType == e_overlapping && dot_product(A, B) < area_t {})
            {
                //++nVisited;
                shouldVisit(xpoints[0]);
                shouldVisit(xpoints[1]);
                return polygon_intersection_type::face;
            }

            if (ABOrientation == oriented_collinear && aHB == oriented_right && bHA == oriented_right)
                return polygon_intersection_type::none;
            else if (ABOrientation == oriented_collinear && aHB == oriented_collinear && bHA == oriented_collinear)
            {
                if (inflag == pgon1_in)
                    advance(b, ba, m);
                else
                    advance(a, aa, n);
            }
            else if (ABOrientation != oriented_right)
            {
                if (bHA == oriented_left)
                {
                    if (inflag == pgon1_in)
                    {
                        ++nVisited;
                        shouldVisit(get_point1(a));
                    }
                    advance(a, aa, n);
                }
                else
                {
                    if (inflag == pgon2_in)
                    {
                        ++nVisited;
                        shouldVisit(get_point2(b));
                    }
                    advance(b, ba, m);
                }
            }
            else
            {
                if (aHB == oriented_left)
                {
                    if (inflag == pgon2_in)
                    {
                        ++nVisited;
                        shouldVisit(get_point2(b));
                    }
                    advance(b, ba, m);
                }
                else
                {
                    if (inflag == pgon1_in)
                    {
                        ++nVisited;
                        shouldVisit(get_point1(a));
                    }
                    advance(a, aa, n);
                }
            }
        } while((aa < n || ba < m) && aa < n2 && ba < m2);

        if (inflag == unknown)
        {
            if (visited.size() == 1) {
				return polygon_intersection_type::vertex;
            }
            //! Check if one vertex of either is inside.
            if (point_in_polygon(get_point1(0), pgon2))
            {
                for (auto i = 0ULL; i < m; ++i)
                    shouldVisit(get_point1(i));
                return polygon_intersection_type::overlapping;
            }
            if (point_in_polygon(get_point2(0), pgon1))
            {
                for (auto i = 0ULL; i < m; ++i)
                    shouldVisit(get_point2(i));
                return polygon_intersection_type::overlapping;
            }
        }

        return nVisited == 0 ? polygon_intersection_type::none : polygon_intersection_type::overlapping;
    }

}//! namespace geometrix;

#endif//! GEOMETRIX_CONVEX_POLYGON_POLYGON_INTERSECTION_HPP


#include "segment.pb.h"            // for ResizeArgs (generated file)
#include "scanner/api/kernel.h"   // for VideoKernel and REGISTER_KERNEL
#include "scanner/api/op.h"       // for REGISTER_OP
#include "scanner/util/memory.h"  // for device-independent memory management
#include "scanner/util/opencv.h"  // for using OpenCV

#include <opencv2/opencv.hpp>
#include <opencv2/ximgproc.hpp>
#include "opencv2/highgui.hpp"
#include "opencv2/core/utility.hpp"

#include <iostream>               // for std::cout

#include <Eigen/Sparse>
typedef float var_t;
typedef Eigen::SparseMatrix<var_t> SpMat;
typedef Eigen::Triplet<var_t> T;

void getPixelNeighbors(int height, int width, std::vector<std::vector<int>>& neighborId){

    for(int i=0; i<height; i++){
        for(int j=0; j<width; j++){

            if(i == 0){
                neighborId[i*width+j].push_back((i+1)*width+j);
            }else if(i == height-1){
                neighborId[i*width+j].push_back((i-1)*width+j);
            }else{
                neighborId[i*width+j].push_back((i+1)*width+j);
                neighborId[i*width+j].push_back((i-1)*width+j);
            }

            if(j == 0){
                neighborId[i*width+j].push_back(i*width+j+1);
            }else if(j == width-1){
                neighborId[i*width+j].push_back(i*width+j-1);
            }else{
                neighborId[i*width+j].push_back(i*width+j+1);
                neighborId[i*width+j].push_back(i*width+j-1);
            }

        }
    }

}

void getLabelPosition(var_t *img, int h, int w, std::map<int, std::vector<int>>& ht){
    for(int i=0; i<h; i++) {
        for (int j = 0; j < w; j++) {

            if(img[i*w+j] >= 1.0){
                int lbl = int(img[i*w+j]-1);
                ht[lbl].push_back(i*w+j);
            }

        }
    }
}

SpMat setU(int N, std::map<int, std::vector<int>>& ht, Eigen::VectorXf& y){
    std::vector<T> tripletList;

    for(std::map<int,std::vector<int>>::iterator it = ht.begin(); it != ht.end(); ++it) {
        std::vector<int> pixLocation = it->second;
        for(int i=0; i<pixLocation.size();i++){
            tripletList.push_back(T(pixLocation[i], pixLocation[i], 1.));
            y[pixLocation[i]] = float(it->first);
        }
    }

    SpMat U(N,N);
    U.setFromTriplets(tripletList.begin(), tripletList.end());
    return U;
}

void setDW(var_t* image, var_t* edges, int h, int w, std::vector<SpMat>& out, float sigma1, float sigma2){

    int N = h * w;
    std::vector<std::vector<int>> neighborId(N);
    getPixelNeighbors(h, w, neighborId);

    std::vector<T> tripletListD;
    std::vector<T> tripletListW;
    int M = 0;

    for(int i=0; i<neighborId.size(); i++){
        int x, y;
        y = i/w;
        x = i%w;
        var_t r1 = image[y*w*3+x*3+0];
        var_t g1 = image[y*w*3+x*3+1];
        var_t b1 = image[y*w*3+x*3+2];
        var_t e1 = edges[y*w+x];

        for(int j=0; j<neighborId[i].size(); j++){

            y = neighborId[i][j]/w;
            x = neighborId[i][j]%w;
            var_t r2 = image[y*w*3+x*3+0];
            var_t g2 = image[y*w*3+x*3+1];
            var_t b2 = image[y*w*3+x*3+2];

            var_t weight0 = exp(-((r1-r2)*(r1-r2) + (g1-g2)*(g1-g2)+ (b1-b2)*(b1-b2))/sigma1);
            var_t weight1 = exp(-(e1*e1)/sigma2);

            // std::cout<<weight0<<" "<<weight1<<std::endl;

            tripletListD.push_back(T(M, i, 1.));
            tripletListD.push_back(T(M, neighborId[i][j], -1.));
            tripletListW.push_back(T(M, M, weight0*weight1));

            M++;

        }
    }

    SpMat D(M, N);
    D.setFromTriplets(tripletListD.begin(), tripletListD.end());

    SpMat W(M, M);
    W.setFromTriplets(tripletListW.begin(), tripletListW.end());
    out.push_back(D);
    out.push_back(W);
}

var_t* segmentFromPoses(var_t *img, var_t *edges, var_t *poseData, int height, int width, float sigma1, float sigma2){
    std::map<int, std::vector<int>> ht;
    getLabelPosition(poseData, height, width, ht);
    Eigen::VectorXf y(height*width);
    SpMat U = setU(height*width, ht, y);
    std::vector<SpMat> DW;
    setDW(img, edges, height, width, DW, sigma1, sigma2);
    SpMat D = DW[0];
    SpMat W = DW[1];

    Eigen::VectorXf b = U*y;

    SpMat A = U + D.transpose()*W*D;

    Eigen::SimplicialCholesky <SpMat> solver(A);
    Eigen::VectorXf x = solver.solve(b);

    var_t *output = new var_t[height*width];
    for(int i=0; i<height*width; i++)
        output[i] = x[i];

    return output;
}

cv::Ptr<cv::ximgproc::StructuredEdgeDetection> pDollar = cv::ximgproc::createStructuredEdgeDetection("/home/krematas/code/soccerontable/soccer3d/instancesegm/model.yml.gz");

/*
 * Ops in Scanner are abstract units of computation that are implemented by
 * kernels. Kernels are pinned to a specific device (CPU or GPU). Here, we
 * implement a custom op to resize an image. After reading this file, look
 * at CMakeLists.txt for how to build the op.
 */

// Custom kernels must inherit the Kernel class or any subclass thereof,
// e.g. the VideoKernel which provides support for processing video frames.
class MySegmentKernel : public scanner::Kernel, public scanner::VideoKernel {
 public:
  // To allow ops to be customized by users at a runtime, e.g. to define the
  // target width and height of the MyResizeKernel, Scanner uses Google's Protocol
  // Buffers, or protobufs, to define serialzable types usable in C++ and
  // Python (see resize_op/args.proto). By convention, ops that take
  // arguments must define a protobuf called <OpName>Args, e.g. ResizeArgs,
  // In Python, users will provide the argument fields to the op constructor,
  // and these will get serialized into a string. This string is part of the
  // general configuration each kernel receives from the runtime, config.args.
  MySegmentKernel(const scanner::KernelConfig& config)
      : scanner::Kernel(config) {
    // The protobuf arguments must be decoded from the input string.
    MySegmentArgs args;
    args.ParseFromArray(config.args.data(), config.args.size());
    width_ = args.w();
    height_ = args.h();
    sigma1 = args.sigma1();
    sigma2 = args.sigma2();
  }

  // Execute is the core computation of the kernel. It maps a batch of rows
  // from an input table to a batch of rows of the output table. Here, we map
  // from one input column from the video, "frame", and return
  // a single column, "frame".
  void execute(const scanner::Elements& input_columns,
               scanner::Elements& output_columns) override {
    auto& frame_col = input_columns[0];
    auto& mask_col = input_columns[1];

    // This must be called at the top of the execute method in any VideoKernel.
    // See the VideoKernel for the implementation check_frame_info.
    check_frame(scanner::CPU_DEVICE, frame_col);
    check_frame(scanner::CPU_DEVICE, mask_col);

    auto& resized_frame_col = output_columns[0];
    scanner::FrameInfo output_frame_info(height_, width_, 3, scanner::FrameType::U8);

    const scanner::Frame* frame = frame_col.as_const_frame();
    cv::Mat image = scanner::frame_to_mat(frame);

    const scanner::Frame* mask = mask_col.as_const_frame();
    cv::Mat poseImage = scanner::frame_to_mat(mask);

    image.convertTo(image, cv::DataType<var_t>::type, 1.0/255.0);
    var_t *imgData = (var_t*)(image.data);
    std::cout<<image.channels()<<std::endl;

    poseImage.convertTo(poseImage, cv::DataType<var_t>::type);
    var_t *poseData = (var_t*)(poseImage.data);
    std::cout<<poseImage.channels()<<std::endl;
    //
    cv::Mat img2;
    image.copyTo(img2);
    img2.convertTo(img2, cv::DataType<var_t>::type);
    cv::Mat edges(img2.size(), img2.type());

    pDollar->detectEdges(img2, edges);
    std::cout<<edges.channels()<<std::endl;

    std::cout<<" -------------------------- "<<std::endl<<std::endl;
    int height = image.rows;
    int width = image.cols;

    var_t *edgesData = (var_t*)(edges.data);

    var_t* segm_output = segmentFromPoses(imgData, edgesData, poseData, height, width, sigma1, sigma2);

    // cv::Mat new_mask(height, width, cv::DataType<var_t>::type, segm_output);

    //copy vector to mat
    // std::cout<<edges.size()<<edges.type()<<std::endl;

    cv::Mat new_mask(height, width, CV_8U);
    for(int i=0; i<height; i++) {
        for (int j = 0; j < width; j++) {
          // std::cout<<segm_output[i*width+j]<<std::endl;
            if(segm_output[i*width+j] > 1.5)
                new_mask.at<uchar>(i,j) = 255;
            else
                new_mask.at<uchar>(i,j) = 0;
        }
    }

    // new_mask.convertTo(new_mask, CV_8UC3, 255.0);
    // cv::Mat output_img;
    // edges.convertTo(output_img, cv::DataType<uint8>::type);
    cv::cvtColor(new_mask, new_mask, cv::COLOR_GRAY2BGR);

    // Allocate a frame for the resized output frame
    scanner::Frame* resized_frame = scanner::new_frame(scanner::CPU_DEVICE, output_frame_info);
    cv::Mat output = scanner::frame_to_mat(resized_frame);

    cv::resize(new_mask, output, cv::Size(width_, height_));

    scanner::insert_frame(resized_frame_col, resized_frame);
  }

 private:
  int width_;
  int height_;
  float sigma1;
  float sigma2;
};

// These functions run statically when the shared library is loaded to tell the
// Scanner runtime about your custom op.

REGISTER_OP(MySegment).frame_input("frame").frame_input("mask").frame_output("frame").protobuf_name("MySegmentArgs");

REGISTER_KERNEL(MySegment, MySegmentKernel)
    .device(scanner::DeviceType::CPU)
    .num_devices(1);


#include <array>
#include <fstream>
#include <iostream>
#include <numeric>
#include <string>
#include <unordered_map>

#include <boost/range/irange.hpp>

using uint64 = std::uint64_t;

struct Arguments {
	std::string strcmd;
	uint64 A, B;
};

auto& operator>>(std::istream& in, Arguments& args) {
	return in >> args.strcmd >> args.A >> args.B;
}

// tmp_screen is unnecessary as we are not doing any
// intermediate steps, thus we don't need to save any state
template<typename S, typename N>
auto set_rectangle(S& screen, S&, N width, N height) {

	for(const auto row : boost::irange(height)) {

		for(const auto col : boost::irange(width)) {

			screen[row][col] = true;
		}
	}
}

template<typename S, typename N>
auto shift_row(S& screen, S& tmp_screen, N row, N shift) {

	tmp_screen = screen;

	// on the temporary screen, the row is set to all-false
	// so that we can lay the original screen over it, and
	// based on the state of the original screen we can set
	// the columns on the temporary screen to true as if we
	// had shifted the row of the original screen
	for(auto& col : tmp_screen[row]) {
		col = false;
	}

	const auto width = tmp_screen[row].size();

	for(const auto col : boost::irange(width)) {

		// if the current column on the original screen is lit,
		// we can start doing the shifting and apply the result
		// to the corresponding column on the temporary screen
		if(screen[row][col]) {

			const auto new_pos = ((col + shift) % width);

			tmp_screen[row][new_pos] = true;
		}
	}

	screen = tmp_screen;
}

// works the same way as shift_row(), except the shift applies to columns instead of rows
template<typename S, typename N>
auto shift_col(S& screen, S& tmp_screen, N col, N shift) {

	tmp_screen = screen;

	for(auto& row : tmp_screen) {
		row[col] = false;
	};

	const auto height = tmp_screen.size();

	for(const auto row : boost::irange(height)) {

		if(screen[row][col]) {

			const auto new_row = ((row + shift) % height);

			tmp_screen[new_row][col] = true;
		}
	}

	screen = tmp_screen;
}

template<typename S>
auto print(const S& screen) {

	for(const auto& row : screen) {

		for(const auto col : row) {
			std::cout << (col ? '#' : ' ');
		}

		std::cout << "\n";
	}

	std::cout.flush();
}

int main() {

	const auto filename = std::string{"operations.txt"};
	auto file = std::fstream{filename};

	if(file.is_open()) {

		constexpr auto width = uint64{50};
		constexpr auto height = uint64{6};

		using Array2D = std::array<std::array<bool, width>, height>;

		auto screen = Array2D{};
		auto tmp_screen = screen;

		using Command = void (*) (decltype(screen)&, decltype(tmp_screen)&, decltype(width), decltype(height));

		auto commands = std::unordered_map<std::string, Command>{
			{"rect", set_rectangle},
			{"row", shift_row},
			{"col", shift_col}
		};

		auto args = Arguments{};

		while(file >> args) {
			commands[args.strcmd](screen, tmp_screen, args.A, args.B);
		}

		print(screen);

	} else {
		std::cerr << "Error! Could not open \"" << filename << "\"!" << std::endl;
	}

	return 0;
}


/**
 * @file   tevol_source_gca.hpp
 * @brief  ODE system for group SIS with group selection mechanism for transmission rates
 *
 * Source code. All parameters passed as arguments, but specify and compile to change precision or output format.
 * g++ -std=c++11 -O3 -o tevol_source_gca ./tevol_source_gca.cpp $(gsl-config --cflags) $(gsl-config --libs)
 *
 * @author  LHD
 * @since   2021-02-06
 */

#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>
#include <fstream>
#include <sstream>

#include <boost/multi_array.hpp>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_odeiv.h>

#include "dyn_gca.hpp"

double binomial(int n, int k, double p);

using namespace std;

int main(int argc, const char *argv[]) {

	//Model parameters
	double beta = atof(argv[1]); //basic diffusion rate of innovation
	double gamma = atof(argv[2]); //recovery rate of innovation
	double rho = atof(argv[3]); //coupling between groups
	double b = atof(argv[4]); //collective benefit per adopter
	double c = atof(argv[5]); //cost per level of promotion
	double mu = atof(argv[6]); //mutation rate of adoption
	double epsilon = atof(argv[7]); //random initial condition
	int n = atoi(argv[8]); //group size
	int L = atoi(argv[9]); //number of levels
    bool timeseries = atoi(argv[10]); //1 for time series, 0 for final state

    if(argc<6) {cerr << "Requires bunch of parameters: basic diffusion rate (beta),\n recovery rate (gamma),"
                    << "\n group coupling (rho),\n adaptation benefit (b),\n promotion cost (c),"
                    << "\n initial adoption (epsilon),\n group size (n),\n number of levels (l_max), \n 1 for time series, 0 for final state.\n" << endl; return 0;}

    const int dim1 = L+1;
    const int dim2 = n+1;
    Sparam param = {beta, gamma, rho, b, c, mu, dim1, dim2};

    // Integrator parameters
    double t = 0;
    double dt = 1e-4;
    double t_step = 1.0;
    const double eps_abs = 1e-8;
    const double eps_rel = 1e-6;

    // Setting initial conditions
    typedef boost::multi_array<double,2> mat_type;
    typedef mat_type::index index;
    mat_type y(boost::extents[dim1][dim2]);
    fill(y.data(),y.data()+y.num_elements(),0.0);

    // Uniform initial conditions
    double lastI = 0.0;
    double lastIL = 0.0;
	for(int l=0; l<dim1; ++l) {
        for(int i=0; i<=n; ++i) {
            y[l][i] = (1.0/(1.0*(L+1)))*binomial(n,i,epsilon); //C_n,i
            lastI += 1.0*i*y[l][i];
        }
    }
    double newI = 0.0;
    double newIL = 0.0;
    double newS = 1.0;

    // Define GSL odeiv parameters
    const gsl_odeiv_step_type * step_type = gsl_odeiv_step_rkf45;
    gsl_odeiv_step * step = gsl_odeiv_step_alloc (step_type, dim1*dim2);
    gsl_odeiv_control * control = gsl_odeiv_control_y_new (eps_abs,eps_rel);
    gsl_odeiv_evolve * evolve = gsl_odeiv_evolve_alloc (dim1*dim2);
    gsl_odeiv_system sys = {dydt, NULL, dim1*dim2, &param};

	//Integration
    int status(GSL_SUCCESS);
    double diff = 1.0;
    double diffL = 1.0;
    for (double t_target = t+t_step; t_target < 10000 || diff > 1e-10; t_target += t_step ) { //stop by time and difference
        while (t < t_target) {
            status = gsl_odeiv_evolve_apply (evolve,control,step,&sys,&t,t_target,&dt,y.data());
            if (status != GSL_SUCCESS) {
				cout << "SNAFU" << endl;
                break;
			}
        } // end while

        //measure adoption
        vector<double> newIvec(dim1, 0.0); vector<double> popvec(dim1, 0.0); newI=0.0;
        for(int l=0; l<dim1; ++l) for(int i=0; i<=n; ++i) {
            newIvec[l] += 1.0*i*y[l][i]; popvec[l] += y[l][i]; newI += 1.0*i*y[l][i]; }

        //timeseries output
        if(timeseries) {
          cout << t;
		  for(int l=0; l<dim1; ++l) cout << " " << newIvec[l]/popvec[l] << " " << popvec[l];
          cout << "\n";
        }
        diff = abs(newIvec[0]/popvec[0] - lastI);
        diffL = abs(newIvec[dim1-1]/popvec[dim1-1] - lastIL);
        lastI=newIvec[0]/popvec[0];
        lastIL=newIvec[dim1-1]/popvec[dim1-1];
	} //end while

    //calculate fitnesses
    vector<double> Zvec_(dim1, 0.0); vector<double> popvec_(dim1, 0.0);
    for(int l=0; l<dim1; ++l) {
        for(int i=0; i<dim2; ++i) {
            Zvec_[l] += exp(b*i-c*l)*y[l][i]; //should this really be proportional to C_i,l?
            popvec_[l] += y[l][i];
        }
        Zvec_[l] /= popvec_[l];
    }
    double Z_ = accumulate(Zvec_.begin(), Zvec_.end(), 0.0);

    //final state output

    if(!timeseries) {
      vector<double> Ivec(dim1, 0.0); vector<double> pop(dim1, 0.0); newI=0.0;
      for(int l=0; l<dim1; ++l) for(int i=0; i<=n; ++i) {
        Ivec[l] += 1.0*i*y[l][i]; pop[l] += y[l][i]; newI += 1.0*i*y[l][i];
      }
      cout << beta << " " << b << " " << c << " " << rho;
      for(int l=0; l<dim1; ++l) cout << " " << Ivec[l]/pop[l] << " " << pop[l];
      for(int l=0; l<dim1; ++l) cout << " " << Zvec_[l]/Z_;
      cout << "\n";
    }

    //output fitness distribution
    /*for(int l=0; l<dim1; ++l) {
        cout << beta << " " << b << " " << c << " " << rho << " " << l;
        for(int i=0; i<dim2; ++i) {
            int g = 0;
            for(; g<int(1000*y[l][i]); ++g) cout << " " << exp(b*i-c*l);
            for(; g<1000; ++g) cout << " " << "nan";
        }
        cout << "\n";
    }*/

    // localization in cliques
    /*double xi = 0.0;
    double It = 0.0;
	for(int s=2; s<dim; ++s) for(int i=0; i<=s; ++i) It += 1.0*i*y[s][i];
    for(int s=2; s<dim; ++s) {
        double In = 0.0;
        for(int i=0; i<=s; ++i) In += 1.0*i*y[s][i];
        xi += (In/It)*(In/It);
    }*/

    /*cout << beta << " " << lastI;
    for(int n=dim-1; n>1; n-=10) {
        lastI=0.0;
        for(int i=0; i<=n; ++i) lastI += 1.0*norms*i*y[n][i]/(1.0*n*pow(1.0*n,-gammas));
	    cout << " " << lastI;
    }
    cout << "\n";
	*/
    cout.flush();

    // Free memory
    gsl_odeiv_evolve_free(evolve);
    gsl_odeiv_control_free(control);
    gsl_odeiv_step_free(step);

    return 0;
}

double binomial(int n, int k, double p)
	{
	//check
	if(k>n || k<0 || n<0) return 0.0;

	double result=0.L;
	double postfix = pow(p,k)*pow(1.L-p,n-k);
	if(n<2*k) k=n-k;

	if(k<=100&&n<=200)
		{
		double numerator=1;
		for(int i=0; i<k; i++) numerator = numerator*((double)(n-i));
		double denominator=1;
		for(int i=2; i<=k; i++) denominator = denominator*((double)i);
		result = (double)(numerator/denominator)*postfix;
		}
	else
		{
		if(k<100||n<300)
			{
			result=1.L;
			for(int i=1; i<=k; i++) result = result*((double)(n-k+i))/((double)i);
			result = result*postfix;
			}
		else if(n<1000)
			{
			double nd=(double)n, kd=(double)k, nkd=(double)(n-k);
			result = nd*log(nd)-nd+0.5L*log(2.L*M_PI*nd)+1.L/(12.L*nd)-1.L/(360.L*pow(nd,3))+1.L/(1260.L*pow(nd,5));
			result -= kd*log(kd)-kd + 0.5L*log(2.L*M_PI*kd) + 1.L/(12.L*kd) - 1.L/(360.L*pow(kd,3)) + 1.L/(1260.L*pow(kd,5));
			result -= nkd*log(nkd)-nkd + 0.5L*log(2.L*M_PI*nkd) + 1.L/(12.L*nkd) - 1.L/(360.L*pow(nkd,3)) + 1.L/(1260.L*pow(nkd,5));
			result = exp(result);
			result = result*postfix;
			}

		else result = (1.L/sqrt(2.L*M_PI*(double)n*p*(1.L-p)))*exp(-pow(k-n*p,2)/(2.L*(double)n*p*(1.L-p)));
		}
	if(result!=result) result=0.L;
	if(isinf(result)) result=0.L;
	return result;
	}


#include "../include/streamcc_test.h"
#define BOOST_TEST_MODULE ClassTest
#define BOOST_TEST_DYN_LINK

#include <boost/test/unit_test.hpp>
#include <random>
#include <map>
#include <iostream>

std::mt19937 gen;

BOOST_AUTO_TEST_CASE(CountSketch_Test) {
  Scc::CountSketch<int> cs(100, 20);
  std::discrete_distribution<int> dist {100, 5, 5, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1};
  std::map<int, int> real; // to keep the actual frequencies

  for (int i = 0; i < 100000; ++i) {
    int p = dist(gen);
    cs.processItem(p);
    real[p] += 1;
  }

  // now check the frequencies
  for (int i = 0; i < 15 ; ++i) {
    BOOST_CHECK(cs.estTotWeight(i) == real[i]);
  }
}

BOOST_AUTO_TEST_CASE(CountMin_Test) {
  Scc::CountMin<int> cm(100, 20);
  std::discrete_distribution<int> dist {100, 5, 5, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1};
  std::map<int, int> real; // to keep the actual frequencies

  for (int i = 0; i < 100000; ++i) {
    int p = dist(gen);
    cm.processItem(p);
    real[p] += 1;
  }

  // now check the frequencies
  for (int i = 0; i < 15 ; ++i) {
    BOOST_CHECK(cm.estTotWeight(i) == real[i]);
  }
}

BOOST_AUTO_TEST_CASE(DistinctCounter_Test) {
  Scc::DistinctCounter<int> f0(100);
  for (int i = 0; i < 10000; ++i) {
    for (int j = 0; j < 10; ++j)
      f0.processItem(i);
  }
  int estF0 = f0.getEstDistinct();
  BOOST_CHECK(estF0 < 10000 * 1.1);
  BOOST_CHECK(estF0 > 10000 * 0.9);
}

BOOST_AUTO_TEST_CASE(F2_Test) {
  std::discrete_distribution<int> dist {100, 5, 5, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1};
  std::map<int, double> real; // to keep the actual frequencies

  Scc::F2<int> f2(50);
  for (int i = 0; i < 10000; ++i) {
    int item = dist(gen);
    double weight = utils::rand_double();
    f2.processItem(item, weight);
    real[item] += weight;
  }

  // calc the actual F2
  double r = 0.;
  for (auto p: real) {
    r += p.second * p.second;
  }

  auto est = f2.getEstF2();
  BOOST_CHECK(est < 1.1 * r);
  BOOST_CHECK(est > 0.9 * r);

}

BOOST_AUTO_TEST_CASE(Zeros_Test) {
  BOOST_CHECK(utils::zeros(1 << 30) == 1);
  BOOST_CHECK(utils::zeros(1 << 10) == 21);
  BOOST_CHECK(utils::zeros(101 << 29) == 0);
}


#define BOOST_TEST_MODULE  physical_calc_Driver

#include <physical/calc/Driver.h>
#include <physical/calc/symbol.h>

#include <boost/test/unit_test.hpp>

#include <complex>
#include <iostream>

namespace {
  using runtime::physical::Quantity;
  using runtime::physical::calc::Driver;
  using runtime::physical::calc::symbol;

  void prepareCalculator() {
    /* prepare infix units calculator. */
    Driver & calc = Driver::instance();

    /* clear the old symbols out */
    calc.symbols.clear();
    calc.addMathLib();
    calc.addPhysicalUnits();

    calc.exec("from physical::constant import *");
    calc.exec("from physical::unit import *");
    calc.exec("from physical import 'unit::pi'");
    calc.exec("from physical import 'element::.*'");
  }

  void cleanCalculator() {
    /* prepare infix units calculator. */
    Driver & calc = Driver::instance();

    /* clear the old symbols out */
    calc.symbols.clear();
  }

}

BOOST_AUTO_TEST_SUITE( CalculatorDriver );//{

  BOOST_AUTO_TEST_CASE( simple_parse ) {
    prepareCalculator();

    BOOST_CHECK_CLOSE(
      Driver::instance().eval( "pi*(0.417*nm)^2").getCoeff<double>(),
      5.4628840494007563e-19,
      1e-10
    );

    cleanCalculator();
  }

  BOOST_AUTO_TEST_CASE( assignment ) {
    Driver & calc = Driver::instance();
    calc.symbols.clear();

    double v0 = calc.eval( "v0 = v1 = 10" ).getCoeff<double>();
    symbol::table::const_iterator it = calc.symbols.find("v0");
    if ( it == calc.symbols.end() )
      BOOST_FAIL("symbol 'v0' was not assigned!");
    else
      BOOST_CHECK_EQUAL( it->second.evaluate().getCoeff<double>(), v0);

    it = calc.symbols.find("v1");
    if ( it == calc.symbols.end() )
      BOOST_FAIL("symbol 'v1' was not assigned!");
    else
      BOOST_CHECK_EQUAL( it->second.evaluate().getCoeff<double>(), v0);

    calc.symbols.clear();
  }

  BOOST_AUTO_TEST_CASE( expressions ) {
    Driver & calc = Driver::instance();
    calc.symbols.clear();

    BOOST_CHECK_CLOSE( calc.eval( "1 + 1" ).getCoeff<double>(), 2., 1e-10 );
    BOOST_CHECK_CLOSE( calc.eval( "1 - 1" ).getCoeff<double>(), 0., 1e-10 );
    BOOST_CHECK_CLOSE( calc.eval( "1 + 1 - 1" ).getCoeff<double>(), 1., 1e-10 );
    BOOST_CHECK_CLOSE( calc.eval( "1 - 1 + 1" ).getCoeff<double>(), 1., 1e-10 );
    BOOST_CHECK_CLOSE( calc.eval( "1 - 2 + 1 + 1" ).getCoeff<double>(), 1., 1e-10 );
    BOOST_CHECK_CLOSE( calc.eval( "2*3 + 2^3" ).getCoeff<double>(), 14., 1e-10 );
    BOOST_CHECK_CLOSE( calc.eval( "9/3 + 5 % 2" ).getCoeff<double>(), 4., 1e-10 );
    BOOST_CHECK_CLOSE( calc.eval( "-3 + 2^-1 + 2*3" ).getCoeff<double>(), 3.5, 1e-10 );
    BOOST_CHECK_CLOSE( calc.eval( "2^3^2" ).getCoeff<double>(), 512., 1e-10 );
    BOOST_CHECK_CLOSE( calc.eval( "+3 + 2^-1 + 2*3" ).getCoeff<double>(), 9.5, 1e-10);
    BOOST_CHECK_CLOSE( calc.eval( "1 - (1 + 1)" ).getCoeff<double>(), -1., 1e-10 );
    BOOST_CHECK_CLOSE( calc.eval( "(1 - 3)*2 + 1" ).getCoeff<double>(), -3., 1e-10 );

  }

BOOST_AUTO_TEST_SUITE_END();//}


//==================================================================================================
/*!
  @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_FNMS_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_FNMS_HPP_INCLUDED

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

  /*!

    @ingroup group-arithmetic
    This function object computes the negated (fused) multiply substract of
    its three parameters.

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

    @par Notes
    The call `fnms(x, y, z)` is similar to `-x*y+z`

    But really conformant fused multiply/add also implies

    - only one rounding

    - no "intermediate" overflow

    fnms provides this for all integral types and also each time it is reasonable
    in terms of performance for floating ones (i.e. if the system has the hard
    wired capability).

    If you need pedantic fnms capabilities in all circumstances in your own
    code you can use the pedantic_ decorator (can be very expensive).

    @par Decorators
    - pedantic_ ensures the fnms properties and allows SIMD acceleration if available.

    @see fms, fma, fnma

    @par Example:

      @snippet fnms.cpp fnms

    @par Possible output:

      @snippet fnms.txt fnms

  **/
  Value fnms(Value const& x, Value const& y, Value const& z);
} }
#endif

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

#endif


/* This file is part of the Tomographer project, which is distributed under the
 * terms of the MIT license.
 *
 * The MIT License (MIT)
 *
 * Copyright (c) 2016 ETH Zurich, Institute for Theoretical Physics, Philippe Faist
 * Copyright (c) 2017 Caltech, Institute for Quantum Information and Matter, Philippe Faist
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#include <cmath>

#include <string>
#include <sstream>
#include <random>

#include <boost/math/constants/constants.hpp>

// include before <Eigen/*> !
#include "test_tomographer.h"

#include <tomographer/densedm/distmeasures.h>
#include <tomographer/densedm/dmtypes.h>

// -----------------------------------------------------------------------------
// fixture(s)

#include "test_densedm_distmeasures_common.h"

// -----------------------------------------------------------------------------
// test suites

BOOST_AUTO_TEST_SUITE(test_densedm_distmeasures)

BOOST_FIXTURE_TEST_SUITE(qubit_d, distmeasures_qubit_fixture<double>)

BOOST_AUTO_TEST_CASE(internal_test_fixture)
{
  internal_test_fixture();
}

BOOST_AUTO_TEST_CASE(traceDistance)
{
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::traceDistance<RealScalar>(rho1, rho1), trdist_with_1(1), tol);
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::traceDistance<RealScalar>(rho1, rho2), trdist_with_1(2), tol);
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::traceDistance<RealScalar>(rho1, rho3), trdist_with_1(3), tol);
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::traceDistance<RealScalar>(rho1, rho4), trdist_with_1(4), tol);
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::traceDistance<RealScalar>(rho1, rho5), trdist_with_1(5), tol);
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::traceDistance<RealScalar>(rho1, rho6), trdist_with_1(6), tol);
}

BOOST_AUTO_TEST_CASE(fidelity)
{
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::fidelity<RealScalar>(rho1, rho1), fid_with_1(1), tol);
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::fidelity<RealScalar>(rho1, rho2), fid_with_1(2), tol);
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::fidelity<RealScalar>(rho1, rho3), fid_with_1(3), tol);
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::fidelity<RealScalar>(rho1, rho4), fid_with_1(4), tol);
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::fidelity<RealScalar>(rho1, rho5), fid_with_1(5), tol);
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::fidelity<RealScalar>(rho1, rho6), fid_with_1(6), tol);
}

BOOST_AUTO_TEST_CASE(fidelityT)
{
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::fidelityT<RealScalar>(T1, T1), fid_with_1(1), tol);
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::fidelityT<RealScalar>(T1, T2), fid_with_1(2), tol);
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::fidelityT<RealScalar>(T1, T2b), fid_with_1(2), tol);
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::fidelityT<RealScalar>(T1, T3), fid_with_1(3), tol);
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::fidelityT<RealScalar>(T1, T4), fid_with_1(4), tol);
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::fidelityT<RealScalar>(T1, T5), fid_with_1(5), tol);
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::fidelityT<RealScalar>(T1, T6), fid_with_1(6), tol);
}

BOOST_AUTO_TEST_SUITE_END()

// -----------------------------------------------

BOOST_FIXTURE_TEST_SUITE(qubit_f, distmeasures_qubit_fixture<float>)

BOOST_AUTO_TEST_CASE(internal_test_fixture)
{
  internal_test_fixture();
}

BOOST_AUTO_TEST_CASE(traceDistance)
{
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::traceDistance<RealScalar>(rho1, rho1), trdist_with_1(1), tol_f);
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::traceDistance<RealScalar>(rho1, rho2), trdist_with_1(2), tol_f);
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::traceDistance<RealScalar>(rho1, rho3), trdist_with_1(3), tol_f);
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::traceDistance<RealScalar>(rho1, rho4), trdist_with_1(4), tol_f);
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::traceDistance<RealScalar>(rho1, rho5), trdist_with_1(5), tol_f);
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::traceDistance<RealScalar>(rho1, rho6), trdist_with_1(6), tol_f);
}

BOOST_AUTO_TEST_CASE(fidelity)
{
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::fidelity<RealScalar>(rho1, rho1), fid_with_1(1), tol_f);
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::fidelity<RealScalar>(rho1, rho2), fid_with_1(2), tol_f);
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::fidelity<RealScalar>(rho1, rho3), fid_with_1(3), tol_f);
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::fidelity<RealScalar>(rho1, rho4), fid_with_1(4), tol_f);
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::fidelity<RealScalar>(rho1, rho5), fid_with_1(5), tol_f);
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::fidelity<RealScalar>(rho1, rho6), fid_with_1(6), tol_f);
}

BOOST_AUTO_TEST_CASE(fidelityT)
{
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::fidelityT<RealScalar>(T1, T1), fid_with_1(1), tol_f);
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::fidelityT<RealScalar>(T1, T2), fid_with_1(2), tol_f);
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::fidelityT<RealScalar>(T1, T2b), fid_with_1(2), tol_f);
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::fidelityT<RealScalar>(T1, T3), fid_with_1(3), tol_f);
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::fidelityT<RealScalar>(T1, T4), fid_with_1(4), tol_f);
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::fidelityT<RealScalar>(T1, T5), fid_with_1(5), tol_f);
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::fidelityT<RealScalar>(T1, T6), fid_with_1(6), tol_f);
}

BOOST_AUTO_TEST_SUITE_END()

// -----------------------------------------------

BOOST_FIXTURE_TEST_SUITE(qudit4_d, distmeasures_qudit4_fixture<double>)

BOOST_AUTO_TEST_CASE(traceDistance)
{
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::traceDistance<RealScalar>(rho1, rho1), trdist_with_1(1), tol);
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::traceDistance<RealScalar>(rho1, rho2), trdist_with_1(2), tol);
}
BOOST_AUTO_TEST_CASE(fidelity)
{
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::fidelity<RealScalar>(rho1, rho1), fid_with_1(1), tol);
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::fidelity<RealScalar>(rho1, rho2), fid_with_1(2), tol);
}
BOOST_AUTO_TEST_CASE(fidelityT)
{
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::fidelityT<RealScalar>(T1, T1), fid_with_1(1), tol);
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::fidelityT<RealScalar>(T1, T2), fid_with_1(2), tol);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_FIXTURE_TEST_SUITE(qudit4_f, distmeasures_qudit4_fixture<float>)

BOOST_AUTO_TEST_CASE(traceDistance)
{
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::traceDistance<RealScalar>(rho1, rho1), trdist_with_1(1), 32*tol_f);
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::traceDistance<RealScalar>(rho1, rho2), trdist_with_1(2), 32*tol_f);
}
BOOST_AUTO_TEST_CASE(fidelity)
{
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::fidelity<RealScalar>(rho1, rho1), fid_with_1(1), 32*tol_f);
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::fidelity<RealScalar>(rho1, rho2), fid_with_1(2), 32*tol_f);
}
BOOST_AUTO_TEST_CASE(fidelityT)
{
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::fidelityT<RealScalar>(T1, T1), fid_with_1(1), 32*tol_f);
  MY_BOOST_CHECK_FLOATS_EQUAL(Tomographer::DenseDM::fidelityT<RealScalar>(T1, T2), fid_with_1(2), 32*tol_f);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


/* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 *
 * optimizer.cpp
 *
 *  Created on: Jul 8, 2020
 *      Author: Gonzalo Ferrer
 *              g.ferrer@skoltech.ru
 *              Mobile Robotics Lab, Skoltech
 */

#include "mrob/optimizer.hpp"
#include <Eigen/LU> // for inverse and determinant
#include <iostream>

using namespace mrob;

Optimizer::Optimizer(matData_t solutionTolerance, matData_t lambda) :
        solutionTolerance_(solutionTolerance), max_iters_(1e2), lambda_(lambda)
{

}

Optimizer::~Optimizer()
{

}

 uint_t Optimizer::solve(optimMethod method, uint_t max_iters, double lambda)
{
    optimization_method_ = method;
    max_iters_ = max_iters;
    switch(method)
    {
      case NEWTON_RAPHSON:
          return optimize_newton_raphson();
      case LEVENBERG_MARQUARDT_SPHER:
      case LEVENBERG_MARQUARDT_ELLIP:
          lambda_ = lambda;
          return optimize_levenberg_marquardt();
    }
    return 0;
}

uint_t OptimizerDense::optimize_newton_raphson_one_iteration(bool useLambda)
{
    // 1) build problem: Gradient and Hessian and re-evaluates
    calculate_gradient_hessian();
    if (useLambda)
    {
        if (optimization_method_ == LEVENBERG_MARQUARDT_SPHER)
        {
            for (Eigen::Index i = 0; i < hessian_.diagonalSize() ; ++i)
                hessian_(i,i) += lambda_;
        }
        if (optimization_method_ == LEVENBERG_MARQUARDT_ELLIP)
        {
            for (Eigen::Index i = 0; i < hessian_.diagonalSize() ; ++i)
                hessian_(i,i) *= 1.0 + lambda_;
        }
    }

    // 2) dx = - h^-1 * grad XXX test for singularities?
    dx_ = - hessian_.inverse() * gradient_;
    // 3) update the solution
    this->update_state();

    return 1;
}

uint_t Optimizer::optimize_newton_raphson()
{
    uint_t iters = 0;
    // Calculate error also estimates planes, which are necessary for gradients. XXX this can cause bugs on the first iteration
    matData_t previous_error = this->calculate_error(), diff_error;
    do
    {
        this->optimize_newton_raphson_one_iteration(false);
        matData_t current_error = this->calculate_error();
        //std::cout << "iter " << iters << ", error = " << current_error <<std::endl;
        diff_error = previous_error - current_error;
        previous_error = current_error;
        iters++;
    }while(fabs(diff_error) > solutionTolerance_ && iters < max_iters_);

    return iters;
}

uint_t Optimizer::optimize_levenberg_marquardt()
{
    // LM trust region as described in Bertsekas (p.105)
    // sigma reference to the fidelity of the model at the proposed solution \in [0,1]
    matData_t sigma1(0.25), sigma2(0.8);// 0 < sigma1 < sigma2 < 1
    matData_t beta1(2.0), beta2(0.25); // lambda updates multiplier values, beta1 > 1 > beta2 >0
    uint_t iters = 0;
    matData_t previous_error = calculate_error();
    bool improvement; // variable for controlling when no update is done and number of iterations is exceeded.
    do
    {
        iters++;
        // 1) solve the current subproblem by Newton Raphson
        this->bookkeep_state();
        optimize_newton_raphson_one_iteration(true);
        auto current_error = calculate_error();
        //std::cout << "iter " << iters << ", error = " << current_error << ", lambda = "<< lambda_ << std::endl;
        auto diff_error = previous_error - current_error;
        improvement = true;

        // 2) Check for convergence, hillclimb
        if (diff_error < 0)
        {
            //std::cout << "no improvement\n";
            lambda_ *= beta1;
            this->update_state_from_bookkeep();
            improvement = false;
            continue;
        }
        previous_error = current_error;

        // 2.1) check for convergence, terminal
        if (diff_error < solutionTolerance_)
            return iters;

        // 3 Fidelity of the quadratized model vs non-linear error evaluation.
        // f = err(x_k) - err(x_k + dx)  ( >0 if upgrade)
        //     err(x_k) - m_k(dx)
        // where m_k is the quadratized model m_k(dx) = err(x_k) + dx'*Grad r + 0.5 dx'(Hessian + LM)dx
        // => f = d err / (-dx'*Grad r - 0.5 dx'(Hessian + LM)dx)
        //matData_t modelFidelity = diff_error / (-dx_.dot(gradient_) - 0.5*dx_.dot(hessian_* dx_));
        matData_t modelFidelity = calculate_model_fidelity(diff_error);

        // 4) update lambda
        if (modelFidelity < sigma1)
            lambda_ *= beta1;
        if (modelFidelity > sigma2)
            lambda_ *= beta2;

    }while(iters < max_iters_);

    if (!improvement)
    {
        this->update_state_from_bookkeep();//If no improvement shown, undo again
    }

    // output
    std::cout << "Optimizer::optimize_levenberg_marquardt: failed to converge after "
              << iters << " iterations and error " << calculate_error()
              << std::endl;

    return iters;
}

OptimizerDense::OptimizerDense(matData_t solutionTolerance, matData_t lambda):
        Optimizer(solutionTolerance, lambda)
{

}
OptimizerDense::~OptimizerDense()
{

}

matData_t OptimizerDense::calculate_model_fidelity(matData_t diff_error)
{

    return diff_error / (-dx_.dot(gradient_) - 0.5*dx_.dot(hessian_* dx_));
}


#include "Frustum.hpp"
#include <Engine/ShaderManager.hpp>
#include <boost/filesystem.hpp>
#include <iostream>
#include <Engine/Camera.hpp>
#include <Engine/MainGraphic.hpp>
#include <NTL.hpp>

Frustum::Frustum(Camera &camera) :
camera_(camera) {
    setDebug();
}

void Frustum::update()
{
    glm::mat4 mat = camera_.getProjectionMatrix() * camera_.getViewMatrix();

    for (int i = 4; i--; )
        planes_[FRUSTUM_PLANE_LEFT][i] = mat[i][3] + mat[i][0];
    for (int i = 4; i--; )
        planes_[FRUSTUM_PLANE_RIGHT][i] = mat[i][3] - mat[i][0];
    for (int i = 4; i--; )
        planes_[FRUSTUM_PLANE_DOWN][i] = mat[i][3] + mat[i][1];
    for (int i = 4; i--; )
        planes_[FRUSTUM_PLANE_UP][i] = mat[i][3] - mat[i][1];
    for (int i = 4; i--; )
        planes_[FRUSTUM_PLANE_NEAR][i] = mat[i][3] + mat[i][2];
    for (int i = 4; i--; )
        planes_[FRUSTUM_PLANE_FAR][i] = mat[i][3] - mat[i][2];

    updateLines();
}

bool		Frustum::pointIn(float x, float y, float z) {
    for(auto plane : planes_) {
        if (glm::dot(plane, glm::vec4(x, y, z, 1.0f)) < 0) {
            return false;
        }
    }
    return true;
}

void Frustum::initDebug() {
}

void Frustum::updateLines() {
	float ar = 1024.f / 720.f;
	float halfHeight = tanf((3.1415926f / 180.f) * (camera_.fov_ / 2.f));
	float halfWidth = halfHeight * ar;

	float xn = halfWidth * camera_.near_;
	float xf = halfWidth * camera_.far_;
	float yn = halfHeight * camera_.near_;
	float yf = halfHeight * camera_.far_;

/*
    glm::mat4 inverse = glm::inverse(view);

    glm::vec4 f[8u] =
            {
                    // near face
                    {xn, yn, -camera_.near_, 1.f},
                    {-xn, yn, -camera_.near_, 1.f},
                    {xn, -yn, -camera_.near_, 1.f},
                    {-xn, -yn, -camera_.near_ , 1.f},

                    // far face
                    {xf, yf, -camera_.far_, 1.f},
                    {-xf, yf, -camera_.far_ , 1.f},
                    {xf, -yf, -camera_.far_ , 1.f},
                    {-xf, -yf, -camera_.far_, 1.f},
            };

    glm::vec3 v[8];
    for (int i = 0; i < 8; i++)
    {
        glm::vec4 ff = inverse * f[i];
        v[i].x = ff.x / ff.w;
        v[i].y = ff.y / ff.w;
        v[i].z = ff.z / ff.w;
    }

    glm::vec3 lines[] = {
            v[0], v[1],
            v[0], v[2],
            v[3], v[1],
            v[3], v[2],
            v[4], v[5],
            v[4], v[6],
            v[7], v[5],
            v[7], v[6],
            v[0], v[4],
            v[1], v[5],
            v[3], v[7],
            v[2], v[6]
    };
*/

	/// Converstion du Clip Space en World Space
	/// ClipSpace   - > View Space  = ClipSpace   *  INV(Projection Matrix)
	/// View Space  - > World Space = View Space  *  INV(View Matrix)
	///
	glm::mat4 inv = glm::inverse(camera_.getViewMatrix()) * glm::inverse(camera_.getProjectionMatrix());
	glm::vec4 ndc[8] =
			{
					// near face
					{ 1, 1, -1, 1.f },
					{ -1, 1, -1, 1.f },
					{ 1, -1, -1, 1.f },
					{ -1, -1, -1, 1.f },

					// far face
					{ 1, 1, 1, 1.f},
					{ -1, 1, 1, 1.f},
					{ 1, -1, 1, 1.f},
					{ -1, -1, 1, 1.f},
			};

	glm::vec3 v[8];
	for (int i = 0; i < 8; i++)
	{
		glm::vec4 wc = inv * ndc[i];

		///Homogenous to cartesian conversion
		v[i].x = wc.x / wc.w;
		v[i].y = wc.y / wc.w;
		v[i].z = wc.z / wc.w;
	}

    linesObject_.clear();
    linesObject_.reserve(24);
	linesObject_.emplace_back(v[0], color);
	linesObject_.emplace_back(v[1], color);
	linesObject_.emplace_back(v[0], color);
	linesObject_.emplace_back(v[2], color);
	linesObject_.emplace_back(v[3], color);
	linesObject_.emplace_back(v[1], color);
	linesObject_.emplace_back(v[3], color);
	linesObject_.emplace_back(v[2], color);
	linesObject_.emplace_back(v[4], color);
	linesObject_.emplace_back(v[5], color);
	linesObject_.emplace_back(v[4], color);
	linesObject_.emplace_back(v[6], color);
	linesObject_.emplace_back(v[7], color);
	linesObject_.emplace_back(v[5], color);
	linesObject_.emplace_back(v[7], color);
	linesObject_.emplace_back(v[6], color);
	linesObject_.emplace_back(v[0], color);
	linesObject_.emplace_back(v[4], color);
	linesObject_.emplace_back(v[1], color);
	linesObject_.emplace_back(v[5], color);
	linesObject_.emplace_back(v[3], color);
	linesObject_.emplace_back(v[7], color);
	linesObject_.emplace_back(v[2], color);
	linesObject_.emplace_back(v[6], color);

    updateDebug();
}


#include <shark/Algorithms/Trainers/LinearRegression.h>
#include <shark/Models/LinearModel.h>
#include <boost/archive/polymorphic_binary_iarchive.hpp>
#include <boost/archive/polymorphic_binary_oarchive.hpp>
using namespace shark;

double func(double x) {
  return 4. + 0.3 * x;  // line coeficients
}

std::pair<Data<RealVector>, Data<RealVector>> GenerateData(size_t n) {
  std::vector<RealVector> x_data(n);
  std::vector<RealVector> y_data(n);

  std::random_device rd;
  std::mt19937 re(rd());
  std::uniform_real_distribution<double> dist(-1.5, 1.5);

  // generate data
  RealVector x_v(1);  // it's a typdef to remora::vector<float>
  RealVector y_v(1);
  for (size_t i = 0; i < n; ++i) {
    x_v(0) = i;
    x_data[i] = x_v;

    y_v(0) = func(i) + dist(re);  // add noise
    y_data[i] = y_v;
  }

  return {createDataFromRange(x_data), createDataFromRange(y_data)};
}

int main() {
  {
    Data<RealVector> x;
    Data<RealVector> y;
    std::tie(x, y) = GenerateData(1000);
    RegressionDataset data(x, y);
    LinearModel<> model;
    LinearRegression trainer;
    trainer.train(model, data);

    std::ofstream ofs("shark-linear.dat");
    // boost::archive::polymorphic_text_oarchive oa(ofs);
    boost::archive::polymorphic_binary_oarchive oa(ofs);
    model.write(oa);
  }

  std::ifstream ifs("shark-linear.dat");
  // boost::archive::polymorphic_text_iarchive ia(ifs);
  boost::archive::polymorphic_binary_iarchive ia(ifs);
  LinearModel<> model;
  model.read(ia);

  std::cout << "Target values: \n";
  std::vector<RealVector> new_x_data;
  for (size_t i = 0; i < 5; ++i) {
    new_x_data.push_back({static_cast<double>(i)});
    std::cout << func(i) << std::endl;
  }
  auto prediction = model(createDataFromRange(new_x_data));
  std::cout << "Predictions: \n" << prediction << std::endl;

  return 0;
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2019 - 2021.
//  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)
//

#include <string>

#if defined(__GNUC__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wconversion"
#endif

#include <boost/lexical_cast.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/multiprecision/uintwide_t_backend.hpp>

#include <math/wide_integer/uintwide_t.h>
#include <math/wide_integer/uintwide_t_test.h>

namespace
{
  constexpr std::size_t local_digits2 = 16384U;
}

using local_uint_type =
  boost::multiprecision::number<boost::multiprecision::uintwide_t_backend<local_digits2>,
                                boost::multiprecision::et_off>;

using boost_uint_backend_type =
  boost::multiprecision::cpp_int_backend<local_digits2,
                                         local_digits2,
                                         boost::multiprecision::unsigned_magnitude>;

using boost_uint_type =
  boost::multiprecision::number<boost_uint_backend_type,
                                boost::multiprecision::et_off>;

bool math::wide_integer::test_uintwide_t_edge_cases()
{
  const local_uint_type u_max_local = (std::numeric_limits<local_uint_type>::max)();
  const boost_uint_type u_max_boost = (std::numeric_limits<boost_uint_type>::max)();

  local_uint_type result_local;
  boost_uint_type result_boost;

  result_local = u_max_local * u_max_local;
  result_boost = u_max_boost * u_max_boost;

  const bool result01_is_ok = ((result_local == 1U) && (result_boost == 1U));

  result_local = (u_max_local - 1U) * u_max_local;
  result_boost = (u_max_boost - 1U) * u_max_boost;

  const bool result02_is_ok = ((result_local == 2U) && (result_boost == 2U));

  const std::string str_seven_and_effs =
    "0x7" + std::string(std::string::size_type((local_digits2 / 4) - 1U), char('F'));

  const local_uint_type u_seven_and_effs_local(str_seven_and_effs.c_str());
  const boost_uint_type u_seven_and_effs_boost(str_seven_and_effs.c_str());

  result_local = u_seven_and_effs_local * u_seven_and_effs_local;
  result_boost = u_seven_and_effs_boost * u_seven_and_effs_boost;

  const bool result03_is_ok = (result_local.convert_to<std::string>() == result_boost.convert_to<std::string>());

  const std::string str_three_quarter_effs_and_zeros =
      "0x"
    + std::string(std::string::size_type((local_digits2 / 4) * 3U), char('F'))
    + std::string(std::string::size_type((local_digits2 / 4) * 1U), char('0'))
    ;

  const local_uint_type u_three_quarter_effs_and_zeros_local(str_three_quarter_effs_and_zeros.c_str());
  const boost_uint_type u_three_quarter_effs_and_zeros_boost(str_three_quarter_effs_and_zeros.c_str());

  result_local = u_three_quarter_effs_and_zeros_local * u_three_quarter_effs_and_zeros_local;
  result_boost = u_three_quarter_effs_and_zeros_boost * u_three_quarter_effs_and_zeros_boost;

  const bool result04_is_ok = (result_local.convert_to<std::string>() == result_boost.convert_to<std::string>());

  const std::string str_one_quarter_effs_and_zeros =
      "0x"
    + std::string(std::string::size_type((local_digits2 / 4) * 1U), char('F'))
    + std::string(std::string::size_type((local_digits2 / 4) * 3U), char('0'))
    ;

  const local_uint_type u_one_quarter_effs_and_zeros_local(str_one_quarter_effs_and_zeros.c_str());
  const boost_uint_type u_one_quarter_effs_and_zeros_boost(str_one_quarter_effs_and_zeros.c_str());

  result_local = u_one_quarter_effs_and_zeros_local * u_one_quarter_effs_and_zeros_local;
  result_boost = u_one_quarter_effs_and_zeros_boost * u_one_quarter_effs_and_zeros_boost;

  const bool result05_is_ok = (result_local.convert_to<std::string>() == result_boost.convert_to<std::string>());

  const local_uint_type one_limb_effs_prior_to_half_and_zeros_local(local_uint_type(UINT32_C(0xFFFFFFFF)) << ((std::numeric_limits<local_uint_type>::digits / 2) - 32));
  const boost_uint_type one_limb_effs_prior_to_half_and_zeros_boost(boost_uint_type(UINT32_C(0xFFFFFFFF)) << ((std::numeric_limits<boost_uint_type>::digits / 2) - 32));

  result_local = one_limb_effs_prior_to_half_and_zeros_local * one_limb_effs_prior_to_half_and_zeros_local;
  result_boost = one_limb_effs_prior_to_half_and_zeros_boost * one_limb_effs_prior_to_half_and_zeros_boost;

  const bool result06_is_ok = (result_local.convert_to<std::string>() == result_boost.convert_to<std::string>());

  const local_uint_type u_mid_local = u_three_quarter_effs_and_zeros_local / typename local_uint_type::backend_type::representation_type::limb_type(2U);
  const boost_uint_type u_mid_boost = u_three_quarter_effs_and_zeros_boost / typename std::iterator_traits<boost_uint_type::backend_type::limb_pointer>::value_type(2U);

  constexpr int signed_shift_amount =
    -(std::numeric_limits<typename local_uint_type::backend_type::representation_type::limb_type>::digits + 7);

  result_local = u_mid_local;
  result_local.backend().representation() >>= signed_shift_amount;
  result_boost = u_mid_boost * (boost_uint_type(1U) << (-signed_shift_amount));

  const bool result07_is_ok = (result_local.convert_to<std::string>() == result_boost.convert_to<std::string>());

  result_local = u_mid_local;
  result_local.backend().representation() <<= signed_shift_amount;
  result_boost = u_mid_boost / (boost_uint_type(1U) << (-signed_shift_amount));

  const bool result08_is_ok = (result_local.convert_to<std::string>() == result_boost.convert_to<std::string>());

  const bool result_is_ok = (   result01_is_ok
                             && result02_is_ok
                             && result03_is_ok
                             && result04_is_ok
                             && result05_is_ok
                             && result06_is_ok
                             && result07_is_ok
                             && result08_is_ok);

  return result_is_ok;
}

#if defined(__GNUC__)
#pragma GCC diagnostic pop
#endif


//File: homography.cc
//Author: Yuxin Wu <ppwwyyxx@gmail.com>

#include "homography.hh"

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

#include "lib/matrix.hh"
#include "lib/polygon.hh"
#include "match_info.hh"

using namespace std;

namespace {
inline Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>
	to_eigenmap(const pano::Homography& m) {
		return Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>(
				(double*)m.data, 3, 3);
	}
}

namespace pano {

Homography Homography::inverse(bool* succ) const {
	using namespace Eigen;
	Homography ret;
	auto res = to_eigenmap(ret),
			 input = to_eigenmap(*this);
	FullPivLU<Eigen::Matrix<double,3,3,RowMajor>> lu(input);
	if (succ == nullptr) {
		m_assert(lu.isInvertible());
	} else {
		*succ = lu.isInvertible();
		if (! *succ) return ret;
	}
	res = lu.inverse().eval();
	return ret;
}

Homography Homography::operator * (const Homography& r) const {
	Homography ret;
	auto m1 = to_eigenmap(*this),
			 m2 = to_eigenmap(r),
			 res = to_eigenmap(ret);
	res = m1 * m2;
	return ret;
}

std::vector<Vec2D> overlap_region(
		const Shape2D& shape1, const Shape2D& shape2,
		const Matrix& homo, const Homography& inv) {
	// use sampled edge points, rather than 4 corner, to deal with distorted homography
	// for distorted homography, the range of projected z coordinate contains 0
	// or equivalently, some point is projected to infinity
	const int NR_POINT_ON_EDGE = 100;
	Matrix edge_points(3, 4 * NR_POINT_ON_EDGE);
	float stepw = shape2.w * 1.0 / NR_POINT_ON_EDGE,
				steph = shape2.h * 1.0 / NR_POINT_ON_EDGE;
	REP(i, NR_POINT_ON_EDGE) {
		Vec2D p{-shape2.halfw() + i * stepw, -shape2.halfh()};
		edge_points.at(0, i * 4) = p.x, edge_points.at(1, i * 4) = p.y;
		p = Vec2D{-shape2.halfw() + i * stepw, shape2.halfh()};
		edge_points.at(0, i * 4 + 1) = p.x, edge_points.at(1, i * 4 + 1) = p.y;
		p = Vec2D{-shape2.halfw(), -shape2.halfh() + i * steph};
		edge_points.at(0, i * 4 + 2) = p.x, edge_points.at(1, i * 4 + 2) = p.y;
		p = Vec2D{shape2.halfw(), -shape2.halfh() + i * steph};
		edge_points.at(0, i * 4 + 3) = p.x, edge_points.at(1, i * 4 + 3) = p.y;
	}
	REP(i, 4 * NR_POINT_ON_EDGE)
		edge_points.at(2, i) = 1;
	auto transformed_pts = homo * edge_points;	//3x4n
	vector<Vec2D> pts2in1;
	REP(i, 4 * NR_POINT_ON_EDGE) {
		float denom = 1.0 / transformed_pts.at(2, i);
		Vec2D pin1{transformed_pts.at(0, i) * denom, transformed_pts.at(1, i) * denom};
		if (shape1.shifted_in(pin1))
			pts2in1.emplace_back(pin1);
	}

	// also add 4 corner of 1 to build convex hull, in case some are valid
	auto corners = shape1.shifted_corner();
	for (auto& c : corners) {
		Vec2D cin2 = inv.trans2d(c);
		if (shape2.shifted_in(cin2))
			pts2in1.emplace_back(c);
	}
	auto ret = convex_hull(pts2in1);
	return ret;
}

}


#include "ros/ros.h"
#include "geometry_msgs/PoseStamped.h"
#include "nav_msgs/Path.h"
#include <opencv2/opencv.hpp>
#include <Eigen/Dense>
#include <chrono>

#include <vector>
using namespace std;
/**
 *  这个是nav_msgs::Path版本的,用的是最原始的策略:OpenCV从中间向两边进行扫描
 *
 *  @author:panguoping
 *  @version:v3.0
 *
 *  @functions:
 *      - transTo3D
 *      - pubMiddleline
 *      - pubMiddleline
 *
 *  @tips: 相比v2.0添加上了以下参数，同时改了main函数
 *      int left_begin;
 *      int left_end;
 *      int right_begin;
 *      int right_end;
 *
 *
 *
 * */

/** 参数定义 **/
const float fx = 760.4862674784594; // 相机内参
const float fy = 761.4971958529285;
const float cx = 631.6715834996345;
const float cy = 329.3054436037627;

const float y = 129.5585;   // 固定相机离地面高度，单位mm，通过计算得到，不一定准

float offset;
float thetaX;
float thetaY;
float scale;
int imgWidth;  // 图片大小
int imgHeight;
int left_begin;
int left_end;
int right_begin;
int right_end;

ros::Publisher pathpub;

/** 将图片中的 点uv 转化为 现实坐标中的 点xy **/
cv::Point3f getXYZ(cv::Point2f& point){
    cv::Point3f point3d;
    point3d.x = (fy * y/(point.y-cy))/scale;
    point3d.y = (-(point.x-cx)*point3d.x/fx);   //todo shan chu /scale
    point3d.z = 0.0;
    return point3d;
}

/** 将二维边线转化为三维边线 **/
void transTo3D(vector<cv::Point2f>& uv,vector<cv::Point3f>& posearray){
    for(auto p:uv){
        cv::Point3f pose = getXYZ(p);
        posearray.push_back(pose);
    }
}

/** 画出中间线 **/
void pubMiddleline(vector<cv::Point2f>& uvLeft,vector<cv::Point2f>& uvRight){
    vector<cv::Point3f> xyLeft,xyRight;
    nav_msgs::Path path;
    transTo3D(uvLeft,xyLeft);
    transTo3D(uvRight,xyRight);
    ROS_INFO("int two middle");
    bool lbigthanr = xyLeft.size() > xyRight.size() ? true:false;
    int ir = 0;
    int il = 0;
    if(lbigthanr){
        while(il<xyLeft.size()&&abs(xyLeft[il].x-xyRight[ir].x)>thetaX){
            ++il;
        }
    } else{
        while(ir<xyRight.size()&&abs(xyLeft[il].x-uvRight[ir].x)>thetaX){
            ++ir;
        }
    }
    ROS_INFO("sizer:%d,sizel:%d",xyRight.size(),xyLeft.size());
//    ROS_INFO("ir:%d,il:%d",ir,il);
    ROS_INFO("sizer1:%d,sizel1:%d",xyRight.size()-5,xyLeft.size()-5);
    ROS_INFO("ir1:%d,il1:%d",ir,il);

    for(ir,il; il<int(xyLeft.size()-5)&&ir<int(uvRight.size()-5);++ir,++il){    //this qiang zhi zhuan huan yongyuande shen
        ROS_INFO("ir:%d,il:%d",ir,il);
//        if(ir>100){
//            while (1);
//        }
        if(abs(xyLeft[il].x-uvRight[ir].x)>thetaX
           && abs(xyLeft[il+1].y-xyLeft[il].y)>thetaY   //todo cha zhi
           && abs(xyRight[ir+1].y-xyRight[ir].y)>thetaY)
            continue;
        ROS_INFO("yes");

        geometry_msgs::PoseStamped pose;
        pose.pose.position.x = (xyLeft[il].x+xyRight[ir].x)/2;
        pose.pose.position.y = (xyLeft[il].y+xyRight[ir].y)/2;
        pose.pose.position.z = 0.0;
        path.poses.push_back(pose);
    }
    ROS_INFO("pathpose:%d",path.poses.size());
    path.header.stamp = ros::Time::now();
    path.header.frame_id = "camera";

    pathpub.publish(path);
    ROS_INFO("publish successfully");
}
/** 画出中间线 **/
void pubMiddleline(vector<cv::Point2f>& uv){
    vector<cv::Point3f> xy;
    nav_msgs::Path path;
    transTo3D(uv,xy);
    for(auto p:xy){
        if(p.y>0.1){
            geometry_msgs::PoseStamped pose;
            pose.pose.position.x = p.x;
            pose.pose.position.y = p.y-offset;
            pose.pose.position.z = 0.0;
            path.poses.push_back(pose);
        }
        else if(p.y<-0.1){
            geometry_msgs::PoseStamped pose;
            pose.pose.position.x = p.x;
            pose.pose.position.y = p.y+offset;
            pose.pose.position.z = 0.0;
            path.poses.push_back(pose);
        }
        else{
            continue;
        }
    }
    ROS_INFO("pathpose:%d",path.poses.size());
    path.header.stamp = ros::Time::now();
    path.header.frame_id = "camera";
    pathpub.publish(path);
    ROS_INFO("publish successfully");
}

int main(int argc ,char **argv){
    ros::init(argc,argv,"linktracking");
    ros::NodeHandle nh("~");
    ros::Time t1 = ros::Time::now();

    pathpub = nh.advertise<nav_msgs::Path>("path",0);
    string video;
    nh.param<string>("video",video,"../dataset/test.mp4");
    nh.param<float>("offset",offset,0.45);
    nh.param<float>("thetaX",thetaX,0.2);
    nh.param<float>("thetaY",thetaY,0.1);
    nh.param<float>("scale",scale,1000.0);
    nh.param<int>("imgWidth",imgWidth,1280);
    nh.param<int>("imgHeight",imgHeight,720);
    nh.param<int>("left_begin",left_begin,imgHeight/2);
    nh.param<int>("left_end",left_end,imgHeight);
    nh.param<int>("right_begin",right_begin,imgWidth/2);
    nh.param<int>("right_end",right_end,imgHeight);

    /** 1.get the video **/
    cv::VideoCapture cap = cv::VideoCapture(video);
    cap.set(CV_CAP_PROP_FRAME_WIDTH, imgWidth);
    cap.set(CV_CAP_PROP_FRAME_HEIGHT, imgHeight);
    cap.set(cv::CAP_PROP_FPS,30);
    // LineState flag;
    ROS_INFO("get");
    ROS_INFO("%s",video.c_str());
    cv::Mat frame;

    while (cap.isOpened()&&ros::ok()) {
        /** 2.get frame and turn to binary **/
        cap.read(frame);
        cv::Mat gray;
//            ROS_INFO("frame.channel: %d",frame.channels());

//            if(frame.channels()==1){
//                ROS_INFO("use time : %f",(ros::Time::now()-t1).toSec());
//                cv::imshow("frame",frame);
//                cv::waitKey(0);
//                continue;
//            }
        cv::cvtColor(frame, gray, CV_RGB2GRAY);
        cv::Mat binary;
        cv::threshold(gray, binary, 0, 255, CV_THRESH_OTSU);
        cv::imshow("frame",binary);
        cv::waitKey(33);
        // cv::erode(binary, binary, cv::Mat(), cv::Point(-1, -1), 2);
//            cv::imshow("show", binary);
//            cv::waitKey(0);

        /** 3.get point of two side **/
        int vline = binary.cols / 2;    //todo verticalThres
        int pline = binary.rows * 2 / 3 - 10; //todo parallelThres and bias
        vector<cv::Point2f> uvRight;
        vector<cv::Point2f> uvLeft;
        // right side
        for (int i = right_begin; i < right_end; ++i) {
            uchar *data = binary.ptr<uchar>(i);
            for (int j = vline; j < binary.cols - 5; ++j) {
                if (data[j] == 0
                    && data[j + 1] == 255&&data[j+2]==255
                    &&data[j+3]==255&&data[j+4]==255) {

                    uvRight.push_back(cv::Point2f(j, i));
                    break;
                }
            }
        }
        // left side
        for (int i = left_begin; i < left_end; ++i) {
            uchar *data = binary.ptr<uchar>(i);
            for (int j = vline; j > 5; --j) {
                if (data[j] == 0
                    && data[j - 1] == 255&& data[j - 2] == 255
                    && data[j - 3] == 255&& data[j - 4] == 255) {
                    uvLeft.push_back(cv::Point2f(j, i));
                    break;
                }
            }
        }
        ROS_INFO("right.size = %d,left.size = %d",uvRight.size(),uvLeft.size());
        /** 4.transport **/
        if(uvLeft.size()>10&&uvRight.size()>10){  //todo sizeThres
            pubMiddleline(uvLeft,uvRight);
        }
        else if(uvLeft.size()>10){
            pubMiddleline(uvLeft);
        }
        else if(uvRight.size()>10){
            pubMiddleline(uvRight);
        }
    }
    ROS_INFO("can't load the video!");
    return 0;
}


#include <CGAL/Cartesian.h>
#include <CGAL/Point_2.h>
#include <CGAL/Segment_2.h>
#include <CGAL/intersections.h>
#include <CGAL/tuple.h>
#include <CGAL/point_generators_2.h>
#include <CGAL/Join_input_iterator.h>
#include <CGAL/iterator.h>

#include <vector>
#include <functional>

#include <boost/variant.hpp>
#include <boost/optional.hpp>
#include <boost/any.hpp>
#include <boost/timer.hpp>
#include <boost/lexical_cast.hpp>

#include <tuple>
#include <functional>
#include <CGAL/Overload.h>

// Intersection_traits
template<typename, typename, typename>
struct Intersection_traits;

template<typename K>
struct Intersection_traits<K, typename K::Segment_2, typename K::Segment_2> {
  typedef typename boost::variant<typename K::Segment_2, typename K::Point_2 > variant_type;
  typedef typename boost::optional< variant_type > result_type;
};

template <class K, class OutputIterator>
OutputIterator intersect_do_iterator(const typename K::Segment_2 &seg1,
                           const typename K::Segment_2 &seg2,
                           const K&, OutputIterator o) {
  typedef CGAL::internal::Segment_2_Segment_2_pair<K> is_t;

  is_t ispair(&seg1, &seg2);
  switch (ispair.intersection_type()) {
  case is_t::NO_INTERSECTION:
  default:
    return o;
  case is_t::POINT:
    *o++ = ispair.intersection_point();
    return o;
  case is_t::SEGMENT:
    *o++ = ispair.intersection_segment();
    return o;
  }
}

template <class K>
boost::optional<
  boost::variant<typename K::Segment_2, typename K::Point_2>
  >
intersection_variant(const typename K::Segment_2 &seg1,
             const typename K::Segment_2 &seg2,
             const K&)
{
  typedef CGAL::internal::Segment_2_Segment_2_pair<K> is_t;

  typedef boost::variant<typename K::Segment_2, typename K::Point_2> Variant;
  typedef boost::optional<Variant> OptVariant;

  is_t ispair(&seg1, &seg2);
  switch (ispair.intersection_type()) {
  case is_t::NO_INTERSECTION:
  default:
    return OptVariant();
  case is_t::POINT:
    return OptVariant(ispair.intersection_point());
  case is_t::SEGMENT:
    return OptVariant(ispair.intersection_segment());
  }
}

template <class K>
boost::any intersection_any(const typename K::Segment_2 &seg1,
                            const typename K::Segment_2 &seg2,
                            const K&)
{
  typedef CGAL::internal::Segment_2_Segment_2_pair<K> is_t;

  is_t ispair(&seg1, &seg2);
  switch (ispair.intersection_type()) {
  case is_t::NO_INTERSECTION:
  default:
    return boost::any();
  case is_t::POINT:
    return boost::any(ispair.intersection_point());
  case is_t::SEGMENT:
    return boost::any(ispair.intersection_segment());
  }
}

using namespace CGAL;

typedef Cartesian<double>    K;
typedef K::Point_2           Point;
typedef Creator_uniform_2<double,Point>  Pt_creator;
typedef K::Segment_2         Segment;
typedef std::vector<Segment> Vector;

template<typename F>
void intersect_each(F f, const Vector& segs) {
  for(Vector::const_iterator it = segs.begin(); it != segs.end(); ++it) {
      const Segment& seg_1 = *it;
      for(Vector::const_iterator it2 = segs.begin(); it2 != segs.end(); ++it2) {
        f(seg_1, *it2);
      }
  }
}

struct Vec_holder {
  Vec_holder(std::vector<Point>* p, std::vector<Segment>* s) : p(p), s(s) { }
protected:
  std::vector<Point>* p;
  std::vector<Segment>* s;
};

struct Visitor : public boost::static_visitor<>, Vec_holder
{
  Visitor(std::vector<Point>* p, std::vector<Segment>* s) :
    Vec_holder(p, s) { }

  void operator()(const Point& point) { p->push_back(point);  }
  void operator()(const Segment& segment) { s->push_back(segment);  }
};

struct Variant_f {
  Variant_f(std::vector<Point>* p, std::vector<Segment>* s) : v(p, s)
    { }
  typedef Intersection_traits<K, Segment, Segment> Traits;
  typedef Traits::result_type result_type;

  Visitor v;
  void operator()(const Segment& s1, const Segment& s2) {
    result_type obj = intersection_variant(s1, s2, K());
    if(obj) {
      boost::apply_visitor(v, *obj);
    }
  }
};

struct Object_f : Vec_holder {
  Object_f(std::vector<Point>* p, std::vector<Segment>* s) :
    Vec_holder(p, s) { }

  void operator()(const Segment& s1, const Segment& s2) {
    Object obj = intersection(s1, s2);
      if (const Point * point = object_cast<Point>(&obj)) {
        p->push_back(*point);
      } else if (const Segment * segment = object_cast<Segment>(&obj)) {
        s->push_back(*segment);
      }
  }
};

struct Any_f : Vec_holder {
  Any_f(std::vector<Point>* p, std::vector<Segment>* s) :
    Vec_holder(p, s) { }

  void operator()(const Segment& s1, const Segment& s2) {
    boost::any obj = intersection_any(s1, s2, K());
    if (const Point * point = boost::any_cast<Point>(&obj)) {
       p->push_back(*point);
    } else if (const Segment * segment = boost::any_cast<Segment>(&obj)) {
      s->push_back(*segment);
    }
  }
};

struct Object_from_variant_f : Vec_holder {
  Object_from_variant_f(std::vector<Point>* p, std::vector<Segment>* s) :
    Vec_holder(p, s) { }

  void operator()(const Segment& s1, const Segment& s2) {
    Object obj = intersection_variant(s1, s2, K());
      if (const Point * point = object_cast<Point>(&obj)) {
        p->push_back(*point);
      } else if (const Segment * segment = object_cast<Segment>(&obj)) {
        s->push_back(*segment);
      }
  }
};

struct Do_f : Vec_holder {
  Do_f(std::vector<Point>* p, std::vector<Segment>* s) :
    Vec_holder(p, s) { }

  typedef typename std::back_insert_iterator< std::vector<Point> >   Iter1;
  typedef typename std::back_insert_iterator< std::vector<Segment> > Iter2;

  void operator()(const Segment& s1, const Segment& s2) {

    CGAL::Dispatch_or_drop_output_iterator<std::tuple<Point,Segment>,
                                           std::tuple<Iter1,Iter2>
                                           > do_it(std::back_inserter(*p), std::back_inserter(*s));

    intersect_do_iterator(s1, s2, K(), do_it);
  }
};

std::tuple<int, int, int> intersect_each_variant_overload(const Vector& segs) {
  std::tuple<int, int, int> ret = std::make_tuple(0, 0, 0);
  typedef Intersection_traits<K, Segment, Segment> Traits;
  typedef Traits::result_type result_type;

  // Calculate the intersections between each segment
  for(Vector::const_iterator it = segs.begin(); it != segs.end(); ++it) {
    const Segment& seg_1 = *it;
    for(Vector::const_iterator it2 = segs.begin(); it2 != segs.end(); ++it2) {
      result_type obj = intersection_variant(seg_1, *it2, K());
      if(obj) {
         // with c++0x
        auto v = make_overload(
          std::make_tuple(std::function<void(const Segment&)>(
                            [&ret](const Segment& s) { (void)s; ++(std::get<1>(ret)); }),
                          std::function<void(const Point&)>(
                            [&ret](const Point& p) { (void)p; ++(std::get<0>(ret)); })));

        boost::apply_visitor(v, *obj);
      } else {
        ++(std::get<2>(ret));
      }
    }
  }

  return ret;
}

int main(int argc, char* argv[]) {
  int repeats = 100;
  int seg_count = 200;

  if(argc > 1)
    repeats = boost::lexical_cast<int>(argv[1]);
  if(argc > 2)
    seg_count = boost::lexical_cast<int>(argv[2]);

  // Create test segment set. Prepare a vector for 200 segments.
  Vector segs;
  segs.reserve(200);

  // Prepare point generator for the horizontal segment, length 200.
  typedef Random_points_on_segment_2<Point,Pt_creator> P1;
  P1 p1( Point(-100,0), Point(100,0));

  // Prepare point generator for random points on circle, radius 250.
  typedef Random_points_on_circle_2<Point,Pt_creator> P2;
  P2 p2( 250);

  // Create segments.
  typedef Creator_uniform_2< Point, Segment> Seg_creator;
  typedef Join_input_iterator_2< P1, P2, Seg_creator> Seg_iterator;
  Seg_iterator g( p1, p2);
  CGAL::copy_n( g, seg_count, std::back_inserter(segs));

  std::vector<Point> points;
  std::vector<Segment> segments;
  points.clear(); segments.clear(); points.reserve(0); segments.reserve(0);
  //one run to get the size
  intersect_each(Variant_f(&points, &segments), segs);

  boost::timer timer;

  // variant vs object vs any

  points.clear(); segments.clear();
  timer.restart();

  for(int i = 0; i < repeats; ++i) {
    intersect_each(Object_f(&points, &segments), segs);
    points.clear(); segments.clear();
  }
  std::cout << "Time for object: " << timer.elapsed() << '\n';

  points.clear(); segments.clear();
  timer.restart();

  for(int i = 0; i < repeats; ++i) {
    intersect_each(Variant_f(&points, &segments), segs);
    points.clear(); segments.clear();
  }
  std::cout << "Time for variant: " << timer.elapsed() << '\n';

  points.clear(); segments.clear();
  timer.restart();

  for(int i = 0; i < repeats; ++i) {
    intersect_each(Any_f(&points, &segments), segs);
    points.clear(); segments.clear();
  }
  std::cout << "Time for any: " << timer.elapsed() << '\n';

  points.clear(); segments.clear();
  timer.restart();

  for(int i = 0; i < repeats; ++i) {
    intersect_each(Object_from_variant_f(&points, &segments), segs);
    points.clear(); segments.clear();
  }
  std::cout << "Time for object_from_variant: " << timer.elapsed() << '\n';

  points.clear(); segments.clear();
  timer.restart();

  for(int i = 0; i < repeats; ++i) {
    intersect_each(Do_f(&points, &segments), segs);
    points.clear(); segments.clear();
  }
  std::cout << "Time for dispatch_output: " << timer.elapsed() << '\n';

  std::cout << std::flush;
}


#include "quadeigs.hpp"
#include "soar.hpp"

#include <Eigen/Dense>
#include <cmath>
#include <fmt/format.h>

using namespace Eigen;

QuadEigs::QuadEigs(const Ref<const MatrixXd> &matM,
                   const Ref<const MatrixXd> &matD,
                   const Ref<const MatrixXd> &matK)
    : ndim_(matM.cols()), matM_(matM), matD_(matD), matK_(matK),
      matA_(ndim_, ndim_), matB_(ndim_, ndim_) {
  MatrixXd matMi = matM.inverse();
  matA_ = -matMi * matD_;
  matB_ = -matMi * matK_;
}

VectorXcd QuadEigs::eigenvalues(int m) {
  Soar soar(matA_, matB_);
  MatrixXd matQm = soar.compute(m);
  MatrixXd matMm = matQm.transpose() * matM_ * matQm;
  MatrixXd matDm = matQm.transpose() * matD_ * matQm;
  MatrixXd matKm = matQm.transpose() * matK_ * matQm;

  MatrixXd matC(2 * m, 2 * m);
  MatrixXd matG(2 * m, 2 * m);
  MatrixXd mzero = MatrixXd::Zero(m, m);
  MatrixXd mone = MatrixXd::Identity(m, m);
  matC << -matDm, -matKm, mone, mzero;
  matG << matMm, mzero, mzero, mone;

  GeneralizedEigenSolver<MatrixXd> ges(matC, matG, false);
  VectorXcd alphas = ges.alphas();
  VectorXd betas = ges.betas();

  fmt::print("{:>30s}, {:>15s}\n", "alphas", "betas");
  for (int i = 0; i < m; ++i) {
    if (std::abs(betas(i)) > 1.0e-8) {
      fmt::print("{:14.5f}+{:14.5f}i\n", alphas(i).real() / betas(i),
                 alphas(i).imag() / betas(i));
    }
  }

  VectorXcd ret(m);
  return ret;
}


#define CATCH_CONFIG_MAIN
#include <catch2/catch.hpp>

#include "../banquet.h"
#include "utils.h"
#include <NTL/GF2E.h>
#include <NTL/GF2EX.h>

TEST_CASE("Precomputed Lagrange Interpolation", "[util]") {
  const banquet_instance_t &instance = banquet_instance_get(Banquet_L1_Param1);
  utils::init_extension_field(instance);
  size_t dimension = 20;
  vec_GF2E x_values;
  vec_GF2E y_values1;
  vec_GF2E y_values2;
  for (size_t i = 0; i < dimension; i++) {
    x_values.append(random_GF2E());
    y_values1.append(random_GF2E());
    y_values2.append(random_GF2E());
  }

  // builtin interpolate
  GF2EX poly1 = interpolate(x_values, y_values1);
  GF2EX poly2 = interpolate(x_values, y_values2);
  // precomputed interpolate
  auto precomputation = utils::precompute_lagrange_polynomials(x_values);
  GF2EX poly1_with_precom =
      utils::interpolate_with_precomputation(precomputation, y_values1);
  GF2EX poly2_with_precom =
      utils::interpolate_with_precomputation(precomputation, y_values2);

  REQUIRE(poly1 == poly1_with_precom);
  REQUIRE(poly2 == poly2_with_precom);
}

TEST_CASE("Basic Lifting tests", "[util]") {
  const banquet_instance_t &instance = banquet_instance_get(Banquet_L1_Param1);
  utils::init_extension_field(instance);
  uint8_t a = 3;
  uint8_t b = 246;

  GF2E a_lifted, b_lifted;
  GF2X tmp;
  // a_lifted should be y^30 + y^23 + y^21 + y^18 + y^14 + y^13 + y^11 + y^9 +
  // y^7 + y^6 + y^5 + y^4 + y^3 + y + 1
  SetCoeff(tmp, 30);
  SetCoeff(tmp, 23);
  SetCoeff(tmp, 21);
  SetCoeff(tmp, 18);
  SetCoeff(tmp, 14);
  SetCoeff(tmp, 13);
  SetCoeff(tmp, 11);
  SetCoeff(tmp, 9);
  SetCoeff(tmp, 7);
  SetCoeff(tmp, 6);
  SetCoeff(tmp, 5);
  SetCoeff(tmp, 4);
  SetCoeff(tmp, 3);
  SetCoeff(tmp, 1);
  SetCoeff(tmp, 0);
  a_lifted = conv<GF2E>(tmp);
  clear(tmp);
  // b_lifted should be y^30 + y^29 + y^27 + y^26 + y^25 + y^19 + y^18 + y^17 +
  // y^14 + y^13 + y^12 + y^10 + y^8 + y^7 + y^4 + y^2 + y
  SetCoeff(tmp, 30);
  SetCoeff(tmp, 29);
  SetCoeff(tmp, 27);
  SetCoeff(tmp, 26);
  SetCoeff(tmp, 25);
  SetCoeff(tmp, 19);
  SetCoeff(tmp, 18);
  SetCoeff(tmp, 17);
  SetCoeff(tmp, 14);
  SetCoeff(tmp, 13);
  SetCoeff(tmp, 12);
  SetCoeff(tmp, 10);
  SetCoeff(tmp, 8);
  SetCoeff(tmp, 7);
  SetCoeff(tmp, 4);
  SetCoeff(tmp, 2);
  SetCoeff(tmp, 1);
  b_lifted = conv<GF2E>(tmp);
  GF2E one;
  set(one);

  REQUIRE(utils::lift_uint8_t(a) == a_lifted);
  REQUIRE(utils::lift_uint8_t(b) == b_lifted);
  REQUIRE(a_lifted * b_lifted == one);
}


#include <mex.h>
#include <math.h>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <iostream>
#include <vector>
#include <time.h>

using namespace Eigen;
using namespace std;

typedef Triplet<double> eigen_entry;
typedef vector<eigen_entry> entry_list;

double get_Fischer_Burmeister(VectorXd &x, VectorXd &pj, int num)
{

    double fb = 0;

    for (int i = 0; i < num; i++)
    {
        double ent = x[i] + pj[i] - sqrt(x[i] * x[i] + pj[i] * pj[i]);
        fb += ent * ent;
    }

    return sqrt(fb);

}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{

    mxArray *output_mex;

    double *J_index, *JT_index, *J_value, *JT_value, *JTJ_info, *pu, *wu, *bu, *max_iter, *step_size;
    double *output;

    J_index = mxGetPr(prhs[0]);
    JT_index = mxGetPr(prhs[1]);
    J_value = mxGetPr(prhs[2]);
    JT_value = mxGetPr(prhs[3]);
    JTJ_info = mxGetPr(prhs[4]);
    pu = mxGetPr(prhs[5]);
    wu = mxGetPr(prhs[6]);
    bu = mxGetPr(prhs[7]);
    max_iter = mxGetPr(prhs[8]);
    step_size = mxGetPr(prhs[9]);

    // JTJ_info : dof_n, max_valence, tri_n, 3 * 2

    int J_row = JTJ_info[0];
    int J_col = JTJ_info[2];
    int max_it = max_iter[0];
    double step = step_size[0];

    output_mex = plhs[0] = mxCreateDoubleMatrix(J_row, 1, mxREAL);

    output = mxGetPr(output_mex);

    SparseMatrix<double> J_u = SparseMatrix<double>(J_row, J_col);
    SparseMatrix<double> JT_u = SparseMatrix<double>(J_col, J_row);
    VectorXd b_u = VectorXd::Zero(J_col);

    entry_list en_list;

    for(int i = 0; i < JTJ_info[0]; i++)
    {
        for(int j = 0; j < JTJ_info[1]; j++)
        {
            int index = i * JTJ_info[1] + j;

            if(J_index[index] >= 0)
            {
                en_list.push_back(eigen_entry(i, J_index[index], J_value[index]));
            }
        }
    }

    J_u.setFromTriplets(en_list.begin(), en_list.end());

    en_list.clear();

    for(int i = 0; i < JTJ_info[2]; i++)
    {
        for(int j = 0; j < JTJ_info[3]; j++)
        {
            int index = i * JTJ_info[3] + j;

            if(JT_index[index] >= 0)
            {
                en_list.push_back(eigen_entry(i, JT_index[index], JT_value[index]));
            }
        }
    }

    JT_u.setFromTriplets(en_list.begin(), en_list.end());

    VectorXd p_u = VectorXd::Zero(J_row);

    for(int i = 0; i < J_row; i++)
    {
        p_u[i] = pu[i];
    }

    for(int i = 0; i < J_col; i++)
    {
        b_u[i] = bu[i];
    }

    VectorXd b = b_u + JT_u * p_u;

    VectorXd lambda = VectorXd::Zero(J_col);

    VectorXd pi;

    double pre_FB, post_FB;

    int iteration = -1;

    ///////////////////////////////////////////////////////////////////////////////////////////////

    pi = JT_u * (J_u * lambda) + b;

    post_FB = get_Fischer_Burmeister(lambda, pi, J_col);

    for(int outer = 0; outer < max_it; outer++)
    {
        iteration = outer;

        pre_FB = post_FB;

        for(int iter = 0; iter < J_col; iter++)
        {
            lambda[iter] = fmax(lambda[iter] - step * pi[iter] / wu[iter], 0.0);
        }

        pi = JT_u * (J_u * lambda) + b;

        post_FB = get_Fischer_Burmeister(lambda, pi, J_col);

				if(abs(post_FB) < 1e-6)
				{
					break;
				}

        if(abs(pre_FB - post_FB) < 1e-3 * abs(pre_FB))
        {
            break;
        }
    }

    VectorXd p_mod = J_u * lambda;

    for(int i = 0; i < J_row; i++)
    {
        output[i] = p_mod[i];
    }

    return;

}


#include <numeric>
#include <boost/geometry/algorithms/intersection.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include "Polygon.hpp"

Polygon cat_reduce(std::vector<unsigned> ids, std::unordered_map<unsigned, std::vector<Polygon>> polys){
  using BoostPoly = boost::geometry::model::polygon<boost::geometry::model::d2::point_xy<float>>;
  BoostPoly a, b;
  std::vector<float> out;
  for (const auto& id : ids){
    const auto& poly_vec = polys[id];
    for (const auto& poly : poly_vec) {
      for (int i = 0; i < poly.size(); i += 2) {
        boost::geometry::append(a.inners(), boost::geometry::model::d2::point_xy{poly[i], poly[i + 1]});
      }
    }
    boost::geometry::intersection(a, b, out);
  }
  //std::accumulate(, , [](){boost::geometry::intersection(, , out);});
}

std::unordered_map<std::string, Polygon> cats_reduce(std::unordered_map<std::string, std::vector<unsigned>> cats, std::unordered_map<unsigned, std::vector<Polygon>> polys){
  std::unordered_map<std::string, Polygon> ret;
  std::transform(cats.begin(), cats.end(), std::inserter(ret, ret.begin()), [](const auto& e){
    const auto& [name, ids] = e;
    return std::make_pair(name, cat_reduce(ids));
  });
  return ret;
}