/*
 * Copyright (c) 2018 Nicholas Corgan (n.corgan@gmail.com)
 *
 * Distributed under the MIT License (MIT) (See accompanying file LICENSE.txt
 * or copy at http://opensource.org/licenses/MIT)
 */

#include <pkmn/config.hpp>

// http://stackoverflow.com/q/6884093
#ifdef PKMN_PLATFORM_WIN32
#define NOMINMAX
#include <windows.h>
#endif

#include "exception_internal.hpp"
#include "utils/misc.hpp"
#include "types/rng.hpp"
#include "utils/floating_point_comparison.hpp"

#include <pkmn/calculations/personality.hpp>

#include <pkmn/calculations/shininess.hpp>
#include <pkmn/database/pokemon_entry.hpp>
#include <pkmn/enums/gender.hpp>

#include <boost/assign/list_of.hpp>

#include <algorithm>
#include <stdexcept>
#include <vector>

namespace pkmn { namespace calculations {

    static uint32_t get_gender_threshold(
        float chance_male
    )
    {
        uint32_t ret = 0;

        if(pkmn::fp_compare_equal(chance_male, 0.875f))
        {
            ret = 31;
        }
        else if(pkmn::fp_compare_equal(chance_male, 0.75f))
        {
            ret = 64;
        }
        else if(pkmn::fp_compare_equal(chance_male, 0.5f))
        {
            ret = 127;
        }
        else
        {
            ret = 191;
        }

        return ret;
    }

    uint32_t generate_personality(
        pkmn::e_species species,
        uint32_t trainer_id,
        bool shiny,
        pkmn::e_ability ability,
        pkmn::e_gender gender,
        pkmn::e_nature nature
    )
    {
        pkmn::enforce_value_in_vector(
            "Gender",
            gender,
            {pkmn::e_gender::MALE, pkmn::e_gender::FEMALE, pkmn::e_gender::GENDERLESS}
        );

        if((species == pkmn::e_species::NONE) || (species == pkmn::e_species::INVALID))
        {
            throw std::invalid_argument("Species cannot be None or Invalid.");
        }
        if(nature == pkmn::e_nature::NONE)
        {
            throw std::invalid_argument("Nature cannot be None.");
        }

        uint32_t ret = 0;

        pkmn::database::pokemon_entry entry(species, pkmn::e_game::OMEGA_RUBY, "");
        pkmn::ability_pair_t abilities = entry.get_abilities();
        pkmn::e_ability hidden_ability = entry.get_hidden_ability();
        float chance_male = entry.get_chance_male();
        float chance_female = entry.get_chance_female();

        // Validate ability input.
        uint32_t ability_modulo = 0; // If first or hidden ability, keep this
        if(ability == abilities.second)
        {
            if(ability != pkmn::e_ability::NONE)
            {
                ability_modulo = 1;
            }
            else
            {
                throw std::invalid_argument("You cannot use NONE.");
            }
        }
        else if((ability != abilities.first) && (ability != hidden_ability))
        {
            throw std::invalid_argument("Invalid ability.");
        }

        // Validate gender input.
        if(pkmn::fp_compare_equal((chance_male + chance_female), 0.0f))
        {
            if(gender != pkmn::e_gender::GENDERLESS)
            {
                throw std::invalid_argument("This Pokémon is genderless.");
            }
        }
        else if(pkmn::fp_compare_equal(chance_male, 1.0f))
        {
            if(gender != pkmn::e_gender::MALE)
            {
                throw std::invalid_argument("This Pokémon is male-only.");
            }
        }
        else if(pkmn::fp_compare_equal(chance_female, 1.0f))
        {
            if(gender != pkmn::e_gender::FEMALE)
            {
                throw std::invalid_argument("This Pokémon is female-only.");
            }
        }
        else if(gender != pkmn::e_gender::MALE and gender != pkmn::e_gender::FEMALE)
        {
            throw std::invalid_argument("Valid genders: Male, Female");
        }

        // TODO: validate
        uint32_t nature_index = static_cast<uint32_t>(nature) - 1;
        static const size_t NUM_NATURES = 25;

        // Start trying to find a valid value.
        uint32_t gender_threshold = get_gender_threshold(chance_male);
        bool found = false;
        pkmn::rng<uint32_t> rng;
        do
        {
            ret = rng.rand();

            // Set the gender if applicable.
            if(gender == pkmn::e_gender::MALE)
            {
                ret &= ~0xFF;
                ret |= (rng.rand() % (0xFF - gender_threshold) + gender_threshold);
            }
            else if(gender == pkmn::e_gender::FEMALE)
            {
                ret &= ~0xFF;
                ret |= (rng.rand() % gender_threshold);
            }

            if((modern_shiny(ret, trainer_id) == shiny) &&
               ((ret % NUM_NATURES) == nature_index) &&
               ((ret % 2) == ability_modulo)
            )
            {
                found = true;
            }
        }
        while(!found);

        return ret;
    }

}}


//==================================================================================================
/*!
  @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_GENERIC_FUNCTION_TWO_SPLIT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_GENERIC_FUNCTION_TWO_SPLIT_HPP_INCLUDED

#include <boost/simd/detail/constant/splitfactor.hpp>
#include <boost/simd/detail/enforce_precision.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>
#include <utility>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;

  BOOST_DISPATCH_OVERLOAD ( two_split_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_< bd::floating_<A0> >
                          )
  {
    using result_t = std::pair<A0,A0>;
    BOOST_FORCEINLINE result_t operator() ( A0 const& a) const BOOST_NOEXCEPT
    {
      detail::enforce_precision<A0> enforcer;
      A0 const c = Splitfactor<A0>()*a;
      A0 const c1 = c-a;
      A0 r0 = c-c1;
      return {r0, a-r0};
    }
  };
} } }

#endif


#include <iostream>

#include <CGAL/Quotient.h>
#include <CGAL/function_objects.h>
#include <boost/bind.hpp>

// functor int x int -> Quotient<int>, (a,b) -> a/b
// ------------------------------------------------
typedef CGAL::Creator_2<int, int, CGAL::Quotient<int> >
Quotient_creator;

// functor Quotient<int> ->  Quotient<int>, a/b -> b/a
// ---------------------------------------------------
struct Quotient_inverter
{
  typedef CGAL::Quotient<int> result_type;
  CGAL::Quotient<int> operator() (const CGAL::Quotient<int>& q) const
  {
    return CGAL::Quotient<int> (q.denominator(), q.numerator());
  }
};

int main()
{
  // create composed functor (a,b) -> b/a...
  // ---------------------------------------
  int three = 3;
  int two = 2;
  std::cout << boost::bind
    (Quotient_inverter(), boost::bind
     (Quotient_creator(), _1, _2))
  // ...and apply it to (3, 2)
  // -------------------------
  (three, two);

  return 0;
}


#include <ros/ros.h>
#include <geometry_msgs/WrenchStamped.h>
#include <tf/transform_listener.h>

#include <boost/thread.hpp>

class WrenchTransformer
{
protected:
    ros::NodeHandle _nh;
    ros::Subscriber _wrenchSub;
    ros::Publisher _wrenchPub;
    tf::TransformListener _tfListener;

    std::string _transFrameId;
    std::string _subTopic;
    std::string _pubTopic;
    tf::TransformListener _listener;

    void wrenchCallback(const geometry_msgs::WrenchStampedConstPtr& data)
    {
        geometry_msgs::WrenchStamped transWrench;
        transWrench.header.stamp = data->header.stamp;
        transWrench.header.frame_id = _transFrameId;

        if (data->header.frame_id.compare(_transFrameId) != 0)
        {

            tf::Matrix3x3 transRotMatrix, M_transMatrix;

            tf::Vector3 transTranslation, F_measured, M_measured , F_target , M_target;

            F_measured[0] = data->wrench.force.x;
            F_measured[1] = data->wrench.force.y;
            F_measured[2] = data->wrench.force.z;

            M_measured[0] = data->wrench.torque.x;
            M_measured[1] = data->wrench.torque.y;
            M_measured[2] = data->wrench.torque.z;

            tf::StampedTransform transform;
            // Listen to FTS to the tool frame
            try{
//                _listener.waitForTransform(data->header.frame_id, _transFrameId, ros::Time(0), ros::Duration(0.1));
                _listener.lookupTransform(_transFrameId, data->header.frame_id, ros::Time(0), transform);
            }
            catch (tf::TransformException ex)
            {
                ROS_ERROR("%s",ex.what());
                return;
            }

            // Finding the rotation Matrix from kms40 to tool_tcp
            transRotMatrix = transform.getBasis();

            //Finding the translation vector from kms40 to tool_tcp
            transTranslation = transform.getOrigin();

            // Calculating the Force in the new tool_tcp
            F_target = transRotMatrix * F_measured;

            // Define the Omega matrix
            M_transMatrix[0][0] = 0;
            M_transMatrix[0][1] = -transTranslation[2];
            M_transMatrix[0][2] = transTranslation[1];
            M_transMatrix[1][0] = transTranslation[2];
            M_transMatrix[1][1] = 0;
            M_transMatrix[1][2] = -transTranslation[0];
            M_transMatrix[2][0] = -transTranslation[1];
            M_transMatrix[2][1] = transTranslation[0];
            M_transMatrix[2][2] = 0;

            //Calculating the moment at the tool_tcp

            M_target = (M_transMatrix * transRotMatrix) * F_measured + transRotMatrix * M_measured;

            transWrench.wrench.force.x = F_target[0];
            transWrench.wrench.force.y = F_target[1];
            transWrench.wrench.force.z = F_target[2];
            transWrench.wrench.torque.x = M_target[0];
            transWrench.wrench.torque.y = M_target[1];
            transWrench.wrench.torque.z = M_target[2];

        }
        else
        {
            transWrench.wrench = data->wrench;
        }

        // publish the transformed wrench
        _wrenchPub.publish(transWrench);

    }

public:
    WrenchTransformer(std::string name)
    {

        if( !ros::param::get("~transformFrameId", _transFrameId) )
        {
            ROS_ERROR("Cannot find transformFrameId @ parameterServer");
            return;
        }

        if( !ros::param::get("~subscribeTopic", _subTopic) )
        {
            ROS_ERROR("Cannot find subscribeTopic @ parameterServer");
            return;
        }

        if( !ros::param::get("~publishTopic", _pubTopic) )
        {
            ROS_ERROR("Cannot find publishTopic @ parameterServer");
            return;
        }

        _wrenchPub = _nh.advertise<geometry_msgs::WrenchStamped>(_pubTopic, 1);
        _wrenchSub = _nh.subscribe(_subTopic, 1, &WrenchTransformer::wrenchCallback, this);

    }

};

int main( int argc, char** argv )
{
    ros::init(argc, argv, "WrenchTransformer");

    WrenchTransformer wrenchTransformer(ros::this_node::getName());

    ros::spin();

    return 0;
}


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

#include "KluSolver.h"

#include <klu.h>

#include <armadillo>

using namespace arma;

bool kluSolve(const arma::SpMat<double>& a, const arma::Col<double>& b, arma::Col<double>& result)
{
    auto n = b.size();
    auto nnz = a.n_nonzero;

    int* ap = new int[n + 1];
    for (unsigned int i = 0; i <= n; ++i) ap[i] = static_cast<int>(a.col_ptrs[i]);

    int* ai = new int[nnz];
    for (unsigned int i = 0; i < nnz; ++i) ai[i] = static_cast<int>(a.row_indices[i]);

    double* ax = new double[nnz];
    for (unsigned int i = 0; i < nnz; ++i) ax[i] = a.values[i];

    double* b1 = new double[n];
    for (unsigned int i = 0; i < n; ++i) b1[i] = b(i);

    klu_symbolic *Symbolic;
    klu_numeric *Numeric;
    klu_common Common;

    klu_defaults (&Common);
    Symbolic = klu_analyze (static_cast<int>(n), ap, ai, &Common);
    Numeric = klu_factor (ap, ai, ax, Symbolic, &Common);
    bool ok = klu_solve(Symbolic, Numeric, static_cast<int>(n), 1, b1, &Common) == 1;

    if (!ok)
    {
        std::cerr << "KLU failed." << std::endl;
        std::cerr << "Status = " << Common.status << std::endl;
    }

    klu_free_symbolic (&Symbolic, &Common);
    klu_free_numeric (&Numeric, &Common);

    result = arma::Col<double>(n, fill::none);
    for (arma::uword i = 0; i < n; ++i)
    {
        result(i) = b1[i];
    }

    delete[] ap;
    delete[] ai;
    delete[] ax;
    delete[] b1;

    return ok;
}


#include <stan/math/prim/scal.hpp>
#include <gtest/gtest.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/math/distributions.hpp>
#include <test/unit/math/prim/scal/prob/util.hpp>
#include <limits>
#include <vector>

TEST(ProbDistributionsBetaProportion, error_check) {
  boost::random::mt19937 rng;
  EXPECT_NO_THROW(stan::math::beta_proportion_rng(0.5, 3.0, rng));
  EXPECT_NO_THROW(stan::math::beta_proportion_rng(1e-10, 1e-10, rng));
  EXPECT_THROW(stan::math::beta_proportion_rng(2.0, -1.0, rng),
               std::domain_error);
  EXPECT_THROW(stan::math::beta_proportion_rng(-2.0, 1.0, rng),
               std::domain_error);
  EXPECT_THROW(stan::math::beta_proportion_rng(-2.0, -1.0, rng),
               std::domain_error);
  EXPECT_THROW(stan::math::beta_proportion_rng(stan::math::positive_infinity(),
                                               1.0, rng),
               std::domain_error);
  EXPECT_THROW(
      stan::math::beta_proportion_rng(2, stan::math::positive_infinity(), rng),
      std::domain_error);
}

TEST(ProbDistributionsBetaProportion, chiSquareGoodnessFitTest) {
  boost::random::mt19937 rng;
  int N = 10000;
  int K = stan::math::round(2 * std::pow(N, 0.4));

  double mu = 0.5;     // location
  double kappa = 3.0;  // precision

  std::vector<double> samples;
  for (int i = 0; i < N; ++i) {
    samples.push_back(stan::math::beta_proportion_rng(mu, kappa, rng));
  }

  // transform from location and precision parameterization
  // into shape1 (alpha) and shape2 (beta) parameterization
  double alpha = mu * kappa;
  double beta = kappa - alpha;

  // Generate quantiles from boost's beta distribution
  boost::math::beta_distribution<> dist(alpha, beta);
  std::vector<double> quantiles;
  for (int i = 1; i < K; ++i) {
    double frac = static_cast<double>(i) / K;
    quantiles.push_back(quantile(dist, frac));
  }
  quantiles.push_back(std::numeric_limits<double>::max());

  // Assert that they match
  assert_matches_quantiles(samples, quantiles, 1e-6);
}

TEST(ProbDistributionsBetaProportion, chiSquareGoodnessFitTest2) {
  boost::random::mt19937 rng;
  int N = 10000;
  int K = stan::math::round(2 * std::pow(N, 0.4));

  double mu = 0.3;     // location
  double kappa = 0.5;  // precision

  std::vector<double> samples;
  for (int i = 0; i < N; ++i) {
    samples.push_back(stan::math::beta_proportion_rng(mu, kappa, rng));
  }

  // transform from location and precision parameterization
  // into shape1 (alpha) and shape2 (beta) parameterization
  double alpha = mu * kappa;
  double beta = kappa - alpha;

  // Generate quantiles from boost's beta distribution
  boost::math::beta_distribution<> dist(alpha, beta);
  std::vector<double> quantiles;
  for (int i = 1; i < K; ++i) {
    double frac = static_cast<double>(i) / K;
    quantiles.push_back(quantile(dist, frac));
  }
  quantiles.push_back(std::numeric_limits<double>::max());

  // Assert that they match
  assert_matches_quantiles(samples, quantiles, 1e-6);
}


//=======================================================================
// Copyright (c)
//
// 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 "test_utils/read_tsplib.hpp"
#include "test_utils/2_local_search_logger.hpp"
#include "test_utils/test_result_check.hpp"
#include "test_utils/get_test_dir.hpp"

#include "paal/local_search/2_local_search/2_local_search.hpp"
#include "paal/data_structures/cycle/simple_cycle.hpp"
#include "paal/data_structures/components/components_replace.hpp"
#include "paal/local_search/custom_components.hpp"

#include <boost/test/unit_test.hpp>

#include <vector>
#include <string>
#include <iomanip>
#include <cmath>

using std::string;
using std::vector;
using namespace paal::local_search;
using namespace paal;

std::string path = paal::system::get_test_data_dir("TSPLIB/symmetrical");

BOOST_AUTO_TEST_CASE(TSPLIB) {
    read_tsplib::TSPLIB_Directory dir(path);
    read_tsplib::TSPLIB_Matrix mtx;
    std::string fname;
    float opt;
    while (dir.get_graph(fname, opt)) {
        std::ifstream is(fname);
        read_tsplib::TSPLIB_Directory::Graph g(is);
        g.load(mtx);
        auto size = mtx.size();
        std::vector<int> v(size);
        std::iota(v.begin(), v.end(), 0);

        // create random solution
        std::random_shuffle(v.begin(), v.end());
        data_structures::simple_cycle<int> cycle(v.begin(), v.end());

        // creating local search
        auto lsc = get_default_two_local_components(mtx);

        // printing
        LOGLN("Graph:\t" << fname);
        LOGLN("Length before\t" << get_cycle_length(mtx, cycle));

        // setting logger
        auto logger = utils::make_two_ls_logger(mtx, 100);

        // search
        two_local_search(cycle, paal::local_search::first_improving_strategy{},
                         logger, utils::always_false(), lsc);
        check_result(float(get_cycle_length(mtx, cycle)), opt,
                     4 * sqrt(size));
    }
}

// currently this is too long !
// hope that in the future stronger algorithms'll handle these cases
//
//

/*using paal::local_search::search_components;

BOOST_AUTO_TEST_CASE(TSPLIB_long) {
    const string indexFName = "index.long";
    read_tsplib::TSPLIB_Directory dir(paal::system::build_path(path + "/long/" + indexFName));
    read_tsplib::TSPLIB_Matrix mtx;
    std::string fname;
    float opt;
    while(dir.get_graph(fname ,opt))  {
        std::ifstream is(fname);
        read_tsplib::TSPLIB_Directory::Graph g(is);
        g.load(mtx);
        auto size = mtx.size();
        std::vector<int> v(size);
        std::iota(v.begin(), v.end(), 0);

        //create random solution
        std::random_shuffle(v.begin(), v.end());
        data_structures::simple_cycle<int> cycle(v.begin(), v.end());
        int startLen = simple_algo::get_length(mtx, cycle);

        //creating local search
        auto lsc = get_default_two_local_components(mtx);
        typedef local_search::search_components_traits<puretype(lsc)>::GainT
GainT;
        typedef paal::local_search::gain_cut_small_improves<GainT, int> CIC;
        double epsilon = 0.001;
        CIC  cut(lsc.get<local_search::Gain>(), startLen, epsilon);
        auto cutLsc =
data_structures::replace<local_search::Gain>(std::move(cut), lsc);

        //setting logger
        auto logger = utils::make_two_ls_logger(mtx, 100);

        //printing
        LOGLN("Graph:\t" << fname);
        LOGLN("Length before\t" << simple_algo::get_length(mtx, cycle));

        //search
        for(int j = 0; j < 20; ++j) {
            epsilon /= 2;
            LOGLN("epsilon = " << epsilon);
            cutLsc.get<local_search::Gain>().set_epsilon(epsilon);
            two_local_search(cycle, logger, utils::always_false(), cutLsc);
        }

        LOGLN("Normal search at the end");
        two_local_search(cycle, logger, utils::always_false(), lsc);
    }
}*/


