// Copyright (c) 2018, ETH Zurich and UNC Chapel Hill.
// 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 ETH Zurich and UNC Chapel Hill 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.
//
// Author: Johannes L. Schoenberger (jsch at inf.ethz.ch)

#include "estimators/triangulation.h"

#include <Eigen/Geometry>

#include "base/projection.h"
#include "base/triangulation.h"
#include "estimators/essential_matrix.h"
#include "optim/combination_sampler.h"
#include "optim/loransac.h"
#include "util/logging.h"
#include "util/math.h"

namespace colmap {

void TriangulationEstimator::SetMinTriAngle(const double min_tri_angle) {
  CHECK_GE(min_tri_angle, 0);
  min_tri_angle_ = min_tri_angle;
}

void TriangulationEstimator::SetResidualType(const ResidualType residual_type) {
  residual_type_ = residual_type;
}

std::vector<TriangulationEstimator::M_t> TriangulationEstimator::Estimate(
    const std::vector<X_t>& point_data,
    const std::vector<Y_t>& pose_data) const {
  CHECK_GE(point_data.size(), 2);
  CHECK_EQ(point_data.size(), pose_data.size());

  if (point_data.size() == 2) {
    // Two-view triangulation.

    const M_t xyz = TriangulatePoint(
        pose_data[0].proj_matrix, pose_data[1].proj_matrix,
        point_data[0].point_normalized, point_data[1].point_normalized);

    if (HasPointPositiveDepth(pose_data[0].proj_matrix, xyz) &&
        HasPointPositiveDepth(pose_data[1].proj_matrix, xyz) &&
        CalculateTriangulationAngle(pose_data[0].proj_center,
                                    pose_data[1].proj_center,
                                    xyz) >= min_tri_angle_) {
      return std::vector<M_t>{xyz};
    }
  } else {
    // Multi-view triangulation.

    std::vector<Eigen::Matrix3x4d> proj_matrices;
    proj_matrices.reserve(point_data.size());
    std::vector<Eigen::Vector2d> points;
    points.reserve(point_data.size());
    for (size_t i = 0; i < point_data.size(); ++i) {
      proj_matrices.push_back(pose_data[i].proj_matrix);
      points.push_back(point_data[i].point_normalized);
    }

    const M_t xyz = TriangulateMultiViewPoint(proj_matrices, points);

    // Check for cheirality constraint.
    for (const auto& pose : pose_data) {
      if (!HasPointPositiveDepth(pose.proj_matrix, xyz)) {
        return std::vector<M_t>();
      }
    }

    // Check for sufficient triangulation angle.
    for (size_t i = 0; i < pose_data.size(); ++i) {
      for (size_t j = 0; j < i; ++j) {
        const double tri_angle = CalculateTriangulationAngle(
            pose_data[i].proj_center, pose_data[j].proj_center, xyz);
        if (tri_angle >= min_tri_angle_) {
          return std::vector<M_t>{xyz};
        }
      }
    }
  }

  return std::vector<M_t>();
}

void TriangulationEstimator::Residuals(const std::vector<X_t>& point_data,
                                       const std::vector<Y_t>& pose_data,
                                       const M_t& xyz,
                                       std::vector<double>* residuals) const {
  CHECK_EQ(point_data.size(), pose_data.size());

  residuals->resize(point_data.size());

  for (size_t i = 0; i < point_data.size(); ++i) {
    if (HasPointPositiveDepth(pose_data[i].proj_matrix, xyz)) {
      if (residual_type_ == ResidualType::REPROJECTION_ERROR) {
        (*residuals)[i] = CalculateReprojectionError(point_data[i].point, xyz,
                                                     pose_data[i].proj_matrix,
                                                     *pose_data[i].camera);
      } else if (residual_type_ == ResidualType::ANGULAR_ERROR) {
        (*residuals)[i] = CalculateAngularError(point_data[i].point_normalized,
                                                xyz, pose_data[i].proj_matrix);
      }
    } else {
      (*residuals)[i] = std::numeric_limits<double>::max();
    }
  }
}

bool EstimateTriangulation(
    const EstimateTriangulationOptions& options,
    const std::vector<TriangulationEstimator::PointData>& point_data,
    const std::vector<TriangulationEstimator::PoseData>& pose_data,
    std::vector<char>* inlier_mask, Eigen::Vector3d* xyz) {
  CHECK_NOTNULL(inlier_mask);
  CHECK_NOTNULL(xyz);
  CHECK_GE(point_data.size(), 2);
  CHECK_EQ(point_data.size(), pose_data.size());
  options.Check();

  // Robustly estimate track using LORANSAC.
  LORANSAC<TriangulationEstimator, TriangulationEstimator,
           InlierSupportMeasurer, CombinationSampler>
      ransac(options.ransac_options);
  ransac.estimator.SetMinTriAngle(options.min_tri_angle);
  ransac.estimator.SetResidualType(options.residual_type);
  ransac.local_estimator.SetMinTriAngle(options.min_tri_angle);
  ransac.local_estimator.SetResidualType(options.residual_type);
  const auto report = ransac.Estimate(point_data, pose_data);
  if (!report.success) {
    return false;
  }

  *inlier_mask = report.inlier_mask;
  *xyz = report.model;

  return report.success;
}

}  // namespace colmap


// Boost.Geometry

// Copyright (c) 2017, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_PARAMETERS_HPP
#define BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_PARAMETERS_HPP

#include <boost/geometry/formulas/andoyer_inverse.hpp>
#include <boost/geometry/formulas/thomas_direct.hpp>
#include <boost/geometry/formulas/thomas_inverse.hpp>
#include <boost/geometry/formulas/vincenty_direct.hpp>
#include <boost/geometry/formulas/vincenty_inverse.hpp>

#include <boost/mpl/assert.hpp>
#include <boost/mpl/integral_c.hpp>

namespace boost { namespace geometry { namespace strategy
{

struct andoyer
{
    template
    <
        typename CT,
        bool EnableCoordinates = true,
        bool EnableReverseAzimuth = false,
        bool EnableReducedLength = false,
        bool EnableGeodesicScale = false
    >
    struct direct
            : formula::thomas_direct
              <
                  CT, false,
                  EnableCoordinates, EnableReverseAzimuth,
                  EnableReducedLength, EnableGeodesicScale
              >
    {};

    template
    <
        typename CT,
        bool EnableDistance,
        bool EnableAzimuth,
        bool EnableReverseAzimuth = false,
        bool EnableReducedLength = false,
        bool EnableGeodesicScale = false
    >
    struct inverse
        : formula::andoyer_inverse
            <
                CT, EnableDistance,
                EnableAzimuth, EnableReverseAzimuth,
                EnableReducedLength, EnableGeodesicScale
            >
    {};
};

struct thomas
{
    template
    <
        typename CT,
        bool EnableCoordinates = true,
        bool EnableReverseAzimuth = false,
        bool EnableReducedLength = false,
        bool EnableGeodesicScale = false
    >
    struct direct
            : formula::thomas_direct
              <
                  CT, true,
                  EnableCoordinates, EnableReverseAzimuth,
                  EnableReducedLength, EnableGeodesicScale
              >
    {};

    template
    <
        typename CT,
        bool EnableDistance,
        bool EnableAzimuth,
        bool EnableReverseAzimuth = false,
        bool EnableReducedLength = false,
        bool EnableGeodesicScale = false
    >
    struct inverse
        : formula::thomas_inverse
            <
                CT, EnableDistance,
                EnableAzimuth, EnableReverseAzimuth,
                EnableReducedLength, EnableGeodesicScale
            >
    {};
};

struct vincenty
{
    template
    <
        typename CT,
        bool EnableCoordinates = true,
        bool EnableReverseAzimuth = false,
        bool EnableReducedLength = false,
        bool EnableGeodesicScale = false
    >
    struct direct
            : formula::vincenty_direct
              <
                  CT, EnableCoordinates, EnableReverseAzimuth,
                  EnableReducedLength, EnableGeodesicScale
              >
    {};

    template
    <
        typename CT,
        bool EnableDistance,
        bool EnableAzimuth,
        bool EnableReverseAzimuth = false,
        bool EnableReducedLength = false,
        bool EnableGeodesicScale = false
    >
    struct inverse
        : formula::vincenty_inverse
            <
                CT, EnableDistance,
                EnableAzimuth, EnableReverseAzimuth,
                EnableReducedLength, EnableGeodesicScale
            >
    {};
};

template <typename FormulaPolicy>
struct default_order
{
    BOOST_MPL_ASSERT_MSG
    (
        false, NOT_IMPLEMENTED_FOR_THIS_TYPE
        , (types<FormulaPolicy>)
    );
};

template<>
struct default_order<andoyer>
    : boost::mpl::integral_c<unsigned int, 1>
{};

template<>
struct default_order<thomas>
    : boost::mpl::integral_c<unsigned int, 2>
{};

template<>
struct default_order<vincenty>
    : boost::mpl::integral_c<unsigned int, 4>
{};

}}} // namespace boost::geometry::strategy

#endif // BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_PARAMETERS_HPP


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

  @copyright 2017 NumScale SAS

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

#include <boost/simd/range/segmented_aligned_range.hpp>
#include <boost/simd/detail/is_aligned.hpp>
#include <boost/simd/function/compare_equal.hpp>
#include <boost/simd/function/load.hpp>
#include <boost/simd/function/all.hpp>
#include <boost/simd/pack.hpp>
#include <type_traits>

namespace boost { namespace simd
{
  /*!
    @ingroup group-algo

    Returns true if the range [first, last) is equal
    to the range [first2, first2 + (last - first)), and false otherwise

    @param first, last    -   the first range of the elements to compare
    @param first2         -   the beginning of the second range of the elements to compare

    @par Requirement

      - @c first, @c last and @c out must be pointer to Vectorizable type.

    @par Example:

      @snippet equalrange.cpp equalrange

    @par Possible output:

      @snippet equalrange.txt equalrange

  **/
  template<typename T>
  bool equal(T const* first, T const* last, const T* first2)
  {
    auto pr = segmented_aligned_range(first,last);

    for(T e : pr.head) if (!(e == *first2++)) return false;

    // main SIMD part - checks if we can load efficiently or not
    if(boost::simd::detail::is_aligned(first2, pack<T>::alignment))
    {
      for(pack<T> e : pr.body )
      {
        if(!compare_equal( pack<T>(first2), e)) return false;
        first2 += pack<T>::static_size;
      }
    }
    else
    {
      for(pack<T> e : pr.body )
      {
        if(!compare_equal(load<pack<T>>(first2), e)) return false;
        first2 += pack<T>::static_size;
      }
    }

    for(T e : pr.tail ) if(!(e == *first2++)) return false;

    return true;
  }

  /*!
    @ingroup group-algo

    Returns true if all the pairs in the range [first1, last1) and the
    range [first2, first2 + (last1 - first1)), satisfy f and false otherwise

    @param first1, last1  -   the first range of the elements to compare
    @param first2         -   the beginning of the second range of the elements to compare
    @param f       binary predicate operation function object that will be applied.

    @par Requirement

      - @c first, @c last and @c out must be pointer to Vectorizable type.

      - @c f must be a polymorphic binary function object, i.e callable on generic types.
      -  boost::simd::pack<T1> and boost::simd::pack<T2> must have the same cardinal.

    @par Example:

      @snippet equalrange.cpp equalrange

    @par Possible output:

      @snippet equalrange.txt equalrange

  **/
  template<typename T, typename Pred>
  bool equal(T const* first1, T const* last1, T const* first2, Pred const& f)
  {
    auto pr = segmented_aligned_range(first1,last1);

    for(T e : pr.head ) if (!f(e, *first2++)) return false;

    if(  boost::simd::detail::is_aligned(first2 , pack<T>::alignment))
    {
      for(pack<T> e : pr.body )
      {
        if(!all(f(e, pack<T>(first2)))) return false;
        first2  += pack<T>::static_size;
      }
    }
    else
    {
      for(pack<T> e : pr.body )
      {
        if(!all(f(e, load<pack<T>>(first2)))) return false;
        first2  += pack<T>::static_size;
      }
    }

    for(T e : pr.tail )  if (!f(e, *first2++)) return false;

    return true;
  }
} }

