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

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

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_BUFFER_BUFFERED_PIECE_COLLECTION_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_BUFFER_BUFFERED_PIECE_COLLECTION_HPP

#include <algorithm>
#include <cstddef>
#include <set>
#include <boost/range.hpp>

#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/core/point_type.hpp>

#include <boost/geometry/algorithms/equals.hpp>
#include <boost/geometry/algorithms/covered_by.hpp>

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

#include <boost/geometry/extensions/algorithms/buffer/buffered_ring.hpp>
#include <boost/geometry/extensions/algorithms/buffer/buffer_policies.hpp>
#include <boost/geometry/extensions/algorithms/buffer/side_on_convex_range.hpp>

#include <boost/geometry/algorithms/detail/overlay/add_rings.hpp>
#include <boost/geometry/algorithms/detail/overlay/assign_parents.hpp>
#include <boost/geometry/algorithms/detail/overlay/calculate_distance_policy.hpp>
#include <boost/geometry/algorithms/detail/overlay/enrich_intersection_points.hpp>
#include <boost/geometry/algorithms/detail/overlay/enrichment_info.hpp>
#include <boost/geometry/algorithms/detail/overlay/ring_properties.hpp>
#include <boost/geometry/algorithms/detail/overlay/traversal_info.hpp>
#include <boost/geometry/algorithms/detail/overlay/traverse.hpp>
#include <boost/geometry/algorithms/detail/overlay/turn_info.hpp>
#include <boost/geometry/algorithms/detail/occupation_info.hpp>
#include <boost/geometry/algorithms/detail/partition.hpp>

