//==============================================================================
//         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 BOOST_SIMD_TOOLBOX_IEEE_FUNCTIONS_SCALAR_EXPONENTBITS_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_IEEE_FUNCTIONS_SCALAR_EXPONENTBITS_HPP_INCLUDED

#include <boost/simd/toolbox/ieee/functions/exponentbits.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/dispatch/meta/adapted_traits.hpp>
#include <boost/simd/include/functions/scalar/bitwise_and.hpp>
#include <boost/simd/include/constants/maxexponent.hpp>
#include <boost/simd/include/constants/nbmantissabits.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::exponentbits_, tag::cpu_
                            , (A0)
                            , (scalar_< fundamental_<A0> >)
                            )
  {
    typedef typename dispatch::meta::as_integer<A0, signed>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return b_and((2*Maxexponent<A0>()+1)<<Nbmantissabits<A0>(), a0);
    }
  };
} } }

#endif


//==================================================================================================
/*!
  @file

  @copyright 2016 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)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_IS_LESS_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_IS_LESS_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-predicates
    Function object implementing is_less capabilities

    Returns  @ref True or @ref False according x is less than y or not.
    Infix notation can be used with operator '<'.

    @par Semantic:

    @code
    auto r = is_less(x,y);
    @endcode

    or

    @code
    auto r = x < y;
    @endcode

    @see  is_less_equal, is_ltz

  **/
  as_logical_t<Value> is_less(Value const& x, Value const& y);
} }
#endif

#include <boost/simd/function/scalar/is_less.hpp>
#include <boost/simd/function/simd/is_less.hpp>

#endif


//---------------------------------------------------------------------------//
//!
//! \file   Utility_BoostSolver.hpp
//! \author Alex Bennett
//! \brief  Boost Solver
//!
//---------------------------------------------------------------------------//

#ifndef UTILITY_BOOST_SOLVER_HPP
#define UTILITY_BOOST_SOLVER_HPP

// Trilinos Includes
#include <Teuchos_Array.hpp>
#include <Teuchos_SerialDenseMatrix.hpp>
#include <Teuchos_RCP.hpp>

// Boost Includes
#include <boost/numeric/ublas/vector.hpp>

namespace Utility {

class BoostSolver
{
friend class BatemanSystem;
friend class BatemanJacobian;

public:
  //Constructor
  BoostSolver(const Teuchos::RCP<Teuchos::SerialDenseMatrix<int,double> >& matrix,
              const Teuchos::Array<double>& y);

  void getNumberDensities(Teuchos::Array<double>& y);

  void Solve(const double& time);

private:
  class BatemanSystem
  {
  public:
    BatemanSystem(const Teuchos::RCP<Teuchos::SerialDenseMatrix<int,double> >& matrix) : m_matrix(matrix) {};
    void operator()(const boost::numeric::ublas::vector<double>& y,
                          boost::numeric::ublas::vector<double>& dxdt,
                          double time);

  private:
    Teuchos::RCP<Teuchos::SerialDenseMatrix<int,double> > m_matrix;
  };

  class BatemanJacobian
  {
  public:
    BatemanJacobian(const Teuchos::RCP<Teuchos::SerialDenseMatrix<int,double> >& matrix): m_matrix(matrix) {};
    void operator()(const boost::numeric::ublas::vector<double>& y,
                          boost::numeric::ublas::matrix<double>& jacobian,
                    const double& time,
                          boost::numeric::ublas::vector<double>& dfdt);

  private:
    Teuchos::RCP<Teuchos::SerialDenseMatrix<int,double> > m_matrix;
  };

  const BatemanSystem& m_system;
  const BatemanJacobian& m_jacobian;

  boost::numeric::ublas::vector<double> m_y;
};

} // end namespace utility

#endif // end UTILITY_BOOST_SOLVER_HPP

//---------------------------------------------------------------------------//
// end Utility_BoostSolver.hpp
//---------------------------------------------------------------------------//


#include <scitbx/array_family/boost_python/flex_fwd.h>

#include <scitbx/array_family/boost_python/flex_wrapper.h>
#include <scitbx/serialization/single_buffered.h>
#include <scitbx/matrix/transpose_multiply.h>
#include <scitbx/math/utils.h>
#include <boost/python/make_constructor.hpp>
#include <boost/python/args.hpp>
#include <boost/python/return_arg.hpp>
#include <boost/format.hpp>
#include "flex_helpers.h"

namespace scitbx { namespace serialization { namespace single_buffered {

  inline
  char* to_string(char* start, vec3<double> const& value)
  {
    return
      to_string(to_string(to_string(start, value[0]), value[1]), value[2]);
  }

  template <>
  struct from_string<vec3<double> >
  {
    from_string(const char* start)
    {
      end = start;
      for(std::size_t i=0;i<3;i++) {
        from_string<double> proxy(end);
        value[i] = proxy.value;
        end = proxy.end;
      }
    }

    vec3<double> value;
    const char* end;
  };

}}} // namespace scitbx::serialization::single_buffered

#include <scitbx/array_family/boost_python/flex_pickle_single_buffered.h>

namespace scitbx { namespace af {
namespace {

  flex<vec3<double> >::type*
  join(
    af::const_ref<double> const& x,
    af::const_ref<double> const& y,
    af::const_ref<double> const& z)
  {
    SCITBX_ASSERT(y.size() == x.size());
    SCITBX_ASSERT(z.size() == x.size());
    af::shared<vec3<double> > result((af::reserve(x.size())));
    for(std::size_t i=0;i<x.size();i++) {
      result.push_back(vec3<double>(x[i],y[i],z[i]));
    }
    return new flex<vec3<double> >::type(result, result.size());
  }

  flex<vec3<double> >::type*
  from_double(
    af::const_ref<double> const& x)
  {
    SCITBX_ASSERT(x.size() % 3 == 0);
    std::size_t result_size = x.size() / 3;
    af::shared<vec3<double> > result((af::reserve(result_size)));
    const double* d = x.begin();
    for(std::size_t i=0;i<result_size;i++) {
      result.push_back(vec3<double>(d));
      d += 3;
    }
    return new flex<vec3<double> >::type(result, result.size());
  }

  boost::python::tuple
  part_names()
  {
    return boost::python::make_tuple("x", "y", "z");
  }

  boost::python::tuple
  parts(
    versa<vec3<double>, flex_grid<> > const& O)
  {
    tiny<versa<double, flex_grid<> >, 3> result;
    std::size_t n = O.size();
    for(std::size_t i=0;i<3;i++) {
      result[i].resize(O.accessor());
      for(std::size_t j=0;j<n;j++) {
        result[i][j] = O[j][i];
      }
    }
    return boost::python::make_tuple(result[0], result[1], result[2]);
  }

  af::shared<vec3<double> >
  rotate_around_origin(
    flex<vec3<double> >::type const& a,
    vec3<double> const& direction,
    double const& angle)
  {
    SCITBX_ASSERT(direction.length() > 0)(direction.length());
    vec3<double> unit = direction.normalize();
    af::shared<vec3<double> > result((af::reserve(a.size())));
    for(std::size_t i=0;i<a.size();i++) {
      result.push_back(a[i].unit_rotate_around_origin(
        unit, angle));
    }
    return result;
  }

  af::shared<vec3<double> >
  rotate_around_origin(
    flex<vec3<double> >::type const& a,
    vec3<double> const& direction,
    flex<double>::type const& angles)
  {
    SCITBX_ASSERT(direction.length() > 0)(direction.length());
    vec3<double> unit = direction.normalize();
    af::shared<vec3<double> > result((af::reserve(a.size())));
    for(std::size_t i=0;i<a.size();i++) {
      result.push_back(a[i].unit_rotate_around_origin(
        unit, angles[i]));
    }
    return result;
  }

  af::shared<vec3<double> >
  rotate_around_origin(
    flex<vec3<double> >::type const& a,
    flex<vec3<double> >::type const& directions,
    flex<double>::type const& angles)
  {
    af::shared<vec3<double> > result((af::reserve(a.size())));
    SCITBX_ASSERT(directions.size() == a.size());
    SCITBX_ASSERT(angles.size() == a.size());
    for(std::size_t i=0;i<a.size();i++) {
      SCITBX_ASSERT(directions[i].length() > 0)(directions[i].length());
      vec3<double> unit = directions[i].normalize();
      result.push_back(a[i].unit_rotate_around_origin(
        unit, angles[i]));
    }
    return result;
  }

  af::shared<double> angle(
      af::const_ref< vec3<double> > const& self,
      vec3<double> other, bool deg) {
    af::shared<double> result(self.size());
    for (std::size_t i = 0; i < self.size(); ++i) {
      boost::optional<double> oa = self[i].angle_rad(other);
      if (oa) {
        double a = *oa;
        if (deg) {
          a = rad_as_deg(a);
        }
        result[i] = a;
      } else {
        result[i] = 0.0;
      }
    }
    return result;
  }

  af::shared<double> angle(
      af::const_ref< vec3<double> > const& self,
      af::const_ref< vec3<double> > const& other,
      bool deg) {
    SCITBX_ASSERT(self.size() == other.size());
    af::shared<double> result(self.size());
    for (std::size_t i = 0; i < self.size(); ++i) {
      boost::optional<double> oa = self[i].angle_rad(other[i]);
      if (oa) {
        double a = *oa;
        if (deg) {
          a = rad_as_deg(a);
        }
        result[i] = a;
      } else {
        result[i] = 0.0;
      }
    }
    return result;
  }

  flex_double
  as_double(flex<vec3<double> >::type const& a)
  {
    SCITBX_ASSERT(a.accessor().is_trivial_1d());
    flex_double result(a.size()*3, init_functor_null<double>());
    double* r = result.begin();
    const_ref<vec3<double> > a_ref = a.const_ref().as_1d();
    for(std::size_t i=0;i<a_ref.size();i++) {
      for(std::size_t j=0;j<3;j++) {
        *r++ = a_ref[i][j];
      }
    }
    return result;
  }

  vec3<double>
  vec3_min(flex<vec3<double> >::type const& a)
  {
    SCITBX_ASSERT(!a.accessor().is_padded());
    vec3<double> result(0,0,0);
    af::const_ref<vec3<double>, af::flex_grid<> > a_ref = a.const_ref();
    if (a_ref.size() > 0) {
      result = a_ref[0];
      for(std::size_t i=1;i<a_ref.size();i++) {
        result.each_update_min(a_ref[i]);
      }
    }
    return result;
  }

  vec3<double>
  vec3_max(flex<vec3<double> >::type const& a)
  {
    SCITBX_ASSERT(!a.accessor().is_padded());
    vec3<double> result(0,0,0);
    af::const_ref<vec3<double>, af::flex_grid<> > a_ref = a.const_ref();
    if (a_ref.size() > 0) {
      result = a_ref[0];
      for(std::size_t i=1;i<a_ref.size();i++) {
        result.each_update_max(a_ref[i]);
      }
    }
    return result;
  }

  af::shared<vec3<double> >
  round(
    af::const_ref<vec3<double> > const& a,
    int n_digits)
  {
    af::shared<vec3<double> > result((af::reserve(a.size())));
    for (std::size_t i=0; i<a.size(); i++) {
      vec3<double> const& src = a[i];
      vec3<double> result_i;
      for (std::size_t j=0; j<3; j++) {
        result_i[j] = math::round(src[j], n_digits);
      }
      result.push_back(result_i);
    }
    return result;
  }

  af::shared<vec3<int> >
  iround(
    af::const_ref<vec3<double> > const& a)
  {
    af::shared<vec3<int> > result((af::reserve(a.size())));
    for (std::size_t i=0; i<a.size(); i++) {
      vec3<double> const& src = a[i];
      vec3<int> result_i;
      for (std::size_t j=0; j<3; j++) {
        result_i[j] = math::iround(src[j]);
      }
      result.push_back(result_i);
    }
    return result;
  }

