/*******************************************************
 * Copyright (c) 2014, ArrayFire
 * All rights reserved.
 *
 * This file is distributed under 3-clause BSD license.
 * The complete license agreement can be obtained at:
 * http://arrayfire.com/licenses/BSD-3-Clause
 ********************************************************/

#include <af/dim4.hpp>
#include <af/defines.h>
#include <ArrayInfo.hpp>
#include <Array.hpp>
#include <set.hpp>
#include <copy.hpp>
#include <sort.hpp>
#include <err_opencl.hpp>
#include <boost/compute/algorithm/set_intersection.hpp>
#include <boost/compute/algorithm/set_union.hpp>
#include <boost/compute/algorithm/sort.hpp>
#include <boost/compute/algorithm/unique.hpp>
#include <boost/compute/iterator/buffer_iterator.hpp>

namespace compute = boost::compute;

namespace opencl
{
    using af::dim4;

    template<typename T>
    Array<T> setUnique(const Array<T> &in,
                       const bool is_sorted)
    {
        try {
            Array<T> out = copyArray<T>(in);

            compute::command_queue queue(getQueue()());

            compute::buffer out_data((*out.get())());

            compute::buffer_iterator<T> begin(out_data, 0);
            compute::buffer_iterator<T> end(out_data, out.dims()[0]);

            if (!is_sorted) {
                compute::sort(begin, end, queue);
            }

            end = compute::unique(begin, end, queue);

            out.resetDims(dim4(std::distance(begin, end), 1, 1, 1));

            return out;
        } catch (std::exception &ex) {
            AF_ERROR(ex.what(), AF_ERR_INTERNAL);
        }
    }

    template<typename T>
    Array<T> setUnion(const Array<T> &first,
                      const Array<T> &second,
                      const bool is_unique)
    {
        try {
            Array<T> unique_first = first;
            Array<T> unique_second = second;

            if (!is_unique) {
                unique_first  = setUnique(first, false);
                unique_second = setUnique(second, false);
            }

            size_t out_size = unique_first.dims()[0] + unique_second.dims()[0];
            Array<T> out = createEmptyArray<T>(dim4(out_size, 1, 1, 1));

            compute::command_queue queue(getQueue()());

            compute::buffer first_data((*unique_first.get())());
            compute::buffer second_data((*unique_second.get())());
            compute::buffer out_data((*out.get())());

            compute::buffer_iterator<T> first_begin(first_data, 0);
            compute::buffer_iterator<T> first_end(first_data, unique_first.dims()[0]);
            compute::buffer_iterator<T> second_begin(second_data, 0);
            compute::buffer_iterator<T> second_end(second_data, unique_second.dims()[0]);
            compute::buffer_iterator<T> out_begin(out_data, 0);

            compute::buffer_iterator<T> out_end = compute::set_union(
                first_begin, first_end, second_begin, second_end, out_begin, queue
                );

            out.resetDims(dim4(std::distance(out_begin, out_end), 1, 1, 1));
            return out;

        } catch (std::exception &ex) {
            AF_ERROR(ex.what(), AF_ERR_INTERNAL);
        }
    }

    template<typename T>
    Array<T> setIntersect(const Array<T> &first,
                          const Array<T> &second,
                          const bool is_unique)
    {
        try {
            Array<T> unique_first = first;
            Array<T> unique_second = second;

            if (!is_unique) {
                unique_first  = setUnique(first, false);
                unique_second = setUnique(second, false);
            }

            size_t out_size = std::max(unique_first.dims()[0], unique_second.dims()[0]);
            Array<T> out = createEmptyArray<T>(dim4(out_size, 1, 1, 1));

            compute::command_queue queue(getQueue()());

            compute::buffer first_data((*unique_first.get())());
            compute::buffer second_data((*unique_second.get())());
            compute::buffer out_data((*out.get())());

            compute::buffer_iterator<T> first_begin(first_data, 0);
            compute::buffer_iterator<T> first_end(first_data, unique_first.dims()[0]);
            compute::buffer_iterator<T> second_begin(second_data, 0);
            compute::buffer_iterator<T> second_end(second_data, unique_second.dims()[0]);
            compute::buffer_iterator<T> out_begin(out_data, 0);

            compute::buffer_iterator<T> out_end = compute::set_intersection(
                first_begin, first_end, second_begin, second_end, out_begin, queue
                );

            out.resetDims(dim4(std::distance(out_begin, out_end), 1, 1, 1));
            return out;
        } catch (std::exception &ex) {
            AF_ERROR(ex.what(), AF_ERR_INTERNAL);
        }
    }

#define INSTANTIATE(T)                                                  \
    template Array<T> setUnique<T>(const Array<T> &in, const bool is_sorted); \
    template Array<T> setUnion<T>(const Array<T> &first, const Array<T> &second, const bool is_unique); \
    template Array<T> setIntersect<T>(const Array<T> &first, const Array<T> &second, const bool is_unique); \

    INSTANTIATE(float)
    INSTANTIATE(double)
    INSTANTIATE(int)
    INSTANTIATE(uint)
    INSTANTIATE(char)
    INSTANTIATE(uchar)
}


//
// Copyright (c) 2018 CNRS
//

#ifndef __pinocchio_math_ccpad_hpp__
#define __pinocchio_math_ccpad_hpp__

// Do not include this file directly.
// Copy and use directly the intructions from <cppad/example/cppad_eigen.hpp>
// to avoid redifinition of EIGEN_MATRIXBASE_PLUGIN for Eigen 3.3.0 and later
//#include <cppad/example/cppad_eigen.hpp>

#ifdef PINOCCHIO_CPPAD_REQUIRES_MATRIX_BASE_PLUGIN
#define EIGEN_MATRIXBASE_PLUGIN <cppad/example/eigen_plugin.hpp>
#endif

#if defined(PINOCCHIO_WITH_CPPADCG_SUPPORT) && defined(PINOCCHIO_WITH_CXX11_SUPPORT)
  #include "pinocchio/math/cppadcg.hpp" // already include <Eigen/Dense>
#else
  #include <cppad/cppad.hpp>
  #include <Eigen/Dense>
#endif

namespace Eigen
{
  namespace internal
  {
    // Specialization of Eigen::internal::cast_impl for CppAD input types
    template<typename Scalar>
    struct cast_impl<CppAD::AD<Scalar>,Scalar>
    {
#if EIGEN_VERSION_AT_LEAST(3,2,90)
      EIGEN_DEVICE_FUNC
#endif
      static inline Scalar run(const CppAD::AD<Scalar> & x)
      {
        return CppAD::Value(x);
      }
    };
  }
}

/// Source from #include <cppad/example/cppad_eigen.hpp>
namespace Eigen
{
  template <class Base> struct NumTraits< CppAD::AD<Base> >
  {  // type that corresponds to the real part of an AD<Base> value
    typedef CppAD::AD<Base>   Real;
    // type for AD<Base> operations that result in non-integer values
    typedef CppAD::AD<Base>   NonInteger;
    //  type to use for numeric literals such as "2" or "0.5".
    typedef CppAD::AD<Base>   Literal;
    // type for nested value inside an AD<Base> expression tree
    typedef CppAD::AD<Base>   Nested;

    enum {
      // does not support complex Base types
      IsComplex             = 0 ,
      // does not support integer Base types
      IsInteger             = 0 ,
      // only support signed Base types
      IsSigned              = 1 ,
      // must initialize an AD<Base> object
      RequireInitialization = 1 ,
      // computational cost of the corresponding operations
      ReadCost              = 1 ,
      AddCost               = 2 ,
      MulCost               = 2
    };

    // machine epsilon with type of real part of x
    // (use assumption that Base is not complex)
    static CppAD::AD<Base> epsilon(void)
    {  return CppAD::numeric_limits< CppAD::AD<Base> >::epsilon(); }

    // relaxed version of machine epsilon for comparison of different
    // operations that should result in the same value
    static CppAD::AD<Base> dummy_precision(void)
    {  return 100. *
      CppAD::numeric_limits< CppAD::AD<Base> >::epsilon();
    }

    // minimum normalized positive value
    static CppAD::AD<Base> lowest(void)
    {  return CppAD::numeric_limits< CppAD::AD<Base> >::min(); }

    // maximum finite value
    static CppAD::AD<Base> highest(void)
    {  return CppAD::numeric_limits< CppAD::AD<Base> >::max(); }

    // number of decimal digits that can be represented without change.
    static int digits10(void)
    {  return CppAD::numeric_limits< CppAD::AD<Base> >::digits10; }
  };
}

/// Source from #include <cppad/example/cppad_eigen.hpp>
namespace CppAD
{
  // functions that return references
  template <class Base> const AD<Base>& conj(const AD<Base>& x)
  {  return x; }
  template <class Base> const AD<Base>& real(const AD<Base>& x)
  {  return x; }

  // functions that return values (note abs is defined by cppad.hpp)
  template <class Base> AD<Base> imag(const AD<Base>& /*x*/)
  {  return CppAD::AD<Base>(0.); }
  template <class Base> AD<Base> abs2(const AD<Base>& x)
  {  return x * x; }
}

#endif // #ifndef __pinocchio_math_ccpad_hpp__


#include <Eigen/Core>

#include <gridmap/grids/grid_2d.h>
#include <gridmap/operations/rasterize.h>
#include <gridmap/operations/raytrace.h>
#include <gtest/gtest.h>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>

/*
TEST(test_raytrace, test_raytrace)
{
    gridmap::MapDimensions map_dims(1, {0, 0}, {1000, 1000});

    gridmap::Grid2D<uint8_t> grid(map_dims);

    cv::Mat cv_im = cv::Mat(grid.dimensions().size().y(), grid.dimensions().size().x(), CV_8U,
                            reinterpret_cast<void*>(grid.cells().data()));

    for (int t=0; t<40; ++t)
    {
        const auto t0 = std::chrono::steady_clock::now();

        const std::vector<Eigen::Array2i> line = gridmap::drawLine(map_dims.size().x(), map_dims.size().y(), 0, 0);

        ROS_INFO_STREAM(
                        "efla took "
                        << std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::steady_clock::now() -
t0).count());

        for (std::size_t i=0; i<255; ++i)
        {
            cv_im.at<uint8_t>(cv::Point(line[i].x(), line[i].y())) = static_cast<uint8_t>(i);
        }
    }

    cv::imwrite("grid.png", cv_im);
}
*/

TEST(test_polyfill, test_polyfill)
{
    gridmap::MapDimensions map_dims(1, {0, 0}, {1000, 1000});

    gridmap::Grid2D<uint8_t> grid(map_dims);

    cv::Mat cv_im = cv::Mat(grid.dimensions().size().y(), grid.dimensions().size().x(), CV_8U,
                            reinterpret_cast<void*>(grid.cells().data()));

    std::vector<Eigen::Array2i> polygon;
    polygon.push_back({500, 500});
    polygon.push_back({600, 500});
    polygon.push_back({600, 600});
    polygon.push_back({400, 600});

    auto lambda = [&cv_im](int x, int y) { cv_im.at<uint8_t>(y, x) = 255; };

    for (int t = 0; t < 40; ++t)
    {
        const auto t0 = std::chrono::steady_clock::now();

        gridmap::rasterPolygonFill(lambda, polygon, 400, 600, 500, 600);

        ROS_INFO_STREAM("rasterPolygonFill took " << std::chrono::duration_cast<std::chrono::duration<double>>(
                                                         std::chrono::steady_clock::now() - t0)
                                                         .count());
    }

    cv::imwrite("grid.png", cv_im);
}

int main(int argc, char** argv)
{
    testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}


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

#include <boost/test/unit_test.hpp>
#include "armnnTfParser/ITfParser.hpp"
#include "ParserPrototxtFixture.hpp"

BOOST_AUTO_TEST_SUITE(TensorflowParser)

struct RsqrtFixture : public armnnUtils::ParserPrototxtFixture<armnnTfParser::ITfParser>
{
    RsqrtFixture()
    {
        m_Prototext = "node {\n"
                      "  name: \"input\"\n"
                      "  op: \"Placeholder\"\n"
                      "  attr {\n"
                      "    key: \"dtype\"\n"
                      "    value {\n"
                      "      type: DT_FLOAT\n"
                      "    }\n"
                      "  }\n"
                      "  attr {\n"
                      "    key: \"shape\"\n"
                      "    value {\n"
                      "      shape {\n"
                      "      }\n"
                      "    }\n"
                      "  }\n"
                      "}\n"
                      "node {\n"
                      "  name: \"Rsqrt\"\n"
                      "  op: \"Rsqrt\"\n"
                      "  input: \"input\"\n"
                      "  attr {\n"
                      "    key: \"T\"\n"
                      "    value {\n"
                      "      type: DT_FLOAT\n"
                      "    }\n"
                      "  }\n"
                      "}\n";

        SetupSingleInputSingleOutput({ 2, 2 }, "input", "Rsqrt");
    }
};

BOOST_FIXTURE_TEST_CASE(ParseRsqrt, RsqrtFixture)
{
    RunTest<2>({ 1.f, 4.f, 16.f, 25.f }, { 1.f, 0.5f, 0.25f, 0.2f });
}

