/*
 * OptimalControlProblem.hpp
 *
 *  Created on: July 7, 2019
 *      Author: Quincy Jones
 *
 * Copyright (c) <2019> <Quincy Jones - quincy@implementedrobotics.com/>
 * 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.
 */
#ifndef NOMAD_CORE_OPTIMALCONTROL_OPTIMALCONTROLPROBLEM_H_
#define NOMAD_CORE_OPTIMALCONTROL_OPTIMALCONTROLPROBLEM_H_

#include <iostream>
#include <Eigen/Dense>
#include <qpOASES.hpp>

namespace OptimalControl
{

    class OptimalControlProblem
    {

    public:
        // Base Class Optimal Control Problem
        // N = Prediction Steps
        // T = Horizon Length
        // num_states = Number of States of OCP
        // num_inputs = Number of Inputs of OCP
        OptimalControlProblem(const unsigned int N, const double T, const unsigned int num_states, const unsigned int num_inputs, const unsigned int max_iterations = 1000);

        // Solve
        virtual void Solve() = 0;

        // Get System State Trajectory Vector
        // TODO: Compute this depending on U solutions.  X_ = Ax+Bu
        // TODO: Make pure virtual
        virtual Eigen::MatrixXd X() const { return X_; }

        //Get Current Input Sequence Solution
        Eigen::MatrixXd U() const { return U_; }

        // Set Weight Matrices
        virtual void SetWeights(const Eigen::VectorXd &Q, const Eigen::VectorXd &R)
        {
            // TODO: Verify Vector Size Matches correct state and inputs
            Q_ = Q.matrix().asDiagonal().toDenseMatrix();
            R_ = R.matrix().asDiagonal().toDenseMatrix();
        }

        // Set Problem Initial Condition
        void SetInitialCondition(const Eigen::VectorXd &x_0) { x_0_ = x_0; }

        // Set Reference Trajectory
        void SetReference(const Eigen::MatrixXd &X_ref) { X_ref_ = X_ref; }

        // Time step sample time
        double SampleTime() const { return T_s_; }

        // Prediction Horizon Steps
        int N() const { return N_; }

    protected:
        Eigen::VectorXd x_0_;   // Current State/Initial Condition
        Eigen::MatrixXd X_ref_; // Reference Trajectory

        Eigen::MatrixXd X_; // System State Trajectory
        Eigen::MatrixXd U_; // Optimal Control Input Sequence Solution

        Eigen::MatrixXd Q_; // State Weights
        Eigen::MatrixXd R_; // Input Weights

        int num_states_; // Number of System States
        int num_inputs_; // Number of System Inputs

        int N_; // Number of Prediction Steps

        double T_s_; // Sample Time
        double T_;   // Horizon Length

        qpOASES::int_t max_iterations_;    // Max Iterations for Solver
        qpOASES::int_t solver_iterations_; // Total number of Solver iterations for solution
        double solver_time_;    // Total time for Solver to compute a solution

        bool solved_;
        // TODO:
        // Infeasible, BlahBlah
    };
} // namespace OptimalControl

namespace OptimalControl
{
    namespace LinearOptimalControl
    {
        class LinearOptimalControlProblem : public OptimalControlProblem
        {
        public:
            // Base Class Linear Optimal Control Problem
            // N = Prediction Steps
            // T = Horizon Length
            // num_states = Number of States of OCP
            // num_inputs = Number of Inputs of OCP
            // max_iterations = Maximum number of solver iterations
            LinearOptimalControlProblem(const unsigned int N,
                                        const double T,
                                        const unsigned int num_states,
                                        const unsigned int num_inputs,
                                        const unsigned int max_iterations = 1000);

            // Set Model Matrices
            void SetModelMatrices(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B)
            {
                A_ = A;
                B_ = B;
            }

            // Solve
            virtual void Solve();

        protected:
            Eigen::MatrixXd A_; // System State Transition Matrix
            Eigen::MatrixXd B_; // Control Input Matrix
        };
    } // namespace LinearOptimalControl
} // namespace OptimalControl

#endif // NOMAD_CORE_OPTIMALCONTROL_OPTIMALCONTROLPROBLEM_H_


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

// Copyright (c) 2009-2011 Barend Gehrels, Amsterdam, the Netherlands.

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

#include <iostream>
#include <sstream>
#include <fstream>
#include <iomanip>
#include <string>

#define BOOST_GEOMETRY_REPORT_OVERLAY_ERROR
#define BOOST_GEOMETRY_NO_BOOST_TEST

#include <boost/program_options.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/timer.hpp>

#include <test_overlay_p_q.hpp>

#include <boost/geometry/geometry.hpp>
#include <boost/geometry/multi/multi.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/multi/geometries/multi_polygon.hpp>
#include <boost/geometry/domains/gis/io/wkt/wkt.hpp>
#include <boost/geometry/extensions/io/svg/svg_mapper.hpp>

template <typename Polygon, typename Generator>
inline void make_polygon(Polygon& polygon, Generator& generator, bool triangular)
{
    typedef typename bg::point_type<Polygon>::type point_type;
    typedef typename bg::coordinate_type<Polygon>::type coordinate_type;

    coordinate_type x, y;
    x = generator();
    y = generator();

    typename bg::ring_type<Polygon>::type& ring = bg::exterior_ring(polygon);

    point_type p;
    bg::set<0>(p, x); bg::set<1>(p, y); ring.push_back(p);
    bg::set<0>(p, x); bg::set<1>(p, y + 1); ring.push_back(p);
    bg::set<0>(p, x + 1); bg::set<1>(p, y + 1); ring.push_back(p);
    bg::set<0>(p, x + 1); bg::set<1>(p, y); ring.push_back(p);
    bg::set<0>(p, x); bg::set<1>(p, y); ring.push_back(p);

    if (triangular)
    {
        // Remove a point depending on generator
        int c = generator() % 4;
        if (c >= 1 && c <= 3)
        {
            ring.erase(ring.begin() + c);
        }
    }
}

template <typename MultiPolygon, typename Generator>
bool test_recursive_boxes(MultiPolygon& result, int& index,
            Generator& generator,
            int level, bool triangular, p_q_settings const& settings)
{
    MultiPolygon p, q;

    // Generate two boxes
    if (level == 0)
    {
        p.resize(1);
        q.resize(1);
        make_polygon(p.front(), generator, triangular);
        make_polygon(q.front(), generator, triangular);
        bg::correct(p);
        bg::correct(q);
    }
    else
    {
        bg::correct(p);
        bg::correct(q);
        if (! test_recursive_boxes(p, index, generator, level - 1, triangular, settings)
            || ! test_recursive_boxes(q, index, generator, level - 1, triangular, settings))
        {
            return false;
        }
    }

    typedef typename boost::range_value<MultiPolygon>::type polygon;

    std::ostringstream out;
    out << "recursive_box_" << index++ << "_" << level;
    if (! test_overlay_p_q
        <
            polygon,
            typename bg::coordinate_type<MultiPolygon>::type
        >(out.str(), p, q, settings))
    {
        return false;
    }

    MultiPolygon mp;
    bg::detail::union_::union_insert
        <
            polygon
        >(p, q, std::back_inserter(mp));

    bg::unique(mp);
    bg::simplify(mp, result, 0.01);
    bg::correct(mp);
    return true;
}

template <typename T, bool Clockwise, bool Closed>
void test_all(int seed, int count, int field_size, int level, bool triangular, p_q_settings const& settings)
{
    boost::timer t;

    typedef boost::minstd_rand base_generator_type;

    base_generator_type generator(seed);

    boost::uniform_int<> random_coordinate(0, field_size - 1);
    boost::variate_generator<base_generator_type&, boost::uniform_int<> >
        coordinate_generator(generator, random_coordinate);

    typedef bg::model::polygon
        <
            bg::model::d2::point_xy<T>, Clockwise, Closed
        > polygon;
    typedef bg::model::multi_polygon<polygon> mp;

    int index = 0;
    for(int i = 0; i < count; i++)
    {
        mp p;
        test_recursive_boxes<mp>(p, index, coordinate_generator, level, triangular, settings);
    }
    std::cout
        << "polygons: " << index
        << " type: " << string_from_type<T>::name()
        << " time: " << t.elapsed()  << std::endl;
}

int main(int argc, char** argv)
{
    try
    {
        namespace po = boost::program_options;
        po::options_description description("=== recursive_polygons ===\nAllowed options");

        int count = 1;
        int seed = static_cast<unsigned int>(std::time(0));
        int level = 3;
        int field_size = 10;
        bool ccw = false;
        bool open = false;
        p_q_settings settings;
        std::string form = "box";

        description.add_options()
            ("help", "Help message")
            ("seed", po::value<int>(&seed), "Initialization seed for random generator")
            ("count", po::value<int>(&count)->default_value(1), "Number of tests")
            ("diff", po::value<bool>(&settings.also_difference)->default_value(false), "Include testing on difference")
            ("level", po::value<int>(&level)->default_value(3), "Level to reach (higher->slower)")
            ("size", po::value<int>(&field_size)->default_value(10), "Size of the field")
            ("form", po::value<std::string>(&form)->default_value("box"), "Form of the polygons (box, triangle)")
            ("ccw", po::value<bool>(&ccw)->default_value(false), "Counter clockwise polygons")
            ("open", po::value<bool>(&open)->default_value(false), "Open polygons")
            ("wkt", po::value<bool>(&settings.wkt)->default_value(false), "Create a WKT of the inputs, for all tests")
            ("svg", po::value<bool>(&settings.svg)->default_value(false), "Create a SVG for all tests")
        ;

        po::variables_map varmap;
        po::store(po::parse_command_line(argc, argv, description), varmap);
        po::notify(varmap);

        if (varmap.count("help")
            || (form != "box" && form != "triangle"))
        {
            std::cout << description << std::endl;
            return 1;
        }

        bool triangular = form != "box";

        if (ccw && open)
        {
            test_all<double, false, false>(seed, count, field_size, level, triangular, settings);
        }
        else if (ccw)
        {
            test_all<double, false, true>(seed, count, field_size, level, triangular, settings);
        }
        else if (open)
        {
            test_all<double, true, false>(seed, count, field_size, level, triangular, settings);
        }
        else
        {
            test_all<double, true, true>(seed, count, field_size, level, triangular, settings);
        }

#if defined(HAVE_TTMATH)
        // test_all<ttmath_big, true, true>(seed, count, max, svg, level);
#endif
    }
    catch(std::exception const& e)
    {
        std::cout << "Exception " << e.what() << std::endl;
    }
    catch(...)
    {
        std::cout << "Other exception" << std::endl;
    }

    return 0;
}


#ifdef WITH_FASTPD
#include <boost/python.hpp>
#include <string>
#include "inf_def_visitor.hxx"

#include <opengm/python/opengmpython.hxx>
#include <opengm/python/converter.hxx>
#include <opengm/python/numpyview.hxx>
#include <opengm/python/pythonfunction.hxx>

//using namespace boost::python;

#ifdef MIN
   #define OLD_MIN_DEF MIN
   #undef MIN
   #define MIN FAST_PD_MIN
#else

#endif

#ifdef MAX
   #define OLD_MAX_DEF MAX
   #undef MAX
   #define MAX FAST_PD_MAX
#else

#endif

#include <param/fastpd_external_param.hxx>
#include <opengm/inference/external/fastPD.hxx>

#ifdef OLD_MIN_DEF
   #undef MIN
   #define MIN OLD_MIN_DEF
#endif

#ifdef OLD_MAX_DEF
   #undef MAX
   #define MAX OLD_MIN_DEF
#endif

template<class GM,class ACC>
void export_fast_pd(){
   using namespace boost::python;
   import_array();
   append_subnamespace("solver");

   // setup
   InfSetup setup;
   setup.algType      = "primal-dual ";
   setup.dependencies = "This algorithm needs the FastPD from  library from  http://www.csd.uoc.gr/~komod/FastPD/ "
                        "compile OpenGM with CMake-Flag ``WITH_FASTPD`` set to ``ON`` ";

   // export parameter
   typedef opengm::external::FastPD<GM>  PyFastPd;
   exportInfParam<PyFastPd>("_FastPd");
   // export inference
   boost::python::class_< PyFastPd>("_FastPd",init<const GM & >())
   .def(InfSuite<PyFastPd,false>(std::string("FastPd"),setup))
   ;
}

template void export_fast_pd<opengm::python::GmAdder, opengm::Minimizer>();

#endif


#include "stdafx.h"
#include "BoostGraph.h"
#include <sstream>
#include <boost/algorithm/string.hpp>

using std::vector;
using std::string;
using std::stringstream;

namespace
{

const char C_INPUT_TEXT_1[] = R"***(
4 5
2 3 5
1 4 5
1 2 6
1 3 1
3 4 3
)***";

const char C_OUTPUT_1[] = R"***(
9
1 3
2 3
3 4
)***";

// Используем split для игнорирования пробелов и переносов строк при проверке.
vector<string> SplitWords(string const& text)
{
	std::string trimmed = boost::trim_copy(text);

	vector<string> words;
	boost::split(words, trimmed, boost::is_space(), boost::token_compress_on);
	return words;
}

