//
//  Copyright (c) 2009
//  Gunter Winkler
//
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//
//

#ifndef _BOOST_UBLAS_SPARSE_VIEW_
#define _BOOST_UBLAS_SPARSE_VIEW_

#include <boost/numeric/ublas/matrix_expression.hpp>
#include <boost/numeric/ublas/detail/matrix_assign.hpp>
#if BOOST_UBLAS_TYPE_CHECK
#include <boost/numeric/ublas/matrix.hpp>
#endif

#include <boost/next_prior.hpp>
#include <boost/type_traits/remove_cv.hpp>

namespace boost { namespace numeric { namespace ublas {

    // view a chunk of memory as ublas array

    template < class T >
    class c_array_view
        : public storage_array< c_array_view<T> > {
    private:
        typedef c_array_view<T> self_type;
        typedef T * pointer;

    public:
        // TODO: think about a const pointer
        typedef const pointer array_type;

        typedef std::size_t size_type;
        typedef std::ptrdiff_t difference_type;

        typedef T value_type;
        typedef const T  &const_reference;
        typedef const T  *const_pointer;

        typedef const_pointer const_iterator;
        typedef std::reverse_iterator<const_iterator> const_reverse_iterator;

        //
        // typedefs required by vector concept
        //

        typedef dense_tag  storage_category;
        typedef const vector_reference<const self_type>    const_closure_type;

        c_array_view(size_type size, array_type data) :
            size_(size), data_(data)
        {}

        ~c_array_view()
        {}

        //
        // immutable methods of container concept
        //

        BOOST_UBLAS_INLINE
        size_type size () const {
            return size_;
        }

        BOOST_UBLAS_INLINE
        const_reference operator [] (size_type i) const {
            BOOST_UBLAS_CHECK (i < size_, bad_index ());
            return data_ [i];
        }

        BOOST_UBLAS_INLINE
        const_iterator begin () const {
            return data_;
        }
        BOOST_UBLAS_INLINE
        const_iterator end () const {
            return data_ + size_;
        }

        BOOST_UBLAS_INLINE
        const_reverse_iterator rbegin () const {
            return const_reverse_iterator (end ());
        }
        BOOST_UBLAS_INLINE
        const_reverse_iterator rend () const {
            return const_reverse_iterator (begin ());
        }

    private:
        size_type  size_;
        array_type data_;
    };

    /** \brief Present existing arrays as compressed array based
     *  sparse matrix.
     *  This class provides CRS / CCS storage layout.
     *
     *  see also http://www.netlib.org/utk/papers/templates/node90.html
     *
     *       \param L layout type, either row_major or column_major
     *       \param IB index base, use 0 for C indexing and 1 for
     *       FORTRAN indexing of the internal index arrays. This
     *       does not affect the operator()(int,int) where the first
     *       row/column has always index 0.
     *       \param IA index array type, e.g., int[]
     *       \param TA value array type, e.g., double[]
     */
    template<class L, std::size_t IB, class IA, class JA, class TA>
    class compressed_matrix_view:
        public matrix_expression<compressed_matrix_view<L, IB, IA, JA, TA> > {

    public:
        typedef typename vector_view_traits<TA>::value_type value_type;

    private:
        typedef value_type &true_reference;
        typedef value_type *pointer;
        typedef const value_type *const_pointer;
        typedef L layout_type;
        typedef compressed_matrix_view<L, IB, IA, JA, TA> self_type;

    public:
#ifdef BOOST_UBLAS_ENABLE_PROXY_SHORTCUTS
        using matrix_expression<self_type>::operator ();
#endif
        // ISSUE require type consistency check
        // is_convertable (IA::size_type, TA::size_type)
        typedef typename boost::remove_cv<typename vector_view_traits<JA>::value_type>::type index_type;
        // for compatibility, should be removed some day ...
        typedef index_type size_type;
        // size_type for the data arrays.
        typedef typename vector_view_traits<JA>::size_type array_size_type;
        typedef typename vector_view_traits<JA>::difference_type difference_type;
        typedef const value_type & const_reference;

        // do NOT define reference type, because class is read only
        // typedef value_type & reference;

        typedef IA rowptr_array_type;
        typedef JA index_array_type;
        typedef TA value_array_type;
        typedef const matrix_reference<const self_type> const_closure_type;
        typedef matrix_reference<self_type> closure_type;

        // FIXME: define a corresponding temporary type
        // typedef compressed_vector<T, IB, IA, TA> vector_temporary_type;

        // FIXME: define a corresponding temporary type
        // typedef self_type matrix_temporary_type;

        typedef sparse_tag storage_category;
        typedef typename L::orientation_category orientation_category;

        //
        // private types for internal use
        //

    private:
        typedef typename vector_view_traits<index_array_type>::const_iterator const_subiterator_type;

        //
        // Construction and destruction
        //
    private:
        /// private default constructor because data must be filled by caller
        BOOST_UBLAS_INLINE
        compressed_matrix_view () { }

    public:
        BOOST_UBLAS_INLINE
        compressed_matrix_view (index_type n_rows, index_type n_cols, array_size_type nnz
                                , const rowptr_array_type & iptr
                                , const index_array_type & jptr
                                , const value_array_type & values):
            matrix_expression<self_type> (),
            size1_ (n_rows), size2_ (n_cols),
            nnz_ (nnz),
            index1_data_ (iptr),
            index2_data_ (jptr),
            value_data_ (values) {
            storage_invariants ();
        }

        BOOST_UBLAS_INLINE
        compressed_matrix_view(const compressed_matrix_view& o) :
            size1_(size1_), size2_(size2_),
            nnz_(nnz_),
            index1_data_(index1_data_),
            index2_data_(index2_data_),
            value_data_(value_data_)
        {}

        //
        // implement immutable iterator types
        //

        class const_iterator1 {};
        class const_iterator2 {};

        typedef reverse_iterator_base1<const_iterator1> const_reverse_iterator1;
        typedef reverse_iterator_base2<const_iterator2> const_reverse_iterator2;

        //
        // implement all read only methods for the matrix expression concept
        //

        //! return the number of rows
        index_type size1() const {
            return size1_;
        }

        //! return the number of columns
        index_type size2() const {
            return size2_;
        }

        //! return value at position (i,j)
        value_type operator()(index_type i, index_type j) const {
            const_pointer p = find_element(i,j);
            if (!p) {
                return zero_;
            } else {
                return *p;
            }
        }

    private:
        //
        // private helper functions
        //

        const_pointer find_element (index_type i, index_type j) const {
            index_type element1 (layout_type::index_M (i, j));
            index_type element2 (layout_type::index_m (i, j));

            const array_size_type itv      = zero_based( index1_data_[element1] );
            const array_size_type itv_next = zero_based( index1_data_[element1+1] );

            const_subiterator_type it_start = boost::next(vector_view_traits<index_array_type>::begin(index2_data_),itv);
            const_subiterator_type it_end = boost::next(vector_view_traits<index_array_type>::begin(index2_data_),itv_next);
            const_subiterator_type it = find_index_in_row(it_start, it_end, element2) ;

            if (it == it_end || *it != k_based (element2))
                return 0;
            return &value_data_ [it - vector_view_traits<index_array_type>::begin(index2_data_)];
        }

        const_subiterator_type find_index_in_row(const_subiterator_type it_start
                                                 , const_subiterator_type it_end
                                                 , index_type index) const {
            return std::lower_bound( it_start
                                     , it_end
                                     , k_based (index) );
        }

    private:
        void storage_invariants () const {
            BOOST_UBLAS_CHECK (index1_data_ [layout_type::size_M (size1_, size2_)] == k_based (nnz_), external_logic ());
        }

        index_type size1_;
        index_type size2_;

        array_size_type nnz_;

        const rowptr_array_type & index1_data_;
        const index_array_type & index2_data_;
        const value_array_type & value_data_;

        static const value_type zero_;

        BOOST_UBLAS_INLINE
        static index_type zero_based (index_type k_based_index) {
            return k_based_index - IB;
        }
        BOOST_UBLAS_INLINE
        static index_type k_based (index_type zero_based_index) {
            return zero_based_index + IB;
        }

        friend class iterator1;
        friend class iterator2;
        friend class const_iterator1;
        friend class const_iterator2;
    };

    template<class L, std::size_t IB, class IA, class JA, class TA  >
    const typename compressed_matrix_view<L,IB,IA,JA,TA>::value_type
    compressed_matrix_view<L,IB,IA,JA,TA>::zero_ = value_type/*zero*/();

    template<class L, std::size_t IB, class IA, class JA, class TA  >
    compressed_matrix_view<L,IB,IA,JA,TA>
    make_compressed_matrix_view(typename vector_view_traits<JA>::value_type n_rows
                                , typename vector_view_traits<JA>::value_type n_cols
                                , typename vector_view_traits<JA>::size_type nnz
                                , const IA & ia
                                , const JA & ja
                                , const TA & ta) {

        return compressed_matrix_view<L,IB,IA,JA,TA>(n_rows, n_cols, nnz, ia, ja, ta);

    }

}}}

#endif


// Copyright 2015-2018 Hans Dembinski
//
// 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 <boost/core/lightweight_test.hpp>
#include <boost/histogram.hpp>
#include <string>
#include <utility>
#include "utility_histogram.hpp"
#include "utility_meta.hpp"

using namespace boost::histogram;

using def = use_default;

using regular = axis::regular<double, def, def, axis::option::growth>;

using integer = axis::integer<
    double, def,
    axis::join<axis::option::underflow, axis::option::overflow, axis::option::growth>>;

using category = axis::category<std::string, def, axis::option::growth>;

class custom_2d_axis {
public:
  auto index(std::tuple<double, double> xy) const {
    const auto x = std::get<0>(xy);
    const auto y = std::get<1>(xy);
    const auto r = std::sqrt(x * x + y * y);
    return std::min(static_cast<axis::index_type>(r), size());
  }

