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

#include <boost/python/class.hpp>
#include <boost/python/args.hpp>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/return_by_value.hpp>
#include <boost/python/copy_const_reference.hpp>

#include <scitbx/math/chebyshev.h>

namespace scitbx { namespace math {

namespace {

  struct chebyshev_base_wrappers
  {
    typedef chebyshev::chebyshev_base<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;

      class_<w_t>("chebyshev_base", no_init)
        .def(init<std::size_t const&,
                  double const&,
                  double const&>((arg("n_terms"),
                                  arg("low_limit"),
                                  arg("high_limit") )))
        .def(init<std::size_t const& ,
                  double const&,
                  double const&,
                  scitbx::af::const_ref<double> const&>
             ((arg("n_terms"),
               arg("low_limit"),
               arg("high_limit"),
               arg("cheb_coefs") )))
        .def("f", (double(w_t::*)(double const&)) &w_t::f)
        .def("f", (scitbx::af::shared<double>(w_t::*)
                   (scitbx::af::const_ref<double> const&))
                    &w_t::f )
        .def("coefs", &w_t::coefs )
        .def("replace", &w_t::replace )
        ;

    }

  };

  struct chebyshev_polynome_wrappers
  {
    typedef chebyshev::chebyshev_polynome<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;

      class_<w_t>("chebyshev_polynome", no_init)
        .def(init<std::size_t const& ,
                  double const&,
                  double const&,
                  scitbx::af::const_ref<double> const&>
             ((arg("n_terms"),
               arg("low_limit"),
               arg("high_limit"),
               arg("cheb_coefs") )))
        .def("f", (double(w_t::*)(double const&)) &w_t::f)
        .def("f", (scitbx::af::shared<double>(w_t::*)
                   (scitbx::af::const_ref<double> const&))
                    &w_t::f )
        .def("coefs", &w_t::coefs )

        .def("dfdx", (double(w_t::*)(double const&)) &w_t::dfdx)
        .def("dfdx", (scitbx::af::shared<double>(w_t::*)
                      (scitbx::af::const_ref<double> const&))
             &w_t::dfdx )
        .def("dfdx_coefs", &w_t::dfdx_coefs)
        ;

    }

  };

  struct chebyshev_fitter_wrappers
  {
    typedef chebyshev::chebyshev_fitter<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;

      class_<w_t>("chebyshev_fitter", no_init)
        .def(init<std::size_t const& ,
                  double const&,
                  double const& >
             ((arg("n_terms"),
               arg("low_limit"),
               arg("high_limit") )))
        .def(init<std::size_t const& ,
                  double const&,
                  double const&,
                  scitbx::af::const_ref<double> const&>
             ((arg("n_terms"),
               arg("low_limit"),
               arg("high_limit"),
               arg("cheb_coefs") )))

        .def("f", (double(w_t::*)(double const&)) &w_t::f)
        .def("f", (scitbx::af::shared<double>(w_t::*)
                   (scitbx::af::const_ref<double> const&))
                    &w_t::f )
        .def("coefs", &w_t::coefs )
        .def("replace", &w_t::replace)
        .def("dfdcoefs", &w_t::dfdcoefs)
        ;

    }

  };

  struct chebyshev_smooth_wrappers
  {
    typedef chebyshev::chebyshev_smooth<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;

      class_<w_t>("chebyshev_smooth", no_init)
        .def(init<std::size_t const&,
                  double const&,
                  double const&>((arg("n_terms"),
                                  arg("low_limit"),
                                  arg("high_limit") )))
        .def(init<std::size_t const& ,
                  double const&,
                  double const&,
                  scitbx::af::const_ref<double> const&>
             ((arg("n_terms"),
               arg("low_limit"),
               arg("high_limit"),
               arg("cheb_coefs") )))
        .def("f", (double(w_t::*)(double const&)) &w_t::f)
        .def("f", (scitbx::af::shared<double>(w_t::*)
                   (scitbx::af::const_ref<double> const&))
                    &w_t::f )
        .def("coefs", &w_t::smooth_coefs )
        .def("replace", &w_t::replace_and_smooth )
        ;

    }

  };

  struct chebyshev_smooth_fitter_wrappers
  {
    typedef chebyshev::chebyshev_smooth_fitter<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;

      class_<w_t>("chebyshev_smooth_fitter", no_init)
        .def(init<std::size_t const&,
                  double const&,
                  double const&>((arg("n_terms"),
                                  arg("low_limit"),
                                  arg("high_limit") )))
        .def(init<std::size_t const& ,
                  double const&,
                  double const&,
                  scitbx::af::const_ref<double> const&>
             ((arg("n_terms"),
               arg("low_limit"),
               arg("high_limit"),
               arg("cheb_coefs") )))
        .def("f", (double(w_t::*)(double const&)) &w_t::f)
        .def("f", (scitbx::af::shared<double>(w_t::*)
                   (scitbx::af::const_ref<double> const&))
                    &w_t::f )
        .def("coefs", &w_t::smooth_coefs )
        .def("replace", &w_t::replace_and_smooth )
        .def("dfdcoefs", &w_t::dfdcoefs)
        ;

    }

  };

  struct chebyshev_lsq_wrappers
  {
    typedef chebyshev::chebyshev_lsq<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;

      class_<w_t>("chebyshev_lsq", no_init)
        .def(init<std::size_t const&,
                  double const&,
                  double const&,
                  scitbx::af::const_ref<double> const&,
                  scitbx::af::const_ref<double> const&,
                  scitbx::af::const_ref<double> const&,
                  scitbx::af::const_ref<bool> const&
                  >((arg("n_terms"),
                     arg("low_limit"),
                     arg("high_limit"),
                     arg("x_obs"),
                     arg("y_obs"),
                     arg("w_obs"),
                     arg("free_flags") )))

        .def("residual", &w_t::residual)
        .def("free_residual", &w_t::free_residual)
        .def("gradient", &w_t::gradient )
        .def("replace", &w_t::replace )
        .def("coefs", &w_t::coefs)
        ;

    }

  };

} // namespace <anonymous>

namespace boost_python {

  void wrap_chebyshev_base()
  {
    chebyshev_base_wrappers::wrap();
  }
  void wrap_chebyshev_polynome()
  {
    chebyshev_polynome_wrappers::wrap();
  }
  void wrap_chebyshev_fitter()
  {
    chebyshev_fitter_wrappers::wrap();
  }

  void wrap_chebyshev_smooth()
  {
    chebyshev_smooth_wrappers::wrap();
  }
  void wrap_chebyshev_smooth_fitter()
  {
    chebyshev_smooth_fitter_wrappers::wrap();
  }

  void wrap_chebyshev_lsq()
  {
    chebyshev_lsq_wrappers::wrap();
  }

}}} // namespace scitbx::math::boost_python


#pragma once
#ifndef DSFMTAVXEQ_HPP
#define DSFMTAVXEQ_HPP
/**
 * @file dSFMTAVXeq.hpp
 */

#include "devavxprng.h"
#include "AlgorithmDSFMTEquidistribution.hpp"
//#include <MTToolBox/AlgorithmEquidistribution.hpp>
//#include <MTToolBox/AlgorithmReducibleRecursionSearch.hpp>
//#include <MTToolBox/period.hpp>
//#include <NTL/GF2X.h>
#include "Annihilate.hpp"
#include "EQOptions.hpp"

namespace MTToolBox {

    template<typename U, typename G, typename P, int bitWidth>
    int dsfmtavx_equidistribution(EQOptions<P> opt)
    {
        using namespace std;
        G sf(opt.params);
        U wseed;
        wseed.u64[0] = opt.seed;
        sf.seed(wseed);
        Annihilate<G, U> annihilate;
        if (!annihilate.anni(sf)) {
            return -1;
        }
        int delta52 = 0;
        int veq52[52];
        DSFMTInfo info;
        info.bitSize = bitWidth; // IMPORTANT
        info.elementNo = bitWidth / 64;
        delta52 = calc_dSFMT_equidistribution<U, G>
            (sf, veq52, 52, info, opt.params.mexp);
        cout << sf.getParamString();
        cout << dec << delta52 << endl;
        if (opt.verbose) {
            cout << "52bit dimension of equidistribution at v-bit accuracy k(v)"
                 << endl;
            for (int j = 0; j < 52; j++) {
                cout << "k(" << dec << (j + 1) << ") = " << dec << veq52[j];
                cout << "\td(" << dec << (j + 1) << ") = " << dec
                     << (opt.params.mexp / (j + 1) - veq52[j]) << endl;
            }
        }
        return 0;
    }
}

#endif // DSFMTAVXEQ_HPP


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

  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)
*/
//==================================================================================================
#include <boost/simd/function/scalar/rem_pio2_straight.hpp>
#include <scalar_test.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/constant/pio_4.hpp>
#include <boost/simd/constant/pio_2.hpp>

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

  using bs::rem_pio2_straight;
  using r_t = decltype(rem_pio2_straight(T()));

  STF_EXPR_IS( (rem_pio2_straight(T()))
                  , (std::pair<T,T>)
                  );

  {
    r_t res = rem_pio2_straight(bs::Pio_4<T>());
    STF_ULP_EQUAL( res.first, bs::One<T>(), 0.5);
    STF_ULP_EQUAL( res.second, -bs::Pio_4<T>(), 0.5);
  }
  {
    r_t res = rem_pio2_straight(bs::Zero<T>());
    STF_ULP_EQUAL( res.first, bs::Zero<T>(), 0.5);
    STF_ULP_EQUAL( res.second, bs::Zero<T>(), 0.5);
  }

}


#include <PCE.h>
#include "GeomCommonFunctions.h"
#include <Gui/Application.h>
#include <BRepBuilderAPI_GTransform.hxx>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include<tchar.h>
#include <iostream>
#include "Standard_Type.hxx"
#include <Standard.hxx>
#include <Standard_DefineAlloc.hxx>
#include <Standard_PrimitiveTypes.hxx>
#include <Geom_Circle.hxx>
#include <clipper/clipper.hpp>

using namespace PartDesignGui;

PCEShape::PCEShape()
{

}

PCEShape::PCEShape(const TopoDS_Shape& ashape_)
{
	ashape = ashape_;
	//LoadShapeGeometry(ashape_);
	GeomFunc::LoadTopoDSGeometry(ashape_, surfs, surf_normals,volume, holes, center);
}

PCEShape::PCEShape(const PCEShape& pce_shape)
{
	index = pce_shape.index;
	ashape = pce_shape.ashape;
	surfs = pce_shape.surfs;
	holes = pce_shape.holes;
	surf_normals = pce_shape.surf_normals;
	center = pce_shape.center;
	volume = pce_shape.volume;
	init_LUM = pce_shape.init_LUM;
	WLC_index = pce_shape.WLC_index;
	transforms = pce_shape.transforms;
	LGM = pce_shape.LGM;
}

void PCEShape::Clear()
{
	Vector3d2().swap(surfs);
	Vector3d1().swap(surf_normals);
	std::vector<std::pair<Vector3d1, double>>().swap(holes);
	//part_LUMs.clear();
	//std::vector<glm::dmat4>().swap(LUMs);
	for (auto& transform : transforms) transform.Clear();
	std::vector<PCETransform>().swap(transforms);
}


#ifndef STAN_MATH_PRIM_SCAL_PROB_PARETO_RNG_HPP
#define STAN_MATH_PRIM_SCAL_PROB_PARETO_RNG_HPP

#include <boost/random/exponential_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_greater_or_equal.hpp>
#include <stan/math/prim/scal/err/check_nonnegative.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/err/check_positive_finite.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/fun/multiply_log.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>

namespace stan {
  namespace math {

    template <class RNG>
    inline double
    pareto_rng(double y_min,
               double alpha,
               RNG& rng) {
      using boost::variate_generator;
      using boost::exponential_distribution;

      static const char* function("pareto_rng");

      check_positive_finite(function, "Scale parameter", y_min);
      check_positive_finite(function, "Shape parameter", alpha);

      variate_generator<RNG&, exponential_distribution<> >
        exp_rng(rng, exponential_distribution<>(alpha));
      return y_min * std::exp(exp_rng());
    }

  }
}
#endif


//==============================================================================
//         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_ARITHMETIC_FUNCTIONS_SIMD_COMMON_NEGS_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_NEGS_HPP_INCLUDED
#include <boost/simd/toolbox/arithmetic/functions/negs.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/unary_minus.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::negs_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<signed_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return if_else(eq(a0, boost::simd::Valmin<A0>()), boost::simd::Valmax<A0>(), unary_minus(a0));
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::negs_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<floating_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1) { return unary_minus(a0); }
  };
} } }

#endif


#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <gtest/gtest.h>
#include <Eigen/Geometry>
TESSERACT_COMMON_IGNORE_WARNINGS_POP