void RunTextInputTestcase(std::string const& inputText, std::string const& expectedOutput)
{
	CBoostGraph graph;
	stringstream input(inputText);
	stringstream output;
	BOOST_CHECK(graph.ReadText(input));
    graph.RunPrima();
    graph.PrintResults(output);
	vector<string> words = SplitWords(output.str());
    vector<string> expectedWords = SplitWords(expectedOutput);
    BOOST_CHECK(words == expectedWords);
}

}

BOOST_AUTO_TEST_SUITE(Graph_with_Bellmand_Ford_algorithm)
	BOOST_AUTO_TEST_CASE(can_load_text_format)
	{
		RunTextInputTestcase(C_INPUT_TEXT_1, C_OUTPUT_1);
    }
BOOST_AUTO_TEST_SUITE_END()


/*
 * Copyright (C) 2014, Computing Systems Laboratory (CSLab), NTUA.
 * Copyright (C) 2014, Vasileios Karakasis
 * All rights reserved.
 *
 * This file is distributed under the BSD License. See LICENSE.txt for details.
 */

/*
 * \file ElementTest.cpp
 * \brief Simple program for testing generic elements
 *
 * \author Vasileios Karakasis
 * \date 2014
 * \copyright This file is distributed under the BSD License. See LICENSE.txt
 * for details.
 */

#include <sparsex/internals/CsxUtil.hpp>
#include <sparsex/internals/DynamicArray.hpp>
#include <sparsex/internals/Element.hpp>
#include <sparsex/internals/SparsePartition.hpp>
#include <sparsex/internals/Statistics.hpp>
#include <sparsex/internals/Utility.hpp>
#include <sparsex/internals/Xform.hpp>
#include <boost/container/vector.hpp>
#include <cassert>
#include <iostream>
#include <vector>

using namespace std;
using namespace sparsex::csx;
using namespace sparsex::utilities;

const size_t RleSize = 4;

