#include <boost/dynamic_bitset.hpp>
#include <algorithm>
#include <iostream>
#include <sstream>
#include <cstring>
#include <cstdlib>
#include <climits>
#include <cmath>
#include <bitset>
#include <vector>
#include <queue>
#include <stack>
#include <set>
#include <map>

#define REP(i,a) for(int i=0;i<(a);i++)
#define FOR(i,a,b) for(int i=(a);i<(b);i++)
#define MAX(a,b) ((a)>(b)?(a):(b))
#define MAX3(a,b,c) MAX(MAX(a,b),c)
#define MAX4(a,b,c,d) MAX(MAX3(a,b,c),d)
#define MIN(a,b) ((a)<(b)?(a):(b))
#define MIN3(a,b,c) MIN(MIN(a,b),c)
#define MIN4(a,b,c,d) MIN(MIN3(a,b,c),d)
#define SZ size()
#define PB push_back

const int oo = INT_MAX>>1;
const int N = 1000000000;

using namespace std;
using namespace boost;

typedef vector<int> VE;
typedef dynamic_bitset<> SET;

vector<bool> prime(N,false);
VE mem1(1<<10,-1);
VE mem2(1<<10,-1);

int numways(SET &s);

bool good(int n) {
    int mod;
    SET s(0);
    while (n != 0) {
        mod = n%10;
        n /= 10;
        if (mod==0) return false;
        if (s[mod]) return false;
        s[mod] = true;
    }
    return true;
}

int possible(SET &s) {
    unsigned long test = s.to_ulong();
    if (mem1[test] != -1) return mem1[test];
    VE perms;
    int total = 0;
    REP(i,s.SZ) if (s[i]) perms.PB(i);
    do {
        int num = 0;
        REP (i,perms.SZ) num = num*10 + perms[i];
        if (prime[num]) total++;
    } while (next_permutation(perms.begin(),perms.end()));
//    cout << s << " " << total << endl;
    return mem1[test] = total;
}

int allsubs(SET &start, SET &cur, int i) {
    if (i == 10) {
        int poss = possible(cur);
        if (poss > 0) {
            SET diff = start-cur;
            int ret = poss*numways(diff);
//            cout << diff << " " << poss << " " << ret << endl;
            return ret;
        }
        return 0;
    }
    if (!cur[i]) return allsubs(start,cur,i+1);
    int tot = 0;
    cur[i] = 0;
    tot += allsubs(start,cur,i+1);
    cur[i] = 1;
    tot += allsubs(start,cur,i+1);
    return tot;
}

int numways(SET &s) {
    unsigned long test = s.to_ulong();
    if (test == 4 || test == 8 || test == 32 || test == 128) return 1;
    if (mem2[test] != -1) return mem2[test];
    SET scpy = s;
    return mem2[test] = allsubs(s,scpy,0)/2;
}

SET makeset(int p) {
    SET s(10,0);
    while (p != 0) {
        s[p%10] = true;
        p /= 10;
    }
    return s;
}

int main() {
    int n;
//    prime[0] = prime[1] = false;
//    for (int i = 2; i*i <= N; i++) {
//        if (!prime[i]) continue;
//        for (int j = i<<1; j < N; j += i) prime[j] = false;
//    }
//    int count = 0;
//    REP(i,N) if (prime[i] && good(i)) {
//        cout << i << endl;
//        count++;
//    }
//    cout << count << endl;
    VE parr;
    while (cin >> n) {
        parr.PB(n);
    }
    vector<SET> arr;
    SET digits(10,(1<<10)-2);
//    SET next(10,0);
    //next[6] = next[3] = next[1] = true;
//    next[1] = next[2] = next[3] = next[5] = true;
//    cout << numways(next) << endl;
//    cout << numways(digits) << endl;
    SET empty(10,0);
    arr.PB(digits);
    cout << parr.SZ << endl;
    int total = 0;
    REP(i,parr.SZ) {
        cout << i << endl;
        SET test = makeset(parr[i]);
        int cursz = arr.SZ;
        REP(j,cursz) {
            if (test.is_subset_of(arr[j])) {
                SET diff = arr[j]-test;
                if (diff == empty) total++;
                else arr.PB(diff);
            }
        }
    }
    cout << total << endl;
    return 0;
}


#include <boost/math/special_functions/factorials.hpp>
#include "gauss_distribution.hpp"

namespace bayesopt
{

  GaussianDistribution::GaussianDistribution(randEngine& eng):
    ProbabilityDistribution(eng)
  {
    mean_ = 0.0;  std_ = 1.0;
  }

  GaussianDistribution::~GaussianDistribution(){}

  double GaussianDistribution::negativeExpectedImprovement(double min,
							   size_t g)
  {

    using boost::math::factorial;

    const double diff = min - mean_;
    const double z = diff / std_;
    const double pdf_z = boost::math::pdf(d_,z);
    const double cdf_z = boost::math::cdf(d_,z);

    if (g == 1)
      return -1.0 * ( diff * cdf_z + std_ * pdf_z );
    else
      {
	const double fg = factorial<double>(g);

	double Tm2 = cdf_z;
	double Tm1 = pdf_z;
	double sumEI = pow(z,static_cast<double>(g))*Tm2 - g*pow(z,static_cast<double>(g-1))*Tm1;

	for (size_t ii = 2; ii < g; ++ii)
	  {
	    double Tact = (ii-1)*Tm2 - pdf_z*pow(z,static_cast<double>(ii-1));
	    sumEI += pow(-1.0,static_cast<double>(ii))*
	      (fg / ( factorial<double>(ii)*factorial<double>(g-ii) ) )*
	      pow(z,static_cast<double>(g-ii))*Tact;

	    //roll-up
	    Tm2 = Tm1;   Tm1 = Tact;
	  }
	return -1.0 * pow(std_,static_cast<double>(g)) * sumEI;
      }

  }  // negativeExpectedImprovement

  double GaussianDistribution::lowerConfidenceBound(double beta)
  {
    return mean_ - beta*std_;
  }  // lowerConfidenceBound

  double GaussianDistribution::negativeProbabilityOfImprovement(double min,
								double epsilon)
  {
    return -cdf(d_,(min - mean_ + epsilon)/std_);
  }  // negativeProbabilityOfImprovement

  double GaussianDistribution::sample_query()
  {
    randNFloat sample(mtRandom,normalDist(mean_,std_));
    return sample();
  } // sample_query

}


#include <iostream>
#include <array>
#include <vector>
#include <fstream>
#include <cmath>
#include <iterator>

#include <boost/iterator/transform_iterator.hpp>
#include <boost/iterator/function_input_iterator.hpp>
#include <boost/log/trivial.hpp>

#include <range/v3/algorithm.hpp>

#include "maikel/hmm/hidden_markov_model.h"
#include "maikel/hmm/algorithm.h"
#include "maikel/hmm/io.h"
#include "maikel/function_profiler.h"

enum Exit_Error_Codes {
  exit_success = 0,
  exit_not_enough_arguments = 1,
  exit_io_error = 2,
  exit_argument_error = 3
};

template <class float_type, class index_type>
  void accumulate_scaling_and_write_alpha_to_file(
      std::vector<index_type> const& sequence,
      maikel::hmm::hidden_markov_model<float_type> const& model)
  {
    std::size_t states = model.states();
//    std::vector<Eigen::RowVectorXd> alphas(T, Eigen::RowVectorXd(states));
    std::ofstream alphas("alphas.bin", std::ofstream::binary);
    BOOST_LOG_TRIVIAL(info) << "Starting forward algorithm with storing scaling factors into std::vector.";
    BOOST_LOG_TRIVIAL(info) << "Use accumulate on a view::transformed scaling list.";
    float_type logprob = 0.0;
    std::size_t datalen = sizeof(float_type)*states;
    { MAIKEL_PROFILER;
      std::ostreambuf_iterator<char> out(alphas);
      for (auto&& scaled_alpha : maikel::hmm::forward(sequence, model)) {
        logprob += std::log(scaled_alpha.first);
        std::copy_n(reinterpret_cast<const char*>(scaled_alpha.second.data()), datalen, out);
      }
    }
    std::cout << -logprob << std::endl;
  }

template <class T>
void read_alphas_from_bin(const maikel::hmm::hidden_markov_model<T>& hmm)
{
  MAIKEL_PROFILER;
  std::ifstream alphas("alphas.bin", std::ifstream::binary);
  Eigen::Matrix<T, 1, Eigen::Dynamic> alpha(hmm.states());
  std::size_t data_len = sizeof(T)*alpha.size();
  std::istreambuf_iterator<char> in(alphas), end;
  while (in != end) {
    std::copy_n(in, data_len, reinterpret_cast<char*>(alpha.data()));
    std::advance(in, data_len+1);
  }
}

int main(int argc, char *argv[])
{
  using namespace std;
  using namespace maikel::hmm;

  if (argc < 3) {
    cerr << "Usage: " << argv[0] << " <model.dat> <sequence.dat>\n";
    return exit_not_enough_arguments;
  }
  using float_type = double;
  using index_type = uint8_t;

  // read model
  ifstream model_input(argv[1]);
  auto model = read_hidden_markov_model<float_type>(model_input);

  vector<int> symbols { 0,1 };
  map<int,index_type> symbol_to_index = maikel::map_from_symbols<index_type>(symbols);
  ifstream sequence_input(argv[2]);
  vector<index_type> sequence = read_sequence(sequence_input, symbol_to_index);

  {
    MAIKEL_NAMED_PROFILER("v2::forward");
    float_type scaling = 0;
    for (auto&& alpha : forward(begin(sequence), end(sequence), model)) {
      scaling += log(alpha.first);
    }
    cout << -scaling << endl;
  }
  maikel::function_profiler::print_statistics(cout);

  return exit_success;
}


// Copyright (C) 2015 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// 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 HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/pose/seven_point_fundamental_matrix.h"

#include <Eigen/Core>
#include <Eigen/LU>
#include <glog/logging.h>
#include <vector>

#include "theia/math/polynomial.h"
#include "theia/sfm/pose/util.h"

