/**

 *  \file   postprocessing.hh
 *  \brief  postprocessing.hh
 **/

#ifndef POSTPROCESSING_HH
#define POSTPROCESSING_HH

#include <cmake_config.h>

#include <dune/fem/operator/projection/l2projection.hh>
#include <dune/fem/io/file/vtkio.hh>
#include <dune/fem/io/file/datawriter.hh>
#include <dune/fem/misc/l2norm.hh>
#include <dune/fem/misc/l2error.hh>

#include <dune/stuff/common/logging.hh>
#include <dune/stuff/common/misc.hh>
#include <dune/stuff/common/filesystem.hh>
#include <dune/stuff/common/parameter/configcontainer.hh>
#include <dune/stuff/fem/customprojection.hh>
#include <dune/stuff/common/print.hh>
#include <dune/stuff/fem/functions/transform.hh>
#include <dune/stuff/fem/functions/integrals.hh>

#include <boost/format.hpp>
#include <cmath>
#include <sstream>
#include <tuple>

//! Error and vtk output wrapper class for Stokes problem/pass
template <  class OseenLDGMethodImp, class ProblemImp >
class PostProcessor
{
    public:
        typedef ProblemImp
            ProblemType;

        typedef OseenLDGMethodImp
            OseenLDGMethodType;
		typedef typename OseenLDGMethodType::Traits::DiscreteOseenFunctionSpaceWrapperType
            DiscreteOseenFunctionSpaceWrapperType;
		typedef typename OseenLDGMethodType::Traits::DiscreteOseenFunctionWrapperType
            DiscreteOseenFunctionWrapperType;

        typedef typename ProblemType::VelocityType
            ContinuousVelocityType;
        typedef typename ProblemType::PressureType
            ContinuousPressureType;
        typedef typename ProblemType::ForceType
            ForceType;
        typedef typename ProblemType::DirichletDataType
            DirichletDataType;

		typedef typename OseenLDGMethodType::Traits::GridPartType
            GridPartType;
        typedef typename GridPartType::GridType
            GridType;

        typedef Dune::SubsamplingVTKIO<GridPartType>
            VTKWriterType;

		typedef typename OseenLDGMethodType::Traits::DiscreteVelocityFunctionType
            DiscreteVelocityFunctionType;
		typedef typename OseenLDGMethodType::Traits::DiscreteVelocityFunctionSpaceType
            DiscreteVelocityFunctionSpaceType;

		typedef typename OseenLDGMethodType::Traits::DiscretePressureFunctionType
            DiscretePressureFunctionType;
		typedef typename OseenLDGMethodType::Traits::DiscretePressureFunctionSpaceType
            DiscretePressureFunctionSpaceType;

        PostProcessor( const DiscreteOseenFunctionSpaceWrapperType& wrapper, const ProblemType& prob )
            :
            problem_( prob ),
            spaceWrapper_( wrapper ),
            gridPart_( wrapper.gridPart() ),
            velocitySpace_ ( wrapper.discreteVelocitySpace() ),
            discreteExactVelocity_( "u_exact", wrapper.discreteVelocitySpace() ),
            discreteExactForce_( "f_exact", wrapper.discreteVelocitySpace() ),
            discreteExactDirichlet_( "gd_exact", wrapper.discreteVelocitySpace() ),
            discreteExactPressure_( "p_exact", wrapper.discretePressureSpace() ),
            errorFunc_velocity_( "err_velocity", wrapper.discreteVelocitySpace() ),
            errorFunc_pressure_( "err_pressure", wrapper.discretePressureSpace() ),
            solutionAssembled_( false ),
            current_refine_level_( std::numeric_limits<int>::min() ),
            l2_error_pressure_( - std::numeric_limits<double>::max() ),
            l2_error_velocity_( - std::numeric_limits<double>::max() ),
            vtkWriter_( wrapper.gridPart() ),
            datadir_( DSC_CONFIG_GET( "fem.io.datadir", std::string("data") ) + "/" )
        {
            DSC::testCreateDirectory( datadir_ );
        }

		/** \brief analytical data is L2 projected
			\todo only use DSC::CustomProjection when really necessary
			**/
        void assembleExactSolution()
        {
            DSFe::CustomProjection::project( problem_.dirichletData(), discreteExactDirichlet_ );

            typedef Dune::L2Projection< double, double, ContinuousVelocityType, DiscreteVelocityFunctionType > ProjectionV;
                ProjectionV projectionV;
            projectionV( problem_.velocity(), discreteExactVelocity_ );

            typedef Dune::L2Projection< double, double, ForceType, DiscreteVelocityFunctionType > ProjectionF;
                ProjectionF projectionF;
            projectionF( problem_.force(), discreteExactForce_ );

            typedef Dune::L2Projection< double, double, ContinuousPressureType, DiscretePressureFunctionType > ProjectionP;
                ProjectionP projectionP;
            projectionP( problem_.pressure(), discreteExactPressure_ );
			if ( DSC_CONFIG_GET( "save_matrices", false ) ) {
                auto& matlabLogStream = DSC_LOG_ERROR;
				DSC::printDiscreteFunctionMatlabStyle( discreteExactVelocity_, "u_exakt", matlabLogStream );
				DSC::printDiscreteFunctionMatlabStyle( discreteExactPressure_, "p_exakt", matlabLogStream );
			}
        }

		//! output function that 'knows' function output mode; assembles filename
        template <class Function>
        void vtk_write( const Function& f ) {
            if ( Function::FunctionSpaceType::DimRange > 1 ) {
                vtkWriter_.addVectorVertexData( f );
                vtkWriter_.addVectorCellData( f );
            }
            else {
                vtkWriter_.addVertexData( f );
                vtkWriter_.addCellData( f );
            }

            std::stringstream path;
            if ( DSC_CONFIG_GET( "per-run-output", false ) )
                path    << datadir_ << "/ref"
                        <<  current_refine_level_ << "_" << f.name();
            else
                path << datadir_ << "/" << f.name();

            vtkWriter_.write( path.str().c_str() );
            vtkWriter_.clear();
        }

		//! use this function if no reference (ie. coarser/finer) solution is available, or an analytical one is
        void save( const GridType& grid, const DiscreteOseenFunctionWrapperType& wrapper, int refine_level )
        {
            if ( ProblemType:: hasMeaningfulAnalyticalSolution ) {
                if ( !solutionAssembled_ || current_refine_level_ != refine_level ) //re-assemble solution if refine level has changed
                    assembleExactSolution();
                current_refine_level_ = refine_level;

				calcError( wrapper );
                vtk_write( discreteExactVelocity_ );
                vtk_write( discreteExactPressure_ );
                vtk_write( discreteExactForce_ );
                vtk_write( discreteExactDirichlet_ );
                vtk_write( errorFunc_pressure_ );
                vtk_write( errorFunc_velocity_ );
            }

            save_common( grid, wrapper, refine_level );
        }

		//! use this save in eoc runs with no analytical solution available
        void save( const GridType& grid, const DiscreteOseenFunctionWrapperType& wrapper, const DiscreteOseenFunctionWrapperType& reference, int refine_level )
        {
            current_refine_level_ = refine_level;
            calcError( wrapper, reference );
            vtk_write( discreteExactVelocity_ );
            vtk_write( discreteExactPressure_ );
            vtk_write( errorFunc_pressure_ );
            vtk_write( errorFunc_velocity_ );

            save_common( grid, wrapper, refine_level );
        }

		//! used by both PostProcessor::save modes, outputs solutions (in grape/vtk form), but no errors or analytical functions
        void save_common( const GridType& /*grid*/, const DiscreteOseenFunctionWrapperType& wrapper, int refine_level )
        {
            current_refine_level_ = refine_level;

            vtk_write( wrapper.discretePressure() );
            vtk_write( wrapper.discreteVelocity() );
#ifndef NLOG
			entityColoration();
#endif
        }

		void calcError( const DiscreteOseenFunctionWrapperType& wrapper )
		{
			calcError( wrapper.discretePressure() , wrapper.discreteVelocity() );
		}

		//! proxy function that is to be used if no analytical solutions are availble to calculate errors against
        void calcError( const DiscreteOseenFunctionWrapperType& computed, const DiscreteOseenFunctionWrapperType& reference )
        {
            discreteExactPressure_.assign( reference.discretePressure() );
            discreteExactVelocity_.assign( reference.discreteVelocity() );
			//set to to true so calcError call does not try to assemble exact solutions again
            solutionAssembled_ = true;
            calcError( computed.discretePressure(), computed.discreteVelocity() );
        }

		//! print and save L2 error(functions)
        void calcError( const DiscretePressureFunctionType& pressure, const DiscreteVelocityFunctionType& velocity )
        {
            if ( !solutionAssembled_ )
                assembleExactSolution();

            errorFunc_pressure_.assign( discreteExactPressure_ );
            errorFunc_pressure_ -= pressure;
            errorFunc_velocity_.assign( discreteExactVelocity_ );
            errorFunc_velocity_ -= velocity;

            Dune::L2Norm< GridPartType > l2_Error( gridPart_ );

            l2_error_pressure_ = l2_Error.norm( errorFunc_pressure_ );
            l2_error_velocity_ = l2_Error.norm( errorFunc_velocity_ );

            const double boundaryInt = DSFe::boundaryIntegral( problem_.dirichletData(), discreteExactVelocity_.space() );
            const double pressureMean = DSFe::integralAndVolume( pressure, pressure.space() ).first;
            const double exactPressureMean = DSFe::integralAndVolume( problem_.pressure(), discreteExactPressure_.space() ).first;

            DSC_LOG_INFO.resume();
            DSC_LOG_INFO << "L2-Error Pressure: " << std::setw(8) << l2_error_pressure_ << "\n"
                            << "L2-Error Velocity: " << std::setw(8) << l2_error_velocity_ << "\n"
							<< boost::format( "Pressure volume integral: %f (discrete), %f (exact)\n") % pressureMean % exactPressureMean
							<< boost::format( "g_D boundary integral: %f\n") % boundaryInt;
        }

		//! used to sore errors in runinfo structure (for eoc latex output)
        std::vector<double> getError()
        {
            std::vector<double> ret;
            ret.push_back( l2_error_velocity_ );
            ret.push_back( l2_error_pressure_ );
            return ret;
        }

		//! assign each entity it's 'id' int and save/(vtk)output it in a discrete function
        void entityColoration()
        {
            DiscretePressureFunctionType cl ( "entitiy-num", spaceWrapper_.discretePressureSpace() );
            unsigned long numberOfEntities = 0;

            typedef typename GridPartType::GridType::template Codim< 0 >::Entity
                EntityType;
            typedef typename GridPartType::template Codim< 0 >::IteratorType
                EntityIteratorType;
            typedef typename GridPartType::IntersectionIteratorType
                IntersectionIteratorType;

            EntityIteratorType entityItEndLog = velocitySpace_.end();
            for (   EntityIteratorType entityItLog = velocitySpace_.begin();
                    entityItLog != entityItEndLog;
                    ++entityItLog, ++numberOfEntities ) {
                const EntityType& entity = *entityItLog;
                typename DiscretePressureFunctionType::LocalFunctionType
                    lf = cl.localFunction( entity );

                for ( int i = 0; i < lf.numDofs(); ++i ){
                    lf[i] = numberOfEntities;
                }
            }
            vtk_write( cl );
        }

    private:

        const ProblemType& problem_;
        const DiscreteOseenFunctionSpaceWrapperType& spaceWrapper_;
        const GridPartType& gridPart_;
        const DiscreteVelocityFunctionSpaceType& velocitySpace_;
        DiscreteVelocityFunctionType discreteExactVelocity_;
        DiscreteVelocityFunctionType discreteExactForce_;
        DiscreteVelocityFunctionType discreteExactDirichlet_;
        DiscretePressureFunctionType discreteExactPressure_;
        DiscreteVelocityFunctionType errorFunc_velocity_;
        DiscretePressureFunctionType errorFunc_pressure_;
        bool solutionAssembled_;
        int current_refine_level_;
        double l2_error_pressure_;
        double l2_error_velocity_;
        VTKWriterType vtkWriter_;
        std::string datadir_;
};

#undef vtk_write

#endif // end of postprocessing.hh

/** Copyright (c) 2012, Rene Milk
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * The views and conclusions contained in the software and documentation are those
 * of the authors and should not be interpreted as representing official policies,
 * either expressed or implied, of the FreeBSD Project.
**/


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

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

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

#ifndef BOOST_GEOMETRY_GEOMETRY_POLICIES_RELATE_INTERSECTION_POINTS_HPP
#define BOOST_GEOMETRY_GEOMETRY_POLICIES_RELATE_INTERSECTION_POINTS_HPP

#include <string>

#include <boost/concept_check.hpp>
#include <boost/numeric/conversion/cast.hpp>

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/strategies/side_info.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>
#include <boost/geometry/util/select_most_precise.hpp>

namespace boost { namespace geometry
{

namespace policies { namespace relate
{

template <typename S1, typename S2, typename ReturnType, typename CalculationType = void>
struct segments_intersection_points
{
    typedef ReturnType return_type;
    typedef S1 segment_type1;
    typedef S2 segment_type2;
    typedef typename select_calculation_type
        <
            S1, S2, CalculationType
        >::type coordinate_type;

    // Get the same type, but at least a double (also used for divisions
    typedef typename select_most_precise
        <
            coordinate_type, double
        >::type promoted_type;

    template <int Dimension>
    static inline return_type rico(
                    coordinate_type const& dm1, coordinate_type const& dn1,
                    coordinate_type const& dm2, coordinate_type const& dn2,
                    S1 const& s1, S2 const& s2)
    {
        promoted_type const a1 = dn1 / dm1;
        promoted_type const a2 = dn2 / dm2;
        promoted_type const da = a1 - a2;

        if (math::equals(da, 0))
        {
            return rico<1 - Dimension>(dn1, dm1, dn2, dm2, s1, s2);
        }

        promoted_type const b1 = get<0, Dimension>(s1) - a1 * get<0, 1 - Dimension>(s1);
        promoted_type const b2 = get<0, Dimension>(s2) - a2 * get<0, 1 - Dimension>(s2);

        promoted_type const v = (b2 - b1) / da;

        return_type result;
        result.count = 1;
        set<1 - Dimension>(result.intersections[0],
            boost::numeric_cast<coordinate_type>(v));
        set<Dimension>(result.intersections[0],
            boost::numeric_cast<coordinate_type>(a1 * v + b1));
        return result;
    }

    static inline return_type cross(S1 const& s1, S2 const& s2)
    {
        // Take one of first segment, and one of second segment
        return_type result;
        result.count = 1;
        set<0>(result.intersections[0], get<0, 0>(s1));
        set<1>(result.intersections[0], get<0, 1>(s2));
        return result;
    }

    static inline return_type segments_intersect(side_info const& sides,
                    coordinate_type const& dx1, coordinate_type const& dy1,
                    coordinate_type const& dx2, coordinate_type const& dy2,
                    S1 const& s1, S2 const& s2)
    {
        bool vertical1 = math::equals(dx1, 0);
        bool horizontal2 = math::equals(dy2, 0);
        if (vertical1 && horizontal2)
        {
            return cross(s1, s2);
        }

        bool vertical2 = math::equals(dx2, 0);
        bool horizontal1 = math::equals(dy1, 0);
        if (horizontal1 && vertical2)
        {
            return cross(s2, s1);
        }
        if (vertical1 || vertical2)
        {
            return rico<0>(dy1, dx1, dy2, dx2, s1, s2);
        }
        else
        {
            // Not crossing, take the most reasonable choice.
            // We want to divide by the largest one.
            //if (

            return rico<1>(dx1, dy1, dx2, dy2, s1, s2);
        }
    }

    static inline return_type collinear_touch(coordinate_type const& x,
                coordinate_type const& y, bool, char)
    {
        return_type result;
        result.count = 1;
        set<0>(result.intersections[0], x);
        set<1>(result.intersections[0], y);
        return result;
    }

    template <typename S>
    static inline return_type collinear_inside(S const& s)
    {
        return_type result;
        result.count = 2;
        set<0>(result.intersections[0], get<0, 0>(s));
        set<1>(result.intersections[0], get<0, 1>(s));
        set<0>(result.intersections[1], get<1, 0>(s));
        set<1>(result.intersections[1], get<1, 1>(s));
        return result;
    }

    template <typename S>
    static inline return_type collinear_interior_boundary_intersect(S const& s, bool, bool)
    {
        return collinear_inside(s);
    }

    static inline return_type collinear_a_in_b(S1 const& s, bool)
    {
        return collinear_inside(s);
    }
    static inline return_type collinear_b_in_a(S2 const& s, bool)
    {
        return collinear_inside(s);
    }

    static inline return_type collinear_overlaps(
                coordinate_type const& x1, coordinate_type const& y1,
                coordinate_type const& x2, coordinate_type const& y2, bool)
    {
        return_type result;
        result.count = 2;
        set<0>(result.intersections[0], x1);
        set<1>(result.intersections[0], y1);
        set<0>(result.intersections[1], x2);
        set<1>(result.intersections[1], y2);
        return result;
    }

    static inline return_type segment_equal(S1 const& s, bool)
    {
        return_type result;
        result.count = 2;
        set<0>(result.intersections[0], get<0, 0>(s));
        set<1>(result.intersections[0], get<0, 1>(s));
        set<0>(result.intersections[1], get<1, 0>(s));
        set<1>(result.intersections[1], get<1, 1>(s));
        return result;
    }

    static inline return_type disjoint()
    {
        return return_type();
    }
    static inline return_type error(std::string const& msg)
    {
        return return_type();
    }

    static inline return_type collinear_disjoint()
    {
        return return_type();
    }
    static inline return_type parallel()
    {
        return return_type();
    }
    static inline return_type degenerate(S1 const& s, bool)
    {
        return_type result;
        result.count = 1;
        set<0>(result.intersections[0], get<0, 0>(s));
        set<1>(result.intersections[0], get<0, 1>(s));
        return result;
    }
};

}} // namespace policies::relate

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_GEOMETRY_POLICIES_RELATE_INTERSECTION_POINTS_HPP


#include "point_distributor.hpp"
#include "geometry.hpp"
#include "util.hpp"
#include "sweepline_state.hpp"

#include <boost/variant.hpp>

#include <iostream>
#include <iomanip>

/// Returns an assignment of points for the given tangents that each imply a facet
///
/// The sweep line algorithm only works if std::stable_sort is used, since the vertices are originally sorted by x-coordinate!
std::vector<point_distributor::point_assignment> point_distributor::operator()(unsigned i, const std::vector<shortcut>& tangents) const
{
    std::vector<point_distributor::point_assignment> assignments;

    const coordinate& origin = line.coordinates[i];
    auto slope_cmp = [&origin](const coordinate& lhs, const coordinate& rhs)
                     {
                          return geometry::slope_compare(origin, lhs, rhs);
                     };

    unsigned points_begin_idx = right_of_vertex_index[i];
    auto point_odering = util::compute_odering(point_coordinates.begin() + points_begin_idx, point_coordinates.end(),
                                               slope_cmp);

    unsigned vertex_begin_idx = i + 1;
    auto vertex_odering = util::compute_odering(line.coordinates.begin() + vertex_begin_idx, line.coordinates.end(),
                                                slope_cmp);

    auto num_vertices = vertex_odering.size();

    sweepline_state state(line.coordinates, i);

    using edge_assignment = std::pair<sweepline_state::edge, unsigned>;
    std::vector<edge_assignment> edge_assignments;

    auto point_vertex_compare =
        [this, &slope_cmp, points_begin_idx, vertex_begin_idx](const std::size_t lhs_idx, const std::size_t rhs_idx)
        {
            unsigned abs_lhs_idx = lhs_idx + points_begin_idx;
            unsigned abs_rhs_idx = rhs_idx + vertex_begin_idx;
            BOOST_ASSERT(abs_lhs_idx < point_coordinates.size());
            BOOST_ASSERT(abs_rhs_idx < line.coordinates.size());
            return slope_cmp(point_coordinates[abs_lhs_idx], line.coordinates[abs_rhs_idx]);
        };

    auto process_point =
        [this, points_begin_idx, &edge_assignments, &state](const std::size_t& point_idx)
        {
            state.move_sweepline(point_coordinates[points_begin_idx + point_idx]);

            BOOST_ASSERT(points_begin_idx + point_idx < point_coordinates.size());
            auto edge_iter = state.get_first_intersecting(point_coordinates[points_begin_idx + point_idx]);
            if (edge_iter != state.intersecting_edges.end())
            {
                edge_assignments.emplace_back(*edge_iter, point_idx);
            }
        };

    // We don't insert segments that are _on_ the sweep line.
    // This only works because we can be sure there is no point on a line segment.
    auto process_vertex =
        [this, num_vertices, vertex_begin_idx, &origin, &state](const std::size_t& vertex_idx)
        {
            state.move_sweepline(line.coordinates[vertex_begin_idx + vertex_idx]);

            // insert edge to the next vertex
            // ignores last vertex because there is no next vertex
            if (vertex_idx < num_vertices - 1)
            {
                // next edge goes down -> new to sweep line
                if (geometry::slope_compare(origin, line.coordinates[vertex_begin_idx + vertex_idx], line.coordinates[vertex_begin_idx + vertex_idx + 1]))
                {
                    state.insert_edge(sweepline_state::edge {vertex_begin_idx + vertex_idx, vertex_begin_idx + vertex_idx + 1});
                }
                // edge goes up -> does not intersect anymore
                else if (geometry::slope_compare(origin, line.coordinates[vertex_begin_idx + vertex_idx + 1], line.coordinates[vertex_begin_idx + vertex_idx]))
                {
                    state.remove_edge(sweepline_state::edge {vertex_begin_idx + vertex_idx, vertex_begin_idx + vertex_idx + 1});
                }
            }
            // insert edge to previous vertex
            // ignores the first and also the second vertices because edges to first vertex are always on the sweepline
            if (vertex_idx > 0)
            {
                // previous edge goes down -> new to sweep line
                if (geometry::slope_compare(origin, line.coordinates[vertex_begin_idx + vertex_idx], line.coordinates[vertex_begin_idx + vertex_idx - 1]))
                {
                    state.insert_edge(sweepline_state::edge {vertex_begin_idx + vertex_idx - 1, vertex_begin_idx + vertex_idx});
                }
                // edge goes up -> does not intersect anymore
                else if (geometry::slope_compare(origin, line.coordinates[vertex_begin_idx + vertex_idx - 1], line.coordinates[vertex_begin_idx + vertex_idx]))
                {
                    state.remove_edge(sweepline_state::edge {vertex_begin_idx + vertex_idx - 1, vertex_begin_idx + vertex_idx});
                }
            }
        };

    // this implements a rotating sweep line algorithm
    util::merge(point_odering.begin(), point_odering.end(),
                vertex_odering.begin(), vertex_odering.end(),
                point_vertex_compare,
                process_point,
                process_vertex);

    // sort assignments by index of first vertex of the edge
    // since these are path edge this will give us a sorting along the path
    std::sort(edge_assignments.begin(), edge_assignments.end(),
             [](const edge_assignment& lhs, const edge_assignment& rhs)
             {
                return lhs.first.first < rhs.first.first;
             });

    // we assume that the tangents are already sorted by the end vertex
    auto assignments_iter = edge_assignments.begin();
    for (auto tangent_idx = 0u; tangent_idx < tangents.size(); ++tangent_idx)
    {
        if (assignments_iter == edge_assignments.end())
        {
            break;
        }

        point_assignment tangent_assignment;
        bool has_assignment = false;

        // while the current edge lies in the interval on the path
        // implied by the tangent
        while (assignments_iter != edge_assignments.end() &&
               assignments_iter->first.second <= tangents[tangent_idx].last)
        {
            auto point_idx = assignments_iter->second;

            BOOST_ASSERT(assignments_iter->first.first >= tangents[tangent_idx].first);
            BOOST_ASSERT(points_begin_idx + point_idx< points.size());

            // only chose the point with minimal/maximal angle
            if (tangents[tangent_idx].classification == shortcut::type::MINIMAL_TANGENT)
            {
                // check if slope is smaller
                if (!has_assignment ||
                    slope_cmp(point_coordinates[points_begin_idx + point_idx], tangent_assignment.first.location))
                {
                    tangent_assignment = point_assignment(points[points_begin_idx + point_idx], tangent_idx);
                    has_assignment = true;
                }
            }
            else
            {
                BOOST_ASSERT(tangents[tangent_idx].classification == shortcut::type::MAXIMAL_TANGENT);

                // check if slope is bigger
                if (!has_assignment ||
                    slope_cmp(tangent_assignment.first.location, point_coordinates[points_begin_idx + point_idx]))
                {
                    tangent_assignment = point_assignment(points[points_begin_idx + point_idx], tangent_idx);
                    has_assignment = true;
                }
            }

            ++assignments_iter;
        }

        if (has_assignment)
        {
            assignments.push_back(tangent_assignment);
        }
    }

    return assignments;
}

/// Sorts points and builds up lookup array for each vertex
/// to determine which points are right of it
void point_distributor::prepare_points(std::vector<point>& points,
                                       std::vector<unsigned>& right_of_vertex_index,
                                       std::vector<coordinate>& point_coordinates) const
{
    // sort points by x coordinate
    std::sort(points.begin(), points.end(),
              [](const point& lhs, const point& rhs)
              {
                return lhs.location.x < rhs.location.x;
              });

    // extract coordinates
    point_coordinates.resize(points.size());
    std::transform(points.begin(), points.end(), point_coordinates.begin(),
                   [](const point& p)
                   {
                     return p.location;
                   });

    right_of_vertex_index.resize(line.coordinates.size());
    for (unsigned points_idx = 0, vertex_idx = 0;
        vertex_idx < line.coordinates.size();)
    {
        if (points_idx < points.size() &&
            points[points_idx].location.x < line.coordinates[vertex_idx].x)
        {
            points_idx++;
            continue;
        }
        right_of_vertex_index[vertex_idx] = points_idx;
        vertex_idx++;
    }
}


#pragma once

#include <Eigen/Dense>

#include <merely3d/material.hpp>
#include <merely3d/types.hpp>

namespace merely3d
{
    template <typename Shape>
    struct Renderable
    {
        Shape                   shape;
        Eigen::Vector3f         position;
        UnalignedQuaternionf    orientation;
        Eigen::Vector3f         scale;
        Material                material;

