#ifndef _MLES_DATA_SET_HPP_
#define _MLES_DATA_SET_HPP_

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

namespace mles
{
    class DataSet
    {
        private:
            unsigned int inputSize, outputSize;
            std::vector < Eigen::VectorXd > inputs;
            std::vector < Eigen::VectorXd > outputs;
            std::vector < std::string > inputHeader;
            std::vector < std::string > outputHeader;
            std::map < std::string, std::vector < std::string > > classMap;
            bool isNumeric(const std::string& text, double& value);
        public:
            DataSet();
            DataSet(const DataSet& dataSet);
            DataSet(unsigned int inputSize, unsigned int outputSize);
            virtual ~DataSet();

            unsigned int getInputSize();
            unsigned int getOutputSize();

            void split(DataSet& trainingSet, DataSet& testSet, double ratio = 0.8);

            Eigen::VectorXd createInput();
            Eigen::VectorXd createOutput();
            Eigen::VectorXd createEntry();

            void add(const Eigen::VectorXd& input, const Eigen::VectorXd& output);
            void add(const Eigen::VectorXd& entry);
            void clear();

            bool load(const std::string& fileName);
            bool save(const std::string& fileName);

            bool toCSV(const std::string& fileName);
            bool fromCSV(const std::string& fileName, bool header = true, int chucksize = -1);

            unsigned int size();
            Eigen::VectorXd& getInput(unsigned int pos);
            Eigen::VectorXd& getOutput(unsigned int pos);
            unsigned int getOutputMaxIndex(unsigned int pos);

            DataSet toTestSet();

            void binarize(double threshold = 0.5);
            void classify();
            void binarizeToMax();

            void print(bool onlyOutput = false);
    };
}

#endif


#ifndef slic3r_Point_hpp_
#define slic3r_Point_hpp_

#include <myinit.h>
#include <vector>
#include <math.h>
#include <string>

namespace Slic3r {

class Line;
class MultiPoint;
class Point;
class Pointf;
typedef Point Vector;
typedef std::vector<Point> Points;
typedef std::vector<Point*> PointPtrs;
typedef std::vector<const Point*> PointConstPtrs;
typedef std::vector<Pointf> Pointfs;

class Point
{
    public:
    coord_t x;
    coord_t y;
    Point(coord_t _x = 0, coord_t _y = 0): x(_x), y(_y) {};
    Point(int _x, int _y): x(_x), y(_y) {};
    Point(long long _x, long long _y): x(_x), y(_y) {};  // for Clipper
    Point(double x, double y);
    bool operator==(const Point& rhs) const;
    std::string wkt() const;
    void scale(double factor);
    void translate(double x, double y);
    void rotate(double angle, const Point &center);
    bool coincides_with(const Point &point) const;
    bool coincides_with_epsilon(const Point &point) const;
    int nearest_point_index(const Points &points) const;
    int nearest_point_index(const PointConstPtrs &points) const;
    int nearest_point_index(const PointPtrs &points) const;
    void nearest_point(const Points &points, Point* point) const;
    double distance_to(const Point &point) const;
    double distance_to(const Line &line) const;
    double ccw(const Point &p1, const Point &p2) const;
    double ccw(const Line &line) const;
    Point projection_onto(const MultiPoint &poly) const;
    Point projection_onto(const Line &line) const;
    Point negative() const;

    #ifdef SLIC3RXS
    void from_SV(SV* point_sv);
    void from_SV_check(SV* point_sv);
    SV* to_SV_pureperl() const;
    #endif
};

Point operator+(const Point& point1, const Point& point2);
Point operator*(double scalar, const Point& point2);

class Point3 : public Point
{
    public:
    coord_t z;
    explicit Point3(coord_t _x = 0, coord_t _y = 0, coord_t _z = 0): Point(_x, _y), z(_z) {};
};

class Pointf
{
    public:
    coordf_t x;
    coordf_t y;
    explicit Pointf(coordf_t _x = 0, coordf_t _y = 0): x(_x), y(_y) {};
    void scale(double factor);
    void translate(double x, double y);
    void rotate(double angle, const Pointf &center);

    #ifdef SLIC3RXS
    bool from_SV(SV* point_sv);
    void from_SV_check(SV* point_sv);
    SV* to_SV_pureperl() const;
    #endif
};

class Pointf3 : public Pointf
{
    public:
    coordf_t z;
    explicit Pointf3(coordf_t _x = 0, coordf_t _y = 0, coordf_t _z = 0): Pointf(_x, _y), z(_z) {};
    void scale(double factor);
    void translate(double x, double y, double z);
};

}

// start Boost
#include <boost/polygon/polygon.hpp>
namespace boost { namespace polygon {
    template <>
    struct geometry_concept<coord_t> { typedef coordinate_concept type; };

    template <>
    struct coordinate_traits<coord_t> {
        typedef coord_t coordinate_type;
        typedef long double area_type;
        typedef long long manhattan_area_type;
        typedef unsigned long long unsigned_area_type;
        typedef long long coordinate_difference;
        typedef long double coordinate_distance;
    };

    template <>
    struct geometry_concept<Point> { typedef point_concept type; };

    template <>
    struct point_traits<Point> {
        typedef coord_t coordinate_type;

        static inline coordinate_type get(const Point& point, orientation_2d orient) {
            return (orient == HORIZONTAL) ? point.x : point.y;
        }
    };

    template <>
    struct point_mutable_traits<Point> {
        typedef coord_t coordinate_type;
        static inline void set(Point& point, orientation_2d orient, coord_t value) {
            if (orient == HORIZONTAL)
                point.x = value;
            else
                point.y = value;
        }
        static inline Point construct(coord_t x_value, coord_t y_value) {
            Point retval;
            retval.x = x_value;
            retval.y = y_value;
            return retval;
        }
    };
} }
// end Boost

#endif


//
// Created by Alex Beccaro on 23/03/18.
//

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include "../../src/problems/101-150/102/problem102.hpp"

BOOST_AUTO_TEST_SUITE( Problem102 )

    BOOST_AUTO_TEST_CASE( Solution ) {
        auto res = problems::problem102::solve();
        BOOST_CHECK_EQUAL(res, 228);
    }

BOOST_AUTO_TEST_SUITE_END()


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<boost::int64_t, 4, 3> test_long_4_3(const Eigen::Matrix<boost::int64_t, 4, 3> & M)
{
	return M;
}
void export_long_4_3()
{
	boost::python::def("test_long_4_3",test_long_4_3);
}


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

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

// This file was modified by Oracle 2017.
// Modifications copyright (c) 2017, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#include <geometry_test_common.hpp>

#include <algorithm>

#include <boost/geometry/algorithms/make.hpp>
#include <boost/geometry/io/dsv/write.hpp>

#include <boost/geometry/policies/compare.hpp>

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

#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/adapted/c_array.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>

#include <test_common/test_point.hpp>

BOOST_GEOMETRY_REGISTER_C_ARRAY_CS(cs::cartesian)
BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)

template <typename Container>
inline std::string coordinates(Container const& points)
{
    std::ostringstream out;
    for (typename boost::range_const_iterator<Container>::type it = boost::begin(points);
        it != boost::end(points);
        ++it)
    {
        out << bg::dsv(*it);
    }
    return out.str();
}

template <typename P>
void test_2d_compare()
{
    P p1 = bg::make<P>(3, 1);
    P p2 = bg::make<P>(3, 1);
    P p3 = bg::make<P>(1, 3);
    P p4 = bg::make<P>(5, 2);
    P p5 = bg::make<P>(3, 2);

    // Test in all dimensions
    {
        bg::equal_to<P> et;
        bg::less<P> lt;
        bg::greater<P> gt;

        BOOST_CHECK_EQUAL(et(p1, p2), true);
        BOOST_CHECK_EQUAL(et(p1, p3), false);
        BOOST_CHECK_EQUAL(et(p1, p4), false);
        BOOST_CHECK_EQUAL(et(p1, p5), false);
        BOOST_CHECK_EQUAL(et(p3, p4), false);

        BOOST_CHECK_EQUAL(lt(p1, p2), false);
        BOOST_CHECK_EQUAL(lt(p1, p3), false);
        BOOST_CHECK_EQUAL(lt(p1, p4), true);
        BOOST_CHECK_EQUAL(lt(p1, p5), true);
        BOOST_CHECK_EQUAL(lt(p3, p4), true);

        BOOST_CHECK_EQUAL(gt(p1, p2), false);
        BOOST_CHECK_EQUAL(gt(p1, p3), true);
        BOOST_CHECK_EQUAL(gt(p1, p4), false);
        BOOST_CHECK_EQUAL(gt(p1, p5), false);
        BOOST_CHECK_EQUAL(gt(p3, p4), false);
    }

    // Test in dimension 0, X
    {
        bg::equal_to<P, 0> et;
        bg::less<P, 0> lt;
        bg::greater<P, 0> gt;

        BOOST_CHECK_EQUAL(et(p1, p2), true);
        BOOST_CHECK_EQUAL(et(p1, p3), false);
        BOOST_CHECK_EQUAL(et(p1, p4), false);
        BOOST_CHECK_EQUAL(et(p1, p5), true);
        BOOST_CHECK_EQUAL(et(p3, p4), false);

        BOOST_CHECK_EQUAL(lt(p1, p2), false);
        BOOST_CHECK_EQUAL(lt(p1, p3), false);
        BOOST_CHECK_EQUAL(lt(p1, p4), true);
        BOOST_CHECK_EQUAL(lt(p1, p5), false);
        BOOST_CHECK_EQUAL(lt(p3, p4), true);

        BOOST_CHECK_EQUAL(gt(p1, p2), false);
        BOOST_CHECK_EQUAL(gt(p1, p3), true);
        BOOST_CHECK_EQUAL(gt(p1, p4), false);
        BOOST_CHECK_EQUAL(gt(p1, p5), false);
        BOOST_CHECK_EQUAL(gt(p3, p4), false);
    }

    // Test in dimension 1, Y
    {
        bg::equal_to<P, 1> et;
        bg::less<P, 1> lt;
        bg::greater<P, 1> gt;

        BOOST_CHECK_EQUAL(et(p1, p2), true);
        BOOST_CHECK_EQUAL(et(p1, p3), false);
        BOOST_CHECK_EQUAL(et(p1, p4), false);
        BOOST_CHECK_EQUAL(et(p1, p5), false);
        BOOST_CHECK_EQUAL(et(p3, p4), false);

        BOOST_CHECK_EQUAL(lt(p1, p2), false);
        BOOST_CHECK_EQUAL(lt(p1, p3), true);
        BOOST_CHECK_EQUAL(lt(p1, p4), true);
        BOOST_CHECK_EQUAL(lt(p1, p5), true);
        BOOST_CHECK_EQUAL(lt(p3, p4), false);

        BOOST_CHECK_EQUAL(gt(p1, p2), false);
        BOOST_CHECK_EQUAL(gt(p1, p3), false);
        BOOST_CHECK_EQUAL(gt(p1, p4), false);
        BOOST_CHECK_EQUAL(gt(p1, p5), false);
        BOOST_CHECK_EQUAL(gt(p3, p4), true);
    }
}