namespace theia {

using Eigen::Matrix;

namespace {

// Sets up the constraint y^t * F * x = 0 such that M * F_v = 0 where M is a 7x9
// matrix and F_v is the vector containing the entries of F.
Matrix<double, 7, 9> SetupEpipolarConstraint(
    const std::vector<Eigen::Vector2d>& image1_points,
    const std::vector<Eigen::Vector2d>& image2_points) {
  Matrix<double, 7, 9> epipolar_constraint;
  for (int i = 0; i < 7; i++) {
    // Fill matrix with the epipolar constraint from q'_t*E*q = 0. Where q is
    // from the first image, and q' is from the second.
    epipolar_constraint.row(i) <<
        image2_points[i].x() * image1_points[i].x(),
        image2_points[i].y() * image1_points[i].x(),
        image1_points[i].x(),
        image2_points[i].x() * image1_points[i].y(),
        image2_points[i].y() * image1_points[i].y(),
        image1_points[i].y(),
        image2_points[i].x(),
        image2_points[i].y(),
        1.0;
  }

  return epipolar_constraint;
}

}  // namespace

bool SevenPointFundamentalMatrix(
    const std::vector<Eigen::Vector2d>& image1_points,
    const std::vector<Eigen::Vector2d>& image2_points,
    std::vector<Eigen::Matrix3d>* fundamental_matrices) {
  CHECK_EQ(image1_points.size(), 7);
  CHECK_EQ(image2_points.size(), 7);
  CHECK_NOTNULL(fundamental_matrices)->clear();

  std::vector<Eigen::Vector2d> norm_img1_points(image1_points.size());
  std::vector<Eigen::Vector2d> norm_img2_points(image2_points.size());

  // Normalize the image points.
  Eigen::Matrix3d img1_norm_mat, img2_norm_mat;
  NormalizeImagePoints(image1_points, &norm_img1_points, &img1_norm_mat);
  NormalizeImagePoints(image2_points, &norm_img2_points, &img2_norm_mat);

  const Matrix<double, 7, 9>& epipolar_constraint =
      SetupEpipolarConstraint(norm_img1_points, norm_img2_points);

  const Eigen::FullPivLU<Matrix<double, 7, 9> > lu(epipolar_constraint);
  if (lu.dimensionOfKernel() != 2) {
    return false;
  }

  // Represent F in terms of its null space such that F = x * F1' + (1 - x) * F2
  // where F1 and F2 are vectors in the null space of F. Note that this can also
  // be parameterized such that:
  //   F = x * F1' + (1 - x) * F2 = x * (F1' - F2) + F2 = x * F1 + F2.
  const Matrix<double, 9, 2>& null_space = lu.kernel();
  const Matrix<double, 9, 1> F1_vec = null_space.col(0) - null_space.col(1);
  const Eigen::Map<const Eigen::Matrix3d> F1(F1_vec.data());
  const Eigen::Map<const Eigen::Matrix3d> F2(null_space.col(1).data());

  // This is the cubic equation resulting from det(x * F1 + F2) = 0.
  Eigen::VectorXd determinant_constraint(4);
  determinant_constraint(0) =
      -(F2(1, 2) * F2(2, 1) - F2(1, 1) * F2(2, 2)) * F2(0, 0) +
      (F2(0, 2) * F2(2, 1) - F2(0, 1) * F2(2, 2)) * F2(1, 0) -
      (F2(0, 2) * F2(1, 1) - F2(0, 1) * F2(1, 2)) * F2(2, 0);
  determinant_constraint(1) =
      -(F2(1, 2) * F2(2, 1) - F2(1, 1) * F2(2, 2)) * F1(0, 0) +
      (F2(0, 2) * F2(2, 1) - F2(0, 1) * F2(2, 2)) * F1(1, 0) -
      (F2(0, 2) * F2(1, 1) - F2(0, 1) * F2(1, 2)) * F1(2, 0) +
      (F1(2, 2) * F2(1, 1) - F1(2, 1) * F2(1, 2) - F1(1, 2) * F2(2, 1) +
       F1(1, 1) * F2(2, 2)) *
          F2(0, 0) -
      (F1(2, 2) * F2(0, 1) - F1(2, 1) * F2(0, 2) - F1(0, 2) * F2(2, 1) +
       F1(0, 1) * F2(2, 2)) *
          F2(1, 0) +
      (F1(1, 2) * F2(0, 1) - F1(1, 1) * F2(0, 2) - F1(0, 2) * F2(1, 1) +
       F1(0, 1) * F2(1, 2)) *
          F2(2, 0);
  determinant_constraint(2) =
      (F1(2, 2) * F2(1, 1) - F1(2, 1) * F2(1, 2) - F1(1, 2) * F2(2, 1) +
       F1(1, 1) * F2(2, 2)) *
          F1(0, 0) -
      (F1(2, 2) * F2(0, 1) - F1(2, 1) * F2(0, 2) - F1(0, 2) * F2(2, 1) +
       F1(0, 1) * F2(2, 2)) *
          F1(1, 0) +
      (F1(1, 2) * F2(0, 1) - F1(1, 1) * F2(0, 2) - F1(0, 2) * F2(1, 1) +
       F1(0, 1) * F2(1, 2)) *
          F1(2, 0) -
      (F1(1, 2) * F1(2, 1) - F1(1, 1) * F1(2, 2)) * F2(0, 0) +
      (F1(0, 2) * F1(2, 1) - F1(0, 1) * F1(2, 2)) * F2(1, 0) -
      (F1(0, 2) * F1(1, 1) - F1(0, 1) * F1(1, 2)) * F2(2, 0);
  determinant_constraint(3) =
      -(F1(1, 2) * F1(2, 1) - F1(1, 1) * F1(2, 2)) * F1(0, 0) +
      (F1(0, 2) * F1(2, 1) - F1(0, 1) * F1(2, 2)) * F1(1, 0) -
      (F1(0, 2) * F1(1, 1) - F1(0, 1) * F1(1, 2)) * F1(2, 0);

  // Solve the cubic equation for x.
  Eigen::VectorXd roots;
  FindPolynomialRoots(determinant_constraint, &roots, NULL);

  for (int i = 0; i < roots.size(); i++) {
    // Compose the fundamental matrix solution from the null space and
    // determinant constraint: F = x * F1 + F2;
    fundamental_matrices->emplace_back(img2_norm_mat.transpose() *
                                       (roots(i) * F1 + F2) * img1_norm_mat);
  }
  return fundamental_matrices->size() > 0;
}

}  // namespace theia


/// \file   interval.hpp
///
/// \brief
///
/// \authors    Maarten P. Scholl
/// \date       2018-01-31
/// \copyright  Copyright 2017-2019 The Institute for New Economic Thinking,
///             Oxford Martin School, University of Oxford
///
///             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.
///
///             You may obtain instructions to fulfill the attribution
///             requirements in CITATION.cff
///
#ifndef ESL_MATHEMATICS_INTERVAL_HPP
#define ESL_MATHEMATICS_INTERVAL_HPP

#include <sstream>
#include <type_traits>

#include <boost/serialization/serialization.hpp>

namespace esl::mathematics {
    ///
    /// \brief  A set of numbers lying between a lower and upper endpoint. The
    ///         endpoints may be included in the set or not, if the lower is
    ///         included the interval is left_closed_, and if the upper is
    ///         included  the interval is right_closed_.
    ///
    /// \tparam number_t_
    /// \tparam left_closed_
    /// \tparam right_closed_
    template<typename number_t_,
             bool left_closed_  = true,
             bool right_closed_ = true>
    struct interval
    {
        ///
        /// \brief Specifies whether the interval includes the lower value
        ///
        /// \return
        constexpr static bool left_closed()
        {
            return left_closed_;
        }

        ///
        /// \brief Specifies whether the interval includes the upper value
        ///
        /// \return
        constexpr static bool right_closed()
        {
            return right_closed_;
        }

        static_assert(std::is_floating_point<number_t_>::value
                      // TODO: || esl::is_rational<number_t_>::value
                      || std::is_integral<number_t_>::value);

        number_t_ lower;
        number_t_ upper;

        ///
        /// \brief  default interval constructor sets the lower and upper bound
        ///         to time_point(0)
        ///
        constexpr interval()
        : lower(0)
        , upper(0)
        {

        }

        ///
        /// \brief  constructs an interval from `lower` to `upper`
        ///
        /// \param lower    lower bound
        /// \param upper    upper bound
        constexpr interval(number_t_ lower, number_t_ upper)
        : lower(lower)
        , upper(upper)
        {

        }

        ///
        /// \return true iff the interval contains no elements
        ///
        [[nodiscard]] constexpr bool empty() const
        {
            if(lower > upper) {
                return true;
            }

            if(lower == upper) {
                return left_closed_ || right_closed_;
            }

            // (lower < upper) is implied
            if(1 == upper - lower){
               return left_closed_ && right_closed_;
            }

            return false;
        }

        ///
        /// \return true iff interval contains exactly one element
        ///
        [[nodiscard]] constexpr bool singleton() const
        {
            bool sufficient_ = !left_closed_ && !right_closed_ && lower == upper;
            if(std::is_floating_point<number_t_>::value || sufficient_) {
                // || TODO: esl::is_rational<number_t_>::value
                return sufficient_;
            }

            // is_integral is implied from here
            bool asymmetric_ =
                (1 == upper - lower) && (left_closed_ != right_closed_);
            bool symmetric_ =
                (2 == upper - lower) && (left_closed_ && right_closed_);
            return (upper > lower) && (asymmetric_ || symmetric_);
        }

        ///
        /// \return true iff the interval is singleton or empty
        ///
        [[nodiscard]] constexpr bool degenerate() const
        {
            return empty() || singleton();
        }

        ///
        /// \param value element to test
        /// \return true iff element is in contained in interval
        ///
        [[nodiscard]] constexpr bool contains(number_t_ value) const
        {
            return (lower < value || (left_closed_ && lower == value))
                   && (upper > value || (right_closed_ && upper == value));
        }

        ///
        /// \brief  renders the interval to a string as detailed in the class's
        ///         ostream operator implementation
        ///
        /// \return
        [[nodiscard]] std::string representation() const
        {
            std::stringstream stream_;
            stream_ << *this;
            return stream_.str();
        }

        ///
        /// \brief  renders the interval using '[' and ']' to denote open lower
        ///         and upper bounds respectively, and '(' and ')' for closed
        ///         lower and upper bounds.
        ///
        /// \param stream
        /// \param self
        /// \return
        friend std::ostream &
        operator<<(std::ostream &stream,
                   const interval<number_t_, left_closed_, right_closed_> &self)
        {
            stream << (left_closed_ ? '[' : '(');
            stream << self.lower << ',' << self.upper;
            stream << (right_closed_ ? ']' : ')');
            return stream;
        }

        ///
        /// \tparam archive_t
        /// \param archive
        /// \param version
        template<class archive_t>
        void serialize(archive_t &archive, const unsigned int version)
        {
            (void)version;
            archive &BOOST_SERIALIZATION_NVP(lower);
            archive &BOOST_SERIALIZATION_NVP(upper);
        }
    };
}  // namespace esl

#endif  // ESL_MATHEMATICS_INTERVAL_HPP


