// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// Eigen is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 3 of the License, or (at your option) any later version.
//
// Alternatively, you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of
// the License, or (at your option) any later version.
//
// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.

#include "main.h"
#include <Eigen/LU>

template<typename Derived>
void doSomeRankPreservingOperations(Eigen::MatrixBase<Derived>& m)
{
  typedef typename Derived::RealScalar RealScalar;
  for(int a = 0; a < 3*(m.rows()+m.cols()); a++)
  {
    RealScalar d = Eigen::ei_random<RealScalar>(-1,1);
    int i = Eigen::ei_random<int>(0,m.rows()-1); // i is a random row number
    int j;
    do {
      j = Eigen::ei_random<int>(0,m.rows()-1);
    } while (i==j); // j is another one (must be different)
    m.row(i) += d * m.row(j);

    i = Eigen::ei_random<int>(0,m.cols()-1); // i is a random column number
    do {
      j = Eigen::ei_random<int>(0,m.cols()-1);
    } while (i==j); // j is another one (must be different)
    m.col(i) += d * m.col(j);
  }
}

template<typename MatrixType> void lu_non_invertible()
{
  /* this test covers the following files:
     LU.h
  */
  // NOTE there seems to be a problem with too small sizes -- could easily lie in the doSomeRankPreservingOperations function
  int rows = ei_random<int>(20,200), cols = ei_random<int>(20,200), cols2 = ei_random<int>(20,200);
  int rank = ei_random<int>(1, std::min(rows, cols)-1);

  MatrixType m1(rows, cols), m2(cols, cols2), m3(rows, cols2), k(1,1);
  m1 = MatrixType::Random(rows,cols);
  if(rows <= cols)
    for(int i = rank; i < rows; i++) m1.row(i).setZero();
  else
    for(int i = rank; i < cols; i++) m1.col(i).setZero();
  doSomeRankPreservingOperations(m1);

  LU<MatrixType> lu(m1);
  typename LU<MatrixType>::KernelResultType m1kernel = lu.kernel();
  typename LU<MatrixType>::ImageResultType m1image = lu.image();

  VERIFY(rank == lu.rank());
  VERIFY(cols - lu.rank() == lu.dimensionOfKernel());
  VERIFY(!lu.isInjective());
  VERIFY(!lu.isInvertible());
  VERIFY(lu.isSurjective() == (lu.rank() == rows));
  VERIFY((m1 * m1kernel).isMuchSmallerThan(m1));
  VERIFY(m1image.lu().rank() == rank);
  MatrixType sidebyside(m1.rows(), m1.cols() + m1image.cols());
  sidebyside << m1, m1image;
  VERIFY(sidebyside.lu().rank() == rank);
  m2 = MatrixType::Random(cols,cols2);
  m3 = m1*m2;
  m2 = MatrixType::Random(cols,cols2);
  lu.solve(m3, &m2);
  VERIFY_IS_APPROX(m3, m1*m2);
  m3 = MatrixType::Random(rows,cols2);
  VERIFY(!lu.solve(m3, &m2));
}

template<typename MatrixType> void lu_invertible()
{
  /* this test covers the following files:
     LU.h
  */
  typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
  int size = ei_random<int>(10,200);

  MatrixType m1(size, size), m2(size, size), m3(size, size);
  m1 = MatrixType::Random(size,size);

  if (ei_is_same_type<RealScalar,float>::ret)
  {
    // let's build a matrix more stable to inverse
    MatrixType a = MatrixType::Random(size,size*2);
    m1 += a * a.adjoint();
  }

  LU<MatrixType> lu(m1);
  VERIFY(0 == lu.dimensionOfKernel());
  VERIFY(size == lu.rank());
  VERIFY(lu.isInjective());
  VERIFY(lu.isSurjective());
  VERIFY(lu.isInvertible());
  VERIFY(lu.image().lu().isInvertible());
  m3 = MatrixType::Random(size,size);
  lu.solve(m3, &m2);
  VERIFY_IS_APPROX(m3, m1*m2);
  VERIFY_IS_APPROX(m2, lu.inverse()*m3);
  m3 = MatrixType::Random(size,size);
  VERIFY(lu.solve(m3, &m2));
}

void test_lu()
{
  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST( lu_non_invertible<MatrixXf>() );
    CALL_SUBTEST( lu_non_invertible<MatrixXd>() );
    CALL_SUBTEST( lu_non_invertible<MatrixXcf>() );
    CALL_SUBTEST( lu_non_invertible<MatrixXcd>() );
    CALL_SUBTEST( lu_invertible<MatrixXf>() );
    CALL_SUBTEST( lu_invertible<MatrixXd>() );
    CALL_SUBTEST( lu_invertible<MatrixXcf>() );
    CALL_SUBTEST( lu_invertible<MatrixXcd>() );
  }
}


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/U.B.P.
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ. Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_SQR_ABS_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SQR_ABS_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

namespace boost { namespace simd { namespace tag
  {
   /*!
      @brief  sqr_abs generic tag

      Represents the sqr_abs function in generic contexts.

      @par Models:
      Hierarchy
    **/
    struct sqr_abs_ : ext::elementwise_<sqr_abs_>
    {
      /// @brief Parent hierarchy
      typedef ext::elementwise_<sqr_abs_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_sqr_abs_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
   template<class Site>
   BOOST_FORCEINLINE generic_dispatcher<tag::sqr_abs_, Site> dispatching_sqr_abs_(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
   {
     return generic_dispatcher<tag::sqr_abs_, Site>();
   }
   template<class... Args>
   struct impl_sqr_abs_;
  }
  /*!
    Computes the square of the absolute value of its parameter.

    @par semantic:
    For any given value @c x of type @c T:

    @code
    T r = sqr_abs(x);
    @endcode

    is equivalent to:

    @code
    T r = sqr(abs(x));
    @endcode

    @par Alias

    @c sqr_modulus

    @param  a0

    @return      a value of the same type as the input.

  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::sqr_abs_, sqr_abs, 1)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::sqr_abs_, sqr_modulus, 1)
} }

#endif


#include "test_benchmarks_boost.h"

#include <boost/config.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/r_c_shortest_paths.hpp>
#include <fstream>
#include <iostream> // cout
#include <sstream>
#include <vector>

#include "utils.h" // loadMaxMinRes, skipLines, writeToFile, getElapsedTime, getBestCost

namespace boost {

/**
 * Define all the necessary data structures
 * Based on:
 * https://www.boost.org/doc/libs/1_76_0/libs/graph/example/r_c_shortest_paths_example.cpp
 */
struct MyVertex {
  MyVertex(const int& n = 0) : id(n) {}
  int id;
};

struct MyAdjVertex {
  MyAdjVertex(
      const int&                 n = 0,
      const double&              w = 0.0,
      const std::vector<double>& r = {})
      : id(n), weight(w), res(r) {}
  int                 id;
  double              weight;
  std::vector<double> res;
};

typedef adjacency_list<vecS, vecS, directedS, MyVertex, MyAdjVertex> MyGraph;

// ResourceContainer model
struct MyResourceContainer {
  MyResourceContainer(
      const double&              w          = 0.0,
      const std::vector<double>& r          = {},
      const std::vector<double>& min_res_in = {},
      const std::vector<double>& max_res_in = {})
      : weight(w), res(r), min_res(min_res_in), max_res(max_res_in) {}

  MyResourceContainer& operator=(const MyResourceContainer& other) {
    if (this == &other)
      return *this;
    this->~MyResourceContainer();
    new (this) MyResourceContainer(other);
    return *this;
  }