template <typename P>
void test_2d_sort()
{
    std::vector<P> v;
    v.push_back(bg::make<P>(3, 1));
    v.push_back(bg::make<P>(2, 3));
    v.push_back(bg::make<P>(2, 2));
    v.push_back(bg::make<P>(1, 3));

    // Sort on coordinates in order x,y,z
    std::sort(v.begin(), v.end(), bg::less<P>());
    std::string s = coordinates(v);
    BOOST_CHECK_EQUAL(s, "(1, 3)(2, 2)(2, 3)(3, 1)");

    // Reverse sort
    std::sort(v.begin(), v.end(), bg::greater<P>());
    s = coordinates(v);
    BOOST_CHECK_EQUAL(s, "(3, 1)(2, 3)(2, 2)(1, 3)");

    // Sort backwards on coordinates in order x,y,z
    //std::sort(v.begin(), v.end(), bg::greater<P>());
    //std::string s = coordinates(v);
    //BOOST_CHECK_EQUAL(s, "(1, 3)(2, 2)(2, 3)(3, 1)");

    // Refill to remove duplicate coordinates
    v.clear();
    v.push_back(bg::make<P>(4, 1));
    v.push_back(bg::make<P>(3, 2));
    v.push_back(bg::make<P>(2, 3));
    v.push_back(bg::make<P>(1, 4));

    // Sort ascending on only x-coordinate
    std::sort(v.begin(), v.end(), bg::less<P, 0>());
    s = coordinates(v);
    BOOST_CHECK_EQUAL(s, "(1, 4)(2, 3)(3, 2)(4, 1)");

    // Sort ascending on only y-coordinate
    std::sort(v.begin(), v.end(), bg::less<P, 1>());
    s = coordinates(v);
    BOOST_CHECK_EQUAL(s, "(4, 1)(3, 2)(2, 3)(1, 4)");

    // Sort descending on only x-coordinate
    std::sort(v.begin(), v.end(), bg::greater<P, 0>());
    s = coordinates(v);
    //BOOST_CHECK_EQUAL(s, "(4, 1)(3, 2)(2, 3)(1, 4)");

    // Sort descending on only y-coordinate
    std::sort(v.begin(), v.end(), bg::greater<P, 1>());
    s = coordinates(v);
    BOOST_CHECK_EQUAL(s, "(1, 4)(2, 3)(3, 2)(4, 1)");

    // Make non-unique vector
    v.push_back(bg::make<P>(4, 1));
    v.push_back(bg::make<P>(3, 2));
    v.push_back(bg::make<P>(2, 3));
    v.push_back(bg::make<P>(1, 4));
    v.push_back(bg::make<P>(1, 5));
    std::sort(v.begin(), v.end(), bg::less<P>());
    s = coordinates(v);
    BOOST_CHECK_EQUAL(s, "(1, 4)(1, 4)(1, 5)(2, 3)(2, 3)(3, 2)(3, 2)(4, 1)(4, 1)");

    std::vector<P> v2;
    std::unique_copy(v.begin(), v.end(), std::back_inserter(v2), bg::equal_to<P>());
    s = coordinates(v2);
    BOOST_CHECK_EQUAL(s, "(1, 4)(1, 5)(2, 3)(3, 2)(4, 1)");
}

template <typename P>
void test_spherical()
{
    //typedef typename bg::coordinate_type<P>::type ct;

    std::vector<P> v;
    v.push_back(bg::make<P>( 180.00, 70.56));
    v.push_back(bg::make<P>( 179.73, 71.56)); // east
    v.push_back(bg::make<P>( 177.47, 71.23)); // less east
    v.push_back(bg::make<P>(-178.78, 72.78)); // further east, = west, this is the most left point
    v.push_back(bg::make<P>(-180.00, 73.12));

    // Sort on coordinates in order x,y,z
    std::sort(v.begin(), v.end(), bg::less<P>());
    std::string s = coordinates(v);
    BOOST_CHECK_EQUAL(s, "(-178.78, 72.78)(177.47, 71.23)(179.73, 71.56)(180, 70.56)(-180, 73.12)");

    // Sort ascending on only y-coordinate
    std::sort(v.begin(), v.end(), bg::less<P, 1>());
    s = coordinates(v);
    BOOST_CHECK_EQUAL(s, "(180, 70.56)(177.47, 71.23)(179.73, 71.56)(-178.78, 72.78)(-180, 73.12)");

    // Sort ascending on only x-coordinate
    std::sort(v.begin(), v.end(), bg::less<P, 0>());
    s = coordinates(v);
    BOOST_CHECK((s == "(-178.78, 72.78)(177.47, 71.23)(179.73, 71.56)(180, 70.56)(-180, 73.12)"
              || s == "(-178.78, 72.78)(177.47, 71.23)(179.73, 71.56)(-180, 73.12)(180, 70.56)"));

    // Sort ascending on only x-coordinate, but override with std-comparison,
    // (so this is the normal sorting behaviour that would have been used
    // if it would not have been spherical)
    //std::sort(v.begin(), v.end(), bg::less<P, 0, std::less<ct> >());
    //s = coordinates(v);
    //BOOST_CHECK_EQUAL(s, "(-178.78, 70.78)(177.47, 71.23)(179.73, 71.56)");
}

int test_main(int, char* [])
{
    test_2d_compare<bg::model::point<int, 2, bg::cs::cartesian> >();
    test_2d_compare<bg::model::point<double, 2, bg::cs::cartesian> >();

    test_2d_sort<bg::model::point<int, 2, bg::cs::cartesian> >();
    test_2d_sort<bg::model::point<float, 2, bg::cs::cartesian> >();
    test_2d_sort<boost::tuple<double, double> >();
    test_2d_sort<bg::model::point<double, 2, bg::cs::cartesian> >();

    test_spherical<bg::model::point<double, 2, bg::cs::spherical<bg::degree> > >();
    test_spherical<bg::model::point<double, 2, bg::cs::spherical_equatorial<bg::degree> > >();
    test_spherical<bg::model::point<double, 2, bg::cs::geographic<bg::degree> > >();

    return 0;
}


/*******************************************************************************
 * Copyright 2013-2014 Sebastian Niemann <niemann@sra.uni-hannover.de>.
 *
 * Licensed under the MIT License (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://opensource.org/licenses/MIT
 *
 * Developers:
 *   Sebastian Niemann - Lead developer
 *   Daniel Kiechle - Unit testing
 ******************************************************************************/
#include <Expected.hpp>
using armadilloJava::Expected;

#include <iostream>
using std::cout;
using std::endl;

#include <utility>
using std::pair;

#include <armadillo>
using arma::is_finite;
using arma::rowvec;
using arma::colvec;

#include <InputClass.hpp>
using armadilloJava::InputClass;

#include <Input.hpp>
using armadilloJava::Input;

#include <vector>

namespace armadilloJava {
  class ExpectedGenDouble : public Expected {
    public:
      ExpectedGenDouble() {
        cout << "Compute ExpectedGenDouble(): " << endl;

        vector<vector<pair<string, void*>>> inputs = Input::getTestParameters({
          InputClass::GenDouble
        });

        for (vector<pair<string, void*>> input : inputs) {
          _fileSuffix = "";

          int n = 0;
          for (pair<string, void*> value : input) {
            switch (n) {
              case 0:
                _fileSuffix += value.first;
                _genDouble = *static_cast<double*>(value.second);
                break;
            }
            ++n;
          }
          _genDoubleArray.push_back(_genDouble);

          cout << "Using input: " << _fileSuffix << endl;

          expectedArmaIs_finite();
        }

        expectedRowVecDoubleArray();
        expectedColVecDoubleArray();
        cout << "done." << endl;
      }

    protected:
      double _genDouble;
      std::vector<double> _genDoubleArray;

      void expectedArmaIs_finite() {
        cout << "- Compute expectedArmaIs_finite() ... ";

        if(is_finite(_genDouble)) {
          save<double>("Arma.is_finite", Mat<double>({1.0}));
        } else {
          save<double>("Arma.is_finite", Mat<double>({0.0}));
        }

        cout << "done." << endl;
      }

      void expectedColVecDoubleArray() {
        cout << "- Compute expectedColVecDoubleArray() ... ";

        save<double>("Col.doubleArray", colvec(_genDoubleArray));

        cout << "done." << endl;
      }

      void expectedRowVecDoubleArray() {
        cout << "- Compute expectedRowVecDoubleArray() ... ";

        save<double>("Row.doubleArray", rowvec(_genDoubleArray));

        cout << "done." << endl;
      }

  };
}


//==================================================================================================
/*!

  Copyright 2015 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)
*/
//==================================================================================================
#include <boost/simd/function/scalar/is_ord.hpp>
#include <simd_test.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/quarter.hpp>

STF_CASE_TPL (" is_ord real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::is_ord;
  using r_t = decltype(is_ord(T(), T()));

  // return type conformity test
  STF_TYPE_IS(r_t, bs::logical<T>);

  // specific values tests
#ifndef STF_NO_INVALIDS
  STF_EQUAL(is_ord(bs::Inf<T>(), bs::Inf<T>()), r_t(true));
  STF_EQUAL(is_ord(bs::Minf<T>(), bs::Minf<T>()), r_t(true));
  STF_EQUAL(is_ord(bs::Nan<T>(), bs::Nan<T>()), r_t(false));
#endif
  STF_EQUAL(is_ord(-bs::Zero<T>(), -bs::Zero<T>()), r_t(true));
  STF_EQUAL(is_ord(bs::Half<T>(), bs::Half<T>()), r_t(true));
  STF_EQUAL(is_ord(bs::Mone<T>(), bs::Mone<T>()), r_t(true));
  STF_EQUAL(is_ord(bs::One<T>(), bs::One<T>()), r_t(true));
  STF_EQUAL(is_ord(bs::Quarter<T>(), bs::Quarter<T>()), r_t(true));
  STF_EQUAL(is_ord(bs::Two<T>(), bs::Two<T>()), r_t(true));
  STF_EQUAL(is_ord(bs::Zero<T>(), bs::Zero<T>()), r_t(true));
}

STF_CASE_TPL (" is_ord signed_int",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::is_ord;
  using r_t = decltype(is_ord(T(), T()));

  // return type conformity test
  STF_TYPE_IS(r_t, bs::logical<T>);

  // specific values tests
  STF_EQUAL(is_ord(bs::Mone<T>(), bs::Mone<T>()), r_t(true));
  STF_EQUAL(is_ord(bs::One<T>(), bs::One<T>()), r_t(true));
  STF_EQUAL(is_ord(bs::Two<T>(), bs::Two<T>()), r_t(true));
  STF_EQUAL(is_ord(bs::Zero<T>(), bs::Zero<T>()), r_t(true));
}

STF_CASE_TPL (" is_ord unsigned_int",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::is_ord;
  using r_t = decltype(is_ord(T(), T()));

  // return type conformity test
  STF_TYPE_IS(r_t, bs::logical<T>);

  // specific values tests
  STF_EQUAL(is_ord(bs::One<T>(), bs::One<T>()), r_t(true));
  STF_EQUAL(is_ord(bs::Two<T>(), bs::Two<T>()), r_t(true));
  STF_EQUAL(is_ord(bs::Zero<T>(), bs::Zero<T>()), r_t(true));
}

STF_CASE ( "is_ord bool")
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::is_ord;
  using r_t = decltype(is_ord(bool(), bool()));

  // return type conformity test
  STF_TYPE_IS(r_t, bool);

  // specific values tests
  STF_EQUAL(is_ord(true, false), true);
  STF_EQUAL(is_ord(false, true), true);
  STF_EQUAL(is_ord(true, true), true);
  STF_EQUAL(is_ord(false, false), true);
}


#ifndef STEREO_LIDAR_CALIBRATION_OPTIMIZATION_HPP
#define STEREO_LIDAR_CALIBRATION_OPTIMIZATION_HPP

#include <Eigen/Core>
#include <ceres/ceres.h>
#include <ceres/rotation.h>     /// ceres::AngleAxisRotatePoint
#include <opencv2/core.hpp>
#include <Eigen/Geometry>
#include <ceres/autodiff_cost_function.h>
#include <pcl/io/ply_io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
#include <pcl/features/eigen.h>
#include <pcl/features/feature.h>
#include <pcl/features/normal_3d.h>
#include <pcl/impl/point_types.hpp>
#include <pcl/features/boundary.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_line.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/common/intersections.h>
#include <pcl/registration/icp.h>
#include <pcl/common/distances.h>
#include <sophus/so3.hpp>
using namespace std;
using namespace Eigen;

