/*
 Copyright (c) 2010, The Barbarian Group
 All rights reserved.

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

    * Redistributions of source code must retain the above copyright notice, this list of conditions and
	the following disclaimer.
    * 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 HOLDER 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 "cinder/PolyLine.h"

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/multi/multi.hpp>

namespace cinder {

template<typename T>
T PolyLine<T>::getPosition( float t ) const
{
	typedef typename T::TYPE R;
	if( mPoints.size() <= 1 ) return T::zero();
	if( t >= 1 ) return mPoints.back();
	if( t <= 0 ) return mPoints[0];

	size_t numSpans = mPoints.size() - 1;
	size_t span = (size_t)math<R>::floor( t * numSpans );
	R lerpT = ( t - span / (R)numSpans ) * numSpans;
	return mPoints[span] * ( 1 - lerpT ) + mPoints[span+1] * lerpT;
}

template<typename T>
T PolyLine<T>::getDerivative( float t ) const
{
	typedef typename T::TYPE R;
	if( mPoints.size() <= 1 ) return T::zero();
	if( t >= 1 ) return mPoints.back() - mPoints[mPoints.size()-2];
	if( t <= 0 ) return mPoints[1] - mPoints[0];

	size_t numSpans = mPoints.size() - 1;
	size_t span = (size_t)math<R>::floor( t * numSpans );
	return mPoints[span+1] - mPoints[span];
}

template<typename T>
void PolyLine<T>::scale( const T &scaleFactor, T scaleCenter )
{
	for( typename std::vector<T>::iterator ptIt = mPoints.begin(); ptIt != mPoints.end(); ++ptIt )
		*ptIt = scaleCenter + ( *ptIt - scaleCenter ) * scaleFactor;
}

template<typename T>
void PolyLine<T>::offset( const T &offsetBy )
{
	for( typename std::vector<T>::iterator ptIt = mPoints.begin(); ptIt != mPoints.end(); ++ptIt )
		*ptIt += offsetBy;
}

template<typename T>
T linearYatX( const Vec2<T> p[2], T x )
{
	if( p[0].x == p[1].x ) 	return p[0].y;
	return p[0].y + (p[1].y - p[0].y) * (x - p[0].x) / (p[1].x - p[0].x);
}

template<typename T>
size_t linearCrossings( const Vec2<T> p[2], const Vec2f &pt )
{
	if( (p[0].x < pt.x && pt.x <= p[1].x ) ||
		(p[1].x < pt.x && pt.x <= p[0].x )) {
		if( pt.y > linearYatX<T>( p, pt.x ) )
			return 1;
	}
	return 0;
}

template<typename T>
bool PolyLine<T>::contains( const Vec2f &pt ) const
{
	if( mPoints.size() <= 2 )
		return false;

	size_t crossings = 0;
	for( size_t s = 0; s < mPoints.size() - 1; ++s ) {
		crossings += linearCrossings( &(mPoints[s]), pt );
	}

	Vec2f temp[2];
	temp[0] = mPoints[mPoints.size()-1];
	temp[1] = mPoints[0];
	crossings += linearCrossings( &(temp[0]), pt );

	return (crossings & 1) == 1;
}

namespace {
typedef boost::geometry::model::polygon<boost::geometry::model::d2::point_xy<double> > polygon;

template<typename T>
std::vector<PolyLine<T> > convertBoostGeometryPolygons( std::vector<polygon> &polygons )
{
	std::vector<PolyLine<T> > result;
	for( std::vector<polygon>::const_iterator outIt = polygons.begin(); outIt != polygons.end(); ++outIt ) {
		typedef polygon::inner_container_type::const_iterator RingIterator;
		typedef polygon::ring_type::const_iterator PointIterator;

		result.push_back( PolyLine<T>() );
		for( PointIterator pt = outIt->outer().begin(); pt != outIt->outer().end(); ++pt )
			result.back().push_back( T( boost::geometry::get<0>(*pt), boost::geometry::get<1>(*pt) ) );

		for( RingIterator crunk = outIt->inners().begin(); crunk != outIt->inners().end(); ++crunk ) {
			PolyLine<T> contour;
			for( PointIterator pt = crunk->begin(); pt != crunk->end(); ++pt )
				contour.push_back( T( boost::geometry::get<0>(*pt), boost::geometry::get<1>(*pt) ) );
			result.push_back( contour );
		}
	}

	return result;
}

template<typename T>
polygon convertPolyLinesToBoostGeometry( const std::vector<PolyLine<T> > &a )
{
	polygon result;

	for( typename std::vector<T>::const_iterator ptIt = a[0].getPoints().begin(); ptIt != a[0].getPoints().end(); ++ptIt )
		result.outer().push_back( boost::geometry::make<boost::geometry::model::d2::point_xy<double> >( ptIt->x, ptIt->y ) );
	for( typename std::vector<PolyLine<T> >::const_iterator plIt = a.begin() + 1; plIt != a.end(); ++plIt ) {
		polygon::ring_type ring;
		for( typename std::vector<T>::const_iterator ptIt = plIt->getPoints().begin(); ptIt != plIt->getPoints().end(); ++ptIt )
			ring.push_back( boost::geometry::make<boost::geometry::model::d2::point_xy<double> >( ptIt->x, ptIt->y ) );
		result.inners().push_back( ring );
	}

	boost::geometry::correct( result );

	return result;
}
} // anonymous namespace

template<typename T>
std::vector<PolyLine<T> > PolyLine<T>::calcUnion( const std::vector<PolyLine<T> > &a, std::vector<PolyLine<T> > &b )
{
	typedef boost::geometry::model::polygon<boost::geometry::model::d2::point_xy<double> > polygon;

	if( a.empty() )
		return b;
	else if( b.empty() )
		return a;

	polygon polyA = convertPolyLinesToBoostGeometry( a );
	polygon polyB = convertPolyLinesToBoostGeometry( b );

	std::vector<polygon> output;
	boost::geometry::union_( polyA, polyB, output );

	return convertBoostGeometryPolygons<T>( output );
}

template<typename T>
std::vector<PolyLine<T> > PolyLine<T>::calcIntersection( const std::vector<PolyLine<T> > &a, std::vector<PolyLine<T> > &b )
{
	typedef boost::geometry::model::polygon<boost::geometry::model::d2::point_xy<double> > polygon;

	if( a.empty() )
		return b;
	else if( b.empty() )
		return a;

	polygon polyA = convertPolyLinesToBoostGeometry( a );
	polygon polyB = convertPolyLinesToBoostGeometry( b );

	std::vector<polygon> output;
	boost::geometry::intersection( polyA, polyB, output );

	return convertBoostGeometryPolygons<T>( output );
}

template<typename T>
std::vector<PolyLine<T> > PolyLine<T>::calcXor( const std::vector<PolyLine<T> > &a, std::vector<PolyLine<T> > &b )
{
	typedef boost::geometry::model::polygon<boost::geometry::model::d2::point_xy<double> > polygon;

	if( a.empty() )
		return b;
	else if( b.empty() )
		return a;

	polygon polyA = convertPolyLinesToBoostGeometry( a );
	polygon polyB = convertPolyLinesToBoostGeometry( b );

	std::vector<polygon> output;
	boost::geometry::sym_difference( polyA, polyB, output );

	return convertBoostGeometryPolygons<T>( output );
}

template<typename T>
std::vector<PolyLine<T> > PolyLine<T>::calcDifference( const std::vector<PolyLine<T> > &a, std::vector<PolyLine<T> > &b )
{
	typedef boost::geometry::model::polygon<boost::geometry::model::d2::point_xy<double> > polygon;

	if( a.empty() )
		return b;
	else if( b.empty() )
		return a;

	polygon polyA = convertPolyLinesToBoostGeometry( a );
	polygon polyB = convertPolyLinesToBoostGeometry( b );

	std::vector<polygon> output;
	boost::geometry::difference( polyA, polyB, output );

	return convertBoostGeometryPolygons<T>( output );
}

template class PolyLine<Vec2f>;
template class PolyLine<Vec2d>;

} // namespace cinder


#include <iostream>
#include <chrono>
#include <sys/stat.h>
#include <sys/types.h>
#include <dirent.h>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>
#include <yaml-cpp/yaml.h>
#include <glog/logging.h>

#include "camera.h"
#include "stereo_calibration.h"
#include "FileSystemTools.h"
#include "YamlFileIO.h"

#ifdef VISUALIZE_TRAJECTORY
#include "viewer.h"
#endif

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

    if (argc < 8)
    {
        std::cerr << "Usage : test_stereocalibrator [global_map.yaml] [cam0_cctag_result.yaml] [cam1_cctag_result.yaml]\n \
                         [cam0_intrin_file] [cam1_intrin_file] [flag_calibrate_hik_teche] [output_folder]\n";
        return -1;
    }

    std::string global_map_fn(argv[1]);
    std::string left_cam_cctag_fn(argv[2]);
    std::string right_cam_cctag_fn(argv[3]);
    // calibrate extrinsic between hik and TECHE camera or hik and hik camera
    bool b_homo_hik_camera = (std::stoi(argv[6]) == 1) ? false : true;
    std::string output_folder(argv[7]);

    if (!common::pathExists(output_folder))
    {
        if (!common::createPath(output_folder))
        {
            LOG(FATAL) << " Failed to create " << output_folder;
            return -1;
        }
    }
    // start timer
    const auto tp_1 = std::chrono::steady_clock::now();

    // stereo frames data of calibration process
    std::vector<std::string> v_cam0_img_paths, v_cam1_img_paths;
    std::vector<std::vector<cv::Point2d>> v_cam0_cctag_centers, v_cam1_cctag_centers;
    bool sts = common::loadCCTagResultFile(left_cam_cctag_fn, v_cam0_img_paths, v_cam0_cctag_centers);
    if (!sts)
    {
        LOG(ERROR) << " Failed to parse " << left_cam_cctag_fn;
        return -1;
    }
    sts = common::loadCCTagResultFile(right_cam_cctag_fn, v_cam1_img_paths, v_cam1_cctag_centers);
    if (!sts)
    {
        LOG(ERROR) << " Failed to parse " << right_cam_cctag_fn;
        return -1;
    }
    assert(v_cam0_img_paths.size() == v_cam1_img_paths.size());
    assert(v_cam0_cctag_centers.size() == v_cam1_cctag_centers.size());

    //! load 2 camera seperately
    std::string cam0_intrinsic_fn(argv[4]);
    std::string cam1_intrinsic_fn(argv[5]);

    StereoCalibrator stereo_calibrator(camera::model_type_t::Fisheye, target::target_type_t::CCTAG,
                                       cam0_intrinsic_fn, cam1_intrinsic_fn,
                                       global_map_fn,
                                       b_homo_hik_camera);

    for (size_t frm_id = 0; frm_id < v_cam0_img_paths.size(); frm_id++)
    {

        sts = stereo_calibrator.addCctagData(v_cam0_img_paths[frm_id], v_cam0_cctag_centers[frm_id],
                                            v_cam1_img_paths[frm_id], v_cam1_cctag_centers[frm_id]);
        if (!sts)
        {
            LOG(ERROR) << " Fail to add calibration data in frame " << frm_id;
            continue;
        }
    }

    sts = stereo_calibrator.calibrate();
    if (!sts)
    {
        LOG(ERROR) << " Fail to calibrate multicamera system!";
        return -1;
    }
    const auto tp_4 = std::chrono::steady_clock::now();
    const auto calib_time = std::chrono::duration_cast<std::chrono::duration<double>>(tp_4 - tp_1).count();
    LOG(INFO) << " Stereo caliration time is " << calib_time << " s";

    // save calibration result
    std::string cam0_img_name, cam1_img_name, path;
    common::splitPathAndFilename(v_cam0_img_paths[0], &path, &cam0_img_name);
    common::splitPathAndFilename(v_cam1_img_paths[0], &path, &cam1_img_name);
    int cam0_idx = std::atoi(&cam0_img_name[0]);
    int cam1_idx = std::atoi(&cam1_img_name[0]);
    Eigen::Matrix4d T_cam0_cam1 = stereo_calibrator.validExtrinsics();
    std::string yml_filepath = output_folder + "/camera"+ std::to_string(cam0_idx) + "_to_camera"+ std::to_string(cam1_idx) +".yml";
    common::saveExtFileOpencv(yml_filepath, T_cam0_cam1);

    // //  visualize calibration result
    // std::vector<std::pair<Eigen::Matrix4d, Eigen::Matrix4d>> v_cam_pose_pairs = stereo_calibrator.validCameraPosePairs();
    // std::vector<StereoFrame> v_visual_frames;
    // for (size_t i = 0; i < v_cam_pose_pairs.size(); ++i)
    // {
    //     StereoFrame sf(i);
    //     sf.v_T_wc[0] = v_cam_pose_pairs[i].first;
    //     sf.v_T_wc[1] = v_cam_pose_pairs[i].second;
    //     v_visual_frames.emplace_back(sf);
    // }

#ifdef VISUALIZE_TRAJECTORY
    Viewer viewer(v_visual_frames);
    std::vector<Eigen::Vector3d> v_map_points = stereo_calibrator.objectPoints();
    viewer.setCurrentMapPoints(v_map_points);
    viewer.run();
#endif

    return 0;
}


// Copyright Louis Dionne 2013-2016
// 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/assert.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/if.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/minus.hpp>
#include <boost/hana/optional.hpp>
#include <boost/hana/pair.hpp>
#include <boost/hana/tuple.hpp>
#include <boost/hana/unfold_left.hpp>
namespace hana = boost::hana;

BOOST_HANA_CONSTANT_CHECK(
    hana::unfold_left<hana::tuple_tag>(hana::int_c<10>, [](auto x) {
        return hana::if_(x == hana::int_c<0>,
            hana::nothing,
            hana::just(hana::make_pair(x - hana::int_c<1>, x))
        );
    })
    ==
    hana::tuple_c<int, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10>
);

int main() { }


//          Copyright Carl Philipp Reh 2009 - 2016.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <fcppt/container/grid/make_pos_range_start_end.hpp>
#include <fcppt/container/grid/min_from_pos.hpp>
#include <fcppt/container/grid/pos.hpp>
#include <fcppt/container/grid/sup_from_pos.hpp>
#include <fcppt/math/vector/null.hpp>
#include <fcppt/preprocessor/disable_gcc_warning.hpp>
#include <fcppt/preprocessor/pop_warning.hpp>
#include <fcppt/preprocessor/push_warning.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/test/unit_test.hpp>
#include <fcppt/config/external_end.hpp>

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(
	container_grid_pos_range
)
{
FCPPT_PP_POP_WARNING

	typedef
	fcppt::container::grid::pos<
		unsigned,
		2
	>
	pos;

	typedef
	fcppt::container::grid::min_from_pos<
		pos
	>
	min;

	typedef
	fcppt::container::grid::sup_from_pos<
		pos
	>
	sup;

	BOOST_CHECK_EQUAL(
		fcppt::container::grid::make_pos_range_start_end(
			min{
				fcppt::math::vector::null<
					pos
				>()
			},
			sup{
				pos(
					3u,
					2u
				)
			}
		).size(),
		6u
	);

	BOOST_CHECK_EQUAL(
		fcppt::container::grid::make_pos_range_start_end(
			min{
				pos(
					3u,
					2u
				)
			},
			sup{
				pos(
					3u,
					1u
				)
			}
		).size(),
		0u
	);

}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_INFINITY_NORM_INCLUDE
#define MTL_INFINITY_NORM_INCLUDE

#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/concept/magnitude.hpp>
#include <boost/numeric/mtl/utility/enable_if.hpp>
#include <boost/numeric/mtl/utility/is_row_major.hpp>
#include <boost/numeric/mtl/utility/category.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/utility/property_map.hpp>
#include <boost/numeric/mtl/operation/max_of_sums.hpp>
#include <boost/numeric/mtl/vector/lazy_reduction.hpp>
#include <boost/numeric/mtl/vector/reduction.hpp>
#include <boost/numeric/mtl/vector/reduction_functors.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace mtl {

    namespace vec {

	template <unsigned long Unroll, typename Vector>
	typename traits::enable_if_vector<Vector, typename RealMagnitude<typename Collection<Vector>::value_type>::type>::type
	inline infinity_norm(const Vector& vector)
	{
	    vampir_trace<2006> tracer;
	    typedef typename RealMagnitude<typename Collection<Vector>::value_type>::type result_type;
	    return reduction<Unroll, infinity_norm_functor, result_type>::apply(vector);
	}

	/*! Infinity-norm for vectors: infinity_norm(x) \f$\rightarrow |x|_\infty\f$.
	    \retval The magnitude type of the respective value type, see Magnitude.

	    The norms are defined as \f$|v|_\infty=\max_i |v_i|\f$.

	    Vector norms are unrolled 8-fold by default.
	    An n-fold unrolling can be generated with infinity_norm<n>(x).
	    The maximum for n is 8 (it might be increased later).
	**/
	template <typename Vector>
	typename mtl::traits::enable_if_vector<Vector, typename RealMagnitude<typename Collection<Vector>::value_type>::type>::type
	inline infinity_norm(const Vector& vector)
	{
	    return infinity_norm<8>(vector);
	}

	template <typename Vector>
	lazy_reduction<Vector, infinity_norm_functor> inline lazy_infinity_norm(const Vector& v)
	{  return lazy_reduction<Vector, infinity_norm_functor>(v); 	}
    }

    namespace mat {

	// Ignore unrolling for matrices
	template <unsigned long Unroll, typename Matrix>
	typename mtl::traits::enable_if_matrix<Matrix, typename RealMagnitude<typename Collection<Matrix>::value_type>::type>::type
	inline infinity_norm(const Matrix& matrix)
	{
	    vampir_trace<3011> tracer;
	    using mtl::impl::max_of_sums;
	    typename mtl::traits::row<Matrix>::type                             row(matrix);
	    return max_of_sums(matrix, mtl::traits::is_row_major<typename OrientedCollection<Matrix>::orientation>(),
			       row, num_rows(matrix));
	}

	/*! Infinity-norm for matrices: infinity_norm(x) \f$\rightarrow |x|_\infty\f$.
	    \retval The magnitude type of the respective value type, see Magnitude.

	    The norms are defined as \f$|A|_\infty=\max_i\{\sum_j(|A_{ij}|)\}\f$.
	    Matrix norms are not (yet) optimized by unrolling.
	**/
	template <typename Matrix>
	typename mtl::traits::enable_if_matrix<Matrix, typename RealMagnitude<typename Collection<Matrix>::value_type>::type>::type
	inline infinity_norm(const Matrix& matrix)
	{
	    return infinity_norm<8>(matrix);
	}
    }

    using vec::infinity_norm;
    using vec::lazy_infinity_norm;
    using mat::infinity_norm;

} // namespace mtl