  vec3<double>
  mean_weighted_a_a(
    af::const_ref<vec3<double> > const& self,
    af::const_ref<double> const& weights)
  {
    return af::mean_weighted(self, weights);
  }

  af::shared<vec3<double> >
  mul_a_scalar(
    af::const_ref<vec3<double> > const& a,
    double f)
  {
    af::shared<vec3<double> > result((af::reserve(a.size())));
    for(std::size_t i=0;i<a.size();i++) {
      result.push_back(a[i] * f);
    }
    return result;
  }

  af::shared<vec3<double> >
  mul_a_a_scalar(
    af::const_ref<vec3<double> > const& lhs,
    af::const_ref<double> const& rhs)
  {
    SCITBX_ASSERT(lhs.size() == rhs.size());
    af::shared<vec3<double> > result((af::reserve(lhs.size())));
    for(std::size_t i=0;i<lhs.size();i++) {
      result.push_back(lhs[i] * rhs[i]);
    }
    return result;
  }

  void
  imul_a_scalar(
    af::ref<vec3<double> > const& a,
    double f)
  {
    for(std::size_t i=0;i<a.size();i++) a[i] *= f;
  }

  af::shared<vec3<double> >
  div_a_as(
    af::ref<vec3<double> > const& lhs,
    af::ref<double> const& rhs)
  {
    SCITBX_ASSERT(lhs.size() == rhs.size());
    af::shared<vec3<double> > result((af::reserve(lhs.size())));
    for(std::size_t i=0;i<lhs.size();i++) {
      SCITBX_ASSERT(rhs[i] != 0);
      result.push_back(lhs[i] / rhs[i]);
    }
    return result;
  }

  af::shared<vec3<double> >
  mul_a_mat3(
    af::const_ref<vec3<double> > const& a,
    mat3<double> const& m)
  {
    af::shared<vec3<double> > result((af::reserve(a.size())));
    for(std::size_t i=0;i<a.size();i++) {
      result.push_back(a[i] * m);
    }
    return result;
  }

  af::shared<vec3<double> >
  rmul_a_mat3(
    af::const_ref<vec3<double> > const& a,
    mat3<double> const& m)
  {
    mat3<double> m_transposed = m.transpose();
    af::shared<vec3<double> > result((af::reserve(a.size())));
    for(std::size_t i=0;i<a.size();i++) {
      result.push_back(a[i] * m_transposed);
    }
    return result;
  }

  af::shared<double>
  dot_a_a(
    af::const_ref<vec3<double> > const& lhs,
    af::const_ref<vec3<double> > const& rhs)
  {
    SCITBX_ASSERT(lhs.size() == rhs.size());
    af::shared<double> result((af::reserve(lhs.size())));
    for(std::size_t i=0;i<lhs.size();i++) {
      result.push_back(lhs[i] * rhs[i]);
    }
    return result;
  }

  af::shared<double>
  dot_a_s(
    af::const_ref<vec3<double> > const& lhs,
    vec3<double> rhs)
  {
    af::shared<double> result((af::reserve(lhs.size())));
    for(std::size_t i=0;i<lhs.size();i++) {
      result.push_back(lhs[i] * rhs);
    }
    return result;
  }

  af::shared<double>
  dot_a(
    af::const_ref<vec3<double> > const& lhs)
  {
    af::shared<double> result((af::reserve(lhs.size())));
    for(std::size_t i=0;i<lhs.size();i++) {
      result.push_back(lhs[i] * lhs[i]);
    }
    return result;
  }

  af::shared<vec3<double> >
  cross_a_a(
    af::const_ref<vec3<double> > const& lhs,
    af::const_ref<vec3<double> > const& rhs)
  {
    SCITBX_ASSERT(lhs.size() == rhs.size());
    af::shared<vec3<double> > result((af::reserve(lhs.size())));
    for(std::size_t i=0;i<lhs.size();i++) {
      result.push_back(lhs[i].cross(rhs[i]));
    }
    return result;
  }

  af::shared<double>
  norms_(
    af::const_ref<vec3<double> > const& lhs)
  {
    af::shared<double> result((af::reserve(lhs.size())));
    for(std::size_t i=0;i<lhs.size();i++) {
      result.push_back(std::sqrt(lhs[i] * lhs[i]));
    }
    return result;
  }

  double
  sum_sq_(
    af::const_ref<vec3<double> > const& self)
  {
    double result = 0;
    for(std::size_t i=0;i<self.size();i++) {
      result += self[i] * self[i];
    }
    return result;
  }

  double
  norm_(
    af::const_ref<vec3<double> > const& self)
  {
    return std::sqrt(sum_sq_(self));
  }

  af::shared<vec3<double> >
  each_normalize(
    af::const_ref<vec3<double> > const& a,
    bool raise_if_length_zero=true)
  {
    af::shared<vec3<double> > result(a.begin(), a.end());
    vec3<double>* r = result.begin();
    std::size_t n_zero = 0;
    for(std::size_t i=0;i<a.size();i++) {
      double length = r[i].length();
      if (length == 0) n_zero++;
      else r[i] *= (1 / length);
    }
    if (n_zero != 0 && raise_if_length_zero) {
      throw std::runtime_error((boost::format(
        "flex.vec3_double.each_normalize():"
        " number of vectors with length zero: %lu of %lu")
          % n_zero % a.size()).str());
    }
    return result;
  }

  double
  min_distance_between_any_pair(
    af::const_ref<vec3<double> > const& lhs,
    af::const_ref<vec3<double> > const& rhs)
  {
    if (lhs.size() == 0) return 0;
    if (rhs.size() == 0) return 0;
    double min_length_sq = (lhs[0]-rhs[0]).length_sq();
    for(std::size_t i=0;i<lhs.size();i++) {
      for(std::size_t j=0;j<rhs.size();j++) {
        math::update_min(min_length_sq, (lhs[i]-rhs[j]).length_sq());
      }
    }
    return std::sqrt(min_length_sq);
  }

  boost::python::tuple
  min_distance_between_any_pair_with_id(
    af::const_ref<vec3<double> > const& lhs,
    af::const_ref<vec3<double> > const& rhs)
  {
    if (lhs.size() == 0) return boost::python::make_tuple(0, 0, 0);
    if (rhs.size() == 0) return boost::python::make_tuple(0, 0, 0);
    double min_length_sq = (lhs[0]-rhs[0]).length_sq();
    double working_length_sq = 0;
    int best_i=0;
    int best_j=0;
    for(std::size_t i=0;i<lhs.size();i++) {
      for(std::size_t j=0;j<rhs.size();j++) {
        working_length_sq=(lhs[i]-rhs[j]).length_sq();
        if (working_length_sq < min_length_sq) {
          best_i=i;
          best_j=j;
          min_length_sq=working_length_sq;
        }
      }
    }
    return boost::python::make_tuple(std::sqrt(min_length_sq), best_i, best_j);
  }

  double
  max_distance(
    af::const_ref<vec3<double> > const& lhs,
    af::const_ref<vec3<double> > const& rhs)
  {
    SCITBX_ASSERT(lhs.size() == rhs.size());
    if (lhs.size() == 0) return 0;
    double max_length_sq = 0;
    for(std::size_t i=0;i<lhs.size();i++) {
      math::update_max(max_length_sq, (lhs[i]-rhs[i]).length_sq());
    }
    return std::sqrt(max_length_sq);
  }

  double
  rms_difference(
    af::const_ref<vec3<double> > const& lhs,
    af::const_ref<vec3<double> > const& rhs)
  {
    SCITBX_ASSERT(lhs.size() == rhs.size());
    if (lhs.size() == 0) return 0;
    double sum_length_sq = 0;
    for(std::size_t i=0;i<lhs.size();i++) {
      sum_length_sq += (lhs[i]-rhs[i]).length_sq();
    }
    return std::sqrt(sum_length_sq / lhs.size());
  }

  double
  rms_length(
    af::const_ref<vec3<double> > const& lhs)
  {
    if (lhs.size() == 0) return 0;
    double sum_length_sq = 0;
    for(std::size_t i=0;i<lhs.size();i++) {
      sum_length_sq += lhs[i].length_sq();
    }
    return std::sqrt(sum_length_sq / lhs.size());
  }

} // namespace <anonymous>

namespace boost_python {

  template <>
  struct flex_default_element<vec3<double> >
  {
    static vec3<double>
    get() { return vec3<double>(0,0,0); }
  };

  void wrap_flex_vec3_double()
  {
    using namespace boost::python;
    using boost::python::arg;
    typedef flex_wrapper<vec3<double> > f_w;
    f_w::plain("vec3_double")
      .def_pickle(flex_pickle_single_buffered<vec3<double>,
        3*pickle_size_per_element<double>::value>())
      .def("__init__", make_constructor(join))
      .def("__init__", make_constructor(from_double))
      .def("part_names", part_names)
      .staticmethod("part_names")
      .def("parts", parts)
      .def("rotate_around_origin",
        (af::shared<vec3<double> >(*)(
          flex<vec3<double> >::type const&,
          vec3<double> const&,
          double const&)) rotate_around_origin)
      .def("rotate_around_origin",
        (af::shared<vec3<double> >(*)(
          flex<vec3<double> >::type const&,
          vec3<double> const&,
          flex<double>::type const&)) rotate_around_origin)
      .def("rotate_around_origin",
        (af::shared<vec3<double> >(*)(
          flex<vec3<double> >::type const&,
          flex<vec3<double> >::type const&,
          flex<double>::type const&)) rotate_around_origin)
      .def("angle",
        (af::shared<double>(*)(
          af::const_ref< vec3<double> > const&,
          vec3<double>, bool)) &angle, (
            arg("other"),
            arg("deg") = false))
      .def("angle",
        (af::shared<double>(*)(
          af::const_ref< vec3<double> > const&,
          af::const_ref< vec3<double> > const&,
          bool)) &angle, (
            arg("other"),
            arg("deg") = false))
      .def("as_double", as_double)
      .def("add_selected",
        (object(*)(
          object const&,
          af::const_ref<std::size_t> const&,
          af::const_ref<vec3<double> > const&)) add_selected_unsigned_a, (
            arg("indices"), arg("values")))
      .def("min", vec3_min)
      .def("max", vec3_max)
      .def("sum", f_w::sum_a)
      .def("mean", f_w::mean_a)
      .def("mean_weighted", mean_weighted_a_a, (arg("weights")))
      .def("__add__", f_w::add_a_s)
      .def("__add__", f_w::add_a_a)
      .def("__iadd__", f_w::iadd_a_s)
      .def("__iadd__", f_w::iadd_a_a)
      .def("__sub__", f_w::sub_a_s)
      .def("__sub__", f_w::sub_a_a)
      .def("__isub__", f_w::isub_a_s)
      .def("__mul__", mul_a_scalar)
      .def("__rmul__", mul_a_scalar)
      .def("__mul__", mul_a_a_scalar)
      .def("__rmul__", mul_a_a_scalar)
      .def("__imul__", imul_a_scalar, return_self<>())
      .def("__div__", div_a_as)
      .def("__truediv__", div_a_as)
      .def("__mul__", mul_a_mat3)
      .def("__rmul__", rmul_a_mat3)
      .def("round", round)
      .def("iround", iround)
      .def("dot", dot_a_s)
      .def("dot", dot_a_a)
      .def("dot", dot_a)
      .def("cross", cross_a_a)
      .def("norms", norms_)
      .def("transpose_multiply",
        (mat3<double>(*)(
          af::const_ref<vec3<double> > const&,
          af::const_ref<vec3<double> > const&)) matrix::transpose_multiply)
      .def("sum_sq", sum_sq_)
      .def("norm", norm_)
      .def("each_normalize", each_normalize, (
        arg("raise_if_length_zero")=true))
      .def("min_distance_between_any_pair", min_distance_between_any_pair)
      .def("min_distance_between_any_pair_with_id",
            min_distance_between_any_pair_with_id)
      .def("max_distance", max_distance)
      .def("rms_difference", rms_difference)
      .def("rms_length", rms_length)
    ;
  }

}}} // namespace scitbx::af::boost_python