#endif


#include <iostream>
#include <string>

#include <boost/program_options.hpp>
#include <amgcl/util.hpp>
#include <amgcl/adapter/crs_tuple.hpp>
#include <amgcl/io/mm.hpp>
#include <amgcl/io/binary.hpp>

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

    using amgcl::precondition;

    po::options_description desc("Options");

    desc.add_options()
        ("help,h", "Show this help.")
        ("dense,d", po::bool_switch()->default_value(false),
         "Matrix is dense.")
        ("input,i", po::value<std::string>()->required(),
         "Input binary file.")
        ("output,o", po::value<std::string>()->required(),
         "Ouput matrix in the MatrixMarket format.")
        ;

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);

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

    po::notify(vm);

    if (vm["dense"].as<bool>()) {
        size_t n, m;
        std::vector<double> v;

        io::read_dense(vm["input"].as<std::string>(), n, m, v);
        io::mm_write(vm["output"].as<std::string>(), v.data(), n, m);

        std::cout
            << "Wrote " << n << " by " << m << " dense matrix"
            << std::endl;
    } else {
        size_t n;
        std::vector<ptrdiff_t> ptr, col;
        std::vector<double> val;

        io::read_crs(vm["input"].as<std::string>(), n, ptr, col, val);
        io::mm_write(vm["output"].as<std::string>(), std::tie(n, ptr, col, val));

        std::cout
            << "Wrote " << n << " by " << n << " sparse matrix, "
            << ptr.back() << " nonzeros" << std::endl;
    }
}


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

    @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-reduction
     This function object returns the sum of the elements of the SIMD argument

    @see sum

    @par Header <boost/simd/function/sum.hpp>

    @par Example:

      @snippet sum.cpp sum

    @par Possible output:

      @snippet sum.txt sum

  **/
  scalar_of_t<Value> sum(Value const& x);
} }
#endif

#include <boost/simd/function/scalar/sum.hpp>
#include <boost/simd/function/simd/sum.hpp>

#endif


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

#include <boost/test/unit_test.hpp>

#include "../unittest_config.h"

#include "clotho/data_spaces/association_matrix.hpp"
#include "clotho/utility/bit_helper.hpp"
#include <vector>
#include <iostream>
#include <iomanip>

using namespace clotho::genetics;

BOOST_AUTO_TEST_SUITE( test_data_space )

BOOST_AUTO_TEST_CASE( association_matrix_sizing_test ) {

    typedef unsigned long long block_type;
    typedef clotho::utility::BitHelper< block_type > bit_helper_type;

    size_t exp_rows = 5, exp_cols = 2;

    size_t exp_size = exp_rows * exp_cols;
    size_t exp_alloc_size = (exp_cols / bit_helper_type::BITS_PER_BLOCK + 1) * exp_rows;

    association_matrix< block_type, column_aligned > amat( exp_rows, exp_cols );

    size_t obs_size = amat.size();
    size_t obs_alloc_size = amat.allocated_size();

    BOOST_REQUIRE_MESSAGE( obs_size == exp_size, "Unexpected matrix size; Observed size: " << obs_size << "; Expected size: " << exp_size );
    BOOST_REQUIRE_MESSAGE( obs_alloc_size == exp_alloc_size, "Unexpected allocated size: Observed allocated size: " << obs_alloc_size << "; Expected allocated size: " << exp_alloc_size );

}

BOOST_AUTO_TEST_CASE( association_matrix_flip_test ) {
    typedef unsigned long long block_type;
    typedef clotho::utility::BitHelper< block_type > bit_helper_type;

    size_t exp_rows = 5, exp_cols = 12;

    association_matrix< block_type, column_aligned > amat( exp_rows, exp_cols );

    size_t flip_row = 3, flip_col = 6;

    bool exp_state = !amat( flip_row, flip_col );

    amat.flip(flip_row, flip_col );
    bool obs_state = amat( flip_row, flip_col );

    BOOST_REQUIRE_MESSAGE( obs_state == exp_state, "Unexpected flip state; Observed state: " << obs_state << "; Expected state: " << exp_state );
}

BOOST_AUTO_TEST_CASE( association_matrix_raw_iterator_test ) {
    typedef unsigned long long block_type;

    typedef unsigned long long block_type;
    typedef clotho::utility::BitHelper< block_type >    bit_helper_type;

    typedef association_matrix< block_type, column_aligned >            association_type;
    typedef typename association_type::block_iterator   iterator;

    size_t exp_rows = 5, exp_cols = 240;

    association_type amat( exp_rows, exp_cols );

    size_t exp_blocks = exp_rows * (exp_cols / bit_helper_type::BITS_PER_BLOCK + 1);

    amat.clear();

    iterator it = amat.raw_iterator();

    size_t obs_blocks = 0;
    size_t i = 0, j = 0;
    while( it.hasNext() ) {
        block_type b = it.next();

        BOOST_REQUIRE_MESSAGE( b == 0, "Unexpected block state at row: " << i << ", column: " << j << "; Observed: " << b << "; Expected: 0" );

        if( ++j >= it.max_columns() ) {
            j = 0;
            ++i;
        }
        ++obs_blocks;
    }

    BOOST_REQUIRE_MESSAGE( exp_blocks == obs_blocks, "Unexpected number of raw blocks; Observed: " << obs_blocks << "; Expected: " << exp_blocks );
}

BOOST_AUTO_TEST_CASE( association_matrix_raw_iterator_test2 ) {
    typedef unsigned long long block_type;

    typedef unsigned long long block_type;
    typedef clotho::utility::BitHelper< block_type >    bit_helper_type;

    typedef association_matrix< block_type, column_aligned >            association_type;
    typedef typename association_type::block_iterator   iterator;

    size_t exp_rows = 5, exp_cols = 240;

    association_type amat( exp_rows, exp_cols );

    size_t exp_block_count = exp_rows * (exp_cols / bit_helper_type::BITS_PER_BLOCK + 1);

    amat.clear();

    std::vector< block_type > exp_blocks;

    size_t i = 0, j = 0, k = 0;
    block_type _b = 0;
    while( i < exp_rows ) {
        amat.flip( i, j );

        if( k != j / bit_helper_type::BITS_PER_BLOCK ) {
            exp_blocks.push_back(_b);
            _b = 0;
            ++k;
        }

        _b |= (1L << (j % bit_helper_type::BITS_PER_BLOCK));

        j += 15;
        if( j >= exp_cols ) {
            exp_blocks.push_back( _b );
            j = 0;
            k = 0;
            _b = 0;
            ++i;
        }
    }

    iterator it = amat.raw_iterator();

    size_t obs_blocks = 0;
    i = 0; j = 0;

    while( it.hasNext() ) {
        block_type b = it.next();

        size_t idx = j * it.max_rows() + i;

        BOOST_REQUIRE_MESSAGE( b == exp_blocks[idx], "Unexpected block state at row: " << std::dec << i << ", column: " << std::dec << j << "; Observed: " << std::hex << std::setw( sizeof( block_type ) * 2) << std::setfill('0') << b << "; Expected [" << idx << "]: " << std::hex << std::setw( sizeof(block_type) * 2) << std::setfill('0') << exp_blocks[idx] );

        if( ++j >= it.max_columns() ) {
            j = 0;
            ++i;
        }
        ++obs_blocks;
    }

    BOOST_REQUIRE_MESSAGE( exp_block_count == obs_blocks, "Unexpected number of raw blocks; Observed: " << obs_blocks << "; Expected: " << exp_block_count );
}

BOOST_AUTO_TEST_CASE( association_matrix_row_iterator_test ) {
    typedef unsigned long long block_type;

    typedef unsigned long long block_type;
    typedef clotho::utility::BitHelper< block_type >    bit_helper_type;

    typedef association_matrix< block_type, column_aligned >            association_type;
    typedef typename association_type::row_iterator   iterator;

    size_t exp_rows = 5, exp_cols = 240;

    association_type amat( exp_rows, exp_cols );

    size_t blocks_per_row = (exp_cols / bit_helper_type::BITS_PER_BLOCK + 1);

    amat.clear();

    std::vector< block_type > exp_blocks;

    size_t i = 0, j = 0, k = 0;
    block_type _b = 0;
    while( i < exp_rows ) {
        amat.flip( i, j );

        if( k != j / bit_helper_type::BITS_PER_BLOCK ) {
            exp_blocks.push_back(_b);
            _b = 0;
            ++k;
        }

        _b |= (1L << (j % bit_helper_type::BITS_PER_BLOCK));

        j += 15;
        if( j >= exp_cols ) {
            exp_blocks.push_back( _b );
            j = 0;
            k = 0;
            _b = 0;
            ++i;
        }
    }

    size_t obs_blocks = 0;
    i = 2; j = 0;

    iterator it = amat.getRowAt( i );

    while( it.hasNext() ) {
        block_type b = it.next();

        size_t idx = i * blocks_per_row + j;

        BOOST_REQUIRE_MESSAGE( b == exp_blocks[idx], "Unexpected block state at row: " << std::dec << i << ", column: " << std::dec << j << "; Observed: " << std::hex << std::setw( sizeof( block_type ) * 2) << std::setfill('0') << b << "; Expected [" << idx << "]: " << std::hex << std::setw( sizeof(block_type) * 2) << std::setfill('0') << exp_blocks[idx] );

        ++j;
        ++obs_blocks;
    }

    BOOST_REQUIRE_MESSAGE( blocks_per_row == obs_blocks, "Unexpected number of raw blocks; Observed: " << obs_blocks << "; Expected: " << blocks_per_row );
}

BOOST_AUTO_TEST_CASE( association_matrix_column_iterator_test ) {
    typedef unsigned long long block_type;

    typedef unsigned long long block_type;
    typedef clotho::utility::BitHelper< block_type >    bit_helper_type;

    typedef association_matrix< block_type, column_aligned >            association_type;
    typedef typename association_type::column_iterator  iterator;

    size_t exp_rows = 5, exp_cols = 240;

    association_type amat( exp_rows, exp_cols );

    size_t blocks_per_row = (exp_cols / bit_helper_type::BITS_PER_BLOCK + 1);
    size_t blocks_per_column = exp_rows;

    amat.clear();

    std::vector< block_type > exp_blocks;

    size_t i = 0, j = 0, k = 0;
    block_type _b = 0;
    while( i < exp_rows ) {
        amat.flip( i, j );

        if( k != j / bit_helper_type::BITS_PER_BLOCK ) {
            exp_blocks.push_back(_b);
            _b = 0;
            ++k;
        }

        _b |= (1L << (j % bit_helper_type::BITS_PER_BLOCK));

        j += 15;
        if( j >= exp_cols ) {
            exp_blocks.push_back( _b );
            j = 0;
            k = 0;
            _b = 0;
            ++i;
        }
    }

    size_t obs_blocks = 0;
    i = 2; j = 0;

    iterator it = amat.getColumnAt( i );

    while( it.hasNext() ) {
        block_type b = it.next();

        size_t idx = j * blocks_per_row + i;

        BOOST_REQUIRE_MESSAGE( b == exp_blocks[idx], "Unexpected block state at row: " << std::dec << i << ", column: " << std::dec << j << "; Observed: " << std::hex << std::setw( sizeof( block_type ) * 2) << std::setfill('0') << b << "; Expected [" << idx << "]: " << std::hex << std::setw( sizeof(block_type) * 2) << std::setfill('0') << exp_blocks[idx] );

        ++j;
        ++obs_blocks;
    }

    BOOST_REQUIRE_MESSAGE( blocks_per_column == obs_blocks, "Unexpected number of raw blocks; Observed: " << obs_blocks << "; Expected: " << blocks_per_column );
}
BOOST_AUTO_TEST_SUITE_END()