  auto update(std::tuple<double, double> xy) {
    const auto x = std::get<0>(xy);
    const auto y = std::get<1>(xy);
    const auto r = std::sqrt(x * x + y * y);
    const auto n = static_cast<int>(r);
    const auto old = size_;
    if (n >= size_) size_ = n + 1;
    return std::make_pair(n, old - size_);
  }

  axis::index_type size() const { return size_; }

private:
  axis::index_type size_ = 0;
};

template <typename Tag>
void run_tests() {
  {
    auto h = make(Tag(), regular(2, 0, 1));
    const auto& a = h.axis();
    BOOST_TEST_EQ(a.size(), 2);
    BOOST_TEST_EQ(h.size(), 2);
    // [0.0, 0.5, 1.0]
    h(0.1);
    h(0.9);
    BOOST_TEST_EQ(a.size(), 2);
    BOOST_TEST_EQ(h.size(), 2);
    h(-std::numeric_limits<double>::infinity());
    h(std::numeric_limits<double>::quiet_NaN());
    h(std::numeric_limits<double>::infinity());
    BOOST_TEST_EQ(a.size(), 2);
    BOOST_TEST_EQ(h.size(), 2);
    h(-0.3);
    // [-0.5, 0.0, 0.5, 1.0]
    BOOST_TEST_EQ(a.size(), 3);
    BOOST_TEST_EQ(h.size(), 3);
    BOOST_TEST_EQ(h[0], 1);
    BOOST_TEST_EQ(h[1], 1);
    BOOST_TEST_EQ(h[2], 1);
    h(1.9);
    // [-0.5, 0.0, 0.5, 1.0, 1.5, 2.0]
    BOOST_TEST_EQ(a.size(), 5);
    BOOST_TEST_EQ(h.size(), 5);
    BOOST_TEST_EQ(h[0], 1);
    BOOST_TEST_EQ(h[1], 1);
    BOOST_TEST_EQ(h[2], 1);
    BOOST_TEST_EQ(h[3], 0);
    BOOST_TEST_EQ(h[4], 1);
  }

  {
    auto h = make_s(Tag(), std::vector<int>(), integer());
    const auto& a = h.axis();
    h(-std::numeric_limits<double>::infinity());
    h(std::numeric_limits<double>::quiet_NaN());
    h(std::numeric_limits<double>::infinity());
    BOOST_TEST_EQ(a.size(), 0);
    BOOST_TEST_EQ(h.size(), 2);
    BOOST_TEST_EQ(h[-1], 1);
    BOOST_TEST_EQ(h[0], 2);
    h(0);
    BOOST_TEST_EQ(a.size(), 1);
    BOOST_TEST_EQ(h.size(), 3);
    BOOST_TEST_EQ(h[-1], 1);
    BOOST_TEST_EQ(h[0], 1);
    BOOST_TEST_EQ(h[1], 2);
    h(2);
    BOOST_TEST_EQ(a.size(), 3);
    BOOST_TEST_EQ(h.size(), 5);
    BOOST_TEST_EQ(h[-1], 1);
    BOOST_TEST_EQ(h[0], 1);
    BOOST_TEST_EQ(h[1], 0);
    BOOST_TEST_EQ(h[2], 1);
    BOOST_TEST_EQ(h[3], 2);
    h(-2);
    BOOST_TEST_EQ(a.size(), 5);
    BOOST_TEST_EQ(h.size(), 7);
    // BOOST_TEST_EQ(h[-1], 1)
    BOOST_TEST_EQ(h[0], 1);
    BOOST_TEST_EQ(h[1], 0);
    BOOST_TEST_EQ(h[2], 1);
    BOOST_TEST_EQ(h[3], 0);
    BOOST_TEST_EQ(h[4], 1);
    BOOST_TEST_EQ(h[5], 2);
  }

  {
    auto h = make_s(Tag(), std::vector<int>(), integer(), category());
    const auto& a = h.axis(0);
    const auto& b = h.axis(1);
    BOOST_TEST_EQ(a.size(), 0);
    BOOST_TEST_EQ(b.size(), 0);
    BOOST_TEST_EQ(h.size(), 0);
    h(0, "x");
    h(-std::numeric_limits<double>::infinity(), "x");
    h(std::numeric_limits<double>::infinity(), "x");
    h(std::numeric_limits<double>::quiet_NaN(), "x");
    BOOST_TEST_EQ(a.size(), 1);
    BOOST_TEST_EQ(b.size(), 1);
    BOOST_TEST_EQ(h.size(), 3);
    h(2, "x");
    BOOST_TEST_EQ(a.size(), 3);
    BOOST_TEST_EQ(b.size(), 1);
    BOOST_TEST_EQ(h.size(), 5);
    h(1, "y");
    BOOST_TEST_EQ(a.size(), 3);
    BOOST_TEST_EQ(b.size(), 2);
    BOOST_TEST_EQ(h.size(), 10);
    BOOST_TEST_EQ(h.at(-1, 0), 1);
    BOOST_TEST_EQ(h.at(-1, 1), 0);
    BOOST_TEST_EQ(h.at(3, 0), 2);
    BOOST_TEST_EQ(h.at(3, 1), 0);
    BOOST_TEST_EQ(h.at(a.index(0), b.index("x")), 1);
    BOOST_TEST_EQ(h.at(a.index(1), b.index("x")), 0);
    BOOST_TEST_EQ(h.at(a.index(2), b.index("x")), 1);
    BOOST_TEST_EQ(h.at(a.index(0), b.index("y")), 0);
    BOOST_TEST_EQ(h.at(a.index(1), b.index("y")), 1);
    BOOST_TEST_EQ(h.at(a.index(2), b.index("y")), 0);

    BOOST_TEST_THROWS(h(0, "x", 42), std::invalid_argument);
  }

  {
    auto h = make_s(Tag{}, std::vector<int>{}, custom_2d_axis{});
    BOOST_TEST_EQ(h.size(), 0);
    h(0, 0);
    BOOST_TEST_EQ(h.size(), 1);
    h(1, 0);
    h(0, 1);
    BOOST_TEST_EQ(h.size(), 2);
    h(10, 0);
    BOOST_TEST_EQ(h.size(), 11);
    BOOST_TEST_EQ(h[0], 1);
    BOOST_TEST_EQ(h[1], 2);
    BOOST_TEST_EQ(h[10], 1);
    BOOST_TEST_THROWS(h(0), std::invalid_argument);
  }
}

int main() {
  run_tests<static_tag>();
  run_tests<dynamic_tag>();

  return boost::report_errors();
}


// Copyright Louis Dionne 2013-2016
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/assert.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/less.hpp>
#include <boost/hana/pair.hpp>
#include <boost/hana/span.hpp>
#include <boost/hana/tuple.hpp>
namespace hana = boost::hana;

constexpr auto xs = hana::make_tuple(hana::int_c<1>, hana::int_c<2>, hana::int_c<3>, hana::int_c<4>);

BOOST_HANA_CONSTANT_CHECK(
    hana::span(xs, hana::less.than(hana::int_c<3>))
    ==
    hana::make_pair(hana::make_tuple(hana::int_c<1>, hana::int_c<2>),
                    hana::make_tuple(hana::int_c<3>, hana::int_c<4>))
);

BOOST_HANA_CONSTANT_CHECK(
    hana::span(xs, hana::less.than(hana::int_c<0>))
    ==
    hana::make_pair(hana::make_tuple(), xs)
);

BOOST_HANA_CONSTANT_CHECK(
    hana::span(xs, hana::less.than(hana::int_c<5>))
    ==
    hana::make_pair(xs, hana::make_tuple())
);

// span.by is syntactic sugar
BOOST_HANA_CONSTANT_CHECK(
    hana::span.by(hana::less.than(hana::int_c<3>), xs)
    ==
    hana::make_pair(hana::make_tuple(hana::int_c<1>, hana::int_c<2>),
                    hana::make_tuple(hana::int_c<3>, hana::int_c<4>))
);

int main() { }


#include "solver_interface.hpp"
#include <iostream>
#include "macros.h"
#include <boost/format.hpp>
#include <sstream>
#include <map>
#include <boost/foreach.hpp>
using namespace std;