// corners refinement error
class BoardCornersError
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    BoardCornersError(const pcl::PointCloud<pcl::PointXYZ> &edge_cloud, const int line_index, const vector<double>& board_length)
        : m_cloud(edge_cloud), m_index(line_index), m_length(board_length)
    {
    }
    template <typename T>
    bool operator()(const T *const corner_v, T *residual) const
    {
        if (m_cloud.empty())
        {
            return false;
        }
        T dist_sum_1 = T(0.0);
        for (size_t i = 0; i < m_cloud.size(); ++i)
        {
            pcl::PointXYZ point = m_cloud.points[i];
            T point_c[3];
            point_c[0] = T(point.x);
            point_c[1] = T(point.y);
            point_c[2] = T(point.z);
            T a[3], b[3];
            a[0] = corner_v[((m_index + 1) % 4) * 3] - corner_v[m_index * 3];
            a[1] = corner_v[((m_index + 1) % 4) * 3 + 1] - corner_v[m_index * 3 + 1];
            a[2] = corner_v[((m_index + 1) % 4) * 3 + 2] - corner_v[m_index * 3 + 2];
            b[0] = corner_v[m_index * 3] - point_c[0];
            b[1] = corner_v[m_index * 3 + 1] - point_c[1];
            b[2] = corner_v[m_index * 3 + 2] - point_c[2];
            T a_norm = ceres::sqrt(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);
            T b_norm = ceres::sqrt(b[0] * b[0] + b[1] * b[1] + b[2] * b[2]);
            T a_dot_b = a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
            T dist = b_norm * ceres::sin(ceres::acos(a_dot_b / (a_norm * b_norm)));
            dist_sum_1 += ceres::abs(dist);
        }
        dist_sum_1 /= T(m_cloud.size());

        T dist_sum_2 = T(0.0);
        T length_diagonal = ceres::sqrt(T(m_length[0])*T(m_length[0]) + T(m_length[1])*T(m_length[1]));
        for (size_t i = 0; i < 4; ++i)
        {
            T a[3], b[3];
            a[0] = corner_v[((i + 1) % 4) * 3] - corner_v[i * 3];
            a[1] = corner_v[((i + 1) % 4) * 3 + 1] - corner_v[i * 3 + 1];
            a[2] = corner_v[((i + 1) % 4) * 3 + 2] - corner_v[i * 3 + 2];
            T a_norm = ceres::sqrt(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);
            dist_sum_2 += ceres::abs(a_norm - T(m_length[i]));
            if (i < 2)
            {
                b[0] = corner_v[(i + 2) * 3] - corner_v[i * 3];
                b[1] = corner_v[(i + 2) * 3 + 1] - corner_v[i * 3 + 1];
                b[2] = corner_v[(i + 2) * 3 + 2] - corner_v[i * 3 + 2];
                T b_norm = ceres::sqrt(b[0] * b[0] + b[1] * b[1] + b[2] * b[2]);
                dist_sum_2 += ceres::abs(b_norm - length_diagonal);
            }
        }
        dist_sum_2 /= T(6.0);

        residual[0] = dist_sum_1 + dist_sum_2;
        return true;
    }

    const pcl::PointCloud<pcl::PointXYZ> m_cloud;
    const int m_index;
    const vector<double> m_length;
};

// closedup error
class ClosedupError
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    ClosedupError(const Eigen::VectorXd &Rt_c1_c2,
                  const double &w) : m_Rt_c1_c2(Rt_c1_c2), m_w(w) {}
    template<typename T>
    bool operator()(const T* const Rt1, const T* const Rt2, T* residual) const{
        T rot1[3*3];
        ceres::AngleAxisToRotationMatrix(Rt1, rot1);
        Eigen::Map<const Eigen::Matrix<T,3,3>> R1(rot1);
        Eigen::Map<const Eigen::Matrix<T,3,1>> t1(Rt1+3);
        Eigen::Matrix<T,4,4> trans1 = Eigen::Matrix<T,4,4>::Identity();
        trans1.block(0,0,3,3) = R1;
        trans1.block(0,3,3,1) = t1;

        T rot2[3*3];
        ceres::AngleAxisToRotationMatrix(Rt2, rot2);
        Eigen::Map<const Eigen::Matrix<T,3,3>> R2(rot2);
        Eigen::Map<const Eigen::Matrix<T,3,1>> t2(Rt2+3);
        Eigen::Matrix<T,4,4> trans2 = Eigen::Matrix<T,4,4>::Identity();
        trans2.block(0,0,3,3) = R2;
        trans2.block(0,3,3,1) = t2;

        // T rot3[3*3];
        // const T* const Rt3 = m_Rt_c1_c2.template cast<T>().data();
        // ceres::AngleAxisToRotationMatrix(Rt3, rot3);
        // Eigen::Map<const Eigen::Matrix<T,3,3>> R3(rot3);
        // Eigen::Map<const Eigen::Matrix<T,3,1>> t3(Rt3+3);
        Eigen::Matrix<T,4,4> trans3 = Eigen::Matrix<T,4,4>::Identity();
        Eigen::Matrix<T,3,3> R3 = Sophus::SO3d::exp(m_Rt_c1_c2.head(3)).matrix().template cast<T>();
        trans3.block(0,0,3,3) = R3;
        trans3.block(0,3,3,1) = m_Rt_c1_c2.tail(3).template cast<T>();

        Eigen::Matrix<T,4,4> loop = trans1*(trans2.inverse().template cast<T>())*trans3;
        residual[0] = loop.block(0,3,3,1).norm();
        residual[0] *= m_w;
        residual[1] = (Matrix<T,3,3>::Identity()-loop.block(0,0,3,3)).trace();
        residual[1] *= m_w;
        return true;
    }
private:
    const double m_w;
    const Eigen::VectorXd& m_Rt_c1_c2;
};

// 点到点的距离损失
class PointToPointError
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    PointToPointError(const Eigen::Vector3d& point_lidar,
                    const Eigen::Vector3d& point_cam,
                    const double& w):
                    m_point_lidar(point_lidar),
                    m_point_cam(point_cam),
                    m_w(w){}

    template<typename T>
    bool operator()(const T* const R_t,T*residual) const{
        T rot[3*3];
        ceres::AngleAxisToRotationMatrix(R_t, rot);
        Eigen::Matrix<T, 3, 3> R = Eigen::Matrix<T, 3, 3>::Identity();
        for (int i = 0; i < 3; ++i) {
            for (int j = 0; j < 3; ++j) {
                R(i, j) = rot[i+j*3];
            }
        }
        Eigen::Map<const Eigen::Matrix<T,3,1>> trans(R_t+3);
        residual[0] = (m_point_cam.template cast<T>() - (R * m_point_lidar.template cast<T>() + trans)).norm();
        residual[0] *= m_w;
        return true;
    }

private:
    const Eigen::Vector3d m_point_lidar;
    const Eigen::Vector3d m_point_cam;
    const double m_w;
};

class PointToPointError1
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    PointToPointError1(const Eigen::Vector3d& point_lidar,
                    const Eigen::Vector3d& point_cam,
                    const double& w):
                    m_point_lidar(point_lidar),
                    m_point_cam(point_cam),
                    m_w(w){}

    template<typename T>
    bool operator()(const T* const R_t,T*residual) const{
        T p1[3] = {
            T(m_point_lidar(0)),
            T(m_point_lidar(1)),
            T(m_point_lidar(2))
        };
        T p2[3];
        ceres::AngleAxisRotatePoint(R_t, p1, p2);
        p2[0] += R_t[3];
        p2[1] += R_t[4];
        p2[2] += R_t[5];
        T p3[3] = {
            T(m_point_cam(0)),
            T(m_point_cam(1)),
            T(m_point_cam(2))
        };
        residual[0] = m_w * (p3[0]-p2[0]);
        residual[1] = m_w * (p3[1]-p2[1]);
        residual[2] = m_w * (p3[2]-p2[2]);
        return true;
    }

private:
    const Eigen::Vector3d m_point_lidar;
    const Eigen::Vector3d m_point_cam;
    const double m_w;
};

// 计算点到直线的距离损失

// class PointToLineError2
// {
// public:
//     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
//     PointToLineError2(const Eigen::Vector3d& pt,
//                     const Eigen::Vector3d& line_pt,
//                     const Eigen::Vector3d& line_dir,
//                     const double& w):
//                     m_pt(pt),
//                     m_line_pt(line_pt),
//                     m_line_dir(line_dir),
//                     m_w(w){}
//     template<typename T>
//     bool operator ()(const T* const R_t, T* residual) const
//     {
//         T p1[3] = {
//             T(m_pt(0)),
//             T(m_pt(1)),
//             T(m_pt(2))
//         };
//         T p2[3];
//         ceres::AngleAxisRotatePoint(R_t, p1, p2);
//         p2[0] += R_t[3];
//         p2[1] += R_t[4];
//         p2[2] += R_t[5];
//         Eigen::Matrix<T,4, 1> pt, line_pt, line_dir;
//         pt<< (T)p2[0], (T)p2[1], (T)p2[2], (T)0.0;
//         line_pt << (T)m_line_pt[0], (T)m_line_pt[1], (T)m_line_pt[2], (T)0.0;
//         line_dir<< (T)m_line_dir[0], (T)m_line_dir[1], (T)m_line_dir[2], (T)0.0;
//         double err = sqrt(pcl::sqrPointToLineDistance(pt, line_pt, line_dir));
//         Eigen::Matrix<double,1,1> err1;
//         err1(0) = err;
//         residual[0] = err1.template cast<T>().norm();
//         residual[0] *= m_w;
//         return true;
//     }
// private:
//     const Eigen::Vector3d m_pt;
//     const Eigen::Vector3d m_line_pt;
//     const Eigen::Vector3d m_line_dir;
//     const double m_w;
// };

// 计算点到直线的损失
class PointToLineError1
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    PointToLineError1(const Eigen::Vector3d& point,
                    const Eigen::Vector3d& line_normal,
                    const double& w):
                    m_point(point),
                    m_line_normal(line_normal),
                    m_w(w){}
    template<typename T>
    bool operator () (const T* const R_t,
                      T* residual) const {
        T l_pt_L[3] = {T(m_point(0)),
                       T(m_point(1)),
                       T(m_point(2))};
        T n_C[3] = {T(m_line_normal(0)),
                    T(m_line_normal(1)),
                    T(m_line_normal(2))};
        T l_pt_C[3];
        ceres::AngleAxisRotatePoint(R_t, l_pt_L, l_pt_C);
        l_pt_C[0] += R_t[3];
        l_pt_C[1] += R_t[4];
        l_pt_C[2] += R_t[5];
        Eigen::Matrix<T, 3, 1> laser_point_C(l_pt_C);
        Eigen::Matrix<T, 3, 1> normal_C(n_C);
        residual[0] = normal_C.normalized().dot(laser_point_C);
        residual[0] = m_w * residual[0];
        return true;
    }
private:
    const Eigen::Vector3d m_point;
    const Eigen::Vector3d m_line_normal;
    const double m_w;
};

// 计算点到平面的损失
class PointToPlaneError
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    PointToPlaneError(const Eigen::Vector3d &point,
                      const Eigen::Vector3d &plane_normal,
                      const Eigen::Vector3d &plane_centroid,
                      const double &w) : m_point(point),
                                         m_plane_normal(plane_normal),
                                         m_plane_centroid(plane_centroid),
                                         m_w(w) {}

    template<typename T>
    bool operator() (const T* const R_t,
                     T* residual) const {
        T l_pt_L[3] = {T(m_point(0)), T(m_point(1)), T(m_point(2))};
        T r_3[3] = {T(m_plane_normal(0)), T(m_plane_normal(1)), T(m_plane_normal(2))};
        T t_vec[3] = {T(m_plane_centroid(0)), T(m_plane_centroid(1)), T(m_plane_centroid(2))};

        T l_pt_C[3];
        ceres::AngleAxisRotatePoint(R_t, l_pt_L, l_pt_C);
        l_pt_C[0] += R_t[3];
        l_pt_C[1] += R_t[4];
        l_pt_C[2] += R_t[5];

        Eigen::Matrix<T, 3, 1> laser_point_C(l_pt_C);
        Eigen::Matrix<T, 3, 1> r3_C(r_3);
        Eigen::Matrix<T, 3, 1> tvec_C(t_vec);
        residual[0] = r3_C.dot(laser_point_C - tvec_C);
        residual[0] = m_w * residual[0];
        return true;
    }