#ifdef BOOST_GEOMETRY_DEBUG_BUFFER_OCCUPATION
#  include <boost/geometry/algorithms/detail/overlay/debug_turn_info.hpp>
#  include <boost/geometry/io/wkt/wkt.hpp>
#endif

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace buffer
{

enum piece_type
{
    buffered_segment, buffered_join, buffered_flat_end
};

enum segment_relation_code
{
    segment_relation_on_left,
    segment_relation_on_right,
    segment_relation_within,
    segment_relation_disjoint
};

// In the end this will go (if we have a multi-point within/covered_by geometry)
// which is optimized for multi-points and skips linestrings
template <typename tag>
struct check_original
{
};

template <>
struct check_original<polygon_tag>
{
    template <typename Point, typename Geometry>
    static inline int apply(Point const& point, Geometry const& geometry)
    {
        return geometry::covered_by(point, geometry) ? 1 : -1;
    }
};

template <>
struct check_original<linestring_tag>
{
    template <typename Point, typename Geometry>
    static inline int apply(Point const& point, Geometry const& geometry)
    {
        return 0;
    }
};

template <typename P>
class relaxed_side
{
public :

    // Template member function, because it is not always trivial
    // or convenient to explicitly mention the typenames in the
    // strategy-struct itself.

    // Types can be all three different. Therefore it is
    // not implemented (anymore) as "segment"

    static inline int apply(P const& p1, P const& p2, P const& p)
    {
        typedef typename coordinate_type<P>::type coordinate_type;

        coordinate_type const x = get<0>(p);
        coordinate_type const y = get<1>(p);

        coordinate_type const sx1 = get<0>(p1);
        coordinate_type const sy1 = get<1>(p1);
        coordinate_type const sx2 = get<0>(p2);
        coordinate_type const sy2 = get<1>(p2);

        // Promote float->double, small int->int
        typedef typename geometry::select_most_precise
            <
                coordinate_type,
                double
            >::type promoted_type;

        promoted_type const dx = sx2 - sx1;
        promoted_type const dy = sy2 - sy1;
        promoted_type const dpx = x - sx1;
        promoted_type const dpy = y - sy1;

        promoted_type const s
            = geometry::detail::determinant<promoted_type>
                (
                    dx, dy,
                    dpx, dpy
                );

        promoted_type const zero = promoted_type();
        promoted_type const relaxed_epsilon = std::numeric_limits<double>::epsilon() * 5.0;

        return math::abs(s) < relaxed_epsilon ? 0
            : s > zero ? 1
            : -1;
    }
};

template <typename Ring>
struct buffered_piece_collection
{
    typedef typename geometry::point_type<Ring>::type point_type;
    typedef typename geometry::coordinate_type<Ring>::type coordinate_type;

    struct piece
    {
        piece_type type;
        int index;

        // These both form a complete clockwise ring for each piece (with one dupped point)

		// 1: half, part of offsetted_rings
        segment_identifier first_seg_id;
        int last_segment_index; // no segment-identifier - it is always the same

		// 2: half, not part (will be indexed in one vector too)
        std::vector<point_type> helper_segments; // 3 points for segment, 2 points for join - 0 points for flat-end
    };

    typedef typename strategy::side::services::default_strategy
		<
			typename cs_tag<point_type>::type
		>::type side_strategy;
    typedef std::vector<piece> piece_vector;

    piece_vector m_pieces;
    buffered_ring_collection<buffered_ring<Ring> > offsetted_rings; // indexed by multi_index
    buffered_ring_collection<Ring> traversed_rings;
    segment_identifier current_segment_id;

    std::map<std::pair<segment_identifier, segment_identifier>, std::set<int> > m_turn_indices_per_segment_pair;

    typedef std::vector<buffer_turn_info<point_type> > turn_vector_type;
    typedef detail::overlay::get_turn_info
        <
            point_type, point_type, buffer_turn_info<point_type>,
            turn_assign_for_buffer
        > turn_policy;
    turn_vector_type m_turns;

	// To check clustered locations we keep track of segments being opposite somewhere
	std::set<segment_identifier> m_in_opposite_segments;

	struct buffer_occupation_info : public occupation_info<angle_info<point_type, coordinate_type> >
	{
		std::set<segment_identifier> seg_ids;
		std::set<int> turn_indices;
	};

    typedef occupation_map<point_type, buffer_occupation_info> occupation_map_type;
    occupation_map_type m_occupation_map;

	struct redundant_turn
	{
		inline bool operator()(buffer_turn_info<point_type> const& turn) const
		{
			// Erase discarded turns (location not OK) and the turns
			// only used to detect oppositeness.
			return turn.location != location_ok
				|| turn.opposite();
		}
	};

    inline bool is_neighbor(piece const& piece1, piece const& piece2) const
    {
        if (piece1.first_seg_id.multi_index != piece2.first_seg_id.multi_index)
        {
            return false;
        }

        if (std::abs(piece1.index - piece2.index) == 1)
        {
            return true;
        }

        int const last = boost::size(m_pieces) - 1;
        return (piece1.index == 0 && piece2.index == last)
            || (piece1.index == last && piece2.index == 0)
            ;
    }

    inline bool skip_neighbor(piece const& piece1, piece const& piece2) const
    {
        return piece1.type != piece2.type && is_neighbor(piece1, piece2);
    }

    template <typename Range, typename Iterator>
    inline void move_to_next_point(Range const& range, Iterator& next) const
    {
        ++next;
		if (next == boost::end(range))
		{
			next = boost::begin(range) + 1;
		}
    }

    template <typename Range, typename Iterator>
    inline Iterator next_point(Range const& range, Iterator it) const
    {
        Iterator result = it;
		move_to_next_point(range, result);
		while(geometry::equals(*it, *result))
		{
			move_to_next_point(range, result);
		}
		return result;
    }

    inline void calculate_turns(piece const& piece1, piece const& piece2)
    {
        typedef typename boost::range_iterator<buffered_ring<Ring> const>::type iterator;

		segment_identifier seg_id1 = piece1.first_seg_id;
		segment_identifier seg_id2 = piece2.first_seg_id;

		if (seg_id1.segment_index < 0 || seg_id2.segment_index < 0)
		{
			return;
		}

		buffered_ring<Ring> const& ring1 = offsetted_rings[seg_id1.multi_index];
		iterator it1_first = boost::begin(ring1) + seg_id1.segment_index;
		iterator it1_last = boost::begin(ring1) + piece1.last_segment_index;

		buffered_ring<Ring> const& ring2 = offsetted_rings[seg_id2.multi_index];
		iterator it2_first = boost::begin(ring2) + seg_id2.segment_index;
		iterator it2_last = boost::begin(ring2) + piece2.last_segment_index;

        buffer_turn_info<point_type> the_model;
        the_model.operations[0].piece_index = piece1.index;
        the_model.operations[0].seg_id = piece1.first_seg_id;

        iterator it1 = it1_first;
        for (iterator prev1 = it1++;
                it1 != it1_last;
                prev1 = it1++, the_model.operations[0].seg_id.segment_index++)
        {
            the_model.operations[1].piece_index = piece2.index;
            the_model.operations[1].seg_id = piece2.first_seg_id;

			iterator next1 = next_point(ring1, it1);

            iterator it2 = it2_first;
            for (iterator prev2 = it2++;
                    it2 != it2_last;
                    prev2 = it2++, the_model.operations[1].seg_id.segment_index++)
            {
                // Revert (this is used more often - should be common function TODO)
                the_model.operations[0].other_id = the_model.operations[1].seg_id;
                the_model.operations[1].other_id = the_model.operations[0].seg_id;

				iterator next2 = next_point(ring2, it2);

				if (piece1.index == 5 && piece2.index == 22)
				{
					int kkk = 0;
				}

                turn_policy::apply(*prev1, *it1, *next1,
                                    *prev2, *it2, *next2,
                                    the_model, std::back_inserter(m_turns));
            }
        }
    }

	inline void fill_opposite_segments()
	{
        for (typename boost::range_iterator<turn_vector_type const>::type it =
            boost::begin(m_turns); it != boost::end(m_turns); ++it)
        {
			if (it->is_opposite)
			{
				m_in_opposite_segments.insert(it->operations[0].seg_id);
				m_in_opposite_segments.insert(it->operations[1].seg_id);
//std::cout << " " << it->operations[0].seg_id.segment_index;
			}
		}
	}

	inline segment_relation_code get_segment_relation(point_type const& point,
                segment_identifier const& seg_id) const
	{
		typedef typename boost::range_iterator<std::vector<point_type> const>::type iterator_type;
		iterator_type it = boost::begin(offsetted_rings[seg_id.multi_index]) + seg_id.segment_index;
		iterator_type prev = it++;
		int side = side_strategy::apply(point, *prev, *it);
        if (side == 0)
        {
            if (geometry::equals(point, *prev))
            {
                return segment_relation_on_left;
            }
            else if (geometry::equals(point, *it))
            {
                return segment_relation_on_right;
            }
            else if (collinear_point_on_segment(point, *prev, *it))
            {
                return segment_relation_within;
            }
        }
        return segment_relation_disjoint;
	}

    inline void add_angles(int turn_index, int operation_index, point_type const& point, buffer_turn_operation<point_type> const& operation)
    {
        point_type mapped_point;
        buffer_occupation_info& info = m_occupation_map.find_or_insert(point, mapped_point);
    	info.turn_indices.insert(turn_index);
        info.seg_ids.insert(operation.seg_id);
        add_incoming_and_outgoing_angles(mapped_point, point,
					offsetted_rings[operation.seg_id.multi_index],
                    turn_index, operation_index,
					operation.seg_id,
					info);
    }

    inline void add_angles(int turn_index)
    {
		if (! m_occupation_map.contains_turn_index(turn_index))
		{
			m_occupation_map.insert_turn_index(turn_index);

			buffer_turn_info<point_type> const& turn = m_turns[turn_index];

//std::cout << "Adding point " << turn_index << " " << geometry::wkt(turn.point) << std::endl;

			add_angles(turn_index, 0, turn.point, turn.operations[0]);
			add_angles(turn_index, 1, turn.point, turn.operations[1]);
		}
    }

    inline void classify_turn(buffer_turn_info<point_type>& turn, piece const& pc) const
    {
        if (pc.type == buffered_flat_end)
        {
            return;
        }

        // Don't check against a piece of which is was preferred in the "situations" for multi - map
        if (turn.piece_indices_to_skip.count(pc.index) > 0)
        {
            return;
        }

		int flat_ends_involved = 0;
        for (int i = 0; i < int(boost::size(turn.operations)); i++)
        {
			// Don't check any turn against a piece of which is itself the result
			if (turn.operations[i].piece_index == pc.index)
			{
				return;
			}

            piece const& piece_from_intersection = m_pieces[turn.operations[i].piece_index];
            if (piece_from_intersection.type == buffered_flat_end)
			{
				flat_ends_involved++;
			}
		}

        int side_helper = side_on_convex_range<side_strategy>(turn.point, pc.helper_segments);
        if (side_helper == 1)
        {
			// Left or outside
            return;
        }

		segment_identifier seg_id = pc.first_seg_id;
		if (seg_id.segment_index < 0)
		{
			// Should not occur
			std::cout << "Warning: negative segment_index" << std::endl;
			return;
		}

        if (turn.operations[0].piece_index == 9 && turn.operations[1].piece_index == 35
            && (pc.index == 3 || pc.index == 9 || pc.index == 35))
        {
            double epsilon = std::numeric_limits<double>::epsilon();
            int kkk = 0;
        }

		segment_identifier on_segment_seg_id;

		buffered_ring<Ring> const& ring = offsetted_rings[seg_id.multi_index];

        int const side_offsetted = side_on_convex_range< /*relaxed_side<point_type> */ side_strategy >(turn.point,
						boost::begin(ring) + seg_id.segment_index,
						boost::begin(ring) + pc.last_segment_index,
						seg_id, on_segment_seg_id);
        if (side_offsetted == 1)
        {
            return;
        }

        if (side_offsetted == -1 && side_helper == -1)
        {
            // It is within (assumed that both halves form a closed convex clockwise ring)
            turn.count_within++;
        }
        if (side_offsetted == 0)
        {
	        turn.count_on_offsetted++;
        }
        if (side_helper == 0)
        {
			if (geometry::equals(turn.point, pc.helper_segments.back())
				|| geometry::equals(turn.point, pc.helper_segments.front()))
            {
                turn.count_on_corner++;
            }
            else
            {
				if (flat_ends_involved == 0)
				{
					turn.count_on_helper++;
#ifdef BOOST_GEOMETRY_DEBUG_WITH_MAPPER
					std::ostringstream out;
					out << "HLP " << pc.index;
					turn.debug_string += out.str();
#endif
				}
				else
				{
					turn.count_on_corner++;
				}
            }
        }
    }

    inline void debug_turns_by_indices(std::string const& caption, std::set<int> const& indices) const
    {
#ifdef BOOST_GEOMETRY_DEBUG_BUFFER_OCCUPATION
        std::cout << caption << ": " << indices.size() << std::endl;
        for (std::set<int>::const_iterator sit = indices.begin(); sit != indices.end(); ++sit)
        {
            int const index = *sit;
            std::cout << "Keep "  << index // << "[" << sit->second << "]"
                << " "<< si(m_turns[index].operations[0].seg_id)
                << " "<< si(m_turns[index].operations[1].seg_id)
                << " " << m_turns[index].operations[0].piece_index
                << "/" << m_turns[index].operations[1].piece_index
                << " " << method_char(m_turns[index].method)
                << " " << operation_char(m_turns[index].operations[0].operation)
                << "/" << operation_char(m_turns[index].operations[1].operation)
				<< std::endl;
        }
#endif
    }

    inline void fill_segment_map()
    {
        m_turn_indices_per_segment_pair.clear();
        int index = 0;
        for (typename boost::range_iterator<turn_vector_type>::type it =
            boost::begin(m_turns); it != boost::end(m_turns); ++it, ++index)
        {
            m_turn_indices_per_segment_pair
                [
                    ordered_pair
                        (
                            m_turns[index].operations[0].seg_id,
                            m_turns[index].operations[1].seg_id
                        )
                ].insert(index);
        }
    }

    // Sets "count_on_multi" (if not kept) or "piece_indices_to_skip" (if kept)
    // for to avoid within operations for these pieces
	inline void process_left_turns(buffer_occupation_info const& info,
                    std::set<int> const& keep_indices)
	{
		for (std::set<int>::const_iterator sit1 = info.turn_indices.begin();
			sit1 != info.turn_indices.end();
			++sit1)
		{
			if (keep_indices.count(*sit1) == 0)
			{
        		m_turns[*sit1].count_on_multi++;
            }
            else
            {
		        for (std::set<int>::const_iterator sit2 = info.turn_indices.begin();
			        sit2 != info.turn_indices.end();
			        ++sit2)
		        {
			        if (sit2 != sit1)
			        {
				        m_turns[*sit1].piece_indices_to_skip.insert(m_turns[*sit2].operations[0].piece_index);
				        m_turns[*sit1].piece_indices_to_skip.insert(m_turns[*sit2].operations[1].piece_index);
			        }
		        }
            }
        }
    }

	inline void get_left_turns(buffer_occupation_info& info)
	{
        debug_turns_by_indices("Examine", info.turn_indices);

        std::set<int> keep_indices;
		info.get_left_turns(m_turns, m_turn_indices_per_segment_pair, keep_indices);

		if (! keep_indices.empty())
		{
#ifdef BOOST_GEOMETRY_DEBUG_BUFFER_OCCUPATION
            std::cout << "USE " << keep_indices.size() << " OF " << info.turn_indices.size() << " TURNS" << std::endl;
#endif
            process_left_turns(info, keep_indices);
		}
	}

	inline int piece_count(buffer_occupation_info const& info)
	{
		std::set<int> piece_indices;

		for (std::set<int>::const_iterator sit = info.turn_indices.begin();
			sit != info.turn_indices.end();
			++sit)
		{
			piece_indices.insert(m_turns[*sit].operations[0].piece_index);
			piece_indices.insert(m_turns[*sit].operations[1].piece_index);
		}
		return piece_indices.size();
	}

	inline void classify_occupied_locations()
	{
        for (typename boost::range_iterator<typename occupation_map_type::map_type>::type it =
	            boost::begin(m_occupation_map.map);
			it != boost::end(m_occupation_map.map); ++it)
        {
	        buffer_occupation_info& info = it->second;

#ifdef BOOST_GEOMETRY_DEBUG_BUFFER_OCCUPATION
            std::cout << std::endl << "left turns: " << piece_count(info) << " "
                //<< std::setprecision(20)
                << geometry::wkt(it->first) << std::endl;
#endif
			if (piece_count(info) > 2)
			{
				if (info.occupied())
				{
#ifdef BOOST_GEOMETRY_DEBUG_BUFFER_OCCUPATION
                    std::cout << "-> occupied" << std::endl;

                    // std::set<int> turn_indices;
                    //info.get_left_turns(it->first, m_turns, turn_indices, keep_indices); // just for debug-info
#endif

					for (std::set<int>::const_iterator sit = info.turn_indices.begin();
						sit != info.turn_indices.end();
						++sit)
					{
						m_turns[*sit].count_on_occupied++;
					}
				}
				else
				{
					get_left_turns(info);
				}
	            //std::cout << geometry::wkt(it->first) << " " << int(info.occupied()) << std::endl;
			}
        }
	}

    // The "get_left_turn" process indicates, if it is a u/u turn (both only applicable
    // for union, two separate turns), which is indicated in the map. If done so, set
    // the other to "none", it is part of an occupied situation and should not be followed.
    inline void process_uu()
    {
        for (typename boost::range_iterator<turn_vector_type>::type it =
            boost::begin(m_turns); it != boost::end(m_turns); ++it)
		{
            if (it->both(detail::overlay::operation_union)
                && (it->operations[0].include_in_occupation_map
                    || it->operations[1].include_in_occupation_map))
            {
                bool set_to_none = false;

                // Avoid both turns of a u/u turn to be included.
                if (! it->operations[0].include_in_occupation_map)
                {
                    it->operations[0].operation = detail::overlay::operation_none;
                    set_to_none = true;
                }
                if (! it->operations[1].include_in_occupation_map)
                {
                    it->operations[1].operation = detail::overlay::operation_none;
                    set_to_none = true;
                }
                if (set_to_none)
                {
                    std::cout << "-";
                }
#if defined(BOOST_GEOMETRY_COUNT_DOUBLE_UU)
                else
                {
                    it->count_on_uu++;
                }
#endif
#ifdef BOOST_GEOMETRY_DEBUG_BUFFER_OCCUPATION
                //std::cout << "Keep "  << *sit
                //    << " "<< si(m_turns[*sit].operations[0].seg_id)
                //    << " "<< si(m_turns[*sit].operations[1].seg_id)
                //    << " " << m_turns[*sit].operations[0].piece_index
                //    << "/" << m_turns[*sit].operations[1].piece_index
                //    << " " << method_char(m_turns[*sit].method)
                //    << " " << operation_char(m_turns[*sit].operations[0].operation)
                //    << "/" << operation_char(m_turns[*sit].operations[1].operation)
					//<< std::endl;
#endif

            }
        }
    }

#define BOOST_GEOMETRY_DEBUG_BUFFER_SITUATION_MAP
#ifdef BOOST_GEOMETRY_DEBUG_BUFFER_SITUATION_MAP
	inline int get_side(point_type const& point, Ring const& ring, int segment_index)
	{
        typedef typename boost::range_iterator<Ring const> iterator_type;
		iterator_type it = boost::begin(ring) + segment_index;
		iterator_type prev = it++;
		return side_strategy::apply(point, *prev, *it);
	}
#endif

    template <typename Iterator>
    static inline point_type const& select_for_side(Iterator first, Iterator second, int index)
    {
        return index == 0 ? *first : *second;
    }

	inline int get_side(segment_identifier const& seg_id1, segment_identifier const& seg_id2, int which = 1) const
	{
        typedef typename boost::range_iterator<Ring const>::type iterator_type;

        Ring const& ring1 = offsetted_rings[seg_id1.multi_index];
        Ring const& ring2 = offsetted_rings[seg_id2.multi_index];

		iterator_type it1 = boost::begin(ring1) + seg_id1.segment_index;
		iterator_type it2 = boost::begin(ring2) + seg_id2.segment_index;

		iterator_type prev1 = it1++;
		iterator_type prev2 = it2++;

		int code1 = side_strategy::apply(select_for_side(prev1, it1, which), *prev2, *it2);
		int code2 = side_strategy::apply(select_for_side(prev2, it2, which), *prev1, *it1);

        if (code1 == 1 && code2 == -1) return 1;
        if (code1 == -1 && code2 == 1) return -1;

        // ROBUSTNESS: in near collinear cases one might be zero, the other non-zero.
        // This happens several times.
        if (code1 != 0) return code1;
        if (code2 != 0) return -code2;

  //      // Check if the other side gives some more info
  //      // (I've never seen this is the case though it might be so, if they are much longer.
		//int code1f = side_strategy::apply(*prev1, *prev2, *it2);
		//int code2f = side_strategy::apply(*prev2, *prev1, *it1);

  //      if (code1f != 0 || code2f != 0)
  //      {
  //          std::cout << "From: " << code1f << " " << code2f << std::endl;
  //          if (code1f != 0) return -code1f;
  //          if (code2f != 0) return code2f;
  //      }

        // Collinear?
#ifdef BOOST_GEOMETRY_DEBUG_BUFFER_SITUATION_MAP
        //std::cout << "Collinear: " << code1 << " " << code2 << std::endl;
#endif
        return 0;
	}

	inline void debug_segment(segment_identifier id)
	{
        typedef typename boost::range_iterator<buffered_ring<Ring> const>::type iterator;

        buffered_ring<Ring> const& ring = offsetted_rings[id.multi_index];
		iterator it = boost::begin(ring) + id.segment_index;
		iterator prev = it++;
		geometry::model::referring_segment<point_type const&> segment(*prev, *it);
        //std::cout << geometry::wkt(*prev) << " " << geometry::wkt(*it) << std::endl;
	}

	struct cluster_info
	{
		inline cluster_info(int i, point_type p, buffer_turn_operation<point_type> op)
			: turn_index(i)
			, point(p)
			, operation(op)
		{}

		inline cluster_info()
			: turn_index(-1)
    	{}

		int turn_index;
		point_type point;
		buffer_turn_operation<point_type> operation;
	};

    struct clustered_info
    {
        int piece_index;
        std::set<segment_identifier> intersecting_ids;
        std::vector<cluster_info> intersecting_segments;
    };

#ifdef OLD
    struct situation_info
    {
        std::set<int> turn_indices;
        std::set<segment_identifier> seg_ids;
    };
#endif

	static inline bool add_mutual_intersection(clustered_info const& cluster, segment_identifier const& seg_id)
	{
		bool result = false;
        //if (cluster.intersecting_ids.count(seg_id) > 0)
        typedef typename boost::range_iterator<std::vector<cluster_info> const>::type iterator_type;
		for(iterator_type it = cluster.intersecting_segments.begin(); it != cluster.intersecting_segments.end(); it++)
		{
			if (it->operation.seg_id == seg_id)
			{
				result = true;
    		}
		}
		return result;
	}

    inline bool mutually_interact(cluster_info const& a, cluster_info const& b, clustered_info const& other) const
    {
		// Either these two segments intersect, or they are perfectly collinear.
		// First check the intersection:
		if (add_mutual_intersection(other, b.operation.seg_id))
		{
			std::cout << "#";
            return true;
		}

		// Then the collinearity
		if (get_side(a.operation.seg_id, b.operation.seg_id) == 0)
		{
			std::cout << "1";
            return true;

		}

		if (get_side(a.operation.seg_id, b.operation.seg_id, 0) == 0)
		{
			std::cout << "0";
            return true;
		}

        if (geometry::equals(a.point, b.point))
        {
			std::cout << "=";
            return true;
        }

        relaxed_less<point_type> comparator;
        if (comparator.equals(a.point, b.point))
		{
			std::cout << "*";
            return true;
		}

        return false;
    }

	inline void add_mutual_intersections(clustered_info const& cluster, std::map<segment_identifier, clustered_info> const& map)
	{
        typedef std::map<segment_identifier, clustered_info> map_type;
        typedef typename boost::range_iterator<map_type const>::type map_it;
        typedef typename boost::range_iterator<std::vector<cluster_info> const>::type cluster_it;
        for(cluster_it it1 = cluster.intersecting_segments.begin(); it1 != cluster.intersecting_segments.end(); it1++)
		{
			map_it const& other_cluster_it = map.find(it1->operation.seg_id);
			if (other_cluster_it != map.end())
			{
				for(cluster_it it2 = it1 + 1; it2 != cluster.intersecting_segments.end(); it2++)
				{
					if (! m_occupation_map.contains_turn_index(it1->turn_index)
						|| ! m_occupation_map.contains_turn_index(it2->turn_index))
					{
                        if (mutually_interact(*it1, *it2, other_cluster_it->second))
                        {
					        add_angles(it1->turn_index);
					        add_angles(it2->turn_index);
                        }
					}
				}
			}
		}
	}

    inline void add_multi_intersections_to_occupation_map()
	{
        // Pass 1: create map of all segments
		typedef std::map<segment_identifier, clustered_info> map_type;
		map_type map;

		int index = 0;
        for (typename boost::range_iterator<turn_vector_type>::type it =
            boost::begin(m_turns); it != boost::end(m_turns); ++it, ++index)
        {
			buffer_turn_info<point_type> const& turn = *it;

			// Take care with all the indices
			map[turn.operations[0].seg_id].piece_index = turn.operations[0].piece_index;
			map[turn.operations[0].seg_id].intersecting_segments.push_back(cluster_info(index, turn.point, turn.operations[1]));
			map[turn.operations[0].seg_id].intersecting_ids.insert(turn.operations[1].seg_id);

			map[turn.operations[1].seg_id].piece_index = turn.operations[1].piece_index;
			map[turn.operations[1].seg_id].intersecting_segments.push_back(cluster_info(index, turn.point, turn.operations[0]));
			map[turn.operations[1].seg_id].intersecting_ids.insert(turn.operations[0].seg_id);
		}

        // Pass 2:
        // Verify all segments crossing with more than one segment, and if they intersect each other,
        // add that pair
		for (typename map_type::const_iterator mit = map.begin(); mit != map.end(); ++mit)
		{
			if (mit->second.intersecting_segments.size() > 1)
			{
				add_mutual_intersections(mit->second, map);
			}
		}
    }

	inline void get_occupation()
    {
		fill_opposite_segments();

        // Pass 1: fill all segments part of opposite segments
        int index = 0;
        for (typename boost::range_iterator<turn_vector_type>::type it =
            boost::begin(m_turns); it != boost::end(m_turns); ++it, ++index)
        {
			buffer_turn_info<point_type>& turn = *it;
//std::cout << "Referring to point " << geometry::wkt(turn.point) << std::endl;
			if (m_in_opposite_segments.count(turn.operations[0].seg_id) > 0
				|| m_in_opposite_segments.count(turn.operations[1].seg_id) > 0)
			{
				add_angles(index);
			}
		}

        // Pass 2: add multi intersection
        add_multi_intersections_to_occupation_map();

        // Pass 3: fill all segments intersecting in points present in the map
        index = 0;
        for (typename boost::range_iterator<turn_vector_type>::type it =
            boost::begin(m_turns); it != boost::end(m_turns); ++it, ++index)
        {
			buffer_turn_info<point_type>& turn = *it;
			if (m_in_opposite_segments.count(turn.operations[0].seg_id) == 0
				&& m_in_opposite_segments.count(turn.operations[1].seg_id) ==  0)
			{
                // See if it is in the map
                if (m_occupation_map.contains(turn.point))
                {
				    add_angles(index);
                }
			}
		}
	}

    inline void classify_turns()
    {

        // Check if it is inside any of the pieces
        // Now: quadratic
        // TODO: in partition.
        for (typename boost::range_iterator<turn_vector_type>::type it =
            boost::begin(m_turns); it != boost::end(m_turns); ++it)
		{
            //if (it->count_on_occupied == 0)
            {
                typename std::vector<piece>::const_iterator pit;
                for (pit = boost::begin(m_pieces);
                    pit != boost::end(m_pieces);
                    ++pit)
                {
                    classify_turn(*it, *pit);
                }
            }
        }
    }

    template <typename Turn>
    inline bool classify_turn_inside(Turn const& turn) const
    {
        return turn.count_within > 0
            || turn.count_on_multi > 0
            || turn.count_on_helper > 0
			|| turn.count_on_occupied > 0
            ;
    }

    inline void classify_inside()
    {
		// Set results:
        for (typename boost::range_iterator<turn_vector_type>::type it =
            boost::begin(m_turns); it != boost::end(m_turns); ++it)
        {
            if (classify_turn_inside(*it))
            {
                it->location = inside_buffer;
            }

#if defined(BOOST_GEOMETRY_COUNT_DOUBLE_UU)
            else if (it->count_on_uu > 0)
            {
                extern int g_count_double_uu;
                g_count_double_uu++;
                std::cout << "UU";
            }
#endif

        }
    }

    template <typename Geometry>
    inline void check_remaining_points(Geometry const& input_geometry, int factor)
    {
        // TODO: this should be done as a collection-of-points, for performance
        for (typename boost::range_iterator<turn_vector_type>::type it =
            boost::begin(m_turns); it != boost::end(m_turns); ++it)
        {
            if (it->location == location_ok)
			{
				int code = check_original
                        <
                            typename geometry::tag<Geometry>::type
                        >::apply(it->point, input_geometry);
				if (code * factor == 1)
				{
					it->location = inside_original;
				}
			}
        }
    }

    template <typename Turns>
    static inline void split_uu_turns(Turns& turns)
    {
        Turns added;

        for (typename boost::range_iterator<Turns>::type it = boost::begin(turns);
            it != boost::end(turns); ++it)
		{
            if (it->both(detail::overlay::operation_union))
            {
                //std::cout << "U";

                typename boost::range_value<Turns>::type turn = *it; // copy by value
                // std::swap(turn.operations[0], turn.operations[1]);
                turn.operations[0].operation = detail::overlay::operation_continue;
                turn.operations[1].operation = detail::overlay::operation_continue;
                it->operations[1].operation = detail::overlay::operation_continue;
                it->operations[0].operation = detail::overlay::operation_continue;
                added.push_back(turn);
            }
        }

        for (typename boost::range_iterator<Turns>::type it = boost::begin(added);
            it != boost::end(added); ++it)
		{
            turns.push_back(*it);
        }

        if (added.size() > 0)
        {
            for (typename boost::range_iterator<Turns>::type it = boost::begin(turns);
                it != boost::end(turns); ++it)
		    {
                std::cout << "Turn"
                    << " "<< si(it->operations[0].seg_id)
                    << " "<< si(it->operations[1].seg_id)
                    << " " << it->operations[0].piece_index
                    << "/" << it->operations[1].piece_index
                    << " " << method_char(it->method)
                    << " " << operation_char(it->operations[0].operation)
                    << "/" << operation_char(it->operations[1].operation)
				    << std::endl;
            }
        }

    }

    template <typename Geometry>
    inline void get_turns(Geometry const& input_geometry, int factor)
    {
		// Now: quadratic
        // TODO use partition

        for(typename piece_vector::const_iterator it1 = boost::begin(m_pieces);
            it1 != boost::end(m_pieces);
            ++it1)
        {
            for(typename piece_vector::const_iterator it2 = it1 + 1;
                it2 != boost::end(m_pieces);
                ++it2)
            {
                if (! skip_neighbor(*it1, *it2))
                {
                    calculate_turns(*it1, *it2);
                }
            }
        }

		//discard_uu_turns();
        for (typename boost::range_iterator<turn_vector_type>::type it =
            boost::begin(m_turns); it != boost::end(m_turns); ++it)
		{
            //if (it->both(detail::overlay::operation_union))
            //{
            //    std::cout << "double UU" << std::endl;
            //}
			//std::cout << std::setprecision(16) << geometry::wkt(it->point)
   //             << " " << it->operations[0].piece_index << "/" << it->operations[1].piece_index
   //             << " " << si(it->operations[0].seg_id) << "/" << si(it->operations[1].seg_id)
			//    << " " << method_char(it->method)
			//	<< ":" << operation_char(it->operations[0].operation)
			//	<< "/" << operation_char(it->operations[1].operation)
   //             << std::endl;
        }

        //split_uu_turns(m_turns);

        fill_segment_map();
		get_occupation();
        classify_occupied_locations();
        process_uu();
        classify_turns();
        classify_inside();

        if (boost::is_same<typename tag_cast<typename tag<Geometry>::type, areal_tag>::type, areal_tag>())
        {
            check_remaining_points(input_geometry, factor);
        }
    }

    inline void start_new_ring()
    {
        int const n = offsetted_rings.size();
        current_segment_id.source_index = 0;
        current_segment_id.multi_index = n;
        current_segment_id.ring_index = -1;
        current_segment_id.segment_index = 0;

        offsetted_rings.resize(n + 1);
    }

    inline int add_point(point_type const& p)
    {
        BOOST_ASSERT
            (
                boost::size(offsetted_rings) > 0
            );

        current_segment_id.segment_index++;
        offsetted_rings.back().push_back(p);
		return offsetted_rings.back().size();
    }

    inline piece& add_piece(piece_type type, bool decrease_by_one)
    {
        piece pc;
        pc.type = type;
        pc.index = boost::size(m_pieces);
        pc.first_seg_id = current_segment_id;

        std::size_t const n = boost::size(offsetted_rings.back());
        pc.first_seg_id.segment_index = decrease_by_one ? n - 1 : n;

        m_pieces.push_back(pc);
        return m_pieces.back();
    }

    inline void add_piece(piece_type type, point_type const& p1, point_type const& p2,
            point_type const& b1, point_type const& b2)
    {
        bool const last_type_join = ! m_pieces.empty()
				&& m_pieces.back().first_seg_id.multi_index == current_segment_id.multi_index
                && m_pieces.back().type == buffered_join;

        piece& pc = add_piece(type, last_type_join);

        // If it follows the same piece-type point both should be added.
        // There should be two intersections later and it should be discarded.
        // But for need it to calculate intersections
        if (! last_type_join)
        {
            add_point(b1);
        }
        pc.last_segment_index = add_point(b2);

        pc.helper_segments.push_back(b2);
        pc.helper_segments.push_back(p2);
        pc.helper_segments.push_back(p1);
        pc.helper_segments.push_back(b1);
    }

    template <typename Range>
    inline piece& add_piece(piece_type type, Range const& range)
    {
        piece& pc = add_piece(type, true);

        bool first = true;
		int last = offsetted_rings.back().size() + 1;
        for (typename Range::const_iterator it = boost::begin(range);
            it != boost::end(range);
            ++it)
        {
            bool add = true;
            if (first)
            {
                // Only for very first one, add first. In all other cases it is shared with previous.
                add = offsetted_rings.back().empty();
                first = false;
            }
            if (add)
            {
                last = add_point(*it);
            }

        }

        pc.last_segment_index = last;

        return pc;
    }

    template <typename Range>
    inline void add_piece(piece_type type, point_type const& p, Range const& range)
    {
        piece& pc = add_piece(type, range);

        if (boost::size(range) > 0)
        {
            pc.helper_segments.push_back(range.back());
            pc.helper_segments.push_back(p);
            pc.helper_segments.push_back(range.front());
        }
    }

    inline void enrich()
    {
        typedef typename strategy::side::services::default_strategy
        <
            typename cs_tag<Ring>::type
        >::type side_strategy_type;

        enrich_intersection_points<false, false>(m_turns,
                    detail::overlay::operation_union,
                    offsetted_rings, offsetted_rings,
                    side_strategy_type());
    }

    // Discards all rings which do have not-OK intersection points only.
    // Those can never be traversed and should not be part of the output.
    inline void discard_rings()
    {
        for (typename boost::range_iterator<turn_vector_type const>::type it =
            boost::begin(m_turns); it != boost::end(m_turns); ++it)
        {
            if (it->location != location_ok)
            {
                offsetted_rings[it->operations[0].seg_id.multi_index].has_discarded_intersections = true;
                offsetted_rings[it->operations[1].seg_id.multi_index].has_discarded_intersections = true;
            }
            else if (! it->both(detail::overlay::operation_union))
            {
                offsetted_rings[it->operations[0].seg_id.multi_index].has_accepted_intersections = true;
                offsetted_rings[it->operations[1].seg_id.multi_index].has_accepted_intersections = true;
            }
        }
    }

    inline void discard_turns()
    {
        m_turns.erase
			(
				std::remove_if(boost::begin(m_turns), boost::end(m_turns),
								redundant_turn()),
				boost::end(m_turns)
			);

    }

   // inline void discard_uu_turns()
   // {
   //     m_turns.erase
			//(
			//	std::remove_if(boost::begin(m_turns), boost::end(m_turns),
			//					uu_turn()),
			//	boost::end(m_turns)
			//);

   // }

    inline void traverse()
    {
        typedef detail::overlay::traverse
            <
                false, false,
                buffered_ring_collection<buffered_ring<Ring> >, buffered_ring_collection<buffered_ring<Ring > >,
                backtrack_for_buffer
            > traverser;

        traversed_rings.clear();
        traverser::apply(offsetted_rings, offsetted_rings,
                        detail::overlay::operation_union,
                        m_turns, traversed_rings);
    }

    template <typename GeometryOutput, typename OutputIterator>
    inline OutputIterator assign(OutputIterator out)
    {
        typedef detail::overlay::ring_properties<point_type> properties;

        std::map<ring_identifier, properties> selected;

        // Select all rings which do not have any self-intersection (other ones should be traversed)
        int index = 0;
        for(typename buffered_ring_collection<buffered_ring<Ring> >::const_iterator it = boost::begin(offsetted_rings);
            it != boost::end(offsetted_rings);
            ++it, ++index)
        {
            if (! it->has_intersections())
            {
                ring_identifier id(0, index, -1);
                selected[id] = properties(*it, true);
            }

//std::cout << geometry::wkt(*it) << std::endl;
        }

        // Select all created rings
        index = 0;
        for (typename boost::range_iterator<buffered_ring_collection<Ring> const>::type
                it = boost::begin(traversed_rings);
                it != boost::end(traversed_rings);
                ++it, ++index)
        {
			ring_identifier id(2, index, -1);
			selected[id] = properties(*it, true);
        }

        detail::overlay::assign_parents(offsetted_rings, traversed_rings, selected, false);
        return detail::overlay::add_rings<GeometryOutput>(selected, offsetted_rings, traversed_rings, out);
    }

};

}} // namespace detail::buffer
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_BUFFER_BUFFERED_PIECE_COLLECTION_HPP