namespace sco {

vector<int> vars2inds(const vector<Var>& vars) {
  vector<int> inds(vars.size());
  for (size_t i=0; i < inds.size(); ++i) inds[i] = vars[i].var_rep->index;
  return inds;
}
vector<int> cnts2inds(const vector<Cnt>& cnts) {
  vector<int> inds(cnts.size());
  for (size_t i=0; i < inds.size(); ++i) inds[i] = cnts[i].cnt_rep->index;
  return inds;
}

void simplify2(vector<int>& inds, vector<double>& vals) {
  typedef std::map<int, double> Int2Double;
  Int2Double ind2val;
  for (unsigned i=0; i < inds.size(); ++i) {
    if (vals[i] != 0) ind2val[inds[i]] += vals[i];
  }
  inds.resize(ind2val.size());
  vals.resize(ind2val.size());
  int i_new = 0;
  BOOST_FOREACH(Int2Double::value_type& iv, ind2val) {
    inds[i_new] = iv.first;
    vals[i_new] = iv.second;
    ++i_new;
  }
}

double AffExpr::value(const double* x) const {
  double out = constant;
  for (size_t i=0; i < size(); ++i) {
    out += coeffs[i] * vars[i].value(x);
  }
  return out;
}
double AffExpr::value(const vector<double>& x) const {
  double out = constant;
  for (size_t i=0; i < size(); ++i) {
    out += coeffs[i] * vars[i].value(x);
  }
  return out;
}
double QuadExpr::value(const vector<double>& x) const {
  double out = affexpr.value(x);
  for (size_t i=0; i < size(); ++i) {
    out += coeffs[i] * vars1[i].value(x) * vars2[i].value(x);
  }
  return out;
}
double QuadExpr::value(const double* x) const {
  double out = affexpr.value(x);
  for (size_t i=0; i < size(); ++i) {
    out += coeffs[i] * vars1[i].value(x) * vars2[i].value(x);
  }
  return out;
}

Var Model::addVar(const string& name, double lb, double ub) {
  Var v = addVar(name);
  setVarBounds(v, lb, ub);
  return v;
}
void Model::removeVar(const Var& var) {
  vector<Var> vars(1,var);
  removeVars(vars);
}
void Model::removeCnt(const Cnt& cnt) {
  vector<Cnt> cnts(1, cnt);
  removeCnts(cnts);
}

double Model::getVarValue(const Var& var) const {
  VarVector vars(1,var);
  return getVarValues(vars)[0];
}

void Model::setVarBounds(const Var& var, double lower, double upper) {
  vector<double> lowers(1,lower), uppers(1, upper);
  vector<Var> vars(1,var);
  setVarBounds(vars, lowers, uppers);
}

ostream& operator<<(ostream& o, const Var& v) {
  if (v.var_rep != NULL)
    o << v.var_rep->name;
  else
    o << "nullvar";
  return o;
}
ostream& operator<<(ostream& o, const Cnt& c) {
  o << c.cnt_rep->expr << ((c.cnt_rep->type == EQ) ? " == 0" : " <= 0");
  return o;
}
ostream& operator<<(ostream& o, const AffExpr& e) {
  o << e.constant;
  for (size_t i=0; i < e.size(); ++i) {
    o << " + " << e.coeffs[i] << "*" << e.vars[i];
  }
  return o;
}
ostream& operator<<(ostream& o, const QuadExpr& e) {
  o << e.affexpr;
  for (size_t i=0; i < e.size(); ++i) {
    o << " + " << e.coeffs[i] << "*" << e.vars1[i] << "*" << e.vars2[i];
  }
  return o;
}

ModelPtr createModel() {

#ifdef HAVE_GUROBI
  extern ModelPtr createGurobiModel();
#endif
#ifdef HAVE_BPMPD
  extern ModelPtr createBPMPDModel();
#endif

  enum ConvexSolver {
    GUROBI,
    BPMPD,
    INVALID
  };

  char* solver_env = getenv("TRAJOPT_CONVEX_SOLVER");

  ConvexSolver solver = INVALID;

  if (solver_env) {
    if (string(solver_env) == "GUROBI") solver = GUROBI;
    else if (string(solver_env) == "BPMPD") solver = BPMPD;
    else PRINT_AND_THROW( boost::format("invalid solver \"%s\"specified by TRAJOPT_CONVEX_SOLVER")%solver_env);
#ifndef HAVE_GUROBI
    if (solver == GUROBI) PRINT_AND_THROW("you didn't build with GUROBI support");
#endif
#ifndef HAVE_BPMPD
    if (solver == BPMPD) PRINT_AND_THROW("you don't have BPMPD support on this platform");
#endif

  }
  else {
#ifdef HAVE_GUROBI
  solver = GUROBI;
#else
  solver = BPMPD;
#endif
  }

#ifdef HAVE_GUROBI
  if (solver == GUROBI) return createGurobiModel();
#endif
#ifdef HAVE_BPMPD
  if (solver == BPMPD) return createBPMPDModel();
#endif
  PRINT_AND_THROW("Failed to create solver");
  return ModelPtr();

}

}


/* pcmsolver_copyright_start */
/*
 *     PCMSolver, an API for the Polarizable Continuum Model
 *     Copyright (C) 2013-2016 Roberto Di Remigio, Luca Frediani and contributors
 *
 *     This file is part of PCMSolver.
 *
 *     PCMSolver is free software: you can redistribute it and/or modify
 *     it under the terms of the GNU Lesser General Public License as published by
 *     the Free Software Foundation, either version 3 of the License, or
 *     (at your option) any later version.
 *
 *     PCMSolver 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 PCMSolver.  If not, see <http://www.gnu.org/licenses/>.
 *
 *     For information on the complete list of contributors to the
 *     PCMSolver API, see: <http://pcmsolver.readthedocs.io/>
 */
/* pcmsolver_copyright_end */

#include "catch.hpp"

#include <iostream>

#include <Eigen/Core>

#include "bi_operators/CollocationIntegrator.hpp"
#include "green/DerivativeTypes.hpp"
#include "cavity/GePolCavity.hpp"
#include "utils/Molecule.hpp"
#include "green/Vacuum.hpp"
#include "TestingMolecules.hpp"
#include "green/UniformDielectric.hpp"
#include "solver/IEFSolver.hpp"
#include "utils/Symmetry.hpp"

/*! \class IEFSolver
 *  \test \b NH3GePol tests IEFSolver using ammonia and a GePol cavity
 */
TEST_CASE("Test solver for the IEFPCM with NH3 molecule and a GePol cavity", "[solver][iefpcm][iefpcm_gepol-NH3]")
{
    Molecule molec = NH3();

    double area = 0.4;
    double probeRadius = 0.0;
    double minRadius = 100.0;
    GePolCavity cavity = GePolCavity(molec, area, probeRadius, minRadius);
    cavity.saveCavity("nh3.npz");

    double permittivity = 78.39;
    Vacuum<> gf_i;
    UniformDielectric<> gf_o(permittivity);
    bool symm = true;
    IEFSolver solver(symm);
    solver.buildSystemMatrix(cavity, gf_i, gf_o);

    double Ncharge = 7.0;
    double Hcharge = 1.0;
    int size = cavity.size();
    Eigen::VectorXd fake_mep = computeMEP(molec, cavity.elements());
    // The total ASC for a dielectric is -Q*(epsilon-1)/epsilon
    Eigen::VectorXd fake_asc = Eigen::VectorXd::Zero(size);
    fake_asc = solver.computeCharge(fake_mep);
    double totalASC = - (Ncharge + 3.0 * Hcharge) * (permittivity - 1) / permittivity;
    double totalFakeASC = fake_asc.sum();
    CAPTURE(totalASC - totalFakeASC);
    REQUIRE(totalASC == Approx(totalFakeASC).epsilon(1.0e-03));
}


// Copyright Louis Dionne 2013-2017
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/assert.hpp>
#include <boost/hana/config.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/replace_if.hpp>
#include <boost/hana/tuple.hpp>
namespace hana = boost::hana;

BOOST_HANA_CONSTEXPR_LAMBDA auto negative = [](auto x) {
    return x < 0;
};

int main() {
    BOOST_HANA_CONSTEXPR_CHECK(
        hana::replace_if(hana::make_tuple(-3, -2, -1, 0, 1, 2, 3), negative, 0)
                ==
        hana::make_tuple(0, 0, 0, 0, 1, 2, 3)
    );
}


// Copyright 2018 The Simons Foundation, Inc. - All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//    http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <Eigen/Dense>
#include <algorithm>
#include <cmath>
#include <iostream>
#include "Utils/random_utils.hpp"
#include <vector>
#include "Utils/json_utils.hpp"
#include "abstract_hilbert.hpp"

#ifndef NETKET_BOSONS_HPP
#define NETKET_BOSONS_HPP

namespace netket {

/**
  Hilbert space for integer or bosons.
  The hilbert space is truncated to some maximum occupation number.
*/

class Boson : public AbstractHilbert {
  int nsites_;

  std::vector<double> local_;

  // total number of bosons
  // if constraint is activated
  int nbosons_;

  bool constraintN_;

  // maximum local occupation number
  int nmax_;

  int nstates_;

 public:
  explicit Boson(const json &pars) {
    if (!FieldExists(pars["Hilbert"], "Nsites")) {
      std::cerr << "Nsites is not defined" << std::endl;
    }

    nsites_ = pars["Hilbert"]["Nsites"];

    if (!FieldExists(pars["Hilbert"], "Nmax")) {
      std::cerr << "Nmax is not defined" << std::endl;
    }

    nmax_ = pars["Hilbert"]["Nmax"];

    Init();

    if (FieldExists(pars["Hilbert"], "Nbosons")) {
      SetNbosons(pars["Hilbert"]["Nbosons"]);
    } else {
      constraintN_ = false;
    }
  }

  void Init() {
    if (nsites_ <= 0) {
      std::cerr << "Invalid number of sites" << std::endl;
      std::abort();
    }

    if (nmax_ <= 0) {
      std::cerr << "Invalid maximum occupation number" << std::endl;
      std::abort();
    }

    nstates_ = nmax_ + 1;

    local_.resize(nstates_);

    for (int i = 0; i < nstates_; i++) {
      local_[i] = i;
    }
  }

  void SetNbosons(int nbosons) {
    constraintN_ = true;
    nbosons_ = nbosons;

    if (nbosons_ > nsites_ * nmax_) {
      std::cerr << "Cannot set the desired number of bosons" << std::endl;
      std::abort();
    }
  }

  bool IsDiscrete() const override { return true; }

  int LocalSize() const override { return nstates_; }

  int Size() const override { return nsites_; }

  std::vector<double> LocalStates() const override { return local_; }

  void RandomVals(Eigen::VectorXd &state,
                  netket::default_random_engine &rgen) const override {
    assert(state.size() == nsites_);

    if (!constraintN_) {
      std::uniform_int_distribution<int> distribution(0, nstates_ - 1);
      // unconstrained random
      for (int i = 0; i < state.size(); i++) {
        state(i) = distribution(rgen);
      }
    } else {
      state.setZero();

      std::uniform_int_distribution<int> distribution(0, nsites_ - 1);
      for (int i = 0; i < nbosons_; i++) {
        int rsite = distribution(rgen);

        while (state(rsite) >= nmax_) {
          rsite = distribution(rgen);
        }

        state(rsite) += 1;
      }
    }
  }