/* Copyright 2018 Ignacio Torroba (ignaciotb@kth.se)
 *
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
 *
 * 2. 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.
 *
 * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef UTILS_MATRICES_HPP
#define UTILS_MATRICES_HPP

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>
#include <boost/numeric/ublas/operation_blocked.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_expression.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>

#include <boost/scoped_ptr.hpp>

#include <boost/math/distributions/chi_squared.hpp>
#include <boost/math/distributions/inverse_chi_squared.hpp>

namespace matrices{

    template<typename T>
    T matDeterminant(const boost::numeric::ublas::matrix<T>& mat_A){
        using namespace boost::numeric::ublas;
        matrix<T> mLu(mat_A);
        permutation_matrix<std::size_t> pivots(mat_A.size1());

        auto isSingular = lu_factorize(mLu, pivots);
        if (isSingular){
            return static_cast<T>(0);
        }

        T det = static_cast<T>(1);
        for (std::size_t i = 0; i < pivots.size(); ++i){
            if (pivots(i) != i){
                det *= static_cast<T>(-1);
            }
            det *= mLu(i, i);
        }
        return det;
    }

    template<typename T>
    bool InvertMatrix (const boost::numeric::ublas::matrix<T>& input, boost::numeric::ublas::matrix<T>& inverse) {
        using namespace boost::numeric::ublas;
        matrix<T> A(input);
        // Perform LU-factorization
        permutation_matrix<std::size_t> pm(A.size1());
        int res = lu_factorize(A,pm);
        if( res != 0 )
            return false;
        inverse.assign(identity_matrix<T>(A.size1()));
        lu_substitute(A, pm, inverse);
        return true;
    }

    template<typename T>
    boost::numeric::ublas::matrix<T> Cholesky(const boost::numeric::ublas::matrix<T>& mat_A){
        // TODO_NACHO: check for matrix conditions to use cholesky
        int n = mat_A.size1();
        boost::numeric::ublas::matrix<T> chol_triang(n, n);
        for(unsigned int i=0; i< mat_A.size1(); i++){
            for(unsigned int j=0; j< i + 1; j++){
                double s = 0;
                for(unsigned int k = 0; k<j; k++){
                    s += chol_triang(i * n + k) * chol_triang(j * n + k);
                }
                chol_triang(i * n + j) = (i = j)?
                            std::sqrt(mat_A(i * n + i) - s):
                            (1.0 / chol_triang(j * n + j) * (mat_A(i * n + j) - s));
            }
        }
        return chol_triang;
    }

    template<typename T>
    boost::numeric::ublas::matrix<T> matTriangDeterminant(const boost::numeric::ublas::matrix<T>& mat_A){
        int n = mat_A.size1();
        T det;
        for(unsigned int i=0; i< mat_A.size1(); i++){
            for(unsigned int j=0; j< mat_A.size2(); j++){
                det *= (i == j)? mat_A(i,j): 1;
            }
        }
        return det;
    }
}

#endif // UTILS_MATRICES_HPP


/*
 * resampling.hpp
 *
 *  Created on: Mar 28, 2019
 *      Author: Gregory Kramida
 *   Copyright: 2019 Gregory Kramida
 *
 *   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.
 */

#pragma once

//libraries
#include <Eigen/Dense>
#include <unsupported/Eigen/CXX11/Tensor>

namespace math {

/**
 * Different strategies for upsampling a discrete field
 */
enum class UpsamplingStrategy {
	NEAREST = 0, ///@see upsampleX2_nearest for details
	LINEAR = 1 ///@see upsampleX2_linear for details
};

/**
 * Different strategies for downsampling a discrete field
 */
enum class DownsamplingStrategy {
	AVERAGE = 0, ///@see downsampleX2_average for details
	LINEAR = 1 ///@see downsampleX2_linear for details
};

/**
 * Upsample the field such that the output is a field 2X larger than the original in each dimension.
 * @param field input field
 * @param upsampling_strategy -- which upsampling strategy to use.
 * @return upsampled field
 */
template<typename Scalar>
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> upsampleX2(
		const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& field,
		UpsamplingStrategy upsampling_strategy = UpsamplingStrategy::NEAREST);
/**
 * Upsample the field such that the output is a field 2X larger than the original in each dimension.
 * @overload
 */
template<typename Scalar>
Eigen::Tensor<Scalar, 3, Eigen::ColMajor> upsampleX2(
		const Eigen::Tensor<Scalar, 3, Eigen::ColMajor>& field,
		UpsamplingStrategy upsampling_strategy = UpsamplingStrategy::NEAREST);

/**
 * Upsample the field such that the output is a field 2X larger than the original in each dimension.
 * This procedure uses a simple box filter / no interpolation, i.e. simply copies the value to it's immediate "children"
 * in the upsampled version.
 * Conceptual example:
 * ⎡1  2⎤
 * ⎣3  4⎦
 * yields
 * ⎡1  1  2  2⎤
 * ⎢1  1  2  2⎥
 * ⎢3  3  4  4⎥
 * ⎣3  3  4  4⎦
 * @param field input field
 * @return upsampled field
 */
template<typename Scalar>
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> upsampleX2_nearest(
		const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& field);

/**
 * Upsample the field such that the output is a field 2X larger than the original in each dimension using the
 * nearest-neighbor (NEAREST) strategy.
 * @overload
 */
template<typename Scalar>
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> upsampleX2_nearest(
		const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>& field);

/**
 * Upsample the field such that the output is a field 2X larger than the original in each dimension.
 * @overload
 */
template<typename Scalar>
Eigen::Tensor<Scalar, 3, Eigen::ColMajor> upsampleX2_nearest(
		const Eigen::Tensor<Scalar, 3, Eigen::ColMajor>& field);

/**
 * Upsample the field such that the output is a field 2X larger than the original in each dimension using bilinear
 * filtering. This procedure uses a simple tent filter in each dimension to compute the values, i.e. bilinear filtering.
 * Conceptual example:
 * The influence coefficients for the voxels in the output field (o) fall off linearly from 1.0 at the
 * current input voxel (X) to 0.0 at it's neighbors (O).
 *    o     o    o     o    o     o
 *       O┈┈┈┈┈┈┈┈┈┈O┈┈┈┈┈┈┈┈┈┈O
 *    o  ┊  o    o     o    o  ┊  o
 *       ┊                     ┊
 *    o  ┊  o    o     o    o  ┊  o
 *       O          X          O
 *    o  ┊  o    o     o    o  ┊  o
 *       ┊                     ┊
 *    o  ┊  o    o     o    o  ┊  o
 *       O┈┈┈┈┈┈┈┈┈┈O┈┈┈┈┈┈┈┈┈┈O
 *    o     o    o     o    o     o
 *  Boundary voxels are processed as if the boundary values of the input repeat infinitely.
 *
 * @param field input field
 * @return upsampled field
 */
template<typename Scalar>
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> upsampleX2_linear(
		const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& field);
/**
 * Upsample the field such that the output is a field 2X larger than the original in each dimension using bilinear
 * interpolation (LINEAR) strategy.
 * @overload
 */
template<typename Scalar>
Eigen::Tensor<Scalar, 3, Eigen::ColMajor> upsampleX2_linear(
		const Eigen::Tensor<Scalar, 3, Eigen::ColMajor>& field);

/**
 * Downsample the provided matrix using a box filter such that each dimension of the downsampled field is half the
 * corresponding dimension of the input field.
 * @param field input field
 * @param downsampling_strategy strategy to use for downsampling
 * @return downsampled field
 */
template<typename Scalar>
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> downsampleX2(
		const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& field,
		DownsamplingStrategy downsampling_strategy = DownsamplingStrategy::AVERAGE);
/**
 * Downsample the provided matrix using a box filter such that each dimension of the downsampled field is half the
 * corresponding dimension of the input field.
 * @overload
 */
template<typename Scalar>
Eigen::Tensor<Scalar, 3, Eigen::ColMajor> downsampleX2(
		const Eigen::Tensor<Scalar, 3, Eigen::ColMajor>& field,
		DownsamplingStrategy downsampling_strategy = DownsamplingStrategy::AVERAGE);

/**
 * Downsample the provided matrix using a box filter such that each dimension of the downsampled field is half the
 * corresponding dimension of the input field. Uses a simple box filter, i.e. each "downsampled" value will be the
 * average of it's source values in the input.
 *
 * Conceptual example (for 2d case):
 * ⎡1  2  4  5⎤
 * ⎢2  3  5  6⎥
 * ⎢1  3  6  7⎥
 * ⎣3  5  7  8⎦
 * yields
 * ⎡2  5⎤
 * ⎣3  7⎦
 *
 * @param field input field
 * @return downsampled field
 */
template<typename Scalar>
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> downsampleX2_average(
		const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& field);
/**
 * Downsample the provided matrix using a box filter such that each dimension of the downsampled field is half the
 * corresponding dimension of the input field using simple averaging (AVERAGE strategy).
 * @overload
 */
template<typename Scalar>
Eigen::Tensor<Scalar, 3, Eigen::ColMajor> downsampleX2_average(
		const Eigen::Tensor<Scalar, 3, Eigen::ColMajor>& field);

/**
 * Downsample the provided matrix using a box filter such that each dimension of the downsampled field is half the
 * corresponding dimension of the input field. Uses a tent filter, i.e. each "downsampled" value will be influenced by
 * values of the input weighted by the inverse ratio of their distance to the neighbor values.
 *
 * Conceptual example:
 * The influence coefficients for the source voxels in the input field (x) fall off linearly from 1.0 at the current
 * target voxel (X) to 0.0 at it's neighbors (O).
 *    o     o    o     o    o     o
 *       O┈┈┈┈┈┈┈┈┈┈O┈┈┈┈┈┈┈┈┈┈O
 *    o  ┊  o    o     o    o  ┊  o
 *       ┊                     ┊
 *    o  ┊  o    o     o    o  ┊  o
 *       O          X          O
 *    o  ┊  o    o     o    o  ┊  o
 *       ┊                     ┊
 *    o  ┊  o    o     o    o  ┊  o
 *       O┈┈┈┈┈┈┈┈┈┈O┈┈┈┈┈┈┈┈┈┈O
 *    o     o    o     o    o     o
 *  Boundary voxels are processed as if the boundary values of the input repeat infinitely.
 *
 * @param field input field
 * @return downsampled field
 */
template<typename Scalar>
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> downsampleX2_linear(
		const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& field);
/**
 * Downsample the provided matrix using a box filter such that each dimension of the downsampled field is half the
 * corresponding dimension of the input field using bilinear interpolation (LINEAR strategy).
 * @overload
 */
template<typename Scalar>
Eigen::Tensor<Scalar, 3, Eigen::ColMajor> downsampleX2_linear(
		const Eigen::Tensor<Scalar, 3, Eigen::ColMajor>& field);

} // namespace math


#include <chrono>
#include <iostream>
#include <limits>
#include <Eigen/Dense>
#include "dopri.h"
#include "kepler.h"
#include "elements.h"

using Eigen::VectorXd;
using Eigen::Vector3d;
using std::pow;
using std::sqrt;

namespace dopri {
    void gravity(int *n, double *x, double *y, double *f, double *rpar, int *ipar) {
        auto r = sqrt(y[0] * y[0] + y[1] * y[1] + y[2] * y[2]);
        auto r3 = r*r*r;
        f[0] = y[3];
        f[1] = y[4];
        f[2] = y[5];
        f[3] = -rpar[0] * y[0] / r3;
        f[4] = -rpar[0] * y[1] / r3;
        f[5] = -rpar[0] * y[2] / r3;
    }