BOOST_FIXTURE_TEST_CASE(ParseRsqrtZeroNegative, RsqrtFixture)
{
    RunTest<2>({ 0.f, -0.f, -25.f, -16.f }, { INFINITY, -INFINITY, -NAN, -NAN });
}

BOOST_AUTO_TEST_SUITE_END()


/*
 * Dijkstra.cpp
 *
 * 	\brief     My dijkstra implementation for the tenth exercise
 *  \details   This class computes the longest shortest path for given startingPoint
 *  \author    Julia Baumbach
 *  \date      15.07.2017
 */

#include "DijkstraSolver.h"

#include <climits>
#include <iostream>
#include <boost/heap/fibonacci_heap.hpp>
#include <exception>

/**
 * \struct compare_function
 * \brief defines how 2 dijkstra pairs should be compared at fibonacci-heap
 * \param edge1 first edge to compare
 * \param edge2 second edge to compare
 * \return true if the weight of edge2 is smaller than edge1's weight
 */
struct compare_function{
	bool operator()(const DijkstraPair edge1, const DijkstraPair edge2) const{
		return edge1.second > edge2.second;
	}
};

/**
 * \fn the constructor
 * \brief initialize a new dijkstra instance while sorting the given edges
 * \param weights Vector of the weights for the given graph
 * \param edges Vector of the edges for the given graph
 * \return the new dijkstra instance
 */
DijkstraSolver::DijkstraSolver(SortedEdges sortedEdges, unsigned int numberOfVertices):
	sortedEdges(sortedEdges), numberOfVertices(numberOfVertices) {
}

/**
 * \fn void dijkstra::computeShortestPath
 * \brief computes all shortest paths from given start vertex for the initialized dijkstra instance and save the weights and predecessorMaps in the given maps
 * \param unsigned int numberOfVertices Number of Vertices for the graph
 * \param WeightMap& weightsToVertices map to store the weight to the i-th node at the i-th position
 * \param vector<int>& predecessorMap map to store the i-th's predecessor at the i-th position
 */
void DijkstraSolver::computeShortestPath(unsigned int startNode, WeightMap& weightsToVertices, std::vector<int>& predecessorMap){
	if(startNode > numberOfVertices){
		std::cerr << "Index of StartVertex must be less or equal to number of vertices" << std::endl;
		throw std::exception();
	}
	VisitedMap alreadyVisited(numberOfVertices+1, false);

	//Start in point startNode
	weightsToVertices[startNode] = 0;
	predecessorMap[startNode] = startNode;
	int currentVertex = startNode;
	int currentDist = 0;

	boost::heap::fibonacci_heap<DijkstraPair, boost::heap::compare<compare_function>> heap;
	//Put the start vertex in the verticesToVisit list
	heap.push(std::make_pair(startNode, 0));

	while(!heap.empty()){
		if (!alreadyVisited.at(heap.top().first)){
			currentVertex = heap.top().first;
			currentDist = heap.top().second;

			heap.pop();

			std::vector<DijkstraPair> currentEdges = sortedEdges.at(currentVertex);

			for (const DijkstraPair pair : currentEdges){
				//Find out the current neighbor vertex and the weight of the current edge
				int neighborVertex = pair.first;
				int currentWeight = pair.second;
				//Search for the right position in verticesToVisit-list. If Vertex is already visited, don't add this edge
				if (!alreadyVisited.at(neighborVertex)){
					heap.push(std::make_pair(neighborVertex, currentDist + currentWeight));
				}

				int currentDistance = currentDist + currentWeight;

				//Update the predecessor and weightsToVerices maps
				if (currentDistance < weightsToVertices[neighborVertex]){
					weightsToVertices[neighborVertex] = currentDistance;
					predecessorMap[neighborVertex] = currentVertex;
				}
			}
			//Current vertex is visited now
			alreadyVisited.at(currentVertex) = true;
		} else {
			heap.pop();
		}
	}
}

/*
 * \fn int updateEdgeWeight(int edgeStart, int edgeEnd)
 * \brief sets EdgeWeight of edge (edgestart, edgeEnd) to 0
 * \param int edgeStart start vertex of the edge
 * \param int edgeEnd end vertex of the edge
 * \return returns the old weight of the edge
 */
int DijkstraSolver::setEdgeWeightToZero(int edgeStart, int edgeEnd){
	std::vector<DijkstraPair>& edges = sortedEdges[edgeStart];

	for(DijkstraPair& pair : edges){
		if(pair.first == edgeEnd){
			int oldWeight = pair.second;
			pair.second = 0;
			return oldWeight;
		}
	}

	return 0;
}


//---------------------------------------------------------------------------//
// Copyright (c) 2014 Roshan <thisisroshansmail@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#ifndef BOOST_COMPUTE_RANDOM_BERNOULLI_DISTRIBUTION_HPP
#define BOOST_COMPUTE_RANDOM_BERNOULLI_DISTRIBUTION_HPP

#include <boost/compute/command_queue.hpp>
#include <boost/compute/function.hpp>
#include <boost/compute/types/fundamental.hpp>
#include <boost/compute/detail/iterator_range_size.hpp>
#include <boost/compute/detail/literal.hpp>

namespace boost {
namespace compute {

///
/// \class bernoulli_distribution
/// \brief Produces random boolean values according to the following
/// discrete probability function with parameter p :
/// P(true/p) = p and P(false/p) = (1 - p)
///
/// The following example shows how to setup a bernoulli distribution to
/// produce random boolean values with parameter p = 0.25
///
/// \snippet test/test_bernoulli_distribution.cpp generate
///
template<class RealType = float>
class bernoulli_distribution
{
public:

    /// Creates a new bernoulli distribution
    bernoulli_distribution(RealType p = 0.5f)
        : m_p(p)
    {
    }

    /// Destroys the bernoulli_distribution object
    ~bernoulli_distribution()
    {
    }

    /// Returns the value of the parameter p
    RealType p() const
    {
        return m_p;
    }

    /// Generates bernoulli distributed booleans and stores
    /// them in the range [\p first, \p last).
    template<class OutputIterator, class Generator>
    void generate(OutputIterator first,
                  OutputIterator last,
                  Generator &generator,
                  command_queue &queue)
    {
        size_t count = detail::iterator_range_size(first, last);

        vector<uint_> tmp(count, queue.get_context());
        generator.generate(tmp.begin(), tmp.end(), queue);

        BOOST_COMPUTE_FUNCTION(bool, scale_random, (const uint_ x),
        {
            return (convert_RealType(x) / MAX_RANDOM) < PARAM;
        });

        scale_random.define("PARAM", detail::make_literal(m_p));
        scale_random.define("MAX_RANDOM", "UINT_MAX");
        scale_random.define(
            "convert_RealType", std::string("convert_") + type_name<RealType>()
        );

        transform(
            tmp.begin(), tmp.end(), first, scale_random, queue
        );
    }

private:
    RealType m_p;
};

} // end compute namespace
} // end boost namespace

#endif // BOOST_COMPUTE_RANDOM_BERNOULLI_DISTRIBUTION_HPP


/**********************************************************************
%
% /// ASAR/MARA Research Group
%
% Technology Arts Sciences TH K�ln
% Technical University of Berlin
% Deutsche Telekom Laboratories
% University of Rostock
% WDR Westdeutscher Rundfunk
% IOSONO GmbH Erfurt
%
% SOFiA sound field analysis
%
% M/F Modal radial filters R13-0306
%     Soft amplification limiting
%     On-axis powerloss compensation with
%     N0plc to N0 interpolation.
%
% Copyright 2011-2017 Benjamin Bernsch�tz, rockzentrale 'AT' me.com
%                        and Nils Peters, nils 'AT' icsi.berkeley.edu
%
% This file is part of the SOFiA toolbox under MIT-License
%
%
% [dn, beam] = SOFIA_MF(N, kr, ac, [a_max], [plc], [fadeover])
% ------------------------------------------------------------------------
% dn          Vector of modal 0-N frequency domain filters
% beam        Expected free field On-Axis kr-response
% ------------------------------------------------------------------------
% N           Maximum Order
% kr          Vector or Matrix of kr values
%             First Row   (M=1) N: kr values Microphone Radius
%             Second Row  (M=2) N: kr values Sphere/Microphone2 Radius
%             [kr_mic;kr_sphere] for Rigid/Dual Sphere Configurations
%             ! If only one kr-vector is given using a Rigid/Dual Sphere
%             Configuration: kr_sphere = kr_mic
% ac          Array Configuration:
%             0  Open Sphere with pressure Transducers (NO plc!)
%             1  Open Sphere with cardioid Transducers
%             2  Rigid Sphere with pressure Transducers
%             3  Rigid Sphere with cardioid Transducers (Thx to Nils Peters!)
%             4  Dual Open Sphere with pressure Transducers (Thx to Nils Peters!)
% a_max       Maximum modal amplification limit in [dB]
% plc         OnAxis powerloss-compensation:
%             0  Off
%             1  Full kr-spectrum plc
%             2  Low kr only -> set fadeover
% fadeover    Number of kr values to fade over +/- around min-distance
%             gap of powerloss compensated filter and normal N0 filters.
%             0 = auto fadeover
%
@ end of header
%
% CONTACT AND LICENSE INFORMATION:
%
% /// ASAR/MARA Research Group
%
%     [1] Technology Arts Sciences TH K�ln
%     [2] Technical University of Berlin
%     [3] Deutsche Telekom Laboratories
%     [4] University of Rostock
%     [5] WDR Westdeutscher Rundfunk
%     [6] IOSONO GmbH Erfurt
%
% SOFiA sound field analysis toolbox
%
% Copyright 2011-2017 Benjamin Bernsch�tz et al.(�)
%
% Contact ------------------------------------
% Technology Arts Sciences TH K�ln
% Institute of Communications Systems
% Betzdorfer Street 2
% D-50679 Germany (Europe)
%
% phone       +49 221 8275 -2496
% cell phone  +49 171 4176069
% mail        rockzentrale 'at' me.com
% --------------------------------------------
%
% This file is part of the SOFiA sound field analysis toolbox
%
% Licence Type: MIT License
%
% 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.
%
%
% (�) Christoph P�rschmann [1]     christoph.poerschmann 'at' th-koeln.de
%     Sascha Spors         [2,3,4] sascha.spors 'at' uni-rostock.de
%     Stefan Weinzierl     [2]     stefan.weinzierl 'at' tu-berlin.de
%     Nils Peters                  nils 'at' icsi.berkeley.edu
%
**********************************************************************/

#include <mex.h>
#include <complex>
#include <math.h>
#include <sofia_radial.h>
#include <boost/math/special_functions/spherical_harmonic.hpp>

#ifndef M_PI
	#define M_PI 3.141592653589793238462643383279502884197169399375105820974944592307816406286208998628034825342117068