  bool CheckConstraint(Eigen::VectorXd &v) const {
    int tot = 0;
    for (int i = 0; i < v.size(); i++) {
      tot += int(v(i));
    }

    return tot == nbosons_;
  }

  void UpdateConf(Eigen::VectorXd &v, const std::vector<int> &tochange,
                  const std::vector<double> &newconf) const override {
    assert(v.size() == nsites_);

    int i = 0;
    for (auto sf : tochange) {
      v(sf) = newconf[i];
      i++;
      assert(v(sf) <= nmax_);
    }

    if (constraintN_) {
      assert(CheckConstraint(v));
    }
  }
};

}  // namespace netket
#endif


// This file is part of LatNet Builder.
//
// Copyright (C) 2012-2021  The LatNet Builder author's, supervised by Pierre L'Ecuyer, Universite de Montreal.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "latbuilder/Parser/CommandLine.h"
#include "latbuilder/TextStream.h"
#include <boost/lexical_cast.hpp>
#include <iostream>

LatBuilder::GeneratingVector searchLattice(
      LatBuilder::Modulus numPoints,
      LatBuilder::Dimension dimension
      )
{
   LatBuilder::Parser::CommandLine<LatBuilder::LatType::ORDINARY> cmd;
   cmd.construction  = "fast-CBC";
   cmd.size          = boost::lexical_cast<std::string>(numPoints);
   cmd.dimension     = boost::lexical_cast<std::string>(dimension);
   cmd.figure        = "CU:P2";
   cmd.weights       = std::vector<std::string>{"product:0.1"};
   auto search = cmd.parse();
   search->execute();
   return search->bestLattice().gen();
}

int main(int argc, const char *argv[])
{
   using LatBuilder::TextStream::operator<<;
   LatBuilder::Modulus n = 65537;
   LatBuilder::Dimension dim = 10;
   LatBuilder::GeneratingVector gen = searchLattice(n, dim);
   std::cout << "number of points:  " << n << std::endl;
   std::cout << "dimension:         " << dim << std::endl;
   std::cout << "generating vector: " << gen << std::endl;
   return 0;
}


#include <boost/test/unit_test.hpp>

#include <sway/math.h>

using namespace sway;

BOOST_AUTO_TEST_SUITE(VertexTestSuite)

/*!
 * \brief
 *    Убеждаемся, что конструктор по умолчанию приводит все компоненты к нулю.
 */
BOOST_AUTO_TEST_CASE(VertexPositionTestCase_DefaultConstructor) {
	const math::VertexPosition vertex;

	BOOST_CHECK_EQUAL(vertex.x, 0.0f);
	BOOST_CHECK_EQUAL(vertex.y, 0.0f);
	BOOST_CHECK_EQUAL(vertex.z, 0.0f);
}

/*!
 * \brief
 *    Убеждаемся, что конструктор устанавливает все значения компонентов в те,
 *    которые были заданы.
 */
BOOST_AUTO_TEST_CASE(VertexPositionTestCase_ComponentConstructor) {
	const math::VertexPosition vertex(math::vec3f_t(1.0f, 2.0f, 3.0f));
	const math::vec3f_t position = vertex.getPosition();

	BOOST_CHECK_EQUAL(position.getX(), 1.0f);
	BOOST_CHECK_EQUAL(position.getY(), 2.0f);
	BOOST_CHECK_EQUAL(position.getZ(), 3.0f);
}

/*!
 * \brief
 *    Убеждаемся, что конструктор по умолчанию приводит все компоненты к нулю.
 */
BOOST_AUTO_TEST_CASE(VertexColorTestCase_DefaultConstructor) {
	const math::VertexColor vertex;

	BOOST_CHECK_EQUAL(vertex.x, 0.0f);
	BOOST_CHECK_EQUAL(vertex.y, 0.0f);
	BOOST_CHECK_EQUAL(vertex.z, 0.0f);

	BOOST_CHECK_EQUAL(vertex.r, 0.0f);
	BOOST_CHECK_EQUAL(vertex.g, 0.0f);
	BOOST_CHECK_EQUAL(vertex.b, 0.0f);
	BOOST_CHECK_EQUAL(vertex.a, 0.0f);
}

/*!
 * \brief
 *    Убеждаемся, что конструктор устанавливает все значения компонентов в те,
 *    которые были заданы.
 */
BOOST_AUTO_TEST_CASE(VertexColorTestCase_ComponentConstructor) {
	const math::VertexColor vertex(math::vec3f_t(1.0f, 2.0f, 3.0f), math::col4f_t(0.1f, 0.2f, 0.3f));
	const math::vec3f_t position = vertex.getPosition();
	const math::col4f_t color = vertex.getColor();

	BOOST_CHECK_EQUAL(position.getX(), 1.0f);
	BOOST_CHECK_EQUAL(position.getY(), 2.0f);
	BOOST_CHECK_EQUAL(position.getZ(), 3.0f);

	BOOST_CHECK_EQUAL(color.getR(), 0.1f);
	BOOST_CHECK_EQUAL(color.getG(), 0.2f);
	BOOST_CHECK_EQUAL(color.getB(), 0.3f);
	BOOST_CHECK_EQUAL(color.getA(), 1.0f);
}

/*!
 * \brief
 *    Убеждаемся, что конструктор по умолчанию приводит все компоненты к нулю.
 */
BOOST_AUTO_TEST_CASE(VertexTexCoordTestCase_DefaultConstructor) {
	const math::VertexTexCoord vertex;

	BOOST_CHECK_EQUAL(vertex.x, 0.0f);
	BOOST_CHECK_EQUAL(vertex.y, 0.0f);
	BOOST_CHECK_EQUAL(vertex.z, 0.0f);

	BOOST_CHECK_EQUAL(vertex.u, 0.0f);
	BOOST_CHECK_EQUAL(vertex.v, 0.0f);
}

/*!
 * \brief
 *    Убеждаемся, что конструктор устанавливает все значения компонентов в те,
 *    которые были заданы.
 */
BOOST_AUTO_TEST_CASE(VertexTexCoordTestCase_ComponentConstructor) {
	const math::VertexTexCoord vertex(math::vec3f_t(1.0f, 2.0f, 3.0f), math::vec2f_t(0.1f, 0.2f));
	const math::vec3f_t position = vertex.getPosition();
	const math::vec2f_t texcoord = vertex.getTexCoord();

	BOOST_CHECK_EQUAL(position.getX(), 1.0f);
	BOOST_CHECK_EQUAL(position.getY(), 2.0f);
	BOOST_CHECK_EQUAL(position.getZ(), 3.0f);

	BOOST_CHECK_EQUAL(texcoord.getX(), 0.1f);
	BOOST_CHECK_EQUAL(texcoord.getY(), 0.2f);
}

BOOST_AUTO_TEST_SUITE_END()


// Copyright (c) 2019 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE ConstExprCastTest

#include <boost/test/unit_test.hpp>
#include <filereader.hpp>
#include <memory>
#include <vector>
#include <popart/builder.hpp>
#include <popart/dataflow.hpp>
#include <popart/half.hpp>
#include <popart/inputshapeinfo.hpp>
#include <popart/ir.hpp>
#include <popart/names.hpp>
#include <popart/op/identity.hpp>
#include <popart/op/l1.hpp>
#include <popart/sgd.hpp>
#include <popart/tensor.hpp>
#include <popart/tensordata.hpp>
#include <popart/tensors.hpp>
#include <popart/testdevice.hpp>

#include <math.h>

using namespace popart;

BOOST_AUTO_TEST_CASE(ConstExprTest_AddCastMatMul) {

  /*********

   The compute graph

-> data, float (7,5) ------------------------|
                                             |- MATMUL --- out:, float (7,3)
                                              \
                                               \
                                                \
-> i0, int32, (5,1)-|                            \
                    |                             \
                    |                              \
                    |- ADD --| CAST -- float (5,3) -|
-> i1, int32 (1,3)--|

  ***********/

  // Build the onnx model described
  // in the schematic above
  int64_t M = 7;
  int64_t K = 5;
  int64_t N = 3;

  std::vector<int64_t> outshape{M, N};
  std::vector<int64_t> weights_shape{K, N};
  std::vector<int64_t> data_shape{M, K};

  std::vector<int> i0(K);
  std::iota(i0.begin(), i0.end(), 1);
  std::vector<int> i1(N);
  std::iota(i1.begin(), i1.end(), 1);
  TensorInfo dataInfo{"FLOAT", std::vector<int64_t>{M, K}};
  ConstVoidData i0cv = {i0.data(), {"INT32", std::vector<int64_t>{K, 1}}};
  ConstVoidData i1cv = {i1.data(), {"INT32", std::vector<int64_t>{1, N}}};
  auto builder       = Builder::create();
  auto aiOnnx        = builder->aiOnnxOpset9();
  auto aiGraphcore   = builder->aiGraphcoreOpset1();
  // The two fixed-point tensors which are added together are Constants
  auto i0Id   = aiOnnx.constant(i0cv, "i0cv");
  auto i1Id   = aiOnnx.constant(i1cv, "i1cv");
  auto dataId = builder->addInputTensor(dataInfo);
  auto i01Id  = aiOnnx.add({i0Id, i1Id});
  auto castId = aiOnnx.cast({i01Id}, "FLOAT");
  auto outId  = aiOnnx.matmul({dataId, castId});
  auto lossId = aiGraphcore.l1loss({outId}, 0.1);

  auto proto      = builder->getModelProto();
  auto modelProto = io::getModelFromString(proto);

  // Create the IR, adding outId as an anchor
  auto art       = AnchorReturnType("All");
  auto dataFlow  = DataFlow(1, {{outId, art}});
  auto optimizer = ConstSGD(0.01);

  auto device = createTestDevice(TEST_TARGET);

  Ir ir;
  ir.prepare({modelProto,
              InputShapeInfo(),
              dataFlow,
              lossId,
              &optimizer,
              *device,
              {}, // no SessionOptions
              Patterns::create({"PostNRepl"}).enableRuntimeAsserts(false)});

  // Check the ir
  // 1) The Matmul Op is present,
  auto matmuls = ir.opsOfType(Onnx::AiOnnx::OpSet9::MatMul);
  BOOST_CHECK(matmuls.size() == 1);
  BOOST_CHECK(matmuls[0]->input->hasIndex(1));
  auto weights = matmuls[0]->input->tensor(1);

  // 2) The shape of the output is correct
  BOOST_CHECK(ir.getMainGraphTensors().get(outId)->info.shape() == outshape);

  // 3) The weights inputs to the matmul are correct,
  BOOST_CHECK(weights->info.shape() == weights_shape);
  BOOST_CHECK(weights->tensorType() == TensorType::Const);
  BOOST_CHECK(weights->hasTensorData() == true);
  auto tensorWData = weights->tensorData()->data();
  auto floatWData  = reinterpret_cast<float *>(tensorWData);
  // The weights were formed as the outer product of two
  // vectors, {1...K} and {1...N}. We therefore expect the very
  // last element of the weights tensor to be K + N
  BOOST_CHECK(floatWData[K * N - 1] == static_cast<float>(K + N));
}