    void solout_dummy(int *nr, double *xold, double *x, double *y, int *n, double *con,
                      int *icomp, int *nd, double *rpar, int *ipar, int *irtrn, double *xout){};

    void integrate(void (*func)(int *, double *, double *, double *, double *, int *),
            double *x, VectorXd *rv, double xend, double rpar[], int ipar[],
            double reltol = 1e-6, double abstol = 1e-8) {
        int n = rv->size();
        double rtol[] = {reltol};
        double atol[] = {abstol};
        int itol = 0;
        int iout = 0;
        int lwork = 11*n+8*n+21;
        int liwork = n + 21;
        double work[lwork];
        memset(work, 0, sizeof(work));
        int iwork[liwork];
        memset(iwork, 0, sizeof(iwork));
        int idid = 0;
        c_dop853(&n, func, x, rv->data(), &xend, rtol, atol, &itol, &solout_dummy,
            &iout, work, &lwork, iwork, &liwork, rpar, ipar, &idid);
    }

    void benchmark(int times) {
        auto mu = 3.986004418e5;
        Vector3d r(8.59072560e+02, -4.13720368e+03, 5.29556871e+03);
        Vector3d v(7.37289205e+00, 2.08223573e+00, 4.39999794e-01);
        VectorXd rv(r.size()+v.size());
        rv << r, v;
        VectorXd rv0(rv);
        auto el = elements::elements(r, v, mu);
        auto x = 0.0;
        double rpar[] = {mu};
        int ipar[] = {0};
        auto xend = kepler::period(el[0], mu);
        auto best = std::numeric_limits<double>::infinity();
        auto worst = -std::numeric_limits<double>::infinity();
        double all = 0;
        for (auto i=0; i < times; i++) {
            auto begin = std::chrono::high_resolution_clock::now();
            integrate(&gravity, &x, &rv0, xend, rpar, ipar);
            auto end = std::chrono::high_resolution_clock::now();
            auto current = std::chrono::duration_cast<std::chrono::nanoseconds>(end-begin).count()/1e9;
            all += current;
            if (current < best) {
                best = current;
            }
            if (current > worst) {
                worst = current;
            }
            rv0 = rv;
            x = 0;
        }
        std::cout << "[" << all/times << "," << best << "," << worst << "]" << std::endl;
    }
}


// Copyright Sergey Nizovtsev 2016
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/assert.hpp>
#include <boost/hana/core/is_a.hpp>
#include <boost/hana/functional/partial.hpp>
#include <boost/hana/product.hpp>
#include <boost/hana/range.hpp>
#include <boost/hana/traits.hpp>
#include <boost/hana/transform.hpp>
#include <boost/hana/tuple.hpp>
#include <boost/hana/type.hpp>

namespace hana = boost::hana;

int main() {
    constexpr auto type = hana::type_c<int[2][3][4]>;

    BOOST_HANA_CONSTANT_CHECK(
        hana::is_an<hana::integral_constant_tag<size_t>>(
            hana::traits::extent(type, hana::uint_c<1>)
        )
    );

    // Check that we can multiple extents in size_t's ring
    hana::product<hana::integral_constant_tag<size_t>>(
        hana::transform(
            hana::to_tuple(
                hana::make_range(
                    hana::size_c<0>,
                    hana::traits::rank(type)
                )
            ),
            hana::partial(hana::traits::extent, type)
        )
    );
}


#include <iostream>
#include <string>
#include <cstdlib>
#include <boost/mpl/string.hpp>
#include <boost/mpl/fold.hpp>
#include <boost/mpl/size_t.hpp>

using namespace std;
using namespace boost;

///////////////////////////////////////////////////////////////////////////////
// exponentiation calculations
template <int accum, int base, int exp> struct POWER_CORE : POWER_CORE<accum * base, base, exp - 1>{};

template <int accum, int base>
struct POWER_CORE<accum, base, 0>
{
    enum : int { val = accum };
};

template <int base, int exp> struct POWER : POWER_CORE<1, base, exp>{};

///////////////////////////////////////////////////////////////////////////////
// # of digit calculations
template <int depth, unsigned int i> struct NUM_DIGITS_CORE : NUM_DIGITS_CORE<depth + 1, i / 10>{};

template <int depth>
struct NUM_DIGITS_CORE<depth, 0>
{
    enum : int { val = depth};
};

template <int i> struct NUM_DIGITS : NUM_DIGITS_CORE<0, i>{};

template <>
struct NUM_DIGITS<0>
{
    enum : int { val = 1 };
};

///////////////////////////////////////////////////////////////////////////////
// Convert digit to character (1 -> '1')
template <int i>
struct DIGIT_TO_CHAR
{
    enum : char{ val = i + 48 };
};

///////////////////////////////////////////////////////////////////////////////
// Find the digit at a given offset into a number of the form 0000000017
template <unsigned int i, int place> // place -> [0 .. 10]
struct DIGIT_AT
{
    enum : char{ val = (i / POWER<10, place>::val) % 10 };
};

struct NULL_CHAR
{
    enum : char{ val = '\0' };
};

///////////////////////////////////////////////////////////////////////////////
// Convert the digit at a given offset into a number of the form '0000000017' to a character
template <unsigned int i, int place> // place -> [0 .. 9]
    struct ALT_CHAR : DIGIT_TO_CHAR< DIGIT_AT<i, place>::val >{};

///////////////////////////////////////////////////////////////////////////////
// Convert the digit at a given offset into a number of the form '17' to a character

// Template description, with specialization to generate null characters for out of range offsets
template <unsigned int i, int offset, int numDigits, bool inRange>
    struct OFFSET_CHAR_CORE_CHECKED{};
template <unsigned int i, int offset, int numDigits>
    struct OFFSET_CHAR_CORE_CHECKED<i, offset, numDigits, false> : NULL_CHAR{};
template <unsigned int i, int offset, int numDigits>
    struct OFFSET_CHAR_CORE_CHECKED<i, offset, numDigits, true>  : ALT_CHAR<i, (numDigits - offset) - 1 >{};

// Perform the range check and pass it on
template <unsigned int i, int offset, int numDigits>
    struct OFFSET_CHAR_CORE : OFFSET_CHAR_CORE_CHECKED<i, offset, numDigits, offset < numDigits>{};

// Calc the number of digits and pass it on
template <unsigned int i, int offset>
    struct OFFSET_CHAR : OFFSET_CHAR_CORE<i, offset, NUM_DIGITS<i>::val>{};

///////////////////////////////////////////////////////////////////////////////
// Integer to char* template. Works on unsigned ints.
template <unsigned int i>
struct IntToStr
{
    const static char str[];
    typedef typename mpl::string<
    OFFSET_CHAR<i, 0>::val,
    OFFSET_CHAR<i, 1>::val,
    OFFSET_CHAR<i, 2>::val,
    OFFSET_CHAR<i, 3>::val,
    OFFSET_CHAR<i, 4>::val,
    OFFSET_CHAR<i, 5>::val,
    /*OFFSET_CHAR<i, 6>::val,
    OFFSET_CHAR<i, 7>::val,
    OFFSET_CHAR<i, 8>::val,
    OFFSET_CHAR<i, 9>::val,*/
    NULL_CHAR::val>::type type;
};

template <unsigned int i>
const char IntToStr<i>::str[] =
{
    OFFSET_CHAR<i, 0>::val,
    OFFSET_CHAR<i, 1>::val,
    OFFSET_CHAR<i, 2>::val,
    OFFSET_CHAR<i, 3>::val,
    OFFSET_CHAR<i, 4>::val,
    OFFSET_CHAR<i, 5>::val,
    OFFSET_CHAR<i, 6>::val,
    OFFSET_CHAR<i, 7>::val,
    OFFSET_CHAR<i, 8>::val,
    OFFSET_CHAR<i, 9>::val,
    NULL_CHAR::val
};

template <bool condition, class Then, class Else>
struct IF
{
    typedef Then RET;
};

template <class Then, class Else>
struct IF<false, Then, Else>
{
    typedef Else RET;
};

template < typename Str1, typename Str2 >
struct concat : mpl::insert_range<Str1, typename mpl::end<Str1>::type, Str2> {};
template <typename Str1, typename Str2, typename Str3 >
struct concat3 : mpl::insert_range<Str1, typename mpl::end<Str1>::type, typename concat<Str2, Str3 >::type > {};

typedef typename mpl::string<'f','i','z','z'>::type fizz;
typedef typename mpl::string<'b','u','z','z'>::type buzz;
typedef typename mpl::string<'\r', '\n'>::type mpendl;
typedef typename concat<fizz, buzz>::type fizzbuzz;

// discovered boost mpl limitation on some length

template <int N>
struct FizzBuzz
{
    typedef typename concat3<typename FizzBuzz<N - 1>::type, typename IF<N % 15 == 0, typename fizzbuzz::type, typename IF<N % 3 == 0, typename fizz::type, typename IF<N % 5 == 0, typename buzz::type, typename IntToStr<N>::type >::RET >::RET >::RET, typename mpendl::type>::type type;
};

template <>
struct FizzBuzz<1>
{
    typedef mpl::string<'1','\r','\n'>::type type;
};

int main(int argc, char** argv)
{
    const int n = 7;
    std::cout << mpl::c_str<FizzBuzz<n>::type>::value << std::endl;
	return 0;
}


/**
 * @file
 * @brief Implementation of IntegrationElement() test for geometry objects
 * @author Anian Ruoss
 * @date   2019-02-11 17:59:17
 * @copyright MIT License
 */

#include "check_integration_element.h"

#include <gtest/gtest.h>

#include <Eigen/LU>

namespace lf::geometry::test_utils {

void checkIntegrationElement(const lf::geometry::Geometry &geom,
                             const Eigen::MatrixXd &eval_points) {
  const size_t num_points = eval_points.cols();
  const size_t dim_local = geom.DimLocal();
  const size_t dim_global = geom.DimGlobal();

  Eigen::MatrixXd jacobians = geom.Jacobian(eval_points);
  Eigen::VectorXd integrationElements = geom.IntegrationElement(eval_points);

  EXPECT_EQ(integrationElements.rows(), num_points)
      << "IntegrationElement has " << integrationElements.rows()
      << " rows instead of " << num_points;
  EXPECT_EQ(integrationElements.cols(), 1)
      << "IntegrationElement has " << integrationElements.cols()
      << " cols instead of " << 1;

  for (int j = 0; j < num_points; ++j) {
    Eigen::MatrixXd jacobian =
        jacobians.block(0, j * dim_local, dim_global, dim_local);

    const double integrationElement = integrationElements(j);
    const double approx_integrationElement =
        std::sqrt((jacobian.transpose() * jacobian).determinant());

    EXPECT_FLOAT_EQ(integrationElement, approx_integrationElement)
        << "IntegrationElement incorrect at point " << eval_points.col(j);
  }
}

}  // namespace lf::geometry::test_utils