#pragma once
#include <cstdint>
#include <cassert>
#include <algorithm>
#include <map>
#include <boost/dynamic_bitset.hpp>
#include "../Basis/AbstractBasis2D.hpp"

template<typename UINT>
class TITFI2D
{
private:
	edlib::AbstractBasis2D<UINT>& basis_;
	double J_;
	double h_;

public:
	TITFI2D(edlib::AbstractBasis2D<UINT>& basis, double J, double h)
		: basis_(basis), J_(J), h_(h)
	{

	}

	std::map<std::size_t,double> getCol(UINT n) const
	{
		uint32_t Lx = basis_.getLx();
		uint32_t Ly = basis_.getLy();

		UINT a = basis_.getNthRep(n);
		const boost::dynamic_bitset<> bs(Lx*Ly, a);

		std::map<std::size_t, double> m;
		for(int nx = 0; nx < Lx; nx++)
		{
			for(int ny = 0; ny < Ly; ny++)
			{
				//Next-nearest
				auto idx = basis_.toIdx(nx,ny);
				int sgn1 = (1-2*bs[idx])*(1-2*bs[basis_.toIdx((nx+1)%Lx,ny)]);
				int sgn2 = (1-2*bs[idx])*(1-2*bs[basis_.toIdx(nx,(ny+1)%Ly)]);

				m[n] += -J_*(sgn1 + sgn2); //ZZ

				UINT s = a;
				UINT t = (UINT(1u) << UINT(idx));
				s ^= t;

				int bidx;
				double coeff;

				std::tie(bidx, coeff) = basis_.hamiltonianCoeff(s, n);

				if(bidx >= 0)
					m[bidx] += -h_*coeff;
			}
		}
		return m;
	}
};


/*
  Copyright (c) <2014> <Thomas Mörwald, Vienna University of Technology>

  All rights reserved.

  Redistribution and use in source and binary forms, with or without
  modification, are permitted (subject to the limitations in the disclaimer
  below) provided that the following conditions are met:

   * Redistributions of source code must retain the above copyright
     notice, this list of conditions and the following disclaimer.

   * Redistributions in binary form must reproduce the above copyright
     notice, this list of conditions and the following disclaimer in the
     documentation and/or other materials provided with the distribution.

   * Neither the name of <Owner Organization> nor the names of its
     contributors may be used to endorse or promote products derived from this
     software without specific prior written permission.

  NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
  LICENSE.  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
  THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

// This file contains mathematical definitions derifed from CGAL and Eigen
// It implements some helper functions like arithmetic mean, principal component
// analysis (PCA) and comparison of floating point values and vectors

#ifndef _TSPLINE_MATH_H_
#define _TSPLINE_MATH_H_

#include <vector>
#include <stdio.h>
#include <stdexcept>
#include <limits>

#undef Success
#include <Eigen/Eigen>

#include <CGAL/Cartesian.h>
#include <CGAL/MP_Float.h>
#include <CGAL/Quotient.h>
#include <CGAL/Arr_segment_traits_2.h>
#include <CGAL/Arrangement_2.h>
#include <CGAL/Arr_extended_dcel.h>

#define SIZE_T_MAX std::numeric_limits<std::size_t>::max()

namespace tspline
{

// derive algebraic objects from CGAL types
typedef CGAL::Cartesian<double> Kernel;
typedef CGAL::Arr_segment_traits_2<Kernel> Traits_2;
typedef Kernel::Point_2 Point2d;
typedef Kernel::Point_3 Point3d;
typedef Kernel::Vector_2 Vector2d;
typedef Kernel::Vector_3 Vector3d;
typedef Kernel::Ray_3 Ray;
typedef Kernel::Direction_3 Direction3d;
typedef Traits_2::X_monotone_curve_2 Segment2;

// numerical limits
static double epsilon = 10.0 * std::numeric_limits<float>::epsilon(); // float because tgModel using OpenGL is float
static double digits = 1e10;
static double div_digits = 1e-10;

/** @brief extends Point3d by the weight entry for control points  (weight is 1.0 by default) */
class Point4d : public Point3d
{
protected:
  double weight;

public:
  Point4d() : Point3d(), weight(1.0) { }
  Point4d(const Point3d& a, const double& w=1.0) : Point3d(a), weight(w) { }
  Point4d(const double& x, const double& y, const double& z, const double& w) :
    Point3d(x,y,z), weight(w) { }
  Point4d(const double& x, const double& y, const double& z) :
    Point3d(x,y,z), weight(1.0) { }

  void operator=(const Point3d& a)
  {
    *this = Point4d(a);
  }

  double w() const { return weight; }

};

/** @brief aligned Eigen::Vector classes */
typedef std::vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > vector_vec4d;
typedef std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > vector_vec3d;
typedef std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > vector_vec2d;

/** @brief compute the arithmetic mean of a vector of Vector3d
 *  @param data input data */
inline Eigen::Vector3d compute_mean (const vector_vec3d &data)
{
  Eigen::Vector3d u (0.0, 0.0, 0.0);

  unsigned s = unsigned (data.size ());
  double ds = 1.0 / s;

  for (unsigned i = 0; i < s; i++)
    u += (data[i] * ds);

  return u;
}

/** @brief compute the principal components of a vector of Vector3d
 *  @param data in: input data
 *  @param mean out: the arithmetic mean of the data
 *  @param eigenvectors out: the eigenvectors sorted descending by their eigenvalues
 *  @param eigenvalues out: descending eigenvalues */
inline void pca (const vector_vec3d &data, Eigen::Vector3d &mean, Eigen::Matrix3d &eigenvectors,
                 Eigen::Vector3d &eigenvalues)
{
  if (data.empty ())
    throw std::runtime_error ("[Math::pca] Error, data is empty\n");

  mean = compute_mean (data);

  unsigned s = unsigned (data.size ());

  Eigen::MatrixXd Q (3, s);

  for (unsigned i = 0; i < s; i++)
    Q.col (i) << (data[i] - mean);

  Eigen::Matrix3d C = Q * Q.transpose ();

  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver (C);
  if (eigensolver.info () != Eigen::Success)
    throw std::runtime_error ("[Math::pca] Can not find eigenvalues.\n");

  // reverse ordering of eigenvalues and eigenvectors
  for (int i = 0; i < 3; ++i)
  {
    eigenvalues (i) = eigensolver.eigenvalues () (2 - i);
    if (i == 2)
      eigenvectors.col (2) = eigenvectors.col (0).cross (eigenvectors.col (1));
    else
      eigenvectors.col (i) = eigensolver.eigenvectors ().col (2 - i);
  }
}

// comparison functions for floating point data types