int main()
{
  int row = 3;
  int col = 5;
  int delta = 2;
  vector<float> vals;
  for (size_t i = 0; i < RleSize; ++i)
    vals.push_back(5 + 2*i);

  //float *bogus = 0;
  Element<int, float> elem(row, col, vals.begin(), RleSize,
			   make_pair(Encoding::Horizontal, delta));
  Element<int, float> new_elem = elem;
  cout << elem << "\n";
  cout << new_elem << " (copied)\n";
  cout << GetPatternId(elem) << " (patt id)\n";
  assert(elem.IsPattern());

  // Test append in place
  boost::container::vector<Element<int, float> > elems;
  elems.emplace_back(row, col, vals.begin(), RleSize,
		     make_pair(Encoding::Horizontal, delta));
  cout << elems.back() << " (emplaced) \n";

  typename TransformFn<int>::type xform_fn =
    GetXformFn<int>(Encoding::Horizontal, Encoding::Diagonal);

  typename TransformFn<int>::type rev_xform_fn =
    GetXformFn<int>(Encoding::Diagonal, Encoding::Horizontal);

  Element<int, float> xformed = elem;
  xformed.Transform(xform_fn(elem.GetCoordinates(), 10, 10));
  cout << xformed << " (xformed)\n";

  Element<int, float> xformed_back = xformed;
  xformed_back.Transform(rev_xform_fn(xformed.GetCoordinates(),
				      10, 10));
  assert(xformed_back == elem);

  Element<int, float> single(1, 1, 4.3);
  cout << single << " (single)\n";
  xformed.GetMarker().Mark(PatternMarker::InPattern |
			   PatternMarker::PatternStart);
  cout << xformed.GetMarker().GetMark() << " (marked) \n";
  cout << xformed.GetMarker().IsMarked(PatternMarker::InPattern |
				       PatternMarker::PatternStart)
       << " (marked?) \n";

  // Test insertion in dynamic array
  DynamicArray<Element<int, float> > da;
  da.Append(elem);
  da.Append(new_elem);
  da.Append(xformed);
  da.Append(xformed_back);
  da.Append(single);
  Element<int, float> *da_elems = da.TakeElems();
  da.GetAllocator().destroy(da_elems, da.GetSize());
  da.GetAllocator().deallocate(da_elems, da.GetSize());

  // Test statistics
  StatsCollection<StatsData> stats;
  stats.AppendStats(make_pair(Encoding::Horizontal, 1), StatsData(10, 2, 0));
  stats.AppendStats(make_pair(Encoding::Horizontal, 1), StatsData(10, 3, 0));
  stats.AppendStats(make_pair(Encoding::Horizontal, 2), StatsData(5, 3, 0));
  stats.AppendStats(make_pair(Encoding::Vertical, 1), StatsData(50, 2, 0));
  stats.AppendStats(make_pair(Encoding::Vertical, 0), StatsData(0, 0, 10));
  //    stats.AppendStats(make_pair(Encoding::BlockRow2, 128), StatsData(768, 3, 0));
  // Test block splitter
  stats.AppendStats(make_pair(Encoding::BlockRow3, 2), StatsData(84, 14, 0));
  stats.AppendStats(make_pair(Encoding::BlockRow3, 3), StatsData(117, 13, 0));
  stats.AppendStats(make_pair(Encoding::BlockRow3, 4), StatsData(36, 3, 0));
  stats.AppendStats(make_pair(Encoding::BlockRow3, 5), StatsData(45, 3, 0));
  stats.AppendStats(make_pair(Encoding::BlockRow3, 6), StatsData(36, 2, 0));
  stats.AppendStats(make_pair(Encoding::BlockRow3, 12), StatsData(72, 2, 0));

  size_t nr_nzeros = 1000;
  set<Encoding::Instantiation> unfiltered;
  StatsDataScaler scaler(2.0);
  CoverageFilter cfilter(nr_nzeros, 0.1, unfiltered);
  BlockSplitter splitter(255, nr_nzeros, 0.1);
  // stats.ManipulateStats(scaler);
  stats.ManipulateStats(splitter);
  stats.ManipulateStats(cfilter);
  cout << stats.GetInstStats(make_pair(Encoding::Horizontal, 1)) << "\n";
  cout << stats.GetTypeStats(Encoding::Horizontal) << " (horiz)\n";
  cout << stats.GetTypeStats(Encoding::Vertical) << " (vert)\n";
  cout << stats.GetTypeStats(Encoding::BlockRow2) << " (br2)\n";
  cout << stats.GetTypeStats(Encoding::BlockRow3) << " (br3)\n";
  cout << stats.GetGlobalStats() << " (total)\n";
  cout << stats << "\n";
  cout << unfiltered.size() << " (total unfiltered inst)\n";
  //delete[] vals;

  // Test gcd and lcm
  cout << "gcd: " << gcd(44, 4) << "\n";
  cout << "abs: " << abs(-45.3) << "\n";
  cout << "lcm: " << lcm(44, 3) << "\n";
  cout << "iceil: " << iceil(5, 3) << "\n";

  // StatsCollection iterators
  StatsCollection<StatsData>::const_iterator si = stats.cbegin();
  StatsCollection<StatsData>::const_iterator se = stats.cend();
  cout << "Testing StatsCollection iterator\n";
  for (; si != se; ++si) {
    Encoding e((*si).first);
    cout << e.GetShortName() << ": " << (*si).second << "\n";
  }

  // Test append type stats
  StatsCollection<StatsData> new_stats;
  new_stats.AppendStats(Encoding::Horizontal, stats);
  cout << "new_stats: " << new_stats << "\n";

  stats.Clear();
  cout << stats.GetGlobalStats() << " (cleared)\n";
  return 0;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2 euler toolbox - spence/simd Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of euler components in simd mode
//////////////////////////////////////////////////////////////////////////////
/// created  by jt the 22/02/2011
///
#include <nt2/euler/include/functions/spence.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/type_traits/is_same.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/meta/as_floating.hpp>
#include <nt2/sdk/meta/as_signed.hpp>
#include <nt2/sdk/meta/upgrade.hpp>
#include <nt2/sdk/meta/downgrade.hpp>
#include <nt2/sdk/meta/scalar_of.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <boost/type_traits/common_type.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/memory/buffer.hpp>
#include <nt2/constant/constant.hpp>
#include <nt2/sdk/meta/cardinal_of.hpp>
#include <nt2/include/functions/splat.hpp>
#include <nt2/include/functions/load.hpp>

NT2_TEST_CASE_TPL ( spence_real__1_0,  NT2_SIMD_REAL_TYPES)
{
  using nt2::spence;
  using nt2::tag::spence_;
  using nt2::load;
  using boost::simd::native;
  using nt2::meta::cardinal_of;
  typedef NT2_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef typename nt2::meta::upgrade<T>::type   u_t;
  typedef native<T,ext_t>                        n_t;
  typedef n_t                                     vT;
  typedef typename nt2::meta::as_integer<T>::type iT;
  typedef native<iT,ext_t>                       ivT;
  typedef typename nt2::meta::call<spence_(vT)>::type r_t;
  typedef typename nt2::meta::call<spence_(T)>::type sr_t;
  typedef typename nt2::meta::scalar_of<r_t>::type ssr_t;

  // specific values tests
  NT2_TEST_ULP_EQUAL(spence(nt2::Inf<vT>())[0], nt2::Nan<sr_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(spence(nt2::Minf<vT>())[0], nt2::Nan<sr_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(spence(nt2::Mone<vT>())[0], nt2::Nan<sr_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(spence(nt2::Nan<vT>())[0], nt2::Nan<sr_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(spence(nt2::One<vT>())[0], nt2::Zero<sr_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(spence(nt2::Zero<vT>())[0], 1.6449340668482264061, 0.5);
} // end of test for floating_


/*
	This file is part of solidity.

	solidity is free software: you can redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.

	solidity is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU General Public License for more details.

	You should have received a copy of the GNU General Public License
	along with solidity.  If not, see <http://www.gnu.org/licenses/>.
*/
/** @file FixedHash.h
 * @author Gav Wood <i@gavwood.com>
 * @date 2014
 *
 * The FixedHash fixed-size "hash" container type.
 */

#pragma once

#include <graphene/utilities/CommonData.hpp>

#include <boost/functional/hash.hpp>
#include <boost/io/ios_state.hpp>

#include <array>
#include <cstdint>
#include <algorithm>

namespace graphene { namespace utilities {

/// Fixed-size raw-byte array container type, with an API optimised for storing hashes.
/// Transparently converts to/from the corresponding arithmetic type; this will
/// assume the data contained in the hash is big-endian.
template <unsigned N>
class FixedHash
{
public:
	/// The corresponding arithmetic type.
	using Arith = boost::multiprecision::number<boost::multiprecision::cpp_int_backend<N * 8, N * 8, boost::multiprecision::unsigned_magnitude, boost::multiprecision::unchecked, void>>;

	/// The size of the container.
	enum { size = N };

	/// Method to convert from a string.
	enum ConstructFromStringType { FromHex, FromBinary };

	/// Method to convert from a string.
	enum ConstructFromHashType { AlignLeft, AlignRight, FailIfDifferent };

	/// Construct an empty hash.
	explicit FixedHash() { m_data.fill(0); }

	/// Construct from another hash, filling with zeroes or cropping as necessary.
	template <unsigned M> explicit FixedHash(FixedHash<M> const& _h, ConstructFromHashType _t = AlignLeft) { m_data.fill(0); unsigned c = std::min(M, N); for (unsigned i = 0; i < c; ++i) m_data[_t == AlignRight ? N - 1 - i : i] = _h[_t == AlignRight ? M - 1 - i : i]; }

	/// Convert from the corresponding arithmetic type.
	FixedHash(Arith const& _arith) { toBigEndian(_arith, m_data); }

	/// Convert from unsigned
	explicit FixedHash(unsigned _u) { toBigEndian(_u, m_data); }

	/// Explicitly construct, copying from a byte array.
	explicit FixedHash(bytes const& _b, ConstructFromHashType _t = FailIfDifferent) { if (_b.size() == N) memcpy(m_data.data(), _b.data(), std::min<unsigned>(_b.size(), N)); else { m_data.fill(0); if (_t != FailIfDifferent) { auto c = std::min<unsigned>(_b.size(), N); for (unsigned i = 0; i < c; ++i) m_data[_t == AlignRight ? N - 1 - i : i] = _b[_t == AlignRight ? _b.size() - 1 - i : i]; } } }

	/// Explicitly construct, copying from a byte array.
	explicit FixedHash(bytesConstRef _b, ConstructFromHashType _t = FailIfDifferent) { if (_b.size() == N) memcpy(m_data.data(), _b.data(), std::min<unsigned>(_b.size(), N)); else { m_data.fill(0); if (_t != FailIfDifferent) { auto c = std::min<unsigned>(_b.size(), N); for (unsigned i = 0; i < c; ++i) m_data[_t == AlignRight ? N - 1 - i : i] = _b[_t == AlignRight ? _b.size() - 1 - i : i]; } } }

	/// Explicitly construct, copying from a  string.
	explicit FixedHash(std::string const& _s, ConstructFromStringType _t = FromHex, ConstructFromHashType _ht = FailIfDifferent): FixedHash(_t == FromHex ? fromHex(_s, WhenError::Throw) : graphene::utilities::asBytes(_s), _ht) {}

	/// Convert to arithmetic type.
	operator Arith() const { return fromBigEndian<Arith>(m_data); }

	/// @returns true iff this is the empty hash.
	explicit operator bool() const { return std::any_of(m_data.begin(), m_data.end(), [](byte _b) { return _b != 0; }); }

	// The obvious comparison operators.
	bool operator==(FixedHash const& _c) const { return m_data == _c.m_data; }
	bool operator!=(FixedHash const& _c) const { return m_data != _c.m_data; }
	/// Required to sort objects of this type or use them as map keys.
	bool operator<(FixedHash const& _c) const { for (unsigned i = 0; i < N; ++i) if (m_data[i] < _c.m_data[i]) return true; else if (m_data[i] > _c.m_data[i]) return false; return false; }

	FixedHash operator~() const { FixedHash ret; for (unsigned i = 0; i < N; ++i) ret[i] = ~m_data[i]; return ret; }

	/// @returns a particular byte from the hash.
	byte& operator[](unsigned _i) { return m_data[_i]; }
	/// @returns a particular byte from the hash.
	byte operator[](unsigned _i) const { return m_data[_i]; }

	/// @returns the hash as a user-readable hex string.
	std::string hex() const { return toHex(ref()); }

	/// @returns a mutable byte vector_ref to the object's data.
	bytesRef ref() { return bytesRef(m_data.data(), N); }

	/// @returns a constant byte vector_ref to the object's data.
	bytesConstRef ref() const { return bytesConstRef(m_data.data(), N); }

	/// @returns a mutable byte pointer to the object's data.
	byte* data() { return m_data.data(); }

	/// @returns a constant byte pointer to the object's data.
	byte const* data() const { return m_data.data(); }

	/// @returns a copy of the object's data as a byte vector.
	bytes asBytes() const { return bytes(data(), data() + N); }

	/// @returns a mutable reference to the object's data as an STL array.
	std::array<byte, N>& asArray() { return m_data; }

	/// @returns a constant reference to the object's data as an STL array.
	std::array<byte, N> const& asArray() const { return m_data; }

	/// Returns the index of the first bit set to one, or size() * 8 if no bits are set.
	inline unsigned firstBitSet() const
	{
		unsigned ret = 0;
		for (auto d: m_data)
			if (d)
			{
				for (;; ++ret, d <<= 1)
					if (d & 0x80)
						return ret;
			}
			else
				ret += 8;
		return ret;
	}

	void clear() { m_data.fill(0); }

private:
	std::array<byte, N> m_data;		///< The binary data.
};

/// Stream I/O for the FixedHash class.
template <unsigned N>
inline std::ostream& operator<<(std::ostream& _out, FixedHash<N> const& _h)
{
	boost::io::ios_all_saver guard(_out);
	_out << std::noshowbase << std::hex << std::setfill('0');
	for (unsigned i = 0; i < N; ++i)
		_out << std::setw(2) << (int)_h[i];
	_out << std::dec;
	return _out;
}

// Common types of FixedHash.
using h256 = FixedHash<32>;
using h160 = FixedHash<20>;

} }


#ifndef MAKE_TRACE_
#define MAKE_TRACE_
#include <headers/trace.hpp>
#include <boost/core/demangle.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>

namespace raytrace {

    namespace fs = boost::filesystem;
    namespace pt = boost::property_tree;
    namespace po = boost::program_options;

    class exml_tree
    {
        private:
            pt::ptree tree_;
        public:
            exml_tree () noexcept;

            template <
                typename T,
                typename = std::enable_if_t<std::is_arithmetic_v<std::remove_reference_t<T>>>
            >
            exml_tree(T && n) : exml_tree()
            {
                tree_.put("Expression.Number", std::forward<T>(n));
            }

            template <typename T>
            exml_tree (const std::vector<T> & vec) noexcept(std::is_nothrow_copy_assignable<T>::value) : exml_tree()
            {
                tree_.put("Expression.Function.Symbol", "List");
                for (const auto & ptr : vec)
                {
                    pt::ptree child = exml_tree(ptr).ptree().get_child("Expression");
                    for(const auto & v : child)
                        if (v.first != "<xmlattr>")
                            tree_.add_child("Expression.Function." + v.first, child.get_child(v.first));
                }
            }

            exml_tree (const Point & pt) noexcept : exml_tree(std::vector<double> {pt.x(), pt.y(), pt.z()}) {}
            exml_tree (const Vector & v) noexcept : exml_tree(std::vector<double> {v.theta(), v.phi()}) {}
            exml_tree (const Line & line) noexcept;
            exml_tree (const Trace & trace) noexcept;
            exml_tree (const TracePoint & tr_pt) noexcept;
            exml_tree (const Sphere & sph) noexcept;
            const pt::ptree & ptree() const noexcept {return tree_; }
    };

    class info_tree
    {
        private:
            pt::ptree tree_;
        public:
            template <typename T>
            info_tree(const std::vector<T> & vec) noexcept(std::is_nothrow_copy_assignable<T>::value)
            {
                tree_.put("List", "");
                for(const auto & x : vec)
                {
                    pt::ptree child = info_tree(x).ptree();
                    for(const auto & v : child)
                        tree_.add_child("List." + v.first, child.get_child(v.first));
                }
            }

            info_tree(const Sphere & sph) noexcept;
            info_tree(const ExpSetup & setup) noexcept;
            const pt::ptree & ptree() const noexcept {return tree_; }
    };

    class base_setup
    {
        private:
            fs::path root_path_;
            mutable fs::path current_path_;
            fs::path make_path(const fs::path & path) const;
            fs::path write_path(const fs::path & filename, const fs::path & ext) const;
        public:
            base_setup(const fs::path & p = fs::current_path()) : root_path_(p), current_path_(p) {}
            void make_dir() const;
            void write_xml(const exml_tree & tree, const std::string & name) const;
            void write_xml(const pt::ptree & tree, const std::string & name) const;
            std::string date() const noexcept;
            const fs::path & current_path() const noexcept {return current_path_; }
            const fs::path & root_path() const noexcept {return root_path_; }
            fs::path & root_path() noexcept {return root_path_; }
    };

    class tracing : private base_setup, private ExpSetup
    {
        private:
            unsigned int threads_num_ {std::thread::hardware_concurrency()};
            struct run_pars
            {
                run_pars(unsigned long rays) : rays_max_(rays), write_partition_(100 * int(sqrt(rays / 1e6) + 1)) {}
                mutable std::atomic<unsigned long> rays_ {0};
                unsigned long rays_max_;
                unsigned int write_partition_;
            };

            void add_result(std::vector<Trace> & vec, Trace & trace) const;
            void add_result(std::vector<TracePoint> & vec, const Trace & trace) const;
            void write_result(std::vector<Trace> & vec) const;
            void write_result(std::vector<TracePoint> & vec) const;

            template <typename BeamType, typename ResType>
            void do_trace_block(const run_pars & pars_) const
            {
                while (pars_.rays_ < pars_.rays_max_)
                {
                    std::vector<ResType> vec_ {};
                    while(vec_.size() < pars_.write_partition_ && ++pars_.rays_ < pars_.rays_max_)
                    {
                        auto trace_ = make_trace<BeamType>(static_cast<ExpSetup>(*this));
                        if(trace_.is_transmitted())
                            add_result(vec_, trace_);
                    }
                    if(vec_.size() != 0)
                        write_result(vec_);
                }
            }

            template <typename BeamType>
            void log(const run_pars & pars) const
            {
                pt::ptree log;
                log.put("Title", "Ray tracing on spherical surfaces by Nikolay Ivanov");
                log.put("Date", base_setup::date());
                log.put("Number-of-rays", pars.rays_max_);
                log.put("Beam-mode", boost::core::demangle(typeid(BeamType).name()));
                log.put("Experimental-setup", "");
                log.put_child("Experimental-setup", info_tree(static_cast<ExpSetup>(*this)).ptree());
                base_setup::write_xml(log, "info");
            }
            void log_wolfram() const;
        public:
            using ExpSetup::add_sphere;
            using ExpSetup::spheres;
            using ExpSetup::reset_sphere;
            using ExpSetup::substrate;
            using base_setup::root_path;
            using ExpSetup::IncAngle;
            using ExpSetup::surface;

            tracing(double RMSHeight = Constants::RMSHeight, double CorrLength = Constants::CorrLength, double inc_ang = 0.0, const fs::path & path = fs::current_path())
            : base_setup(path), ExpSetup(inc_ang, RMSHeight, CorrLength) {}
            void set_threads(unsigned int n) noexcept {threads_num_ = n; }

            template <typename BeamType, typename ResType>
            void run(unsigned long rays, bool verbose = false) const
            {
                auto t_ = std::chrono::high_resolution_clock::now();
                run_pars pars (rays);
                base_setup::make_dir();
                if(verbose)
                    std::clog << "Running ray tracing of " << rays << " rays\n"
                            << "Writing results to " << base_setup::current_path() << std::endl;
                log<BeamType>(pars);
                if(std::is_same<Trace, ResType>::value)
                    log_wolfram();
                std::vector<std::future<void>> futures;
                for(int i = 0; i < threads_num_; i++)
                    futures.emplace_back(std::async(&tracing::do_trace_block<BeamType, ResType>, this, std::ref(pars)));
                for(auto & e : futures)
                    e.get();
                if(verbose)
                    std::clog << "Ray tracing has finished\nElapsed time: "
                            << std::chrono::duration_cast<std::chrono::duration<float>>(std::chrono::high_resolution_clock::now() - t_).count() << " s\n";
            }
    };

    class trace_parser : private tracing
    {
        private:
            po::variables_map vm_;
        public:
            trace_parser(const po::variables_map & vm, const fs::path & path = fs::current_path());
            void run() const;
    };

}

#endif


#include <boost/python.hpp>
#include <scitbx/array_family/shared.h>
#include <scitbx/array_family/flex_types.h>
#include <cctype>
#include <dxtbx/model/panel.h>
#include <iostream>

namespace recviewer { namespace ext {
  using namespace scitbx;

  typedef scitbx::af::flex<vec3<double> >::type flex_vec3_double;
  typedef scitbx::af::flex<vec2<double> >::type flex_vec2_double;

  static af::shared<vec2<double> > get_target_pixels(dxtbx::model::Panel panel,
                                                     vec3<double> s0,
                                                     int xlim,
                                                     int ylim,
                                                     double maxres) {
    af::shared<vec2<double> > ret;
    vec2<double> xy;

    for (size_t x = 0; x < xlim; x++) {
      for (size_t y = 0; y < ylim; y++) {
        xy[0] = x;
        xy[1] = y;

        // get_resolution_at_pixel() no longer returns INF, so this is safe
        if (panel.get_resolution_at_pixel(s0, xy) > maxres) {
          ret.push_back(xy);
        }
      }
    }
    return ret;
  }

  static void fill_voxels(const af::flex_int &image,
                          af::flex_double &grid,
                          af::flex_int &counts,
                          const flex_vec3_double &rotated_S,
                          const flex_vec2_double &xy,
                          const double rec_range) {
    int npoints = grid.accessor().all()[0];
    double step = 2 * rec_range / npoints;

    for (int i = 0, ilim = xy.size(); i < ilim; i++) {
      int ind_x = rotated_S[i][0] / step + npoints / 2 + 0.5;
      int ind_y = rotated_S[i][1] / step + npoints / 2 + 0.5;
      int ind_z = rotated_S[i][2] / step + npoints / 2 + 0.5;
      int x = xy[i][0];
      int y = xy[i][1];

      if (ind_x >= npoints || ind_y >= npoints || ind_z >= npoints || ind_x < 0
          || ind_y < 0 || ind_z < 0)
        continue;
      grid(ind_x, ind_y, ind_z) += image(y, x);
      counts(ind_x, ind_y, ind_z)++;
    }
  }

  static void normalize_voxels(af::flex_double &grid, af::flex_int &counts) {
    for (int i = 0, ilim = grid.size(); i < ilim; i++) {
      if (counts[i] != 0) {
        grid[i] /= counts[i];
      }
    }
  }

  void init_module() {
    using namespace boost::python;
    def("get_target_pixels", get_target_pixels);
    def("fill_voxels", fill_voxels);
    def("normalize_voxels", normalize_voxels);
  }

}}  // namespace recviewer::ext

BOOST_PYTHON_MODULE(recviewer_ext) {
  recviewer::ext::init_module();
}


/*******************************************************************************
 *         Copyright 2003 & onward LASMEA UMR 6602 CNRS/Univ. Clermont II
 *         Copyright 2009 & onward 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
 ******************************************************************************/
#define NT2_UNIT_MODULE "nt2::modulo"

#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/functor/operators.hpp>
#include <boost/type_traits/is_same.hpp>

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

////////////////////////////////////////////////////////////////////////////////
// Test behavior for modulo
////////////////////////////////////////////////////////////////////////////////
NT2_TEST_CASE_TPL ( modulo, NT2_INTEGRAL_TYPES )
{
  using boost::is_same;
  using nt2::tag::modulo_;

  NT2_TEST( (boost::is_same < typename nt2::meta::call<modulo_(T,T)>::type
                            , BOOST_TYPEOF(T() % T(1))
                            >::value
            )
          );

  NT2_TEST_EQUAL( nt2::modulo(T(0),T(2)), T(0) );
  NT2_TEST_EQUAL( nt2::modulo(T(1),T(2)), T(1) );
  NT2_TEST_EQUAL( nt2::modulo(T(2),T(2)), T(0) );
}


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

// Copyright (c) 2015, Oracle and/or its affiliates.

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

#ifndef BOOST_TEST_MODULE
#define BOOST_TEST_MODULE test_intersection_pointlike_linear
#endif

#include <iostream>
#include <algorithm>

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

#include "../test_set_ops_pointlike.hpp"

#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/segment.hpp>
#include <boost/geometry/geometries/multi_point.hpp>
#include <boost/geometry/geometries/multi_linestring.hpp>

typedef bg::model::point<double, 2, bg::cs::cartesian>  point_type;
typedef bg::model::segment<point_type>  segment_type;
typedef bg::model::linestring<point_type>  linestring_type;
typedef bg::model::multi_point<point_type>  multi_point_type;
typedef bg::model::multi_linestring<linestring_type>  multi_linestring_type;

//===========================================================================
//===========================================================================
//===========================================================================

BOOST_AUTO_TEST_CASE( test_intersection_point_segment )
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl << std::endl << std::endl;
    std::cout << "*** POINT / SEGMENT INTERSECTION ***" << std::endl;
    std::cout << std::endl;
#endif

    typedef point_type P;
    typedef segment_type S;
    typedef multi_point_type MP;

    typedef test_set_op_of_pointlike_geometries
        <
            P, S, MP, bg::overlay_intersection
        > tester;

    tester::apply
        ("psi01",
         from_wkt<P>("POINT(0 0)"),
         from_wkt<S>("SEGMENT(1 1,2 2)"),
         from_wkt<MP>("MULTIPOINT()")
         );

    tester::apply
        ("psi02",
         from_wkt<P>("POINT(0 0)"),
         from_wkt<S>("SEGMENT(0 0,1 1)"),
         from_wkt<MP>("MULTIPOINT(0 0)")
         );

    tester::apply
        ("psi03",
         from_wkt<P>("POINT(1 1)"),
         from_wkt<S>("SEGMENT(0 0,2 2)"),
         from_wkt<MP>("MULTIPOINT(1 1)")
         );

    tester::apply
        ("psi04",
         from_wkt<P>("POINT(3 3)"),
         from_wkt<S>("SEGMENT(0 0,2 2)"),
         from_wkt<MP>("MULTIPOINT()")
         );
}

BOOST_AUTO_TEST_CASE( test_intersection_point_linestring )
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl << std::endl << std::endl;
    std::cout << "*** POINT / LINESTRING INTERSECTION ***" << std::endl;
    std::cout << std::endl;
#endif

    typedef point_type P;
    typedef linestring_type L;
    typedef multi_point_type MP;

    typedef test_set_op_of_pointlike_geometries
        <
            P, L, MP, bg::overlay_intersection
        > tester;

    tester::apply
        ("pli01",
         from_wkt<P>("POINT(0 0)"),
         from_wkt<L>("LINESTRING(1 1,2 2)"),
         from_wkt<MP>("MULTIPOINT()")
         );

    tester::apply
        ("pli02",
         from_wkt<P>("POINT(0 0)"),
         from_wkt<L>("LINESTRING(0 0,1 1)"),
         from_wkt<MP>("MULTIPOINT(0 0)")
         );

    tester::apply
        ("pli03",
         from_wkt<P>("POINT(1 1)"),
         from_wkt<L>("LINESTRING(0 0,2 2)"),
         from_wkt<MP>("MULTIPOINT(1 1)")
         );

    tester::apply
        ("pli04",
         from_wkt<P>("POINT(3 3)"),
         from_wkt<L>("LINESTRING(0 0,2 2)"),
         from_wkt<MP>("MULTIPOINT()")
         );

    // linestrings with more than two points
    tester::apply
        ("pli05",
         from_wkt<P>("POINT(1 1)"),
         from_wkt<L>("LINESTRING(0 0,1 1,2 2)"),
         from_wkt<MP>("MULTIPOINT(1 1)")
         );

    tester::apply
        ("pli06",
         from_wkt<P>("POINT(2 2)"),
         from_wkt<L>("LINESTRING(0 0,1 1,4 4)"),
         from_wkt<MP>("MULTIPOINT(2 2)")
         );

    tester::apply
        ("pli07",
         from_wkt<P>("POINT(10 10)"),
         from_wkt<L>("LINESTRING(0 0,1 1,4 4)"),
         from_wkt<MP>("MULTIPOINT()")
         );

    tester::apply
        ("pli08",
         from_wkt<P>("POINT(0 1)"),
         from_wkt<L>("LINESTRING(0 0,1 1,4 4)"),
         from_wkt<MP>("MULTIPOINT()")
         );

    tester::apply
        ("pli09",
         from_wkt<P>("POINT(4 4)"),
         from_wkt<L>("LINESTRING(0 0,1 1,4 4)"),
         from_wkt<MP>("MULTIPOINT(4 4)")
         );

    tester::apply
        ("pli10",
         from_wkt<P>("POINT(0 0)"),
         from_wkt<L>("LINESTRING(0 0,1 1,4 4)"),
         from_wkt<MP>("MULTIPOINT(0 0)")
         );
}

BOOST_AUTO_TEST_CASE( test_intersection_point_multilinestring )
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl << std::endl << std::endl;
    std::cout << "*** POINT / MULTILINESTRING INTERSECTION ***" << std::endl;
    std::cout << std::endl;
#endif

    typedef point_type P;
    typedef multi_linestring_type ML;
    typedef multi_point_type MP;

    typedef test_set_op_of_pointlike_geometries
        <
            P, ML, MP, bg::overlay_intersection
        > tester;

    tester::apply
        ("pmli01",
         from_wkt<P>("POINT(0 0)"),
         from_wkt<ML>("MULTILINESTRING((1 1,2 2))"),
         from_wkt<MP>("MULTIPOINT()")
         );

    tester::apply
        ("pmli02",
         from_wkt<P>("POINT(0 0)"),
         from_wkt<ML>("MULTILINESTRING((0 0,1 1))"),
         from_wkt<MP>("MULTIPOINT(0 0)")
         );

    tester::apply
        ("pmli03",
         from_wkt<P>("POINT(1 1)"),
         from_wkt<ML>("MULTILINESTRING((0 0,2 2))"),
         from_wkt<MP>("MULTIPOINT(1 1)")
         );

    tester::apply
        ("pmli04",
         from_wkt<P>("POINT(3 3)"),
         from_wkt<ML>("MULTILINESTRING((0 0,2 2))"),
         from_wkt<MP>("MULTIPOINT()")
         );

    // linestrings with more than two points
    tester::apply
        ("pmli05",
         from_wkt<P>("POINT(1 1)"),
         from_wkt<ML>("MULTILINESTRING((0 0,1 1,2 2))"),
         from_wkt<MP>("MULTIPOINT(1 1)")
         );

    tester::apply
        ("pmli06",
         from_wkt<P>("POINT(2 2)"),
         from_wkt<ML>("MULTILINESTRING((0 0,1 1,4 4))"),
         from_wkt<MP>("MULTIPOINT(2 2)")
         );

    tester::apply
        ("pmli07",
         from_wkt<P>("POINT(10 10)"),
         from_wkt<ML>("MULTILINESTRING((0 0,1 1,4 4))"),
         from_wkt<MP>("MULTIPOINT()")
         );

    tester::apply
        ("pmli08",
         from_wkt<P>("POINT(0 1)"),
         from_wkt<ML>("MULTILINESTRING((0 0,1 1,4 4))"),
         from_wkt<MP>("MULTIPOINT()")
         );

    tester::apply
        ("pmli09",
         from_wkt<P>("POINT(4 4)"),
         from_wkt<ML>("MULTILINESTRING((0 0,1 1,4 4))"),
         from_wkt<MP>("MULTIPOINT(4 4)")
         );

    tester::apply
        ("pmli10",
         from_wkt<P>("POINT(0 0)"),
         from_wkt<ML>("MULTILINESTRING((0 0,1 1,4 4))"),
         from_wkt<MP>("MULTIPOINT(0 0)")
         );

    // multilinestrings with more than one linestring
    tester::apply
        ("pmli11",
         from_wkt<P>("POINT(0 0)"),
         from_wkt<ML>("MULTILINESTRING((-10,-10),(0 0,1 1,4 4))"),
         from_wkt<MP>("MULTIPOINT(0 0)")
         );

    tester::apply
        ("pmli12",
         from_wkt<P>("POINT(0 0)"),
         from_wkt<ML>("MULTILINESTRING((-10 0,0 0,10 0),(0 0,1 1,4 4))"),
         from_wkt<MP>("MULTIPOINT(0 0)")
         );

    tester::apply
        ("pmli13",
         from_wkt<P>("POINT(0 0)"),
         from_wkt<ML>("MULTILINESTRING((-10 0,10 0),(0 0,1 1,4 4))"),
         from_wkt<MP>("MULTIPOINT(0 0)")
         );

    tester::apply
        ("pmli14",
         from_wkt<P>("POINT(-20 0)"),
         from_wkt<ML>("MULTILINESTRING((-10 0,10 0),(0 0,1 1,4 4))"),
         from_wkt<MP>("MULTIPOINT()")
         );

    tester::apply
        ("pmli15",
         from_wkt<P>("POINT(0 1)"),
         from_wkt<ML>("MULTILINESTRING((-10 0,10 0),(0 0,1 1,4 4))"),
         from_wkt<MP>("MULTIPOINT()")
         );

    tester::apply
        ("pmli16",
         from_wkt<P>("POINT(1 0)"),
         from_wkt<ML>("MULTILINESTRING((-10 0,10 0),(-1 0,2 0,20 0))"),
         from_wkt<MP>("MULTIPOINT(1 0)")
         );

    tester::apply
        ("pmli17",
         from_wkt<P>("POINT(1 0)"),
         from_wkt<ML>("MULTILINESTRING((-10 0,10 0),(0 -1,0 2,0 4))"),
         from_wkt<MP>("MULTIPOINT(1 0)")
         );
}

BOOST_AUTO_TEST_CASE( test_intersection_multipoint_segment )
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl << std::endl << std::endl;
    std::cout << "*** MULTIPOINT / SEGMENT INTERSECTION ***" << std::endl;
    std::cout << std::endl;
#endif

    typedef multi_point_type MP;
    typedef segment_type S;

    typedef test_set_op_of_pointlike_geometries
        <
            MP, S, MP, bg::overlay_intersection
        > tester;

    tester::apply
        ("mpsi01",
         from_wkt<MP>("MULTIPOINT(0 0)"),
         from_wkt<S>("SEGMENT(1 1,2 2)"),
         from_wkt<MP>("MULTIPOINT()")
         );

    tester::apply
        ("mpsi02",
         from_wkt<MP>("MULTIPOINT(0 0)"),
         from_wkt<S>("SEGMENT(0 0,1 1)"),
         from_wkt<MP>("MULTIPOINT(0 0)")
         );

    tester::apply
        ("mpsi03",
         from_wkt<MP>("MULTIPOINT(0 0,0 0)"),
         from_wkt<S>("SEGMENT(1 1,2 2)"),
         from_wkt<MP>("MULTIPOINT()")
         );

    tester::apply
        ("mpsi04",
         from_wkt<MP>("MULTIPOINT(0 0,0 0)"),
         from_wkt<S>("SEGMENT(0 0,1 1)"),
         from_wkt<MP>("MULTIPOINT(0 0,0 0)")
         );

    tester::apply
        ("mpsi05",
         from_wkt<MP>("MULTIPOINT(0 0,0 0,1 0)"),
         from_wkt<S>("SEGMENT(1 1,2 2)"),
         from_wkt<MP>("MULTIPOINT()")
         );

    tester::apply
        ("mpsf06",
         from_wkt<MP>("MULTIPOINT(0 0,0 0,1 0)"),
         from_wkt<S>("SEGMENT(1 0,2 0)"),
         from_wkt<MP>("MULTIPOINT(1 0)")
         );

    tester::apply
        ("mpsi07",
         from_wkt<MP>("MULTIPOINT(0 0,0 0,2 0)"),
         from_wkt<S>("SEGMENT(0 0,1 0)"),
         from_wkt<MP>("MULTIPOINT(0 0,0 0)")
         );

    tester::apply
        ("mpsi07a",
         from_wkt<MP>("MULTIPOINT(0 0,0 0,2 0)"),
         from_wkt<S>("SEGMENT(0 0,3 0)"),
         from_wkt<MP>("MULTIPOINT(0 0,0 0,2 0)")
         );

    tester::apply
        ("mpsi08",
         from_wkt<MP>("MULTIPOINT()"),
         from_wkt<S>("SEGMENT(0 0,1 1)"),
         from_wkt<MP>("MULTIPOINT()")
         );

    tester::apply
        ("mpsi09",
         from_wkt<MP>("MULTIPOINT(0 0,0 0,2 0)"),
         from_wkt<S>("SEGMENT(-1 0,1 0)"),
         from_wkt<MP>("MULTIPOINT(0 0,0 0)")
         );

    tester::apply
        ("mpsi10",
         from_wkt<MP>("MULTIPOINT(0 0,0 0,2 0)"),
         from_wkt<S>("SEGMENT(1 0,3 0)"),
         from_wkt<MP>("MULTIPOINT(2 0)")
         );

    tester::apply
        ("mpsi11",
         from_wkt<MP>("MULTIPOINT(0 0,0 0,2 0)"),
         from_wkt<S>("SEGMENT(-1 0,3 0)"),
         from_wkt<MP>("MULTIPOINT(0 0,0 0,2 0)")
         );

    tester::apply
        ("mpsi12",
         from_wkt<MP>("MULTIPOINT(0 0,0 0,2 0)"),
         from_wkt<S>("SEGMENT(3 0,3 0)"),
         from_wkt<MP>("MULTIPOINT()")
         );

    tester::apply
        ("mpsi12a",
         from_wkt<MP>("MULTIPOINT(0 0,2 0,0 0)"),
         from_wkt<S>("SEGMENT(3 0,3 0)"),
         from_wkt<MP>("MULTIPOINT()")
         );

    tester::apply
        ("mpsi12b",
         from_wkt<MP>("MULTIPOINT(0 0,2 0,0 0,2 0)"),
         from_wkt<S>("SEGMENT(3 0,3 0)"),
         from_wkt<MP>("MULTIPOINT()")
         );

    tester::apply
        ("mpsi12c",
         from_wkt<MP>("MULTIPOINT(0 0,2 0,0 0,2 0,0 0)"),
         from_wkt<S>("SEGMENT(3 0,3 0)"),
         from_wkt<MP>("MULTIPOINT()")
         );

    tester::apply
        ("mpsi13",
         from_wkt<MP>("MULTIPOINT(0 0,0 0,2 0)"),
         from_wkt<S>("SEGMENT(2 0,2 0)"),
         from_wkt<MP>("MULTIPOINT(2 0)")
         );

    tester::apply
        ("mpsi14",
         from_wkt<MP>("MULTIPOINT(0 0,0 0,2 0)"),
         from_wkt<S>("SEGMENT(0 0,0 0)"),
         from_wkt<MP>("MULTIPOINT(0 0,0 0)")
         );

    tester::apply
        ("mpsi15",
         from_wkt<MP>("MULTIPOINT()"),
         from_wkt<S>("SEGMENT(0 0,1 0)"),
         from_wkt<MP>("MULTIPOINT()")
         );

    tester::apply
        ("mpsi16",
         from_wkt<MP>("MULTIPOINT(0 0,2 0,0 0,2 0,0 0)"),
         from_wkt<S>("SEGMENT(-1 0,10 0)"),
         from_wkt<MP>("MULTIPOINT(0 0,0 0,0 0,2 0,2 0)")
         );
}

BOOST_AUTO_TEST_CASE( test_intersection_multipoint_linestring )
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl << std::endl << std::endl;
    std::cout << "*** MULTIPOINT / LINESTRING INTERSECTION ***" << std::endl;
    std::cout << std::endl;
#endif

    typedef multi_point_type MP;
    typedef linestring_type L;

    typedef test_set_op_of_pointlike_geometries
        <
            MP, L, MP, bg::overlay_intersection
        > tester;

    tester::apply
        ("mpli01",
         from_wkt<MP>("MULTIPOINT(0 0)"),
         from_wkt<L>("LINESTRING(1 1,2 2,3 3,4 4,5 5)"),
         from_wkt<MP>("MULTIPOINT()")
         );

    tester::apply
        ("mpli02",
         from_wkt<MP>("MULTIPOINT(0 0)"),
         from_wkt<L>("LINESTRING(0 0,1 1)"),
         from_wkt<MP>("MULTIPOINT(0 0)")
         );

    tester::apply
        ("mpli03",
         from_wkt<MP>("MULTIPOINT(0 0,0 0)"),
         from_wkt<L>("LINESTRING(1 1,2 2)"),
         from_wkt<MP>("MULTIPOINT()")
         );

    tester::apply
        ("mpli04",
         from_wkt<MP>("MULTIPOINT(0 0,0 0)"),
         from_wkt<L>("LINESTRING(0 0,1 1)"),
         from_wkt<MP>("MULTIPOINT(0 0,0 0)")
         );

    tester::apply
        ("mpli05",
         from_wkt<MP>("MULTIPOINT(0 0,0 0,1 0)"),
         from_wkt<L>("LINESTRING(1 1,2 2)"),
         from_wkt<MP>("MULTIPOINT()")
         );

    tester::apply
        ("mpli06",
         from_wkt<MP>("MULTIPOINT(0 0,0 0,1 0)"),
         from_wkt<L>("LINESTRING(1 0,2 0)"),
         from_wkt<MP>("MULTIPOINT(1 0)")
         );

    tester::apply
        ("mpli07",
         from_wkt<MP>("MULTIPOINT(0 0,0 0,2 0)"),
         from_wkt<L>("LINESTRING(0 0,1 0)"),
         from_wkt<MP>("MULTIPOINT(0 0,0 0)")
         );

    tester::apply
        ("mpli08",
         from_wkt<MP>("MULTIPOINT()"),
         from_wkt<L>("LINESTRING(0 0,1 1)"),
         from_wkt<MP>("MULTIPOINT()")
         );

    tester::apply
        ("mpli09",
         from_wkt<MP>("MULTIPOINT(0 0,0 0,2 0)"),
         from_wkt<L>("LINESTRING(-1 0,1 0)"),
         from_wkt<MP>("MULTIPOINT(0 0,0 0)")
         );

    tester::apply
        ("mpli10",
         from_wkt<MP>("MULTIPOINT(0 0,0 0,2 0)"),
         from_wkt<L>("LINESTRING(1 0,3 0)"),
         from_wkt<MP>("MULTIPOINT(2 0)")
         );

    tester::apply
        ("mpli10a",
         from_wkt<MP>("MULTIPOINT(2 0,0 0,2 0,0 0,2 0)"),
         from_wkt<L>("LINESTRING(1 0,3 0)"),
         from_wkt<MP>("MULTIPOINT(2 0,2 0,2 0)")
         );

    tester::apply
        ("mpli11",
         from_wkt<MP>("MULTIPOINT(0 0,0 0,2 0)"),
         from_wkt<L>("LINESTRING(-1 0,3 0)"),
         from_wkt<MP>("MULTIPOINT(2 0,0 0,0 0)")
         );

    tester::apply
        ("mpli12",
         from_wkt<MP>("MULTIPOINT(0 0,0 0,2 0)"),
         from_wkt<L>("LINESTRING(3 0,3 0)"),
         from_wkt<MP>("MULTIPOINT()")
         );

    tester::apply
        ("mpli13",
         from_wkt<MP>("MULTIPOINT(0 0,0 0,2 0)"),
         from_wkt<L>("LINESTRING(2 0,2 0)"),
         from_wkt<MP>("MULTIPOINT(2 0)")
         );

    tester::apply
        ("mpli14",
         from_wkt<MP>("MULTIPOINT(0 0,0 0,2 0)"),
         from_wkt<L>("LINESTRING(0 0,0 0)"),
         from_wkt<MP>("MULTIPOINT(0 0,0 0)")
         );

    tester::apply
        ("mpli15",
         from_wkt<MP>("MULTIPOINT()"),
         from_wkt<L>("LINESTRING(0 0,1 0,2 0,3 0)"),
         from_wkt<MP>("MULTIPOINT()")
         );

    tester::apply
        ("mpli16",
         from_wkt<MP>("MULTIPOINT()"),
         from_wkt<L>("LINESTRING()"),
         from_wkt<MP>("MULTIPOINT()")
         );
}

BOOST_AUTO_TEST_CASE( test_intersection_multipoint_multilinestring )
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl << std::endl << std::endl;
    std::cout << "*** MULTIPOINT / MULTILINESTRING INTERSECTION ***"
              << std::endl;
    std::cout << std::endl;
#endif

    typedef multi_point_type MP;
    typedef multi_linestring_type ML;

    typedef test_set_op_of_pointlike_geometries
        <
            MP, ML, MP, bg::overlay_intersection
        > tester;

    tester::apply
        ("mpmli01",
         from_wkt<MP>("MULTIPOINT(0 0,1 0,2 0)"),
         from_wkt<ML>("MULTILINESTRING((1 0,1 1,1 1,4 4))"),
         from_wkt<MP>("MULTIPOINT(1 0)")
         );

    tester::apply
        ("mpmli02",
         from_wkt<MP>("MULTIPOINT(2 2,3 3,0 0,0 0,2 2,1 1,1 1,1 0,1 0)"),
         from_wkt<ML>("MULTILINESTRING((1 0,1 1,1 1,4 4))"),
         from_wkt<MP>("MULTIPOINT(2 2,2 2,3 3,1 1,1 1,1 0,1 0)")
         );

    tester::apply
        ("mpmli03",
         from_wkt<MP>("MULTIPOINT(5 5,3 3,0 0,0 0,5 5,1 1,1 1,1 0,1 0)"),
         from_wkt<ML>("MULTILINESTRING((1 0,1 1,1 1,4 4))"),
         from_wkt<MP>("MULTIPOINT(1 0,1 0,3 3,1 1,1 1)")
         );

    tester::apply
        ("mpmli04",
         from_wkt<MP>("MULTIPOINT(0 0,1 1,1 0,1 1)"),
         from_wkt<ML>("MULTILINESTRING((1 0,0 0,1 1,0 0))"),
         from_wkt<MP>("MULTIPOINT(1 0,0 0,1 1,1 1)")
         );

    tester::apply
        ("mpmli05",
         from_wkt<MP>("MULTIPOINT(0 0,1 0,2 0,5 0)"),
         from_wkt<ML>("MULTILINESTRING((0 1,0 0,1 2,1 0),\
                      (0 1,2 0,0 10,2 0,2 10,5 10,5 0,5 10))"),
         from_wkt<MP>("MULTIPOINT(0 0,1 0,2 0,5 0)")
         );

    tester::apply
        ("mpmli06",
         from_wkt<MP>("MULTIPOINT(0 0,1 1,1 0,1 1)"),
         from_wkt<ML>("MULTILINESTRING(())"),
         from_wkt<MP>("MULTIPOINT()")
         );

    tester::apply
        ("mpmli07",
         from_wkt<MP>("MULTIPOINT()"),
         from_wkt<ML>("MULTILINESTRING(())"),
         from_wkt<MP>("MULTIPOINT()")
         );

    tester::apply
        ("mpmli08",
         from_wkt<MP>("MULTIPOINT()"),
         from_wkt<ML>("MULTILINESTRING((0 0,1 0),(9 0,10 0))"),
         from_wkt<MP>("MULTIPOINT()")
         );

    tester::apply
        ("mpmli09",
         from_wkt<MP>("MULTIPOINT(0 0,1 1,0 0,0 0,0 0,0 0,0 0,0 0,\
                      0 0,0 0,0 0,0 0,0 0)"),
         from_wkt<ML>("MULTILINESTRING((0 0,0 1),(0 0,2 0),(0 0,0 3),\
                      (0 0,4 0),(0 0,0 5),(0 0,6 0),(0 0,7 0),(0 0,8 0))"),
         from_wkt<MP>("MULTIPOINT(0 0,0 0,0 0,0 0,0 0,0 0,0 0,\
                      0 0,0 0,0 0,0 0,0 0)")
         );

    tester::apply
        ("mpmli09a",
         from_wkt<MP>("MULTIPOINT(0 0,1 1,0 0)"),
         from_wkt<ML>("MULTILINESTRING((0 0,0 1),(0 0,2 0),(0 0,0 3),\
                      (0 0,4 0),(0 0,0 5),(0 0,6 0),(0 0,7 0),(0 0,8 0))"),
         from_wkt<MP>("MULTIPOINT(0 0,0 0)")
         );
}