//==================================================================================================
/**
  Copyright 2016 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)
**/
//==================================================================================================
#ifndef CAST_TEST_BASE_HPP_INCLUDED
#define CAST_TEST_BASE_HPP_INCLUDED

#include <boost/simd/function/enumerate.hpp>
#include <boost/simd/function/abs.hpp>
#include <type_traits>

namespace bs = boost::simd;

template<typename T, typename R> struct res
{
  using result = typename T::template rebind<R>;
  static result comp(const T& a0)
  {
    result r;
    for(size_t i=0; i < T::static_size; ++i)
    {
      r[i] = static_cast<R>(a0[i]);
    }
    return r;
  }
};

template <typename T, typename Env> void test_val(Env& runtime)
{
  using sT =  typename T::value_type;
  sT base = (sT(-1) < 0) ? sT(-1) : sT(0);
  T x =  bs::enumerate<T>(base, sT(4)/3);

  STF_EQUAL(bs::pack_cast<float        >(x), (res<T,float        >::comp(x)));
  STF_EQUAL(bs::pack_cast<double       >(x), (res<T,double       >::comp(x)));
  STF_EQUAL(bs::pack_cast<std::int8_t  >(x), (res<T,std::int8_t  >::comp(x)));
  STF_EQUAL(bs::pack_cast<std::int16_t >(x), (res<T,std::int16_t >::comp(x)));
  STF_EQUAL(bs::pack_cast<std::int32_t >(x), (res<T,std::int32_t>::comp(x)));
  STF_EQUAL(bs::pack_cast<std::int64_t >(x), (res<T,std::int64_t >::comp(x)));

  /*
    As per the standard static_cast<std::uint*_t>(-1.) is undefined so we don't test
    for undefined cases.

    See: http://www.embeddeduse.com/2013/08/25/casting-a-negative-float-to-an-unsigned-int/
    See: http://www.open-std.org/jtc1/sc22/wg21/docs/papers/2012/n3337.pdf

    Section 4.9:
    " A prvalue of a floating point type can be converted to a prvalue of an
      integer type. The conversion truncates; that is, the fractional part
      is discarded. The behavior is undefined if the truncated value cannot
      be represented in the destination type."
  */
  if(std::is_floating_point<sT>::value)
  {
    x = bs::abs(x);
  }

  STF_EQUAL(bs::pack_cast<std::uint8_t >(x), (res<T,std::uint8_t >::comp(x)));
  STF_EQUAL(bs::pack_cast<std::uint16_t>(x), (res<T,std::uint16_t>::comp(x)));
  STF_EQUAL(bs::pack_cast<std::uint32_t>(x), (res<T,std::uint32_t>::comp(x)));
  STF_EQUAL(bs::pack_cast<std::uint64_t>(x), (res<T,std::uint64_t>::comp(x)));

  std::cout << "=====================================================================\n" << std::endl;
}

#endif


/**
* 2D NDT-MCL for offline processing (reads directly the bag files).
* This application runs the ndt-mcl localization based on a map and laser scanner and odometry.
*
* This is based now on our sample file (see data folder). The sample file also has ground truth pose information,
* which we use now for the initialization (replace with manual input if you will).
*
* The visualization depends on mrpt-gui
*
* More details about the algorithm:
* Jari Saarinen, Henrik Andreasson, Todor Stoyanov and Achim J. Lilienthal, Normal Distributions Transform Monte-Carlo Localization (NDT-MCL)
* IEEE/RSJ International Conference on Intelligent Robots and Systems November 3-8, 2013, Tokyo Big Sight, Japan
*
* Known issues: in launch file (or hard coded parameters) you have to set the same resolution for NDT map as is saved -- otherwise it wont work
* @author Jari Saarinen (jari.p.saarinen@gmail.com)
*
*/

#define USE_VISUALIZATION_DEBUG ///< Enable / Disable visualization

#ifdef USE_VISUALIZATION_DEBUG
    #include <ndt_visualisation/ndt_viz.h>
NDTViz ndt_viz;
#endif

#include <ros/ros.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <tf/transform_listener.h>
#include <boost/foreach.hpp>
#include <sensor_msgs/LaserScan.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/OccupancyGrid.h>
#include <tf/transform_broadcaster.h>

#include <ndt_map/ndt_map.h>
#include "ndt_mcl/tfMessageReader.h"
#include "ndt_mcl/ndt_mcl.h"

/**
 * Convert x,y,yaw to Eigen::Affine3d
 */
Eigen::Affine3d getAsAffine(float x, float y, float yaw ){
	Eigen::Matrix3d m;
	m = Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX())
			* Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY())
			* Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ());
	Eigen::Translation3d v(x,y,0);
	Eigen::Affine3d T = v*m;

	return T;
}
double getDoubleTime()
{
    struct timeval time;
    gettimeofday(&time,NULL);
    return time.tv_sec + time.tv_usec * 1e-6;
}

/////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////
/// Update measurement
/////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////
/// Globals
/////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////
NDTMCL *ndtmcl;

///Laser sensor offset
float offx = 0;
float offy = 0;
float offa = 0;

static bool has_sensor_offset_set = false;
static bool isFirstLoad=true;
Eigen::Affine3d Told,Todo;
FILE *flog = fopen("ndtmcl_result.txt","wt");

/////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////
double time_end;
std::string tf_odo_topic =   "odom_base_link";
std::string tf_state_topic = "base_link";
std::string tf_laser_link =  "base_laser_link";

/**
 * This is where the localization is done... not really callback in the offline version
 */
void callback(const sensor_msgs::LaserScan &scan, tf::Transform odo_pose, tf::Transform state_pose)
{
	static int counter = 0;

	static tf::TransformListener tf_listener;

	double time_now = getDoubleTime();
	double looptime = time_end - time_now;
	fprintf(stderr,"Lt( %.1lfms %.1lfHz seq:%d) -",looptime*1000,1.0/looptime,scan.header.seq);

	if(has_sensor_offset_set == false) return;

	double gx,gy,gyaw,x,y,yaw;

	///Get the ground truth
	gyaw = tf::getYaw(state_pose.getRotation());
	gx = state_pose.getOrigin().x();
	gy = state_pose.getOrigin().y();

	///Get the odometry
	yaw = tf::getYaw(odo_pose.getRotation());
	x = odo_pose.getOrigin().x();
	y = odo_pose.getOrigin().y();

	int N =(scan.angle_max - scan.angle_min)/scan.angle_increment; ///< number of scan lines

	/////
	/// Pose conversions
	////

	Eigen::Affine3d T = getAsAffine(x,y,yaw);
	Eigen::Affine3d Tgt = getAsAffine(gx,gy,gyaw);

	/**
	 * We are now using the information from the ground truth to initialize the filter
	 */
	if(isFirstLoad){
		fprintf(stderr,"Initializing to (%lf, %lf, %lf)\n",gx,gy,gyaw);
		///Initialize the filter with 1m^2 variance in position and 20deg in heading
		ndtmcl->initializeFilter(gx, gy,gyaw,1.0, 1.0, 20.0*M_PI/180.0, 450);
		Told = T;
		Todo = Tgt;
	}

	///Compute the differential motion between two time steps
	Eigen::Affine3d Tmotion = Told.inverse() * T;
	Todo = Todo*Tmotion;
	Told = T;

	///Get the laser pose with respect to the base
	float dy =offy;
	float dx = offx;
	float alpha = atan2(dy,dx);
	float L = sqrt(dx*dx+dy*dy);

	///Laser pose in base frame
	float lpx = L * cos(alpha);
	float lpy = L * sin(alpha);
	float lpa = offa;

	///Laser scan to PointCloud transformed with respect to the base
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
	for (int j=0;j<N;j++){
		double r  = scan.ranges[j];
		if(r>=scan.range_min && r<scan.range_max && r>0.3 && r<20.0){
			double a  = scan.angle_min + j*scan.angle_increment;
			pcl::PointXYZ pt;
			pt.x = r*cos(a+lpa)+lpx;
			pt.y = r*sin(a+lpa)+lpy;
			pt.z = 0.1+0.02 * (double)rand()/(double)RAND_MAX;
			cloud->push_back(pt);
		}
	}
	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	/// Now we have the differential motion and pointcloud -- Lets do MCL
	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	ndtmcl->updateAndPredict(Tmotion, *cloud); ///< does the prediction, update and resampling steps (see ndt_mcl.hpp)

	Eigen::Vector3d dm = ndtmcl->getMean(); ///<Maximum aposteriori estimate of the pose
	Eigen::Matrix3d cov = ndtmcl->pf.getDistributionVariances(); ///distribution variances
	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	time_end = getDoubleTime();
	fprintf(stderr,"Time elapsed %.1lfms (%lf %lf %lf) \n",time_end-time_now,dm[0],dm[1],dm[2]);
	isFirstLoad = false;
	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

	//If visualization desired
#ifdef USE_VISUALIZATION_DEBUG
	if(counter%500==0){
	    ndt_viz.clear();
	    ndt_viz.plotNDTSAccordingToOccupancy(-1,&ndtmcl->map);
            ndt_viz.displayParticles();
            ndt_viz.displayTrajectory();
	}
	ndt_viz.clearParticles();
	for(int i=0;i<ndtmcl->pf.NumOfParticles;i++){
	    ndt_viz.addParticle(ndtmcl->pf.Particles[i].x, ndtmcl->pf.Particles[i].y, 0.5, 1.0, 1.0, 1.0);
	}

	ndt_viz.addTrajectoryPoint(dm[0],dm[1],0.5,1.0,0,0);
	ndt_viz.addTrajectoryPoint(Tgt.translation()(0), Tgt.translation()(1),0.5,1.0,1.0,1.0);
	ndt_viz.addTrajectoryPoint(Todo.translation()(0), Todo.translation()(1),0.5,0.0,1.0,0.0);

	ndt_viz.win3D->setCameraPointingToPoint(dm[0],dm[1],3.0);
	ndt_viz.repaint();
//        ndt_viz.win3D->process_events();
#endif

        counter++;
}

