/**
 * Copyright (c) 2017 Melown Technologies SE
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * *  Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */
/*
 * color.cpp
 */

#include "color.hpp"

#include <boost/numeric/ublas/matrix.hpp>
#include <cmath>

namespace imgproc {

/* class RGBColor */

RGBColor::RGBColor( float r, float g, float b )
    : ublas::vector<float>(3) {
    (*this)(0) = r; (*this)(1) = g; (*this)(2) = b;
}

RGBColor::RGBColor( const ublas::vector<float> & c )
    : ublas::vector<float>( c ) {}

RGBColor::RGBColor( const YCCColor & c )
    : ublas::vector<float>(3) {

    //assert( c(0) <= 1.0 && c(0) >= 0.0 );

    ublas::matrix<float> a( 3, 3 );
    a( 0, 0 ) = 1.f; a( 0, 1 ) = 9.2674E-4f; a( 0, 2 ) = 1.4017f;
    a( 1, 0 ) = 1.f; a( 1, 1 ) =  -0.34370f; a( 1, 2 ) = -0.7142f;
    a( 2, 0 ) = 1.f; a( 2, 1 ) = 1.7722f; a( 2, 2 ) = 9.9022E-4f;

    *this = RGBColor( ublas::prod( a, c ) );

    // clip to fit into rgb gamut
    ublas::vector<float> yccnochroma( 3 );
    yccnochroma(0) = c(0); yccnochroma(1) = yccnochroma(2) = 0.f;
    ublas::vector<float> nochroma = ublas::prod( a, yccnochroma );

    if ( (*this)(0) < 0.f ) {
        ublas::vector<float> diff = *this - nochroma;
        float u = ( 0.f - nochroma(0) ) / ( (*this)(0) - nochroma(0) );
        *this = RGBColor( nochroma + u * diff );
    }

    if ( (*this)(0) > 1.f ) {
        ublas::vector<float> diff = *this - nochroma;
        float u = ( 1.f - nochroma(0) ) / ( (*this)(0) - nochroma(0) );
        *this = RGBColor( nochroma + u * diff );
    }

    if ( (*this)(1) < 0.f ) {
        ublas::vector<float> diff = *this - nochroma;
        float u = ( 0.f - nochroma(1) ) / ( (*this)(1) - nochroma(1) );
        *this = RGBColor( nochroma + u * diff );
    }

    if ( (*this)(1) > 1.f ) {
        ublas::vector<float> diff = *this - nochroma;
        float u = ( 1.f - nochroma(1) ) / ( (*this)(1) - nochroma(1) );
        *this = RGBColor( nochroma + u * diff );
    }

    if ( (*this)(2) < 0.f ) {
        ublas::vector<float> diff = *this - nochroma;
        float u = ( 0.f - nochroma(2) ) / ( (*this)(2) - nochroma(2) );
        *this = RGBColor( nochroma + u * diff );
    }

    if ( (*this)(2) > 1.f ) {
        ublas::vector<float> diff = *this - nochroma;
        float u = ( 1.f - nochroma(2) ) / ( (*this)(2) - nochroma(2) );
        *this = RGBColor( nochroma + u * diff );
    }
}

gil::rgb8_pixel_t RGBColor::rgbpixel() const {

    return gil::rgb8_pixel_t(
        (unsigned char)(round( 0xff * (*this)(0) )),
        (unsigned char)(round( 0xff * (*this)(1) )),
        (unsigned char)(round( 0xff * (*this)(2) )));
}

RGBColor::RGBColor( const gil::rgb8_pixel_t & p )
    : ublas::vector<float>(3) {

    (*this)(0) = (float) p[0] / 0xff;
    (*this)(1) = (float) p[1] / 0xff;
    (*this)(2) = (float) p[2] / 0xff;
}

/* class YCCColor */

YCCColor::YCCColor( float y, float cb, float cr )
    : ublas::vector<float>(3) {
    (*this)(0) = y; (*this)(1) = cb; (*this)(2) = cr;
}

YCCColor::YCCColor( const ublas::vector<float> & c )
    : ublas::vector<float>( c ) {}

YCCColor::YCCColor( const RGBColor & c )
    : ublas::vector<float>(3) {

    /*assert( c(0) <= 1.0 && c(0) >= 0.0 );
    assert( c(1) <= 1.0 && c(1) >= 0.0 );
    assert( c(2) <= 1.0 && c(2) >= 0.0 );*/

    ublas::matrix<float> a( 3, 3 );
    a( 0, 0 ) = 0.299f; a( 0, 1 ) = 0.587f; a( 0, 2 ) = 0.114f;
    a( 1, 0 ) = -0.169f; a( 1, 1 ) = -0.331f; a( 1, 2 ) = 0.500f;
    a( 2, 0 ) = 0.500f; a( 2, 1 ) = -0.419f; a( 2, 2 ) = -0.081f;

    //std::cout << ublas::prod( a, c );

    *this = YCCColor( ublas::prod( a, c ) );

    //std::cout << " =? " << *this << std::endl;
}

YCCColor::YCCColor( const gil::rgb8_pixel_t & yccpixel )
    : ublas::vector<float>(3) {

    (*this)(0) = ( (float) yccpixel[0] / 0xff ) - 0.5f;
    (*this)(1) = ( (float) yccpixel[1] / 0xff ) - 0.5f;
    (*this)(2) = ( (float) yccpixel[2] / 0xff ) - 0.5f;
}

YCCColor::YCCColor( const gil::rgb32f_pixel_t & yccpixel )
    : ublas::vector<float>(3) {

    (*this)(0) = ( yccpixel[0] / 0xff ) - 0.5f;
    (*this)(1) = ( yccpixel[1] / 0xff ) - 0.5f;
    (*this)(2) = ( yccpixel[2] / 0xff ) - 0.5f;
}

gil::rgb8_pixel_t YCCColor::yccpixel() const {

    return gil::rgb8_pixel_t(
        (int) round( 0xff * (*this)(0) ),
        (int) round( 0xff * ( (*this)(1) + 0.5f ) ),
        (int) round( 0xff * ( (*this)(2) + 0.5f ) ) );
}

float ccDiff( const YCCColor & color1, const YCCColor & color2 ) {

    float hue1, hue2;

    hue1 = atan2( color1( 2 ), color1( 1 ) );

    hue2 = atan2( color2( 2 ), color2( 1 ) );

    return std::min( fabs( hue1 - hue2 ), float(2 * M_PI) - fabs( hue1 - hue2 ) );

    //return sqr( color2( 1 ) - color1( 1 ) ) + sqr( color2( 2 ) - color1( 2 ) );
}

} // namespace imgproc


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_IEEE_FUNCTIONS_SIMD_COMMON_LDEXP_HPP_INCLUDED
#define BOOST_SIMD_IEEE_FUNCTIONS_SIMD_COMMON_LDEXP_HPP_INCLUDED

#include <boost/simd/ieee/functions/ldexp.hpp>
#include <boost/simd/include/functions/simd/shift_left.hpp>
#include <boost/simd/include/functions/simd/multiplies.hpp>
#include <boost/simd/include/functions/simd/plus.hpp>
#include <boost/simd/include/functions/simd/seladd.hpp>
#include <boost/simd/include/functions/simd/selsub.hpp>
#include <boost/simd/include/functions/simd/is_equal.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/simd/include/functions/simd/rshl.hpp>
#include <boost/simd/include/functions/simd/multiplies.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/nbmantissabits.hpp>
#include <boost/simd/include/constants/maxexponent.hpp>
#include <boost/simd/include/constants/limitexponent.hpp>
#include <boost/simd/sdk/meta/as_logical.hpp>
#include <boost/simd/sdk/config.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/dispatch/attributes.hpp>

#ifndef BOOST_SIMD_NO_DENORMALS
#include <boost/simd/include/functions/simd/is_less.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/selsub.hpp>
#include <boost/simd/include/constants/minexponent.hpp>
#include <boost/simd/include/constants/smallestposval.hpp>
#endif

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT_IF          ( ldexp_, tag::cpu_,(A0)(A1)(X)
                                       , (boost::mpl::equal_to < boost::simd::meta::cardinal_of<A0>
                                                               , boost::simd::meta::cardinal_of<A1>
                                                               >
                                         )
                                       , ((simd_<integer_<A0>,X>))
                                         (generic_<integer_<A1> >)
                                       )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(2)
    {
      return rshl(a0, a1);
    }
  };

  BOOST_DISPATCH_IMPLEMENT_IF          ( ldexp_, tag::cpu_,(A0)(A1)(X)(Y)
                                       , (boost::mpl::equal_to < boost::simd::meta::cardinal_of<A0>
                                                               , boost::simd::meta::cardinal_of<A1>
                                                               >
                                         )
                                       , ((simd_<floating_<A0>,X>))
                                         ((simd_<integer_<A1>,Y>))
                                       )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(2)
    {
      typedef typename dispatch::meta::as_integer<A0>::type  iA0;
      typedef typename meta::as_logical<iA0>::type           bA0;

      iA0 e = a1;
      A0 f = One<A0>();
#ifndef BOOST_SIMD_NO_DENORMALS
      bA0 denormal =  lt(e, Minexponent<A0>());
      e = selsub(denormal, e, Minexponent<A0>());
      f = if_else(denormal, Smallestposval<A0>(), One<A0>());
#endif
      bA0 test = eq(e, Limitexponent<A0>());
      f = seladd(test, f, One<A0>());
      e = selsub(test, e, One<iA0>());
      e += Maxexponent<A0>();
      e = shl(e, Nbmantissabits<A0>());
      return a0*bitwise_cast<A0>(e)*f;
    }
  };

} } }

#endif


#include <gsa/snapshot.hh>

#include <boost/test/unit_test.hpp>
#include <random>

BOOST_AUTO_TEST_SUITE( ts_gsa_snapshot )

BOOST_AUTO_TEST_CASE( basic )  {
  auto f = [](common::vector_type v) {
             return v.squaredNorm();
           };

  common::vector_type pos(2);

  pos << 3, 4;

  gsa::snapshot snap(f, pos);

  BOOST_CHECK_CLOSE( 3.0, snap.position()(0), 1e-10);
  BOOST_CHECK_CLOSE( 4.0, snap.position()(1), 1e-10);
  BOOST_CHECK_CLOSE(25.0, snap.fitness(), 1e-10);
}

BOOST_AUTO_TEST_SUITE_END()


/*****************************************************************************
 *
 * This file is part of Mapnik (c++ mapping toolkit)
 *
 * Copyright (C) 2011 Artem Pavlenko
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 *****************************************************************************/

#ifndef MAPNIK_SVG_TRANSFORM_GRAMMAR_HPP
#define MAPNIK_SVG_TRANSFORM_GRAMMAR_HPP

// mapnik
#include <mapnik/global.hpp>

// agg
#include <agg_trans_affine.h>

// spirit
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_function.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <boost/spirit/include/phoenix_object.hpp>

namespace mapnik { namespace svg {

    using namespace boost::spirit;
    using namespace boost::fusion;
    using namespace boost::phoenix;

    inline double deg2rad(double d)
    {
        return M_PI * d / 180.0;
    }

    template <typename TransformType>
    struct process_matrix
    {
        template <typename T0, typename T1, typename T2, typename T3, typename T4, typename T5>
        struct result
        {
            typedef void type;
        };

        explicit process_matrix( TransformType & tr)
            :tr_(tr) {}

        void operator () (double a, double b, double c, double d, double e, double f) const
        {
            tr_ = agg::trans_affine(a,b,c,d,e,f) * tr_;
        }

        TransformType & tr_;
    };

    template <typename TransformType>
    struct process_rotate
    {
        template <typename T0, typename T1, typename T2>
        struct result
        {
            typedef void type;
        };

        explicit process_rotate( TransformType & tr)
            :tr_(tr) {}