#include <tesseract_urdf/origin.h>
#include "tesseract_urdf_common_unit.h"

TEST(TesseractURDFUnit, parse_extra_delimeters)  // NOLINT
{
  {
    std::string str = R"(<origin xyz="0   2.5   0" rpy="3.14159265359   0  0.0"/>)";
    Eigen::Isometry3d origin;
    EXPECT_TRUE(runTest<Eigen::Isometry3d>(origin, &tesseract_urdf::parseOrigin, str, "origin", 2));
    EXPECT_TRUE(origin.translation().isApprox(Eigen::Vector3d(0, 2.5, 0), 1e-8));
    EXPECT_TRUE(origin.matrix().col(0).head(3).isApprox(Eigen::Vector3d(1, 0, 0), 1e-8));
    EXPECT_TRUE(origin.matrix().col(1).head(3).isApprox(Eigen::Vector3d(0, -1, 0), 1e-8));
    EXPECT_TRUE(origin.matrix().col(2).head(3).isApprox(Eigen::Vector3d(0, 0, -1), 1e-8));
  }
}


// Boost.Geometry Index
//
// R-tree strategies
//
// Copyright (c) 2019, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle
//
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_STRATEGIES_CARTESIAN_INDEX_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_INDEX_HPP

#include <boost/geometry/strategies/cartesian/box_in_box.hpp>
#include <boost/geometry/strategies/cartesian/disjoint_box_box.hpp>
//#include <boost/geometry/strategies/cartesian/disjoint_segment_box.hpp>
#include <boost/geometry/strategies/cartesian/distance_projected_point.hpp>
#include <boost/geometry/strategies/cartesian/distance_pythagoras.hpp>
#include <boost/geometry/strategies/cartesian/distance_pythagoras_point_box.hpp>
#include <boost/geometry/strategies/cartesian/distance_segment_box.hpp>
#include <boost/geometry/strategies/cartesian/envelope_box.hpp>
#include <boost/geometry/strategies/cartesian/envelope_point.hpp>
#include <boost/geometry/strategies/cartesian/envelope_segment.hpp>
#include <boost/geometry/strategies/cartesian/expand_box.hpp>
#include <boost/geometry/strategies/cartesian/expand_point.hpp>
#include <boost/geometry/strategies/cartesian/expand_segment.hpp>
#include <boost/geometry/strategies/cartesian/intersection.hpp>
#include <boost/geometry/strategies/cartesian/point_in_box.hpp>
#include <boost/geometry/strategies/cartesian/point_in_point.hpp>
#include <boost/geometry/strategies/cartesian/point_in_poly_winding.hpp>

#include <boost/geometry/strategies/index.hpp>

namespace boost { namespace geometry { namespace strategy { namespace index
{

template
<
    typename CalculationType = void
>
struct cartesian
{
    typedef cartesian_tag cs_tag;

    typedef geometry::strategy::envelope::cartesian_point envelope_point_strategy_type;
    typedef geometry::strategy::envelope::cartesian_box envelope_box_strategy_type;
    typedef geometry::strategy::envelope::cartesian_segment
        <
            CalculationType
        > envelope_segment_strategy_type;

    static inline envelope_segment_strategy_type get_envelope_segment_strategy()
    {
        return envelope_segment_strategy_type();
    }

    typedef geometry::strategy::expand::cartesian_point expand_point_strategy_type;
    typedef geometry::strategy::expand::cartesian_box expand_box_strategy_type;
    typedef geometry::strategy::expand::cartesian_segment expand_segment_strategy_type;

    static inline expand_segment_strategy_type get_expand_segment_strategy()
    {
        return expand_segment_strategy_type();
    }

    typedef geometry::strategy::covered_by::cartesian_point_box covered_by_point_box_strategy_type;
    typedef geometry::strategy::covered_by::cartesian_box_box covered_by_box_box_strategy_type;
    typedef geometry::strategy::within::cartesian_point_point within_point_point_strategy_type;
    /*
    typedef geometry::strategy::within::cartesian_point_box within_point_box_strategy_type;
    typedef geometry::strategy::within::cartesian_box_box within_box_box_strategy_type;
    typedef geometry::strategy::within::cartesian_winding
        <
            void, void, CalculationType
        > within_point_segment_strategy_type;

    static inline within_point_segment_strategy_type get_within_point_segment_strategy()
    {
        return within_point_segment_strategy_type();
    }
    */

    // used in equals(Seg, Seg) but only to get_point_in_point_strategy()
    typedef geometry::strategy::intersection::cartesian_segments
        <
            CalculationType
        > relate_segment_segment_strategy_type;

    static inline relate_segment_segment_strategy_type get_relate_segment_segment_strategy()
    {
        return relate_segment_segment_strategy_type();
    }

    // used in intersection_content
    typedef geometry::strategy::disjoint::cartesian_box_box disjoint_box_box_strategy_type;

    typedef geometry::strategy::distance::comparable::pythagoras
        <
            CalculationType
        > comparable_distance_point_point_strategy_type;

    static inline comparable_distance_point_point_strategy_type get_comparable_distance_point_point_strategy()
    {
        return comparable_distance_point_point_strategy_type();
    }

    typedef geometry::strategy::distance::comparable::pythagoras_point_box
        <
            CalculationType
        > comparable_distance_point_box_strategy_type;

    static inline comparable_distance_point_box_strategy_type get_comparable_distance_point_box_strategy()
    {
        return comparable_distance_point_box_strategy_type();
    }

    // TODO: comparable version should be possible
    typedef geometry::strategy::distance::projected_point
        <
            CalculationType,
            geometry::strategy::distance::pythagoras<CalculationType>
        > comparable_distance_point_segment_strategy_type;

    static inline comparable_distance_point_segment_strategy_type get_comparable_distance_point_segment_strategy()
    {
        return comparable_distance_point_segment_strategy_type();
    }

    typedef geometry::strategy::distance::cartesian_segment_box
        <
            CalculationType,
            geometry::strategy::distance::pythagoras<CalculationType>
        > comparable_distance_segment_box_strategy_type;

    static inline comparable_distance_segment_box_strategy_type get_comparable_distance_segment_box_strategy()
    {
        return comparable_distance_segment_box_strategy_type();
    }
};

namespace services
{

template <typename Geometry>
struct default_strategy<Geometry, cartesian_tag>
{
    typedef cartesian<> type;
};

// within and relate (MPt, Mls/MPoly)
template <typename Point1, typename Point2, typename CalculationType>
struct from_strategy<within::cartesian_winding<Point1, Point2, CalculationType> >
{
    typedef strategy::index::cartesian<CalculationType> type;

    static inline type get(within::cartesian_winding<Point1, Point2, CalculationType> const&)
    {
        return type();
    }
};

// distance (MPt, MPt)
template <typename CalculationType>
struct from_strategy<distance::comparable::pythagoras<CalculationType> >
{
    typedef strategy::index::cartesian<CalculationType> type;

    static inline type get(distance::comparable::pythagoras<CalculationType> const&)
    {
        return type();
    }
};

// distance (MPt, Linear/Areal)
template <typename CalculationType, typename PPStrategy>
struct from_strategy<distance::projected_point<CalculationType, PPStrategy> >
{
    typedef strategy::index::cartesian<CalculationType> type;

    static inline type get(distance::projected_point<CalculationType, PPStrategy> const&)
    {
        return type();
    }
};

} // namespace services

}}}} // namespace boost::geometry::strategy::index

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_INDEX_HPP


//==================================================================================================
/*!
  @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_CONSTANT_MFIVE_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_MFIVE_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-constant

    Generate the constant mfive.

    @return The Mfive constant for the proper type
  **/
  template<typename T> T Mfive();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant
      Generate the  constant mfive.

      @return The Mfive constant for the proper type
    **/
    Value Mfive();
  }
} }
#endif

#include <boost/simd/constant/definition/mfive.hpp>
#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


/*
 * Copyright (c) 2019 Opticks Team. All Rights Reserved.
 *
 * This file is part of Opticks
 * (see https://bitbucket.org/simoncblyth/opticks).
 *
 * 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 <cstddef>
#include <cassert>
#include <cstring>
#include <cstdlib>
#include <cstdio>
#include <cfloat>
#include <vector>

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>

#include "NPY.hpp"
#include "NPlanck.hpp"
#include "NCIE.hpp"

#include "GAry.hh"
#include "GDomain.hh"

#include "PLOG.hh"

template <typename T>
T GAry<T>::np_interp(const T z, GAry<T>* xp, GAry<T>* fp )
{
    // input domain and values
    //
    //  :param z:   "x" domain value for which the interpolated "y" value is to be obtained
    //  :param xp : domain x-coordinates of the data points, must be increasing.
    //  :param fp : value  y-coordinates of the data points, same length as `xp`.
    //
    //  :return:   y value (ie linear interpolation of fp values) corresponding to the ordinate supplied
    //

    T* dx = xp->getValues();

    T* dy = fp->getValues();
    T left = fp->getLeft();
    T right = fp->getRight();

    int len = xp->getLength();

    assert( len > 0 );

    if(dy[len-1] != right )
    {
        LOG(warning) << "GAry<T>::np_interp "
                     << " len " << len
                     << " dy[len-1] " << dy[len-1]
                     << " right " << right
                     << " left " << left
                     ;
    }

    assert(dy[len-1] == right );

/*
   This assert is firing occasionally... but unreproducibly

[2015-Jul-22 15:28:16.828602]: GProperty::save 2d array of length 275 to : /tmp/reemissionCDF.npy
createZeroTrimmed ifr 0 ito 273
np_sliced ifr 0 ito 273  alen 275 blen 273
np_sliced ifr 0 ito 273  alen 275 blen 273
GBoundaryLib::createReemissionBuffer icdf  : 570b234e132f398d4213400cc88f427b : 4096
d       0.000      0.063      0.125      0.188      0.250      0.313      0.375      0.438      0.500      0.563      0.625      0.688      0.750      0.813      0.875      0.938
v     799.898    463.793    450.234    441.314    434.113    428.762    424.561    420.824    417.127    413.177    408.946    404.907    401.409    398.164    394.649    389.214
[2015-Jul-22 15:28:16.834756]: GProperty::save 2d array of length 4096 to : /tmp/invertedReemissionCDF.npy
Assertion failed: (dy[len-1] == right), function np_interp, file /Users/blyth/env/optix/ggeo/GAry.cc, line 39.
Abort trap: 6

[2015-Jul-24 10:30:36.403821]: GProperty::save 2d array of length 4096 to : /tmp/invertedReemissionCDF.npy
[2015-Jul-24 10:30:36.443812]: GAry<T>::np_interp  len 4096 dy[len-1] nan right nan left 799.898
Assertion failed: (dy[len-1] == right), function np_interp, file /Users/blyth/env/optix/ggeo/GAry.cc, line 50.
/Users/blyth/env/graphics/ggeoview/ggeoview.bash: line 550:  2369 Abort trap: 6           $bin $*

*/

    T ival ;
    int j = xp->binary_search(z);  // find low side domain index corresponding to domain value z
    if(j == -1)
    {
        ival = left;
    }
    else if(j == len - 1)
    {
        ival = dy[j]; // right
    }
    else if(j == len )
    {
        ival = right;
    }
    else
    {
        const T slope  = (dy[j + 1] - dy[j])/(dx[j + 1] - dx[j]);
        ival = slope*(z - dx[j]) + dy[j];
    }
    return ival ;
}

template <typename T>
GAry<T>* GAry<T>::np_interp(GAry<T>* xi, GAry<T>* xp, GAry<T>* fp )
{
    //
    // Loosely follow np.interp signature and implementation from
    //    https://github.com/numpy/numpy/blob/v1.9.1/numpy/lib/src/_compiled_base.c#L599
    //

    assert(xp->getLength() == fp->getLength());  // input domain and values must be of same length

    GAry<T>* res = new GAry<T>(xi->getLength()); // Ary to be filled with interpolated values
    T* dres = res->getValues();

    for (unsigned int i = 0; i < res->getLength() ; i++)
    {
        dres[i] = np_interp( xi->getValue(i), xp, fp ) ;
    }
    return res ;
}

template <typename T>
GAry<T>* np_product(GAry<T>* a, GAry<T>* b)
{
    assert(a->getLength() == b->getLength());
    GAry<T>* prod = new GAry<T>(a->getLength()); // Ary to be filled with interpolated values
    for (unsigned int i = 0; i < prod->getLength() ; i++)
    {
       T ab = a->getValue(i) * b->getValue(i);
       prod->setValue(i, ab );
    }
    return prod ;
}

template <typename T>
GAry<T>* np_subtract(GAry<T>* a, GAry<T>* b)
{
    assert(a->getLength() == b->getLength());
    GAry<T>* result = new GAry<T>(a->getLength());
    for (unsigned int i = 0; i < result->getLength() ; i++)
    {
       T ab = b->getValue(i) - a->getValue(i);
       result->setValue(i, ab );
    }
    return result ;
}