#endif

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
   using namespace std;
   using namespace boost::math;

   int N, plc, ac, noplcflag, limiteronflag, normalizebeam, zeroflag;
   long long unsigned int ctr, ctrb, ctrc, numberOfkrValues, locatemindistance, fadeover;
   long long signed int ctrd;
   double *kr, *krm, *krs;
   double *ReturnReal,*ReturnImag,*ReturnRealB,*ReturnImagB;
   double a_max, a_maxdB, amplicalc, mindistance, mix, filtergap;
   complex<double> bnval;
   complex<double> *xi, *beamresponse;
   complex<double> **OutputArray;

   #ifndef DEBUG
    mexPrintf("SOFiA M/F - Modal radial filters R13-0306\n");
   #endif

   if(nrhs<3) {mexErrMsgIdAndTxt("SOFiA:MF:notEnoughInputs",
                 "Minimum 3 Inputs required: N, kr, ac, [a_max], [plc], [fadeover]");}

    //Get first 3 ARGS
    N           =           (int)mxGetScalar(prhs[0]);
    kr          =                    mxGetPr(prhs[1]);
    ac          =           (int)mxGetScalar(prhs[2]);

    if(nrhs<4)
    {
        a_max=1;
        a_maxdB=0;
        limiteronflag=0;
    }
    else
    {
        a_maxdB =(double)mxGetScalar(prhs[3]);
        limiteronflag=1;
        a_max=pow(10,(a_maxdB/(double)20)); //dB-Value to double skalar
    }

    if(nrhs<5) {plc=0;}
    else
    {
       plc = (int)mxGetScalar(prhs[4]);
    }

    if(nrhs<6) {fadeover=0;}
    else
    {
       fadeover = (long long int)mxGetScalar(prhs[5]);
    }

    numberOfkrValues=mxGetN(prhs[1]);

    if(mxGetM(prhs[1])>2)
    {
        {mexErrMsgIdAndTxt("SOFiA:MF:InputArgError",
                 "Vector size not valid: NxM with N=1 or 2 and M=[...] expected.");}
    }

    if(N<0)
    {
        {mexErrMsgIdAndTxt("SOFiA:MF:InputArgError",
                 "N: Order not valid.");}
    }
    if(a_maxdB<-20.0f)
    {
        {mexErrMsgIdAndTxt("SOFiA:MF:InputArgError",
                 "a_max: Amplification limit too low.");}
    }

    if(ac!=0 && ac!=1 && ac!=2 && ac!=3 && ac!=4)
    {
        {mexErrMsgIdAndTxt("SOFiA:MF:InputArgError",
                 "ac: Array configuration not valid. ac=[0,1,2,3,4]");}
    }

    if(plc!=0 && plc!=1 && plc!=2)
    {
        {mexErrMsgIdAndTxt("SOFiA:MF:InputArgError",
                 "plc: Choice not valid. plc=[0,1,2]");}
    }

    try
     {
        //Allocate Output Array Size [N]*[size(kr)]
        OutputArray = new complex<double>*[N+1];

        for(ctr = 0; ctr <= N; ctr++)
           {OutputArray[ctr] = new complex<double>[numberOfkrValues];}

        //Initializate Output Array
        for(ctr = 0; ctr <= N; ctr++)
           {
            for(ctrb = 0; ctrb < numberOfkrValues; ctrb++)
               {
                    OutputArray[ctr][ctrb] = complex<double>(0,0);
               }
           }

     }
     catch(...)
     { mexErrMsgIdAndTxt("SOFiA:MF:OutArrayAllocation",
                 "Not able to allocate memory for the output Matrix. Maybe to large?");
     }

    //Make Dynamic Arrays
    krm    = new double [mxGetN(prhs[1])];
    krs    = new double [mxGetN(prhs[1])];

    for(ctr=0; ctr<(mxGetN(prhs[1])); ctr++)// ctr=ctr+mxGetM(prhs[1]))
    {
        krm[ctr]    = (double)kr[ctr*mxGetM(prhs[1])];
        krs[ctr]    = (double)kr[ctr*mxGetM(prhs[1])+mxGetM(prhs[1])-1];
    }

     //Check for Zero-Elements in kr-Vector
     for(ctr = 0; ctr < numberOfkrValues; ctr++)
     {
        zeroflag=0;
        if (krm[ctr]<=0)
        {
            //mexPrintf("\nWarning: kr (mic) contains zero element or negative value.\n");
            zeroflag=1;

            for(ctrb = ctr; ctrb < numberOfkrValues; ctrb++) //Try to repair with next valid element
            {
                if(krm[ctrb]>0)
                {
                    krm[ctr]=krm[ctrb];
                    //mexPrintf("         Replaced by next valid kr element [%f].\n", krm[ctrb]);
                    zeroflag=0;
                    break;
                }
            }
            if(zeroflag==1)
              {
                 mexErrMsgIdAndTxt("SOFiA:MF:DataNotValid",
                "SOFiA MF Error: kr vector is not valid.");
              }

        }

        zeroflag=0;
        if (krs[ctr]<=0)
        {
            //mexPrintf("\nWarning: kr (sphere) contains zero element or negative value.\n");
            zeroflag=1;

            for(ctrb = ctr; ctrb < numberOfkrValues; ctrb++) //Try to repair with next valid element
            {
                if(krs[ctrb]>0)
                {
                    krs[ctr]=krs[ctrb];
                    //mexPrintf("         Replaced by next valid kr element [%f].\n", krs[ctrb]);
                    zeroflag=0;
                    break;
                }
            }
            if(zeroflag==1)
              {
                 mexErrMsgIdAndTxt("SOFiA:MF:DataNotValid",
                "SOFiA MF Error: kr vector is not valid.");
              }

        }
     }

    //DO bn-Filter Calculation
     for(ctr = 0; ctr <= N; ctr++)
           {

            for(ctrb = 0; ctrb < numberOfkrValues; ctrb++)
               {
                    bnval=bn(ctr, krm[ctrb], krs[ctrb], ac);
                    if(limiteronflag==1)
                    {
                        amplicalc=(2*a_max/M_PI)*abs(bnval)*atan(M_PI/(2*a_max*abs(bnval)));
                    }
                    else {amplicalc=1;}
                    OutputArray[ctr][ctrb]=complex<double>(amplicalc,0)/bnval;
               }
           }

     if(numberOfkrValues<32 && plc!=0)
      {mexPrintf("\nWARNING: Not enough kr values for PLC fading. PLC disabled.\n");
          plc=0;}

     //POWERLOSS COMPENSATION FILTER
     noplcflag=0;
     if(plc!=0)
     {
         xi= new complex<double>[numberOfkrValues]; //Array for xi-Term

         for(ctr=0; ctr<numberOfkrValues; ctr++)
         {
            xi[ctr]=0;
             for(ctrb=0; ctrb<=N; ctrb++)
             {
                 xi[ctr]+=complex<double>(2*ctrb+1,0)*(complex<double>(1,0)-OutputArray[ctrb][ctr]*bn(ctrb,krm[ctr],krs[ctr],ac));
             }
            xi[ctr]*=complex<double>(1,0)/bn(0,krm[ctr],krs[ctr],ac);
            xi[ctr]+=OutputArray[0][ctr];
         }
     }//plc!=0 flag

     if(plc==1) // -------------------- low kr only
     {

         //Find minimum distance
         mindistance=(double)abs(OutputArray[0][0]-xi[0]);
         locatemindistance=0;

         for(ctr=0; ctr<numberOfkrValues; ctr++)
         {
             if((double)abs(OutputArray[0][ctr]-xi[ctr]) < mindistance)
             {
                 mindistance=(double)abs(OutputArray[0][ctr]-xi[ctr]);
                 locatemindistance=ctr;
             }

         }

         filtergap=20*log10(1/abs(OutputArray[0][locatemindistance]/xi[locatemindistance]));
         mexPrintf("\nFilter fade gap: %4.2f dB",filtergap);

         if (filtergap>(double)20.0f || filtergap<(double)-20.0f)
         {mexPrintf("\nWARNING: Filtergap is too large. Nonsense filter expected.\nNo powerloss compensation filter applied.");
          noplcflag=1;}

         //Overwrite order 0 filter in output matrix
         if (noplcflag==0)
         {
             if (filtergap>(double)5.0f)
                {mexPrintf("\nWARNING: Filtergap is very large.");}

             if(fadeover==0) //Auto fadeover size
             {
                 fadeover=(long long int)(numberOfkrValues/100);
                 if (a_maxdB>0)
                 {
                     fadeover=fadeover/(long long int)ceil(a_max/4);
                 }

                if((fadeover>locatemindistance) || ((locatemindistance+fadeover)>numberOfkrValues))
                     {
                     if ((long long int)(locatemindistance-fadeover)<(long long int)(numberOfkrValues-(locatemindistance+fadeover)))
                      {
                         fadeover=locatemindistance;
                      }
                       else
                       {
                         fadeover=numberOfkrValues-locatemindistance;
                      }
                      }
                  mexPrintf("\nAuto filter size: %d Taps.", fadeover);
             }

             if((fadeover>locatemindistance) || ((locatemindistance+fadeover)>numberOfkrValues))
             {
               if ((long long int)(locatemindistance-fadeover)<(long long int)(numberOfkrValues-(locatemindistance+fadeover)))
                {
                    fadeover=locatemindistance;
                }
                 else
                 {
                    fadeover=numberOfkrValues-locatemindistance;
                }
                mexPrintf("\nWARNING: Filter fade size too high. Reduced to %d Taps.", fadeover);
             }

             mix=0;
             for(ctr=0; ctr<=locatemindistance-fadeover; ctr++)
             {
                OutputArray[0][ctr]=xi[ctr];
             }

             for(ctr=locatemindistance-fadeover+1; ctr<=locatemindistance+fadeover-1; ctr++)
                 {
                    mix+=1/(double)(2*fadeover);
                    OutputArray[0][ctr]=OutputArray[0][ctr]*complex<double>(mix,0)+xi[ctr]*complex<double>(1.0f-mix,0);
                }

             }//NoPLCFlagEnd
     }//PLC=1 low kr only

     if(plc==2) // full spectrum
     {
             for(ctr=0; ctr<numberOfkrValues; ctr++)
             {
                OutputArray[0][ctr]=xi[ctr];
             }
     }

     //Ouput kr-Response
     beamresponse= new complex<double>[numberOfkrValues]; //Array for kr-response-Term

     normalizebeam=((N+1)*(N+1));

     for(ctr=0; ctr<numberOfkrValues; ctr++)
     {
         beamresponse[ctr]=0;                            //Initialize
         for(ctrb=0; ctrb<=N; ctrb++)                   //ctrb=n;
         {
             for(ctrc=0; ctrc<=(2*ctrb); ctrc++)        //ctrc=m;
             {
                 beamresponse[ctr]=beamresponse[ctr]+bn(ctrb,krm[ctr],krs[ctr],ac)*OutputArray[ctrb][ctr];
             }
         }
         beamresponse[ctr]/=complex<double>((double)normalizebeam,0);
     }

     //Declare return ARG Matrix
     plhs[0] = mxCreateDoubleMatrix(N+1,numberOfkrValues,mxCOMPLEX);
     plhs[1] = mxCreateDoubleMatrix(1,numberOfkrValues,mxCOMPLEX);

     ReturnReal = mxGetPr(plhs[0]);
     ReturnImag = mxGetPi(plhs[0]);

     //Return ARG Matrix Fill
     ctrc=0;
     for(ctr=0;ctr<numberOfkrValues;ctr++)
        {
        for(ctrb=0;ctrb<=N;ctrb++)
             {
                ReturnReal[ctrc]=real(OutputArray[ctrb][ctr]);
                ReturnImag[ctrc]=imag(OutputArray[ctrb][ctr]);
                ctrc++;
              }
        }

    //Return Beamresponse
     ReturnRealB = mxGetPr(plhs[1]);
     ReturnImagB = mxGetPi(plhs[1]);

     for(ctr=0;ctr<numberOfkrValues;ctr++)
         {
                ReturnRealB[ctr]=real(beamresponse[ctr]);
                ReturnImagB[ctr]=imag(beamresponse[ctr]);
         }

      if(plc!=0)
      {delete [] xi;}
      delete [] beamresponse;
      for(ctr = 0; ctr < (N+1); ctr++) delete OutputArray[ctr];

}


#include <random>
#include <unordered_map>
#include <cstdio>

#include <boost/functional/hash.hpp>

#include "agms.hh"

using namespace std;

//---------------------------
// hash_family methods
//---------------------------

using namespace agms;

static mt19937_64 engine;

hash_family::hash_family(depth_type _D) : D(_D) {
    if (_D == 0) throw std::domain_error("0 depth in hash family");

    uniform_int_distribution<int64_t> U;

    for (size_t i = 0; i < 6; i++) {
        F[i] = new int64_t[_D];
        for (size_t d = 0; d < _D; d++)
            F[i][d] = U(engine);
    }

#if 0  // printout the random projection seeds
    for(depth_type d=0; d<D; d++) {
        printf("%4d:",d);
        for(size_t i=0;i<6; i++) {
            printf(" %12ld", F[i][d]);
        }
        printf("\n");
    }
#endif
}

hash_family::~hash_family() {
    for (size_t i = 0; i < 6; i++)
        delete[] F[i];
}

// A cache type for hash families, which is cleared at destruction
struct agms_hf_cache
        : unordered_map<depth_type, hash_family *> {
    ~agms_hf_cache() {
        for (auto x : *this)
            delete x.second;
    }
};

// The hash family cache variable
static agms_hf_cache cache;

hash_family *hash_family::get_cached(depth_type D) {
    if (cache.find(D) != cache.end())
        return cache[D];
    else {
        hash_family *ret = new hash_family(D);
        cache[D] = ret;
        return ret;
    }
}

//
// Return a 31-bit random hash
//
inline int64_t hash31(int64_t a, int64_t b, int64_t x) {
    // use 64-bit arithmetic
    int64_t result = (a * x) + b;
    return ((result >> 31) ^ result) & 2147483647ll;
}

size_t hash_family::hash(depth_type d, size_t x) const {
    assert(d < D);
    return hash31(F[0][d], F[1][d], x);
}

/// Return a 4-wise independent bit
bool hash_family::fourwise(depth_type d, size_t x) const {
    return
            hash31(hash31(hash31(x, F[2][d], F[3][d]), x, F[4][d]), x, F[5][d])
            & (1 << 15);
}

void projection::update_index(size_t key, Index &idx) const {
    assert(idx.size() == depth());
    size_t stride = 0;
    for (size_t d = 0; d < depth(); d++) {
        idx[d] = stride + hf->hash(d, key) % L;
        stride += width();
    }
}

void projection::update_mask(size_t key, Mask &mask) const {
    assert(mask.size() == depth());
    for (size_t d = 0; d < depth(); d++) {
        mask[d] = hf->fourwise(d, key);
    }
}

size_t std::hash<projection>::operator()(const projection &p) const {
    using boost::hash_value;
    using boost::hash_combine;

    size_t seed = 0;
    hash_combine(seed, hash_value(p.hashf()));
    hash_combine(seed, hash_value(p.width()));
    return seed;
}

template<typename T>
void print_vec(const string &name, const T &a) {
    size_t n = a.size();
    cout << name << "[" << n << "]={";
    for (size_t i = 0; i < n; i++)
        cout << (i ? "," : "") << a[i];
    cout << "}" << endl;
}

inc_sketch_updater::inc_sketch_updater(sketch &_sk)
        : sk(_sk),
          delta(_sk.proj.depth()),
          mask(_sk.proj.depth()) {}