#include <Eigen/Dense>
#include <iostream>

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  Matrix4d X = Matrix4d::Random(4,4);
Matrix4d A = X + X.transpose();
cout << "Here is a random symmetric 4x4 matrix:" << endl << A << endl;
Tridiagonalization<Matrix4d> triOfA(A);
Vector3d hc = triOfA.householderCoefficients();
cout << "The vector of Householder coefficients is:" << endl << hc << endl;

  return 0;
}


/**
 * @file    MatrixPseudoInverse.cpp
 * @brief   Functions for matrix pseudo inverse, and matrix square root.
 * @author  Jianzhu Huai
 */

#ifndef INCLUDE_OKVIS_MATRIX_PSEUDO_INVERSE_HPP
#define INCLUDE_OKVIS_MATRIX_PSEUDO_INVERSE_HPP

#include <Eigen/Core>
#include <Eigen/Eigenvalues>

#include <okvis/assert_macros.hpp>

namespace okvis {

/**
 * \brief Compute the square root of a matrix using the LDLt decomposition for
 * square, positive semidefinite matrices
 *
 * To reconstruct the input matrix, \f$ \mathbf A \f$, from the returned matrix,
 * \f$ \mathbf S \f$, use, \f$ \mathbf A = \mathbf S \mathbf S^T \f$.
 *
 *
 * @param inMatrix      The square matrix whose square root should be computed.
 * @param outMatrixSqrt The output square root.
 */
template <typename DERIVED1, typename DERIVED2>
/*Eigen::ComputationInfo*/ void
computeMatrixSqrt(const Eigen::MatrixBase<DERIVED1> &inMatrix,
                  const Eigen::MatrixBase<DERIVED2> &outMatrixSqrt) {
  OKVIS_ASSERT_EQ_DBG(std::runtime_error, inMatrix.rows(), inMatrix.cols(),
                      "This method is only valid for square input matrices");

  DERIVED2 &result = const_cast<DERIVED2 &>(outMatrixSqrt.derived());

  // This is tricky. Using the output matrix type causes the input matrix
  // type to be upgraded to a real numeric matrix. This is useful if,
  // for example, the inMatrix is something like Eigen::Matrix3d::Identity(),
  // which is not an actual matrix. Using DERIVED1 as the template argument
  // in that case will cause a firestorm of compiler errors.
  Eigen::LDLT<DERIVED2> ldlt(inMatrix.derived());
  result = ldlt.matrixL();
  result = ldlt.transpositionsP().transpose() * result;
  result *= ldlt.vectorD().array().sqrt().matrix().asDiagonal();

  // return ldlt.info();
}

class MatrixPseudoInverse
{
public:
  OKVIS_DEFINE_EXCEPTION(Exception,std::runtime_error)
  /**
   * @brief Pseudo inversion of a symmetric matrix.
   * @warning   This uses Eigen-decomposition, it assumes the input is symmetric positive semi-definite
   *            (negative Eigenvalues are set to zero).
   * @tparam Derived Matrix type (auto-deducible).
   * @param[in] a Input Matrix
   * @param[out] result Output, i.e. pseudo-inverse.
   * @param[in] epsilon The tolerance.
   * @param[out] rank Optional rank.
   * @return
   */
  template<typename Derived>
  static bool pseudoInverseSymm(
      const Eigen::MatrixBase<Derived>&a,
      const Eigen::MatrixBase<Derived>&result, double epsilon =
          std::numeric_limits<typename Derived::Scalar>::epsilon(), int * rank = 0);

  /**
   * @brief Pseudo inversion and square root (Cholesky decomposition) of a symmetric matrix.
   * @warning   This uses Eigen-decomposition, it assumes the input is symmetric positive semi-definite
   *            (negative Eigenvalues are set to zero). Also if the input is positive semi-definite,
   *            its zero eigenvalues are at the lower part of the diagonal.
   * @tparam Derived Matrix type (auto-deducible).
   * @param[in] a Input Matrix, \f$A\f$.
   * @param[out] result Output, \f$L\f$, i.e. the Cholesky decomposition of a pseudo-inverse, \f$A^{-1} = L L^*\f$.
   * @param[in] epsilon The tolerance.
   * @param[out] rank The rank, if of interest.
   * @return
   */
  template<typename Derived>
  static bool pseudoInverseSymmSqrt(
      const Eigen::MatrixBase<Derived>&a,
      const Eigen::MatrixBase<Derived>&result, double epsilon =
          std::numeric_limits<typename Derived::Scalar>::epsilon(),
      int* rank = NULL);

  /**
   * @brief Pseudo square root (Cholesky decomposition) of a symmetric matrix.
   * @warning   This uses Eigen-decomposition, it assumes the input is symmetric positive semi-definite
   *            (negative Eigenvalues are set to zero). Also if the input is positive semi-definite,
   *            its zero eigenvalues are at the lower part of the diagonal.
   * @tparam Derived Matrix type (auto-deducible).
   * @param[in] a Input Matrix, \f$A\f$.
   * @param[out] result Output, \f$L\f$, i.e. the Cholesky decomposition of a pseudo-inverse, \f$A = L L^*\f$.
   * @param[in] epsilon The tolerance.
   * @param[out] rank The rank, if of interest.
   * @return
   */
  template<typename Derived>
  static bool pseudoSymmSqrt(
      const Eigen::MatrixBase<Derived>&a,
      const Eigen::MatrixBase<Derived>&result, double epsilon =
          std::numeric_limits<typename Derived::Scalar>::epsilon(),
      int* rank = NULL);

  /**
   * @brief Block-wise pseudo inversion of a symmetric matrix with non-zero diagonal blocks.
   * @warning   This uses Eigen-decomposition, it assumes the input is symmetric positive semi-definite
   *            (negative Eigenvalues are set to zero).
   * @tparam Derived Matrix type (auto-deducible).
   * @tparam blockDim The block size of the diagonal blocks.
   * @param[in] M_in Input Matrix
   * @param[out] M_out Output, i.e. thepseudo-inverse.
   * @param[in] epsilon The tolerance.
   * @return
   */
  template<typename Derived, int blockDim>
  static void blockPinverse(
      const Eigen::MatrixBase<Derived>& M_in,
      const Eigen::MatrixBase<Derived>& M_out, double epsilon =
          std::numeric_limits<typename Derived::Scalar>::epsilon());

  /**
   * @brief Block-wise pseudo inversion and square root (Cholesky decomposition)
   *        of a symmetric matrix with non-zero diagonal blocks.
   * @warning   This uses Eigen-decomposition, it assumes the input is symmetric positive semi-definite
   *            (negative Eigenvalues are set to zero).
   * @tparam Derived Matrix type (auto-deducible).
   * @tparam blockDim The block size of the diagonal blocks.
   * @param[in] M_in Input Matrix
   * @param[out] M_out Output, i.e. the Cholesky decomposition of a pseudo-inverse.
   * @param[in] epsilon The tolerance.
   * @return
   */
  template<typename Derived, int blockDim>
  static void blockPinverseSqrt(
      const Eigen::MatrixBase<Derived>& M_in,
      const Eigen::MatrixBase<Derived>& M_out, double epsilon =
          std::numeric_limits<typename Derived::Scalar>::epsilon());

};

// Pseudo inversion of a symmetric matrix.
// attention: this uses Eigen-decomposition, it assumes the input is symmetric positive semi-definite
// (negative Eigenvalues are set to zero)
template<typename Derived>
bool MatrixPseudoInverse::pseudoInverseSymm(
    const Eigen::MatrixBase<Derived>&a, const Eigen::MatrixBase<Derived>&result,
    double epsilon, int * rank) {

  OKVIS_ASSERT_TRUE_DBG(Exception, a.rows() == a.cols(),
                        "matrix supplied is not quadratic");

  Eigen::SelfAdjointEigenSolver<Derived> saes(a);

  typename Derived::Scalar tolerance = epsilon * a.cols()
      * saes.eigenvalues().array().maxCoeff();

  const_cast<Eigen::MatrixBase<Derived>&>(result) = (saes.eigenvectors())
      * Eigen::VectorXd(
          (saes.eigenvalues().array() > tolerance).select(
              saes.eigenvalues().array().inverse(), 0)).asDiagonal()
      * (saes.eigenvectors().transpose());

  if (rank) {
    *rank = 0;
    for (int i = 0; i < a.rows(); ++i) {
      if (saes.eigenvalues()[i] > tolerance)
        (*rank)++;
    }
  }

  return true;
}

// Pseudo inversion and square root (Cholesky decomposition) of a symmetric matrix.
// attention: this uses Eigen-decomposition, it assumes the input is symmetric positive semi-definite
// (negative Eigenvalues are set to zero)
template<typename Derived>
bool MatrixPseudoInverse::pseudoInverseSymmSqrt(
    const Eigen::MatrixBase<Derived>&a, const Eigen::MatrixBase<Derived>&result,
    double epsilon, int * rank) {

  OKVIS_ASSERT_TRUE_DBG(Exception, a.rows() == a.cols(),
                        "matrix supplied is not quadratic");

  Eigen::SelfAdjointEigenSolver<Derived> saes(a);

  typename Derived::Scalar tolerance = epsilon * a.cols()
      * saes.eigenvalues().array().maxCoeff();

  const_cast<Eigen::MatrixBase<Derived>&>(result) = (saes.eigenvectors())
      * Eigen::VectorXd(
          Eigen::VectorXd(
              (saes.eigenvalues().array() > tolerance).select(
                  saes.eigenvalues().array().inverse(), 0)).array().sqrt())
          .asDiagonal();

  if (rank) {
    *rank = 0;
    for (int i = 0; i < a.rows(); ++i) {
      if (saes.eigenvalues()[i] > tolerance)
        (*rank)++;
    }
  }

  return true;
}

template <typename Derived>
bool MatrixPseudoInverse::pseudoSymmSqrt(
    const Eigen::MatrixBase<Derived> &a,
    const Eigen::MatrixBase<Derived> &result, double epsilon, int *rank) {

  OKVIS_ASSERT_TRUE_DBG(Exception, a.rows() == a.cols(),
                        "matrix supplied is not quadratic");

  Eigen::SelfAdjointEigenSolver<Derived> saes(a);

  typename Derived::Scalar tolerance =
      epsilon * a.cols() * saes.eigenvalues().array().maxCoeff();

  const_cast<Eigen::MatrixBase<Derived> &>(result) =
      (saes.eigenvectors()) *
      Eigen::VectorXd(
          Eigen::VectorXd((saes.eigenvalues().array() > tolerance)
                              .select(saes.eigenvalues().array(), 0))
              .array()
              .sqrt())
          .asDiagonal();

  if (rank) {
    *rank = 0;
    for (int i = 0; i < a.rows(); ++i) {
      if (saes.eigenvalues()[i] > tolerance)
        (*rank)++;
    }
  }

  return true;
}

// Block-wise pseudo inversion of a symmetric matrix with non-zero diagonal blocks.
// attention: this uses Eigen-decomposition, it assumes the input is symmetric positive semi-definite
// (negative Eigenvalues are set to zero)
template<typename Derived, int blockDim>
void MatrixPseudoInverse::blockPinverse(
    const Eigen::MatrixBase<Derived>& M_in,
    const Eigen::MatrixBase<Derived>& M_out, double epsilon) {

  OKVIS_ASSERT_TRUE_DBG(Exception, M_in.rows() == M_in.cols(),
                        "matrix supplied is not quadratic");

  const_cast<Eigen::MatrixBase<Derived>&>(M_out).resize(M_in.rows(),
                                                        M_in.rows());
  const_cast<Eigen::MatrixBase<Derived>&>(M_out).setZero();
  for (int i = 0; i < M_in.cols(); i += blockDim) {
    Eigen::Matrix<double, blockDim, blockDim> inv;
    const Eigen::Matrix<double, blockDim, blockDim> in = M_in
        .template block<blockDim, blockDim>(i, i);
    //const Eigen::Matrix<double,blockDim,blockDim> in1=0.5*(in+in.transpose());
    pseudoInverseSymm(in, inv, epsilon);
    const_cast<Eigen::MatrixBase<Derived>&>(M_out)
        .template block<blockDim, blockDim>(i, i) = inv;
  }
}

// Block-wise pseudo inversion and square root (Cholesky decomposition)
// of a symmetric matrix with non-zero diagonal blocks.
// attention: this uses Eigen-decomposition, it assumes the input is symmetric positive semi-definite
// (negative Eigenvalues are set to zero)
template<typename Derived, int blockDim>
void MatrixPseudoInverse::blockPinverseSqrt(
    const Eigen::MatrixBase<Derived>& M_in,
    const Eigen::MatrixBase<Derived>& M_out, double epsilon) {

  OKVIS_ASSERT_TRUE_DBG(Exception, M_in.rows() == M_in.cols(),
                        "matrix supplied is not quadratic");

  const_cast<Eigen::MatrixBase<Derived>&>(M_out).resize(M_in.rows(),
                                                        M_in.rows());
  const_cast<Eigen::MatrixBase<Derived>&>(M_out).setZero();
  for (int i = 0; i < M_in.cols(); i += blockDim) {
    Eigen::Matrix<double, blockDim, blockDim> inv;
    const Eigen::Matrix<double, blockDim, blockDim> in = M_in
        .template block<blockDim, blockDim>(i, i);
    //const Eigen::Matrix<double,blockDim,blockDim> in1=0.5*(in+in.transpose());
    pseudoInverseSymmSqrt(in, inv, epsilon);
    const_cast<Eigen::MatrixBase<Derived>&>(M_out)
        .template block<blockDim, blockDim>(i, i) = inv;
  }
}

} // namespace okvis
#endif // INCLUDE_OKVIS_MATRIX_PSEUDO_INVERSE_HPP