#endif // MTL_INFINITY_NORM_INCLUDE


#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_01.hpp>

typedef std::vector<std::vector<double>> Matrix2D;

class TrafficMatrix
{

public:
    TrafficMatrix() {}
    TrafficMatrix(int seed)
    {
        this->seed = seed;
        this->rng.seed(this->seed);
        this->_generated = false;
    }

    ~TrafficMatrix() {}

    void generateRandomTrafficMatrix(int size, int trafficFactor)
    {
        std::cout << "size is" << size << std::endl;
        for (int i = 0; i < size; i++)
        {
            this->trafficMatrix.push_back(std::vector<double>());
            for (int j = 0; j < size; j++)
            {
                if (j == i)
                {
                    this->trafficMatrix[i].push_back(0);
                }
                else {
                    double trafficDensity = this->uniformGenerator(this->rng) * 0.9 + 0.1;
                    this->trafficMatrix[i].push_back(trafficDensity * trafficFactor / (size - 1));
                }
            }
        }
        this->_generated = true;
    }

    const Matrix2D &getTrafficMatrix() const
    {
        if (this->_generated == false)
        {
            std::cout << "ERROR! Traffic Matrix has not been generated." << std::endl;
        }
        return this->trafficMatrix;
    }

    void serializeTrafficMatrix(const char *filename) const
    {

        if (this->_generated == false)
        {
            std::cout << "ERROR! Traffic Matrix has not been generated." << std::endl;
        }
        std::ofstream FILE;
        FILE.open(filename);
        for (auto &row : this->trafficMatrix)
        {
            for (auto &value : row)
            {
                FILE << value << " ";
            }
            FILE << std::endl;
        }
        FILE.close();
    }

private:
    Matrix2D trafficMatrix;
    int seed;
    boost::random::uniform_01<double> uniformGenerator;
    boost::random::mt19937 rng;
    bool _generated;
};


/*=============================================================================
    Copyright (c) 2002-2003 Joel de Guzman
    Copyright (c) 2002 Juan Carlos Arevalo-Baeza
    http://spirit.sourceforge.net/

    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 <boost/spirit/core.hpp>
#include <boost/spirit/utility/functor_parser.hpp>
#include <boost/spirit/actor/assign_actor.hpp>
#include <iostream>
#include <vector>
#include <string>

///////////////////////////////////////////////////////////////////////////////
//
//  Demonstrates the functor_parser. This is discussed in the
//  "Functor Parser" chapter in the Spirit User's Guide.
//
///////////////////////////////////////////////////////////////////////////////
using namespace std;
using namespace boost::spirit;

///////////////////////////////////////////////////////////////////////////////
//
//  Our parser functor
//
///////////////////////////////////////////////////////////////////////////////
struct number_parser
{
    typedef int result_t;
    template <typename ScannerT>
    int
    operator()(ScannerT const& scan, result_t& result) const
    {
        if (scan.at_end())
            return -1;

        char ch = *scan;
        if (ch < '0' || ch > '9')
            return -1;

        result = 0;
        int len = 0;

        do
        {
            result = result*10 + int(ch - '0');
            ++len;
            ++scan;
        } while (!scan.at_end() && (ch = *scan, ch >= '0' && ch <= '9'));

        return len;
    }
};

functor_parser<number_parser> number_parser_p;

///////////////////////////////////////////////////////////////////////////////
//
//  Our number parser functions
//
///////////////////////////////////////////////////////////////////////////////
bool
parse_number(char const* str, int& n)
{
    return parse(str, lexeme_d[number_parser_p[assign_a(n)]], space_p).full;
}

bool
parse_numbers(char const* str, std::vector<int>& n)
{
    return
        parse(
            str,
            lexeme_d[number_parser_p[push_back_a(n)]]
                >> *(',' >> lexeme_d[number_parser_p[push_back_a(n)]]),
            space_p
        ).full;
}

////////////////////////////////////////////////////////////////////////////
//
//  Main program
//
////////////////////////////////////////////////////////////////////////////
int
main()
{
    cout << "/////////////////////////////////////////////////////////\n\n";
    cout << "\t\tA number parser implemented as a functor for Spirit...\n\n";
    cout << "/////////////////////////////////////////////////////////\n\n";

    cout << "Give me an integer number command\n";
    cout << "Commands:\n";
    cout << "  A <num> --> parses a single number\n";
    cout << "  B <num>, <num>, ... --> parses a series of numbers ";
    cout << "separated by commas\n";
    cout << "  Q --> quit\n\n";

    string str;
    while (getline(cin, str))
    {
        if (str.empty() || str[0] == 'q' || str[0] == 'Q')
            break;

        else if (str[0] == 'a' || str[0] == 'A')
        {
            int n;
            if (parse_number(str.c_str()+1, n))
            {
                cout << "-------------------------\n";
                cout << "Parsing succeeded\n";
                cout << str << " Parses OK: " << n << endl;
                cout << "-------------------------\n";
            }
            else
            {
                cout << "-------------------------\n";
                cout << "Parsing failed\n";
                cout << "-------------------------\n";
            }
        }

        else if (str[0] == 'b' || str[0] == 'B')
        {
            std::vector<int> n;
            if (parse_numbers(str.c_str()+1, n))
            {
                cout << "-------------------------\n";
                cout << "Parsing succeeded\n";
                int size = n.size();
                cout << str << " Parses OK: " << size << " number(s): " << n[0];
                for (int i = 1; i < size; ++i) {
                    cout << ", " << n[i];
                }
                cout << endl;
                cout << "-------------------------\n";
            }
            else
            {
                cout << "-------------------------\n";
                cout << "Parsing failed\n";
                cout << "-------------------------\n";
            }
        }

        else
        {
            cout << "-------------------------\n";
            cout << "Unrecognized command!!";
            cout << "-------------------------\n";
        }
    }

    cout << "Bye... :-) \n\n";
    return 0;
}


/*
Maximilien Danisch
Mai 2016
http://bit.ly/maxdan94
maximilien.danisch@telecom-paristech.fr

Info:
Feel free to use these lines as you wish. This program computes the exact density-friendly decomposition.

To compile:
g++ exactDF.cpp -fopenmp -fpermissive -o exactDF -O3

To execute:
./exactDF ncpu iter net.txt rates.txt pavafit.txt cuts.txt exact.txt

- nthreads is the number of threads to use
- iter is the number of iterations over all edges to perform
- net.txt should contain the graph (one edge on each line: 2 unsigned separated by a space)
- rates.txt will contain the density value for each node
- pavafit.txt will contain the profile given by the PAVA fit, that is the week approximation of the density-friendly ("size density density-upperbound" on each line)
- cuts.txt will contain the profile given by correct cuts, that is the strong approximation of the density-friendly ("size density density-upperbound" on each line)
- exact.txt will contain the exact density-friendly ("size density" on each line (it is not necesarily in decreasing order of density))
Some information will be printed in the terminal.

*/