void inc_sketch_updater::update(size_t key, double freq) {
    sk.proj.update_index(key, delta.index);
    sk.proj.update_mask(key, mask);

    delta.xold = sk[delta.index];
    for (size_t d = 0; d < mask.size(); d++) {
        if (mask[d])
            delta.xnew[d] = delta.xold[d] + freq;
        else
            delta.xnew[d] = delta.xold[d] - freq;
    }

    sk[delta.index] = delta.xnew;
}

isketch::isketch(const projection &_proj)
        : sketch(_proj),
          delta(proj.depth()),
          mask(proj.depth()) {}

void isketch::update(size_t key, double freq) {
    proj.update_index(key, delta.index);
    proj.update_mask(key, mask);

    delta.xold = (*this)[delta.index];
    for (size_t d = 0; d < mask.size(); d++) {
        if (mask[d])
            delta.xnew[d] = delta.xold[d] + freq;
        else
            delta.xnew[d] = delta.xold[d] - freq;
    }

    (*this)[delta.index] = delta.xnew;
}


#include <cuda.h>
#include <cuda_runtime_api.h>
#include <blitz.h>
#include "../../../include/kernels/sass_function.h"

using namespace blitz;

// K C R S
Shape filter_shape(4);
// K R S C
Shape filter_shuffle_shape(4);

void set_filter_shape_kcrs(size_t K, size_t C, size_t R, size_t S) {
  filter_shape[0] = K;
  filter_shape[1] = C;
  filter_shape[2] = R;
  filter_shape[3] = S;
}

void set_filter_shuffle_shape_krsc(size_t K, size_t R, size_t S, size_t C) {
  filter_shuffle_shape[0] = K;
  filter_shuffle_shape[1] = R;
  filter_shuffle_shape[2] = S;
  filter_shuffle_shape[3] = C;
}

// cpu shuffle
void cpu_shuffle(size_t K, size_t C, size_t R, size_t S,
  const float* input,
  float* output) {
  const size_t SC = S * C;
  const size_t RS = R * S;
  const size_t RSC = R * SC;
  for (size_t i = 0; i < K; ++i) {
    for (size_t j = 0; j < R; ++j) {
      for (size_t k = 0; k < S; ++k) {
        for (size_t v = 0; v < C; ++v) {
          output[i * RSC + j * SC + k * C + v] =
            input[i * RSC + v * RS + j * S + k];
        }
      }
    }
  }
}

void compare(float* output_cpu, float* output_gpu, size_t size) {
  for (size_t i = 0; i < size; ++i) {
    if (output_cpu[i] > output_gpu[i] + 1e-2 ||
      output_cpu[i] < output_gpu[i] - 1e-2) {
      std::cout << "Index: " << i << ", CPU: " << output_cpu[i] <<
        ", GPU: " << output_gpu[i] << std::endl;
    }
  }
}

void shuffle(size_t K, size_t C, size_t R, size_t S) {
  // set up cpu
  CPUTensor<float> filter_cpu(filter_shape);
  CPUTensor<float> filter_shuffle_cpu(filter_shuffle_shape);
  // set up gpu
  GPUTensor<float> filter_gpu(filter_shape);
  GPUTensor<float> filter_shuffle_gpu(filter_shuffle_shape);
  // set up copy
  CPUTensor<float> filter_copy(filter_shuffle_shape);
  // init values
  Backend<CPUTensor, float>::UniformDistributionFunc(&filter_cpu, 0.0, 1.0);
  cudaMemcpy(filter_gpu.data(), filter_cpu.data(),
    filter_cpu.size() * sizeof(float), cudaMemcpyHostToDevice);
  // cpu shuffle
  cpu_shuffle(K, C, R, S,
    filter_cpu.data(),
    filter_shuffle_cpu.data());
  // gpu shuffle
  kernels::Filter2DShuffle<float>(filter_gpu.data(), filter_shuffle_gpu.data(),
    K, C, R, S);
  // copy from gpu to cpu
  cudaMemcpy(filter_copy.data(), filter_shuffle_gpu.data(),
    filter_shuffle_gpu.size() * sizeof(float), cudaMemcpyDeviceToHost);
  compare(filter_shuffle_cpu.data(), filter_copy.data(), filter_shuffle_cpu.size());
}

int main(int argc, char** argv) {
  const size_t NUM_ARGS = 4;
  // K C R S
  if (argc != NUM_ARGS + 1) {
    std::cerr << "Not enough args!" << std::endl;
    exit(1);
  }
  // args
  const size_t K = atoi(argv[1]);
  const size_t C = atoi(argv[2]);
  const size_t R = atoi(argv[3]);
  const size_t S = atoi(argv[4]);
  // set shapes
  set_filter_shape_kcrs(K, C, R, S);
  set_filter_shuffle_shape_krsc(K, R, S, C);
  // run
  shuffle(K, C, R, S);
  return 0;
}


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

template<typename T>
inline size_t sampleDisc(const Eigen::Matrix<T,Eigen::Dynamic,1>& pdf,
    std::mt19937& rnd) {
  std::uniform_real_distribution<T> unif(0.,1.);
  T u = unif(rnd);
  T cdf = pdf[0];
  size_t k;
  for (k=1; k< (size_t)pdf.rows(); ++k) {
    if (u <= cdf) {
      return k-1;
    }
    cdf += pdf[k];
  }
  return pdf.rows()-1;
}


#include <algorithm>
#include <cmath>
#include <functional>
#include <memory>
#include <iostream>
#include <fstream>
#include <map>
#include <boost/algorithm/string/trim.hpp>
#include <string.h>
#include <vector>
#include "Eigen/Core"
#include <Eigen/Geometry>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

using namespace std;
template<class T> struct Point{
    template <typename NewType> Point<NewType> cast() const{
        return Point<NewType>();
    }
};
struct tt{
	int x;
	int y;
};
int main()
{
	Eigen::Quaterniond quat0(1, 2,0,0);
	std::cout<<"quat0: " <<quat0.w()<<" "<<quat0.x()<<" "<<quat0.y()<<" "<<quat0.z()<<"\n";
	Eigen::Matrix3d mat0 = quat0.toRotationMatrix();
	std::cout<<"mat0 (from quat0): \n" <<mat0 <<"\n";
	Eigen::Quaterniond quat1=quat0.conjugate();
	std::cout<<"quat0 conjugate: " <<quat1.w()<<" "<<quat1.x()<<" "<<quat1.y()<<" "<<quat1.z()<<"\n";
	std::cout<<"quat0: " <<quat0.w()<<" "<<quat0.x()<<" "<<quat0.y()<<" "<<quat0.z()<<"\n";
	quat0.normalize();
	std::cout<<"quat0: " <<quat0.w()<<" "<<quat0.x()<<" "<<quat0.y()<<" "<<quat0.z()<<"\n";
	Eigen::Matrix3d mat1 = quat0.toRotationMatrix();
	std::cout<<"mat1 (quat0 normalized): \n" <<mat1 <<"\n";
	Eigen::Quaterniond quat(1, 0,0,0);
	std::string ss0;
	std::ofstream outFile("/home/student//Documents/cartographer/test_ceres_pcd/sweep4.txt");
	std::ostringstream ssout("~/Documents/cartographer/test_ceres_pcd/sweep4.txt");
	ssout<<"hello"<<2;
	ssout.str("");//reset
	ssout<<"newhello"<<2;
	outFile<<"hello"<<3<<"hello";
	outFile<<ssout.str();
	outFile<<1<<","<<2;
	std::cout<<ssout.str();

    std::cout<<"Eigen::Quaterniond: "<< quat.w() <<"\n";
	Eigen::Matrix3d mat3 = Eigen::Quaterniond(1, 0,0,0).toRotationMatrix();
    Eigen::Vector3d v3d(-1., 0., 0.);
    Eigen::Transform<double, 3, Eigen::Affine > pose_transform;

    pose_transform.translation()=v3d;
    pose_transform.rotate(Eigen::AngleAxis<double>(0.5, Eigen::Vector3d::UnitX()));
    std::cout<<"Eigen::Transform: rotation"<< pose_transform.rotation() <<"\n";
    pose_transform.rotate(quat);
    pose_transform.linear()=mat3; //Eigen::AngleAxis<double>(0.5, Eigen::Vector3d::UnitX());
    //pose_transform.rotate() =mat3;
    //pose_transform.block<3,3>(0,0) =mat3;
    std::cout<<"Eigen::Matrix3f: "<< mat3 <<"\n";
    std::cout<<"Eigen::Vector3d: "<< v3d <<v3d[0]<<"\n";
    std::cout<<"Eigen::Transform: "<< pose_transform.translation() <<"\n";
    std::cout<<"Eigen::Transform: rotation"<< pose_transform.rotation() <<"\n";
    Eigen::Matrix<float, 3, 1> translation0(2,2,2);
    Eigen::Matrix<double, 3, 1> translation1(translation0.cast<double>());

    Point<float> p1;
     Point<double> p2;
    tt t1{1,2};
    //tt t2(1,2); bad structure initialization
    std::cout<<"struct tt: " <<t1.x<<"\n";
    std::vector<int> v = { 7, 5, 16, 8 };
    std::vector<int> intvec1;
    std::cout <<"intvec1.size(): "<< intvec1.size();
    intvec1.push_back(3);
    std::cout <<"\nintvec1.front(): "<< intvec1.front()<<"\n";

    //std::cout<<p1 <<" "<<p2 <<"\n";
    p2 = p1.cast<double>();
	std::array<int,3> a1{{1,2,3}};
	std::cout<<a1.data()[2];
	std::cout<<a1.at(2);

    std::cout<<"hello";
    std::string filename="testopt2.txt";

    std::cout<<NULL<<"----\n";
std::ifstream stream(filename.c_str());
//    std::ifstream stream("testopt2.txt");
    std::string myoptstr= std::string((std::istreambuf_iterator<char>(stream)),
                     std::istreambuf_iterator<char>());
    std::cout<< myoptstr;
map<string, string> mymap;
char *token;
token = strtok(const_cast<char*>(myoptstr.c_str()), "\n");
    while (token != NULL) {
        string s(token);
        size_t pos = s.find(":");
        //mymap[s.substr(0, pos)] = boost::algorithm::trim(s);
	string svalue = s.substr(pos + 1, string::npos);
	boost::algorithm::trim(svalue);
        mymap[s.substr(0, pos)] = svalue;
	//boost::algorithm::trim(s.substr(pos + 1, string::npos));
        token = strtok(NULL, "\n");

	if (s.substr(0, pos)=="tst"){
	std::cout<<"test tst"<<std::endl;
		char * token2;
		//std::cout<<svalue<<std::endl;
		token2 = strtok(const_cast<char*>(svalue.c_str()), ",");
		while (token2 != NULL) {
			string ss(token2);
			std::cout<<token2 <<" ";
        		token2 = strtok(NULL, ",");
		}

	}
    }

    std::cout<<"\n*****************\ncout mymap:\n";
    for (auto keyval : mymap)
        cout << keyval.first << ":" << keyval.second << endl;

      pcl::PointCloud<pcl::PointXYZ> cloud;

  // Fill in the cloud data
  cloud.width    = 5;
  cloud.height   = 1;
  cloud.is_dense = false;
  cloud.points.resize (cloud.width * cloud.height);

  for (auto& point: cloud)
  {
    point.x = 1024 * rand () / (RAND_MAX + 1.0f);
    point.y = 1024 * rand () / (RAND_MAX + 1.0f);
    point.z = 1024 * rand () / (RAND_MAX + 1.0f);
  }

  pcl::io::savePCDFileASCII ("test_pcd.pcd", cloud);

    return 0;
}


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/assert.hpp>
#include <boost/hana/ring.hpp>
#include <boost/hana/tuple.hpp>

#include <test/auto/base.hpp>
#include <test/auto/ring.hpp>
using namespace boost::hana;

namespace boost { namespace hana { namespace test {
    template <> auto objects<int> = make<Tuple>(0,1,2,3,4,5);
    template <> auto objects<long> = make<Tuple>(0l,1l,2l,3l,4l,5l);
}}}

int main() {
    test::laws<Ring, int>();
    test::laws<Ring, long>();

    using T = int;
    constexpr T x = 6, y = 4;

    // one
    {
        BOOST_HANA_CONSTEXPR_CHECK(one<T>() == static_cast<T>(1));
    }

    // mult
    {
        BOOST_HANA_CONSTEXPR_CHECK(mult(x, y) == x * y);
    }
}


/**
 * @file powerCalcTest.cpp
 * @author Luiz Victor Linhares Rocha (luizvictorlrocha@gmail.com)
 * @brief
 * @version 0.1
 * @date 2018-10-03
 *
 * @copyright Copyright (c) 2018
 *
 */

#define BOOST_TEST_DYN_LINK
#include "../libraries/Eigen/Eigen"
#include <boost/test/unit_test.hpp>
#include <boost/test/test_tools.hpp>
#include "../rede/barra.hpp"
#include "../rede/branch.hpp"
#include "../rede/powerNet.hpp"
#include "../rede/powerCalc.hpp"
#include "../rede/admitanceCalc.hpp"
#include "../util/complexutils.h"