#include <frovedis.hpp>
#include <frovedis/matrix/crs_matrix.hpp>

#include <math.h>
#include <boost/program_options.hpp>

using namespace boost;
using namespace frovedis;
using namespace std;

void normalize_row(crs_matrix_local<double>& mat) {
  crs_matrix_local<double> ret;
  for(size_t r = 0; r < mat.local_num_row; r++) {
    double sq = 0;
    for(size_t c = mat.off[r]; c < mat.off[r+1]; c++) {
      sq += mat.val[c] * mat.val[c];
    }
    double root = sqrt(sq);
    for(size_t c = mat.off[r]; c < mat.off[r+1]; c++) {
      mat.val[c] /= root;
    }
  }
}

void do_normalize_row(const string& input, const string& output, bool binary) {
  if(binary) {
    time_spent t(DEBUG);
    auto mat = make_crs_matrix_loadbinary<double>(input);
    t.show("load matrix: ");
    mat.data.mapv(normalize_row);
    t.show("normalize time: ");
    mat.savebinary(output);
    t.show("save: ");
  } else {
    time_spent t(DEBUG);
    auto mat = make_crs_matrix_load<double>(input);
    t.show("load matrix: ");
    mat.data.mapv(normalize_row);
    t.show("normalize time: ");
    mat.save(output);
    t.show("save: ");
  }
}

int main(int argc, char* argv[]){
  use_frovedis use(argc, argv);

  using namespace boost::program_options;

  options_description opt("option");
  opt.add_options()
    ("help,h", "print help")
    ("input,i", value<string>(), "input matrix file")
    ("output,o", value<string>(), "output matrix file")
    ("binary,b", "use binary input/output");

  variables_map argmap;
  store(command_line_parser(argc,argv).options(opt).allow_unregistered().
        run(), argmap);
  notify(argmap);

  string input, output;
  bool binary = false;

  if(argmap.count("help")){
    cerr << opt << endl;
    exit(1);
  }

  if(argmap.count("input")){
    input = argmap["input"].as<string>();
  } else {
    cerr << "input matrix file is not specified" << endl;
    cerr << opt << endl;
    exit(1);
  }

  if(argmap.count("output")){
    output = argmap["output"].as<string>();
  } else {
    cerr << "output matrix file is not specified" << endl;
    cerr << opt << endl;
    exit(1);
  }

  if(argmap.count("binary")){
    binary = true;
  }
  do_normalize_row(input, output, binary);
}


#include <iostream>
//#include <boost/multiprecision/cpp_int.hpp>
// using namespace boost::multiprecision;
const int mx = 1e6 + 5;
const long int inf = 2e9;
typedef long long ll;
#define rep(i, n) for (i = 0; i < n; i++)
#define repp(i, a, b) for (i = a; i <= b; i++)
#define pii pair<int, int>
#define vpii vector<pii>
#define vi vector<int>
#define vll vector<ll>
#define r(x) scanf("%d", &x)
#define rs(s) scanf("%s", s)
#define gc getchar_unlocked
#define pc putchar_unlocked
#define mp make_pair
#define pb push_back
#define lb lower_bound
#define ub upper_bound
#define endl "\n"
#define fast                          \
    ios_base::sync_with_stdio(false); \
    cin.tie(NULL);                    \
    cout.tie(NULL);
using namespace std;
void in(int &x) {
    register int c = gc();
    x = 0;
    int neg = 0;
    for (; ((c < 48 || c > 57) && c != '-'); c = gc())
        ;
    if (c == '-') {
        neg = 1;
        c = gc();
    }
    for (; c > 47 && c < 58; c = gc()) {
        x = (x << 1) + (x << 3) + c - 48;
    }
    if (neg)
        x = -x;
}
void out(int n) {
    int N = n, rev, count = 0;
    rev = N;
    if (N == 0) {
        pc('0');
        return;
    }
    while ((rev % 10) == 0) {
        count++;
        rev /= 10;
    }
    rev = 0;
    while (N != 0) {
        rev = (rev << 3) + (rev << 1) + N % 10;
        N /= 10;
    }
    while (rev != 0) {
        pc(rev % 10 + '0');
        rev /= 10;
    }
    while (count--) pc('0');
}
ll parent[mx], arr[mx], node, edge;
vector<pair<ll, pair<ll, ll>>> v;
void initial() {
    int i;
    rep(i, node + edge) parent[i] = i;
}
int root(int i) {
    while (parent[i] != i) {
        parent[i] = parent[parent[i]];
        i = parent[i];
    }
    return i;
}
void join(int x, int y) {
    int root_x = root(x);  // Disjoint set union by rank
    int root_y = root(y);
    parent[root_x] = root_y;
}
ll kruskal() {
    ll mincost = 0, i, x, y;
    rep(i, edge) {
        x = v[i].second.first;
        y = v[i].second.second;
        if (root(x) != root(y)) {
            mincost += v[i].first;
            join(x, y);
        }
    }
    return mincost;
}
int main() {
    fast;
    while (1) {
        int i, j, from, to, cost, totalcost = 0;
        cin >> node >> edge;  // Enter the nodes and edges
        if (node == 0 && edge == 0)
            break;  // Enter 0 0 to break out
        initial();  // Initialise the parent array
        rep(i, edge) {
            cin >> from >> to >> cost;
            v.pb(mp(cost, mp(from, to)));
            totalcost += cost;
        }
        sort(v.begin(), v.end());
        // rep(i,v.size())
        // 	cout<<v[i].first<<"  ";
        cout << kruskal() << endl;
        v.clear();
    }
    return 0;
}


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

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_AVERAGE_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_AVERAGE_HPP_INCLUDED

#include <boost/simd/constant/half.hpp>
#include <boost/simd/function/bitwise_and.hpp>
#include <boost/simd/function/bitwise_xor.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/plus.hpp>
#include <boost/simd/function/shift_right.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( average_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::arithmetic_<A0> >
                          , bd::scalar_< bd::arithmetic_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      return bitwise_and(a0, a1)+shift_right(bitwise_xor(a0, a1),1);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( average_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      return fma(a0,Half<A0>(),a1*Half<A0>());
    }
  };
} } }

#endif


//
//  Copyright (c) 2018, Cem Bassoy, cem.bassoy@gmail.com
//  Copyright (c) 2019, Amit Singh, amitsingh19975@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)
//
//  The authors gratefully acknowledge the support of
//  Google and Fraunhofer IOSB, Ettlingen, Germany
//

#include <boost/numeric/ublas/tensor.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>

#include <ostream>