        Renderable(Shape shape)
                : shape(std::move(shape)),
                  position(Eigen::Vector3f::Zero()),
                  orientation(Eigen::Quaternionf::Identity()),
                  scale(Eigen::Vector3f(1.0f, 1.0f, 1.0f))
        {}

        Renderable(Shape shape,
                   const Eigen::Vector3f & position,
                   const Eigen::Quaternionf & orientation,
                   const Eigen::Vector3f & scale,
                   Material material)
                : shape(std::move(shape)),
                  position(position),
                  orientation(orientation),
                  scale(scale),
                  material(material)
        {}

        template <typename OtherShape>
        Renderable<OtherShape> with_shape(OtherShape shape)
        {
            auto result = Renderable<OtherShape>(std::move(shape));
            result.position = position;
            result.orientation = orientation;
            result.scale = scale;
            result.material = material;
            return result;
        }

        Renderable<Shape> with_position(const Eigen::Vector3f & position)
        {
            auto result = *this;
            result.position = position;
            return result;
        }

        Renderable<Shape> with_position(float x, float y, float z)
        {
            return with_position(Eigen::Vector3f(x, y, z));
        }

        template <typename IntoOrientation>
        Renderable<Shape> with_orientation(const IntoOrientation & orientation)
        {
            auto result = *this;
            result.orientation = orientation;
            return result;
        }

        Renderable<Shape> with_scale(const Eigen::Vector3f & scale)
        {
            auto result = *this;
            result.scale = scale;
            return result;
        }

        Renderable<Shape> with_scale(float x_scale, float y_scale, float z_scale)
        {
            return with_scale(Eigen::Vector3f(x_scale, y_scale, z_scale));
        }

        Renderable<Shape> with_uniform_scale(float scale)
        {
            return with_scale(scale, scale, scale);
        }

        Renderable<Shape> with_material(const Material & material)
        {
            auto result = *this;
            result.material = material;
            return result;
        }
    };

    template <typename Shape>
    inline Renderable<Shape> renderable(Shape shape)
    {
        return Renderable<Shape>(std::move(shape));
    }
}


#pragma once

#include <boost/functional/hash.hpp>
#include <chrono>
#include <cstdlib>
#include <ctime>
#include <functional>
#include <random>
#include <vector>

namespace ulid {

/**
 * ULID is a 16 byte Universally Unique Lexicographically Sortable Identifier
 * */
typedef __uint128_t ULID;

/* Added by Alec */
struct ULIDHasher {
  std::size_t operator()(const ULID &t) const {
    // Start with a hash value of 0    .
    std::size_t seed = 0;

    // Modify 'seed' by XORing and bit-shifting in
    // one member of 'Key' after the other:
    boost::hash_combine(seed, boost::hash_value(t));

    // Return the result.
    return seed;
  }
};

/**
 * EncodeTime will encode the first 6 bytes of a uint8_t array to the passed
 * timestamp
 * */
void EncodeTime(time_t timestamp, ULID &ulid);

/**
 * EncodeTimeNow will encode a ULID using the time obtained using
 * std::time(nullptr)
 * */
void EncodeTimeNow(ULID &ulid);

/**
 * EncodeTimeSystemClockNow will encode a ULID using the time obtained using
 * std::chrono::system_clock::now() by taking the timestamp in milliseconds.
 * */
void EncodeTimeSystemClockNow(ULID &ulid);

/**
 * EncodeEntropy will encode the last 10 bytes of the passed uint8_t array with
 * the values generated using the passed random number generator.
 * */
void EncodeEntropy(const std::function<uint8_t()> &rng, ULID &ulid);

/**
 * EncodeEntropyRand will encode a ulid using std::rand
 *
 * std::rand returns values in [0, RAND_MAX]
 * */
void EncodeEntropyRand(ULID &ulid);

extern std::uniform_int_distribution<uint8_t> Distribution_0_255;

/**
 * EncodeEntropyMt19937 will encode a ulid using std::mt19937
 *
 * It also creates a std::uniform_int_distribution to generate values in [0,
 * 255]
 * */
void EncodeEntropyMt19937(std::mt19937 &generator, ULID &ulid);

/**
 * Encode will create an encoded ULID with a timestamp and a generator.
 * */
void Encode(time_t timestamp, const std::function<uint8_t()> &rng, ULID &ulid);

/**
 * EncodeNowRand = EncodeTimeNow + EncodeEntropyRand.
 * */
void EncodeNowRand(ULID &ulid);

/**
 * Create will create a ULID with a timestamp and a generator.
 * */
ULID Create(time_t timestamp, const std::function<uint8_t()> &rng);

/**
 * CreateNowRand:EncodeNowRand = Create:Encode.
 * */
ULID CreateNowRand();

/**
 * Crockford's Base32
 * */
extern const char Encoding[33];

/**
 * MarshalTo will marshal a ULID to the passed character array.
 *
 * Implementation taken directly from oklog/ulid
 * (https://sourcegraph.com/github.com/oklog/ulid@0774f81f6e44af5ce5e91c8d7d76cf710e889ebb/-/blob/ulid.go#L162-190)
 *
 * timestamp:
 * dst[0]: first 3 bits of data[0]
 * dst[1]: last 5 bits of data[0]
 * dst[2]: first 5 bits of data[1]
 * dst[3]: last 3 bits of data[1] + first 2 bits of data[2]
 * dst[4]: bits 3-7 of data[2]
 * dst[5]: last bit of data[2] + first 4 bits of data[3]
 * dst[6]: last 4 bits of data[3] + first bit of data[4]
 * dst[7]: bits 2-6 of data[4]
 * dst[8]: last 2 bits of data[4] + first 3 bits of data[5]
 * dst[9]: last 5 bits of data[5]
 *
 * entropy:
 * follows similarly, except now all components are set to 5 bits.
 * */
void MarshalTo(const ULID &ulid, char dst[26]);

/**
 * Marshal will marshal a ULID to a std::string.
 * */
std::string Marshal(const ULID &ulid);

/**
 * MarshalBinaryTo will Marshal a ULID to the passed byte array
 * */
void MarshalBinaryTo(const ULID &ulid, uint8_t dst[16]);

/**
 * MarshalBinary will Marshal a ULID to a byte vector.
 * */
std::vector<uint8_t> MarshalBinary(const ULID &ulid);

/**
 * dec storesdecimal encodings for characters.
 * 0xFF indicates invalid character.
 * 48-57 are digits.
 * 65-90 are capital alphabets.
 * */
extern const uint8_t dec[256];

/**
 * UnmarshalFrom will unmarshal a ULID from the passed character array.
 * */
void UnmarshalFrom(const char str[26], ULID &ulid);

/**
 * Unmarshal will create a new ULID by unmarshaling the passed string.
 * */
ULID Unmarshal(const std::string &str);

/**
 * UnmarshalBinaryFrom will unmarshal a ULID from the passed byte array.
 * */
void UnmarshalBinaryFrom(const uint8_t b[16], ULID &ulid);

/**
 * Unmarshal will create a new ULID by unmarshaling the passed byte vector.
 * */
ULID UnmarshalBinary(const std::vector<uint8_t> &b);

/**
 * CompareULIDs will compare two ULIDs.
 * returns:
 *     -1 if ulid1 is Lexicographically before ulid2
 *      1 if ulid1 is Lexicographically after ulid2
 *      0 if ulid1 is same as ulid2
 * */
int CompareULIDs(const ULID &ulid1, const ULID &ulid2);

/**
 * Time will extract the timestamp used to generate a ULID
 * */
time_t Time(const ULID &ulid);

// Added by Alec Wang.
bool validate_strict(const std::string &v);

// Added by Alec Wang.
bool validate_weak(const std::string &v);

// Added by Alec Wang.
bool Validate(const std::string &str, bool strict);

};  // namespace ulid


#include <alps/scheduler.h>
#include <alps/alea/detailedbinning.h>

#include <boost/python.hpp>
#include <boost/python/extract.hpp>
#include <numpy/arrayobject.h>
#include <string>

#include <iostream>

PyObject * get_raw_data(boost::python::str &path, const int &measure, PyObject *obs_str_list)
{
    import_array1(NULL);
    char *s = boost::python::extract<char *>(path);
    std::string str_parms = std::string(s) + ".task1.out.xml";
    alps::scheduler::SimpleMCFactory<alps::scheduler::DummyMCRun> factory;
    alps::scheduler::init(factory);
    boost::filesystem::path p(str_parms.c_str(), boost::filesystem::native);
    alps::ProcessList nowhere;
    alps::scheduler::MCSimulation sim(nowhere,p);

    alps::RealVectorObsevaluator G=sim.get_measurements()["Greens"];
    int N = int(sim.get_parameters()["N"]),
        FLAVORS = int(sim.get_parameters()["SPINS"])*int(sim.get_parameters()["SITES"]);
    alps::RealVectorObsevaluator n=sim.get_measurements()["n"];
    alps::RealObsevaluator sign=sim.get_measurements()["Sign"];

    npy_intp dim[2] = {N+1, FLAVORS};
    PyArrayObject *Gtau = (PyArrayObject *) PyArray_SimpleNew(2, dim, NPY_DOUBLE),
                  *Gerr = (PyArrayObject *) PyArray_SimpleNew(2, dim, NPY_DOUBLE);
    Py_INCREF(Gtau); Py_INCREF(Gerr);
    double *buf = (double *)PyArray_DATA(Gtau),
           *buf_err = (double *)PyArray_DATA(Gerr);

    for (int j=0; j<FLAVORS; j++) {
        buf[j] =  -(1-n.mean()[j]);
        buf_err[j] = n.error()[j];
    }
    for (int i=1; i<N; i++)
        for (int j=0; j<FLAVORS; j++) {
            buf[j + i*FLAVORS] = -(G.mean()[j*(N+1)+i]);
            buf_err[j + i*FLAVORS] = G.error()[j*(N+1)+i];
        }
    for (int j=0; j<FLAVORS; j++) {
        buf[j + N*FLAVORS] =  -(n.mean()[j]);
        buf_err[j + N*FLAVORS] =  n.error()[j];
    }

    // measure other observables
    PyObject *obs = PyDict_New();

    npy_intp sign_size = 1;
    PyArrayObject *sign_array = (PyArrayObject *) PyArray_SimpleNew(1, &sign_size, NPY_DOUBLE);
    double *sign_buffer = (double *)PyArray_DATA(sign_array);
    sign_buffer[0] = sign.mean();
    PyDict_SetItemString(obs, "sign", PyArray_Return(sign_array));
    Py_DECREF(sign_array);

    if (measure > 0) {
        int Nlist = PyList_Size(obs_str_list);
        for (int i = 0; i < Nlist; ++i) {
            alps::RealVectorObsevaluator o = sim.get_measurements()[PyString_AsString(PyList_GetItem(obs_str_list, i))];
            npy_intp o_size = o.mean().size();
            PyArrayObject *tmp = (PyArrayObject *) PyArray_SimpleNew(1, &o_size, NPY_DOUBLE);
            double *buffer = (double *)PyArray_DATA(tmp);
            for (int j = 0; j < o_size; ++j)
                buffer[j] = o.mean()[j];
            PyDict_SetItem(obs, PyList_GetItem(obs_str_list, i), PyArray_Return(tmp));
            Py_DECREF(tmp);
        }
    }
    PyObject *t = PyTuple_New(3);
    PyTuple_SetItem(t, 0, PyArray_Return(Gtau));
    PyTuple_SetItem(t, 1, PyArray_Return(Gerr));
    PyTuple_SetItem(t, 2, obs);
    return t;
}


#include <glog/logging.h>

#include <ros/ros.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <geometry_msgs/PoseStamped.h>

#include "GPS_AHRS_sync.h"
#include "ndt_algo.h"
#include "Timer.h"

#include <Eigen/Geometry>

class GPS_AHRS_Publisher;
void gps_ahrs_sync_callback_wrapper(const sensor_msgs::NavSatFixConstPtr& gps_msg, const nav_msgs::OdometryConstPtr& odom);

GPS_AHRS_Publisher* pPublisher;
class GPS_AHRS_Publisher
{
public:

    GPS_AHRS_Synchronizer gas;
    NDTAlgo* p_algo;
    ros::Publisher gps_ahrs_pose_pub;
    std::shared_ptr<ros::NodeHandle> pNH;

    void init_node(int argc,char** argv)
    {
        ros::init(argc,argv,"gps_ahrs_publisher_node");
        this->pNH = std::shared_ptr<ros::NodeHandle>(new ros::NodeHandle);
        this->gps_ahrs_pose_pub = this->pNH->advertise<geometry_msgs::PoseStamped>("/gaas/localization/gps_ahrs_original",1);

        //load gps info.
        this->p_algo = new NDTAlgo(&this->gas);
        p_algo->loadPCDMap();
        LOG(ERROR)<<"[original gps ahrs publisher] PCD Map loaded!"<<endl;

        message_filters::Subscriber<sensor_msgs::NavSatFix> gps_sub(*pNH, "/mavros/global_position/raw/fix", 1);
        //message_filters::Subscriber<sensor_msgs::NavSatFix> gps_sub(nh, "/mavros/global_position/global", 1);
        message_filters::Subscriber<nav_msgs::Odometry> ahrs_sub(*pNH, "/mavros/local_position/odom", 1);
        typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::NavSatFix, nav_msgs::Odometry> MySyncPolicy;
        // ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
        message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(3), gps_sub, ahrs_sub);
        sync.registerCallback(boost::bind(&gps_ahrs_sync_callback_wrapper, _1, _2));
        while(ros::ok())
        {
            ros::spinOnce();
        }
    }
    void gps_ahrs_sync_callback(const sensor_msgs::NavSatFixConstPtr& gps_msg, const nav_msgs::OdometryConstPtr& odom)// 同步gps和ahrs.
    {
        LOG(ERROR)<<"Enter GPS_AHRS_CALLBACK!!!"<<endl;
        const sensor_msgs::NavSatFix& gps = *gps_msg;
        const nav_msgs::Odometry& ahrs = *odom;
        Eigen::Matrix4f gps_ahrs_initial_guess;
        {
            double x,y,z;
            this->p_algo->map_gps_info.getRelativeXYZFromLonLatAltInNWUCoordinate(gps.longitude,gps.latitude,gps.altitude,x,y,z);
            LOG(INFO)<<"GPS relative XYZ:"<<x<<";"<<y<<";"<<z<<endl;
            auto orient = ahrs.pose.pose.orientation;
            Eigen::Quaternionf original;
            original.x() = orient.x;
            original.y() = orient.y;
            original.z() = orient.z;
            original.w() = orient.w;

            //P_nwu = T_nwu_enu*T_enu_body*P_body
            Eigen::Matrix3f R_nwu_enu;
            R_nwu_enu<<0,1,0, -1,0,0 ,0,0,1; //plane right.
            Eigen::Matrix3f NWU_R = (R_nwu_enu*original.toRotationMatrix());

            Eigen::Quaternionf NWU_orient(NWU_R);
            LOG(INFO)<<"NWU coord quaternion initial guess = "<<NWU_orient.x()<<","<<NWU_orient.y()<<","<<NWU_orient.z()<<","<<NWU_orient.w()<<endl;

            gps_ahrs_initial_guess.block(0,0,3,3) = NWU_R;
            Eigen::Vector3f xyz_(x,y,z);

            gps_ahrs_initial_guess(0,3) = xyz_(0);
            gps_ahrs_initial_guess(1,3) = xyz_(1);
            gps_ahrs_initial_guess(2,3) = xyz_(2) - this->p_algo->lidar_height_compensation;
            gps_ahrs_initial_guess(3,3) = 1;

        }

        {
            Eigen::Matrix3f rotmat = gps_ahrs_initial_guess.block(0,0,3,3);
            Eigen::Quaternionf quat(rotmat);
            geometry_msgs::PoseStamped ndt_pose_msg;

            ndt_pose_msg.header.frame_id="map";//地图坐标系的定位.
            ndt_pose_msg.header.stamp = gps.header.stamp;//与gps同步.

            auto& pos = ndt_pose_msg.pose.position;
            auto& orient = ndt_pose_msg.pose.orientation;
            orient.x = quat.x();
            orient.y = quat.y();
            orient.z = quat.z();
            orient.w = quat.w();

            pos.x = gps_ahrs_initial_guess(0,3);
            pos.y = gps_ahrs_initial_guess(1,3);
            pos.z = gps_ahrs_initial_guess(2,3);

            this->gps_ahrs_pose_pub.publish(ndt_pose_msg);
            LOG(INFO)<<"GPS AHRS pose published!"<<endl;
        }
    }
};
void gps_ahrs_sync_callback_wrapper(const sensor_msgs::NavSatFixConstPtr& gps_msg, const nav_msgs::OdometryConstPtr& odom)
{
    pPublisher->gps_ahrs_sync_callback(gps_msg,odom);
}

int main(int argc,char** argv)
{
//    FLAGS_alsologtostderr = 1;
//    google::InitGoogleLogging("gps_ahrs_publisher_node");
//    GPS_AHRS_Publisher gap;
//    gap.init_node(argc,argv);
    return 0;
}


/*
 * eigen_types.hpp
 *
 *  Created on: March 6, 2009
 *      Author: benoit bolsee
 */

#ifndef EIGEN_TYPES_HPP_
#define EIGEN_TYPES_HPP_

#include <Eigen/Core>
#include "kdl/frames.hpp"
#include "kdl/tree.hpp"
#include "kdl/chain.hpp"
#include "kdl/jacobian.hpp"
#include "kdl/jntarray.hpp"

namespace iTaSC{

using KDL::Twist;
using KDL::Frame;
using KDL::Joint;
using KDL::Inertia;
using KDL::SegmentMap;
using KDL::Tree;
using KDL::JntArray;
using KDL::Jacobian;
using KDL::Segment;
using KDL::Rotation;
using KDL::Vector;
using KDL::Vector2;
using KDL::Chain;

extern const Frame F_identity;

#define e_scalar double
#define e_vector Eigen::Matrix<e_scalar, Eigen::Dynamic, 1>
#define e_zero_vector Eigen::Matrix<e_scalar, Eigen::Dynamic, 1>::Zero
#define e_matrix Eigen::Matrix<e_scalar, Eigen::Dynamic, Eigen::Dynamic>
#define e_matrix6 Eigen::Matrix<e_scalar,6,6>
#define e_identity_matrix Eigen::Matrix<e_scalar, Eigen::Dynamic, Eigen::Dynamic>::Identity
#define e_scalar_vector Eigen::Matrix<e_scalar, Eigen::Dynamic, 1>::Constant
#define e_zero_matrix Eigen::Matrix<e_scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero
#define e_random_matrix Eigen::Matrix<e_scalar, Eigen::Dynamic, Eigen::Dynamic>::Random
#define e_vector6 Eigen::Matrix<e_scalar,6,1>
#define e_vector3 Eigen::Matrix<e_scalar,3,1>

class Range {
public:
	int start;
	int count;
	Range(int _start, int _count) { start = _start; count=_count; }
	Range(const Range& other) { start=other.start; count=other.count; }
};

template<typename MatrixType> inline Eigen::Block<MatrixType> project(MatrixType& m, Range r)
{
	return Eigen::Block<MatrixType>(m,r.start,0,r.count,1);
}

template<typename MatrixType> inline Eigen::Block<MatrixType> project(MatrixType& m, Range r, Range c)
{
	return Eigen::Block<MatrixType>(m,r.start,c.start,r.count,c.count);
}

template<typename Derived> inline static int changeBase(Eigen::MatrixBase<Derived>& J, const Frame& T) {

    if (J.rows() != 6)
        return -1;
    for (int j = 0; j < J.cols(); ++j) {
		typename Derived::ColXpr Jj = J.col(j);
		Twist arg;
        for(unsigned int i=0;i<6;++i)
            arg(i)=Jj[i];
		Twist tmp(T*arg);
        for(unsigned int i=0;i<6;++i)
            Jj[i]=e_scalar(tmp(i));
    }
    return 0;
}

}
#endif /* UBLAS_TYPES_HPP_ */


// STL includes
#include <iostream>
#include <iomanip>
#include <fstream>
#include <vector>
#include <random>
#include <utility>
#include <thread>

// Local includes
#include <tests/Tests.hpp>
#include <tests/SH.hpp>
#include <include/Utils.hpp>
#include <include/SphericalHarmonics.hpp>
#include <include/DirectionsSampling.hpp>
#include <include/SphericalIntegration.hpp>

// GLM include
#include <glm/glm.hpp>

// Include Eigen
#include <Eigen/Core>

// ALTA includes
#include <core/params.h>
#include <core/function.h>
#include <core/data.h>
#include <core/plugins_manager.h>

struct AltaBRDF {