#pragma once

#include <igl/AABB.h>

#include <Eigen/Dense>

namespace polyfem
{
class InterpolatedFunction2d
{
public:
	InterpolatedFunction2d() {}
	InterpolatedFunction2d(const Eigen::MatrixXd &fun, const Eigen::MatrixXd &pts, const Eigen::MatrixXi &tris);
	void init(const Eigen::MatrixXd &fun, const Eigen::MatrixXd &pts, const Eigen::MatrixXi &tris);

	Eigen::MatrixXd interpolate(const Eigen::MatrixXd &pts) const;

private:
	igl::AABB<Eigen::MatrixXd, 2> tree_;
	Eigen::MatrixXd fun_;
	Eigen::MatrixXd pts_;
	Eigen::MatrixXi tris_;
};
} // namespace polyfem


#pragma warning( disable : 4127 ) // todo is there a solution to fix this issue?

#include "communityidgenerator.h"

#include <boost/algorithm/string.hpp>
#include <boost/archive/iterators/base64_from_binary.hpp>
#include <boost/archive/iterators/binary_from_base64.hpp>
#include <boost/archive/iterators/transform_width.hpp>

#include <openssl/md5.h>
#include <openssl/sha.h>
#include <boost/asio/ip/address.hpp>
#include <boost/endian/buffers.hpp>
#include <iostream>
#include <sstream>
#include <string>
#include <vector>