  double weight;
  // Current resource usage
  std::vector<double> res;
  // Bounds
  std::vector<double> min_res;
  std::vector<double> max_res;
};

bool operator==(
    const MyResourceContainer& res_cont_1,
    const MyResourceContainer& res_cont_2) {
  if (res_cont_1.weight != res_cont_2.weight) {
    return false;
  }
  // Check resources one by one
  const int& num_res = res_cont_1.res.size();
  for (int r = 0; r < num_res; ++r) {
    if (res_cont_1.res[r] != res_cont_2.res[r]) {
      return false;
    }
  }
  return true;
}

bool operator<(
    const MyResourceContainer& res_cont_1,
    const MyResourceContainer& res_cont_2) {
  if (res_cont_1.weight > res_cont_2.weight) {
    return false;
  }
  const int& num_res = res_cont_1.res.size();
  for (int r = 0; r < num_res; ++r) {
    if (res_cont_1.res[r] >= res_cont_2.res[r]) {
      return false;
    }
  }
  return true;
}

// ResourceExtensionFunction model
class MyResourceExtensionFunction {
 public:
  inline bool operator()(
      const MyGraph&                         g,
      MyResourceContainer&                   new_cont,
      const MyResourceContainer&             old_cont,
      graph_traits<MyGraph>::edge_descriptor ed) const {
    const MyAdjVertex& arc_prop  = get(edge_bundle, g)[ed];
    const MyVertex&    vert_prop = get(vertex_bundle, g)[target(ed, g)];
    new_cont.weight              = old_cont.weight + arc_prop.weight;
    // Pass min / max resources
    new_cont.min_res = old_cont.min_res;
    new_cont.max_res = old_cont.max_res;

    std::vector<double> new_resources = old_cont.res;
    std::transform(
        new_resources.begin(),
        new_resources.end(),
        arc_prop.res.begin(),
        new_resources.begin(),
        std::plus<double>());
    new_cont.res = new_resources;
    // Check resource feasibility
    const int&                 num_res = old_cont.res.size();
    const std::vector<double>& min_res = old_cont.min_res;
    const std::vector<double>& max_res = old_cont.max_res;
    for (int r = 0; r < num_res; ++r) {
      if (min_res[r] <= new_cont.res[r] && new_cont.res[r] <= max_res[r]) {
        ;
      } else {
        return false;
      }
    }
    return true;
  }
};

// DominanceFunction model
class MyDominanceFunction {
 public:
  inline bool operator()(
      const MyResourceContainer& res_cont_1,
      const MyResourceContainer& res_cont_2) const {
    const int& num_res = res_cont_1.res.size();
    if (res_cont_1.weight > res_cont_2.weight) {
      return false;
    }
    for (int i = 0; i < num_res; i++) {
      if (res_cont_1.res[i] > res_cont_2.res[i]) {
        return false;
      }
    }
    return true;
  }
};

void loadGraph(
    MyGraph&           my_graph,
    const int&         num_nodes,
    const int&         num_arcs,
    const int&         num_resources,
    const std::string& path_to_instance) {
  // Init nodes
  for (int i = 1; i <= num_nodes; ++i) {
    boost::add_vertex(boost::MyVertex(i), my_graph);
  }
  // Load graph
  std::ifstream      instance_file(path_to_instance);
  std::istringstream iss;
  std::string        line;

  skipLines(instance_file, line, 3);
  skipLines(instance_file, line, num_nodes);

  for (int i = 0; i < num_arcs; ++i) {
    std::getline(instance_file, line);
    iss.clear();
    iss.str(line);
    int                 tail, head;
    double              weight;
    std::vector<double> res_cost(num_resources, 0.0);
    iss >> tail >> head >> weight;
    for (int j = 0; j < num_resources; ++j) {
      iss >> res_cost[j];
    }
    if (tail == 1) {
      add_edge(1, head, MyAdjVertex(i, weight, res_cost), my_graph);
    } else if (head == 1) {
      ;
    } else if (head == num_nodes) {
      add_edge(tail, num_nodes, MyAdjVertex(i, weight, res_cost), my_graph);
    } else if (tail == num_nodes) {
      ;
    } else {
      add_edge(tail, head, MyAdjVertex(i, weight, res_cost), my_graph);
    }
  }
}

/* Instance tests */

TEST_P(TestBenchmarksBoost, test_r_c_shortest_paths) {
  const std::string path_to_instance =
      path_to_data + "rcsp" + std::to_string(instance_number) + ".txt";
  int                 num_nodes, num_arcs, num_resources;
  std::vector<double> max_res, min_res;
  loadMaxMinRes(
      &max_res,
      &min_res,
      &num_nodes,
      &num_arcs,
      &num_resources,
      path_to_instance);

  MyGraph my_graph;
  loadGraph(my_graph, num_nodes, num_arcs, num_resources, path_to_instance);

  clock_t start = clock();
  std::vector<std::vector<graph_traits<MyGraph>::edge_descriptor>>
                                   opt_solutions;
  std::vector<MyResourceContainer> pareto_opt;
  // Source, sink
  graph_traits<MyGraph>::vertex_descriptor s = 1;
  graph_traits<MyGraph>::vertex_descriptor t = num_nodes;

  std::vector<double> zeros(num_resources, 0.0);

  r_c_shortest_paths(
      my_graph,
      get(&MyVertex::id, my_graph),
      get(&MyAdjVertex::id, my_graph),
      s,
      t,
      opt_solutions,
      pareto_opt,
      MyResourceContainer(0.0, zeros, min_res, max_res),
      MyResourceExtensionFunction(),
      MyDominanceFunction(),
      std::allocator<r_c_shortest_paths_label<MyGraph, MyResourceContainer>>(),
      default_r_c_shortest_paths_visitor());

  double     cost               = 1000000.0;
  const int& opt_solutions_size = opt_solutions.size();
  for (int i = 0; i < opt_solutions_size; ++i) {
    if (pareto_opt[i].weight < cost) {
      cost = pareto_opt[i].weight;
    }
  }
  if (instance_number == 14) {
    ASSERT_EQ(opt_solutions_size, 0);
  } else {
    ASSERT_EQ(cost, getBestCost(path_to_data, instance_number));
  }
  writeToFile(
      "/root/build/",
      "results_boost.txt",
      std::to_string(instance_number) + " " +
          std::to_string(getElapsedTime(start)));
}

INSTANTIATE_TEST_SUITE_P(
    TestBenchmarksBeasleyBoost,
    TestBenchmarksBoost,
    ::testing::Range(1, 25));

} // namespace boost


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    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 Delft University of Technology 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.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      110501    L. van der Ham    File created.
 *      110607    L. van der Ham    Make code compatible with Tudat revision 114.
 *      110629    L. van der Ham    Modifications according to comments first code check.
 *      110710    K. Kumar          Restructured code; added subtests.
 *      111024    K. Kumar          Error spotted in L1/L2 tests; locations seem swapped.
 *                                  Tests commented out; needs to be fixed.
 *      111027    K. Kumar          Uncommented out tests as bugs fixed by L. van der Ham.
 *      120307    K. Kumar          Moved file.
 *      120326    D. Dirkx          Changed raw pointers to shared pointers.
 *      120518    K. Kumar          Boostified unit test.
 *      120813    P. Musegaas       Updated unit test to new root finding structure.
 *      130319    K. Kumar          Removed dependence on obsolete central gravity field to new
 *                                  celestial body constants file.
 *
 *    References
 *      Mireles James, J.D. Celestial Mechanics Notes Set 4: The Circular Restricted Three Body
 *          Problem, 2006, http://www.math.utexas.edu/users/jjames/hw4Notes.pdf,
 *          last accessed: 26 May, 2012.
 *      JPL, NASA. Astrodynamic Constants, http://ssd.jpl.nasa.gov/?constants,
 *        last updated: 13 Dec, 2012, last accessed: 19th March, 2013.
 *
 *    Notes
 *      Reference values for position Lagrange libration points are taken from (Mireles James,
 *      2006). There seems to be a bug in the computation of the L3 location! Note that this code
 *      uses the Planet class (in Bodies sub-directory), which is marked for deprecation.
 *
 */

#define BOOST_TEST_MAIN

#include <cmath>
#include <limits>

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

#include "Tudat/Astrodynamics/BasicAstrodynamics/celestialBodyConstants.h"
#include "Tudat/Astrodynamics/Gravitation/librationPoint.h"
#include "Tudat/Mathematics/RootFinders/newtonRaphson.h"

namespace tudat
{
namespace unit_tests
{

namespace crtbp = gravitation::circular_restricted_three_body_problem;
using namespace root_finders;

BOOST_AUTO_TEST_SUITE( test_libration_points )

//! Test if computation of mass parameter is working correctly.
BOOST_AUTO_TEST_CASE( testComputationOfMassParameter )
{
    // Set expected mass parameter for Earth-Moon system.
    const double expectedMassParameter = 0.0121505811805237;

    // Set Earth gravitational parameter.
    const double earthGravitationalParameter
            = celestial_body_constants::EARTH_GRAVITATIONAL_PARAMETER;

    // Set Moon gravitational parameter (Earth/Moon mass ratio taken from (JPL, 2012).
    const double moonGravitationalParameter = earthGravitationalParameter / 81.30059;

    // Compute mass parameter.
    const double computedMassParameter = crtbp::computeMassParameter(
                earthGravitationalParameter, moonGravitationalParameter );

    // Check if computed value corresponds to expected mass parameter.
    BOOST_CHECK_CLOSE_FRACTION( expectedMassParameter, computedMassParameter, 1.0e-14 );
}

//! Test if computation of location of L1 Lagrange libration point is working correctly.
BOOST_AUTO_TEST_CASE( testComputationOfLocationOfL1LibrationPoint )
{
    // Declare and initialize Earth-Moon mass parameter from (Mireles James, 2006).
    const double earthMoonMassParameter = 0.012277471;

    // Set expected location of L1.
    const Eigen::Vector3d expectedLocationOfL1( 0.83629259089993, 0.0, 0.0 );

    // Declare L1 libration point object with Earth-Moon mass parameter and Newton-Raphson method
    // with 1000 iterations as maximum and 1.0e-14 relative X-tolerance.
    crtbp::LibrationPoint librationPointL1( earthMoonMassParameter,
                                            boost::make_shared< NewtonRaphson >( 1.0e-14, 1000 ) );

    // Compute location of Lagrange libration point.
    librationPointL1.computeLocationOfLibrationPoint( crtbp::LibrationPoint::l1 );

    // Determine location of libration point in Earth-Moon system.
    const Eigen::Vector3d positionOflibrationPointL1
            = librationPointL1.getLocationOfLagrangeLibrationPoint( );

    // Check if computed location of L1 matches expected location.
    BOOST_CHECK_CLOSE_FRACTION( expectedLocationOfL1.x( ),
                                positionOflibrationPointL1.x( ),
                                1.0e-14 );
    BOOST_CHECK_SMALL( positionOflibrationPointL1.y( ), std::numeric_limits< double >::min( ) );
    BOOST_CHECK_SMALL( positionOflibrationPointL1.z( ), std::numeric_limits< double >::min( ) );
}

//! Test if computation of location of L2 Lagrange libration point is working correctly.
BOOST_AUTO_TEST_CASE( testComputationOfLocationOfL2LibrationPoint )
{
    // Declare and initialize Earth-Moon mass parameter from (Mireles James, 2006).
    const double earthMoonMassParameter = 0.012277471;

    // Set expected location of L2.
    const Eigen::Vector3d expectedLocationOfL2( 1.15616816590553, 0.0, 0.0 );

    // Declare L2 libration point object with Earth-Moon mass parameter and Newton-Raphson method
    // with 1000 iterations as maximum and 1.0e-14 relative X-tolerance.
    crtbp::LibrationPoint librationPointL2( earthMoonMassParameter,
                                            boost::make_shared< NewtonRaphson >( 1.0e-14, 1000 ) );

    // Compute location of Lagrange libration point.
    librationPointL2.computeLocationOfLibrationPoint( crtbp::LibrationPoint::l2 );

    // Determine location of libration point in Earth-Moon system.
    const Eigen::Vector3d positionOflibrationPointL2
            = librationPointL2.getLocationOfLagrangeLibrationPoint( );

    // Check if computed location of L2 matches expected location.
    BOOST_CHECK_CLOSE_FRACTION( expectedLocationOfL2.x( ),
                                positionOflibrationPointL2.x( ),
                                1.0e-14 );
    BOOST_CHECK_SMALL( positionOflibrationPointL2.y( ), std::numeric_limits< double >::min( ) );
    BOOST_CHECK_SMALL( positionOflibrationPointL2.z( ), std::numeric_limits< double >::min( ) );
}

//! Test if computation of location of L3 Lagrange libration point is working correctly.
// THERE IS A BUG IN THIS CASE!
BOOST_AUTO_TEST_CASE( testComputationOfLocationOfL3LibrationPoint )
{
    // Declare and initialize Earth-Moon mass parameter from (Mireles James, 2006).
    const double earthMoonMassParameter = 0.012277471;

    // Set expected location of L3.
    const Eigen::Vector3d expectedLocationOfL3( -1.00511551160689, 0.0, 0.0 );

    // Declare L3 libration point object with Earth-Moon mass parameter and Newton-Raphson method
    // with 1000 iterations as maximum and 1.0e-14 relative X-tolerance.
    crtbp::LibrationPoint librationPointL3( earthMoonMassParameter,
                                            boost::make_shared< NewtonRaphson >( 1.0e-14, 1000 ) );

    // Compute location of Lagrange libration point.
    librationPointL3.computeLocationOfLibrationPoint( crtbp::LibrationPoint::l3 );

    // Determine location of libration point in Earth-Moon system.
    const Eigen::Vector3d positionOflibrationPointL3
            = librationPointL3.getLocationOfLagrangeLibrationPoint( );

    // Check if computed location of L3 matches expected location.
    BOOST_CHECK_CLOSE_FRACTION( expectedLocationOfL3.x( ),
                                positionOflibrationPointL3.x( ),
                                1.0e-2 );
    BOOST_CHECK_SMALL( positionOflibrationPointL3.y( ), std::numeric_limits< double >::min( ) );
    BOOST_CHECK_SMALL( positionOflibrationPointL3.z( ), std::numeric_limits< double >::min( ) );
}

//! Test if computation of location of L4 Lagrange libration point is working correctly.
BOOST_AUTO_TEST_CASE( testComputationOfLocationOfL4LibrationPoint )
{
    // Declare and initialize Earth-Moon mass parameter from (Mireles James, 2006).
    const double earthMoonMassParameter = 0.012277471;

    // Set expected location of L4.
    const Eigen::Vector3d expectedLocationOfL4( 0.487722529, 0.86602540378444, 0.0 );

    // Declare L4 libration point object with Earth-Moon mass parameter and Newton-Raphson method
    // with 1000 iterations as maximum and 1.0e-14 relative X-tolerance.
    crtbp::LibrationPoint librationPointL4( earthMoonMassParameter,
                                            boost::make_shared< NewtonRaphson >( 1.0e-14, 1000 ) );

    // Compute location of Lagrange libration point.
    librationPointL4.computeLocationOfLibrationPoint( crtbp::LibrationPoint::l4 );

    // Determine location of libration point in Earth-Moon system.
    const Eigen::Vector3d positionOflibrationPointL4
            = librationPointL4.getLocationOfLagrangeLibrationPoint( );

    // Check if computed location of L4 matches expected location.
    BOOST_CHECK_CLOSE_FRACTION( expectedLocationOfL4.x( ),
                                positionOflibrationPointL4.x( ),
                                1.0e-15 );
    BOOST_CHECK_CLOSE_FRACTION( expectedLocationOfL4.y( ),
                                positionOflibrationPointL4.y( ),
                                1.0e-14 );
    BOOST_CHECK_SMALL( positionOflibrationPointL4.z( ), std::numeric_limits< double >::min( ) );
}

//! Test if computation of location of L5 Lagrange libration point is working correctly.
BOOST_AUTO_TEST_CASE( testComputationOfLocationOfL5LibrationPoint )
{
    // Declare and initialize Earth-Moon mass parameter from (Mireles James, 2006).
    const double earthMoonMassParameter = 0.012277471;

    // Set expected location of L5.
    const Eigen::Vector3d expectedLocationOfL5( 0.487722529, -0.86602540378444, 0.0 );

    // Declare L5 libration point object with Earth-Moon mass parameter and Newton-Raphson method
    // with 1000 iterations as maximum and 1.0e-14 relative X-tolerance.
    crtbp::LibrationPoint librationPointL5( earthMoonMassParameter,
                                            boost::make_shared< NewtonRaphson >( 1.0e-14, 1000 ) );

    // Compute location of Lagrange libration point.
    librationPointL5.computeLocationOfLibrationPoint( crtbp::LibrationPoint::l5 );

    // Determine location of libration point in Earth-Moon system.
    const Eigen::Vector3d positionOflibrationPointL5
            = librationPointL5.getLocationOfLagrangeLibrationPoint( );

    // Check if computed location of L5 matches expected location.
    BOOST_CHECK_CLOSE_FRACTION( expectedLocationOfL5.x( ),
                                positionOflibrationPointL5.x( ),
                                1.0e-15 );
    BOOST_CHECK_CLOSE_FRACTION( expectedLocationOfL5.y( ),
                                positionOflibrationPointL5.y( ),
                                1.0e-14 );
    BOOST_CHECK_SMALL( positionOflibrationPointL5.z( ), std::numeric_limits< double >::min( ) );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


#include "libs/ml_models/support_vector_machine.h"
#include "svm_impl/svm_model.h"
#include "svm_impl/classifiers_generator.h"
#include "svm_impl/regressions_generator.h"
//#include "shared_operations.hpp"
#include "utils/missing_values.hpp"
#include <opencv2/core.hpp>
#include <opencv/ml.h>
#include <iostream>
#include <boost/assign.hpp>
#include <map>
//#define _PRINT_MODEL_PARAMS
#ifdef _PRINT_MODEL_PARAMS
#include <boost/range/algorithm/copy.hpp>
#include <iterator>
#endif // _PRINT_MODEL_PARAMS
namespace mlmodels
{

namespace
{

support_vector_machine::model_type
    generate_reg_model(const training_data& data,
                const class_data& classes, const regression_svm::args& a)
{
    if (a.type != svm::type_t::NU_SVR) {
        return support_vector_machine::model_type{};
    }
    switch (a.kernel) {
        case svm::kernel_type::LINEAR:
            return svm::regression::create_model(
                    svm::regression::nu_linear_train{}, data, classes
                );
        case svm::kernel_type::POLY:
            return svm::regression::create_model(
                    svm::regression::nu_poly_train{}, data, classes
                );
        case svm::kernel_type::RBF:
            return svm::regression::create_model(
                        svm::regression::nu_rbf_train{}, data, classes
                    );
        case svm::kernel_type::SIGMOID:
            return svm::regression::create_model(
                        svm::regression::nu_sig_train{}, data, classes
                    );
        default:
            return support_vector_machine::model_type{};
    }
}

support_vector_machine::model_type
    generate_class_model(const training_data& data,
                const class_data& classes, const classifier_svm::args& a)
{
    if (a.type != svm::type_t::C_SVC) {
        return support_vector_machine::model_type{};
    }
    switch (a.kernel) {
        case svm::kernel_type::LINEAR:
            return svm::classifier::create_model(
                    svm::classifier::c_linear_train{}, data, classes
                );
        case svm::kernel_type::POLY:
            return svm::classifier::create_model(
                    svm::classifier::c_poly_train{}, data, classes
                );
        case svm::kernel_type::RBF:
            return svm::classifier::create_model(
                        svm::classifier::c_rbf_train{}, data, classes
                    );
        case svm::kernel_type::SIGMOID:
            return svm::classifier::create_model(
                        svm::classifier::c_sig_train{}, data, classes
                    );
        default:
            return support_vector_machine::model_type{};
    }
}

}   // end of local namespce

svm::kernel_type
    support_vector_machine::string2kernel(const std::string& from)
{
    return svm::str2kernel(from);
}

svm::type_t
    support_vector_machine::string2type(const std::string& from)
{
    return svm::str2type(from);
}

classifier_svm::model_type
classifier_svm::train(const training_data& data,
        const class_data& classes, const args& a) const
{
    return generate_class_model(data, classes, a);
}

support_vector_machine::_args::_args(svm::kernel_type k, svm::type_t t) :
                kernel(k), type(t)
{
}

support_vector_machine::model_type support_vector_machine::load(const char* from)
{
    return svm::load(from);
}

value_type support_vector_machine::predict(model_type model,
                const class_data& samples) const
{
    if (model) {
        return svm::predict(model, samples);
    }
    return missing_value<value_type>();
}

bool support_vector_machine::predict(model_type model,
        const testing_data& data,
        testing_result& predications) const
{
    if (model) {
        predications = svm::predict(model, data);
        return not predications.empty();
    } else {
        return false;
    }
}

support_vector_machine::row_output support_vector_machine::test(model_type model,
        const testing_data& data, int ) const
{
    if (model) {
        return svm::test(model, data);
    } else {
        return row_output{};
    }
}

void save_model(support_vector_machine::model_type model, const char* path)
{
    if (model) {
        svm::save(model, path);
    }
}

classifier_svm::args::args() : base_t{}
{
    type = svm::C_SVC;
}

classifier_svm::args::args(svm::kernel_type k) : base_t{k, svm::C_SVC}
{
}

classifier_svm::row_output classifier_svm::test(model_type model, const testing_data& data,
            int classes) const
{
    return base_t::test(model, data, classes);
}

regression_svm::args::args() :
        base_t{}
{
    type = svm::NU_SVR;
}

regression_svm::args::args(svm::kernel_type k) : base_t{k, svm::NU_SVR}
{
}

regression_svm::model_type regression_svm::train(const training_data& data,
            const class_data& classes, const args& a) const
{
    return generate_reg_model(data, classes, a);
}

regression_svm::row_output regression_svm::test(model_type model, const testing_data& data, int ) const
{
    return base_t::test(model, data, -1);
}

}   // end of namespace mlmodels


#include <iostream>
#include <string>
#include <random>

#include <boost/program_options.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/preprocessor/seq/for_each.hpp>

#if defined(SOLVER_BACKEND_VEXCL)
#  include <amgcl/value_type/static_matrix.hpp>
#  include <amgcl/adapter/block_matrix.hpp>
#  include <amgcl/backend/vexcl.hpp>
#  include <amgcl/backend/vexcl_static_matrix.hpp>
   typedef amgcl::backend::vexcl<double> Backend;
#elif defined(SOLVER_BACKEND_VIENNACL)
#  include <amgcl/backend/viennacl.hpp>
   typedef amgcl::backend::viennacl< viennacl::compressed_matrix<double> > Backend;
#elif defined(SOLVER_BACKEND_CUDA)
#  include <amgcl/backend/cuda.hpp>
#  include <amgcl/relaxation/cusparse_ilu0.hpp>
   typedef amgcl::backend::cuda<double> Backend;
#elif defined(SOLVER_BACKEND_EIGEN)
#  include <amgcl/backend/eigen.hpp>
   typedef amgcl::backend::eigen<double> Backend;
#elif defined(SOLVER_BACKEND_BLAZE)
#  include <amgcl/backend/blaze.hpp>
   typedef amgcl::backend::blaze<double> Backend;
#else
#  ifndef SOLVER_BACKEND_BUILTIN
#    define SOLVER_BACKEND_BUILTIN
#  endif
#  include <amgcl/backend/builtin.hpp>
#  include <amgcl/value_type/static_matrix.hpp>
#  include <amgcl/adapter/block_matrix.hpp>
   typedef amgcl::backend::builtin<double> Backend;
#endif

#include <amgcl/relaxation/runtime.hpp>
#include <amgcl/coarsening/runtime.hpp>
#include <amgcl/coarsening/rigid_body_modes.hpp>
#include <amgcl/solver/runtime.hpp>
#include <amgcl/preconditioner/runtime.hpp>
#include <amgcl/make_solver.hpp>
#include <amgcl/amg.hpp>
#include <amgcl/adapter/crs_tuple.hpp>
#include <amgcl/adapter/reorder.hpp>
#include <amgcl/io/mm.hpp>
#include <amgcl/io/binary.hpp>

#include <amgcl/profiler.hpp>

#include "sample_problem.hpp"

#ifndef AMGCL_BLOCK_SIZES
#  define AMGCL_BLOCK_SIZES (3)(4)
#endif

namespace amgcl { profiler<> prof; }
using amgcl::prof;
using amgcl::precondition;

#ifdef SOLVER_BACKEND_BUILTIN
//---------------------------------------------------------------------------
template <int B>
std::tuple<size_t, double> block_solve(
        const boost::property_tree::ptree &prm,
        size_t rows,
        std::vector<ptrdiff_t> const &ptr,
        std::vector<ptrdiff_t> const &col,
        std::vector<double>    const &val,
        std::vector<double>    const &rhs,
        std::vector<double>          &x,
        bool reorder
        )
{
    typedef amgcl::static_matrix<double, B, B> value_type;
    typedef amgcl::static_matrix<double, B, 1> rhs_type;
    typedef amgcl::backend::builtin<value_type> BBackend;

    typedef amgcl::make_solver<
        amgcl::runtime::preconditioner<BBackend>,
        amgcl::runtime::solver::wrapper<BBackend>
        > Solver;

    auto As = std::tie(rows, ptr, col, val);
    auto Ab = amgcl::adapter::block_matrix<value_type>(As);

    std::tuple<size_t, double> info;

    if (reorder) {
        prof.tic("reorder");
        amgcl::adapter::reorder<> perm(Ab);
        prof.toc("reorder");

        prof.tic("setup");
        Solver solve(perm(Ab), prm);
        prof.toc("setup");

        std::cout << solve << std::endl;

        rhs_type const * fptr = reinterpret_cast<rhs_type const *>(&rhs[0]);
        rhs_type       * xptr = reinterpret_cast<rhs_type       *>(&x[0]);

        amgcl::backend::numa_vector<rhs_type> F(perm(amgcl::make_iterator_range(fptr, fptr + rows/B)));
        amgcl::backend::numa_vector<rhs_type> X(perm(amgcl::make_iterator_range(xptr, xptr + rows/B)));

        prof.tic("solve");
        info = solve(F, X);
        prof.toc("solve");

        perm.inverse(X, xptr);
    } else {
        prof.tic("setup");
        Solver solve(Ab, prm);
        prof.toc("setup");

        std::cout << solve << std::endl;

        rhs_type const * fptr = reinterpret_cast<rhs_type const *>(&rhs[0]);
        rhs_type       * xptr = reinterpret_cast<rhs_type       *>(&x[0]);

        amgcl::backend::numa_vector<rhs_type> F(fptr, fptr + rows/B);
        amgcl::backend::numa_vector<rhs_type> X(xptr, xptr + rows/B);

        prof.tic("solve");
        info = solve(F, X);
        prof.toc("solve");

        std::copy(X.data(), X.data() + X.size(), xptr);
    }

    return info;
}
#endif

#ifdef SOLVER_BACKEND_VEXCL
//---------------------------------------------------------------------------
template <int B>
std::tuple<size_t, double> block_solve(
        const boost::property_tree::ptree &prm,
        size_t rows,
        std::vector<ptrdiff_t> const &ptr,
        std::vector<ptrdiff_t> const &col,
        std::vector<double>    const &val,
        std::vector<double>    const &rhs,
        std::vector<double>          &x,
        bool reorder
        )
{
    typedef amgcl::static_matrix<double, B, B> value_type;
    typedef amgcl::static_matrix<double, B, 1> rhs_type;
    typedef amgcl::backend::vexcl<value_type> BBackend;

    typedef amgcl::make_solver<
        amgcl::runtime::preconditioner<BBackend>,
        amgcl::runtime::solver::wrapper<BBackend>
        > Solver;

    typename BBackend::params bprm;

    vex::Context ctx(vex::Filter::Env);
    std::cout << ctx << std::endl;
    bprm.q = ctx;
    bprm.fast_matrix_setup = prm.get("fast", true);

    vex::scoped_program_header header(ctx,
            amgcl::backend::vexcl_static_matrix_declaration<double,B>());

    auto As = std::tie(rows, ptr, col, val);
    auto Ab = amgcl::adapter::block_matrix<value_type>(As);

    std::tuple<size_t, double> info;

    if (reorder) {
        prof.tic("reorder");
        amgcl::adapter::reorder<> perm(Ab);
        prof.toc("reorder");

        prof.tic("setup");
        Solver solve(perm(Ab), prm, bprm);
        prof.toc("setup");

        std::cout << solve << std::endl;

        rhs_type const * fptr = reinterpret_cast<rhs_type const *>(&rhs[0]);
        rhs_type       * xptr = reinterpret_cast<rhs_type       *>(&x[0]);

        std::vector<rhs_type> tmp(rows / B);

        perm.forward(amgcl::make_iterator_range(fptr, fptr + rows/B), tmp);
        vex::vector<rhs_type> f_b(ctx, tmp);

        perm.forward(amgcl::make_iterator_range(xptr, xptr + rows/B), tmp);
        vex::vector<rhs_type> x_b(ctx, tmp);

        prof.tic("solve");
        info = solve(f_b, x_b);
        prof.toc("solve");

        vex::copy(x_b, tmp);
        perm.inverse(tmp, xptr);
    } else {
        prof.tic("setup");
        Solver solve(Ab, prm, bprm);
        prof.toc("setup");

        std::cout << solve << std::endl;

        rhs_type const * fptr = reinterpret_cast<rhs_type const *>(&rhs[0]);
        rhs_type       * xptr = reinterpret_cast<rhs_type       *>(&x[0]);

        vex::vector<rhs_type> f_b(ctx, rows/B, fptr);
        vex::vector<rhs_type> x_b(ctx, rows/B, xptr);

        prof.tic("solve");
        info = solve(f_b, x_b);
        prof.toc("solve");

        vex::copy(x_b.begin(), x_b.end(), xptr);
    }

    return info;
}
#endif

//---------------------------------------------------------------------------
std::tuple<size_t, double> scalar_solve(
        const boost::property_tree::ptree &prm,
        size_t rows,
        std::vector<ptrdiff_t> const &ptr,
        std::vector<ptrdiff_t> const &col,
        std::vector<double>    const &val,
        std::vector<double>    const &rhs,
        std::vector<double>          &x,
        bool reorder
        )
{
    Backend::params bprm;

#if defined(SOLVER_BACKEND_VEXCL)
    vex::Context ctx(vex::Filter::Env);
    std::cout << ctx << std::endl;
    bprm.q = ctx;
#elif defined(SOLVER_BACKEND_VIENNACL)
    std::cout
        << viennacl::ocl::current_device().name()
        << " (" << viennacl::ocl::current_device().vendor() << ")\n\n";
#elif defined(SOLVER_BACKEND_CUDA)
    cusparseCreate(&bprm.cusparse_handle);
    {
        int dev;
        cudaGetDevice(&dev);

        cudaDeviceProp prop;
        cudaGetDeviceProperties(&prop, dev);
        std::cout << prop.name << std::endl << std::endl;
    }
#endif

    typedef amgcl::make_solver<
        amgcl::runtime::preconditioner<Backend>,
        amgcl::runtime::solver::wrapper<Backend>
        > Solver;

    std::tuple<size_t, double> info;

    if (reorder) {
        prof.tic("reorder");
        amgcl::adapter::reorder<> perm(std::tie(rows, ptr, col, val));
        prof.toc("reorder");

        prof.tic("setup");
        Solver solve(perm(std::tie(rows, ptr, col, val)), prm, bprm);
        prof.toc("setup");

        std::cout << solve << std::endl;

        std::vector<double> tmp(rows);

        perm.forward(rhs, tmp);
        auto f_b = Backend::copy_vector(tmp, bprm);

        perm.forward(x, tmp);
        auto x_b = Backend::copy_vector(tmp, bprm);

        prof.tic("solve");
        info = solve(*f_b, *x_b);
        prof.toc("solve");

#if defined(SOLVER_BACKEND_VEXCL)
        vex::copy(*x_b, tmp);
#elif defined(SOLVER_BACKEND_VIENNACL)
        viennacl::fast_copy(*x_b, tmp);
#elif defined(SOLVER_BACKEND_CUDA)
        thrust::copy(x_b->begin(), x_b->end(), tmp.begin());
#else
        std::copy(&(*x_b)[0], &(*x_b)[0] + rows, &tmp[0]);
#endif

        perm.inverse(tmp, x);
    } else {
        prof.tic("setup");
        Solver solve(std::tie(rows, ptr, col, val), prm, bprm);
        prof.toc("setup");

        std::cout << solve << std::endl;

        auto f_b = Backend::copy_vector(rhs, bprm);
        auto x_b = Backend::copy_vector(x,   bprm);

        prof.tic("solve");
        info = solve(*f_b, *x_b);
        prof.toc("solve");

#if defined(SOLVER_BACKEND_VEXCL)
        vex::copy(*x_b, x);
#elif defined(SOLVER_BACKEND_VIENNACL)
        viennacl::fast_copy(*x_b, x);
#elif defined(SOLVER_BACKEND_CUDA)
        thrust::copy(x_b->begin(), x_b->end(), x.begin());
#else
        std::copy(&(*x_b)[0], &(*x_b)[0] + rows, &x[0]);
#endif
    }

    return info;
}

#define AMGCL_CALL_BLOCK_SOLVER(z, data, B)                                    \
  case B:                                                                      \
    return block_solve<B>(prm, rows, ptr, col, val, rhs, x, reorder);

//---------------------------------------------------------------------------
std::tuple<size_t, double> solve(
        const boost::property_tree::ptree &prm,
        size_t rows,
        std::vector<ptrdiff_t> const &ptr,
        std::vector<ptrdiff_t> const &col,
        std::vector<double>    const &val,
        std::vector<double>    const &rhs,
        std::vector<double>          &x,
        int block_size,
        bool reorder
        )
{
    switch (block_size) {
        case 1:
            return scalar_solve(prm, rows, ptr, col, val, rhs, x, reorder);
#if defined(SOLVER_BACKEND_BUILTIN) || defined(SOLVER_BACKEND_VEXCL)
        BOOST_PP_SEQ_FOR_EACH(AMGCL_CALL_BLOCK_SOLVER, ~, AMGCL_BLOCK_SIZES)
#endif
        default:
            precondition(false, "Unsupported block size");
            return std::make_tuple(0, 0.0);
    }
}

//---------------------------------------------------------------------------
int main(int argc, char *argv[]) {
    namespace po = boost::program_options;
    namespace io = amgcl::io;

    using amgcl::prof;
    using std::vector;
    using std::string;

    po::options_description desc("Options");

    desc.add_options()
        ("help,h", "Show this help.")
        ("prm-file,P",
         po::value<string>(),
         "Parameter file in json format. "
        )
        (
         "prm,p",
         po::value< vector<string> >()->multitoken(),
         "Parameters specified as name=value pairs. "
         "May be provided multiple times. Examples:\n"
         "  -p solver.tol=1e-3\n"
         "  -p precond.coarse_enough=300"
        )
        ("matrix,A",
         po::value<string>(),
         "System matrix in the MatrixMarket format. "
         "When not specified, solves a Poisson problem in 3D unit cube. "
        )
        (
         "rhs,f",
         po::value<string>(),
         "The RHS vector in the MatrixMarket format. "
         "When omitted, a vector of ones is used by default. "
         "Should only be provided together with a system matrix. "
        )
        (
         "f0",
         po::bool_switch()->default_value(false),
         "Use zero RHS vector. Implies --random-initial and solver.ns_search=true"
        )
        (
         "f1",
         po::bool_switch()->default_value(false),
         "Set RHS = Ax where x = 1"
        )
        (
         "null,N",
         po::value<string>(),
         "The near null-space vectors in the MatrixMarket format. "
         "Should be a dense matrix of size N*M, where N is the number of "
         "unknowns, and M is the number of null-space vectors. "
         "Should only be provided together with a system matrix. "
        )
        (
         "coords,C",
         po::value<string>(),
         "Coordinate matrix where number of rows corresponds to the number of grid nodes "
         "and the number of columns corresponds to the problem dimensionality (2 or 3). "
         "Will be used to construct near null-space vectors as rigid body modes. "
         "Should only be provided together with a system matrix. "
        )
        (
         "binary,B",
         po::bool_switch()->default_value(false),
         "When specified, treat input files as binary instead of as MatrixMarket. "
         "It is assumed the files were converted to binary format with mm2bin utility. "
        )
        (
         "scale,s",
         po::bool_switch()->default_value(false),
         "Scale the matrix so that the diagonal is unit. "
        )
        (
         "block-size,b",
         po::value<int>()->default_value(1),
         "The block size of the system matrix. "
         "When specified, the system matrix is assumed to have block-wise structure. "
         "This usually is the case for problems in elasticity, structural mechanics, "
         "for coupled systems of PDE (such as Navier-Stokes equations), etc. "
        )
        (
         "size,n",
         po::value<int>()->default_value(32),
         "The size of the Poisson problem to solve when no system matrix is given. "
         "Specified as number of grid nodes along each dimension of a unit cube. "
         "The resulting system will have n*n*n unknowns. "
        )
        (
         "anisotropy,a",
         po::value<double>()->default_value(1.0),
         "The anisotropy value for the generated Poisson value. "
         "Used to determine problem scaling along X, Y, and Z axes: "
         "hy = hx * a, hz = hy * a."
        )
        (
         "single-level,1",
         po::bool_switch()->default_value(false),
         "When specified, the AMG hierarchy is not constructed. "
         "Instead, the problem is solved using a single-level smoother as preconditioner. "
        )
        (
         "reorder,r",
         po::bool_switch()->default_value(false),
         "When specified, the matrix will be reordered to improve cache-locality"
        )
        (
         "initial,x",
         po::value<double>()->default_value(0),
         "Value to use as initial approximation. "
        )
        (
         "random-initial",
         po::bool_switch()->default_value(false),
         "Use random initial approximation. "
        )
        (
         "output,o",
         po::value<string>(),
         "Output file. Will be saved in the MatrixMarket format. "
         "When omitted, the solution is not saved. "
        )
        ;

    po::positional_options_description p;
    p.add("prm", -1);

    po::variables_map vm;
    po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm);
    po::notify(vm);

    if (vm.count("help")) {
        std::cout << desc << std::endl;
        return 0;
    }

    for (int i = 0; i < argc; ++i) {
        if (i) std::cout << " ";
        std::cout << argv[i];
    }
    std::cout << std::endl;

    boost::property_tree::ptree prm;
    if (vm.count("prm-file")) {
        read_json(vm["prm-file"].as<string>(), prm);
    }

    if (vm.count("prm")) {
        for(const string &v : vm["prm"].as<vector<string> >()) {
            amgcl::put(prm, v);
        }
    }

    size_t rows, nv = 0;
    vector<ptrdiff_t> ptr, col;
    vector<double> val, rhs, null, x;

    if (vm.count("matrix")) {
        auto t = prof.scoped_tic("reading");

        string Afile  = vm["matrix"].as<string>();
        bool   binary = vm["binary"].as<bool>();

        if (binary) {
            io::read_crs(Afile, rows, ptr, col, val);
        } else {
            size_t cols;
            std::tie(rows, cols) = io::mm_reader(Afile)(ptr, col, val);
            precondition(rows == cols, "Non-square system matrix");
        }

        if (vm.count("rhs")) {
            string bfile = vm["rhs"].as<string>();

            size_t n, m;

            if (binary) {
                io::read_dense(bfile, n, m, rhs);
            } else {
                std::tie(n, m) = io::mm_reader(bfile)(rhs);
            }

            precondition(n == rows && m == 1, "The RHS vector has wrong size");
        } else if (vm["f1"].as<bool>()) {
            rhs.resize(rows);
            for(size_t i = 0; i < rows; ++i) {
                double s = 0;
                for(ptrdiff_t j = ptr[i], e = ptr[i+1]; j < e; ++j)
                    s += val[j];
                rhs[i] = s;
            }
        } else {
            rhs.resize(rows, vm["f0"].as<bool>() ? 0.0 : 1.0);
        }

        if (vm.count("null")) {
            string nfile = vm["null"].as<string>();

            size_t m;

            if (binary) {
                io::read_dense(nfile, m, nv, null);
            } else {
                std::tie(m, nv) = io::mm_reader(nfile)(null);
            }

            precondition(m == rows, "Near null-space vectors have wrong size");
        } else if (vm.count("coords")) {
            string cfile = vm["coords"].as<string>();
            std::vector<double> coo;

            size_t m, ndim;

            if (binary) {
                io::read_dense(cfile, m, ndim, coo);
            } else {
                std::tie(m, ndim) = io::mm_reader(cfile)(coo);
            }

            precondition(m * ndim == rows && (ndim == 2 || ndim == 3), "Coordinate matrix has wrong size");

            nv = amgcl::coarsening::rigid_body_modes(ndim, coo, null);
        }

        if (nv) {
            prm.put("precond.coarsening.nullspace.cols", nv);
            prm.put("precond.coarsening.nullspace.rows", rows);
            prm.put("precond.coarsening.nullspace.B",    &null[0]);
        }
    } else {
        auto t = prof.scoped_tic("assembling");
        rows = sample_problem(vm["size"].as<int>(), val, col, ptr, rhs, vm["anisotropy"].as<double>());
    }

    if (vm["scale"].as<bool>()) {
        std::vector<double> dia(rows, 1.0);

        for (ptrdiff_t i = 0; i < static_cast<ptrdiff_t>(rows); ++i) {
            double d = 1.0;
            for(ptrdiff_t j = ptr[i], e = ptr[i+1]; j < e; ++j) {
                if (col[j] == i) {
                    d = 1 / sqrt(val[j]);
                }
            }
            if (!std::isnan(d)) dia[i] = d;
        }

        for (ptrdiff_t i = 0; i < static_cast<ptrdiff_t>(rows); ++i) {
            rhs[i] *= dia[i];
            for(ptrdiff_t j = ptr[i], e = ptr[i+1]; j < e; ++j) {
                val[j] *= dia[i] * dia[col[j]];
            }
        }
    }

    x.resize(rows, vm["initial"].as<double>());
    if (vm["random-initial"].as<bool>() || vm["f0"].as<bool>()) {
        std::mt19937 rng;
        std::uniform_real_distribution<double> rnd(-1, 1);
        for(auto &v : x) v = rnd(rng);
    }

    if (vm["f0"].as<bool>()) {
        prm.put("solver.ns_search", true);
    }

    size_t iters;
    double error;

    int block_size = vm["block-size"].as<int>();

    if (vm["single-level"].as<bool>())
        prm.put("precond.class", "relaxation");

    std::tie(iters, error) = solve(
            prm, rows, ptr, col, val, rhs, x,
            block_size, vm["reorder"].as<bool>());

    if (vm.count("output")) {
        auto t = prof.scoped_tic("write");
        amgcl::io::mm_write(vm["output"].as<string>(), &x[0], x.size());
    }

    std::cout << "Iterations: " << iters << std::endl
              << "Error:      " << error << std::endl
              << prof << std::endl;
}


// Copyright © 2017-2020 Khaos Wallet.
//
// This file is part of Trust. The full Trust copyright notice, including
// terms governing use, modification, and redistribution, is contained in the
// file LICENSE at the root of the source code distribution tree.

#include "Crc.h"

#include <boost/crc.hpp>  // for boost::crc_32_type

#include <string>

using namespace TW;

uint16_t Crc::crc16(uint8_t* bytes, uint32_t length) {
    // Calculate checksum for existing bytes
    uint16_t crc = 0x0000;
    const uint16_t polynomial = 0x1021;

    for (auto i = 0; i < length; i++) {
        const auto byte = bytes[i];
        for (auto bitidx = 0; bitidx < 8; bitidx++) {
            const auto bit = ((byte >> (7 - bitidx) & 1) == 1);
            const auto c15 = ((crc >> 15 & 1) == 1);
            crc <<= 1;
            if (c15 ^ bit) {
                crc ^= polynomial;
            }
        }
    }

    return crc & 0xffff;
}

uint32_t Crc::crc32(const Data& data)
{
    boost::crc_32_type result;
    result.process_bytes((const void*)data.data(), data.size());
    return (uint32_t)result.checksum();
}

uint32_t Crc::crc32C(const Data& data)
{
    using crc_32c_type = boost::crc_optimal<32, 0x1EDC6F41, 0xFFFFFFFF, 0xFFFFFFFF, true, true>;
    crc_32c_type result;
    result.process_bytes((const void*)data.data(), data.size());
    return (uint32_t)result.checksum();
}


#ifdef COMPILATION_INSTRUCTIONS
clang++ -O3 -Ofast -std=c++14 -DNDEBUG -Wall -Wextra -Wpedantic -Werror $0 -o $0.x -lboost_timer && time $0.x $@ && rm -f $0.x; exit
#endif

#include "../array_ref.hpp"

#include <boost/timer/timer.hpp>

#include<iostream>
#include<numeric>
#include<vector>
#include<numeric>

using std::cout; using std::cerr;
namespace multi = boost::multi;

int main(){

	assert(0); // check that NDEBUG is on
{
	std::ptrdiff_t NX = 20000;
	std::ptrdiff_t NY = 20000;

	std::vector<double> data(NX*NY);
	std::iota(begin(data), end(data), 0.);
	multi::array_cref<double, 2> data2D_cref({NX, NY}, data.data());

	cout << "size " << data.size()*sizeof(double)/1e6 << "MB\n";

	iota(begin(data), end(data), 1.2); iota(begin(data), end(data), 10.1); data[1234] = 929.1;
	double sum_raw;
	{
		double sum = 0.;
		boost::timer::auto_cpu_timer t{std::cerr, 3, "sum: raw %t seconds\n"};
		for(auto const& e : data) sum += e;
		sum_raw = sum;
	}
	iota(begin(data), end(data), 1.2); iota(begin(data), end(data), 202.2); data[1234] = 399.1;
	double sum_2D;
	{
		double sum = 0.;
		boost::timer::auto_cpu_timer t{std::cerr, 3, "sum: 2D %t seconds\n"};
		auto ext = extensions(data2D_cref);
		for(auto i : std::get<0>(ext)){
		//	auto const& data2D_crefi = data2D_cref[i]; // not necessary in any clang or gcc
		//	for(auto j : std::get<1>(ext)) sum += data2D_crefi[j];
			for(auto j : std::get<1>(ext)) sum += data2D_cref(i, j);
		}
		sum_2D = sum;
	}
	iota(begin(data), end(data), 1.2); iota(begin(data), end(data), 2.21); data[1234] = 3299.1;
	double sum_2D_acc;
	{
		double sum = 0.;
		boost::timer::auto_cpu_timer t{cerr, 3, "sum: 2D acc %t seconds\n"};
		sum = std::accumulate(
			begin(data2D_cref), end(data2D_cref), 0.,
			[](auto&& a, auto&& b){return a + std::accumulate(begin(b), end(b), 0.);}
		);
		sum_2D_acc = sum;
	}
	iota(begin(data), end(data), 1.2); iota(begin(data), end(data), 2.15); data[1234] = 3419.1;
	double sum_2Dwrong_acc;
	{
		boost::timer::auto_cpu_timer t{cerr, 3, "sum: 2Dwrong acc %t seconds\n"};
		sum_2Dwrong_acc = std::accumulate(
			begin(rotated(data2D_cref)), end(rotated(data2D_cref)), 0.,
			[](auto&& a, auto&& b){return a + std::accumulate(begin(b), end(b), 0.);}
		);
	}
	iota(begin(data), end(data), 1.2); iota(begin(data), end(data), 11.2); data[1234] = 199.1;
	double sum_2Dwrong;
	{
		double sum = 0.;
		boost::timer::auto_cpu_timer t{std::cerr, 3, "sum: 2Dwrong sum %t seconds\n"};
		auto ext = extensions(data2D_cref);
		for(auto j : std::get<1>(ext)){
		//	auto const& data2D_crefXj = data2D_cref.rotated(1)[j]; // not necessary in clang or gcc
			for(auto i : std::get<0>(ext)){
			//	sum += data2D_crefXj[i];
				sum += data2D_cref[i][j];
			}
		}
		sum_2Dwrong = sum;
	}
	iota(begin(data), end(data), 1.2); iota(begin(data), end(data), 10.112); data[1234] = 99.1;
	double sum_raw2;
	{
		double sum = 0.;
		boost::timer::auto_cpu_timer t{cerr, 3, "sum: raw %t seconds\n"};
		for(auto const& e : data) sum += e;
		sum_raw2 = sum;
	}
	iota(begin(data), end(data), 1.2); iota(begin(data), end(data), 33.112); data[1234] = 1199.1;
	double sum_raw_acc;
	{
		boost::timer::auto_cpu_timer t{cerr, 3, "sum: raw acc %t seconds\n"};
		sum_raw_acc = std::accumulate(data.data(), data.data() + data.size(), 0.);
	}
	cout<< sum_2D + sum_2D_acc + sum_2Dwrong_acc + sum_2Dwrong + sum_raw + sum_raw2 + sum_raw_acc <<std::endl;
}
cout<<'\n';
{
	std::ptrdiff_t NX = 700, NY = 700, NZ = 700;
	std::vector<double> v(NX*NY*NZ);
	cout<<"3D data "<< v.size()*sizeof(double)/1e6 <<"MB\n";
	iota(begin(v), end(v), 0.1);

	multi::array_cref<double, 3> v3D_cref({NX, NY, NZ}, v.data());
	assert( num_elements(v3D_cref) == std::ptrdiff_t(v.size()) );
	{
		double sum = 0.;
		boost::timer::auto_cpu_timer t{std::cerr, 3, "sum: 3D raw %t seconds\n"};
		for(auto const& e : v) sum += e;
		cout << sum << '\n';
	}
	iota(begin(v), end(v), 1.2);
	{
		double sum = 0.;
		boost::timer::auto_cpu_timer t{std::cerr, 3, "sum: 3D indexed %t seconds\n"};
		auto ext = extensions(v3D_cref);
		for(auto i : std::get<0>(ext)){
			auto const& v3D_crefi = v3D_cref[i];
			for(auto j : std::get<1>(ext)){
				auto const& v3D_crefij = v3D_crefi[j];
				for(auto k : std::get<2>(ext))
					sum += v3D_crefij[k];
			}
		}
		cout << sum << '\n';
	}
	iota(begin(v), end(v), 4444.5);
	{
		double sum = 0.;
		boost::timer::auto_cpu_timer t{std::cerr, 3, "sum: 3Dwrong indexed %t seconds\n"};
		for(auto k : v3D_cref.extension(2))
			for(auto j : v3D_cref.extension(1))
				for(auto i : v3D_cref.extension(0))
					sum += v3D_cref[i][j][k];
		cout << sum << '\n';
	}
}
{
	std::ptrdiff_t NX = 150, NY = 150, NZ = 150, NA = 150;
	std::vector<double> v(NX*NY*NZ*NA);
	multi::array_cref<double, 4> v4D_cref({NX, NY, NZ, NA}, v.data());
	assert( v4D_cref.num_elements() == std::ptrdiff_t(v.size()) );
	cout<<"4D data "<< num_elements(v4D_cref)*sizeof(double)/1e6 <<"MB\n";
	iota(begin(v), end(v), 0.1);
	{
		double sum = 0.;
		boost::timer::auto_cpu_timer t{std::cerr, 3, "sum: 4D raw %t seconds\n"};
		for(auto const& e : v) sum += e;
		cout<< sum <<'\n';
	}
	iota(begin(v), end(v), 1222.1);
	{
		double sum = 0.;
		boost::timer::auto_cpu_timer t{std::cerr, 3, "sum: 4D indexed %t seconds\n"};
		auto ext = extensions(v4D_cref);
		for(auto i : std::get<0>(ext)){
		//	auto const& v4D_crefi = v4D_cref[i]; // not necessary in clang or gcc
			for(auto j : std::get<1>(ext)){
			//	auto const& v4D_crefij = v4D_crefi[j]; // not necessary in clang or gcc
				for(auto k : std::get<2>(ext)){
				//	auto const& v4D_crefijk = v4D_crefij[k]; // not necessary in clang or gcc
					for(auto l : std::get<3>(ext))
						sum += v4D_cref[i][j][k][l];
				}
			}
		}
		cout<< std::to_string(sum)[0] <<'\n';
	}
}
	return 0;
}


#include "multi_dimension_array.h"
#include <vector>
using std::vector;

#include <stdio.h>
#include <sys/time.h>

#define BOOST_DISABLE_ASSERTS
#include <boost/multi_array.hpp>

#include <blitz/array.h>

int main()
{
    const size_t num_samples = 50;
    const size_t iterations = num_samples * num_samples;

    size_t size = 2;
    const size_t rowlen = num_samples;
    vector<size_t> dims;
    dims.resize(size, rowlen);

    vector<size_t> position;
    position.resize(size, 0);

    double **raw_array = new double*[rowlen];
    double *flat_array = new double[rowlen * rowlen];
    boost::multi_array<double, 2> boost_array(boost::extents[rowlen][rowlen]);
    blitz::Array<double, 2> blitz_array(rowlen, rowlen);
    blitz::TinyVector<size_t, 2> index;
    for (size_t i = 0; i < rowlen; ++i) {
        raw_array[i] = new double[rowlen];
        index[0] = i;
        for (size_t j = 0; j < rowlen; ++j) {
            raw_array[i][j] = 42.0;
            flat_array[i * rowlen + j] = 42.0;
            boost_array[i][j] = 42.0;

            index[1] = j;
            blitz_array(index) = 42.0;
        }
    }
    MultiDimensionArray<double> array(dims, 42.0);

    // ===== RAW ARRAY ===== //

    struct timeval begin, end, diff;
    gettimeofday(&begin, NULL);
    double sum = 0.0;
    for (size_t z = 0; z < iterations; ++z) {
        for (size_t i = 0; i < rowlen; ++i) {
            for (size_t j = 0; j < rowlen; ++j) {
                sum += raw_array[i][j];
            }
        }
    }
    gettimeofday(&end, NULL);
    diff.tv_sec = end.tv_sec - begin.tv_sec;
    if (end.tv_usec < begin.tv_usec) {
        --diff.tv_sec;
        end.tv_usec += 1000000;
    }
    diff.tv_usec = end.tv_usec - begin.tv_usec;
    double seconds = diff.tv_sec + (diff.tv_usec / 1000000.0);

    fprintf(stderr, "raw array took %f seconds; sum=%f\n", seconds, sum);

    // ===== FLAT ARRAY ===== //

    gettimeofday(&begin, NULL);
    sum = 0.0;
    for (size_t z = 0; z < iterations; ++z) {
        for (size_t i = 0; i < rowlen; ++i) {
            for (size_t j = 0; j < rowlen; ++j) {
                sum += flat_array[i * rowlen + j];
            }
        }
    }
    gettimeofday(&end, NULL);
    diff.tv_sec = end.tv_sec - begin.tv_sec;
    if (end.tv_usec < begin.tv_usec) {
        --diff.tv_sec;
        end.tv_usec += 1000000;
    }
    diff.tv_usec = end.tv_usec - begin.tv_usec;
    seconds = diff.tv_sec + (diff.tv_usec / 1000000.0);

    fprintf(stderr, "flat array took %f seconds; sum=%f\n", seconds, sum);

    // ===== MultiDimensionArray ===== //

    gettimeofday(&begin, NULL);
    sum = 0.0;
    for (size_t z = 0; z < iterations; ++z) {
        for (size_t i = 0; i < rowlen; ++i) {
            position[0] = i;
            for (size_t j = 0; j < rowlen; ++j) {
                position[1] = j;
                sum += array.at(position);
            }
        }
    }
    gettimeofday(&end, NULL);
    diff.tv_sec = end.tv_sec - begin.tv_sec;
    if (end.tv_usec < begin.tv_usec) {
        --diff.tv_sec;
        end.tv_usec += 1000000;
    }
    diff.tv_usec = end.tv_usec - begin.tv_usec;
    seconds = diff.tv_sec + (diff.tv_usec / 1000000.0);

    fprintf(stderr, "MultiDimensionArray took %f seconds; sum=%f\n", seconds, sum);

    // ===== boost::multi_array ===== //

    gettimeofday(&begin, NULL);
    sum = 0.0;
    for (size_t z = 0; z < iterations; ++z) {
        for (size_t i = 0; i < rowlen; ++i) {
            for (size_t j = 0; j < rowlen; ++j) {
                sum += boost_array[i][j];
            }
        }
    }
    gettimeofday(&end, NULL);
    diff.tv_sec = end.tv_sec - begin.tv_sec;
    if (end.tv_usec < begin.tv_usec) {
        --diff.tv_sec;
        end.tv_usec += 1000000;
    }
    diff.tv_usec = end.tv_usec - begin.tv_usec;
    seconds = diff.tv_sec + (diff.tv_usec / 1000000.0);

    fprintf(stderr, "boost::multi_array took %f seconds; sum=%f\n", seconds, sum);

    // ===== blitz::Array ===== //

    gettimeofday(&begin, NULL);
    sum = 0.0;
    for (size_t z = 0; z < iterations; ++z) {
        for (size_t i = 0; i < rowlen; ++i) {
            index[0] = i;
            for (size_t j = 0; j < rowlen; ++j) {
                index[1] = j;
                sum += blitz_array(index);
            }
        }
    }
    gettimeofday(&end, NULL);
    diff.tv_sec = end.tv_sec - begin.tv_sec;
    if (end.tv_usec < begin.tv_usec) {
        --diff.tv_sec;
        end.tv_usec += 1000000;
    }
    diff.tv_usec = end.tv_usec - begin.tv_usec;
    seconds = diff.tv_sec + (diff.tv_usec / 1000000.0);

    fprintf(stderr, "blitz::Array took %f seconds; sum=%f\n", seconds, sum);

    return 0;
}


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
/*!
 * \file
**/
#ifndef BOOST_SIMD_TOOLBOX_CONSTANT_CONSTANTS_MAXINIT_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_CONSTANT_CONSTANTS_MAXINIT_HPP_INCLUDED

#include <boost/simd/include/functor.hpp>
#include <boost/simd/sdk/meta/int_c.hpp>
#include <boost/simd/sdk/meta/float.hpp>
#include <boost/simd/sdk/meta/double.hpp>
#include <boost/simd/sdk/constant/constant.hpp>

#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable: 4146)
#endif

/*!
 * \ingroup boost_simd_constant
 * \defgroup boost_simd_constant_maxinit Maxinit
 *
 * \par Description
 * Constant Maxinit, maximum value of a type.
 * \arg int8    -128, uint8    0,
 * \arg int16 -32768, uint16 0,
 * \arg int32 -2147483648, uint32 0,
 * \arg int64 -9223372036854775808, uint64 0,\arg float \f$-\infty\f$, double \f$-\infty\f$,
 * \par
 * The value of this constant is type dependant. This means that for different
 * types it does not represent the same mathematical number.
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/maxinit.hpp>
 * \endcode
 *
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class T,class A0>
 *     meta::call<tag::maxinit_(A0)>::type
 *     Maxinit();
 * }
 * \endcode
 *
 *
 * \param T template parameter of Maxinit
 *
 * \return type T value
 *
 *
**/

namespace boost { namespace simd
{
  namespace tag
  {
    /*!
     * \brief Define the tag Maxinit of functor Maxinit
     *        in namespace boost::simd::tag for toolbox boost.simd.constant
    **/
    struct Maxinit : ext::pure_constant_<Maxinit>
    {
      typedef double default_type;
      template<class Target, class Dummy=void>
      struct  apply
            : meta::int_c < typename Target::type, 0> {};
    };