int main(int argc, char **argv){
	ros::init(argc, argv, "sauna_mcl");
	double resolution=0.2;
	ros::NodeHandle n;
	ros::NodeHandle nh;
	ros::NodeHandle paramHandle ("~");
	time_end = getDoubleTime();
	//////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////
	/// Parameters for the mapper
	//////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////
	bool loadMap = false; ///< flag to indicate that we want to load a map
	std::string mapName("basement.ndmap"); ///<name and the path to the map

	bool makeMapVeryStatic = false; ///< indicates if we should make the map over confident (stationary that is)

	bool saveMap = true;						///< indicates if we want to save the map in a regular intervals
	std::string output_map_name = std::string("ndt_mapper_output.ndmap");

	//////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////
	/// Set the values from a config file
	//////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////

	std::string input_laser_topic;
	paramHandle.param<std::string>("input_laser_topic", input_laser_topic, std::string("/base_scan"));

	paramHandle.param<std::string>("tf_base_link", tf_state_topic, std::string("/state_base_link"));
	paramHandle.param<std::string>("tf_laser_link", tf_laser_link, std::string("/hokuyo1_link"));

	bool use_sensor_pose;
	paramHandle.param<bool>("use_sensor_pose", use_sensor_pose, false);
	double sensor_pose_x, sensor_pose_y, sensor_pose_th;
	paramHandle.param<double>("sensor_pose_x", sensor_pose_x, 0.);
	paramHandle.param<double>("sensor_pose_y", sensor_pose_y, 0.);
	paramHandle.param<double>("sensor_pose_th", sensor_pose_th, 0.);

	paramHandle.param<bool>("load_map_from_file", loadMap, false);
	paramHandle.param<std::string>("map_file_name", mapName, std::string("basement.ndmap"));

	paramHandle.param<bool>("save_output_map", saveMap, true);
	paramHandle.param<std::string>("output_map_file_name", output_map_name, std::string("ndt_mapper_output.ndmap"));

	paramHandle.param<double>("map_resolution", resolution , 0.2);
	bool forceSIR=false;
	paramHandle.param<bool>("forceSIR", forceSIR, false);

	std::string bagfilename="bagfile_unset.bag";
	paramHandle.param<std::string>("bagfile_name", bagfilename, std::string("bagfile_unset.bag"));

	//////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////
	/// Prepare the map
	//////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////
	fprintf(stderr,"USING RESOLUTION %lf\n",resolution);
	perception_oru::NDTMap ndmap(new perception_oru::LazyGrid(resolution));

	ndmap.setMapSize(80.0, 80.0, 1.0);

	if(loadMap){
		fprintf(stderr,"Loading Map from '%s'\n",mapName.c_str());
		ndmap.loadFromJFF(mapName.c_str());
	}

	ndtmcl = new NDTMCL(resolution,ndmap,-0.5);
	if(forceSIR) ndtmcl->forceSIR=true;

	fprintf(stderr,"*** FORCE SIR = %d****",forceSIR);

	//////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////
	/// Open the bag file for reading
	//////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////

	tfMessageReader<sensor_msgs::LaserScan> reader(bagfilename,
																								 input_laser_topic,
																								 std::string("/world"),
																								 tf_odo_topic);

	///Set sensor offsets
	offa = sensor_pose_th;
	offx = sensor_pose_x;
	offy = sensor_pose_y;
	has_sensor_offset_set = true;

	fprintf(stderr,"Sensor Pose = (%lf %lf %lf)\n",offx, offy, offa);

#ifdef USE_VISUALIZATION_DEBUG
        ndt_viz.win3D->start_main_loop_own_thread();
#endif
	///Loop while we have data in the bag
	while(!reader.bagEnd() && ros::ok()){
		sensor_msgs::LaserScan s;
		tf::Transform odo_pose;
		bool hasOdo = false;
		bool hasState = false;

		if(reader.getNextMessage(s,  odo_pose)){
			hasOdo = true;
		}

		tf::Transform state_pose;

		if(reader.getTf(tf_state_topic, s.header.stamp, state_pose)){
			hasState = true;
		}

		///If we have the data then lets run the localization
		if(hasState && hasOdo){
			 callback(s,odo_pose,state_pose);
		}
#ifdef USE_VISUALIZATION_DEBUG
//                ndt_viz.win3D->process_events();
#endif
	}

	fprintf(stderr,"-- THE END --\n");

	return 0;
}


#include <iostream>
#include <fstream>
#include <iomanip>
#include "gausslegendre128_quadrature.h"
#include "lebedev59_quadrature.h"
#include "solid_real_sphericalharmonics_maxl5.h"
#include "slater_poisson_maxn7.h"
#include "sto_params.h"

#include "site_type.h"

#include "json/json.h"

#include <Eigen/Eigen>

#include "site.h"
#include "computations.h"
#include "timer.h"

//using namespace std;
bool cmdOptionExists(char** begin, char** end, const std::string& option)
{
    return std::find(begin, end, option) != end;
}

int main(int argc, char * argv[])
{
    try
    {

        bool test = false;        test = true;
        if(cmdOptionExists(argv, argv+argc, "-t")) test = true;

        std::string rho_cube_file = "rho.cube";
        bool cube = false;;
        if(cmdOptionExists(argv, argv+argc, "-c")) cube = true;

        //std::string input_file = "input.json";
        std::string input_file = "test/t1.json";

        std::cout << "# ################### #" << std::endl;
        std::cout << "# STO based DFT code! #" << std::endl;
        std::cout << "# ################### #" << std::endl;
        std::cout << std::endl;

        Timer general_time;
        std::cout << "# Loading input... " << std::endl;
        Json::Value root;

        std::string test_reference;
        if(test)
        {

            std::cout << "# LOADING TEST VERSION!" << std::endl;
            std::istringstream is( "{ \"labels\" : [ \"Li\" , \"Li\" ], "
                                   "\"positions\" : [ [0.0 , 0.0, 0.0 ] , [1.0, 0.0, 0.0 ] ], "
                                   "\"a1\" : [10.0,0.0,0.0], \"a2\" : [0.0,10.0,0.0], \"a3\" : [0.0,0.0,10.0], "
                                   "\"scf\" : { "
                                   "\"alpha\" : 0.5, "
                                   "\"pulay_number\" : 4, "
                                   "\"max_iteration\" : 50, "
                                   "\"total_energy_tolerance\" :0.0000000001, "
                                   "\"density_matrix_norm_tolerance\" : 0.0000000001 }, "
                                   " \"Li\" : { \"symbol\" : \"Li\" , \"zindex\" : 3.0 , \"nelectron\" : 3.0 , \"radius\" : 10.0 , "
                                               "\"mass\" : 3.0 , \"grid\" : { \"scale\" : 0.6, \"gauss\" : 32, \"lebedev\" : 29 }, "
                                               "\"name\" : \"Lithium\", \"title\" : \"Lithium (SZ)\", "
                                               "\"basis\" : [ [\"1S\" , 2.69], [\"2S\" , 0.80], [\"2P\" , 0.80] ], "
                                               "\"qbasis\" : [ [\"1S\" , 5.38 , 0.488016982152593E+00], [\"2S\" , 5.41 , -0.369857854120576E+00], [\"2S\" , 3.36 , -0.154262136775740E+00], [\"3S\" , 3.08 , -0.630098272767401E-01], [\"3S\" , 2.06 , 0.526058672124576E-01], [\"3S\" , 1.38 , 0.115711696734655E-01], [\"3S\" , 0.92 , -0.236602213283877E-03], [\"2P\" , 3.49], [\"2P\" , 1.96], [\"2P\" , 1.10], [\"3P\" , 0.92], [\"3D\" , 1.60], [\"3D\" , 1.21], [\"3D\" , 0.92], [\"4F\" , 5.00], [\"4F\" , 3.50], [\"5G\" , 3.50] ] } }");
            is >> root;
            test_reference = "# ###### TEST REFERENCE #### ITERATION:   19 ###### TEST REFERENCE ##### #\n"
                             "#       homo-2         homo-1           homo           lumo         lumo+1\n"
                             "    2.00000000     2.00000000     2.00000000     0.00000000     0.00000000\n"
                             "   -3.03288837    -1.46004831    -0.14705546    -0.09326319    -0.09326319\n"
                             "#         Etot          Ecoul            Exc          dEtot          dDens\n"
                             "  -22.94421063    13.66422634    -4.58908025    -0.00027827     0.00664729\n"
                             "# ---------------------------------------------------------------------- #\n"
                             "# Self-sonsitent iteration stopped in 6.53s = 0.11m = 0.00h\n"
                             "# ###### TEST REFERENCE #### ############### ###### TEST REFERENCE ##### #\n";
        }
        else
        {

            std::ifstream config_doc(input_file, std::ifstream::binary);
            config_doc >> root;

        }
        Computation computation;
        computation.init_from_json(root);

        std::cout << "# Number of atoms     = " << computation.multisites.sites.size() << std::endl;
        std::cout << "# Number of basis     = " << computation.multisites.basis_ref.size() << std::endl;
        std::cout << "# Number of qbasis    = " << computation.multisites.qbasis_ref.size() << std::endl;
        std::cout << "# Number of electrons = " << computation.multisites.total_electron << std::endl;

        for(int i = 0; i < computation.multisites.sites.size(); i++ )
        {
            std::cout << "# Site " << i+1 << " " << computation.multisites.sites[i].label << " " << std::endl;
        }
        std::cout << "# Input is loaded! ("<< general_time.elapsed() <<"s)"<< std::endl;

        general_time.reset();
        std::cout << "# Initializing the system... " << std::endl;
        computation.compute_Hinit2();
        std::cout << "# Number of grid points = " << computation.rho_grid.size() << std::endl;
        std::cout << "# System is initialized! ("<< general_time.elapsed() <<"s)"<< std::endl;

        std::cout << std::endl;
        std::cout << "# Starting self-consistent calculation... "<< std::endl;
        std::cout << computation.dDensdiff << std::endl;

        general_time.reset();
        for(size_t i = 0; i<computation.diis.max_iteration; i++)
        {
            computation.compute_Hscf2();
            std::cout << "# ########################## ITERATION: " << std::setw(4) << i << " ########################### #"<< std::endl;

            std::cout << '#';
            std::cout << std::setprecision(8) << std::fixed << std::setw(13)<< "homo-2" << ' ';
            std::cout << std::setprecision(8) << std::fixed << std::setw(14)<< "homo-1" << ' ';
            std::cout << std::setprecision(8) << std::fixed << std::setw(14)<< "homo" << ' ';
            std::cout << std::setprecision(8) << std::fixed << std::setw(14)<< "lumo"  << ' ';
            std::cout << std::setprecision(8) << std::fixed << std::setw(14)<< "lumo+1" << ' ';
            std::cout << std::endl;
            for(size_t k = std::max(0,int(computation.ihomo-2)); k < std::min(computation.ilumo+2,int(computation.occ.size())); k++)
            {
                std::cout << std::setprecision(8) << std::fixed << std::setw(14) << computation.occ[k] << ' ';
            }
            std::cout << std::endl;
            for(size_t k = std::max(0,int(computation.ihomo-2)); k < std::min(computation.ilumo+2,int(computation.occ.size())); k++)
            {
                std::cout << std::setprecision(8) << std::fixed << std::setw(14) << computation.eE[k] << ' ';
            }
            std::cout << std::endl;
            /*
            for(size_t k = 0; k < computation.occ.size(); k++) if (computation.occ[k==0?k:k-1]>0.0) {
                std::cout << std::setprecision(8) << std::setw(12) << std::fixed << computation.eE(k) << ' ';
            }
            std::cout << std::endl;*/

            std::cout << '#';
            std::cout << std::setprecision(8) << std::fixed << std::setw(13)<< "Etot" << ' ';
            std::cout << std::setprecision(8) << std::fixed << std::setw(14)<< "Ecoul" << ' ';
            std::cout << std::setprecision(8) << std::fixed << std::setw(14)<< "Exc" << ' ';
            std::cout << std::setprecision(8) << std::fixed << std::setw(14)<< "dEtot"  << ' ';
            std::cout << std::setprecision(8) << std::fixed << std::setw(14)<< "dDens" << ' ';
            std::cout << std::endl;

            std::cout << std::setprecision(8) << std::fixed << std::setw(14)<< computation.Etot << ' ';
            std::cout << std::setprecision(8) << std::fixed << std::setw(14)<< computation.Ecoul << ' ';
            std::cout << std::setprecision(8) << std::fixed << std::setw(14)<< computation.Exc << ' ';
            std::cout << std::setprecision(8) << std::fixed << std::setw(14)<< computation.dEtot << ' ';
            std::cout << std::setprecision(8) << std::fixed << std::setw(14)<< computation.dDensdiff << ' ';
            std::cout << std::endl;
            if(std::abs(computation.dEtot)<computation.diis.total_energy_tolerance) break;
            if(std::abs(computation.dDensdiff)<computation.diis.density_matrix_norm_tolerance) break;
            std::cout << "# ---------------------------------------------------------------------- #"<< std::endl;

        }

        std::cout << std::setprecision(2) << std::fixed << "# Self-sonsitent iteration stopped in " << general_time.elapsed() << "s = " << general_time.elapsed()/60.0<< "m = " << general_time.elapsed()/3600.0 << "h"<< std::endl;

        if (test)
        {
            std::cout << test_reference << std::endl;
        }
        if (cube)
        {
            general_time.reset();
            std::cout << "# Saving qcoeff to cube file... " << std::endl;
            save_cube_from_qcoeff(computation.multisites.sites,computation.qcoeff, {-2,-2,-2}, {3,2,2},0.1,rho_cube_file);
            std::cout << "# qcoeff is saved! ("<< general_time.elapsed() <<"s)"<< std::endl;
        }

    }
    catch (const char * e)
    {
        std::cerr << e << std::endl;
    }
    return 0;
}


// Align a rigid object to a scene with clutter and occlusions