template <typename T>
GAry<T>* np_add(GAry<T>* a, GAry<T>* b)
{
    assert(a->getLength() == b->getLength());
    GAry<T>* result = new GAry<T>(a->getLength());
    for (unsigned int i = 0; i < result->getLength() ; i++)
    {
       T ab = b->getValue(i) + a->getValue(i);
       result->setValue(i, ab );
    }
    return result ;
}

template <typename T>
GAry<T>* np_clip(GAry<T>* a, GAry<T>* low, GAry<T>* high, GAry<T>* low_fallback, GAry<T>* high_fallback )
{
    assert(a->getLength() == low->getLength());
    assert(a->getLength() == high->getLength());
    if(low_fallback) assert(a->getLength() == low_fallback->getLength());
    if(high_fallback) assert(a->getLength() == high_fallback->getLength());

    GAry<T>* cc = new GAry<T>(a->getLength());
    for (unsigned int i = 0; i < cc->getLength() ; i++)
    {
        T aa = a->getValue(i);
        T lo = low->getValue(i);
        T hi = high->getValue(i);

        if( aa < lo )
        {
            cc->setValue(i, low_fallback ? low_fallback->getValue(i) : lo );
        }
        else if(aa > hi )
        {
            cc->setValue(i, high_fallback ? high_fallback->getValue(i) : hi );
        }
        else
        {
            cc->setValue(i, aa );

        }
    }
    return cc ;
}

template <typename T>
T np_maxdiff(GAry<T>* a, GAry<T>* b, bool dump)
{

    unsigned alen = a->getLength() ;
    unsigned blen = b->getLength() ;

    if(dump) LOG(info) << " np_maxdiff "
                       << " a " << std::setw(5) << alen
                       << " b " << std::setw(5) << blen
                       ;

    assert(alen == blen);
    T max(0);
    for (unsigned int i = 0; i < a->getLength() ; i++)
    {
       T av = a->getValue(i);
       T bv = b->getValue(i);
       T ab = bv - av ;

       max = std::max( max, ab );

       if(dump) LOG(info)
                    <<  " i " << std::setw(4) << i
                    <<  " av " << std::setw(10) << av
                    <<  " bv " << std::setw(10) << bv
                    <<  " ab " << std::setw(10) << ab
                    ;

    }

    if(dump) LOG(info) << " maxdiff " << max ;

    return max ;
}

template <typename T>
GAry<T>* np_cumsum(GAry<T>* y, unsigned int offzero)
{
    unsigned int len = y->getLength();
    GAry<T>* cy = new GAry<T>(len+offzero); // Ary to be filled with cumsum values
    T sum(0);
    for (unsigned int j = 0; j < offzero ; j++)
    {
        cy->setValue(j, 0);
    }
    for (unsigned int i = 0; i < len ; i++)
    {
        sum += y->getValue(i);
        cy->setValue(offzero+i, sum);
    }
    return cy ;
}

template <typename T>
GAry<T>* np_reversed(GAry<T>* y, bool reciprocal, T scale)
{
    unsigned int len = y->getLength();
    GAry<T>* ry = new GAry<T>(len);

    for (unsigned int i = 0; i < len ; i++)
    {
        T val = y->getValue(i) ;
        ry->setValue(len-1-i, reciprocal ? scale/val : scale*val );
    }
    return ry ;
}

template <typename T>
GAry<T>* GAry<T>::reciprocal(GAry<T>* y, T scale)
{
    unsigned int len = y->getLength();
    GAry<T>* ry = new GAry<T>(len);
    for (unsigned int i = 0; i < len ; i++)
    {
        T val = y->getValue(i) ;
        ry->setValue(i, scale/val );
    }
    return ry ;
}

template <typename T>
GAry<T>* g4_groupvel_bintrick(GAry<T>* y)
{
   /*
    This curious bin shifting is used by G4 GROUPVEL calc

    In order not to loose a bin
    a tricky manoever of using the 1st and last bin and
    the average of the body bins
    which means the first bin is half width, and last is 1.5 width

    ::

            0  +  1  +  2  +  3  +  4  +  5        <--- 6 original values
            |    /     /     /     /      |
            |   /     /     /     /       |
            0  1     2     3     4        5        <--- still 6

    bb = np.zeros_like(aa)
    bb[0] = aa[0]
    bb[1:-1] = (aa[1:-1] + aa[:-2])/2.
    bb[-1] = aa[-1]
    return bb

   */

    GAry<T>& Y = *y ;
    unsigned int len = y->getLength();
    GAry<T>* ry = new GAry<T>(len);

    ry->setValue(0    , Y[0]);
    ry->setValue(len-1, Y[-1]);

    T two(2);

    for (unsigned i = 1; i < len - 1 ; i++)
    {
        T val = (Y[i-1] + Y[i])/two ;
        ry->setValue(i, val );
    }
    return ry ;
}

template <typename T>
GAry<T>* np_gradient(GAry<T>* y)
{
    /*
    Take a look at np.gradient??

    The gradient is computed using second order accurate central differences
    in the interior and either first differences or second order accurate
    one-sides (forward or backwards) differences at the boundaries. The
    returned gradient hence has the same shape as the input array.

          0    <--- (0,1)

          1    <--- (0,2)/2
          2    <--- (1,3)/2
          3    <--  (2,4)/2
          4    <--  (3,5)/2
          5    <--  (4,6)/2
          6    <--  (5,7)/2
          7    <--  (6,8)/2
          8    <--  (7,9)/2

          9    <--  (8,9)

    ::

        out = np.zeros_like(y)
        out[0] = y[1] - y[0]
        out[1:-1] = (y[2:]-y[:-2])/2.0
        out[-1] = y[-1] - y[-2]
        return o
    */

    GAry<T>& Y = *y ;
    unsigned int len = y->getLength();
    assert(len >= 2 );
    T two(2);

    GAry<T>* g = new GAry<T>(len);
    g->setValue(0    , Y[1] - Y[0] );
    for (unsigned i = 1; i < len - 1 ; i++) g->setValue(i, (Y[i+1] - Y[i-1])/two );
    g->setValue(len-1, Y[-1] - Y[-2]);

    return g ;
}

template <typename T>
GAry<T>* np_sliced(GAry<T>* a, int ifr, int ito)
{
   /*

In [13]: a = np.linspace(0,1,11)

In [14]: a
Out[14]: array([ 0. ,  0.1,  0.2,  0.3,  0.4,  0.5,  0.6,  0.7,  0.8,  0.9,  1. ])

In [16]: a[0:-1]
Out[16]: array([ 0. ,  0.1,  0.2,  0.3,  0.4,  0.5,  0.6,  0.7,  0.8,  0.9])

In [17]: a[0:11]
Out[17]: array([ 0. ,  0.1,  0.2,  0.3,  0.4,  0.5,  0.6,  0.7,  0.8,  0.9,  1. ])

   */
    int alen = a->getLength();
    if(ifr < 0 ) ifr += alen ;
    if(ito < 0 ) ito += alen ;
    assert(ifr >=0 && ifr <  alen);
    assert(ito >=0 && ito <= alen);

    int blen = ito - ifr ;   // py style 0-based one-beyond "ito"
    assert( ito >= ifr );

    //printf("Gary.cc:np_sliced ifr %d ito %d  alen %u blen %u \n", ifr, ito, alen, blen );

    GAry<T>* b = new GAry<T>(blen);

    for (int ia = 0; ia < alen ; ia++)
    {
        if( ia >= ifr && ia < ito )
        {
            unsigned int ib = ia - ifr ;
            T val = a->getValue(ia) ;
            b->setValue( ib, val );
        }
    }
    return b ;
}

template <typename T>
GAry<T>* np_diff(GAry<T>* y)
{
    unsigned int len = y->getLength() ;
    GAry<T>* dy = new GAry<T>(len - 1);
    for (unsigned int i = 0; i < len - 1 ; i++)
    {
        T diff = y->getValue(i+1) - y->getValue(i) ;
        dy->setValue(i, diff );
    }
    return dy ;
}

template <typename T>
GAry<T>* np_mid(GAry<T>* y)
{
    unsigned int len = y->getLength() ;
    GAry<T>* my = new GAry<T>(len - 1);
    T two(2);
    for (unsigned int i = 0; i < len - 1 ; i++)
    {
        T mid = (y->getValue(i) + y->getValue(i+1))/two ;
        my->setValue(i, mid );
    }
    return my ;
}

template <typename T>
GAry<T>* GAry<T>::urandom(unsigned int length)
{
    GAry<T>* u = new GAry<T>(length);
    T* vals = u->getValues();

    typedef boost::mt19937          RNG_t;
    typedef boost::uniform_real<>   Distrib_t;
    typedef boost::variate_generator< RNG_t, Distrib_t > Generator_t ;

    RNG_t rng;
    Distrib_t distrib(0,1);
    Generator_t gen(rng, distrib);

    for (unsigned int i = 0; i < length ; i++) vals[i] = gen();
    return u ;
}

template <typename T>
GAry<T>* GAry<T>::from_domain(GDomain<T>* domain)
{
    GAry<T>* ary = new GAry<T>( domain->getLength(), domain->getValues() );
    return ary ;
}

template <typename T>
GAry<T>* GAry<T>::planck_spectral_radiance(GAry<T>* nm, T blackbody_temp_kelvin)
{
    T* nmv = nm->getValues();
    GAry<T>* ary = new GAry<T>( nm->getLength(), NULL );
    T* vals = ary->getValues();
    for(unsigned int i=0 ; i < ary->getLength(); i++) vals[i] = ::planck_spectral_radiance(nmv[i], blackbody_temp_kelvin) ;
    return ary ;
}

template <typename T>
GAry<T>* GAry<T>::cie_weight(GAry<T>* nm, unsigned int component)
{
    T* nmv = nm->getValues();
    GAry<T>* ary = new GAry<T>( nm->getLength(), NULL );
    T* vals = ary->getValues();
    for(unsigned int i=0 ; i < ary->getLength(); i++)
    {
        switch(component)
        {
            case 0:vals[i] = NCIE::X(nmv[i]) ;break;
            case 1:vals[i] = NCIE::Y(nmv[i]) ;break;
            case 2:vals[i] = NCIE::Z(nmv[i]) ;break;
        }
    }
    return ary ;
}

template <typename T>
GAry<T>* GAry<T>::cie_X(GAry<T>* nm)
{
    return cie_weight(nm, 0);
}
template <typename T>
GAry<T>* GAry<T>::cie_Y(GAry<T>* nm)
{
    return cie_weight(nm, 1);
}
template <typename T>
GAry<T>* GAry<T>::cie_Z(GAry<T>* nm)
{
    return cie_weight(nm, 2);
}

template <typename T>
GAry<T>* GAry<T>::create_from_floats(unsigned int length, float* values)
{
    // promoting floats to doubles
    T* tvalues = new T[length];
    for(unsigned int i=0 ; i < length ; i++) tvalues[i] = values[i];
    GAry<T>* ary = new GAry<T>(length, tvalues);
    delete[] tvalues ;
    return ary ;
}

template <typename T>
GAry<T>* GAry<T>::product(GAry<T>* a, GAry<T>* b)
{
    return np_product(a, b);
}

template <typename T>
GAry<T>* GAry<T>::subtract(GAry<T>* a, GAry<T>* b)
{
    return np_subtract(a, b);
}
template <typename T>
GAry<T>* GAry<T>::add(GAry<T>* a, GAry<T>* b)
{
    return np_add(a, b);
}

template <typename T>
GAry<T>* GAry<T>::clip(GAry<T>* low, GAry<T>* high, GAry<T>* low_fallback, GAry<T>* high_fallback )
{
    return np_clip(this, low, high, low_fallback, high_fallback );
}

template <typename T>
T GAry<T>::maxdiff(GAry<T>* a, GAry<T>* b, bool dump)
{
    return np_maxdiff(a, b, dump);
}

template <typename T>
GAry<T>* GAry<T>::reciprocal(T scale_)
{
    return reciprocal(this, scale_);
}

template <typename T>
GAry<T>* GAry<T>::from_constant(unsigned int length, T value )
{
    GAry<T>* ary = new GAry<T>( length, NULL );
    T* vals = ary->getValues();
    for(unsigned int i=0 ; i < length; i++) vals[i] = value ;
    return ary ;
}

template <typename T>
GAry<T>* GAry<T>::zeros(unsigned int length)
{
    return GAry<T>::from_constant(length, 0);
}

template <typename T>
GAry<T>* GAry<T>::ones(unsigned int length)
{
    return GAry<T>::from_constant(length, 1);
}