        template <typename T0,typename T1,typename T2>
        void operator () (T0 a, T1 cx, T2 cy) const
        {
            if (cx == 0.0 && cy == 0.0)
            {
                tr_ = agg::trans_affine_rotation(deg2rad(a)) * tr_;
            }
            else
            {
                agg::trans_affine t = agg::trans_affine_translation(-cx,-cy);
                t *= agg::trans_affine_rotation(deg2rad(a));
                t *= agg::trans_affine_translation(cx, cy);
                tr_ = t * tr_;
            }
        }

        TransformType & tr_;
    };

    template <typename TransformType>
    struct process_translate
    {
        template <typename T0, typename T1>
        struct result
        {
            typedef void type;
        };

        explicit process_translate( TransformType & tr)
            :tr_(tr) {}

        template <typename T0,typename T1>
        void operator () (T0 tx, T1 ty) const
        {
            if (ty) tr_ = agg::trans_affine_translation(tx,*ty) * tr_;
            else tr_ = agg::trans_affine_translation(tx,0.0) * tr_;
        }

        TransformType & tr_;
    };

    template <typename TransformType>
    struct process_scale
    {
        template <typename T0, typename T1>
        struct result
        {
            typedef void type;
        };

        explicit process_scale( TransformType & tr)
            :tr_(tr) {}

        template <typename T0,typename T1>
        void operator () (T0 sx, T1 sy) const
        {
            if (sy) tr_ = agg::trans_affine_scaling(sx,*sy) * tr_;
            else tr_ = agg::trans_affine_scaling(sx,sx) * tr_;
        }

        TransformType & tr_;
    };

    template <typename TransformType>
    struct process_skew
    {
        template <typename T0, typename T1>
        struct result
        {
            typedef void type;
        };

        explicit process_skew( TransformType & tr)
            :tr_(tr) {}

        template <typename T0,typename T1>
        void operator () (T0 skew_x, T1 skew_y) const
        {
            tr_ = agg::trans_affine_skewing(deg2rad(skew_x),deg2rad(skew_y)) * tr_;
        }

        TransformType & tr_;
    };

    // commented as this does not appear used and crashes clang when used with pch
    /*
      struct print_action
      {
      template <typename T>
      void operator()(T const& c, qi::unused_type, qi::unused_type) const
      {
        MAPNIK_LOG_DEBUG(svg) << typeid(c).name();
      }
      };
    */

    template <typename Iterator, typename SkipType, typename TransformType>
    struct svg_transform_grammar : qi::grammar<Iterator,SkipType>
    {
        explicit svg_transform_grammar(TransformType & tr)
            : svg_transform_grammar::base_type(start),
              matrix_action(process_matrix<TransformType>(tr)),
              rotate_action(process_rotate<TransformType>(tr)),
              translate_action(process_translate<TransformType>(tr)),
              scale_action(process_scale<TransformType>(tr)),
              skew_action(process_skew<TransformType>(tr))
        {
            using qi::_1;
            using qi::_2;
            using qi::_3;
            using qi::_4;
            using qi::_5;
            using qi::_6;
            using qi::_a;
            using qi::_b;
            using qi::_c;
            using qi::_val;
            using qi::double_;
            using qi::no_case;

            start =  +transform_ ;

            transform_ = matrix | rotate | translate | scale | rotate | skewX | skewY ;

            matrix = no_case[lit("matrix")]
                >> lit('(')
                >> (
                    double_ >> -lit(',')
                    >> double_ >> -lit(',')
                    >> double_ >> -lit(',')
                    >> double_ >> -lit(',')
                    >> double_ >> -lit(',')
                    >> double_) [ matrix_action(_1,_2,_3,_4,_5,_6) ]
                >>  lit(')')
                ;

            translate = no_case[lit("translate")]
                >> lit('(')
                >> (double_ >> -lit(',')
                    >> -double_) [ translate_action(_1,_2) ]
                >> lit(')');

            scale = no_case[lit("scale")]
                >> lit('(')
                >> (double_ >> -lit(',')
                    >> -double_ )[ scale_action(_1,_2)]
                >>  lit(')');

            rotate = no_case[lit("rotate")]
                >> lit('(')
                >> double_[_a = _1] >> -lit(',')
                >> -(double_ [_b = _1] >> -lit(',') >> double_[_c = _1])
                >> lit(')') [ rotate_action(_a,_b,_c)];

            skewX = no_case[lit("skewX")] >> lit('(') >> double_ [ skew_action(_1, 0.0)] >> lit(')');

            skewY = no_case[lit("skewY")] >> lit('(') >> double_ [ skew_action(0.0, _1)] >> lit(')');

        }

        // rules
        qi::rule<Iterator,SkipType> start;
        qi::rule<Iterator,SkipType> transform_;
        qi::rule<Iterator,SkipType> matrix;
        qi::rule<Iterator,SkipType> translate;
        qi::rule<Iterator,SkipType> scale;
        qi::rule<Iterator,qi::locals<double,double,double>, SkipType> rotate;
        qi::rule<Iterator,SkipType> skewX;
        qi::rule<Iterator,SkipType> skewY;

        // actions
        function<process_matrix<TransformType> > matrix_action;
        function<process_rotate<TransformType> > rotate_action;
        function<process_translate<TransformType> > translate_action;
        function<process_scale<TransformType> > scale_action;
        function<process_skew<TransformType> > skew_action;
    };

    }}

#endif // MAPNIK_SVG_TRANSFORM_GRAMMAR_HPP


/*-----------------------------------------------------------------------------+
Copyright (c) 2010-2010: Joachim Faulhaber
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
#ifndef BOOST_ICL_TYPE_TRAITS_INFINITY_HPP_JOFA_100322
#define BOOST_ICL_TYPE_TRAITS_INFINITY_HPP_JOFA_100322

#include <string>
#include <boost/static_assert.hpp>
#include <boost/icl/type_traits/is_numeric.hpp>
#include <boost/mpl/if.hpp>

namespace boost{ namespace icl
{

#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable:4127) // conditional expression is constant
#endif

    template <class Type> struct numeric_infinity
    {
        typedef numeric_infinity type;

        static Type value()
        {
            BOOST_STATIC_ASSERT((is_numeric<Type>::value));
            if(std::numeric_limits<Type>::has_infinity)
                return std::numeric_limits<Type>::infinity();
            else
                return (std::numeric_limits<Type>::max)();
        }
    };

#ifdef BOOST_MSVC
#pragma warning(pop)
#endif

    template <class Type> struct infinity
    {
        typedef infinity type;

        static Type value()
        {
            return
            mpl::if_<is_numeric<Type>,
                     numeric_infinity<Type>,
                     identity_element<Type> >::type::value();
        }
    };

}} // namespace boost icl

#endif


#ifndef __PTSS_DSE
#define __PTSS_DSE

#include <iostream>
#include <vector>
#include <set>
#include <iterator>
#include <cmath>
#include <string>
#include <chrono>
#include <exception>
#include <queue>
#include <boost/math/distributions/normal.hpp>
#include <sys/time.h>
#include <rapidcsv.h>
#include "ptss_config.hpp"

using namespace std;

typedef vector<int> alloc_t;
typedef set<alloc_t> all_alloc_t;

class phase_t {
    public :
        unsigned int bench_id;
        unsigned int alloc;

        bool operator ==(const phase_t &b) const;
        bool operator !=(const phase_t &b) const;
        bool operator <(const phase_t &b) const;
        bool operator >(const phase_t &b) const;
        bool operator <=(const phase_t &b) const;
        bool operator >=(const phase_t &b) const;

        phase_t();
        phase_t(unsigned int bench_id,unsigned int alloc);
};

typedef vector<phase_t> alloc2_t;
typedef set<alloc2_t> all_alloc2_t;

bool is_eq(const alloc2_t &a, const alloc2_t &b);
bool is_gt(const alloc2_t &a, const alloc2_t &b);
bool is_lt(const alloc2_t &a, const alloc2_t &b);
bool is_incomparable(const alloc2_t &a, const alloc2_t &b);
bool lex_comp(const alloc2_t &a, const alloc2_t &b);

// ostream& operator<<(ostream& os, const vector<double>& pht);
ostream& operator<<(ostream& os, const phase_t& pht);
ostream& operator<<(ostream& os, const alloc_t& vi);
ostream& operator<<(ostream& os, const all_alloc_t& vvi);
ostream& operator<<(ostream& os, const alloc2_t& vi);
ostream& operator<<(ostream& os, const all_alloc2_t& vvi);

double compute_risk(const alloc_t &x);
double compute_execution_time(const alloc_t &x);
double compute_execution_time(const alloc2_t &x);
double estimate_exec_time(const int x, const int bench);
// int inv_estimate_exec_time(const double y, const int bench);
double estimate_power(const int x, const int bench);
int inv_estimate_power(const double y, const int bench);
double compute_estimated_util(const alloc_t &x);
double compute_pkpower(const alloc2_t &x);
set<unsigned int> compute_bottleneck(const alloc2_t &x);
set<unsigned int> compute_maxgrad(const alloc2_t &x);
void balance_out(alloc2_t &x);
void construct_alloc_2();

/* Utility functions */
void construct_alloc(all_alloc2_t &vvi, \
                     const vector<int> &bench, \
                     double deadline, \
                     int ph,\
                     alloc2_t &opt_point,\
                     double &opt_pkp_power,\
                     double &opt_exec_time);
unsigned int gen_bench_id();

class ptss_DSE {
    private :
        /*
         * The ptss_DSE partitions
         * the spaces into feasible
         * and discarded space
         */
        all_alloc_t search_space;
        all_alloc_t feasible_space;
        all_alloc_t discarded_space;

        double dmr;

        /* Optimal Point, just for testing */
        alloc_t opt_point;
        double opt_util;
        double opt_risk;

        // Lower and Upper "limits" points for DMR
        alloc_t lower;
        alloc_t upper;

        // Get the initial allocated point
        void init_point(double dmr);

        bool is_initialized;

    public :
        ptss_DSE();
        ptss_DSE(double dmr);

        // Evaluate all points in the Design Space
        void evaluate_all();

        // Comparison based elimination
        // void eliminate_points_rule12();
        void explore();

        // Display
        void display();
};

class ptss_DSE_hrt {
    private :
        /*
         * The ptss_DSE partitions
         * the spaces into feasible
         * and discarded space
         */
        all_alloc2_t search_space;
        double deadline;
        double pkp_cap;      /* Maximum peak power */

        /* Allocation Points */
        alloc2_t opt_point;  /* Oracle Computed pkp min point */
        alloc2_t ext_point;  /* Point of Highest Power Consumption, also serves as an init_point in some algorithms */
        alloc2_t ext_point2; /* Minimum allocation point*/
        alloc2_t cvx_point;  /* Optimal Point found using continuous relaxation and then dicretization */
        alloc2_t dggd_point; /* Point Found using discrete greedy gradient descent */
        alloc2_t opt_point2;  /* Oracle Computed et min point */
        alloc2_t dggd_point2; /* Dual of dggd_point */

        /* Corresponding Objective Function (and constraint) Value */
        // double opt_pkp_power;
        // double opt_exec_time;
        double cvx_pkp_min;   /* Continuous domain minimum obj function value */

        /* Parameter values for power and execution time characteristics */
        vector<int> bench; /* Benchmark Composition */
        vector<double> a_et;
        vector<double> b_et;
        vector<double> a_p;
        vector<double> b_p;

        double compute_cvx();
        double bench_create();
        double bench_create2(const vector<int>&);

    public :
        ptss_DSE_hrt(double,double);
        ptss_DSE_hrt() : ptss_DSE_hrt(2000,15) {};
        ptss_DSE_hrt(rapidcsv::Document &,ostream &,double,unsigned int,bool &);    /* Read the workload from a CSV file */
        void construct_alloc2();

        // Evaluate all points in the Design Space, and return the optimal value
        // double oracle();

