#include "cerealHelper.hpp"
#include <Eigen/Dense>
#include <boost/program_options.hpp>
#include <iostream>
#include <opencv2/highgui/highgui.hpp>
#include <pcl/common/common.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
#include <thread>

namespace po = boost::program_options;

struct DataCollection
{
    Eigen::Affine3d affine;
    cv::Mat image;
    pcl::PointCloud<pcl::PointXYZI> cloud;

    template<class Archive>
    void serialize(Archive& archive)
    {
        archive(affine, image, cloud);
    }
};

void colorizeCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud)
{
    // pack r/g/b into rgb
    std::uint8_t r = 0, g = 255, b = 0;
    std::uint32_t rgb = ((std::uint32_t)r << 16 | (std::uint32_t)g << 8 | (std::uint32_t)b);
    for (auto& p : *cloud) {
        p.rgb = *reinterpret_cast<float*>(&rgb);
    }
}

int main(int argc, char** argv)
{
    // Program options
    po::options_description desc("Create different data and send to cereal binary archive\n\n"
                                 "Options");
    std::string pcdpath, imgpath;
    desc.add_options()("help,h", "Print this help")(
        "pcdpath,p", po::value<std::string>(&pcdpath)->required(), "Set the input path for the .pcd")(
        "imgpath,i", po::value<std::string>(&imgpath)->required(), "Set the input path for the image");
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    DataCollection dataCollection;

    // --- Processing Normal (Numpy) Matrix

    Eigen::Matrix3d rotMat;
    dataCollection.affine = Eigen::Affine3d::Identity();
    rotMat = Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ()) *
             Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitY()) *
             Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitX());
    dataCollection.affine.translate(Eigen::Vector3d(20, 30, 40));
    dataCollection.affine.rotate(rotMat);

    std::cout << "htMat: " << std::endl << dataCollection.affine.matrix() << std::endl;

    // ---

    // --- Image (OpenCV)

    std::cout << "opening image: " << imgpath << std::endl;
    dataCollection.image = cv::imread(imgpath);
    cv::imshow("image", dataCollection.image);
    cv::waitKey(0);

    // ---

    // --- PointCloud (PCL)

    std::cout << "opening pcd: " << pcdpath << std::endl;
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    if (pcl::io::loadPCDFile<pcl::PointXYZRGB>(pcdpath, *cloud) == -1) {
        throw std::runtime_error("cannot load pcd");
    }
    colorizeCloud(cloud);
    pcl::copyPointCloud(*cloud, dataCollection.cloud);

    pcl::visualization::CloudViewer viewer("Cloud Viewer");
    viewer.showCloud(cloud);
    while (!viewer.wasStopped()) {
    }

    // ---

    // Write output

    std::ofstream outfile("cereal.bin", std::ifstream::out | std::ios::binary);
    cereal::BinaryOutputArchive outputArchive(outfile);
    outputArchive(CEREAL_NVP(dataCollection));

    return 0;
}


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

// Copyright (c) 2010-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_TEST_BUFFER_HPP
#define BOOST_GEOMETRY_TEST_BUFFER_HPP

#define BOOST_GEOMETRY_DEBUG_WITH_MAPPER
#define TEST_WITH_SVG

#include <fstream>
#include <iomanip>

#include <boost/foreach.hpp>
#include <geometry_test_common.hpp>

#include <boost/geometry/algorithms/envelope.hpp>
#include <boost/geometry/algorithms/area.hpp>
#include <boost/geometry/algorithms/buffer.hpp>
#include <boost/geometry/algorithms/centroid.hpp>
#include <boost/geometry/algorithms/union.hpp>

#include <boost/geometry/algorithms/detail/overlay/debug_turn_info.hpp>

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

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

#include <boost/geometry/algorithms/disjoint.hpp>
#include <boost/geometry/algorithms/intersects.hpp>

#include <boost/geometry/extensions/algorithms/buffer/buffer_inserter.hpp>

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

#include <boost/geometry/io/wkt/wkt.hpp>

#if defined(TEST_WITH_SVG)
#  include <boost/geometry/extensions/io/svg/svg_mapper.hpp>
#endif

#include <boost/geometry/algorithms/detail/overlay/self_turn_points.hpp>
template <typename Geometry, typename Mapper>
void post_map(Geometry const& geometry, Mapper& mapper)
{
    typedef bg::detail::overlay::turn_info
    <
        typename bg::point_type<Geometry>::type
    > turn_info;

    std::vector<turn_info> turns;

    bg::detail::self_get_turn_points::no_interrupt_policy policy;
    bg::self_turns
        <
            bg::detail::overlay::assign_null_policy
        >(geometry, turns, policy);

    BOOST_FOREACH(turn_info const& turn, turns)
    {
        mapper.map(turn.point, "fill:rgb(255,128,0);stroke:rgb(0,0,100);stroke-width:1", 3);
    }
}

template
<
    typename GeometryOut,
    template<typename, typename> class JoinStrategy,
    typename Geometry
>
void test_buffer(std::string const& caseid, Geometry const& geometry,
            char join,
            bool check, double expected_area,
            double distance_left, double distance_right, int expected_self_tangencies)
{
    namespace bg = boost::geometry;

    typedef typename bg::coordinate_type<Geometry>::type coordinate_type;
    typedef typename bg::point_type<Geometry>::type point_type;
    typedef bg::strategy::buffer::distance_assymetric<coordinate_type> distance;

    typedef typename bg::ring_type<GeometryOut>::type ring_type;

	typedef typename bg::tag<Geometry>::type tag;
	std::string type = boost::is_same<tag, bg::polygon_tag>::value ? "poly"
		: boost::is_same<tag, bg::linestring_tag>::value ? "line"
		: boost::is_same<tag, bg::multi_polygon_tag>::value ? "multipoly"
		: boost::is_same<tag, bg::multi_linestring_tag>::value ? "multiline"
		: ""
		;

    std::ostringstream complete;
    complete
        << type << "_"
        << caseid << "_"
        << string_from_type<coordinate_type>::name()
        << "_" << join;

    std::cout << complete.str() << std::endl;

    std::ostringstream filename;
    filename << "buffer_" << complete.str() << ".svg";

    std::ofstream svg(filename.str().c_str());

    bg::svg_mapper<point_type> mapper(svg, 1000, 1000);

    {
        bg::model::box<point_type> box;
        bg::envelope(geometry, box);
        double d = std::abs(distance_left);
		if (distance_right > -998)
		{
			d += std::abs(distance_right);
		}
        else
        {
            distance_right = distance_left;
        }

        bg::buffer(box, box, d * (join == 'm' ? 2.0 : 1.1));
        mapper.add(box);
    }

    typedef JoinStrategy
        <
            point_type,
            typename bg::point_type<GeometryOut>::type
        > join_strategy_type;

    join_strategy_type join_strategy;

    typedef bg::strategy::buffer::distance_assymetric<coordinate_type> distance_strategy_type;
    distance_strategy_type distance_strategy(distance_left, distance_right);

    std::vector<GeometryOut> buffered;

    bg::buffer_inserter<GeometryOut>(geometry, std::back_inserter(buffered),
                        distance_strategy,
                        join_strategy
#ifdef BOOST_GEOMETRY_DEBUG_WITH_MAPPER
                        , mapper
#endif
                                );

    typename bg::default_area_result<GeometryOut>::type area = 0;
    BOOST_FOREACH(GeometryOut const& polygon, buffered)
    {
        area += bg::area(polygon);
    }

    //std::cout << caseid << " " << distance_left << std::endl;
    //std::cout << "INPUT: " << bg::wkt(geometry) << std::endl;
    //std::cout << "OUTPUT: " << area << std::endl;
    //BOOST_FOREACH(GeometryOut const& polygon, buffered)
    //{
    //    std::cout << bg::wkt(polygon) << std::endl;
    //}

    if (expected_area > -0.1)
    {
		typename bg::default_area_result<GeometryOut>::type tolerance = 0.01;
		if (join == 'r')
		{
			tolerance = 0.1;
		}

		if (expected_area < 1.0e-5)
		{
			tolerance = 1.0e-10;
		}

        BOOST_CHECK_MESSAGE
            (
                bg::math::abs(area - expected_area) < tolerance,
                complete.str() << " not as expected. "
                << " Expected: "  << expected_area
                << " Detected: "  << area
            );

        // Be sure resulting polygon does not contain
        // self-intersections
        // But indentation5 should contain 1 self-ip TODO give this check as an argument
        if (expected_self_tangencies == 0
            && ! boost::contains(complete.str(), "indentation5_d_r")
            && ! boost::contains(complete.str(), "flower25_d_r"))
        {
            BOOST_FOREACH(GeometryOut const& polygon, buffered)
            {
                BOOST_CHECK_MESSAGE
                    (
                        ! bg::intersects(polygon),
                        complete.str() << " is self-intersecting. "
                    );
            }
        }
    }

    // Map input geometry in green
    mapper.map(geometry, "opacity:0.5;fill:rgb(0,128,0);stroke:rgb(0,128,0);stroke-width:10");

    BOOST_FOREACH(GeometryOut const& polygon, buffered)
    {
        mapper.map(polygon, "opacity:0.4;fill:rgb(255,255,128);stroke:rgb(0,0,0);stroke-width:3");
        //mapper.map(polygon, "opacity:0.2;fill:none;stroke:rgb(255,0,0);stroke-width:3");
        post_map(polygon, mapper);
    }
}

#ifdef BOOST_GEOMETRY_CHECK_WITH_POSTGIS
static int counter = 0;
#endif

template
<
    typename Geometry,
    template<typename, typename> class JoinStrategy,
    typename GeometryOut
>
void test_one(std::string const& caseid, std::string const& wkt,
        char join, double expected_area,
        double distance_left, double distance_right = -999, int expected_self_tangencies = 0)
{
    namespace bg = boost::geometry;
    Geometry g;
    bg::read_wkt(wkt, g);

    typedef typename bg::point_type<Geometry>::type point_type;

#ifdef BOOST_GEOMETRY_CHECK_WITH_POSTGIS
    std::cout
        << (counter > 0 ? "union " : "")
        << "select " << counter++
        << ", '" << caseid << "' as caseid"
        << ", ST_Area(ST_Buffer(ST_GeomFromText('" << wkt << "'), "
        << distance_left
        << ", 'endcap=flat join=" << (join == 'm' ? "miter" : "round") << "'))"
        << ", "  << expected_area
        << std::endl;
#endif

    test_buffer<GeometryOut, JoinStrategy>
            (caseid, g, join, false, expected_area,
            distance_left, distance_right, expected_self_tangencies);
}

#endif


#include <fstream>
#include <sstream>
#include <iostream>
#include <fmt/format.h>
#include <numeric>
#include <algorithm>
#include <set>
#include <chrono>
#include <map>
#include <range/v3/all.hpp>