template <typename T>
GAry<T>* GAry<T>::ramp(unsigned int length, T low, T step )
{
    GAry<T>* ary = new GAry<T>( length, NULL );
    T* vals = ary->getValues();
    for(unsigned int i=0 ; i < length; i++) vals[i] = low + step*i ;
    return ary ;
}

template <typename T>
T GAry<T>::step(T num, T start, T stop)
{
    return (stop - start)/(num - 1) ;
}

template <typename T>
GAry<T>* GAry<T>::linspace(T num, T start, T stop)
{
   /*
In [11]: np.linspace(0,1,10)
Out[11]:
array([ 0.   ,  0.111,  0.222,  0.333,  0.444,  0.556,  0.667,  0.778,
        0.889,  1.   ])

In [12]: np.linspace(0,1,11)
Out[12]: array([ 0. ,  0.1,  0.2,  0.3,  0.4,  0.5,  0.6,  0.7,  0.8,  0.9,  1. ])

   */
    T step_ = step(num, start, stop);
    GAry<T>* ary = new GAry<T>( num, NULL );
    T* vals = ary->getValues();
    for(unsigned int i=0 ; i < num ; i++) vals[i] = start + step_*i ;
    return ary ;
}

template <typename T>
GAry<T>* GAry<T>::copy()
{
    return new GAry<T>(this);
}

template <typename T>
GAry<T>::GAry(GAry<T>* other) : m_length(other->getLength())
{
    m_values = new T[m_length];
    for(unsigned int i=0 ; i < m_length; i++) m_values[i] = other->getValue(i) ;
}

template <typename T>
GAry<T>::GAry(unsigned int length, T* values) : m_length(length)
{
    m_values = new T[m_length];
    if(values)
    {
        for(unsigned int i=0 ; i < length; i++) m_values[i] = values[i] ;
    }
}

template <typename T>
GAry<T>::~GAry()
{
    delete m_values ;
}

template <typename T>
void GAry<T>::setValues(T val)
{
    for(unsigned int i=0 ; i < m_length; i++) m_values[i] = val ;
}

template <typename T>
GAry<T>* GAry<T>::cumsum(unsigned int offzero)
{
    return np_cumsum(this, offzero) ;
}

template <typename T>
GAry<T>* GAry<T>::g4_groupvel_bintrick()
{
    return ::g4_groupvel_bintrick(this) ;
}

template <typename T>
GAry<T>* GAry<T>::gradient()
{
    return ::np_gradient(this) ;
}

template <typename T>
GAry<T>* GAry<T>::diff()
{
    return np_diff(this) ;
}     // domain bin widths

template <typename T>
GAry<T>* GAry<T>::mid()
{
    return np_mid(this) ;
}  // average of values at bin edges, ie linear approximation of mid bin "y" value

template <typename T>
GAry<T>* GAry<T>::reversed(bool reciprocal_, T scale_)
{
     return np_reversed(this, reciprocal_, scale_) ;
}

template <typename T>
GAry<T>* GAry<T>::sliced(int ifr_, int ito_)
{
     return np_sliced(this, ifr_, ito_) ;
}

template <typename T>
void GAry<T>::Summary(const char* msg, unsigned int imod, T presentation_scale)
{
    printf("%s length %u  leftzero %u rightzero %u \n", msg, m_length, getLeftZero(), getRightZero() );

    if(m_length < 100)
    {
        for(unsigned int i=0 ; i < m_length ; i++ ) if(i%imod == 0) printf(" %10.3f ", getValue(i)*presentation_scale);
        printf("\n");
    }
    else
    {
        for(unsigned int i=0 ; i < m_length ; i++ )
        {
            if( i < 10 || i > m_length - 10 )
            {
                printf(" %10.3f ", getValue(i)*presentation_scale);
            }
            else if ( i == 10 )
            {
                printf(" ... ");
            }
        }
        printf("\n");
   }

    T rng[2] ;
    unsigned int idx[2] ;
    rng[0] = min(idx[0]);
    rng[1] = max(idx[1]);

    printf("mi/mx idx %u %u  range %15.5f %15.5f  1/range %15.5f %15.5f \n", idx[0], idx[1], rng[0], rng[1], 1./rng[0], 1./rng[1] );

}

template <typename T>
T GAry<T>::getValueFractional(T findex)
{
    unsigned int idx(findex);
    T dva ;

    if(idx + 1 < m_length )
    {
       T frac(findex - T(idx));
       dva = m_values[idx]*(1.-frac) + m_values[idx+1]*frac ;
    }
    else
    {
       dva = m_values[m_length-1];
    }
    return dva ;
}

template <typename T>
T GAry<T>::getValueLookup(T u)
{
    // convert u (0:1) into fractional bin, and use that to lookup values
    assert( u <= 1. && u >= 0. );
    T findex = u * (m_length - 1) ;
    return getValueFractional(findex);
}

template <typename T>
unsigned int GAry<T>::getLeftZero()
{
    // when the values start with a string of zeros,
    // return the index of rightmost such zero minus 1,
    // otherwise return 0

    T zero(0);
    int ifr(0);
    for(unsigned int i=0 ; i < m_length ; i++)
    {
        if( m_values[i] == zero ) ifr = i  ;
        else
            break ;
    }

    return ifr > 0 ? ifr - 1 : 0 ;
}

template <typename T>
unsigned int GAry<T>::getRightZero()
{
    // when the values end with a string of zeros,
    // return the lowest index + 1,
    // otherwise return m_length

    T zero(0);
    unsigned int ito(m_length);
    for(unsigned int i=0 ; i < m_length ; i++)
    {
        unsigned int j = m_length - 1 - i ;   // looks at bins from the right
        if( m_values[j] == zero ) ito = j  ;
        else
            break ;
    }
    return ito < m_length ? ito + 1 : m_length ;
}

template <typename T>
void GAry<T>::add(GAry<T>* other)
{
    assert(other->getLength() == m_length);
    for(unsigned int i=0 ; i < m_length ; i++ ) m_values[i] += other->getValue(i) ;
}

template <typename T>
void GAry<T>::subtract(GAry<T>* other)
{
    assert(other->getLength() == m_length);
    for(unsigned int i=0 ; i < m_length ; i++ ) m_values[i] -= other->getValue(i) ;
}

template <typename T>
void GAry<T>::scale(T sc)
{
    for(unsigned int i=0 ; i < m_length ; i++ ) m_values[i] *= sc ;
}

template <typename T>
void GAry<T>::reciprocate()
{
    T one(1);
    for(unsigned int i=0 ; i < m_length ; i++ ) m_values[i] = one/m_values[i] ;
}

template <typename T>
int GAry<T>::linear_search(T key)
{
    // for checking edge case behaviour of binary_search
    // expected to return same values as binary_search more slowly

    if(key < m_values[0])
    {
        return -1 ;        // indicates "below-lower-bound"
    }
    else if(key > m_values[m_length-1])
    {
        return m_length ;   // indicates "above-upper-bound"
    }
    else if(key == m_values[m_length-1])
    {
        return m_length - 1 ;   //  at upper bound
    }
    else
    {
        for(unsigned int i=0 ; i < m_length - 1 ; ++i )
        {
            //   m_values[0] : m_values[1]
            //   ...
            //   m_values[m_length-2] : m_values[m_length-1]
            //
            if(key >= m_values[i] && key < m_values[i+1]) return i ;
        }
    }
    assert(0); // not expected here
    return -2 ;
}

template <typename T>
int GAry<T>::binary_search(T key)
{
   // Find bin index containing the key
   //
   // :param key: value to be "placed" within the sequence of ascending values
   // :return: index of the low side value
   //
   //      * normally index is range 0:m_length-1
   //      * if the key exceeds m_values[m_length-1] m_length is returned, meaning "to the right"
   //
   // NB this is used by np_interp

    if(key > m_values[m_length-1])
    {
        return m_length ;
    }
    unsigned int imin = 0 ;
    unsigned int imax = m_length ;
    unsigned int imid ;

    while(imin < imax)
    {
        imid = imin + ((imax - imin) >> 1);
        if (key >= m_values[imid])
        {
            imin = imid + 1;
        }
        else
        {
            imax = imid;
        }
    }

    assert( imin == imax );
    //if(imin != imax ) printf("GAry<T>::binary_search key %10.5f  imin %u imax %u len %u \n", key, imin, imax, m_length );

    return imin - 1;
}

template <typename T>
T GAry<T>::fractional_binary_search(T u)
{
    // the advantage in dealing in fractional indices
    // is can delay resorting to using domain information for a bit longer

    int idx = binary_search(u);
    T frac  = (u - m_values[idx])/(m_values[idx+1]-m_values[idx]);  // fraction of bin
    return T(idx) + frac ;
}

template <typename T>
unsigned int GAry<T>::sample_cdf(T u)
{
    // other than edge cases, this gives same results as binary_search

    int lower = 0;
    int upper = m_length - 1;

    while(lower < upper-1)
    {
        int half = (lower + upper) / 2;
        if (u < m_values[half])
        {
            upper = half;
        }
        else
        {
            lower = half;
        }
    }

    assert( lower == upper - 1 );
    return lower ;
}

template <typename T>
T GAry<T>::min(unsigned& idx) const
{
   T mi(FLT_MAX);
   for(unsigned int i=0 ; i < m_length ; i++ )
   {
       T v = m_values[i];
       if(v < mi)
       {
           mi = v ;
           idx = i ;
       }
   }
   return mi ;
}

template <typename T>
T GAry<T>::max(unsigned& idx) const
{
   T mx(-FLT_MAX);
   for(unsigned int i=0 ; i < m_length ; i++ )
   {
       T v = m_values[i];
       if(v > mx)
       {
           mx = v ;
           idx = i ;
       }
   }
   return mx ;
}

template <typename T>
void GAry<T>::save(const char* path)
{
    std::vector<int> shape ;
    shape.push_back(m_length);

    std::string metadata = "{}" ;

    std::vector<T> data ;
    for(unsigned int i=0 ; i < m_length ; i++ ) data.push_back(m_values[i]) ;

    LOG(info) << "GAry::save 1d array of length " << m_length << " to : " << path ;
    NPY<T> npy(shape, data, metadata);
    npy.save(path);
}

/*
* :google:`move templated class implementation out of header`
* http://www.drdobbs.com/moving-templates-out-of-header-files/184403420

A compiler warning "declaration does not declare anything" was avoided
by putting the explicit template instantiation at the tail rather than the
head of the implementation.
*/

template class GAry<float>;
template class GAry<double>;   // needs work on NPY for this


#include <algorithm>
#include <chrono>
#include <cstdlib>
#include <dlib/image_processing.h>
#include <iostream>
#include <numeric>
#include <resnet.h>

namespace chrono = std::chrono;
using fms = chrono::duration<float, std::milli>;

int main(const int argc, const char** argv)try
{
    setenv("CUDA_LAUNCH_BLOCKING", "1", 1);
    chrono::time_point<chrono::steady_clock> t0, t1;
    float duration{};

    size_t mini_batch_size = 1;
    size_t image_size = 224;
    if (argc > 1)
    {
        mini_batch_size = std::stoul(argv[1]);
    }
    if (argc > 2)
    {
        image_size = std::stoul(argv[2]);
    }

    // Create the images to test
    dlib::matrix<dlib::rgb_pixel> image(image_size, image_size);
    dlib::assign_all_pixels(image, dlib::rgb_pixel(0, 0, 0));
    const std::vector<dlib::matrix<dlib::rgb_pixel>> minibatch(mini_batch_size, image);

    // The input tensor of the network
    dlib::resizable_tensor x;

    {
        // Declare the network
        t0 = chrono::steady_clock::now();
        resnet::infer_50 net;
        t1 = chrono::steady_clock::now();
        duration = chrono::duration_cast<fms>(t1 - t0).count();
        std::cout << "instantiation time: " << duration << " ms" << std::endl;
        std::cin.get();

        // Convert them to a tensor for this network. This way we only measure inference and not
        // data transfer. In PyTorch, this would be: x = x.cuda()
        net.to_tensor(minibatch.begin(), minibatch.end(), x);
        t0 = chrono::steady_clock::now();
        net.forward(x);
        t1 = chrono::steady_clock::now();
        duration = chrono::duration_cast<fms>(t1 - t0).count();
        std::cout << "1st inference time: " << duration << " ms" << std::endl;
        std::cout << "input shape: " << x.num_samples() << 'x' << x.k() << 'x' << x.nr() << 'x'
                  << x.nc() << std::endl;
        std::cout << "parameters: " << dlib::count_parameters(net) << std::endl;
        std::cin.get();

        std::array<float, 100> times;
        for (auto& time : times)
        {
            net.to_tensor(minibatch.begin(), minibatch.end(), x);
            t0 = chrono::steady_clock::now();
            net.forward(x);
            t1 = chrono::steady_clock::now();
            duration = chrono::duration_cast<fms>(t1 - t0).count();
            std::cout << "2nd inference time: " << duration << " ms   \r" << std::flush;
            time = duration;
        }
        std::cout << "\navg: " << std::fixed << std::setprecision(3)
                  << std::accumulate(times.begin(), times.end(), 0.f, std::plus<float>()) /
                         times.size()
                  << " ms" << std::endl;
        std::cin.get();
        net.clean();
    }

    // Create some labels
    const std::vector<unsigned long> labels(mini_batch_size, 0);
    resnet::train_50 net;
    // measure backward pass
    std::vector<dlib::sgd> solvers(net.num_computational_layers, dlib::sgd(0.0005, 0.9));
    net.to_tensor(minibatch.begin(), minibatch.end(), x);
    net.compute_loss(x, labels.begin());
    net.back_propagate_error(x);
    net.update_parameters(solvers, 0.1);
    std::array<float, 100> times;
    for (auto& time : times)
    {
        t0 = chrono::steady_clock::now();
        net.to_tensor(minibatch.begin(), minibatch.end(), x);
        net.compute_loss(x, labels.begin());
        net.back_propagate_error(x);
        net.update_parameters(solvers, 0.1);
        t1 = chrono::steady_clock::now();
        duration = chrono::duration_cast<fms>(t1 - t0).count();
        std::cout << "backward pass time: " << duration << " ms   \r" << std::flush;
        time = duration;
    }
    std::cout << "\navg: " << std::fixed << std::setprecision(3)
              << std::accumulate(times.begin(), times.end(), 0.f, std::plus<float>()) /
                     times.size()
              << " ms" << std::endl;
    std::cin.get();

    return EXIT_SUCCESS;
}
catch (const std::exception& e)
{
    std::cout << e.what() << std::endl;
    return EXIT_FAILURE;
}