namespace bai = boost::archive::iterators;
namespace errc = boost::system::errc;
namespace ip = boost::asio::ip;

namespace zeek {

typedef bai::binary_from_base64<const char*> base64_str;
typedef bai::transform_width<base64_str, 8, 6> base64_dec;
typedef bai::transform_width<std::string::const_iterator, 6, 8> base64_enc;
typedef bai::base64_from_binary<base64_enc> it_base64;

size_t length_ = SHA_DIGEST_LENGTH;
void* ctx_ = static_cast<SHA_CTX*>(malloc(sizeof(SHA_CTX)));

const std::string v1 = "1:";

const int supported_protocols[6] = {
    1,  // ICMP
    6,  // TCP
    17, // UDP
    46, // RSVP
    58, // ICMP6
    132 // SCTP
};

bool checkProtocol(const int64_t proto) {
  bool supported = std::find(std::begin(supported_protocols),
                             std::end(supported_protocols), proto) != std::end(supported_protocols);
  return supported;
}

std::string communityIDv1(const std::string saddr_str, const std::string daddr_str,
                          int64_t sport, int64_t dport, int64_t proto) {

  uint16_t seed = 0;

  boost::system::error_code ec;
  ip::address saddr = ip::make_address(saddr_str, ec);
  if (ec.value() != errc::success) {
    std::cout << "Community ID hash saddr cannot be parsed as IP" << "\n";
    return "";
  }
  ip::address daddr = ip::make_address(daddr_str, ec);
  if (ec.value() != errc::success) {
    std::cout << "Community ID hash daddr cannot be parsed as IP" << "\n";
    return "";
  }

  // Check source and destination ports
  if (sport < 0 || sport > UINT16_MAX || dport < 0 || dport > UINT16_MAX) {
    std::cout << "Community ID ports must fit in 2 bytes" << "\n";
    return "";
  }

  //Ensure ordering; if source address is larger switch order
  if (!(saddr < daddr || (saddr == daddr && sport < dport))) {
    std::swap(saddr, daddr);
    int64_t tmpPort = sport;
    sport = dport;
    dport = tmpPort;
  }

  std::stringstream bytes;
  bytes.write(reinterpret_cast<const char*>(&seed), 2);

  if (saddr.is_v4()) {
    bytes.write(reinterpret_cast<const char*>(saddr.to_v4().to_bytes().data()),
                4);
  } else {
    bytes.write(reinterpret_cast<const char*>(saddr.to_v6().to_bytes().data()),
                16);
  }
  if (daddr.is_v4()) {
    bytes.write(reinterpret_cast<const char*>(daddr.to_v4().to_bytes().data()),
                4);
  } else {
    bytes.write(reinterpret_cast<const char*>(daddr.to_v6().to_bytes().data()),
                16);
  }

  bytes.write(reinterpret_cast<const char*>(&proto), 1);
  bytes.put(0);
  bytes.put(0);
  bytes.write(reinterpret_cast<const char*>(&sport), 2);
  bytes.write(reinterpret_cast<const char*>(&dport), 2);

  std::string res = bytes.str();

  update(res.c_str(), res.size());
  auto result = v1 + digest();

  return result.c_str();
}

void update (const void* buffer, size_t size) {
  SHA1_Init(static_cast<SHA_CTX*>(ctx_));
  SHA1_Update(static_cast<SHA_CTX*>(ctx_), buffer, size);
}

std::string digest() {
  std::vector<unsigned char> hash;
  hash.assign(length_, '\0');

  SHA1_Final(hash.data(), static_cast<SHA_CTX*>(ctx_));

  std::stringstream digest;
  for (size_t i=0; i < length_; i++) {
    digest << hash[i];
  }

  return encode(digest.str());
}

std::string encode(const std::string &unencoded) {
  if (unencoded.empty()) {
    return unencoded;
  }

  size_t writePaddChars = (3U - unencoded.length() % 3U) % 3U;
  try {
    auto encoded =
        std::string(it_base64(unencoded.begin()), it_base64(unencoded.end()));
    encoded.append(std::string(writePaddChars, '='));
    return encoded;
  } catch (const boost::archive::iterators::dataflow_exception& e) {
    std::cout << "Could not base64 encode string: " << e.what();
    return "";
  }
}

} // namespace zeek