template <typename T> std::string getTypeName();

template <> std::string getTypeName<int32_t>() { return "INT32"; }
template <> std::string getTypeName<uint32_t>() { return "UINT32"; }
template <> std::string getTypeName<float>() { return "FLOAT"; }
template <> std::string getTypeName<float16_t>() { return "FLOAT16"; }

template <typename FROM, typename TO> void ConstExprTest_AddCastMatMul_Type() {

  /*********

   The compute graph

-> data, float (7,5) ------------------------|
                                             |- MATMUL --- out:, float (7,3)
                                              \
                                               \
                                                \
-> i0, int32, (5,1)-|                            \
                    |                             \
                    |                              \
                    |- ADD --| CAST -- float (5,3) -|
-> i1, int32 (1,3)--|

  ***********/

  // Build the onnx model described
  // in the schematic above
  int64_t M = 7;
  int64_t K = 5;
  int64_t N = 3;

  std::vector<int64_t> outshape{M, N};
  std::vector<int64_t> weights_shape{K, N};
  std::vector<int64_t> data_shape{M, K};

  std::vector<FROM> i0(K);
  std::iota(i0.begin(), i0.end(), 1);
  std::vector<FROM> i1(N);
  std::iota(i1.begin(), i1.end(), 1);
  // TensorInfo dataInfo{"FLOAT", std::vector<int64_t>{M, K}};
  TensorInfo dataInfo{getTypeName<TO>(), std::vector<int64_t>{M, K}};
  ConstVoidData i0cv = {i0.data(),
                        {getTypeName<FROM>(), std::vector<int64_t>{K, 1}}};
  ConstVoidData i1cv = {i1.data(),
                        {getTypeName<FROM>(), std::vector<int64_t>{1, N}}};
  auto builder       = Builder::create();
  auto aiOnnx        = builder->aiOnnxOpset9();
  auto aiGraphcore   = builder->aiGraphcoreOpset1();
  // The two fixed-point tensors which are added together are Constants
  auto i0Id   = aiOnnx.constant(i0cv, "i0cv");
  auto i1Id   = aiOnnx.constant(i1cv, "i1cv");
  auto dataId = builder->addInputTensor(dataInfo);
  auto i01Id  = aiOnnx.add({i0Id, i1Id});
  auto castId = aiOnnx.cast(
      {i01Id}, dataInfo.getDataTypeInfo()->name()); // DataType::FLOAT);
  auto outId  = aiOnnx.matmul({dataId, castId});
  auto lossId = aiGraphcore.l1loss({outId}, 0.1);

  auto proto      = builder->getModelProto();
  auto modelProto = io::getModelFromString(proto);

  // Create the IR, adding outId as an anchor
  auto art       = AnchorReturnType("All");
  auto dataFlow  = DataFlow(1, {{outId, art}});
  auto optimizer = ConstSGD(0.01);
  auto device    = createTestDevice(TEST_TARGET);

  Ir ir;
  ir.prepare({modelProto,
              InputShapeInfo(),
              dataFlow,
              lossId,
              &optimizer,
              *device,
              {}, // no SessionOptions
              Patterns::create({"PostNRepl"}).enableRuntimeAsserts(false)});

  // Check the ir
  // 1) The Matmul Op is present,
  auto matmuls = ir.opsOfType(Onnx::AiOnnx::OpSet9::MatMul);
  BOOST_CHECK(matmuls.size() == 1);
  BOOST_CHECK(matmuls[0]->input->hasIndex(1));
  auto weights = matmuls[0]->input->tensor(1);

  // 2) The shape of the output is correct
  BOOST_CHECK(ir.getMainGraphTensors().get(outId)->info.shape() == outshape);

  // 3) The weights inputs to the matmul are correct,
  BOOST_CHECK(weights->info.shape() == weights_shape);
  BOOST_CHECK(weights->tensorType() == TensorType::Const);
  BOOST_CHECK(weights->hasTensorData() == true);
  auto tensorWData = weights->tensorData()->data();
  auto floatWData  = reinterpret_cast<TO *>(tensorWData);
  // The weights were formed as the outer product of two
  // vectors, {1...K} and {1...N}. We therefore expect the very
  // last element of the weights tensor to be K + N
  logging::info("{} ? {}", floatWData[K * N - 1], static_cast<TO>(K + N));
  BOOST_CHECK(floatWData[K * N - 1] == static_cast<TO>(K + N));
}

BOOST_AUTO_TEST_CASE(ConstExprTest_AddCastMatMul_Types) {
  ConstExprTest_AddCastMatMul_Type<int32_t, float_t>();
  ConstExprTest_AddCastMatMul_Type<int32_t, float16_t>();
  ConstExprTest_AddCastMatMul_Type<uint32_t, float_t>();
  ConstExprTest_AddCastMatMul_Type<uint32_t, float16_t>();
}


#include "media_integrator.hxx"
#include "sampler.hxx"
#include "scene.hxx"
#include "gtest/gtest.h"
#include <cstdio>
#include <thread>
#include <chrono>
#include <algorithm>

#ifdef HAVE_JSON
#include <rapidjson/document.h>
#endif

#include <boost/filesystem.hpp>

#include "renderingalgorithms_simplebase.hxx"
#include "rendering_randomwalk_impl.hxx"

using namespace RandomWalk;

void RenderingMediaTransmission1Helper(
  const Scene &scene,
  const Ray &ray,
  const Span<int> medium_changes,
  const Span<double> intersect_pos,
  int num_hits)
{
  std::unordered_map<int, const Medium*> media_table;

  MediumTracker mt(scene);
  mt.initializePosition(ray.org);

  auto CheckOrInsert = [&](int medium_id) -> bool
  {
    auto[it, inserted] = media_table.insert({ medium_id, &mt.getCurrentMedium() });
    return inserted || it->second == &mt.getCurrentMedium();
  };

  CheckOrInsert(medium_changes[0]);

  std::printf("Media Trace:\n");

  int i = 0;
  auto intersections = scene.IntersectionsWithVolumes(ray, 0., LargeNumber);
  ASSERT_EQ(intersections.size(), num_hits);

  for (const auto &intersection : intersections)
  {
    assert(intersection.geom >= 0 && intersection.prim >= 0);
    mt.goingThroughSurface(ray.dir, intersection);
    const Double3 pos = ray.PointAt(intersection.t);
    const bool medium_ok = CheckOrInsert(medium_changes[i + 1]);
    std::printf("IS[%i]: pos=%f, med_expect=%p, got=%p\n", i, pos[2], media_table[medium_changes[i+1]], &mt.getCurrentMedium());
    EXPECT_NEAR(pos[2], intersect_pos[i], 1.e-6);
    EXPECT_TRUE(medium_ok);
    ++i;
  }
}

void CheckVolumePdfCoefficientsForMedium(const VolumePdfCoefficients &coeff, double tr, double sigma_start, double sigma_end, double tol)
{
  EXPECT_NEAR(coeff.transmittance, tr, tol);
  EXPECT_NEAR(coeff.pdf_scatter_fwd, tr*sigma_end, tol);
  EXPECT_NEAR(coeff.pdf_scatter_bwd, tr*sigma_start, tol);
}

TEST(Rendering, MediaTransmission1)
{
  const std::string scenestr {R"""(
shader none
medium med1 1 1 1 2 2 2
medium med2 1 1 1 2 2 2

medium med1
s 0 0 -1 1

medium med2
s 0 0 2 1

medium med1
s 0 0 3 1

medium med1
s 0 0 6 1

medium med2
s 0 0 6 0.5
)"""};
  Scene scene;
  scene.ParseNFFString(scenestr);
  scene.BuildAccelStructure();

  static constexpr int NHITS = 10;
  auto intersect_pos = std::array<double, NHITS>{
    -2, 0, 1, 2, 3, 4, 5, 5.5, 6.5, 7
  };
  auto medium_changes = std::array<int, NHITS + 1>{
  0, 1, 0, 2, 2, 1, 0, 1, 2, 1, 0
  };
  /*
    -2 -- 0  // med 1
            1 -- 3 // med 2
               2 --- 4 // med 1
                            5 ---------- 7    // med 1
                              5.5 -- 6.5 // med 2
  */
  Ray ray{{0, 0, -10}, {0, 0, 1}};
  RenderingMediaTransmission1Helper(scene, ray, AsSpan(medium_changes), AsSpan(intersect_pos), NHITS);
}