/* CirKit: A circuit toolkit
 * Copyright (C) 2009-2015  University of Bremen
 * Copyright (C) 2015-2017  EPFL
 *
 * Permission is hereby granted, free of charge, to any person
 * obtaining a copy of this software and associated documentation
 * files (the "Software"), to deal in the Software without
 * restriction, including without limitation the rights to use,
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following
 * conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 * OTHER DEALINGS IN THE SOFTWARE.
 */
#include "read_pla_to_cirkit_bdd.hpp"

#include <core/io/pla_processor.hpp>
#include <core/io/pla_parser.hpp>

#include <classical/dd/bdd.hpp>

#include <boost/format.hpp>
#include <boost/filesystem/fstream.hpp>
#include <boost/timer.hpp>

namespace cirkit
{

namespace
{

class from_bdd_pla_processor : public pla_processor
{
public:
  explicit from_bdd_pla_processor( unsigned log_max_objs, bool verbose )
    : m_log_max_objs( log_max_objs  ),
      m_verbose( verbose )
    {}

    void on_comment(const std::string &comment)
    {
    }

    void on_num_inputs(unsigned num_inputs)
    {
      m_inputs = num_inputs;
      m_timer.restart();
    }

    void on_num_outputs(unsigned num_outputs) final
    {
      m_outputs = num_outputs;
    }

    void on_num_products(unsigned num_products) final
    {
    }

    void on_input_labels(const std::vector<std::string> &input_labels) final
    {
      m_inputNames = input_labels;
    }

    void on_output_labels(const std::vector<std::string> &output_labels) final
    {
      m_outputNames = output_labels;
    }

    void on_end() final
    {
      if ( true /* m_verbose */ )
      {
        std::cout
          << boost::format ("[i] took %.2f seconds to reading pla-file into BDD.") % m_timer.elapsed()
          << std::endl;
      }
    }

    void on_type(const std::string &type)
    {
    }

    void setPortNames()
    {
      assert( m_inputNames.size() <= m_inputs );
      assert( m_outputNames.size() <= m_outputs );

      for ( auto i = 0u; i < m_inputNames.size(); ++i )
      {
        m_function->setInputName ( i, m_inputNames.at ( i ) );
      }

      for ( auto i = 0u; i < m_outputNames.size(); ++i )
      {
        m_function->setOutputName ( i, m_outputNames.at ( i ) );
      }
    }

    void initializeInputPorts()
    {
      for ( auto i = 0u; i < m_inputs; ++i )
      {
        m_function->pushInput ( i );
      }
    }

    void initializeOutputPorts(bdd_manager_ptr manager)
    {
      auto falseNode = manager->bdd_bot ();
      std::cout << falseNode << std::endl;
      for ( auto i = 0u; i < m_outputs; ++i )
      {
        m_function->setOutputVar ( i, falseNode );
      }
    }

    void initializeFunction () {
      assert ( m_inputs != 0 );
      assert ( m_outputs != 0 );

      try {
        auto function = new bdd_function (
          m_inputs, m_log_max_objs, m_verbose
        );
        m_function.reset ( function );

        initializeInputPorts();
        initializeOutputPorts( m_function->manager() );
        setPortNames();
      }
      catch ( std::exception const& e )
      {
        std::cerr << "[e] unable to create bdd_function: " << e.what() << std::endl;
        throw;
      }
    }

    void on_cube(const std::string &in, const std::string &out) final
    {
      if ( !m_function ) {
        initializeFunction ();
      }

      auto term = getBddCube ( in );

      addToOutputBdds ( term, out );
    }

    bdd_function_cptr function() const {
      return m_function;
    }

  private:
    void addToOutputBdds ( const bdd& term, const std::string& out ) {

      auto relevantValue = [] ( const char& val )
      {
        return val == '1';
      };

      for ( auto i = 0u; i < out.size(); ++i )
      {
        auto const& value = out.at( i );
        if ( relevantValue ( value ) ) {
          auto output = m_function->lookupOutput ( i );
//          std::cout << "Output: " << i  << " is relevant: " << output << std::endl;

          m_function->setOutputVar ( i, output || term );
        }
      }
    }

    bdd getBddCube ( const std::string& cube )
    {
      auto manager = m_function->manager ();

      assert ( cube.size() == m_inputs );
      bdd term = manager->bdd_top ();

      for ( auto i = 0u; i < cube.size(); ++i ) {
        auto const& val = cube.at( i );

        if ( val != '-') {
          auto input = m_function->lookupInput( i );

          if ( val == '0')
          {
            term = !input && term;
          }
          else
          {
            term = input && term;
          }
        }
      }

      if ( m_verbose )
      {
        std::cout << "[i] term result:" << term << std::endl;
      }
      return term;
    }

private:
  unsigned         m_log_max_objs;
  bool             m_verbose;
  bdd_function_ptr m_function;
  unsigned         m_inputs;
  unsigned         m_outputs;

  boost::timer     m_timer;

  std::vector<std::string> m_inputNames;
  std::vector<std::string> m_outputNames;
};

} // anonymous namespace

bdd_function::bdd_function(
      unsigned nvars
    , unsigned log_max_objs
    , bool verbose
) : m_manager ( bdd_manager::create ( nvars, log_max_objs, verbose ) )
{
}

bdd_function::bdd_function(bdd_manager_ptr manager)
  : m_manager ( manager )
{
}

bdd_function::~bdd_function()
{
}

void bdd_function::pushInput ( unsigned index ) {
  assert ( m_outputs.empty() );
  auto bdd = m_manager->bdd_var( index );
  m_inputs.emplace ( index, bdd );
}

bdd::const_param_ref bdd_function::lookupInput(unsigned index) const
{
  auto iter = m_inputs.find ( index );
  assert ( iter != m_inputs.end() );
  return iter->second;
}

bdd::const_param_ref bdd_function::lookupOutput(unsigned index) const
{
  auto iter = m_outputs.find ( index );
  assert ( iter != m_outputs.end() );
  return iter->second;
}

void bdd_function::setOutputVar(unsigned index, const bdd &var)
{
  auto iter = m_outputs.find ( index );
  if ( iter != m_outputs.end() ) {
    iter->second = var;
  }
  else {
    m_outputs.emplace ( index, var );
  }
}

bdd_manager_ptr bdd_function::manager() const
{
  return m_manager;
}

void bdd_function::setInputName(unsigned index, const std::string &name)
{
  m_inputNames.emplace ( index, name );
}

void bdd_function::setOutputName(unsigned index, const std::string &name)
{
  m_outputNames.emplace ( index, name );
}

bdd_function_cptr read_pla_into_cirkit_bdd_job( boost::filesystem::ifstream& stream, unsigned log_max_objs, bool verbose )
{
  assert ( stream );

  from_bdd_pla_processor processor ( log_max_objs, verbose );
  try {
    pla_parser ( stream, processor );
  } catch ( std::exception const& e ) {
    std::cerr << "[e] unable to parse PLA file: " << e.what() << std::endl;
    return bdd_function_ptr ();
  }

  return processor.function();
}

bdd_function_cptr read_pla_into_cirkit_bdd( const boost::filesystem::path &filename,
                                            const properties::ptr& settings )
{
  assert ( !filename.empty() );

  auto log_max_objs = get( settings, "log_max_objs", 24u );
  auto verbose      = get( settings, "verbose",      false );

  boost::filesystem::ifstream stream( filename );
  if ( !stream ) {
    std::cerr << "[e] unable to open file " << filename << std::endl;
    return bdd_function_ptr();
  }

  return read_pla_into_cirkit_bdd_job( stream, log_max_objs, verbose );
}

std::vector<std::string> bdd_function::input_labels() const {
  std::vector<std::string> input_vars;
  for (auto const &itr: m_inputNames)
    input_vars.push_back(itr.second);
  return input_vars;
}
std::vector<std::string> bdd_function::output_labels() const{
  std::vector<std::string> output_vars;
  for (auto const &itr: m_outputNames)
    output_vars.push_back(itr.second);
  return output_vars;
}

} // namespace cirkit

// Local Variables:
// c-basic-offset: 2
// eval: (c-set-offset 'substatement-open 0)
// eval: (c-set-offset 'innamespace 0)
// End:


/*
 * Copyright (c) 2016 Abhishek Agrawal (abhishek.agrawal@protonmail.com)
 * Distributed under the MIT License.
 * See accompanying file LICENSE.md or copy at http://opensource.org/licenses/MIT
 */

#ifndef PARTICLE_AROUND_SPHEROID_AND_ELLIPSOID_GRAVITATIONAL_POTENTIAL_HPP
#define PARTICLE_AROUND_SPHEROID_AND_ELLIPSOID_GRAVITATIONAL_POTENTIAL_HPP

#include <cstdlib>
#include <iostream>
#include <sstream>
#include <fstream>
#include <string>
#include <cmath>
#include <limits>
#include <vector>

#include <boost/numeric/odeint.hpp>

#include "NAOS/constants.hpp"
#include "NAOS/basicMath.hpp"
#include "NAOS/basicAstro.hpp"
#include "NAOS/misc.hpp"
#include "NAOS/ellipsoidGravitationalAcceleration.hpp"

namespace naos
{
//! particle around spheroid integration
/*!
 * integrate the equations of motion for a particle around a spheroid. The gravitational accelerations
 * calculated using the ellipsoid gravitational potential model.
 */
void executeParticleAroundSpheroid( const double alpha,
                                    const double gravParameter,
                                    std::vector< double > asteroidRotationVector,
                                    std::vector< double > &initialOrbitalElements,
                                    const double initialStepSize,
                                    const double startTime,
                                    const double endTime,
                                    std::ostringstream &outputFilePath,
                                    const int dataSaveIntervals );
} // namespace naos

#endif // PARTICLE_AROUND_SPHEROID_AND_ELLIPSOID_GRAVITATIONAL_POTENTIAL_HPP


#define _USE_MATH_DEFINES
#include <ctgmath>
#include <cmath>

#include "Scene.h"

#include "ChronoTimer.h"
#include "Rigid.h"
#include "RigidBodyMain.h"

#include "online/Camera.h"
#include "online/MatrixStack.h"
#include "online/Program.h"

#ifndef _GLIBCXX_USE_NANOSLEEP
#define _GLIBCXX_USE_NANOSLEEP
#endif

#ifndef GLM_FORCE_RADIANS
#define GLM_FORCE_RADIANS
#endif
#include <glm/gtc/type_ptr.hpp>

#ifdef REDMAX_MKL
#define EIGEN_USE_MKL
#endif
#ifndef EIGEN_DONT_ALIGN_STATICALLY
#define EIGEN_DONT_ALIGN_STATICALLY
#endif
#ifndef EIGEN_DONT_ALIGN
#define EIGEN_DONT_ALIGN
#endif
#ifndef EIGEN_NO_STATIC_ASSERT
#define EIGEN_NO_STATIC_ASSERT
#endif
#ifndef EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT
#define EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT
#endif
#include <Eigen/SparseCholesky>
#include <Eigen/OrderingMethods>
#include <Eigen/Core>
#include <Eigen/Geometry>