/*
 * This file is part of the DUDS project. It is subject to the BSD-style
 * license terms in the LICENSE file found in the top-level directory of this
 * distribution and at http://www.somewhere.org/somepath/license.html.
 * No part of DUDS, including this file, may be copied, modified, propagated,
 * or distributed except according to the terms contained in the LICENSE file.
 *
 * Copyright (C) 2018  Jeff Jackowski
 */
#include <duds/hardware/devices/instruments/APDS9301.hpp>
#include <duds/hardware/interface/linux/DevSmbus.hpp>
#include <iostream>
#include <thread>
#include <iomanip>
#include <assert.h>
#include <boost/exception/diagnostic_information.hpp>
#include <boost/program_options.hpp>

constexpr int valw = 8;
bool quit = false;

void runtest(duds::hardware::devices::instruments::APDS9301 &meter)
try {
	duds::data::Unit chku = duds::data::units::Watt / (
			duds::data::units::Meter * duds::data::units::Meter
		);
	std::cout.precision(6);
	meter.resume();
	do {
		std::this_thread::sleep_for(std::chrono::seconds(1));
		meter.sample();
		duds::data::Quantity visB = meter.irradiance();
		duds::data::Quantity irB = meter.irradianceIr();
		duds::data::Quantity ill = meter.illuminance();
		assert(visB.unit == chku);
		assert(irB.unit == chku);
		assert(ill.unit == duds::data::units::Lux);
		std::cout << "Visible: " << std::setw(10) << visB.value << "W/m2  IR: " <<
		std::setw(10) << irB.value << "W/m2  Illuminance: " << std::setw(10) <<
		ill.value << " lux" << std::endl;
	} while (!quit);
} catch (...) {
	std::cerr << "Program failed in runtest(): " <<
	boost::current_exception_diagnostic_information() << std::endl;
}

int main(int argc, char *argv[])
try {
	std::string i2cpath;
	float inttime;
	int i2caddr;
	bool hgain = false;
	{ // option parsing
		boost::program_options::options_description optdesc(
			"Options for APDS9301 test"
		);
		optdesc.add_options()
			( // help info
				"help,h",
				"Show this help message"
			)
			( // the I2C device file to use
				"smbdev,i",
				boost::program_options::value<std::string>(&i2cpath)->
					default_value("/dev/i2c-1"),
				"Specify Smbus device file"
			)
			( // the I2C device address to use
				"smbaddr,a",
				boost::program_options::value<int>(&i2caddr)->
					default_value(0x39),
				"Specify Smbus device address"
			)
			( // integration time
				"integrate,e",
				boost::program_options::value<float>(&inttime)->
					default_value(0.014f),
				"Maximum integration time"
			)
			( // high gain
				"highgain,g",
				"Use high gain (16x)"
			)
		;
		boost::program_options::variables_map vm;
		boost::program_options::store(
			boost::program_options::parse_command_line(argc, argv, optdesc),
			vm
		);
		boost::program_options::notify(vm);
		if (vm.count("help")) {
			std::cout << "Test program for light meter APDS9301\n" << argv[0]
			<< " [options]\n" << optdesc << std::endl;
			return 0;
		}
		if (vm.count("highgain")) {
			hgain = true;
		}
	}
	std::unique_ptr<duds::hardware::interface::Smbus> smb(
		new duds::hardware::interface::linux::DevSmbus(
			i2cpath, i2caddr, duds::hardware::interface::Smbus::NoPec()
		)
	);
	duds::hardware::devices::instruments::APDS9301 meter(smb);
	meter.init(inttime, hgain);
	std::cout << "Integration period is " << meter.period() << " seconds.\n"
	"Maximum reportable irradiance is " << meter.maxIrradiance().value <<
	std::endl;
	std::this_thread::sleep_for(std::chrono::milliseconds(2));
	std::thread doit(runtest, std::ref(meter));
	std::cin.get();
	quit = true;
	doit.join();
} catch (...) {
	std::cerr << "Program failed in main(): " <<
	boost::current_exception_diagnostic_information() << std::endl;
	return 1;
}


/*  _______________________________________________________________________

    DAKOTA: Design Analysis Kit for Optimization and Terascale Applications
    Copyright 2014 Sandia Corporation.
    This software is distributed under the GNU Lesser General Public License.
    For more information, see the README file in the top Dakota directory.
    _______________________________________________________________________ */

//- Class:	 NonDGPMSABayesCalibration
//- Description: Derived class for Bayesian inference using QUESO/GPMSA
//- Owner:       Laura Swiler, Brian Adams
//- Checked by:
//- Version:

// place Dakota headers first to minimize influence of QUESO defines
#include "NonDGPMSABayesCalibration.hpp"
#include "NonDLHSSampling.hpp"
#include "ProblemDescDB.hpp"
#include "ParallelLibrary.hpp"
#include "DakotaModel.hpp"

// then system-related headers
#include <boost/filesystem/path.hpp>
namespace bfs = boost::filesystem;

// then list QUESO headers
#include "queso/GPMSA.h"
#include "queso/StatisticalInverseProblem.h"
#include "queso/GslVector.h"
#include "queso/GslMatrix.h"

using QUESO::GslVector;
using QUESO::GslMatrix;

static const char rcsId[]="@(#) $Id$";

/* TODO / QUESTIONS:

 * Write tests that use DOE vs import

    - Notify users what data we're trying to read from tabular

 * A number of improvements in DACE / imported build data

    - Decide on meaning of buildSamples for read vs. DACE iterator control

    - Read build data in base class?

    - Read using residualModel's DataTransformModel vars/resp?

    - Handle string variables (convert to indices), probably be
      reading into a Variables object

    - Instead of assuming config vars = 0.5, use initial_state?

    - Read the build data early so we know buildSamples; would be nice
      to integrate with populating the data below so we can avoid the
      temporary

    - Document: theta, x, f ordering; document allVariables on DACE
      iterator

 * Implement m_realizer? We implement only what's needed to compute
   log prior, not realize()

 * Can one disable inference of the hyper-parameters?  Option to
   calibrate theta vs. config vars, e.g.,
   mhVarOptions->m_parameterDisabledSet.insert(0);

 * Normalization: Is it truly optional?  It's not clear what normalize
   means in context of functional data?  Do the config vars need to be
   normalized?  Need to verify that normalization works in single
   config and single experiment cases.

 * Functional data omission of scenarios; set to 0.5?
   Need to be sure that no other code is assuming these params don't exist...
   Also need to make sure to send a valid domain

 * BJW: How to handle LHS design generation?  Over all variables or
        just theta?  Probably all variables; that's being assumed in
        some places.

 * Can't use MAP pre-solve since don't have access to the likelihood?
   Perhaps get access through GPMSAFactory?

 * Allow basic vs. advanced user options
   ??? Appears only GPMSA allows default construct with parse override?
   ??? Passed prefix is ignored by some EnvOptions ctors ???
   ??? Can't construct QUESO env with C++ options, then override from file...

 * Terminate handler might be getting called recursively in serial, at
   least when run in debugger.  Perhaps because old_terminate_handler
   is NULL and causing a segfault when called.

 * Emulator mean must be set to something to avoid nan/nan issue.
   Probably want to be able to omit parameters that aren't being
   calibrated statistically...

 * Allow variable Experiment sizes

 * Experiment covariance: correlation or covariance;
   normalized by simulation std deviation as in example?  Does this
   need to be defined if identity?

 * Export chain to named file

 */