   alta::ptr<alta::data>     _d;
   alta::ptr<alta::function> _f;

   AltaBRDF(const std::string& filename,
            const std::string& plugin,
            const std::string& plugin_type) {
      // Load the ALTA material
      if(plugin_type == "data") {
         _d = alta::ptr<alta::data>(alta::plugins_manager::get_data(plugin));
         _d->load(filename);
      } else {
         _f = alta::ptr<alta::function>(alta::plugins_manager::load_function(filename));
      }

      // Exception when a ALTA material cannot be loaded.
      if(!_f && !_d) {
         throw 1;
      }
   }

   AltaBRDF(const alta::ptr<alta::data>& d,
            const alta::ptr<alta::function>& f) : _d(d), _f(f) {}

   template<class RGB, class Vector>
   RGB eval(const Vector &wo, const Vector &wi) const {
      if (wi.z <= 0 || wo.z <= 0) {
         return RGB(0.0f, 0.0f, 0.0f);
      }

      double cart[6];
      cart[0] = wi[0];
      cart[1] = wi[1];
      cart[2] = wi[2];
      cart[3] = wo[0];
      cart[4] = wo[1];
      cart[5] = wo[2];

      /* Return the value of the BRDF from the function object */
      if(!_d) {
         vec x(_f->dimX());
         alta::params::convert(&cart[0], alta::params::CARTESIAN, _f->input_parametrization(), &x[0]);
         vec y = _f->value(x);
         RGB res;
         if(_f->dimY() == 3) {
            res = RGB(std::max(y[0], 0.0), std::max(y[1], 0.0), std::max(y[2], 0.0));
         } else {
            const auto ym = std::max(y[0], 0.0);
            res = RGB(ym, ym, ym);
         }
         return res;

         /* Treat the case of a BRDF from interpolated data */
      } else {
         vec x(_d->dimX());
         alta::params::convert(&cart[0], alta::params::CARTESIAN, _d->input_parametrization(), &x[0]);

         vec y = _d->value(x);
         RGB res;
         if(_d->dimY() == 3) {
            res = RGB(std::max(y[0], 0.0), std::max(y[1], 0.0), std::max(y[2], 0.0));
         } else {
            const auto ym = std::max(y[0], 0.0);
            res = RGB(ym, ym, ym);
         }
         return res;
      }
   }

   template<class RGB, class Vector>
   RGB value(const Vector &wo, const Vector &wi) const {
#ifdef FORCE_BILATERAL_SYMMETRY
      return 0.5*(eval<RGB, Vector>(wo, wi) + eval<RGB, Vector>(wi, wo));
#else
      return eval<RGB, Vector>(wo, wi);
#endif
   }
};

struct AltaProjectionThread : public std::thread {

   std::vector<Eigen::MatrixXf> cijs;

   AltaProjectionThread(const AltaBRDF* brdf,
                        const std::vector<Vector>* ws,
                        int _order, int _skip, int _nthread) :
      std::thread(&AltaProjectionThread::run, this, brdf, ws, _skip, _order, _nthread) {}

   void run(const AltaBRDF* brdf,
            const std::vector<Vector>* dirs,
            int skip, int order, int nthread) {

      // Allocate memory
      cijs = std::vector<Eigen::MatrixXf>(6, Eigen::MatrixXf::Zero(SH::Terms(order), SH::Terms(order)));

      const int size = SH::Terms(order);
      Eigen::VectorXf ylmo(size);
      Eigen::VectorXf ylmi(size);
      for(unsigned int i=skip; i<dirs->size(); i+=nthread) {
         const Vector& wo = (*dirs)[i];

         if(skip == 0) {
            std::cout << "Progress: " << i << " / " << dirs->size() << "     \r";
            std::cout.flush();
         }

         // Skip below the horizon configuration
         if(wo.z < 0.0) continue;
         SH::FastBasis(wo, order, ylmo);

         for(unsigned int j=0; j<dirs->size(); ++j) {
            const Vector& wi = (*dirs)[j];
            // Skip below the horizon configuration
            if(wi.z < 0.0) continue;

            // Evaluate the BRDF value
            const auto rgb = brdf->value<Vector, Vector>(wi, wo);
            SH::FastBasis(wi, order, ylmi);

            Eigen::MatrixXf mat = ylmo * ylmi.transpose();
#ifndef SYMMETRIZE
            mat = 0.5f*(mat + mat.transpose());
#endif
            cijs[0] += rgb[0] * mat;
            cijs[1] += rgb[1] * mat;
            cijs[2] += rgb[2] * mat;
            // Note: Here the correct weighting should be with respect to wi.z
            // but I use wo.z since it allows to reduce the ringing drastically.
#ifdef LOOKS_BETTER
            cijs[3] += rgb[0] * wo.z * mat;
            cijs[4] += rgb[1] * wo.z * mat;
            cijs[5] += rgb[2] * wo.z * mat;
#else // CORRECT
            cijs[3] += rgb[0] * wi.z * mat;
            cijs[4] += rgb[1] * wi.z * mat;
            cijs[5] += rgb[2] * wi.z * mat;
#endif
         }
      }
   }
};

int AltaProjectionMatrix(const std::string& filename,
                         const std::string& plugin,
                         const std::string& type,
                         int order = 15, int N = 100000) {

   // Constants
   const int size = SH::Terms(order);

   // Load the BRDF
   AltaBRDF brdf(filename, plugin, type);

   const auto k = filename.rfind('.');
   std::string ofilename = filename;
   ofilename.replace(k, std::string::npos, ".mats");
   std::cout << "Will output to \"" << ofilename << "\"" << std::endl;

   // Values
   std::vector<Eigen::MatrixXf> cijs(6, Eigen::MatrixXf::Zero(size, size));
   const auto dirs = SamplingFibonacci<Vector>(N);

   const int nbthreads = std::thread::hardware_concurrency();
   std::vector<AltaProjectionThread*> threads;
   for(int k=0; k<nbthreads; ++k) {
      AltaProjectionThread* th = new AltaProjectionThread(&brdf, &dirs, order, k, nbthreads);
      threads.push_back(th);
   }

   for(AltaProjectionThread* th : threads) {
      th->join();
      cijs[0] += th->cijs[0];
      cijs[1] += th->cijs[1];
      cijs[2] += th->cijs[2];
      cijs[3] += th->cijs[3];
      cijs[4] += th->cijs[4];
      cijs[5] += th->cijs[5];
      delete th;
   }
   const float factor = 16.0*M_PI*M_PI / float(N*N);
   cijs[0] *= factor;
   cijs[1] *= factor;
   cijs[2] *= factor;
   cijs[3] *= factor;
   cijs[4] *= factor;
   cijs[5] *= factor;

   SaveMatrices(ofilename, cijs);

   // Print values
   std::string gfilename = filename;
   gfilename.replace(k, std::string::npos, ".gnuplot");
   std::ofstream file(gfilename.c_str(), std::ios_base::trunc);
   const float thetai = -0.5f*M_PI * 30.f/90.f;
   const Vector wi(sin(thetai), 0, cos(thetai));
   const auto ylmi = SH::FastBasis(wi, order);
   Vector wo;
   for(int i=0; i<90; ++i) {
      const float theta = 0.5*M_PI * i / float(90);
      wo.x = sin(theta);
      wo.y = 0;
      wo.z = cos(theta);

      // Ref
      const auto RGB = brdf.value<Vector, Vector>(wi, wo);
      const float R = RGB[0];
      const float G = RGB[1];
      const float B = RGB[2];

      // SH expansion
      const auto ylmo = SH::FastBasis(wo, order);
      const Eigen::VectorXf rlm = cijs[0] * ylmo;
      const Eigen::VectorXf glm = cijs[1] * ylmo;
      const Eigen::VectorXf blm = cijs[2] * ylmo;
      const float r = ylmi.dot(rlm);
      const float g = ylmi.dot(glm);
      const float b = ylmi.dot(blm);

      file << theta << "\t" << r << "\t" << g << "\t" << b
         << "\t" << R << "\t" << G << "\t" << B
         << std::endl;
   }

   int nb_fails = 0;
   return nb_fails;
}

bool parseArguments(int argc, char** argv, std::string& filename,
      std::string& plugin, std::string& type, int& order, int& nb) {

   // Loop over all the different elements of the command line and search for
   // some patterns
   for(int k=0; k<argc; ++k) {
      if(argv[k] == std::string("-h") || argv[k] == std::string("--help")) {
         std::cerr << "Usage: alta2sh [options] [-p plugin_name] [-t {data|func}] filename" << std::endl;
         std::cerr << "       + -o  [int] maxiumum SH order (default = 3)" << std::endl;
         std::cerr << "       + -nb [int] number of integration samples (default = 10000)" << std::endl;
         return false;
      }
      if((argv[k] == std::string( "-o") || argv[k] == std::string("--order")) && k+1<argc) {
         order = std::atoi(argv[k+1]);
      }
      if((argv[k] == std::string( "-n") || argv[k] == std::string("--nb")) && k+1<argc) {
         nb = std::atoi(argv[k+1]);
      }
      if((argv[k] == std::string( "-p") || argv[k] == std::string("--plugin")) && k+1<argc) {
         plugin = argv[k+1];
      }
      if((argv[k] == std::string( "-t") || argv[k] == std::string("--type")) && k+1<argc) {
         type = argv[k+1];
      }
   }

   // The filename to convert is the last argument of the command line.
   if(argc > 1) {
      filename = argv[argc-1];
      return true;
   } else {
      return false;
   }
}

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

   int nb_fails = 0;
   std::string filename;
   std::string plugin, type;
   int order = 3;
   int nb = 10000;
   if(! parseArguments(argc, argv, filename, plugin, type, order, nb)) {
      return EXIT_SUCCESS;
   }

   // Load an example
   nb_fails += AltaProjectionMatrix(filename, plugin, type, order, nb);

   if(nb_fails > 0) {
      return EXIT_FAILURE;
   } else {
      return EXIT_SUCCESS;
   }
}


/*
 * CurrentFlowGroupCloseness.cpp
 *
 *      Author: gstoszek
 */