//==================================================================================================
/*!
  @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_BITWISE_HPP_INCLUDED
#define BOOST_SIMD_BITWISE_HPP_INCLUDED

namespace boost { namespace simd
{
  /*!
    @ingroup group-functions
    @defgroup group-bitwise Bitwise functions

      These functions provide scalar and SIMD algorithms for bit manipulations and bitwise
      operators. Bitwise operations are provided along more specific ones like bit-level rotations,
      bit set, get and toggling and mapping for compiler-specific first bit set or population count
      operations.

      Note that bitwise functions operators may accept IEEE types as arguments. The implied semantic
      is then that the operations are applied to the IEEE bit patterns.

      - Bitwise operators

         | name               | name                 | name                 | name                |  name            |
         |:------------------:|:--------------------:|:--------------------:|:-------------------:|:----------------:|
         | @ref bitwise_and   | @ref bitwise_cast    | @ref bitwise_notor   | @ref bitwise_ornot  | @ref bitwise_xor |
         | @ref bitwise_andnot| @ref bitwise_notand  | @ref bitwise_or      | @ref bitwise_select | @ref complement  |

       Be aware that all binary bitwise functions return a result of the type of the first argument
       and that the second argument is only required to be of the same bit size as the first.
       This behaviour is intended to help bit masking in SIMD mode

      - Bit manipulation, counts and locations

         | name         | name                 | name              | name             |
         |:------------:|:--------------------:|:-----------------:|:----------------:|
         | @ref bits    | @ref ffs             | @ref hi           | @ref swapbytes   |
         | @ref bitget  | @ref firstbitset     | @ref ilog2        | @ref twopower    |
         | @ref bitset  | @ref firstbitunset   | @ref lo           |                  |
         | @ref clz     | @ref genmask         | @ref popcnt       |                  |
         | @ref ctz     | @ref genmaskc        | @ref reversebits  |                  |

      - Shifts and rotations

         | name     | name      | name       | name             |  name    |
         |:--------:|:---------:|:----------:|:----------------:|:--------:|
         | @ref rol | @ref rrol | @ref rshl  | @ref shift_left  | @ref shl |
         | @ref ror | @ref rror | @ref rshr  | @ref shift_right | @ref shr |

       shift_left and shift_right are logical shift, shl and shr arithmetic ones
       and rol and ror are rotations respectively to the left and to the right.
       For these functions negative or excessive values of the shift (greater
       or equal to the bit size of each shifted element) are errors.

       For the other ones negative values reverse the direction,  but excessive
       values are still erroneous.

  **/
} }