namespace Dakota {

// initialization of statics
NonDGPMSABayesCalibration* NonDGPMSABayesCalibration::nonDGPMSAInstance(NULL);

/** This constructor is called for a standard letter-envelope iterator
    instantiation.  In this case, set_db_list_nodes has been called and
    probDescDB can be queried for settings from the method specification. */
NonDGPMSABayesCalibration::
NonDGPMSABayesCalibration(ProblemDescDB& problem_db, Model& model):
  NonDQUESOBayesCalibration(problem_db, model),
  buildSamples(probDescDB.get_int("method.build_samples")),
  approxImportFile(probDescDB.get_string("method.import_build_points_file")),
  approxImportFormat(probDescDB.get_ushort("method.import_build_format")),
  approxImportActiveOnly(
    probDescDB.get_bool("method.import_build_active_only")),
  userConfigVars(expData.num_config_vars()),
  gpmsaConfigVars(std::max(userConfigVars, (unsigned int) 1)),
  gpmsaNormalize(probDescDB.get_bool("method.nond.gpmsa_normalize"))
{
  bool found_error = false;

  // Input spec should prevent this, but be sure.  It's possible
  // through user input that the inbound model is of type surrogate
  // and that's okay.
  if (emulatorType != NO_EMULATOR) {
    Cerr << "\nError: Dakota emulators not supported with GPMSA\n";
    found_error = true;
  }

  // TODO: Do we want to allow a single field group to allow the full
  // multi-variate case?
  const SharedResponseData& srd = model.current_response().shared_data();
  if (srd.num_field_response_groups() > 0 && outputLevel >= NORMAL_OUTPUT)
    Cout << "\nWarning: GPMSA does not yet treat field_responses; they will be "
	 << "treated as a\n         single multivariate response set."
	 << std::endl;

  // REQUIRE experiment data
  if (expData.num_experiments() < 1) {
    Cerr << "\nError: GPMSA requires experimental data\n";
    found_error = true;
  }

  if (userConfigVars > 0 && !approxImportFile.empty() &&
      approxImportActiveOnly && outputLevel >= NORMAL_OUTPUT)
    Cout << "\nWarning: Experimental data presented to GPMSA has configuration variables, but\n         simulation data import specifies active_only, so nominal values of\n         configuration variables will be used." << std::endl;

  if (found_error)
    abort_handler(METHOD_ERROR);

  // TODO: use base class to manage any problem transformations and
  // probably the surrogate build data management

  // TODO: conditionally enable sampler only if needed to augment
  // samples and allow both to be specified
  // if (approxImportFile.empty())
  //   buildSamples = probDescDB.get_int("method.build_samples");
  // else buildSamples will get set after reading the file at run-time

  // BMA TODO: should we always instantiate this or not?  Allow augmentation?
  int samples = approxImportFile.empty() ? buildSamples : 0;
  const String& rng = probDescDB.get_string("method.random_number_generator");
  unsigned short sample_type = SUBMETHOD_DEFAULT;
  lhsIter.assign_rep(new NonDLHSSampling(mcmcModel, sample_type, samples,
					 randomSeed, rng), false);
}

NonDGPMSABayesCalibration::~NonDGPMSABayesCalibration()
{ }

void NonDGPMSABayesCalibration::derived_init_communicators(ParLevLIter pl_iter)
{
  // lhsIter uses NoDBBaseConstructor, so no need to manage DB list nodes
  // at this level
  lhsIter.init_communicators(pl_iter);
  NonDBayesCalibration::derived_init_communicators(pl_iter);
}

void NonDGPMSABayesCalibration::derived_set_communicators(ParLevLIter pl_iter)
{
  // lhsIter uses NoDBBaseConstructor, so no need to manage DB list nodes
  // at this level
  lhsIter.set_communicators(pl_iter);
  NonDBayesCalibration::derived_set_communicators(pl_iter);
}

void NonDGPMSABayesCalibration::derived_free_communicators(ParLevLIter pl_iter)
{
  NonDBayesCalibration::derived_free_communicators(pl_iter);
  lhsIter.free_communicators(pl_iter);
}

/** Perform the uncertainty quantification */
void NonDGPMSABayesCalibration::calibrate()
{
  // TODO: base class needs runtime update as well; decouple these:
  nonDQUESOInstance = this;
  nonDGPMSAInstance = this;

  if (outputLevel >= NORMAL_OUTPUT)
    Cout << ">>>>> GPMSA: Setting up calibration." << std::endl;

  // no emulators will be setup, but need to initialize the prob transforms
  initialize_model();

  // paramSpace, paramMins/paramMaxs, paramDomain, paramInitials, priorRV
  // Note: The priorRV is only defined over the calibration parameters
  // (and any Dakota-managed hyper-parameters)
  init_parameter_domain();

  // proposal may depend on the parameter space properties
  init_proposal_covariance();

  // GPMSA scenario space = configuration space. GPMSA requires at least 1
  // configuration variable, set to 0.5 for all scenarios if needed
  configSpace.reset(new QUESO::VectorSpace<GslVector, GslMatrix>
		    (*quesoEnv, "scenario_", gpmsaConfigVars, NULL));

  // GPMSA output space.  TODO: The simulation output space eta won't
  // always have same size as experiment space.  Moreover, each
  // experiment might have different size. Generalize this to
  // multi-experiment of varying size, and fields.
  unsigned int num_eta = numFunctions;
  nEtaSpace.reset(new QUESO::VectorSpace<GslVector, GslMatrix>
                  (*quesoEnv, "output_", num_eta, NULL));

  // BMA TODO: manage experiment size (each experiment need not have
  // the same size and one sim may inform multiple experiments through
  // DataTransform...)
  unsigned int experiment_size = expData.all_data(0).length();
  experimentSpace.reset(new QUESO::VectorSpace<GslVector, GslMatrix>
                        (*quesoEnv,"experimentspace_", experiment_size, NULL));

  // Construct with default options
  // default constructed options will have recommended settings, then
  // we can override via C++ API or input file (parse)
  gpmsaOptions.reset(new QUESO::GPMSAOptions());

  // C++ API options may override defaults
  // insert Dakota parameters here: gpmsaOptions.m_emulatorPrecisionShape

  // TODO: user option for min/max vs. mu/sigma?  Also, when user
  // gives bounds on distro or bounds on configs instead of data,
  // should we use those?  They might not be finite... For that
  // matter, autoscale may break on infinite domains?
  if (gpmsaNormalize) {
    for (unsigned int i = 0; i < (numContinuousVars + numHyperparams); ++i)
      gpmsaOptions->set_autoscale_minmax_uncertain_parameter(i);
    for (unsigned int i = 0; i < gpmsaConfigVars; ++i)
      gpmsaOptions->set_autoscale_minmax_scenario_parameter(i);
    for (unsigned int i = 0; i < num_eta; i++)
      gpmsaOptions->set_autoscale_meanvar_output(i);
  }

  // File-based power user parameters have the final say
  if (!advancedOptionsFile.empty())
    gpmsaOptions->parse(*quesoEnv, "");

  if (outputLevel >= DEBUG_OUTPUT)
    Cout << "\nGPMSA Final Options:" << *gpmsaOptions << std::endl;

  gpmsaFactory.reset(new QUESO::GPMSAFactory<GslVector, GslMatrix>
                     (*quesoEnv, gpmsaOptions.get(), *priorRv, *configSpace,
                      *paramSpace, *nEtaSpace, buildSamples,
                      expData.num_experiments()));

  // Populate simulation build data and experiment data
  fill_simulation_data();
  fill_experiment_data();

  // solver setup must follow factory instantiation
  init_queso_solver();

  // Override only the calibration parameter values with the ones the
  // user specified (or TODO: the map point if we pre-solve)
  GslVector full_param_initials(
    gpmsaFactory->prior().imageSet().vectorSpace().zeroVector());
  // Initial condition of the chain: overlay user params on default GPMSA values
  overlay_initial_params(full_param_initials);

  if (outputLevel >= VERBOSE_OUTPUT)
    Cout << "INFO (GPMSA): Final initial point\n [ "
	 << full_param_initials << " ]" << std::endl;

  GslMatrix full_proposal_cov(
    gpmsaFactory->prior().imageSet().vectorSpace().zeroVector());
  overlay_proposal_covariance(full_proposal_cov);

  if (outputLevel >= VERBOSE_OUTPUT)
    Cout << "INFO (GPMSA): Final proposal covariance matrix\n [ "
	 << full_proposal_cov << " ]" << std::endl;

  if (outputLevel >= NORMAL_OUTPUT) {
    Cout << ">>>>> GPMSA: Performing calibration with " << mcmcType << " using "
       << calIpMhOptionsValues->m_rawChainSize << " MCMC samples." << std::endl;
    if (outputLevel > NORMAL_OUTPUT)
      Cout << "\n  Calibrating " << numHyperparams << " error hyperparameters."
	   << std::endl;
  }

  inverseProb->solveWithBayesMetropolisHastings(calIpMhOptionsValues.get(),
                                                full_param_initials,
                                                &full_proposal_cov);

  if (outputLevel >= NORMAL_OUTPUT) {
    Cout << ">>>>> GPMSA: Calibration complete. Generating statistics and ouput.\n";

    Cout << "  Info: MCMC details are in the QuesoDiagnostics directory:\n"
	 << "          display_sub0.txt contains MCMC diagnostics\n";
    if (standardizedSpace)
      Cout << "          Matlab files contain chain values (in "
	   << "standardized probability space)\n";
    else
      Cout << "          Matlab files contain chain values\n";

    Cout << "  Info: GPMSA cannot currently retrieve response function statistics."
	 << std::endl;
  }

  cache_acceptance_chain();
  compute_statistics();
}

void NonDGPMSABayesCalibration::init_queso_solver()
{
  // Note: The postRv includes GP-associated hyper-parameters
  postRv.reset(new QUESO::GenericVectorRV<GslVector, GslMatrix>
               ("post_", gpmsaFactory->prior().imageSet().vectorSpace()));

  // TODO: consider what options are relevant for GPMSA vs. QUESO
  set_ip_options();
  set_mh_options();

  inverseProb.reset(new QUESO::StatisticalInverseProblem<GslVector, GslMatrix>
                    ("", calIpOptionsValues.get(), *gpmsaFactory, *postRv));
}

void NonDGPMSABayesCalibration::
overlay_proposal_covariance(GslMatrix& full_prop_cov) const
{
  // Start with the covariance matrix for the whole prior, including
  // GPMSA hyper-parameters.
  gpmsaFactory->prior().pdf().distributionVariance(full_prop_cov);

  if (outputLevel >= DEBUG_OUTPUT)
    Cout << "INFO (GPMSA): Proposal covariance matrix from GPMSA prior:\n [ "
	 << full_prop_cov << " ]" << std::endl;

  // Now override with user (or iterative algorithm-updated values)
  unsigned int num_calib_params = numContinuousVars + numHyperparams;
  for (unsigned int i=0; i<num_calib_params; ++i)
    for (unsigned int j=0; j<num_calib_params; ++j)
      full_prop_cov(i,j) = (*proposalCovMatrix)(i,j);

  if (outputLevel >= DEBUG_OUTPUT)
    Cout << "INFO (GPMSA): Proposal covariance matrix after overlay: [ \n"
	 << full_prop_cov << " ]" << std::endl;

  // Power users can override the covariance values with a file
  std::string initial_cov_filename =
    "initial_proposal_covariance_sub" + quesoEnv->subIdString();
  if (bfs::exists(initial_cov_filename + ".m")) {
    std::set<unsigned int> sid_set;
    sid_set.insert(quesoEnv->subId());
    full_prop_cov.subReadContents(initial_cov_filename, "m", sid_set);
    if (outputLevel >= NORMAL_OUTPUT)
      Cout << "INFO (GPMSA): Initial proposal covariance overridden with values"
	   << " from " << (initial_cov_filename + ".m") << std::endl;
  }
}

void NonDGPMSABayesCalibration::
overlay_initial_params(GslVector& full_param_initials)
{
  // Start with the mean of the prior
  gpmsaFactory->prior().pdf().distributionMean(full_param_initials);

  if (outputLevel >= DEBUG_OUTPUT)
    Cout << "INFO (GPMSA): Initial point from GPMSA prior:\n [ "
	 << full_param_initials << " ]" << std::endl;

  // But override whatever we want, e.g., with user-specified values:
  unsigned int num_calib_params = numContinuousVars + numHyperparams;;
  for (unsigned int i=0; i<num_calib_params; ++i)
    full_param_initials[i] = (*paramInitials)[i];

  // Example of manually adjusting initial values:
  // NOTE: This can also be done via file read in QUESO input file

  // full_param_initials[num_calib_params + 0]  = 0.4; // Emulator mean (unused, but set to avoid NaN!)
  // full_param_initials[num_calib_params + 1]  = 0.4; // emulator precision
  // full_param_initials[num_calib_params + 2]  = 0.4; // weights0 precision
  // full_param_initials[num_calib_params + 3]  = 0.4; // weights1 precision
  // full_param_initials[num_calib_params + 4]  = 0.97; // emulator corr str
  // full_param_initials[num_calib_params + 5]  = 0.97; // emulator corr str
  // full_param_initials[num_calib_params + 6]  = 0.97; // emulator corr str
  // full_param_initials[num_calib_params + 7]  = 0.97; // emulator corr str
  // full_param_initials[num_calib_params + 8]  = 0.20; // emulator corr str
  // full_param_initials[num_calib_params + 9]  = 0.80; // emulator corr str
  // full_param_initials[num_calib_params + 10] = 10.0; // discrepancy precision
  // full_param_initials[num_calib_params + 11] = 0.97; // discrepancy corr str
  // full_param_initials[num_calib_params + 12] = 8000.0; // emulator data precision
  // full_param_initials[num_calib_params + 13] = 1.0;  // observation error precision

  if (outputLevel >= DEBUG_OUTPUT)
    Cout << "INFO (GPMSA): Initial point after overlay:\n [ "
	 << full_param_initials << " ]" << std::endl;

  // Power users can override the initial values with a file
  std::string initial_point_filename =
    "initial_point_sub" + quesoEnv->subIdString();
  if (bfs::exists(initial_point_filename + ".m")) {
    std::set<unsigned int> sid_set;
    sid_set.insert(quesoEnv->subId());
    full_param_initials.subReadContents(initial_point_filename, "m", sid_set);
    if (outputLevel >= NORMAL_OUTPUT)
      Cout << "INFO (GPMSA): Initial point overridden with values from "
	   << (initial_point_filename + ".m") << std::endl;
  }
}

void NonDGPMSABayesCalibration::acquire_simulation_data(RealMatrix& sim_data)
{
  if (outputLevel >= NORMAL_OUTPUT)
    Cout << ">>>>> GPMSA: Acquiring simulation data." << std::endl;

  // Rationale: Trying to keep this agnostic to mocked up config vars...

  sim_data.shape(buildSamples,
		 numContinuousVars + userConfigVars + numFunctions);

  if (approxImportFile.empty()) {

    // NOTE: Assumes the design is performed over the config vars
    // TODO: make this modular on the dimensions of config vars...
    lhsIter.run(methodPCIter->mi_parallel_level_iterator(miPLIndex));
    const RealMatrix&  all_samples = lhsIter.all_samples();
    const IntResponseMap& all_resp = lhsIter.all_responses();

    if (all_samples.numCols() != buildSamples ||
        all_resp.size() != buildSamples) {
      Cerr << "\nError: GPMSA has insufficient surrogate build data.\n";
      abort_handler(-1);
    }

    IntRespMCIter resp_it = all_resp.begin();
    for (unsigned int i = 0; i < buildSamples; i++, ++resp_it) {
      for (int j=0; j<numContinuousVars; ++j)
	sim_data(i, j) = all_samples(j, i);
      for (int j=0; j<userConfigVars; ++j)
	sim_data(i, numContinuousVars + j) =
	  all_samples(numContinuousVars + j, i);
      for (int j=0; j<numFunctions; ++j)
	sim_data(i, numContinuousVars + userConfigVars + j) =
	  resp_it->second.function_values()[j];
    }

  }
  else {

    // Read surrogate build data ( theta, [x, ], fns ) depending on
    // active_only and number of user-specified config vars.  If
    // reading active_only, have to assume all simulations conducted
    // at nominal state variable values.
    size_t record_len = (approxImportActiveOnly) ?
      numContinuousVars + numFunctions :
      numContinuousVars + userConfigVars + numFunctions;
    if (outputLevel >= NORMAL_OUTPUT)
      Cout << "GPMSA: Importing simulation data from '" << approxImportFile
	   << "'\n       with " << numContinuousVars
	   << " calibration variable(s), " << userConfigVars
	   << " configuration variable(s),\n       and " << numFunctions
	   << " simulation output(s)." << std::endl;
    bool verbose = (outputLevel > NORMAL_OUTPUT);
    TabularIO::read_data_tabular(approxImportFile, "GMPSA simulation data",
				 sim_data, buildSamples, record_len,
				 approxImportFormat, verbose);
    // TODO: Have to fill in configuration variable values for
    // active_only, or error and move the function data over if so...
  }

}

void NonDGPMSABayesCalibration::fill_simulation_data()
{
  // simulations are described by configuration, parameters, output values
  std::vector<QUESO::SharedPtr<GslVector>::Type >
    sim_scenarios(buildSamples),  // config var values
    sim_params(buildSamples),     // theta var values
    sim_outputs(buildSamples);    // simulation output (response) values

  // Instantiate each of the simulation points/outputs
  for (unsigned int i = 0; i < buildSamples; i++) {
    sim_scenarios[i].reset(new GslVector(configSpace->zeroVector()));
    sim_params[i].reset(new GslVector(paramSpace->zeroVector()));
    sim_outputs[i].reset(new GslVector(nEtaSpace->zeroVector())); // eta
  }

  /// simulation data, one row per simulation build sample, columns
  /// for calibration variables, configuration variables, function
  /// values (duplicates storage, but unifies import vs. DOE cases)
  RealMatrix sim_data;
  acquire_simulation_data(sim_data);

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

    for (int j=0; j<numContinuousVars; ++j)
       (*sim_params[i])[j] = sim_data(i, j);

    for (int j=0; j<gpmsaConfigVars; ++j)
       if (userConfigVars > 0)
	 (*sim_scenarios[i])[j] = sim_data(i, numContinuousVars + j);
       else
	 (*sim_scenarios[i])[j] = 0.5;

    for (int j=0; j<numFunctions; ++j)
      (*sim_outputs[i])[j] =
	sim_data(i, numContinuousVars + userConfigVars + j);

  }