    template<class T, class Dummy>
    struct  Maxinit::apply<boost::dispatch::meta::single_<T>,Dummy>
          : meta::single_<0xFF7FFFFFUL> {};

    template<class T, class Dummy>
    struct  Maxinit::apply<boost::dispatch::meta::double_<T>,Dummy>
          : meta::double_<0xFFEFFFFFFFFFFFFFULL> {};

    template<class T, class Dummy>
    struct  Maxinit::apply<boost::dispatch::meta::int8_<T>,Dummy>
          : meta::int_c<boost::simd::int8_t,boost::simd::int8_t(-128)> {};

    template<class T, class Dummy>
    struct  Maxinit::apply<boost::dispatch::meta::int16_<T>,Dummy>
          : meta::int_c<boost::simd::int16_t,boost::simd::int16_t(-32768)> {};

    template<class T, class Dummy>
    struct  Maxinit::apply<boost::dispatch::meta::int32_<T>,Dummy>
    : meta::int_c < boost::simd::int32_t
                  , boost::simd::int32_t(-boost::simd::uint32_t(2147483648UL))
                  >
    {};

    template<class T, class Dummy>
    struct  Maxinit::apply<boost::dispatch::meta::int64_<T>,Dummy>
      : meta::int_c < boost::simd::int64_t
                    , boost::simd::
                      int64_t(-boost::simd::uint64_t(9223372036854775808ULL))
                    >
    {};
  }