#include <fmt/format.h>
#include <fmt/chrono.h>
#include <boost/icl/closed_interval.hpp>
#include <boost/icl/interval_set.hpp>
#include <boost/icl/interval_map.hpp>
#include <date/tz.h>
namespace icl = boost::icl;
namespace views = ranges::views;
class Guards
{
  std::map<int, icl::interval_map<int, int>> guard_timevalset_;
  std::map<int, int> sleepTime_;

public:
  int count() const
  {
    int max = 0;
    int tvf = 0;
    int key;
    std::pair<int, int> sumcnt(0, 0);
    for (auto [k, v] : sleepTime_) {
      if (v > max) {
        max = v;
        key = k;
      }
    }
    max = 0;
    if (auto it = guard_timevalset_.find(key); it != guard_timevalset_.end()) {
      for (auto &&[k, v] : it->second) {
        if (v > max) {
          max = v;
          tvf = k.lower();
        }
      }
    }
    return key * tvf;
  }
  friend std::istream &operator>>(std::istream &ifs, Guards &g)
  {
    std::string s;
    int curguardid = 0;
    int falls = 0;
    int wakesup = 0;
    while (std::getline(ifs, s)) {

      using namespace std;
      std::istringstream iss(s);
      {
        std::chrono::sys_time<std::chrono::minutes> t;
        iss >> date::parse("%F %H:%M", t);
        int n = t.time_since_epoch().count();
        std::string guard;
        iss >> guard;
        if (guard == "Guard") {
          iss >> curguardid;
        } else if (guard == "falls") {
          falls = t.time_since_epoch().count();
        } else {
          wakesup = t.time_since_epoch().count();
          icl::interval<int>::type p(falls, wakesup);
          g.sleepTime_[curguardid] += (wakesup - falls);
          if (auto i = g.guard_timevalset_.find(curguardid); i == g.guard_timevalset_.end()) {
            g.guard_timevalset_.insert(std::pair{ curguardid, std::pair{ p, 1 } });
          } else {
            i->second += std::pair{ p, 1 };
          }
        }
      };
    }
    return ifs;
  }
};

int main(int argc, char **argv)
{
  if (argc > 1) {
    std::ifstream ifs(argv[1]);
    Guards g;
    ifs >> g;
    std::cout << g.count() << std::endl;
  }
}


//
// Created by wei on 5/22/18.
//

#include "common/ConfigParser.h"
#include "core/warp_solver/RigidSolver.h"

#include <Eigen/Eigen>

surfelwarp::RigidSolver::RigidSolver() {
	//Init the intrisic for projection
	const auto& config = ConfigParser::Instance();
	m_project_intrinsic = config.rgb_intrinsic_clip();
	m_image_rows = config.clip_image_rows();
	m_image_cols = config.clip_image_cols();

	//Init the world2camera
	m_curr_world2camera = mat34::identity();

	//Allocate the buffer
	allocateReduceBuffer();
}

surfelwarp::RigidSolver::~RigidSolver() {

}

void surfelwarp::RigidSolver::SetInputMaps(
	const surfelwarp::Renderer::SolverMaps &solver_maps,
	const surfelwarp::CameraObservation &observation,
	const mat34& init_world2camera
) {
	m_solver_maps.live_vertex_map = solver_maps.warp_vertex_map;
	m_solver_maps.live_normal_map = solver_maps.warp_normal_map;

	m_observation.vertex_map = observation.vertex_config_map;
	m_observation.normal_map = observation.normal_radius_map;

	m_curr_world2camera = init_world2camera;
}

surfelwarp::mat34 surfelwarp::RigidSolver::Solve(int max_iters, cudaStream_t stream) {
	//The solver iteration
	for(int i = 0; i < max_iters; i++) {
		rigidSolveDeviceIteration(stream);
		rigidSolveHostIterationSync(stream);
	}

	//The updated world2camera
	return m_curr_world2camera;
}

void surfelwarp::RigidSolver::rigidSolveHostIterationSync(cudaStream_t stream) {
	//Sync before using the data
	cudaSafeCall(cudaStreamSynchronize(stream));

	//Load the hsot array
	const auto& host_array = m_reduced_matrix_vector.HostArray();

	//Load the data into Eigen
	auto shift = 0;
#pragma unroll
	for (int i = 0; i < 6; i++) {
		for (int j = i; j < 6; j++) {
			const float value = host_array[shift++];
			JtJ_(i, j) = value;
			JtJ_(j, i) = value;
		}
	}
	for (int i = 0; i < 6; i++) {
		const float value = host_array[shift++];
		JtErr_[i] = value;
	}

	//Solve it
	Eigen::Matrix<float, 6, 1> x = JtJ_.llt().solve(JtErr_).cast<float>();

	//Update the se3
	const float3 twist_rot = make_float3(x(0), x(1), x(2));
	const float3 twist_trans = make_float3(x(3), x(4), x(5));
	const mat34 se3_update(twist_rot, twist_trans);
	m_curr_world2camera = se3_update * m_curr_world2camera;
}


/*
@copyright Louis Dionne 2014
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/hana/integral.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/group/laws.hpp>
#include <boost/hana/list/instance.hpp>
using namespace boost::hana;

int main() {
    BOOST_HANA_CONSTANT_ASSERT(negate(int_<1>) == int_<-1>);
    BOOST_HANA_CONSTANT_ASSERT(minus(int_<1>, int_<3>) == int_<-2>);
    BOOST_HANA_CONSTEXPR_ASSERT(minus(int_<1>, 3) == -2);
    BOOST_HANA_CONSTEXPR_ASSERT(minus(1, int_<3>) == -2);

    BOOST_HANA_CONSTEXPR_ASSERT(Group::laws::check(list(
        int_<1>, short_<2>, long_<3>, ullong<4>, 5, 6ull
    )));
}


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

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::is_included_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<arithmetic_<A0>,X>))
                              ((simd_<arithmetic_<A0>,X>))
                            )
  {
    typedef typename meta::scalar_of<A0>::type sA0;
    typedef typename meta::as_logical<sA0>::type result_type;
    inline result_type operator()(A0 const& a0,A0 const& a1) const
    {
      return simd::all(eq(b_or(a0, a1), a1));
    }
  };

} } }
#endif


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

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

// This file was modified by Oracle on 2015.
// Modifications copyright (c) 2015, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, 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)

#ifndef BOOST_GEOMETRY_STRATEGIES_CARTESIAN_BUFFER_END_ROUND_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_BUFFER_END_ROUND_HPP

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

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

#include <boost/geometry/io/wkt/wkt.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace buffer
{

/*!
\brief Let the buffer create rounded ends
\ingroup strategies
\details This strategy can be used as EndStrategy for the buffer algorithm.
    It creates a rounded end for each linestring-end. It can be applied
    for (multi)linestrings. Also it is applicable for spikes in (multi)polygons.
    This strategy is only applicable for Cartesian coordinate systems.

\qbk{
[heading Example]
[buffer_end_round]
[heading Output]
[$img/strategies/buffer_end_round.png]
[heading See also]
\* [link geometry.reference.algorithms.buffer.buffer_7_with_strategies buffer (with strategies)]
\* [link geometry.reference.strategies.strategy_buffer_end_flat end_flat]
}
 */
class end_round
{
private :
    std::size_t m_points_per_circle;

    template
    <
        typename Point,
        typename PromotedType,
        typename DistanceType,
        typename RangeOut
    >
    inline void generate_points(Point const& point,
                PromotedType alpha, // by value
                DistanceType const& buffer_distance,
                RangeOut& range_out) const
    {
        PromotedType const two_pi = geometry::math::two_pi<PromotedType>();

        std::size_t point_buffer_count = m_points_per_circle;

        PromotedType const diff = two_pi / PromotedType(point_buffer_count);

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

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

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

public :

    //! \brief Constructs the strategy
    //! \param points_per_circle points which would be used for a full circle
    //! (if points_per_circle is smaller than 4, it is internally set to 4)
    explicit inline end_round(std::size_t points_per_circle = 90)
        : m_points_per_circle((points_per_circle < 4u) ? 4u : points_per_circle)
    {}

#ifndef DOXYGEN_SHOULD_SKIP_THIS

    //! Fills output_range with a flat end
    template <typename Point, typename RangeOut, typename DistanceStrategy>
    inline void apply(Point const& penultimate_point,
                Point const& perp_left_point,
                Point const& ultimate_point,
                Point const& perp_right_point,
                buffer_side_selector side,
                DistanceStrategy const& distance,
                RangeOut& range_out) const
    {
        typedef typename coordinate_type<Point>::type coordinate_type;

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

        promoted_type const alpha = calculate_angle<promoted_type>(perp_left_point, ultimate_point);

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

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

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

        if (m_points_per_circle % 2 == 1)
        {
            // For a half circle, if the number of points is not even,
            // we should insert the end point too, to generate a full cap
            range_out.push_back(perp_right_point);
        }
    }

    template <typename NumericType>
    static inline NumericType max_distance(NumericType const& distance)
    {
        return distance;
    }

    //! Returns the piece_type (flat end)
    static inline piece_type get_piece_type()
    {
        return buffered_round_end;
    }
#endif // DOXYGEN_SHOULD_SKIP_THIS
};

}} // namespace strategy::buffer

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_BUFFER_END_ROUND_HPP


#ifndef TOPOLOGICAL_ORDER_HPP__
#define TOPOLOGICAL_ORDER_HPP__

#include <boost/graph/topological_sort.hpp>
#include <crab/analysis/graphs/sccg_bgl.hpp>

/*
   Topological order of a graph
 */

namespace crab {
  namespace analyzer {
   namespace graph_algo {

     // res contains the reverse topological order of a graph g
     // pre: g is a DAG.
     template <typename G>
     void rev_topo_sort (const G &g, std::vector<typename G::node_t>& res) {

       typedef boost::unordered_map<typename G::node_t,boost::default_color_type> color_map_t;
       typedef boost::associative_property_map<color_map_t> property_color_map_t;

       color_map_t colormap;

       for (auto const &v: boost::make_iterator_range (vertices (g)))
         colormap [v] = boost::default_color_type();

       res.reserve(num_vertices (g));

       boost::topological_sort(g,
                               std::back_inserter(res),
                               color_map(property_color_map_t(colormap)));
     }

     // res contains the topological order of g.
     // pre: g is a DAG.
     template <typename G>
     void topo_sort (const G &g, std::vector<typename G::node_t>& res) {
       rev_topo_sort(g, res);
       std::reverse(res.begin(), res.end());
     }

     // return the reversed topological order of g with possibly cycles.
     // XXX: related concept but this is not Bourdoncle's WTO
     template<class G>
     std::vector<typename G::node_t> weak_rev_topo_sort(G g) {
       std::vector<typename G::node_t> sccg_order;
       crab::analyzer::graph_algo::scc_graph <G> scc_g (g, false /*postorder within scc*/);
       crab::analyzer::graph_algo::rev_topo_sort (scc_g, sccg_order);
       std::vector<typename G::node_t> order;
       for (auto &n : sccg_order) {
         auto &members = scc_g.get_component_members (n);
         order.insert (order.end (), members.begin (), members.end ());
       }
       return order;
     }

     // return the topological order of g with possibly cycles.
     // XXX: related concept but this is not Bourdoncle's WTO
     template<class G>
     std::vector<typename G::node_t> weak_topo_sort(G g) {
       std::vector<typename G::node_t> sccg_order;
       crab::analyzer::graph_algo::scc_graph <G> scc_g (g, true /*preorder within scc*/);
       crab::analyzer::graph_algo::topo_sort (scc_g, sccg_order);
       std::vector<typename G::node_t> order;
       for (auto &n : sccg_order) {
         auto &members = scc_g.get_component_members (n);
         order.insert (order.end (), members.begin (), members.end ());
       }
       return order;
     }

   } // end namespace graph_algo
  } // end namespace analyzer
} // end namespace crab

#endif


//==================================================================================================
/*!
  @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_DETAIL_CONSTANT_SPLITFACTOR_HPP_INCLUDED
#define BOOST_SIMD_DETAIL_CONSTANT_SPLITFACTOR_HPP_INCLUDED

#include <boost/simd/config.hpp>
#include <boost/simd/detail/brigand.hpp>
#include <boost/simd/detail/dispatch.hpp>
#include <boost/simd/detail/constant_traits.hpp>
#include <boost/simd/detail/dispatch/function/make_callable.hpp>
#include <boost/simd/detail/dispatch/hierarchy/functions.hpp>
#include <boost/simd/detail/dispatch/as.hpp>

/*

    @ingroup group-constant

    Generate  a constant used to split a floating number in two parts,
    in floating point routines (such @ref two_add and
    @ref two_prod) to get extra precision.

    @par Semantic:

    @code
    T r = Splitfactor<T>();
    @endcode

    is similar to:

    @code
    if T is integral
      r = 0
    else if T is double
      r =  pow(2, 27);
    else if T is float
      r =  pow(2, 13);
    @endcode

    @return The Splitfactor constant for the proper type
  */