#include <boost/graph/edge_list.hpp>
#include <fstream>
#include <iostream>
#include <string>
#include <boost/config.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/boykov_kolmogorov_max_flow.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/lexical_cast.hpp>
#include <vector>
#include <algorithm>
#include <utility>
#include <stdio.h>
#include <stdlib.h>
#include <omp.h>
#include <time.h>

#define NLINKS 500000000 //maximum number of edges for memory allocation, will increase if needed

typedef struct {
	unsigned s;
	unsigned t;
  double a;//alpha value of edge (s,t)
} edge;

typedef struct {
	unsigned n;
	double r;//rate value of node n
} node;

typedef struct {
	unsigned n;//number of nodes
	unsigned e;//number of edges
	unsigned *map;//correspondance between old and new nodeID
	edge *edges;//list of all edges
	node *nodes;//value associated to each node
  double *ne;//ne[i]=number of edges from i to nodes before (used for pava)
	unsigned *cd;//cumulative degree
	unsigned *cuts;
  unsigned iter;//number of iterations
} optim;

//compute the maximum of three unsigned
inline unsigned max3(unsigned a,unsigned b,unsigned c){
	a=(a>b) ? a : b;
	return (a>c) ? a : c;
}

optim* readedgelist(char* edgelist){
	unsigned e1=NLINKS;
	optim *opt=(optim*)malloc(sizeof(optim));
	FILE *file;

	opt->n=0;
	opt->e=0;
	file=fopen(edgelist,"r");
	opt->edges=(edge*)malloc(e1*sizeof(edge));
	while (fscanf(file,"%u %u", &(opt->edges[opt->e].s), &(opt->edges[opt->e].t))==2) {
		opt->n=max3(opt->n,opt->edges[opt->e].s,opt->edges[opt->e].t);
		if (opt->e++==e1) {
			e1+=NLINKS;
			opt->edges=(edge*)realloc(opt->edges,e1*sizeof(edge));
		}
	}
	fclose(file);
	opt->n++;
	opt->edges=(edge*)realloc(opt->edges,opt->e*sizeof(edge));
	return opt;
}

void relabel(optim *opt) {
	unsigned i,j;
	unsigned *newlabel;

	newlabel=(unsigned*)malloc(opt->n*sizeof(unsigned));
	for (i=0;i<opt->n;i++) {
		newlabel[i]=opt->n;
	}
	opt->map=(unsigned*)malloc(opt->n*sizeof(unsigned));
	j=0;
	for (i=0;i<opt->e;i++) {
		if (newlabel[opt->edges[i].s]==opt->n){
			newlabel[opt->edges[i].s]=j;
			opt->map[j++]=opt->edges[i].s;
		}
		if (newlabel[opt->edges[i].t]==opt->n){
			newlabel[opt->edges[i].t]=j;
			opt->map[j++]=opt->edges[i].t;
		}
		opt->edges[i].s=newlabel[opt->edges[i].s];
		opt->edges[i].t=newlabel[opt->edges[i].t];
	}
	opt->n=j;
	free(newlabel);
	opt->map=(unsigned*)realloc(opt->map,opt->n*sizeof(unsigned));
}

//Step 1: Frank-Wolf gradiant descent

//initialize the optim datastructure
void init(optim *opt){
  opt->iter=0;
	opt->nodes=(node*)malloc(opt->n*sizeof(node));
  for (unsigned k=0;k<opt->n;k++){
    opt->nodes[k].n=k;
    opt->nodes[k].r=0;
  }
	for (unsigned k=0;k<opt->e;k++){
    opt->edges[k].a=.5;
		opt->nodes[opt->edges[k].s].r+=.5;
		opt->nodes[opt->edges[k].t].r+=.5;
	}
}

//one pass over all edges
void onepass(optim *opt){
	unsigned i,j,k;
	double gamma;
	opt->iter++;
	gamma=2./(2.+opt->iter);
	#pragma omp parallel for private(i,j,k)
	for (k=0;k<opt->e;k++){//parfor
		i=opt->edges[k].s;
		j=opt->edges[k].t;
		if (opt->nodes[i].r<opt->nodes[j].r){
			#pragma omp atomic update
			opt->nodes[i].r+=gamma*(1-opt->edges[k].a);//carefull
			#pragma omp atomic update
			opt->nodes[j].r-=gamma*(1-opt->edges[k].a);
			opt->edges[k].a=(1.-gamma)*opt->edges[k].a+gamma;
		}
		else if (opt->nodes[i].r>opt->nodes[j].r){
			#pragma omp atomic update
			opt->nodes[i].r-=gamma*opt->edges[k].a;//carefull
			#pragma omp atomic update
			opt->nodes[j].r+=gamma*opt->edges[k].a;
			opt->edges[k].a*=(1.-gamma);
		}
	}
}

//to print in file the value for each stub: NOT USED
void print_alphas(optim* opt, char* alphas){
	FILE *file=fopen(alphas,"w");
	unsigned long long k;
	for (k=0;k<opt->e;k++){
		fprintf(file,"%u %u %e\n",opt->map[opt->edges[k].s],opt->map[opt->edges[k].t],opt->edges[k].a);
	}
	fclose(file);
}

void freeoptim(optim *opt){
	free(opt->map);
	free(opt->edges);
	free(opt->nodes);
  free(opt->ne);
	free(opt->cuts);
	free(opt);
}

//Step 2: Isotonic regression with PAVA

//used for quicksort (greatest hit in CS before this algorithm)
static int compare_nodes(void const *a, void const *b){
	if (((node*)a)->r <= ((node*)b)->r)
		return 1;
	return -1;
}
//used for quicksort (greatest hit in CS before this algorithm)
static int compare_edges(void const *a, void const *b){
	edge *pa = (edge *)a;
	edge *pb = (edge *)b;
	if ((*pa).s>(*pb).s)
		return 1;
	if ((*pa).s<(*pb).s)
		return -1;
	if ((*pa).t<=(*pb).t)
		return 1;
	return -1;
}

void prepava(optim *opt){
  unsigned u,v;
  unsigned *newlabel=(unsigned*)malloc(opt->n*sizeof(unsigned));
  qsort(opt->nodes,opt->n,sizeof(node),compare_nodes);

  for (unsigned i=0;i<opt->n;i++){
    newlabel[opt->nodes[i].n]=i;
  }
  for (unsigned i=0;i<opt->e;i++){
		u=newlabel[opt->edges[i].s];
		v=newlabel[opt->edges[i].t];
		if (u<v){
			opt->edges[i].s=u;
			opt->edges[i].t=v;
		}
		else {
			opt->edges[i].s=v;
			opt->edges[i].t=u;
			opt->edges[i].a=1-opt->edges[i].a;
		}
	}
  //free(newlabel);
  qsort(opt->edges,opt->e,sizeof(edge),compare_edges);

	opt->cd=(unsigned*)calloc((opt->n+1),sizeof(unsigned));
	for (unsigned i=0;i<opt->e;i++){
		opt->cd[opt->edges[i].s+1]++;
	}
	for (unsigned i=0;i<opt->n;i++){
		opt->cd[i+1]+=opt->cd[i];
	}

  opt->ne=(double*)calloc(opt->n,sizeof(double));
  for (unsigned i=0;i<opt->e;i++){
    u=opt->edges[i].s;
    v=opt->edges[i].t;
    opt->ne[(u>v)?u:v]++;
  }
}

//to print in file the value for each node
void print_rates(optim* opt,char* rates){
	FILE *file=fopen(rates,"w");
	unsigned i;
	for (i=0;i<opt->n;i++){
		fprintf(file,"%u %e\n",opt->map[opt->nodes[i].n],opt->nodes[i].r);
	}
	fclose(file);
}

//fit data structure:
typedef struct {
	unsigned n;//total number of aggregated points
	unsigned *nag;//nag[i]=number of points aggregated in i
	double *val;//val[i]=value of the aggregated points
} isoreg;

//Pool Adjacent Violators Algorithm. Values to fit in vect and size of vect.
isoreg *pava(double *vect,unsigned n){
	isoreg *fit=(isoreg*)malloc(sizeof(isoreg));
	unsigned *nag=(unsigned*)malloc(n*sizeof(unsigned));
	double *val=(double*)malloc(n*sizeof(double));
	unsigned i,j;

	nag[0]=1;
	val[0]=vect[0];
	j=0;
	for (i=1;i<n;i++){
		j+=1;
		val[j]=vect[i];
		nag[j]=1;
		while ((j>0) && (val[j]>val[j-1]-1e-10)){//do val[j]>val[j-1] to have a non-increasing monotonic regression.
			val[j-1]=(nag[j]*val[j]+nag[j-1]*val[j-1])/(nag[j]+nag[j-1]);
			nag[j-1]+=nag[j];
			j--;
		}
	}
	fit->n=j+1;
	fit->nag=nag;
	fit->val=val;
	return fit;
}

//printing the result in file output: "nag val" on each line
void print_fit(isoreg *fit,char* output){
	FILE *file=fopen(output,"w");
	for (unsigned i=0;i<fit->n;i++){
		fprintf(file,"%u %.10le\n",fit->nag[i],fit->val[i]);
	}
	fclose(file);
}

void freeisoreg(isoreg *fit){
	free(fit->nag);
	free(fit->val);
	free(fit);
}

//Step 3: Checking if the cuts given by PAVA are correct

//printing the result in file output: "nag val" on each line
isoreg* mkcut(isoreg* fit, optim *opt){
	unsigned j1,j2,ncuts=0;
	double *r=(double*)malloc(opt->n*sizeof(double));
	double *r2=(double*)malloc(opt->n*sizeof(double));
	double min, max;
	edge ed;
	unsigned *d=(unsigned*)calloc(opt->n,sizeof(unsigned));
	unsigned *d2=(unsigned*)malloc(opt->n*sizeof(unsigned));
	isoreg *fit2=(isoreg*)malloc(sizeof(isoreg));
	fit2->nag=(unsigned*)calloc(opt->n,sizeof(unsigned));
	fit2->val=(double*)calloc(opt->n,sizeof(double));
	fit2->n=0;

	opt->cuts=(unsigned*)malloc(opt->n*sizeof(unsigned));

	for (unsigned k=0;k<opt->n;k++){
		r[k]=opt->nodes[k].r;
		r2[k]=r[k];
	}

	j1=0;
	j2=0;
	for (unsigned i=0;i<fit->n;i++){
		fit2->nag[ncuts]+=fit->nag[i];
		fit2->val[ncuts]+=fit->nag[i]*fit->val[i];
		j2+=fit->nag[i];
		for (unsigned u=j1;u<j2;u++){
			d2[u]=0;
			for (unsigned k=opt->cd[u]+d[u];k<opt->cd[u+1];k++){
				ed=opt->edges[k];
				if (ed.t>=j2){
					r2[u]-=ed.a;
					r2[ed.t]+=ed.a;
					d2[u]++;
				}
				else{
					break;
				}
			}
		}
		min=opt->nodes[0].r;
		for (unsigned k=j1;k<j2;k++){
			min=(min<r2[k])?min:r2[k];
		}
		max=0;
		for (unsigned k=j2;k<opt->n;k++){
			max=(max>r2[k])?max:r2[k];
		}
		if (max<min){
			for (unsigned k=j1;k<j2;k++){
				opt->cuts[k]=ncuts;
			}
			fit2->val[ncuts]/=fit2->nag[ncuts];
			for (unsigned u=j1;u<j2;u++){
				d[u]+=d2[u];
			}
			for (unsigned k=j1;k<opt->n;k++){///////////
				r[k]=r2[k];
			}
			ncuts++;
			j1=j2;
		}
		else{
			for (unsigned k=j1;k<opt->n;k++){///////////
				r2[k]=r[k];
			}
		}
	}
	fit2->n=ncuts;//+1;
	return fit2;
}

//Step 4: Density-Friendly in each indepent subgraphs

typedef struct {
	unsigned s;
	unsigned t;
} edge2;

typedef struct {
	unsigned n;//number of nodes
	unsigned e;//number of edges
	unsigned l;//number of loops
	edge2 *edges;//list of edges
	unsigned *loops;//loops[i]=number of loops of node i
	unsigned *deg;//deg[i]=degree of node i
} subgraph;