#include <iostream>
#include <fstream>
#include <string>

static double randDouble(double l, double h)
{
	double r = rand() / (double)RAND_MAX;
	return (1.0 - r) * l + r * h;
}

Scene::Scene(const std::string &RESOURCE_DIR)
{
	t = 0.0;
	rigid_body = std::make_shared<RigidBodyMain>(RESOURCE_DIR, t);
}

Scene::~Scene()
{
}

void Scene::init()
{
	rigid_body->displayUserActions();
}

void Scene::load(const std::string &FILENAME, std::shared_ptr<Camera> &c)
{
	// load the linkage system
	///rigid_body->load(FILENAME);
	rigid_body->setSolveType(simType::PCG);
	//rigid_body->loadTree(5);
	//rigid_body->loadSimpleTree(6);
	//c->setTranslations(0.0f, 14.0f, -34.8f);
   // rigid_body->loadUmbrella(16);
	//rigid_body->loadTest(5);
    rigid_body->loadBridge(20, 20);
	//rigid_body->loadSimpleBridge(30);

	//c->setTranslations(0.0f, -5.0f, -24.8f);
	c->setTranslations(0.0f, -0.0f, -52.8f);
}

int Scene::keyPressed(unsigned int key)
{
	int rbindicator = rigid_body->handleKeyPress(key);

	switch (key) {
	case 'x':
		//rigid_body->load_kf_com();
		std::cout << "keyframing active" << std::endl;
		return false;
	case 'v':
		//rigid_body->validatepartials();
		std::cout << "validating" << std::endl;
		return false;
	}

	return rbindicator;
}

int Scene::clickPress(float x, float y, float z)
{
	return 0;
}

int Scene::clickRelease(float x, float y, float z)
{
	return 0;
}

Eigen::VectorXd Scene::step()
{
	Eigen::VectorXd result = rigid_body->step();
	//brender->exportBrender(rigid_body->getTime());
	return result;
}

void Scene::batchTest()
{
	// save to file named...
	std::string filename = "../simdata/pcg_tree_short.m";//pcg_umbrella_optimize.m";

	// choose simulations to run
	std::vector<simType> testSimTypes{
		PCG
	};

	// name every test
	std::vector<std::string> testNames{
		"pcg"
	};

	std::vector<int> testSizes{
		5
		//8, 16, 32, 64, 128
		//10, 20, 50, 100, 200, 500, 1000
		//5, 6, 7, 8, 9, 10//, 11, 12
		//10, 20, 40, 80, 160, 320, 640, 1280
		//6, 12, 24, 48, 96, 192, 384
		//30, 60, 120, 240, 480, 960
	};

	// select test quantities
	bool ground_truth = false;  // prerequisite for using avg step error
	bool avg_step_lstsq = false; // using the first result as ground truth
	bool qdot_cert = false;
	bool leaf_pos_cert = true;
	bool iterations = true;
	// timing enabled

	// rendering?
	bool render = false;
	int torend = 0;
#ifdef REDMAX_JSONCPP
	if (render)
	{
		brender = BrenderManager::getInstance();
		brender->add(rigid_body);
	}
#endif

	// run the scene simulation for 10 simulation seconds and collect data
	std::vector<std::vector<Eigen::VectorXd>> raw_result;  // ground truth, EXPENSIVE
	raw_result.resize(testSimTypes.size());

	// save last qdot
	std::vector<std::vector<Eigen::VectorXd>> qdot_certificates;
	qdot_certificates.resize(testSimTypes.size());

	// save location of single leaf at the bottom of the tree
	std::vector<std::vector<Eigen::VectorXd>> leaf_pos_certificates;
	leaf_pos_certificates.resize(testSimTypes.size());

	// average total accum error per step
	std::vector<std::vector<double>> avg_step_error;
	avg_step_error.resize(testSimTypes.size());

	// average num iterations of CG (-1 if N\A)
	std::vector<std::vector<double>> CG_iterations;
	CG_iterations.resize(testSimTypes.size());

	// total time trackers
	std::vector<std::vector<double>> total_time;
	total_time.resize(testSimTypes.size());

	// solver time trackers
	std::vector<std::vector<double>> solve_time;
	solve_time.resize(testSimTypes.size());

	// temporary trackers
	double accum_error;
	double accum_iterations;
	double accum_total_time;
	double accum_solve_time;

	std::vector<int> num_links;
	std::vector<int> r_dofs;
	std::vector<int> r_r;
	std::vector<int> m_dofs;
	for (int n = 0; n < testSizes.size(); ++n)
	{
		int n_save = n;
		n = testSizes[n];

		for (int i = 0; i < testSimTypes.size(); ++i)
		{
			Eigen::VectorXd result;
			int count = 0;
			accum_error = 0;
			accum_iterations = 0;
			accum_total_time = 0;
			accum_solve_time = 0;

			rigid_body->reset();
			rigid_body->setSolveType(testSimTypes[i]);
			//rigid_body->loadUmbrella(n);
			//rigid_body->loadBridge(n, n);
			//rigid_body->loadSimpleBridge(n);
			//rigid_body->loadTree(n);
			rigid_body->loadSimpleTree(n);
			///rigid_body->loadTest(1);
			if (i == 0)
			{
				num_links.push_back(rigid_body->getNumLinks());
				r_dofs.push_back(rigid_body->getRedmaxDOFs());
				m_dofs.push_back(rigid_body->getMaximalDOFs());
				r_r.push_back(rigid_body->getRedmaxVsizes());
			}

			if (render)
			{
				brender->setExportDir("../brenders");
				rigid_body->export_part = 0;
				brender->exportBrender(t);
				rigid_body->export_part = 1;
			}

			if (avg_step_lstsq)
				avg_step_error[i].push_back(0);
			while (t < 0.1)
			{
				if (std::abs(std::fmod(t, 1.0)) < 1e-2)
					std::cout << int(t) << " ";

				auto start = std::chrono::steady_clock::now();

				result = rigid_body->step();

				auto end = std::chrono::steady_clock::now();
				auto diff = end - start;
				auto time = std::chrono::duration<double, std::nano>(diff).count();
				accum_total_time += time;

				if (render && torend % 2 == 0)
				{
					brender->exportBrender(t);
				}
				torend++;

				// save all 'ground truth' results
				if (ground_truth)
					raw_result[i].push_back(rigid_body->step());

				accum_solve_time += rigid_body->getSolveTime();

				if(avg_step_lstsq)
					avg_step_error[i][n] += (raw_result[0][n] - result).norm();

				// number of CG iterations (-1 if no CG performed)
				if (iterations)
					accum_iterations = (accum_iterations*count + rigid_body->getCGIterations()) / (double)(count + 1.0);

				++count;
			}
			if (qdot_cert)
				qdot_certificates[i].push_back(result);
			if (leaf_pos_cert)
				leaf_pos_certificates[i].push_back(rigid_body->getLeafCertificate());
			if (avg_step_lstsq)
				avg_step_error[i][n] = avg_step_error[i][n] / (double)count;
			if (iterations)
				CG_iterations[i].push_back(accum_iterations);
			solve_time[i].push_back(accum_solve_time);
			total_time[i].push_back(accum_total_time);
			std::cout << std::endl << std::endl;
			std::cout << "Simulation  " << std::to_string(n) <<
				" complete for " << testNames[i] << std::endl;
		}

		if (render)
		{
			rigid_body->export_part = 2;
			brender->exportBrender(t);
		}

		std::ofstream outfile(filename, std::ios::out);

		outfile << "n_dofs = [\n";
		for (int r = 0; r < r_dofs.size(); r++)
		{
			outfile << std::to_string(r_dofs[r]) << "; ";
			//outfile << std::to_string((std::pow(2, r - 1) - 2) + (std::pow(2, r) - 1)) << "; ";
		}
		outfile << "];\n";

		outfile << "n_r = [\n";
		for (int r = 0; r < r_r.size(); r++)
		{
			outfile << std::to_string(r_r[r]) << "; ";
			//outfile << std::to_string((std::pow(2, r - 1) - 2) + (std::pow(2, r) - 1)) << "; ";
		}
		outfile << "];\n";

		outfile << "max_dofs = [\n";
		for (int r = 0; r < m_dofs.size(); r++)
		{
			outfile << std::to_string(m_dofs[r]) << "; ";
			//outfile << std::to_string((std::pow(2, r - 1) - 2) + (std::pow(2, r) - 1)) << "; ";
		}
		outfile << "];\n";

		outfile << "n_links = [\n";
		for (int r = 0; r < num_links.size(); r++)
		{
			outfile << std::to_string(num_links[r]) << "; ";
			//outfile << std::to_string((std::pow(2, r - 1) - 2) + (std::pow(2, r) - 1)) << "; ";
		}
		outfile << "];\n";

		// total times
		for (int i = 0; i < testSimTypes.size(); ++i)
		{
			outfile << testNames[i] << "_total_time = [\n";
			for (int r = 0; r < total_time[i].size(); r++)
			{
				outfile << total_time[i][r] << "; ";
			}
			outfile << "];\n";
		}

		// solver times
		for (int i = 0; i < testSimTypes.size(); ++i)
		{
			outfile << testNames[i] << "_solve_time = [\n";
			for (int r = 0; r < solve_time[i].size(); r++)
			{
				outfile << solve_time[i][r] << "; ";
			}
			outfile << "];\n";
		}

		// iterations
		if (iterations)
		{
			for (int i = 0; i < testSimTypes.size(); ++i)
			{
				outfile << testNames[i] << "_iterations = [\n";
				for (int r = 0; r < CG_iterations[i].size(); r++)
				{
					outfile << CG_iterations[i][r] << "; ";
				}
				outfile << "];\n";
			}
		}

		// accumulated squared error
		if (avg_step_lstsq)
		{
			for (int i = 0; i < testSimTypes.size(); ++i)
			{
				outfile << testNames[i] << "_error = [\n";
				for (int r = 0; r < avg_step_error[i].size(); r++)
				{
					outfile << avg_step_error[i][r] << "; ";
				}
				outfile << "];\n";
			}
		}

		// certificates
		if (qdot_cert)
		{
			for (int i = 0; i < testSimTypes.size(); ++i)
			{
				outfile << testNames[i] << "_qdot_certificates = [\n";
				for (int r = 0; r < qdot_certificates[i].size(); r++)
				{
					outfile << qdot_certificates[i][r] << "; ";
				}
				outfile << "];\n";
			}
		}

		if (leaf_pos_cert)
		{
			for (int i = 0; i < testSimTypes.size(); ++i)
			{
				outfile << testNames[i] << "_leaf_pos_certificates = [\n";
				for (int r = 0; r < leaf_pos_certificates[i].size(); r++)
				{
					outfile << leaf_pos_certificates[i][r][0] << " "
						<< leaf_pos_certificates[i][r][1] << " "
						<< leaf_pos_certificates[i][r][2] << "; ";
				}
				outfile << "];\n";
			}
		}
		outfile.close();

		n = n_save;
	}
}

void Scene::displayActions()
{
	rigid_body->displayUserActions();
}

void Scene::draw(std::unique_ptr<MatrixStack> &MV, const std::unique_ptr<Program> &prog) const
{
	rigid_body->draw(MV, prog);
}

void Scene::drawJoints(std::unique_ptr<MatrixStack>& MV) const
{
	rigid_body->drawJoints(MV);
}

void Scene::drawPoints(std::unique_ptr<MatrixStack>& MV, const std::unique_ptr<Program>& prog) const
{
	rigid_body->drawPoints(MV, prog);
}


// Needed for backward compatibility

#include "simpleFilter.h"
#include "simConst.h"
#include "MyMath.h"
#include <boost/lexical_cast.hpp>
#include <boost/format.hpp>

CSimpleFilter::CSimpleFilter()
{
    _filterType=-1;
    _customFilterHeader=-1;
    _customFilterID=-1;
    _customFilterName="";
    _enabled=true;
    _filterVersion=0;
    static int uniqueIDCounter=0;
    _uniqueID=uniqueIDCounter++;
}

CSimpleFilter::~CSimpleFilter()
{

}