private:
    const Eigen::Vector3d m_point;
    const Eigen::Vector3d m_plane_normal;
    const Eigen::Vector3d m_plane_centroid;
    const double m_w;
};

// class PoseLoopClosingError
// {
// public:
//     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
//     PoseLoopClosingError(Sophus::SE3d t_ij):m_t_ij(t_ij){}
//     template <typename T>
//     bool operator()(const T *const R_t_L1_C1,
//                     const T *const R_t_L1_C2,
//                     T *residual) const
//     {
//         Eigen::Map<const Sophus::SE3<T>> t_i(R_t_L1_C1);
//         Eigen::Map<const Sophus::SE3<T>> t_j(R_t_L1_C2);
//         Eigen::Map<Eigen::Matrix<T, 6, 1>> residual(residual_ptr);

//         residual = (t_i * t_ij.template cast<T>() * t_j.inverse()).log();
//         return true;
//     }

// private:
//     Sophus::SE3d m_t_ij;
// };

class StereoMatchingError
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    StereoMatchingError(const Eigen::Vector2d &camera1_corner,
                        const Eigen::Vector2d &camera2_corner,
                        const double &w) : m_camera1_corner(camera1_corner),
                                           m_camera2_corner(camera2_corner),
                                           m_w(w) {}

    template<typename T>
    bool operator() (const T* const R_t,
                     T* residual) const {
        // T rot[3*3];
        // ceres::AngleAxisToRotationMatrix(R_t, rot);
        // Eigen::Matrix<T, 3, 3> R = Eigen::Matrix<T, 3, 3>::Identity();
        // for (int i = 0; i < 3; ++i) {
        //     for (int j = 0; j < 3; ++j) {
        //         R(i, j) = rot[i+3*j];
        //     }
        // }
        // Eigen::Map<const Eigen::Matrix<T,3,1>> t(R_t+3);
        T rot[3*3];
        ceres::AngleAxisToRotationMatrix(R_t, rot);
        Eigen::Map<const Eigen::Matrix<T,3,3>> R(rot);
        Eigen::Map<const Eigen::Matrix<T,3,1>> t(R_t+3);
        Eigen::Matrix<T, 3, 1> camera1_corner(T(m_camera1_corner(0)),T(m_camera1_corner(1)), T(1.0));
        Eigen::Matrix<T, 3, 1> camera2_corner(T(m_camera2_corner(0)),T(m_camera2_corner(1)), T(1.0));
        Eigen::Matrix<T, 3, 3> skew_t;
        skew_t << T(0), T(t(0)), T(-t(1)), T(-t(0)), T(0), T(t(2)), T(t(1)), T(-t(2)), T(0);
        Eigen::Matrix<T, 3, 3> E_matrix = skew_t*R;
        residual[0] = camera1_corner.transpose().dot(E_matrix*camera2_corner);
        residual[0] = m_w* residual[0];
        return true;
    }

private:
    const Eigen::Vector2d m_camera1_corner;
    const Eigen::Vector2d m_camera2_corner;
    const double m_w;
};

class StereoReprojectionError
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    StereoReprojectionError(const Vector3d &P1,
                            const Vector2d &p1,
                            const Vector3d &P2,
                            const Vector2d &p2,
                            const double &w) : m_P1(P1),
                                               m_p1(p1),
                                               m_P2(P2),
                                               m_p2(p2),
                                               m_w(w) {}

    template <typename T>
    bool operator()(const T* const Rt,
                    T* residuals) const
    {
        T P1[3] = {T(m_P1(0)),
                   T(m_P1(1)),
                   T(m_P1(2))};
        T P12[3];
        ceres::AngleAxisRotatePoint(Rt, P1, P12);
        P12[0] += Rt[3];
        P12[1] += Rt[4];
        P12[2] += Rt[5];
        Eigen::Map<const Eigen::Matrix<T,3,1>> p2(P12);
        residuals[0] = m_w*(T(m_p2(0))-p2(0)/p2(2));
        residuals[1] = m_w*(T(m_p2(1))-p2(1)/p2(2));

        T rot[3*3];
        ceres::AngleAxisToRotationMatrix(Rt, rot);
        Eigen::Map<const Eigen::Matrix<T,3,3>> R(rot);
        Eigen::Map<const Eigen::Matrix<T,3,1>> t(Rt+3);
        Eigen::Matrix<T,4,4> trans = Eigen::Matrix<T,4,4>::Identity();
        trans.block(0,0,3,3) = R;
        trans.block(0,3,3,1) = t;
        Eigen::Matrix<T,4,4> trans_inv = trans.inverse();
        Eigen::Matrix<T,3,3> R_inv = trans_inv.block(0,0,3,3);
        Eigen::Matrix<T,3,1> t_inv = trans_inv.block(0,3,3,1);
        Eigen::Matrix<T,3,1> p1 = R_inv * m_P2 + t_inv;
        residuals[2] = m_w*(T(m_p1(0))-p1(0)/p1(2));
        residuals[3] = m_w*(T(m_p1(1))-p1(1)/p1(2));
        return true;
    }

private:
    const Vector3d m_P1, m_P2;
    const Vector2d m_p1, m_p2;
    const MatrixXd m_K1, m_K2;
    const double m_w;
};

class Optimizer
{
public:
    Optimizer() {}
    ~Optimizer() {}
    void addPointToPointConstriants(ceres::Problem &problem,
                                    std::vector<Vector3d> &image_corners_3d,
                                    std::vector<Vector3d> &lidar_corners_3d,
                                    VectorXd &params);
    void addStereoMatchingConstraints(ceres::Problem &problem,
                                      std::vector<Vector2d> &left_image_corners,
                                      std::vector<Vector2d> &right_image_corners,
                                      VectorXd &params);

    void addClosedLoopConstraints(ceres::Problem &problem,
                                    VectorXd& params1,
                                    VectorXd& params2,
                                    VectorXd& params3);

    void addStereoMatchingConstraints(ceres::Problem &problem,
                                      vector<Vector3d> &P1,
                                      vector<Vector2d> &p1,
                                      vector<Vector3d> &P2,
                                      vector<Vector2d> &p2,
                                      VectorXd &params);

    void addPointToPlaneConstriants(ceres::Problem &problem,
                                    pcl::PointCloud<pcl::PointXYZ> &plane_cloud,
                                    VectorXd &image_plane,
                                    VectorXd &params);
};

class OptimizationLC
{
public:
    OptimizationLC(){}
    ~OptimizationLC(){}
    OptimizationLC(Eigen::VectorXd &R_t) : m_R_t(R_t) {}
    void addPointToPlaneConstraints(ceres::Problem &problem,
                                    pcl::PointCloud<pcl::PointXYZ>::Ptr &plane_pcd,
                                    Eigen::Vector3d &plane_normal,
                                    Eigen::Vector3d &plane_centroid);
    void addPointToLineConstriants(ceres::Problem &problem,
                                   std::vector<pcl::PointCloud<pcl::PointXYZ>> &lines_pcd,
                                   std::vector<Eigen::VectorXd> &line_normals);
    // ceres::Problem constructProblem();
    void addPointToPointConstriants(ceres::Problem &problem,
                                    std::vector<Vector3d> &image_corners_3d,
                                    std::vector<Vector3d> &lidar_corners_3d);

    Eigen::VectorXd get_R_t()
    {
        return m_R_t;
    }

private:
    Eigen::VectorXd m_R_t;
    // Eigen::Vector3d m_plane_normal;
    // Eigen::Vector3d m_plane_centroid;
    // std::vector<Eigen::Vector3d> m_line_normal;
};

class OptimizationLCC : public OptimizationLC
{
public:
    OptimizationLCC(){}
    ~OptimizationLCC(){}
    OptimizationLCC(Eigen::VectorXd &Rt_l1_c1,
                    Eigen::VectorXd &Rt_l1_c2,
                    Eigen::VectorXd &Rt_c1_c2) : m_Rt_l1_c1(Rt_l1_c1),
                                                 m_Rt_l1_c2(Rt_l1_c2),
                                                 m_Rt_c1_c2(Rt_c1_c2) {}

    OptimizationLCC(Eigen::VectorXd &Rt_c1_c2)
        :m_Rt_c1_c2(Rt_c1_c2) {}

    void addStereoMatchingConstraints(ceres::Problem &problem,
                                    std::vector<Vector2d> &left_image_corners,
                                    std::vector<Vector2d> &right_image_corners);

    void addPointToPlaneConstraints(ceres::Problem &problem,
                                    pcl::PointCloud<pcl::PointXYZ>::Ptr &plane_pcd,
                                    Eigen::Vector3d &plane_normal,
                                    Eigen::Vector3d &plane_centroid,
                                    Eigen::VectorXd &params);
    void addPointToLineConstriants(ceres::Problem &problem,
                                   std::vector<pcl::PointCloud<pcl::PointXYZ>> &lines_pcd,
                                   std::vector<Eigen::VectorXd> &line_normals,
                                   Eigen::VectorXd &params);
    // ceres::Problem constructProblem();
    void addPointToPointConstriants(ceres::Problem &problem,
                                    vector<Vector3d> &image_corners_3d,
                                    vector<Vector3d> &lidar_corners_3d,
                                    VectorXd &params);

    Eigen::VectorXd get_Rt_l1_c1()
    {
        return m_Rt_l1_c1;
    }
    Eigen::VectorXd get_Rt_l1_c2()
    {
        return m_Rt_l1_c2;
    }
    Eigen::VectorXd get_Rt_c1_c2()
    {
        return m_Rt_c1_c2;
    }
private:
    Eigen::VectorXd m_Rt_l1_c1;
    Eigen::VectorXd m_Rt_l1_c2;
    Eigen::VectorXd m_Rt_c1_c2;
    // Eigen::Vector3d m_plane_normal_l1_c1, m_plane_normal_l1_c2;
    // Eigen::Vector3d m_plane_centroid_l1_c1, m_plane_centroid_l1_c2;
    // vector<Eigen::Vector3d> m_line_normal_l1_c1, m_line_normal_l1_c2;
};

#endif


#include <Master.h>
#define BOOST_TEST_MODULE poker
#include <boost/test/unit_test.hpp>
#include "ut_phases.hpp"
#include <iostream>

#if TEST_ROYAL
BOOST_AUTO_TEST_CASE(ROYAL)
{
	hand_vector s = {card(ace,spades),card(king,spades) ,card(queen,spades) ,card(jack,spades) ,card(ten,spades) };
	BOOST_CHECK(isRoyalFlush(s));
	hand_vector c = { card(ace,clubs),card(king,clubs) ,card(queen,clubs) ,card(jack,clubs) ,card(ten,clubs) };
	BOOST_CHECK(isRoyalFlush(c));
	hand_vector d = { card(ace,diamonds),card(king,diamonds) ,card(queen,diamonds) ,card(jack,diamonds) ,card(ten,diamonds) };
	BOOST_CHECK(isRoyalFlush(d));
	hand_vector h = { card(ace,hearts),card(king,hearts) ,card(queen,hearts) ,card(jack,hearts) ,card(ten,hearts) };
	BOOST_CHECK(isRoyalFlush(h));
	hand_vector n = { card(ace,spades),card(king,diamonds) ,card(queen,hearts) ,card(jack,hearts) ,card(ten,hearts) };
	BOOST_CHECK(!isRoyalFlush(n));
}
#endif