namespace tt = boost::test_tools;

using neuralFlux::PowerNet;
using neuralFlux::PowerNetPtr;
using neuralFlux::complexo;

struct FfPower2 {
    FfPower2():net( new PowerNet() ) {
        net->addSlackBar(1, 1., 0.);
        net->addPQBar(2, 0.2, 0.);
        net->addPQBar(3, 0.2, 0.);
        net->addPQBar(4, 0.2, 0.);
        net->addPQBar(5, 0.2, 0.);

        net->connect(1, 2, 0., 0.1, 0., 0.98);
        net->connect(2, 3, 0., 0.1, 0., 1, 0.2);
        net->connect(2, 5, 0.1, 0.1);
        net->connect(3, 4, 0.1, 1.0);
        net->connect(4, 5, 0.1, 1.0);

        net->setFlatStart();
        net->updateY();

        answer<< 0.,  -0.196,
                 1.98669, 0.399334,
                 -1.98669,  0.199334,
                 0.,  -0.,
                 0., -0.;
    }

    PowerNetPtr net;
    Eigen::Matrix<double, 5, 2> answer;
};

BOOST_FIXTURE_TEST_CASE(testPower, FfPower2) {
    complexo valor;
    for (size_t i = 0; i < 5; i++) {
        BOOST_TEST(net->getSk(i).real() == answer(i, 0), tt::tolerance(0.0001));
        BOOST_TEST(net->getSk(i).imag() == answer(i, 1), tt::tolerance(0.0001) );
    }
}


/*
 * Frame3D.cpp
 *
 *  Created on: 15 Nov 2015
 *      Author: Morris Franken @ 3DUniversum
 */

#include <fstream>
#include <sstream>
#include <string>
#include <vector>
#include <iostream>
#include <stdexcept>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgcodecs/imgcodecs_c.h>

#include "Frame3D.h"
#include "FileUtils.h"

#if USE_EIGEN
#include <Eigen/Core>
#include <opencv2/core/eigen.hpp>
#endif

using namespace std;
using namespace cv;

string Frame3D::extension = ".3df";
vector<int> Frame3D::paramsJPG  = {CV_IMWRITE_JPEG_QUALITY, 90};
vector<int> Frame3D::paramsWEBP = {CV_IMWRITE_WEBP_QUALITY, 90};
vector<int> Frame3D::paramsPNG  = {CV_IMWRITE_PNG_COMPRESSION, 3};

Frame3D::~Frame3D() {}
Frame3D::Frame3D() : version_(-1), focal_length_(0) {}
Frame3D::Frame3D(string path) : version_(-1), focal_length_(0) {
	load(path);
}
Frame3D::Frame3D(Mat rgb_image, Mat depth_image, Mat rotation_translation, float focal_length, std::string device_name/*=""*/) :
	version_(-1),
	rgb_image_(rgb_image),
	depth_image_(depth_image),
	transformation_(rotation_translation),
	focal_length_(focal_length),
	device_name_(device_name)
{
	assert(rgb_image.type() == CV_8UC3);
	assert(depth_image.type() == CV_16U);
	assert(transformation_.size() == Size(4,4));
	syncTrans();
}
Frame3D::Frame3D(Mat rgb_image, Mat depth_image, Mat rotation, Mat translation, float focal_length, std::string device_name/*=""*/) :
	version_(-1),
	rgb_image_(rgb_image),
	depth_image_(depth_image),
	focal_length_(focal_length),
	device_name_(device_name)
{
	assert(rgb_image.type() == CV_8UC3);
	assert(depth_image.type() == CV_16U);
	assert(rotation.size() == Size(3,3));
	assert(translation.size() == Size(1,3));
	syncTrans(rotation, translation);
}

void Frame3D::syncTrans(cv::Mat &rotation, cv::Mat &translation) {
	transformation_ = Mat::zeros(4,4,CV_32F);
	rotation.copyTo(transformation_(Rect(0,0,3,3)));
	translation.copyTo(transformation_(Rect(3,0,1,3)));
	syncTrans();
}

void Frame3D::syncTrans() {
	rotation_ = transformation_(Rect(0,0,3,3));
	translation_ = transformation_(Rect(3,0,1,3));
}

void Frame3D::setTranslation(cv::Mat &translation) {
	assert(translation.size() == Size(1,3));
	translation.copyTo(transformation_(Rect(3,0,1,3)));
	translation_ = transformation_(Rect(3,0,1,3));
}

void Frame3D::setRotation(cv::Mat &rotation) {
	assert(rotation.size() == Size(3,3));
	rotation.copyTo(transformation_(Rect(0,0,3,3)));
	rotation_ = transformation_(Rect(0,0,3,3));
}

void Frame3D::setTranformation(cv::Mat &transformation) {
	assert(transformation_.size() == Size(4,4));
	transformation_ = transformation;
	syncTrans();
}

/* Get Eigen Matrix format, convienient single matrix transformation in Eigen format. Enable it if you have Eigen installed
 */

#if USE_EIGEN
Eigen::Matrix4f Frame3D::getEigenTransform() const {
	Eigen::Matrix4f eigen_matrix;
	cv2eigen(transformation_, eigen_matrix);
        eigen_matrix(3, 3) = 1.;
	return eigen_matrix;
}
#endif

/* Save 3D frame to file. 2 options are available:
 *   - version 0.5		: Frame is saved without any compression, a simple memory dump
 *   - version 1		: Frame is saved using JPG compression (90%) on the rgb_image, and PNG compression(type 3) on the depth_image.
 *   					  This compression comes at a cost, since it is about 5 times slower to load than simple memory dump
 *   - version 1.2		: In addition to version 1, in version 1.2 the rotation and translation matrices are stored in 1 4x4 matrix
 *   - version 1.3		: Device name is added
 */
void Frame3D::save(string path, double version/*=1.3*/) {
	if (version != 0.5 && version != 1 && version != 1.2 && version != 1.3)
		throw(runtime_error("Frame3D::save : invalid version number!, currently only version 0.5, 1, 1.2 and 1.3 are supported"));

	FileWriter writer(path);
	writer.appendDouble(version);
	if (version >= 1) {
		writer.appendImage(rgb_image_, ".jpg", paramsJPG);
		writer.appendImage(depth_image_, ".png", paramsPNG);
	} else {
		writer.appendMat(rgb_image_);
		writer.appendMat(depth_image_);
	}
	if (version >= 1.2) {
		writer.appendMat(transformation_);
	} else {
		Mat temp1 = rotation_.clone();
		Mat temp2 = translation_.clone();
		writer.appendMat(temp1);
		writer.appendMat(temp2);
	}
	writer.appendDouble(focal_length_);
	if (version >= 1.3)
		writer.appendString(device_name_);
}

/* Load .3df file, the variables have to be loaded in the same order as they have been saved
 * TODO(morris) : make something like a header to check whether or not this is actually a legit .3DF file
 */
void Frame3D::load(string path) {
	int index = path.find_last_of('.');
	if (index > 0 && path.substr(index) == Frame3D::extension) {
		FileReader reader(path);

		version_ = reader.readDouble();
		if (version_ >= 1) {
			rgb_image_ 	 	 = reader.readImage();
			depth_image_ 	 = reader.readImage();
		} else {
			rgb_image_ 	 	 = reader.readMat();
			depth_image_  	 = reader.readMat();
		}
		if (version_ >= 1.2) {
			transformation_  = reader.readMat();
			syncTrans();
		} else {
			rotation_ 	 	 = reader.readMat();
			translation_ 	 = reader.readMat();
			syncTrans(rotation_, translation_);
		}
		focal_length_ = reader.readDouble();
		if (version_ >= 1.3)
			device_name_	 = reader.readString();
	} else {
		throw(runtime_error("this is not a valid " + Frame3D::extension + " file: " + path));
	}
}

vector<Frame3D> Frame3D::loadFrames(string frame_dir_path) {
	vector<Frame3D> frames;
	vector<string> file_paths = ListFilesRecursive(frame_dir_path, extension);
	frames.reserve(file_paths.size());
	for (const string &path : file_paths) {
		frames.emplace_back(Frame3D(path));
	}

	return frames;
}


//          Copyright Carl Philipp Reh 2009 - 2016.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <fcppt/bit/mask.hpp>
#include <fcppt/bit/mask_c.hpp>
#include <fcppt/bit/shifted_mask.hpp>
#include <fcppt/bit/shifted_mask_c.hpp>
#include <fcppt/bit/test.hpp>
#include <fcppt/preprocessor/disable_gcc_warning.hpp>
#include <fcppt/preprocessor/pop_warning.hpp>
#include <fcppt/preprocessor/push_warning.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/test/unit_test.hpp>
#include <fcppt/config/external_end.hpp>

namespace
{

static_assert(
	fcppt::bit::shifted_mask_c<
		unsigned,
		10
	>().get()
	==
	1u
	<< 10,
	""
);

}

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(
	bit
)
{
FCPPT_PP_POP_WARNING

	BOOST_CHECK_EQUAL(
		fcppt::bit::shifted_mask<
			unsigned
		>(
			10
		).get(),
		1u
		<<
		10
	);

	auto const mask_c(
		fcppt::bit::mask_c<
			unsigned,
			0xFFu
		>()
	);

	BOOST_CHECK_EQUAL(
		mask_c.get(),
		0xFFu
	);

	BOOST_CHECK(
		!fcppt::bit::test(
			0xF0u,
			fcppt::bit::mask<
				unsigned
			>(
				0x0FU
			)
		)
	);

	BOOST_CHECK(
		fcppt::bit::test(
			0xF0u,
			fcppt::bit::mask<
				unsigned
			>(
				0x1FU
			)
		)
	);
}


// Copyright András Vukics 2006–2020. Distributed under the Boost Software License, Version 1.0. (See accompanying file LICENSE.txt)
#include "BlitzTinyExtensions.h"

#include <blitz/tinyvec2io.cc>

#define BOOST_TEST_MODULE BlitzTinyExtensions test
#include <boost/test/unit_test.hpp>

using namespace std;
using namespace blitzplusplus;

template <int I> using IntTiny=blitz::TinyVector<int,I>;

BOOST_AUTO_TEST_CASE( BlitzTinyExtensions )
{
  {
    IntTiny<3> v1{0,1,2};
    IntTiny<4> v2{3,4,5,6};

    BOOST_CHECK( blitz::all(concatenateTinies(v1,v2)==IntTiny<7>{0,1,2,3,4,5,6}) ) ;
    BOOST_CHECK( blitz::all(concatenateTinies(v2,v1)==IntTiny<7>{3,4,5,6,0,1,2}) ) ;
  }
  {
    IntTiny<6> v{3,4,2,3,4,2};

    BOOST_CHECK( blitz::all(halfCutTiny(v)==IntTiny<3>{3,4,2}) ) ;
  }
#ifndef   NDEBUG
  IntTiny<6> v{3,4,2,1,4,2};

  BOOST_CHECK_THROW( halfCutTiny(v), std::invalid_argument ) ;
#endif // NDEBUG

}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file  Moses3.cpp
 * @brief 3D Pose
 */

//#include <gtsam/3rdparty/Sophus/sophus/sim3.cpp>
//#include <gtsam/3rdparty/Sophus/sophus/so3.cpp>
//#include <gtsam/3rdparty/Sophus/sophus/scso3.cpp>

#include <gtsam/geometry/Moses3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/concepts.h>
#include <gtsam/base/Lie-inl.h>
#include <boost/foreach.hpp>
#include <iostream>
#include <cmath>

using namespace std;
using namespace Sophus;