void CSimpleFilter::setFilterType(int t)
{ // Is called during loading!! (keep simple)
    _filterType=t;
    _byteParameters.clear();
    _floatParameters.clear();
    _intParameters.clear();

    if (_filterType>=sim_filtercomponent_customized_deprecated)
        _filterType=-1;

    // Now set default parameter values
    if (_filterType==sim_filtercomponent_uniformimage_deprecated)
    {
        _floatParameters.push_back(1.0f); // r
        _floatParameters.push_back(1.0f); // g
        _floatParameters.push_back(1.0f); // b
    }
    if (_filterType==sim_filtercomponent_3x3filter_deprecated)
    {
        _intParameters.push_back(1); // number of passes
        // We calculate a Gaussian blur:
        // 9 matrix elements, plus one element for the multiplication factor
        _floatParameters.resize(9+1,1.0f);
        const float sigma=2.0f;
        float tot=0.0f;
        for (int i=-1;i<2;i++)
        {
            for (int j=-1;j<2;j++)
            {
                float v=pow(2.7182818f,-(i*i+j*j)/(2.0f*sigma*sigma))/(2.0f*piValue_f*sigma*sigma);
                _floatParameters[i+1+(j+1)*3]=v;
                tot+=v;
            }
        }
        for (int i=0;i<9;i++)
            _floatParameters[i]/=tot;
    }
    if (_filterType==sim_filtercomponent_5x5filter_deprecated)
    {
        _intParameters.push_back(1); // number of passes
        // We calculate a Gaussian blur:
        // 25 matrix elements, plus one element for the multiplication factor
        _floatParameters.resize(25+1,1.0f);
        const float sigma=2.0f;
        float tot=0.0f;
        for (int i=-2;i<3;i++)
        {
            for (int j=-2;j<3;j++)
            {
                float v=pow(2.7182818f,-(i*i+j*j)/(2.0f*sigma*sigma))/(2.0f*piValue_f*sigma*sigma);
                _floatParameters[i+2+(j+2)*5]=v;
                tot+=v;
            }
        }
        for (int i=0;i<25;i++)
            _floatParameters[i]/=tot;
    }
    if (_filterType==sim_filtercomponent_keeporremovecolors_deprecated)
    {
        _byteParameters.push_back(1+2); // bit 0: keep (otherwise remove), bit 1: rgb (otherwise hsl), bit 2: copy removed part to buffer 1
        _floatParameters.push_back(0.5f); // r or h value
        _floatParameters.push_back(0.5f); // g or s value
        _floatParameters.push_back(0.5f); // b or l value
        _floatParameters.push_back(0.25); // r or h tolerance (x --> +-x)
        _floatParameters.push_back(0.25); // g or s tolerance (x --> +-x)
        _floatParameters.push_back(0.25); // b or l tolerance (x --> +-x)
    }
    if (_filterType==sim_filtercomponent_scaleandoffsetcolors_deprecated)
    {
        _byteParameters.push_back(2); // bit 0: not used yet, bit 1: rgb (otherwise hsl)
        _floatParameters.push_back(0.0f); // r or h offset (before scaling)
        _floatParameters.push_back(0.0f); // g or s offset (before scaling)
        _floatParameters.push_back(0.0f); // b or l offset (before scaling)
        _floatParameters.push_back(1.2f); // r or h scale
        _floatParameters.push_back(1.2f); // g or s scale
        _floatParameters.push_back(1.2f); // b or l scale
        _floatParameters.push_back(0.0f); // r or h offset (after scaling)
        _floatParameters.push_back(0.0f); // g or s offset (after scaling)
        _floatParameters.push_back(0.0f); // b or l offset (after scaling)
    }
    if (_filterType==sim_filtercomponent_correlationwithbuffer1_deprecated)
    {
        _byteParameters.push_back(8+16); // 8: overlay display, 16: greyscale calculations
        _floatParameters.push_back(0.5f); // sub-image size X (template)
        _floatParameters.push_back(0.5f); // sub-image size Y (template)
        _floatParameters.push_back(1.0f); // search frame X
        _floatParameters.push_back(1.0f); // search frame Y
    }
    if (_filterType==sim_filtercomponent_shift_deprecated)
    {
        _byteParameters.push_back(0); // bit 0: if set, then no wrapping
        _floatParameters.push_back(0.5f); // x-shift (-1 to 1)
        _floatParameters.push_back(0.5f); // y-shift (-1 to 1)
    }
    if (_filterType==sim_filtercomponent_circularcut_deprecated)
    {
        _byteParameters.push_back(0); // bit 0: if set, cut part is copied to buffer 1
        _floatParameters.push_back(1.0f); // circle radius relative to smallest half-size
    }
    if (_filterType==sim_filtercomponent_colorsegmentation_deprecated)
        _floatParameters.push_back(0.3f); // max color-color distance
    if (_filterType==sim_filtercomponent_blobextraction_deprecated)
    {
        _byteParameters.push_back(1); // bit0: use different color for each blob
        _floatParameters.push_back(0.1f); // threshold
        _floatParameters.push_back(0.0f); // min blob size
    }
    if (_filterType==sim_filtercomponent_imagetocoord_deprecated)
    {
        _intParameters.push_back(32); // number of points along x
        _intParameters.push_back(32); // number of points along y
        _byteParameters.push_back(0); // BitCoded. bit 1 indicates we want overlap with visible pixels
    }
    if (_filterType==sim_filtercomponent_velodyne_deprecated)
    {
        _intParameters.push_back(32); // number of points along x
        _intParameters.push_back(32); // number of points along y
        _intParameters.push_back(0); // reserved
        _intParameters.push_back(0); // reserved
        _floatParameters.push_back(30.0f*piValue_f/180.0f); // vertical scan angle
        _floatParameters.push_back(0.0f); // reserved
        _floatParameters.push_back(0.0f); // reserved
    }

    if (_filterType==sim_filtercomponent_pixelchange_deprecated)
    {
        _floatParameters.push_back(0.1f); // threshold in percent
    }
    if (_filterType==sim_filtercomponent_rectangularcut_deprecated)
    {
        _byteParameters.push_back(0); // bit 0: if set, cut part is copied to buffer 1
        _floatParameters.push_back(0.9f); // x-size (0-1)
        _floatParameters.push_back(0.9f); // y-size (0-1)
    }
    if (_filterType==sim_filtercomponent_resize_deprecated)
    {
        _floatParameters.push_back(1.2f); // x-scale
        _floatParameters.push_back(1.2f); // y-scale
    }
    if (_filterType==sim_filtercomponent_rotate_deprecated)
    {
        _floatParameters.push_back(45.0f*degToRad_f); // rotation angle
    }
    if (_filterType==sim_filtercomponent_edge_deprecated)
        _floatParameters.push_back(0.1f); // threshold (0 to 1)
    if (_filterType==sim_filtercomponent_intensityscale_deprecated)
    {
        _intParameters.push_back(1); // 0=grey scale, 1=intensity scale
        _floatParameters.push_back(0.0f); // scale start
        _floatParameters.push_back(1.0f); // scale end
    }
    if (_filterType==sim_filtercomponent_binary_deprecated)
    {
        _byteParameters.push_back(4+8); // 4:trigger enabled, 8:overlay display
        _floatParameters.push_back(0.1f); // threshold
        _floatParameters.push_back(0.0f); // free to use
        _floatParameters.push_back(0.0f); // free to use
        _floatParameters.push_back(0.0f); // free to use
        _floatParameters.push_back(0.5f); // '1' proportion for trigger
        _floatParameters.push_back(0.25f); // '1' proportion tolerance for trigger
        _floatParameters.push_back(0.5f); // x center of mass for trigger
        _floatParameters.push_back(0.25f); // x center of mass tolerance for trigger
        _floatParameters.push_back(0.5f); // y center of mass for trigger
        _floatParameters.push_back(0.25f); // y center of mass tolerance for trigger
        _floatParameters.push_back(0.0f); // orientation for trigger
        _floatParameters.push_back(90.01f*degToRad_f); // orientation tolerance for trigger
        _floatParameters.push_back(0.9f); // roundness for trigger
    }
}

int CSimpleFilter::getFilterType()
{
    return(_filterType);
}

void CSimpleFilter::serialize(CSer& ar)
{
    if (!ar.isStoring())
    {       // Loading
        int byteQuantity;
        std::string theName="";
        while (theName.compare(SER_END_OF_OBJECT)!=0)
        {
            theName=ar.readDataName();
            if (theName.compare(SER_END_OF_OBJECT)!=0)
            {
                bool noHit=true;
                if (theName.compare("Sft")==0)
                {
                    noHit=false;
                    ar >> byteQuantity;
                    ar >> _filterType;
                    setFilterType(_filterType); // to reserve default space of byte-, int- and float-parameters!
                    // above command overwrites header, id and name!!
                    ar >> _customFilterHeader >> _customFilterID;
                }
                if (theName.compare("Sfv")==0)
                {
                    noHit=false;
                    ar >> byteQuantity;
                    ar >> _filterVersion;
                }
                if (theName.compare("Cfn")==0)
                {
                    noHit=false;
                    ar >> byteQuantity;
                    ar >> _customFilterName;
                }
                if (theName.compare("Pab")==0)
                {
                    noHit=false;
                    ar >> byteQuantity;
                    int s;
                    ar >> s;
                    unsigned char ddd;
                    for (int i=0;i<s;i++)
                    {
                        ar >> ddd;
                        if (i<int(_byteParameters.size())) // make sure we use the latest vector size
                            _byteParameters[i]=ddd;
                    }
                }
                if (theName.compare("Pai")==0)
                {
                    noHit=false;
                    ar >> byteQuantity;
                    int s;
                    ar >> s;
                    int ddd;
                    for (int i=0;i<s;i++)
                    {
                        ar >> ddd;
                        if (i<int(_intParameters.size())) // make sure we use the latest vector size
                            _intParameters[i]=ddd;
                    }
                }
                if (theName.compare("Paf")==0)
                {
                    noHit=false;
                    ar >> byteQuantity;
                    int s;
                    ar >> s;
                    float ddd;
                    for (int i=0;i<s;i++)
                    {
                        ar >> ddd;
                        if (i<int(_floatParameters.size())) // make sure we use the latest vector size
                            _floatParameters[i]=ddd;
                    }
                }
                if (theName.compare("Cfp")==0)
                {
                    noHit=false;
                    ar >> byteQuantity;
                    int s;
                    ar >> s;
                    _customFilterParameters.clear();
                    unsigned char ddd;
                    for (int i=0;i<s;i++)
                    {
                        ar >> ddd;
                        _customFilterParameters.push_back(ddd);
                    }
                }
                if (theName=="Sfe")
                {
                    noHit=false;
                    ar >> byteQuantity;
                    unsigned char nothing;
                    ar >> nothing;
                    _enabled=SIM_IS_BIT_SET(nothing,0);
                }
                if (noHit)
                    ar.loadUnknownData();
            }
        }
    }
}