#include <iostream>
#include <ros/ros.h>
#include <Eigen/Core>
#include <vector>
//#include <Matrix.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/visualization/cloud_viewer.h>
//#include <pcl/common/centroid.h>
#include <pcl/registration/icp.h>
#include <pcl/point_cloud.h>
#include "pcl/common/transformation_from_correspondences.h"
//#include <boost/thread/thread.hpp>
#include <pcl/filters/project_inliers.h>
#include <pcl/common/time.h>
#include <pcl/console/print.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/filters/filter.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/sample_consensus_prerejective.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <vtkCellArray.h>
#include <iostream>
#include <ros/ros.h>
#include <Eigen/Core>
#include <vector>
//#include <Matrix.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/visualization/cloud_viewer.h>
//#include <pcl/common/centroid.h>

#include <boost/thread/thread.hpp>

typedef pcl::PointNormal PointNT;
typedef pcl::PointCloud<PointNT> PointCloudT;
typedef pcl::FPFHSignature33 FeatureT;
typedef pcl::FPFHEstimationOMP<PointNT,PointNT,FeatureT> FeatureEstimationT;
typedef pcl::PointCloud<FeatureT> FeatureCloudT;
typedef pcl::visualization::PointCloudColorHandlerCustom<PointNT> ColorHandlerT;

PointCloudT::Ptr object (new PointCloudT);

pcl::PointCloud<pcl::PointXYZ>::Ptr cluster (new pcl::PointCloud<pcl::PointXYZ>);

void
cloud_cb (const pcl::PCLPointCloud2ConstPtr& cloud_blob)
{
  // Read in the cloud data

  PointCloudT::Ptr object_aligned (new PointCloudT);
  PointCloudT::Ptr scene (new PointCloudT);
  FeatureCloudT::Ptr object_features (new FeatureCloudT);
  FeatureCloudT::Ptr scene_features (new FeatureCloudT);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cluster (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
  //pcl::PCLPointCloud2::Ptr cloud_filtered_blob (new pcl::PCLPointCloud2);
  pcl::fromPCLPointCloud2 (*cloud_blob, *cloud);
  std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl; //*

  // Create the filtering object: downsample the dataset using a leaf size of 1cm
  pcl::VoxelGrid<pcl::PointXYZ> vg;

  vg.setInputCloud (cloud);
  vg.setLeafSize (0.01f, 0.01f, 0.01f);
  vg.filter (*cloud_filtered);
  std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size ()  << " data points." << std::endl; //*

  // Create the segmentation object for the planar model and set all the parameters
  pcl::SACSegmentation<pcl::PointXYZ> seg;
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());
  pcl::PCDWriter writer;
  seg.setOptimizeCoefficients (true);
  seg.setModelType (pcl::SACMODEL_PLANE);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setMaxIterations (100);
  seg.setDistanceThreshold (0.02);

  int i=0, nr_points = (int) cloud_filtered->points.size ();
  while (cloud_filtered->points.size () > 0.3 * nr_points)
  {
    // Segment the largest planar component from the remaining cloud
    seg.setInputCloud (cloud_filtered);
    seg.segment (*inliers, *coefficients);
    if (inliers->indices.size () == 0)
    {
      std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
      break;
    }

    // Extract the planar inliers from the input cloud
    pcl::ExtractIndices<pcl::PointXYZ> extract;
    extract.setInputCloud (cloud_filtered);
    extract.setIndices (inliers);
    extract.setNegative (false);

    // Get the points associated with the planar surface
    extract.filter (*cloud_plane);
    std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;

    // Remove the planar inliers, extract the rest
    extract.setNegative (true);
    extract.filter (*cloud_f);
    *cloud_filtered = *cloud_f;
  }

  // Creating the KdTree object for the search method of the extraction
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
  tree->setInputCloud (cloud_filtered);

  std::vector<pcl::PointIndices> cluster_indices;
  pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
  ec.setClusterTolerance (0.02); // 2cm
  ec.setMinClusterSize (100);
  ec.setMaxClusterSize (25000);
  ec.setSearchMethod (tree);
  ec.setInputCloud (cloud_filtered);
  ec.extract (cluster_indices);

  int j = 0;
  //cloud_filtered_blob = cloud_plane;
  for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
  {
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
    for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
      cloud_cluster->points.push_back (cloud_filtered->points[*pit]); //*
    cloud_cluster->width = cloud_cluster->points.size ();
    cloud_cluster->height = 1;
    cloud_cluster->is_dense = true;
      //uint8_t r = 0, g = 255, b = 0;
      //uint32_t rgb = ((uint32_t)r << 16 | (uint32_t)g << 8 | (uint32_t)b);
      //cloud_filtered->at(*it).rgb = *reinterpret_cast<float*>(&rgb);

    std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
    std::stringstream ss;
    ss << "cloud_cluster_" << j << ".pcd";
    writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); //* ---you should be able to write a  normal cloud here  pcl::PCDWriter writer;   writer.write<pcl::PointNormal> (ss.str (), *cloud_cluster_don, false);
    //pcl_conversions::toPCL(*cloud_msg, *cloud);
    //pcl::toPCLPointCloud2 (*cloud_cluster, *cloud_filtered_blob);
    //pcl::concatenatePointCloud (cloud_cluster,cloud_filtered_blob, cloud_filtered_blob);
    pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");
    viewer.showCloud (cloud_cluster);
    while (!viewer.wasStopped ())
    {
    }
    //pcl::compute3DCentroid(*cloud_cluster, const pcl::PointIndices& *indices, Eigen::Matrix<float,4,1>& centroid);

    j++;
  }
  //////////////---conversion of the cloud to cloud with normals
  // Normal estimation*
  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
  pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_cloud (new pcl::search::KdTree<pcl::PointXYZ>);
  tree_cloud->setInputCloud (cloud);
  n.setInputCloud (cloud);
  n.setSearchMethod (tree_cloud);
  n.setKSearch (20);
  n.compute (*normals);
  //* normals should not contain the point normals + surface curvatures

  // Concatenate the XYZ and normal fields*
  pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals (new pcl::PointCloud<pcl::PointNormal>);
  pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);
  //* cloud_with_normals = cloud + normals

  ////////-----pose estimation
  scene = cloud_with_normals;

  //pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
  pcl::PointCloud<pcl::Normal>::Ptr normals_cluster (new pcl::PointCloud<pcl::Normal>);
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_cluster (new pcl::search::KdTree<pcl::PointXYZ>);
  tree_cluster->setInputCloud (cluster);
  n.setInputCloud (cluster);
  n.setSearchMethod (tree_cluster);
  n.setKSearch (20);
  n.compute (*normals_cluster);
  //* normals should not contain the point normals + surface curvatures

  // Concatenate the XYZ and normal fields*
  pcl::PointCloud<pcl::PointNormal>::Ptr cluster_with_normals (new pcl::PointCloud<pcl::PointNormal>);
  pcl::concatenateFields (*cluster, *normals_cluster, *cluster_with_normals);

  object = cluster_with_normals;
  // Downsample
  pcl::console::print_highlight ("Downsampling...\n");
  pcl::VoxelGrid<PointNT> grid;
  const float leaf = 0.005f;
  grid.setLeafSize (leaf, leaf, leaf);
  grid.setInputCloud (object);
  grid.filter (*object);
  grid.setInputCloud (scene);
  grid.filter (*scene);

  // Estimate normals for scene
  pcl::console::print_highlight ("Estimating scene normals...\n");
  pcl::NormalEstimationOMP<PointNT,PointNT> nest;
  nest.setRadiusSearch (0.01);
  nest.setInputCloud (scene);
  nest.compute (*scene);

  // Estimate features
  pcl::console::print_highlight ("Estimating features...\n");
  FeatureEstimationT fest;
  fest.setRadiusSearch (0.025);
  fest.setInputCloud (object);
  fest.setInputNormals (object);
  fest.compute (*object_features);
  fest.setInputCloud (scene);
  fest.setInputNormals (scene);
  fest.compute (*scene_features);

  // Perform alignment
  pcl::console::print_highlight ("Starting alignment...\n");
  pcl::SampleConsensusPrerejective<PointNT,PointNT,FeatureT> align;
  align.setInputSource (object);
  align.setSourceFeatures (object_features);
  align.setInputTarget (scene);
  align.setTargetFeatures (scene_features);
  align.setMaximumIterations (10000); // Number of RANSAC iterations
  align.setNumberOfSamples (3); // Number of points to sample for generating/prerejecting a pose
  align.setCorrespondenceRandomness (2); // Number of nearest features to use
  align.setSimilarityThreshold (0.9f); // Polygonal edge length similarity threshold
  align.setMaxCorrespondenceDistance (1.5f * leaf); // Inlier threshold
  align.setInlierFraction (0.25f); // Required inlier fraction for accepting a pose hypothesis
  {
    pcl::ScopeTime t("Alignment");
    align.align (*object_aligned);
  }

  if (align.hasConverged ())
  {
    // Print results
    printf ("\n");
    Eigen::Matrix4f transformation = align.getFinalTransformation ();
    pcl::console::print_info ("    | %6.3f %6.3f %6.3f | \n", transformation (0,0), transformation (0,1), transformation (0,2));
    pcl::console::print_info ("R = | %6.3f %6.3f %6.3f | \n", transformation (1,0), transformation (1,1), transformation (1,2));
    pcl::console::print_info ("    | %6.3f %6.3f %6.3f | \n", transformation (2,0), transformation (2,1), transformation (2,2));
    pcl::console::print_info ("\n");
    pcl::console::print_info ("t = < %0.3f, %0.3f, %0.3f >\n", transformation (0,3), transformation (1,3), transformation (2,3));
    pcl::console::print_info ("\n");
    pcl::console::print_info ("Inliers: %i/%i\n", align.getInliers ().size (), object->size ());

    // Show alignment
    pcl::visualization::PCLVisualizer visu("Alignment");
    visu.addPointCloud (scene, ColorHandlerT (scene, 0.0, 255.0, 0.0), "scene");
    visu.addPointCloud (object_aligned, ColorHandlerT (object_aligned, 0.0, 0.0, 255.0), "object_aligned");
    visu.spin ();
  }
  else
  {
    pcl::console::print_error ("Alignment failed!\n");

  }

}

int
main (int argc, char** argv)
{
  // Initialize ROS
  ros::init (argc, argv, "obstacle_projection");
  ros::NodeHandle nh;

  // Create a ROS subscriber for the input point cloud
  ros::Subscriber sub = nh.subscribe ("input", 1, cloud_cb);

    // Load object and scene
  pcl::console::print_highlight ("Loading point clouds...\n");
  if (pcl::io::loadPCDFile<PointXYZ> (argv[2], *cluster) < 0)
  {
    pcl::console::print_error ("Error loading object/scene file!\n");
    return (1);
  }
  ros::spin ();
}


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

#pragma once

#include <Eigen/Dense>
#include <cmath>

namespace zdock {

class TransformUtil {
private:
  typedef Eigen::Transform<double, 3, Eigen::Affine> Transform;
  typedef Eigen::Matrix<double, 3, Eigen::Dynamic> Matrix;

public:
  static const double PI;

  // Euler angles to Z-X-Z transformation matrix
  static inline const Transform eulerRotation(const double (&r)[3],
                                              bool rev = false) {
    Transform t;

    using Eigen::AngleAxisd;
    using Eigen::Vector3d;

    t = AngleAxisd(r[0], Vector3d::UnitZ()) *
        AngleAxisd(r[1], Vector3d::UnitX()) *
        AngleAxisd(r[2], Vector3d::UnitZ());
    return (rev ? t.inverse() : t);
  }

  // Adjust grid coordinates to fall inside box of size 'boxsize',
  // NOTE: returns Vector3d (double).
  static inline const Eigen::Vector3d boxedGridCoord(const int (&v)[3],
                                                     const int boxsize) {
    Eigen::Vector3d d;
    d << (v[0] >= boxsize / 2 ? v[0] - boxsize : v[0]),
        (v[1] >= boxsize / 2 ? v[1] - boxsize : v[1]),
        (v[2] >= boxsize / 2 ? v[2] - boxsize : v[2]);
    return d;
  }
};

} // namespace zdock