#if TEST_SF
BOOST_AUTO_TEST_CASE(SF)
{
	hand_vector s = { card(ace,spades),card(king,spades) ,card(queen,spades) ,card(jack,spades) ,card(ten,spades) };
	BOOST_CHECK(isStraightFlush(s).is);
	hand_vector c = { card(ace,clubs),card(king,clubs) ,card(queen,clubs) ,card(jack,clubs) ,card(ten,clubs) };
	BOOST_CHECK(isStraightFlush(c).is);
	hand_vector d = { card(ace,diamonds),card(king,diamonds) ,card(queen,diamonds) ,card(jack,diamonds) ,card(ten,diamonds) };
	BOOST_CHECK(isStraightFlush(d).is);
	hand_vector h = { card(ace,hearts),card(king,hearts) ,card(queen,hearts) ,card(jack,hearts) ,card(ten,hearts) };
	BOOST_CHECK(isStraightFlush(h).is);

	hand_vector low = { card(ace,spades),card(two,spades) ,card(three,spades) ,card(four,spades) ,card(five,spades) };
	BOOST_CHECK(isStraightFlush(low).is);
	hand_vector mid = { card(six,spades), card(nine,spades),card(ten,spades) ,card(jack,spades) ,card(eight,spades) ,card(seven,spades) };
	BOOST_CHECK(isStraightFlush(mid).is);
	hand_vector sup = { card(nine,spades),card(ten,spades) ,card(jack,spades) ,card(eight,spades) ,card(seven,spades) };
	BOOST_CHECK(isStraightFlush(mid).hand_ == sup);

	hand_vector n = { card(ace,spades),card(king,diamonds) ,card(queen,hearts) ,card(jack,hearts) ,card(ten,hearts) };
	BOOST_CHECK(!isStraightFlush(n).is);
}
#endif

#if TEST_QUADS
BOOST_AUTO_TEST_CASE(QUADS)
{

	hand_vector low = { card(ace,spades),card(ace,diamonds) ,card(ace,hearts) ,card(two,spades) ,card(two,hearts) };
	BOOST_CHECK(!isQuads(low).is);
	hand_vector mid = { card(six,spades), card(six,hearts),card(six,diamonds) ,card(six,clubs) ,card(eight,spades) ,card(seven,spades) };
	BOOST_CHECK(isQuads(mid).is);
}
#endif

#if TEST_FH
BOOST_AUTO_TEST_CASE(FULLHOUSE)
{

	hand_vector is = { card(ace,spades),card(ace,diamonds) ,card(ace,hearts) ,card(two,spades) ,card(two,hearts) };
	BOOST_CHECK(isFullHouse(is).is);
	hand_vector notfh = { card(six,spades), card(six,hearts),card(six,diamonds) ,card(six,clubs) ,card(eight,spades) ,card(seven,spades) };
	BOOST_CHECK(!isFullHouse(notfh).is);
}
#endif

#if TEST_FLUSH
BOOST_AUTO_TEST_CASE(FLUSH)
{

	hand_vector is = { card(ace,spades),card(king,spades) ,card(queen,spades) ,card(jack,spades) ,card(ten,spades) };
	BOOST_CHECK(isFlush(is).is);
	is = { card(ace,spades),card(king,spades) ,card(queen,spades) ,card(jack,spades) ,card(two,spades),card(two,diamonds),card(two,hearts) };
	BOOST_CHECK(isFlush(is).is);
	hand_vector notfl = { card(six,spades), card(six,hearts),card(six,diamonds) ,card(six,clubs) ,card(eight,spades) ,card(seven,spades) };
	BOOST_CHECK(!isFullHouse(notfl).is);
}
#endif

#if TEST_STRAIGHT
BOOST_AUTO_TEST_CASE(STRAIGHT)
{

	hand_vector is = { card(ace,spades),card(king,spades) ,card(queen,spades) ,card(jack,clubs) ,card(ten,spades) };
	BOOST_CHECK(isStraight(is).is);
	hand_vector notfl = { card(six,spades), card(six,spades),card(six,spades) ,card(six,clubs) ,card(eight,spades) ,card(seven,spades) };
	BOOST_CHECK(!isStraight(notfl).is);
}
#endif

#if TEST_TRIPS
BOOST_AUTO_TEST_CASE(TRIPS)
{

	hand_vector is = { card(ace,spades),card(ace,diamonds) ,card(queen,spades) ,card(jack,clubs) ,card(ace,clubs) };
	BOOST_CHECK(isTrips(is).is);
	hand_vector notfl = { card(six,spades), card(six,spades),card(six,spades) ,card(six,clubs) ,card(eight,spades) ,card(seven,spades) };
	BOOST_CHECK(!isTrips(notfl).is);
}
#endif

#if TEST_TWOPAIR
BOOST_AUTO_TEST_CASE(TWOPAIR)
{

	hand_vector is = { card(ace,clubs),card(ace,clubs) ,card(queen,clubs), card(king,clubs),card(queen,clubs),card(jack,clubs),card(two,clubs) };
	hand_vector sb = { card(ace,clubs),card(ace,clubs) ,card(queen,clubs),card(queen,clubs), card(king,clubs) };
	BOOST_CHECK(isTwoPair(is).is);
	BOOST_CHECK(isTwoPair(is).hand_ == sb);
	hand_vector notfl = { card(six,spades), card(six,spades),card(six,spades) ,card(six,clubs) ,card(eight,spades) ,card(seven,spades) };
	BOOST_CHECK(!isTwoPair(notfl).is);

	hand_vector threepair = { card(ace,clubs), card(ace,clubs),card(six,clubs) ,card(six,clubs) ,card(eight,clubs) ,card(eight,clubs) };
	hand_vector sp = { card(ace,clubs), card(ace,clubs),card(six,clubs) ,card(eight,clubs) ,card(eight,clubs) };
	BOOST_CHECK(isTwoPair(threepair).hand_ == sp);

	hand_vector s = { card(ace,spades),card(ace,clubs) ,card(two,diamonds) ,card(two,spades) ,card(ten,spades) };
	BOOST_CHECK(isTwoPair(s).is);
}
#endif

#if TEST_PAIR
BOOST_AUTO_TEST_CASE(ONEPAIR)
{
	BOOST_ASSERT(1 == 1);
	hand_vector is = { card(ace,clubs),card(ace,clubs) ,card(queen,clubs), card(king,clubs),card(three,clubs),card(jack,clubs),card(two,clubs) };
	hand_vector sb = { card(ace,clubs),card(ace,clubs) ,card(queen,clubs), card(king,clubs),card(jack,clubs) };
	BOOST_CHECK(isPair(is).is);
	BOOST_CHECK(isPair(is).hand_ == sb);
}
#endif


#ifndef C2M_TYPES_HPP
#define C2M_TYPES_HPP

#include <OpenMesh/Core/Geometry/VectorT.hh>
#include <OpenMesh/Core/Utils/vector_traits.hh>
#include <OpenMesh/Core/Geometry/QuadricT.hh>

#include <Eigen/Dense>

#include <highfive/H5Exception.hpp>
#include <highfive/H5File.hpp>

namespace c2m
{
using uint = unsigned int;
/**
 * @brief Types to be used for the identifiers of HDF5 entities
 *
 */
using entid_t = int; // needs to be >= nodeid_t, elemid_t, partid_t
using nodeid_t = int;
using elemid_t = int;
using partid_t = int;
#define ID_NULL 0

/**
 * @brief Exception to handle when reading from HDF5
 *
 */
using H5Exception = HighFive::Exception;
/**
 * @brief HDF5 file handle
 *
 */
using H5File = HighFive::File;

using OMVec3 = OpenMesh::Vec3f;

/**
 * @brief 3D float vector for use with OpenVolumeMesh
 *
 */
using Vec3 = Eigen::Vector3f;
using Vec4 = Eigen::Vector4f;
using VecX = Eigen::VectorXf;

template <typename Scalar, int Rows, int Cols>
using Mat = Eigen::Matrix<Scalar, Rows, Cols>;
using Eigen::Dynamic;

using Mat4 = Eigen::Matrix4f;
using Mat3 = Eigen::Matrix3f;
using Mat3X = Eigen::Matrix3Xf;
using MatX3 = Eigen::MatrixX3f;
using MatX = Eigen::MatrixXf;

#ifdef C2M_PROB_QUADRICS
    using Quadric = Mat4;
#else
    using Quadric = OpenMesh::Geometry::Quadricf;
#endif

} // namespace c2m

#endif


#include "util/Base.h"
#include "util/Conf.h"
#include "structs/Matrix.h"
#include "util/Logger.h"
#include "alg/Naive.h"
#include "alg/tree/BallTreeSearch.h"
#include "alg/svd/SVDIncrPrune.h"
#include "alg/svd/SVDIntUpperBoundIncrPrune.h"
#include "alg/svd/SVDIntUpperBoundIncrPrune2.h"
#include "alg/int/IntUpperBound.h"
#include "alg/tree/FastMKS.h"
#include "alg/transformation/TransformIncrPrune.h"
#include "alg/simd/SIMDIntUpperBound.h"
#include "alg/svd/SVDIncrPruneIndividualReorder.h"
#include "alg/transformation/TransformSVDIncrPrune.h"
#include "alg/transformation/TransformSVDIncrPrune2.h"
#include "alg/int/IntUpperBound2.h"
#include <boost/program_options.hpp>

namespace po = boost::program_options;

void basicLog(const Matrix &q, const Matrix &p, const int k) {
    Logger::Log("q path: " + to_string(Conf::qDataPath));
    Logger::Log("p path: " + to_string(Conf::pDataPath));
    Logger::Log("q: " + to_string(q.rowNum) + "," + to_string(q.colNum));
    Logger::Log("p: " + to_string(p.rowNum) + "," + to_string(p.colNum));
    Logger::Log("Algorithm: " + Conf::algName);
    Logger::Log("k: " + to_string(k));
}

int main(int argc, char **argv) {

    po::options_description desc("Allowed options");
    desc.add_options()
            ("help", "produce help message")
            ("alg", po::value<string>(&(Conf::algName))->default_value("naive"), "Algorithm")
            ("k", po::value<int>(&(Conf::k))->default_value(1), "K")
            ("dataset", po::value<string>(&(Conf::dataset)), "name of dataset for log output")
            ("q", po::value<string>(&(Conf::qDataPath)), "file path of q Data")
            ("p", po::value<string>(&(Conf::pDataPath)), "file path of p Data")
            ("scalingValue", po::value<int>(&(Conf::scalingValue))->default_value(127), "maximum value for scaling")
            ("SIGMA", po::value<double>(&(Conf::SIGMA))->default_value(0.8), "percentage of SIGMA for SVD incremental prune")
            ("log", po::value<bool>(&(Conf::log))->default_value(true), "whether it outputs log")
            ("logPathPrefix", po::value<string>(&(Conf::logPathPrefix))->default_value("./log"), "output path of log file (Prefix)")
            ("outputResult", po::value<bool>(&(Conf::outputResult))->default_value(true), "whether it outputs results")
            ("resultPathPrefix", po::value<string>(&(Conf::resultPathPrefix))->default_value("./result"), "output path of result file (Prefix)");

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help")) {
        cout << desc << "\n";
        return 0;
    } else if (Conf::qDataPath == "" || Conf::pDataPath == "") {
        cout << "Please specify path to data files" << endl << endl;
        cout << desc << endl;
        return 0;
    }