namespace boost { namespace simd
{
  namespace tag
  {
    struct splitfactor_ : boost::dispatch::constant_value_<splitfactor_>
    {
      BOOST_DISPATCH_MAKE_CALLABLE(ext,splitfactor_,boost::dispatch::constant_value_<splitfactor_>);
      BOOST_SIMD_REGISTER_CONSTANT(0, 0x46000000UL, 0x41A0000000000000ULL);
    };
  }

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag, splitfactor_)
  }

  namespace detail
  {
    BOOST_DISPATCH_CALLABLE_DEFINITION(tag::splitfactor_,splitfactor);
  }

  template<typename T> BOOST_FORCEINLINE auto Splitfactor()
  BOOST_NOEXCEPT_DECLTYPE(detail::splitfactor( boost::dispatch::as_<T>{}))
  {
    return detail::splitfactor( boost::dispatch::as_<T>{} );
  }
} }

#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


// -----------------------------------------------------------------------------
// Fern © Geoneric
//
// This file is part of Geoneric Fern which is available under the terms of
// the GNU General Public License (GPL), version 2. If you do not want to
// be bound by the terms of the GPL, you may purchase a proprietary license
// from Geoneric (http://www.geoneric.eu/contact).
// -----------------------------------------------------------------------------
#define BOOST_TEST_MODULE fern algorithm algebra elementary divide
#include <boost/test/unit_test.hpp>
#include "fern/core/data_customization_point/scalar.h"
#include "fern/core/type_traits.h"
#include "fern/core/types.h"
#include "fern/algorithm/core/test/test_utils.h"
#include "fern/algorithm/algebra/elementary/divide.h"

namespace fa = fern::algorithm;

template<
    class Value1,
    class Value2>
using OutOfDomainPolicy = fa::divide::OutOfDomainPolicy<Value1, Value2>;

BOOST_AUTO_TEST_CASE(out_of_domain_policy)
{
    {
        OutOfDomainPolicy<int32_t, int32_t> policy;
        BOOST_CHECK(policy.within_domain(5, 6));
        BOOST_CHECK(policy.within_domain(-5, -6));
        BOOST_CHECK(!policy.within_domain(0, 0));
        BOOST_CHECK(!policy.within_domain(5, 0));
        BOOST_CHECK(policy.within_domain(0, 6));
    }
}

template<
    class Value1,
    class Value2,
    class Result>
using OutOfRangePolicy = fa::divide::OutOfRangePolicy<Value1, Value2,
    Result>;

template<
    class Value1,
    class Value2,
    class Result>
struct VerifyWithinRange
{
    bool operator()(
        Value1 const& value1,
        Value2 const& value2)
    {
        fa::SequentialExecutionPolicy sequential;

        OutOfRangePolicy<Value1, Value2, Result> policy;
        Result result;

        fa::algebra::divide(sequential, value1, value2, result);

        return policy.within_range(value1, value2, result);
    }
};

BOOST_AUTO_TEST_CASE(out_of_range_policy)
{
    // signed int / signed int
    {
        auto max_int32 = fern::max<int32_t>();
        auto min_int32 = fern::min<int32_t>();

        {
            VerifyWithinRange<int32_t, int32_t, int32_t> verify;
            BOOST_CHECK_EQUAL(verify(0, 5), true);
            BOOST_CHECK_EQUAL(verify(4, 5), true);
            BOOST_CHECK_EQUAL(verify(-4, 5), true);
            BOOST_CHECK_EQUAL(verify(-4, -5), true);

            // Both values positive.
            BOOST_CHECK_EQUAL(verify(0, max_int32), true);
            BOOST_CHECK_EQUAL(verify(1, max_int32), true);
            BOOST_CHECK_EQUAL(verify(max_int32, max_int32), true);

            // One value positive, one value negative.
            BOOST_CHECK_EQUAL(verify(0, min_int32), true);
            BOOST_CHECK_EQUAL(verify(1, min_int32), true);
            BOOST_CHECK_EQUAL(verify(max_int32, min_int32), true);
            BOOST_CHECK_EQUAL(verify(-1, max_int32), true);
            BOOST_CHECK_EQUAL(verify(min_int32, max_int32), true);

            // Both values negative.
            BOOST_CHECK_EQUAL(verify(-1, min_int32), true);
            BOOST_CHECK_EQUAL(verify(min_int32, min_int32), true);
            BOOST_CHECK_EQUAL(verify(-1, min_int32), true);
            BOOST_CHECK_EQUAL(verify(min_int32, min_int32), true);
        }
    }

    // unsigned int / unsigned int
    {
        auto max_uint32 = fern::max<uint32_t>();

        {
            VerifyWithinRange<uint32_t, uint32_t, uint32_t> verify;
            BOOST_CHECK_EQUAL(verify(0, 5), true);
            BOOST_CHECK_EQUAL(verify(4, 5), true);
            BOOST_CHECK_EQUAL(verify(1, max_uint32), true);
            BOOST_CHECK_EQUAL(verify(max_uint32, max_uint32), true);
        }
    }

    // float / float
    {
        auto max_float32 = fern::max<fern::f32>();
        auto min_float32 = fern::min<fern::f32>();

        {
            VerifyWithinRange<fern::f32, fern::f32, fern::f32> verify;

            BOOST_CHECK_EQUAL(verify(0.0, 5.0), true);
            BOOST_CHECK_EQUAL(verify(4.0, 5.0), true);
            BOOST_CHECK_EQUAL(verify(-4.0, 5.0), true);
            BOOST_CHECK_EQUAL(verify(4.0, -5.0), true);
            BOOST_CHECK_EQUAL(verify(-4.0, -5.0), true);
            BOOST_CHECK_EQUAL(verify(max_float32, max_float32), true);
            BOOST_CHECK_EQUAL(verify(min_float32, min_float32), true);
            BOOST_CHECK_EQUAL(verify(min_float32, max_float32), true);
            BOOST_CHECK_EQUAL(verify(max_float32, min_float32), false);
        }
    }

    // unsigned int / signed int
    {
        auto max_uint32 = fern::max<uint32_t>();
        auto max_int32 = fern::max<int32_t>();

        {
            VerifyWithinRange<uint32_t, int32_t, int64_t> verify;

            BOOST_CHECK_EQUAL(verify(0, 5), true);
            BOOST_CHECK_EQUAL(verify(4, 5), true);
            BOOST_CHECK_EQUAL(verify(4, -5), true);
            BOOST_CHECK_EQUAL(verify(1, max_int32), true);
            BOOST_CHECK_EQUAL(verify(max_uint32, max_int32), true);
            BOOST_CHECK_EQUAL(verify(max_uint32, 1), true);
        }
    }

    // unsigned int / float
    {
        auto min_uint32 = fern::min<uint32_t>();
        auto max_uint32 = fern::max<uint32_t>();
        auto min_float32 = fern::min<fern::f32>();
        auto max_float32 = fern::max<fern::f32>();

        {
            VerifyWithinRange<uint32_t, fern::f32, fern::f32> verify;

            BOOST_CHECK_EQUAL(verify(0, 5.0), true);
            BOOST_CHECK_EQUAL(verify(4, 5.0), true);
            BOOST_CHECK_EQUAL(verify(0, min_float32), true);
            BOOST_CHECK_EQUAL(verify(1, min_float32), true);
            BOOST_CHECK_EQUAL(verify(0, max_float32), true);
            BOOST_CHECK_EQUAL(verify(1, max_float32), true);
            BOOST_CHECK_EQUAL(verify(min_uint32, min_float32), true);
            BOOST_CHECK_EQUAL(verify(min_uint32, max_float32), true);
            BOOST_CHECK_EQUAL(verify(max_uint32, min_float32), false);
            BOOST_CHECK_EQUAL(verify(max_uint32, max_float32), true);
        }
    }

    // int / float
    {
        auto min_int32 = fern::min<int32_t>();
        auto max_int32 = fern::max<int32_t>();
        auto min_float32 = fern::min<fern::f32>();
        auto max_float32 = fern::max<fern::f32>();

        {
            VerifyWithinRange<int32_t, fern::f32, fern::f32> verify;

            BOOST_CHECK_EQUAL(verify(0, 5.0), true);
            BOOST_CHECK_EQUAL(verify(4, 5.0), true);
            BOOST_CHECK_EQUAL(verify(0, min_float32), true);
            BOOST_CHECK_EQUAL(verify(1, min_float32), true);
            BOOST_CHECK_EQUAL(verify(0, max_float32), true);
            BOOST_CHECK_EQUAL(verify(1, max_float32), true);
            BOOST_CHECK_EQUAL(verify(min_int32, min_float32), false);
            BOOST_CHECK_EQUAL(verify(min_int32, max_float32), true);
            BOOST_CHECK_EQUAL(verify(max_int32, min_float32), false);
            BOOST_CHECK_EQUAL(verify(max_int32, max_float32), true);
        }
    }

    // float / int
    {
        auto min_int32 = fern::min<int32_t>();
        auto max_int32 = fern::max<int32_t>();
        auto min_float32 = fern::min<fern::f32>();
        auto max_float32 = fern::max<fern::f32>();

        {
            VerifyWithinRange<fern::f32, int32_t, fern::f32> verify;

            BOOST_CHECK_EQUAL(verify(0.0, 5), true);
            BOOST_CHECK_EQUAL(verify(4.0, 5), true);
            BOOST_CHECK_EQUAL(verify(0.0, min_int32), true);
            BOOST_CHECK_EQUAL(verify(1.0, min_int32), true);
            BOOST_CHECK_EQUAL(verify(0.0, max_int32), true);
            BOOST_CHECK_EQUAL(verify(1.0, max_int32), true);
            BOOST_CHECK_EQUAL(verify(min_float32, min_int32), true);
            BOOST_CHECK_EQUAL(verify(min_float32, max_int32), true);
            BOOST_CHECK_EQUAL(verify(max_float32, min_int32), true);
            BOOST_CHECK_EQUAL(verify(max_float32, max_int32), true);
        }
    }
}

template<
    class Value1,
    class Value2,
    class Result>
void verify_value(
    Value1 const& value1,
    Value2 const& value2,
    Result const& result_we_want)
{
    fa::SequentialExecutionPolicy sequential;

    Result result_we_get;
    fa::algebra::divide(sequential, value1, value2, result_we_get);
    BOOST_CHECK_EQUAL(result_we_get, result_we_want);
}

BOOST_AUTO_TEST_CASE(algorithm)
{
    verify_value<int8_t, int8_t, int8_t>(-30, -5, 6);
    verify_value<int8_t, int8_t, int8_t>(30, -5, -6);
    verify_value<uint8_t, uint8_t, uint8_t>(30, 6, 5);
    verify_value<double, float, double>(30.0, 6.0f, 5.0);
}


#include <gtest/gtest.h>

#include <iostream>

#include <ocs2_core/misc/LinearInterpolation.h>
#include <Eigen/Dense>