TEST(Rendering, MediaTransmission2)
{
  const std::string scenestr {R"""(
shader none
medium med1 1 1 1 2 2 2
medium med2 1 1 1 2 2 2

medium med1
transform 0 0 -1 0 0 0 2 2 2
m testing/scenes/unitcube.dae

medium med2
transform 0 0 2 0 0 0 2 2 2
m testing/scenes/unitcube.dae

medium med1
transform 0 0 3 0 0 0 2 2 2
m testing/scenes/unitcube.dae
)"""};
  Scene scene;
  scene.ParseNFFString(scenestr);
  scene.BuildAccelStructure();

  static constexpr int NHITS = 5;
  auto intersect_pos = std::array<double, NHITS>{
     0,   1,  2,  3,   4
  };
  auto media_changes = std::array<int, NHITS+1> {
  1, 0,   2,  2,  1,   0
  };
  /*
   -2 .. 0  // med1
            1  .. 3  // med2
               2 .. 4 // med1
  */
  const Ray ray{{0, 0, -1.}, {0, 0, 1}};
  RenderingMediaTransmission1Helper(scene, ray, AsSpan(media_changes), AsSpan(intersect_pos), NHITS);
}

namespace Eigen
{

template<class Derived>
auto ToVector(const ArrayBase<Derived> &v)
{
  using Scalar = typename internal::traits<Derived>::Scalar;
  if (v.rows() > 1 && v.cols() > 1)
    throw std::runtime_error("Need 1D array!");
  const auto temp = v.eval();
  return ToyVector<Scalar>(temp.data(), temp.data() + temp.size());
}

};

namespace test_medium
{

struct ControlPoint
{
  double x;
  Eigen::ArrayXd mu_s;
  Eigen::ArrayXd mu_a;
};

struct Result
{
  Eigen::ArrayXd transmittance;
  Eigen::ArrayXd mu_s, mu_a;
};

/*
  ControlPoints define a piecewise constant medium.
  Its values are valid from x to the next control point.
*/
Result ComputeTransmittance(double x, const ToyVector<ControlPoint> &medium)
{
  const auto dim = medium.empty() ? 0 : medium.front().mu_s.size();

  Result res{};
  res.transmittance.resize(dim);
  res.transmittance.setOnes();
  res.mu_s.resize(dim);
  res.mu_s.setZero();
  res.mu_a.resize(dim);
  res.mu_a.setZero();

  if (medium.empty() || x < medium.front().x)
    return res;

  for (std::size_t i = 0; i<medium.size(); ++i)
  {
    double next_x = (i + 1 == medium.size()) ? std::numeric_limits<double>::infinity() : medium[i + 1].x;
    const auto &c = medium[i];
    const bool is_x_in_segment = x < next_x;
    if (is_x_in_segment)
    {
      next_x = x;
      res.mu_a = c.mu_a;
      res.mu_s = c.mu_s;
    }
    res.transmittance *= ((c.mu_a + c.mu_s)*(c.x - next_x)).exp();
    if (is_x_in_segment)
      break;
  }
  return res;
}

}

TEST(Rendering, TransmittanceEstimate)
{
  using namespace test_medium;
  Scene scene;
  const char* scenestr = R"""(
shader none
medium med1 1 1 1 2 2 2

m testing/scenes/unitcube.dae

transform 0 0 2 0 0 0
m testing/scenes/unitcube.dae
)""";
  LambdaSelection wavelengths = SelectRgbPrimaryWavelengths();
  const auto sigma_s = Color::RGBToSpectralSelection(RGB{ 1._rgb }, wavelengths.indices);
  const auto sigma_a = Color::RGBToSpectralSelection(RGB{ 2._rgb }, wavelengths.indices);
  const auto zero = decltype(sigma_s)::Zero();
  const auto exact = ComputeTransmittance(LargeNumber, {
    { -0.5, sigma_s, sigma_a},
    {  0.5, zero,   zero},
    {  1.5, sigma_s, sigma_a},
    {  2.5, zero ,   zero},
  });

  scene.ParseNFFString(scenestr);
  scene.BuildAccelStructure();

  RaySegment seg{{{0.,0.,-10.}, {0.,0.,1.}}, LargeNumber};
  MediumTracker medium_tracker(scene);
  medium_tracker.initializePosition(seg.ray.org);
  Sampler sampler;
  VolumePdfCoefficients volume_pdf_coeff{};
  auto res = ::TransmittanceEstimate(scene, seg, medium_tracker, PathContext{wavelengths}, sampler, &volume_pdf_coeff);

  for (int i=0; i<static_size<Spectral3>(); ++i)
    ASSERT_NEAR(res[i], exact.transmittance[i], 1.e-3);
  CheckVolumePdfCoefficientsForMedium(
    volume_pdf_coeff, exact.transmittance.mean(), 0, 0, 1.e-6);
}

TEST(Rendering, NextInteractionEstimation)
{
  using namespace test_medium;
  Scene scene;
  const char* scenestr = R"""(
shader none
medium med1 3 3 3 0 0 0

m testing/scenes/unitcube.dae

transform 0 0 2 0 0 0
m testing/scenes/unitcube.dae
)""";
  LambdaSelection wavelengths = SelectRgbPrimaryWavelengths();
  Spectral3 sigma = Color::RGBToSpectralSelection(RGB{3._rgb,3._rgb,3._rgb}, wavelengths.indices);
  const auto zero = decltype(sigma)::Zero();
  const auto mediumTestDescr = ToyVector<ControlPoint>{
    { -0.5, sigma, zero},
    {  0.5, zero,   zero},
    {  1.5, sigma, zero},
    {  2.5, zero ,   zero}
  };

  scene.ParseNFFString(scenestr);
  scene.BuildAccelStructure();

  Sampler sampler;
  Ray ray{{0.,0.,-10}, {0.,0.,1.}};
  MediumTracker medium_tracker(scene);
  medium_tracker.initializePosition(ray.org);
  ASSERT_EQ(&medium_tracker.getCurrentMedium(), &scene.GetEmptySpaceMedium());

  int num_escaped = 0;
  Spectral3 weight_sum_interacted{0.};
  Spectral3 weight_sum_escaped{0.};

  constexpr int NUM_SAMPLES = 1000;
  for (int sample_num = 0; sample_num < NUM_SAMPLES; ++sample_num)
  {
    VolumePdfCoefficients volume_pdf_coeff{};
    //RadianceEstimatorBase::CollisionData collision(ray);
    medium_tracker.initializePosition(ray.org);
    TrackToNextInteraction(scene, ray, PathContext{wavelengths}, Spectral3::Ones(), sampler, medium_tracker, &volume_pdf_coeff,
      /*surface_visitor=*/[&](const SurfaceInteraction &intersection, double distance, const Spectral3 &weight)
      {
        FAIL();
      },
      /*volume visitor=*/[&](const VolumeInteraction &interaction, double distance, const Spectral3 &weight)
      {
        double x = distance + ray.org[2];
        const auto exact = ComputeTransmittance(x, mediumTestDescr);

        CheckVolumePdfCoefficientsForMedium(volume_pdf_coeff, exact.transmittance.mean(), 0., (exact.mu_a + exact.mu_s).mean(), 1.e-5);
        weight_sum_interacted += weight*interaction.sigma_s;
      },
      /* escape_visitor=*/[&](const Spectral3 &weight)
      {
        weight_sum_escaped += weight;
        num_escaped++;
      }
    );
  }
  weight_sum_escaped *= 1./(NUM_SAMPLES);
  weight_sum_interacted *= 1./(NUM_SAMPLES);

  auto sigma_e = Color::RGBToSpectralSelection(RGB{3._rgb}, wavelengths.indices);
  Spectral3 expected_transmissions = (-2.*sigma_e).exp();
  // No idea what the variance is. So I determine the acceptance threshold experimentally.
  for (int i=0; i<static_size<Spectral3>(); ++i)
  {
    ASSERT_NEAR(weight_sum_escaped[i], expected_transmissions[i], 1.e-2);
    ASSERT_NEAR(weight_sum_interacted[i], 1.-expected_transmissions[i], 1.e-2);
  }
}