#include <boost/graph/distributed/rmat_graph_generator.hpp>


#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <dart/math/Geometry.hpp>
#include <gtest/gtest.h>

#include <aikido/common/RNG.hpp>
#include <aikido/common/memory.hpp>
#include <aikido/constraint/Differentiable.hpp>
#include <aikido/constraint/dart/TSR.hpp>
#include <aikido/statespace/SE3.hpp>

using aikido::common::RNG;
using aikido::common::RNGWrapper;
using aikido::constraint::ConstraintType;
using aikido::constraint::dart::TSR;
using aikido::statespace::SE3;

using DefaultRNG = RNGWrapper<std::default_random_engine>;

TEST(TSR, InitializesToIdentity)
{
  TSR tsr;
  TSR tsr_default;

  EXPECT_EQ(
      tsr.mT0_w.translation(), Eigen::Isometry3d::Identity().translation());
  EXPECT_EQ(tsr.mT0_w.rotation(), Eigen::Isometry3d::Identity().rotation());
  EXPECT_EQ(tsr.mBw, Eigen::MatrixXd::Zero(6, 2));
  EXPECT_EQ(
      tsr.mTw_e.translation(), Eigen::Isometry3d::Identity().translation());
  EXPECT_EQ(tsr.mTw_e.rotation(), Eigen::Isometry3d::Identity().rotation());

  EXPECT_EQ(
      tsr_default.mT0_w.translation(),
      Eigen::Isometry3d::Identity().translation());
  EXPECT_EQ(
      tsr_default.mT0_w.rotation(), Eigen::Isometry3d::Identity().rotation());
  EXPECT_EQ(tsr_default.mBw, Eigen::MatrixXd::Zero(6, 2));
  EXPECT_EQ(
      tsr_default.mTw_e.translation(),
      Eigen::Isometry3d::Identity().translation());
  EXPECT_EQ(
      tsr_default.mTw_e.rotation(), Eigen::Isometry3d::Identity().rotation());
}

TEST(TSR, CopyConstructor)
{
  TSR tsr;
  TSR tsr2(tsr);

  EXPECT_EQ(
      tsr2.mT0_w.translation(), Eigen::Isometry3d::Identity().translation());
  EXPECT_EQ(tsr2.mT0_w.rotation(), Eigen::Isometry3d::Identity().rotation());
  EXPECT_EQ(tsr2.mBw, Eigen::MatrixXd::Zero(6, 2));
  EXPECT_EQ(
      tsr2.mTw_e.translation(), Eigen::Isometry3d::Identity().translation());
  EXPECT_EQ(tsr2.mTw_e.rotation(), Eigen::Isometry3d::Identity().rotation());

  tsr2.mBw(0, 0) = 3;
  EXPECT_THROW(tsr2.validate(), std::logic_error);
  EXPECT_NO_THROW(tsr.validate());
}

TEST(TSR, AssignmentOperator)
{
  TSR tsr;
  TSR tsr2 = tsr;

  EXPECT_EQ(
      tsr2.mT0_w.translation(), Eigen::Isometry3d::Identity().translation());
  EXPECT_EQ(tsr2.mT0_w.rotation(), Eigen::Isometry3d::Identity().rotation());
  ;
  EXPECT_EQ(tsr2.mBw, Eigen::MatrixXd::Zero(6, 2));
  EXPECT_EQ(
      tsr2.mTw_e.translation(), Eigen::Isometry3d::Identity().translation());
  EXPECT_EQ(tsr2.mTw_e.rotation(), Eigen::Isometry3d::Identity().rotation());

  tsr2.mBw(0, 0) = 3;

  EXPECT_THROW(tsr2.validate(), std::logic_error);
  EXPECT_NO_THROW(tsr.validate());
}

TEST(TSR, Validate)
{
  TSR tsr;

  tsr.mBw = Eigen::Matrix<double, 6, 2>::Zero();
  tsr.mBw(3, 0) = M_PI / 2;
  tsr.mBw(3, 1) = -M_PI / 2;

  EXPECT_THROW(tsr.validate(), std::logic_error);

  Eigen::MatrixXd Bw = Eigen::MatrixXd::Zero(6, 2);
  Bw(3, 0) = M_PI;
  Bw(3, 1) = 2 * M_PI;
  tsr.mBw = Bw;

  EXPECT_NO_THROW(tsr.validate());
}

TEST(TSRSampleGenerator, SampleIdentity)
{
  TSR tsr;
  auto state = tsr.getSE3()->createState();

  ASSERT_TRUE(tsr.createSampleGenerator()->sample(state));
  ASSERT_TRUE(state.getIsometry().isApprox(Eigen::Isometry3d::Identity()));
}

TEST(TSRSampleGenerator, SamplePointTSR)
{
  TSR tsr;

  Eigen::Matrix3d rotation;
  rotation = Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ())
             * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY())
             * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX());
  Eigen::Isometry3d T0_w;
  T0_w.setIdentity();
  T0_w.linear() = rotation;
  tsr.mT0_w = T0_w;
  tsr.validate();

  auto state = tsr.getSE3()->createState();
  auto generator = tsr.createSampleGenerator();

  ASSERT_TRUE(generator->canSample());
  ASSERT_EQ(1, generator->getNumSamples());
  ASSERT_TRUE(generator->sample(state));
  EXPECT_TRUE(state.getIsometry().isApprox(T0_w));

  ASSERT_FALSE(generator->canSample());
  ASSERT_EQ(0, generator->getNumSamples());
}

TEST(TSRSampleGenerator, SampleWithinBounds)
{
  TSR tsr;

  Eigen::MatrixXd Bw = Eigen::Matrix<double, 6, 2>::Zero();
  Bw(0, 0) = -1;
  Bw(0, 1) = 1;

  tsr.mBw = Bw;

  auto sampler = tsr.createSampleGenerator();
  auto state = tsr.getSE3()->createState();

  for (int i = 0; i < 10; i++)
  {
    ASSERT_TRUE(sampler->sample(state));

    Eigen::Vector3d translation = state.getIsometry().translation();
    EXPECT_TRUE(translation(0) >= -1 && translation(1) <= 1);
    EXPECT_DOUBLE_EQ(translation(1), 0);
    EXPECT_DOUBLE_EQ(translation(2), 0);
  }
}

TEST(TSRSampleGenerator, SampleSameSequence)
{
  TSR tsr;

  Eigen::MatrixXd Bw = Eigen::Matrix<double, 6, 2>::Zero();
  Bw(0, 0) = -1;
  Bw(0, 1) = 1;

  tsr.mBw = Bw;

  auto sampler1 = tsr.createSampleGenerator();
  auto sampler2 = tsr.createSampleGenerator();

  auto state1 = tsr.getSE3()->createState();
  auto state2 = tsr.getSE3()->createState();

  for (int i = 0; i < 10; i++)
  {
    ASSERT_TRUE(sampler1->sample(state1));
    ASSERT_TRUE(sampler2->sample(state2));
    EXPECT_TRUE(state1.getIsometry().isApprox(state2.getIsometry()));
  }
}

TEST(TSR, GetValue)
{
  TSR tsr;

  // non-trivial translation bounds
  Eigen::MatrixXd Bw = Eigen::Matrix<double, 6, 2>::Zero();
  Bw(0, 0) = 0;
  Bw(0, 1) = 2;

  tsr.mBw = Bw;

  auto state = tsr.getSE3()->createState();

  // strictly inside TSR
  Eigen::Isometry3d isometry(Eigen::Isometry3d::Identity());
  isometry.translation() = Eigen::Vector3d(1.5, 0, 0);
  state.setIsometry(isometry);
  Eigen::VectorXd value;
  tsr.getValue(state, value);
  EXPECT_TRUE(value.isApproxToConstant(0, 1e-3));

  // strictly inside TSR
  isometry.translation() = Eigen::Vector3d(1, 0, 0);
  state.setIsometry(isometry);
  tsr.getValue(state, value);
  EXPECT_TRUE(value.isApproxToConstant(0));

  // boundary of TSR
  isometry.translation() = Eigen::Vector3d(2, 0, 0);
  state.setIsometry(isometry);
  tsr.getValue(state, value);
  EXPECT_TRUE(value.isApproxToConstant(0));

  // outside TSR
  isometry.translation() = Eigen::Vector3d(3, 0, 0);
  state.setIsometry(isometry);

  Eigen::Vector6d expected(Eigen::Vector6d::Zero());
  expected(0) = 1;
  tsr.getValue(state, value);
  EXPECT_TRUE(value.isApprox(expected));

  // TSR non-trivial angle bounds
  Bw = Eigen::Matrix<double, 6, 2>::Zero();
  Bw(3, 0) = 0;
  Bw(3, 1) = M_PI;

  tsr.mBw = Bw;

  // strictly inside TSR
  Eigen::Matrix3d rotation;
  rotation = Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ())
             * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY())
             * Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitX());

  isometry = Eigen::Isometry3d::Identity();
  isometry.linear() = rotation;
  state.setIsometry(isometry);
  tsr.getValue(state, value);

  EXPECT_TRUE(value.isApproxToConstant(0, 1e-3));

  // [PI/2, PI/2+2PI-0.1]
  Bw(3, 0) = M_PI_2;
  Bw(3, 1) = M_PI_2 + M_PI * 2 - 0.1;
  tsr.mBw = Bw;

  rotation = Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ())
             * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY())
             * Eigen::AngleAxisd(M_PI_4, Eigen::Vector3d::UnitX());
  isometry.linear() = rotation;
  state.setIsometry(isometry);
  tsr.getValue(state, value);
  expected = Eigen::Vector6d::Zero();
  EXPECT_TRUE(value.isApproxToConstant(0, 1e-3));

  /* cyclic bound */
  Bw(3, 0) = M_PI_2;
  Bw(3, 1) = M_PI_2 + M_PI * 2;
  tsr.mBw = Bw;

  rotation = Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ())
             * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY())
             * Eigen::AngleAxisd(M_PI_4, Eigen::Vector3d::UnitX());
  isometry.linear() = rotation;
  state.setIsometry(isometry);
  tsr.getValue(state, value);
  expected = Eigen::Vector6d::Zero();
  EXPECT_TRUE(value.isApproxToConstant(0, 1e-3));

  // boundary of TSR
  rotation = Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ())
             * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY())
             * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX());

  isometry.linear() = rotation;
  state.setIsometry(isometry);
  tsr.getValue(state, value);
  EXPECT_TRUE(value.isApproxToConstant(0));

  /* outside TSR */
  // [PI/2, PI] bound
  Bw(3, 0) = M_PI_2;
  Bw(3, 1) = M_PI;
  tsr.mBw = Bw;

  rotation = Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ())
             * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY())
             * Eigen::AngleAxisd(M_PI_2 - 0.1, Eigen::Vector3d::UnitX());
  isometry.linear() = rotation;
  state.setIsometry(isometry);
  tsr.getValue(state, value);
  expected = Eigen::Vector6d::Zero();
  expected(3) = 0.1;
  EXPECT_TRUE(value.isApprox(expected));

  // Same bound, but with 5*2*pi added to each end.
  Bw(3, 0) = M_PI_2 + 5 * (M_PI * 2);
  Bw(3, 1) = M_PI + 5 * (M_PI * 2);

  tsr.mBw = Bw;

  rotation = Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ())
             * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY())
             * Eigen::AngleAxisd(M_PI_2 - 0.1, Eigen::Vector3d::UnitX());
  isometry.linear() = rotation;
  state.setIsometry(isometry);
  tsr.getValue(state, value);
  expected = Eigen::Vector6d::Zero();
  expected(3) = 0.1;
  EXPECT_TRUE(value.isApprox(expected));

  // [PI/2, 2*PI]
  Bw(3, 0) = M_PI_2;
  Bw(3, 1) = M_PI * 2;
  tsr.mBw = Bw;

  rotation = Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ())
             * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY())
             * Eigen::AngleAxisd(M_PI_4 - 0.1, Eigen::Vector3d::UnitX());
  isometry.linear() = rotation;
  state.setIsometry(isometry);
  tsr.getValue(state, value);
  expected = Eigen::Vector6d::Zero();
  expected(3) = M_PI_4 - 0.1;
  EXPECT_TRUE(value.isApprox(expected, 1e-3));
}