  BOOST_SIMD_CONSTANT_IMPLEMENTATION(boost::simd::tag::Maxinit, Maxinit)
} }

#ifdef BOOST_MSVC
#pragma warning(pop)
#endif

#include <boost/simd/sdk/constant/common.hpp>

#endif


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

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

 * See LICENSE for the license information

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

/**
 *  @file  testTOAFactor.cpp
 *  @brief Unit tests for "Time of Arrival" factor
 *  @author Frank Dellaert
 *  @author Jay Chakravarty
 *  @date December 2014
 */

#include <gtsam_unstable/geometry/Event.h>
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
#include <gtsam/nonlinear/expressions.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/base/numericalDerivative.h>

#include <CppUnitLite/TestHarness.h>
#include <boost/format.hpp>
#include <boost/bind.hpp>

using namespace std;
using namespace gtsam;

// typedefs
typedef Expression<Point3> Point3_;
typedef Expression<Event> Event_;

// units
static const double ms = 1e-3;
static const double cm = 1e-2;

// Create a noise model for the TOA error
static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5 * ms));

static const double timeOfEvent = 25;
static const Event exampleEvent(timeOfEvent, 1, 0, 0);
static const Point3 microphoneAt0(0,0,0);

//*****************************************************************************
TEST( TOAFactor, NewWay ) {
  Key key = 12;
  Event_ eventExpression(key);
  Point3_ microphoneConstant(microphoneAt0); // constant expression
  double measurement = 7;
  Double_ expression(&Event::toa, eventExpression, microphoneConstant);
  ExpressionFactor<double> factor(model, measurement, expression);
}

//*****************************************************************************
TEST( TOAFactor, WholeEnchilada ) {

  static const bool verbose = false;

  // Create microphones
  const double height = 0.5;
  vector<Point3> microphones;
  microphones.push_back(Point3(0, 0, height));
  microphones.push_back(Point3(403 * cm, 0, height));
  microphones.push_back(Point3(403 * cm, 403 * cm, height));
  microphones.push_back(Point3(0, 403 * cm, 2 * height));
  EXPECT_LONGS_EQUAL(4, microphones.size());
//  microphones.push_back(Point3(200 * cm, 200 * cm, height));

  // Create a ground truth point
  const double timeOfEvent = 0;
  Event groundTruthEvent(timeOfEvent, 245 * cm, 201.5 * cm, (212 - 45) * cm);

  // Simulate simulatedTOA
  size_t K = microphones.size();
  vector<double> simulatedTOA(K);
  for (size_t i = 0; i < K; i++) {
    simulatedTOA[i] = groundTruthEvent.toa(microphones[i]);
    if (verbose) {
      cout << "mic" << i << " = " << microphones[i] << endl;
      cout << "z" << i << " = " << simulatedTOA[i] / ms << endl;
    }
  }

  // Now, estimate using non-linear optimization
  ExpressionFactorGraph graph;
  Key key = 12;
  Event_ eventExpression(key);
  for (size_t i = 0; i < K; i++) {
    Point3_ microphone_i(microphones[i]); // constant expression
    Double_ predictTOA(&Event::toa, eventExpression, microphone_i);
    graph.addExpressionFactor(predictTOA, simulatedTOA[i], model);
  }

  /// Print the graph
  if (verbose)
    GTSAM_PRINT(graph);

  // Create initial estimate
  Values initialEstimate;
  //Event estimatedEvent(timeOfEvent -10, 200 * cm, 150 * cm, 350 * cm);
  Vector4 delta;
  delta << 0.1, 0.1, -0.1, 0.1;
  Event estimatedEvent = groundTruthEvent.retract(delta);
  initialEstimate.insert(key, estimatedEvent);

  // Print
  if (verbose)
    initialEstimate.print("Initial Estimate:\n");

  // Optimize using Levenberg-Marquardt optimization.
  LevenbergMarquardtParams params;
  params.setAbsoluteErrorTol(1e-10);
  if (verbose)
    params.setVerbosity("ERROR");
  LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, params);
  Values result = optimizer.optimize();
  if (verbose)
    result.print("Final Result:\n");