// ==
static bool equal(const double& a, const double& b)
{
  if(std::abs<double>(a-b) < epsilon)
    return true;
  else
    return false;
}

// >
static bool greater(const double &a, const double &b)
{
  if(equal(a,b)) // equal
    return false;
  else if(a < b)
    return false;
  else
    return true;
}

// <
static bool smaller(const double &a, const double &b)
{
  if(equal(a,b)) // equal
    return false;
  else if(a > b)
    return false;
  else
    return true;
}

// >=
static bool gequal(const double &a, const double &b)
{
  if(equal(a,b)) // equal
    return true;
  else if(a > b)
    return true;
  else
    return false;
}

// <=
static bool sequal(const double &a, const double &b)
{
  if(equal(a,b))
    return true;
  else if(a < b)
    return true;
  else
    return false;
}

// ==
static bool equal( const Point2d& a, const Point2d& b)
{
  if(equal(a.x(), b.x()) && equal(a.y(), b.y()))
    return true;
  else
    return false;
}

// ==
static bool equal( const Point3d& a, const Point3d& b)
{
  if(equal(a.x(), b.x()) && equal(a.y(), b.y()) && equal(a.z(), b.z()))
    return true;
  else
    return false;
}

// ==
static bool equal( const Eigen::Vector3d& a, const Eigen::Vector3d& b)
{
  if(equal(a(0), b(0)) && equal(a(1), b(1)) && equal(a(2),b(2)))
    return true;
  else
    return false;
}

/** @brief adjust a double value to a grid of minimal resolution */
static double adjust(const double &a)
{
  return a;
//  return double(float(a));
  double b = round(a * digits) * div_digits;
//  if(equal(b,0.0))
//    b = 0.0;
  return b;
}

/** @brief adjust a Point2D value to a grid of minimal resolution */
static Point2d adjust(const Point2d &p)
{
  return Point2d(adjust(p.x()), adjust(p.y()));
}

/** @brief adjust a Point3D value to a grid of minimal resolution */
static Point3d adjust(const Point3d &p)
{
  return Point3d(adjust(p.x()), adjust(p.y()), adjust(p.z()));
}

/** @brief dot product of CGAL::Vector3d (NOT Eigen)*/
static double dot( const Vector3d& a, const Vector3d& b )
{
  return ( a.x()*b.x() + a.y()*b.y() + a.z()*b.z() );
}

/** @brief L2 norm of CGAL::Vector3d (NOT Eigen) */
static double norm( const Vector3d& a )
{
  return sqrt(a.squared_length());
}

/** @brief angle between two CGAL::Vector3d (NOT Eigen) */
static double angle( const Vector3d& a, const Vector3d& b )
{
  return acos( dot(a,b) / (norm(a) * norm(b)) );
}

}

#endif


#ifndef SVD_HPP

#define SVD_HPP

#include "print.hpp"
#include "modify.hpp"
#include "sub.hpp"
#include "multiply.hpp"
#include "special.hpp"
#include "../workspace.hpp"
#include "../tools.hpp"
#include <mkl.h>

#include <boost/timer/timer.hpp>
#include <boost/chrono.hpp>

#include <vector>
#include <utility>

inline bool comparator ( const max_pair& l, const max_pair& r) { return l.first > r.first; }

namespace dqmc {
    namespace la {

	// extern "C" {
	//     void dgejsv_( char* joba, char* jobu, char* jobv, char* jobr, char* jobt,
	// 		  char* jobp,
	// 		  const int* m, const int* n, double* a, const int* lda,
	// 		  double* sva, double* u, const int* ldu,
	// 		  double* v, const int* ldv, double* work,
	// 		  const int* lwork, int* iwork, int* info );

	//     int LAPACKE_dgejsv( int matrix_layout, char joba, char jobu, char jobv,
	//     			char jobr, char jobt, char jobp, int m,
	//     			int n, const double* a,
	//     			int lda, double* sva, double* u,
	//     			int ldu, double* v, int ldv,
	//     			double* stat, int* istat );

	//     void dgesvj_( const char* joba, const char* jobu, const char* jobv,
	// 		  const int* m, const int* n, double* a,
	// 		  const int* lda, double* sva, const int* mv, double* v,
	// 		  const int* ldv, double* work, int* lwork,
	// 		  int* info );
	// }

	template <typename M, typename V>
	inline void close_svd(int sites, M&__restrict__ U, V&__restrict__ D,
			      M&__restrict__ T,
			      M&__restrict__ out) {
	    using namespace std;

	    for (int col = sites - 1; col >= 0; col--) {
		for (int row = sites - 1; row >= 0; row--) {
		    out(row, col) = 0.;
		    for (int k = sites - 1; k >= 0; k--) {
			if (U(row, k) != 0 && T(k, col) != 0 && D(k) != 0)
			    out(row, col) += ( U(row, k) * T(k, col)) * D(k);
		    }
		}
	    }
	}

	template <typename M, typename V>
	inline void close_svd_inv_diag(int sites, M&__restrict__ U, V&__restrict__ D,
				       M&__restrict__ T,
				       M& out) {
	    using namespace std;

	    for (int col = sites - 1; col >= 0; col--) {
		for (int row = sites - 1; row >= 0; row--) {
		    out(row, col) = 0.;
		    for (int k = sites - 1; k >= 0; k--) {
			if (U(row, k) != 0 && T(k, col) != 0 && D(k) != 0)
			    out(row, col) += ( U(row, k) * T(k, col)) / D(k);
		    }
		}
	    }
	}

	inline int decompose_dgejsv_nt(pmat_t&__restrict__ in, pmat_t&__restrict__ U,
				       pvec_t&__restrict__ D, pmat_t&__restrict__ Tt,
				       pvec_t& work, ivec& ivywork) {
	    using namespace std;

	    int info;
	    int lwork = work.size() - 10;

	    char f = 'F';
	    char j = 'J';
	    char n = 'N';
	    char p = 'P';
	    int rows = U.rows();
	    int cols = U.cols();
	    int T_rows = Tt.rows();
	    // arma::vec work(6);
	    ivec iwork(in.rows() + 3 * in.cols());
	    work.setZero();
	    iwork.setZero();

	    dgejsv_( &f, &f, &j, &n, &n, &p,
	    	     &rows, &cols, in.data(), &rows,
	    	     D.data(), U.data(), &rows,
	    	     Tt.data(), &T_rows, work.data(),
	    	     &lwork, iwork.data(), &info);
	    return info;
	    // info = LAPACKE_dgejsv_work( 102, f, f, j, n, n, p,
	    // 				rows, cols, in.data(), rows,
	    // 				D.data(), U.data(), rows,
	    // 				Tt.data(), T_rows, work.data(),
	    // 				work.size(),
	    // 				iwork.data());

	    // return info;
	}

	inline int decompose_dgejsv_col_nt(pmat_t&__restrict__ in, pmat_t&__restrict__ U,
				       pvec_t&__restrict__ D, pmat_t&__restrict__ Tt,
				       pvec_t& work, ivec& iwork) {
	    using namespace std;

	    int info;
	    int lwork = work.size() - 10; //std::max(1024, int(in.rows()*in.rows()*in.rows()));
	    // pvec_t work(lwork);
	    // ivec iwork(std::max(1024, int(in.rows() * in.cols())));

	    char c = 'C';
	    char u = 'U';
	    char j = 'J';
	    char n = 'N';
	    char p = 'P';
	    int rows = U.rows();
	    int cols = U.cols();
	    int T_rows = Tt.rows();

	    work.setZero();
	    iwork.setZero();
	    // info = LAPACKE_dgejsv_work( 102, c, j, j, n, n, p,
	    // 			   rows, cols, in.data(), rows,
	    // 			   D.data(), U.data(), rows,
	    // 			   Tt.data(), T_rows, work.data(),
	    // 				work.size(),
	    // 			   iwork.data());

	    dgejsv_( &c, &u, &j, &n, &n, &p, // 6
	    	     &rows, &cols, in.data(), &rows, //10
	    	     D.data(), U.data(), &rows, // 13
	    	     Tt.data(), &T_rows, work.data(),
	    	     &lwork, iwork.data(), &info); // 17
	    // std::cout << info << std::endl;
	    return info;
	}

	inline int decompose_jsvd_nt(pmat_t&__restrict__ in, pmat_t&__restrict__ U,
				     pvec_t&__restrict__ D, pmat_t&__restrict__ Tt) {
	    using namespace std;

	    char joba = 'G';
	    char jobu = 'U';
	    char jobv = 'V';

	    U = in;

	    int rows = in.rows();
	    int cols = in.cols();
	    int U_rows = in.rows();
	    int T_rows = Tt.rows();

	    int lwork = max(1024, rows * cols);
	    pvec_t work(lwork);
	    work.setOnes();
	    int mv = 0;
	    int info;

	    dgesvj_(&joba, &jobu, &jobv, &rows, &cols, U.data(), &rows,
		    D.data(), &mv, Tt.data(), &T_rows, work.data(), &lwork, &info);

	    if (work(0) != 1.) {
		cout << "MKL ERROR: SVD HAVE TO BE SCALED! " << work(0) << endl;
		throw std::runtime_error("SVD Error");
	    }

	    return info;
	}

	inline int decompose_jsvd_nt(pmat_t&__restrict__ in, pmat_t&__restrict__ U,
				     pvec_t&__restrict__ D, pmat_t&__restrict__ Tt,
				     pvec_t&__restrict__ work) {
	    using namespace std;

	    char joba = 'G';
	    char jobu = 'U';
	    char jobv = 'V';

	    U = in;

	    int rows = in.rows();
	    int cols = in.cols();
	    int U_rows = in.rows();
	    int T_rows = Tt.rows();

	    int lwork = work.size();
	    //pvec_t work(lwork);
	    work.setOnes();
	    int mv = 0;
	    int info;

	    dgesvj_(&joba, &jobu, &jobv, &rows, &cols, U.data(), &rows,
		    D.data(), &mv, Tt.data(), &T_rows, work.data(), &lwork, &info);

	    if (work(0) != 1.) {
		cout << "MKL ERROR: SVD HAVE TO BE SCALED! " << work(0) << endl;
		throw std::runtime_error("SVD Error");
	    }

	    return info;
	}

	inline perm_mat row_sort(mat&__restrict in, mat&__restrict out) {
	    using namespace std;
	    using namespace Eigen;
	    vector<int> max_index(in.rows());
	    vector<max_pair> max_indices;
	    vec max_vals(in.rows());
	    for(int i = 0; i < in.rows(); ++i) {
		max_vals(i) = in.cwiseAbs().row(i).maxCoeff( &max_index[i] );
		max_indices.push_back(max_pair(max_vals(i), i));
	    }
	    std::sort(max_indices.begin(), max_indices.end(), comparator);

	    ivec row_ids(in.rows());
	    for (std::vector<max_pair>::iterator it=max_indices.begin(); it != max_indices.end(); ++it) {
		row_ids((*it).second) = std::distance(max_indices.begin(), it);
		// cout << row_ids((*it).second) <<  " ";
	    }
	    // cout << endl;
	    perm_mat row_perm(row_ids);

	    out = row_perm * in;
	    return row_perm;
	}

	inline perm_mat col_sort(mat&__restrict in, mat&__restrict out) {
	    using namespace std;
	    using namespace Eigen;
	    vector<int> max_index(in.cols());
	    vector<max_pair> max_indices;
	    vec max_vals(in.cols());
	    for(int i = 0; i < in.cols(); ++i) {
		max_vals(i) = in.cwiseAbs().col(i).maxCoeff( &max_index[i] );
		max_indices.push_back(max_pair(max_vals(i), i));
	    }
	    std::sort(max_indices.begin(), max_indices.end(), comparator);

	    ivec col_ids(in.cols());
	    for (std::vector<max_pair>::iterator it=max_indices.begin();
		 it != max_indices.end(); ++it) {
		col_ids((*it).second) = std::distance(max_indices.begin(), it);
		// cout << row_ids((*it).second) <<  " ";
	    }
	    // cout << endl;
	    perm_mat col_perm(col_ids);

	    out = in * col_perm;
	    return col_perm;
	}

	inline void decompose_udt_full_piv(mat&__restrict__ in, mat&__restrict__ U, vec&__restrict__ D, mat&__restrict__ T) {
	    using namespace std;
	    using namespace Eigen;

	    if (U.rows() != in.rows() || U.cols() != in.cols())
		throw std::runtime_error("dimensions of U are wrong");

	    if (D.size() != in.cols()) {
		cout << D.size() << endl;
		throw std::runtime_error("dimensions of D are wrong");
	    }

	    if (T.rows() != in.cols() || U.cols() != in.cols())
		throw std::runtime_error("dimensions of T are wrong");

	    double scale = pow(in.cwiseAbs().maxCoeff(), 0.5);
	    Eigen::FullPivHouseholderQR<mat> qr(in);
	    in /= scale;

	    qr.compute(in);
	    U = qr.matrixQ().block(0, 0, in.rows(), in.cols());
	    mat upper = qr.matrixQR().triangularView<Upper>();// .block(0, 0, in.cols(), in.cols());
	    D = upper.block(0, 0, in.cols(), in.cols()).diagonal();
	    vec d_inv = D;

	    for (int i = 0; i < in.cols(); ++i) {
		if (D(i) == 0.) {
		    std::cout << "culprit " << D(i) << " was "
			      << upper(i, i) << std::endl;
		    throw std::runtime_error("Invalid upper triangle"); }
		d_inv(i) = 1./D(i);
		if (D(i) < 0) {
		    D(i) *= -1.;
		    d_inv(i) *= -1.; }
	    }

	    T = (d_inv.asDiagonal() * upper.block(0, 0, in.cols(), in.cols()))
		* qr.colsPermutation().transpose();
	    D *= scale;
	    if (D(0) != D(0)) std::cout << "NaN alert" << endl;
	}