//NOLINTNEXTLINE
int main()
{
  namespace ublas = boost::numeric::ublas;

  try {
    using value   = float;
    using layout  = ublas::layout::first_order; // storage format
    using tensor  = ublas::tensor_dynamic<value,layout>;
//    constexpr auto ones  = ublas::ones<value,layout>{};
    constexpr auto zeros = ublas::zeros<value,layout>{};

    // creates a three-dimensional tensor with extents 3,4 and 2
    // tensor A stores single-precision floating-point number according
    // to the first-order storage format

    tensor A = zeros(3,4,2);

    // initializes the tensor with increasing values along the first-index
    // using a single index.
    auto vf = 1.0f;
    for(auto i = 0u; i < A.size(); ++i, vf += 1.0f)
      A[i] = vf;

    // formatted output
    std::cout << "% --------------------------- " << std::endl;
    std::cout << "% --------------------------- " << std::endl << std::endl;
    std::cout << "A=" << A << ";" << std::endl << std::endl;
  } catch (const std::exception& e) {
    std::cerr << "Cought exception " << e.what();
    std::cerr << "in the main function of access-tensor." << std::endl;
  }

  try {
    using value   = std::complex<boost::multiprecision::cpp_bin_float_double_extended>;
    using layout  = ublas::layout::last_order; // storage format
    using tensor  = ublas::tensor_dynamic<value,layout>;
    using shape   = typename tensor::extents_type;
    constexpr auto zeros = ublas::zeros<value,layout>{};

    // creates a four-dimensional tensor with extents 5,4,3 and 2
    // tensor A stores complex floating-point extended double precision numbers
    // according to the last-order storage format
    // and initializes it with the default value.

    //NOLINTNEXTLINE
    tensor B = zeros(5,4,3,2);

    // initializes the tensor with increasing values along the last-index
    // using a single-index
    auto vc = value(0,0);
    for(auto i = 0u; i < B.size(); ++i, vc += value(1,1))
      B[i] = vc;

    // formatted output
    std::cout << "% --------------------------- " << std::endl;
    std::cout << "% --------------------------- " << std::endl << std::endl;
    std::cout << "B=" << B << ";" << std::endl << std::endl;

    auto C = tensor(B.extents());
    // computes the complex conjugate of elements of B
    // using multi-index notation.
    for(auto i = 0u; i < B.size(0); ++i)
      for(auto j = 0u; j < B.size(1); ++j)
        for(auto k = 0u; k < B.size(2); ++k)
          for(auto l = 0u; l < B.size(3); ++l)
            C.at(i,j,k,l) = std::conj(B.at(i,j,k,l));

    std::cout << "% --------------------------- " << std::endl;
    std::cout << "% --------------------------- " << std::endl << std::endl;
    std::cout << "C=" << C << ";" << std::endl << std::endl;

    // computes the complex conjugate of elements of B
    // using iterators.
    auto D = tensor(B.extents());
    std::transform(B.begin(), B.end(), D.begin(), [](auto const& b){ return std::conj(b); });
    std::cout << "% --------------------------- " << std::endl;
    std::cout << "% --------------------------- " << std::endl << std::endl;
    std::cout << "D=" << D << ";" << std::endl << std::endl;

    // reshaping tensors.
    auto new_extents = B.extents().base();
    std::next_permutation( new_extents.begin(), new_extents.end() );
    auto E = reshape( D, shape(new_extents)  );
    std::cout << "% --------------------------- " << std::endl;
    std::cout << "% --------------------------- " << std::endl << std::endl;
    std::cout << "E=" << E << ";" << std::endl << std::endl;

  } catch (const std::exception& e) {
    std::cerr << "Cought exception " << e.what();
    std::cerr << "in the main function of access-tensor." << std::endl;
  }
}


#pragma once

#include <iostream>   //cout screen output
#include <math.h>
#include <Eigen/Dense>

//debugging purposes
#define DEBUGGING // compute manually the vector cross product
#define DEBUG_NORM_ERROR 0.0005

// gravity values
#define G_EARTH 9.80
#define G_EARTH_DOUBLE 2*G_EARTH
#define G_EARTH_SQUARED G_EARTH*G_EARTH

// roll pitch and yaw
#define ROLL       0
#define PITCH      1
#define YAW        2

namespace complementary_filter
{

//Convert an error rotation vector into a quaternion
void vec2quat(const Eigen::Vector3d& vec, Eigen::Quaterniond& quat);

//Convert a quaternion into an error rotation vector
void quat2vec(const Eigen::Quaterniond& quat, Eigen::Ref<Eigen::Vector3d>& vec);

}


/* vim:set ts=3 sw=3 sts=3 et: */
/**
 * \file       log2.cpp
 * \brief      Test cases for base-2 integer log algorithm.
 * \author     Marcus Holland-Moritz (marcus@last.fm)
 * \copyright  Copyright © 2008-2013 Last.fm Limited
 *
 * This file is part of libmoost.
 *
 * Permission is hereby granted, free of charge, to any person
 * obtaining a copy of this software and associated documentation
 * files (the "Software"), to deal in the Software without restriction,
 * including without limitation the rights to use, copy, modify, merge,
 * publish, distribute, sublicense, and/or sell copies of the Software,
 * and to permit persons to whom the Software is furnished to do so,
 * subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 * OTHER DEALINGS IN THE SOFTWARE.
 */

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

#include "../../include/moost/math/integer/log2.hpp"

using namespace moost;

BOOST_AUTO_TEST_SUITE(int_log2_test)

BOOST_AUTO_TEST_CASE(int_log2_test)
{
   BOOST_CHECK_EQUAL(math::integer::log2(1U), 0);
   BOOST_CHECK_EQUAL(math::integer::log2_compat(1U), 0);

   BOOST_CHECK_EQUAL(math::integer::log2(2U), 1);
   BOOST_CHECK_EQUAL(math::integer::log2_compat(2U), 1);

   BOOST_CHECK_EQUAL(math::integer::log2(3U), 1);
   BOOST_CHECK_EQUAL(math::integer::log2_compat(3U), 1);

   BOOST_CHECK_EQUAL(math::integer::log2(4U), 2);
   BOOST_CHECK_EQUAL(math::integer::log2_compat(4U), 2);

   BOOST_CHECK_EQUAL(math::integer::log2(65535U), 15);
   BOOST_CHECK_EQUAL(math::integer::log2_compat(65535U), 15);

   BOOST_CHECK_EQUAL(math::integer::log2(65535UL), 15);
   BOOST_CHECK_EQUAL(math::integer::log2_compat(65535UL), 15);

   BOOST_CHECK_EQUAL(math::integer::log2(65536UL), 16);
   BOOST_CHECK_EQUAL(math::integer::log2_compat(65536UL), 16);

   BOOST_CHECK_EQUAL(math::integer::log2(65537UL), 16);
   BOOST_CHECK_EQUAL(math::integer::log2_compat(65537UL), 16);

   BOOST_CHECK_EQUAL(math::integer::log2(0xFFFFFFFFUL), 31);
   BOOST_CHECK_EQUAL(math::integer::log2_compat(0xFFFFFFFFUL), 31);

   BOOST_CHECK_EQUAL(math::integer::log2(UINT64_C(0xFFFFFFFF)), 31);
   BOOST_CHECK_EQUAL(math::integer::log2_compat(UINT64_C(0xFFFFFFFF)), 31);

   BOOST_CHECK_EQUAL(math::integer::log2(UINT64_C(0x100000000)), 32);
   BOOST_CHECK_EQUAL(math::integer::log2_compat(UINT64_C(0x100000000)), 32);

   BOOST_CHECK_EQUAL(math::integer::log2(UINT64_C(0x100000001)), 32);
   BOOST_CHECK_EQUAL(math::integer::log2_compat(UINT64_C(0x100000001)), 32);

   BOOST_CHECK_EQUAL(math::integer::log2(UINT64_C(0xFFFFFFFFFFFFFFFF)), 63);
   BOOST_CHECK_EQUAL(math::integer::log2_compat(UINT64_C(0xFFFFFFFFFFFFFFFF)), 63);
}

BOOST_AUTO_TEST_SUITE_END()


#include "ScalePyramid.hpp"
#include <Danvil/Tools/MoreMath.h>
#include <boost/assert.hpp>
#include <iostream>
//----------------------------------------------------------------------------//
namespace density {
//----------------------------------------------------------------------------//

Eigen::MatrixXf SumMipMapWithBlackBorder(const Eigen::MatrixXf& img_big)
{
	size_t w_big = img_big.rows();
	size_t h_big = img_big.cols();
	// the computed mipmap will have 2^i size
	unsigned int size = Danvil::MoreMath::P2Ceil(std::max(w_big, h_big));
	Eigen::MatrixXf img_small(size / 2, size / 2);
	img_small.fill({0.0f});
	// only the part where at least one of the four pixels lies in the big image is iterated
	// the rest was set to 0 with the fill op
	size_t w_small = w_big / 2 + ((w_big % 2 == 0) ? 0 : 1);
	size_t h_small = h_big / 2 + ((h_big % 2 == 0) ? 0 : 1);
	for(size_t y = 0; y < h_small; y++) {
		size_t y_big = y * 2;
		for(size_t x = 0; x < w_small; x++) {
			size_t x_big = x * 2;
			// We sum over all four pixels in the big image (if they are valid).
			// May by invalid because the big image is considered to be enlarged
			// to have a size of 2^i.
			float sum = 0.0f;
			// Since we only test the part where at least one pixel is in also in the big image
			// we do not need to test that (x_big,y_big) is a valid pixel in the big image.
			const float* p_big = &img_big(x_big, y_big);
			sum += *(p_big);
			if(x_big + 1 < w_big) {
				sum += *(p_big + 1);
			}
			if(y_big + 1 < h_big) {
				sum += *(p_big + w_big);
				if(x_big + 1 < w_big) {
					sum += *(p_big + w_big + 1);
				}
			}
			img_small(x, y) = sum;
		}
	}
	return img_small;
}

Eigen::MatrixXf ScaleUp(const Eigen::MatrixXf& img_small, const unsigned int S)
{
	if(S == 0) {
		return Eigen::MatrixXf::Zero(0,0);
	}
	if(S == 1) {
		return img_small;
	}
	// size of original image
	const unsigned int w_sma = img_small.rows();
	const unsigned int h_sma = img_small.cols();
	// size of scaled up image
	const unsigned int w_big = w_sma * S;
	const unsigned int h_big = h_sma * S;
	Eigen::MatrixXf img_big(w_big, h_big);
	for(unsigned int y=0; y<h_sma; ++y) {
		const unsigned int y_big = S*y;
		for(unsigned int x=0; x<w_sma; ++x) {
			const unsigned int x_big = S*x;
			const float val = img_small(x, y);
			for(unsigned int i=0; i<S; ++i) {
				for(unsigned int j=0; j<S; ++j) {
					img_big(x_big+j, y_big+i) = val;
				}
			}
		}
	}
	return img_big;
}

std::vector<Eigen::MatrixXf> ComputeMipmaps(const Eigen::MatrixXf& img, unsigned int min_size)
{
	// find number of required mipmap level
	unsigned int max_size = std::max(img.rows(), img.cols());
	int n_mipmaps = Danvil::MoreMath::PowerOfTwoExponent(max_size);
	n_mipmaps -= Danvil::MoreMath::PowerOfTwoExponent(min_size);
	BOOST_ASSERT(n_mipmaps >= 1);
	std::vector<Eigen::MatrixXf> mipmaps(n_mipmaps);
	mipmaps[0] = SumMipMapWithBlackBorder(img);
	// create remaining mipmaps
	for(unsigned int i=1; i<n_mipmaps; i++) {
		BOOST_ASSERT(mipmaps[i-1].rows() == mipmaps[i-1].cols());
		BOOST_ASSERT(mipmaps[i-1].rows() >= 1);
		mipmaps[i] = SumMipMap<2>(mipmaps[i - 1]);
//		std::cout << std::accumulate(mipmaps[i].begin(), mipmaps[i].end(), 0.0f, [](float sum, float x) { return sum + x; }) << std::endl;
	}
	return mipmaps;
}

std::vector<Eigen::MatrixXf> ComputeMipmapsLevels(const Eigen::MatrixXf& img, unsigned int n_mipmaps)
{
	// find number of required mipmap level
	unsigned int max_size = std::max(img.rows(), img.cols());
	BOOST_ASSERT(n_mipmaps >= 1);
	std::vector<Eigen::MatrixXf> mipmaps(n_mipmaps);
	mipmaps[0] = SumMipMapWithBlackBorder(img);
	// create remaining mipmaps
	for(unsigned int i=1; i<n_mipmaps; i++) {
		BOOST_ASSERT(mipmaps[i-1].rows() == mipmaps[i-1].cols());
		BOOST_ASSERT(mipmaps[i-1].rows() >= 1);
		mipmaps[i] = SumMipMap<2>(mipmaps[i - 1]);
//		std::cout << std::accumulate(mipmaps[i].begin(), mipmaps[i].end(), 0.0f, [](float sum, float x) { return sum + x; }) << std::endl;
	}
	return mipmaps;
}

std::vector<Eigen::MatrixXf> ComputeMipmaps640x480(const Eigen::MatrixXf& img)
{
	// 640 = 4*32*5
	// 480 = 3*32*5
	// 32 = 2^5
	if(img.rows() != 640 || img.cols() != 480) {
		throw std::runtime_error("ERROR: ComputeMipmaps640x480 required size 640x480!");
	}
	std::vector<Eigen::MatrixXf> v(6);
	v[0] = SumMipMap<5>(img);
	for(unsigned int i=0; i<5; i++) {
		v[i+1] = SumMipMap<2>(v[i]);
	}
	return v;
}

std::vector<std::pair<Eigen::MatrixXf,Eigen::MatrixXf>> ComputeMipmapsWithAbs(const Eigen::MatrixXf& img, unsigned int min_size)
{
	// find number of required mipmap level
	unsigned int max_size = std::max(img.rows(), img.cols());
	int n_mipmaps = Danvil::MoreMath::PowerOfTwoExponent(max_size);
	n_mipmaps -= Danvil::MoreMath::PowerOfTwoExponent(min_size);
	BOOST_ASSERT(n_mipmaps >= 1);
	std::vector<std::pair<Eigen::MatrixXf,Eigen::MatrixXf>> mipmaps(n_mipmaps + 1);
	mipmaps[0].first = img;
	mipmaps[0].second = img.cwiseAbs();
	mipmaps[1].first = SumMipMapWithBlackBorder(mipmaps[0].first);
	mipmaps[1].second = SumMipMapWithBlackBorder(mipmaps[0].second);
	for(unsigned int i=2; i<=n_mipmaps; i++) {
		//BOOST_ASSERT(mipmaps[i-1].width() == mipmaps[i-1].height());
		//BOOST_ASSERT(mipmaps[i-1].width() >= 1);
		mipmaps[i].first = SumMipMap<2>(mipmaps[i - 1].first);
		mipmaps[i].second = SumMipMap<2>(mipmaps[i - 1].second);
	}
	return mipmaps;
}

//----------------------------------------------------------------------------//
}
//----------------------------------------------------------------------------//