namespace gtsam {

/** Explicit instantiation of base class to export members */
INSTANTIATE_LIE(Moses3);

/** instantiate concept checks */
GTSAM_CONCEPT_POSE_INST(Moses3);

static const Matrix3 I3 = eye(3), Z3 = zeros(3, 3), _I3 = -I3;
static const Matrix6 I6 = eye(6);
static const Matrix7 I7 = eye(7);

/* ************************************************************************* */
Moses3::Moses3(const Pose2& pose2) :
    Sim3(ScSO3(Rot3::rodriguez(0, 0, pose2.theta()).matrix()), (
        Point3(pose2.x(), pose2.y(), 0)).vector()) {
}

/* ************************************************************************* */
// Calculate Adjoint map
// Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi)
// Experimental - unit tests of derivatives based on it do not check out yet
Matrix7 Moses3::AdjointMap() const {
/*
  const Matrix3 R = R_.matrix();
  const Vector3 t = t_.vector();
  Matrix3 A = skewSymmetric(t) * R;
  Matrix6 adj;
  adj << R, Z3, A, R;
  return adj;
*/
  return this->Adj();
}

/* ************************************************************************* */
// Calculate Adjoint map
// From Sim3
Matrix7 Moses3::AdjointMap7() const {
  return this->Adj();
}

/* ************************************************************************* */
//Remove
Matrix7 Moses3::adjointMap(const Vector& xi) {
  /*
  Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2));
  Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5));
  Matrix6 adj;
  adj << w_hat, Z3, v_hat, w_hat;*/

  return adjointMap7(xi);
}

/* ************************************************************************* */
Matrix7 Moses3::adjointMap7(const Vector& xi) {
  /*
  Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2));
  Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5));
  Matrix6 adj;
  adj << w_hat, Z3, v_hat, w_hat;*/

  return d_lieBracketab_by_d_a(xi);
}

/* ************************************************************************* */
Vector Moses3::adjoint(const Vector& xi, const Vector& y,
    boost::optional<Matrix&> H) {
  if (H) {
    *H = zeros(7, 7);
    for (int i = 0; i < 7; ++i) {
      Vector dxi = zero(7);
      dxi(i) = 1.0;
      Matrix Gi = adjointMap7(dxi);
      (*H).col(i) = Gi * y;
    }
  }
  return adjointMap7(xi) * y; // Should it be = Sim3::lieBracket(xi, y) or Sim3::lieBracket(y, xi). [a,b] = d/dt(adj(a)*b).
}

/* ************************************************************************* */
Vector Moses3::adjointTranspose(const Vector& xi, const Vector& y,
    boost::optional<Matrix&> H) {
  if (H) {
    *H = zeros(7, 7);
    for (int i = 0; i < 7; ++i) {
      Vector dxi = zero(7);
      dxi(i) = 1.0;
      Matrix GTi = adjointMap7(dxi).transpose();
      (*H).col(i) = GTi * y;
    }
  }
  return adjointMap7(xi).transpose() * y;
}

/* *************************************************************************
Matrix6 Moses3::dExpInv_exp(const Vector& xi) {
  // Bernoulli numbers, from Wikipedia
  static const Vector B = (Vector(9) << 1.0, -1.0 / 2.0, 1. / 6., 0.0, -1.0 / 30.0,
      0.0, 1.0 / 42.0, 0.0, -1.0 / 30);
  static const int N = 5; // order of approximation
  Matrix res = I6;
  Matrix6 ad_i = I6;
  Matrix6 ad_xi = adjointMap(xi);
  double fac = 1.0;
  for (int i = 1; i < N; ++i) {
    ad_i = ad_xi * ad_i;
    fac = fac * i;
    res = res + B(i) / fac * ad_i;
  }
  return res;
}*/

/* ************************************************************************* */
void Moses3::print(const string& s) const {
  cout << s;
  cout << *this; //DOES *this WORK here ??? Defined in Sim3
  //R_.print("R:\n");
  //t_.print("t: ");
}

/* ************************************************************************* */
bool Moses3::equals(const Moses3& pose, double tol) const {
  return (Rot3(this->rotation_matrix())).equals(Rot3(pose.rotation_matrix()), tol) && (Point3(this->Sim3::translation())).equals(Point3(pose.Sim3::translation()), tol) && (fabs(this->scale() - pose.scale()) < tol) ;
}

/* ************************************************************************* */
/** Test test */
Moses3 Moses3::Expmap(const Vector& xi) {
	return Moses3(Sim3::exp(xi));
}

/* **************REMOVE*********************************************************** */
Vector7 Moses3::Logmap(const Moses3& p) {
	Vector7 vec;
	return  Sim3::log(Sim3(p.scso3(), p.Sim3::translation()));
}

/* btak remove************************************************************************* */
Vector7  Moses3::Logmap7(const Moses3& p) {
	Vector7 vec;
	return  Sim3::log(Sim3(p.scso3(), p.Sim3::translation()));
}

/* *************************************************************************
Moses3 Moses3::retractFirstOrder(const Vector& xi) const {
  Vector3 omega(sub(xi, 0, 3));
  Point3 v(sub(xi, 3, 6));
  Rot3 R = R_.retract(omega); // R is done exactly
  Point3 t = t_ + R_ * v; // First order t approximation
  return Moses3(R, t);
}*/

/* ************************************************************************* */
// Different versions of retract
Moses3 Moses3::retract(const Vector& xi, Moses3::CoordinatesMode mode) const {
  if (mode == Moses3::EXPMAP) {
    // Lie group exponential map, traces out geodesic
    return compose(Expmap(xi));

  } /*else if (mode == Moses3::FIRST_ORDER) {
    // First order
    return retractFirstOrder(xi);

  }*/
  else {
    // Point3 t = t_.retract(v.vector()); // Incorrect version retracts t independently
    // Point3 t = t_ + R_ * (v+Point3(omega).cross(v)/2); // Second order t approximation
    assert(false);
    exit(1);
  }
}

/* ************************************************************************* */
// different versions of localCoordinates
Vector7 Moses3::localCoordinates(const Moses3& T,
    Moses3::CoordinatesMode mode) const {
  if (mode == Moses3::EXPMAP) {
    // Lie group logarithm map, exact inverse of exponential map
    return Logmap7(between(T));
  } else {
    assert(false);
    exit(1);
  }
}

/* ************************************************************************* */
Matrix4 Moses3::matrix() const {
  return this->Sim3::matrix();
}

/* ************************************************************************* */
// Check scale man
Moses3 Moses3::transform_to(const Moses3& pose) const {
  //Rot3 cRv = R_ * Rot3(pose.R_.inverse());// Scale should not be lost here!! Using scso3.

	const Point3 Tt(this->Sim3::translation());

  ScSO3 cRv = (scso3_) * pose.scso3().inverse();// Scale should not be lost here!! Using scso3. So this has scale included.
  Point3 t = pose.transform_to(Tt); // But this has scale included too. WTF.
  return Moses3(cRv, t);
}

/* ************************************************************************* */
Point3 Moses3::transform_from(const Point3& p, boost::optional<Matrix&> Dpose,
    boost::optional<Matrix&> Dpoint) const {

	const Rot3 Rr(this->Sim3::rotation_matrix());
	const double Ss(this->Sim3::scale());
	const Point3 Tt(this->Sim3::translation());

  if (Dpose) {
    const Matrix R = Rr.matrix();
    Matrix DR = R * skewSymmetric(-p.x(), -p.y(), -p.z()); //Should it have scso3 or just so3 ? It should have scso3 coz y = sRx + st. dy/dx = sR
    Dpose->resize(3, 7);
    Vector3 scaleCol=(Ss*(R*p.vector()));//TODO This needs checking. Formula obtained by matching with numerical derivative values. Make sure it theritically makes sense.
    //(scaleCol) << 0.2, 1.25977596, -1.70380883;//Bullshit hack REMOVE
    (*Dpose) <<  Ss*R, (Ss)*DR, scaleCol; //TODO This needs checking. Mostly incorrect. //should the scale component of jacobi be zero ? or 0 0 1 ?
  }
  if (Dpoint)
    *Dpoint = Ss*Rr.matrix(); //TODO This needs checking. Mostly incorrect.
  return Point3(static_cast<const Sim3&>(*this)*p.vector()); //Using Sim3 group action. //TODO This needs checking. Mostly correct.
}

/* ************************************************************************* */
// check scale man
// unrotate(p - t_)*s_ : P_world of point. t_ translation of new frame wrt world cordi in world coordi. Multiply (or divide?) with scale (axis strech).
Point3 Moses3::transform_to(const Point3& p, boost::optional<Matrix&> Dpose,
    boost::optional<Matrix&> Dpoint) const {

	const Rot3 Rr(this->Sim3::rotation_matrix());
	const double Ss(this->Sim3::scale());
	const Point3 Tt(this->Sim3::translation());

  const Point3 result = Rr.unrotate((p - Tt)/Ss); // should it be (p - 1/s_*t_) or 1/s_*(p - t_). Multiply or divide by scale. ?
  												// We are multiplying in transform_from (t_ + s*Rot(P)), so here we divide. //TODO This needs checking. Mostly correct.
  if (Dpose) {
    const Point3& q = result;
    Matrix DR = skewSymmetric(q.x(), q.y(), q.z());
    Dpose->resize(3, 7);
    Vector3 scaleCol=(-1)*q.vector();//TODO This needs checking. Mostly incorrect.
    (*Dpose) << _I3, DR, scaleCol; //TODO This needs checking. Mostly incorrect.
  }
  if (Dpoint)
    *Dpoint = (1/Ss)*Rr.transpose();  //TODO This needs checking. Mostly incorrect.
  return result;
}

/* ************************************************************************* */
Moses3 Moses3::compose(const Moses3& p2, boost::optional<Matrix&> H1,
    boost::optional<Matrix&> H2) const {
  if (H1)
    *H1 = p2.inverse().AdjointMap();
  if (H2)
    *H2 = I7;
  return (*this) * p2;
}

/* *************************************************************************
 // TODO: Test
  //	   Implement Derivatives
  */
Moses3 Moses3::inverse(boost::optional<Matrix&> H1) const {
  if (H1)
    *H1 = -AdjointMap(); // TODO Replace with correct function here
  Rot3 Rt = R_.inverse();
  Moses3 inv(this->Sim3::inverse());
  return inv;
}

/* ************************************************************************* */
// between = compose(p2,inverse(p1));
Moses3 Moses3::between(const Moses3& p2, boost::optional<Matrix&> H1,
    boost::optional<Matrix&> H2) const {
  Moses3 result = inverse() * p2;
  if (H1)
    *H1 = -result.inverse().AdjointMap();
  if (H2)
    *H2 = I7;
  return result;
}

/* ************************************************************************* */
double Moses3::range(const Point3& point, boost::optional<Matrix&> H1,
    boost::optional<Matrix&> H2) const {
  if (!H1 && !H2)
    return transform_to(point).norm();
  Point3 d = transform_to(point, H1, H2);
  double x = d.x(), y = d.y(), z = d.z(), d2 = x * x + y * y + z * z, n = sqrt(
      d2);
  Matrix D_result_d = (Matrix(1, 3) << x / n, y / n, z / n);
  if (H1)
    *H1 = D_result_d * (*H1);
  if (H2)
    *H2 = D_result_d * (*H2);
  return n;
}

/* ************************************************************************* */
// This needs proper testing
double Moses3::range(const Moses3& point, boost::optional<Matrix&> H1,
    boost::optional<Matrix&> H2) const {
  double r = range(Point3(point.Sim3::translation()), H1, H2); //IMPORTANT!!! Should it be just translation or scaled translation ?? Scaled translation spoils derivatives.
  if (H2) {
    Matrix H2_ = *H2 * point.scso3().matrix(); //Checked via numerical derivatives
    *H2 = zeros(1, 7);
    insertSub(*H2, H2_, 0, 0);
  }
  return r;
}

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

/*
boost::optional<Moses3> align(const vector<Point3Pair>& pairs) {
  const size_t n = pairs.size();
  if (n < 3)
    return boost::none; // we need at least three pairs

  // calculate centroids
  Vector cp = zero(3), cq = zero(3);
  BOOST_FOREACH(const Point3Pair& pair, pairs){
  cp += pair.first.vector();
  cq += pair.second.vector();
}
  double f = 1.0 / n;
  cp *= f;
  cq *= f;

  // Add to form H matrix
  Matrix H = zeros(3, 3);
  BOOST_FOREACH(const Point3Pair& pair, pairs){
  Vector dp = pair.first.vector() - cp;
  Vector dq = pair.second.vector() - cq;
  H += dp * dq.transpose();
}

// Compute SVD
  Matrix U, V;
  Vector S;
  svd(H, U, S, V);

  // Recover transform with correction from Eggert97machinevisionandapplications
  Matrix UVtranspose = U * V.transpose();
  Matrix detWeighting = eye(3, 3);
  detWeighting(2, 2) = UVtranspose.determinant();
  Rot3 R(Matrix(V * detWeighting * U.transpose()));
  Point3 t = Point3(cq) - R * Point3(cp);
  return Moses3(R, t);
}

*/
/* ************************************************************************* */
std::ostream &operator<<(std::ostream &os, const Moses3& pose) {
  os << pose.Sim3::scso3() << "\n" << Point3(pose.Sim3::translation()) << endl;
  return os;
}

} // namespace gtsam


//==============================================================================
//         Copyright 2015          J.T. Lapreste
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <boost/simd/arithmetic/include/functions/round.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/mone.hpp>
#include <boost/simd/include/constants/inf.hpp>
#include <boost/simd/include/constants/minf.hpp>
#include <boost/simd/include/constants/nan.hpp>
#include <boost/simd/include/constants/valmax.hpp>
#include <boost/simd/include/constants/valmin.hpp>
#include <boost/simd/include/constants/maxflint.hpp>
#include <boost/simd/include/constants/half.hpp>
#include <boost/simd/sdk/config.hpp>
#include <boost/simd/include/functions/prev.hpp>
#include <boost/simd/include/functions/next.hpp>