	inline void decompose_udt_full_sort(mat&__restrict__ in, mat&__restrict__ U, vec&__restrict__ D, mat&__restrict__ T) {
	    using namespace std;
	    using namespace Eigen;

	    if (U.rows() != in.rows() || U.cols() != in.cols())
		throw std::runtime_error("dimensions of U are wrong");

	    if (D.size() != in.cols()) {
		cout << D.size() << endl;
		throw std::runtime_error("dimensions of D are wrong");
	    }

	    if (T.rows() != in.cols() || U.cols() != in.cols())
		throw std::runtime_error("dimensions of T are wrong");

	    mat scaled_in = mat::Zero(in.rows(), in.cols());
	    perm_mat row_perm = dqmc::la::row_sort(in, scaled_in);
	    perm_mat col_perm = dqmc::la::col_sort(scaled_in, in);

	    // Very crude scaling...
	    double scale = pow(in.cwiseAbs().maxCoeff(), 0.5);

	    vec taus(in.cols());
	    ivec iwork(in.cols());

	    // vec work(in.rows() * in.rows() * in.rows());
	    int lwork = -1;
	    int info = 0;
	    int M = in.rows();
	    int N = in.cols();

	    scaled_in.block(0, 0, in.rows(), in.cols()) = in/scale;
	    iwork.setZero();
	    // cout << iwork.size() << " " << work.size() << " " << taus.size() << endl;
	    lwork = -1;
	    dgeqp3_(&M, &N, scaled_in.data(), &M, iwork.data(),
		    taus.data(), taus.data(),
		    &lwork, &info);
	    lwork = taus(0);
	    vec work(lwork);
	    work.setZero();
	    taus.setZero();

	    dgeqp3_(&M, &N, scaled_in.data(), &M, iwork.data(),
		    taus.data(), work.data(),
		    &lwork, &info);

	    mat upper = scaled_in.triangularView<Upper>();
	    D = upper.block(0, 0, in.cols(), in.cols()).diagonal();
	    vec d_inv = D;

	    for (int i = 0; i < in.cols(); ++i) {
		if (D(i) == 0.) {
		    std::cout << "culprit " << D(i) << " was "
			      << upper(i, i) << std::endl;
		    throw std::runtime_error("Invalid upper triangle"); }
		d_inv(i) = 1./D(i);
		if (D(i) < 0) { D(i) *= -1.;
		    d_inv(i) *= -1.; }
	    }
	    lwork = -1;
	    dorgqr_(&M, &N, &N, scaled_in.data(), &M, taus.data(),
		    work.data(), &lwork, &info);
	    // if (work(0) > work.size()) {
	    // 	cout << "Need more lwork for dorgqr" << endl;
	    // }
	    lwork = work(0);
	    work.resize(lwork);
	    // cout << "in" << endl << scaled_in << endl << endl;
	    dorgqr_(&M, &N, &N, scaled_in.data(), &M, taus.data(),
		    work.data(), &lwork, &info);
	    // cout << "out" << endl << scaled_in << endl << endl;
	    U = row_perm.inverse() * scaled_in.block(0, 0, in.rows(), in.cols());
	    ivec col_ids(in.cols());
	    for (int i = 0; i < in.cols(); ++i) {
		col_ids(i) = iwork(i) - 1;
	    }

	    Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> lapack_col_perm(col_ids);
	    T = (d_inv.asDiagonal() * upper.block(0, 0, in.cols(), in.cols()))
		* lapack_col_perm.transpose() * col_perm.transpose();
	    D *= scale;
	    if (D(0) != D(0)) std::cout << "NaN alert" << endl;
	}

	inline void decompose_udt_row_sort(mat&__restrict__ in, mat&__restrict__ U, vec&__restrict__ D, mat&__restrict__ T) {
	    using namespace std;
	    using namespace Eigen;

	    if (U.rows() != in.rows() || U.cols() != in.cols())
		throw std::runtime_error("dimensions of U are wrong");

	    if (D.size() != in.cols()) {
		cout << D.size() << endl;
		throw std::runtime_error("dimensions of D are wrong");
	    }

	    if (T.rows() != in.cols() || U.cols() != in.cols())
		throw std::runtime_error("dimensions of T are wrong");

	    vector<int> max_index(in.rows());
	    vector<max_pair> max_indices;
	    vec max_vals(in.rows());
	    for(int i = 0; i < in.rows(); ++i) {
		max_vals(i) = in.cwiseAbs().row(i).maxCoeff( &max_index[i] );
		max_indices.push_back(max_pair(max_vals(i), i));
	    }
	    std::sort(max_indices.begin(), max_indices.end(), comparator);

	    ivec row_ids(in.rows());
	    for (std::vector<max_pair>::iterator it=max_indices.begin(); it != max_indices.end(); ++it) {
		row_ids((*it).second) = std::distance(max_indices.begin(), it);
		// cout << row_ids((*it).second) <<  " ";
	    }
	    // cout << endl;
	    Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> row_perm(row_ids);

	    // Very crude scaling...
	    double scale = pow(in.cwiseAbs().maxCoeff(), 0.5);

	    vec taus(in.cols());
	    ivec iwork(in.cols());

	    // vec work(in.rows() * in.rows() * in.rows());
	    int lwork = -1;
	    int info = 0;
	    int M = in.rows();
	    int N = in.cols();
	    mat scaled_in = mat::Zero(in.rows(), in.cols());

	    scaled_in.block(0, 0, in.rows(), in.cols()) = (row_perm * in)/scale;
	    iwork.setZero();
	    // cout << iwork.size() << " " << work.size() << " " << taus.size() << endl;
	    lwork = -1;
	    dgeqp3_(&M, &N, scaled_in.data(), &M, iwork.data(),
		    taus.data(), taus.data(),
		    &lwork, &info);
	    lwork = taus(0);
	    vec work(lwork);
	    work.setZero();
	    taus.setZero();

	    dgeqp3_(&M, &N, scaled_in.data(), &M, iwork.data(),
		    taus.data(), work.data(),
		    &lwork, &info);

	    mat upper = scaled_in.triangularView<Upper>();
	    D = upper.block(0, 0, in.cols(), in.cols()).diagonal();
	    vec d_inv = D;

	    for (int i = 0; i < in.cols(); ++i) {
		if (D(i) == 0.) {
		    std::cout << "culprit " << D(i) << " was "
			      << upper(i, i) << std::endl;
		    throw std::runtime_error("Invalid upper triangle"); }
		d_inv(i) = 1./D(i);
		if (D(i) < 0) { D(i) *= -1.;
		    d_inv(i) *= -1.; }
	    }
	    lwork = -1;
	    dorgqr_(&M, &N, &N, scaled_in.data(), &M, taus.data(),
		    work.data(), &lwork, &info);
	    // if (work(0) > work.size()) {
	    // 	cout << "Need more lwork for dorgqr" << endl;
	    // }
	    lwork = work(0);
	    work.resize(lwork);
	    // cout << "in" << endl << scaled_in << endl << endl;
	    dorgqr_(&M, &N, &N, scaled_in.data(), &M, taus.data(),
		    work.data(), &lwork, &info);
	    // cout << "out" << endl << scaled_in << endl << endl;
	    U = row_perm.inverse() * scaled_in.block(0, 0, in.rows(), in.cols());
	    ivec col_ids(in.cols());
	    for (int i = 0; i < in.cols(); ++i) {
		col_ids(i) = iwork(i) - 1;
	    }

	    Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> col_perm(col_ids);
	    T = (d_inv.asDiagonal() * upper.block(0, 0, in.cols(), in.cols()))
		* col_perm.transpose();
	    D *= scale;
	    if (D(0) != D(0)) std::cout << "NaN alert" << endl;
	}

	inline void decompose_udt_row_sort_sqr_q(mat&__restrict__ in, mat&__restrict__ U, vec&__restrict__ D, mat&__restrict__ T) {
	    using namespace std;
	    using namespace Eigen;

	    vector<int> max_index(in.rows());
	    vector<max_pair> max_indices;
	    vec max_vals(in.rows());
	    for(int i = 0; i < in.rows(); ++i) {
		max_vals(i) = in.cwiseAbs().row(i).maxCoeff( &max_index[i] );
		max_indices.push_back(max_pair(max_vals(i), i));
	    }
	    std::sort(max_indices.begin(), max_indices.end(), comparator);

	    ivec row_ids(in.rows());
	    for (std::vector<max_pair>::iterator it=max_indices.begin(); it != max_indices.end(); ++it) {
		row_ids((*it).second) = std::distance(max_indices.begin(), it);
		// cout << row_ids((*it).second) <<  " ";
	    }
	    // cout << endl;
	    Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> row_perm(row_ids);

	    // Very crude scaling...
	    double scale = pow(in.cwiseAbs().maxCoeff(), 0.3);

	    vec taus(in.rows());
	    // vec work(in.rows() * in.rows() * in.rows());
	    int lwork = -1;
	    ivec iwork(in.rows());
	    int info = 0;
	    int M = in.rows();
	    int N = in.cols();
	    mat scaled_in = mat::Zero(in.rows(), in.rows());

	    scaled_in.block(0, 0, in.rows(), in.cols()) = (row_perm * in)/scale;
	    iwork.setZero();
	    // cout << iwork.size() << " " << work.size() << " " << taus.size() << endl;
	    lwork = -1;
	    dgeqp3_(&M, &N, scaled_in.data(), &M, iwork.data(),
		    taus.data(), taus.data(),
		    &lwork, &info);
	    lwork = taus(0);
	    vec work(lwork);
	    dgeqp3_(&M, &N, scaled_in.data(), &M, iwork.data(),
		    taus.data(), work.data(),
		    &lwork, &info);

	    {
		{
		    mat upper = scaled_in.triangularView<Upper>();
		    D = upper.block(0, 0, in.cols(), in.cols()).diagonal();
		    vec d_inv = D;

		    for (int i = 0; i < in.cols(); ++i) {
			if (D(i) == 0.) {
			    std::cout << "culprit " << D(i) << " was "
				      << upper(i, i) << std::endl;
			    throw std::runtime_error("Invalid upper triangle"); }
			d_inv(i) = 1./D(i);
			if (D(i) < 0) { D(i) *= -1.;
			    d_inv(i) *= -1.; }
		    }
		    dorgqr_(&M, &M, &N, scaled_in.data(), &M, taus.data(),
			    work.data(), &lwork, &info);
		    U = row_perm.inverse() * scaled_in.block(0, 0, in.rows(), in.cols());
		    ivec col_ids(in.cols());
		    for (int i = 0; i < in.cols(); ++i) {
			col_ids(i) = iwork(i) - 1;
		    }

		    Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> col_perm(col_ids);
		    T = (d_inv.asDiagonal() * upper.block(0, 0, in.cols(), in.cols()))
			* col_perm.transpose();
		    D *= scale;
		    if (D(0) != D(0)) std::cout << "NaN alert" << endl;
		}
	    }
	}

#ifdef USE_DD
	inline void decompose_udt_row_sort_dd(mat&__restrict__ in, mat&__restrict__ U, vec&__restrict__ D, mat&__restrict__ T) {
	    using namespace std;
	    using namespace Eigen;

	    vector<int> max_index(in.rows());
	    vector<max_pair> max_indices;
	    vec max_vals(in.rows());
	    for(int i = 0; i < in.rows(); ++i) {
		max_vals(i) = in.cwiseAbs().row(i).maxCoeff( &max_index[i] );
		max_indices.push_back(max_pair(max_vals(i), i));
	    }
	    std::sort(max_indices.begin(), max_indices.end(), comparator);

	    ivec row_ids(in.rows());
	    for (std::vector<max_pair>::iterator it=max_indices.begin(); it != max_indices.end(); ++it) {
		row_ids((*it).second) = std::distance(max_indices.begin(), it);
		// cout << row_ids((*it).second) <<  " ";
	    }
	    // cout << endl;
	    Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> row_perm(row_ids);

	    // Very crude scaling...
	    double scale = pow(in.cwiseAbs().maxCoeff(), 0.5);

	    vec taus(in.rows());
	    // vec work(in.rows() * in.rows() * in.rows());
	    int lwork = -1;
	    ivec iwork(in.rows());
	    int info = 0;
	    int M = in.rows();
	    int N = in.cols();
	    mat temp = mat::Zero(in.rows(), in.cols());
	    dd_mat dd_temp = dd_mat::Zero(in.cols(), in.cols());
	    dd_mat scaled_in = dd_mat::Zero(in.rows(), in.cols());
	    temp = ((row_perm * in)/scale);
	    scaled_in.block(0, 0, in.rows(), in.cols()) = temp.cast<ddouble>();
	    Eigen::FullPivHouseholderQR<dd_mat> qr(scaled_in);
	    // cout << scaled_in << endl << endl;
	    qr.compute(scaled_in);
	    dd_temp = qr.matrixQR().block(0, 0, in.cols(), in.cols()).triangularView<Upper>();
	    mat upper = mat::Zero(in.cols(), in.cols());
	    // cout << "from dd_temp" << endl << endl << upper << endl << endl;
	    // cout << dd_temp.rows() << " x " << dd_temp.cols() << " - " <<
	    // 	upper.rows() << " x " << upper.cols() << endl;
	    dqmc::la::copy_from_dd(dd_temp, upper);
	    // cout << "from dd_temp" << endl << endl << upper << endl << endl;
	    D = upper.block(0, 0, in.cols(), in.cols()).diagonal();
	    vec d_inv = D;

	    for (int i = 0; i < in.cols(); ++i) {
		if (D(i) == 0.) {
		    std::cout << "culprit " << D(i) << " was "
			      << upper(i, i) << std::endl;
		    throw std::runtime_error("Invalid upper triangle"); }
		d_inv(i) = 1./D(i);
		if (D(i) < 0) { D(i) *= -1.;
		    d_inv(i) *= -1.; }
	    }
	    //dd_mat qrQ  = qr.householderQ();
	    dd_mat qrQ  = qr.matrixQ();
	    dd_temp = qrQ.block(0, 0, in.rows(), in.cols());

	    dqmc::la::copy_from_dd(dd_temp, temp);
	    // temp = dd_temp.cast<double>();
	    U = row_perm.inverse() * temp;

	    T = (d_inv.asDiagonal() * upper.block(0, 0, in.cols(), in.cols()))
		* qr.colsPermutation().transpose();
	    D *= scale;
	    if (D(0) != D(0)) std::cout << "NaN alert" << endl;
	}
#endif