void freesubgraph(subgraph* sg){
	free(sg->edges);
	free(sg->loops);
	free(sg->deg);
	free(sg);
}

subgraph **mkindep(optim* opt,unsigned *nsub){
	*nsub=opt->cuts[opt->n-1]+1;
	subgraph **sgs=(subgraph**)malloc((*nsub)*sizeof(subgraph*));
	unsigned *newlabel=(unsigned*)malloc(opt->n*sizeof(unsigned));
	unsigned j,p,u,v,u2,v2;
	for (unsigned k=0;k<*nsub;k++){
		sgs[k]=(subgraph*)malloc(sizeof(subgraph));
		sgs[k]->n=0;
		sgs[k]->e=0;
		sgs[k]->l=0;
	}
	j=0;
	for (unsigned k=0;k<opt->n;k++){
		sgs[opt->cuts[k]]->n++;
		newlabel[k]=j++;
		if ((k<opt->n-1) && (opt->cuts[k] != opt->cuts[k+1])){
			j=0;
		}
	}
	for (unsigned k=0;k<opt->e;k++){
		u=opt->edges[k].s;
		v=opt->edges[k].t;
		p=opt->cuts[v];
		if (opt->cuts[u]==p){
			sgs[p]->e++;
		}
	}
	for (unsigned k=0;k<*nsub;k++){
		sgs[k]->edges=(edge2*)malloc(sgs[k]->e*sizeof(edge2));
		sgs[k]->loops=(unsigned*)calloc(sgs[k]->n,sizeof(unsigned));
		sgs[k]->deg=(unsigned*)calloc(sgs[k]->n,sizeof(unsigned));
		sgs[k]->e=0;
	}
	for (unsigned k=0;k<opt->e;k++){
		u=opt->edges[k].s;
		v=opt->edges[k].t;
		u2=newlabel[u];
		v2=newlabel[v];
		p=opt->cuts[v];
		if (opt->cuts[u]==p){
			sgs[p]->edges[sgs[p]->e].s=u2;
			sgs[p]->edges[sgs[p]->e++].t=v2;
			sgs[p]->deg[u2]++;
			sgs[p]->deg[v2]++;
		}
		else{
			sgs[p]->loops[v2]++;
			sgs[p]->l++;
		}
	}
	return sgs;
}

subgraph *allocsubgraph(unsigned n,unsigned e){
	subgraph *sg=(subgraph*)malloc(sizeof(subgraph));
	sg->n=n;
	sg->e=0;
	sg->l=0;
	sg->deg=(unsigned*)calloc(n,sizeof(unsigned));
	sg->loops=(unsigned*)calloc(n,sizeof(unsigned));
	sg->edges=(edge2*)malloc(e*sizeof(edge2));
	return sg;
}

using namespace boost;
typedef adjacency_list_traits < vecS, vecS, directedS > Traits; //the associated types of the adjacency_list class
typedef adjacency_list < vecS, vecS, directedS,
	    property < vertex_name_t, unsigned,
	    property < vertex_index_t, long,
	    property < vertex_color_t, boost::default_color_type,
	    property < vertex_distance_t, double,///////////???
	    property < vertex_predecessor_t, Traits::edge_descriptor > > > > >,
	    property < edge_capacity_t, double,
	    property < edge_residual_capacity_t, double,
	    property < edge_reverse_t, Traits::edge_descriptor > > > > Graph; //the associated properties of the ajacency_list

void AddEdge(Traits::vertex_descriptor &v1, Traits::vertex_descriptor &v2, property_map < Graph, edge_reverse_t >::type &rev, const double capacity, Graph &g) {
  Traits::edge_descriptor e1 = add_edge(v1, v2, g).first;
  Traits::edge_descriptor e2 = add_edge(v2, v1, g).first;
  put(edge_capacity, g, e1, capacity);
   rev[e1] = e2;
   rev[e2] = e1;
}

void ReReadGraph(Traits::vertex_descriptor &s, Traits::vertex_descriptor &t, subgraph *sg, Graph &g) {
	graph_traits<Graph>::vertex_descriptor u, v;
	property_map < Graph, edge_reverse_t >::type rev = get(edge_reverse, g);
	double alpha=(sg->e+sg->l)/((double)(sg->n))+1./(((double)sg->n)*((double)sg->n));
	double capacity1=2*alpha, capacity2, capacity3=1.;
	for (unsigned i=0; i<sg->n; i++) {
		u=add_vertex(g);
		capacity2=(sg->deg[i]+2.*sg->loops[i]);
		AddEdge(s,u, rev, capacity2, g);
		AddEdge(u,t, rev, capacity1, g);
	}
	for(unsigned i=0; i<sg->e; i++){
		u=vertex(sg->edges[i].s+2,g);
		v=vertex(sg->edges[i].t+2,g);
		AddEdge(u,v,rev, capacity3, g);
		AddEdge(v,u,rev, capacity3, g);
	}
}

void maxflow(subgraph* sg,FILE *file) {
	Traits::vertex_descriptor s, t;
	Graph g;
	property_map < Graph, vertex_color_t >::type col = get(vertex_color, g);
	s=add_vertex(g);
  t=add_vertex(g);
	ReReadGraph(s, t, sg, g);
	double flow = boykov_kolmogorov_max_flow(g ,s, t);
	//std::cout << "number of nodes: " << sg->n << std::endl;
	//std::cout << "number of edges: " << sg->e << std::endl;
	//std::cout << "number of loops: " << sg->l << std::endl;
	//std::cout << "flow: " << flow << std::endl;

	bool *isin=(bool *)calloc(sg->n,sizeof(bool));
	unsigned n_nodes=0;
	for(unsigned i=0;i<sg->n;i++){
		if (col[vertex(i+2,g)]==4){
			isin[i]=1;
			n_nodes++;
			//std::cout << i << " " << col[vertex(i+2,g)] << std::endl;
		}
	}
	g.clear();
	if (n_nodes>0){
		subgraph *sg1=allocsubgraph(n_nodes,sg->e);
		subgraph *sg2=allocsubgraph(sg->n-n_nodes,sg->e);
		unsigned *newlabel=(unsigned*)malloc(sg->n*sizeof(unsigned));
		unsigned j1=0,j2=0;
		for(unsigned i=0;i<sg->n;i++){
			if (isin[i]){
				sg1->loops[j1]=sg->loops[i];
				sg1->l+=sg->loops[i];
				newlabel[i]=j1++;
			}
			else{
				sg2->loops[j2]=sg->loops[i];
				sg2->l+=sg->loops[i];
				newlabel[i]=j2++;
			}
		}
		j1=0,j2=0;
		for(unsigned i=0;i<sg->e;i++){
			if (isin[sg->edges[i].s] && isin[sg->edges[i].t]){
				sg1->edges[j1].s=newlabel[sg->edges[i].s];
				sg1->edges[j1].t=newlabel[sg->edges[i].t];
				sg1->deg[sg1->edges[j1].s]++;
				sg1->deg[sg1->edges[j1].t]++;
				j1++;
			}
			else if (isin[sg->edges[i].s] && (isin[sg->edges[i].t]==0)){
				sg2->loops[newlabel[sg->edges[i].t]]++;
				sg2->l++;
			}
			else if ((isin[sg->edges[i].s]==0) && isin[sg->edges[i].t]){
				sg2->loops[newlabel[sg->edges[i].s]]++;
				sg2->l++;
			}
			else{
				sg2->edges[j2].s=newlabel[sg->edges[i].s];
				sg2->edges[j2].t=newlabel[sg->edges[i].t];
				sg2->deg[sg2->edges[j2].s]++;
				sg2->deg[sg2->edges[j2].t]++;
				j2++;
			}
		}
		sg1->e=j1;
		sg2->e=j2;
		free(isin);
		freesubgraph(sg);
		maxflow(sg1,file);
		maxflow(sg2,file);
	}
	else {
		//<< "n,e,d = "
		#pragma omp critical
		{
			fprintf(file,"%u %le\n",sg->n,((double)(sg->e+sg->l))/sg->n);
			//fprintf(file,"%u %u %u %le\n",sg->n,sg->e,sg->l,((double)(sg->e+sg->l))/sg->n);
			//std::cout << sg->n << " " << sg->e << " " << sg->l << " " << ((double)(sg->e+sg->l))/sg->n << std::endl;
		}
		free(isin);
		freesubgraph(sg);
	}
}

int main(int argc,char** argv){
	optim* opt;
  isoreg *fit,*fit2;
	subgraph** sgs;
	unsigned nsgs;
	unsigned nthreads=atoi(argv[1]);
	unsigned rep=atoi(argv[2]);
  char* edgelist=argv[3];
  char* rates=argv[4];
  char* pavafit=argv[5];
	char* cuts=argv[6];
	char* exact=argv[7];

  omp_set_num_threads(nthreads);

	time_t t1,t2,t3;
	t1=time(NULL);
	printf("- Reading edgelist from file %s\n",edgelist);
	opt=readedgelist(edgelist);
	t2=time(NULL);
	printf("- Time = %ldh%ldm%lds\n",(t2-t1)/3600,((t2-t1)%3600)/60,((t2-t1)%60));
	printf("- Building the datastructure\n");
	t1=time(NULL);
	relabel(opt);
	t2=time(NULL);
	printf("- Time = %ldh%ldm%lds\n",(t2-t1)/3600,((t2-t1)%3600)/60,((t2-t1)%60));
	printf("- Building the datastructure\n");
	printf("- Number of nodes = %u\n",opt->n);
	printf("- Number of edges = %u\n",opt->e);
	printf("- Computing the locally densest decomposition\n");

  printf("- Step 1: Frank-Wolf gradiant descent (%u iterations)\n",rep);
	t1=time(NULL);
	init(opt);
	for (unsigned i=0;i<rep;i++){
		//printf("%u\n",i);
		onepass(opt);
	}
	t2=time(NULL);
	printf("- Time = %ldh%ldm%lds\n",(t2-t1)/3600,((t2-t1)%3600)/60,((t2-t1)%60));
  //print_alphas(opt,alphas);

  printf("- Step 2: Isotonic regression with PAVA\n");
	t1=time(NULL);
  prepava(opt);
  fit=pava(opt->ne,opt->n);
	t2=time(NULL);
	printf("- Time = %ldh%ldm%lds\n",(t2-t1)/3600,((t2-t1)%3600)/60,((t2-t1)%60));

  print_fit(fit,pavafit);
  print_rates(opt,rates);

  printf("- Step 3: Checking if the %u cuts given by PAVA are correct\n",fit->n);
	t1=time(NULL);
	fit2=mkcut(fit,opt);
	freeisoreg(fit);
  print_fit(fit2,cuts);
	t2=time(NULL);
	printf("- Time = %ldh%ldm%lds\n",(t2-t1)/3600,((t2-t1)%3600)/60,((t2-t1)%60));

  printf("- Step 4: Density-Friendly in each %u indepent subgraphs\n",fit2->n-1);
	freeisoreg(fit2);
	t1=time(NULL);
	sgs=mkindep(opt,&nsgs);
	freeoptim(opt);
	FILE *file=fopen(exact,"w");
	unsigned k;
	#pragma omp parallel for schedule(dynamic) private(k)
	for(k=0;k<nsgs;k++){
		maxflow(sgs[k],file);
	}
	fclose(file);
	t2=time(NULL);
	printf("- Time = %ldh%ldm%lds\n",(t2-t1)/3600,((t2-t1)%3600)/60,((t2-t1)%60));

	return 0;
}


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2021, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include <CTraitsTest.h>
#include <gtest/gtest.h>
#include <mrpt/math/transform_gaussian.h>
#include <mrpt/poses/CPosePDFGaussian.h>
#include <mrpt/random.h>

#include <Eigen/Dense>

using namespace mrpt;
using namespace mrpt::poses;
using namespace mrpt::math;
using namespace std;

template class mrpt::CTraitsTest<CPosePDFGaussian>;

class PosePDFGaussTests : public ::testing::Test
{
   protected:
	void SetUp() override {}
	void TearDown() override {}
	static CPosePDFGaussian generateRandomPose2DPDF(
		double x, double y, double phi, double std_scale)
	{
		CMatrixDouble31 r;
		mrpt::random::getRandomGenerator().drawGaussian1DMatrix(
			r, 0, std_scale);
		CMatrixDouble33 cov;
		cov.matProductOf_AAt(r);  // random semi-definite positive matrix:
		for (int i = 0; i < 3; i++)
			cov(i, i) += 1e-7;
		CPosePDFGaussian pdf(CPose2D(x, y, phi), cov);
		return pdf;
	}