  EXPECT(assert_equal(groundTruthEvent, result.at<Event>(key), 1e-6));
}
//*****************************************************************************
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
//*****************************************************************************


/*
 * Copyright (c) 2016 Andreas Pohl
 * Licensed under MIT (see COPYING)
 *
 * Author: Andreas Pohl
 */

#include "example.h"
#include <boost/utility/string_ref.hpp>

DECLARE_LIB_BEGIN(example);
ENABLE_LIB_LOAD();
ENABLE_LIB_UNLOAD();
ENABLE_LIB_STATE_INIT();
ADD_LIB_FUNCTION(work_static);
ADD_LIB_METHOD(work_member);
DECLARE_LIB_END();

int example::work_static(lua_State* L) {
    double n1 = luaL_checknumber(L, 1);
    double n2 = luaL_checknumber(L, 2);
    lua_pushnumber(L, n1 + n2);
    return 1;  // one result
}

int example::work_member(lua_State* L) {
    boost::string_ref str = luaL_checkstring(L, 1);
    lua_pushnumber(L, str.size());
    return 1;  // one result
}


#include <Eigen/Dense>
#include "voxelize.h"

#include <iostream>
#include <fstream>
#include <string>
#include <algorithm>
#include <vector>
#include <cxxopts.hpp>

#ifdef _OPENMP
#include <omp.h>
#endif

using namespace Eigen;
using namespace std;

bool verbose_output = false, parallel = false;
int resolution = 16;
float voxelsize = 100.0f;

Vector3f min_corner, max_corner;

vector<tetrahedra> mesh;
vector<Eigen::Vector3f> vertex;
vector<float> sample_coord_x, sample_coord_y, sample_coord_z;
bool voxels[256][256][256] = {0};

inline float min(float a, float b, float c, float d)
{
	return min(min(min(a,b),c),d);
}

inline float max(float a, float b, float c, float d)
{
	return max(max(max(a,b),c),d);
}

void NodesInput(const char *fname)
{
	int input_count = 1, id = 1, polyface = 3, type1 = 0, type2 = 0;
	double x_coord, y_coord, z_coord;

	ifstream fin1(fname);
	fin1 >> input_count >> polyface >> type1 >> type2;

	//assert(polyface == 3 && type1 == 0 && type2 == 0);
	vertex.resize(input_count + 1);
	vertex.shrink_to_fit();

	for(int i = 1; i <= input_count; ++i)
	{
		fin1 >> id >> x_coord >> y_coord >> z_coord;
		//assert(id == i);
		vertex[i] << x_coord, y_coord, z_coord;
	}

	fin1.close();
}

void ElementInput(const char *fname)
{
	int input_count = 1, id = 1;
	int poly_face = 4, type = 0, id_A, id_B, id_C, id_D;

	ifstream fin(fname);
	fin >> input_count >> poly_face >> type;

	//assert(poly_face == 4 && type == 0);
	mesh.resize(input_count + 1);
	mesh.shrink_to_fit();

	for(int i = 1; i <= input_count; ++i)
	{
		fin >> id >> id_A >> id_B >> id_C >> id_D;
		//assert(id == i);
		tetrahedra t;
		t.a = vertex[id_A];
		t.b = vertex[id_B];
		t.c = vertex[id_C];
		t.d = vertex[id_D];

		t.bound_min << min(t.a(0), t.b(0), t.c(0), t.d(0)), min(t.a(1), t.b(1), t.c(1), t.d(1)), min(t.a(2), t.b(2), t.c(2), t.d(2));
		t.bound_max << max(t.a(0), t.b(0), t.c(0), t.d(0)), max(t.a(1), t.b(1), t.c(1), t.d(1)), max(t.a(2), t.b(2), t.c(2), t.d(2));

		mesh[i] = t;
	}

	if(verbose_output)
		cout << "ElementCount: " << id << endl;
	string message;
	fin.ignore();
	getline(fin, message);
	if(verbose_output)
		cout << message << endl;
	fin.close();
}

void MshInput(string fname = "")
{
	string nd = "$Nodes", ednd = "$EndNodes", ele = "$Elements", edele = "$EndElements";
	string s;
	int input_count;
	int id;
	float x_coord, y_coord, z_coord;
	int poly_face = 4, type = 0, id_A, id_B, id_C, id_D;

	if(fname.empty())
	{
		while(cin >> s)
		{
			if(s == nd)
			{
				cin >> input_count;
				vertex.resize(input_count);
				for(int i = 1; i <= input_count; ++i)
				{
					cin >> id >> x_coord >> y_coord >> z_coord;
					//assert(id == i);
					vertex[i] << x_coord, y_coord, z_coord;
				}

				if(verbose_output)
					cout << "NodeCount: " << id << endl;
				cin >> s;
				//assert(s == ednd);
			}
			if(s == ele)
			{
				cin >> input_count;
				mesh.resize(input_count);
				for(int i = 1; i <= input_count; ++i)
				{
					cin >> id >> poly_face >> type >> id_A >> id_B >> id_C >> id_D;
					//assert(id == i && poly_face == 4 && type == 0);
					tetrahedra t;
					t.a = vertex[id_A];
					t.b = vertex[id_B];
					t.c = vertex[id_C];
					t.d = vertex[id_D];

					t.bound_min << min(t.a(0), t.b(0), t.c(0), t.d(0)), min(t.a(1), t.b(1), t.c(1), t.d(1)), min(t.a(2), t.b(2), t.c(2), t.d(2));
					t.bound_max << max(t.a(0), t.b(0), t.c(0), t.d(0)), max(t.a(1), t.b(1), t.c(1), t.d(1)), max(t.a(2), t.b(2), t.c(2), t.d(2));

					mesh[i] = t;
				}
				if(verbose_output)
					cout << "ElementCount: " << id << endl;
				cin >> s;
				//assert(s == edele);
			}
		}
	}
	else
	{
		ifstream fin(fname.c_str());
		while(fin >> s)
		{
			if(s == nd)
			{
				fin >> input_count;
				vertex.resize(input_count);
				for(int i = 1; i <= input_count; ++i)
				{
					fin >> id >> x_coord >> y_coord >> z_coord;
					//assert(id == i);
					vertex[i] << x_coord, y_coord, z_coord;
				}

				if(verbose_output)
					cout << "NodeCount: " << id << endl;
				fin >> s;
				//assert(s == ednd);
			}
			if(s == ele)
			{
				fin >> input_count;
				mesh.resize(input_count);
				for(int i = 1; i <= input_count; ++i)
				{
					fin >> id >> poly_face >> type >> id_A >> id_B >> id_C >> id_D;
					//assert(id == i && poly_face == 4 && type == 0);
					tetrahedra t;
					t.a = vertex[id_A];
					t.b = vertex[id_B];
					t.c = vertex[id_C];
					t.d = vertex[id_D];

					t.bound_min << min(t.a(0), t.b(0), t.c(0), t.d(0)), min(t.a(1), t.b(1), t.c(1), t.d(1)), min(t.a(2), t.b(2), t.c(2), t.d(2));
					t.bound_max << max(t.a(0), t.b(0), t.c(0), t.d(0)), max(t.a(1), t.b(1), t.c(1), t.d(1)), max(t.a(2), t.b(2), t.c(2), t.d(2));

					mesh[i] = t;
				}
				if(verbose_output)
					cout << "ElementCount: " << id << endl;
				fin >> s;
				//assert(s == edele);
			}
		}
		fin.close();
	}
}

void NodesElementsInput(string fname)
{
	//vertex.resize(300);
	NodesInput((fname + ".node").c_str());
	ElementInput((fname + ".ele").c_str());
	//mesh.resize(792);
}

void StandardInput()
{
	int T;
	cin >> resolution;
	cin >> T;
	while(T--)
	{
		tetrahedra t;
		float inx, iny, inz;
		cin >> inx >> iny >> inz;
		t.a << inx, iny, inz;
		cin >> inx >> iny >> inz;
		t.b << inx, iny, inz;
		cin >> inx >> iny >> inz;
		t.c << inx, iny, inz;
		cin >> inx >> iny >> inz;
		t.d << inx, iny, inz;

		t.bound_min << min(t.a(0), t.b(0), t.c(0), t.d(0)), min(t.a(1), t.b(1), t.c(1), t.d(1)), min(t.a(2), t.b(2), t.c(2), t.d(2));
		t.bound_max << max(t.a(0), t.b(0), t.c(0), t.d(0)), max(t.a(1), t.b(1), t.c(1), t.d(1)), max(t.a(2), t.b(2), t.c(2), t.d(2));

		mesh.push_back(t);
	}
}

void BoundingBox()
{
	int mesh_size = mesh.size();
	#ifdef _OPENMP
	#pragma omp parallel for num_threads(8) if(parallel)
	#endif
	for(int i = 1; i < mesh_size; ++i)
	{
		min_corner << min(mesh[i].bound_min(0), min_corner(0)), min(mesh[i].bound_min(1), min_corner(1)), min(mesh[i].bound_min(2), min_corner(2));
		max_corner << max(mesh[i].bound_max(0), max_corner(0)), max(mesh[i].bound_max(1), max_corner(1)), max(mesh[i].bound_max(2), max_corner(2));
	}
	voxelsize = (max_corner - min_corner).maxCoeff() / resolution;
}

bool Philipp(Vector3f A, Vector3f B, Vector3f C, Vector3f D, Vector3f P)
{
	Vector3f a = A - P, b = B - P, c = C - P, d = D - P;
	Matrix3f Da, Db, Dc, Dd;
	Da << b, c, d;
	Db << a, c, d;
	Dc << a, b, d;
	Dd << a, b, c;

	if((Da.determinant() * Dc.determinant() >= 0) && (Db.determinant() * Dd.determinant() >= 0) && (Da.determinant() * Db.determinant() <= 0))
		return true;
	return false;
}

void voxelize()
{
	sample_coord_x.resize(resolution);
	sample_coord_y.resize(resolution);
	sample_coord_z.resize(resolution);
	sample_coord_x[0] = min_corner(0);
	sample_coord_y[0] = min_corner(1);
	sample_coord_z[0] = min_corner(2);
	// lookup table for the coords
	for(int i = 1; i < resolution; ++i)
	{
		sample_coord_x[i] = sample_coord_x[i - 1] + voxelsize;
		sample_coord_y[i] = sample_coord_y[i - 1] + voxelsize;
		sample_coord_z[i] = sample_coord_z[i - 1] + voxelsize;
	}
	int mesh_size = mesh.size();
	#ifdef _OPENMP
	#pragma omp parallel for num_threads(8) if(parallel)
	#endif
	for(int i = 1; i < mesh_size; ++i)
	{
		Vector3f starting = mesh[i].bound_min - min_corner;
		Vector3f ending = mesh[i].bound_max - min_corner;
		Vector3f st = (starting / voxelsize);
		Vector3f ed = (ending / voxelsize);
		for(int i = int(st(0)); i <= ed(0); ++i)
		{
			for(int j = int(st(1)); j <= ed(1); ++j)
			{
				for(int k = int(st(2)); k <= ed(2); ++k)
				{
					if(voxels[i][j][k]) continue;
					Vector3f p;
					p << sample_coord_x[i], sample_coord_y[j], sample_coord_z[k];
					if( Philipp(mesh[i].a, mesh[i].b, mesh[i].c, mesh[i].d, p) )
					{
						voxels[i][j][k] = 1;
					}
				}
			}
		}
	}
}

void VoxelOutput(string fname = "")
{
	if(fname.empty())
	{
		printf("%d x %d x %d\n", resolution, resolution, resolution);
		for(int i = 0; i < resolution; ++i)
		{
			for(int j = 0; j < resolution; ++j)
			{
				for(int k = 0; k < resolution; ++k)
				{
					cout << voxels[i][j][k] << " ";
				}
			}
		}
		cout << endl;
	}
	else
	{
		ofstream fout(fname.c_str());
		fout << "Resolution: " << resolution << "x" << resolution << "x" << resolution << endl;
		for(int i = 0; i < resolution; ++i)
		{
			for(int j = 0; j < resolution; ++j)
			{
				for(int k = 0; k < resolution; ++k)
				{
					fout << voxels[i][j][k] << " ";
				}
			}
		}
		fout << endl;
		fout.close();
	}
}

int main(int argc, char** argv)
{
	vertex.reserve(1500000);
	mesh.reserve(10000000);

	cxxopts::Options options("exe.out", "One line description of MyProgram");
	options.add_options()
	("p,parallel", "Enable parallel computation on CUDA") // a bool parameter
	("n,nodes", "Enable nodes and elements input")
  ("r,resolution", "adjust the resolution of voxel output", cxxopts::value<int>())
  ("i,input", "Input file path", cxxopts::value<std::string>())
  ("o,output", "Output file path", cxxopts::value<std::string>())
  ("v,verbose", "Verbose output", cxxopts::value<bool>()->default_value("false"))
  ("h,help", "List all options")
  ;

  auto result = options.parse(argc, argv);

  if(result.count("help"))
  {
    cout << options.help({"", "Group"}) << endl;
    return 0;
  }

  if(result.count("parallel"))
  {
  	parallel = true;
  }

 	if(result.count("resolution"))
 	{
 		resolution = result["resolution"].as<int>();
 		cout << "Output resolution is set to " << resolution << "x" << resolution << "x" << resolution << endl;
 	}

 	if(result.count("verbose"))
 	{
 		verbose_output = result["verbose"].as<bool>();
 	}

	if(result.count("nodes"))
	{
		if(result.count("input"))
		{
			NodesElementsInput(result["input"].as<string>());
		}
		else
		{
			cout << "Nodes input requires specify input file path!\n";
			exit(0);
		}
	}
	else if(result.count("input"))
	{
		MshInput(result["input"].as<string>());
	}
	else
	{
		MshInput();
	}
	min_corner << INF, INF, INF;
	max_corner << -INF, -INF, -INF;
	BoundingBox();

	if(verbose_output)
		cout << min_corner << endl  << endl << max_corner << endl;
  voxelize();

  if(result.count("output"))
  {
  	VoxelOutput(result["output"].as<string>());
  }
  else
  {
		VoxelOutput();
  }
	return 0;
}


// 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 <Eigen/Core>
#include <glog/logging.h>
#include <gflags/gflags.h>
#include <theia/theia.h>

#include <algorithm>
#include <memory>
#include <string>

DEFINE_string(reconstruction, "", "Reconstruction file");

void ComputeReprojectionErrors(const theia::Reconstruction& reconstruction) {
  std::vector<double> reprojection_errors;
  int num_projections_behind_camera = 0;
  for (const theia::TrackId track_id : reconstruction.TrackIds()) {
    const theia::Track* track = CHECK_NOTNULL(reconstruction.Track(track_id));
    for (const theia::ViewId view_id : track->ViewIds()) {
      const theia::Feature* feature =
          reconstruction.View(view_id)->GetFeature(track_id);

      // Reproject the observations.
      Eigen::Vector2d projection;
      if (reconstruction.View(view_id)
              ->Camera().ProjectPoint(track->Point(), &projection) < 0) {
        ++num_projections_behind_camera;
      }

      // Compute reprojection error.
      const double reprojection_error = (*feature - projection).norm();
      reprojection_errors.emplace_back(reprojection_error);
    }
  }

  std::sort(reprojection_errors.begin(), reprojection_errors.end());
  const double mean_reprojection_error =
      std::accumulate(reprojection_errors.begin(),
                      reprojection_errors.end(),
                      0.0) / static_cast<double>(reprojection_errors.size());
  const double median_reprojection_error =
      reprojection_errors[reprojection_errors.size() / 2];

  LOG(INFO) << "\nNum observations: " << reprojection_errors.size()
            << "\nNum reprojections behind camera: "
            << num_projections_behind_camera
            << "\nMean reprojection error = " << mean_reprojection_error
            << "\nMedian reprojection_error = " << median_reprojection_error;
}

void ComputeTrackLengthHistogram(const theia::Reconstruction& reconstruction) {
  std::vector<int> histogram_bins = {2, 3,  4,  5,  6,  7, 8,
                                     9, 10, 15, 20, 25, 50};
  theia::Histogram<int> histogram(histogram_bins);
  for (const theia::TrackId track_id : reconstruction.TrackIds()) {
    const theia::Track* track = reconstruction.Track(track_id);
    histogram.Add(track->NumViews());
  }
  const std::string hist_msg = histogram.PrintString();
  LOG(INFO) << "Track lengths = \n" << hist_msg;
}

int main(int argc, char* argv[]) {
  google::InitGoogleLogging(argv[0]);
  THEIA_GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);