#include <boost/simd/function/bitget.hpp>
#include <boost/simd/function/bitset.hpp>
#include <boost/simd/function/bits.hpp>
#include <boost/simd/function/bitwise_and.hpp>
#include <boost/simd/function/bitwise_andnot.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/function/bitwise_notand.hpp>
#include <boost/simd/function/bitwise_notor.hpp>
#include <boost/simd/function/bitwise_or.hpp>
#include <boost/simd/function/bitwise_ornot.hpp>
#include <boost/simd/function/bitwise_select.hpp>
#include <boost/simd/function/bitwise_xor.hpp>
#include <boost/simd/function/clz.hpp>
#include <boost/simd/function/ctz.hpp>
#include <boost/simd/function/ffs.hpp>
#include <boost/simd/function/firstbitset.hpp>
#include <boost/simd/function/firstbitunset.hpp>
#include <boost/simd/function/genmaskc.hpp>
#include <boost/simd/function/genmask.hpp>
#include <boost/simd/function/hi.hpp>
#include <boost/simd/function/ilog2.hpp>
#include <boost/simd/function/lo.hpp>
#include <boost/simd/function/popcnt.hpp>
#include <boost/simd/function/reversebits.hpp>
#include <boost/simd/function/rol.hpp>
#include <boost/simd/function/ror.hpp>
#include <boost/simd/function/rrol.hpp>
#include <boost/simd/function/rror.hpp>
#include <boost/simd/function/rshl.hpp>
#include <boost/simd/function/rshr.hpp>
#include <boost/simd/function/shift_left.hpp>
#include <boost/simd/function/shift_right.hpp>
#include <boost/simd/function/shl.hpp>
#include <boost/simd/function/shr.hpp>
#include <boost/simd/function/swapbytes.hpp>
#include <boost/simd/function/twopower.hpp>