        // Display
        void display();

        // Getters
        // alloc2_t& get_init_point();
        // double get_opt_pkp_power();
        // double get_opt_exec_time();

        // Membership testing
        bool contains_point(const alloc2_t&);

        /* The DGGD algorithm (Discrete Greedy Gradient Descent also referred to as
        pkmin in the paper) */
        double ptss_pkmin();
        double ptss_etmin(); /* Dual of previous problem */

};

/* Objective Function Constraints */
#endif


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE LibmeshEntityLocalMapTester
#include <boost/test/included/unit_test.hpp>

#include <algorithm>
#include <cassert>
#include <cmath>
#include <cstdlib>
#include <iostream>
#include <sstream>
#include <vector>

#include <LibmeshEntity.hpp>
#include <LibmeshEntityExtraData.hpp>
#include <LibmeshEntityLocalMap.hpp>

#include <Teuchos_Array.hpp>
#include <Teuchos_ArrayRCP.hpp>
#include <Teuchos_CommHelpers.hpp>
#include <Teuchos_DefaultComm.hpp>
#include <Teuchos_DefaultMpiComm.hpp>
#include <Teuchos_OpaqueWrapper.hpp>
#include <Teuchos_RCP.hpp>
#include <Teuchos_Tuple.hpp>
#include <Teuchos_TypeTraits.hpp>

#include <libmesh/cell_hex8.h>
#include <libmesh/equation_systems.h>
#include <libmesh/libmesh.h>
#include <libmesh/linear_implicit_system.h>
#include <libmesh/mesh.h>
#include <libmesh/node.h>
#include <libmesh/parallel.h>
#include <libmesh/point.h>
#include <libmesh/system.h>

//---------------------------------------------------------------------------//
// MPI Setup
//---------------------------------------------------------------------------//

template <class Ordinal>
Teuchos::RCP<const Teuchos::Comm<Ordinal>> getDefaultComm()
{
#ifdef HAVE_MPI
    return Teuchos::DefaultComm<Ordinal>::getComm();
#else
    return Teuchos::rcp( new Teuchos::SerialComm<Ordinal>() );
#endif
}

//---------------------------------------------------------------------------//
// TEST EPSILON
//---------------------------------------------------------------------------//

const double epsilon = 1.0e-14;

//---------------------------------------------------------------------------//
// Hex-8 test.
BOOST_AUTO_TEST_CASE( checkEntityLocalMap )
{
	const std::string argv_string = "unit_test --keep-cout";
	const char *argv_char = argv_string.c_str();
	Teuchos::GlobalMPISession mpiSession(
			&boost::unit_test::framework::master_test_suite().argc,
			&boost::unit_test::framework::master_test_suite().argv);
	auto comm = Teuchos::DefaultComm<int>::getComm();
	auto mpi_comm = Teuchos::rcp_dynamic_cast<const Teuchos::MpiComm<int>>(
			comm);
	auto opaque_comm = mpi_comm->getRawMpiComm();
	auto raw_comm = (*opaque_comm)();

    // Create the mesh.
    int space_dim = 3;
    libMesh::LibMeshInit libmesh_init( 1, &argv_char, raw_comm );
    BOOST_VERIFY( libMesh::initialized() );
    BOOST_VERIFY( (int)libmesh_init.comm().rank() == comm->getRank() );
    Teuchos::RCP<libMesh::Mesh> mesh =
        Teuchos::rcp( new libMesh::Mesh( libmesh_init.comm(), space_dim ) );

    // Create the nodes.
    int rank = comm->getRank();
    Teuchos::Array<libMesh::Node *> nodes( 8 );
    double node_coords[3];
    node_coords[0] = 0.0;
    node_coords[1] = 0.0;
    node_coords[2] = -2.0;
    nodes[0] = mesh->add_point(
        libMesh::Point( node_coords[0], node_coords[1], node_coords[2] ), 0,
        rank );

    node_coords[0] = 2.0;
    node_coords[1] = 0.0;
    node_coords[2] = -2.0;
    nodes[1] = mesh->add_point(
        libMesh::Point( node_coords[0], node_coords[1], node_coords[2] ), 1,
        rank );

    node_coords[0] = 2.0;
    node_coords[1] = 2.0;
    node_coords[2] = -2.0;
    nodes[2] = mesh->add_point(
        libMesh::Point( node_coords[0], node_coords[1], node_coords[2] ), 2,
        rank );

    node_coords[0] = 0.0;
    node_coords[1] = 2.0;
    node_coords[2] = -2.0;
    nodes[3] = mesh->add_point(
        libMesh::Point( node_coords[0], node_coords[1], node_coords[2] ), 3,
        rank );

    node_coords[0] = 0.0;
    node_coords[1] = 0.0;
    node_coords[2] = 0.0;
    nodes[4] = mesh->add_point(
        libMesh::Point( node_coords[0], node_coords[1], node_coords[2] ), 4,
        rank );

    node_coords[0] = 2.0;
    node_coords[1] = 0.0;
    node_coords[2] = 0.0;
    nodes[5] = mesh->add_point(
        libMesh::Point( node_coords[0], node_coords[1], node_coords[2] ), 5,
        rank );

    node_coords[0] = 2.0;
    node_coords[1] = 2.0;
    node_coords[2] = 0.0;
    nodes[6] = mesh->add_point(
        libMesh::Point( node_coords[0], node_coords[1], node_coords[2] ), 6,
        rank );

    node_coords[0] = 0.0;
    node_coords[1] = 2.0;
    node_coords[2] = 0.0;
    nodes[7] = mesh->add_point(
        libMesh::Point( node_coords[0], node_coords[1], node_coords[2] ), 7,
        rank );

    // Make a hex-8.
    libMesh::Elem *hex_elem = mesh->add_elem( new libMesh::Hex8 );
    hex_elem->processor_id() = rank;
    hex_elem->set_id() = 2 * rank;
    for ( int i = 0; i < 8; ++i )
        hex_elem->set_node( i ) = nodes[i];

    // Check libmesh validity.
    mesh->libmesh_assert_valid_parallel_ids();

    // Make an adjacency data structure.
    LibmeshAdapter::LibmeshAdjacencies adjacencies( mesh );

    // Create a  entity for the hex.
    LibmeshAdapter::LibmeshEntity<libMesh::Elem> dtk_entity =
        LibmeshAdapter::LibmeshEntity<libMesh::Elem>(
            Teuchos::ptr( hex_elem ), mesh.ptr(),
            Teuchos::ptrFromRef( adjacencies ) );

    // Make a libmesh system. We will put a first order linear basis on the
    // elements.
    libMesh::EquationSystems equation_systems( *mesh );
    libMesh::LinearImplicitSystem &system =
        equation_systems.add_system<libMesh::LinearImplicitSystem>( "Test" );
    system.add_variable( "test_var", libMesh::FIRST );

    // Create a local map from the libmesh mesh.
    auto local_map =
        Teuchos::rcp( new LibmeshAdapter::LibmeshEntityLocalMap(
            mesh, Teuchos::rcpFromRef( system ) ) );

    // Test the measure.
    BOOST_TEST( local_map->measure( dtk_entity ) == 8.0, boost::test_tools::tolerance(epsilon) );

    // Test the centroid.
    Teuchos::Array<double> centroid( space_dim, 0.0 );
    local_map->centroid( dtk_entity, centroid() );
    BOOST_VERIFY( centroid[0] == 1.0 );
    BOOST_VERIFY( centroid[1] ==  1.0 );
    BOOST_VERIFY( centroid[2] == -1.0 );

    // Make a good point and a bad point.
    Teuchos::Array<double> good_point( space_dim );
    good_point[0] = 0.5;
    good_point[1] = 1.5;
    good_point[2] = -1.0;
    Teuchos::Array<double> bad_point( space_dim );
    bad_point[0] = 0.75;
    bad_point[1] = -1.75;
    bad_point[2] = 0.35;

    // Test the reference frame safeguard.
    BOOST_VERIFY(
        local_map->isSafeToMapToReferenceFrame( dtk_entity, good_point() ) );
    BOOST_VERIFY(
        !local_map->isSafeToMapToReferenceFrame( dtk_entity, bad_point() ) );

    // Test the mapping to reference frame.
    Teuchos::Array<double> ref_good_point( space_dim );
    bool good_map = local_map->mapToReferenceFrame( dtk_entity, good_point(),
                                                    ref_good_point() );
    BOOST_VERIFY( good_map );
    BOOST_TEST( ref_good_point[0] == -0.5, boost::test_tools::tolerance(epsilon) );
    BOOST_TEST( ref_good_point[1] == 0.5, boost::test_tools::tolerance(epsilon) );
    BOOST_VERIFY( std::abs( ref_good_point[2] ) < epsilon );

    Teuchos::Array<double> ref_bad_point( space_dim );
    local_map->mapToReferenceFrame( dtk_entity, bad_point(), ref_bad_point() );

    // Test the point inclusion.
    BOOST_VERIFY(
        local_map->checkPointInclusion( dtk_entity, ref_good_point() ) );
    BOOST_VERIFY(
        !local_map->checkPointInclusion( dtk_entity, ref_bad_point() ) );

    // Test the map to physical frame.
    Teuchos::Array<double> phy_good_point( space_dim );
    local_map->mapToPhysicalFrame( dtk_entity, ref_good_point(),
                                   phy_good_point() );
    BOOST_TEST( good_point[0] == phy_good_point[0], boost::test_tools::tolerance(epsilon) );
    BOOST_TEST( good_point[1] == phy_good_point[1], boost::test_tools::tolerance(epsilon) );
    BOOST_TEST( good_point[2] == phy_good_point[2], boost::test_tools::tolerance(epsilon) );

    Teuchos::Array<double> phy_bad_point( space_dim );
    local_map->mapToPhysicalFrame( dtk_entity, ref_bad_point(),
                                   phy_bad_point() );
    BOOST_TEST( bad_point[0] == phy_bad_point[0], boost::test_tools::tolerance(epsilon) );
    BOOST_TEST( bad_point[1] == phy_bad_point[1], boost::test_tools::tolerance(epsilon) );
    BOOST_TEST( bad_point[2] == phy_bad_point[2], boost::test_tools::tolerance(epsilon) );

    // Test the coordinates of the points extracted through the centroid
    // function.
    Teuchos::Array<double> point_coords( space_dim );
    int num_nodes = 8;
    for ( int n = 0; n < num_nodes; ++n )
    {
        auto dtk_node = LibmeshAdapter::LibmeshEntity<libMesh::Node>(
            Teuchos::ptr( nodes[n] ), mesh.ptr(),
            Teuchos::ptrFromRef( adjacencies ) );
        local_map->centroid( dtk_node, point_coords() );
        BOOST_TEST( ( *nodes[n] )( 0 ) == point_coords[0] );
        BOOST_TEST( ( *nodes[n] )( 1 ) == point_coords[1] );
        BOOST_TEST( ( *nodes[n] )( 2 ) == point_coords[2] );
    }
}

//---------------------------------------------------------------------------//
// end tstLibmeshEntityLocalMap.cpp
//---------------------------------------------------------------------------//


/* Copyright (c) 2014, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */
#pragma once

#include <iostream>
#include <stdint.h>
#include <algorithm>
#include <Eigen/Dense>

#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

// CUDA runtime
#include <cuda_runtime.h>
#include <nvidia/helper_cuda.h>
#include <jsCore/gpuMatrix.hpp>

#include <cudaPcl/root_includes.hpp>
#include <cudaPcl/convolutionSeparable_common_small.h>
#include <cudaPcl/cuda_pc_helpers.h>
#include <cudaPcl/openniVisualizer.hpp>

using namespace Eigen;
using std::cout;
using std::endl;

void depth2xyzGPU(uint16_t* d, float* x, float* y, float* z,
    float invF, int w, int h, float *xyz=NULL);