TEST(testLinearInterpolation, testInterpolation) {
  using Data_T = Eigen::Matrix<double, 2, 1>;

  // Create data
  std::vector<double> t = {0.0, 1.0, 2.0, 3.0, 3.0, 4.0};
  std::vector<Data_T, Eigen::aligned_allocator<Data_T>> v;
  for (auto& t_k : t) {
    v.emplace_back(t_k * Data_T::Ones());
  }

  std::cout << "TEST DATA" << std::endl;
  for (int k = 0; k < t.size(); k++) {
    std::cout << "time: " << t[k] << " v: " << v[k].transpose() << std::endl;
  }

  // Test function
  auto test_interpolation = [&](double time, int index, double value) {
    const auto indexAlpha = ocs2::LinearInterpolation::timeSegment(time, t);
    const auto v_t = ocs2::LinearInterpolation::interpolate(indexAlpha, v);
    const auto foundIndex = indexAlpha.first;
    const auto alpha = indexAlpha.second;
    std::cout << "time: " << time << " index: " << foundIndex << " v: " << v_t.transpose() << " alpha: " << alpha << std::endl;
    EXPECT_EQ(foundIndex, index);
    EXPECT_DOUBLE_EQ(v_t(0), value);
    ASSERT_TRUE(std::isfinite(alpha));
    ASSERT_GE(alpha, 0.0);
    ASSERT_LE(alpha, 1.0);
  };

  // Before start
  test_interpolation(-1.0, 0, 0.0);
  // At start
  test_interpolation(t[0], 0, t[0]);
  // First interval - off center
  test_interpolation(0.25 * t[0] + 0.75 * t[1], 0, 0.25 * t[0] + 0.75 * t[1]);
  // First interval
  test_interpolation(0.5 * t[0] + 0.5 * t[1], 0, 0.5 * t[0] + 0.5 * t[1]);
  // Boundary to second interval
  test_interpolation(t[1], 0, t[1]);
  // Event time
  test_interpolation(t[3], 2, t[3]);
  // Last interval
  test_interpolation(0.5 * t[4] + 0.5 * t[5], 4, 0.5 * t[4] + 0.5 * t[5]);
  // At End
  test_interpolation(t[5], 4, t[5]);
  // Beyond end
  test_interpolation(t[5] + 1.0, 4, t[5]);
}

TEST(testLinearInterpolation, testEventTimeInterpolation) {
  {  // Only an event
    std::vector<double> time{1.0, 1.0};
    const auto indexAlpha = ocs2::LinearInterpolation::timeSegment(1.0, time);
    ASSERT_EQ(indexAlpha.first, 0);
    ASSERT_TRUE(std::isfinite(indexAlpha.second));
    ASSERT_GE(indexAlpha.second, 0.0);
    ASSERT_LE(indexAlpha.second, 1.0);
  }

  {  // Short time between samples
    constexpr auto eps = ocs2::numeric_traits::weakEpsilon<ocs2::scalar_t>();
    std::vector<double> time{0.0, 1.0, 1.0 + eps, 2.0};
    // Close to 1.0
    const auto indexAlphaLhs = ocs2::LinearInterpolation::timeSegment(1.0 + 0.1 * eps, time);
    ASSERT_EQ(indexAlphaLhs.first, 1);
    ASSERT_EQ(indexAlphaLhs.second, 1.0);
    // Close to 1.0 + eps
    const auto indexAlphaRhs = ocs2::LinearInterpolation::timeSegment(1.0 + 0.9 * eps, time);
    ASSERT_EQ(indexAlphaRhs.first, 1);
    ASSERT_EQ(indexAlphaRhs.second, 0.0);
  }
}

TEST(testLinearInterpolation, testSizeOneTime) {
  using Data_T = Eigen::Matrix<double, 2, 1>;

  // Create data
  std::vector<double> t = {1.0};
  std::vector<Data_T, Eigen::aligned_allocator<Data_T>> v;
  for (auto& t_k : t) {
    v.emplace_back(t_k * Data_T::Ones());
  }

  std::cout << "TEST DATA" << std::endl;
  for (int k = 0; k < t.size(); k++) {
    std::cout << "time: " << t[k] << " v: " << v[k].transpose() << std::endl;
  }

  // Test function
  auto test_interpolation = [&](double time, int index, double value) {
    const auto indexAlpha = ocs2::LinearInterpolation::timeSegment(time, t);
    const auto v_t = ocs2::LinearInterpolation::interpolate(indexAlpha, v);
    const auto foundIndex = indexAlpha.first;
    const auto alpha = indexAlpha.second;
    std::cout << "time: " << time << " index: " << foundIndex << " v: " << v_t.transpose() << " alpha: " << alpha << std::endl;
    EXPECT_EQ(foundIndex, index);
    EXPECT_DOUBLE_EQ(v_t(0), value);
    ASSERT_TRUE(std::isfinite(alpha));
    ASSERT_GE(alpha, 0.0);
    ASSERT_LE(alpha, 1.0);
  };

  // Before time
  test_interpolation(-1.0, 0, 1.0);
  // Beyond time
  test_interpolation(2.0, 0, 1.0);
}