  gpmsaFactory->addSimulations(sim_scenarios, sim_params, sim_outputs);
}

void NonDGPMSABayesCalibration::fill_experiment_data()
{
  unsigned int num_experiments = expData.num_experiments();
  unsigned int experiment_size = expData.all_data(0).length();

  // characterization of the experiment data
  std::vector<QUESO::SharedPtr<GslVector>::Type >
   exp_scenarios(num_experiments),   // config var values
   exp_outputs(num_experiments);        // experiment (response) values

  // BMA: Why is sim_outputs based on nEtaSpace?  Is simulation allowed
  // to be differently sized from experiments?

  // Load the information on the experiments (scenarios and data)
  const RealVectorArray& exp_config_vars = expData.config_vars();
  for (unsigned int i = 0; i < num_experiments; i++) {

    exp_scenarios[i].reset(new GslVector(configSpace->zeroVector()));
    exp_outputs[i].reset(new GslVector(experimentSpace->zeroVector()));

    // NOTE: copy_gsl will resize the target objects rather than erroring.
    if (userConfigVars > 0)
      copy_gsl(exp_config_vars[i], *exp_scenarios[i]);
    else
      (*exp_scenarios[i])[0] = 0.5;

    copy_gsl(expData.all_data(i), *exp_outputs[i]);

  }
  // TODO: experiment space may not be numFunctions in field case

  // Experimental observation error covariance (default = I)
  QUESO::VectorSpace<GslVector, GslMatrix>
    total_exp_space(*quesoEnv, "experimentspace_",
                    num_experiments * experiment_size, NULL);
  QUESO::SharedPtr<GslMatrix>::Type exp_covariance
    (new GslMatrix(total_exp_space.zeroVector(), 1.0));

  if (expData.variance_active()) {
    for (unsigned int i = 0; i < num_experiments; i++) {
      RealSymMatrix exp_cov;
      expData.covariance(i, exp_cov);
      for (unsigned int j = 0; j < experiment_size; j++) {
        for (unsigned int k = 0; k < experiment_size; k++) {
          (*exp_covariance)(experiment_size*i+j, experiment_size*i+k) =
            exp_cov(j,k);
        }
      }
    }
  }

  gpmsaFactory->addExperiments(exp_scenarios, exp_outputs, exp_covariance);

}

/** This is a subset of the base class retrieval, but we can't do the
    fn value lookups.  Eventually should be able to retrieve them from
    GPMSA. */
void NonDGPMSABayesCalibration::cache_acceptance_chain()
{
  int num_params = numContinuousVars + numHyperparams;
  int total_chain_length = chainSamples * chainCycles;

  const QUESO::BaseVectorSequence<QUESO::GslVector,QUESO::GslMatrix>&
    mcmc_chain = inverseProb->chain();
  unsigned int num_mcmc = mcmc_chain.subSequenceSize();

  if (num_mcmc != total_chain_length && outputLevel >= NORMAL_OUTPUT) {
    Cout << "GPMSA Warning: Final chain is length " << num_mcmc
	 << ", not expected length " << total_chain_length << std::endl;
  }

  acceptanceChain.shapeUninitialized(numContinuousVars + numHyperparams,
				     total_chain_length);
  acceptedFnVals.shapeUninitialized(numFunctions, total_chain_length);

  // The posterior includes GPMSA hyper-parameters, so use the postRv space
  QUESO::GslVector qv(postRv->imageSet().vectorSpace().zeroVector());
  RealVector nan_fn_vals(numFunctions);
  nan_fn_vals = std::numeric_limits<double>::quiet_NaN();

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

    // translate the QUESO vector into x-space acceptanceChain
    mcmc_chain.getPositionValues(i, qv); // extract GSLVector from sequence
    if (standardizedSpace) {
      // u_rv and x_rv omit any hyper-parameters
      RealVector u_rv(numContinuousVars, false);
      copy_gsl_partial(qv, 0, u_rv);
      Real* acc_chain_i = acceptanceChain[i];
      RealVector x_rv(Teuchos::View, acc_chain_i, numContinuousVars);
      natafTransform.trans_U_to_X(u_rv, x_rv);
      for (int j=numContinuousVars; j<num_params; ++j)
	acc_chain_i[j] = qv[j]; // trailing hyperparams are not transformed
    }
    else {
      // A view that includes calibration params and Dakota-managed
      // hyper-parameters, to facilitate copying from the longer qv
      // into acceptanceChain:
      RealVector theta_hp(Teuchos::View, acceptanceChain[i], num_params);
      copy_gsl_partial(qv, 0, theta_hp);
    }

    // TODO: Find a way to set meaningful function values
    Teuchos::setCol(nan_fn_vals, i, acceptedFnVals);
  }
}

void NonDGPMSABayesCalibration::
print_results(std::ostream& s, short results_state)
{
  //  TODO: additional GPMSA output

  NonDBayesCalibration::print_results(s, results_state);
}

} // namespace Dakota


//
// Created by gzhang8 on 5/5/20.
//

#include <string>
// #include <Eigen/Core>
#include <Eigen/Geometry>
#include <pangolin/pangolin.h>
#include <pangolin/display/display_internal.h>

extern "C"
{

void pangolin_create_window(char* window_name, int width, int height, int panel) {

    if (pangolin::FindContext(std::string(window_name)) != NULL){
        return;
    }

    width += panel;

    pangolin::Params windowParams;

    windowParams.Set("SAMPLE_BUFFERS", 0);
    windowParams.Set("SAMPLES", 0);

    pangolin::CreateWindowAndBind(std::string(window_name), width, height, windowParams);

}

void* pangolin_create_render_state(
        double* proj_mat_ptr,
        double* model_view_ptr
        ){
    // Define Projection and initial ModelView matrix
    pangolin::OpenGlRenderState* s_cam_ptr =  new pangolin::OpenGlRenderState(
        pangolin::OpenGlMatrix::ColMajor4x4(proj_mat_ptr),
        pangolin::OpenGlMatrix::ColMajor4x4(model_view_ptr)
    );

    return reinterpret_cast<void*>(s_cam_ptr);
}

void* pangolin_create_render_state_no_params(){
    // Define Projection and initial ModelView matrix
    pangolin::OpenGlRenderState* s_cam_ptr =  new pangolin::OpenGlRenderState(
             pangolin::ProjectionMatrix(640,480,525,525,320,240,0.1,1000),
             pangolin::ModelViewLookAt(0, 0 , -1, 0, 0, 1, pangolin::AxisNegY)
    );

    return reinterpret_cast<void*>(s_cam_ptr);
}

void pangolin_get_projection_model_view_matrix(void* s_cam_void_ptr, double* out){
    pangolin::OpenGlRenderState* s_cam_ptr = (pangolin::OpenGlRenderState*)s_cam_void_ptr;
    memcpy(out, s_cam_ptr->GetProjectionModelViewMatrix().m, 16*sizeof(double));

}

void pangolin_set_model_view_matrix(void* s_cam_void_ptr, double* in){
    pangolin::OpenGlRenderState* s_cam_ptr = (pangolin::OpenGlRenderState*)s_cam_void_ptr;
    s_cam_ptr->SetModelViewMatrix(pangolin::OpenGlMatrix::ColMajor4x4(in));
}

void pangolin_set_model_view_matrix_with_conversion(
        void* s_cam_void_ptr, double* in) {
    pangolin::OpenGlMatrix mv;

    Eigen::Map<Eigen::Matrix<double,4,4,Eigen::ColMajor> > currPose(in);
    Eigen::Matrix3d currRot = currPose.topLeftCorner(3, 3);

    Eigen::Quaterniond currQuat(currRot);
    Eigen::Vector3d forwardVector(0, 0, 1);
    bool iclnuim = false;
    Eigen::Vector3d upVector(0, iclnuim ? 1 : -1, 0);

    Eigen::Vector3d forward = (currQuat * forwardVector).normalized();
    Eigen::Vector3d up = (currQuat * upVector).normalized();

    Eigen::Vector3d eye(currPose(0, 3), currPose(1, 3), currPose(2, 3));

    eye -= forward;

    Eigen::Vector3d at = eye + forward;

    Eigen::Vector3d z = (eye - at).normalized();  // Forward
    Eigen::Vector3d x = up.cross(z).normalized(); // Right
    Eigen::Vector3d y = z.cross(x);

    Eigen::Matrix4d m;// = fix_view_pose.cast<double>();
    m << x(0),  x(1),  x(2),  -(x.dot(eye)),
        y(0),  y(1),  y(2),  -(y.dot(eye)),
        z(0),  z(1),  z(2),  -(z.dot(eye)),
        0,     0,     0,              1;

    memcpy(&mv.m[0], m.data(), sizeof(Eigen::Matrix4d));

    pangolin::OpenGlRenderState* s_cam_ptr = (pangolin::OpenGlRenderState*)s_cam_void_ptr;
    s_cam_ptr->SetModelViewMatrix(mv);
}

void* pangolin_create_handler3d(void* renderstate_ptr){
    pangolin::OpenGlRenderState* s_cam_ptr = reinterpret_cast<pangolin::OpenGlRenderState*>(renderstate_ptr);
    pangolin::Handler3D* handler_ptr = new pangolin::Handler3D(*s_cam_ptr); //(s_cam);
    return (void*) handler_ptr;
}

// return a view in window
void* pangolin_create_display(void* handler_void_ptr){

    // Create Interactive View in window
    pangolin::Handler3D* handler_ptr = (pangolin::Handler3D*)handler_void_ptr;
    pangolin::View& d_cam = pangolin::CreateDisplay()
            .SetBounds(0.0, 1.0, 0.0, 1.0, -640.0f/480.0f)
            .SetHandler(handler_ptr);

    return &d_cam;
}

// 1 means true; 0 -> false
int pangolin_should_quit(){
    int res = pangolin::ShouldQuit()? 1:0;
    // reset flag
    pangolin::GetCurrentContext()->quit = false;
    return res;
}

// first is a view: d_cam
// second is a rendersate: s_cam
void pangolin_view_active_render_sate(void* view_void_ptr, void* rendersate_ptr){
    pangolin::OpenGlRenderState* s_cam_ptr = reinterpret_cast<pangolin::OpenGlRenderState*>(rendersate_ptr);
    pangolin::View* d_cam_ptr = reinterpret_cast<pangolin::View*>(view_void_ptr);
    d_cam_ptr->Activate(*s_cam_ptr);

}

void pangolin_view_show(void* view_void_ptr, int flag){
    pangolin::View* d_cam_ptr = reinterpret_cast<pangolin::View*>(view_void_ptr);
    d_cam_ptr->Show(flag != 0);
}

// Swap frames and Process Events
void pangolin_finish_frame(){
    pangolin::FinishFrame();
}

void pangolin_destroy_window(char* window_name){
    pangolin::DestroyWindow(std::string(window_name));
}

// for testing
void pangolin_glDrawColouredCube(){
    pangolin::glDrawColouredCube();

}

int make_window_context_current(char* window_name){
    pangolin::PangolinGl * ctx = pangolin::FindContext(std::string(window_name));
    if (ctx != NULL){
        ctx->MakeCurrent();
        return 1;
    } else {
        printf("Warning: cannot find the window name");
        return 0;
    }
}

} // end extern C