#include "RegularizationLoss.h"
#include <sstream>
#include <boost/lexical_cast.hpp>
#include "Generic/common/UnexpectedInputException.h"

using Eigen::VectorXd;

RegularizationLoss::RegularizationLoss(const VectorXd& params,
	const VectorXd& pos_regularization_weights,
	const VectorXd& neg_regularization_weights, double lossWeight)
: _params(params), _pos_regularization_weights(pos_regularization_weights),
_neg_regularization_weights(neg_regularization_weights),
_lossWeight(lossWeight)
{
	if (_pos_regularization_weights.size() != _params.size()
			|| _neg_regularization_weights.size() != _params.size()) {
		throw UnexpectedInputException(
			"RegularizationLoss::RegularizationLoss",
			"Size of regularization weights vector must "
			"equal the number of features.");
	}
	for (int i=0; i<_pos_regularization_weights.size(); ++i) {
		if (_pos_regularization_weights[i] == 0.0
				|| _neg_regularization_weights[i] == 0.0)
		{
			std::wstringstream err;
			err << L"Feature " << i << L" has a regularization weight of 0.0;"
				<< L" your model will not converge properly.";
			throw UnexpectedInputException("RegularizationLoss::RegularizationLoss",
				err);
		}
	}
}

double RegularizationLoss::operator()(VectorXd& gradient, size_t thread_idx,
		size_t n_threads) const
{
	double loss = 0.0;
	size_t sz = _params.size();

	for (size_t i=thread_idx; i<sz; i+=n_threads) {
		// regularization is weighted both overall and per-feature
		double weight = (_params[i]<0)?_neg_regularization_weights[i]:
			_pos_regularization_weights[i];

		loss -= weight* _lossWeight*_params[i]*_params[i];
		gradient[i] -= 2.0*weight* _lossWeight*_params[i];
	}

	_debug_loss = loss;
	return loss;
}

std::wstring RegularizationLoss::status() const {
	return L"reg_loss = " + boost::lexical_cast<std::wstring>(_debug_loss);
}

void RegularizationLoss::snapshot() {
}

double RegularizationLoss::forInstance(int inst, Eigen::SparseVector<double>& gradient) const {
	return 0.0;
}


/*

PICCANTE
The hottest HDR imaging library!
http://vcg.isti.cnr.it/piccante

Copyright (C) 2014
Visual Computing Laboratory - ISTI CNR
http://vcg.isti.cnr.it
First author: Francesco Banterle

This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.

*/

#ifndef PIC_COMPUTER_VISION_NELDER_MEAD_OPT_ICP_2D_HPP
#define PIC_COMPUTER_VISION_NELDER_MEAD_OPT_ICP_2D_HPP

#include "../util/eigen_util.hpp"
#include "../util/std_util.hpp"
#include "../util/matrix_3_x_3.hpp"
#include "../util/nelder_mead_opt_base.hpp"

#include "../computer_vision/iterative_closest_point_2D.hpp"

#ifndef PIC_DISABLE_EIGEN
#ifndef PIC_EIGEN_NOT_BUNDLED
   #include "../externals/Eigen/Dense"
#else
    #include <Eigen/Dense>
#endif
#endif

namespace pic {

#ifndef PIC_DISABLE_EIGEN

class NelderMeadOptICP2D: public NelderMeadOptBase<float>
{
public:
    std::vector< Eigen::Vector2f > points_pattern, points;

    /**
     * @brief NelderMeadOptICP2D
     * @param points_pattern
     * @param points
     */
    NelderMeadOptICP2D(std::vector< Eigen::Vector2f > &points_pattern,
                       std::vector< Eigen::Vector2f > &points) : NelderMeadOptBase()
    {
        std::copy(points_pattern.begin(), points_pattern.end(),
                  std::back_inserter(this->points_pattern));

        std::copy(points.begin(), points.end(),
                  std::back_inserter(this->points));
    }

    /**
     * @brief function
     * @param x
     * @param n
     * @return
     */
    float function(float *x, unsigned int n)
    {
        float scale = 1.0f;
        if(n == 4) {
            if(x[3] < 1.0f) {
                return FLT_MAX;
            }

            scale = x[3];
        }

        ICP2DTransform t(x[0], x[1], x[2], scale);

        std::vector< Eigen::Vector2f > out;
        t.applyC(points_pattern, out);

        return getErrorPointsList(out, points);
    }
};
#endif

}

#endif // PIC_COMPUTER_VISION_NELDER_MEAD_OPT_ICP_2D_HPP


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_ASHAPE_INCLUDE
#define MTL_ASHAPE_INCLUDE

#include <vector>
#include <boost/type_traits/is_same.hpp>
#include <boost/mpl/if.hpp>

#include <boost/numeric/mtl/mtl_fwd.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/utility/root.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/utility/static_assert.hpp>

// Not elegant but necessary to treat ITL types right
#include <boost/numeric/itl/itl_fwd.hpp>

#ifdef MTL_WITH_INITLIST
# include <initializer_list>
#endif