//    Conf::qDataPath = "../../data/MovieLens/q.txt";
//    Conf::pDataPath = "../../data/MovieLens/p.txt";
//    Conf::dataset = "MovieLens";
//    Conf::k = 1;
//    Conf::SIGMA = 0.7;
//    Conf::algName = "FEIPR-I2";
//    Conf::algName = "FEIPR-I";
//    Conf::algName = "FEIPR-S";

    Conf::Output();

    Matrix q;
    Matrix p;
    q.readData(Conf::qDataPath);
    p.readData(Conf::pDataPath);
    Conf::dimension = p.colNum;

    cout << "-----------------------" << endl;

    // ToDo: replace the old name (FEIPR) with FEXIPRO

    if (Conf::algName == "Naive") {
        string logFileName = Conf::logPathPrefix + "-" + Conf::dataset + "-" + Conf::algName + "-" + to_string(Conf::k) + ".txt";
        Logger::open(logFileName);
        basicLog(q, p, Conf::k);

        naive(Conf::k, q, p);

    } else if (Conf::algName == "SIR") {

        string logFileName = Conf::logPathPrefix + "-" + Conf::dataset + "-" + Conf::algName + "-" + to_string(Conf::k) + "-" + to_string(Conf::scalingValue) + "-" + to_string(Conf::SIGMA) + ".txt";
        Logger::open(logFileName);
        basicLog(q, p, Conf::k);
        Logger::Log("SIGMA: " + to_string(Conf::SIGMA));
        Logger::Log("Scaling Value: " + to_string(Conf::scalingValue));

        SIRPrune sirPrune(Conf::k, Conf::scalingValue, Conf::SIGMA, &q, &p);
        sirPrune.topK();

    } else if (Conf::algName == "SR") {

        string logFileName = Conf::logPathPrefix + "-" + Conf::dataset + "-" + Conf::algName + "-" + to_string(Conf::k) + "-" + to_string(Conf::scalingValue) + "-" + to_string(Conf::SIGMA) + ".txt";
        Logger::open(logFileName);
        basicLog(q, p, Conf::k);
        Logger::Log("SIGMA: " + to_string(Conf::SIGMA));

        TransformSVDIncrPrune2 transformSVDIncrPrune2(Conf::k, Conf::SIGMA, &q, &p);
        transformSVDIncrPrune2.topK();

    } else if (Conf::algName == "I") {

        string logFileName = Conf::logPathPrefix + "-" + Conf::dataset + "-" + Conf::algName + "-" + to_string(Conf::k) + "-" + to_string(Conf::scalingValue) + ".txt";
        Logger::open(logFileName);
        basicLog(q, p, Conf::k);
        Logger::Log("Scaling Value: " + to_string(Conf::scalingValue));

        IntUpperBound intUpperBound(Conf::k, Conf::scalingValue, &q, &p);
        intUpperBound.topK();

    } else if (Conf::algName == "I-SIMD") {

        string logFileName = Conf::logPathPrefix + "-" + Conf::dataset + "-" + Conf::algName + "-" + to_string(Conf::k) + "-" + to_string(Conf::scalingValue) + ".txt";
        Logger::open(logFileName);
        basicLog(q, p, Conf::k);
        Logger::Log("Scaling Value: " + to_string(Conf::scalingValue));

        SIMDIntUpperBound simdIntUpperBound(Conf::k, &q, &p);
        simdIntUpperBound.topK();

    } else if (Conf::algName == "S"){
        string logFileName = Conf::logPathPrefix + "-" + Conf::dataset  + "-" + Conf::algName + "-" + to_string(Conf::k) + "-" +
                             to_string(Conf::SIGMA) + ".txt";
        Logger::open(logFileName);
        basicLog(q, p, Conf::k);
        Logger::Log("SIGMA: " + to_string(Conf::SIGMA));

        SVDIncrPrune svdIncrPrune(Conf::k, Conf::SIGMA, &q, &p);
        svdIncrPrune.topK();

    } else if (Conf::algName == "S-Ind"){
        string logFileName = Conf::logPathPrefix + "-" + Conf::dataset  + "-" + Conf::algName + "-" + to_string(Conf::k) + "-" +
                             to_string(Conf::SIGMA) + ".txt";
        Logger::open(logFileName);
        basicLog(q, p, Conf::k);
        Logger::Log("SIGMA: " + to_string(Conf::SIGMA));

        SVDIncrPruneIndividualReorder svdIncrPruneIndividualReorder(Conf::k, Conf::SIGMA, &q, &p);
        svdIncrPruneIndividualReorder.topK();

    } else if (Conf::algName == "SI") {

        string logFileName = Conf::logPathPrefix + "-" + Conf::dataset  + "-" + Conf::algName + "-" + to_string(Conf::k) + "-" + to_string(Conf::scalingValue) + "-" +
                             to_string(Conf::SIGMA) + ".txt";
        Logger::open(logFileName);
        basicLog(q, p, Conf::k);
        Logger::Log("SIGMA: " + to_string(Conf::SIGMA));
        Logger::Log("Scaling Value: " + to_string(Conf::scalingValue));

        SVDIntUpperBoundIncrPrune svdIntUpperBoundIncrPrune(Conf::k, Conf::scalingValue, Conf::SIGMA, &q, &p);
        svdIntUpperBoundIncrPrune.topK();

    } else if (Conf::algName == "BallTree") {

        string logFileName = Conf::logPathPrefix + "-" + Conf::dataset  + "-" + Conf::algName + "-" + to_string(Conf::k) + ".txt";
        Logger::open(logFileName);
        basicLog(q, p, Conf::k);

        ballTreeTopK(Conf::k, q, p);

    } else if (Conf::algName == "FastMKS") {

        string logFileName = Conf::logPathPrefix + "-" + Conf::dataset  + "-" + Conf::algName + "-" + to_string(Conf::k) + ".txt";
        Logger::open(logFileName);
        basicLog(q, p, Conf::k);

        fastMKS(Conf::k, Conf::pDataPath, Conf::qDataPath);
    } else {
        cout << "unrecognized method" << endl;
    }

    return 0;
}


// Make unsigned
struct unsigne; // No typo: `unsigned` is a keyword, we can not use it.

// Make constant
struct constant;

// Otherwise we do not change type
struct no_change;

// we'll need this at step 3
#include <boost/mpl/size.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/static_assert.hpp>

// we'll need this at step 4
#include <boost/mpl/if.hpp>
#include <boost/type_traits/make_unsigned.hpp>
#include <boost/type_traits/add_const.hpp>

// we'll need this at step 5
#include <boost/mpl/transform.hpp>

template <class Types, class Modifiers>
struct do_modifications {
    BOOST_STATIC_ASSERT((boost::is_same<
        typename boost::mpl::size<Types>::type,
        typename boost::mpl::size<Modifiers>::type
    >::value));

    BOOST_STATIC_ASSERT((
        boost::mpl::size<Types>::type::value
        ==
        boost::mpl::size<Modifiers>::type::value
    ));

    typedef boost::mpl::if_<
        boost::is_same<boost::mpl::_2, unsigne>,
        boost::make_unsigned<boost::mpl::_1>,
        boost::mpl::if_<
            boost::is_same<boost::mpl::_2, constant>,
            boost::add_const<boost::mpl::_1>,
            boost::mpl::_1
        >
    > binary_operator_t;

    // >::type binary_operator_t; // INCORRECT!

    typedef typename boost::mpl::transform<
        Types,
        Modifiers,
        binary_operator_t
    >::type type;
};

/*
void boost_mpl_transform_pseoudocode() {

    vector result;
    for (std::size_t i = 0; i < Types.size(); ++i) {
        result.push_back(
            binary_operator_t(Types[i], Modifiers[i])
        );
    }
    return result;
}
*/

#include <boost/mpl/vector.hpp>
typedef boost::mpl::vector<unsigne, no_change, constant, unsigne> modifiers;
typedef boost::mpl::vector<int, char, short, long> types;

typedef do_modifications<types, modifiers>::type result_type;

#include <boost/mpl/at.hpp>

BOOST_STATIC_ASSERT((boost::is_same<
    boost::mpl::at_c<result_type, 0>::type,
    unsigned int
>::value));

BOOST_STATIC_ASSERT((boost::is_same<
    boost::mpl::at_c<result_type, 1>::type,
    char
>::value));

BOOST_STATIC_ASSERT((boost::is_same<
    boost::mpl::at_c<result_type, 2>::type,
    const short
>::value));

BOOST_STATIC_ASSERT((boost::is_same<
    boost::mpl::at_c<result_type, 3>::type,
    unsigned long
>::value));

// GCC4.6 has problems with variadic templates:
// # sorry, unimplemented: cannot expand ‘T ...’ into a fixed-length argument list
// NOTE: Clang compiler defines __GNUC__
#if defined(__clang__) || !(defined(__GNUC__) && (__GNUC__ == 4) && (__GNUC_MINOR__ < 7))

#include <boost/config.hpp>
#ifndef BOOST_NO_CXX11_VARIADIC_TEMPLATES

template <class... T>
struct vt_example {
    typedef typename boost::mpl::vector<T...> type;
};

BOOST_STATIC_ASSERT((boost::is_same<
    boost::mpl::at_c<vt_example<int, char, short>::type, 0>::type,
    int
>::value));

#endif

#endif // gcc4.6 workaround

#include <iostream>
#include <typeinfo>
int main() {
    std::cout << typeid(boost::mpl::size<types>::type).name() << '\n';
}


#ifndef TEST_UNIT_MATH_MIX_MAT_VECTORIZE_EXPECT_MIX_VECTOR_VALUE_HPP
#define TEST_UNIT_MATH_MIX_MAT_VECTORIZE_EXPECT_MIX_VECTOR_VALUE_HPP

#include <vector>
#include <Eigen/Dense>
#include <test/unit/math/mix/mat/vectorize/build_mix_matrix.hpp>
#include <test/unit/math/mix/mat/vectorize/expect_val_deriv_eq.hpp>

template <typename F, typename T>
void expect_mix_vector_value() {
  using std::vector;
  typedef Eigen::Matrix<T, Eigen::Dynamic, 1> vector_t;

  size_t num_inputs = F::valid_inputs().size();
  vector_t template_v(num_inputs);

  for (size_t i = 0; i < num_inputs; ++i) {
    vector_t a = build_mix_matrix<F>(template_v, i);
    vector_t b = build_mix_matrix<F>(template_v, i);
    vector_t fb = F::template apply<vector_t>(b);
    EXPECT_EQ(b.size(), fb.size());
    expect_val_deriv_eq(F::apply_base(a(i)), a(i), fb(i), b(i));
  }

  size_t vector_vector_size = 2;
  for (size_t i = 0; i < vector_vector_size; ++i) {
    for (size_t j = 0; j < num_inputs; ++j) {
      vector<vector_t> c;
      vector<vector_t> d;
      for (size_t k = 0; k < vector_vector_size; ++k)
        if (k == i) {
          c.push_back(build_mix_matrix<F>(template_v, j));
          d.push_back(build_mix_matrix<F>(template_v, j));
        }
        else {
          c.push_back(build_mix_matrix<F>(template_v));
          d.push_back(build_mix_matrix<F>(template_v));
        }
      vector<vector_t> fd = F::template apply<vector<vector_t> >(d);
      EXPECT_EQ(d.size(), fd.size());
      EXPECT_EQ(d[i].size(), fd[i].size());
      expect_val_deriv_eq(F::apply_base(c[i](j)), c[i](j),
                                        fd[i](j), d[i](j));
    }
  }
}

#endif


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<double, 3, 3> test_double_3_3(const Eigen::Matrix<double, 3, 3> & M)
{
	return M;
}
void export_double_3_3()
{
	boost::python::def("test_double_3_3",test_double_3_3);
}


/**
 * @author Eric Cousineau <eacousineau@gmail.com>, member of Dr. Aaron
 * Ames's AMBER Lab
 */
#include <gtest/gtest.h>

#include <Eigen/Dense>
#include <eigen_utilities/assert_size.hpp>