	static void func_inverse(
		const CVectorFixedDouble<3>& x, [[maybe_unused]] const double& dummy,
		CVectorFixedDouble<3>& Y)
	{
		const CPose2D p1(x[0], x[1], x[2]);
		const CPose2D p1_inv = CPose2D() - p1;
		for (int i = 0; i < 3; i++)
			Y[i] = p1_inv[i];
	}

	void testPoseInverse(double x, double y, double phi, double std_scale)
	{
		CPosePDFGaussian pdf1 = generateRandomPose2DPDF(x, y, phi, std_scale);

		CPosePDFGaussian pdf1_inv;
		pdf1.inverse(pdf1_inv);

		// Numeric approximation:
		CVectorFixedDouble<3> y_mean;
		CMatrixFixed<double, 3, 3> y_cov;
		{
			CVectorFixedDouble<3> x_mean;
			for (int i = 0; i < 3; i++)
				x_mean[i] = pdf1.mean[i];

			CMatrixFixed<double, 3, 3> x_cov = pdf1.cov;

			double DUMMY = 0;
			CVectorFixedDouble<3> x_incrs;
			x_incrs.fill(1e-6);
			transform_gaussian_linear(
				x_mean, x_cov, func_inverse, DUMMY, y_mean, y_cov, x_incrs);
		}
		// Compare:
		EXPECT_NEAR(0, (y_cov - pdf1_inv.cov).array().abs().mean(), 1e-5)
			<< "pdf1 mean: " << pdf1.mean << endl
			<< "Numeric approximation of covariance: " << endl
			<< y_cov << endl
			<< "Returned covariance: " << endl
			<< pdf1_inv.cov << endl;
	}
};

TEST_F(PosePDFGaussTests, Inverse)
{
	testPoseInverse(0, 0, 0, 0.01);
	testPoseInverse(0, 0, 0, 0.1);

	testPoseInverse(1, 0, 0, 0.1);
	testPoseInverse(0, 1, 0, 0.1);
	testPoseInverse(0, 0, 1, 0.1);

	testPoseInverse(-5, 0, 0, 0.1);
	testPoseInverse(0, -5, 0, 0.1);
	testPoseInverse(0, 0, -5, 0.1);

	testPoseInverse(4, 6, 10.0_deg, 0.1);
	testPoseInverse(4, 6, -10.0_deg, 0.1);

	testPoseInverse(-7, 2, 30.0_deg, 0.1);
	testPoseInverse(-7, 2, -30.0_deg, 0.1);
}


/*
 * Copyright 2017-2018, Justin Carpentier, LAAS-CNRS
 *
 * This file is part of eigenpy.
 * eigenpy is free software: you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation, either version 3 of
 * the License, or (at your option) any later version.
 * eigenpy 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 Lesser General Public License for more details.  You should
 * have received a copy of the GNU Lesser General Public License along
 * with eigenpy.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <Eigen/Core>

#if EIGEN_VERSION_AT_LEAST(3,2,0)
#include "eigenpy/solvers/preconditioners.hpp"
#include "eigenpy/solvers/BasicPreconditioners.hpp"
//#include "eigenpy/solvers/BFGSPreconditioners.hpp"

namespace eigenpy
{

  void exposePreconditioners()
  {
    using namespace Eigen;

    DiagonalPreconditionerVisitor<double>::expose();
#if EIGEN_VERSION_AT_LEAST(3,3,5)
    LeastSquareDiagonalPreconditionerVisitor<double>::expose();
#endif
    IdentityPreconditionerVisitor::expose();
//      LimitedBFGSPreconditionerBaseVisitor< LimitedBFGSPreconditioner<double,Eigen::Dynamic,Eigen::Upper|Eigen::Lower> >::expose("LimitedBFGSPreconditioner");

  }

} // namespace eigenpy

#endif


/// @file simulate.hpp Simulate the dynamic model to generate synthetic datasets

#ifndef BIGGLES_SIMULATE_HPP__
#define BIGGLES_SIMULATE_HPP__

#include <boost/shared_ptr.hpp>

#include "model.hpp"
#include "point_2d.hpp"
#include "track.hpp"

namespace biggles {

/// @brief Simulate ground truth data and synthetic input for the Biggles tracker
namespace simulate
{

/// @brief Simulate a single track.
///
/// Given the model parameters specified in \p parameters, simulate a single track with birth time \p first_time_stamp. The
/// state evolution covariance matrix, \f$ Q \f$, is set to
///
/// \f[
/// Q = \left[
/// \begin{array}{cccc}
/// s_p^2 & 0 & 0 & 0 \\ 0 & s_v^2 & 0 & 0 \\ 0 & 0 & s_p^2 & 0 \\ 0 & 0 & 0 & s_v^2
/// \end{array}
/// \right]
/// \f]
///
/// where \f$ s_p \f$ is the value of \p position_sigma and \f$ s_v \f$ is the value of \p velocity_sigma.
///
/// @param parameters The model parameters to generate this track with.
/// @param first_time_stamp The birth time of the new track.
/// @param initial_position The initial position of the track.
/// @param initial_velocity The initial velocity of the track.
/// @param position_sigma The standard deviation of the position random walk.
/// @param velocity_sigma The standard deviation of the velocity random walk.
///
/// @return A shared pointer to the new track.
boost::shared_ptr<track> generate_track(std::deque<state_t> &states,
                                        const model::parameters& parameters,
                                        const point_2d& initial_position = point_2d(),
                                        const point_2d& initial_velocity = point_2d(),
                                        time_stamp first_time_stamp = 0,
                                        float position_sigma = 0.25f,
                                        float velocity_sigma = 0.01f);

boost::shared_ptr<track> generate_track(const model::parameters& parameters,
                                        const point_2d& initial_position = point_2d(),
                                        const point_2d& initial_velocity = point_2d(),
                                        time_stamp first_time_stamp = 0,
                                        float position_sigma = 0.25f,
                                        float velocity_sigma = 0.01f);

/// @brief Simulate an entire partition configuration.
///
/// @note All generated observations are included, even those outside of the bounds specified in the parameters. If you
/// want observations bounded to a region, use biggles::crop_partition() after calling this function.
///
/// @sa biggles::simulate_track()
/// @sa biggles::crop_partition()
///
/// @param[out] output_partition Write the generated partition to this reference.
/// @param params The model parameters.
/// @param start_time_stamp The first frame of data to generate.
/// @param end_time_stamp The last frame of data to birth tracks on. This may not be the last time stamp generated, see
/// the function documentation.
/// @param image_width The width of the output image. Used for generating clutter and choosing track initial position.
/// @param image_height The height of the output image. Used for generating clutter and choosing track initial position.
/// @param position_sigma The standard deviation of the position random walk.
/// @param velocity_sigma The standard deviation of the velocity random walk.
/// @param initial_velocity_sigma The standard deviation of the initial velocity for the track.
void generate_partition(partition_ptr_t& output_part_ptr,
                        const model::parameters& params,
                        time_stamp start_time_stamp = 0,
                        time_stamp end_time_stamp = 128,
                        float image_width = 128.f,
                        float image_height = 128.f,
                        float position_sigma = 0.25f,
                        float velocity_sigma = 0.01f,
                        float initial_velocity_sigma = 1.f);

/// @brief Crop out observations from a partition within a spatio-temporal window.
///
/// Remove observations from a partition whose x co-ordinate does not lie on [ \p first_x, \p last_x ) or whose y
/// co-ordinate does not lie on [ \p first_y, \p last_y ) or whose time stamp does not lie on [ \p first_time_stamp, \p
/// last_time_stamp ).
///
/// Tracks which end up with no observations are removed from the partition. Track birth and death times are modified to
/// be cropped to [ \p first_time_stamp, \p last_time_stamp ).
///
/// It is safe for \p output_partition and \p input_partition to refer to the same partition.
///
/// @param output_partition
/// @param input_partition
/// @param first_x
/// @param last_x
/// @param first_y
/// @param last_y
/// @param first_time_stamp
/// @param last_time_stamp
void crop_partition(partition_ptr_t& output_part_ptr,
                    const partition_ptr_t& input_part_ptr,
                    float first_x, float last_x,
                    float first_y, float last_y,
                    time_stamp first_time_stamp,
                    time_stamp last_time_stamp);

/// @brief Demote all tracks in a partition to the clutter.
///
/// Take all the tracks in \p input_partition and demote all their observations to the clutter. This function is useful
/// for generating synthetic input data sets from ground truths.
///
/// It is safe for \p output_partition and \p input_partition to refer to the same partition.
///
/// @param[out] output_partition overwritten with new partition
/// @param input_partition
void demote_all_tracks_from_partition(partition_ptr_t& output_part_ptr,
                                      const partition_ptr_t& input_part_ptr);

}

}

#endif // BIGGLES_SIMULATE_HPP__


/*
 * poh_color_bfs.hpp
 * Author: Aven Bross
 *
 * Implementation of Poh path 3-coloring algorithm for triangulated plane graphs
 * that uses a breadth first search to find chordless paths.
 */

#ifndef __POH_COLOR_BFS_HPP
#define __POH_COLOR_BFS_HPP

// STL headers
#include <vector>
#include <queue>
#include <stdexcept>
#include <utility>
#include <string>

// Basic graph headers
#include <boost/graph/properties.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>

// Local project headers
#include "incidence_list_helpers.hpp"

/*
 * poh_color_bfs_recursive
 *
 * inputs: A weakly triangulated planar graph with vertex indices (predfined
 *     boost property), a valid planar embedding of the graph modeling the boost
 *     PlanarEmbedding concept, a read-write-able vertex property map to store
 *     vertex colors, a read-write-able vertex property map to store integer
 *     marks for BFS, a read-write-able vertex property map to store the parent
 *     vertex for backtracking after the BFS, the first and last vertex of two
 *     disjoint colored paths P=p_0...p_n and Q=q_0...q_m such that p_0...p_nq_m
 *     ...q_0 is a cycle, an unsigned int count such that all vertex marks
 *     assigned so far are less than count, and finally the third color that has
 *     not been used on the paths P and Q.
 *
 * output: The coloring vertex property will contain a valid path 3-coloring of
 *     the subgraph bounded by p_0...p_nq_m...q_0 such that no interior vertex
 *     shares a color with a neighbor in P or Q.
 */