std::string CSimpleFilter::getCodeEquivalent()
{
    std::string str;
    if (_filterType==sim_filtercomponent_originalimage_deprecated)
        str="simVision.sensorImgToWorkImg(inData.handle)";
    if (_filterType==sim_filtercomponent_originaldepth_deprecated)
        str="simVision.sensorDepthMapToWorkImg(inData.handle)";
    if (_filterType==sim_filtercomponent_uniformimage_deprecated)
        str=boost::str(boost::format("simVision.uniformImgToWorkImg(inData.handle,{%f,%f,%f})") % _floatParameters[0] % _floatParameters[1] % _floatParameters[2]);
    if (_filterType==sim_filtercomponent_tooutput_deprecated)
        str="simVision.workImgToSensorImg(inData.handle,false)";
    if (_filterType==sim_filtercomponent_todepthoutput_deprecated)
        str="simVision.workImgToSensorDepthMap(inData.handle,false)";

    if (_filterType==sim_filtercomponent_tobuffer1_deprecated)
        str="simVision.workImgToBuffer1(inData.handle)";
    if (_filterType==sim_filtercomponent_tobuffer2_deprecated)
        str="simVision.workImgToBuffer2(inData.handle)";

    if (_filterType==sim_filtercomponent_frombuffer1_deprecated)
        str="simVision.buffer1ToWorkImg(inData.handle)";
    if (_filterType==sim_filtercomponent_frombuffer2_deprecated)
        str="simVision.buffer2ToWorkImg(inData.handle)";

    if (_filterType==sim_filtercomponent_swapwithbuffer1_deprecated)
        str="simVision.swapWorkImgWithBuffer1(inData.handle)";
    if (_filterType==sim_filtercomponent_addtobuffer1_deprecated)
        str="simVision.addWorkImgToBuffer1(inData.handle)";
    if (_filterType==sim_filtercomponent_subtractfrombuffer1_deprecated)
        str="simVision.subtractWorkImgFromBuffer1(inData.handle)";

    if (_filterType==sim_filtercomponent_swapbuffers_deprecated)
        str="simVision.swapBuffers(inData.handle)";

    if (_filterType==sim_filtercomponent_addbuffer1_deprecated)
        str="simVision.addBuffer1ToWorkImg(inData.handle)";
    if (_filterType==sim_filtercomponent_subtractbuffer1_deprecated)
        str="simVision.subtractBuffer1FromWorkImg(inData.handle)";
    if (_filterType==sim_filtercomponent_multiplywithbuffer1_deprecated)
        str="simVision.multiplyWorkImgWithBuffer1(inData.handle)";

    if (_filterType==sim_filtercomponent_horizontalflip_deprecated)
        str="simVision.horizontalFlipWorkImg(inData.handle)";
    if (_filterType==sim_filtercomponent_verticalflip_deprecated)
        str="simVision.verticalFlipWorkImg(inData.handle)";
    if (_filterType==sim_filtercomponent_rotate_deprecated)
        str=boost::str(boost::format("simVision.rotateWorkImg(inData.handle,%f)") % _floatParameters[0]);
    if (_filterType==sim_filtercomponent_shift_deprecated)
        str=boost::str(boost::format("simVision.shiftWorkImg(inData.handle,{%f,%f},%s)") % _floatParameters[0] % _floatParameters[1] % ( ((_byteParameters[0]&1)==0) ? "true" : "false") );
    if (_filterType==sim_filtercomponent_resize_deprecated)
        str=boost::str(boost::format("simVision.resizeWorkImg(inData.handle,{%f,%f})") % _floatParameters[0] % _floatParameters[1]);
    if (_filterType==sim_filtercomponent_3x3filter_deprecated)
        str=boost::str(boost::format("simVision.matrix3x3OnWorkImg(inData.handle,%i,%f,{%f,%f,%f,%f,%f,%f,%f,%f,%f})") % _intParameters[0] % _floatParameters[9] % _floatParameters[0] % _floatParameters[1] % _floatParameters[2] % _floatParameters[3] % _floatParameters[4] % _floatParameters[5] % _floatParameters[6] % _floatParameters[7] % _floatParameters[8]);
    if (_filterType==sim_filtercomponent_5x5filter_deprecated)
        str=boost::str(boost::format("simVision.matrix5x5OnWorkImg(inData.handle,%i,%f,{%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f})") % _intParameters[0] % _floatParameters[25] % _floatParameters[0] % _floatParameters[1] % _floatParameters[2] % _floatParameters[3] % _floatParameters[4] % _floatParameters[5] % _floatParameters[6] % _floatParameters[7] % _floatParameters[8] % _floatParameters[9] % _floatParameters[10] % _floatParameters[11] % _floatParameters[12] % _floatParameters[13] % _floatParameters[14] % _floatParameters[15] % _floatParameters[16] % _floatParameters[17] % _floatParameters[18] % _floatParameters[19] % _floatParameters[20] % _floatParameters[21] % _floatParameters[22] % _floatParameters[23] % _floatParameters[24]);
    if (_filterType==sim_filtercomponent_rectangularcut_deprecated)
        str=boost::str(boost::format("simVision.rectangularCutWorkImg(inData.handle,{%f,%f},%s)") % _floatParameters[0] % _floatParameters[1] % ( ((_byteParameters[0]&1)!=0) ? "true" : "false"));
    if (_filterType==sim_filtercomponent_circularcut_deprecated)
        str=boost::str(boost::format("simVision.circularCutWorkImg(inData.handle,%f,%s)") % _floatParameters[0] % ( ((_byteParameters[0]&1)!=0) ? "true" : "false"));

    if (_filterType==sim_filtercomponent_normalize_deprecated)
        str="simVision.normalizeWorkImg(inData.handle)";
    if (_filterType==sim_filtercomponent_colorsegmentation_deprecated)
        str=boost::str(boost::format("simVision.colorSegmentationOnWorkImg(inData.handle,%f)") % _floatParameters[0]);
    if (_filterType==sim_filtercomponent_intensityscale_deprecated)
        str=boost::str(boost::format("simVision.intensityScaleOnWorkImg(inData.handle,%f,%f,%s)") % _floatParameters[0] % _floatParameters[1]  % ( ((_intParameters[0]&1)==0) ? "true" : "false"));
    if (_filterType==sim_filtercomponent_keeporremovecolors_deprecated)
        str=boost::str(boost::format("simVision.selectiveColorOnWorkImg(inData.handle,{%f,%f,%f},{%f,%f,%f},%s,%s,%s)") % _floatParameters[0] % _floatParameters[1] % _floatParameters[2] % _floatParameters[3] % _floatParameters[4] % _floatParameters[5] % ( ((_byteParameters[0]&2)!=0) ? "true" : "false") % ( ((_byteParameters[0]&1)!=0) ? "true" : "false") % ( ((_byteParameters[0]&4)!=0) ? "true" : "false"));
    if (_filterType==sim_filtercomponent_scaleandoffsetcolors_deprecated)
        str=boost::str(boost::format("simVision.scaleAndOffsetWorkImg(inData.handle,{%f,%f,%f},{%f,%f,%f},{%f,%f,%f},%s)") % _floatParameters[0] % _floatParameters[1] % _floatParameters[2] % _floatParameters[3] % _floatParameters[4] % _floatParameters[5] % _floatParameters[6] % _floatParameters[7] % _floatParameters[8] % ( ((_byteParameters[0]&2)!=0) ? "true" : "false"));
//    if (_filterType==sim_filtercomponent_correlationwithbuffer1)
//

    if (_filterType==sim_filtercomponent_sharpen_deprecated)
        str="simVision.sharpenWorkImg(inData.handle)";
    if (_filterType==sim_filtercomponent_edge_deprecated)
        str=boost::str(boost::format("simVision.edgeDetectionOnWorkImg(inData.handle,%f)") % _floatParameters[0]);
    if (_filterType==sim_filtercomponent_binary_deprecated)
        str=boost::str(boost::format("local trig,packedPacket=simVision.binaryWorkImg(inData.handle,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%s)\n    if trig then\n        retVal.trigger=true\n    end\n    if packedPacket then\n        retVal.packedPackets[#retVal.packedPackets+1]=packedPacket\n    end") % _floatParameters[0] % _floatParameters[4] % _floatParameters[5] % _floatParameters[6] % _floatParameters[7] % _floatParameters[8] % _floatParameters[9] % _floatParameters[10] % _floatParameters[11] % _floatParameters[12] % ( ((_byteParameters[0]&4)!=0) ? "true" : "false"));
    if (_filterType==sim_filtercomponent_blobextraction_deprecated)
        str=boost::str(boost::format("local trig,packedPacket=simVision.blobDetectionOnWorkImg(inData.handle,%f,%f,%s)\n    if trig then\n        retVal.trigger=true\n    end\n    if packedPacket then\n        retVal.packedPackets[#retVal.packedPackets+1]=packedPacket\n    end") % _floatParameters[0] % _floatParameters[1] % ( ((_byteParameters[0]&1)!=0) ? "true" : "false"));
    if (_filterType==sim_filtercomponent_imagetocoord_deprecated)
        str=boost::str(boost::format("local trig,packedPacket=simVision.coordinatesFromWorkImg(inData.handle,{%i,%i},%s)\n    if trig then\n        retVal.trigger=true\n    end\n    if packedPacket then\n        retVal.packedPackets[#retVal.packedPackets+1]=packedPacket\n    end") % _intParameters[0] % _intParameters[1] % ( ((_byteParameters[0]&1)!=0) ? "true" : "false"));
    if (_filterType==sim_filtercomponent_pixelchange_deprecated)
        str=boost::str(boost::format("local trig,packedPacket=simVision.changedPixelsOnWorkImg(inData.handle,%f)\n    if trig then\n        retVal.trigger=true\n    end\n    if packedPacket then\n        retVal.packedPackets[#retVal.packedPackets+1]=packedPacket\n    end") % _floatParameters[0]);
    if (_filterType==sim_filtercomponent_velodyne_deprecated)
        str=boost::str(boost::format("local trig,packedPacket=simVision.velodyneDataFromWorkImg(inData.handle,{%i,%i},%f)\n    if trig then\n        retVal.trigger=true\n    end\n    if packedPacket then\n        retVal.packedPackets[#retVal.packedPackets+1]=packedPacket\n    end") % _intParameters[0] % _intParameters[1] % _floatParameters[0]);
    if (!_enabled)
        str=std::string("-- ")+str;
    return(str);
}


/*
 * ----------------- BEGIN LICENSE BLOCK ---------------------------------
 *
 * Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
 * de Barcelona (UAB).
 * Copyright (C) 2019-2021 Intel Corporation
 *
 * SPDX-License-Identifier: MIT
 *
 * ----------------- END LICENSE BLOCK -----------------------------------
 */

#include "opendrive/parser/GeoReferenceParser.h"
#include "opendrive/types.hpp"

#include <boost/algorithm/string.hpp>
#include <iostream>
#include <limits>
#include <vector>

#define ACCEPT_USE_OF_DEPRECATED_PROJ_API_H
#include <proj_api.h>

namespace opendrive {
namespace parser {

::opendrive::geom::GeoLocation GeoReferenceParser::Parse(const std::string &geo_reference_string)
{
  ::opendrive::geom::GeoLocation result{
    std::numeric_limits<double>::quiet_NaN(), std::numeric_limits<double>::quiet_NaN(), 0.0, ""};
  result.projection = geo_reference_string;

  auto projPtr = pj_init_plus(geo_reference_string.c_str());

  if (projPtr != nullptr)
  {
    projXY refPoint;
    refPoint.u = 0;
    refPoint.v = 0;
    const auto geoPoint = pj_inv(refPoint, projPtr);
    result.longitude = geoPoint.u * RAD_TO_DEG;
    result.latitude = geoPoint.v * RAD_TO_DEG;
  }
  else
  {
    std::vector<std::string> geo_ref_splitted;
    boost::split(geo_ref_splitted, geo_reference_string, boost::is_any_of(" "));
    for (auto const &element : geo_ref_splitted)
    {
      std::vector<std::string> geo_ref_key_value;
      boost::split(geo_ref_key_value, element, boost::is_any_of("="));
      if (geo_ref_key_value.size() != 2u)
      {
        continue;
      }

      if (geo_ref_key_value[0] == "+lat_0")
      {
        result.latitude = std::stod(geo_ref_key_value[1]);
      }
      else if (geo_ref_key_value[0] == "+lon_0")
      {
        result.longitude = std::stod(geo_ref_key_value[1]);
      }
    }
  }

  return result;
}

} // namespace parser
} // namespace opendrive


/**
*	@file	:	TransformTEST.cpp
*	@author	:	Berkay Gonenc Goksal->berkaygg@hotmail.com
*	@date	:	12.12.2019
*	@brief	:	Bu kod parcacigi Transform classinin fonksiyonlarini test eder.
*/
#include <iostream>
#include <list>
#include <iterator>
#include <Eigen/Dense>
#include"Transform.h"
using namespace std;
int main()
{
	list<Point> A, C, temp;
	Point p1, *p2 = new Point[2];
	list<Point>::iterator iter;
	Transform T;
	PointCloud D;
	Eigen::Matrix4d testMatrix;
	p1.setX(150);
	p1.setY(150);
	p1.setZ(-50);
	A.push_back(p1);

	D.setpointNumber(2);
	T.setAngles(0, 0, -90.0);
	T.setTrans(100, 500, 50);
	double *ang, *tr;
	ang = T.getAngles();
	tr = T.getTrans();
	//Aci degerlerimiz duzgun atandi mi diye kontrol edildi.
	for (int i = 0; i < 3; i++) {
		cout << *(ang + i) << endl;
	}
	cout << "---------------------------" << endl;
	//Trans x,y,z degerlerimiz duzgun atandi mi diye kontrol edildi.
	for (int i = 0; i < 3; i++)
	{
		cout << *(tr + i) << endl;
	}
	cout << "---------------------------" << endl;
	T.setRotation(T.getAngles());//Rotasyon matrisi olusturuldu.
	T.setTranslation(T.getTrans());//Donusum matrisi olusturuldu.
	int k = 0;
	testMatrix = T.getTransMatrix();
	for (int i = 0; i < 4; i++)
	{
		for (int j = 0; j < 4; j++) {
			cout << testMatrix(i, j) << " ";//Donusum matrisi duzgun sekilde olusturuldu mu diye kontrol edildi.
		}
		cout << endl;
	}
	cout << "---------------------------" << endl;

	A = T.doTransform(A);//Point donusumu yapiliyor mu diye kontrol edildi.
	iter = A.begin();
	cout << "C " << iter->getX() << " " << iter->getY() << " " << iter->getZ() << endl;
	cout << "---------------------------" << endl;
	p2[0].setX(150);
	p2[0].setY(150);
	p2[0].setZ(-50);

	p2[1].setX(2);
	p2[1].setY(5);
	p2[1].setZ(1);
	temp.push_back(*p2);

	for (int i = 0; i < 2; i++)//PointCloud donusumu yapildi mi diye kontrol edildi
	{
		D.setPoints(temp);
		T.doTransform(D);
	}
	system("pause");
}