#define ARMA_DONT_PRINT_ERRORS
#include "CurrentFlowGroupCloseness.h"
#include "Centrality.h"
#include "../algebraic/CSRMatrix.h"
#include "../numerics/LAMG/Lamg.h"
#include "../auxiliary/Log.h"
#include <chrono>
#include <stdlib.h>
#include <cmath>
#include <algorithm>
#include "EffectiveResistanceDistance.h"
#include "ERDLevel.h"
#include "../components/ConnectedComponents.h"
#include <armadillo>

namespace NetworKit {

   CurrentFlowGroupCloseness::CurrentFlowGroupCloseness(Graph& G,const count k, const count CB,const double epsilon, const bool doInvert)
    : G(G),k(k),CB(CB),epsilon(epsilon),doInvert(doInvert){
     if (G.isDirected()) throw std::runtime_error("Graph is directed!");
     ConnectedComponents cc(G);
     cc.run();
     if (cc.getPartition().numberOfSubsets() > 1) throw std::runtime_error("Graph has more then one component!");
     if(k>=G.numberOfNodes()) throw std::runtime_error("Size of Group greater then number of nodes!");
     auto start = std::chrono::high_resolution_clock::now();
     auto end = std::chrono::high_resolution_clock::now();
     std::chrono::duration<double> diff;

     S.resize(k);
     n=G.numberOfNodes();

     end = std::chrono::high_resolution_clock::now();
     diff = end-start;
     std::cout << "Constructor finished in " << diff.count() << "(s)" << "\n";
     std::cout << "Number of Nodes=" << G.numberOfNodes() << "\n";
   }

   void CurrentFlowGroupCloseness::run() {
     bool coarse;
     count ID, minDegree;
     auto start = std::chrono::high_resolution_clock::now();
     auto end = std::chrono::high_resolution_clock::now();
     std::chrono::duration<double> diff;
     std::vector<node> vecOfChosenNodes;

     ID=0;
     if(CB>0){
       ID++;
       minDegree=1;
       mergePeripheralNodes();
       ID++;
       minDegree=updateMinDegree();
       coarse=true;
       while((minDegree<CB)&&(coarse)){
         vecOfChosenNodes=coarsingIndices(minDegree, false);
         coarseGraph(vecOfChosenNodes,ID);
         ID++;
         minDegree=updateMinDegree();
         if(!(vecOfChosenNodes.size()>1)||!(G.numberOfNodes()>k)){
           coarse=false;
         }
        }
      }
      end = std::chrono::high_resolution_clock::now();
      diff = end-start;
      std::cout << "Coarsening of G finished in: " << diff.count() << "(s)" << "\n";
      std::cout << "Number of nodes after coarsening: " << G.numberOfNodes() << "\n";

      /*
      start = std::chrono::high_resolution_clock::now();
      if(CB>1){
        ID=LevelList[LevelList.size()-1].getID();
        std::cout<<"ID="<<ID<<"\n";
        while(ID>1){
          uncoarseEfffectiveResistanceDistanceMatrix(ID);
          //greedy();
          //std::cout<<"Level:" <<ID<<" with the value: " << CFGCC << "\n";
          ID--;
        }
      }
      end = std::chrono::high_resolution_clock::now();
      diff = end-start;
      std::cout << "Uncoarsening in: " << diff.count() << "(s)" << "\n";
      */
      std::cout << "Starting Greedy-Algorithm\n";
      start = std::chrono::high_resolution_clock::now();
      if(doInvert)
        greedy();
      else{
        greedyLAMG();
      }
      end = std::chrono::high_resolution_clock::now();
      diff = end-start;
      std::cout << "Greedy in: " << diff.count() << "(s)" << "\n\n";
    }
    std::vector<node> CurrentFlowGroupCloseness::getNodesofGroup(){
      return S;
    }
    double CurrentFlowGroupCloseness::getCFGCC() {
      return CFGCC;
    }
    void CurrentFlowGroupCloseness::greedy(){
      count v_i,w_i;
      node s,v,w;
      double centrality,prevCFGCC,bestMarginalGain,distance;
      std::vector<bool> V,W;
      std::vector<node> vecOfNodes,vecOfSamples, vecOfPeriphs,cutNodes;
      std::vector<count> reverse;
      std::vector<double> mindst, dst, bst, minApprox, bstApprox, marginalGain;

      arma::Mat<double> Pinv;
      Pinv=computePinvOfLaplacian();

      CFGCC=n*n*n;
      prevCFGCC=CFGCC;
      V.resize(G.numberOfNodes(),true);
      W.resize(G.numberOfNodes(),true);
      mindst.resize(G.numberOfNodes(),n*n);
      marginalGain.resize(G.numberOfNodes(),CFGCC);

      vecOfSamples.resize(0);
      vecOfPeriphs.resize(0);
      vecOfNodes = G.nodes();
      reverse.resize(G.upperNodeIdBound());
      for(count i=0;i<vecOfNodes.size();i++){
        reverse[vecOfNodes[i]]=i;
      }
      for(count i=0;i<vecOfNodes.size();i++){
        v=vecOfNodes[i];
        if(std::find(vecOfPeripheralNodes.begin(), vecOfPeripheralNodes.end(), v) != vecOfPeripheralNodes.end())
          vecOfSamples.push_back(v);
        else{
          vecOfPeriphs.push_back(v);
        }
      }
      for(count i=0;i<k;i++){
        std::random_shuffle (vecOfSamples.begin(), vecOfSamples.end());
        bestMarginalGain=0.;
        for (count j=0; j<vecOfSamples.size();j++) {
          v=vecOfSamples[j];
          v_i=reverse[v];
          if(V[v_i] && (bestMarginalGain<marginalGain[v_i])){
            dst=mindst;
            centrality = 0.;
            cutNodes.resize(0);
            for (count l = 0; l < vecOfSamples.size(); l++) {
              w=vecOfSamples[l];
              w_i=reverse[w];
              if(W[w_i]){
                distance=Pinv(v_i,v_i)+Pinv(w_i,w_i)-2*Pinv(v_i,w_i);
                if (distance< mindst[w_i]){
                  dst[w_i]=distance;
                  if(distance<bst[w_i])
                    cutNodes.push_back(w_i);
                }
              }
              centrality +=dst[w_i];
            }
            for (count l = 0 ; l < vecOfPeriphs.size(); l++) {
              w=vecOfPeriphs[l];
              w_i=reverse[w];
              if(W[w_i]){
                distance=Pinv(v_i,v_i)+Pinv(w_i,w_i)-2*Pinv(v_i,w_i);
                if (distance< mindst[w_i]){
                  dst[w_i]=distance;
                  if(distance<bst[w_i])
                    cutNodes.push_back(w_i);
                }
                centrality += dst[w_i];
              }
            }
            marginalGain[v_i]=prevCFGCC-centrality;
            if (centrality < CFGCC) {
              CFGCC = centrality;
              bst=dst;
              s = v;
              bestMarginalGain=marginalGain[v_i];
            }
          }
        }
        S[i]=s;
        V[s]=false;
        mindst=bst;
        prevCFGCC=CFGCC;
      }
     CFGCC = (double)(n)/CFGCC;
    }
    void CurrentFlowGroupCloseness::greedyLAMG(){
      count v_i,w_i;
      node s,v,w;
      double centrality,prevCFGCC,bestMarginalGain,distance;
      std::vector<bool> V;
      std::vector<node> vecOfNodes,vecOfSamples, vecOfPeriphs;
      std::vector<count> reverse;
      std::vector<double> mindst, dst, bst, minApprox, bstApprox, marginalGain;
      std::cout<<"***************Setup***************\n";
      Lamg<CSRMatrix> lamg;
      CSRMatrix matrix = CSRMatrix::laplacianMatrix(G);
      lamg.setupConnected(matrix);
      std::cout<<"**************Setup***************\n";
      CFGCC=n*n*n;
      prevCFGCC=CFGCC;
      mindst.resize(G.numberOfNodes(),n*n);
      marginalGain.resize(G.numberOfNodes(),CFGCC);
      V.resize(G.numberOfNodes(),true);
      vecOfSamples.resize(0);
      vecOfPeriphs.resize(0);
      vecOfNodes = G.nodes();
      reverse.resize(G.upperNodeIdBound());
      for(count i=0;i<vecOfNodes.size();i++){
        reverse[vecOfNodes[i]]=i;
      }
      for(count i=0;i<vecOfNodes.size();i++){
        v=vecOfNodes[i];
        if(std::find(vecOfPeripheralNodes.begin(), vecOfPeripheralNodes.end(), v) != vecOfPeripheralNodes.end())
          vecOfSamples.push_back(v);
        else{
          vecOfPeriphs.push_back(v);
        }
      }
      Vector result(vecOfNodes.size());
      Vector rhs(vecOfNodes.size(), 0.);
      Vector zeroVector(vecOfNodes.size(), 0.);
      std::cout<<"********************1***************\n";
      for(count i=0;i<k;i++){
        std::random_shuffle (vecOfSamples.begin(), vecOfSamples.end());
        bestMarginalGain=0.;
        for (count j=0; j<vecOfSamples.size();j++) {
          v=vecOfSamples[j];
          v_i=reverse[v];
          if(V[v_i] && (bestMarginalGain<marginalGain[v_i])){
            rhs[v_i]=1.;
            dst=mindst;
            centrality = 0.;
            for (count l = 0; l < vecOfSamples.size(); l++) {
              if(v!=w){
                w=vecOfSamples[l];
                w_i=reverse[w];
                rhs[w_i]=-1.;
                result=zeroVector;
                lamg.solve(rhs, result);
                distance=fabs(result[v_i]-result[w_i]);
                if (distance< mindst[w_i])
                  dst[w_i]=distance;
                centrality +=dst[w_i];
                rhs[w_i]=0.;
              }
            }
            for (count l = 0 ; l < vecOfPeriphs.size(); l++) {
              w=vecOfPeriphs[l];
              w_i=reverse[w];
              rhs[w_i]=-1.;
              result=zeroVector;
              lamg.solve(rhs, result);
              distance=fabs(result[v_i]-result[w_i]);
              if (distance< mindst[w_i])
                dst[w_i]=distance;
              centrality += dst[w_i];
              rhs[w_i]=0.;
            }
            marginalGain[v_i]=prevCFGCC-centrality;
            if (centrality < CFGCC) {
              CFGCC = centrality;
              bst=dst;
              s = v;
              bestMarginalGain=marginalGain[v_i];
            }
            rhs[v_i]=0.;
          }
        }
        S[i]=s;
        V[s]=false;
        mindst=bst;
        prevCFGCC=CFGCC;
      }
     CFGCC = (double)(n)/CFGCC;
    }
    count CurrentFlowGroupCloseness::updateMinDegree(){
      bool search;
      count min;
      count i;
      std::vector<node> vecOfNodes;
      node v;
      search=true;
      min=n;
      i=0;
      vecOfNodes=G.nodes();
      while((i<vecOfNodes.size())&&(search)){
        v=vecOfNodes[i];
        if((G.degree(v)<min) && (G.degree(v)>1)){
          min=G.degree(v);
          if(min==2){
            search=false;
          }
        }
        i++;
      }
      return min;
    }