namespace mtl {

/// Namespace for algebraic shapes; used for sophisticated dispatching between operations
namespace ashape {

// forward declaration
template <typename T> struct ashape_aux;

/// Tag for arbitrary algebraic shape
struct universe {};

// Types (tags)
/// Scalar algebraic shape
struct scal : universe {};

/// Non-scalar algebraic shape
struct nonscal : universe {};
/// Row vector as algebraic shape
template <typename Value> struct rvec : nonscal {};
/// Column vector as algebraic shape
template <typename Value> struct cvec : nonscal {};
/// Matrix as algebraic shape
template <typename Value> struct mat : nonscal {};
/// Undefined shape, e.g., for undefined results of operations
struct ndef {};
/// Future shape, i.e. after appropriate evaluation it will have the shape \p Value
template <typename Value> struct future : nonscal {};

/// Meta-function for algebraic shape of T
/** Unknown types are treated like scalars. ashape of collections are template
    parameterized with ashape of their elements, e.g., ashape< matrix < vector < double > > >::type is
    mat< rvec < scal > > >.
    Implemented with ashape_aux after type is cleaned up with mtl::traits::root.
**/
template <typename T>
struct ashape
  : ashape_aux<typename mtl::traits::root<T>::type> {};

template <typename T>
struct ashape_aux
{
    typedef scal type;
};

/// Vectors must be distinguished between row and column vectors
template <typename Value, typename Parameters>
struct ashape_aux<mtl::vector::dense_vector<Value, Parameters> >
{
    typedef typename boost::mpl::if_<
	boost::is_same<typename Parameters::orientation, row_major>
      , rvec<typename ashape<Value>::type>
      , cvec<typename ashape<Value>::type>
    >::type type;
};

/// Same as dense vector
template <typename Value, typename Parameters>
struct ashape_aux<vector::strided_vector_ref<Value, Parameters> >
  : ashape<mtl::vector::dense_vector<Value, Parameters> > {};

/// Same as dense vector
template <typename Value, typename Parameters>
struct ashape_aux<vector::sparse_vector<Value, Parameters> >
  : ashape<mtl::vector::dense_vector<Value, Parameters> > {};

/// One-dimensional arrays have rvec ashape; 2D arrays are matrices see below
template <typename Value, unsigned Rows>
struct ashape_aux<Value[Rows]>
{
    typedef rvec<typename ashape<Value>::type> type;
};

#ifdef MTL_WITH_INITLIST
/// Non-nested initializer_list have rvec ashape, nested lists are matrices see below
template <typename Value>
struct ashape_aux<std::initializer_list<Value> >
{
    typedef rvec<typename ashape<Value>::type> type;
};
#endif

/// std::vectors have rvec ashape
template <typename Value, typename Allocator>
struct ashape_aux<std::vector<Value, Allocator> >
{
    typedef rvec<typename ashape<Value>::type> type;
};

/// One-dimensional arrays have rvec ashape; 2D arrays are matrices see below
template <typename Value>
struct ashape_aux<Value*>
{
    typedef rvec<typename ashape<Value>::type> type;
};

template <typename E1, typename E2, typename SFunctor>
struct ashape_aux< vector::vec_vec_pmop_expr<E1, E2, SFunctor> >
{
    MTL_STATIC_ASSERT((boost::is_same<typename ashape<E1>::type,
			                typename ashape<E2>::type>::value), "Operands must have same algebraic shape.");
    typedef typename ashape<E1>::type type;
};

template <typename E1, typename E2, typename SFunctor>
struct ashape_aux< vector::vec_vec_op_expr<E1, E2, SFunctor> >
{
#if 0 // not sure if this is true in all operations
    MTL_STATIC_ASSERT((boost::is_same<typename ashape<E1>::type,
				      typename ashape<E2>::type>::value), "Operands must have same algebraic shape.");
#endif
    typedef typename ashape<E1>::type type;
};

template <typename E1, typename E2, typename SFunctor>
struct ashape_aux< vector::vec_vec_aop_expr<E1, E2, SFunctor> >
{
    typedef typename ashape<E1>::type type;
};

template <typename E1, typename E2, typename SFunctor>
struct ashape_aux< vector::vec_scal_aop_expr<E1, E2, SFunctor> >
{
    typedef typename ashape<E1>::type type;
};

template <typename Vector>
struct ashape_aux< vector::vec_const_ref_expr<Vector> >
{
    typedef typename ashape<Vector>::type type;
};

// ========
// Matrices
// ========

template <typename Value, typename Parameters>
struct ashape_aux<mtl::matrix::compressed2D<Value, Parameters> >
{
    typedef mat<typename ashape<Value>::type> type;
};

template <typename Value, typename Parameters>
struct ashape_aux<mtl::matrix::coordinate2D<Value, Parameters> >
{
    typedef mat<typename ashape<Value>::type> type;
};

template <typename Value, typename Parameters>
struct ashape_aux<mtl::matrix::sparse_banded<Value, Parameters> >
{
    typedef mat<typename ashape<Value>::type> type;
};

template <typename Value, typename Parameters>
struct ashape_aux<mtl::matrix::dense2D<Value, Parameters> >
{
    typedef mat<typename ashape<Value>::type> type;
};

template <typename Value, std::size_t Mask, typename Parameters>
struct ashape_aux<mtl::matrix::morton_dense<Value, Mask, Parameters> >
{
    typedef mat<typename ashape<Value>::type> type;
};

template <typename Functor>
struct ashape_aux<mtl::matrix::implicit_dense<Functor> >
{
    typedef mat<typename ashape<typename Functor::result_type>::type> type;
};

/// Two-dimensional arrays have mat ashape; 1D arrays are vectors see above
template <typename Value, unsigned Rows, unsigned Cols>
struct ashape_aux<Value[Rows][Cols]>
{
    typedef mat<typename ashape<Value>::type> type;
};

/// Two-dimensional arrays have mat ashape; 1D arrays are vectors see above
template <typename Value, unsigned Cols>
struct ashape_aux<Value (*)[Cols]>
{
    typedef mat<typename ashape<Value>::type> type;
};

#ifdef MTL_WITH_INITLIST
/// Nested initializer_list are matrices, non-nested are vectors see above
template <typename Value>
struct ashape_aux<std::initializer_list<std::initializer_list<Value> > >
{
    typedef mat<typename ashape<Value>::type> type;
};
#endif

template <typename Vector>
struct ashape_aux<mtl::matrix::multi_vector<Vector> >
{
    typedef mat<typename ashape<typename mtl::Collection<mtl::matrix::multi_vector<Vector> >::value_type>::type> type;
};

template <typename Value>
struct ashape_aux<matrix::element_structure<Value> >
{
   typedef mat<typename ashape<Value>::type> type;
};

template <typename Value, typename Parameters>
struct ashape_aux<mtl::matrix::ell_matrix<Value, Parameters> >
{
   typedef mat<typename ashape<Value>::type> type;
};

template <typename Vector>
struct ashape_aux<matrix::multi_vector_range<Vector> >
{
    typedef mat<typename ashape<typename mtl::Collection<matrix::multi_vector_range<Vector> >::value_type>::type> type;
};

template <typename E1, typename E2, typename SFunctor>
struct ashape_aux< matrix::mat_mat_op_expr<E1, E2, SFunctor> >
{
    MTL_STATIC_ASSERT((boost::is_same<typename ashape<E1>::type,
				      typename ashape<E2>::type>::value), "Operands must have same algebraic shape.");
    typedef typename ashape<E1>::type type;
};

template <typename Vector1, typename Vector2>
struct ashape< matrix::outer_product_matrix<Vector1, Vector2> >
{
    // BOOST_STATIC_ASSERT((boost::is_same<typename ashape<E1>::type,
    // 			                typename transposed_shape<typename ashape<E2>::type>::type>::value));
    typedef mat<typename ashape<typename mtl::Collection<Vector1>::value_type>::type> type;
};

template <typename Matrix, typename VectorIn>
struct ashape< vector::mat_cvec_multiplier<Matrix, VectorIn> >
{
    typedef cvec<scal> type;
};

// =====
// Views
// =====

template <typename Functor, typename Coll>
struct ashape_aux<matrix::map_view<Functor, Coll> >
{
    typedef typename ashape<Coll>::type type;
};

template <typename Functor, typename Coll>
struct ashape_aux<vector::map_view<Functor, Coll> >
{
    typedef typename ashape<Coll>::type type;
};

template <typename Coll>
struct ashape_aux<vector::conj_view<Coll> >
{
    typedef typename ashape<Coll>::type type;
};

template <typename Coll>
struct ashape_aux<vector::real_view<Coll> >
{
    typedef typename ashape<Coll>::type type;
};

template <typename Coll>
struct ashape_aux<vector::imag_view<Coll> >
{
    typedef typename ashape<Coll>::type type;
};

#if 1
// shouldn't be needed
template <typename Coll>
struct ashape_aux<mtl::matrix::transposed_view<const matrix::conj_view<Coll> > >
{
    typedef typename ashape<Coll>::type type;
};
#endif

template <typename Matrix>
struct ashape_aux<matrix::transposed_view<Matrix> >
{
    typedef typename ashape<Matrix>::type type;
};

template <typename Matrix>
struct ashape_aux<matrix::banded_view<Matrix> >
{
    typedef typename ashape<Matrix>::type type;
};

template <typename Matrix>
struct ashape_aux<matrix::indirect<Matrix> >
{
    typedef typename ashape<Matrix>::type type;
};

// Rule out other types as algebraic shape
template <typename IFStream, typename OFStream>
struct ashape_aux<io::matrix_file<IFStream, OFStream> >
{
    typedef ndef type;
};

// =====================
// Shapes of products:
// =====================

// a) The result's shape
// b) Classify operation in terms of shape

// Operation types:

struct scal_scal_mult {};
struct cvec_rvec_mult {}; // outer product
struct rvec_cvec_mult {}; // inner product (without conj)
struct rvec_mat_mult {};
struct mat_cvec_mult {};
struct mat_mat_mult {};
struct scal_rvec_mult {};
struct scal_cvec_mult {};
struct scal_mat_mult {};
struct rvec_scal_mult {};
struct cvec_scal_mult {};
struct mat_scal_mult {};

// =====================
// Results of operations
// =====================

/*
      s  cv  rv   m
-------------------
 s |  s  cv* rv*  m*
cv | cv*  x   m   x
rv | rv*  s   x  rv
 m |  m* cv   x   m

 * only on outer level, forbidden for elements of collections

*/

// Results for elements of collections, i.e. scalar * matrix (vector) are excluded

/// Algebraic shape of multiplication's result when elements of collections are multiplied.
/** The types are the same as for multiplications of entire collections except that scalar *
    matrix (or vector) is excluded to avoid ambiguities.
    emult_shape <Shape1, Shape2> is only properly defined if emult_op <Shape1, Shape2>::type is not ndef!
**/
template <typename Shape1, typename Shape2>
struct emult_shape
{
    typedef ndef type;
};

/// Type of operation when values of Shape1 and Shape2 are multiplied (so far only for elements of collections)
/** The types are the same as for multiplications of entire collections except that scalar *
    matrix (or vector) is excluded to avoid ambiguities. **/
template <typename Shape1, typename Shape2>
struct emult_op
{
    typedef ndef type;
};

// Scalar * scalar -> scalar
template <>
struct emult_shape<scal, scal>
{
    typedef scal type;
};

template <>
struct emult_op<scal, scal>
{
    typedef scal_scal_mult type;
};

// Column times row vector, i.e. outer product
template <typename Value1, typename Value2>
struct emult_shape<cvec<Value1>, rvec<Value2> >
{
    typedef mat<typename emult_shape<Value1, Value2>::type> type;
};

template <typename Value1, typename Value2>
struct emult_op<cvec<Value1>, rvec<Value2> >
{
    // if product of elements is undefined then product is undefined too
    typedef typename boost::mpl::if_<
	boost::is_same<typename emult_op<Value1, Value2>::type, ndef>
      , ndef
      , cvec_rvec_mult
    >::type type;
};

// Row times column vector, i.e. inner product (without conj)
template <typename Value1, typename Value2>
struct emult_shape<rvec<Value1>, cvec<Value2> >
{
    typedef typename emult_shape<Value1, Value2>::type type;
};

template <typename Value1, typename Value2>
struct emult_op<rvec<Value1>, cvec<Value2> >
{
    // if product of elements is undefined then product is undefined too
    typedef typename boost::mpl::if_<
	boost::is_same<typename emult_op<Value1, Value2>::type, ndef>
      , ndef
      , rvec_cvec_mult
    >::type type;
};

// Row vector times matrix
template <typename Value1, typename Value2>
struct emult_shape<rvec<Value1>, mat<Value2> >
{
    typedef rvec<typename emult_shape<Value1, Value2>::type> type;
};

template <typename Value1, typename Value2>
struct emult_op<rvec<Value1>, mat<Value2> >
{
    // if product of elements is undefined then product is undefined too
    typedef typename boost::mpl::if_<
	boost::is_same<typename emult_op<Value1, Value2>::type, ndef>
      , ndef
      , rvec_mat_mult
    >::type type;
};

// Matrix times column vector
template <typename Value1, typename Value2>
struct emult_shape<mat<Value1>, cvec<Value2> >
{
    typedef cvec<typename emult_shape<Value1, Value2>::type> type;
};

template <typename Value1, typename Value2>
struct emult_op<mat<Value1>, cvec<Value2> >
{
    // if product of elements is undefined then product is undefined too
    typedef typename boost::mpl::if_<
	boost::is_same<typename emult_op<Value1, Value2>::type, ndef>
      , ndef
      , mat_cvec_mult
    >::type type;
};

// Matrix product
template <typename Value1, typename Value2>
struct emult_shape<mat<Value1>, mat<Value2> >
{
    typedef mat<typename emult_shape<Value1, Value2>::type> type;
};

template <typename Value1, typename Value2>
struct emult_op<mat<Value1>, mat<Value2> >
{
    // if product of elements is undefined then product is undefined too
    typedef typename boost::mpl::if_<
	boost::is_same<typename emult_op<Value1, Value2>::type, ndef>
      , ndef
      , mat_mat_mult
    >::type type;
};

// Results for entire collections, i.e. scalar * matrix (vector) are allowed

// Multiplying collections as emult

template  <typename Shape1, typename Shape2>
struct mult_shape
    : public emult_shape<Shape1, Shape2>
{};

template  <typename Shape1, typename Shape2>
struct mult_op
    : public emult_op<Shape1, Shape2>
{};

// Scale collection from left

template <typename Shape2>
struct mult_shape<scal, Shape2>
{
    typedef Shape2 type;
};

template <typename Value2>
struct mult_op<scal, rvec<Value2> >
{
    typedef scal_rvec_mult type;
};

template <typename Value2>
struct mult_op<scal, cvec<Value2> >
{
    typedef scal_cvec_mult type;
};

template <typename Value2>
struct mult_op<scal, mat<Value2> >
{
    typedef scal_mat_mult type;
};

// Scale collection from right

template <typename Shape1>
struct mult_shape<Shape1, scal>
{
    typedef Shape1 type;
};

template <typename Value1>
struct mult_op<rvec<Value1>, scal>
{
    typedef rvec_scal_mult type;
};

template <typename Value1>
struct mult_op<cvec<Value1>, scal>
{
    typedef cvec_scal_mult type;
};

template <typename Value1>
struct mult_op<mat<Value1>, scal>
{
    typedef mat_scal_mult type;
};

// Arbitration
template <>
struct mult_shape<scal, scal>
{
    typedef scal type;
};

// Needs to be verified for nested matrix types, cf. #140
template <typename E1, typename E2>
struct ashape< matrix::mat_mat_times_expr<E1, E2> >
{
    // typedef typename ashape<E1>::type type;
    typedef typename mult_shape<typename ashape<E1>::type,
				typename ashape<E2>::type>::type type;
};

template <typename E1, typename E2>
struct ashape< mat_cvec_times_expr<E1, E2> >
{
    // Resulting vector has the same shape as the multiplied
    typedef typename ashape<E2>::type type;
};

template <typename E1, typename E2>
struct ashape< vector::rvec_mat_times_expr<E1, E2> >
{
    // Resulting vector has the same shape as the multiplied
    typedef typename ashape<E1>::type type;
};

// added by Hui Li (below) -----------------------------------------

// =====================
// Shapes of divisions:
// =====================

// Operation types:

struct scal_scal_div {};
struct cvec_scal_div {};
struct rvec_scal_div {};
struct mat_scal_div {};

template < typename Shape1, typename Shape2 >
struct div_shape
{
	typedef ndef type;
};

template < typename Shape1, typename Shape2 >
struct div_op
{
	typedef ndef type;
};

template <>
struct div_shape<scal,scal>
{
	typedef scal type;
};

template<>
struct div_op<scal,scal>
{
	typedef scal type;
};

template < typename Value1 >
struct div_shape < rvec<Value1>, scal >
{
	typedef typename boost::mpl::if_<
		typename boost::is_same<typename div_shape<Value1,scal>::type,ndef>::type,
		ndef,
		rvec<typename div_shape<Value1,scal>::type>
	>::type type;
};

template < typename Value1 >
struct div_op< rvec<Value1>, scal >
{
	typedef typename boost::mpl::if_<
		typename boost::is_same<typename div_shape<rvec<Value1>,scal>::type,ndef>::type,
		ndef,
		rvec_scal_div
	>::type type;
};

template < typename Value1 >
struct div_shape < cvec<Value1>, scal >
{
	typedef typename boost::mpl::if_<
		typename boost::is_same<typename div_shape<Value1,scal>::type,ndef>::type,
		ndef,
		cvec<typename div_shape<Value1,scal>::type>
	>::type type;
};

template < typename Value1 >
struct div_op< cvec<Value1>, scal >
{
	typedef typename boost::mpl::if_<
		typename boost::is_same<typename div_shape<cvec<Value1>,scal>::type,ndef>::type,
		ndef,
		cvec_scal_div
	>::type type;
};

template < typename Value1 >
struct div_shape < mat<Value1>, scal >
{
	typedef typename boost::mpl::if_<
		typename boost::is_same<typename div_shape<Value1,scal>::type,ndef>::type,
		ndef,
		mat<typename div_shape<Value1,scal>::type>
	>::type type;
};

template < typename Value1 >
struct div_op < mat<Value1>, scal >
{
	typedef typename boost::mpl::if_<
		typename boost::is_same<typename div_shape<mat<Value1>,scal>::type,ndef>::type,
		ndef,
		mat_scal_div
	>::type type;
};

// added by Hui Li (above) -----------------------------------------

// ==================== ITL types ==================================

template <typename PC, typename Vector, bool Adjoint>
struct ashape<itl::pc::solver<PC, Vector, Adjoint> >
{
    typedef future<cvec<scal> > type; // might be a problem with nested matrices and vectors
};

}} // namespace mtl::ashape