  // Load the SIFT descriptors into the cameras.
  std::unique_ptr<theia::Reconstruction> reconstruction(
      new theia::Reconstruction());
  CHECK(theia::ReadReconstruction(FLAGS_reconstruction, reconstruction.get()))
      << "Could not read reconstruction file.";

  LOG(INFO) << "\nNum views: " << reconstruction->NumViews()
            << "\nNum 3D points: " << reconstruction->NumTracks();

  // Check that the reprojection errors are sane.
  ComputeReprojectionErrors(*reconstruction);

  // Compute track length statistics.
  ComputeTrackLengthHistogram(*reconstruction);

  return 0;
}


#pragma once

#include <boost/mpl/identity.hpp>
#include <deal.II/base/graph_coloring.h>
#include <deal.II/dofs/dof_handler.h>

namespace boltzmann {
namespace local_ {

template <typename DH>
struct make_graph_coloring_traits
{
};

template <int DIM>
struct make_graph_coloring_traits<dealii::DoFHandler<DIM> >
{
  typedef typename dealii::DoFHandler<DIM>::active_cell_iterator active_iterator_t;
};
}  // end namespace _local

template <typename DH, typename INDEXER>
std::vector<std::vector<typename local_::make_graph_coloring_traits<DH>::active_iterator_t> >
make_graph_coloring(const DH& dof_handler, const INDEXER& indexer)
{
  // number of dofs...
  const unsigned int dofs_per_cell = dof_handler.get_fe().dofs_per_cell;

  auto begin = dof_handler.begin_active();
  auto end = dof_handler.end();

  typedef decltype(begin) iterator_t;

  /*
   *   The function make_graph_coloring creates a colored graph of the FEM Matrix
   *
   *   Hint: indexer.to_global is used to take into account periodic boundary
   *   conditions (if present).
   */
  const std::function<std::vector<dealii::types::global_dof_index>(const iterator_t&)>
      get_conflict_indices = [&](const iterator_t& cell) {
        std::vector<dealii::types::global_dof_index> dof_idxs(dofs_per_cell);
        // apply indexer, set velocity index j = 0;
        cell->get_dof_indices(dof_idxs);

        const unsigned int j = 0;
        for (unsigned int i = 0; i < dofs_per_cell; ++i) {
          dof_idxs[i] = indexer.to_global(dof_idxs[i], j);
        }
        return dof_idxs;
      };

  return dealii::GraphColoring::make_graph_coloring(begin, end, get_conflict_indices);
}

}  // end namespace boltzmann


/**************************************************************
 *
 * Licensed to the Apache Software Foundation (ASF) under one
 * or more contributor license agreements.  See the NOTICE file
 * distributed with this work for additional information
 * regarding copyright ownership.  The ASF licenses this file
 * to you 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.
 *
 *************************************************************/

// MARKER(update_precomp.py): autogen include statement, do not remove
#include "precompiled_basegfx.hxx"
// autogenerated file with codegen.pl

#include "preextstl.h"
#include "gtest/gtest.h"
#include "postextstl.h"

#include <basegfx/tools/b2dclipstate.hxx>
#include <basegfx/range/b2dpolyrange.hxx>
#include <basegfx/polygon/b2dpolygon.hxx>
#include <basegfx/polygon/b2dpolygontools.hxx>
#include <basegfx/polygon/b2dpolypolygontools.hxx>
#include <basegfx/polygon/b2dpolypolygon.hxx>
#include <basegfx/numeric/ftools.hxx>

#include <boost/bind.hpp>

using namespace ::basegfx;

// FIXME:
#define RUN_OLD_FAILING_TESTS 0

namespace basegfx2d
{

class clipstate : public ::testing::Test
{
protected:
    tools::B2DClipState aUnion1;
    tools::B2DClipState aUnion2;
    tools::B2DClipState aIntersect;
    tools::B2DClipState aXor;
    tools::B2DClipState aSubtract;

public:
    virtual void SetUp()
    {
        B2DRange aCenter(100, 100, -100, -100);
        B2DRange aNorth(-10, -110, 10, -90);
        B2DRange aWest(-110, -10, -90, 10);
        B2DRange aSouth(-10, 110, 10, 90);
        B2DRange aEast(110, -10, 90, 10);

        aUnion1.unionRange(aCenter);
        aUnion1.unionRange(aNorth);
        aUnion1.unionRange(aWest);
        aUnion1.unionRange(aSouth);
        aUnion1.unionRange(aEast);

        aUnion2.makeNull();
        aUnion2.unionRange(aCenter);
        aUnion2.unionRange(aNorth);
        aUnion2.unionRange(aWest);
        aUnion2.unionRange(aSouth);
        aUnion2.unionRange(aEast);

        aIntersect.intersectRange(aCenter);
        aIntersect.intersectRange(aNorth);
        aIntersect.intersectRange(aWest);
        aIntersect.intersectRange(aSouth);
        aIntersect.intersectRange(aEast);

        aXor.makeNull();
        aXor.xorRange(aCenter);
        aXor.xorRange(aNorth);
        aXor.xorRange(aWest);
        aXor.xorRange(aSouth);
        aXor.xorRange(aEast);

        aSubtract.intersectRange(aCenter);
        aSubtract.subtractRange(aNorth);
        aSubtract.subtractRange(aWest);
        aSubtract.subtractRange(aSouth);
        aSubtract.subtractRange(aEast);
    }