#include "Checkpoint_Parser.hxx"

#include "../../../Verbosity.hxx"
#include "../../../ostream_vector.hxx"
#include "../../../ostream_set.hxx"

#include <El.hpp>

#include <rapidjson/istreamwrapper.h>
#include <boost/algorithm/string/predicate.hpp>
#include <boost/optional.hpp>
#include <boost/filesystem.hpp>
#include <boost/iostreams/device/file.hpp>
#include <boost/iostreams/filtering_stream.hpp>
#include <boost/iostreams/filter/gzip.hpp>

#include <algorithm>

void load_checkpoint(
  const boost::filesystem::path &checkpoint_directory,
  const Verbosity &verbosity, boost::optional<int64_t> &backup_generation,
  int64_t &current_generation,
  El::DistMatrix<El::BigFloat, El::STAR, El::STAR> &yp_to_y_star,
  El::DistMatrix<El::BigFloat, El::STAR, El::STAR> &dual_objective_b_star,
  El::Matrix<El::BigFloat> &yp, std::vector<std::set<El::BigFloat>> &points,
  El::BigFloat &threshold, El::BigFloat &primal_c_scale)
{
  if(boost::filesystem::exists(checkpoint_directory)
     && boost::filesystem::is_directory(checkpoint_directory))
    {
      boost::filesystem::directory_iterator entry(checkpoint_directory);
      int64_t max_generation(-1);
      const std::string prefix("checkpoint_"), suffix(".json.gz");
      for(boost::filesystem::directory_iterator entry(checkpoint_directory);
          entry != boost::filesystem::directory_iterator(); ++entry)
        {
          const std::string name(entry->path().filename().string());
          if(boost::algorithm::starts_with(name, "checkpoint_"))
            {
              max_generation
                = std::max(max_generation,
                           int64_t(std::stoll(name.substr(prefix.size()))));
            }
        }
      if(max_generation != -1)
        {
          backup_generation = max_generation;
          current_generation = max_generation + 1;

          const std::string checkpoint_name(
            (checkpoint_directory
             / (prefix + std::to_string(max_generation) + suffix))
              .string());

          if(El::mpi::Rank() == 0 && verbosity >= Verbosity::regular)
            {
              std::cout << "Loading checkpoint: " << checkpoint_name << "\n";
            }

          boost::iostreams::filtering_istream input_stream;
          input_stream.push(boost::iostreams::gzip_decompressor());
          input_stream.push(boost::iostreams::file_source(checkpoint_name));

          rapidjson::IStreamWrapper wrapper(input_stream);
          Checkpoint_Parser parser;
          rapidjson::Reader reader;
          reader.Parse(wrapper, parser);

          if(parser.generation_state.value != El::BigFloat(max_generation))
            {
              std::stringstream ss;
              ss << "Invalid checkpoint.  The generation from the file name "
                 << max_generation
                 << " does not match the generation stored inside: "
                 << parser.generation_state.value;
              throw std::runtime_error(ss.str());
            }

          yp_to_y_star.Resize(parser.y_transform_state.value.size(),
                              parser.y_transform_state.value.size());
          for(int64_t row(0); row < yp_to_y_star.LocalHeight(); ++row)
            {
              for(int64_t column(0); column < yp_to_y_star.LocalWidth();
                  ++column)
                {
                  yp_to_y_star.SetLocal(
                    row, column,
                    parser.y_transform_state.value.at(row).at(column));
                }
            }

          dual_objective_b_star.Resize(parser.b_state.value.size(), 1);
          for(int64_t row(0); row < dual_objective_b_star.Height(); ++row)
            {
              dual_objective_b_star.SetLocal(row, 0,
                                             parser.b_state.value[row]);
            }

          yp.Resize(parser.yp_state.value.size(), 1);
          for(int64_t row(0); row < yp.Height(); ++row)
            {
              yp(row, 0) = parser.yp_state.value[row];
            }

          points.clear();
          for(auto &block : parser.points_state.value)
            {
              points.emplace_back();
              auto &back(points.back());
              for(auto &point : block)
                {
                  back.emplace(point);
                }
            }
          threshold = parser.threshold_state.value;
          primal_c_scale = parser.c_scale_state.value;
        }
    }
}


#include "cnn/nodes.h"
#include "cnn/cnn.h"
#include "cnn/rnn.h"
#include "cnn/gru.h"
#include "cnn/lstm.h"
#include "cnn/training.h"
#include "cnn/gpu-ops.h"
#include "cnn/expr.h"

#include "utils.h"
#include "enc-dec-attn.h"

#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>

#include <iostream>
#include <fstream>
#include <unordered_map>

using namespace std;
using namespace cnn;
using namespace cnn::expr;

int main(int argc, char** argv) {
  cnn::Initialize(argc, argv);

  string vocab_filename = argv[1];  // vocabulary of words/characters
  string morph_filename = argv[2];
  string train_filename = argv[3];
  string test_filename = argv[4];
  unsigned hidden_size = atoi(argv[5]);
  unsigned num_iter = atoi(argv[6]);
  float reg_strength = atof(argv[7]);
  unsigned layers = atoi(argv[8]);
  string model_outputfilename = argv[9];

  unordered_map<string, unsigned> char_to_id, morph_to_id;
  unordered_map<unsigned, string> id_to_char, id_to_morph;

  ReadVocab(vocab_filename, &char_to_id, &id_to_char);
  unsigned vocab_size = char_to_id.size();
  ReadVocab(morph_filename, &morph_to_id, &id_to_morph);
  unsigned morph_size = morph_to_id.size();

  vector<string> train_data;  // Read the training file in a vector
  ReadData(train_filename, &train_data);

  vector<string> test_data;  // Read the dev file in a vector
  ReadData(test_filename, &test_data);

  vector<Model*> m;
  vector<AdadeltaTrainer> optimizer;

  for (unsigned i = 0; i < morph_size; ++i) {
    m.push_back(new Model());
    AdadeltaTrainer ada(m[i], reg_strength);
    optimizer.push_back(ada);
  }

  unsigned char_size = vocab_size;
  EncDecAttn nn(char_size, hidden_size, vocab_size, layers, morph_size,
              &m, &optimizer);

  // Pre-train all models on all datasets.
  /*cerr << "Pre-training... " << endl;
  for (unsigned morph_id = 0; morph_id < morph_size; ++morph_id) {
    cerr << "Morph class: " << morph_id + 1 << endl;
    for (string& line : train_data) {
      vector<string> items = split_line(line, '|');
      vector<unsigned> input_ids, target_ids;
      input_ids.clear(); target_ids.clear();
      for (const string& ch : split_line(items[0], ' ')) {
        input_ids.push_back(char_to_id[ch]);
      }
      for (const string& ch : split_line(items[1], ' ')) {
        target_ids.push_back(char_to_id[ch]);
      }
      //unsigned morph_id = morph_to_id[items[2]];
      double temp_loss = nn.Train(morph_id, input_ids, target_ids,
                                  &optimizer[morph_id]);
    }
  }
  cerr << "complete." << endl;*/

  // Read the training file and train the model
  double best_score = -1;
  vector<EncDecAttn*> object_list;
  object_list.push_back(&nn);
  for (unsigned iter = 0; iter < num_iter; ++iter) {
    unsigned line_id = 0;
    random_shuffle(train_data.begin(), train_data.end());
    vector<float> loss(morph_size, 0.0f);
    for (string& line : train_data) {
      vector<string> items = split_line(line, '|');
      vector<unsigned> input_ids, target_ids;
      input_ids.clear(); target_ids.clear();
      for (const string& ch : split_line(items[0], ' ')) {
        input_ids.push_back(char_to_id[ch]);
      }
      for (const string& ch : split_line(items[1], ' ')) {
        target_ids.push_back(char_to_id[ch]);
      }
      unsigned morph_id = morph_to_id[items[2]];
      loss[morph_id] += nn.Train(morph_id, input_ids, target_ids,
                                 &optimizer[morph_id]);
      cerr << ++line_id << "\r";
    }

    // Read the test file and output predictions for the words.
    string line;
    double correct = 0, total = 0;
    for (string& line : test_data) {
      vector<string> items = split_line(line, '|');
      vector<unsigned> input_ids, target_ids, pred_target_ids;
      input_ids.clear(); target_ids.clear(); pred_target_ids.clear();
      for (const string& ch : split_line(items[0], ' ')) {
        input_ids.push_back(char_to_id[ch]);
      }
      for (const string& ch : split_line(items[1], ' ')) {
        target_ids.push_back(char_to_id[ch]);
      }
      unsigned morph_id = morph_to_id[items[2]];
      EnsembleDecode(morph_id, char_to_id, input_ids, &pred_target_ids,
                     &object_list);

      string prediction = "";
      for (unsigned i = 0; i < pred_target_ids.size(); ++i) {
        prediction += id_to_char[pred_target_ids[i]];
        if (i != pred_target_ids.size() - 1) {
          prediction += " ";
        }
      }
      if (prediction == items[1]) {
        correct += 1;
      }
      total += 1;
    }
    double curr_score = correct / total;
    cerr << "Iter " << iter + 1 << " " << "Loss: " << accumulate(loss.begin(), loss.end(), 0.);
    cerr << "Prediction Accuracy: " << curr_score << endl;
    if (curr_score > best_score) {
      best_score = curr_score;
      Serialize(model_outputfilename, nn, &m);
    }
  }
  return 1;
}