void depth2xyzGPU(uint16_t* d, double* x, double* y, double* z,
    double invF, int w, int h, double *xyz=NULL);

void depth2xyzFloatGPU(float* d, float* x, float* y, float* z,
    float invF, int w, int h, float *xyz=NULL);
void depth2xyzFloatGPU(double* d, double* x, double* y, double* z,
    double invF, int w, int h, double *xyz=NULL);

void copyShuffleGPU(float* in, float* out, uint32_t* ind, int32_t N, int32_t step);

void haveDataGpu(float* d_x, uint8_t* d_haveData, int32_t N, uint32_t step);
void haveDataGpu(double* d_x, uint8_t* d_haveData, int32_t N, uint32_t step);

void derivatives2normalsGPU(float* d_x, float* d_y, float* d_z,
    float* d_xu, float* d_yu, float* d_zu,
    float* d_xv, float* d_yv, float* d_zv,
    float* d_n, uint8_t* d_haveData, int w, int h);

void derivatives2normalsGPU(double* d_x, double* d_y, double* d_z,
    double* d_xu, double* d_yu, double* d_zu,
    double* d_xv, double* d_yv, double* d_zv,
    double* d_n, uint8_t* d_haveData, int w, int h);

void derivatives2normalsGPU(float* d_z, float* d_zu, float* d_zv, float* d_n,
    uint8_t* d_haveData, float invF, int w, int h);
void derivatives2normalsGPU(double* d_z, double* d_zu, double* d_zv, double*
    d_n, uint8_t* d_haveData, double invF, int w, int h);

void xyzImg2PointCloudXYZRGB(double* d_xyzImg, float* d_pclXYZRGB, int32_t w,
    int32_t h);

void xyzImg2PointCloudXYZRGB(float* d_xyzImg, float* d_pclXYZRGB, int32_t w,
    int32_t h);

namespace cudaPcl {

struct CfgSmoothNormals
{
  CfgSmoothNormals() : f_d(540.f), eps(0.2f*0.2f), B(9), compress(true){};
  float f_d;
  float eps;
  uint32_t B;
  bool compress;
};

template<typename T>
class NormalExtractSimpleGpu
{
public:
  NormalExtractSimpleGpu(T f_depth, uint32_t w, uint32_t h, bool compress = false);
  ~NormalExtractSimpleGpu();

  void compute(const uint16_t* data, uint32_t w, uint32_t h);
  void compute(const pcl::PointCloud<pcl::PointXYZ>::Ptr & pc, T radius=0.03);

  void computeGpu(uint16_t* d_depth);
  void computeGpu(T* d_depth);
  void computeAreaWeights();
  void compressNormals( uint32_t w, uint32_t h);
//  void compressNormalsCpu(float* n, uint32_t w, uint32_t h);

  void setNormalsCpu(T* n, uint32_t w, uint32_t h);
  void setNormalsCpu(const pcl::PointCloud<pcl::Normal>& normals);

  // compressed normals
  cv::Mat normalsComp(int32_t& nComp);
  float* d_normalsComp(int32_t& nComp){nComp = nComp_; return d_normalsComp_.data();};

  void uncompressCpu(const uint32_t* in, uint32_t Nin,  uint32_t* out, uint32_t Nout);

  cv::Mat compInd();
  uint32_t* d_compInd(){return d_compInd_.data();};

  // normals as a 3 channel float image
  cv::Mat normalsImg();
  T* d_normalsImg(){return d_nImg_.data();}; // get pointer to memory with normal image

  // uint8_t where we have data
  cv::Mat haveData();
  uint8_t* d_haveData(){return d_haveData_.data();}; // get pointer to memory with normal image

  // normals as a point cloud
  pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr normalsPc();
  float* d_normalsPcl();
//  pcl::PointCloud<pcl::PointXYZRGB> normals();
//  pcl::PointCloud<pcl::PointXYZ>::ConstPtr pointCloud();
//  pcl::PointCloud<pcl::PointXYZ> pointCloud();

  T* d_weights(){return d_w;};
  uint32_t d_step(){return X_STEP;};
  uint32_t d_offset(){return X_OFFSET;};

protected:

    // sets up the memory on the GPU device
    void prepareCUDA();
    // computes derivatives of d_x, d_y, d_z on GPU
    void computeDerivatives(uint32_t w,uint32_t h);
    // smoothes the derivatives (iterations) times
    void smoothDerivatives(uint32_t iterations, uint32_t w,uint32_t h);
    // convert the d_depth into smoothed xyz
    void depth2smoothXYZ(T invF, uint32_t w,uint32_t h);

    T invF_;
    // Width and height of in and output
    uint32_t w_,h_;
    // Width and height for internal processing pruposes
    uint32_t wProc_,hProc_;
    bool cudaReady_, nCachedPc_, nCachedImg_, pcComputed_, nCachedComp_;
    bool compress_;
    jsc::GpuMatrix<T> d_z;
    jsc::GpuMatrix<T> d_zu;
    T* d_x, *d_y;
    T* d_xu, *d_yu;
    T* d_xv, *d_yv, *d_zv;
    jsc::GpuMatrix<T> d_nImg_;    // normal image - simple 3channel image
    float *d_nPcl; // using pcl conventions as if it were a PointCloud<PointXYZRGB>
    T *d_xyz;
    T *a,*b,*c; // for intermediate computations
    T *d_w;

    jsc::GpuMatrix<uint8_t> d_haveData_;

    int32_t nComp_;
    jsc::GpuMatrix<T> d_normalsComp_;
    jsc::GpuMatrix<uint32_t> d_compInd_;
    cv::Mat normalsComp_;
    cv::Mat compInd_;
    std::vector<uint32_t> indMap_;
    cv::Mat haveData_;

    T *h_nPcl;
    T *h_xyz;

    uint16_t* d_depth;
    T h_sobel_dif[KERNEL_LENGTH];
    T h_sobel_sum[KERNEL_LENGTH];

    pcl::PointCloud<pcl::PointXYZRGB> n_;
    pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr n_cp_;
//    pcl::PointCloud<pcl::PointXYZ> pc_;
//    pcl::PointCloud<pcl::PointXYZ>::ConstPtr pc_cp_;

    cv::Mat nImg_;
};

template <class T>
static void DoNotFree(T*)
{ }
}
#include <cudaPcl/normalExtractSimpleGpu_impl.hpp>


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/table.hpp>
#include <nt2/include/functions/times.hpp>
#include <boost/array.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/basic.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>

NT2_TEST_CASE( view_over_pointer )
{
  using nt2::table;
  using nt2::of_size;
  using nt2::view;

  boost::array<double,12> data =  {{ 1., 2., 3., 4. , 5. , 6.
                                   , 7., 8., 9., 10., 11., 12.
                                  }};

  boost::array<double,12> ref =  {{ 10., 20., 30., 40. , 50. , 60.
                                   ,70., 80., 90., 100., 110., 120.
                                  }};

  view< table<double> > v(&data[0], of_size(3,4));
  NT2_TEST_EQUAL(data, v);

  v.reset(&data[0], of_size(6,2));
  NT2_TEST_EQUAL(data, v);

  v = 10. * v;
  NT2_TEST_EQUAL(v, ref);
  NT2_TEST_EQUAL(data, ref);
}


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

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/ifnotinc.hpp>
#include <simd_test.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/two.hpp>