//
// Copyright (c) 2015-2020 CNRS INRIA
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

#include "pinocchio/math/fwd.hpp"
#include "pinocchio/multibody/joint/joints.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"

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

using namespace pinocchio;

template<typename D>
void addJointAndBody(Model & model,
                     const JointModelBase<D> & jmodel,
                     const Model::JointIndex parent_id,
                     const SE3 & joint_placement,
                     const std::string & joint_name,
                     const Inertia & Y)
{
  Model::JointIndex idx;

  idx = model.addJoint(parent_id,jmodel,joint_placement,joint_name);
  model.appendBodyToJoint(idx,Y);
}

BOOST_AUTO_TEST_SUITE(JointPlanar)

BOOST_AUTO_TEST_CASE(spatial)
{
  SE3 M(SE3::Random());
  Motion v(Motion::Random());

  MotionPlanar mp(1.,2.,3.);
  Motion mp_dense(mp);

  BOOST_CHECK(M.act(mp).isApprox(M.act(mp_dense)));
  BOOST_CHECK(M.actInv(mp).isApprox(M.actInv(mp_dense)));

  BOOST_CHECK(v.cross(mp).isApprox(v.cross(mp_dense)));
}

BOOST_AUTO_TEST_CASE(vsFreeFlyer)
{
  using namespace pinocchio;
  typedef SE3::Vector3 Vector3;
  typedef Eigen::Matrix <double, 6, 1> Vector6;
  typedef Eigen::Matrix <double, 4, 1> VectorPl;
  typedef Eigen::Matrix <double, 7, 1> VectorFF;
  typedef SE3::Matrix3 Matrix3;

  Model modelPlanar, modelFreeflyer;

  Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
  SE3 pos(1); pos.translation() = SE3::LinearType(1.,0.,0.);

  addJointAndBody(modelPlanar,JointModelPlanar(),0,SE3::Identity(),"planar",inertia);
  addJointAndBody(modelFreeflyer,JointModelFreeFlyer(),0,SE3::Identity(),"free-flyer",inertia);

  Data dataPlanar(modelPlanar);
  Data dataFreeFlyer(modelFreeflyer);

  VectorPl q; q << 1, 1, 0, 1; // Angle is PI /2;
  VectorFF qff; qff << 1, 1, 0, 0, 0, sqrt(2)/2, sqrt(2)/2 ;
  Eigen::VectorXd v = Eigen::VectorXd::Ones(modelPlanar.nv);
  Vector6 vff; vff << 1, 1, 0, 0, 0, 1;
  Eigen::VectorXd tauPlanar = Eigen::VectorXd::Ones(modelPlanar.nv);
  Eigen::VectorXd tauff = Eigen::VectorXd::Ones(modelFreeflyer.nv);
  Eigen::VectorXd aPlanar = Eigen::VectorXd::Ones(modelPlanar.nv);
  Eigen::VectorXd aff(vff);

  forwardKinematics(modelPlanar, dataPlanar, q, v);
  forwardKinematics(modelFreeflyer, dataFreeFlyer, qff, vff);

  computeAllTerms(modelPlanar, dataPlanar, q, v);
  computeAllTerms(modelFreeflyer, dataFreeFlyer, qff, vff);

  BOOST_CHECK(dataFreeFlyer.oMi[1].isApprox(dataPlanar.oMi[1]));
  BOOST_CHECK(dataFreeFlyer.liMi[1].isApprox(dataPlanar.liMi[1]));
  BOOST_CHECK(dataFreeFlyer.Ycrb[1].matrix().isApprox(dataPlanar.Ycrb[1].matrix()));
  BOOST_CHECK(dataFreeFlyer.f[1].toVector().isApprox(dataPlanar.f[1].toVector()));

  Eigen::VectorXd nle_expected_ff(3); nle_expected_ff << dataFreeFlyer.nle[0],
                                                         dataFreeFlyer.nle[1],
                                                         dataFreeFlyer.nle[5]
                                                         ;
  BOOST_CHECK(nle_expected_ff.isApprox(dataPlanar.nle));
  BOOST_CHECK(dataFreeFlyer.com[0].isApprox(dataPlanar.com[0]));

  // InverseDynamics == rnea
  tauPlanar = rnea(modelPlanar, dataPlanar, q, v, aPlanar);
  tauff = rnea(modelFreeflyer, dataFreeFlyer, qff, vff, aff);

  Vector3 tau_expected; tau_expected << tauff(0), tauff(1), tauff(5);
  BOOST_CHECK(tauPlanar.isApprox(tau_expected));

  // ForwardDynamics == aba
  Eigen::VectorXd aAbaPlanar = aba(modelPlanar,dataPlanar, q, v, tauPlanar);
  Eigen::VectorXd aAbaFreeFlyer = aba(modelFreeflyer,dataFreeFlyer, qff, vff, tauff);
  Vector3 a_expected; a_expected << aAbaFreeFlyer[0],
                                    aAbaFreeFlyer[1],
                                    aAbaFreeFlyer[5]
                                    ;
  BOOST_CHECK(aAbaPlanar.isApprox(a_expected));

  // crba
  crba(modelPlanar, dataPlanar,q);
  crba(modelFreeflyer, dataFreeFlyer, qff);

  Eigen::Matrix<double, 3, 3> M_expected;
  M_expected.block<2,2>(0,0) = dataFreeFlyer.M.block<2,2>(0,0);
  M_expected.block<1,2>(2,0) = dataFreeFlyer.M.block<1,2>(5,0);
  M_expected.block<2,1>(0,2) = dataFreeFlyer.M.col(5).head<2>();
  M_expected.block<1,1>(2,2) = dataFreeFlyer.M.col(5).tail<1>();

  BOOST_CHECK(dataPlanar.M.isApprox(M_expected));

  // Jacobian
  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_planar;jacobian_planar.resize(6,3); jacobian_planar.setZero();
  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_ff;jacobian_ff.resize(6,6);jacobian_ff.setZero();
  computeJointJacobians(modelPlanar, dataPlanar, q);
  computeJointJacobians(modelFreeflyer, dataFreeFlyer, qff);
  getJointJacobian(modelPlanar, dataPlanar, 1, LOCAL, jacobian_planar);
  getJointJacobian(modelFreeflyer, dataFreeFlyer, 1, LOCAL, jacobian_ff);

  Eigen::Matrix<double, 6, 3> jacobian_expected; jacobian_expected << jacobian_ff.col(0),
                                                                      jacobian_ff.col(1),
                                                                      jacobian_ff.col(5)
                                                                      ;

  BOOST_CHECK(jacobian_planar.isApprox(jacobian_expected));
}

BOOST_AUTO_TEST_SUITE_END()


#include "integration.hpp"
#include <iostream>
#include <Eigen/Sparse>
#include <Eigen/IterativeLinearSolvers>
using namespace std;

void explicitEulerStep(PhysicalSystem *system, double dt) {
    forwardEulerStep(system, dt);
}

void forwardEulerStep(PhysicalSystem *system, double dt) {
    int n = system->getDOFs();
    VectorXd x0(n), v0(n);
    system->getState(x0, v0);
    MatrixXd M(n,n);
    system->getInertia(M);
    VectorXd f0(n);
    system->getForces(f0);
    VectorXd a0(n); // acceleration
    for (int i = 0; i < n; i++)
        a0(i) = f0(i)/M(i,i);
    VectorXd x1 = x0 + v0*dt;
    VectorXd v1 = v0 + a0*dt;
    system->setState(x1, v1);
}

VectorXd solve(const MatrixXd &A, const VectorXd &b) {
    SparseMatrix<double> spA = A.sparseView();
    ConjugateGradient< SparseMatrix<double> > solver;
    solver.setTolerance(1e-3);
    return solver.compute(spA).solve(b);
}

void backwardEulerStep(PhysicalSystem *system, double dt) {
    int n = system->getDOFs();
    VectorXd x0(n), v0(n);
    system->getState(x0, v0);
    static MatrixXd M(n,n);
    system->getInertia(M);
    VectorXd f(n);
    static MatrixXd Jx(n,n), Jv(n,n);
    system->getForces(f);
    system->getJacobians(Jx, Jv);
    MatrixXd A = (M - Jx*dt*dt - Jv*dt);
    VectorXd b = (f + Jx*v0*dt)*dt;
    VectorXd v1 = v0 + solve(A, b);
    system->setState(x0 + v1*dt, v1);
}