TEST(eigen_utilities, basic_check)
{
    Eigen::VectorXd X(10);
    EXPECT_NO_THROW(eigen_utilities::assert_size(X, 10));
    EXPECT_THROW(eigen_utilities::assert_size(X, 8), eigen_utilities::assert_error);

    Eigen::MatrixXd A(10, 10);
    EXPECT_NO_THROW(eigen_utilities::assert_size(A, 10, 10));
    EXPECT_THROW(eigen_utilities::assert_size(A, 8, 4), eigen_utilities::assert_error);

    // Print out error
    try
    {
        eigen_utilities::assert_size(X, 8, 4);
    }
    catch (eigen_utilities::assert_error &ex)
    {
        std::cout << ex.what() << std::endl;
    }
    // Print out error
    try
    {
        assert_size_matrix(X, 8, 4);
    }
    catch (eigen_utilities::assert_error &ex)
    {
        std::cout << ex.what() << std::endl;
    }
}


#include <iostream>
#include <fstream>
#include <random>
#include <time.h>
#include <chrono>
#include <vector>

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

using namespace std;
//using namespace boost::numeric::ublas;

int DATA_NUM = 100;
int DATA_DIM = 2;
int COMPONENT_NUM = 3;

std::string FILE_NAME = "./gmm.dat";

class Gaussian_Model
{
public:
	Gaussian_Model(std::size_t dim)
	{
		dim_ = dim;
		count_ = 0;
		prior_ = 0.0f;
		mptr_ = 0;

	/*	mean_ = new float[dim];
		var_ = new float[dim * dim];*/
	}

	Gaussian_Model(std::size_t dim, float m, float s, float p)
	{
		dim_ = dim;
		count_ = 0;
		prior_ = p;
		mptr_ = 0;
	/*	mean_ = new float[dim];
		var_ = new float[dim * dim];

		int i;
		for (i = 0; i < dim; i++)
		{
			mean_[i] = m;
			var_[i + i * dim] = s;
		}*/
	}

	~Gaussian_Model()
	{
		if (mptr_)
		{
			delete[] mptr_;
			mptr_ = 0;
		}
	}

	void init()
	{
		mptr_ = new float[dim_ + dim_ * dim_ * 2];
		mean_ = mptr_;
		var_ = mean_ + dim_ * dim_;
		invvar_ = var_ + dim_ * dim_;
	}

	float* getmean() { return mean_; };
	float* getstd() { return var_; };
	float getprior() { return prior_; };
	void setprior(float v) { prior_ = v; };
	float getcount() { return count_; };
	void setcount(float v) { count_ = v; };
	int getdim() { return dim_; };

	//mean = alpha
	void setmean(float alpha)
	{
		int i;
		for (i = 0; i < dim_; i++)
		{
			mean_[i] = alpha;
		}
	}

	//mean = mean + alpha * data
	void accumulatemean(float alpha, float* data)
	{
		int i;
		for (i = 0; i < dim_; i++)
		{
			mean_[i] += alpha * data[i];
		}
	}

	void setmean(float* data)
	{
		memcpy(mean_, data, dim_ * sizeof(float));
	}

	void scalemean(float alpha)
	{
		int i;
		for (i = 0; i < dim_; i++)
		{
			mean_[i] *= alpha;
		}
	}

	void accumulatecount(float count)
	{
		count_ += count;
	}

	void accumulatevar(float alpha, float* data)
	{
		int i, j;
		float v;
		for (i = 0; i < dim_; i++)
		{
			for (j = i; j < dim_; j++)
			{
				v = data[i] * data[j] * alpha;
				var_[i * dim_ + j] += v;
				var_[j * dim_ + i] += v;
			}
		}
	}

	void accumulatevar2(float alpha, float* data)
	{
		int i, j;
		float* temp = new float[dim_];

		for (i = 0;i < dim_; i++)
		{
			temp[i] = data[i] * alpha;
		}

		for (i = 0; i < dim_; i++)
		{
			for (j = 0; j < dim_; j++)
			{
				var_[i * dim_ + j] += temp[i] * data[j];
			}
		}

		delete[] temp;
	}

	void setvar(float alpha)
	{
		int i;
		for (i = 0; i < dim_ * dim_; i++)
		{
			var_[i] = alpha;
			invvar_[i] = 1 / alpha;
		}
	}

	void setdiagvar(float alpha)
	{
		int i;
		for (i = 0; i < dim_; i++)
		{
			var_[i + i * dim_] = alpha;
			invvar_[i + i * dim_] = 1 / alpha;
		}
	}

	void scalevar(float alpha)
	{
		int i;
		for (i = 0; i < dim_ * dim_; i++)
		{
			var_[i] *= alpha;
			invvar_[i] /= alpha;
		}
	}

	float calprob(float* data)
	{
		return 1.0f;
	};

	float calinvvar()
	{

	}

	void preprocess()
	{
		this->setmean(0.0f);
		this->setcount(0.0f);
		this->setvar(0.0f);
	};

	void updatestep(float prob, float* x)
	{
		this->accumulatecount(prob);
		this->accumulatemean(prob, x);
		this->accumulatevar(prob, x);
	};

	void postprocess(int N)
	{
		this->scalemean(1.0f / N);
		this->setprior(this->getcount() / N);
		this->scalevar(1.0f / N);
	}

	void printinfo()
	{
		int i, j;
		std::cout << "dim: " << dim_ << std::endl;
		std::cout << "mean: ";
		for (i = 0;i < dim_; i++)
		{
			std::cout << mean_[i] << " ";
		}
		std::cout << std::endl;

		std::cout << "prior: " << prior_ << std::endl;
		std::cout << "count: " << count_ << std::endl;

		std::cout << "var: " << std::endl;
		for (i = 0; i < dim_; i++)
		{
			for (j = 0;j < dim_; j++)
			{
				std::cout << var_[i * dim_ + j] << " ";
			}

			std::cout << std::endl;
		}

		std::cout << std::endl;
	}

private:
	float* mptr_;
	float* mean_;
	float* var_;
	float prior_;
	float count_;
	std::size_t dim_;
	float* invvar_;
};

void generatedata()
{
	unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();

	std::default_random_engine generator(seed);
	std::normal_distribution<float> distribution(0.0, 1.0);

	int i, j;
	ofstream ouF;
	ouF.open(FILE_NAME, std::ofstream::binary | std::ofstream::out);
	ouF.write(reinterpret_cast<const char*>(&DATA_NUM), sizeof(DATA_NUM));
	ouF.write(reinterpret_cast<const char*>(&DATA_DIM), sizeof(DATA_DIM));

	for (i = 0; i < DATA_NUM; i++)
	{
		for (j = 0; j < DATA_DIM; j++)
		{
			float x = distribution(generator);
			ouF.write(reinterpret_cast<const char*>(&x), sizeof(float));
		}
	}

	ouF.close();
}

float* readdata()
{
	ifstream inF;
	inF.open(FILE_NAME);

	inF.read(reinterpret_cast<char*>(&DATA_NUM), sizeof(DATA_NUM));
	inF.read(reinterpret_cast<char*>(&DATA_DIM), sizeof(DATA_DIM));

	float* data = new float[DATA_NUM * DATA_DIM];
	inF.read(reinterpret_cast<char*>(data), DATA_DIM * DATA_NUM);

	inF.close();

	return data;
}

void fitdata(std::vector<Gaussian_Model> & models, float* fit_data, int iteration=1)
{
	float* data_prob = new float[COMPONENT_NUM * DATA_NUM];

	int i, j;
	float* prob = data_prob;
	int r;

	for (r = 0; r < iteration; r++)
	{
		//E-step
		float sum;
		for (i = 0; i < DATA_NUM; i++)
		{
			sum = 0.0f;
			for (j = 0; j < COMPONENT_NUM; j++)
			{
				prob[j] = models[j].getprior() * models[j].calprob(fit_data + i * DATA_DIM);
				sum += prob[j];
			}

			for (j = 0; j < COMPONENT_NUM; j++)
			{
				prob[j] /= (sum + 0.000001f);
			}

			prob += COMPONENT_NUM;
		}

		//M-step
		for (i = 0; i < COMPONENT_NUM; i++)
		{
			models[i].preprocess();
		}

		float maxprob;
		int comindex = 0;
		prob = data_prob;
		for (i = 0; i < DATA_NUM; i++)
		{
			maxprob = *prob;
			comindex = 0;
			for (j = 1; j < COMPONENT_NUM; j++)
			{
				if (prob[j] > maxprob)
				{
					maxprob = prob[j];
					comindex = j;
				}
			}

			models[comindex].updatestep(prob[comindex], fit_data + i * DATA_DIM);

			prob += COMPONENT_NUM;
		}

		for (i = 0; i < COMPONENT_NUM; i++)
		{
			models[i].postprocess(DATA_NUM);
		}
	}

	delete[] data_prob;
}

class Test
{
public:
	Test()
		: m_ptr()
	{
		m_ptr = new float[20];
	};

	~Test()
	{
		if (m_ptr)
			delete [] m_ptr;
	}

	void Init()
	{

	}

private:
	float* m_ptr;

};

int main(int /*argc*/, char* /*argv*/[])
{
	cout << sizeof(DATA_NUM) << " " << sizeof(DATA_DIM) << endl;

	//matrix<float, row_major, unbounded_array<float, std::allocator<float>>> c1;

	//identity_matrix<double, std::allocator> m(3, 3);

	//std::vector<Test> tests;
	////tests.resize(3);
	//Test test1;
	//Test test2;
	//Test test3;

	//shared_ptr<Test> ptest1(new Test());
	//shared_ptr<Test> ptest2(new Test());
	//shared_ptr<Test> ptest3(new Test());

	//tests.push_back(*ptest1);
	//tests.push_back(*ptest2);
	//tests.push_back(*ptest3);

	//int n;
	//for (n = 0;n < tests.size(); n++)
	//{
	//	tests[n].Init();
	//}

	//generatedata();

	float* fit_data = readdata();

	std::vector<Gaussian_Model> models;

	models.push_back(Gaussian_Model(DATA_DIM));
	models.push_back(Gaussian_Model(DATA_DIM));
	models.push_back(Gaussian_Model(DATA_DIM));

	models[0].init();
	models[0].setmean(0.0f);
	models[0].setdiagvar(1.0f);
	models[0].setprior(1.0f);

	models[1].init();
	models[1].setmean(1.0f);
	models[1].setdiagvar(1.0f);
	models[1].setprior(1.0f);

	models[2].init();
	models[2].setmean(-1.0f);
	models[2].setdiagvar(1.0f);
	models[2].setprior(1.0f);

	fitdata(models, fit_data);

	delete[] fit_data;

	int i;
	for (i = 0; i < COMPONENT_NUM; i++)
	{
		models[i].printinfo();
	}

	return 1;
}


//==================================================================================================
/*!
  @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_BITFLOATING_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_BITFLOATING_HPP_INCLUDED

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

  /*!
    @ingroup group-ieee
    This function object  transforms an integer value to a floating point one
    with a preserving order formula. This function is the converse of @ref bitinteger.

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

    @par Note:
    This  function is  related to the computation of
    floating successors or predecessors as the ordering of the integers obtained
    by @c bifloating is the same as the original order on the floating elements,
    an increment can be used to find the successor.

    @see next, prev, successor,  predecessor,  nextafter, bitinteger

    @par Example:

      @snippet bitfloating.cpp bitfloating

    @par Possible output:

      @snippet bitfloating.txt bitfloating

  **/
  as_floating_t<Value> bitfloating(Value const& x);
} }
#endif

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

#endif