namespace {
    template<
            typename graph_t,
            typename planar_embedding_t,
            typename color_map_t,
            typename mark_map_t,
            typename parent_map_t,
            typename color_t,
            typename vertex_t
                = typename boost::graph_traits<graph_t>::vertex_descriptor
        >
    void poh_color_bfs_recursive(
            const graph_t & graph,
            const planar_embedding_t & planar_embedding,
            color_map_t & color_map,
            mark_map_t & mark_map,
            parent_map_t & parent_map,
            vertex_t p_0, vertex_t p_n, vertex_t q_0,
            vertex_t q_m, std::size_t count, color_t new_color
        )
    {
        color_t p_color = color_map[p_0], q_color = color_map[q_0];
        vertex_t t_0 = p_0, t_l = p_n;

        // Remove triangles from the end until we find an interior vertex t_l
        do {
            if(p_0 == p_n && q_0 == q_m) return;

            // Find the edge between p_n and q_m in q_m's incidence list
            auto edge_iter = find_edge_iterator(
                    q_m, p_n, planar_embedding, graph
                );

            // Find the neighbor t_l counterclockwise from p_n around q_m
            if(edge_iter == planar_embedding[q_m].end()) {
                throw std::runtime_error("No edge between p_n and q_m).");
            }
            else if(++edge_iter == planar_embedding[q_m].end()) {
                t_l = get_incident_vertex(
                        q_m, *planar_embedding[q_m].begin(), graph
                    );
            }
            else {
                t_l = get_incident_vertex(q_m, *edge_iter, graph);
            }

            // If t_l is in P or Q we have found a colored face and remove it
            if(color_map[t_l] == p_color) {
                p_n = t_l;
            }
            else if(color_map[t_l] == q_color) {
                q_m = t_l;
            }
        } while(color_map[t_l] == p_color || color_map[t_l] == q_color);

        vertex_t current_vertex, p_i = p_0, q_j = q_0;

        // Perform a BFS from t_l to find and color a path T between P and Q
        {
            std::queue<vertex_t> bfs_queue;
            std::size_t bfs_mark = count++;
            bfs_queue.push(t_l);
            mark_map[t_l] = bfs_mark;
            parent_map[t_l] = t_l;

            while(t_0 == p_0 && !bfs_queue.empty()) {
                current_vertex = bfs_queue.front();
                bfs_queue.pop();

                auto edge_iter = planar_embedding[current_vertex].begin();
                vertex_t last_neighbor = get_incident_vertex(
                        current_vertex, *edge_iter, graph
                    );

                // Loop through the neighbors of current_vertex
                do {
                    // If we hit the end of the list, wrap to the start
                    if(++edge_iter == planar_embedding[current_vertex].end())
                        edge_iter = planar_embedding[current_vertex].begin();

                    vertex_t neighbor = get_incident_vertex(
                            current_vertex, *edge_iter, graph
                        );

                    std::size_t mark = mark_map[neighbor];
                    color_t color = color_map[neighbor];
                    color_t last_color = color_map[last_neighbor];

                    // If we hit an unmarked vertex, add it to the queue
                    if(
                            mark != bfs_mark && color != p_color &&
                            color != q_color
                        )
                    {
                        parent_map[neighbor] = current_vertex;
                        mark_map[neighbor] = bfs_mark;
                        bfs_queue.push(neighbor);
                    }
                    // If we find an edge P to Q, we have completed T
                    else if(color == q_color && last_color == p_color) {
                        t_0 = current_vertex;
                        p_i = last_neighbor;
                        q_j = neighbor;

                        break;
                    }

                    last_neighbor = neighbor;
                } while(edge_iter != planar_embedding[current_vertex].begin());
            }

            if(t_0 == p_0) {
                throw std::runtime_error(
                        "BFS failed to find edge between paths."
                    );
            }
        }

        // If the edge p_iq_j is a chord we must color the rest
        if(p_i != p_0 || q_j != q_0) {
            poh_color_bfs_recursive(
                    graph, planar_embedding, color_map, mark_map, parent_map,
                    p_0, p_i, q_0, q_j, count, new_color
                );
        }

        color_map[current_vertex] = new_color;

        // Backtrack through BFS tree from t_0 to color the path T
        while(parent_map[current_vertex] != current_vertex) {
            current_vertex = parent_map[current_vertex];
            color_map[current_vertex] = new_color;
        }

        // Color the subgraph bounded by p_i...p_n and T
        poh_color_bfs_recursive(
                graph, planar_embedding, color_map, mark_map, parent_map,
                p_i, p_n, t_0, t_l, count, q_color
            );

        // Color the subgraph bounded by T and q_m...q_j
        poh_color_bfs_recursive(
                graph, planar_embedding, color_map, mark_map, parent_map,
                q_m, q_j, t_l, t_0, count, p_color
            );
    }
}

/*
 * poh_color_bfs
 *
 * inputs: A weakly triangulated planar graph with vertex indices (predfined
 *     boost property), a valid planar embedding of the graph modeling the boost
 *     PlanarEmbedding concept, a read-write-able vertex property map to store
 *     vertex colors, pairs of bidirectional iterators for lists of vertices
 *     of two disjoint colored paths P=p_0...p_n and Q=q_0...q_m such that
 *     p_0...p_nq_m...q_0 is a cycle, and three colors for the path 3-coloring
 *     (no vertex should be assigned any of these three colors before calling
 *     this function).
 *
 * output: The coloring vertex property will contain a valid path 3-coloring of
 *     the subgraph bounded by p_0...p_nq_m...q_0 such that vertices in P are
 *     recieve the first color, vertices in Q the second color, and no interior
 *     vertex shares a color with a neighbor in P or Q.
 */

template<
        typename graph_t,
        typename planar_embedding_t,
        typename color_map_t,
        typename mark_map_t,
        typename parent_map_t,
        typename vertex_iterator_t,
        typename color_t
    >
void poh_color_bfs(
        const graph_t & graph,
        const planar_embedding_t & planar_embedding,
        vertex_iterator_t p_begin, vertex_iterator_t p_end,
        vertex_iterator_t q_begin, vertex_iterator_t q_end,
        color_t c_0, color_t c_1, color_t c_2,
        mark_map_t & mark_map,
        parent_map_t & parent_map,
        color_map_t & color_map
    )
{
    // Color the path P
    for(vertex_iterator_t p_iter = p_begin; p_iter != p_end; ++p_iter) {
        color_map[*p_iter] = c_0;
    }

    // Color the path Q
    for(vertex_iterator_t q_iter = q_begin; q_iter != q_end; ++q_iter) {
        color_map[*q_iter] = c_1;
    }

    // Construct the path 3-coloring
    poh_color_bfs_recursive(
            graph, planar_embedding, color_map, mark_map, parent_map,
            *p_begin, *(--p_end), *q_begin, *(--q_end), 1, c_2
        );
}

/*
 * A wrapper that automatically constructs fast property maps for the parent_map
 * and neighbor_range_map, but requires that graph_t is some definition of
 * boost::adjacency_list.
 */

template<
        typename graph_t,
        typename planar_embedding_t,
        typename vertex_iterator_t,
        typename color_t,
        typename color_map_t
    >
void poh_color_bfs(
        const graph_t & graph,
        const planar_embedding_t & planar_embedding,
        vertex_iterator_t p_begin, vertex_iterator_t p_end,
        vertex_iterator_t q_begin, vertex_iterator_t q_end,
        color_t c_0, color_t c_1, color_t c_2,
        color_map_t & color_map
    )
{
    // Vertex type
    typedef typename boost::graph_traits<graph_t>::vertex_descriptor vertex_t;

    // Vertex property map to store vertex marks
    typedef boost::iterator_property_map<
            std::vector<int>::iterator,
            typename boost::property_map<
                    graph_t, boost::vertex_index_t
                >::const_type
        > integer_property_map_t;

    // Vertex property map to store BFS tree
    typedef boost::iterator_property_map<
            typename std::vector<vertex_t>::iterator,
            typename boost::property_map<
                    graph_t, boost::vertex_index_t
                >::const_type
        > parent_map_t;

    // Construct a vertex property map to store vertex marks
    std::vector<int> mark_storage(num_vertices(graph));
    integer_property_map_t mark_map(
            mark_storage.begin(), boost::get(boost::vertex_index, graph)
        );

    // Construct a vertex property map to store neighbor ranges
    std::vector<vertex_t> parent_storage(num_vertices(graph));
    parent_map_t parent_map(
            parent_storage.begin(), boost::get(boost::vertex_index, graph)
        );

    // Construct the path 3-coloring
    poh_color_bfs(
            graph,
            planar_embedding,
            p_begin, p_end,
            q_begin, q_end,
            c_0, c_1, c_2,
            mark_map,
            parent_map,
            color_map
        );
}

#endif


#include <Eigen/Dense>
#include <iostream>

int main()
{
  Eigen::MatrixXf m(3,3);
  m << 1,2,3,
       4,5,6,
       7,8,9;
  std::cout << "2nd Row: " << m.row(1) << std::endl;
  m.col(0) += m.col(2);
  std::cout << "m after adding third column to first:\n";
  std::cout << m << std::endl;
}


// ======================================================================
/*!
 * \file NFmiWebMercatorArea.cpp
 * \brief Implementation of class NFmiWebMercatorArea
 */
// ======================================================================
/*!
 * \class NFmiWebMercatorArea
 *
 * Provides for equidistance cylindrical projection. Maps geodetic
 * coordinates (in degrees) to rectangular mercator xy coordinates
 * (in meters) and vice versa.
 *
 * Projection is based on EPSG:3857
 */
// ======================================================================

#include "NFmiWebMercatorArea.h"
#include <boost/functional/hash.hpp>
#include <fmt/format.h>
#include <macgyver/Exception.h>
#include <limits>

using namespace std;

const double kSemiAxis = 6378137.0;

// ----------------------------------------------------------------------
/*!
 * Default constructor
 */
// ----------------------------------------------------------------------

NFmiWebMercatorArea::NFmiWebMercatorArea()
    : NFmiArea(),
      itsBottomLeftLatLon(),
      itsTopRightLatLon(),
      itsXScaleFactor(),
      itsYScaleFactor(),
      itsWorldRect()
{
}

// ----------------------------------------------------------------------
/*!
 * Constructor
 *
 * \param theBottomLeftLatLon Undocumented
 * \param theTopRightLatLon Undocumented
 * \param theTopLeftXY Undocumented
 * \param theBottomRightXY Undocumented
 */
// ----------------------------------------------------------------------