#endif // MTL_ASHAPE_INCLUDE


#include <iostream>
#include <stdexcept>

#include <boost/lexical_cast.hpp>

#include "Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/timeConversions.h"
#include "Tudat/External/SpiceInterface/spiceInterface.h"
#include "Tudat/External/SpiceInterface/spiceRotationalEphemeris.h"

namespace tudat
{

namespace ephemerides
{

//! Function to calculate the rotation quaternion from target frame to original frame.
Eigen::Quaterniond SpiceRotationalEphemeris::getRotationToBaseFrame(
        const double secondsSinceEpoch, const double julianDayAtEpoch )
{
    // Set number of seconds since J2000.
    double ephemerisTime = secondsSinceEpoch;
    if ( julianDayAtEpoch != basic_astrodynamics::JULIAN_DAY_ON_J2000 )
    {
        ephemerisTime -= ( basic_astrodynamics::JULIAN_DAY_ON_J2000 - julianDayAtEpoch )
                * physical_constants::JULIAN_DAY;
    }

    // Get rotational quaternion from spice wrapper function
    return spice_interface::computeRotationQuaternionBetweenFrames(
                targetFrameOrientation_, baseFrameOrientation_, ephemerisTime );
}

//! Function to calculate the derivative of the rotation matrix from target frame to original
//! frame.
Eigen::Matrix3d SpiceRotationalEphemeris::getDerivativeOfRotationToBaseFrame(
        const double secondsSinceEpoch, const double julianDayAtEpoch )
{
    // Set number of seconds since J2000.
    double ephemerisTime = secondsSinceEpoch;
    if ( julianDayAtEpoch != basic_astrodynamics::JULIAN_DAY_ON_J2000 )
    {
        ephemerisTime -= ( basic_astrodynamics::JULIAN_DAY_ON_J2000 - julianDayAtEpoch )
                * physical_constants::JULIAN_DAY;
    }

    // Get rotation matrix derivative from spice wrapper function
    return spice_interface::computeRotationMatrixDerivativeBetweenFrames(
                targetFrameOrientation_, baseFrameOrientation_, ephemerisTime );
}

//! Function to calculate the full rotational state at given time
void SpiceRotationalEphemeris::getFullRotationalQuantitiesToTargetFrame(
        Eigen::Quaterniond& currentRotationToLocalFrame,
        Eigen::Matrix3d& currentRotationToLocalFrameDerivative,
        Eigen::Vector3d& currentAngularVelocityVectorInGlobalFrame,
        const double secondsSinceEpoch, const double julianDayAtEpoch)
{
    // Set number of seconds since J2000.
    double ephemerisTime = secondsSinceEpoch;
    if ( julianDayAtEpoch != basic_astrodynamics::JULIAN_DAY_ON_J2000 )
    {
        ephemerisTime -= ( basic_astrodynamics::JULIAN_DAY_ON_J2000 - julianDayAtEpoch )
                * physical_constants::JULIAN_DAY;
    }

    // Calculate rotation (and its time derivative) directly from spice.
    std::pair< Eigen::Quaterniond, Eigen::Matrix3d > fullRotation =
            spice_interface::computeRotationQuaternionAndRotationMatrixDerivativeBetweenFrames(
                baseFrameOrientation_, targetFrameOrientation_, ephemerisTime );
    currentRotationToLocalFrame = fullRotation.first;
    currentRotationToLocalFrameDerivative = fullRotation.second;

    // Calculate angular velocity vector.
    currentAngularVelocityVectorInGlobalFrame = getRotationalVelocityVectorInBaseFrameFromMatrices(
                Eigen::Matrix3d( currentRotationToLocalFrame ), currentRotationToLocalFrameDerivative.transpose( ) );
}

} // namespace ephemerides

} // namespace tudat


#define BOOST_TEST_MODULE lue framework core linear_curve
#include <hpx/config.hpp>
#include <boost/test/unit_test.hpp>
#include "lue/framework/core/linear_curve.hpp"
#include "lue/framework/test/stream.hpp"
#include <tuple>
#include <vector>

namespace {

template<
    typename Shape,
    typename Coordinates>
void fill_and_test(
    Shape const& shape,
    Coordinates const& result_we_want)
{
    constexpr lue::Rank rank{lue::rank<Shape>};

    Coordinates result_we_got(lue::nr_elements(shape));

    lue::linear_curve<rank>(shape,

            [&result_we_got](
                lue::Index const idx,
                auto... idxs)
            {
                result_we_got[idx] = std::make_tuple(idxs...);
            }

        );

    BOOST_CHECK_EQUAL_COLLECTIONS(
        result_we_got.begin(), result_we_got.end(),
        result_we_want.begin(), result_we_want.end());
}

}  // Anonymous namespace

BOOST_AUTO_TEST_CASE(d1)
{
    constexpr lue::Rank rank = 1;
    using Shape = lue::Shape<lue::Count, rank>;
    using Coordinate = std::tuple<lue::Index>;
    using Coordinates = std::vector<Coordinate>;

    {
        Shape const shape{4};
        Coordinates const result_we_want{
                { 0 },
                { 1 },
                { 2 },
                { 3 },
            };

        fill_and_test(shape, result_we_want);
    }

    {
        Shape const shape{1};
        Coordinates const result_we_want{
                { 0 },
            };

        fill_and_test(shape, result_we_want);
    }

    {
        Shape const shape{0};
        Coordinates const result_we_want{};
        fill_and_test(shape, result_we_want);
    }
}

BOOST_AUTO_TEST_CASE(d2)
{
    constexpr lue::Rank rank = 2;
    using Shape = lue::Shape<lue::Count, rank>;
    using Coordinate = std::tuple<lue::Index, lue::Index>;
    using Coordinates = std::vector<Coordinate>;

    {
        Shape const shape{4, 3};
        Coordinates const result_we_want{
                { 0, 0 },
                { 0, 1 },
                { 0, 2 },
                { 1, 0 },
                { 1, 1 },
                { 1, 2 },
                { 2, 0 },
                { 2, 1 },
                { 2, 2 },
                { 3, 0 },
                { 3, 1 },
                { 3, 2 },
            };

        fill_and_test(shape, result_we_want);
    }

    {
        Shape const shape{1, 1};
        Coordinates const result_we_want{
                { 0, 0 },
            };

        fill_and_test(shape, result_we_want);
    }

    {
        Shape const shape{0, 0};
        Coordinates const result_we_want{};
        fill_and_test(shape, result_we_want);
    }

    {
        Shape const shape{0, 1};
        Coordinates const result_we_want{};
        fill_and_test(shape, result_we_want);
    }

    {
        Shape const shape{1, 0};
        Coordinates const result_we_want{};
        fill_and_test(shape, result_we_want);
    }
}

BOOST_AUTO_TEST_CASE(d3)
{
    constexpr lue::Rank rank = 3;
    using Shape = lue::Shape<lue::Count, rank>;
    using Coordinate = std::tuple<lue::Index, lue::Index, lue::Index>;
    using Coordinates = std::vector<Coordinate>;

    {
        Shape const shape{2, 4, 3};
        Coordinates const result_we_want{
                { 0, 0, 0 },
                { 0, 0, 1 },
                { 0, 0, 2 },
                { 0, 1, 0 },
                { 0, 1, 1 },
                { 0, 1, 2 },
                { 0, 2, 0 },
                { 0, 2, 1 },
                { 0, 2, 2 },
                { 0, 3, 0 },
                { 0, 3, 1 },
                { 0, 3, 2 },
                { 1, 0, 0 },
                { 1, 0, 1 },
                { 1, 0, 2 },
                { 1, 1, 0 },
                { 1, 1, 1 },
                { 1, 1, 2 },
                { 1, 2, 0 },
                { 1, 2, 1 },
                { 1, 2, 2 },
                { 1, 3, 0 },
                { 1, 3, 1 },
                { 1, 3, 2 },
            };

        fill_and_test(shape, result_we_want);
    }

    {
        Shape const shape{1, 1, 1};
        Coordinates const result_we_want{
                { 0, 0, 0 },
            };

        fill_and_test(shape, result_we_want);
    }

    {
        Shape const shape{0, 0, 0};
        Coordinates const result_we_want{};
        fill_and_test(shape, result_we_want);
    }

    {
        Shape const shape{0, 0, 1};
        Coordinates const result_we_want{};
        fill_and_test(shape, result_we_want);
    }

    {
        Shape const shape{0, 1, 0};
        Coordinates const result_we_want{};
        fill_and_test(shape, result_we_want);
    }

    {
        Shape const shape{1, 0, 0};
        Coordinates const result_we_want{};
        fill_and_test(shape, result_we_want);
    }
}


////////////////////////////////////////////////////////////////////////////////
// MSHFieldParser.hh
////////////////////////////////////////////////////////////////////////////////
/*! @file
//      Read Scalar/vector/matrix fields in the MSH format.
//      Fields are identified by name, so all fields (or, at the very least,
//      all fields of the same type) should have distinct names.
*/
//  Author:  Julian Panetta (jpanetta), julian.panetta@gmail.com
//  Company:  New York University
//  Created:  07/23/2014 13:25:57
////////////////////////////////////////////////////////////////////////////////
#ifndef MSHFIELDPARSER_HH
#define MSHFIELDPARSER_HH

#include <iosfwd>
#include <string>
#include <map>
#include <vector>
#include <stdexcept>
#include <Eigen/Dense>

#include <MeshFEM/MeshIO.hh>
#include <MeshFEM/Types.hh>
#include <MeshFEM/Functions.hh>
#include <MeshFEM/Fields.hh>
#include <MeshFEM/SymmetricMatrix.hh>

#include <MeshFEM_export.h>

// N: spatial dimension
template<size_t N>
class MSHFieldParser {
public:
    typedef VectorField<Real, N>          VField;
    typedef ScalarField<Real>             SField;
    typedef SymmetricMatrixField<Real, N> SMField;

    using FlattenedSymmetricMatrix = Eigen::Matrix<Real, flatLen(N), 1>;
    using SMatrix = SymmetricMatrix<N, FlattenedSymmetricMatrix>;
    // Note: all interpolant fields are degree 2; degree 1 interpolants are upscaled.
    using  ISField = std::vector<Interpolant<       Real, N, 2>>;
    using  IVField = std::vector<Interpolant<VectorND<N>, N, 2>>;
    using ISMField = std::vector<Interpolant<    SMatrix, N, 2>>;

    ////////////////////////////////////////////////////////////////////////////
    // Constructor parses the mesh and fields in the MSH file.
    ////////////////////////////////////////////////////////////////////////////
    MESHFEM_EXPORT MSHFieldParser(const std::string &mshPath, bool permitDimMismatch = false);