	inline void decompose_udt_row_sort(mat &__restrict__ in, mat &__restrict__ U, vec &__restrict__ D, mat &__restrict__ T,
					   mat &__restrict__ scaled_in, mat &__restrict__ T_temp,
					   ivec &__restrict__ row_ids, ivec &__restrict__ col_ids,
					   ivec &__restrict__ max_index, vec &__restrict__ max_vals, std::vector<max_pair>& max_pairs,
					   vec &__restrict__ taus, vec &__restrict__ work_old, ivec &__restrict__ iwork_old ) {
	    using namespace std;
	    using namespace Eigen;

	    max_pairs.clear();
	    for(int i = 0; i < in.rows(); ++i) {
		max_vals(i) = in.cwiseAbs().row(i).maxCoeff( &max_index(i) );
		max_pairs.push_back(max_pair(max_vals(i), i));
		// cout << i << endl;
	    }

	    // for (std::vector<max_pair>::iterator it=max_pairs.begin(); it != max_pairs.end(); ++it) {
	    // 	row_ids((*it).second) = std::distance(max_pairs.begin(), it);
	    // 	cout << (*it).second << " | " << std::distance(max_pairs.begin(), it) << "   ";
	    // }
	    // cout << endl;

	    std::sort(max_pairs.begin(), max_pairs.end(), comparator);
	    // cout << "Sorted" << endl;
	    // cout << row_ids.size();
	    for (std::vector<max_pair>::iterator it=max_pairs.begin(); it != max_pairs.end(); ++it) {
		row_ids((*it).second) = std::distance(max_pairs.begin(), it);
		// cout << (*it).second << " | " << std::distance(max_pairs.begin(), it) << "   ";
	    }
	    // cout << endl;
	    Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> row_perm(row_ids);
	    // cout << "permutation" << endl;

	    // Very crude scaling...
	    double scale = pow(in.cwiseAbs().maxCoeff(), 0.3);
	    int lwork; // = work.size();
	    // vec work(in.rows() * in.rows() * in.rows());
	    // int lwork = work.size();

	    int info = 0;
	    int M = in.rows();
	    int N = in.cols();
	    ivec iwork(in.cols());

	    // cout << "Scaled" << endl;
	    // cout << row_perm.indices().transpose() << endl << endl;
	    // cout << row_perm.toDenseMatrix() << endl << endl;
	    // cout << in << endl << endl;
	    {
		{
		    scaled_in = (row_perm * in)/scale;
		    // cout << scaled_in << endl << endl;
		    iwork.setZero();
		    // cout << "QRing" << endl;
		    // cout << iwork.size() << " " << work.size() << " " << taus.size() << endl;
		    // ivec iwork(4*in.rows());
		    lwork = -1;
		    dgeqp3_(&M, &N, scaled_in.data(), &M, iwork.data(),
			    taus.data(), taus.data(),
			    &lwork, &info);
		    lwork = taus(0);
		    vec work(lwork);

		    dgeqp3_(&M, &N, scaled_in.data(), &M, iwork.data(),
			    taus.data(), work.data(),
			    &lwork, &info);
		}
	    }
	    {
		{

		    // cout << "To zero!" << endl;
		    // cout << "Getting diagonal" << endl;
		    D = scaled_in.diagonal();
		    vec d_inv = D;
		    // cout << "Gitgotthat" << endl;

		    for (int i = 0; i < in.cols(); ++i) {
			if (D(i) == 0.) {
			    std::cout << "culprit " << D(i) << std::endl;
			    throw std::runtime_error("Invalid upper triangle"); }

			d_inv(i) = 1./D(i);
			if (D(i) < 0) { D(i) *= -1.;
			    d_inv(i) *= -1.; }
		    }

		    for (int i = 0; i < in.cols(); ++i) col_ids(i) = iwork(i) - 1;
		    Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> col_perm(col_ids);
		    T_temp = scaled_in.triangularView<Upper>();
		    T = (d_inv.asDiagonal() * T_temp) * col_perm.transpose();
		    // cout << T << endl << endl;

		    D *= scale;
		    if (D(0) != D(0)) std::cout << "NaN alert" << endl;
		    lwork = -1;
		    dorgqr_(&M, &M, &N, scaled_in.data(), &M, taus.data(), taus.data(), &lwork, &info);
		    lwork = taus(0);
		    vec work(lwork);
		    dorgqr_(&M, &M, &N, scaled_in.data(), &M, taus.data(), work.data(), &lwork, &info);
		    U = row_perm.inverse() * scaled_in;
		}
	    }
	    return;
	}

	inline void decompose_udt_col_piv(mat&__restrict__ in,
					  mat&__restrict__ U,
					  vec&__restrict__ D,
					  mat&__restrict__ T) {
	    using namespace std;
	    using namespace Eigen;
	    if (U.rows() != in.rows() || U.cols() != in.cols() || U.cols() == 0) {
		cout << U.rows() << " " << U.cols() << endl;
		throw std::runtime_error("dimensions of U are wrong");
	    }

	    if (D.size() != in.cols() || D.size() == 0) {
		cout << D.size() << endl;
		throw std::runtime_error("dimensions of D are wrong");
	    }

	    if (T.rows() != in.cols() || T.cols() == 0)
		throw std::runtime_error("dimensions of T are wrong");

	    mat_t scaled_in = mat_t::Zero(in.rows(), in.cols());

	    vec_t taus(in.cols());
	    ivec iwork(in.cols());

	    int lwork = -1;
	    int M = in.rows();
	    int N = in.cols();

	    perm_mat row_sort_perm = row_sort(in, scaled_in);
	    // scaled_in.block(0, 0, in.rows(), in.cols()) = in;
	    iwork.setZero();
	    lapack_int info = LAPACKE_dgeqp3( LAPACK_COL_MAJOR, M, N, scaled_in.data(),
					      M, iwork.data(), taus.data() );

	    mat_t upper = scaled_in.triangularView<Upper>();
	    D = upper.block(0, 0, in.cols(), in.cols()).diagonal();
	    vec d_inv = D;

	    for (int i = 0; i < in.cols(); ++i) {
		if (D(i) == 0.) {
		    // std::cout << "culprit " << D(i) << " was "
		    // 	      << upper(i, i) << std::endl;
		    throw std::runtime_error("Invalid upper triangle"); }
		d_inv(i) = 1./D(i);
		if (D(i) < 0) { D(i) *= -1.;
		    d_inv(i) *= -1.; }
	    }
	    lwork = -1;

	    info = LAPACKE_dorgqr( LAPACK_COL_MAJOR, M, N, N,
				   scaled_in.data(), M, taus.data() );

	    U = (row_sort_perm.inverse() * scaled_in).block(0, 0, in.rows(), in.cols());
	    ivec col_ids(in.cols());
	    for (int i = 0; i < in.cols(); ++i) {
		col_ids(i) = iwork(i) - 1;
		if (col_ids(i) == -1) {
		    dqmc::tools::abort("Permutation fails");
		}
	    }
	    Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> lapack_col_perm(col_ids);

	    T = (d_inv.asDiagonal()
		 * upper.block(0, 0, in.cols(), in.cols()))
		* lapack_col_perm.transpose();
	    if (D(0) != D(0)) std::cout << "NaN alert" << endl;
	}

    }
}
#endif


//---------------------------------------------------------------------------//
// 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.
//---------------------------------------------------------------------------//

#include <algorithm>
#include <iostream>
#include <vector>

#include <boost/compute/system.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/random/default_random_engine.hpp>
#include <boost/compute/random/discrete_distribution.hpp>

#include "perf.hpp"

namespace compute = boost::compute;

int main(int argc, char *argv[])
{
    perf_parse_args(argc, argv);
    std::cout << "size: " << PERF_N << std::endl;

    compute::device device = compute::system::default_device();
    compute::context context(device);
    compute::command_queue queue(context, device);

    compute::vector<compute::uint_> vector(PERF_N, context);

    int weights[] = {1, 1};

    compute::default_random_engine rng(queue);
    compute::discrete_distribution<compute::uint_> dist(weights, weights+2);

    perf_timer t;
    t.start();
    dist.generate(vector.begin(), vector.end(), rng, queue);
    queue.finish();
    t.stop();
    std::cout << "time: " << t.min_time() / 1e6 << " ms" << std::endl;

    return 0;
}


// Author(s): Jeroen Keiren
// Copyright: see the accompanying file COPYING or copy at
// https://github.com/mCRL2org/mCRL2/blob/master/COPYING
//
// 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)
//
/// \file data_expression_test.cpp
/// \brief Basic regression test for data expressions.

#include <boost/test/minimal.hpp>
#include <iostream>

#include "mcrl2/data/standard_numbers_utility.h"

using namespace mcrl2;
using namespace mcrl2::data;

void recogniser_test()
{
  variable i("i", sort_int::int_());
  application minus(sort_int::minus(i, sort_int::int_("5")));
  application negate(sort_int::negate(i));

  BOOST_CHECK(sort_int::is_minus_application(minus));
  BOOST_CHECK(!sort_int::is_negate_application(minus));
  BOOST_CHECK(sort_int::is_negate_application(negate));
  BOOST_CHECK(!sort_int::is_minus_application(negate));
}

int test_main(int argc, char** argv)
{
  recogniser_test();

  return EXIT_SUCCESS;
}


#include <boost/shared_ptr.hpp>
#include <gtest/gtest.h>
#include "nuturtlebot/WheelCommands.h"
#include "nuturtlebot/SensorData.h"
#include <geometry_msgs/Twist.h>
#include <ros/ros.h>
#include <ros/console.h>
#include <sensor_msgs/JointState.h>
#include <thread>
#include <chrono>
#include <cmath>

/// \file
/// Test cases for ros tesing all nodes in turtle interface file

constexpr float PI = 3.141592653589793238;

class TurtleInterface {

   public:

     ros::Publisher cmdVelPublisher, sensorDataPublisher;
     ros::Subscriber wheelCmdSubscriber, jspSubscriber;
     double maxMotorVelocity, robotMaxTransVel, robotMaxRotVel;
     double wheelBase, wheelRadius, encoderTicksPerRevolution;
     std::string leftWheelJoint, rightWheelJoint;
     bool twistMessageReceived, jspMessageReceived;
     int maxWheelCommand;
     nuturtlebot::WheelCommands receivedCommand;
     sensor_msgs::JointState receivedJoint;
     ros::NodeHandle n;

        TurtleInterface()
        {

         //ros::Rate loop_rate(10);
         ros::param::get("max_rot_vel_motor", maxMotorVelocity);
         ros::param::get("max_trans_vel", robotMaxTransVel);
         ros::param::get("max_rot_vel_motor", robotMaxRotVel);
         ros::param::get("wheel_base", wheelBase);
         ros::param::get("wheel_radius", wheelRadius);
         ros::param::get("~left_wheel_joint", leftWheelJoint);
         ros::param::get("~right_wheel_joint", rightWheelJoint);
         ros::param::get("encoder_ticks_per_rev", encoderTicksPerRevolution);
         ros::param::get("max_wheel_command", maxWheelCommand);

         cmdVelPublisher = n.advertise<geometry_msgs::Twist>("/cmd_vel", 1000, true);
         sensorDataPublisher = n.advertise<nuturtlebot::SensorData>("/sensor_data", 1000, true);
         wheelCmdSubscriber = n.subscribe("/wheel_cmd", 1000, &TurtleInterface::wheelCmdCallback, this);
         jspSubscriber = n.subscribe("/joint_states", 1000, &TurtleInterface::jspCallback, this);
         twistMessageReceived = false;
         jspMessageReceived = true;
         //loop_rate.sleep();
        }