TEST(testLinearInterpolation, testDifferentEigenSizes) {
  using Data_T = Eigen::MatrixXd;
  std::vector<double> times = {0.0, 1.0};
  std::vector<Data_T, Eigen::aligned_allocator<Data_T>> data = {Data_T::Zero(2, 3), Data_T::Ones(4, 4)};

  Data_T result;

  result = ocs2::LinearInterpolation::interpolate(0.4, times, data);
  EXPECT_TRUE(result.isApprox(data[0]));

  result = ocs2::LinearInterpolation::interpolate(0.6, times, data);
  EXPECT_TRUE(result.isApprox(data[1]));

  result = ocs2::LinearInterpolation::interpolate(-0.1, times, data);
  EXPECT_TRUE(result.isApprox(data[0]));

  result = ocs2::LinearInterpolation::interpolate(1.1, times, data);
  EXPECT_TRUE(result.isApprox(data[1]));
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2 euler toolbox - erf/simd Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of euler components in scalar mode
//////////////////////////////////////////////////////////////////////////////
/// created  by jt the 22/02/2011
///
#include <nt2/euler/include/functions/erf.hpp>
#include <boost/simd/sdk/simd/native.hpp>
extern "C" {long double cephes_erfl(long double);}

#include <boost/type_traits/is_same.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/meta/as_floating.hpp>
#include <nt2/sdk/meta/as_signed.hpp>
#include <nt2/sdk/meta/upgrade.hpp>
#include <nt2/sdk/meta/downgrade.hpp>
#include <nt2/sdk/meta/scalar_of.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <boost/type_traits/common_type.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/memory/buffer.hpp>
#include <nt2/constant/constant.hpp>

NT2_TEST_CASE_TPL ( erf_real__1_0,  NT2_SIMD_REAL_TYPES)
{
  using nt2::erf;
  using nt2::tag::erf_;
  using boost::simd::native;
  using nt2::meta::cardinal_of;
  typedef NT2_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef typename nt2::meta::upgrade<T>::type   u_t;
  typedef native<T,ext_t>                        n_t;
  typedef n_t                                     vT;
  typedef typename nt2::meta::as_integer<T>::type iT;
  typedef native<iT,ext_t>                       ivT;
  typedef typename nt2::meta::call<erf_(vT)>::type r_t;
  typedef typename nt2::meta::call<erf_(T)>::type sr_t;
  typedef typename nt2::meta::scalar_of<r_t>::type ssr_t;

  // specific values tests
  NT2_TEST_ULP_EQUAL(erf(nt2::Inf<vT>())[0], nt2::One<sr_t>(), 10);
  NT2_TEST_ULP_EQUAL(erf(nt2::Mzero<vT>())[0], nt2::Zero<sr_t>(), 10);
  NT2_TEST_ULP_EQUAL(erf(nt2::Nan<vT>()[0]), nt2::Nan<sr_t>(), 10);
  NT2_TEST_ULP_EQUAL(erf(nt2::One<vT>())[0], T(0.842700792949715), 10);
  NT2_TEST_ULP_EQUAL(erf(nt2::Half<vT>())[0], T( 0.520499877813047), 10);
  NT2_TEST_ULP_EQUAL(erf(nt2::Two<vT>())[0], T(0.995322265018953), 10);
  NT2_TEST_ULP_EQUAL(erf(nt2::Ten<vT>())[0],  nt2::One <sr_t>(), 10);
  NT2_TEST_ULP_EQUAL(erf(nt2::Zero<vT>())[0], nt2::Zero<sr_t>(), 10);
} // end of test for floating_


#pragma once

// Switching to Blitz Based Arrays
#include <blitz/array.h>
#include <fftw3.h>
#include <iostream>
#include <fstream>
#include <algorithm>
#include <vector>
#include <cmath>
#include <string.h>
#include <complex>
#include <omp.h>

// It should be ok at populate namespace with complex
using std::complex;

namespace NDarray {

using namespace blitz;

// FFT Libraries complex Float
void fftshift( Array<complex<float>,3>& temp);
void fft( Array<complex<float>,3>& temp);
void ifft( Array<complex<float>,3>& temp);

void fft( Array<complex<float>,3>& temp,int);
void ifft( Array<complex<float>,3>& temp,int);
void fft3( Array<complex<float>,3>& temp,int,int);

inline void endian_swap( int& x){
    x = ( x<<24 & 0xFF000000) |
        ( x<<8  & 0x00FF0000) |
        ( x>>8  & 0x0000FF00) |
        ( x>>24 & 0x000000FF);
}

template< typename T, int N>
void ArrayRead( blitz::Array< T,N>& temp, const char *name){
	 	FILE *fid;
		if( (fid=fopen(name,"r")) == NULL){
			cout << "Array:Can't Open " << name << endl;
			cout << "Exiting" << endl;
			exit(1);
		}else{
			int j;
			if( (j=fread(temp.data(),sizeof(T),temp.numElements(),fid)) != (int)(temp.numElements())){
				cout << "Array3:Not enough data: only read " << j << "points of" <<  temp.numElements() << endl;
				exit(1);
			}
		}
}

template< typename T, const int N_rank>
void ArrayWriteMagAppend( Array<complex<T>,N_rank>& temp, const char *name){

	 ofstream ofs(name, ios_base::binary | ios_base::app);
	 for(typename Array<complex<T>,N_rank>::iterator miter=temp.begin(); miter!=temp.end(); miter++){
		T val = abs( *miter);
		ofs.write( (char *)&val,sizeof(T));
     }
}

template< typename T, const int N_rank>
void ArrayWrite( Array< T , N_rank>& temp, const char *name){
	 ofstream ofs(name, ios_base::binary);
	 for(typename Array<T,N_rank>::iterator miter=temp.begin(); miter!=temp.end(); miter++){
			T val= *miter;
			ofs.write( (char *)&val,sizeof(T));
     }
}

template< typename T, const int N_rank>
void ArrayWriteMag( Array<complex<T>, N_rank>& temp, const char *name){
	 ofstream ofs(name, ios_base::binary);
	 for(typename Array<complex<T>,N_rank>::iterator miter=temp.begin(); miter!=temp.end(); miter++){
			T val=abs( *miter);
			ofs.write( (char *)&val,sizeof(T));
     }
}

template< typename T, const int N_rank >
T ArrayEnergy( Array< complex< T >, N_rank >& temp){
	T EE=0;
	for(typename Array<complex<T>,N_rank>::iterator miter=temp.begin(); miter!=temp.end(); miter++){
		EE+= norm( *miter );
	}
    return(EE);
}

template< typename T, const int N_rank, const int M_rank >
T ArrayEnergy( Array< Array< complex< T >,N_rank>, M_rank >& temp){
	T EE=0;
	for(typename Array< Array< complex<T>,N_rank>,M_rank >::iterator miter=temp.begin(); miter!=temp.end(); miter++){
		EE+= ArrayEnergy( *miter );
	}
    return(EE);
}

template< typename T, const int N_rank>
void ArrayWritePhase( Array<complex<T>,N_rank>& temp, const char *name){

	 ofstream ofs(name, ios_base::binary);
	 for(typename Array<complex<T>,N_rank>::iterator miter=temp.begin(); miter!=temp.end(); miter++){
	 	T val= arg( *miter);
		ofs.write( (char *)&val,sizeof(T));
     }
}

template< typename T, const int N_rank>
void ArrayWritePhaseAppend( Array<complex<T>,N_rank>& temp, const char *name){

	 ofstream ofs(name, ios_base::binary  | ios_base::app);
	 for(typename Array<complex<T>,N_rank>::iterator miter=temp.begin(); miter!=temp.end(); miter++){
	 	T val= arg( *miter);
		ofs.write( (char *)&val,sizeof(T));
     }
}

template < typename T >
Array< Array<T,3>, 1> Alloc4DContainer( int x, int y, int z, int t){
	Array< Array<T,3>,1> temp;
	temp.setStorage(ColumnMajorArray<1>());
	temp.resize( t );
	for(int i1=0; i1< t; i1++){
		temp(i1).setStorage(ColumnMajorArray<3>());
		temp(i1).resize(x,y,z);
		temp(i1)= (T )0;
	}
	return(temp);
}

template < typename T >
Array< Array<T,3>, 3> Alloc6DContainer( int x, int y, int z, int d1, int d2, int d3){
	Array< Array<T,3>,3> temp;
	temp.setStorage(ColumnMajorArray<3>());
	temp.resize( d1,d2,d3);

	for( typename Array<Array<T,3>,3>::iterator miter=temp.begin();   miter !=temp.end(); miter++){
		(*miter).setStorage(ColumnMajorArray<3>());
		(*miter).resize(x,y,z);
		(*miter)= (T )0;
	}
	return(temp);
}

template < typename T >
Array< Array<T,3>, 2> Alloc5DContainer( int x, int y, int z, int d1, int d2){
	Array< Array<T,3>,2> temp;
	temp.setStorage(ColumnMajorArray<2>());
	temp.resize( d1,d2);

	for( typename Array< Array<T,3>,2>::iterator miter=temp.begin();   miter !=temp.end(); miter++){
		(*miter).setStorage(ColumnMajorArray<3>());
		(*miter).resize(x,y,z);
		(*miter)= (T )0;
	}
	return(temp);
}

}// Namespace


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2018 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// you should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#ifndef GRAPH_CLUSTERING_HH
#define GRAPH_CLUSTERING_HH

#include "config.h"

#include "hash_map_wrap.hh"
#include <boost/mpl/if.hpp>

#ifdef _OPENMP
#include "omp.h"
#endif

#ifndef __clang__
#include <ext/numeric>
using __gnu_cxx::power;
#else
template <class Value>
Value power(Value value, int n)
{
    return pow(value, n);
}
#endif

namespace graph_tool
{
using namespace boost;
using namespace std;

// calculates the number of triangles to which v belongs
template <class Graph, class VProp>
pair<int,int>
get_triangles(typename graph_traits<Graph>::vertex_descriptor v, VProp& mark,
              const Graph& g)
{
    size_t triangles = 0;

    for (auto n : adjacent_vertices_range(v, g))
    {
        if (n == v)
            continue;
        mark[n] = true;
    }

    for (auto n : adjacent_vertices_range(v, g))
    {
        if (n == v)
            continue;
        for (auto n2 : adjacent_vertices_range(n, g))
        {
            if (n2 == n)
                continue;
            if (mark[n2])
                ++triangles;
        }
    }

    for (auto n : adjacent_vertices_range(v, g))
        mark[n] = false;

    size_t k = out_degree(v, g);
    if (graph_tool::is_directed(g))
        return make_pair(triangles, (k * (k - 1)));
    else
        return make_pair(triangles / 2, (k * (k - 1)) / 2);
}

// retrieves the global clustering coefficient
struct get_global_clustering
{
    template <class Graph>
    void operator()(const Graph& g, double& c, double& c_err) const
    {
        size_t triangles = 0, n = 0;
        vector<bool> mask(num_vertices(g), false);

        #pragma omp parallel if (num_vertices(g) > OPENMP_MIN_THRESH) \
            firstprivate(mask) reduction(+:triangles, n)
        parallel_vertex_loop_no_spawn
                (g,
                 [&](auto v)
                 {
                     auto temp = get_triangles(v, mask, g);
                     triangles += temp.first;
                     n += temp.second;
                 });
        c = double(triangles) / n;

        // "jackknife" variance
        c_err = 0.0;
        double cerr = 0.0;
        #pragma omp parallel if (num_vertices(g) > OPENMP_MIN_THRESH) \
            firstprivate(mask) reduction(+:cerr)
        parallel_vertex_loop_no_spawn
                (g,
                 [&](auto v)
                 {
                     auto temp = get_triangles(v, mask, g);
                     double cl = double(triangles - temp.first) /
                         (n - temp.second);
                     cerr += power(c - cl, 2);
                 });
        c_err = sqrt(cerr);
    }
};

// sets the local clustering coefficient to a property
struct set_clustering_to_property
{
    template <class Graph, class ClustMap>
    void operator()(const Graph& g, ClustMap clust_map) const
    {
        typedef typename property_traits<ClustMap>::value_type c_type;
        vector<bool> mask(num_vertices(g), false);

        #pragma omp parallel if (num_vertices(g) > OPENMP_MIN_THRESH) \
            firstprivate(mask)
        parallel_vertex_loop_no_spawn
            (g,
             [&](auto v)
             {
                 auto triangles = get_triangles(v, mask, g);
                 double clustering = (triangles.second > 0) ?
                     double(triangles.first)/triangles.second :
                     0.0;
                 clust_map[v] = c_type(clustering);
             });
    }

    template <class Graph>
    struct get_undirected_graph
    {
        typedef typename mpl::if_
           <std::is_convertible<typename graph_traits<Graph>::directed_category,
                                directed_tag>,
            const undirected_adaptor<Graph>,
            const Graph& >::type type;
    };
};

} //graph-tool namespace

#endif // GRAPH_CLUSTERING_HH


#include "stdafx.h"
#include <cmath>
#include <fmt/format.h>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include "contest_types.h"
#include "solver_registry.h"
#include "visual_editor.h"
#include "solver_util.h"
#include "judge.h"
#include "timer.h"

namespace RandomRestartSolver {

std::vector<Point> enumerate_interior_points(const SProblem& problem) {
  integer ymin = INT_MAX, ymax = INT_MIN;
  integer xmin = INT_MAX, xmax = INT_MIN;
  for (auto p : problem.hole_polygon) {
    chmin(xmin, get_x(p));
    chmin(ymin, get_y(p));
    chmax(xmax, get_x(p));
    chmax(ymax, get_y(p));
  }

  std::vector<Point> points;
  for (int y = ymin; y <= ymax; ++y) {
    for (int x = xmin; x <= xmax; ++x) {
      if (contains(problem.hole_polygon, {x, y}) != EContains::EOUT) { // include points on the edge/vertex of the hole.
        points.emplace_back(x, y);
      }
    }
  }

  LOG(INFO) << fmt::format("found {} interior points", points.size());
  return points;
}

class Solver : public SolverBase {
 public:
  SolverOutputs solve(const SolverArguments& args) override {
    auto seed = std::random_device()();
    LOG(INFO) << fmt::format("seed: {}", seed);
    rng.seed(seed);

    constexpr int num_random_restarts = 2;
    const auto interior_points = enumerate_interior_points(*args.problem);

    SSolutionPtr best_solution = args.problem->create_solution();
    integer best_dislikes = std::numeric_limits<integer>::max();

    for (int i = 0; i < num_random_restarts; ++i) {
      LOG(INFO) << fmt::format("solve {}/{}", i, num_random_restarts);
      auto init_pose = args.problem->create_solution();
      if (i % 2 == 0) {
        // random init
        for (auto& v : init_pose->vertices) {
          v = interior_points[std::uniform_int_distribution<size_t>(0ull, interior_points.size())(rng)];
        }
      }

      auto solver = SolverRegistry::getSolver("HopGridAnnealingSolver");
      CHECK(solver);
      SolverArguments sub_args = args;
      sub_args.optional_initial_solution = init_pose;
      sub_args.random_seed = rng();
      Timer t;
      SolverOutputs trial_res = solver->solve(sub_args);

      auto trial_judge = judge(*args.problem, *trial_res.solution);
      LOG(INFO) << fmt::format("solve {}/{} .. elapsed {} ms. is_valid={}, fit_hole={}, DL={}",
        i, num_random_restarts,
        t.elapsed_ms(), trial_judge.is_valid(), trial_judge.fit_in_hole(), trial_judge.dislikes);

      if (trial_judge.is_valid() && trial_judge.dislikes < best_dislikes) {
        const std::string filename = args.problem->problem_id ? fmt::format("{}.bestsofar.pose.json", *args.problem->problem_id) : "bestsofar.pose.json";
        save_solution(args.problem, trial_res.solution, "RandomRestartSolver", filename);

        LOG(INFO) << fmt::format("@@@@@@ trial {} update best {} -> {}. saved: {}", i, best_dislikes, trial_judge.dislikes, filename);
        best_solution = trial_res.solution;
        best_dislikes = trial_judge.dislikes;
      }
    }

    SolverOutputs outputs;
    outputs.solution = best_solution;

    return outputs;
  }

 private:
  std::mt19937 rng;
};

}

REGISTER_SOLVER("RandomRestartSolver", RandomRestartSolver::Solver);
// vim:ts=2 sw=2 sts=2 et ci


#include <stdio.h>
#include <cfloat>
#include <list>
#include <vector>
#include <math.h>
#include <iostream>
#include <algorithm>
#include <numeric>
#include <boost/algorithm/clamp.hpp>
#include <random>
#include "third_party/eigen3/unsupported/Eigen/CXX11/Tensor"
#include "tensorflow/core/framework/op.h"
#include "tensorflow/core/framework/op_kernel.h"
#include "tensorflow/core/framework/tensor_shape.h"
#include "tensorflow/core/framework/common_shape_fns.h"
#include "tensorflow/core/util/work_sharder.h"
#include "bboxes.h"
#include "wtoolkit.h"

using namespace tensorflow;
using namespace std;

typedef Eigen::ThreadPoolDevice CPUDevice;
/*
 * scales:list of float
 * aspect_ratios: list of floats (w/h)
 * shape:[H,W]
 * size:[h,w]: whole image size, use the same metrics as scales does，for example scales use relative unit (0.1,0.2) then size is (1,1)
 * scales use pixel unit (128,128), then size use input image size (640,800)
 * output:
 * [out_nr,4], out_nr=len(scales)*len(aspect_ratios)*shape[0]*shape[1]
 * 表示每个位置，每个比率，每个尺寸的anchor box, 使用相对坐标
 * 即[box0(s0,r0),box1(s1,r0),box2(s0,r1),box3(s1,r1),...]
 */
REGISTER_OP("AnchorGenerator")
    .Attr("scales: list(float)")
    .Attr("aspect_ratios:list(float)")
	.Input("shape:int32")
	.Input("size:float")
	.Output("anchors:float")
	.SetShapeFn([](shape_inference::InferenceContext* c) {
			c->set_output(0, c->Matrix(-1, 4));
			return Status::OK();
			});

template <typename Device, typename T>
class AnchorGeneratorOp: public OpKernel {
	public:
		explicit AnchorGeneratorOp(OpKernelConstruction* context) : OpKernel(context) {
			OP_REQUIRES_OK(context, context->GetAttr("scales", &scales_));
			OP_REQUIRES_OK(context, context->GetAttr("aspect_ratios", &aspect_ratios_));
		}

		void Compute(OpKernelContext* context) override
		{
			const Tensor &_in_shape= context->input(0);
			const Tensor &_in_size = context->input(1);
			auto          in_shape = _in_shape.tensor<int,1>();
			auto          in_size = _in_size.tensor<float,1>();

			OP_REQUIRES(context, _in_shape.dims() == 1, errors::InvalidArgument("shape data must be 1-dimensional"));
			OP_REQUIRES(context, _in_size.dims() == 1, errors::InvalidArgument("size data must be 1-dimensional"));
            const int out_nr = scales_.size()*aspect_ratios_.size()*in_shape(0)*in_shape(1);

			int dims_2d[2] = {out_nr,4};
			TensorShape  outshape;
			Tensor      *output_anchors = NULL;

			TensorShapeUtils::MakeShape(dims_2d, 2, &outshape);

			OP_REQUIRES_OK(context, context->allocate_output(0, outshape, &output_anchors));

			auto oanchors = output_anchors->template tensor<T,2>();

            oanchors.setZero();
            auto index = 0;

            const auto x_delta = 1.0/(in_shape(1));
            const auto y_delta = 1.0/(in_shape(0));
            auto y_pos = y_delta/2.0;
            for(auto i=0; i<in_shape(0); ++i) {
                auto x_pos = x_delta/2.0;
                for(auto j=0; j<in_shape(1); ++j) {
                    for(auto& a:aspect_ratios_) {
                        for(auto& s:scales_) {
                            const auto sa = sqrt(a);
                            const auto hw = s/(2*in_size(1))*sa;
                            const auto hh = s/(2*in_size(0))/sa;
                            oanchors(index,0) = y_pos-hh;
                            oanchors(index,1) = x_pos-hw;
                            oanchors(index,2) = y_pos+hh;
                            oanchors(index,3) = x_pos+hw;
                            ++index;
                        }
                    }
                    x_pos += x_delta;
                }
                y_pos += y_delta;
            }
		}
	private:
        std::vector<float> scales_;
        std::vector<float> aspect_ratios_;
};
REGISTER_KERNEL_BUILDER(Name("AnchorGenerator").Device(DEVICE_CPU), AnchorGeneratorOp<CPUDevice, float>);

/*
 * scales:list of float
 * aspect_ratios: list of floats, len(scales)==len(aspect_ratios)
 * shape:[H,W]
 * size:[h,w]: use the same metrics as scales does
 * output:
 * [out_nr,4], out_nr=len(scales)*shape[0]*shape[1]
 * 表示每个位置，每个比率及尺寸的组合的anchor box
 * 即[box0(s0,r0),box1(s1,r1),box2(s0,r0),box3(s1,r1),...]
 */
REGISTER_OP("MultiAnchorGenerator")
    .Attr("scales: list(float)")
    .Attr("aspect_ratios:list(float)")
	.Input("shape:int32")
	.Input("size:float")
	.Output("anchors:float")
	.SetShapeFn([](shape_inference::InferenceContext* c) {
			c->set_output(0, c->Matrix(-1, 4));
			return Status::OK();
			});

template <typename Device, typename T>
class MultiAnchorGeneratorOp: public OpKernel {
	public:
        explicit MultiAnchorGeneratorOp(OpKernelConstruction* context) : OpKernel(context) {
            OP_REQUIRES_OK(context, context->GetAttr("scales", &scales_));
            OP_REQUIRES_OK(context, context->GetAttr("aspect_ratios", &aspect_ratios_));
        }

		void Compute(OpKernelContext* context) override
		{
			const Tensor &_in_shape= context->input(0);
			const Tensor &_in_size = context->input(1);
			auto          in_shape = _in_shape.tensor<int,1>();
			auto          in_size = _in_size.tensor<float,1>();

			OP_REQUIRES(context, _in_shape.dims() == 1, errors::InvalidArgument("shape data must be 1-dimensional"));
			OP_REQUIRES(context, _in_size.dims() == 1, errors::InvalidArgument("size data must be 1-dimensional"));
            const int out_nr = scales_.size()*in_shape(0)*in_shape(1);

			int dims_2d[2] = {out_nr,4};
			TensorShape  outshape;
			Tensor      *output_anchors = NULL;

			TensorShapeUtils::MakeShape(dims_2d, 2, &outshape);

			OP_REQUIRES_OK(context, context->allocate_output(0, outshape, &output_anchors));

			auto oanchors = output_anchors->template tensor<T,2>();

            oanchors.setZero();
            auto index = 0;

            const auto x_delta = 1.0/(in_shape(1));
            const auto y_delta = 1.0/(in_shape(0));
            auto y_pos = y_delta/2.0;
            for(auto i=0; i<in_shape(0); ++i) {
                auto x_pos = x_delta/2.0;
                for(auto j=0; j<in_shape(1); ++j) {
                    for(auto k=0; k<aspect_ratios_.size(); ++k) {
                        auto a = aspect_ratios_[k];
                        auto s = scales_[k];
                        const auto sa = sqrt(a);
                        const auto hw = s/(2*in_size(1))*sa;
                        const auto hh = s/(2*in_size(0))/sa;
                        oanchors(index,0) = y_pos-hh;
                        oanchors(index,1) = x_pos-hw;
                        oanchors(index,2) = y_pos+hh;
                        oanchors(index,3) = x_pos+hw;
                        ++index;
                    }
                    x_pos += x_delta;
                }
                y_pos += y_delta;
            }
		}
	private:
        std::vector<float> scales_;
        std::vector<float> aspect_ratios_;
};
REGISTER_KERNEL_BUILDER(Name("MultiAnchorGenerator").Device(DEVICE_CPU), MultiAnchorGeneratorOp<CPUDevice, float>);

/*
 * scales:list of float
 * aspect_ratios: list of floats
 * shape:[H,W]
 * size:[h,w]: use the same metrics as scales does
 */
REGISTER_OP("LineAnchorGenerator")
    .Attr("scales: list(float)")
	.Input("shape:int32")
	.Input("size:float")
	.Output("anchors:float")
	.SetShapeFn([](shape_inference::InferenceContext* c) {
			c->set_output(0, c->Matrix(-1, 4));
			return Status::OK();
			});

template <typename Device, typename T>
class LineAnchorGeneratorOp: public OpKernel {
	public:
		explicit LineAnchorGeneratorOp(OpKernelConstruction* context) : OpKernel(context) {
			OP_REQUIRES_OK(context, context->GetAttr("scales", &scales_));
		}

		void Compute(OpKernelContext* context) override
		{
            TIME_THISV1("LineAnchorGenerator");
			const Tensor &_in_shape= context->input(0);
			const Tensor &_in_size = context->input(1);
			auto          in_shape = _in_shape.tensor<int,1>();
			auto          in_size = _in_size.tensor<float,1>();

			OP_REQUIRES(context, _in_shape.dims() == 1, errors::InvalidArgument("shape data must be 1-dimensional"));
			OP_REQUIRES(context, _in_size.dims() == 1, errors::InvalidArgument("size data must be 1-dimensional"));

            const int out_nr = scales_.size()*in_shape(0)*in_shape(1);

			int dims_2d[2] = {out_nr,4};
			TensorShape  outshape;
			Tensor      *output_anchors = NULL;

			TensorShapeUtils::MakeShape(dims_2d, 2, &outshape);

			OP_REQUIRES_OK(context, context->allocate_output(0, outshape, &output_anchors));

			auto oanchors = output_anchors->template tensor<T,2>();

            oanchors.setZero();
            auto index = 0;

            const auto x_delta = 1.0/(in_shape(1));
            const auto y_delta = 1.0/(in_shape(0));
            auto y_pos = y_delta/2.0;
            const auto hh = y_delta/2.0f;

            for(auto i=0; i<in_shape(0); ++i) {
                auto x_pos = x_delta/2.0;
                for(auto j=0; j<in_shape(1); ++j) {
                    for(auto& s:scales_) {
                        const auto hw = s/(2*in_size(1));
                        oanchors(index,0) = y_pos-hh;
                        oanchors(index,1) = x_pos-hw;
                        oanchors(index,2) = y_pos+hh;
                        oanchors(index,3) = x_pos+hw;
                        ++index;
                    }
                    x_pos += x_delta;
                }
                y_pos += y_delta;
            }
		}
	private:
        std::vector<float> scales_;
};
REGISTER_KERNEL_BUILDER(Name("LineAnchorGenerator").Device(DEVICE_CPU), LineAnchorGeneratorOp<CPUDevice, float>);


/*
   Copyright 2015 Ruben Moreno Montoliu <ruben3d at gmail dot com>

   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.
*/

//
// SphereAreaLight.cpp - Created on 2009.01.15
//

#include <cmath>
#include <string>
#include <boost/lexical_cast.hpp>
#include "SphereAreaLight.h"
#include "Object.h"
#include "Polar3.h"
#include "Random.h"
#include "Ray.h"
#include "Sphere.h"
#include "Material.h"

unsigned int SphereAreaLight::GlobalCounter = 0;

SphereAreaLight::SphereAreaLight(const Transform& tr, const double intensity, const Color& color,
								const double radius, const bool show, const Attenuation attenuation)
	: Light(tr, intensity, color), LightHasPosition(tr(Point(0,0,0))),
		LightHasAttenuation(attenuation), LightHasGeometry(show), m_radius(radius),
		m_minSamples(16), m_maxSamples(32)
{
	Light::addAttribute(LightHasPosition::ATTRIBUTE);
	Light::addAttribute(LightHasAttenuation::ATTRIBUTE);
	Light::addAttribute(LightHasGeometry::ATTRIBUTE);

	m_id = ++GlobalCounter;
}

bool SphereAreaLight::computeSample(const Point& P, const Vector& N,
							const std::list<Object*>& objects,
							unsigned int& haltonSeq,
							Color& I, Vector& L) const
{
	// Light surface normal at sample point
	Vector LN;

	// Generates a random sample. We don't want a sample facing away
	do
	{
		haltonSeq++;
		double rotPhi = acos(Random::Instance().haltonSeq(haltonSeq,2)*2-1);
		double rotTheta = Random::Instance().haltonSeq(haltonSeq,3)*PI*2;
		Polar3 p(1,rotPhi,rotTheta);
		LN=p;
	}
	while (N.dot(LN) <= 0);

	// Point at the light surface
	Point LP = /*getPosition() +*/ Point(LN * m_radius);

	// Ray from point of intersection to light surface
	L = LP - P;
	const double lightRayLength = L.length();
	L = L.normalize();
	Ray lightRay = T(Ray(P, L));	// <- Local to World
	lightRay.tmax = lightRayLength;

	// Test if the light ray is obstructed
	bool obstructed = false;
	std::list<Object*>::const_iterator obj;
	for (obj = objects.begin(); obj != objects.end(); obj++)
	{
		if (!(*obj)->castShadow()) continue;

		double t;
		const Ray lightRayT = (*obj)->T.applyInv(lightRay);	// World to object
		bool intersects = (*obj)->getGeometry()->intersects(lightRayT,t);
		if (intersects)
		{
			obstructed = true;
			break;
		}
	}

	I = obstructed ? Color(0,0,0) : getColor()*getIntensity()*getAttFactor(lightRayLength)*L.dot(LN);
	return obstructed;
}

void SphereAreaLight::computeIntensity(const Vector& N, const Point& P,
							const std::list<Object*>& objects,
							std::list<Color>& Is, std::list<Vector>& Ls) const
{
	bool penumbra = false;
	unsigned int haltonSeq = Random::Instance().generate(1024);

	Vector tN = T.applyInv(N);	// World to local
	Point tP = T.applyInv(P);
	Color I;
	Vector L;
	bool shadow = computeSample(tP,tN,objects,haltonSeq,I,L);
	Is.push_back(I);
	Ls.push_back(T(L));	// Local to world

	for (int i=1; i<m_minSamples; i++)
	{
		Color I;
		Vector L;
		bool blocked = computeSample(tP,tN,objects,haltonSeq,I,L);
		Is.push_back(I);
		Ls.push_back(T(L));	// Local to world

		if (blocked != shadow) penumbra = true;
	}

	if (penumbra)
	{
		for (int n=0; n<m_maxSamples; n++)
		{
			Color I;
			Vector L;
			computeSample(tP,tN,objects,haltonSeq,I,L);
			Is.push_back(I);
			Ls.push_back(T(L));	// Local to world
		}
	}
}

Geometry* SphereAreaLight::getGeometry() const
{
	return new Sphere(m_radius);
}

Material* SphereAreaLight::getMaterial() const
{
	Material *m = new Material(std::string("_sal_")+boost::lexical_cast<std::string>(m_id), getColor());
	m->setEmission(getIntensity());
	m->setSpecularIntensity(0.0);
	m->setSpecularGlossiness(0.0);
	return m;
}


/*
 [auto_generated]
 boost/numeric/odeint/stepper/detail/adams_bashforth_call_algebra.hpp

 [begin_description]
 Algebra caller for the Adams Bashforth stepper.
 [end_description]

 Copyright 2011-2012 Karsten Ahnert
 Copyright 2011 Mario Mulansky

 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 BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_ADAMS_BASHFORTH_CALL_ALGEBRA_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_ADAMS_BASHFORTH_CALL_ALGEBRA_HPP_INCLUDED

#include <boost/assert.hpp>

namespace boost {
namespace numeric {
namespace odeint {
namespace detail {

template< size_t Step , class Algebra , class Operations >
struct adams_bashforth_call_algebra;

template< class Algebra , class Operations >
struct adams_bashforth_call_algebra< 1 , Algebra , Operations >
{
    template< class StateIn , class StateOut , class StepStorage , class Coefficients , class Time >
    void operator()( Algebra &algebra , const StateIn &in , StateOut &out , const StepStorage &steps , const Coefficients &coef , Time dt ) const
    {
        typedef typename Coefficients::value_type value_type;
        algebra.for_each3( out , in , steps[0].m_v , typename Operations::template scale_sum2< value_type , Time >( 1.0 , dt * coef[0] ) );
    }
};

template< class Algebra , class Operations >
struct adams_bashforth_call_algebra< 2 , Algebra , Operations >
{
    template< class StateIn , class StateOut , class StepStorage , class Coefficients , class Time >
    void operator()( Algebra &algebra , const StateIn &in , StateOut &out , const StepStorage &steps , const Coefficients &coef , Time dt ) const
    {
        typedef typename Coefficients::value_type value_type;
        algebra.for_each4( out , in , steps[0].m_v , steps[1].m_v ,
                typename Operations::template scale_sum3< value_type , Time , Time >( 1.0 , dt * coef[0] , dt * coef[1] ) );
    }
};

template< class Algebra , class Operations >
struct adams_bashforth_call_algebra< 3 , Algebra , Operations >
{
    template< class StateIn , class StateOut , class StepStorage , class Coefficients , class Time >
    void operator()( Algebra &algebra , const StateIn &in , StateOut &out , const StepStorage &steps , const Coefficients &coef , Time dt ) const
    {
        typedef typename Coefficients::value_type value_type;
        algebra.for_each5( out , in , steps[0].m_v , steps[1].m_v , steps[2].m_v ,
                typename Operations::template scale_sum4< value_type , Time , Time , Time >( 1.0 , dt * coef[0] , dt * coef[1] , dt * coef[2] ) );
    }
};

template< class Algebra , class Operations >
struct adams_bashforth_call_algebra< 4 , Algebra , Operations >
{
    template< class StateIn , class StateOut , class StepStorage , class Coefficients , class Time >
    void operator()( Algebra &algebra , const StateIn &in , StateOut &out , const StepStorage &steps , const Coefficients &coef , Time dt ) const
    {
        typedef typename Coefficients::value_type value_type;
        algebra.for_each6( out , in , steps[0].m_v , steps[1].m_v , steps[2].m_v , steps[3].m_v ,
                typename Operations::template scale_sum5< value_type , Time , Time , Time >(
                        1.0 , dt * coef[0] , dt * coef[1] , dt * coef[2] , dt * coef[3] ) );
    }
};

template< class Algebra , class Operations >
struct adams_bashforth_call_algebra< 5 , Algebra , Operations >
{
    template< class StateIn , class StateOut , class StepStorage , class Coefficients , class Time >
    void operator()( Algebra &algebra , const StateIn &in , StateOut &out , const StepStorage &steps , const Coefficients &coef , Time dt ) const
    {
        typedef typename Coefficients::value_type value_type;
        algebra.for_each7( out , in , steps[0].m_v , steps[1].m_v , steps[2].m_v , steps[3].m_v , steps[4].m_v ,
                typename Operations::template scale_sum6< value_type , Time , Time , Time , Time >(
                        1.0 , dt * coef[0] , dt * coef[1] , dt * coef[2] , dt * coef[3] , dt * coef[4] ) );
    }
};

template< class Algebra , class Operations >
struct adams_bashforth_call_algebra< 6 , Algebra , Operations >
{
    template< class StateIn , class StateOut , class StepStorage , class Coefficients , class Time >
    void operator()( Algebra &algebra , const StateIn &in , StateOut &out , const StepStorage &steps , const Coefficients &coef , Time dt ) const
    {
        typedef typename Coefficients::value_type value_type;
        algebra.for_each8( out , in , steps[0].m_v , steps[1].m_v , steps[2].m_v , steps[3].m_v , steps[4].m_v , steps[5].m_v ,
                typename Operations::template scale_sum7< value_type , Time , Time , Time , Time , Time >(
                        1.0 , dt * coef[0] , dt * coef[1] , dt * coef[2] , dt * coef[3] , dt * coef[4] , dt * coef[5] ) );
    }
};

template< class Algebra , class Operations >
struct adams_bashforth_call_algebra< 7 , Algebra , Operations >
{
    template< class StateIn , class StateOut , class StepStorage , class Coefficients , class Time >
    void operator()( Algebra &algebra , const StateIn &in , StateOut &out , const StepStorage &steps , const Coefficients &coef , Time dt ) const
    {
        //BOOST_ASSERT( false ); // not implemented
        typedef typename Coefficients::value_type value_type;
        algebra.for_each9( out , in , steps[0].m_v , steps[1].m_v , steps[2].m_v , steps[3].m_v , steps[4].m_v , steps[5].m_v , steps[6].m_v ,
                            typename Operations::template scale_sum8< value_type , Time , Time , Time , Time , Time , Time >(
                                1.0 , dt * coef[0] , dt * coef[1] , dt * coef[2] , dt * coef[3] , dt * coef[4] , dt * coef[5] , dt * coef[6] ) );
    }
};

template< class Algebra , class Operations >
struct adams_bashforth_call_algebra< 8 , Algebra , Operations >
{
    template< class StateIn , class StateOut , class StepStorage , class Coefficients , class Time >
    void operator()( Algebra &algebra , const StateIn &in , StateOut &out , const StepStorage &steps , const Coefficients &coef , Time dt ) const
    {
        //BOOST_ASSERT( false ); // not implemented
        typedef typename Coefficients::value_type value_type;
        algebra.for_each10( out , in , steps[0].m_v , steps[1].m_v , steps[2].m_v , steps[3].m_v , steps[4].m_v , steps[5].m_v , steps[6].m_v , steps[7].m_v ,
                             typename Operations::template scale_sum9< value_type , Time , Time , Time , Time , Time , Time , Time >(
                                 1.0 , dt * coef[0] , dt * coef[1] , dt * coef[2] , dt * coef[3] , dt * coef[4] , dt * coef[5] , dt * coef[6] , dt * coef[7] ) );
    }
};

} // detail
} // odeint
} // numeric
} // boost

#endif // BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_ADAMS_BASHFORTH_CALL_ALGEBRA_HPP_INCLUDED


#include <starpu.h>
#include <starpu_mpi.h>
#include <vector>
#include <memory>
#include <random>
#include <iostream>
#ifdef USE_MKL
#include <mkl_cblas.h>
#include <mkl_lapacke.h>
#else
#include <cblas.h>
#include <lapacke.h>
#endif
#include <Eigen/Core>
#include <Eigen/Cholesky>
#include <mpi.h>

using namespace std;
using namespace Eigen;

void potrf(void *buffers[], void *cl_arg) {
    double *A = (double *)STARPU_MATRIX_GET_PTR(buffers[0]);
    int m = STARPU_MATRIX_GET_NX(buffers[0]);
    int n = STARPU_MATRIX_GET_NY(buffers[0]);
    assert(m == n);
    LAPACKE_dpotrf(LAPACK_COL_MAJOR, 'L', m, A, m);
}
struct starpu_codelet potrf_cl = {
    .where = STARPU_CPU,
    .cpu_funcs = { potrf, NULL },
    .nbuffers = 1,
    .modes = { STARPU_RW }
};

void trsm(void *buffers[], void *cl_arg) {
    double *A = (double *)STARPU_MATRIX_GET_PTR(buffers[0]);
    double *B = (double *)STARPU_MATRIX_GET_PTR(buffers[1]);
    int m = STARPU_MATRIX_GET_NX(buffers[1]);
    int n = STARPU_MATRIX_GET_NY(buffers[1]);
    assert(STARPU_MATRIX_GET_NX(buffers[0]) == n);
    assert(STARPU_MATRIX_GET_NY(buffers[0]) == n);
    cblas_dtrsm(CblasColMajor, CblasRight, CblasLower, CblasTrans, CblasNonUnit, m, n, 1.0, A, n, B, m);
}
struct starpu_codelet trsm_cl = {
    .where = STARPU_CPU,
    .cpu_funcs = { trsm, NULL },
    .nbuffers = 2,
    .modes = { STARPU_R, STARPU_RW }
};

void syrk(void *buffers[], void *cl_arg) {
    double *A = (double *)STARPU_MATRIX_GET_PTR(buffers[0]);
    double *C = (double *)STARPU_MATRIX_GET_PTR(buffers[1]);
    int m = STARPU_MATRIX_GET_NX(buffers[0]);
    int k = STARPU_MATRIX_GET_NY(buffers[0]);
    assert(STARPU_MATRIX_GET_NX(buffers[1]) == m);
    assert(STARPU_MATRIX_GET_NY(buffers[1]) == m);
    cblas_dsyrk(CblasColMajor, CblasLower, CblasNoTrans, m, k, -1.0, A, m, 1.0, C, m);
}
struct starpu_codelet syrk_cl = {
    .where = STARPU_CPU,
    .cpu_funcs = { syrk, NULL },
    .nbuffers = 2,
    .modes = { STARPU_R, STARPU_RW }
};

void gemm(void *buffers[], void *cl_arg) {
    double *A = (double *)STARPU_MATRIX_GET_PTR(buffers[0]);
    double *B = (double *)STARPU_MATRIX_GET_PTR(buffers[1]);
    double *C = (double *)STARPU_MATRIX_GET_PTR(buffers[2]);
    int m = STARPU_MATRIX_GET_NX(buffers[2]);
    int n = STARPU_MATRIX_GET_NY(buffers[2]);
    int k = STARPU_MATRIX_GET_NY(buffers[0]);
    assert(STARPU_MATRIX_GET_NX(buffers[0]) == m);
    assert(STARPU_MATRIX_GET_NX(buffers[1]) == n);
    assert(STARPU_MATRIX_GET_NY(buffers[1]) == k);
    cblas_dgemm(CblasColMajor, CblasNoTrans, CblasTrans, m, n, k, -1.0, A, m, B, n, 1.0, C, m);
  }
struct starpu_codelet gemm_cl = {
    .where = STARPU_CPU,
    .cpu_funcs = { gemm, NULL },
    .nbuffers = 3,
    .modes = { STARPU_R, STARPU_R, STARPU_RW }
};

void cholesky(const int block_size, const int num_blocks, const int rank, const int size, const int test, const int nrow, const int ncol, const bool prune, const int upper_block_size) {
    auto val = [&](int i, int j) { return  1.0/(double)((i-j)*(i-j)+1.0); };
    vector<MatrixXd*> blocks(num_blocks*num_blocks);
    vector<starpu_data_handle_t> dataA(num_blocks*num_blocks);
    auto block_2_rank = [&](int i, int j){return (i % nrow) * ncol + j % ncol;};
    const int ncores = starpu_worker_get_count_by_type(STARPU_CPU_WORKER);
    const int matrix_size = block_size * num_blocks;

    // Warmup MKL
    {
        Eigen::MatrixXd A = Eigen::MatrixXd::Identity(256,256);
        Eigen::MatrixXd B = Eigen::MatrixXd::Identity(256,256);
        Eigen::MatrixXd C = Eigen::MatrixXd::Identity(256,256);
        for(int i = 0; i < 10; i++) {
            cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, 256, 256, 256, 1.0, A.data(), 256, B.data(), 256, 1.0, C.data(), 256);
        }
    }

    std::mt19937 gen(2020);
    assert(upper_block_size <= 2*block_size);
    const int lower_block_size = 2*block_size - upper_block_size;
    std::uniform_int_distribution<> distrib(lower_block_size,upper_block_size); // average is block_size
    if(rank == 0) printf("lower_block_size %d, upper_block_size %d\n", upper_block_size, lower_block_size);
    std::vector<int> block_sizes(num_blocks, block_size);
    {
        int n = 0;
        for(int i = 0; i < num_blocks-1; i++) {
            int bs = std::min(matrix_size - n, distrib(gen));
            n += bs;
            block_sizes[i] = bs;
        }
        assert(matrix_size - n >= 0);
        block_sizes[num_blocks-1] = matrix_size - n;
    }
    int total = std::accumulate(block_sizes.begin(), block_sizes.end(), 0);
    assert(total == matrix_size);
    std::vector<int> block_displ(num_blocks+1, 0);
    for(int i = 1; i < num_blocks+1; i++) {
        block_displ[i] = block_displ[i-1] + block_sizes[i-1];
    }
    assert(block_displ[num_blocks] == matrix_size);
    if(rank == 0) {
        printf("block sizes: ");
        for(int i = 0; i < num_blocks; i++) {
            assert(block_sizes[i] >= 0);
            printf("%d ", block_sizes[i]);
        };
        printf("\n");
        printf("block displ: ");
        for(int i = 0; i < num_blocks+1; i++) {
            assert(block_displ[i] >= 0);
            printf("%d ", block_displ[i]);
        };
        printf("\n");
    }

    for (int ii=0; ii<num_blocks; ii++) {
        for (int jj=0; jj<=ii; jj++) {
            int mpi_rank = block_2_rank(ii,jj);
            blocks[ii+jj*num_blocks] = new MatrixXd();
            if (mpi_rank == rank) {
                auto val_block = [&](int i, int j) { return val(block_displ[ii]+i,block_displ[jj]+j); };
                *blocks[ii+jj*num_blocks] = MatrixXd::NullaryExpr(block_sizes[ii], block_sizes[jj], val_block);
            }
        }
    }

    auto is_block_non_empty = [&](int i, int j){ return block_sizes[i] > 0 && block_sizes[j] > 0; };

    size_t num_pruned = 0;
    starpu_mpi_barrier(MPI_COMM_WORLD);
    double start = starpu_timing_now();

    for (int ii=0; ii<num_blocks; ii++) {
        for (int jj=0; jj<=ii; jj++) {
            int mpi_rank = block_2_rank(ii,jj);
            if(is_block_non_empty(ii,jj)){
                if (mpi_rank == rank) {
                    starpu_matrix_data_register(&dataA[ii+jj*num_blocks], STARPU_MAIN_RAM, (uintptr_t)blocks[ii+jj*num_blocks]->data(), block_sizes[ii], block_sizes[ii], block_sizes[jj], sizeof(double));
                } else {
                    starpu_matrix_data_register(&dataA[ii+jj*num_blocks], -1, (uintptr_t)NULL, block_sizes[ii], block_sizes[ii], block_sizes[jj], sizeof(double));
                }
                if (dataA[ii+jj*num_blocks]) {
                    starpu_mpi_data_register(dataA[ii+jj*num_blocks], ii+jj*num_blocks, mpi_rank);
                }
            }
        }
    }

    for (int kk = 0; kk < num_blocks; ++kk) {
        // POTRF
        if( (!prune) || block_2_rank(kk,kk) == rank) {
            if(is_block_non_empty(kk,kk)){
                starpu_mpi_task_insert(MPI_COMM_WORLD,&potrf_cl,
                    STARPU_RW, dataA[kk+kk*num_blocks],
                0);
            }
        } else {
            num_pruned++;
        }

        for (int ii = kk+1; ii < num_blocks; ++ii) {
            // TRSM
            if( (!prune) || block_2_rank(kk,kk) == rank || block_2_rank(ii,kk) == rank) {
                if(is_block_non_empty(ii,kk)){
                    starpu_mpi_task_insert(MPI_COMM_WORLD,&trsm_cl,
                        STARPU_R,  dataA[kk+kk*num_blocks],
                        STARPU_RW, dataA[ii+kk*num_blocks],
                    0);
                }
            } else {
                num_pruned++;
            }
            if(block_sizes[kk] > 0) {
                starpu_mpi_cache_flush(MPI_COMM_WORLD, dataA[kk+kk*num_blocks]);
            }

            // SYRK
            if( (!prune) || block_2_rank(ii,kk) == rank || block_2_rank(ii,ii) == rank) {
                if(is_block_non_empty(ii,kk)){
                    starpu_mpi_task_insert(MPI_COMM_WORLD,&syrk_cl,
                        STARPU_R,  dataA[ii+kk*num_blocks],
                        STARPU_RW, dataA[ii+ii*num_blocks],
                    0);
                }
            } else {
                num_pruned++;
            }

            for (int jj = kk+1; jj < ii; ++jj) {
                // GEMM
                if( (!prune) || block_2_rank(ii,kk) == rank || block_2_rank(jj,kk) == rank || block_2_rank(ii,jj) == rank) {
                    if(is_block_non_empty(ii,kk) && is_block_non_empty(jj,kk)){
                        starpu_mpi_task_insert(MPI_COMM_WORLD,&gemm_cl,
                            STARPU_R,  dataA[ii+kk*num_blocks],
                            STARPU_R,  dataA[jj+kk*num_blocks],
                            STARPU_RW, dataA[ii+jj*num_blocks],
                        0);
                    }
                } else {
                    num_pruned++;
                }
            }
            if(block_sizes[kk] > 0 && block_sizes[ii] > 0) {
                starpu_mpi_cache_flush(MPI_COMM_WORLD, dataA[ii+kk*num_blocks]);
            }
        }
    }

    double end_insertion = starpu_timing_now();
    starpu_task_wait_for_all();
    starpu_mpi_barrier(MPI_COMM_WORLD);
    double end = starpu_timing_now();

    // Makes grep/import to excel easier ; just do
    // cat output | grep -P '\[0\]\>\>\>\>'
    // to extract rank 0 info
    printf(">>>>test rank nranks ncores matrix_size block_size num_blocks total_time insertion_time prune num_pruned upper_block_size\n");
    printf("[%d]>>>>chol_starpu %d %d %d %d %d %d %e %e %d %zd %d\n",rank,rank,size,ncores,matrix_size,block_size,num_blocks,(end-start)/1e6,(end_insertion-start)/1e6,prune,num_pruned,upper_block_size);

    for (int ii=0; ii<num_blocks; ii++) {
        for (int jj=0; jj<=ii; jj++) {
            if(block_sizes[ii] > 0 && block_sizes[jj] > 0) {
                starpu_data_unregister(dataA[ii+jj*num_blocks]);
            }
        }
    }

    if (test) {
        printf("Testing...\n");
        for (int ii=0; ii<num_blocks; ii++) {
            for (int jj=0; jj<=ii; jj++) {
                int mpi_rank = block_2_rank(ii,jj);
                if (rank == 0 && rank != mpi_rank) {
                    blocks[ii+jj*num_blocks] = new MatrixXd(block_sizes[ii], block_sizes[jj]);
                    MPI_Recv(blocks[ii+jj*num_blocks]->data(), block_sizes[ii]*block_sizes[jj], MPI_DOUBLE, mpi_rank, 0, MPI_COMM_WORLD, MPI_STATUS_IGNORE);
                } else if (rank == mpi_rank && rank != 0) {
                    MPI_Send(blocks[ii+jj*num_blocks]->data(), block_sizes[ii]*block_sizes[jj], MPI_DOUBLE, 0, 0, MPI_COMM_WORLD);
                }
            }
        }
        if (rank==0) {
            MatrixXd B = MatrixXd::NullaryExpr(matrix_size, matrix_size, val);
            MatrixXd L = MatrixXd::Zero(matrix_size, matrix_size);
            for (int ii=0; ii<num_blocks; ii++) {
                for (int jj=0; jj<num_blocks; jj++) {
                    if (jj<=ii)  {
                        L.block(block_displ[ii],block_displ[jj],block_sizes[ii],block_sizes[jj])=*blocks[ii+jj*num_blocks];
                    }
                }
            }
            auto L1=L.triangularView<Lower>();
            VectorXd x = VectorXd::Random(matrix_size);
            VectorXd b = B*x;
            VectorXd bref = b;
            L1.solveInPlace(b);
            L1.transpose().solveInPlace(b);
            double error = (b - x).norm() / x.norm();
            printf("\nError solve: %e\n\n", error);
            assert(error < 1e-6);
        }
    }
}

int main(int argc, char **argv)
{
    int req = MPI_THREAD_SERIALIZED;
    int prov = -1;
    MPI_Init_thread(NULL, NULL, req, &prov);
    starpu_mpi_init_conf(&argc, &argv, 0, MPI_COMM_WORLD, NULL);
    int rank, size;
    starpu_mpi_comm_rank(MPI_COMM_WORLD, &rank);
    starpu_mpi_comm_size(MPI_COMM_WORLD, &size);
    if (rank==0) {
        printf("Running on %d CPU cores per rank,", starpu_worker_get_count_by_type(STARPU_CPU_WORKER));
        printf("and %d ranks in total\n", size);
    }
    int block_size=10;
    int num_blocks=1;
    int test=0;
    int nrow=1;
    int ncol=1;
    bool prune = true;
    int upper_block_size = block_size;

    if (argc >= 2)
    {
        block_size = atoi(argv[1]);
    }
    if (argc >= 3)
    {
        num_blocks = atoi(argv[2]);
    }

    if (argc >= 4) {
        test = atoi(argv[3]);
    }

    if (argc >= 6) {
        nrow = atoi(argv[4]);
        ncol = atoi(argv[5]);
    }

    if (argc >= 7) {
        prune = atoi(argv[6]);
    }

    if (argc >= 8) {
        upper_block_size = atoi(argv[7]);
    } else {
        upper_block_size = block_size;
    }

    assert(nrow * ncol == size);
    printf("Usage: ./cholesky_mpi block_size num_blocks test nrow ncol prune upper_block_size\n");
    printf("block_size,%d\n",block_size);
    printf("num_blocks,%d\n",num_blocks);
    printf("test,%d\n",test);
    printf("nprocs_row,%d\n",nrow);
    printf("nprocs_col,%d\n",ncol);
    printf("prune,%d\n",prune);
    printf("upper_block_size,%d\n",upper_block_size);
    cholesky(block_size, num_blocks, rank, size, test, nrow, ncol, prune, upper_block_size);
    starpu_mpi_shutdown();
    MPI_Finalize();
    return 0;
}