    // Constructor used to avoid re-parsing the mesh part of the file.
    // (Often code parses the input mesh first to determine the dimension, then
    //  constructs that dimension's instantiation of MSHFieldParser)
    MESHFEM_EXPORT MSHFieldParser(std::istream &is, const MeshIO::MeshType type,
                                  std::vector<MeshIO::IOElement> &&elements,
                                  std::vector<MeshIO::IOVertex>  &&vertices,
                                  const bool binary, bool permitDimMismatch = false);

    const std::vector<MeshIO::IOElement> &elements() const { return m_elements; }
    const std::vector<MeshIO::IOVertex > &vertices() const { return m_vertices; }
    size_t meshDegree()                              const { return MeshIO::meshDegree(m_type); }
    size_t meshDimension()                           const { return MeshIO::meshDimension(m_type); }
    size_t nodesPerElement() const {
        // Note: throws exception for mixed element type meshes (as it should).
        return MeshIO::nodesPerElement(m_type);
    }

    // Reposition the nodes in this mesh.
    template<class PtType>
    void setNodePositions(const std::vector<PtType> &pos) {
        const size_t nnodes = numVertices();
        if (pos.size() != nnodes)
            throw std::runtime_error("Attempted to reposition nodes with incorrectly sized array.");
        for (size_t i = 0; i < nnodes; ++i)
            m_vertices[i].point = pos[i];
    }

    MeshIO::MeshType meshType() const { return m_type; }

    size_t numElements() const { return m_elements.size(); }
    size_t numVertices() const { return m_vertices.size(); }

    void replaceMesh(const std::vector<MeshIO::IOElement> &elements,
                     const std::vector<MeshIO::IOVertex > &vertices) {
        m_elements = elements;
        m_vertices = vertices;
                            m_vectorFields.clear();
                            m_scalarFields.clear();
                   m_symmetricMatrixFields.clear();
                 m_vectorInterpolantFields.clear();
                 m_scalarInterpolantFields.clear();
        m_symmetricMatrixInterpolantFields.clear();
    }

    ////////////////////////////////////////////////////////////////////////////
    // Field accessors.
    // Take name and optional domain type of field (per-element, per-node).
    // Return matching field or throw exception if it doesn't exist.
    // Also report back the domain type if it isn't specified.
    ////////////////////////////////////////////////////////////////////////////
    const VField &vectorField(const std::string &name,
                              DomainType reqType = DomainType::ANY) const {
        return m_getField(m_vectorFields, name, reqType);
    }
    const VField &vectorField(const std::string &name,
                              DomainType reqType, DomainType &actualType) const {
        actualType = reqType;
        return m_getField(m_vectorFields, name, actualType);
    }

    const SField &scalarField(const std::string &name,
                              DomainType reqType = DomainType::ANY) const {
        return m_getField(m_scalarFields, name, reqType);
    }
    const SField &scalarField(const std::string &name,
                              DomainType reqType, DomainType &actualType) const {
        actualType = reqType;
        return m_getField(m_scalarFields, name, actualType);
    }

    const SMField &symmetricMatrixField(const std::string &name,
                              DomainType reqType = DomainType::ANY) const {
        return m_getField(m_symmetricMatrixFields, name, reqType);
    }
    const SMField &symmetricMatrixField(const std::string &name,
                              DomainType reqType, DomainType &actualType) const {
        actualType = reqType;
        return m_getField(m_symmetricMatrixFields, name, actualType);
    }

    const IVField &vectorInterpolantField(const std::string &name,
                              DomainType reqType = DomainType::ANY) const {
        return m_getField(m_vectorInterpolantFields, name, reqType);
    }
    const IVField &vectorInterpolantField(const std::string &name,
                              DomainType reqType, DomainType &actualType) const {
        actualType = reqType;
        return m_getField(m_vectorInterpolantFields, name, actualType);
    }

    const ISField &scalarInterpolantField(const std::string &name,
                              DomainType reqType = DomainType::ANY) const {
        return m_getField(m_scalarInterpolantFields, name, reqType);
    }
    const ISField &scalarInterpolantField(const std::string &name,
                              DomainType reqType, DomainType &actualType) const {
        actualType = reqType;
        return m_getField(m_scalarInterpolantFields, name, actualType);
    }

    const ISMField &symmetricMatrixInterpolantField(const std::string &name,
                              DomainType reqType = DomainType::ANY) const {
        return m_getField(m_symmetricMatrixInterpolantFields, name, reqType);
    }
    const ISMField &symmetricMatrixInterpolantField(const std::string &name,
                              DomainType reqType, DomainType &actualType) const {
        actualType = reqType;
        return m_getField(m_symmetricMatrixInterpolantFields, name, actualType);
    }

    ////////////////////////////////////////////////////////////////////////////
    // Get names of all the fields of a particular type.
    ////////////////////////////////////////////////////////////////////////////
    std::vector<std::string>          vectorFieldNames(DomainType type = DomainType::ANY) const { return m_getKeysOfType(         m_vectorFields, type); }
    std::vector<std::string>          scalarFieldNames(DomainType type = DomainType::ANY) const { return m_getKeysOfType(         m_scalarFields, type); }
    std::vector<std::string> symmetricMatrixFieldNames(DomainType type = DomainType::ANY) const { return m_getKeysOfType(m_symmetricMatrixFields, type); }

    std::vector<std::string>          vectorInterpolantFieldNames(DomainType type = DomainType::ANY) const { return m_getKeysOfType(         m_vectorInterpolantFields, type); }
    std::vector<std::string>          scalarInterpolantFieldNames(DomainType type = DomainType::ANY) const { return m_getKeysOfType(         m_scalarInterpolantFields, type); }
    std::vector<std::string> symmetricMatrixInterpolantFieldNames(DomainType type = DomainType::ANY) const { return m_getKeysOfType(m_symmetricMatrixInterpolantFields, type); }

private:
    std::vector<MeshIO::IOElement> m_elements;
    std::vector<MeshIO::IOVertex > m_vertices;
    MeshIO::MeshType m_type;

    std::map<std::string, std::pair<DomainType,  VField>>          m_vectorFields;
    std::map<std::string, std::pair<DomainType,  SField>>          m_scalarFields;
    std::map<std::string, std::pair<DomainType, SMField>> m_symmetricMatrixFields;

    std::map<std::string, std::pair<DomainType,  IVField>>          m_vectorInterpolantFields;
    std::map<std::string, std::pair<DomainType,  ISField>>          m_scalarInterpolantFields;
    std::map<std::string, std::pair<DomainType, ISMField>> m_symmetricMatrixInterpolantFields;

    // Find a particular "type" of field of a particular name.
    // This "type" comprises both the domain type (i.e. DomainType) and range
    // type (vector, scalar, symmetric matrix...)
    // Throws exception if no matching field is found.
    template<class _Field>
    static const _Field &m_getField(const std::map<std::string, std::pair<DomainType, _Field> > &fields,
                                    const std::string &name, DomainType &type) {
        std::runtime_error notFound("Field query unmatched.");
        auto it = fields.find(name);
        if (it != fields.end()) {
            if ((type == DomainType::ANY) || it->second.first == type) {
                type = it->second.first; // Report actual type (for ANY case)
                return it->second.second;
            }
            throw notFound;
        }
        else { throw notFound; }
    }

    // Get the names of all fields of a particular "type."
    // This "type" comprises both the domain type (i.e. DomainType) and range
    // type (vector, scalar, symmetric matrix...)
    template<class CollectionType>
    static std::vector<std::string> m_getKeysOfType(const CollectionType &fields, DomainType type) {
        std::vector<std::string> result;
        for (auto entry : fields) {
            if ((type == DomainType::ANY) || entry.second.first == type)
                result.push_back(entry.first);
        }
        return result;
    }

    void m_parseFields(std::istream &s, const bool binary);
    bool m_parseField(std::istream &is, const std::string &header, std::string &name,
                      Eigen::Matrix<Real, Eigen::Dynamic, Eigen::Dynamic> &fieldData,
                      DomainType &type, bool binary = false);
};

#endif /* end of include guard: MSHFIELDPARSER_HH */


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

#include <boost/hana/applicative/mcd.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/detail/minimal/applicative.hpp>
#include <boost/hana/detail/minimal/comparable.hpp>

#include <tuple>
using namespace boost::hana;

template <int i>
constexpr auto x = detail::minimal::comparable<>(i);

template <typename mcd>
void test() {
    constexpr auto a = detail::minimal::applicative<mcd>;
    BOOST_HANA_CONSTEXPR_LAMBDA auto f = [](auto ...xs) {
        return std::make_tuple(xs...);
    };

    BOOST_HANA_CONSTEXPR_ASSERT(ap(a(f), a(x<0>)) == a(f(x<0>)));
    BOOST_HANA_CONSTEXPR_ASSERT(ap(a(f), a(x<0>), a(x<1>)) == a(f(x<0>, x<1>)));
    BOOST_HANA_CONSTEXPR_ASSERT(ap(a(f), a(x<0>), a(x<1>), a(x<2>)) == a(f(x<0>, x<1>, x<2>)));
    BOOST_HANA_CONSTEXPR_ASSERT(ap(a(f), a(x<0>), a(x<1>), a(x<2>), a(x<3>)) == a(f(x<0>, x<1>, x<2>, x<3>)));
}

int main() {
    test<Applicative::mcd>();
}


#ifndef NETWORK_HPP_
#define NETWORK_HPP_

#include <boost/endian/conversion.hpp>

namespace network {

template <typename T>
T ntoh(T v) {
    return boost::endian::big_to_native(v);
}

template <>
inline char ntoh(char v) {
    return v;
}

template <typename T>
T hton(T v) {
    return boost::endian::native_to_big(v);
}

template <>
inline char hton(char v) {
    return v;
}

// A code from Beej's Guide to Network Programming by Brian "Beej Jorgensen" Hall.
// Extracted from http://beej.us/guide/bgnet/examples/ieee754.c and adapted to C++.
// #define pack754_32(f) (pack754((f), 32, 8))
// #define pack754_64(f) (pack754((f), 64, 11))
// #define unpack754_32(i) (unpack754((i), 32, 8))
// #define unpack754_64(i) (unpack754((i), 64, 11))

inline uint64_t pack754(long double f, unsigned bits, unsigned expbits)
{
	long double fnorm;
	int shift;
	long long sign, exp, significand;
	unsigned significandbits = bits - expbits - 1; // -1 for sign bit

	if (f == 0.0) return 0; // get this special case out of the way

	// check sign and begin normalization
	if (f < 0) { sign = 1; fnorm = -f; }
	else { sign = 0; fnorm = f; }

	// get the normalized form of f and track the exponent
	shift = 0;
	while(fnorm >= 2.0) { fnorm /= 2.0; shift++; }
	while(fnorm < 1.0) { fnorm *= 2.0; shift--; }
	fnorm = fnorm - 1.0;

	// calculate the binary form (non-float) of the significand data
	significand = fnorm * ((1LL<<significandbits) + 0.5f);

	// get the biased exponent
	exp = shift + ((1<<(expbits-1)) - 1); // shift + bias

	// return the final answer
	return (sign<<(bits-1)) | (exp<<(bits-expbits-1)) | significand;
}

inline long double unpack754(uint64_t i, unsigned bits, unsigned expbits)
{
	long double result;
	long long shift;
	unsigned bias;
	unsigned significandbits = bits - expbits - 1; // -1 for sign bit

	if (i == 0) return 0.0;

	// pull the significand
	result = (i&((1LL<<significandbits)-1)); // mask
	result /= (1LL<<significandbits); // convert back to float
	result += 1.0f; // add the one back on

	// deal with the exponent
	bias = (1<<(expbits-1)) - 1;
	shift = ((i>>significandbits)&((1LL<<expbits)-1)) - bias;
	while(shift > 0) { result *= 2.0; shift--; }
	while(shift < 0) { result /= 2.0; shift++; }

	// sign it
	result *= (i>>(bits-1))&1? -1.0: 1.0;

	return result;
}

inline uint32_t pack754(float f)
{
    return pack754(f, 32, 8);
}

inline uint64_t pack754(double d)
{
    return pack754(d, 64, 11);
}

inline float unpack754(uint32_t i)
{
    return unpack754(i, 32, 8);
}

inline double unpack754(uint64_t l)
{
    return unpack754(l, 64, 11);
}

} /* network */