STF_CASE_TPL (" ifnotinc signed_int",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::ifnotinc;

  // return type conformity test
  STF_EXPR_IS( ifnotinc(T(), T()), T);

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

STF_CASE_TPL (" ifnotinc unsigned_int",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::ifnotinc;

  // return type conformity test

  // specific values tests
  STF_EXPR_IS( ifnotinc(T(), T()), T);

  // specific values tests
  STF_EQUAL(ifnotinc(bs::Zero<T>(), bs::One<T>()), bs::Two<T>());
  STF_EQUAL(ifnotinc(bs::Zero<T>(), bs::Valmax<T>()), bs::Zero<T>());
  STF_EQUAL(ifnotinc(bs::Zero<T>(), bs::Zero<T>()), bs::One<T>());
  STF_EQUAL(ifnotinc(bs::One<T>(), bs::One<T>()), bs::One<T>());
  STF_EQUAL(ifnotinc(bs::One<T>(), bs::Zero<T>()), bs::Zero<T>());}

STF_CASE_TPL(" ifnotinc _floating", STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::ifnotinc;

  // return type conformity test
  STF_EXPR_IS( ifnotinc(T(), T()), T);

  // specific values tests
  STF_EQUAL(ifnotinc(bs::Zero<T>(), bs::Mone<T>()), bs::Zero<T>());
  STF_EQUAL(ifnotinc(bs::Zero<T>(), bs::One<T>()), bs::Two<T>());
  STF_EQUAL(ifnotinc(bs::Zero<T>(), bs::Valmax<T>()), bs::Valmax<T>());
  STF_EQUAL(ifnotinc(bs::Zero<T>(), bs::Zero<T>()), bs::One<T>());
  STF_EQUAL(ifnotinc(bs::One<T>(), bs::Mone<T>()), bs::Mone<T>());
  STF_EQUAL(ifnotinc(bs::One<T>(), bs::One<T>()), bs::One<T>());
  STF_EQUAL(ifnotinc(bs::One<T>(), bs::Zero<T>()), bs::Zero<T>());
}


#include <algorithm>
#include <iostream>
#include <stdint.h>
#include <string>
#include <vector>
#include "kdtree.h"
#include <cstdlib>
#include <boost/lambda/lambda.hpp>
#include <boost/lambda/bind.hpp>

using namespace std;
using namespace boost::lambda;

vec3f makeVec3f(float x, float y, float z)
{
  vec3f a;
  a[0] = x;
  a[1] = y;
  a[2] = z;
  return a;
}

vec3f makeRandomPoint(int size)
{
  vec3f a;
  a[0] = random() % size;
  a[1] = random() % size;
  a[2] = random() % size;
  return a;
}

int main(int argc, char **argv)
{
  time_t t = time(NULL);
  srandom(t);
  cout << ctime(&t) << endl;

  std::vector<vec3f> points(3);
  std::generate(points.begin(), points.end(), bind(makeRandomPoint, 100));
  std::for_each(points.begin(), points.end(), std::cout << _1 << "\n");

  BoundingBox<vec3f> bbox(&points[0], points.size());
  cout << bbox << endl;
  cout << endl;

  KdTree<vec3f> tree;
  tree.build(points);

  cout << tree << endl;
  cout << bbox.size(0) << ", " << bbox.size(1) << ", " << bbox.size(2) << endl;
  return 0;
}


#include <cstdio>
#include <vector>

#include <Eigen/Core>
#include <Eigen/StdVector>
#include <maplab-common/test/testing-entrypoint.h>

#include "vocabulary-tree/distance.h"
#include "vocabulary-tree/simple-kmeans.h"
#include "vocabulary-tree/tree-builder.h"
#include "vocabulary-tree/types.h"

#include "./floating-point-test-helpers.h"

// This tests checks the kNearestNeighbor search on the kd-tree top level.
TEST(BucketizedTree, KNearestNeighbors) {
  std::mt19937 generator(40);
  static const size_t kNumfeaturesPerCluster = 20;
  static const size_t kNumClusters = 100;
  static const size_t kNumLevels = 1;

  FLAGS_lc_kdtree_accelerator_eps = 0.2;

  DescriptorVector gt_centers;
  DescriptorVector descriptors;
  std::vector<unsigned int> membership;
  std::vector<unsigned int> gt_membership;

  GenerateTestData(
      kNumfeaturesPerCluster, kNumClusters, generator(), &gt_centers,
      &descriptors, &membership, &gt_membership);

  DescriptorType descriptor_zero;
  descriptor_zero.setConstant(
      kDescriptorDimensionality, 1, static_cast<Scalar>(0));

  typedef loop_closure::distance::L2<DescriptorType> DistanceType;

  typedef loop_closure::TreeBuilder<DescriptorType, DistanceType> TreeBuilder;

  TreeBuilder builder(descriptor_zero);
  builder.kmeans().SetRestarts(5);
  builder.Build(descriptors, kNumClusters, kNumLevels);

  const TreeBuilder::Tree& tree = builder.tree();
  const DescriptorVector& centers = tree.centers();

  loop_closure::distance::L2<DescriptorType> l2_distance;

  // Search all descriptors, compute nearest neighbors.
  unsigned int num_nearest_neighbors = 4;
  for (const DescriptorType& feature : descriptors) {
    typedef std::pair<int, typename DistanceType::result_type> Candidate;
    std::vector<Candidate> candidates;
    for (size_t i = 0; i < centers.size(); ++i) {
      const DescriptorType& center = centers[i];
      double distance = l2_distance(feature, center);
      candidates.emplace_back(i, distance);
    }
    std::sort(
        candidates.begin(), candidates.end(),
        [](const Candidate& lhs, const Candidate& rhs) {
          return lhs.second < rhs.second;
        });

    std::vector<loop_closure::Word> nearest_neighbors;
    std::vector<typename DistanceType::result_type> distances;
    tree.GetNearestNeighborTopLevel(
        feature, num_nearest_neighbors, &nearest_neighbors, &distances);

    EXPECT_EQ(nearest_neighbors.size(), num_nearest_neighbors);
    EXPECT_EQ(nearest_neighbors.size(), distances.size());

    for (unsigned int i = 0; i < num_nearest_neighbors; ++i) {
      EXPECT_EQ(nearest_neighbors[i], candidates[i].first);
      EXPECT_NEAR(distances[i], candidates[i].second, 1e-4);
    }
  }
}

MAPLAB_UNITTEST_ENTRYPOINT


// Copyright (C) 2015 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// 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 HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/global_pose_estimation/least_unsquared_deviation_position_estimator.h"

#include <ceres/rotation.h>
#include <Eigen/Core>
#include <Eigen/SparseCore>

#include <limits>
#include <unordered_map>
#include <vector>

#include "theia/math/qp_solver.h"
#include "theia/sfm/global_pose_estimation/pairwise_translation_and_scale_error.h"
#include "theia/sfm/types.h"
#include "theia/util/map_util.h"
#include "theia/util/util.h"

namespace theia {
namespace {

using Eigen::Matrix3d;
using Eigen::Vector3d;

Vector3d GetRotatedTranslation(const Vector3d& rotation_angle_axis,
                               const Vector3d& translation) {
  Matrix3d rotation;
  ceres::AngleAxisToRotationMatrix(
      rotation_angle_axis.data(),
      ceres::ColumnMajorAdapter3x3(rotation.data()));
  return rotation.transpose() * translation;
}

}  // namespace

LeastUnsquaredDeviationPositionEstimator::
    LeastUnsquaredDeviationPositionEstimator(
        const LeastUnsquaredDeviationPositionEstimator::Options& options)
    : options_(options) {
  CHECK_GT(options_.max_num_iterations, 0);
  CHECK_GT(options_.max_num_reweighted_iterations, 0);
}

bool LeastUnsquaredDeviationPositionEstimator::EstimatePositions(
    const std::unordered_map<ViewIdPair, TwoViewInfo>& view_pairs,
    const std::unordered_map<ViewId, Vector3d>& orientations,
    std::unordered_map<ViewId, Vector3d>* positions) {
  CHECK_NOTNULL(positions)->clear();

  InitializeIndexMapping(view_pairs, orientations);

  // Set up the linear system.
  SetupConstraintMatrix(view_pairs, orientations);
  solution_.setZero(constraint_matrix_.cols());
  weights_.setConstant(constraint_matrix_.rows(), 1.0);

  // Set the lower bounds for the QP. The positions should be unbounded while
  // the scales havea lower bound of 1.0.
  Eigen::VectorXd lower_bound(constraint_matrix_.cols());
  lower_bound.head(3 * orientations.size())
      .setConstant(-std::numeric_limits<double>::infinity());
  lower_bound.tail(view_pairs.size()).setConstant(1.0);

  // Solve for the camera positions using an IRLS scheme. The values p and r are
  // constant at zero.
  Eigen::SparseMatrix<double> P(constraint_matrix_.rows(),
                                constraint_matrix_.rows());
  Eigen::VectorXd q(constraint_matrix_.cols());
  q.setZero();
  const double r = 0;
  QPSolver::Options qp_solver_options;
  qp_solver_options.max_num_iterations = 10;
  for (int i = 0; i < options_.max_num_reweighted_iterations; i++) {
    if (i > 0) {
      UpdateConstraintWeights();
    }

    // Compute P = A^t * W * A, the quadratic matrix term for our QP.
    P = constraint_matrix_.transpose() * weights_.matrix().asDiagonal() *
        constraint_matrix_;

    // Solve the quadratic program. Increase the number of possible iterations
    // each time.
    qp_solver_options.max_num_iterations = std::min(
        options_.max_num_iterations, 2 * qp_solver_options.max_num_iterations);
    QPSolver qp_solver(qp_solver_options, P, q, r);
    const Eigen::VectorXd prev_solution = solution_;
    qp_solver.SetLowerBound(lower_bound);
    if (!qp_solver.Solve(&solution_)) {
      LOG(WARNING) << "Could not solve the Quadratic Program for the least "
                      "unsquared deviations position solver.";
      return false;
    }
  }

  // Set the estimated positions.
  for (const auto& view_id_index : view_id_to_index_) {
    const int index = view_id_index.second;
    const ViewId view_id = view_id_index.first;
    if (index == kConstantViewIndex) {
      (*positions)[view_id] = Eigen::Vector3d::Zero();
    } else {
      (*positions)[view_id] = solution_.segment<3>(index);
    }
  }

  return true;
}

void LeastUnsquaredDeviationPositionEstimator::InitializeIndexMapping(
    const std::unordered_map<ViewIdPair, TwoViewInfo>& view_pairs,
    const std::unordered_map<ViewId, Vector3d>& orientations) {
  std::unordered_set<ViewId> views;
  for (const auto& view_pair : view_pairs) {
    if (ContainsKey(orientations, view_pair.first.first)) {
      views.insert(view_pair.first.first);
    }
    if (ContainsKey(orientations, view_pair.first.second)) {
      views.insert(view_pair.first.second);
    }
  }

  // Create a mapping from the view id to the index of the linear system.
  int index = kConstantViewIndex;
  view_id_to_index_.reserve(orientations.size());
  for (const ViewId view_id : views) {
    view_id_to_index_[view_id] = index;
    index += 3;
  }

  // Create a mapping from the view id pair to the index of the linear system.
  view_id_pair_to_index_.reserve(view_pairs.size());
  for (const auto& view_pair : view_pairs) {
    view_id_pair_to_index_[view_pair.first] = index;
    ++index;
  }
}

void LeastUnsquaredDeviationPositionEstimator::SetupConstraintMatrix(
    const std::unordered_map<ViewIdPair, TwoViewInfo>& view_pairs,
    const std::unordered_map<ViewId, Vector3d>& orientations) {
  constraint_matrix_.resize(3 * view_pairs.size(),
                            3 * orientations.size() + view_pairs.size());

  // Add the camera to camera constraints.
  std::vector<Eigen::Triplet<double> > triplet_list;
  triplet_list.reserve(9 * view_pairs.size());
  int row = 0;
  for (const auto& view_pair : view_pairs) {
    const ViewIdPair view_id_pair = view_pair.first;

    const int view1_index = FindOrDie(view_id_to_index_, view_id_pair.first);
    const int view2_index = FindOrDie(view_id_to_index_, view_id_pair.second);
    const int scale_index =
        FindOrDieNoPrint(view_id_pair_to_index_, view_id_pair);

    // Rotate the relative translation so that it is aligned to the global
    // orientation frame.
    const Vector3d translation_direction =
        GetRotatedTranslation(FindOrDie(orientations, view_id_pair.first),
                              view_pair.second.position_2);

    // Add the constraint for view 1 in the minimization:
    //   position2 - position1 - scale_1_2 * translation_direction.
    if (view1_index != kConstantViewIndex) {
      triplet_list.emplace_back(row + 0, view1_index + 0, -1.0);
      triplet_list.emplace_back(row + 1, view1_index + 1, -1.0);
      triplet_list.emplace_back(row + 2, view1_index + 2, -1.0);
    }

    // Add the constraint for view 2 in the minimization:
    //   position2 - position1 - scale_1_2 * translation_direction.
    if (view2_index != kConstantViewIndex) {
      triplet_list.emplace_back(row + 0, view2_index + 0, 1.0);
      triplet_list.emplace_back(row + 1, view2_index + 1, 1.0);
      triplet_list.emplace_back(row + 2, view2_index + 2, 1.0);
    }

    // Add the constraint for scale in the minimization:
    //   position2 - position1 - scale_1_2 * translation_direction.
    triplet_list.emplace_back(row + 0, scale_index, -translation_direction[0]);
    triplet_list.emplace_back(row + 1, scale_index, -translation_direction[1]);
    triplet_list.emplace_back(row + 2, scale_index, -translation_direction[2]);

    row += 3;
  }

  constraint_matrix_.setFromTriplets(triplet_list.begin(), triplet_list.end());

  VLOG(2) << view_pairs.size() << " camera to camera constraints were added "
                                    "to the position estimation problem.";
}

// Compute the error:
//     err_i_j = || c_j - c_i - scale_i_j * t_i_j ||^2
//
// For each pairwise constraint, set w_i_j = (err_i_j + delta)^(-1/2) to
// reweight the QP solver for robustness.
void LeastUnsquaredDeviationPositionEstimator::UpdateConstraintWeights() {
  static const double delta = 1e-12;
  // Compute the errors with a simple matrix multiplication.
  const Eigen::VectorXd errors = constraint_matrix_ * solution_;
  weights_ = (errors.array().square() + delta).sqrt().inverse();
}

}  // namespace theia


#define BOOST_TEST_MODULE spherical_equatorial_representation_test

#include <boost/test/unit_test.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/systems/si/plane_angle.hpp>
#include <boost/units/systems/si/prefixes.hpp>
#include <boost/units/systems/si/length.hpp>
#include <boost/units/systems/angle/degrees.hpp>
#include <boost/astronomy/coordinate/arithmetic.hpp>
#include <boost/astronomy/coordinate/representation.hpp>

using namespace std;
using namespace boost::astronomy::coordinate;
using namespace boost::units::si;
using namespace boost::geometry;
using namespace boost::units;
namespace bud = boost::units::degree;

BOOST_AUTO_TEST_SUITE(spherical_equatorial_representation_constructors)

BOOST_AUTO_TEST_CASE(spherical_equatorial_representation_default_constructor)
{
    //using set functions
    spherical_equatorial_representation<double, quantity<bud::plane_angle>, quantity<bud::plane_angle>,
    quantity<si::length>> point1;
    point1.set_lat_lon_dist(45.0 * bud::degrees, 18.0 * bud::degrees, 3.5 * meters);
    BOOST_CHECK_CLOSE(point1.get_lat().value(), 45.0, 0.001);
    BOOST_CHECK_CLOSE(point1.get_lon().value(), 18.0, 0.001);
    BOOST_CHECK_CLOSE(point1.get_dist().value(), 3.5, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(point1.get_lat()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point1.get_lon()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point1.get_dist()), quantity<si::length>>::value));
}