    virtual void TearDown()
    {}

    void verifyPoly(const char* sName, const char* sSvg, const tools::B2DClipState& toTest)
    {
#if defined(VERBOSE)
        fprintf(stderr, "%s - svg:d=\"%s\"\n",
                sName, rtl::OUStringToOString(
                    basegfx::tools::exportToSvgD(toTest.getClipPoly(), true, true, false),
                    RTL_TEXTENCODING_UTF8).getStr() );
#endif

        B2DPolyPolygon aTmp1;
        ASSERT_TRUE(tools::importFromSvgD(
                               aTmp1, rtl::OUString::createFromAscii(sSvg), false, 0)) << sName;

        const rtl::OUString aSvg=
            tools::exportToSvgD(toTest.getClipPoly(), true, true, false);
        B2DPolyPolygon aTmp2;
        ASSERT_TRUE(tools::importFromSvgD(aTmp2, aSvg, false, 0)) << sName;

        ASSERT_TRUE(aTmp2 == aTmp1) << sName;
    }
};

TEST_F(clipstate, verifySimpleRange)
{
    const char* unionSvg="m100 10v90h-90v10h-20v-10h-90v-90h-10v-20h10v-90h90v-10h20v10h90v90h10v20z";
    const char* intersectSvg="m-100 10v-20h10v20zm80 90v-10h20v10zm-20-190v-10h20v10zm80 100v-20h10v20z";
    const char* xorSvg="m-100 10h10v-20h-10zm90 110h20v-10h-20zm0-180h20v-10h-20zm100 110h10v-20h-10zm10 20v90h-90v10h-20v-10h-90v-90h-10v-20h10v-90h90v-10h20v10h90v90h10v20z";
    const char* subtractSvg="m-90 10v-20h-10v-90h90v10h20v-10h90v90h-10v20h10v90h-90v-10h-20v10h-90v-90z";

    ASSERT_TRUE(aUnion1.isCleared()) << "cleared clip stays empty under union operation";
    verifyPoly("union", unionSvg, aUnion2);
#if RUN_OLD_FAILING_TESTS
    verifyPoly("intersect", intersectSvg, aIntersect);
    verifyPoly("xor", xorSvg, aXor);
#endif
    verifyPoly("subtract", subtractSvg, aSubtract);
}

TEST_F(clipstate, verifyMixedClips)
{
    tools::B2DClipState aMixedClip;

    const char* unionSvg="m100 10v90h-90v10h-20v-10h-90v-90h-10v-20h10v-90h90v-10h20v10h90v90h10v20z";

    B2DPolyPolygon aTmp1;
    tools::importFromSvgD(
        aTmp1, rtl::OUString::createFromAscii(unionSvg), false, 0);

    aMixedClip.intersectPolyPolygon(aTmp1);
    aMixedClip.subtractRange(B2DRange(-20,-150,20,0));
    aMixedClip.subtractRange(B2DRange(-150,-20,0,20));
    aMixedClip.xorRange(B2DRange(-150,-150,150,150));

    const char* mixedClipSvg="m0 0v20h-100v80h90v10h20v-10h90v-90h10v-20h-10v-90h-80v100zm-40-20v-80h-80v80zm-50 170v-300h300v300z";
#if RUN_OLD_FAILING_TESTS
    verifyPoly("mixed clip", mixedClipSvg, aMixedClip);
#endif
}

// -----------------------------------------------------------------------------

} // namespace basegfx2d