#endif /* ifndef NETWORK_HPP_ */


/**
 * @file GConstrainedFPTTest.cpp
 */

/********************************************************************************
 *
 * This file is part of the Geneva library collection. The following license
 * applies to this file:
 *
 * ------------------------------------------------------------------------------
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *    http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 * ------------------------------------------------------------------------------
 *
 * Note that other files in the Geneva library collection may use a different
 * license. Please see the licensing information in each file.
 *
 ********************************************************************************
 *
 * Geneva was started by Dr. Rüdiger Berlich and was later maintained together
 * with Dr. Ariel Garcia under the auspices of Gemfony scientific. For further
 * information on Gemfony scientific, see http://www.gemfomy.eu .
 *
 * The majority of files in Geneva was released under the Apache license v2.0
 * in February 2020.
 *
 * See the NOTICE file in the top-level directory of the Geneva library
 * collection for a list of contributors and copyright information.
 *
 ********************************************************************************/

/**
 * This test takes a GConstrainedDoubleObject object:
 * a) It examines the mapping from internal to external representation of its value.
 * b) It tests the "distortion" of a gaussian when going through the mapping from
 *    internal to external value.
 *
 * Additional tests (including error handling) of the GConstrainedDoubleObject class have been
 * implemented as part of the unit tests.
 *
 * In order to see the results of this test, you need the Root toolkit from http://root.cern.ch.
 * Once installed call "root -l mapping.C" .
 */

// Standard header files go here
#include <vector>
#include <iostream>
#include <fstream>
#include <string>

// Boost header files go here
#include <boost/filesystem.hpp>
#include <boost/filesystem/fstream.hpp>

// Geneva header files go here
#include "hap/GRandomT.hpp"
#include "common/GPlotDesigner.hpp"
#include "geneva/GConstrainedDoubleObject.hpp"
#include "geneva/GDoubleGaussAdaptor.hpp"

using namespace Gem::Geneva;
using namespace Gem::Common;
using namespace boost;

const std::uint32_t NTESTS=10000;

int main(int argc, char **argv){
	//***************************************************************************
	// Test a: Mapping from internal to external value

	double internalValue = 0., externalValue = 0.;
	std::shared_ptr<GGraph2D> mapping_ptr(new GGraph2D());
	mapping_ptr->setPlotLabel("Mapping from internal to external value");

	GConstrainedDoubleObject gbd13(-1.,3.); // lower boundary -1, upper Boundary 3

	for(std::uint32_t i=0; i<NTESTS; i++){
		internalValue=-30.+50.*double(i)/double(NTESTS);

		externalValue = gbd13.transfer(internalValue);
		*mapping_ptr & std::tuple<double,double>(internalValue, externalValue);
	}

	GPlotDesigner gpd("Manual tests of GConstrainedDoubleObject", 1,1);

	gpd.setCanvasDimensions(1200,1200);
	gpd.registerPlotter(mapping_ptr);

	gpd.writeToFile("mapping.C");

	//***************************************************************************
	// Test b: Distortian of gaussians when being translated from internal to
	// external value. The test produces 10 gaussian random number distributions,
	// whose mean is shifted from left to right of a [-1,1] range

	Gem::Hap::GRandomT<Gem::Hap::RANDFLAVOURS::RANDOMPROXY> gr;
	GConstrainedDoubleObject gbd_distortion(-1.,1.);

	boost::filesystem::ofstream distortion("distortion.C");

	distortion << "{" << std::endl
	<< "  gROOT->SetStyle(\"Plain\");" << std::endl
	<< "  gStyle -> SetOptStat(kFALSE);" << std::endl
	<< std::endl
	<< "  TCanvas *cc = new TCanvas(\"cc\",\"cc\",0,0,800,1200);" << std::endl
	<< "  cc->Divide(2,7);" << std::endl
	<< std::endl
	<< "  TH1F *external0 = new TH1F(\"external0\",\"external0\",301,-1.5,1.5);" << std::endl
	<< "  TH1F *external1 = new TH1F(\"external1\",\"external1\",301,-1.5,1.5);" << std::endl
	<< "  TH1F *external2 = new TH1F(\"external2\",\"external2\",301,-1.5,1.5);" << std::endl
	<< "  TH1F *external3 = new TH1F(\"external3\",\"external3\",301,-1.5,1.5);" << std::endl
	<< "  TH1F *external4 = new TH1F(\"external4\",\"external4\",301,-1.5,1.5);" << std::endl
	<< "  TH1F *external5 = new TH1F(\"external5\",\"external5\",301,-1.5,1.5);" << std::endl
	<< "  TH1F *external6 = new TH1F(\"external6\",\"external6\",301,-1.5,1.5);" << std::endl
	<< "  TH1F *external7 = new TH1F(\"external7\",\"external7\",301,-1.5,1.5);" << std::endl
	<< "  TH1F *external8 = new TH1F(\"external8\",\"external8\",301,-1.5,1.5);" << std::endl
	<< "  TH1F *external9 = new TH1F(\"external9\",\"external9\",301,-1.5,1.5);" << std::endl
	<< "  TH1F *external10 = new TH1F(\"external10\",\"external10\",301,-1.5,1.5);" << std::endl
	<< "  TH1F *external11 = new TH1F(\"external11\",\"external11\",301,-1.5,1.5);" << std::endl
	<< "  TH1F *external12 = new TH1F(\"external12\",\"external12\",301,-1.5,1.5);" << std::endl
	<< "  TH1F *external13 = new TH1F(\"external13\",\"external13\",301,-1.5,1.5);" << std::endl
	<< "  TH1F *internal0 = new TH1F(\"internal0\",\"internal0\",301,-1.5,1.5);" << std::endl
	<< "  TH1F *internal1 = new TH1F(\"internal1\",\"internal1\",301,-1.5,1.5);" << std::endl
	<< "  TH1F *internal2 = new TH1F(\"internal2\",\"internal2\",301,-1.5,1.5);" << std::endl
	<< "  TH1F *internal3 = new TH1F(\"internal3\",\"internal3\",301,-1.5,1.5);" << std::endl
	<< "  TH1F *internal4 = new TH1F(\"internal4\",\"internal4\",301,-1.5,1.5);" << std::endl
	<< "  TH1F *internal5 = new TH1F(\"internal5\",\"internal5\",301,-1.5,1.5);" << std::endl
	<< "  TH1F *internal6 = new TH1F(\"internal6\",\"internal6\",301,-1.5,1.5);" << std::endl
	<< "  TH1F *internal7 = new TH1F(\"internal7\",\"internal7\",301,-1.5,1.5);" << std::endl
	<< "  TH1F *internal8 = new TH1F(\"internal8\",\"internal8\",301,-1.5,1.5);" << std::endl
	<< "  TH1F *internal9 = new TH1F(\"internal9\",\"internal9\",301,-1.5,1.5);" << std::endl
	<< "  TH1F *internal10 = new TH1F(\"internal10\",\"internal10\",301,-1.5,1.5);" << std::endl
	<< "  TH1F *internal11 = new TH1F(\"internal11\",\"internal11\",301,-1.5,1.5);" << std::endl
	<< "  TH1F *internal12 = new TH1F(\"internal12\",\"internal12\",301,-1.5,1.5);" << std::endl
	<< "  TH1F *internal13 = new TH1F(\"internal13\",\"internal13\",301,-1.5,1.5);" << std::endl
	<< std::endl;

    std::normal_distribution<double> normal_distribution(0.,0.1);
	for(std::size_t i=0; i<NTESTS; i++) {
		internalValue = normal_distribution(gr);

		distortion << "  external0->Fill(" << gbd_distortion.transfer(internalValue - 1.1) << ");" << std::endl
		<< "  external1->Fill(" << gbd_distortion.transfer(internalValue - 1.0) << ");" << std::endl
		<< "  external2->Fill(" << gbd_distortion.transfer(internalValue - 0.9) << ");" << std::endl
		<< "  external3->Fill(" << gbd_distortion.transfer(internalValue - 0.7) << ");" << std::endl
		<< "  external4->Fill(" << gbd_distortion.transfer(internalValue - 0.5) << ");" << std::endl
		<< "  external5->Fill(" << gbd_distortion.transfer(internalValue - 0.3) << ");" << std::endl
		<< "  external6->Fill(" << gbd_distortion.transfer(internalValue - 0.1) << ");" << std::endl
		<< "  external7->Fill(" << gbd_distortion.transfer(internalValue + 0.1) << ");" << std::endl
		<< "  external8->Fill(" << gbd_distortion.transfer(internalValue + 0.3) << ");" << std::endl
		<< "  external9->Fill(" << gbd_distortion.transfer(internalValue + 0.5) << ");" << std::endl
		<< "  external10->Fill(" << gbd_distortion.transfer(internalValue + 0.7) << ");" << std::endl
		<< "  external11->Fill(" << gbd_distortion.transfer(internalValue + 0.9) << ");" << std::endl
		<< "  external12->Fill(" << gbd_distortion.transfer(internalValue + 1.0) << ");" << std::endl
		<< "  external13->Fill(" << gbd_distortion.transfer(internalValue + 1.1) << ");" << std::endl
		<< "  internal0->Fill(" << internalValue - 1.1 << ");" << std::endl
		<< "  internal1->Fill(" << internalValue - 1.0 << ");" << std::endl
		<< "  internal2->Fill(" << internalValue - 0.9 << ");" << std::endl
		<< "  internal3->Fill(" << internalValue - 0.7 << ");" << std::endl
		<< "  internal4->Fill(" << internalValue - 0.5 << ");" << std::endl
		<< "  internal5->Fill(" << internalValue - 0.3 << ");" << std::endl
		<< "  internal6->Fill(" << internalValue - 0.1 << ");" << std::endl
		<< "  internal7->Fill(" << internalValue + 0.1 << ");" << std::endl
		<< "  internal8->Fill(" << internalValue + 0.3 << ");" << std::endl
		<< "  internal9->Fill(" << internalValue + 0.5 << ");" << std::endl
		<< "  internal10->Fill(" << internalValue + 0.7 << ");" << std::endl
		<< "  internal11->Fill(" << internalValue + 0.9 << ");" << std::endl
		<< "  internal12->Fill(" << internalValue + 1.0 << ");" << std::endl
		<< "  internal13->Fill(" << internalValue + 1.1 << ");" << std::endl;
	}

	distortion << std::endl
	<< "  cc->cd(1);" << std::endl
	<< "  external0->SetFillColor(4);" << std::endl
	<< "  external0->SetFillStyle(1001);" << std::endl
	<< "  external0->Draw();" << std::endl
	<< "  internal0->SetFillColor(2);" << std::endl
	<< "  internal0->SetFillStyle(3004);" << std::endl
	<< "  internal0->Draw(\"same\");" << std::endl
	<< std::endl
	<< "  cc->cd(2);" << std::endl
	<< "  external1->SetFillColor(4);" << std::endl
	<< "  external1->SetFillStyle(1001);" << std::endl
	<< "  external1->Draw();" << std::endl
	<< "  internal1->SetFillColor(2);" << std::endl
	<< "  internal1->SetFillStyle(3004);" << std::endl
	<< "  internal1->Draw(\"same\");" << std::endl
	<< std::endl
	<< "  cc->cd(3);" << std::endl
	<< "  external2->SetFillColor(4);" << std::endl
	<< "  external2->SetFillStyle(1001);" << std::endl
	<< "  external2->Draw();" << std::endl
	<< "  internal2->SetFillColor(2);" << std::endl
	<< "  internal2->SetFillStyle(3004);" << std::endl
	<< "  internal2->Draw(\"same\");" << std::endl
	<< std::endl
	<< "  cc->cd(4);" << std::endl
	<< "  external3->SetFillColor(4);" << std::endl
	<< "  external3->SetFillStyle(1001);" << std::endl
	<< "  external3->Draw();" << std::endl
	<< "  internal3->SetFillColor(2);" << std::endl
	<< "  internal3->SetFillStyle(3004);" << std::endl
	<< "  internal3->Draw(\"same\");" << std::endl
	<< std::endl
	<< "  cc->cd(5);" << std::endl
	<< "  external4->SetFillColor(4);" << std::endl
	<< "  external4->SetFillStyle(1001);" << std::endl
	<< "  external4->Draw();" << std::endl
	<< "  internal4->SetFillColor(2);" << std::endl
	<< "  internal4->SetFillStyle(3004);" << std::endl
	<< "  internal4->Draw(\"same\");" << std::endl
	<< std::endl
	<< "  cc->cd(6);" << std::endl
	<< "  external5->SetFillColor(4);" << std::endl
	<< "  external5->SetFillStyle(1001);" << std::endl
	<< "  external5->Draw();" << std::endl
	<< "  internal5->SetFillColor(2);" << std::endl
	<< "  internal5->SetFillStyle(3004);" << std::endl
	<< "  internal5->Draw(\"same\");" << std::endl
	<< std::endl
	<< "  cc->cd(7);" << std::endl
	<< "  external6->SetFillColor(4);" << std::endl
	<< "  external6->SetFillStyle(1001);" << std::endl
	<< "  external6->Draw();" << std::endl
	<< "  internal6->SetFillColor(2);" << std::endl
	<< "  internal6->SetFillStyle(3004);" << std::endl
	<< "  internal6->Draw(\"same\");" << std::endl
	<< std::endl
	<< "  cc->cd(8);" << std::endl
	<< "  external7->SetFillColor(4);" << std::endl
	<< "  external7->SetFillStyle(1001);" << std::endl
	<< "  external7->Draw();" << std::endl
	<< "  internal7->SetFillColor(2);" << std::endl
	<< "  internal7->SetFillStyle(3004);" << std::endl
	<< "  internal7->Draw(\"same\");" << std::endl
	<< std::endl
	<< "  cc->cd(9);" << std::endl
	<< "  external8->SetFillColor(4);" << std::endl
	<< "  external8->SetFillStyle(1001);" << std::endl
	<< "  external8->Draw();" << std::endl
	<< "  internal8->SetFillColor(2);" << std::endl
	<< "  internal8->SetFillStyle(3004);" << std::endl
	<< "  internal8->Draw(\"same\");" << std::endl
	<< std::endl
	<< "  cc->cd(10);" << std::endl
	<< "  external9->SetFillColor(4);" << std::endl
	<< "  external9->SetFillStyle(1001);" << std::endl
	<< "  external9->Draw();" << std::endl
	<< "  internal9->SetFillColor(2);" << std::endl
	<< "  internal9->SetFillStyle(3004);" << std::endl
	<< "  internal9->Draw(\"same\");" << std::endl
	<< "  cc->cd();" << std::endl
	<< std::endl
	<< "  cc->cd(11);" << std::endl
	<< "  external10->SetFillColor(4);" << std::endl
	<< "  external10->SetFillStyle(1001);" << std::endl
	<< "  external10->Draw();" << std::endl
	<< "  internal10->SetFillColor(2);" << std::endl
	<< "  internal10->SetFillStyle(3004);" << std::endl
	<< "  internal10->Draw(\"same\");" << std::endl
	<< std::endl
	<< "  cc->cd(12);" << std::endl
	<< "  external11->SetFillColor(4);" << std::endl
	<< "  external11->SetFillStyle(1001);" << std::endl
	<< "  external11->Draw();" << std::endl
	<< "  internal11->SetFillColor(2);" << std::endl
	<< "  internal11->SetFillStyle(3004);" << std::endl
	<< "  internal11->Draw(\"same\");" << std::endl
	<< std::endl
	<< "  cc->cd(13);" << std::endl
	<< "  external12->SetFillColor(4);" << std::endl
	<< "  external12->SetFillStyle(1001);" << std::endl
	<< "  external12->Draw();" << std::endl
	<< "  internal12->SetFillColor(2);" << std::endl
	<< "  internal12->SetFillStyle(3004);" << std::endl
	<< "  internal12->Draw(\"same\");" << std::endl
	<< std::endl
	<< "  cc->cd(14);" << std::endl
	<< "  external13->SetFillColor(4);" << std::endl
	<< "  external13->SetFillStyle(1001);" << std::endl
	<< "  external13->Draw();" << std::endl
	<< "  internal13->SetFillColor(2);" << std::endl
	<< "  internal13->SetFillStyle(3004);" << std::endl
	<< "  internal13->Draw(\"same\");" << std::endl
	<< "  cc->cd();" << std::endl
	<< "}" << std::endl;

	distortion.close();

	return 0;
}


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