   void publishTwist(const geometry_msgs::Twist zeroRotTwist)
   {
     twistMessageReceived = false;

   cmdVelPublisher.publish(zeroRotTwist);

   }

   void publishSensorData(const nuturtlebot::SensorData turtleBotData)
   {
   sensorDataPublisher.publish(turtleBotData);
   jspMessageReceived = false;
   }

   void wheelCmdCallback(const nuturtlebot::WheelCommands movementCommand)
   {
     receivedCommand = movementCommand;
     twistMessageReceived = true;

   }

  void jspCallback(const sensor_msgs::JointState jointState)
  {
    receivedJoint = jointState;
    jspMessageReceived = true;

  }

};

    TEST(TurtleBotMovement, PureRotationVelocityCommand)
    {

    TurtleInterface turtleInterface;
    double wheelRadius = turtleInterface.wheelRadius;
    double maxWheelCommand = turtleInterface.maxWheelCommand;
    double maxMotorVelocity = turtleInterface.maxMotorVelocity;
    double wheelBase = turtleInterface.wheelBase;

      ROS_INFO("Entered test case");
      geometry_msgs::Twist pureRotTwist;

      pureRotTwist.linear.x = 0;
      pureRotTwist.linear.y = 0;
      pureRotTwist.linear.z = 0;
      pureRotTwist.angular.x = 0;
      pureRotTwist.angular.y = 0;
      pureRotTwist.angular.z = PI/2;

      turtleInterface.publishTwist(pureRotTwist);

      while(not turtleInterface.twistMessageReceived)
      {

        ros::spinOnce();
      }
      turtleInterface.twistMessageReceived = false;
      ROS_INFO("Compare results");
      double expectedWheelMovement = wheelBase / 2.0 * PI/2;
      auto receivedCommand = turtleInterface.receivedCommand;

      int expected_output = round(expectedWheelMovement / wheelRadius * maxWheelCommand / maxMotorVelocity);
      ASSERT_EQ(receivedCommand.right_velocity, expected_output)<<"Velocities commands don't match the"
                                                                 "expected value for a pure rotation";
      ASSERT_EQ(receivedCommand.left_velocity, -expected_output)<<"Velocities commands don't match the"
                                                                 "expected value for a pure rotation";
    }

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

   ros::init(argc, argv, "turtle_interface_test");
   ::testing::InitGoogleTest(&argc, argv);
   ROS_INFO("stated node");
   return RUN_ALL_TESTS();

  // testing::InitGoogleTest(&argc, argv);
  // ros::init(argc, argv, "TurtleInterfaceTest");

  // ros::AsyncSpinner spinner(1);
  // spinner.start();
  // int ret = RUN_ALL_TESTS();
  // spinner.stop();
  // ros::shutdown();
  // return ret;
}


#pragma once

#include <iostream>

#include <vector>
#include <algorithm>
#include <climits>
#include <boost/array.hpp>

#include "../common.h"

template<size_t L> struct InternalNode;

template<size_t L>
struct LeafNode {
    typedef boost::array<byte, L> Element;

    size_t size_of() const {
        return es.capacity() * sizeof(Element);
    }

    void add(Element const & e) {
        es.insert(std::upper_bound(es.begin(), es.end(), e), e);
    }

    bool contains(Element const & e) const {
        return std::find(es.begin(), es.end(), e) != es.end();
    }

    std::vector<Element> const & elements() const {
        return es;
    }

private:
    std::vector<Element> es;
};

template<size_t L>
struct Node {
    typedef boost::array<byte, L> Element;

    Node() : is_leaf(true) {
        raw_node.leaf = new LeafNode<L>();
    }

    ~Node() {
        if (is_leaf) {
            delete raw_node.leaf;
        } else {
            delete raw_node.internal;
        }
    }

    void add(Element const & e) {
        if (is_leaf) {
            raw_node.leaf->add(e);
            if (raw_node.leaf->size_of() > InternalNode<L>::size_of()) {
                split();
            }
        } else {
            raw_node.internal->add(e);
        }
    }

    bool contains(Element const & e) const {
        if (is_leaf) {
            return raw_node.leaf->contains(e);
        } else {
            return raw_node.internal->contains(e);
        }
    }

private:
    bool is_leaf;
    union {
        InternalNode<L> * internal;
        LeafNode<L> * leaf;
    } raw_node;

    void split() {
        InternalNode<L> * new_node = new InternalNode<L>();
        std::vector<Element> const & es(raw_node.leaf->elements());
        for (size_t i = 0; i < es.size(); ++i) {
            new_node->add(es[i]);
        }
        delete raw_node.leaf;
        raw_node.internal = new_node;
        is_leaf = false;
    }
};

template<>
struct Node<1> {
    typedef boost::array<byte, 1> Element;

    Node() : es(std::vector<bool>(1 << CHAR_BIT)) {
    }

    void add(Element const & e) {
        es[e.front()] = true;
    }

    bool contains(Element const & e) const {
        return es[e.front()];
    }

private:
    std::vector<bool> es;

};

template<size_t L>
struct InternalNode {
    typedef boost::array<byte, L> Element;

    static size_t size_of() {
        return (1 << CHAR_BIT) * sizeof(Node<L - 1>);
    }

    void add(Element const & e) {
        boost::array<byte, L - 1> tail;
        std::copy(e.begin() + 1, e.end(), tail.begin());
        children[e.front()].add(tail);
    }

    bool contains(Element const & e) const {
        boost::array<byte, L - 1> tail;
        std::copy(e.begin() + 1, e.end(), tail.begin());
        return children[e.front()].contains(tail);
    }

private:
    boost::array<Node<L - 1>, 1 << CHAR_BIT> children;
};


// Copyright (C) 2021 Athena Elafrou (ae488@cam.ac.uk)
// SPDX-License-Identifier:    MIT

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <iostream>
#include <limits>
#include <mpi.h>
#include <set>

#include <spmv/spmv.h>

// Compare double precision floating-point numbers (taken from the "Art of
// Computer Programming")
bool essentially_equal(double a, double b, double epsilon)
{
  return fabs(a - b) <= ((fabs(a) > fabs(b) ? fabs(b) : fabs(a)) * epsilon);
}

// Divide range into ~equal chunks
static std::vector<int> compute_ranges(int size, int N)
{
  // Compute number of items per process and remainder
  const int n = N / size;
  const int r = N % size;

  // Compute local range
  std::vector<int> ranges;
  for (int rank = 0; rank < (size + 1); ++rank) {
    if (rank < r)
      ranges.push_back(rank * (n + 1));
    else
      ranges.push_back(rank * n + r);
  }

  return ranges;
}