TEST(TSR, GetJacobian)
{
  TSR tsr;

  Eigen::MatrixXd Bw = Eigen::Matrix<double, 6, 2>::Zero();
  Bw(0, 0) = 0;
  Bw(0, 1) = 2;

  tsr.mBw = Bw;

  auto state = tsr.getSE3()->createState();

  // strictly inside TSR
  Eigen::Isometry3d isometry(Eigen::Isometry3d::Identity());
  isometry.translation() = Eigen::Vector3d(1.5, 0, 0);
  state.setIsometry(isometry);

  Eigen::MatrixXd jacobian;
  tsr.getJacobian(state, jacobian);
  EXPECT_TRUE(jacobian.isApproxToConstant(0));

  // outside TSR
  isometry.translation() = Eigen::Vector3d(3, 0, 0);
  state.setIsometry(isometry);
  tsr.getJacobian(state, jacobian);

  Eigen::Matrix6d expected(Eigen::Matrix6d::Zero());
  expected(0, 3) = 1;
  EXPECT_TRUE(jacobian.isApprox(expected, 1e-3));
}

TEST(TSR, GetValueAndJacobian)
{
  TSR tsr;

  Eigen::MatrixXd Bw = Eigen::Matrix<double, 6, 2>::Zero();
  Bw(0, 0) = 0;
  Bw(0, 1) = 2;

  tsr.mBw = Bw;

  auto state = tsr.getSE3()->createState();

  // strictly inside TSR
  Eigen::Isometry3d isometry(Eigen::Isometry3d::Identity());
  isometry.translation() = Eigen::Vector3d(1.5, 0, 0);
  state.setIsometry(isometry);

  Eigen::VectorXd valExpected, val;
  Eigen::MatrixXd jacExpected, jac;
  std::pair<Eigen::VectorXd, Eigen::MatrixXd> valueAndJacobian;
  tsr.getValue(state, valExpected);
  tsr.getJacobian(state, jacExpected);
  tsr.getValueAndJacobian(state, val, jac);

  EXPECT_TRUE(valExpected.isApprox(val));
  EXPECT_TRUE(jacExpected.isApprox(jac));
}

TEST(TSR, GetConstraintTypes)
{
  // This tests current behavior, but it may fail.
  TSR tsr;
  auto types = tsr.getConstraintTypes();

  EXPECT_EQ(6, types.size());

  for (auto t : types)
  {
    EXPECT_EQ(t, ConstraintType::INEQUALITY);
  }
}

TEST(TSR, GetConstraintDimension)
{
  TSR tsr;
  auto dim = tsr.getConstraintDimension();

  EXPECT_EQ(6, dim);
}

TEST(TSR, IsSatisfied)
{
  TSR tsr;

  Eigen::MatrixXd Bw = Eigen::Matrix<double, 6, 2>::Zero();
  Bw(0, 0) = 0;
  Bw(0, 1) = 2;

  tsr.mBw = Bw;

  auto state = tsr.getSE3()->createState();

  // strictly inside TSR
  Eigen::Isometry3d isometry(Eigen::Isometry3d::Identity());
  isometry.translation() = Eigen::Vector3d(1.5, 0, 0);
  state.setIsometry(isometry);

  EXPECT_TRUE(tsr.isSatisfied(state));

  // outside TSR
  isometry.translation() = Eigen::Vector3d(3, 0, 0);
  state.setIsometry(isometry);

  EXPECT_FALSE(tsr.isSatisfied(state));
}

TEST(TSR, getSE3EqualToGetStateSpace)
{
  TSR tsr;
  auto se3space = tsr.getSE3();
  auto space = tsr.getStateSpace();

  EXPECT_EQ(se3space, space);
}


//==============================================================================
//         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 BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_COMMON_IROUND_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_COMMON_IROUND_HPP_INCLUDED
#include <boost/simd/arithmetic/functions/iround.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/include/functions/simd/toint.hpp>
#include <boost/simd/include/constants/valmin.hpp>
#include <boost/simd/include/constants/valmax.hpp>
#include <boost/simd/include/functions/splat.hpp>
#include <boost/simd/include/functions/is_less.hpp>
#include <boost/simd/include/functions/is_greater.hpp>
#include <boost/simd/include/functions/round.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::iround_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<arithmetic_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1) { return a0; }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::iround_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<floating_<A0>,X>))
                            )
  {
    typedef typename dispatch::meta::as_integer<A0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
      {
        typedef typename meta::scalar_of<result_type>::type sr_type;
        A0 tmp = round(a0);
        const A0 vx = splat<A0>(Valmax<sr_type>());
        const A0 vn = splat<A0>(Valmin<sr_type>());
        return if_else(gt(tmp, vx), Valmax<result_type>(),
                       if_else(lt(tmp, vn), Valmin<result_type>(),
                               toint(tmp)));
      }
  };
} } }

#endif