#endif


//
// Copyright © 2019 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include <boost/test/unit_test.hpp>
#include "ParserFlatbuffersFixture.hpp"
#include "../TfLiteParser.hpp"

#include <string>
#include <iostream>

BOOST_AUTO_TEST_SUITE(TensorflowLiteParser)

    struct DequantizeFixture : public ParserFlatbuffersFixture
    {
        explicit DequantizeFixture(const std::string & inputShape,
                                   const std::string & outputShape,
                                   const std::string & dataType)
        {
            m_JsonString = R"(
            {
                "version": 3,
                "operator_codes": [ { "builtin_code": "DEQUANTIZE" } ],
                "subgraphs": [ {
                    "tensors": [
                        {
                            "shape": )" + inputShape + R"(,
                            "type": )" + dataType + R"(,
                            "buffer": 0,
                            "name": "inputTensor",
                            "quantization": {
                                "min": [ 0.0 ],
                                "max": [ 255.0 ],
                                "scale": [ 1.5 ],
                                "zero_point": [ 0 ],
                            }
                        },
                        {
                            "shape": )" + outputShape + R"( ,
                            "type": "FLOAT32",
                            "buffer": 1,
                            "name": "outputTensor",
                            "quantization": {
                                "min": [ 0.0 ],
                                "max": [ 255.0 ],
                                "scale": [ 1.0 ],
                                "zero_point": [ 0 ],
                            }
                        }
                    ],
                    "inputs": [ 0 ],
                    "outputs": [ 1 ],
                    "operators": [
                        {
                            "opcode_index": 0,
                            "inputs": [ 0 ],
                            "outputs": [ 1 ],
                            "builtin_options_type": "DequantizeOptions",
                            "builtin_options": {
                            },
                            "custom_options_format": "FLEXBUFFERS"
                        }
                ],
                } ],
                "buffers" : [
                    { },
                    { },
                ]
            }
        )";
            SetupSingleInputSingleOutput("inputTensor", "outputTensor");
        }
    };

    struct SimpleDequantizeFixtureQAsymm8 : DequantizeFixture
    {
        SimpleDequantizeFixtureQAsymm8() : DequantizeFixture("[ 1, 6 ]",
                                                             "[ 1, 6 ]",
                                                             "UINT8") {}
    };

    BOOST_FIXTURE_TEST_CASE(SimpleDequantizeQAsymm8, SimpleDequantizeFixtureQAsymm8)
    {
        RunTest<2, armnn::DataType::QAsymmU8 , armnn::DataType::Float32>(
                0,
                {{"inputTensor",  { 0u,   1u,   5u,   100u,   200u,   255u }}},
                {{"outputTensor", { 0.0f, 1.5f, 7.5f, 150.0f, 300.0f, 382.5f }}});
    }

    struct SimpleDequantizeFixtureQSymm16 : DequantizeFixture
    {
        SimpleDequantizeFixtureQSymm16() : DequantizeFixture("[ 1, 6 ]",
                                                             "[ 1, 6 ]",
                                                             "INT16") {}
    };

    BOOST_FIXTURE_TEST_CASE(SimpleDequantizeQsymm16, SimpleDequantizeFixtureQSymm16)
    {
        RunTest<2, armnn::DataType::QSymmS16 , armnn::DataType::Float32>(
                0,
                {{"inputTensor",  { 0,    1,    5,    32767,    -1,   -32768 }}},
                {{"outputTensor", { 0.0f, 1.5f, 7.5f, 49150.5f, -1.5f,-49152.0f }}});
    }

    struct SimpleDequantizeFixtureQAsymmS8 : DequantizeFixture
    {
        SimpleDequantizeFixtureQAsymmS8() : DequantizeFixture("[ 1, 6 ]",
                                                             "[ 1, 6 ]",
                                                             "INT8") {}
    };

    BOOST_FIXTURE_TEST_CASE(SimpleDequantizeQAsymmS8, SimpleDequantizeFixtureQAsymmS8)
    {
        RunTest<2, armnn::DataType::QAsymmS8 , armnn::DataType::Float32>(
                0,
                {{"inputTensor",  { 0,    1,    5,    127,    -128,   -1 }}},
                {{"outputTensor", { 0.0f, 1.5f, 7.5f, 190.5f, -192.0f, -1.5f }}});
    }

BOOST_AUTO_TEST_SUITE_END()


#ifndef QST_UTILITIES_HPP
#define QST_UTILITIES_HPP
#include <Eigen/Core>
#include <sstream>
#include <vector>
#include <stdio.h>
#include <iostream>
#include <stdlib.h>
#include <string>
#include <map>
#include <boost/format.hpp>
#include <fstream>
#include "parameters.hpp"

namespace qst{

void LoadTrainingData(std::string &baseName, Parameters &par,Eigen::MatrixXd & trainSamples,std::vector<std::vector<std::string> >& trainBases)
{
    int trainSize = par.nb_ * par.ns_;
    std::string fileName;

    trainSamples.resize(trainSize,par.nv_);
    trainBases.resize(trainSize,std::vector<std::string>(par.nv_));

    fileName = baseName + "train_samples.txt";
    std::ifstream fin_samples(fileName);

    for (int n=0; n<trainSize; n++) {
        for (int j=0; j<par.nv_; j++) {
            fin_samples>> trainSamples(n,j);
        }
    }
    if (par.basis_.compare("std")!=0){
        fileName = baseName + "train_bases.txt";
        std::ifstream fin_bases(fileName);

        for (int n=0; n<trainSize; n++) {
            for (int j=0; j<par.nv_; j++) {
                fin_samples>> trainSamples(n,j);
                fin_bases>> trainBases[n][j];
            }
        }
    }
    else {
        for (int n=0; n<trainSize; n++) {
            for (int j=0; j<par.nv_; j++) {
                trainBases[n][j] = "Z";
            }
        }
    }
}

void LoadWavefunction(Parameters & par,std::string &wf_fileName,Eigen::VectorXd & wf){
    std::ifstream fin(wf_fileName);
    wf.resize(1<<par.nv_);
    for(int i=0;i<1<<par.nv_;i++){
        fin >> wf(i);
    }
}

void LoadWavefunction(Parameters & par,std::string &wf_fileName,Eigen::VectorXcd & wf,std::vector<Eigen::VectorXcd> & rotated_wf){

    std::ifstream fin(wf_fileName);
    wf.resize(1<<par.nv_);
    double x_in;

    if (par.basis_.compare("std")!=0){
        for(int i=0;i<1<<par.nv_;i++){
            fin >> x_in;
            wf.real()(i)=x_in;
            fin >> x_in;
            wf.imag()(i)=x_in;
        }
        Eigen::VectorXcd tmp(1<<par.nv_);
        for(int b=1;b<par.nb_;b++){
            for(int i=0;i<1<<par.nv_;i++){
                fin >> x_in;
                tmp.real()(i)=x_in;
                fin >> x_in;
                tmp.imag()(i)=x_in;
            }
            rotated_wf.push_back(tmp);
        }
    }
    else {
        for(int i=0;i<1<<par.nv_;i++){
            fin >> x_in;
            wf.real()(i)=x_in;
            wf.imag()(i)=0.0;
        }

    }
}
void LoadBasesConfigurations(Parameters &par,std::string &basis_name,std::vector<std::vector<std::string> > &basis) {

    std::ifstream fin(basis_name);
    basis.resize(par.nb_,std::vector<std::string>(par.nv_));
    for (int b=0;b<par.nb_;b++){
        for(int j=0;j<par.nv_;j++){
            fin >> basis[b][j];
        }
    }
}

void SetNumberOfBases(Parameters &par){
    if (par.basis_ == "std")    par.nb_ = 1;
    if (par.basis_ == "xy1")    par.nb_ = 2*par.nv_+1;
}
void GenerateUnitaryRotations(std::map<std::string,Eigen::MatrixXcd> & U){

    double oneDivSqrt2 = 1.0/sqrt(2.0);

    //X rotation
    U["X"].setZero(2,2);
    U["X"].real()<< oneDivSqrt2,oneDivSqrt2,
                    oneDivSqrt2,-oneDivSqrt2;
    //Y rotation
    U["Y"].resize(2,2);
    U["Y"].real()<< oneDivSqrt2,0.0,
                    oneDivSqrt2,0.0;
    U["Y"].imag()<< 0.0,-oneDivSqrt2,
                    0.0,oneDivSqrt2;

}

}

#endif


<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    Notes
 *      This unit test does not test the parseStream functionality of TextParser. Such a test must
 *      be implemented at a later stage.
 *
 */

#define BOOST_TEST_MAIN

#include <string>

#include <boost/make_shared.hpp>
#include <memory>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include "Tudat/InputOutput/linearFieldTransform.h"