    std::vector<node> CurrentFlowGroupCloseness::coarsingIndices(count courseningDegree, bool Random){
        bool search;
        count l;
        node c,w;
        std::vector<node> vecOfNodes, vecOfChosenNodes, vecOfNeighbors,vecOfOccupiedNodes,vecOfCandidates;

        vecOfOccupiedNodes.resize(0);
        vecOfCandidates.resize(0);
        vecOfNodes=G.nodes();
        for(count i=0;i<vecOfNodes.size();i++){
          c=vecOfNodes[i];
          if(G.degree(c)==courseningDegree){
            vecOfCandidates.push_back(c);
          }
        }
        if(Random){
          std::random_shuffle (vecOfCandidates.begin(), vecOfCandidates.end());
        }
        for(count i=0;i<vecOfCandidates.size();i++){
          c=vecOfCandidates[i];
          if(std::find(vecOfOccupiedNodes.begin(), vecOfOccupiedNodes.end(), c) == vecOfOccupiedNodes.end()){
            vecOfNeighbors=G.neighbors(c);
            search=true;
            l=0;
            if((search)&&(l<vecOfNeighbors.size())){
              w=vecOfNeighbors[l];
              if(std::find(vecOfOccupiedNodes.begin(), vecOfOccupiedNodes.end(), w) != vecOfOccupiedNodes.end()){
                search=false;
              }
              else{
                l++;
              }
            }
            if(search){
              for(count j=0;j<vecOfNeighbors.size();j++){
                w=vecOfNeighbors[j];
                vecOfOccupiedNodes.push_back(w);
              }
              vecOfChosenNodes.push_back(c);
            }
          }
        }
        return vecOfChosenNodes;
      }
    /**************************************************************************
    void CurrentFlowGroupCloseness::uncoarseEfffectiveResistanceDistanceMatrix(count ID){
      bool search;
      count l,j;
      node c,s,w;
      double edgeWeightcs,edgeWeightcw,edgeWeightsw;
      std::vector<std::tuple<node,node,count,double,double,double>> vecOfTriangles;
      std::tuple<node,node,count,double,double,double> triangle;

      vecOfTriangles=LevelList[ID-1].getVecOfTriangles();
      for(count i=0;i<vecOfTriangles.size();i++){
        j=vecOfTriangles.size()-1-i;
        triangle=vecOfTriangles[j];
        ERD.uncoarseTriangle(vecOfNodes,triangle);
        c=std::get<0>(triangle);
        s=std::get<1>(triangle);
        w=std::get<2>(triangle);
        edgeWeightcs=std::get<3>(triangle);
        edgeWeightcw=std::get<4>(triangle);
        edgeWeightsw=1./edgeWeightcs+1./edgeWeightsw;
        edgeWeightsw=1./edgeWeightsw;
        //vecOfNodes.push_back(c);
        //Adj[s].push_back (c);
        //Adj[c].push_back(s);
        if(std::get<4>(triangle)>0){
          //Adj[c].push_back (w);
          //Adj[w].push_back(c);
        }
        if(!(std::get<5>(triangle)==edgeWeightsw)){
          search = true;
          l= 0;
          while((l<Adj[s].size())&&(search)){
            if(Adj[s][l]==w){
              Adj[s].erase(Adj[s].begin()+l);
              search=false;
            }
            else{
              l++;
            }
          }//end while
          search = true;
          l= 0;
          while((l<Adj[w].size())&&(search)){
            if(Adj[w][l]==s){
                Adj[w].erase(Adj[w].begin()+l);
                search=false;
              }
          else{
            l++;
          }
          }//end while
        }//end if
        numberOfCoarsedNodes--;
      }//end for
    }
    /***************************************************************************/
    void CurrentFlowGroupCloseness::mergePeripheralNodes(){
      bool search;
      count l;
      node c,s,w;
      double edgeWeightcs,edgeWeightsw;
      std::vector<node> vecOfNodes,vecOfSupernodes;
      std::vector<double> vecOfWeights;
      std::vector<std::pair<node,node>> mapping;

      vecOfSupernodes.resize(0);
      mapping.resize(0);
      vecOfNodes=G.nodes();
      vecOfWeights.resize(1);
      for(count i=0;i<vecOfNodes.size();i++){
        c=vecOfNodes[i];
        if(G.degree(c)==1){
          s=G.randomNeighbor(c);
          if(std::find(vecOfSupernodes.begin(), vecOfSupernodes.end(), s) == vecOfSupernodes.end()){
            vecOfSupernodes.push_back(s);
            mapping.push_back (std::make_pair(s,c));
            vecOfPeripheralNodes.push_back(c);
          }
          else{
            search = true;
            l= 0;
            while(search){
              if(vecOfSupernodes[l]==s){
                w=mapping[l].second;
                search=false;
              }
              l++;
            }//end while
            edgeWeightcs=G.weight(c,s);
            edgeWeightsw=G.weight(s,w);
            edgeWeightsw=edgeWeightcs+edgeWeightsw;
            G.setWeight(s,w,edgeWeightsw);
            vecOfWeights[0]=edgeWeightcs;
            ERDLevel coarsed(c,G.neighbors(c),vecOfWeights);
            coarsedNodes.push_back(coarsed);
            G.removeNode(c);
          }//end else
        }
      }
    }
    /***************************************************************************/
    void CurrentFlowGroupCloseness::coarseGraph(std::vector<node> vecOfChosenNodes,count degree){
      node c,s,w;
      double edgeWeightcs,edgeWeightcw,edgeWeightsw;
      std::vector<node> vecOfNeighbors;
      std::vector<double> vecOfWeights;

      vecOfWeights.resize(degree);
      /*Path Coarsenening*/
      if(degree==2){
        for(count i=0;i<vecOfChosenNodes.size();i++){
          c=vecOfChosenNodes[i];
          vecOfNeighbors=G.neighbors(c);
          s=vecOfNeighbors[0];
          w=vecOfNeighbors[1];
          edgeWeightcs=G.weight(c,s);
          edgeWeightcw=G.weight(c,w);
          edgeWeightsw=edgeWeightcs+edgeWeightcw;
          /*Path Merge*/
          if(G.weight(s,w)!=0){
            edgeWeightsw=1./edgeWeightsw;
            edgeWeightsw+=1./G.weight(s,w);
            edgeWeightsw=1./edgeWeightsw;
          }
          G.setWeight(s,w,edgeWeightsw);
          for(count j=0;j<vecOfNeighbors.size();j++){
            vecOfWeights[j]=G.weight(c,vecOfNeighbors[j]);
          }
          ERDLevel coarsed(c,vecOfNeighbors,vecOfWeights);
          G.removeNode(c);
        }
      }
      /*Star*/
      else{
        for(count i=0;i<vecOfChosenNodes.size();i++){
          c=vecOfChosenNodes[i];
          computeStarCliqueWeights(c);
          vecOfNeighbors=G.neighbors(c);
          for(count j=0;j<vecOfNeighbors.size();j++){
            vecOfWeights[j]=G.weight(c,vecOfNeighbors[j]);
          }
	  //TODO: Here problem when creating coarsed for the second time
          ERDLevel coarsed(c,vecOfNeighbors,vecOfWeights);
          G.removeNode(c);
        }
      }
    }

    void CurrentFlowGroupCloseness::computeStarCliqueWeights(node c){
      node v,w,x,y;
      double S,S2,weight;
      std::vector<node> vecOfNeighbors;

      S=0.;
      vecOfNeighbors=G.neighbors(c);
      for(count i=0;i<vecOfNeighbors.size();i++){
        v=vecOfNeighbors[i];
        S2=0.;
        for(count j=0;j<vecOfNeighbors.size();j++){
          w=vecOfNeighbors[j];
          if(w!=v){
            S2*=G.weight(v,w);
          }
        }
        S+=S2;
      }
      for(count i=0;i<vecOfNeighbors.size();i++){
        v=vecOfNeighbors[i];
        for(count j=0;j<vecOfNeighbors.size();j++){
          w=vecOfNeighbors[j];
          if(v!=w){
            weight=0.;
            for(count k=0;k<vecOfNeighbors.size();k++){
              x=vecOfNeighbors[k];
              if((x!=v)&&(x!=w)){
                for(count l=0;l<vecOfNeighbors.size();l++){
                  y=vecOfNeighbors[l];
                  if((y!=v)&&(y!=w)&&(y!=x)){
                    weight*=G.weight(x,y);
                  }//if y
                }//for y
              }//if x
            }//for x
	    //TODO: Here weight can be zero! Segfault when dividing with zero!!
	    // maria's example
            if(weight > 0.)
	      weight = S/weight;
	    else weight = 1.; // not sure if you want it one here.
	    // maria's example

            if(G.weight(v,w)!=0){
              weight=1./weight;
              weight+=1./G.weight(v,w);
              weight=1./weight;
            }
            G.setWeight(x,y,weight);
          }// if w
        }// for w
      }// for v
    }
    arma::Mat<double> CurrentFlowGroupCloseness::computePinvOfLaplacian(){
      node v,w;
      count w_i;
      double factor, weight;
      std::vector<node> vecOfNodes, vecOfNeighbours;
      std::vector<count> reverse;
      vecOfNodes=G.nodes();
      arma::Mat<double> L(G.numberOfNodes(),G.numberOfNodes());
      L.zeros();
      reverse.resize(G.upperNodeIdBound());
      for(count i=0;i<vecOfNodes.size();i++){
        reverse[vecOfNodes[i]]=i;
      }
      for(count i=0;i<G.numberOfNodes();i++){
        v=vecOfNodes[i];
        vecOfNeighbours=G.neighbors(v);
        for(count j=0;j<vecOfNeighbours.size();j++){
          w=vecOfNeighbours[j];
          w_i=reverse[w];
          weight=G.weight(v,w);
          L(i,w_i)=-weight;
          L(i,i)+=weight;
        }
      }
      arma::Mat<double> J(L.n_rows,L.n_rows);
      factor=1./(double)(L.n_rows);
      J.fill(factor);
      L= L-J;
      L=arma::inv(L);
      L= L+J;
      return L;
    }
} /* namespace NetworKit*/


#include <GL/freeglut.h>
#include <vector>
#include <Eigen/Eigen>
#include <iostream>
#include <iomanip>

#include "structures.h"
#include "transformations.h"
#include "ray_intersection_observation_equation_jacobian.h"

const unsigned int window_width = 1920;
const unsigned int window_height = 1080;
int mouse_old_x, mouse_old_y;
int mouse_buttons = 0;
float rotate_x = 0.0, rotate_y = 0.0;
float translate_z = -10.0;
float translate_x, translate_y = 0.0;

std::vector<Eigen::Affine3d> bundle_of_rays;
Eigen::Vector3d intersection(0,0,0);

bool initGL(int *argc, char **argv);
void display();
void keyboard(unsigned char key, int x, int y);
void mouse(int button, int state, int x, int y);
void motion(int x, int y);
void reshape(int w, int h);
void printHelp();

int main(int argc, char *argv[]){
	for(size_t i = 0; i < 10; i++){
		TaitBryanPose pose;

		pose.px = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 + 5;
		pose.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 + 5;
		pose.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01 + 5;

		pose.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 2;
		pose.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 2;
		pose.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 2;

		bundle_of_rays.push_back(affine_matrix_from_pose_tait_bryan(pose));
	}

	if (false == initGL(&argc, argv)) {
		return 4;
	}

	printHelp();
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMouseFunc(mouse);
	glutMotionFunc(motion);
	glutMainLoop();

	return 0;
}

bool initGL(int *argc, char **argv) {
	glutInit(argc, argv);
	glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE);
	glutInitWindowSize(window_width, window_height);
	glutCreateWindow("bundle_of_rays_intersection");
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMotionFunc(motion);

	// default initialization
	glClearColor(1.0, 1.0, 1.0, 1.0);
	glEnable(GL_DEPTH_TEST);

	// viewport
	glViewport(0, 0, window_width, window_height);

	// projection
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) window_width / (GLfloat) window_height, 0.01,
			10000.0);
	glutReshapeFunc(reshape);

	return true;
}