BOOST_AUTO_TEST_CASE(spherical_equatorial_representation_quantities_constructor)
{
    //checking construction from value
    auto point1 = make_spherical_equatorial_representation
    (15.0 * bud::degrees, 39.0 * bud::degrees, 3.0*si::centi*meter);
    BOOST_CHECK_CLOSE(point1.get_lat().value(), 15.0, 0.001);
    BOOST_CHECK_CLOSE(point1.get_lon().value(), 39.0, 0.001);
    BOOST_CHECK_CLOSE(point1.get_dist().value(), 3.0, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(point1.get_lat()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point1.get_lon()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point1.get_dist()), quantity<decltype(si::centi*meter)>>::value));

    spherical_equatorial_representation<double, quantity<bud::plane_angle>, quantity<bud::plane_angle>,
    quantity<si::length>> point2(1.5 * bud::degrees, 9.0 * bud::degrees, 3.0 * meter);
    BOOST_CHECK_CLOSE(point2.get_lat().value(), 1.5, 0.001);
    BOOST_CHECK_CLOSE(point2.get_lon().value(), 9.0, 0.001);
    BOOST_CHECK_CLOSE(point2.get_dist().value(), 3, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(point2.get_lat()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point2.get_lon()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point2.get_dist()), quantity<si::length>>::value));
}

BOOST_AUTO_TEST_CASE(spherical_equatorial_representation_copy_constructor)
{
    //checking construction from value
    auto point1 = make_spherical_equatorial_representation
    (15.0 * bud::degrees, 30.0 * bud::degrees, 3.0*si::centi*meter);
    BOOST_CHECK_CLOSE(point1.get_lat().value(), 15.0, 0.001);
    BOOST_CHECK_CLOSE(point1.get_lon().value(), 30.0, 0.001);
    BOOST_CHECK_CLOSE(point1.get_dist().value(), 3, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(point1.get_lat()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point1.get_lon()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point1.get_dist()), quantity<decltype(si::centi*meter)>>::value));

    //copy constructor
    auto point2 = make_spherical_equatorial_representation(point1);
    BOOST_CHECK_CLOSE(point1.get_lat().value(), point2.get_lat().value(), 0.001);
    BOOST_CHECK_CLOSE(point1.get_lon().value(), point2.get_lon().value(), 0.001);
    BOOST_CHECK_CLOSE(point1.get_dist().value(), point2.get_dist().value(), 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(point2.get_lat()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point2.get_lon()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point2.get_dist()), quantity<decltype(si::centi*meter)>>::value));
}

BOOST_AUTO_TEST_CASE(spherical_equatorial_representation_copy_constructor_with_different_units)
{
    //checking construction from value
    auto point1 = make_spherical_equatorial_representation
    (15.0 * bud::degrees, 10.0 * bud::degrees, 3.0*si::centi*meter);
    BOOST_CHECK_CLOSE(point1.get_lat().value(), 15.0, 0.001);
    BOOST_CHECK_CLOSE(point1.get_lon().value(), 10.0, 0.001);
    BOOST_CHECK_CLOSE(point1.get_dist().value(), 3, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(point1.get_lat()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point1.get_lon()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point1.get_dist()), quantity<decltype(si::centi*meter)>>::value));

    //Conversion from one unit type to other
    auto point2 = make_spherical_equatorial_representation
    <double, quantity<bud::plane_angle>, quantity<bud::plane_angle>, quantity<si::length>>(point1);
    BOOST_CHECK_CLOSE(point2.get_lat().value(), 15.0, 0.001);
    BOOST_CHECK_CLOSE(point2.get_lon().value(), 10.0, 0.001);
    BOOST_CHECK_CLOSE(point2.get_dist().value(), 0.03, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(point2.get_lat()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point2.get_lon()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point2.get_dist()), quantity<si::length>>::value));
}

BOOST_AUTO_TEST_CASE(spherical_equatorial_representation_geometry_point_constructor)
{
    //constructing from boost::geometry::model::point
    model::point<double, 3, cs::cartesian> model_point(30, 60, 10);
    auto point1 = make_spherical_equatorial_representation
    <double,quantity<bud::plane_angle>,quantity<bud::plane_angle>,quantity<si::length>>(model_point);
    BOOST_CHECK_CLOSE(point1.get_lat().value(), 63.434948822922, 0.001);
    BOOST_CHECK_CLOSE(point1.get_lon().value(), 8.47871315, 0.001);
    BOOST_CHECK_CLOSE(point1.get_dist().value(), 67.823299831253, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(point1.get_lat()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point1.get_lon()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point1.get_dist()), quantity<si::length>>::value));

    spherical_equatorial_representation<double, quantity<bud::plane_angle>, quantity<bud::plane_angle>,
    quantity<si::length>> point2(model_point);
    BOOST_CHECK_CLOSE(point2.get_lat().value(), 63.434948822922, 0.001);
    BOOST_CHECK_CLOSE(point2.get_lon().value(), 8.47871315, 0.001);
    BOOST_CHECK_CLOSE(point2.get_dist().value(), 67.823299831253, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(point2.get_lat()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point2.get_lon()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point2.get_dist()), quantity<si::length>>::value));
}

BOOST_AUTO_TEST_CASE(spherical_equatorial_representation_conversion_from_cartesian_representation)
{
    //constructing from spherical representation
    auto cartesian_point = make_cartesian_representation(20.0 * meters, 60.0 * meters, 1.0 * meter);
    auto point1 = make_spherical_equatorial_representation(cartesian_point);
    BOOST_CHECK_CLOSE(point1.get_lat().value(), 1.2490457723983, 0.001);
    BOOST_CHECK_CLOSE(point1.get_lon().value(), 0.0158100741, 0.001);
    BOOST_CHECK_CLOSE(point1.get_dist().value(), 63.253458403474, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(point1.get_lat()), quantity<si::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point1.get_lon()), quantity<si::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point1.get_dist()), quantity<si::length>>::value));
}

BOOST_AUTO_TEST_CASE(spherical_equatorial_representation_conversion_from_spherical_representation)
{
    //constructing from spherical_equitorial representation
    auto spherical_point = make_spherical_representation
    (0.523599 * si::radian, 60.0 * bud::degrees, 1.0 * meter);
    auto point2 = make_spherical_equatorial_representation(spherical_point);
    BOOST_CHECK_CLOSE(point2.get_lat().value(), 0.523599, 0.001);
    BOOST_CHECK_CLOSE(point2.get_lon().value(), 0.523598776, 0.001);
    BOOST_CHECK_CLOSE(point2.get_dist().value(), 1.0, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(point2.get_lat()), quantity<si::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point2.get_lon()), quantity<si::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(point2.get_dist()), quantity<si::length>>::value));
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(spherical_equatorial_representation_operators)