namespace tudat
{
namespace unit_tests
{

// Define Boost test suite.
BOOST_AUTO_TEST_SUITE( test_Linear_Field_Transform )

//! Test the linear transformation functionality.
BOOST_AUTO_TEST_CASE( testLinearFieldTransform1 )
{
    // Using declaration.
    using namespace input_output;

    // Set tolerance.
    const double tolerance = 1.0e-6;

    // Create test string.
    std::string testString( "1.5362" );

    // Create a linear field transform object: y = 2 * x + 36.
    LinearFieldTransform testLinearFieldTransform1( 2.0, 36.0 );

    // Define expected value: 2 * 1.5362 + 36 = 39.0724.
    const double expectedValue = 39.0724;

    // Transform test string.
    std::shared_ptr< std::string > returnedValue =
            testLinearFieldTransform1.transform( testString );

    // Check that returned and expected strings are identical.
    BOOST_CHECK_CLOSE_FRACTION( std::stod( *returnedValue ), expectedValue,
                                tolerance );
}

//! Test the linear transformation functionality with no slope (a=0).
BOOST_AUTO_TEST_CASE( testLinearFieldTransform2 )
{
    // Using declaration.
    using namespace input_output;

    // Set tolerance.
    const double tolerance = 1.0e-6;

    // Create test string.
    std::string testString( "1.5362" );

    // Create a linear field transform object: y = 0 * x + 36.
    LinearFieldTransform testLinearFieldTransform2( 0.0, 36.0 );

    // Define expected value: 0 * 1.5362 + 36 = 36.
    const double expectedValue = 36.0;

    // Transform test string.
    std::shared_ptr< std::string > returnedValue =
            testLinearFieldTransform2.transform( testString );

    // Check that returned and expected strings are identical.
    BOOST_CHECK_CLOSE_FRACTION( std::stod( *returnedValue ), expectedValue,
                                tolerance );
}

//! Test the linear transformation functionality with no intercept (b=0).
BOOST_AUTO_TEST_CASE( testLinearFieldTransform3 )
{
    // Using declaration.
    using namespace input_output;

    // Set tolerance.
    const double tolerance = 1.0e-6;

    // Create test string.
    std::string testString( "1.5362" );

    // Create a linear field transform object: y = -56 * x + 0.
    LinearFieldTransform testLinearFieldTransform3( -56.0, 0.0 );

    // Define expected value: -56 * 1.5362 + 0 = -86.0272.
    const double expectedValue = -86.0272;

    // Transform test string.
    std::shared_ptr< std::string > returnedValue =
            testLinearFieldTransform3.transform( testString );

    // Check that returned and expected strings are identical.
    BOOST_CHECK_CLOSE_FRACTION( std::stod( *returnedValue ), expectedValue,
                                tolerance );
}

//! Test the linear transformation functionality with no slope nor intercept (a=b=0).
BOOST_AUTO_TEST_CASE( testLinearFieldTransform4 )
{
    // Using declaration.
    using namespace input_output;

    // Set tolerance.
    const double tolerance = 1.0e-6;

    // Create test string.
    std::string testString( "1.5362" );

    // Create a linear field transform object: y = 0 * x + 0.
    LinearFieldTransform testLinearFieldTransform4( 0.0, 0.0 );

    // Transform test string.
    std::shared_ptr< std::string > returnedValue =
            testLinearFieldTransform4.transform( testString );

    // Check that returned and expected strings are identical.
    BOOST_CHECK_SMALL( std::stod( *returnedValue ), tolerance );
}

// Close Boost test suite.
BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


#include "profile_axi.hpp"
//#include "profile_python.hpp"
#include <boost/python.hpp>
#include <pyublas/numpy.hpp>

namespace gd {
using namespace boost::python;

template<class ProfileType, class Ctor>
	void py_export_profile_potential_axi(string name) {
		class_<ProfileType, bases<PotentialAxi> >(name.c_str(), Ctor())
			.def("potentialrhoz", (&ProfileType::potentialxz))
			.def("potentialxz_eff", (&ProfileType::potentialxz_eff))
			;
	}
template<class ProfileType, class Ctor>
	class_<ProfileType, bases<ProfileAxi> > py_export_profile_profile_axi(string name) {
		return class_<ProfileType, bases<ProfileAxi> >(name.c_str(), Ctor())
			.def("densityxz", (&ProfileType::densityxz))
			.def("potentialxz", (&ProfileType::potentialxz))
			.def("potentialxz_eff", (&ProfileType::potentialxz_eff))
			.def("dphidx", (&ProfileType::dphidx))
			.def("dphidz", (&ProfileType::dphidz))
			.def("dphidx_eff", (&ProfileType::dphidx_eff))
			.def("dphidz_eff", (&ProfileType::dphidz_eff))
			;
	}

void py_export_profile_axi() {
	class_< PotentialAxi, boost::noncopyable >("PotentialAxi", no_init);
	class_< ProfileAxi, boost::noncopyable >("ProfileAxi", no_init);
	class_< ProfileModelAxi, boost::noncopyable >("ProfileModel", no_init)
		;
	class_<ProfileModelAxi1C, bases<ProfileModelAxi>, boost::noncopyable  >("ProfileModelAxi1C", init<ProfileAxi*>())
		.def("densityxz", (&ProfileModelAxi1C::densityxz))
		.def("potentialxz", (&ProfileModelAxi1C::potentialxz))
		.def("potentialxz_eff", (&ProfileModelAxi1C::potentialxz_eff))
		.def("dphidx", (&ProfileModelAxi1C::dphidx))
		.def("dphidz", (&ProfileModelAxi1C::dphidz))
		.def("dphidx_eff", (&ProfileModelAxi1C::dphidx_eff))
		.def("dphidz_eff", (&ProfileModelAxi1C::dphidz_eff))
		;
	/*class_<ProfileModel2C, bases<ProfileModel>, boost::noncopyable  >("ProfileModel2C", init<Profile*,Profile*>())
		.def("densityr", (&ProfileModel2C::densityr))
		.def("dphidr", (&ProfileModel2C::dphidr))
		.def("potentialr", (&ProfileModel2C::potentialr))
		;

	*/
		//.def("densityr", pure_virtual(&Profile::densityr))
	/*class_< DensityAxi, boost::noncopyable >("DensityAxi", no_init)
		//.def("densityr", pure_virtual(&Density::densityr))
		;*/
	py_export_profile_profile_axi<LogarithmicAxi, init<double, double, double, double>>("LogarithmicAxi");
	py_export_profile_profile_axi<PerfectSpheroidOblate, init<double, double, double, double>>("PerfectSpheroidOblate")
		.def("density_conf", (&PerfectSpheroidOblate::density_conf))
		.def("G_tau", (&PerfectSpheroidOblate::G_tau))
		.def("G_tau_prime", (&PerfectSpheroidOblate::G_tau_prime))
		.def("dphi_dla", (&PerfectSpheroidOblate::dphi_dla))
		.def("dphi_dnu", (&PerfectSpheroidOblate::dphi_dnu))
		.def("dphi_dx", (&PerfectSpheroidOblate::dphidx))
		.def("dphi_dx", (&PerfectSpheroidOblate::dphidz))
		;

}

}


// Distributed under the MIT License.
// See LICENSE.txt for details.

#pragma once

#include <boost/preprocessor/list/for_each.hpp>
#include <boost/preprocessor/tuple/to_list.hpp>
#include <cstddef>

#include "DataStructures/DataBox/Prefixes.hpp"
#include "DataStructures/Tensor/Tensor.hpp"
#include "Evolution/Systems/GeneralizedHarmonic/Tags.hpp"
#include "NumericalAlgorithms/LinearOperators/PartialDerivatives.hpp"  // for tags
#include "Options/Options.hpp"
#include "PointwiseFunctions/GeneralRelativity/Tags.hpp"
#include "Utilities/GenerateInstantiations.hpp"
#include "Utilities/TMPL.hpp"
#include "Utilities/TaggedTuple.hpp"

// IWYU pragma: no_forward_declare Tags::deriv

/// \cond
class DataVector;
namespace PUP {
class er;
}  // namespace PUP
/// \endcond

namespace GeneralizedHarmonic {
namespace Solutions {

/*!
 * \brief A wrapper for general-relativity analytic solutions that loads
 * the analytic solution and then adds a function that returns
 * any combination of the generalized-harmonic evolution variables,
 * specifically `gr::Tags::SpacetimeMetric`, `GeneralizedHarmonic::Tags::Pi`,
 * and `GeneralizedHarmonic::Tags::Phi`
 */
template <typename SolutionType>
class WrappedGr : public SolutionType {
 public:
  using SolutionType::SolutionType;

  static constexpr size_t volume_dim = SolutionType::volume_dim;
  using options = typename SolutionType::options;
  static constexpr Options::String help = SolutionType::help;
  static std::string name() { return Options::name<SolutionType>(); }

  using DerivLapse = ::Tags::deriv<gr::Tags::Lapse<DataVector>,
                                   tmpl::size_t<volume_dim>, Frame::Inertial>;
  using DerivShift =
      ::Tags::deriv<gr::Tags::Shift<volume_dim, Frame::Inertial, DataVector>,
                    tmpl::size_t<volume_dim>, Frame::Inertial>;
  using DerivSpatialMetric = ::Tags::deriv<
      gr::Tags::SpatialMetric<volume_dim, Frame::Inertial, DataVector>,
      tmpl::size_t<volume_dim>, Frame::Inertial>;
  using TimeDerivLapse = ::Tags::dt<gr::Tags::Lapse<DataVector>>;
  using TimeDerivShift =
      ::Tags::dt<gr::Tags::Shift<volume_dim, Frame::Inertial, DataVector>>;
  using TimeDerivSpatialMetric = ::Tags::dt<
      gr::Tags::SpatialMetric<volume_dim, Frame::Inertial, DataVector>>;

  using IntermediateVars = tuples::tagged_tuple_from_typelist<
      typename SolutionType::template tags<DataVector>>;

  template <typename DataType>
  using tags = tmpl::push_back<
      typename SolutionType::template tags<DataType>,
      gr::Tags::SpacetimeMetric<volume_dim, Frame::Inertial, DataType>,
      GeneralizedHarmonic::Tags::Pi<volume_dim, Frame::Inertial>,
      GeneralizedHarmonic::Tags::Phi<volume_dim, Frame::Inertial>>;

  template <typename... Tags>
  tuples::TaggedTuple<Tags...> variables(
      const tnsr::I<DataVector, volume_dim>& x, double t,
      tmpl::list<Tags...> /*meta*/) const {
    // Get the underlying solution's variables using the solution's tags list,
    // store in IntermediateVariables
    const IntermediateVars& intermediate_vars = SolutionType::variables(
        x, t, typename SolutionType::template tags<DataVector>{});

    return {
        get<Tags>(variables(x, t, tmpl::list<Tags>{}, intermediate_vars))...};
  }

  template <typename Tag>
  tuples::TaggedTuple<Tag> variables(const tnsr::I<DataVector, volume_dim>& x,
                                     double t, tmpl::list<Tag> /*meta*/) const {
    const IntermediateVars& intermediate_vars = SolutionType::variables(
        x, t, typename SolutionType::template tags<DataVector>{});
    return {get<Tag>(variables(x, t, tmpl::list<Tag>{}, intermediate_vars))};
  }

  // overloads for wrapping analytic data

  template <typename... Tags>
  tuples::TaggedTuple<Tags...> variables(
      const tnsr::I<DataVector, volume_dim>& x,
      tmpl::list<Tags...> /*meta*/) const {
    // Get the underlying solution's variables using the solution's tags list,
    // store in IntermediateVariables
    const IntermediateVars intermediate_vars = SolutionType::variables(
        x, typename SolutionType::template tags<DataVector>{});

    return {get<Tags>(variables(x, tmpl::list<Tags>{}, intermediate_vars))...};
  }

  template <typename Tag>
  tuples::TaggedTuple<Tag> variables(const tnsr::I<DataVector, volume_dim>& x,
                                     tmpl::list<Tag> /*meta*/) const {
    const IntermediateVars intermediate_vars = SolutionType::variables(
        x, typename SolutionType::template tags<DataVector>{});
    return {get<Tag>(variables(x, tmpl::list<Tag>{}, intermediate_vars))};
  }

  // NOLINTNEXTLINE(google-runtime-references)
  void pup(PUP::er& p) { SolutionType::pup(p); }

 private:
  // Preprocessor logic to avoid declaring variables() functions for
  // tags other than the three the wrapper adds (i.e., other than
  // gr::Tags::SpacetimeMetric, GeneralizedHarmonic::Tags::Pi, and
  // GeneralizedHarmonic:Tags::Phi)
  using TagShift = gr::Tags::Shift<volume_dim, Frame::Inertial, DataVector>;
  using TagSpatialMetric =
      gr::Tags::SpatialMetric<volume_dim, Frame::Inertial, DataVector>;
  using TagInverseSpatialMetric =
      gr::Tags::InverseSpatialMetric<volume_dim, Frame::Inertial, DataVector>;
  using TagExCurvature =
      gr::Tags::ExtrinsicCurvature<volume_dim, Frame::Inertial, DataVector>;

  template <
      typename Tag,
      Requires<tmpl::list_contains_v<
          typename SolutionType::template tags<DataVector>, Tag>> = nullptr>
  tuples::TaggedTuple<Tag> variables(
      const tnsr::I<DataVector, volume_dim>& /*x*/, double /*t*/,
      tmpl::list<Tag> /*meta*/,
      const IntermediateVars& intermediate_vars) const {
    return {get<Tag>(intermediate_vars)};
  }

  template <
      typename Tag,
      Requires<tmpl::list_contains_v<
          typename SolutionType::template tags<DataVector>, Tag>> = nullptr>
  tuples::TaggedTuple<Tag> variables(
      const tnsr::I<DataVector, volume_dim>& /*x*/, tmpl::list<Tag> /*meta*/,
      const IntermediateVars& intermediate_vars) const {
    return {get<Tag>(intermediate_vars)};
  }

  template <
      typename Tag,
      Requires<not tmpl::list_contains_v<
          typename SolutionType::template tags<DataVector>, Tag>> = nullptr>
  tuples::TaggedTuple<Tag> variables(
      const tnsr::I<DataVector, volume_dim>& x, double /*t*/,
      tmpl::list<Tag> tag_list,
      const IntermediateVars& intermediate_vars) const {
    return variables(x, tag_list, intermediate_vars);
  }