void display() {
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glTranslatef(translate_x, translate_y, translate_z);
	glRotatef(rotate_x, 1.0, 0.0, 0.0);
	glRotatef(rotate_y, 0.0, 0.0, 1.0);

	glBegin(GL_LINES);
	glColor3f(1.0f, 0.0f, 0.0f);
	glVertex3f(0.0f, 0.0f, 0.0f);
	glVertex3f(1.0f, 0.0f, 0.0f);

	glColor3f(0.0f, 1.0f, 0.0f);
	glVertex3f(0.0f, 0.0f, 0.0f);
	glVertex3f(0.0f, 1.0f, 0.0f);

	glColor3f(0.0f, 0.0f, 1.0f);
	glVertex3f(0.0f, 0.0f, 0.0f);
	glVertex3f(0.0f, 0.0f, 1.0f);
	glEnd();

	glColor3f(0,1,0);
	glBegin(GL_LINES);
	for(size_t i = 0; i < bundle_of_rays.size(); i++){
		Eigen::Vector3d z_begin(0, 0,-100);
		Eigen::Vector3d z_end(0, 0, 100);

		Eigen::Vector3d z_begin_t = bundle_of_rays[i] * z_begin;
		Eigen::Vector3d z_end_t = bundle_of_rays[i] * z_end;

		glVertex3f(z_begin_t.x(), z_begin_t.y(), z_begin_t.z());
		glVertex3f(z_end_t.x(), z_end_t.y(), z_end_t.z());
	}
	glEnd();

	glPointSize(10);
	glColor3f(1,0,0);
	glBegin(GL_POINTS);
	glVertex3f(intersection.x(), intersection.y(), intersection.z());
	glEnd();
	glPointSize(1);

	glutSwapBuffers();
}

void keyboard(unsigned char key, int /*x*/, int /*y*/) {
	switch (key) {
		case (27): {
			glutDestroyWindow(glutGetWindow());
			return;
		}
		case 't':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			for(size_t i = 0; i < bundle_of_rays.size(); i++){
				Eigen::Vector3d vx(bundle_of_rays[i](0,0), bundle_of_rays[i](1,0), bundle_of_rays[i](2,0));
				Eigen::Vector3d vy(bundle_of_rays[i](0,1), bundle_of_rays[i](1,1), bundle_of_rays[i](2,1));

				Eigen::Matrix<double, 2, 1> delta;
				ray_intersection_observation_equation(delta,
						intersection.x(), intersection.y(), intersection.z(),
						bundle_of_rays[i](0,3), bundle_of_rays[i](1,3), bundle_of_rays[i](2,3),
						vx.x(), vx.y(), vx.z(), vy.x(), vy.y(), vy.z());
				Eigen::Matrix<double, 2, 3> delta_jacobian;
				ray_intersection_observation_equation_jacobian(delta_jacobian,
						intersection.x(), intersection.y(), intersection.z(),
						bundle_of_rays[i](0,3), bundle_of_rays[i](1,3), bundle_of_rays[i](2,3),
						vx.x(), vx.y(), vx.z(), vy.x(), vy.y(), vy.z());

				int ir = tripletListB.size();
				tripletListA.emplace_back(ir, 0, -delta_jacobian(0,0));
				tripletListA.emplace_back(ir, 1, -delta_jacobian(0,1));
				tripletListA.emplace_back(ir, 2, -delta_jacobian(0,2));
				tripletListA.emplace_back(ir + 1, 0, -delta_jacobian(1,0));
				tripletListA.emplace_back(ir + 1, 1, -delta_jacobian(1,1));
				tripletListA.emplace_back(ir + 1, 2, -delta_jacobian(1,2));

				tripletListP.emplace_back(ir    , ir    ,  1);
				tripletListP.emplace_back(ir + 1, ir + 1,  1);

				tripletListB.emplace_back(ir    , 0,  delta(0,0));
				tripletListB.emplace_back(ir + 1, 0,  delta(1,0));
			}

			Eigen::SparseMatrix<double> matA(tripletListB.size(), 3);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(3, 3);
			Eigen::SparseMatrix<double> AtPB(3, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			tripletListA.clear();
			tripletListP.clear();
			tripletListB.clear();

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == 3){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				intersection.x() += h_x[0];
				intersection.y() += h_x[1];
				intersection.z() += h_x[2];
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'n':{
			for(size_t i = 0; i < bundle_of_rays.size(); i++){
				TaitBryanPose pose;

				pose.px = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01;
				pose.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01;
				pose.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 2.0 * 0.01;

				pose.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 2;
				pose.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 2;
				pose.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 2;

				bundle_of_rays[i] = bundle_of_rays[i] * affine_matrix_from_pose_tait_bryan(pose);
			}
			break;
		}
	}

	printHelp();
	glutPostRedisplay();
}

void mouse(int button, int state, int x, int y) {
	if (state == GLUT_DOWN) {
		mouse_buttons |= 1 << button;
	} else if (state == GLUT_UP) {
		mouse_buttons = 0;
	}

	mouse_old_x = x;
	mouse_old_y = y;
}

void motion(int x, int y) {
	float dx, dy;
	dx = (float) (x - mouse_old_x);
	dy = (float) (y - mouse_old_y);

	if (mouse_buttons & 1) {
		rotate_x += dy * 0.2f;
		rotate_y += dx * 0.2f;

	} else if (mouse_buttons & 4) {
		translate_z += dy * 0.05f;
	} else if (mouse_buttons & 3) {
		translate_x += dx * 0.05f;
		translate_y -= dy * 0.05f;
	}

	mouse_old_x = x;
	mouse_old_y = y;

	glutPostRedisplay();
}

void reshape(int w, int h) {
	glViewport(0, 0, (GLsizei) w, (GLsizei) h);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) w / (GLfloat) h, 0.01, 10000.0);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
}

void printHelp() {
	std::cout << "-------help-------" << std::endl;
	std::cout << "n: modify rays" << std::endl;
	std::cout << "t: optimize" << std::endl;
}


/*
Copyright(c) 2014 Akihiro Nishimura

This software is released under the MIT License.
http://opensource.org/licenses/mit-license.php
*/

#ifndef SIGTM_CTR_VALIDATION_HPP
#define SIGTM_CTR_VALIDATION_HPP

#include "../model/ctr.h"
#include "../helper/metrics.hpp"
#include "../helper/cross_validation.hpp"
//#include <boost/numeric/ublas/matrix_sparse.hpp>

namespace sigtm
{

template <class Derived>
struct CTR_PR_IMPL
{
	using RatingContainer_ = RatingContainer<CTR::RatingValueType>;

	CTR_PR_IMPL(sig::Maybe<uint> top_n, sig::Maybe<double> threshold) : top_n_(top_n), threshold_(threshold){}
	virtual ~CTR_PR_IMPL() = default;

	auto operator()(CTRPtr model, std::vector<RatingContainer_>& test_set, bool is_user_test) const->double
	{
		std::vector<double> result;

		if (is_user_test){
			for (UserId id = 0, size = test_set.size(); id < size; ++id){
				auto est = model->recommend(id, true, top_n_, threshold_);

				auto val = static_cast<const Derived*>(this)->impl(
					sig::map([](CTR::EstValueType const& e){ return e.first; }, est),
					sig::map([](CTR::RatingPtr_ const& e){ return e->item_id_; }, test_set[id]),
					false,
					std::less<double>()
				);
				if(val){
					//std::cout << "user:" << id << ", val:" << *val << ", #ans:" << test_set[id] .size() << ", #est:" << est.size() << std::endl;
					result.push_back(*val);
				}
			}
		}
		else{
			for (ItemId id = 0, size = test_set.size(); id < size; ++id){
				auto val = static_cast<const Derived*>(this)->impl(
					sig::map([](CTR::EstValueType const& e){ return e.first; }, model->recommend(id, false, top_n_, threshold_)),
					sig::map([](CTR::RatingPtr_ const& e){ return e->user_id_; }, test_set[id]),
					false,
					std::less<double>()
				);
				if(val) result.push_back(*val);
			}
		}
		return sig::average(result);
	}

private:
	sig::Maybe<uint> top_n_;
	sig::Maybe<double> threshold_;
};

// precision for user (item recommendation)
template <>
struct Precision<CTR> : public PrecisionBase, public CTR_PR_IMPL<Precision<CTR>>
{
	Precision(sig::Maybe<uint> top_n, sig::Maybe<double> threshold) : CTR_PR_IMPL(top_n, threshold){}
};

template <>
struct Recall<CTR> : public RecallBase, public CTR_PR_IMPL<Recall<CTR>>
{
	Recall(sig::Maybe<uint> top_n, sig::Maybe<double> threshold) : CTR_PR_IMPL(top_n, threshold){}
};

template <>
struct F_Measure<CTR> : public F_MeasureBase, private CTR_PR_IMPL<Precision<CTR>>, private CTR_PR_IMPL<Recall<CTR>>
{
	F_Measure(sig::Maybe<uint> top_n, sig::Maybe<double> threshold) : CTR_PR_IMPL<Precision<CTR>>(top_n, threshold), CTR_PR_IMPL<Recall<CTR>>(top_n, threshold){}

	double operator()(CTRPtr model, std::vector<RatingContainer<CTR::RatingValueType>>& test_set, bool is_user_test) const
	{
		return impl(
			CTR_PR_IMPL<Precision<CTR>>::operator()(model, test_set, is_user_test),
			CTR_PR_IMPL<Recall<CTR>>::operator()(model, test_set, is_user_test)
		);
	}
};

template <>
class CrossValidation<CTR> : public CrossValidationBase<CTR::RatingValueType>
{
	std::vector<CTRPtr> models_;
	bool is_user_test_;
	uint topic_num_;
	CTRHyperParamPtr hparam_;
	DocumentSetPtr docs_;

public:
	CrossValidation(uint n, bool is_user_test, uint topic_num, CTRHyperParamPtr hparam, DocumentSetPtr docs, RatingMatrixPtr<CTR::RatingValueType> ratings, uint max_iter, uint min_iter, uint save_lag)
		: CrossValidationBase(n), models_(0), is_user_test_(is_user_test), topic_num_(topic_num), hparam_(hparam), docs_(docs)
	{
		rating_chunks_ = devide_adjusted_random(n, *ratings, is_user_test_);

		auto train_model = [=](uint i){
			auto model = CTR::makeInstance(topic_num_, hparam_, docs_, SparseBooleanMatrix::makeInstance(join_chunc(rating_chunks_, i)));
			//model->train(max_iter, min_iter, save_lag);

			return model;
		};

		models_ = run_parallel<CTRPtr>(train_model, n, sig::seqn(0u, 1u, div_num_).begin());
	}

	// F: R evaluation_func(CTRPtr model, std::vector<RatingContainer_>& test_set, bool is_user_test, double threshold)
	template <class F>
	auto run(F&& evaluation_func)
	{
		using R = decltype(sig::impl::eval(
			std::forward<F>(evaluation_func),
			std::declval<CTRPtr>(),
			rating_chunks_[0],
			is_user_test_
		));

		auto validation = [&](uint test_index)
		{
			return std::forward<F>(evaluation_func)(models_[test_index], rating_chunks_[test_index], is_user_test_);
		};

		std::vector<R> result;

		return run_parallel<R>(validation, div_num_, sig::seqn(0u, 1u, div_num_).begin());
	}

	//void debug_set_u(MatrixUK<double> v){ for(auto& m : models_) m->debug_set_u(v); }
	//void debug_set_v(MatrixIK<double> v){ for(auto& m : models_) m->debug_set_v(v); }
};

}
#endif