NT2_TEST_CASE_TPL ( round_real,  BOOST_SIMD_REAL_TYPES)
{

  using boost::simd::round;
  using boost::simd::tag::round_;
  using boost::simd::next;
  using boost::simd::prev;
  typedef typename boost::dispatch::meta::call<round_(T)>::type r_t;
  typedef T wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS( r_t, wished_r_t );

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_EQUAL(round(boost::simd::Inf<T>()), boost::simd::Inf<r_t>());
  NT2_TEST_EQUAL(round(boost::simd::Minf<T>()), boost::simd::Minf<r_t>());
  NT2_TEST_EQUAL(round(boost::simd::Nan<T>()), boost::simd::Nan<r_t>());
#endif
  NT2_TEST_EQUAL(round(boost::simd::Mhalf<T>()), boost::simd::Mone<r_t>());
  NT2_TEST_EQUAL(round(boost::simd::Mone<T>()), boost::simd::Mone<r_t>());
  NT2_TEST_EQUAL(round(boost::simd::One<T>()), boost::simd::One<r_t>());
  NT2_TEST_EQUAL(round(boost::simd::Zero<T>()), boost::simd::Zero<r_t>());
  NT2_TEST_EQUAL(round(boost::simd::Maxflint<T>()-boost::simd::Half<T>()),boost::simd::Maxflint<T>());
  NT2_TEST_EQUAL(round(boost::simd::Maxflint<T>()),boost::simd::Maxflint<T>());
  NT2_TEST_EQUAL(round(T(1.4)), T(1));
  NT2_TEST_EQUAL(round(T(1.5)), T(2));
  NT2_TEST_EQUAL(round(T(1.6)), T(2));
  NT2_TEST_EQUAL(round(T(2.5)), T(3));
  NT2_TEST_EQUAL(round(T(-1.4)), T(-1));
  NT2_TEST_EQUAL(round(T(-1.5)), T(-2));
  NT2_TEST_EQUAL(round(T(-1.6)), T(-2));
  NT2_TEST_EQUAL(round(T(-2.5)), T(-3));
  NT2_TEST_EQUAL(round(boost::simd::Half<T>()), boost::simd::One<r_t>());
  NT2_TEST_EQUAL(round(prev(prev(boost::simd::Half<T>()))),  boost::simd::Zero<T>());
  NT2_TEST_EQUAL(round(prev(boost::simd::Half<T>())),  boost::simd::Zero<T>());
  NT2_TEST_EQUAL(round(     boost::simd::Half<T>()) ,  boost::simd::One <T>());
  NT2_TEST_EQUAL(round(next(boost::simd::Half<T>())),  boost::simd::One <T>());
  T z = boost::simd::Maxflint < T>()/T(2)+T(1);
  NT2_TEST_EQUAL(round(z), z);

} // end of test for floating_

NT2_TEST_CASE_TPL ( round_real2,  BOOST_SIMD_REAL_TYPES)
{

  using boost::simd::round;
  using boost::simd::tag::round_;
  using boost::simd::next;
  using boost::simd::prev;
  typedef typename boost::dispatch::meta::call<round_(T)>::type r_t;
  typedef T wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS( r_t, wished_r_t );

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_EQUAL(round(boost::simd::Inf<T>(), 2), boost::simd::Inf<r_t>());
  NT2_TEST_EQUAL(round(boost::simd::Minf<T>(), 2), boost::simd::Minf<r_t>());
  NT2_TEST_EQUAL(round(boost::simd::Nan<T>(), 2), boost::simd::Nan<r_t>());
#endif
  NT2_TEST_EQUAL(round(boost::simd::Mhalf<T>(), 2), boost::simd::Mhalf<r_t>());
  NT2_TEST_EQUAL(round(boost::simd::Mone<T>(), 2), boost::simd::Mone<r_t>());
  NT2_TEST_EQUAL(round(boost::simd::One<T>(), 2), boost::simd::One<r_t>());
  NT2_TEST_EQUAL(round(boost::simd::Zero<T>(), 2), boost::simd::Zero<r_t>());
  NT2_TEST_EQUAL(round(boost::simd::Maxflint<T>()-boost::simd::Half<T>(), 2),boost::simd::Maxflint<T>());
  NT2_TEST_EQUAL(round(boost::simd::Maxflint<T>(), 2),boost::simd::Maxflint<T>());
  NT2_TEST_EQUAL(round(T(1.44), 1), T(1.4));
  NT2_TEST_EQUAL(round(T(1.45), 1), T(1.5));
  NT2_TEST_EQUAL(round(T(1.46), 1), T(1.5));
  NT2_TEST_EQUAL(round(T(2.45), 1), T(2.5));
  NT2_TEST_EQUAL(round(T(-1.44), 1), T(-1.4));
  NT2_TEST_EQUAL(round(T(-1.45), 1), T(-1.5));
  NT2_TEST_EQUAL(round(T(-1.46), 1), T(-1.5));
  NT2_TEST_EQUAL(round(T(-2.45), 1), T(-2.5));
  NT2_TEST_ULP_EQUAL(round(T(145), -2), T(100), 0.5);
  NT2_TEST_ULP_EQUAL(round(T(150), -2), T(200), 0.5);
  NT2_TEST_ULP_EQUAL(round(T(156), -2), T(200), 0.5);
  NT2_TEST_ULP_EQUAL(round(T(250), -2), T(300), 0.5);
  NT2_TEST_ULP_EQUAL(round(T(-145), -2), T(-100), 0.5);
  NT2_TEST_ULP_EQUAL(round(T(-155), -2), T(-200), 0.5);
  NT2_TEST_ULP_EQUAL(round(T(-156), -2), T(-200), 0.5);
  NT2_TEST_ULP_EQUAL(round(T(-255), -2), T(-300), 0.5);
} // end of test for floating_

NT2_TEST_CASE_TPL ( round_unsigned_int__1_0,  BOOST_SIMD_UNSIGNED_TYPES)
{

  using boost::simd::round;
  using boost::simd::tag::round_;
  typedef typename boost::dispatch::meta::call<round_(T)>::type r_t;
  typedef T wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS( r_t, wished_r_t );

  // specific values tests
  NT2_TEST_EQUAL(round(boost::simd::One<T>()), boost::simd::One<r_t>());
  NT2_TEST_EQUAL(round(boost::simd::Zero<T>()), boost::simd::Zero<r_t>());
} // end of test for unsigned_int_

NT2_TEST_CASE_TPL ( round_signed_int__1_0,  BOOST_SIMD_INTEGRAL_SIGNED_TYPES)
{

  using boost::simd::round;
  using boost::simd::tag::round_;
  typedef typename boost::dispatch::meta::call<round_(T)>::type r_t;
  typedef T wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS( r_t, wished_r_t );

  // specific values tests
  NT2_TEST_EQUAL(round(boost::simd::Mone<T>()), boost::simd::Mone<r_t>());
  NT2_TEST_EQUAL(round(boost::simd::One<T>()), boost::simd::One<r_t>());
  NT2_TEST_EQUAL(round(boost::simd::Zero<T>()), boost::simd::Zero<T>());
} // end of test for signed_int_


#ifndef STAN_MATH_PRIM_MAT_FUN_MDIVIDE_LEFT_TRI_HPP
#define STAN_MATH_PRIM_MAT_FUN_MDIVIDE_LEFT_TRI_HPP

#include <boost/math/tools/promotion.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/mat/fun/promote_common.hpp>
#include <stan/math/prim/mat/err/check_multiplicable.hpp>
#include <stan/math/prim/mat/err/check_square.hpp>

namespace stan {
  namespace math {

    /**
     * Returns the solution of the system Ax=b when A is triangular
     * @param A Triangular matrix.  Specify upper or lower with TriView
     * being Eigen::Upper or Eigen::Lower.
     * @param b Right hand side matrix or vector.
     * @return x = A^-1 b, solution of the linear system.
     * @throws std::domain_error if A is not square or the rows of b don't
     * match the size of A.
     */
    template <int TriView, typename T1, typename T2,
              int R1, int C1, int R2, int C2>
    inline
    Eigen::Matrix<typename boost::math::tools::promote_args<T1, T2>::type,
                  R1, C2>
    mdivide_left_tri(const Eigen::Matrix<T1, R1, C1> &A,
                     const Eigen::Matrix<T2, R2, C2> &b) {
      check_square("mdivide_left_tri", "A", A);
      check_multiplicable("mdivide_left_tri", "A", A, "b", b);
      return promote_common<Eigen::Matrix<T1, R1, C1>,
                            Eigen::Matrix<T2, R1, C1> >(A)
        .template triangularView<TriView>()
        .solve(promote_common<Eigen::Matrix<T1, R2, C2>,
               Eigen::Matrix<T2, R2, C2> >(b));
    }

    /**
     * Returns the solution of the system Ax=b when A is triangular and b=I.
     * @param A Triangular matrix.  Specify upper or lower with TriView
     * being Eigen::Upper or Eigen::Lower.
     * @return x = A^-1 .
     * @throws std::domain_error if A is not square
     */
    template<int TriView, typename T, int R1, int C1>
    inline
    Eigen::Matrix<T, R1, C1>
    mdivide_left_tri(const Eigen::Matrix<T, R1, C1> &A) {
      check_square("mdivide_left_tri", "A", A);
      int n = A.rows();
      Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> b;
      b.setIdentity(n, n);
      A.template triangularView<TriView>().solveInPlace(b);
      return b;
    }

  }
}
#endif


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

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_LDEXP_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_LDEXP_HPP_INCLUDED
#include <boost/simd/function/pedantic.hpp>
#include <boost/simd/function/std.hpp>

#ifndef BOOST_SIMD_NO_DENORMALS
#include <boost/simd/detail/constant/minexponent.hpp>
#include <boost/simd/constant/smallestposval.hpp>
#endif
#include <boost/simd/detail/constant/limitexponent.hpp>
#include <boost/simd/detail/constant/maxexponent.hpp>
#include <boost/simd/constant/nbmantissabits.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/function/is_flint.hpp>
#include <boost/simd/function/shift_left.hpp>
#include <boost/simd/function/toint.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>
#include <cmath>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  // a0 a1 integers
  BOOST_DISPATCH_OVERLOAD ( ldexp_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::scalar_< bd::integer_<A0> >
                          , bd::scalar_< bd::integer_<A1> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A1 a1) const BOOST_NOEXCEPT
    {
      return (a1>0)?(a0<<a1):(a0>>a1);
    }
  };

  // a0 floating
  BOOST_DISPATCH_OVERLOAD ( ldexp_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bs::pedantic_tag
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::integer_<A1> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const pedantic_tag &
                                    ,  A0 a0, A1 a1) const BOOST_NOEXCEPT
    {
      using i_t = bd::as_integer_t<A0>;
      i_t e =  a1;
      A0 f = One<A0>();
    #ifndef BOOST_SIMD_NO_DENORMALS
      if (BOOST_UNLIKELY((e < Minexponent<A0>())))
      {
        e -= Minexponent<A0>();
        f = Smallestposval<A0>();
      }
    #endif
      i_t b = (e == Limitexponent<A0>());
      f += A0(b);
      e -= b;
      e += Maxexponent<A0>();
      e = shift_left(e, Nbmantissabits<A0>());
      return a0*bitwise_cast<A0>(e)*f;
    }
  };
  BOOST_DISPATCH_OVERLOAD ( ldexp_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bs::pedantic_tag
                          , bd::scalar_< bd::integer_<A0> >
                          , bd::scalar_< bd::integer_<A1> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const pedantic_tag &
                                    , A0 a0, A1 a1) const BOOST_NOEXCEPT
    {
      return bs::ldexp(a0, a1);
    }
  };

  // a0 floating
  BOOST_DISPATCH_OVERLOAD ( ldexp_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::integer_<A1> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A1 a1) const BOOST_NOEXCEPT
    {
      using i_t = bd::as_integer_t<A0>;
      i_t ik =  a1+Maxexponent<A0>();
      ik = shift_left(ik, Nbmantissabits<A0>());
      return a0*bitwise_cast<A0>(ik);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( ldexp_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , boost::simd::std_tag
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::integer_<A1> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const std_tag &
                                    ,  A0 a0, A1 a1 ) const BOOST_NOEXCEPT
    {
      return std::ldexp(a0, a1);
    }
  };
  BOOST_DISPATCH_OVERLOAD ( ldexp_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , boost::simd::std_tag
                          , bd::scalar_< bd::integer_<A0> >
                          , bd::scalar_< bd::integer_<A1> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const std_tag &
                                    ,  A0 a0, A1 a1 ) const BOOST_NOEXCEPT
    {
      return bs::ldexp(a0, a1);
    }
  };
  BOOST_DISPATCH_OVERLOAD ( ldexp_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , boost::simd::std_tag
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::floating_<A1> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const std_tag &
                                    ,  A0 a0, A1 a1 ) const BOOST_NOEXCEPT
    {
      BOOST_ASSERT_MSG(is_flint(a1), "parameter is not a flint");
      return std::ldexp(a0, toint(a1));
    }
  };
} } }

#endif


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

#ifndef BOOST_MATH_CCMATH_ISFINITE
#define BOOST_MATH_CCMATH_ISFINITE

#include <cmath>
#include <type_traits>
#include <boost/math/tools/is_constant_evaluated.hpp>
#include <boost/math/ccmath/isinf.hpp>
#include <boost/math/ccmath/isnan.hpp>

namespace boost::math::ccmath {

template <typename T>
inline constexpr bool isfinite(T x)
{
    if(BOOST_MATH_IS_CONSTANT_EVALUATED(x))
    {
        // bool isfinite (IntegralType arg) is a set of overloads accepting the arg argument of any integral type
        // equivalent to casting the integral argument arg to double (e.g. static_cast<double>(arg))
        if constexpr (std::is_integral_v<T>)
        {
            return !boost::math::ccmath::isinf(static_cast<double>(x)) && !boost::math::ccmath::isnan(static_cast<double>(x));
        }
        else
        {
            return !boost::math::ccmath::isinf(x) && !boost::math::ccmath::isnan(x);
        }
    }
    else
    {
        using std::isfinite;

        if constexpr (!std::is_integral_v<T>)
        {
            return isfinite(x);
        }
        else
        {
            return isfinite(static_cast<double>(x));
        }
    }
}

}