static bool test_spmv(bool symmetric, spmv::CommunicationModel cm)
{
  int mpi_rank;
  MPI_Comm_rank(MPI_COMM_WORLD, &mpi_rank);
  int mpi_size;
  MPI_Comm_size(MPI_COMM_WORLD, &mpi_size);

  // Define a global N x N matrix
  const int N = 5;
  const int NNZ = 15;
  int rowptr[N + 1] = {0, 3, 6, 9, 13, 15};

  int colind[NNZ] = {0, 1, 3, 0, 1, 3, 2, 3, 4, 0, 1, 2, 3, 2, 4};

  double values[NNZ] = {1.0,  -2.0, -3.0, -2.0, 5.0, 4.0,  6.0, 4.0,
                        -4.0, -3.0, 4.0,  4.0,  8.0, -4.0, 8.0};

  // Define a global input vector
  Eigen::VectorXd x(N);
  for (int i = 0; i < N; ++i) {
    double z = (double)(i) / double(N);
    x[i] = exp(-10 * pow(5 * (z - 0.5), 2.0));
  }

  // Compute reference result sequentially
  Eigen::VectorXd y_ref(N);
  for (int i = 0; i < N; ++i) {
    y_ref(i) = 0.0;
    for (int j = rowptr[i]; j < rowptr[i + 1]; ++j) {
      y_ref(i) += values[j] * x(colind[j]);
    }
  }
  double norm_ref = y_ref.norm();

  // Divide matrix by rows evenly across processes
  std::vector<int> ranges = compute_ranges(mpi_size, N);
  int row_start = ranges[mpi_rank];
  int row_end = ranges[mpi_rank + 1];
  int nrows_local = row_end - row_start;
  int ncols_local = nrows_local;
  int nnz_local = rowptr[row_end] - rowptr[row_start];

  // Define local part of the matrix
  int* rowptr_local = rowptr + row_start;
  int* colind_local = colind + rowptr[row_start];
  double* values_local = values + rowptr[row_start];

  // Adapt colind to local indexing
  // Determine column ghosts and remap corresponding indices to re-defined local
  // indices
  std::set<int> ghost_indices;
  for (int i = 0; i < nnz_local; ++i) {
    int global_index = colind_local[i];
    if (global_index < row_start || global_index >= row_end) {
      ghost_indices.insert(global_index);
    }
  }
  std::vector<std::int64_t> col_ghosts(ghost_indices.begin(),
                                       ghost_indices.end());

  // Need mapping of global to local column indices
  for (int i = 0; i < nnz_local; ++i) {
    int global_index = colind_local[i];
    if (global_index < row_start || global_index >= row_end) {
      // Binary search in col_ghosts to find local index
      auto it = std::lower_bound(col_ghosts.begin(), col_ghosts.end(),
                                 global_index);
      colind_local[i] = ncols_local + std::distance(col_ghosts.begin(), it);
    } else {
      colind_local[i] -= row_start;
    }
  }

  // Adapt rowptr to local indexing
  int row_offset = rowptr[row_start];
  for (int i = 0; i < nrows_local + 1; i++)
    rowptr_local[i] -= row_offset;

  spmv::Matrix<double>* A = spmv::Matrix<double>::create_matrix(
      MPI_COMM_WORLD, rowptr_local, colind_local, values_local, nrows_local,
      ncols_local, {}, col_ghosts, symmetric, cm);

  // Define local vectors
  std::shared_ptr<const spmv::L2GMap> l2g = A->col_map();
  Eigen::VectorXd y_local(nrows_local);
  Eigen::VectorXd x_local(l2g->local_size() + l2g->num_ghosts());
  // Initialize from global vector
  memcpy(x_local.data(), x.data() + row_start, ncols_local * sizeof(double));

  // Compute SpMV
  l2g->update(x_local.data());
  y_local = A->mult(x_local);

  double norm = y_local.squaredNorm();
  double norm_test;
  MPI_Allreduce(&norm, &norm_test, 1, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
  norm_test = sqrt(norm_test);

  // Cleanup
  delete A;

  if (essentially_equal(norm_test, norm_ref,
                        std::numeric_limits<double>::epsilon())) {
    std::cout << "PASSED (Rank " << mpi_rank << ")" << std::endl;
    return true;
  } else {
    std::cout << "FAILED (Rank " << mpi_rank << ")" << std::endl;
    return false;
  }
}

int main(int argc, char** argv)
{
#ifdef _OPENMP
  int provided;
  MPI_Init_thread(&argc, &argv, MPI_THREAD_FUNNELED, &provided);
  if (provided < MPI_THREAD_FUNNELED) {
    std::cerr << "The threading support level is lesser than required"
              << std::endl;
    MPI_Abort(MPI_COMM_WORLD, EXIT_FAILURE);
  }
#else
  MPI_Init(&argc, &argv);
#endif

  int mpi_rank;
  MPI_Comm_rank(MPI_COMM_WORLD, &mpi_rank);

  bool ret = true;
  bool symmetric;
  spmv::CommunicationModel cm;

  symmetric = false;
  if (mpi_rank == 0)
    std::cout << "Running vanilla SpMV on CPU with blocking collective "
                 "communication... "
              << std::endl;
  cm = spmv::CommunicationModel::collective_blocking;
  ret &= test_spmv(symmetric, cm);
  MPI_Barrier(MPI_COMM_WORLD);

  if (mpi_rank == 0)
    std::cout << "Running vanilla SpMV on CPU with non-blocking collective "
                 "communication... "
              << std::endl;
  cm = spmv::CommunicationModel::collective_nonblocking;
  ret &= test_spmv(symmetric, cm);
  MPI_Barrier(MPI_COMM_WORLD);

  if (mpi_rank == 0)
    std::cout << "Running vanilla SpMV on CPU with blocking point-to-point "
                 "communication... "
              << std::endl;
  cm = spmv::CommunicationModel::p2p_blocking;
  ret &= test_spmv(symmetric, cm);
  MPI_Barrier(MPI_COMM_WORLD);

  if (mpi_rank == 0)
    std::cout << "Running vanilla SpMV on CPU with non-blocking point-to-point "
                 "communication... "
              << std::endl;
  cm = spmv::CommunicationModel::p2p_nonblocking;
  ret &= test_spmv(symmetric, cm);
  MPI_Barrier(MPI_COMM_WORLD);

  if (mpi_rank == 0)
    std::cout
        << "Running vanilla SpMV on CPU with one-sided active communication... "
        << std::endl;
  cm = spmv::CommunicationModel::onesided_put_active;
  ret &= test_spmv(symmetric, cm);
  MPI_Barrier(MPI_COMM_WORLD);

  if (mpi_rank == 0)
    std::cout << "Running vanilla SpMV on CPU with one-sided passive "
                 "communication... "
              << std::endl;
  cm = spmv::CommunicationModel::onesided_put_passive;
  ret &= test_spmv(symmetric, cm);
  MPI_Barrier(MPI_COMM_WORLD);

  symmetric = true;
  if (mpi_rank == 0)
    std::cout << "Running symmetric SpMV on CPU with blocking collective "
                 "communication... "
              << std::endl;
  cm = spmv::CommunicationModel::collective_blocking;
  ret &= test_spmv(symmetric, cm);
  MPI_Barrier(MPI_COMM_WORLD);

  if (mpi_rank == 0)
    std::cout << "Running symmetric SpMV on CPU with non-blocking collective "
                 "communication... "
              << std::endl;
  cm = spmv::CommunicationModel::collective_nonblocking;
  ret &= test_spmv(symmetric, cm);
  MPI_Barrier(MPI_COMM_WORLD);

  if (mpi_rank == 0)
    std::cout << "Running symmetric SpMV on CPU with blocking point-to-point "
                 "communication... "
              << std::endl;
  cm = spmv::CommunicationModel::p2p_blocking;
  ret &= test_spmv(symmetric, cm);
  MPI_Barrier(MPI_COMM_WORLD);

  if (mpi_rank == 0)
    std::cout << "Running symmetric SpMV on CPU with non-blocking "
                 "point-to-point communication... "
              << std::endl;
  cm = spmv::CommunicationModel::p2p_nonblocking;
  ret &= test_spmv(symmetric, cm);
  MPI_Barrier(MPI_COMM_WORLD);

  if (mpi_rank == 0)
    std::cout << "Running symmetric SpMV on CPU with one-sided active "
                 "communication... "
              << std::endl;
  cm = spmv::CommunicationModel::onesided_put_active;
  ret &= test_spmv(symmetric, cm);
  MPI_Barrier(MPI_COMM_WORLD);

  if (mpi_rank == 0)
    std::cout << "Running symmetric SpMV on CPU with one-sided passive "
                 "communication... "
              << std::endl;
  cm = spmv::CommunicationModel::onesided_put_passive;
  ret &= test_spmv(symmetric, cm);
  MPI_Barrier(MPI_COMM_WORLD);

  MPI_Finalize();
  return (ret) ? 0 : 1;
}


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<double, 6, 4> test_double_6_4(const Eigen::Matrix<double, 6, 4> & M)
{
	return M;
}
void export_double_6_4()
{
	boost::python::def("test_double_6_4",test_double_6_4);
}


/* vim: set tabstop=4 expandtab shiftwidth=4 softtabstop=4: */

/**
 * \file boost/numeric/ublasx/operation/seq.hpp
 *
 * \brief Create a vector sequence.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright (c) 2010, Marco Guazzone
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_SEQ_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_SEQ_HPP

#include <boost/numeric/ublasx/container/sequence_vector.hpp>
#include <boost/numeric/ublas/storage.hpp>

namespace boost { namespace numeric { namespace ublasx {

using namespace boost::numeric::ublas;

template <typename ValueT, typename SizeT>
sequence_vector<ValueT> seq(ValueT from, SizeT size)
{
    return sequence_vector<ValueT>(from, size);
}

template <typename ValueT, typename StrideT, typename SizeT>
sequence_vector<ValueT, StrideT> seq(ValueT from, StrideT stride, SizeT size)
{
    return sequence_vector<ValueT, StrideT>(from, stride, size);
}

sequence_vector<> seq(sequence_vector<>::value_type from, sequence_vector<>::size_type size)
{
    return sequence_vector<>(from, size);
}

sequence_vector<> seq(sequence_vector<>::value_type from, sequence_vector<>::stride_type stride, sequence_vector<>::size_type size)
{
    return sequence_vector<>(from, stride, size);
}

template <typename SizeT, typename DifferenceT>
sequence_vector<SizeT,DifferenceT> seq(basic_range<SizeT,DifferenceT> const& r)
{
    return sequence_vector<SizeT,DifferenceT>(r);
}

template <typename SizeT, typename DifferenceT>
sequence_vector<SizeT,DifferenceT> seq(basic_slice<SizeT,DifferenceT> const& s)
{
    return sequence_vector<SizeT,DifferenceT>(s);
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_SEQ_HPP


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

#include <iomanip>
#include <cassert>

#include "NPY.hpp"
#include "GLMPrint.hpp"
#include "GLMFormat.hpp"

#include "Camera.hh"
#include "View.hh"

#include "OPTICKS_LOG.hh"
#include "Opticks.hh"
#include "Composition.hh"

#include <boost/math/constants/constants.hpp>

void test_rotate()
{
    glm::vec3 X(1,0,0);
    glm::vec3 Y(0,1,0);
    glm::vec3 Z(0,0,1);

    float angle = 0.f ;
    for(unsigned int i=0 ; i < 6 ; i++)
    {
        switch(i)
        {
            case 0:angle = 0.f ; break;
            case 1:angle = 30.f ; break;
            case 2:angle = 45.f ; break;
            case 3:angle = 60.f ; break;
            case 4:angle = 90.f ; break;
            case 5:angle = 180.f ; break;
        }

        float pi = boost::math::constants::pi<float>() ;
        float a = angle*pi/180. ;
        printf(" angle %10.4f a %10.4f \n", angle, a );

        glm::mat4 rotX = glm::rotate(glm::mat4(1.0), a, X );
        glm::mat4 rotY = glm::rotate(glm::mat4(1.0), a, Y );
        glm::mat4 rotZ = glm::rotate(glm::mat4(1.0), a, Z );

        glm::mat4 irotX = glm::transpose(rotX);
        glm::mat4 irotY = glm::transpose(rotY);
        glm::mat4 irotZ = glm::transpose(rotZ);

        print(rotX, "rotX");
        print(irotX, "irotX");

        print(rotY, "rotY");
        print(irotY, "irotY");

        print(rotZ, "rotZ");
        print(irotZ, "irotZ");

   }
}

void test_center_extent(Opticks* ok)
{
   NPY<float>* dom = NPY<float>::load("domain", "1", "dayabay");
   if(!dom) return ;

   dom->dump();
   glm::vec4 ce = dom->getQuad(0,0);
   print(ce, "ce");

   Composition c(ok) ;
   c.setCenterExtent(ce);
   c.update();
   c.dumpAxisData();
}

void test_depth(Opticks* ok)
{
   Composition* comp = new Composition(ok) ;
   View* view = comp->getView();
   Camera* cam = comp->getCamera();

   float s = 100.0 ;

   glm::vec4 ce(0.,0.,0.,s);

    // extent normalized inputs to view
   view->setEye(-1,0,0) ;
   view->setLook(0,0,0) ;
   view->setUp(0,1,0) ;

   bool autocam ;
   comp->setCenterExtent(ce, autocam=true );

   cam->Summary("test_depth cam");
   view->Summary("test_depth view");

   glm::vec4 vp = comp->getViewpoint();
   glm::vec4 lp = comp->getLookpoint();
   glm::vec4 gaze = comp->getGaze();
   glm::vec4 front = glm::normalize(gaze);

   print(vp, "viewpoint");
   print(lp, "lookpoint");
   print(gaze, "gaze");
   print(front, "front");

   comp->update();

   glm::vec4 zproj ;
   cam->fillZProjection(zproj);

   print(zproj, "zproj");

   //unsigned int ix = cam->getWidth()/2 ;
   //unsigned int iy = cam->getHeight()/2 ;
   float near = cam->getNear();
   float far  = cam->getFar();

   std::cout
        << " near " << near
        << " far " << far
        << std::endl ;

   std::cout << " step along the gaze direction, from near to far  " << std::endl ;
   // (world frame : X axis from -100 to 0)
   // (eye frame   : Z axis from    0 to -100 )

   int N = 20 ;
   for(int i=0 ; i <= N ; i++)
   {
       float t = near + (far - near)*float(i)/float(N)  ;

       const glm::vec4 p_world = vp + front*t ;
       const glm::vec4 p_eye = comp->transformWorldToEye(p_world);
       const glm::vec3 p_ndc = comp->getNDC(p_world);
       const glm::vec3 p_ndc2 = comp->getNDC2(p_world);

       float eyeDist = p_eye.z ;   // from eye frame definition, this is general
       assert(eyeDist <= 0.f );
       float ndc_z = -zproj.z - zproj.w/eyeDist ;     // range -1:1 for visibles
       float ndc_z2 = comp->getNDCDepth(p_world);

       float clip_z = 0.5f*ndc_z + 0.5f ;             // range  0:1
       float depth = clip_z ;
       float clip_z2 = comp->getClipDepth(p_world);

       glm::vec3 unp = comp->unProject(0,0,-depth); // ix,iy ??

       std::cout << " t " << std::setw(5) << t
                 << " p_world " << std::setw(30) << gformat(p_world)
                 << " p_eye   " << std::setw(30) << gformat(p_eye)
                 << " p_ndc " << std::setw(30) << gformat(p_ndc)
                 << " p_ndc2 " << std::setw(30) << gformat(p_ndc2)
                 << " eyeDist " << std::setw(8) << eyeDist
                 << " ndc_z " << std::setw(8) << ndc_z
                 << " ndc_z2 " << std::setw(8) << ndc_z2
                 << " clip_z " << std::setw(8) << clip_z
                 << " clip_z2 " << std::setw(8) << clip_z2
                 << " unp   " << std::setw(20) << gformat(unp)
                 << std::endl ;

   }
}

/*

w2m
   scale+translate 3D object of arbitrary center/extent into unit box

      m2w 500.000   0.000   0.000   0.000
            0.000 500.000   0.000   0.000
            0.000   0.000 500.000   0.000
          100.000 100.000 100.000   1.000

      w2m   0.002   0.000   0.000   0.000
            0.000   0.002   0.000   0.000
            0.000   0.000   0.002   0.000
           -0.200  -0.200  -0.200   1.000

*/

void test_setCenterExtent()
{
    glm::vec4 ce(100.,100.,100.,500.);
    glm::vec3 sc(ce.w);              // scale factor from 1 to extent
    glm::vec3 tr(ce.x, ce.y, ce.z);  // translation from origin to center

    glm::vec3 isc(1.f/ce.w);

    glm::mat4 m_model_to_world = glm::scale( glm::translate(glm::mat4(1.0), tr), sc);

    glm::mat4 m_world_to_model = glm::translate( glm::scale(glm::mat4(1.0), isc), -tr);

    glm::mat4 check = m_world_to_model * m_model_to_world ;

    print(m_model_to_world, "m_model_to_world");
    print(m_world_to_model, "m_world_to_model");
    print(check, "check");

   std::cout << gpresent("m2w", m_model_to_world ) << std::endl ;
   std::cout << gpresent("w2m", m_world_to_model ) << std::endl ;

   std::vector<glm::vec4> world ;
   world.push_back( { ce.x       , ce.y        , ce.z       , 1.0 } );
   world.push_back( { ce.x + ce.w, ce.y + ce.w,  ce.z + ce.w, 1.0 } );
   world.push_back( { ce.x - ce.w, ce.y - ce.w,  ce.z - ce.w, 1.0 } );

   for(unsigned i=0 ; i < world.size() ; i++)
   {
       const glm::vec4& wpos = world[i] ;

       glm::vec4 mpos = m_world_to_model * wpos ;

       std::cout
               << gpresent("w", wpos )
               << gpresent("m", mpos )
               << std::endl ;

   }

}

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

    Opticks ok(argc, argv);
    ok.configure();

   //test_rotate();
   //test_center_extent(&ok);
   test_setCenterExtent();
   //test_depth(&ok);

   return 0 ;
}


#include <iostream>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/variates/covariate.hpp>
#include <boost/accumulators/statistics/tail.hpp>
#include <boost/accumulators/statistics/tail_variate.hpp>

using namespace boost::accumulators;

int main()
{
	//define a feature for tracking covariatel data
	typedef tag::tail_variate<int, tag::covariate1, left> my_tail_variate_tag;

	//this will calculate the left tail and my_tail_variate_tag for n == 2
	//using tag::tail<left::cache_size named paramter

	accumulator_set<double, features<my_tail_variate_tag> > acc(tag::tail<left>::cache_size = 2);
	acc(1.2, covariate1 = 12);
	acc(2.3, covariate1 = -23);
	acc(3.4, covariate1 = 34);
	acc(4.5, covariate1 = -45);

	extractor< my_tail_variate_tag > my_tail_variate;

	//write the tail statistic to std::cout , this will print 4.5 and 3.4
	std::ostream_iterator< double> dout (std::cout, ",");
	std::copy(tail(acc).begin(), tail(acc).end(), dout);
	std::cout<< std::endl;

	//write the tail_variate statistic to std::cout,this will print -45 and 34
	std::ostream_iterator<int> iout(std::cout, ",");
	std::copy(my_tail_variate(acc).begin(), my_tail_variate(acc).end(), iout);
	std::cout<< std::endl;

	return 0;
}