/*
Copyright (c) 2014, Kai Klindworth
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice, this
   list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include "ml_region_optimizer_disp.h"
#include "ml_region_optimizer.h"

#include <omp.h>
#include "disparity_region.h"
#include "disparity_region_algorithms.h"
#include <segmentation/intervals.h>
#include <segmentation/intervals_algorithms.h>
#include "disparity_toolkit/disparity_utils.h"
#include "disparity_toolkit/genericfunctions.h"
#include "disparity_toolkit/disparity_range.h"

#include <fstream>
#include <boost/lexical_cast.hpp>
#include <neural_network/network.h>
#include <neural_network/data_normalizer.h>
#include "costmap_utils.h"
#include "debugmatstore.h"
#include "ml_region_optimizer_algorithms.h"

using namespace neural_network;

void init_network_disp(network<double>& net, int /*crange*/, int nvector, int pass)
{
	//net.emplace_layer<vector_extension_layer>(ml_region_optimizer::vector_size_per_disp, ml_region_optimizer::vector_size);
	//net.emplace_layer<vector_connected_layer>(nvector*2, nvector*2, pass);
	//net.emplace_layer<relu_layer>();
	net.emplace_layer<vector_connected_layer>(nvector*2, nvector, pass);
	net.emplace_layer<relu_layer>();
	net.emplace_layer<vector_connected_layer>(nvector, nvector*2, pass);
	net.emplace_layer<relu_layer>();
	net.emplace_layer<fully_connected_layer>(nvector);
	net.emplace_layer<relu_layer>();
	net.emplace_layer<fully_connected_layer>(nvector);
	net.emplace_layer<relu_layer>();
	//net.emplace_layer<fully_connected_layer>(4);
	//net.emplace_layer<relu_layer>();
	net.emplace_layer<fully_connected_layer>(2);
	net.emplace_layer<softmax_output_layer>();
	//net.emplace_layer<tanh_output_layer>();
}

void ml_region_optimizer_disp::optimize_ml(region_container& base, const region_container& match, std::vector<std::vector<float>>& optimization_vectors_base, std::vector<std::vector<float>>& optimization_vectors_match, int delta, const std::string& filename)
{
	int crange = 164;
	std::cout << "optimize_dsip" << std::endl;

	std::ifstream istream(filename);

	if(!istream.is_open())
		throw std::runtime_error("file not found: " + filename);

	int sample_size = vector_size_per_disp*2+vector_size;

	network<double> net(sample_size);

	int nvector = ml_region_optimizer_disp::vector_size_per_disp;
	int pass = ml_region_optimizer_disp::vector_size;
	init_network_disp(net, sample_size, nvector, pass);

	data_normalizer<double> normalizer(istream);
	istream >> net;

	const std::size_t regions_count = base.regions.size();
	const short sign = (base.task.range.start() < 0) ? -1 : 1;

	std::vector<std::vector<double>> region_optimization_vectors(omp_get_max_threads(), std::vector<double>(base.task.range.size()*vector_size_per_disp*2+vector_size));
	//#pragma omp parallel for
	std::vector<double> temp(sample_size);
	for(std::size_t j = 0; j < regions_count; ++j)
	{
		std::vector<double>& optimization_vector = region_optimization_vectors[omp_get_thread_num()];
		merge_with_corresponding_feature_vector<vector_size, vector_size_per_disp>(optimization_vector.data(), base.regions[j], optimization_vectors_base[j], optimization_vectors_match, match, delta, base.task);

		const double* temp_ptr = optimization_vector.data();

		double best_output = -1;
		int best_idx = -1;

		for(int i = 0; i < crange; ++i)
		{

			auto copy_sample = [&](int disp) {
				int per_disp = vector_size_per_disp*2;
				std::copy(temp_ptr + std::abs(disp)*per_disp, temp_ptr + (std::abs(disp)+1)*per_disp, temp.data());
				std::copy(temp_ptr + crange*per_disp, temp_ptr + crange*per_disp+vector_size, temp.data() + per_disp);
			};

			copy_sample(i);
			normalizer.apply(temp);
			double res = net.output(temp.data())[1];

			if(res > best_output)
			{
				best_idx = i;
				best_output = res;
			}
		}
		base.regions[j].disparity = best_idx * sign;
	}

	refresh_warped_regions(base);
}

void ml_region_optimizer_disp::prepare_training_sample(std::vector<short>& dst_gt, std::vector<std::vector<double>>& dst_data, const std::vector<std::vector<float>>& base_optimization_vectors, const std::vector<std::vector<float>>& match_optimization_vectors, const region_container& base, const region_container& match, int delta)
{
	dst_gt.reserve(dst_gt.size() + base.regions.size());
	std::vector<short> gt;
	gt.reserve(base.regions.size());
	average_region_ground_truth(base.regions, base.task.groundTruth, std::back_inserter(gt));

	const int crange = base.task.range.size();

	const std::size_t regions_count = base.regions.size();
	dst_data.reserve(dst_data.size() + base_optimization_vectors.size());

	int draw_eps = 4;
	std::mt19937 rng;
	std::uniform_int_distribution<> dist(0, crange - 2*draw_eps - 2);

	assert(gt.size() == regions_count);
	//#pragma omp parallel for default(none) shared(base, match, delta, normalization_vector)
	int hardcase_advance = 0;
	std::vector<double> temp_vector(vector_size_per_disp*2*crange+vector_size);
	result_eps_calculator diff_calc;
	for(std::size_t j = 0; j < regions_count; ++j)
	{
		if(gt[j] != 0 && gt[j] < crange)
		{
			int sample_size = vector_size_per_disp*2+vector_size;
			double *temp_ptr = temp_vector.data();
			merge_with_corresponding_feature_vector<vector_size, vector_size_per_disp>(temp_ptr, base.regions[j], base_optimization_vectors[j], match_optimization_vectors, match, delta, base.task);

			auto copy_sample = [&](int disp, bool correct) {
				dst_data.emplace_back(sample_size);
				int per_disp = vector_size_per_disp*2;
				std::copy(temp_ptr + std::abs(disp)*per_disp, temp_ptr + (std::abs(disp)+1)*per_disp, dst_data.back().data());
				std::copy(temp_ptr + crange*per_disp, temp_ptr + crange*per_disp+vector_size, dst_data.back().data() + per_disp);

				dst_gt.push_back(correct ? 1 : 0);
			};

			auto draw_neg_sample = [&]() {
				int neg_disp_sample_idx = dist(rng);
				if(std::abs(gt[j]) - draw_eps < neg_disp_sample_idx)
					neg_disp_sample_idx += 2*draw_eps+1;

				assert(neg_disp_sample_idx >= 0 && neg_disp_sample_idx < crange);

				copy_sample(neg_disp_sample_idx, false);
			};

			//copy_sample(gt[j], true);

			//draw_neg_sample();

			if(std::abs(base.regions[j].disparity - gt[j]) > 5)
			{
				copy_sample(base.regions[j].disparity, false);
				copy_sample(gt[j], true);

				draw_neg_sample();
				draw_neg_sample();

				hardcase_advance++;
			}
			else if(hardcase_advance > 0)
			{
				draw_neg_sample();
				draw_neg_sample();
				draw_neg_sample();

				copy_sample(gt[j], true);

				hardcase_advance--;
			}
			//else
				//draw_neg_sample();

			//std::cout << base.regions[j].disparity << " vs " << gt[j] << std::endl;
			diff_calc(base.regions[j].disparity, gt[j]);
		}
	}
	std::cout << diff_calc << std::endl;
}

void ml_region_optimizer_disp::reset_internal()
{
	const int crange = 164;
	int dims = crange * vector_size_per_disp*2+vector_size;
	//int nvector = ml_region_optimizer::vector_size_per_disp + ml_region_optimizer::vector_size;
	int nvector = ml_region_optimizer_disp::vector_size_per_disp;
	int pass = ml_region_optimizer_disp::vector_size;
	nnet = std::unique_ptr<network<double>>(new network<double>(dims));
	init_network_disp(*nnet, crange, nvector, pass);
}

ml_region_optimizer_disp::ml_region_optimizer_disp()
{
	reset_internal();
	training_iteration = 0;
	filename_left_prefix = "weights-left-";
	filename_right_prefix = "weights-right-";
}

ml_region_optimizer_disp::~ml_region_optimizer_disp()
{
}

void ml_region_optimizer_disp::reset(const region_container& /*left*/, const region_container& /*right*/)
{
	reset_internal();
}

void training_internal_per_disp(std::vector<std::vector<double>>& samples, std::vector<short>& samples_gt, const std::string& filename)
{
	std::cout << "start disp actual training" << std::endl;
	std::cout << "samples: " << samples.size() << std::endl;

	data_normalizer<double> normalizer(ml_region_optimizer_disp::vector_size_per_disp*2+ml_region_optimizer_disp::vector_size, 0);
	normalizer.gather(samples);
	normalizer.apply(samples);

	randomize_dataset(samples, samples_gt);

	int dims = samples.front().size();
	network<double> net(dims);

	std::vector<unsigned int> stats(2, 0);
	for(std::size_t i = 0; i < samples_gt.size(); ++i)
		++(stats[std::abs(samples_gt[i])]);
	for(std::size_t i = 0; i < stats.size(); ++i)
		std::cout << "[" << i << "] " << (float)stats[i]/samples_gt.size() << "\n";
	std::cout << std::endl;

	//int nvector = ml_region_optimizer::vector_size_per_disp + ml_region_optimizer::vector_size;
	int nvector = ml_region_optimizer_disp::vector_size_per_disp;
	int pass = ml_region_optimizer_disp::vector_size;

	init_network_disp(net, dims, nvector, pass);
	net.multi_training(samples, samples_gt, 16, 61, 4);

	std::ofstream ostream(filename);
	ostream.precision(17);
	normalizer.write(ostream);

	ostream << net;
	ostream.close();

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

void ml_region_optimizer_disp::training()
{
	training_internal_per_disp(samples_left, samples_gt_left, filename_left_prefix + std::to_string(training_iteration) + "-disp.txt");
	std::cout << "\n\n ------------------------------------------- next side --------------------------------------------" << std::endl;
	training_internal_per_disp(samples_right, samples_gt_right, filename_right_prefix + std::to_string(training_iteration) + "-disp.txt");
}


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

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

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

#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>
#include <boost/simd/function/simd/round2even.hpp>
#include <boost/simd/function/simd/toint.hpp>
#include <boost/simd/function/simd/multiplies.hpp>
#include <boost/simd/function/simd/minus.hpp>
#include <boost/simd/function/simd/bitwise_and.hpp>
#include <boost/simd/constant/three.hpp>
#include <boost/simd/constant/twoopi.hpp>
#include <boost/simd/constant/pio2_1.hpp>
#include <boost/simd/constant/pio2_2.hpp>
#include <boost/simd/constant/pio2_3.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/fusion/include/std_pair.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( rem_pio2_cephes_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::generic_ < bd::floating_<A0> >
                          , bd::generic_ < bd::integer_<A1>  >
                          , bd::generic_ < bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE void operator() ( A0 const& x, A1 & n, A0 & xr)
    {
      n = bs::rem_pio2_cephes(x, xr);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( rem_pio2_cephes_
                          , (typename A0)
                             , bd::cpu_
                            , bd::generic_ < bd::floating_<A0> >
                            )
  {
    using i_t = bd::as_integer_t<A0>;
    using result_t = std::pair<i_t, A0>              ;

    BOOST_FORCEINLINE result_t operator() ( A0 const& a0) const
    {
      A0 second;
      i_t const first = bs::rem_pio2_cephes(a0,second);
      return {first, second};
    }
  };

  BOOST_DISPATCH_OVERLOAD ( rem_pio2_cephes_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_< bd::floating_<A0> >
                          , bd::generic_< bd::floating_<A0> >
                          )
  {
    using i_t = bd::as_integer_t<A0>;
    BOOST_FORCEINLINE i_t operator() ( A0 const& x, A0 & xr) const
    {
      A0 xi =  bs::round2even(x*bs::Twoopi<A0>());
//       xr -= xi*bs::Pio2_2<A0>();//TODO
//       xr -= xi*bs::Pio2_3<A0>();
      xr  = x-xi*bs::Pio2_1<A0>();
      xr = xr-xi*bs::Pio2_2<A0>();
      xr = xr-xi*bs::Pio2_3<A0>();
      return bitwise_and(bs::toint(xi), Three<i_t>());
    }
  };
} } }

#endif