#include <algorithm>
#include <boost/graph/adjacency_list.hpp>
#include <catch2/catch.hpp>
#include <cmath>
#include <iostream>
#include <vector>

#include "Architecture/Architecture.hpp"
#include "Graphs/ArticulationPoints.hpp"

namespace tket {
namespace graphs {
namespace test_Architectures {

SCENARIO("Testing FullyConnected") {
  unsigned n_nodes = 10;
  FullyConnected arch(n_nodes);
  node_vector_t nodes_vec = arch.get_all_nodes_vec();
  node_set_t nodes(nodes_vec.begin(), nodes_vec.end());

  REQUIRE(arch.n_nodes() == nodes.size());
  for (const UnitID &uid : arch.nodes()) {
    REQUIRE(nodes.count(Node(uid)));
  }
  for (auto [n1, n2] : arch.get_all_edges_vec()) {
    REQUIRE(nodes.count(n1));
    REQUIRE(nodes.count(n2));
  }

  for (unsigned i = 0; i < n_nodes; i++) {
    for (unsigned j = 0; j < n_nodes; j++) {
      if (i != j) {
        Node n1("fcNode", i);
        Node n2("fcNode", j);
        REQUIRE(arch.edge_exists(n1, n2));
      }
    }
  }
}

SCENARIO("Testing RingArch") {
  unsigned n_nodes = 10;
  RingArch arch(n_nodes);
  node_vector_t nodes_vec = arch.get_all_nodes_vec();
  node_set_t nodes(nodes_vec.begin(), nodes_vec.end());

  REQUIRE(arch.n_nodes() == nodes.size());
  for (const UnitID &uid : arch.nodes()) {
    REQUIRE(nodes.count(Node(uid)));
  }
  for (auto [n1, n2] : arch.get_all_edges_vec()) {
    REQUIRE(nodes.count(n1));
    REQUIRE(nodes.count(n2));
  }

  for (unsigned i = 0; i < n_nodes; i++) {
    Node n1("ringNode", i);
    Node n2("ringNode", (i + 1) % n_nodes);
    REQUIRE(arch.edge_exists(n1, n2));
  }
}

SCENARIO("Testing SquareGrid") {
  unsigned ver = 5;
  unsigned hor = 5;
  unsigned layer = 2;
  SquareGrid arch(ver, hor, layer);
  node_vector_t nodes_vec = arch.get_all_nodes_vec();
  node_set_t nodes(nodes_vec.begin(), nodes_vec.end());

  REQUIRE(nodes.size() == arch.n_nodes());
  for (const UnitID &uid : arch.nodes()) {
    REQUIRE(nodes.count(Node(uid)));
  }
  for (auto [n1, n2] : arch.get_all_edges_vec()) {
    REQUIRE(nodes.count(n1));
    REQUIRE(nodes.count(n2));
  }

  for (const Node &n : nodes) {
    int row = n.index()[0], col = n.index()[1], l = n.index()[2];
    for (const Node &neigh : arch.get_neighbour_nodes(n)) {
      int row_neigh = neigh.index()[0], col_neigh = neigh.index()[1],
          l_neigh = neigh.index()[2];
      REQUIRE(
          abs(row - row_neigh) + abs(col - col_neigh) + abs(l - l_neigh) == 1);
    }
  }
}

SCENARIO("Diameters") {
  GIVEN("an empty architecture") {
    Architecture arc;
    CHECK_THROWS(arc.get_diameter());
  }
  GIVEN("a singleton architecture") {
    Architecture arc;
    arc.add_node(Node(0));
    CHECK(arc.get_diameter() == 0);
  }
  GIVEN("a connected architecture") {
    Architecture arc({{0, 1}, {1, 2}, {2, 3}, {3, 0}});
    CHECK(arc.get_diameter() == 2);
  }
  GIVEN("a disconnected architecture") {
    // TKET-1425
    Architecture arc({{0, 1}, {1, 2}, {2, 0}, {3, 4}});
    CHECK_THROWS(arc.get_diameter());
  }
}

SCENARIO("connectivity") {
  GIVEN("simple architecture") {
    const Architecture archi(
        {{Node(0), Node(1)},
         {Node(0), Node(2)},
         {Node(1), Node(2)},
         {Node(2), Node(3)}});
    MatrixXb connectivity(4, 4);
    connectivity << 0, 1, 1, 0,  // 0
        1, 0, 1, 0,              // 1
        1, 1, 0, 1,              // 2
        0, 0, 1, 0;              // 3

    REQUIRE(archi.get_connectivity() == connectivity);
  }
  GIVEN("connected architecture") {
    const Architecture archi(
        {{Node(0), Node(1)},
         {Node(0), Node(2)},
         {Node(0), Node(3)},
         {Node(1), Node(2)},
         {Node(1), Node(3)},
         {Node(2), Node(3)}});
    MatrixXb connectivity(4, 4);
    connectivity << 0, 1, 1, 1,  // 0
        1, 0, 1, 1,              // 1
        1, 1, 0, 1,              // 2
        1, 1, 1, 0;              // 3

    REQUIRE(archi.get_connectivity() == connectivity);
  }
}

}  // namespace test_Architectures
}  // namespace graphs
}  // namespace tket


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

#ifndef FCPPT_MPL_ALL_OF_HPP_INCLUDED
#define FCPPT_MPL_ALL_OF_HPP_INCLUDED

#include <fcppt/preprocessor/disable_gcc_warning.hpp>
#include <fcppt/preprocessor/pop_warning.hpp>
#include <fcppt/preprocessor/push_warning.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/mpl/and.hpp>
#include <boost/mpl/bind.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/fold.hpp>
#include <boost/mpl/lambda.hpp>
#include <boost/mpl/placeholders.hpp>
#include <fcppt/config/external_end.hpp>

namespace fcppt
{
namespace mpl
{

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

/**
\brief Checks if a predicate is true for all elements of an MPL sequence

\ingroup fcpptmpl

For every element in \a Sequence, it is checked whether \a Predicate holds

\tparam Sequence The MPL sequence to check

\tparam Predicate A unary MPL predicate to apply to every element
*/
template<
	typename Sequence,
	typename Predicate
>
struct all_of
:
boost::mpl::fold<
	Sequence,
	boost::mpl::true_,
	boost::mpl::and_<
		boost::mpl::_1,
		boost::mpl::bind<
			typename boost::mpl::lambda<
				Predicate
			>::type,
			boost::mpl::_2
		>
	>
>::type
{
};

FCPPT_PP_POP_WARNING

}
}

#endif