NFmiWebMercatorArea::NFmiWebMercatorArea(const NFmiPoint& theBottomLeftLatLon,
                                         const NFmiPoint& theTopRightLatLon,
                                         const NFmiPoint& theTopLeftXY,
                                         const NFmiPoint& theBottomRightXY,
                                         bool usePacificView)
    : NFmiArea(theTopLeftXY, theBottomRightXY, usePacificView),
      itsBottomLeftLatLon(theBottomLeftLatLon),
      itsTopRightLatLon(theTopRightLatLon),
      itsXScaleFactor(),
      itsYScaleFactor(),
      itsWorldRect()
{
  try
  {
    Init();
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Copy constructor
 *
 * \param theLatLonArea The other object being copied
 */
// ----------------------------------------------------------------------

NFmiWebMercatorArea::NFmiWebMercatorArea(const NFmiWebMercatorArea& theLatLonArea)

    = default;

// ----------------------------------------------------------------------
/*!
 * \return Undocumented
 * \todo Should return an boost::shared_ptr instead
 */
// ----------------------------------------------------------------------

NFmiArea* NFmiWebMercatorArea::Clone() const
{
  try
  {
    return new NFmiWebMercatorArea(*this);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \param fKeepWorldRect Undocumented
 */
// ----------------------------------------------------------------------

void NFmiWebMercatorArea::Init(bool fKeepWorldRect)
{
  try
  {
    if (itsTopRightLatLon.X() < itsBottomLeftLatLon.X())
      itsTopRightLatLon += NFmiPoint(360., 0.);

    if (!fKeepWorldRect)
      itsWorldRect =
          NFmiRect(LatLonToWorldXY(itsBottomLeftLatLon), LatLonToWorldXY(itsTopRightLatLon));

    itsXScaleFactor = Width() / itsWorldRect.Width();
    itsYScaleFactor = Height() / itsWorldRect.Height();

    NFmiArea::Init(fKeepWorldRect);

    const char* fmt = "+proj=webmerc +R={}";
    itsProjStr = fmt::format(fmt, kRearth);
    itsSpatialReference = std::make_shared<Fmi::SpatialReference>(itsProjStr);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \param theLatLonPoint Undocumented
 * \return Undocumented
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiWebMercatorArea::LatLonToWorldXY(const NFmiPoint& theLatLonPoint) const
{
  try
  {
    // Limit Y-values to prevent infinity

    double y = std::max(std::min(theLatLonPoint.Y(), 89.9999), -89.9999);

    return NFmiPoint(kSemiAxis * FmiRad(theLatLonPoint.X()),
                     kSemiAxis * log(tan(FmiRad(45. + 0.5 * y))));
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \param theLatLonPoint Undocumented
 * \return Undocumented
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiWebMercatorArea::ToXY(const NFmiPoint& theLatLonPoint) const
{
  try
  {
    // Transforms input geodetic coordinates (longitude,latitude) into local (relative)
    // coordinates on xy-plane.
    double xLocal, yLocal;

    // Transform input geodetic coordinates into world coordinates (meters) on xy-plane.
    NFmiPoint latlon(FixLongitude(theLatLonPoint.X()), theLatLonPoint.Y());
    NFmiPoint xyWorld(LatLonToWorldXY(latlon));

    // Finally, transform world xy-coordinates into local xy-coordinates
    xLocal = Left() + itsXScaleFactor * (xyWorld.X() - itsWorldRect.Left());
    yLocal = Top() + itsYScaleFactor * (itsWorldRect.Bottom() - xyWorld.Y());

    return NFmiPoint(xLocal, yLocal);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \param theXYPoint Undocumented
 * \return Undocumented
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiWebMercatorArea::WorldXYToLatLon(const NFmiPoint& theXYPoint) const
{
  try
  {
    // Computes the geodetic coordinates (in degrees) from the input (metric) world xy coordinates

    double worldY = theXYPoint.Y();
    double lon = NFmiLongitude(FmiDeg(theXYPoint.X() / kSemiAxis), PacificView()).Value();
    double lat = FmiDeg(2.0 * atan(exp(worldY / kSemiAxis)) - 0.5 * kPii);

    return NFmiPoint(lon, lat);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \param theXYPoint Undocumented
 * \return Undocumented
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiWebMercatorArea::XYToWorldXY(const NFmiPoint& theXYPoint) const
{
  try
  {
    // Transform local xy-coordinates into world xy-coordinates (meters).
    double xWorld = itsWorldRect.Left() + (theXYPoint.X() - Left()) / itsXScaleFactor;
    double yWorld = itsWorldRect.Bottom() - (theXYPoint.Y() - Top()) / itsYScaleFactor;
    return NFmiPoint(xWorld, yWorld);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \param theXYPoint Undocumented
 * \return Undocumented
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiWebMercatorArea::WorldXYToXY(const NFmiPoint& theWorldXYPoint) const
{
  try
  {
    double x = itsXScaleFactor * (theWorldXYPoint.X() - itsWorldRect.Left()) + Left();
    double y = Top() - itsYScaleFactor * (theWorldXYPoint.Y() - itsWorldRect.Bottom());
    return NFmiPoint(x, y);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \param theXYPoint Undocumented
 * \return Undocumented
 */
// ----------------------------------------------------------------------

const NFmiPoint NFmiWebMercatorArea::ToLatLon(const NFmiPoint& theXYPoint) const
{
  try
  {
    // Transforms input local xy-coordinates into geodetic coordinates
    // (longitude,latitude) on globe.
    return WorldXYToLatLon(XYToWorldXY(theXYPoint));
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \return Undocumented
 */
// ----------------------------------------------------------------------

double NFmiWebMercatorArea::XScale() const
{
  try
  {
    return 1. / itsXScaleFactor;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \return Undocumented
 */
// ----------------------------------------------------------------------

double NFmiWebMercatorArea::YScale() const
{
  try
  {
    return 1. / itsYScaleFactor;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \param theBottomLeftLatLon Undocumented
 * \param theTopRightLatLon Undocumented
 * \return Undocumented
 * \todo Should return an boost::shared_ptr instead
 */
// ----------------------------------------------------------------------

NFmiArea* NFmiWebMercatorArea::NewArea(const NFmiPoint& theBottomLeftLatLon,
                                       const NFmiPoint& theTopRightLatLon,
                                       bool allowPacificFix) const
{
  try
  {
    if (allowPacificFix)
    {
      PacificPointFixerData fixedPointData =
          NFmiArea::PacificPointFixer(theBottomLeftLatLon, theTopRightLatLon);
      return new NFmiWebMercatorArea(fixedPointData.itsBottomLeftLatlon,
                                     fixedPointData.itsTopRightLatlon,
                                     TopLeft(),
                                     BottomRight(),
                                     fixedPointData.fIsPacific);
    }

    return new NFmiWebMercatorArea(
        theBottomLeftLatLon, theTopRightLatLon, TopLeft(), BottomRight(), PacificView());
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Write the object to the given output stream
 *
 * \param file The output stream to write to
 * \return The output stream written to
 */
// ----------------------------------------------------------------------

std::ostream& NFmiWebMercatorArea::Write(std::ostream& file) const
{
  try
  {
    NFmiArea::Write(file);
    file << itsBottomLeftLatLon;
    file << itsTopRightLatLon;

    // Dummies to replace old removed variables
    file << "0 0\n0 0\n";

    file << itsXScaleFactor << " ";
    file << itsYScaleFactor << std::endl;
    return file;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Read new object contents from the given input stream
 *
 * \param file The input stream to read from
 * \return The input stream read from
 */
// ----------------------------------------------------------------------

std::istream& NFmiWebMercatorArea::Read(std::istream& file)
{
  try
  {
    double dummy;

    NFmiArea::Read(file);
    file >> itsBottomLeftLatLon;
    file >> itsTopRightLatLon;
    PacificView(NFmiArea::IsPacificView(itsBottomLeftLatLon, itsTopRightLatLon));

    file >> dummy >> dummy >> dummy >> dummy;  // old removed variables

    file >> itsXScaleFactor;
    file >> itsYScaleFactor;

    itsWorldRect =
        NFmiRect(LatLonToWorldXY(itsBottomLeftLatLon), LatLonToWorldXY(itsTopRightLatLon));

    Init();

    return file;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

const std::string NFmiWebMercatorArea::AreaStr() const
{
  try
  {
    std::ostringstream out;
    out << "webmercator:" << BottomLeftLatLon().X() << ',' << BottomLeftLatLon().Y() << ','
        << TopRightLatLon().X() << ',' << TopRightLatLon().Y();
    return out.str();
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Return Well Known Text representation of the GCS
 *
 * We shall return EPSG:3857, but may have to return the following if using the plain
 * EPSG does not work for all software
 *
 * PROJCS["WGS 84 / Pseudo-Mercator",
 *    GEOGCS["WGS 84",
 *        DATUM["WGS_1984",
 *            SPHEROID["WGS 84",6378137,298.257223563,
 *                AUTHORITY["EPSG","7030"]],
 *            AUTHORITY["EPSG","6326"]],
 *        PRIMEM["Greenwich",0,
 *            AUTHORITY["EPSG","8901"]],
 *        UNIT["degree",0.0174532925199433,
 *            AUTHORITY["EPSG","9122"]],
 *        AUTHORITY["EPSG","4326"]],
 *    PROJECTION["Mercator_1SP"],
 *    PARAMETER["central_meridian",0],
 *    PARAMETER["scale_factor",1],
 *    PARAMETER["false_easting",0],
 *    PARAMETER["false_northing",0],
 *    UNIT["metre",1,
 *        AUTHORITY["EPSG","9001"]],
 *    AXIS["X",EAST],
 *    AXIS["Y",NORTH],
 *    EXTENSION["PROJ4","+proj=merc +a=6378137 +b=6378137 +lat_ts=0.0 +lon_0=0.0 +x_0=0.0 +y_0=0
 * +k=1.0 +units=m +nadgrids=@null +wktext  +no_defs"], AUTHORITY["EPSG","3857"]]
 *
 */
// ----------------------------------------------------------------------

const std::string NFmiWebMercatorArea::WKT() const
{
  return "EPSG:3857";
}

// ----------------------------------------------------------------------
/*!
 * Equality comparison with an NFmiWebMercatorArea.
 *
 * \param theArea The other area being compared to
 * \return True, if the NFmiWebMercatorArea parts are equivalent
 * \todo Use static_cast instead of C-style cast
 */
// ----------------------------------------------------------------------

bool NFmiWebMercatorArea::operator==(const NFmiArea& theArea) const
{
  try
  {
    return *this == static_cast<const NFmiWebMercatorArea&>(theArea);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Equality comparison
 *
 * \param theArea The other area being compared to
 * \return True, if the areas are equivalent
 * \todo Investigate whether NFmiArea::operator== should also be called.
 */
// ----------------------------------------------------------------------

bool NFmiWebMercatorArea::operator==(const NFmiWebMercatorArea& theArea) const
{
  try
  {
    if ((itsBottomLeftLatLon == theArea.itsBottomLeftLatLon) &&
        (itsTopRightLatLon == theArea.itsTopRightLatLon) &&
        (itsXScaleFactor == theArea.itsXScaleFactor) &&
        (itsYScaleFactor == theArea.itsYScaleFactor) && (itsWorldRect == theArea.itsWorldRect))
      return true;

    return false;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Hash value
 */
// ----------------------------------------------------------------------

std::size_t NFmiWebMercatorArea::HashValue() const
{
  try
  {
    std::size_t hash = NFmiArea::HashValue();
    boost::hash_combine(hash, itsBottomLeftLatLon.HashValue());
    boost::hash_combine(hash, itsTopRightLatLon.HashValue());
    boost::hash_combine(hash, boost::hash_value(itsXScaleFactor));
    boost::hash_combine(hash, boost::hash_value(itsYScaleFactor));
    boost::hash_combine(hash, itsWorldRect.HashValue());
    return hash;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ======================================================================


// Copyright (c) 2021 George E. Brown and Rahul Narain
//
// WRAPD uses the MIT License (https://opensource.org/licenses/MIT)
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is furnished
// to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
// INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
// PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
// SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
//
// By George E. Brown (https://www-users.cse.umn.edu/~brow2327/)

#ifndef SRC_MATH_HPP_
#define SRC_MATH_HPP_

#include <Eigen/SparseCholesky>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <Eigen/IterativeLinearSolvers>

#include <iostream>
#include <memory>
#include <string>
#include <vector>

namespace wrapd {
namespace math {

using VecX = Eigen::VectorXd;
using VecXi = Eigen::VectorXi;
using MatX = Eigen::MatrixXd;
using MatXi = Eigen::MatrixXi;
using MatX2 = Eigen::MatrixX2d;
using Mat2X = Eigen::Matrix2Xd;
using SpMat = Eigen::SparseMatrix<double, Eigen::RowMajor>;
using DiagMat = Eigen::DiagonalMatrix<double, Eigen::Dynamic>;
using RowVec3 = Eigen::RowVector3d;

using Triplet = Eigen::Triplet<double>;
using Triplets = std::vector<Triplet>;

using Doubles = std::vector<double>;

using Cholesky = Eigen::SimplicialLDLT< Eigen::SparseMatrix<double>, Eigen::Lower>;

using AlignedBox = Eigen::AlignedBox<double, 3>;

// Singular value decomposition
template <typename TYPE>
using JacobiSVD = Eigen::JacobiSVD<TYPE>;
static constexpr int ComputeFullU = Eigen::ComputeFullU;
static constexpr int ComputeFullV = Eigen::ComputeFullV;

// Mapping between vectors and matrices
template <typename TYPE>
using Map = Eigen::Map<TYPE>;

// Vectors of doubles
template <int DIM>
using Vec = Eigen::Matrix<double, DIM, 1>;
using Vec2 = Vec<2>;
using Vec3 = Vec<3>;
using Vec4 = Vec<4>;

// Vectors of floats
template <int DIM>
using Vecf = Eigen::Matrix<float, DIM, 1>;
using Vec2f = Vecf<2>;
using Vec3f = Vecf<3>;
using Vec4f = Vecf<4>;

// Vectors of integers
template <int DIM>
using Veci = Eigen::Matrix<int, DIM, 1>;
using Vec1i = Veci<1>;
using Vec2i = Veci<2>;
using Vec3i = Veci<3>;
using Vec4i = Veci<4>;

template <int ROWS, int COLS>
using Mat = Eigen::Matrix<double, ROWS, COLS>;

using Mat1x3 = Mat<1, 3>;
using Mat2x2 = Mat<2, 2>;
using Mat2x3 = Mat<2, 3>;
using Mat3x2 = Mat<3, 2>;
using Mat3x3 = Mat<3, 3>;
using Mat3x4 = Mat<3, 4>;

inline double clamp(const double minval, const double val, const double maxval) {
    double retval = val;
    if (val < minval) {
        retval = minval;
    }
    if (val > maxval) {
        retval = maxval;
    }
    return retval;
}

inline void svd(
        const math::Mat2x2& F,
        math::Vec2& sigma,
        math::Mat2x2& U,
        math::Mat2x2& V,
        bool prevent_flips = true) {
    math::JacobiSVD<math::Mat2x2> svd(F, math::ComputeFullU | math::ComputeFullV);
    sigma = svd.singularValues();
    U = svd.matrixU();
    V = svd.matrixV();

    if (prevent_flips && (U*V.transpose()).determinant() < 0.) {
        math::Mat2x2 J = math::Mat2x2::Identity();
        J(1, 1) = -1.0;
        if (U.determinant() < 0.) {
            U = U * J;
            sigma[1] = -sigma[1];
        }
        if (V.determinant() < 0.0) {
            math::Mat2x2 Vt = V.transpose();
            Vt = J * Vt;
            V = Vt.transpose();
            sigma[1] = -sigma[1];
        }
    }
}

inline void polar(
        const math::Mat2x2& F,
        math::Mat2x2& R,
        math::Mat2x2& S,
        bool prevent_flips = true) {
    math::Vec2 sigma;
    math::Mat2x2 U;
    math::Mat2x2 V;
    svd(F, sigma, U, V, prevent_flips);
    R = U * V.transpose();
    S = V * sigma.asDiagonal() * V.transpose();
}

inline math::Mat2x2 rot(const math::Mat2x2& A) {
    math::Vec2 b(A(0, 0) + A(1, 1), A(0, 1) - A(1, 0));
    b.normalize();
    math::Mat2x2 R2;
    R2(0, 0) = R2(1, 1) = b(0);
    R2(0, 1) = b(1);
    R2(1, 0) = -b(1);
    return R2;
}

inline math::Mat2x2 sym(const math::Mat2x2& A) {
    math::Mat2x2 R = rot(A);
    return R.transpose() * A;
}

}  // namespace math
} // namespace wrapd

#endif  // SRC_MATH_HPP_


#pragma once

#include <Eigen/Core>
#include <Eigen/Dense>

namespace avoidance {

class Grid {
 public:
  Grid(const float grid_size, const float cell_size) : grid_size_(grid_size), cell_size_(cell_size) {
    resize(grid_size_, cell_size_);
  }
  ~Grid() = default;

  void reset() {
    mean_.fill(0.f);
    variance_.fill(0.f);
    semantics_.fill(0);
    counter_.fill(0);
    land_.fill(0);
  }

  void resize(float grid_size, float cell_size) {
    grid_size_ = grid_size;
    cell_size_ = cell_size;
    grid_row_col_size_ = static_cast<int>(std::ceil(grid_size_ / cell_size_));
    mean_.resize(grid_row_col_size_, grid_row_col_size_);
    variance_.resize(grid_row_col_size_, grid_row_col_size_);
    semantics_.resize(grid_row_col_size_, grid_row_col_size_);
    counter_.resize(grid_row_col_size_, grid_row_col_size_);
    land_.resize(grid_row_col_size_, grid_row_col_size_);
    reset();
  }

  void setMean(const Eigen::Vector2i &idx, float value) { mean_(idx.x(), idx.y()) = value; }
  void setVariance(const Eigen::Vector2i &idx, float value) { variance_(idx.x(), idx.y()) = value; }
  void setSemantics(const Eigen::Vector2i &idx, int value) { semantics_(idx.x(), idx.y()) = value; }
  void increaseCounter(const Eigen::Vector2i &idx) { counter_(idx.x(), idx.y()) = counter_(idx.x(), idx.y()) + 1; }
  void setCounter(const Eigen::Vector2i &idx, int value) { counter_(idx.x(), idx.y()) = value; }

  Eigen::MatrixXf getMean() const { return mean_; }
  Eigen::MatrixXf getVariance() const { return variance_; }
  Eigen::MatrixXi getSemantics() const { return semantics_; }
  Eigen::MatrixXi getCounter() const { return counter_; }

  float getMean(const Eigen::Vector2i &idx) { return mean_(idx.x(), idx.y()); }
  float getVariance(const Eigen::Vector2i &idx) { return variance_(idx.x(), idx.y()); }
  int getSemantics(const Eigen::Vector2i &idx) { return semantics_(idx.x(), idx.y()); }
  int getCounter(const Eigen::Vector2i &idx) { return counter_(idx.x(), idx.y()); }
  int getRowColSize() const { return grid_row_col_size_; }
  float getGridSize() const { return grid_size_; }
  float getCellSize() const { return cell_size_; }

  void setFilterLimits(const Eigen::Vector3f &pos) {
    corner_min_.x() = pos.x() - grid_size_ / 2.f;
    corner_min_.y() = pos.y() - grid_size_ / 2.f;
    corner_max_.x() = pos.x() + grid_size_ / 2.f;
    corner_max_.y() = pos.y() + grid_size_ / 2.f;
  }

  void getGridLimits(Eigen::Vector2f &min, Eigen::Vector2f &max) const {
    min = corner_min_;
    max = corner_max_;
  }

  void combine(const Grid &prev_grid, float alpha) {
    mean_ = alpha * prev_grid.mean_ + (1.f - alpha) * mean_;
    variance_ = alpha * prev_grid.variance_ + (1.f - alpha) * variance_;
  }

  Eigen::MatrixXi land_;
  Eigen::MatrixXf mean_;

 private:
  Eigen::Vector2f corner_min_;
  Eigen::Vector2f corner_max_;
  Eigen::MatrixXf variance_;
  Eigen::MatrixXi semantics_;
  Eigen::MatrixXi counter_;

  float grid_size_;
  float cell_size_;
  int grid_row_col_size_;
};
}


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2013 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "cotmatrix.h"
#include <vector>

// For error printing
#include <cstdio>
#include "cotmatrix_entries.h"

// Bug in unsupported/Eigen/SparseExtra needs iostream first
#include <iostream>

template <typename DerivedV, typename DerivedF, typename Scalar>
IGL_INLINE void igl::cotmatrix(
  const Eigen::MatrixBase<DerivedV> & V,
  const Eigen::MatrixBase<DerivedF> & F,
  Eigen::SparseMatrix<Scalar>& L)
{
  using namespace Eigen;
  using namespace std;

  L.resize(V.rows(),V.rows());
  Matrix<int,Dynamic,2> edges;
  int simplex_size = F.cols();
  // 3 for triangles, 4 for tets
  assert(simplex_size == 3 || simplex_size == 4);
  if(simplex_size == 3)
  {
    // This is important! it could decrease the comptuation time by a factor of 2
    // Laplacian for a closed 2d manifold mesh will have on average 7 entries per
    // row
    L.reserve(10*V.rows());
    edges.resize(3,2);
    edges <<
      1,2,
      2,0,
      0,1;
  }else if(simplex_size == 4)
  {
    L.reserve(17*V.rows());
    edges.resize(6,2);
    edges <<
      1,2,
      2,0,
      0,1,
      3,0,
      3,1,
      3,2;
  }else
  {
    return;
  }
  // Gather cotangents
  Matrix<Scalar,Dynamic,Dynamic> C;
  cotmatrix_entries(V,F,C);

  vector<Triplet<Scalar> > IJV;
  IJV.reserve(F.rows()*edges.rows()*4);
  // Loop over triangles
  for(int i = 0; i < F.rows(); i++)
  {
    // loop over edges of element
    for(int e = 0;e<edges.rows();e++)
    {
      int source = F(i,edges(e,0));
      int dest = F(i,edges(e,1));
      IJV.push_back(Triplet<Scalar>(source,dest,C(i,e)));
      IJV.push_back(Triplet<Scalar>(dest,source,C(i,e)));
      IJV.push_back(Triplet<Scalar>(source,source,-C(i,e)));
      IJV.push_back(Triplet<Scalar>(dest,dest,-C(i,e)));
    }
  }
  L.setFromTriplets(IJV.begin(),IJV.end());
}

#include "massmatrix.h"
#include "pinv.h"
#include "cotmatrix_entries.h"
#include "diag.h"
#include "massmatrix.h"
#include <Eigen/Geometry>

template <
  typename DerivedV,
  typename DerivedI,
  typename DerivedC,
  typename Scalar>
IGL_INLINE void igl::cotmatrix(
  const Eigen::MatrixBase<DerivedV> & V,
  const Eigen::MatrixBase<DerivedI> & I,
  const Eigen::MatrixBase<DerivedC> & C,
  Eigen::SparseMatrix<Scalar>& L,
  Eigen::SparseMatrix<Scalar>& M,
  Eigen::SparseMatrix<Scalar>& P)
{
  typedef Eigen::Matrix<Scalar,1,3> RowVector3S;
  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic> MatrixXS;
  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1> VectorXS;
  typedef Eigen::Index Index;
  // number of vertices
  const Index n = V.rows();
  // number of polyfaces
  const Index m = C.size()-1;
  assert(V.cols() == 2 || V.cols() == 3);
  std::vector<Eigen::Triplet<Scalar> > Lfijv;
  std::vector<Eigen::Triplet<Scalar> > Mfijv;
  std::vector<Eigen::Triplet<Scalar> > Pijv;
  // loop over vertices; set identity for original vertices
  for(Index i = 0;i<V.rows();i++) { Pijv.emplace_back(i,i,1); }
  // loop over faces
  for(Index p = 0;p<C.size()-1;p++)
  {
    // number of faces/vertices in this simple polygon
    const Index np = C(p+1)-C(p);
    // Working "local" list of vertices; last vertex is new one
    // this needs to have 3 columns so Eigen doesn't complain about cross
    // products below.
    Eigen::Matrix<Scalar,Eigen::Dynamic,3> X = decltype(X)::Zero(np+1,3);
    for(Index i = 0;i<np;i++){ X.row(i).head(V.cols()) = V.row(I(C(p)+i)); };
    // determine weights definig position of inserted vertex
    {
      MatrixXS A = decltype(A)::Zero(np+1,np);
      // My equation (38) would be A w = b.
      VectorXS b = decltype(b)::Zero(np+1);
      for(Index k = 0;k<np;k++)
      {
        const RowVector3S Xkp1mk = X.row((k+1)%np)-X.row(k);
        const RowVector3S Xkp1mkck = Xkp1mk.cross(X.row(k));
        for(Index i = 0;i<np;i++)
        {
          b(i) -= 2.*(X.row(i).cross(Xkp1mk)).dot(Xkp1mkck);
          for(Index j = 0;j<np;j++)
          {
            A(i,j) += 2.*(X.row(j).cross(Xkp1mk)).dot(X.row(i).cross(Xkp1mk));
          }
        }
      }
      A.row(np).setConstant(1);
      b(np) = 1;
      const VectorXS w =
        Eigen::CompleteOrthogonalDecomposition<Eigen::MatrixXd>(A).solve(b);
      X.row(np) = w.transpose()*X.topRows(np);
      // scatter w into new row of P
      for(Index i = 0;i<np;i++) { Pijv.emplace_back(n+p,I(C(p)+i),w(i)); }
    }
    // "local" fan of faces. These could be statically cached, but this will
    // not be the bottleneck.
    Eigen::MatrixXi F(np,3);
    for(Index i = 0;i<np;i++)
    {
      F(i,0) = i;
      F(i,1) = (i+1)%np;
      F(i,2) = np;
    }
    // Cotangent contributions
    MatrixXS K;
    igl::cotmatrix_entries(X,F,K);
    // Massmatrix entried
    VectorXS Mp;
    {
      Eigen::SparseMatrix<Scalar> M;
      igl::massmatrix(X,F,igl::MASSMATRIX_TYPE_DEFAULT,M);
      Mp = M.diagonal();
    }
    // Scatter into fine Laplacian and mass matrices
    const auto J = [&n,&np,&p,&I,&C](Index i)->Index{return i==np?n+p:I(C(p)+i);};
    // Should just build Mf as a vector...
    for(Index i = 0;i<np+1;i++) { Mfijv.emplace_back(J(i),J(i),Mp(i)); }
    // loop over faces
    for(Index f = 0;f<np;f++)
    {
      for(Index c = 0;c<3;c++)
      {
        const Index i = F(f,(c+1)%3);
        const Index j = F(f,(c+2)%3);
        // symmetric off-diagonal
        Lfijv.emplace_back(J(i),J(j),K(f,c));
        Lfijv.emplace_back(J(j),J(i),K(f,c));
        // diagonal
        Lfijv.emplace_back(J(i),J(i),-K(f,c));
        Lfijv.emplace_back(J(j),J(j),-K(f,c));
      }
    }
  }
  P.resize(n+m,n);
  P.setFromTriplets(Pijv.begin(),Pijv.end());
  Eigen::SparseMatrix<Scalar> Lf(n+m,n+m);
  Lf.setFromTriplets(Lfijv.begin(),Lfijv.end());
  Eigen::SparseMatrix<Scalar> Mf(n+m,n+m);
  Mf.setFromTriplets(Mfijv.begin(),Mfijv.end());
  L = P.transpose() * Lf * P;
  // "unlumped" M
  const Eigen::SparseMatrix<Scalar> PTMP = P.transpose() * Mf * P;
  // Lump M
  const VectorXS Mdiag = PTMP * VectorXS::Ones(n,1);
  igl::diag(Mdiag,M);

  MatrixXS Vf = P*V;
  Eigen::MatrixXi Ff(I.size(),3);
  {
    Index f = 0;
    for(Index p = 0;p<C.size()-1;p++)
    {
      const Index np = C(p+1)-C(p);
      for(Index c = 0;c<np;c++)
      {
        Ff(f,0) = I(C(p)+c);
        Ff(f,1) = I(C(p)+(c+1)%np);
        Ff(f,2) = V.rows()+p;
        f++;
      }
    }
    assert(f == Ff.rows());
  }
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
// generated by autoexplicit.sh
template void igl::cotmatrix<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, 1, 0, -1, 1>, Eigen::Matrix<int, -1, 1, 0, -1, 1>, double>(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&, Eigen::SparseMatrix<double, 0, int>&, Eigen::SparseMatrix<double, 0, int>&, Eigen::SparseMatrix<double, 0, int>&);
// generated by autoexplicit.sh
template void igl::cotmatrix<Eigen::Matrix<double, -1, -1, 1, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, double>(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 1, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::SparseMatrix<double, 0, int>&);
template void igl::cotmatrix<Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 4, 0, -1, 4>, double>(Eigen::MatrixBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 4, 0, -1, 4> > const&, Eigen::SparseMatrix<double, 0, int>&);
template void igl::cotmatrix<Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3>, double>(Eigen::MatrixBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> > const&, Eigen::SparseMatrix<double, 0, int>&);
template void igl::cotmatrix<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, double>(Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::SparseMatrix<double, 0, int>&);
#endif


#include <boost/math/special_functions/chebyshev.hpp>