#endif // BOOST_MATH_CCMATH_ISFINITE


//==================================================================================================
/*!
  @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_ARCH_COMMON_SCALAR_FUNCTION_LOG1P_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_LOG1P_HPP_INCLUDED
#include <boost/simd/function/std.hpp>
#include <boost/simd/function/plain.hpp>

#include <boost/simd/function/scalar/inc.hpp>
#include <boost/simd/function/musl.hpp>
#include <boost/simd/function/scalar/oneminus.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>
#include <cmath>
#include <boost/simd/function/bitwise_and.hpp>
#include <boost/simd/function/dec.hpp>
#include <boost/simd/function/scalar/inc.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/frexp.hpp>
#include <boost/simd/function/horn.hpp>
#include <boost/simd/function/musl.hpp>
#include <boost/simd/function/plain.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/function/std.hpp>
#include <boost/simd/detail/constant/log_2hi.hpp>
#include <boost/simd/detail/constant/log_2lo.hpp>

#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( log1p_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0) const BOOST_NOEXCEPT
    {
      return musl_(log1p)(a0);
    }
  };
  BOOST_DISPATCH_OVERLOAD ( log1p_
                          , (typename A0)
                          , bd::cpu_
                          , bs::std_tag
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const std_tag &, A0 a0) const BOOST_NOEXCEPT
    {
      return std::log1p(a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( log1p_
                          , (typename A0)
                          , bd::cpu_
                          , bs::musl_tag
                          , bd::scalar_< bd::single_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const musl_tag &, A0 x) const BOOST_NOEXCEPT
    {
      using uiA0 = bd::as_integer_t<A0, unsigned>;
      using iA0 = bd::as_integer_t<A0,   signed>;
      uiA0 ix = bitwise_cast<uiA0>(x);
      iA0 k = 1;
      A0 c = Zero<A0>(), f = x;
      if (ix < 0x3ed413d0 || ix>>31)               /* 1+x < sqrt(2)+  */
      {
        if (ix >= 0xbf800000)                       /* x <= -1.0 */
        {
          if (x == Mone<A0>())  return Minf<A0>();  /* log1p(-1)=-inf */
          return Nan<A0>();                         /* log1p(x<-1)=NaN */
        }
        if (ix<<1 < 0x33800000<<1)                  /* |x| < 2**-24 */
        {
          if ((ix&0x7f800000) == 0) return x;
        }
        if (ix <= 0xbe95f619)                       /* sqrt(2)/2- <= 1+x < sqrt(2)+ */
        {
          k = 0;
        }
      }
      else if (ix >= 0x7f800000)  return x;
      if (k)
      {
        /* reduce u into [sqrt(2)/2, sqrt(2)] */
        A0 uf =  inc(x);
        uiA0 iu = bitwise_cast<uiA0>(uf);
        iu += 0x3f800000 - 0x3f3504f3;
        k = bitwise_cast<iA0>(iu>>23) - 0x7f;
        /* correction term ~ log(1+x)-log(u), avoid underflow in c/u */
        if (k < 25)
        {
          c = k >= 2 ? oneminus(uf-x) : x-dec(uf);
          c /= uf;
        }

        /* reduce u into [sqrt(2)/2, sqrt(2)] */
        iu = (iu&0x007fffff) + 0x3f3504f3;
        f =  dec(bitwise_cast<A0>(iu));
      }
      A0 s = f/(2.0f + f);
      A0 z = sqr(s);
      A0 w = sqr(z);
      A0 t1= w*horn<A0, 0x3eccce13, 0x3e789e26>(w);
      A0 t2= z*horn<A0, 0x3f2aaaaa, 0x3e91e9ee>(w);
      A0 R = t2 + t1;
      A0 hfsq = 0.5f*sqr(f);
      A0 dk = k;
      return  fma(dk, Log_2hi<A0>(), ((fma(s, (hfsq+R), dk*Log_2lo<A0>()+c) - hfsq) + f));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( log1p_
                          , (typename A0)
                          , bd::cpu_
                          , bs::musl_tag
                          , bd::scalar_< bd::double_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const musl_tag &, A0 x) const BOOST_NOEXCEPT
    {
      using uiA0 = bd::as_integer_t<A0, unsigned>;
      using iA0 = bd::as_integer_t<A0,   signed>;
      uiA0 hx = bitwise_cast<uiA0>(x) >> 32;
      iA0 k = 1;

      A0 c = Zero<A0>(), f = x;
      if (hx < 0x3fda827a || hx>>31)               /* 1+x < sqrt(2)+ */
      {
        if (hx >= 0xbff00000)                      /* x <= -1.0 */
        {
          if (x == Mone<A0>()) return Minf<A0>();  /* log1p(-1)=-inf */
          return Nan<A0>();                        /* log1p(x<-1)=NaN */
        }
        if (hx<<1 < 0x3ca00000<<1)                 /* |x| < 2**-53 */
        {
          if ((hx&0x7ff00000) == 0) return x;
        }
        if (hx <= 0xbfd2bec4)                      /* sqrt(2)/2- <= 1+x < sqrt(2)+ */
        {
          k = 0;
        }
      } else if (hx >= 0x7ff00000) return x;
      if (k)
      {
        /* reduce x into [sqrt(2)/2, sqrt(2)] */
        A0 uf =  inc(x);
        uiA0 hu = bitwise_cast<uiA0>(uf)>>32;
        hu += 0x3ff00000 - 0x3fe6a09e;
        k = (int)(hu>>20) - 0x3ff;
        /* correction term ~ log(1+x)-log(u), avoid underflow in c/u */
        if (k < 54)
        {
          c = k >= 2 ? oneminus(uf-x) : x-dec(uf);
          c /= uf;
        }
        hu =  (hu&0x000fffff) + 0x3fe6a09e;
        f = bitwise_cast<A0>( bitwise_cast<uiA0>(hu<<32) | (bitwise_and(0xffffffffull, bitwise_cast<uiA0>(uf))));
        f = dec(f);
      }

      A0 hfsq = 0.5*sqr(f);
      A0 s = f/(2.0f + f);
      A0 z = sqr(s);
      A0 w = sqr(z);
      A0 t1= w*horn<A0, 0x3fd999999997fa04ll, 0x3fcc71c51d8e78afll, 0x3fc39a09d078c69fll > (w);
      A0 t2= z*horn<A0, 0x3fe5555555555593ll, 0x3fd2492494229359ll
                      , 0x3fc7466496cb03dell, 0x3fc2f112df3e5244ll> (w);
      A0 R = t2 + t1;
      A0 dk = k;
      return  fma(dk, Log_2hi<A0>(), ((fma(s, (hfsq+R), dk*Log_2lo<A0>()+c) - hfsq) + f));
    }
  };

    BOOST_DISPATCH_OVERLOAD ( log1p_
                          , (typename A0)
                          , bd::cpu_
                          , bs::plain_tag
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const plain_tag &, A0 x) const BOOST_NOEXCEPT
    {
      return musl_(log1p)(x); //the "plain" version of the algorithm is never speedier than the "musl" version.
      // the call is here to allow a scalar fallback to simd calls
    }
  };

} } }

#endif


#include "geometry/polygon_3.h"

#include "geometry/rotation.h"

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>

namespace gca {

  namespace bg = boost::geometry;

  typedef bg::model::d2::point_xy<double> boost_point_2;
  typedef bg::model::polygon<boost_point_2> boost_poly_2;
  //typedef bg::model::multi_polygon<boost_poly_2> boost_multipoly_2;
  //typedef bg::model::multi_point<boost_point_2> boost_multipoint_2;

  typedef bg::model::linestring<boost_point_2> boost_linestring_2;
  //typedef bg::model::multi_linestring<boost_linestring_2> boost_multilinestring_2;

  polygon_3
  build_clean_polygon_3(const std::vector<point>& vertices,
			const std::vector<std::vector<point>>& hole_verts) {
    vector<point> outer_ring = clean_vertices(vertices);
    delete_antennas(outer_ring);

    // There is an occasional test failure here in simple box
    if (!(outer_ring.size() >= 3)) {
      cout << "ERROR: Outer ring size = " << outer_ring.size() << endl;
      //vtk_debug_ring(outer_ring);

      DBG_ASSERT(outer_ring.size() >= 3);
    }

    vector<vector<point>> inner_rings;
    for (auto h : hole_verts) {

      auto new_h = clean_vertices(h);
      delete_antennas(new_h);

      if (!(new_h.size() >= 3)) {
	cout << "ERROR: Inner ring size = " << h.size() << endl;
      } else {
	inner_rings.push_back(new_h);
      }
    }

    return polygon_3(outer_ring, inner_rings, true);
  }

  polygon_3
  build_clean_polygon_3(const std::vector<point>& vertices) {
    return build_clean_polygon_3(vertices, {});
  }

//   // NOTE: These should be references
  polygon_3::polygon_3(const std::vector<point>& vertices,
		       const std::vector<std::vector<point>>& hole_verts,
		       const bool dummy_param) :
    outer_ring(vertices),
    inner_rings{} {

    for (auto h : hole_verts) {
      inner_rings.push_back(h);
    }
  }

  boost_poly_2
  to_boost_poly_2(const labeled_polygon_3& p) {
    boost_poly_2 pr;
    for (auto poly : p.vertices()) {
      boost::geometry::append(pr, boost::geometry::model::d2::point_xy<double>(poly.x, poly.y));
    }

    boost::geometry::interior_rings(pr).resize(p.holes().size());
    for (int i = 0; i < p.holes().size(); i++) {
      auto& h = p.holes()[i];
      for (auto p : h) {
        boost::geometry::append(pr, boost::geometry::model::d2::point_xy<double>(p.x, p.y), i);
      }
    }

    boost::geometry::correct(pr);

    return pr;
  }

  boost_poly_2 rotate_to_2D(const labeled_polygon_3& p) {
    const rotation r = rotate_from_to(p.normal(), point(0, 0, 1));
    return to_boost_poly_2(apply(r, p));
  }

  double area(const polygon_3& p) {
    auto td = rotate_to_2D(p);
    return bg::area(td);
  }

  bool
  add_to_existing_polys_as_hole(const polygon_3& next,
				std::vector<polygon_3>& polygons) {
    if (polygons.size() == 0) { return false; }

    const rotation r = rotate_from_to(next.normal(), point(0, 0, 1));
    boost_poly_2 next_boost_p = to_boost_poly_2(apply(r, next));

    int max_ind = -1;
    double outer_area_max = -1.0;
    cout << "outer max = " << outer_area_max << endl;
    for (unsigned i = 0; i < polygons.size(); i++) {
      const auto& current_poly = polygons[i];

      // cout << "Current polygon" << endl;
      // vtk_debug_polygon(current_poly);
      // cout << "Both polygons" << endl;
      // vtk_debug_polygons({current_poly, next});

      boost_poly_2 current_boost_p = to_boost_poly_2(apply(r, current_poly));
      double outer_area = area(build_clean_polygon_3(current_poly.vertices()));
      cout << "outer area = " << outer_area << endl;

      bool within_current = bg::within(next_boost_p, current_boost_p);
      cout << "within_current = " << within_current << endl;

      if (within_current && outer_area > outer_area_max) {
	outer_area_max = outer_area;
	max_ind = i;
	cout << "set max_ind = " << max_ind << endl;
      }

      cout << "outer max = " << outer_area_max << endl;
    }

    if (max_ind >= 0) {
      auto& current_poly = polygons[max_ind];
      unsigned old_num_holes = current_poly.holes().size();
      current_poly.add_hole(next.vertices());

      unsigned new_num_holes = polygons[max_ind].holes().size();

      DBG_ASSERT(new_num_holes == (old_num_holes + 1));

      cout << "ADDED TO POLYGON AS HOLE" << endl;

      return true;
    }

    return false;
  }

  std::vector<polygon_3>
  arrange_rings(const std::vector<std::vector<point>>& rings) {
    cout << "# of rings before arranging = " << rings.size() << endl;
    if (rings.size() == 0) { return {}; }

    vector<polygon_3> ring_polys;
    for (auto r : rings) {
      ring_polys.push_back(build_clean_polygon_3(r));
    }

    vector<polygon_3> polygons;
    while (ring_polys.size() > 0) {
      auto next_poly =
	max_element(begin(ring_polys), end(ring_polys),
		    [](const polygon_3& l, const polygon_3& r) {
		      return area(l) < area(r);
		    });

      DBG_ASSERT(next_poly != end(ring_polys));

      polygon_3 next_p = *next_poly;

      ring_polys.erase(next_poly);

      bool is_hole = add_to_existing_polys_as_hole(next_p, polygons);

      if (!is_hole) {
	polygons.push_back(next_p);
      }
    }

    cout << "# of polygons after arranging = " << polygons.size() << endl;

    return polygons;
  }

}