TEST(Rendering, TrackBeam)
{
  using namespace test_medium;
  Scene scene;
  const char* scenestr = R"""(
shader none
medium med1 3 0

m testing/scenes/unitcube.dae

transform 0 0 2 0 0 0
m testing/scenes/unitcube.dae
)""";

  LambdaSelection wavelengths = SelectRgbPrimaryWavelengths();
  Spectral3 sigma = Color::RGBToSpectralSelection(RGB{3._rgb,3._rgb,3._rgb}, wavelengths.indices);
  const auto zero = decltype(sigma)::Zero();
  const auto mediumTestDescr = ToyVector<ControlPoint>{
    { -0.5, sigma, zero},
    {  0.5, zero,   zero},
    {  1.5, sigma, zero},
    {  2.5, zero ,   zero}
  };

  constexpr int NUM_PROBES = 5;
  double probe_locations[NUM_PROBES] = {
    -1., 0., 1., 2., 5.
  };

  scene.ParseNFFString(scenestr);
  scene.BuildAccelStructure();

  Sampler sampler;
  Ray ray{ {0.,0.,-10}, {0.,0.,1.} };
  MediumTracker medium_tracker(scene);

  int num_escaped = 0;
  Spectral3 weight_sum_probed[NUM_PROBES];
  for (int i=0; i<NUM_PROBES; ++i)
    weight_sum_probed[i] = Spectral3::Zero();
  Spectral3 weight_sum_escaped{0.};

  constexpr int NUM_SAMPLES = 10000;
  for (int sample_num = 0; sample_num < NUM_SAMPLES; ++sample_num)
  {
    medium_tracker.initializePosition(ray.org);
    TrackBeam(scene, ray, PathContext{wavelengths}, sampler, medium_tracker,
      /*surface_visitor=*/[&](const SurfaceInteraction &intersection, const Spectral3 &weight)
      {
        FAIL();
      },
      /*segment visitor=*/[&](const RaySegment &segment, const Medium &medium, const PiecewiseConstantTransmittance &pct, const Spectral3 &weight)
      {
        for (int i = 0; i<NUM_PROBES; ++i)
        {
          // Convert probe location to distance along local segment.
          double distance = probe_locations[i]-segment.ray.org[2];
          if (distance >= 0 && distance < segment.length)
            weight_sum_probed[i] += weight*pct(distance);
        }
      },
      /* escape_visitor=*/[&](const Spectral3 &weight)
      {
        weight_sum_escaped += weight;
        num_escaped++;
      }
    );
  }

  for (int probe_idx=0; probe_idx<NUM_PROBES; ++probe_idx)
  {
    weight_sum_probed[probe_idx] *= 1./(NUM_SAMPLES);
    const auto exact = ComputeTransmittance(probe_locations[probe_idx], mediumTestDescr);
    for (int i=0; i<static_size<Spectral3>(); ++i)
    {
      ASSERT_NEAR(weight_sum_probed[probe_idx][i], exact.transmittance[i], 1.e-2);
    }
  }

  weight_sum_escaped *= 1./(NUM_SAMPLES);
  auto sigma_e = Color::RGBToSpectralSelection(RGB{3._rgb}, wavelengths.indices);
  Spectral3 expected_transmissions = (-2.*sigma_e).exp();
  // No idea what the variance is. So I determine the acceptance threshold experimentally.
  for (int i=0; i<static_size<Spectral3>(); ++i)
  {
    ASSERT_NEAR(weight_sum_escaped[i], expected_transmissions[i], 1.e-2);
  }
}

TEST(Rendering,PiecewiseConstantTransmittance)
{
  PiecewiseConstantTransmittance pct;
  pct.PushBack(0., Spectral3{2.});
  ASSERT_NEAR(pct(-0.1)[0], 2., 1.e-3);
  ASSERT_NEAR(pct(0.1)[0], 0., 1.e-3);

  pct.PushBack(1., Spectral3{1.});
  ASSERT_NEAR(pct(-0.1)[0], 2., 1.e-3);
  ASSERT_NEAR(pct(0.1)[0], 1., 1.e-3);
  ASSERT_NEAR(pct(0.9)[0], 1., 1.e-3);
  ASSERT_NEAR(pct(1.1)[0], 0., 1.e-3);

  pct.PushBack(2., Spectral3{3.});
  ASSERT_NEAR(pct(-0.1)[0], 2., 1.e-3);
  ASSERT_NEAR(pct(0.1)[0], 1., 1.e-3);
  ASSERT_NEAR(pct(0.9)[0], 1., 1.e-3);
  ASSERT_NEAR(pct(1.1)[0], 3., 1.e-3);
  ASSERT_NEAR(pct(1.9)[0], 3., 1.e-3);
  ASSERT_NEAR(pct(2.1)[0], 0., 1.e-3);

  pct.PushBack(3., Spectral3{4.});
  ASSERT_NEAR(pct(-0.1)[0], 2., 1.e-3);
  ASSERT_NEAR(pct(0.1)[0], 1., 1.e-3);
  ASSERT_NEAR(pct(0.9)[0], 1., 1.e-3);
  ASSERT_NEAR(pct(1.1)[0], 3., 1.e-3);
  ASSERT_NEAR(pct(1.9)[0], 3., 1.e-3);
  ASSERT_NEAR(pct(2.1)[0], 4., 1.e-3);
  ASSERT_NEAR(pct(2.9)[0], 4., 1.e-3);
  ASSERT_NEAR(pct(3.1)[0], 0., 1.e-3);

  pct.PushBack(InfinityFloat, Spectral3{5.});
  ASSERT_NEAR(pct(InfinityFloat)[0], 5., 1.e-3);
  ASSERT_NEAR(pct(Infinity)[0], 5., 1.e-3);
}

#if 0
TEST(NullPath, Mis)
{
  /* Transmission estimation using
     a) Delta Tracking
     b) Ratio Tracking
     MIS combination of a and b.
  */
  const double density = 0.6931471805599453; // Half of particles make it through.
  const double expected_transmissions = std::exp(-density);
  // NOTE: I think there is some bias in the result because the
  // scatter coefficients given here are not converted by their exact value
  // to the spectral representation!
  const auto scenestr = fmt::format(R"""(
shader none
medium med1 {} 0
m testing/scenes/unitcube.dae

medium default
shader black
transform 0 0 2.5 0 0 0
m testing/scenes/unitcube.dae
)""", density);

  Scene scene;
  scene.ParseNFFString(scenestr);
  scene.BuildAccelStructure();

  Sampler sampler;
  RaySegment segment{{ {0.,0.,-2}, {0.,0.,1.} }, 4};
  MediumTracker medium_tracker(scene);
  LambdaSelection wavelengths{};
  wavelengths.indices = Index3{ 0,0,0 };
  PathContext context{wavelengths};

  Accumulators::OnlineVariance<double> n_track_through;
  Accumulators::OnlineVariance<double> transmission_estimate;
  Accumulators::OnlineVariance<double> combined_estimate;
  Accumulators::OnlineVariance<double> mis_weights_track;
  Accumulators::OnlineVariance<double> mis_weights_transmit;
  const int N = 1000;
  for (int iter=0; iter<N; ++iter)
  {
    double combined_estimator = 0;
    {
      medium_tracker.initializePosition(segment.ray.org);
      auto [interaction, dist, factors] = nullpath::Tracking(scene, segment.ray, sampler, medium_tracker, context);
      assert (interaction);
      double estimator = (factors.throughput[0] / factors.pdf_track[0]) *
          ((interaction && mpark::get_if<SurfaceInteraction>(&(*interaction))) ? 1. : 0.);
      n_track_through += estimator;
      double mis_weight = factors.pdf_track[0] / (factors.pdf_track[0] + factors.pdf_nulls[0]);
      combined_estimator += mis_weight * estimator;
      mis_weights_track += mis_weight;
    }
    {
      medium_tracker.initializePosition(segment.ray.org);
      auto factors = nullpath::Transmission(scene, segment, sampler, medium_tracker, context);
      double estimator = factors.throughput[0] / factors.pdf_nulls[0];
      transmission_estimate += estimator;
      double mis_weight = factors.pdf_nulls[0] / (factors.pdf_track[0] + factors.pdf_nulls[0]);
      combined_estimator += mis_weight * estimator;
      mis_weights_transmit += mis_weight;
    }
    combined_estimate += combined_estimator;
  }
  fmt::print("transmission avg = {} +/- {}\n", transmission_estimate.Mean(),transmission_estimate.MeanErr());
  fmt::print("tracking avg = {} +/- {}\n", n_track_through.Mean(),n_track_through.MeanErr());
  fmt::print("combined estimate = {} +/- {}\n", combined_estimate.Mean(), combined_estimate.MeanErr());
  fmt::print("mis_weights_track = {}\n", mis_weights_track.Mean());
  fmt::print("mis_weights_transmit = {}\n", mis_weights_transmit.Mean());
  fmt::print("expected = {}\n", expected_transmissions);
  EXPECT_NEAR(transmission_estimate.Mean(), expected_transmissions, 2.*transmission_estimate.MeanErr());
  EXPECT_NEAR(n_track_through.Mean(), expected_transmissions, 2.*n_track_through.MeanErr());
  EXPECT_NEAR(combined_estimate.Mean(), expected_transmissions, 2.*combined_estimate.MeanErr());
  EXPECT_LE(combined_estimate.MeanErr(), transmission_estimate.MeanErr());
  EXPECT_LE(combined_estimate.MeanErr(), n_track_through.MeanErr());
}
#endif


#pragma once

#include <boost/optional.hpp>

#include "evaluator.hpp"
#include "general.hpp"

#if 0
// This cannot be parallelized, since we are doing a fold (essentially)
template <template <typename> class Trig>
void cover_polygon(Inserter& insert, Evaluator& eval,
                   const OpenConvexPolygonQ& polygon, const ClosedRectangleQ& rect,
                   bool subset, const uint32_t mags_left) {

    // The bigger rect was not a subset, so check if the smaller rect is a subset
    if (!subset) {
        subset = geometry::subset(rect, polygon);

        if (!subset && !geometry::intersects(rect, polygon)) {
            // We don't need to worry about coloring this rectangle, so ignore it
            return;
        }
        // Else if it is a subset or it intersects, then we can continue
    }

    const auto c = center(rect);
    const auto rx = radius_x(rect);
    const auto ry = radius_y(rect);

    for (const auto& kv : insert.sines) {
        const auto& eq = kv.first;
        const auto& info = kv.second;
        if (info.still_in_the_running) {

            const auto pos = eval.is_positive(eq, info.bx, info.by, c, rx, ry);

            if (pos && mags_left == 0) {
                // keep going
            }

            if (pos && mags_left != 0) {
                // don't check subdivisions
            }

            if (!pos and mags_left == 0) {
                // failed for good
            }

            if (!pos and mags_left != 0) {
                // try subdividing
            }

        }
    }

    if (mags_left != 0) {
        // Else we have magnifications left, so try subdividing

        const auto quarters = subdivide(rect);

        for (const auto& quarter : quarters) {
            if (!cover_polygon(polygon, quarter, eq, bx, by, subset, mags_left - 1, eval)) {
                // if we were unable to cover a rect, short-circuit false
                return false;
            }
        }

        // Each quarter was covered, so the entire square was covered too
        return true;
    } else {
        // There are no magnifications left, so everything that has currently failed has
        // permanently failed
    }

    // no magnifications left, so false
    return false;
}