/* Copyright 2022 Zuru Tech HK Limited.
 *
 * 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 <cstring>
#include <stdexcept>

#include <Eigen/Dense>

#include <petscksp.h>

#include <solvers/PetscSolver.hpp>
#include <solvers/SparseSystem.hpp>

namespace solvers {

PetscSolver::PetscSolver(const PetscMethod method,
                         const PetscPreconditioner preconditioner)
{
    KSPCreate(PETSC_COMM_SELF, &_ksp);
    KSPType ksp_type;
    PCType pc_type = "";
    switch (method) {
        case PetscMethod::CG:
            ksp_type = KSPCG;
            break;
        case PetscMethod::FlexibleCG:
            ksp_type = KSPFCG;
            break;
        case PetscMethod::GMRES:
            ksp_type = KSPGMRES;
            break;
        case PetscMethod::FlexibleGMRES:
            ksp_type = KSPFGMRES;
            break;
        case PetscMethod::QMR:
            ksp_type = KSPTCQMR;
            break;
        case PetscMethod::CR:
            ksp_type = KSPCR;
            break;
        case PetscMethod::MinRes:
            ksp_type = KSPMINRES;
            break;
        case PetscMethod::SymmLQ:
            ksp_type = KSPSYMMLQ;
            break;
        case PetscMethod::Cholesky:
            ksp_type = KSPPREONLY;
            pc_type = PCCHOLESKY;
            break;
        case PetscMethod::LU:
            ksp_type = KSPPREONLY;
            pc_type = PCLU;
            break;
        case PetscMethod::QR:
            ksp_type = KSPPREONLY;
            pc_type = PCQR;
            break;
        default:
            throw std::logic_error("Invalid solving method");
            break;
    }
    KSPSetType(_ksp, ksp_type);
    PC pc;
    KSPGetPC(_ksp, &pc);
    if (strlen(pc_type) != 0) {
        if (preconditioner != PetscPreconditioner::None) {
            throw std::logic_error(
                "The preconditioner cannot be used with direct methods");
        }
    }
    else {
        switch (preconditioner) {
            case PetscPreconditioner::None:
                pc_type = PCNONE;
                break;
            case PetscPreconditioner::Jacobi:
                pc_type = PCJACOBI;
                break;
            case PetscPreconditioner::SOR:
                pc_type = PCSOR;
                break;
            case PetscPreconditioner::Eisenstat:
                pc_type = PCEISENSTAT;
                break;
            case PetscPreconditioner::ILU:
                pc_type = PCILU;
                break;
            case PetscPreconditioner::ICC:
                pc_type = PCICC;
                break;
            default:
                throw std::logic_error("Invalid preconditioner");
                break;
        }
    }
    PCSetType(pc, pc_type);
    KSPSetFromOptions(_ksp);
}

Eigen::VectorXd PetscSolver::solve(const SparseSystem& system,
                                   double& duration) const
{
    auto [A, b] = system.toPetscCSR();
    KSPReset(_ksp);
    KSPSetOperators(_ksp, A, A);
    double start;
    double end;
    PetscTime(&start);
    KSPSolve(_ksp, b, b);
    PetscTime(&end);
    double* result_data;
    VecGetArray(b, &result_data);
    Eigen::VectorXd result = Eigen::Map<Eigen::VectorXd>(
        result_data, static_cast<int64_t>(system.dim()));
    duration = end - start;
    return result;
}

}    // namespace solvers


#include "hoCuNDArray_utils.h"
#include "radial_utilities.h"
#include "cuNDArray_fileio.h"
#include "cuNDArray_math.h"
#include "imageOperator.h"
#include "identityOperator.h"
#include "hoPartialDerivativeOperator.h"
#include "hoCuConebeamProjectionOperator.h"
#include "cuConvolutionOperator.h"
#include "hoCuNDArray_blas.h"
#include "hoCuNDArray_elemwise.h"
#include "hoCuNDArray_blas.h"
#include "cgSolver.h"
#include "CBCT_acquisition.h"
#include "complext.h"
#include "encodingOperatorContainer.h"
#include "vector_td_io.h"
#include "hoPartialDerivativeOperator.h"
#include "hoCuGPBBSolver.h"
#include "hoCuTvOperator.h"
#include "hoCuTvPicsOperator.h"
#include "hoCuNCGSolver.h"
#include "hoCuCgDescentSolver.h"
#include "hoNDArray_utils.h"
#include "hoCuPartialDerivativeOperator.h"
#include "cuTvOperator.h"
#include "cuTv1dOperator.h"
#include "cuTvPicsOperator.h"
#include "CBSubsetOperator.h"
#include "osSPSSolver.h"
#include "osMOMSolver.h"
#include "osMOMSolverD.h"
#include "osMOMSolverD2.h"
#include "hoCuNCGSolver.h"
#include "osMOMSolverD3.h"
#include "osMOMSolverL1.h"
#include "osMOMSolverF.h"
#include "osAHZCSolver.h"
#include "hoCuOSNESTSolver.h"
#include "ADMMSolver.h"
#include <iostream>
#include <algorithm>
#include <sstream>
#include <math_constants.h>
#include <boost/program_options.hpp>
#include <boost/make_shared.hpp>
#include <GPUTimer.h>
#include <operators/cuGaussianFilterOperator.h>
#include <multiplicationOperatorContainer.h>
#include "cuSolverUtils.h"
#include "osPDsolver.h"
#include "osLALMSolver.h"
#include "osLALMSolver2.h"
#include "cuATrousOperator.h"
#include "hdf5_utils.h"
#include "cuEdgeATrousOperator.h"
#include "cuDCTOperator.h"
#include "cuDCTDerivativeOperator.h"
#include "dicomWriter.h"
#include "conebeam_projection.h"
#include "weightingOperator.h"
#include "hoNDArray_math.h"
#include "cuNCGSolver.h"
#include "CT_acquisition.h"
using namespace std;
using namespace Gadgetron;

namespace po = boost::program_options;

boost::shared_ptr<hoCuNDArray<float>> downsample_projections(hoCuNDArray<float>* projections, unsigned int num_downsamples )
{

	if (num_downsamples == 0) return boost::make_shared<hoCuNDArray<float>>(*projections);

	auto tmp = Gadgetron::downsample<float,2>(projections);

	for (int k = 1; k < num_downsamples; k++)
		tmp = Gadgetron::downsample<float,2>(tmp.get());

	return boost::make_shared<hoCuNDArray<float>>(*tmp);
}

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

	string acquisition_filename;
	string outputFile;
	uintd3 imageSize;
	floatd3 voxelSize;
	int device;
	unsigned int downsamples;
	unsigned int iterations;
	unsigned int subsets;
	float rho,tau;
	float tv_weight,pics_weight, wavelet_weight,huber,sigma,dct_weight;
	float tv_4d,atv_4d;
    bool use_non_negativity;
	int reg_iter;

	po::options_description desc("Allowed options");

	desc.add_options()
    				("help", "produce help message")
    				("acquisition,a", po::value<string>(&acquisition_filename)->default_value("acquisition.hdf5"), "Acquisition data")
    				("samples,n",po::value<unsigned int>(),"Number of samples per ray")
    				("output,f", po::value<string>(&outputFile)->default_value("reconstruction.hdf5"), "Output filename")
    				("size,s",po::value<uintd3>(&imageSize)->default_value(uintd3(512,512,1)),"Image size in pixels")
    				("binning,b",po::value<string>(),"Binning file for 4d reconstruction")
    				("SAG","Use exact SAG correction if present")
    				("voxelSize,v",po::value<floatd3>(&voxelSize)->default_value(floatd3(0.488f,0.488f,1.0f)),"Voxel size in mm")
    				("dimensions,d",po::value<floatd3>(),"Image dimensions in mm. Overwrites voxelSize.")
    				("iterations,i",po::value<unsigned int>(&iterations)->default_value(10),"Number of iterations")
    				("device",po::value<int>(&device)->default_value(0),"Number of the device to use (0 indexed)")
    				("downsample,D",po::value<unsigned int>(&downsamples)->default_value(0),"Downsample projections this factor")
    				("subsets,u",po::value<unsigned int>(&subsets)->default_value(10),"Number of subsets to use")
    				("TV",po::value<float>(&tv_weight)->default_value(0),"Total variation weight in spatial dimensions")
							("TV4D",po::value<float>(&tv_4d)->default_value(0),"Total variation weight in temporal dimensions")
							("ATV4D",po::value<float>(&atv_4d)->default_value(0),"Advanced Total variation weight in temporal dimensions")
    				("PICS",po::value<float>(&pics_weight)->default_value(0),"PICS weight")
    				("Wavelet,W",po::value<float>(&wavelet_weight)->default_value(0),"Weight of the wavelet operator")
    				("Huber",po::value<float>(&huber)->default_value(0),"Huber weight")

    				("3D","Only use binning for selecting valid projections")
							("tau",po::value<float>(&tau)->default_value(1e-5),"Tau value for solver")
							("reg_iter",po::value<int>(&reg_iter)->default_value(2))
    				;

	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, desc), vm);
	po::notify(vm);

	if (vm.count("help")) {
		cout << desc << "\n";
		return 1;
	}

	std::stringstream command_line_string;
	std::cout << "Command line options:" << std::endl;
	for (po::variables_map::iterator it = vm.begin(); it != vm.end(); ++it){
		boost::any a = it->second.value();
		command_line_string << it->first << ": ";
		if (a.type() == typeid(std::string)) command_line_string << it->second.as<std::string>();
		else if (a.type() == typeid(int)) command_line_string << it->second.as<int>();
		else if (a.type() == typeid(unsigned int)) command_line_string << it->second.as<unsigned int>();
		else if (a.type() == typeid(float)) command_line_string << it->second.as<float>();
		else if (a.type() == typeid(vector_td<float,3>)) command_line_string << it->second.as<vector_td<float,3> >();
		else if (a.type() == typeid(vector_td<int,3>)) command_line_string << it->second.as<vector_td<int,3> >();
		else if (a.type() == typeid(vector_td<unsigned int,3>)) command_line_string << it->second.as<vector_td<unsigned int,3> >();
        else if (a.type() == typeid(bool)) command_line_string << it->second.as<bool>();
		else command_line_string << "Unknown type" << std::endl;
		command_line_string << std::endl;
	}
	std::cout << command_line_string.str();

	cudaSetDevice(device);
	cudaDeviceReset();

	//Really weird stuff. Needed to initialize the device?? Should find real bug.
	cudaDeviceManager::Instance()->lockHandle();
	cudaDeviceManager::Instance()->unlockHandle();

	boost::shared_ptr<CBCT_acquisition> ps(new CBCT_acquisition());
	ps->load(acquisition_filename);
	ps->get_geometry()->print(std::cout);

	float SDD = ps->get_geometry()->get_SDD();
	float SAD = ps->get_geometry()->get_SAD();

	boost::shared_ptr<CBCT_binning> binning(new CBCT_binning());
	if (vm.count("binning")){
		std::cout << "Loading binning data" << std::endl;
		binning->load(vm["binning"].as<string>());
		if (vm.count("3D"))
			binning = binning->get_3d_binning();
	} else binning->set_as_default_3d_bin(ps->get_projections()->get_size(2));
	binning->print(std::cout);

	floatd3 imageDimensions;
	if (vm.count("dimensions")){
		imageDimensions = vm["dimensions"].as<floatd3>();
		voxelSize = imageDimensions/imageSize;
	}
	else imageDimensions = voxelSize*imageSize;

	float lengthOfRay_in_mm = norm(imageDimensions);
	unsigned int numSamplesPerPixel = 3;
	float minSpacing = min(voxelSize)/numSamplesPerPixel;

	unsigned int numSamplesPerRay;
	if (vm.count("samples")) numSamplesPerRay = vm["samples"].as<unsigned int>();
	else numSamplesPerRay = ceil( lengthOfRay_in_mm / minSpacing );

	float step_size_in_mm = lengthOfRay_in_mm / numSamplesPerRay;
	size_t numProjs = ps->get_projections()->get_size(2);
	size_t needed_bytes = 2 * prod(imageSize) * sizeof(float);
	std::vector<size_t> is_dims = to_std_vector((uint64d3)imageSize);

	std::cout << "IS dimensions " << is_dims[0] << " " << is_dims[1] << " " << is_dims[2] << std::endl;
	std::cout << "Image size " << imageDimensions << std::endl;

	is_dims.push_back(binning->get_number_of_bins());

	//scatter_correct(binning,ps,ps->get_projections().get(),is_dims,imageDimensions);

	ps->downsample(downsamples);

	//osLALMSolver<cuNDArray<float>> solver;
	hoCuOSNESTSolver<float> solver;
	//osMOMSolverL1<cuNDArray<float>> solver;
	//osAHZCSolver<cuNDArray<float>> solver;
	//osMOMSolverF<cuNDArray<float>> solver;
	//ADMMSolver<cuNDArray<float>> solver;
	solver.set_dump(false);

	solver.set_max_iterations(iterations);
	solver.set_output_mode(osSPSSolver<cuNDArray<float>>::OUTPUT_VERBOSE);
	solver.set_tau(tau);
	solver.set_non_negativity_constraint(use_non_negativity);
	solver.set_huber(huber);
	solver.set_reg_steps(reg_iter);
	//solver.set_rho(rho);
	solver.set_beta(1e-6);
  if (tv_weight > 0) {

	  auto Dx = boost::make_shared<cuPartialDerivativeOperator<float, 4>>(0);
	  Dx->set_weight(tv_weight);
	  Dx->set_domain_dimensions(&is_dims);
	  Dx->set_codomain_dimensions(&is_dims);

	  auto Dy = boost::make_shared<cuPartialDerivativeOperator<float, 4>>(1);
	  Dy->set_weight(tv_weight);
	  Dy->set_domain_dimensions(&is_dims);
	  Dy->set_codomain_dimensions(&is_dims);

	  auto Dz = boost::make_shared<cuPartialDerivativeOperator<float, 4>>(2);
	  Dz->set_weight(tv_weight);
	  Dz->set_domain_dimensions(&is_dims);
	  Dz->set_codomain_dimensions(&is_dims);

	  solver.add_regularization_group({Dx, Dy, Dz});

	  if (tv_4d > 0) {
		  auto Dt = boost::make_shared<cuPartialDerivativeOperator<float, 4>>(3);
		  Dt->set_weight(tv_4d);
		  Dt->set_domain_dimensions(&is_dims);
		  Dt->set_codomain_dimensions(&is_dims);
		  solver.add_regularization_operator(Dt);
	  }
  }

      if (atv_4d > 0) {
          auto Dt = boost::make_shared<cuPartialDerivativeOperator<float, 4>>(3);
          Dt->set_domain_dimensions(&is_dims);
          Dt->set_codomain_dimensions(&is_dims);

          auto Dx = boost::make_shared<cuPartialDerivativeOperator<float,4>>(0);
          Dx->set_domain_dimensions(&is_dims);
          Dx->set_codomain_dimensions(&is_dims);

          auto Dy = boost::make_shared<cuPartialDerivativeOperator<float, 4>>(1);
          Dy->set_domain_dimensions(&is_dims);
          Dy->set_codomain_dimensions(&is_dims);

          auto Dz = boost::make_shared<cuPartialDerivativeOperator<float, 4>>(2);
          Dz->set_domain_dimensions(&is_dims);
          Dz->set_codomain_dimensions(&is_dims);

          auto Dx2 = boost::make_shared<multiplicationOperatorContainer<cuNDArray<float>>>();
          Dx2->add_operator(Dx);
          Dx2->add_operator(Dt);
          Dx2->set_weight(atv_4d);

          auto Dy2 = boost::make_shared<multiplicationOperatorContainer<cuNDArray<float>>>();
          Dy2->add_operator(Dy);
          Dy2->add_operator(Dt);
          Dy->set_weight(atv_4d);

          auto Dz2 = boost::make_shared<multiplicationOperatorContainer<cuNDArray<float>>>();
          Dz2->add_operator(Dz);
          Dz2->add_operator(Dt);
          Dz2->set_weight(atv_4d);

          solver.add_regularization_group({Dx2, Dy2, Dz2});

      }

	auto E = boost::make_shared<CBSubsetOperator<cuNDArray> >(subsets);

	//E->setup(ps,binning,imageDimensions);
	E->setup(ps,binning,imageDimensions);
	E->set_domain_dimensions(&is_dims);
	E->set_codomain_dimensions(ps->get_projections()->get_dimensions().get());

	solver.set_encoding_operator(E);

	auto projections = ps->get_projections();
	std::cout << "Projection norm:" << nrm2(projections.get()) << std::endl;
	//E->set_use_offset_correction(false);

	//boost::shared_ptr<cuNDArray<bool>> mask;
	//mask = E->calculate_mask(projections,0.03f);
	//ps->set_projections(boost::shared_ptr<hoCuNDArray<float>>()); //Clear projections from host memory.

/*
	{
		cuNDArray<float> cu_proj(*projections);
		E->offset_correct(&cu_proj);
		*projections = cu_proj;
	}

	{
		cuNDArray<float> cu_air(*ps->get_airscan());
		E->offset_correct(&cu_air);
		*ps->get_airscan() = cu_air;
	}
*/

	//E->set_mask(mask);
	std::cout << "Projection norm:" << nrm2(projections.get()) << std::endl;

	//solver.set_damping(1e-6);

	/*
    boost::shared_ptr<hoCuNDArray<float> > prior;

  if (vm.count("use_prior")) {
  	prior = calculate_prior(binning,ps,projections,is_dims,imageDimensions);
  	solver.set_x0(prior);
  }
	 */
	auto airscan = downsample_projections(ps->get_airscan().get(),downsamples);

	boost::shared_ptr<hoCuNDArray<float>> result;
	{
		GPUTimer tim("Solver");
		result = solver.solve(projections.get(),airscan.get());
	}
//	global_timer.reset();
	std::cout << "Penguin" << nrm2(result.get()) << std::endl;

	std::cout << "Result sum " << asum(result.get()) << std::endl;

	//apply_mask(result.get(),mask.get());

	std::cout << "Result sum " << asum(result.get()) << std::endl;
	//saveNDArray2HDF5(result.get(),outputFile,imageDimensions,vector_td<float,3>(0),command_line_string.str(),iterations);

	saveNDArray2HDF5(result.get(),outputFile,imageDimensions,floatd3(0,0,0),command_line_string.str(),iterations);
//	write_nd_array(result.get(),"reconstruction.real");
	write_dicom(result.get(),command_line_string.str(),imageDimensions);

}