  tuples::TaggedTuple<
      gr::Tags::SpacetimeMetric<volume_dim, Frame::Inertial, DataVector>>
  variables(const tnsr::I<DataVector, volume_dim>& /*x*/,
            tmpl::list<gr::Tags::SpacetimeMetric<volume_dim, Frame::Inertial,
                                                 DataVector>> /*meta*/,
            const IntermediateVars& intermediate_vars) const;
  tuples::TaggedTuple<
      GeneralizedHarmonic::Tags::Pi<volume_dim, Frame::Inertial>>
  variables(const tnsr::I<DataVector, volume_dim>& /*x*/,
            tmpl::list<GeneralizedHarmonic::Tags::Pi<volume_dim,
                                                     Frame::Inertial>> /*meta*/,
            const IntermediateVars& intermediate_vars) const;
  tuples::TaggedTuple<
      GeneralizedHarmonic::Tags::Phi<volume_dim, Frame::Inertial>>
  variables(
      const tnsr::I<DataVector, volume_dim>& /*x*/,
      tmpl::list<
          GeneralizedHarmonic::Tags::Phi<volume_dim, Frame::Inertial>> /*meta*/,
      const IntermediateVars& intermediate_vars) const;
};

template <typename SolutionType>
inline constexpr bool operator==(const WrappedGr<SolutionType>& lhs,
                                 const WrappedGr<SolutionType>& rhs) {
  return dynamic_cast<const SolutionType&>(lhs) ==
         dynamic_cast<const SolutionType&>(rhs);
}

template <typename SolutionType>
inline constexpr bool operator!=(const WrappedGr<SolutionType>& lhs,
                                 const WrappedGr<SolutionType>& rhs) {
  return not(lhs == rhs);
}
}  // namespace Solutions
}  // namespace GeneralizedHarmonic


/*
# Copyright 2021 Harderthan
# Original Code is from https://github.com/Geonhee-LEE/mpc_ros
#
# 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.
*/

#ifndef MPC_H
#define MPC_H

#include <Eigen/Core>
#include <map>
#include <vector>

using namespace std;

class MPCCore {
public:
  MPCCore();

  // Solve the model given an initial state and polynomial coefficients.
  // Return the first actuatotions.
  vector<double> Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs);
  vector<double> mpc_x;
  vector<double> mpc_y;
  vector<double> mpc_theta;

  void LoadParams(const std::map<string, double> &params);

private:
  // Parameters for mpc solver
  double _max_angvel, _max_throttle, _bound_value;
  int _mpc_steps, _x_start, _y_start, _theta_start, _v_start, _cte_start,
      _etheta_start, _angvel_start, _a_start;
  std::map<string, double> _params;

  unsigned int dis_cnt;
};

#endif /* MPC_H */


#include <iostream>
#include "CustomLibrary/StateSaver.h"
#include <Eigen/Dense>

const int ncampioni = 1;

double** StateSaver::buffer_7 = nullptr;
double** StateSaver::buffer_6 = nullptr;

Eigen::Matrix<double,7,1> StateSaver::tau_measured;
Eigen::Matrix<double,7,1> StateSaver::q;
Eigen::Matrix<double, 7, 1> StateSaver::qdot;
Eigen::Matrix<double, 7, 1> StateSaver::pose;
Eigen::Matrix<double, 6, 1> StateSaver::error;

int main(){

    StateSaver::buffer_7 = new double*[ncampioni * 7];
    StateSaver::buffer_6 = new double*[ncampioni * 6];

for(int i = 0; i< ncampioni*7; i++)
    StateSaver::buffer_7[i] = new double[4]; // tau_m, q, dq, pose

for(int i = 0; i< ncampioni*6; i++)
    StateSaver::buffer_6[i] = new double[1]; // error

StateSaver sv;

Eigen::Matrix<double,6,1> error;
error << 1,1,1,1,1,1;
sv.error = error;

Eigen::Matrix<double,7,1> tau_m;
tau_m << 3,1,3,3,2,2,2;
sv.tau_measured = tau_m;

Eigen::Matrix<double,7,1> q;
q << 1,4,4,4,33,3,3;
sv.q = q;

Eigen::Matrix<double,7,1> qdot;
qdot << 1,2,3,4,5,5,5;
sv.qdot = qdot;

Eigen::Matrix<double,7,1> pose;
pose << 2,2,2,2,3,4,6;
sv.pose = pose;

    // Si lanciano due thread che bufferizzano i dati raccolti
        sv.fill_buffer();
    // Si attende che i thread terminino la scrittura nei buffer
        sv.wait_filler();

    // Scrittura su file
    sv.scrivi_su_file(ncampioni);
}


// -*- mode:c++; fill-column: 100; -*-

#include "racecar_potential_field_controller/controller.h"

#include <algorithm>
#include <cmath>
#include <cassert>
#include <sstream>

#include <Eigen/Dense>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <boost/bind.hpp>

namespace racecar_potential_field_controller
{

/* helper functions for visualization */
visualization_msgs::Marker marker(const std::string& frame_id, const std::string& ns, int id);
visualization_msgs::Marker arrow(visualization_msgs::Marker viz, double x, double y);
visualization_msgs::Marker lines(visualization_msgs::Marker viz, int skip,
                                 const Eigen::ArrayXf& points_x, const Eigen::ArrayXf& points_y,
                                 const Eigen::ArrayXf& forces_x, const Eigen::ArrayXf& forces_y);
visualization_msgs::Marker text(visualization_msgs::Marker viz, double x, double y, std::string text);

Controller::Controller(ros::NodeHandle nh,
		       ros::NodeHandle private_nh) :
  force_scale_x_(0.07), force_scale_y_(0.07), force_offset_x_(100), force_offset_y_(0),
  speed_p_gain_(0.05), steering_p_gain_(1.0), steering_d_gain_(0.1), viz_forces_scale_(0.07),
  viz_net_force_scale_(0.014), force_angle_last_(0.0)
{
  // dynamic parameters
  dynamic_param_server_.setCallback(boost::bind(&Controller::paramCallback, this, _1, _2));

  // create velocity publisher
  vel_pub_ = nh.advertise<ackermann_msgs::AckermannDriveStamped>("navigation", 10);
  viz_pub_ = nh.advertise<visualization_msgs::MarkerArray>("/visualization_marker_array", 10);

  // subscribe to laser scanner
  scan_sub_ = nh.subscribe("/scan", 10, &Controller::scanCallback, this);

  // create a 40Hz timer for setting commands
  timer_ = nh.createTimer(ros::Duration(1.0/40.0), &Controller::timerCallback, this);
}

void Controller::paramCallback(RacecarPotentialFieldControllerConfig& cfg, uint32_t level)
{
  ROS_INFO("racecar_potential_field_controller reconfigure request");
  force_scale_x_ = cfg.force_scale_x;
  force_scale_y_ = cfg.force_scale_y;
  force_offset_x_ = cfg.force_offset_x;
  force_offset_y_ = cfg.force_offset_y;
  speed_p_gain_ = cfg.speed_p_gain;
  steering_p_gain_ = cfg.steering_p_gain;
  steering_d_gain_ = cfg.steering_d_gain;
  viz_forces_scale_ = cfg.viz_forces_scale;
  viz_net_force_scale_ = cfg.viz_net_force_scale;
}

void Controller::timerCallback(const ros::TimerEvent& event)
{
  static bool updating(false);
  ackermann_msgs::AckermannDriveStamped::Ptr cmd(new ackermann_msgs::AckermannDriveStamped);

  if (ros::Time::now() - last_cmd_.header.stamp > ros::Duration(0.1)) {
    if (updating) {
      ROS_WARN("racecar_potential_field_controller not updating navigation command");
      updating = false;
    }
    return;
  }
  else if (!updating) {
    ROS_INFO("racecar_potential_field_controller updating navigation command");
    updating = true;
  }

  *cmd = last_cmd_;
  vel_pub_.publish(cmd);
}

void Controller::scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
  assert(scan && scan->ranges.size() > 0);

  // get index of scans between -/+ 90 degrees
  int idx_begin = ceil((-M_PI_2 - scan->angle_min) / scan->angle_increment);
  int idx_end = ceil((M_PI_2 - scan->angle_min) / scan->angle_increment); // past-the-end
  assert(idx_begin >= 0 && idx_begin < scan->ranges.size() &&
         idx_end >= 0 && idx_end <= scan->ranges.size() && idx_begin < idx_end);

  // copy ranges
  Eigen::ArrayXf ranges(idx_end - idx_begin);
  std::copy(scan->ranges.begin() + idx_begin, scan->ranges.begin() + idx_end, ranges.data());

  // convert scan index to angles
  Eigen::ArrayXf angles(Eigen::ArrayXf::LinSpaced(idx_end - idx_begin,
      scan->angle_min + idx_begin * scan->angle_increment,
      scan->angle_min + (idx_end - 1) * scan->angle_increment));
  assert(ranges.size() == angles.size());

  // compute "force" from points
  Eigen::ArrayXf neg_ranges_2(-1*ranges.square());
  Eigen::ArrayXf angles_cos(angles.cos());
  Eigen::ArrayXf angles_sin(angles.sin());
  Eigen::ArrayXf forces_x(angles_cos/neg_ranges_2);
  Eigen::ArrayXf forces_y(angles_sin/neg_ranges_2);
  Eigen::ArrayXf points_x(ranges * angles_cos);
  Eigen::ArrayXf points_y(ranges * angles_sin);
  float net_force_x = force_scale_x_ * forces_x.sum() + force_offset_x_;
  float net_force_y = force_scale_y_ * forces_y.sum() + force_offset_y_;

  double force_angle = atan2(net_force_y, net_force_x);
  if (fabs(force_angle) > 0.5) {
    force_angle = (force_angle < 0 ? -1.0 : 1.0) * 0.5;
  }
  last_cmd_.header.stamp = scan->header.stamp;
  last_cmd_.drive.speed = speed_p_gain_ * sqrt(net_force_x*net_force_x + net_force_y*net_force_y);
  if (net_force_x < 0)
    last_cmd_.drive.speed *= -1;
  last_cmd_.drive.steering_angle = steering_p_gain_ * force_angle +
    steering_d_gain_ * (force_angle - force_angle_last_);

  // set force_angle_last_ for use in derivative term on next iteration
  // (note: there are better approximations for derivative)
  force_angle_last_ = force_angle;

  // visualization of "forces"
  visualization_msgs::MarkerArray::Ptr viz(new visualization_msgs::MarkerArray);
  if (fabs(viz_net_force_scale_) > 0) {
    viz->markers.push_back(arrow(marker("laser", "net_force", 0),
                                 net_force_x * viz_net_force_scale_,
                                 net_force_y * viz_net_force_scale_));
  }
  if (fabs(viz_forces_scale_) > 0) {
    viz->markers.push_back(lines(marker("laser", "forces", 0), 12,
                                 points_x, points_y,
                                 forces_x * viz_forces_scale_,
                                 forces_y * viz_forces_scale_));
  }
  std::ostringstream status;
  status << std::setprecision(2) << "Speed: " << last_cmd_.drive.speed << std::setprecision(3) << "\nSteering: " << last_cmd_.drive.steering_angle;
  viz->markers.push_back(text(marker("base_link", "status", 0), -0.5, 0, status.str()));
  viz_pub_.publish(viz);
}

visualization_msgs::Marker marker(const std::string& frame_id, const std::string& ns, int id)
{
  // Marker constructor zero-initializes
  visualization_msgs::Marker viz;

  viz.header.frame_id = frame_id;
  viz.ns = ns;
  viz.id = id;
  viz.action = 0;
  viz.pose.orientation.w = 1;
  viz.color.a = 1;
  viz.lifetime = ros::Duration(0.1);
  viz.frame_locked = true;
  viz.points.clear();
  viz.colors.clear();

  return viz;
}

visualization_msgs::Marker arrow(visualization_msgs::Marker viz, double x, double y)
{
  viz.type = visualization_msgs::Marker::ARROW;
  viz.scale.x = 0.03;
  viz.scale.y = 0.06;
  viz.scale.z = 0.06;
  viz.color.r = 1;
  viz.color.g = 0;
  viz.color.b = 1;
  viz.points.resize(2);
  viz.points[0].x = 0;
  viz.points[0].y = 0;
  viz.points[0].z = viz.scale.z;
  viz.points[1].x = x;
  viz.points[1].y = y;
  viz.points[1].z = viz.scale.z;

  return viz;
}

visualization_msgs::Marker lines(visualization_msgs::Marker viz, int skip,
                                 const Eigen::ArrayXf& points_x, const Eigen::ArrayXf& points_y,
                                 const Eigen::ArrayXf& forces_x, const Eigen::ArrayXf& forces_y)
{
  assert(points_x.size() == points_y.size() &&
         points_y.size() == forces_x.size() &&
         forces_x.size() == forces_y.size());

  viz.type = visualization_msgs::Marker::LINE_LIST;
  viz.scale.x = 0.03;
  viz.scale.y = 0;
  viz.scale.z = 0;
  viz.color.r = 0;
  viz.color.g = 0;
  viz.color.b = 1;
  viz.points.reserve(points_x.size() / skip * 2);

  for (int i = 0; i < points_x.size(); i += skip) {
    visualization_msgs::Marker::_points_type::value_type origin, end;
    origin.x = points_x(i);
    origin.y = points_y(i);
    end.x = points_x(i) + forces_x(i);
    end.y = points_y(i) + forces_y(i);
    viz.points.push_back(origin);
    viz.points.push_back(end);
  }

  return viz;
}

visualization_msgs::Marker text(visualization_msgs::Marker viz, double x, double y, std::string text)
{
  viz.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
  viz.pose.position.x = x;
  viz.pose.position.y = y;
  viz.scale.z = 0.3;
  viz.color.r = 1;
  viz.color.g = 1;
  viz.color.b = 1;
  viz.text = text;

  return viz;
}

} // namespace racecar_potential_field_controller