template <template <typename> class Trig>
bool cover_segment(const ClosedSegmentQ& seg,
                   const EqMap<Trig>& eq, const Coeff64 bx, const Coeff64 by,
                   const uint32_t mags_left, Evaluator& eval) {

    const auto c = center(seg);
    const auto rx = radius_x(seg);
    const auto ry = radius_y(seg);

    if (eval.is_positive(eq, bx, by, c, rx, ry)) {
        // The equation is positive over this segment
        return true;
    }

    if (mags_left != 0) {
        // Else we have magnifications left, so try subdividing

        const auto sub_segs = subdivide(seg);

        for (const auto& sub_seg : sub_segs) {
            if (!cover_segment(sub_seg, eq, bx, by, mags_left - 1, eval)) {
                // If we were unable to cover a segment, short-circuit false
                return false;
            }
        }

        // Both sub-segments were covered, so the entire segment was covered too
        return true;
    }

    // no magnifications left, so false
    return false;
}

// If it fails on the current square, you check the smaller square to see if that failed.
// If an equation fails on a square where we cannot subdivide, then it fails for good,
// and it is no longer in the running.
// If it is still in the running and positive on the current square, we do not need to check any subdivisions
// If it is positive on the current square, then we do not need to check any subdivisions
// If it is not positive on the current square, then

struct ChopInfo {
    Coeff64 bx;
    Coeff64 by;
    bool still_in_the_running;
    bool check_subsquare;

    explicit ChopInfo(const Coeff64 bx_, const Coeff64 by_, const bool pos_)
        : bx{bx_}, by{by_}, pos{pos_} {}
};

struct Inserter {
    // TODO change to EqVec
    // There may be duplicates when doing insertion, so we must have a map
    std::map<EqMap<Sin>, ChopInfo> sines;
    std::map<EqMap<Cos>, ChopInfo> cosines;

    explicit Inserter() : sines{}, cosines{} {}

    void insert(const EqMap<Sin>& eq) {

        const auto b = gradient_bounds(eq);

        sines.emplace(eq, ChopInfo{b.first, b.second, true});
    }

    void insert(const EqMap<Cos>& eq) {

        const auto b = gradient_bounds(eq);

        cosines.emplace(eq, ChopInfo{b.first, b.second, true});
    }
};
#endif

struct Inserter {
    Curves curves;
    Evaluator eval;
    const PointQ& center;
    const Rational& rx;
    const Rational& ry;

    explicit Inserter(const PointQ& center_, const Rational& rx_, const Rational& ry_)
        : curves{}, eval{64}, center{center_}, rx{rx_}, ry{ry_} {}

    void insert(const EqMap<Sin>& eq) {

        const auto b = gradient_bounds(eq);

        if (!eval.is_positive(eq, b.first, b.second, center, rx, ry)) {
            curves.first.insert(eq);
        }
    }

    void insert(const EqMap<Cos>& eq) {

        const auto b = gradient_bounds(eq);

        if (!eval.is_positive(eq, b.first, b.second, center, rx, ry)) {
            curves.second.insert(eq);
        }
    }
};

void divide_out_lines(const LinComMapZ<Sin<LinComArrZ<XY>>>& lin_com, const XYZ first, const XYZ second, Inserter& insert);

void divide_out_lines(const LinComMapZ<Cos<LinComArrZ<XY>>>& lin_com, const XYZ first, const XYZ second, Inserter& insert);

void divide_out_lines(const LinComMapZ<Sin<LinComArrZ<XY>>>& lin_com, Curves& curves, const XYZ first, const XYZ second);

void divide_out_lines(const LinComMapZ<Cos<LinComArrZ<XY>>>& lin_com, Curves& curves, const XYZ first, const XYZ second);

boost::optional<LinComMapZ<Cos<LinComArrZ<XY>>>>
divide_once(const LinComMapZ<Sin<LinComArrZ<XY>>>& eq, const LinComMapZ<Sin<LinComArrZ<XY>>>& factor);

boost::optional<LinComMapZ<Sin<LinComArrZ<XY>>>>
divide_once(const LinComMapZ<Sin<LinComArrZ<XY>>>& eq, const LinComMapZ<Cos<LinComArrZ<XY>>>& factor);

boost::optional<LinComMapZ<Sin<LinComArrZ<XY>>>>
divide_once(const LinComMapZ<Cos<LinComArrZ<XY>>>& eq, const LinComMapZ<Sin<LinComArrZ<XY>>>& factor);

boost::optional<LinComMapZ<Cos<LinComArrZ<XY>>>>
divide_once(const LinComMapZ<Cos<LinComArrZ<XY>>>& eq, const LinComMapZ<Cos<LinComArrZ<XY>>>& factor);


/*
 * Copyright (c) 2008, Willow Garage, Inc.
 * Copyright (c) 2015-2016, Jiri Horner
 * 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 Jiri Horner 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 OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 */

/**
 * \file
 *
 * Implementation for combine_grids.h
 *
 * \author Bhaskara Marthi
 * \author Jiri Horner
 */

#include <occupancy_grid_utils/combine_grids.h>
#include <boost/bind.hpp>
#include <boost/ref.hpp>
#include <boost/make_shared.hpp> /* used only in deprecated functions */

namespace occupancy_grid_utils
{

namespace nm=nav_msgs;
namespace gm=geometry_msgs;

using boost::bind;
using boost::ref;
using std::vector;
using std::multiset;
using std::set;
using std::min;
using std::max;

inline Cell point32Cell (const nm::MapMetaData& info, const gm::Point32& p)
{
  gm::Point pt;
  pt.x = p.x;
  pt.y = p.y;
  pt.z = p.z;
  return pointCell(info, pt);
}

// Does this cell contain a vertex of this polygon?
inline bool containsVertex (const nm::MapMetaData& info, const Cell& c, const gm::Polygon& poly)
{
  BOOST_FOREACH (const gm::Point32& p, poly.points) {
    if (point32Cell(info, p)==c)
      return true;
  }
  return false;
}

// Do the two cells (on different grids) intersect?
inline bool cellsIntersect (const nm::MapMetaData& info1, const Cell& c1, const nm::MapMetaData& info2, const Cell& c2)
{
  const gm::Polygon p1=cellPolygon(info1, c1);
  const gm::Polygon p2=cellPolygon(info2, c2);
  return containsVertex(info1, c1, p2) || containsVertex(info2, c2, p1);
}

inline gm::Polygon expandPolygon(const gm::Polygon& p, const double r)
{
  double sx=0;
  double sy=0;
  double sz=0;
  const size_t n = p.points.size();
  for (unsigned i=0; i<n; i++) {
    sx += p.points[i].x;
    sy += p.points[i].y;
    sz += p.points[i].z;
  }
  sx /= n;
  sy /= n;
  sz /= n;
  gm::Polygon p2;
  p2.points.resize(n);
  for (unsigned i=0; i<n; i++) {
    p2.points[i].x = sx + r*(p.points[i].x-sx);
    p2.points[i].y = sy + r*(p.points[i].y-sy);
    p2.points[i].z = sz + r*(p.points[i].z-sz);
  }
  return p2;
}

// Return set of intersecting cells in grid info, of cell cell2 in info2
set<Cell> intersectingCells (const nm::MapMetaData& info, const nm::MapMetaData& info2, const Cell& cell2)
{
  // The expansion is to avoid weird effects due to rounding when intersecting parallel grids
  const gm::Polygon poly=expandPolygon(cellPolygon(info2, cell2), 1.0001);

  // Figure out the candidates
  vector<Cell> corners(4);
  transform(poly.points.begin(), poly.points.end(), corners.begin(),
            bind(point32Cell, boost::ref(info), _1));
  const coord_t min_x=min(corners[0].x, min(corners[1].x, min(corners[2].x, corners[3].x)));
  const coord_t min_y=min(corners[0].y, min(corners[1].y, min(corners[2].y, corners[3].y)));
  const coord_t max_x=max(corners[0].x, max(corners[1].x, max(corners[2].x, corners[3].x)));
  const coord_t max_y=max(corners[0].y, max(corners[1].y, max(corners[2].y, corners[3].y)));

  set<Cell> cells;
  for (coord_t x=min_x; x<=max_x; x++) {
    for (coord_t y=min_y; y<=max_y; y++) {
      const Cell candidate(x, y);
      if (withinBounds(info, candidate) &&
          cellsIntersect(info, candidate, info2, cell2))
        cells.insert(candidate);
    }
  }

  return cells;
}

gm::Pose transformPose (const tf::Pose trans, const gm::Pose p)
{
  tf::Pose pose;
  tf::poseMsgToTF(p, pose);
  gm::Pose transformed;
  tf::poseTFToMsg(trans*pose, transformed);
  return transformed;
}

/* deprecated functions */

nav_msgs::OccupancyGrid::Ptr combineGrids(const std::vector<nav_msgs::OccupancyGrid::ConstPtr>& grids, double resolution) {
  vector<boost::reference_wrapper<nav_msgs::OccupancyGrid const> > grids_refs;
  grids_refs.reserve(grids.size());
  nav_msgs::OccupancyGrid::Ptr result = boost::make_shared<nm::OccupancyGrid>();

  BOOST_FOREACH (const nav_msgs::OccupancyGrid::ConstPtr& g, grids) {
    grids_refs.push_back(boost::cref(*(g.get())));
  }

  combineGrids(grids_refs.begin(), grids_refs.end(), resolution, *result);

  return result;
}

nav_msgs::OccupancyGrid::Ptr combineGrids(const std::vector<nav_msgs::OccupancyGrid::ConstPtr>& grids) {
  ROS_ASSERT (!grids.empty());
  return combineGrids(grids, grids[0]->info.resolution);
}

} // namespace occupancy_grid_utils