/* Boost check_mkl.cpp test file

 Copyright 2009 Karsten Ahnert
 Copyright 2009 Mario Mulansky

 This file tests the odeint library with the intel mkl blas1 routines

 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)
*/

#define BOOST_TEST_MODULE test_mkl

#include <boost/test/unit_test.hpp>

#include <boost/numeric/odeint/stepper/euler.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta4.hpp>
#include <boost/numeric/odeint/algebra/vector_space_algebra.hpp>
#include <boost/numeric/odeint/external/mkl/mkl_operations.hpp>

using namespace boost::numeric::odeint;

typedef double value_type;
typedef boost::array< value_type , 1 > state_type;

void constant_system( state_type &x , state_type &dxdt , value_type t )
{
    dxdt[0] = 1.0;
}

const double eps = 1E-14;

BOOST_AUTO_TEST_CASE( test_mkl )
{

    //to use mkl routines we have to use the vector_space_algebra and the mkl_operations
    runge_kutta4< state_type , value_type , state_type , value_type , vector_space_algebra , mkl_operations > stepper;
    state_type x;
    x[0] = 0.0;

    stepper.do_step( constant_system , x , 0.0 , 0.1 );

    using std::abs;

    std::cout << x[0] << " ?= " << 0.1 << std::endl;
    BOOST_CHECK_SMALL( abs( x[0] - 0.1 ) , eps );

}


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

/** @file
 * Tests that exercise the functionality of methods we define related to rigid transforms.
 */

#include <Eigen/Core>
#include <Eigen/Geometry>
#include "SurgSim/Math/RigidTransform.h"
#include "SurgSim/Math/Quaternion.h"
#include "gtest/gtest.h"

template <class T>
class MakeRigidTransform3Tests : public testing::Test
{
public:
	typedef T RigidTransform;
};

typedef ::testing::Types<SurgSim::Math::RigidTransform3d,
						 SurgSim::Math::RigidTransform3f> RigidTransform3Variants;
TYPED_TEST_CASE(MakeRigidTransform3Tests, RigidTransform3Variants);

template <class T>
class MakeAllRigidTransformTests : public testing::Test
{
public:
	typedef T RigidTransform;
};

typedef ::testing::Types<SurgSim::Math::RigidTransform2d,
						 SurgSim::Math::RigidTransform2f,
						 SurgSim::Math::RigidTransform3d,
						 SurgSim::Math::RigidTransform3f> AllRigidTransformVariants;
TYPED_TEST_CASE(MakeAllRigidTransformTests, AllRigidTransformVariants);

/// Test makeRigidTransform using a rotation matrix and a translation
TYPED_TEST(MakeAllRigidTransformTests, MakeRigidTransformWithMatrix)
{
	typedef typename TestFixture::RigidTransform RigidTransform;
	typedef typename RigidTransform::LinearMatrixType RotationMatrix;
	typedef typename RigidTransform::TranslationType::VectorType Translation;
	const int DIM = RigidTransform::Dim;

	RotationMatrix rotationMatrix = RotationMatrix::Identity();
	rotationMatrix.row(0).swap(rotationMatrix.row(DIM-1));
	Translation translation(Translation::Random());
	RigidTransform transform = SurgSim::Math::makeRigidTransform(rotationMatrix, translation);

	typename RigidTransform::MatrixType matrix = transform.matrix();
	EXPECT_TRUE(rotationMatrix.isApprox(matrix.block(0,0,DIM,DIM), 1e-6f)) <<
		"Rotation part of transform is not properly set";
	EXPECT_TRUE(translation.isApprox(matrix.block(0,DIM,DIM,1), 1e-6f)) <<
		"Translation part of transform is not properly set";
	EXPECT_NEAR(1.0, matrix(DIM,DIM), 1e-6) <<
		"Transform matrix is not 1.0 in bottom right corner";
	EXPECT_TRUE(matrix.block(DIM,0,1,DIM).isApproxToConstant(0.0, 1e-6f)) <<
		"Bottom row of matrix is not all zeros (except for last column).";
}

/// Test makeRigidTransform using a quaternion and a translation
TYPED_TEST(MakeRigidTransform3Tests, MakeRigidTransformWithQuaternion)
{
	typedef typename TestFixture::RigidTransform RigidTransform;
	typedef typename RigidTransform::TranslationType::VectorType Translation;
	typedef typename RigidTransform::Scalar Scalar;
	typedef typename Eigen::Quaternion<Scalar> Quaternion;
	typedef typename RigidTransform::LinearMatrixType RotationMatrix;
	const int DIM = RigidTransform::Dim;

	Translation translation = Translation::Random();
	Quaternion quaternion(0.0, 1.0, 0.0, 0.0);
	RotationMatrix quaternionRotationMatrix;
	quaternionRotationMatrix << 1.0, 0.0, 0.0,
								0.0,-1.0, 0.0,
								0.0, 0.0,-1.0;
	RigidTransform transform = SurgSim::Math::makeRigidTransform(quaternion, translation);

	typename RigidTransform::MatrixType matrix = transform.matrix();
	EXPECT_TRUE(quaternionRotationMatrix.isApprox(matrix.block(0,0,DIM,DIM), 1e-6f)) <<
		"Rotation part of transform is not properly set";
	EXPECT_TRUE(translation.isApprox(matrix.block(0,DIM,DIM,1), 1e-6f)) <<
		"Translation part of transform is not properly set";
	EXPECT_NEAR(1.0, matrix(DIM,DIM), 1e-6) <<
		"Transform matrix is not 1.0 in bottom right corner";
	EXPECT_TRUE(matrix.block(DIM,0,1,DIM).isApproxToConstant(0.0, 1e-6f)) <<
		"Bottom row of matrix is not all zeros (except for last column).";
}


/*
 * Cuboid.cpp
 *
 *  Created on: 08 feb 2018
 *      Author: lorenzo
 */

#include "Cuboid.h"

#include <Eigen/Eigen>

namespace ashell {

Cuboid::Cuboid(double L) :
				Box(),
				_box(L, L, L) {

}

Cuboid::Cuboid(double Lx, double Ly, double Lz) :
				Box(),
				_box(Lx, Ly, Lz) {

}

Cuboid::~Cuboid() {

}

std::string Cuboid::as_string() {
	std::string box_string;
	if(_box(0) == _box(1) && _box(1) == _box(2)) {
		box_string = boost::str(boost::format("cube %lf") % _box(0));
	}
	else {
		box_string = boost::str(boost::format("cuboid %lf %lf %lf") % _box(0) % _box(1) % _box(2));
	}

	return box_string;
}

vec3 Cuboid::random_point_in_box() {
	// Random() returns numbers between -1 and 1
	return _box.array() * ((vec3::Random() + vec3(1., 1., 1.)) * 0.5).array();
}

vec3 Cuboid::minimum_image(const vec3 &p, const vec3 &q) {
	vec3 diff = q - p;
	return diff - ((diff.array() / _box.array()).round() * _box.array()).matrix();
}

} /* namespace ashell */

#ifdef ASHELL_PYTHON
#include "../defs_to_python.h"

namespace ashell {

void export_cuboid() {
	// Cuboid inherits from a noncopyable parent (Box), which means it is non-copyable itself
	bpy::class_<Cuboid, std::shared_ptr<Cuboid>, bpy::bases<Box>, boost::noncopyable>("Cuboid", bpy::init<double>())
			.def(bpy::init<double, double, double>());
}

}
#endif


// Std includes
#include <cmath>
#include <iostream>
#include <memory>
// Thirdparties includes
#include <Eigen/Dense>
// Lib includes
#include "s0s/runge_kutta_fehlberg.h"
#include "sl0/point.h"
#include "sl0/chain/dynamic.h"
// Simple includes
#include "flow.h"

using TypeScalar = double;
// State
template<int Size>
using TypeVector = Eigen::Matrix<TypeScalar, Size, 1>;
// Space
constexpr unsigned int DIM = 2;
using TypeSpaceVector = Eigen::Matrix<TypeScalar, DIM, 1>;
// Ref and View
template<typename ...Args>
using TypeRef = Eigen::Ref<Args...>;
template<typename ...Args>
using TypeView = Eigen::Map<Args...>;
// Group Parameters
using TypeStepPoint = sl0::StepPoint<TypeVector, DIM, TypeView, Flow>;
// Solver
using TypeSolver = s0s::SolverRungeKuttaFehlberg<TypeVector<-1>, TypeView>;

const unsigned int np = 11;

int main () {
    // Parameters
    TypeSpaceVector x0 = TypeSpaceVector::Zero();
    TypeScalar t0 = 0.0;
    TypeScalar dt = 1e-3;
    unsigned int nt = std::round(1.0 / dt);
    double dl = 0.1;
    double l = 1.0;
    // Create chain
    std::shared_ptr<TypeStepPoint> sStepPoint = std::make_shared<TypeStepPoint>(std::make_shared<Flow>());
    sl0::ChainDynamic<TypeVector, DIM, TypeView, TypeRef, TypeStepPoint, TypeSolver> chain(sStepPoint, dl, 4);
    // Init
    for(std::size_t i = 0; i < np; i++) {
        chain.sStep->addMember(chain.state);
        sStepPoint->x(chain.sStep->memberState(chain.state.data(), i)) = x0;
        sStepPoint->x(chain.sStep->memberState(chain.state.data(), i))[0] += i * dl;
    }
    std::cout << "Init Length : " << "\n" << chain.sStep->length(chain.state.data()) << "\n";
    std::cout << "Init Size : " << "\n" << chain.sStep->size() << "\n";
    chain.t = t0;
    // Computation
    std::cout << "Computing" << "\n";
    for(std::size_t i = 0; i < nt; i++) {
        chain.update(dt);
    }
    // out
    std::cout << "\n";
    std::cout << "Chain advected following a an exponential flow, exp(" << chain.t << ") = " << "\n";
    std::cout << "\n";
    std::cout << "Final Length : " << "\n" << chain.sStep->length(chain.state.data()) << "\n";
    std::cout << "Final Size : " << "\n" << chain.sStep->size() << "\n";
    std::cout << std::endl;
}


//#ifndef AMPAR_GILLESPIE_CLASS_HPP_INCLUDED
//#define AMPAR_GILLESPIE_CLASS_HPP_INCLUDED

#include <iostream>
#include <stdlib.h>
#include <cmath>
#include <vector>
#include <ctime>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <boost/numeric/odeint.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/generator_iterator.hpp>
#include <boost/random/linear_congruential.hpp>

#include "astron_utility_functions.hpp"
#include "stl_vector_operation_functions.hpp"
#include "boost_vector_matrix_operation_functions.hpp"

namespace bno=boost::numeric::odeint;
namespace bnu=boost::numeric::ublas;

typedef double value_type;
typedef bnu::matrix< value_type > matrix_type;

class stochastic_ampar
{
   private:
      unsigned int seed;
      boost::minstd_rand rng;   //Boost random number generator
      boost::uniform_real<double> uni_dist;   //Uniform random number distribution which produces values b/w 0 and 1 (0 inclusive, 1 exlusive).
      boost::variate_generator<boost::minstd_rand&, boost::uniform_real< double> > uni_rand;// variate generator
      //--------------
      double kf1 = 4.59E06;    //M^-1 S^-1
      double kb1 = 4.26E03;    //S^-1
      double kf2 = 28.4E06;    //M^-1 * S^-1
      double kb2 = 3.26E03;    //S^-1
      double kf3 = 1.27E06;    //M^-1 S^-1
      double kb3 = 45.7;       //S^-1
      double a0 = 4.24E03;     //S^-1
      double b0 = 900.0;       //S^-1
      double a1 = 2.89E03;     //S^-1
      double b1 = 39.2;        //S^-1
      double a2 = 172.0;       //S^-1
      double b2 = 0.727;       //S^-1
      double a3 = 17.7;        //S^-1
      double b3 = 4.0;         //S^-1
      double a4 = 16.8;        //S^-1
      double b4 = 190.4;       //S^-1
      double Volume = 1.0;
   public:
      //Construct vectors
      matrix_type X;       // A matrix to hold the states
      matrix_type P;       // A matrix that holds the probability at time t
      matrix_type T;       // The transition probability matrix
      matrix_type Q;       // The infinitly small matrix, Q matrix
      /* stochastic_ampar class constructor */
      stochastic_ampar(matrix_type X_, unsigned int seed_=std::time(0)):   seed(seed_),
                                                                        uni_dist(boost::uniform_real<double>(0,1)),
                                                                        uni_rand(boost::variate_generator<boost::minstd_rand&, boost::uniform_real<double> >(rng, uni_dist)),
                                                                        X(X_)
      {
         rng.seed(seed);
         std::vector<double> Tv;
         Tv = {
      };
      //------------------ Function declarations
      void operator() (  matrix_type &p, matrix_type &dpdt , const double  t )
      {
         dpdt = bnu::prod(p,Q);
      };
};
int main(int argc, char **argv)
{
   matrix_type mat(3,2);
   mat(2,1) = 32;
   stochastic_ampar test(mat);
   std::cout << test.X << std::endl;
}