BOOST_AUTO_TEST_CASE(spherical_equatorial_representation_addition_operator)
{
    auto point1 = make_spherical_equatorial_representation(15.0 * bud::degrees, 30.0 * bud::degrees, 10.0 * meters);
    auto point2 = make_spherical_equatorial_representation(30.0 * bud::degrees, 45.0 * bud::degrees, 20.0 * meters);

    auto sum = make_spherical_equatorial_representation(point1 + point2);

    BOOST_CHECK_CLOSE(sum.get_lat().value(), 24.3128259, 0.001);
    BOOST_CHECK_CLOSE(sum.get_lon().value(), 40.2412189, 0.001);
    BOOST_CHECK_CLOSE(sum.get_dist().value(), 29.6314681, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(sum.get_lat()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(sum.get_lon()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(sum.get_dist()), quantity<si::length>>::value));
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(spherical_equatorial_representation_arithmetic_functions)

// BOOST_AUTO_TEST_CASE(spherical_equatorial_representation_cross_product)
// {
//     auto point1 = make_spherical_equatorial_representation(3.0 * bud::degrees, 50.0 * bud::degrees, 40.0 * meters);
//     auto point2 = make_spherical_equatorial_representation(30.0 * bud::degrees, 45.0 * bud::degrees, 14.0 * meters);

//     auto result = cross(point1, point2);

//     BOOST_CHECK_CLOSE(result.get_lat().value(), -143.4774246228, 0.001);
//     BOOST_CHECK_CLOSE(result.get_lon().value(), 45.186034054587, 0.001);
//     BOOST_CHECK_CLOSE(result.get_dist().value(), 195.39050840581, 0.001);

//     //checking whether quantity stored is as expected or not
//     BOOST_TEST((std::is_same<decltype(result.get_lat()), quantity
//         <bu::multiply_typeof_helper<si::length, si::length>::type>>::value));
//     BOOST_TEST((std::is_same<decltype(result.get_lon()), quantity
//         <bu::multiply_typeof_helper<si::length, si::length>::type>>::value));
//     BOOST_TEST((std::is_same<decltype(result.get_dist()), quantity
//         <bu::multiply_typeof_helper<si::length, si::length>::type>>::value));
// }

BOOST_AUTO_TEST_CASE(spherical_equatorial_representation_dot_product)
{
    auto point1 = make_spherical_equatorial_representation(15.0 * bud::degrees, 30.0 * bud::degrees, 10.0 * meters);
    auto point2 = make_spherical_equatorial_representation(30.0 * bud::degrees, 45.0 * bud::degrees, 20.0 * meters);

    auto result = dot(point1, point2);

    BOOST_CHECK_CLOSE(result.value(), 189.011948, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(result), quantity
        <bu::multiply_typeof_helper<si::length, si::length>::type>>::value));
}

BOOST_AUTO_TEST_CASE(spherical_equatorial_representation_unit_vector)
{
    auto point1 = make_spherical_equatorial_representation(25.0 * bud::degrees, 30.0 * bud::degrees, 90.0*meter);

    auto result = boost::astronomy::coordinate::unit_vector(point1);

    BOOST_CHECK_CLOSE(result.get_lat().value(), 25.0, 0.001);
    BOOST_CHECK_CLOSE(result.get_lon().value(), 30.0, 0.001);
    BOOST_CHECK_CLOSE(result.get_dist().value(), 1, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(result.get_lat()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(result.get_lon()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(result.get_dist()), quantity<si::length>>::value));
}

BOOST_AUTO_TEST_CASE(spherical_equatorial_representation_magnitude)
{
    auto point1 = make_spherical_equatorial_representation(25.0 * bud::degrees, 36.0 * bud::degrees, 9.0 * meters);

    auto result = boost::astronomy::coordinate::magnitude(point1);

    BOOST_CHECK_CLOSE(result.value(), 9, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(result), quantity<si::length>>::value));

}

BOOST_AUTO_TEST_CASE(spherical_equatorial_representation_sum)
{
    auto point1 = make_spherical_equatorial_representation(15.0 * bud::degrees, 30.0 * bud::degrees, 10.0 * meters);
    auto point2 = make_spherical_equatorial_representation(30.0 * bud::degrees, 45.0 * bud::degrees, 20.0 * meters);

    auto result = boost::astronomy::coordinate::sum(point1, point2);

    BOOST_CHECK_CLOSE(result.get_lat().value(), 24.3128259, 0.001);
    BOOST_CHECK_CLOSE(result.get_lon().value(), 40.2412189, 0.001);
    BOOST_CHECK_CLOSE(result.get_dist().value(), 29.6314681, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(result.get_lat()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(result.get_lon()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(result.get_dist()), quantity<si::length>>::value));
}

BOOST_AUTO_TEST_CASE(spherical_equatorial_representation_mean)
{
    auto point1 = make_spherical_equatorial_representation(15.0 * bud::degrees, 30.0 * bud::degrees, 10.0 * meter);
    auto point2 = make_spherical_equatorial_representation(30.0 * bud::degrees, 45.0 * bud::degrees, 20.0 * meter);

    auto result = boost::astronomy::coordinate::mean(point1, point2);

    BOOST_CHECK_CLOSE(result.get_lat().value(), 24.3128259, 0.001);
    BOOST_CHECK_CLOSE(result.get_lon().value(), 40.2412189, 0.001);
    BOOST_CHECK_CLOSE(result.get_dist().value(), 14.8157341, 0.001);

    //checking whether quantity stored is as expected or not
    BOOST_TEST((std::is_same<decltype(result.get_lat()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(result.get_lon()), quantity<bud::plane_angle>>::value));
    BOOST_TEST((std::is_same<decltype(result.get_dist()), quantity<si::length>>::value));
}

BOOST_AUTO_TEST_SUITE_END()


#include <procedural_objects/geometry_system.h>

#include <procedural_objects/geometry_component.h>
#include <procedural_objects/hierarchical_component.h>
#include <procedural_objects/hierarchical_system.h>
#include <procedural_objects/procedural_object.h>
#include <procedural_objects/procedural_object_system.h>
#include <procedural_objects/scope_axis_direction_predicate.h>

#include <gtest/gtest.h>

#include <boost/qvm/map_vec_mat.hpp>

using namespace pagoda;

class ProceduralObjectPredicateTest : public ::testing::Test
{
protected:
	void SetUp()
	{
		m_objectSystem = std::make_shared<ProceduralObjectSystem>();
		m_geometrySystem = std::make_shared<GeometrySystem>();
		m_objectSystem->RegisterProceduralComponentSystem(m_geometrySystem);
		m_object = m_objectSystem->CreateProceduralObject();
	}

	void TearDown() {}

	ProceduralObjectSystemPtr m_objectSystem;
	GeometrySystemPtr m_geometrySystem;
	ProceduralObjectPtr m_object;
};

TEST_F(ProceduralObjectPredicateTest,
       when_testing_scope_axis_direction_should_evaluate_whether_the_axis_is_facing_the_direction)
{
	auto component = m_geometrySystem->CreateComponentAs<GeometryComponent>(m_object);
	component->SetScope(Scope({0, 0, 0}, {1, 1, 1}, boost::qvm::diag_mat(XYZ(Vec3F{1, 1, 1}))));

	{
		ScopeAxisDirectionPredicate pred(m_objectSystem, 'x', {1, 0, 0});
		EXPECT_TRUE(pred(m_object));
	}

	{
		ScopeAxisDirectionPredicate pred(m_objectSystem, 'y', {1, 0, 0});
		EXPECT_FALSE(pred(m_object));
	}

	{
		ScopeAxisDirectionPredicate pred(m_objectSystem, 'x', {1, 1, 0}, Degrees<float>(45.0f));
		EXPECT_TRUE(pred(m_object));
	}

	{
		ScopeAxisDirectionPredicate pred(m_objectSystem, 'x', {1, 1, 0}, Degrees<float>(44.0f));
		EXPECT_FALSE(pred(m_object));
	}
}


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_EXPONENTIAL_CONSTANTS_MINLOG10_HPP_INCLUDED
#define NT2_EXPONENTIAL_CONSTANTS_MINLOG10_HPP_INCLUDED

#include <nt2/include/functor.hpp>
#include <boost/simd/constant/hierarchy.hpp>
#include <boost/simd/constant/register.hpp>

namespace nt2
{
  namespace tag
  {
    /*!
      @brief Minlog10 generic tag

      Represents the Minlog10 constant in generic contexts.

      @par Models:
      Hierarchy
    **/
    BOOST_SIMD_CONSTANT_REGISTER( Minlog10, double
                                , 0, 0xc2179999UL
                                , 0xc0734413509f79feULL
                                )
  }
  namespace ext
  {
    template<class Site, class... Ts>
    BOOST_FORCEINLINE generic_dispatcher<tag::Minlog10, Site> dispatching_Minlog10(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
    {
      return generic_dispatcher<tag::Minlog10, Site>();
    }
    template<class... Args>
    struct impl_Minlog10;
  }
  /*!
    Generates constant Minlog10 used in logarithm/exponential computations

    @par Semantic:

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

    is similar to:

    @code
    if T is double
    r = -308.2547155599167;
    else if T is float
    r = -37.89999771118164;

    @endcode

  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(tag::Minlog10, Minlog10);
}

#endif


#include "tasktorrent/tasktorrent.hpp"
#include <mpi.h> // has to be before plasma
#ifdef USE_MKL
#include <mkl.h>
#include <mkl_cblas.h>
#include <mkl_lapacke.h>
#else
#include <cblas.h>
#include <lapacke.h>
#endif
#ifdef USE_PLASMA
#include "coreblas_.h" // plasma
#endif
// and plasma goes before mkl while linking
#include <Eigen/Core>  // goes after mkl
#include <Eigen/Dense> // goes after mkl
#include <fstream>
#include <array>
#include <random>
#include <mutex>
#include <iostream>
#include <map>
#include <tuple>
#include <memory>
#include <set>
#include <cxxopts.hpp>

using namespace std;
using namespace Eigen;
using namespace ttor;

typedef array<int, 2> int2;
typedef array<int, 3> int3;
typedef array<int, 4> int4;
typedef array<int, 5> int5;
typedef array<int, 6> int6;
typedef array<int, 7> int7;

struct scoped_timer {
  private:
    std::atomic<long long int>* time_us_;
    ttor::timer time_init_;
  public:
    scoped_timer(std::atomic<long long int>* time_us) {
        time_us_ = time_us;
        time_init_= ttor::wctime();
    }
    ~scoped_timer() {
        ttor::timer time_end_ = ttor::wctime();
        *time_us_ += static_cast<long long int>(1e6 * ttor::elapsed(time_init_, time_end_));
    }
};

ttor::view<double> make_view(Eigen::MatrixXd* A) {
    return ttor::view<double>(A->data(), A->size());
}

Eigen::MatrixXd make_from_view(ttor::view<double> A, int nrows) {
    Eigen::MatrixXd Add = Eigen::MatrixXd::Zero(nrows, nrows);
    assert(nrows * nrows == A.size());
    memcpy(Add.data(), A.data(), sizeof(double) * A.size());
    return Add;
}

void copy_from_view(Eigen::MatrixXd* dest, const ttor::view<double> A) {
    assert(dest->size() == A.size());
    memcpy(dest->data(), A.data(), sizeof(double) * A.size());
}

void accumulate(Eigen::MatrixXd* dest, const Eigen::MatrixXd* src) {
    assert(dest->size() == src->size());
    #pragma omp parallel for
    for(int k = 0; k < dest->size(); k++) {
        (*dest)(k) += (*src)(k);
    }
}

std::string to_string(int2 ij) {
    return to_string(ij[0]) + "_" + to_string(ij[1]);
}

std::string to_string(int3 ijk) {
    return to_string(ijk[0]) + "_" + to_string(ijk[1]) + "_" + to_string(ijk[2]);
}

double val_global(int i, int j) {
    return static_cast<double>( (i % 49) + (j % 37) * 53 );
}

void warmup_mkl(int n_threads) {
    ttor::Communicator comm(MPI_COMM_WORLD);
    ttor::Threadpool tp(n_threads, &comm);
    ttor::Taskflow<int> warmup(&tp);

    warmup.set_task([&](int i){
        Eigen::MatrixXd A = Eigen::MatrixXd::Identity(256,256);
        Eigen::MatrixXd B = Eigen::MatrixXd::Identity(256,256);
        Eigen::MatrixXd C = Eigen::MatrixXd::Identity(256,256);
    	cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, 256, 256, 256, 1.0, A.data(), 256, B.data(), 256, 1.0, C.data(), 256);
    }).set_indegree([&](int i) {
        return 1;
    }).set_mapping([&](int i) {
        return i % n_threads;
    });

    for(int i = 0; i < 64; i++) {
        warmup.fulfill_promise(i);
    }
    tp.join();
    MPI_Barrier(MPI_COMM_WORLD);
}

template<typename T>
Eigen::MatrixXd get_gaussian(const int rows, const int cols, T* gen) {
    std::normal_distribution<double> norm_dist(0.0, 1.0);
    Eigen::MatrixXd W(rows, cols);
    for(int j = 0; j < cols; j++){
        for(int i = 0; i < rows; i++){
            W(i,j) = norm_dist(*gen);
        }
    }
    Eigen::VectorXd col_norms = W.colwise().norm();
    assert(col_norms.size() == cols);
    assert(col_norms.minCoeff() >= 0);
    if (col_norms.minCoeff() > 0) {
        return W.normalized();
    }
    return W;
}

// Create the thin Q in v
void orgqr(Eigen::MatrixXd* v, Eigen::VectorXd* h) {
    int m = v->rows();
    int k = v->cols();
    assert(h->size() == k);
    if(m == 0 || k == 0)
        return;
    int info = LAPACKE_dorgqr(LAPACK_COL_MAJOR, m, k, k, v->data(), m, h->data());
    assert(info == 0);
}

// RRQR
void geqp3(Eigen::MatrixXd* A, Eigen::VectorXi* jpvt, Eigen::VectorXd* tau) {
    int m = A->rows();
    int n = A->cols();
    if (m == 0 || n == 0)
        return;
    assert(jpvt->size() == n);
    assert(tau->size() == min(m,n));
    int info = LAPACKE_dgeqp3(LAPACK_COL_MAJOR, m, n, A->data(), m, jpvt->data(), tau->data());
    assert(info == 0);
    for (int i = 0; i < jpvt->size(); i++)
        (*jpvt)[i] --;
}

// Rank revealing QR using dlaqps
// Code originally from Abeynaya Gnanasekaran
void laqps(Eigen::MatrixXd* A, Eigen::VectorXi* jpvt, Eigen::VectorXd* tau, int block_size, int& rank) {
#ifdef USE_MKL
    int m = A->rows();
    int n = A->cols();
    if (m == 0 || n == 0) {
        rank=0;
        return;
    }
    assert(jpvt->size() == n);
    assert(tau->size() == min(m,n));
    int offset = 0;
    int kb =0;
    Eigen::VectorXd vn1 = A->colwise().norm();
    Eigen::VectorXd vn2 = vn1;

    Eigen::VectorXd aux(block_size); // Auxilary vector
    aux.setZero();
    jpvt->setLinSpaced(n, 1, n);

    if (block_size > min(m,n)){
        block_size = min(m,n);
    }
    Eigen::MatrixXd F(n, block_size);
    F.setZero();

    dlaqps(&m, &n, &offset, &block_size, &kb, A->data(), &m, jpvt->data(),
           tau->data(), vn1.data(), vn2.data(), aux.data(), F.data(), &n);
    offset += kb;
    rank = offset;
    for (int i = 0; i < jpvt->size(); i++)
        (*jpvt)[i] --;

#else
    printf("Error: laqps requires MKL\n");
    exit(1);
#endif
    return;
}


// Copyright (C) 2015 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// 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 HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/global_pose_estimation/least_unsquared_deviation_position_estimator.h"

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <ceres/rotation.h>

#include <limits>
#include <unordered_map>
#include <vector>

#include "theia/math/constrained_l1_solver.h"
#include "theia/sfm/types.h"
#include "theia/util/map_util.h"
#include "theia/util/util.h"

namespace theia {
namespace {

using Eigen::Matrix3d;
using Eigen::Vector3d;

Vector3d GetRotatedTranslation(const Vector3d& rotation_angle_axis,
                               const Vector3d& translation) {
  Matrix3d rotation;
  ceres::AngleAxisToRotationMatrix(
      rotation_angle_axis.data(),
      ceres::ColumnMajorAdapter3x3(rotation.data()));
  return rotation.transpose() * translation;
}

}  // namespace

LeastUnsquaredDeviationPositionEstimator::
    LeastUnsquaredDeviationPositionEstimator(
        const LeastUnsquaredDeviationPositionEstimator::Options& options)
    : options_(options) {
  CHECK_GT(options_.max_num_iterations, 0);
  CHECK_GT(options_.max_num_reweighted_iterations, 0);
}

bool LeastUnsquaredDeviationPositionEstimator::EstimatePositions(
    const std::unordered_map<ViewIdPair, TwoViewInfo>& view_pairs,
    const std::unordered_map<ViewId, Vector3d>& orientations,
    std::unordered_map<ViewId, Vector3d>* positions) {
  CHECK_NOTNULL(positions)->clear();

  InitializeIndexMapping(view_pairs, orientations);
  const int num_views = view_id_to_index_.size();
  const int num_view_pairs = view_id_pair_to_index_.size();

  // Set up the linear system.
  SetupConstraintMatrix(view_pairs, orientations);
  Eigen::VectorXd solution;
  solution.setZero(constraint_matrix_.cols());

  // Create the lower bound constraint enforcing that all scales are > 1.
  Eigen::SparseMatrix<double> geq_mat(num_view_pairs,
                                      constraint_matrix_.cols());
  for (int i = 0; i < num_view_pairs; i++) {
    geq_mat.insert(i, 3 * (num_views - 1) + i) = 1.0;
  }
  Eigen::VectorXd geq_vec(num_view_pairs);
  geq_vec.setConstant(1.0);

  Eigen::VectorXd b(constraint_matrix_.rows());
  b.setZero();

  // Solve for camera positions by solving a constrained L1 problem to enforce
  // all relative translations scales > 1.
  ConstrainedL1Solver::Options l1_options;
  ConstrainedL1Solver solver(
      l1_options, constraint_matrix_, b, geq_mat, geq_vec);
  solver.Solve(&solution);

  // Set the estimated positions.
  for (const auto& view_id_index : view_id_to_index_) {
    const int index = view_id_index.second;
    const ViewId view_id = view_id_index.first;
    if (index == kConstantViewIndex) {
      (*positions)[view_id] = Eigen::Vector3d::Zero();
    } else {
      (*positions)[view_id] = solution.segment<3>(index);
    }
  }

  return true;
}

void LeastUnsquaredDeviationPositionEstimator::InitializeIndexMapping(
    const std::unordered_map<ViewIdPair, TwoViewInfo>& view_pairs,
    const std::unordered_map<ViewId, Vector3d>& orientations) {
  std::unordered_set<ViewId> views;
  for (const auto& view_pair : view_pairs) {
    if (ContainsKey(orientations, view_pair.first.first) &&
        ContainsKey(orientations, view_pair.first.second)) {
      views.insert(view_pair.first.first);
      views.insert(view_pair.first.second);
    }
  }

  // Create a mapping from the view id to the index of the linear system.
  int index = kConstantViewIndex;
  view_id_to_index_.reserve(views.size());
  for (const ViewId view_id : views) {
    view_id_to_index_[view_id] = index;
    index += 3;
  }

  // Create a mapping from the view id pair to the index of the linear system.
  view_id_pair_to_index_.reserve(view_pairs.size());
  for (const auto& view_pair : view_pairs) {
    if (ContainsKey(view_id_to_index_, view_pair.first.first) &&
        ContainsKey(view_id_to_index_, view_pair.first.second)) {
      view_id_pair_to_index_[view_pair.first] = index;
      ++index;
    }
  }
}

void LeastUnsquaredDeviationPositionEstimator::SetupConstraintMatrix(
    const std::unordered_map<ViewIdPair, TwoViewInfo>& view_pairs,
    const std::unordered_map<ViewId, Vector3d>& orientations) {
  constraint_matrix_.resize(
      3 * view_id_pair_to_index_.size(),
      3 * (view_id_to_index_.size() - 1) + view_pairs.size());

  // Add the camera to camera constraints.
  std::vector<Eigen::Triplet<double> > triplet_list;
  triplet_list.reserve(9 * view_pairs.size());
  int row = 0;
  for (const auto& view_pair : view_pairs) {
    const ViewIdPair view_id_pair = view_pair.first;
    if (!ContainsKey(view_id_to_index_, view_id_pair.first) ||
        !ContainsKey(view_id_to_index_, view_id_pair.second)) {
      continue;
    }

    const int view1_index = FindOrDie(view_id_to_index_, view_id_pair.first);
    const int view2_index = FindOrDie(view_id_to_index_, view_id_pair.second);
    const int scale_index =
        FindOrDieNoPrint(view_id_pair_to_index_, view_id_pair);

    // Rotate the relative translation so that it is aligned to the global
    // orientation frame.
    const Vector3d translation_direction =
        GetRotatedTranslation(FindOrDie(orientations, view_id_pair.first),
                              view_pair.second.position_2);

    // Add the constraint for view 1 in the minimization:
    //   position2 - position1 - scale_1_2 * translation_direction.
    if (view1_index != kConstantViewIndex) {
      triplet_list.emplace_back(row + 0, view1_index + 0, -1.0);
      triplet_list.emplace_back(row + 1, view1_index + 1, -1.0);
      triplet_list.emplace_back(row + 2, view1_index + 2, -1.0);
    }

    // Add the constraint for view 2 in the minimization:
    //   position2 - position1 - scale_1_2 * translation_direction.
    if (view2_index != kConstantViewIndex) {
      triplet_list.emplace_back(row + 0, view2_index + 0, 1.0);
      triplet_list.emplace_back(row + 1, view2_index + 1, 1.0);
      triplet_list.emplace_back(row + 2, view2_index + 2, 1.0);
    }

    // Add the constraint for scale in the minimization:
    //   position2 - position1 - scale_1_2 * translation_direction.
    triplet_list.emplace_back(row + 0, scale_index, -translation_direction[0]);
    triplet_list.emplace_back(row + 1, scale_index, -translation_direction[1]);
    triplet_list.emplace_back(row + 2, scale_index, -translation_direction[2]);

    row += 3;
  }

  constraint_matrix_.setFromTriplets(triplet_list.begin(), triplet_list.end());

  VLOG(2) << view_pairs.size() << " camera to camera constraints were added "
                                  "to the position estimation problem.";
}

std::unordered_map<ViewId, Eigen::Vector3d> LeastUnsquaredDeviationPositionEstimator::EstimatePositionsWrapper(
      const std::unordered_map<ViewIdPair, TwoViewInfo>& view_pairs,
      const std::unordered_map<ViewId, Eigen::Vector3d>& orientation) {
  std::unordered_map<ViewId, Eigen::Vector3d> positions;
  EstimatePositions(view_pairs, orientation, &positions);
  return positions;
}

}  // namespace theia


//          Copyright Tom Westerhout 2017-2018.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <chrono>
#include <fstream>
#include <iostream>
#include <random>
#include <unordered_map>

#include <benchmark/benchmark.h>

#include <boost/static_views.hpp>

// Configuration
#if !defined(BUCKET_SIZE)
#error "Need BUCKET_SIZE"
#endif

#if !defined(BUCKET_COUNT)
#error "Need BUCKET_COUNT"
#endif

// #define BOOST_STATIC_VIEWS_CONSTEXPR

namespace sv = boost::static_views;

using key_type       = std::uint16_t;
using mapped_type    = std::uint64_t;
using field_type     = std::pair<key_type, mapped_type>;
using hasher_type    = sv::hash_c;
using key_equal_type = std::equal_to<>;

constexpr auto bucket_size       = BUCKET_SIZE;
constexpr auto number_of_buckets = BUCKET_COUNT;

static constexpr field_type data_table[] = {
#include "test_data.ipp"
};

constexpr auto data_size = std::end(data_table) - std::begin(data_table);

auto generate_lookup_keys(std::size_t const n, double const positive_frac,
    std::size_t seed) -> std::vector<key_type>
{
    if (n < static_cast<std::size_t>(data_size)) {
        throw std::invalid_argument{"Currently, only `n`s greater or equal to "
                                    "`data_size` are supported."};
    }
    if (positive_frac < 0 || positive_frac > 1) {
        throw std::invalid_argument{
            "Fraction of positive samples must be between 0 and 1."};
    }

    std::mt19937                            gen(seed);
    std::uniform_int_distribution<key_type> dist;
    std::vector<key_type>                   keys(n);
    auto const pos_n = static_cast<std::size_t>(positive_frac * n);

    std::transform(std::begin(data_table), std::end(data_table),
        std::begin(keys), [](auto const& x) { return x.first; });
    std::shuffle(std::begin(keys), std::begin(keys) + data_size, gen);
    // Yes, I know this is extremely slow and may even never terminate.
    // But it's simple! :)
    for (auto i = std::begin(keys) + pos_n; i != std::end(keys); ++i) {
        auto const end = std::begin(keys) + pos_n;
        auto       k   = dist(gen);
        while (std::find(std::begin(keys), end, k) != end) {
            ++k;
        }
        *i = k;
    }
    std::shuffle(std::begin(keys), std::end(keys), gen);

    return keys;
}

auto lookup_noop(key_type const k) -> field_type* { return nullptr; }

auto benchmark_empty_lookup(benchmark::State& state)
{
    auto const seed = state.range(0);
    auto const n    = state.range(1);
    auto const keys = generate_lookup_keys(n, 0.5, seed);

    for (auto _ : state) {
        for (auto k : keys) {
            benchmark::DoNotOptimize(lookup_noop(k));
        }
    }
}

auto benchmark_static_map_lookup(benchmark::State& state)
{
    auto const seed = state.range(0);
    auto const n    = state.range(1);
    auto const keys = generate_lookup_keys(n, 0.5, seed);

    static constexpr auto map =
        sv::make_static_map_impl<number_of_buckets, bucket_size>{}(
            sv::raw_view(data_table), &field_type::first, &field_type::second,
            key_equal_type{}, hasher_type{});

    for (auto _ : state) {
        for (auto k : keys) {
            benchmark::DoNotOptimize(map.find(k));
        }
    }

    state.SetComplexityN(state.range(1));
}

auto benchmark_unordered_map_lookup(benchmark::State& state)
{
    auto const seed = state.range(0);
    auto const n    = state.range(1);
    auto const keys = generate_lookup_keys(n, 0.5, seed);

    auto const map =
        std::unordered_map<key_type, mapped_type, hasher_type, key_equal_type>{
            std::begin(data_table), std::end(data_table), number_of_buckets};

    for (auto _ : state) {
        for (auto k : keys) {
            benchmark::DoNotOptimize(map.find(k));
        }
    }

    state.SetComplexityN(state.range(1));
}

constexpr auto N = static_cast<long>(1.5 * data_size);

// clang-format off
BENCHMARK(benchmark_static_map_lookup)
    ->Args({123, 1 * N})
    ->Args({124, 1 * N})
    ->Args({125, 1 * N})
    ->Args({126, 1 * N})
    ->Args({127, 2 * N})
    ->Args({128, 2 * N})
    ->Args({129, 2 * N})
    ->Args({130, 2 * N})
    ->Args({131, 3 * N})
    ->Args({132, 3 * N})
    ->Args({133, 3 * N})
    ->Args({134, 3 * N})
    ->Args({135, 6 * N})
    ->Args({136, 6 * N})
    ->Args({137, 6 * N})
    ->Args({138, 6 * N})
    ->Args({139, 8 * N})
    ->Args({140, 8 * N})
    ->Args({141, 8 * N})
    ->Args({142, 8 * N})
    ->Complexity([](auto const n) -> double { return n; });

BENCHMARK(benchmark_unordered_map_lookup)
    ->Args({123, 1 * N})
    ->Args({124, 1 * N})
    ->Args({125, 1 * N})
    ->Args({126, 1 * N})
    ->Args({127, 2 * N})
    ->Args({128, 2 * N})
    ->Args({129, 2 * N})
    ->Args({130, 2 * N})
    ->Args({131, 3 * N})
    ->Args({132, 3 * N})
    ->Args({133, 3 * N})
    ->Args({134, 3 * N})
    ->Args({135, 6 * N})
    ->Args({136, 6 * N})
    ->Args({137, 6 * N})
    ->Args({138, 6 * N})
    ->Args({139, 8 * N})
    ->Args({140, 8 * N})
    ->Args({141, 8 * N})
    ->Args({142, 8 * N})
    ->Complexity([](auto const n) -> double { return n; });
// clang-format on

BENCHMARK_MAIN();