//==================================================================================================
/**
  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)
**/
//==================================================================================================
#include <boost/simd/pack.hpp>
#include <boost/simd/function/toint.hpp>
#include <boost/simd/meta/cardinal_of.hpp>
#include <simd_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>

template <typename T, int N, typename Env>
void test(Env& runtime)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;

  using p_t = bs::pack<T, N>;
  using iT = bd::as_integer_t<T>;
  using i_t = bs::pack<iT, N>;

  T a1[N];
  iT b[N];
  for(int i = 0; i < N; ++i)
  {
    a1[i] = T(i)/N;
    b[i] = bs::toint(a1[i]) ;
  }
  p_t aa1(&a1[0], &a1[0]+N);
  i_t bb (&b[0], &b[0]+N);
  STF_EQUAL(bs::toint(aa1), bb);
}

STF_CASE_TPL("Check toint on pack" , STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  using p_t = bs::pack<T>;
  static const std::size_t N = bs::cardinal_of<p_t>::value;
  test<T, N>(runtime);
  test<T, N/2>(runtime);
  test<T, N*2>(runtime);
}

STF_CASE_TPL (" toint real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::toint;
  using p_t = bs::pack<T>;
  using r_t = decltype(toint(p_t()));

  // return type conformity test
  STF_TYPE_IS(r_t, (bd::as_integer_t<p_t, signed>));

  // specific values tests
  STF_EQUAL(toint(p_t(0.4)), r_t(0));
  STF_EQUAL(toint(p_t(0.5)), r_t(0));
  STF_EQUAL(toint(p_t(0.6)), r_t(0));
  STF_EQUAL(toint(bs::Mone<p_t>()), bs::Mone<r_t>());
  STF_EQUAL(toint(bs::One<p_t>()),  bs::One<r_t>());
  STF_EQUAL(toint(bs::Zero<p_t>()), bs::Zero<r_t>());

} // end of test for floating_

STF_CASE_TPL (" toint unsigned_int",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::toint;
  using p_t = bs::pack<T>;
  using r_t = decltype(toint(p_t()));

  // return type conformity test
  STF_TYPE_IS(r_t, (bd::as_integer_t<p_t, signed>));

  // specific values tests
  STF_EQUAL(toint(bs::One<p_t>()),  bs::One<r_t>());
  STF_EQUAL(toint(bs::Zero<p_t>()), bs::Zero<r_t>());
} // end of test for unsigned_int_

STF_CASE_TPL (" toint signed",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::toint;
  using p_t = bs::pack<T>;
  using r_t = decltype(toint(p_t()));

  // return type conformity test
  STF_TYPE_IS(r_t, (bd::as_integer_t<p_t, signed>));

  // specific values tests
  STF_EQUAL(toint(bs::Mone<p_t>()), bs::Mone<r_t>());
  STF_EQUAL(toint(bs::One<p_t>()),  bs::One<r_t>());
  STF_EQUAL(toint(bs::Zero<p_t>()), bs::Zero<r_t>());
} // end of test for signed_int_


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

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

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

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

 /*!

    @ingroup group-swar
    Function object implementing interleave_even capabilities

    Computes a vector from a combination of the two inputs.

    @par Semantic:

    For every parameters of types respectively T0:

    @code
    T0 r = interleave_even(a,b);
    @endcode

    is equivalent to :

    @code
    r = [ a[0] b[0] a[2] b[2] ... a[n/2] b[n/2] ]
    @endcode

    with <tt> n = cardinal_of<T>::value </tt>

  **/
  const boost::dispatch::functor<tag::interleave_even_> interleave_even = {};
} }
#endif

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

#endif


#define BOOST_TEST_MODULE "test_PWMcos_interaction"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <test/util/utility.hpp>

#include <mjolnir/forcefield/PWMcos/PWMcosInteraction.hpp>
#include <mjolnir/core/VerletList.hpp>
#include <mjolnir/core/SpatialPartitionBase.hpp>
#include <mjolnir/core/NaivePairCalculation.hpp>

BOOST_AUTO_TEST_CASE(PWMcos_Interaction)
{
    namespace test = mjolnir::test;
    mjolnir::LoggerManager::set_default_logger("test_pdns_interaction.log");

    using real_type         = double;
    using traits_type       = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type         = traits_type::real_type;
    using coord_type        = traits_type::coordinate_type;
    using boundary_type     = traits_type::boundary_type;
    using system_type       = mjolnir::System<traits_type>;
    using topology_type     = mjolnir::Topology;

    using interaction_type       = mjolnir::PWMcosInteraction<traits_type>;
    using potential_type         = typename interaction_type::potential_type;
    using parameter_list_type    = typename interaction_type::parameter_list_type;
    using base_kind              = typename parameter_list_type::base_kind;
    using contact_parameter_type = typename parameter_list_type::contact_parameter_type;
    using dna_parameter_type     = typename parameter_list_type::dna_parameter_type;
    using ignore_molecule_type   = typename parameter_list_type::ignore_molecule_type;
    using ignore_group_type      = typename parameter_list_type::ignore_group_type;
    using partition_type         = mjolnir::NaivePairCalculation<traits_type, potential_type>;

    // protein     DNA
    //
    //  0 o       3 o --o  |
    //     \     4         |
    //    1 o === o -- o 6 |
    //     /               |
    //  2 o       5 o--o   |

    constexpr auto pi  = mjolnir::math::constants<real_type>::pi();

    const real_type r0       = 5.0;
    const real_type theta1_0 = pi * 0.4;
    const real_type theta2_0 = pi * 0.5;
    const real_type theta3_0 = pi * 0.6;

    const real_type sigma  = 1.0;
    const real_type delta  = pi / 18.0;

    const contact_parameter_type p_pro{1, 0, 2, -1.2, 0.0, r0, theta1_0, theta2_0, theta3_0, r0 + 5*sigma, {{1.0, 1.0, 1.0, 1.0}} };
    const dna_parameter_type     p_dna{base_kind::A, 4, 6, 3, 5};

    parameter_list_type parameter_list(
        sigma, delta, 1.0, 0.0, 5.0, {p_pro}, {p_dna}, {/*exclusion*/},
        ignore_molecule_type("Nothing"), ignore_group_type({}));

    interaction_type interaction(potential_type{}, parameter_list_type(parameter_list),
        mjolnir::SpatialPartition<traits_type, potential_type>(
            mjolnir::make_unique<partition_type>()));

    topology_type topol(7);
    topol.add_connection(0, 1, "bond");
    topol.add_connection(1, 2, "bond");
    topol.add_connection(3, 4, "bond");
    topol.add_connection(4, 5, "bond");
    topol.add_connection(4, 6, "bond");
    topol.construct_molecules();

    system_type sys(7, boundary_type{});
    test::clear_everything(sys);

    interaction.initialize(sys, topol);

    std::mt19937 rng(123456789);

    for(const auto r     : {r0 - 0.2, r0 + 0.5})
    {
    for(const auto theta1 : {theta1_0 + delta * 0.2,
                             theta1_0 + delta * 0.7,
                             theta1_0 + delta * 1.2,
                             theta1_0 - delta * 1.2,
                             theta1_0 - delta * 0.7,
                             theta1_0 - delta * 0.2})
    {
    for(const auto theta2 : {theta2_0 + delta * 0.2,
                             theta2_0 + delta * 0.7,
                             theta2_0 + delta * 1.2,
                             theta2_0 - delta * 1.2,
                             theta2_0 - delta * 0.7,
                             theta2_0 - delta * 0.2})
    {
    for(const auto theta3 : {theta3_0 + delta * 0.2,
                             theta3_0 + delta * 0.7,
                             theta3_0 + delta * 1.2,
                             theta3_0 - delta * 1.2,
                             theta3_0 - delta * 0.7,
                             theta3_0 - delta * 0.2})
    {
        // r
        sys.position(1) = mjolnir::math::make_coordinate<coord_type>(r, 0, 0);
        sys.position(4) = mjolnir::math::make_coordinate<coord_type>(0, 0, 0);
        // theta1
        sys.position(6) = mjolnir::math::make_coordinate<coord_type>(std::cos(theta1), std::sin(theta1), 0);
        // theta2
        sys.position(3) = mjolnir::math::make_coordinate<coord_type>(std::cos(theta2+pi), std::sin(theta2+pi), 0);
        sys.position(5) = mjolnir::math::make_coordinate<coord_type>(std::cos(theta2),    std::sin(theta2),    0);
        // theta3
        sys.position(0) = mjolnir::math::make_coordinate<coord_type>(r + std::cos(theta3),    std::sin(theta3),    0);
        sys.position(2) = mjolnir::math::make_coordinate<coord_type>(r + std::cos(theta3+pi), std::sin(theta3+pi), 0);

        test::apply_random_rotation(sys, rng);

        // check configuration is okay
        {
            // XXX here it does not use boundary condition.
            const auto v41 = sys.position(1) - sys.position(4); // B -> C
            BOOST_TEST_REQUIRE(mjolnir::math::length(v41) == r,
                               boost::test_tools::tolerance(1e-3));

            const auto v46 = sys.position(6) - sys.position(4); // B -> S
            const auto v35 = sys.position(5) - sys.position(3); // B-1 -> B+1
            const auto v20 = sys.position(0) - sys.position(2); // C+1 -> C-1

            const auto dot1 = mjolnir::math::dot_product(v41, v46) *
                              mjolnir::math::rlength(v41) *
                              mjolnir::math::rlength(v46);
            const auto cos1 = mjolnir::math::clamp<real_type>(dot1, -1, 1);
            BOOST_TEST_REQUIRE(std::acos(cos1) == theta1,
                               boost::test_tools::tolerance(1e-3));

            const auto dot2 = mjolnir::math::dot_product(v41, v35) *
                              mjolnir::math::rlength(v41) *
                              mjolnir::math::rlength(v35);
            const auto cos2 = mjolnir::math::clamp<real_type>(dot2, -1, 1);
            BOOST_TEST_REQUIRE(std::acos(cos2) == theta2,
                               boost::test_tools::tolerance(1e-3));

            const auto dot3 = mjolnir::math::dot_product(v41, v20) *
                              mjolnir::math::rlength(v41) *
                              mjolnir::math::rlength(v20);
            const auto cos3 = mjolnir::math::clamp<real_type>(dot3, -1, 1);
            BOOST_TEST_REQUIRE(std::acos(cos3) == theta3,
                               boost::test_tools::tolerance(1e-3));
        }

        test::apply_random_perturbation(sys, rng, 0.01);

        interaction.initialize(sys, topol);

        constexpr real_type tol = 1e-4;
        constexpr real_type dr  = 1e-5;

        test::check_force(sys, interaction, tol, dr);
        test::check_virial(sys, interaction, tol);
        test::check_force_and_virial(sys, interaction, tol);
        test::check_force_and_energy(sys, interaction, tol);
        test::check_force_energy_virial(sys, interaction, tol);
    } // theta3
    } // theta2
    } // theta1
    } // rbp
}


#ifndef STAN_MATH_PRIM_SCAL_FUN_LOCSCALE_FREE_HPP
#define STAN_MATH_PRIM_SCAL_FUN_LOCSCALE_FREE_HPP

#include <stan/math/prim/scal/fun/identity_free.hpp>
#include <stan/math/prim/scal/err/check_positive_finite.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <boost/math/tools/promotion.hpp>
#include <cmath>
#include <limits>

namespace stan {
namespace math {

/**
 * Return the unconstrained scalar that transforms to the
 * specified location and scale constrained scalar given the specified
 * location and scale.
 *
 *
The transfrom in locscale_constrain(T, double, double),
 * is reversed by the reverse affine transformation,
 *
 *
\f$f^{-1}(y) = \frac{y - L}{S}\f$
 *
 * where \f$L\f$ and \f$S\f$ are the location and scale.
 *
 *
If the location is zero and scale is one,
 * this function reduces to  identity_free(y).
 *
 * @tparam T type of scalar
 * @tparam L type of location
 * @tparam S type of scale
 * @param y constrained value
 * @param[in] mu location of constrained output
 * @param[in] sigma scale of constrained output
 * @return the free scalar that transforms to the input scalar
 *   given the location and scale
 * @throw std::domain_error if sigma <= 0
 * @throw std::domain_error if mu is not finite
 */
template <typename T, typename L, typename S>
inline typename boost::math::tools::promote_args<T, L, S>::type locscale_free(
    const T& y, const L& mu, const S& sigma) {
  check_finite("locscale_free", "location", mu);
  if (sigma == 1) {
    if (mu == 0)
      return identity_free(y);
    return y - mu;
  }
  check_positive_finite("locscale_free", "scale", sigma);
  return (y - mu) / sigma;
}

}  // namespace math
}  // namespace stan
#endif


#ifndef OBJECTIVEFUNCTIONINFO_H
#define OBJECTIVEFUNCTIONINFO_H

#include <Eigen/Dense>
#include <opengv/types.hpp>

class ObjectiveFunctionInfo{
 public:

  virtual double objective_function_value(const opengv::rotation_t & rotation, const opengv::translation_t & translation) = 0;
  virtual opengv::rotation_t rotation_gradient(const opengv::rotation_t & rotation,
				       const opengv::translation_t & translation) = 0;
  virtual opengv::translation_t translation_gradient(const opengv::rotation_t & rotation,
					     const opengv::translation_t & translation) = 0;
};

#endif


// 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_MAX_INCLUDE
#define MTL_MAX_INCLUDE

#include <iostream>
#include <cmath>

#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/utility/category.hpp>
#include <boost/numeric/mtl/vector/reduction.hpp>
#include <boost/numeric/mtl/vector/reduction_functors.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace mtl { namespace vector {

    namespace impl {

	// Do we really need this for matrices?
	// Then we need a different dispatching

	template <unsigned long Unroll, typename Vector>
	typename Collection<Vector>::value_type
	inline max(const Vector& vector, tag::vector)
	{
	    typedef typename Collection<Vector>::value_type result_type;
	    return vector::reduction<Unroll, vector::max_functor, result_type>::apply(vector);
	}

    } // namespace impl

///Returns maximal entry of %vector v
template <unsigned long Unroll, typename Value>
typename Collection<Value>::value_type
inline max(const Value& value)
{
    vampir_trace<2010> tracer;
    return impl::max<Unroll>(value, typename traits::category<Value>::type());
}

template <typename Value>
typename Collection<Value>::value_type
inline max(const Value& value)
{
    return max<8>(value);
}

} // namespace vector

using vector::max;

} // namespace mtl::vector

#endif // MTL_MAX_INCLUDE


/*=========================================================================

  Program:   Visualization Toolkit
  Module:    vtkBoostConnectedComponents.cxx

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/
/*-------------------------------------------------------------------------
  Copyright 2008 Sandia Corporation.
  Under the terms of Contract DE-AC04-94AL85000 with Sandia Corporation,
  the U.S. Government retains certain rights in this software.
-------------------------------------------------------------------------*/

#include "vtkBoostConnectedComponents.h"

#include "vtkCellArray.h"
#include "vtkCellData.h"
#include "vtkDataArray.h"
#include "vtkFloatArray.h"
#include "vtkInformation.h"
#include "vtkInformationVector.h"
#include "vtkIntArray.h"
#include "vtkMath.h"
#include "vtkObjectFactory.h"
#include "vtkPointData.h"

#include "vtkBoostGraphAdapter.h"
#include <boost/graph/strong_components.hpp>

using namespace boost;

vtkStandardNewMacro(vtkBoostConnectedComponents);

vtkBoostConnectedComponents::vtkBoostConnectedComponents() = default;

vtkBoostConnectedComponents::~vtkBoostConnectedComponents() = default;

int vtkBoostConnectedComponents::RequestData(vtkInformation* vtkNotUsed(request),
  vtkInformationVector** inputVector, vtkInformationVector* outputVector)
{
  // get the info objects
  vtkInformation* inInfo = inputVector[0]->GetInformationObject(0);
  vtkInformation* outInfo = outputVector->GetInformationObject(0);

  // get the input and output
  vtkGraph* input = vtkGraph::SafeDownCast(inInfo->Get(vtkDataObject::DATA_OBJECT()));
  vtkGraph* output = vtkGraph::SafeDownCast(outInfo->Get(vtkDataObject::DATA_OBJECT()));

  // Send the data to output.
  output->ShallowCopy(input);

  // Compute connected components.
  if (vtkDirectedGraph::SafeDownCast(input))
  {
    vtkDirectedGraph* g = vtkDirectedGraph::SafeDownCast(input);
    vtkIntArray* comps = vtkIntArray::New();
    comps->SetName("component");
    vector_property_map<default_color_type> color;
    vector_property_map<vtkIdType> root;
    vector_property_map<vtkIdType> discoverTime;
    strong_components(g, comps, color_map(color).root_map(root).discover_time_map(discoverTime));
    output->GetVertexData()->AddArray(comps);
    comps->Delete();
  }
  else
  {
    vtkUndirectedGraph* g = vtkUndirectedGraph::SafeDownCast(input);
    vtkIntArray* comps = vtkIntArray::New();
    comps->SetName("component");
    vector_property_map<default_color_type> color;
    connected_components(g, comps, color_map(color));
    output->GetVertexData()->AddArray(comps);
    comps->Delete();
  }

  return 1;
}

void vtkBoostConnectedComponents::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os, indent);
}


//////////////////////////////////////////////////////////////////////////////
// data::field::survival::functional::elapsed_time.hpp         				//
//                                                                          //
//                                                                          //
//  (C) Copyright 2010 Erwann Rogard                                        //
//  Use, modification and distribution are subject to the                   //
//  Boost Software License, Version 1.0. (See accompanying file             //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DATA_FIELD_SURVIVAL_FUNCTIONAL_ELAPSED_TIME_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_DATA_FIELD_SURVIVAL_FUNCTIONAL_ELAPSED_TIME_HPP_ER_2010
#include <boost/statistics/detail/data/field/framework/float_valued.hpp>
#include <boost/statistics/detail/data/field/survival/fields/entry_time.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace data{
namespace field{
namespace functional{

	template<typename T>
 	struct elapsed_time{
    	typedef typename field::meta::entry_time<T>::type f_;

		typedef T result_type;

		elapsed_time(const T& t):t_(t){}

        T operator()(const f_& t)const{
        	return field::elapsed_time(f,this->t_);
        }

		private:
		T t_;

    };

}// functional
}// field
}// data
}// detail
}// statistics
}// boost

#endif


/*
 * GraphBLAS Template Library (GBTL), Version 3.0
 *
 * Copyright 2020 Carnegie Mellon University, Battelle Memorial Institute, and
 * Authors.
 *
 * THIS MATERIAL WAS PREPARED AS AN ACCOUNT OF WORK SPONSORED BY AN AGENCY OF
 * THE UNITED STATES GOVERNMENT.  NEITHER THE UNITED STATES GOVERNMENT NOR THE
 * UNITED STATES DEPARTMENT OF ENERGY, NOR THE UNITED STATES DEPARTMENT OF
 * DEFENSE, NOR CARNEGIE MELLON UNIVERSITY, NOR BATTELLE, NOR ANY OF THEIR
 * EMPLOYEES, NOR ANY JURISDICTION OR ORGANIZATION THAT HAS COOPERATED IN THE
 * DEVELOPMENT OF THESE MATERIALS, MAKES ANY WARRANTY, EXPRESS OR IMPLIED, OR
 * ASSUMES ANY LEGAL LIABILITY OR RESPONSIBILITY FOR THE ACCURACY, COMPLETENESS,
 * OR USEFULNESS OR ANY INFORMATION, APPARATUS, PRODUCT, SOFTWARE, OR PROCESS
 * DISCLOSED, OR REPRESENTS THAT ITS USE WOULD NOT INFRINGE PRIVATELY OWNED
 * RIGHTS.
 *
 * Released under a BSD-style license, please see LICENSE file or contact
 * permission@sei.cmu.edu for full terms.
 *
 * [DISTRIBUTION STATEMENT A] This material has been approved for public release
 * and unlimited distribution.  Please see Copyright notice for non-US
 * Government use and distribution.
 *
 * This Software includes and/or makes use of the following Third-Party Software
 * subject to its own license:
 *
 * 1. Boost Unit Test Framework
 * (https://www.boost.org/doc/libs/1_45_0/libs/test/doc/html/utf.html)
 * Copyright 2001 Boost software license, Gennadiy Rozental.
 *
 * DM20-0442
 */

#include <iostream>

#include <graphblas/graphblas.hpp>

using namespace grb;

#define BOOST_TEST_MAIN
#define BOOST_TEST_MODULE matrix_methods_test_suite

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

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

//****************************************************************************
BOOST_AUTO_TEST_CASE(matrix_resize_test)
{
    IndexArrayType      i = {1, 2, 3, 4};
    std::vector<double> v = {1, 2, 3, 4};

    // matrix = diag{ -, 1, 2, 3, 4, -};

    IndexType const NSIZE = 6;
    Matrix<double> m1(NSIZE, NSIZE);
    m1.build(i, i, v);
    //print_matrix(std::cerr, m1, "m1(6,6)");

    BOOST_CHECK_EQUAL(m1.nvals(), i.size());
    BOOST_CHECK_EQUAL(m1.nrows(), NSIZE);
    BOOST_CHECK_EQUAL(m1.ncols(), NSIZE);

    // Make it bigger and set an element
    m1.resize(2*NSIZE, 2*NSIZE);
    //print_matrix(std::cerr, m1, "m1(12,12)");
    BOOST_CHECK_EQUAL(m1.nvals(), i.size());
    BOOST_CHECK_EQUAL(m1.nrows(), 2*NSIZE);
    BOOST_CHECK_EQUAL(m1.ncols(), 2*NSIZE);

    m1.setElement(NSIZE + 1, NSIZE + 1, 99);

    BOOST_CHECK_EQUAL(m1.nvals(), i.size() + 1);
    BOOST_CHECK_EQUAL(m1.extractElement(NSIZE + 1, NSIZE + 1), 99);

    // Reduce num columns check remaining elements
    m1.resize(2*NSIZE, 4UL);
    //print_matrix(std::cerr, m1, "m1(12, 4)");
    BOOST_CHECK_EQUAL(m1.nvals(), 3);
    BOOST_CHECK_EQUAL(m1.nrows(), 2*NSIZE);
    BOOST_CHECK_EQUAL(m1.ncols(), 4);
    BOOST_CHECK_EQUAL(m1.extractElement(1, 1), 1);
    BOOST_CHECK_EQUAL(m1.extractElement(2, 2), 2);
    BOOST_CHECK_EQUAL(m1.extractElement(3, 3), 3);

    // Reduce num rows check remaining elements
    m1.resize(3UL, 4UL);
    //print_matrix(std::cerr, m1, "m1(3, 4)");
    BOOST_CHECK_EQUAL(m1.nvals(), 2);
    BOOST_CHECK_EQUAL(m1.nrows(), 3);
    BOOST_CHECK_EQUAL(m1.ncols(), 4);
    BOOST_CHECK_EQUAL(m1.extractElement(1, 1), 1);
    BOOST_CHECK_EQUAL(m1.extractElement(2, 2), 2);

    // reduce both and check remaining elements
    m1.resize(2UL, 2UL);
    //print_matrix(std::cerr, m1, "m1(2,2)");
    BOOST_CHECK_EQUAL(m1.nvals(), 1);
    BOOST_CHECK_EQUAL(m1.nrows(), 2);
    BOOST_CHECK_EQUAL(m1.ncols(), 2);
    BOOST_CHECK_EQUAL(m1.extractElement(1, 1), 1);

    // Make it bigger show that elements don't reappear
    m1.resize(2*NSIZE, 2*NSIZE);
    //print_matrix(std::cerr, m1, "m1(12,12)");
    BOOST_CHECK_EQUAL(m1.nvals(), 1);
    BOOST_CHECK_EQUAL(m1.nrows(), 2*NSIZE);
    BOOST_CHECK_EQUAL(m1.ncols(), 2*NSIZE);
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(matrix_removeElement_test)
{
    IndexArrayType      i = {0, 0, 0, 1, 1, 1, 2, 2};
    IndexArrayType      j = {1, 2, 3, 0, 2, 3, 0, 1};
    std::vector<double> v = {1, 2, 3, 4, 6, 7, 8, 9};

    //std::vector<std::vector<double> > mat = {{0, 1, 2, 3},
    //                                         {4, 0, 6, 7},
    //                                         {8, 9, 0, 0}};

    Matrix<double, DirectedMatrixTag> m1(3, 4);
    m1.build(i, j, v);

    BOOST_CHECK_EQUAL(m1.nvals(), i.size());

    // remove something that does not exist
    BOOST_CHECK(!m1.hasElement(1, 1));
    m1.removeElement(1, 1);
    BOOST_CHECK_EQUAL(m1.nvals(), i.size());
    BOOST_CHECK(!m1.hasElement(1, 1));

    // remove something that exists
    BOOST_CHECK(m1.hasElement(1, 2));
    m1.removeElement(1, 2);
    BOOST_CHECK_EQUAL(m1.nvals(), i.size() - 1);
    BOOST_CHECK(!m1.hasElement(1, 2));
}

BOOST_AUTO_TEST_SUITE_END()


/**
 * cartesian_pose_control.cc
 *
 * Copyright 2019. All Rights Reserved.
 * Stanford IPRL
 *
 * Created: January 16, 2019
 * Authors: Mengyuan Yan
 */

#include "control_thread.h"

#include <algorithm>  // std::all_of
#include <array>      // std::array
#include <cmath>      // std::abs, std::acos, std::cos, std::pow, std::sin, std::sqrt

#include <Eigen/Eigen>
#include <franka/exception.h>
#include <franka/control_types.h>
#include <franka/duration.h>
#include <franka/robot.h>
#include <franka/robot_state.h>

#include "shared_memory.h"

namespace franka_driver {

/**
 * Sets a default collision behavior, joint impedance, Cartesian impedance, and filter frequency.
 *
 * @param[in] robot Robot instance to set behavior on.
 */
void setDefaultBehavior(franka::Robot& robot);

/**
 * Generate a joint pose motion to a goal position. Adapted from:
 * Wisama Khalil and Etienne Dombre. 2002. Modeling, Identification and Control of Robots
 * (Kogan Page Science Paper edition).
 */
class MotionGenerator {
  public:

  /**
   * Creates a new MotionGenerator instance.
   */
  MotionGenerator();

  /**
   * reset MotionGenerator for a target q.
   *
   * @param[in] speed_factor General speed factor in range [0, 1].
   * @param[in] pose_goal Target end-effector pose.
   */
  void reset(double speed_factor, const std::array<double, 16> pose_goal);

  /**
   * Sends next pose command
   *
   * @param[in] robot_state Current state of the robot.
   * @param[in] period Duration of execution.
   *
   * @return end-effector pose for use inside a control loop.
   */
  franka::CartesianPose operator()(const franka::RobotState& robot_state, franka::Duration period);

  private:
  using Vector3d = Eigen::Matrix<double, 3, 1, Eigen::ColMajor>;
  using Vector2d = Eigen::Matrix<double, 2, 1, Eigen::ColMajor>;
  using Matrix4x4d = Eigen::Matrix<double, 4, 4, Eigen::ColMajor>;
  using Matrix3x3d = Eigen::Matrix<double, 3, 3, Eigen::ColMajor>;

  bool calculateDesiredValues(double t, Vector2d* delta_pose_d) const;
  void calculateSynchronizedValues();
  void preCalculateRotationMatrices();

  // tranlational and rotation angle residual threshold
  static constexpr std::array<double, 2> kDeltaMotionFinished = {1e-4, 0.01};
  Matrix4x4d pose_goal_;
  Matrix4x4d pose_start_;
  Vector3d unit_T_;
  Vector3d axis_R_;
  Vector2d delta_q_; // delta_trans_, delta_angle_

  Matrix3x3d precompute_pose_eye_;
  Matrix3x3d precompute_pose_dot_;
  Matrix3x3d precompute_pose_cross_;

  Vector2d dq_max_sync_;
  Vector2d t_1_sync_;
  Vector2d t_2_sync_;
  Vector2d t_f_sync_;
  Vector2d q_1_;

  double time_ = 0.0;

  const Vector2d default_dq_max_ = (Vector2d() << 0.1, 0.4).finished(); // 0.2m/s, 1.0rad/s
  const Vector2d default_ddq_max_start_ = (Vector2d() << 0.2, 0.5).finished();
  const Vector2d default_ddq_max_goal_ = (Vector2d() << 0.2, 0.5).finished();
  Vector2d dq_max_;
  Vector2d ddq_max_start_;
  Vector2d ddq_max_goal_;

};

void setDefaultBehavior(franka::Robot& robot) {
  robot.setCollisionBehavior(
      {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}},
      {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}},
      {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}},
      {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}});
  robot.setJointImpedance({{3000, 3000, 3000, 2500, 2500, 2000, 2000}});
  robot.setCartesianImpedance({{3000, 3000, 3000, 300, 300, 300}});
}

// This is for gcc 5.4, does not need this line with gcc6.4
// Refer to https://stackoverflow.com/questions/8016780/undefined-reference-to-static-constexpr-char
constexpr std::array<double, 2> MotionGenerator::kDeltaMotionFinished;

MotionGenerator::MotionGenerator() {
  dq_max_sync_.setZero();
  pose_start_.setZero();
  pose_goal_.setZero();
  unit_T_.setZero();
  axis_R_.setZero();
  delta_q_.setZero();
  t_1_sync_.setZero();
  t_2_sync_.setZero();
  t_f_sync_.setZero();
  q_1_.setZero();
}

void MotionGenerator::reset(double speed_factor, const std::array<double, 16> pose_goal)
{
  dq_max_ = default_dq_max_ * speed_factor;
  ddq_max_start_ = default_ddq_max_start_ * speed_factor;
  ddq_max_goal_ = default_ddq_max_goal_ * speed_factor;
  dq_max_sync_.setZero();
  pose_start_.setZero();
  pose_goal_ = Matrix4x4d(pose_goal.data());
  unit_T_.setZero();
  axis_R_.setZero();
  delta_q_.setZero();
  t_1_sync_.setZero();
  t_2_sync_.setZero();
  t_f_sync_.setZero();
  q_1_.setZero();
  time_ = 0;
}

bool MotionGenerator::calculateDesiredValues(double t, Vector2d* delta_q_d) const {
  // Vector2d t_d = t_2_sync_ - t_1_sync_;
  Vector2d delta_t_2_sync = t_f_sync_ - t_2_sync_;
  std::array<bool, 2> motion_finished{};

  for (size_t i = 0; i < 2; i++) {
    if (std::abs(delta_q_[i]) < kDeltaMotionFinished[i]) {
      (*delta_q_d)[i] = 0;
      motion_finished[i] = true;
    } else {
      if (t < t_1_sync_[i]) {
        (*delta_q_d)[i] = -1.0 / (t_1_sync_[i]*t_1_sync_[i]*t_1_sync_[i]) * dq_max_sync_[i] *
                          (0.5 * t - t_1_sync_[i]) * (t*t*t);
      } else if (t >= t_1_sync_[i] && t < t_2_sync_[i]) {
        (*delta_q_d)[i] = q_1_[i] + (t - t_1_sync_[i]) * dq_max_sync_[i];
      } else if (t >= t_2_sync_[i] && t < t_f_sync_[i]) {
        double tt = t - t_2_sync_[i];
        (*delta_q_d)[i] =
            delta_q_[i] + 0.5 * (1.0 / (delta_t_2_sync[i]*delta_t_2_sync[i]*delta_t_2_sync[i]) *
                                   (tt - 2.0 * delta_t_2_sync[i]) * (tt*tt*tt) +
                                (2.0 * tt - delta_t_2_sync[i])) * dq_max_sync_[i];
      } else {
        (*delta_q_d)[i] = delta_q_[i];
        motion_finished[i] = true;
      }
    }
  }
  return std::all_of(motion_finished.cbegin(), motion_finished.cend(),
                     [](bool x) { return x; });
}

void MotionGenerator::calculateSynchronizedValues() {
  Vector2d dq_max_reach(dq_max_);
  Vector2d t_f = Vector2d::Zero();
  Vector2d delta_t_2 = Vector2d::Zero();
  Vector2d t_1 = Vector2d::Zero();
  Vector2d delta_t_2_sync = Vector2d::Zero();

  unit_T_ = pose_goal_.block<3,1>(0,3) - pose_start_.block<3,1>(0,3);
  delta_q_[0] = unit_T_.norm();
  if (delta_q_[0] > 0) unit_T_ = unit_T_ / delta_q_[0];

  Matrix3x3d delta_R_;
  delta_R_ = pose_goal_.block<3,3>(0,0) * pose_start_.block<3,3>(0,0).transpose();
  // matrix to axis-angle
  double cos_angle = (delta_R_.trace()-1)/2;
  delta_q_[1] = std::acos(cos_angle);
  if (cos_angle < 0) {
    // more stable way of calculating axis when angle close to pi.
    axis_R_(0) = std::sqrt((delta_R_(0,0)-cos_angle)/2);
    axis_R_(1) = std::sqrt((delta_R_(1,1)-cos_angle)/2);
    axis_R_(2) = std::sqrt((delta_R_(2,2)-cos_angle)/2);
    if (delta_R_(0,1)-delta_R_(1,0) > 0) axis_R_(2) = -axis_R_(2);
    if (delta_R_(2,0)-delta_R_(0,2) > 0) axis_R_(1) = -axis_R_(1);
    if (delta_R_(1,2)-delta_R_(2,1) > 0) axis_R_(0) = -axis_R_(0);
    axis_R_.normalize();
  } else {
    // more stable way of calculating axis when angle close to 0.
    axis_R_(0) = delta_R_(2,1) - delta_R_(1,2);
    axis_R_(1) = delta_R_(0,2) - delta_R_(2,0);
    axis_R_(2) = delta_R_(1,0) - delta_R_(0,1);
    if (axis_R_.norm() > 0) axis_R_.normalize();
  }

  for (size_t i = 0; i < 2; i++) {
    if (std::abs(delta_q_[i]) > kDeltaMotionFinished[i]) {
      if (std::abs(delta_q_[i]) < (3.0 / 4.0 * (std::pow(dq_max_[i], 2.0) / ddq_max_start_[i]) +
                                   3.0 / 4.0 * (std::pow(dq_max_[i], 2.0) / ddq_max_goal_[i]))) {
        dq_max_reach[i] = std::sqrt(4.0 / 3.0 * delta_q_[i] *
                                    (ddq_max_start_[i] * ddq_max_goal_[i]) /
                                    (ddq_max_start_[i] + ddq_max_goal_[i]));
      }
      t_1[i] = 1.5 * dq_max_reach[i] / ddq_max_start_[i];
      delta_t_2[i] = 1.5 * dq_max_reach[i] / ddq_max_goal_[i];
      t_f[i] = t_1[i] / 2.0 + delta_t_2[i] / 2.0 + std::abs(delta_q_[i]) / dq_max_reach[i];
    }
  }
  double max_t_f = t_f.maxCoeff();
  for (size_t i = 0; i < 2; i++) {
    if (std::abs(delta_q_[i]) > kDeltaMotionFinished[i]) {
      double a = 1.5 / 2.0 * (ddq_max_goal_[i] + ddq_max_start_[i]);
      double b = -1.0 * max_t_f * ddq_max_goal_[i] * ddq_max_start_[i];
      double c = std::abs(delta_q_[i]) * ddq_max_goal_[i] * ddq_max_start_[i];
      double delta = b * b - 4.0 * a * c;
      if (delta < 0.0) {
        delta = 0.0;
      }
      dq_max_sync_[i] = (-1.0 * b - std::sqrt(delta)) / (2.0 * a);
      t_1_sync_[i] = 1.5 * dq_max_sync_[i] / ddq_max_start_[i];
      delta_t_2_sync[i] = 1.5 * dq_max_sync_[i] / ddq_max_goal_[i];
      t_f_sync_[i] =
          (t_1_sync_)[i] / 2.0 + delta_t_2_sync[i] / 2.0 + std::abs(delta_q_[i] / dq_max_sync_[i]);
      t_2_sync_[i] = t_f_sync_[i] - delta_t_2_sync[i];
      q_1_[i] = dq_max_sync_[i] * 0.5 * t_1_sync_[i];
    }
  }
}

void MotionGenerator::preCalculateRotationMatrices() {
  precompute_pose_eye_ = pose_start_.block<3,3>(0,0);
  precompute_pose_dot_ = axis_R_*axis_R_.transpose()*precompute_pose_eye_;
  precompute_pose_cross_ = Eigen::MatrixXd::Zero(3,3);
  precompute_pose_cross_(0,1) = -axis_R_(2);
  precompute_pose_cross_(1,0) = axis_R_(2);
  precompute_pose_cross_(0,2) = axis_R_(1);
  precompute_pose_cross_(2,0) = -axis_R_(1);
  precompute_pose_cross_(1,2) = -axis_R_(0);
  precompute_pose_cross_(2,1) = axis_R_(0);
  precompute_pose_cross_ = precompute_pose_cross_ * precompute_pose_eye_;
}

franka::CartesianPose MotionGenerator::operator()(const franka::RobotState& robot_state,
                                                   franka::Duration period) {
  time_ += period.toSec();

  if (time_ == 0.0) {
    pose_start_ = Matrix4x4d(robot_state.O_T_EE_c.data());
    calculateSynchronizedValues();
    preCalculateRotationMatrices();
  }

  Vector2d delta_q_d;
  bool motion_finished = calculateDesiredValues(time_, &delta_q_d);

  std::array<double, 16> command_pose;
  Eigen::Map<Matrix4x4d> command_pose_e(&command_pose[0]);
  command_pose_e = pose_start_;

  if (delta_q_d[0] > 0) {
    command_pose_e.block<3,1>(0,3) += delta_q_d[0] * unit_T_;
  }
  if (delta_q_d[1] > 0) {
    command_pose_e.block<3,3>(0,0) = std::cos(delta_q_d[1]) * precompute_pose_eye_ +
                                     (1 - std::cos(delta_q_d[1])) * precompute_pose_dot_ +
                                     std::sin(delta_q_d[1]) * precompute_pose_cross_;
  }
//  std::cout << command_pose_e << std::endl;
//  std::cout << command_pose_e.transpose()*command_pose_e << std::endl;
  franka::CartesianPose output(command_pose);
  output.motion_finished = motion_finished;
  return output;
}

std::function<franka::CartesianPose(const franka::RobotState&, franka::Duration)>
CreateCartesianPoseController(const Args& args, const std::shared_ptr<SharedMemory>& globals,
                              franka::Robot& robot, const franka::Model& model) {
  std::shared_ptr<MotionGenerator> motion_generator = std::make_shared<MotionGenerator>();

  franka::RobotState state = robot.readOnce();
  std::array<double, 16> pose_command = globals->pose_command;
  if (globals->control_mode == ControlMode::DELTA_CARTESIAN_POSE) {
    Eigen::Map<Eigen::Matrix4d> start_pose_e(state.O_T_EE.data());
    Eigen::Map<Eigen::Matrix4d> command_pose_e(pose_command.data());
    command_pose_e.topRightCorner<3,1>() += start_pose_e.topRightCorner<3,1>();
    command_pose_e.topLeftCorner<3,3>() = command_pose_e.topLeftCorner<3,3>() * start_pose_e.topLeftCorner<3,3>();
  }
  motion_generator->reset(1., pose_command);

  return [globals, motion_generator](const franka::RobotState& state, franka::Duration dt) -> franka::CartesianPose {
    static const auto t_start = std::chrono::steady_clock::now();
    if (!*globals->runloop) {
      throw std::runtime_error("TorqueController(): SIGINT.");
    }

    // Set sensor values
    globals->q    = state.q;
    globals->dq   = state.dq;
    globals->tau  = state.tau_J;
    globals->dtau = state.dtau_J;

    // Set time
    const auto ms_now = std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::steady_clock::now() - t_start);
    globals->time = globals->time + ms_now.count();

    // Get command torques
    if (globals->control_mode != ControlMode::CARTESIAN_POSE) {
      throw SwitchControllerException("cartesian_pose");
    }

    return (*motion_generator)(state, dt);
  };
}

}  // namespace franka_driver


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

#ifndef BOOST_COMPUTE_ALGORITHM_DETAIL_INPLACE_REDUCE_HPP
#define BOOST_COMPUTE_ALGORITHM_DETAIL_INPLACE_REDUCE_HPP

#include <iterator>

#include <boost/utility/result_of.hpp>

#include <boost/compute/command_queue.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/detail/iterator_range_size.hpp>
#include <boost/compute/memory/local_buffer.hpp>

namespace boost {
namespace compute {
namespace detail {

template<class Iterator, class BinaryFunction>
inline void inplace_reduce(Iterator first,
                           Iterator last,
                           BinaryFunction function,
                           command_queue &queue)
{
    typedef typename
        std::iterator_traits<Iterator>::value_type
        value_type;

    size_t input_size = iterator_range_size(first, last);
    if(input_size < 2){
        return;
    }

    const context &context = queue.get_context();

    size_t block_size = 64;
    size_t values_per_thread = 8;
    size_t block_count = input_size / (block_size * values_per_thread);
    if(block_count * block_size * values_per_thread != input_size)
        block_count++;

    vector<value_type> output(block_count, context);

    meta_kernel k("inplace_reduce");
    size_t input_arg = k.add_arg<value_type *>(memory_object::global_memory, "input");
    size_t input_size_arg = k.add_arg<const uint_>("input_size");
    size_t output_arg = k.add_arg<value_type *>(memory_object::global_memory, "output");
    size_t scratch_arg = k.add_arg<value_type *>(memory_object::local_memory, "scratch");
    k <<
        "const uint gid = get_global_id(0);\n" <<
        "const uint lid = get_local_id(0);\n" <<
        "const uint values_per_thread =\n"
            << uint_(values_per_thread) << ";\n" <<

        // thread reduce
        "const uint index = gid * values_per_thread;\n" <<
        "if(index < input_size){\n" <<
            k.decl<value_type>("sum") << " = input[index];\n" <<
            "for(uint i = 1;\n" <<
                 "i < values_per_thread && (index + i) < input_size;\n" <<
                 "i++){\n" <<
            "    sum = " <<
                     function(k.var<value_type>("sum"),
                              k.var<value_type>("input[index+i]")) << ";\n" <<
            "}\n" <<
            "scratch[lid] = sum;\n" <<
        "}\n" <<

        // local reduce
        "for(uint i = 1; i < get_local_size(0); i <<= 1){\n" <<
        "    barrier(CLK_LOCAL_MEM_FENCE);\n" <<
        "    uint mask = (i << 1) - 1;\n" <<
        "    uint next_index = (gid + i) * values_per_thread;\n"
        "    if((lid & mask) == 0 && next_index < input_size){\n" <<
        "        scratch[lid] = " <<
                     function(k.var<value_type>("scratch[lid]"),
                              k.var<value_type>("scratch[lid+i]")) << ";\n" <<
        "    }\n" <<
        "}\n" <<

        // write output for block
        "if(lid == 0){\n" <<
        "    output[get_group_id(0)] = scratch[0];\n" <<
        "}\n"
        ;

    const buffer *input_buffer = &first.get_buffer();
    const buffer *output_buffer = &output.get_buffer();

    kernel kernel = k.compile(context);

    while(input_size > 1){
        kernel.set_arg(input_arg, *input_buffer);
        kernel.set_arg(input_size_arg, static_cast<uint_>(input_size));
        kernel.set_arg(output_arg, *output_buffer);
        kernel.set_arg(scratch_arg, local_buffer<value_type>(block_size));

        queue.enqueue_1d_range_kernel(kernel,
                                      0,
                                      block_count * block_size,
                                      block_size);

        input_size =
            static_cast<size_t>(
                std::ceil(float(input_size) / (block_size * values_per_thread)
            )
        );

        block_count = input_size / (block_size * values_per_thread);
        if(block_count * block_size * values_per_thread != input_size)
            block_count++;

        std::swap(input_buffer, output_buffer);
    }

    if(input_buffer != &first.get_buffer()){
        ::boost::compute::copy(output.begin(),
                               output.begin() + 1,
                               first,
                               queue);
    }
}

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

#endif // BOOST_COMPUTE_ALGORITHM_DETAIL_INPLACE_REDUCE_HPP


#define CATCH_CONFIG_MAIN
#include <catch2/catch.hpp>
#define MITAMA_PANIC_WITH_STACKTRACE
#include <mitama/maybe/maybe.hpp>
#include <mitama/result/result.hpp>
#include <mitama/result/result_io.hpp>
#include <utest_utility/is_invalid_expr.hpp>

#include <boost/hana/assert.hpp>
#include <boost/hana/functional/overload.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/xpressive/xpressive.hpp>
#include <boost/type_index.hpp>
#include <functional>
#include <cstdint>
#include <iostream>
#include <regex>
#include <stdexcept>
#include <string>
#include <cassert>
#include <memory>
#include <array>
#include <map>

using namespace boost::xpressive;
using namespace mitama;
using namespace std::string_literals;
using namespace std::string_view_literals;
namespace hana = boost::hana;
using boost::lambda::_1;
using boost::lambda::_2;

using str = std::string;
using u32 = std::uint32_t;
using i32 = std::int32_t;
template < class T >
using Rc = std::shared_ptr<T>;

std::vector<std::string> split_impl(std::string &&s, std::regex &&r)
{
  return {std::sregex_token_iterator{s.begin(), s.end(), r, -1},
          std::sregex_token_iterator{}};
}

auto split(std::string s, std::string pattern)
{
  return split_impl(std::move(s), std::regex{pattern});
}

template <class T>
auto parse = [](str s) -> result<T, str> {
  try
  {
    if (std::is_integral_v<T>){
      using namespace boost::xpressive;
      sregex re1 = +range('0', '9');
      smatch what;
      if (!regex_match(s, what, re1))
        return failure("parse error at string: "s + s);
      if constexpr (std::is_unsigned_v<T>) {
        sregex re2 = as_xpr("-") >> +range('0', '9');
        if (regex_match(s, what, re1))
          return failure("negative value: "s + s);
      };
      if constexpr (std::is_same_v<T, int>)
      {
        return success(std::stoi(s));
      }
      else if constexpr (std::is_same_v<T, long>)
      {
        return success(std::stol(s));
      }
      else if constexpr (std::is_same_v<T, unsigned long>)
      {
        return success(std::stoul(s));
      }
      else if constexpr (std::is_same_v<T, long long>)
      {
        return success(std::stoll(s));
      }
      else if constexpr (std::is_same_v<T, unsigned long long>)
      {
        return success(std::stoull(s));
      }
    }
    else if constexpr (std::is_floating_point_v<T>){
      if constexpr (std::is_same_v<T, float>)
      {
        return success(std::stof(s));
      }
      else if constexpr (std::is_same_v<T, double>)
      {
        return success(std::stod(s));
      }
      else if constexpr (std::is_same_v<T, long double>)
      {
        return success(std::stold(s));
      }
    }
    else
    {
      static_assert(mitama::always_true_v<T>);
    }
  }
  catch (std::invalid_argument const &e)
  {
    return failure(e.what());
  }
};

TEST_CASE("is_ok() test", "[result][is_ok]"){
  result<u32, str> x = success(-3);
  REQUIRE(x.is_ok() == true);

  result<u32, str> y = failure("some error message"s);
  REQUIRE(y.is_ok() == false);
}

TEST_CASE("is_err() test", "[result][is_err]"){
  result<u32, str> x = success(-3);
  REQUIRE(x.is_err() == false);

  result<u32, str> y = failure("some error message"s);
  REQUIRE(y.is_err() == true);
}

TEST_CASE("ok() test", "[result][ok]"){
  result<u32, str> x = success(2);
  REQUIRE(x.ok() == just(2u));

  result<int, str> y = failure("Nothing here"s);
  REQUIRE(y.ok() == nothing);
}

TEST_CASE("err() test", "[result][err]"){
  result<u32, str> x = success(2u);
  REQUIRE(x.err() == nothing);

  result<u32, str> y = failure("Nothing here"s);
  REQUIRE(y.err() == just("Nothing here"s));
}

TEST_CASE("map() test", "[result][map]"){
  std::string line = "1,3,5,7";

  for (auto num : split(line, ","))
  {
    if (auto res = parse<i32>(num).map(_1 * 2); res.is_ok())
    {
      REQUIRE(res.ok().unwrap() % 2 == 0);
    }
  }

  result<int, int> some_num = success(1);
  result<int, int> z = some_num.map(std::plus{}, 1);
  REQUIRE(z == success(2));
}

TEST_CASE("map_apply() test", "[result][map_apply]"){
  result<std::tuple<int, int>, int> res = success(1, 2);
  result<int, int> z = res.map_apply(std::plus<>{});
  REQUIRE(z == success(3));
}

TEST_CASE("map_or_else(F, M) test", "[result][map_or_else]"){
  auto k = 21;
  {
    result<str, str> x = success("foo"s);
    REQUIRE(x.map_or_else([k](auto){ return k * 2; }, [](auto v) { return v.length(); }) == 3);
  }
  {
    result<str, str> x = failure("bar"s);
    REQUIRE(x.map_or_else([k](auto){ return k * 2; }, [](auto v) { return v.length(); }) == 42);
  }
}

TEST_CASE("map_anything_else(F) test", "[result][map_anything_else]"){
  {
    result<str, str> x = success("foo"s);
    REQUIRE(x.map_anything_else([](auto v) { return v.length(); }) == 3);
  }
  {
    result<str, str> x = failure("bar"s);
    REQUIRE(x.map_anything_else([](auto v) { return v.length(); }) == 3);
  }
}

TEST_CASE("map_err() test", "[result][map_err]"){
  auto stringify = [](u32 x) -> str {
    return "error code: "s + std::to_string(x);
  };

  result<u32, u32> x = success(2);
  REQUIRE(x.map_err(stringify) == success(2u));

  result<u32, u32> y = failure(13);
  REQUIRE(y.map_err(stringify) == failure("error code: 13"s));

  result<int, int> some_num = failure(1);
  result<int, int> z = some_num.map_err(std::plus{}, 1);
  REQUIRE(z == failure(2));
}

TEST_CASE("map_apply_err() test", "[result][map_apply_err]"){
  result<int, std::tuple<int, int>> res = failure(1, 1);
  result<int, int> z = res.map_apply_err([](auto... a){ return (... + a); }, 1);
  REQUIRE(z == failure(3));
}

TEST_CASE("conj test", "[result][conj]"){
  {
    result<u32, str> x = success(2);
    result<str, str> y = failure("late error"s);
    REQUIRE(x.conj(y) == failure("late error"s));
    REQUIRE((x && y) == failure("late error"s));
  }

  {
    result<u32, str> x = failure("early error"s);
    result<str, str> y = success("foo"s);
    REQUIRE(x.conj(y) == failure("early error"s));
    REQUIRE((x && y) == failure("early error"s));
  }
  {
    result<u32, str> x = failure("not a 2"s);
    result<str, str> y = failure("late error"s);
    REQUIRE(x.conj(y) == failure("not a 2"s));
    REQUIRE((x && y) == failure("not a 2"s));
  }

  {
    result<u32, str> x = success(2);
    result<str, str> y = success("different result type"s);
    REQUIRE(x.conj(y) == success("different result type"s));
    REQUIRE((x && y) == success("different result type"s));
  }
}

TEST_CASE("and_then() test", "[result][and_then]"){
  auto sq = [](u32 x) -> result<u32, u32> { return success(x * x); };
  auto err = [](u32 x) -> result<u32, u32> { return failure(x); };
  auto eq = [](u32 x, u32 y) -> result<u32, u32> { return x == y ? result<u32, u32>(success(x)) : failure(x); };

  REQUIRE(result<u32, u32>{success(2u)}.and_then(sq).and_then(sq) == success(16u));
  REQUIRE(result<u32, u32>{success(2u)}.and_then(sq).and_then(err) == failure(4u));
  REQUIRE(result<u32, u32>{success(2u)}.and_then(err).and_then(sq) == failure(2u));
  REQUIRE(result<u32, u32>{failure(3u)}.and_then(sq).and_then(sq) == failure(3u));
  REQUIRE(result<u32, u32>{failure(3u)}.and_then(sq).and_then(sq) == failure(3u));
  REQUIRE(result<u32, u32>{success(3u)}.and_then(eq, 3u) == success(3u));
  REQUIRE(result<u32, u32>{failure(3u)}.and_then(eq, 1u) == failure(3u));
}

TEST_CASE("and_then_apply() test", "[result][and_then_apply]"){
  auto fn = [](u32 x, u32 y) -> result<u32, u32> {
    if ( (x + y) % 2 == 0 ) return success(x+y);
    else return failure(x+y);
  };
  result<std::tuple<u32,u32>, u32> ok1 = success(1u, 2u);
  result<std::tuple<u32,u32>, u32> ok2 = success(1u, 1u);
  result<std::tuple<u32,u32>, u32> err = failure(1u);
  REQUIRE(ok1.and_then_apply(fn) == failure(3u));
  REQUIRE(ok2.and_then_apply(fn) == success(2u));
  REQUIRE(err.and_then_apply(fn) == failure(1u));
}

TEST_CASE("or_else_apply() test", "[result][or_else_apply]"){
  auto fn = [](u32 x, u32 y) -> result<u32, u32> {
    if ( (x + y) % 2 == 0 ) return success(x+y);
    else return failure(x+y);
  };
  result<u32, std::tuple<u32,u32>> ok = success(1u);
  result<u32, std::tuple<u32,u32>> err1 = failure(1u, 1u);
  result<u32, std::tuple<u32,u32>> err2 = failure(1u, 2u);
  REQUIRE(ok.or_else_apply(fn) == success(1u));
  REQUIRE(err1.or_else_apply(fn) == success(2u));
  REQUIRE(err2.or_else_apply(fn) == failure(3u));
}

TEMPLATE_TEST_CASE("is_convertible_result_with meta test", "[is_convertible_result_with][meta]",
                    int, unsigned, std::string, std::vector<int>)
{
  REQUIRE(is_convertible_result_with_v<mitama::result<int, TestType>, mitama::failure_t<TestType>>);
  REQUIRE(!is_convertible_result_with_v<result<unsigned, std::vector<TestType>>, mitama::failure_t<TestType>>);
}

TEST_CASE("disj test", "[result][disj]"){
  {
    result<u32, str> x = success(2);
    result<u32, str> y = failure("late error"s);
    REQUIRE(x.disj(y) ==  success(2u));
    REQUIRE((x || y) ==  success(2u));
  }
  {
    result<u32, str> x = failure("early error"s);
    result<u32, str> y = success(2);
    REQUIRE(x.disj(y) ==  success(2u));
    REQUIRE((x || y) ==  success(2u));
  }
  {
    result<u32, str> x = failure("not a 2"s);
    result<u32, str> y = failure("late error"s);
    REQUIRE(x.disj(y) ==  failure("late error"s));
    REQUIRE((x || y) ==  failure("late error"s));
  }
  {
    result<u32, str> x = success(2);
    result<u32, str> y = success(100);
    REQUIRE(x.disj(y) ==  success(2u));
    REQUIRE((x || y) ==  success(2u));
  }
}

TEST_CASE("or_else() test", "[result][or_else]"){
  auto sq = [](u32 x) -> result<u32, u32> { return success(x * x); };
  auto err = [](u32 x) -> result<u32, u32> { return failure(x); };
  auto eq = [](u32 x, u32 y) -> result<u32, u32> { return x == y ? result<u32, u32>(success(x)) : failure(x); };

  REQUIRE(result<u32, u32>{success(2)}.or_else(sq).or_else(sq) ==  success(2u));
  REQUIRE(result<u32, u32>{success(2)}.or_else(err).or_else(sq) ==  success(2u));
  REQUIRE(result<u32, u32>{failure(3)}.or_else(sq).or_else(err) ==  success(9u));
  REQUIRE(result<u32, u32>{failure(3)}.or_else(err).or_else(err) ==  failure(3u));
  REQUIRE(result<u32, u32>{failure(3)}.or_else(eq, 3u) ==  success(3u));
  REQUIRE(result<u32, u32>{success(3)}.or_else(eq, 3u) ==  success(3u));
  REQUIRE(result<u32, u32>{failure(3)}.or_else(eq, 1u) ==  failure(3u));
}

TEST_CASE("unwrap_or() test", "[result][unwrap_or]"){
  result<u32, u32> err = failure(2);
  result<u32, u32> ok = success(2);

  REQUIRE(ok.unwrap_or(1u) ==  2u);
  REQUIRE(err.unwrap_or(1u) ==  1u);
}

TEST_CASE("unwrap_or_else() test", "[result][unwrap_or_else]"){
  auto count = [](str x) -> size_t { return x.size(); };

  REQUIRE(result<u32, str>{success(2)}.unwrap_or_else(count) ==  2);
  REQUIRE(result<u32, str>{failure("foo"s)}.unwrap_or_else(count) ==  3ull);
  REQUIRE(result<u32, str>{failure("foo"s)}.unwrap_or_else([]{ return 3ull; }) ==  3ull);
}

TEST_CASE("unwrap() test", "[result][unwrap]"){
  {
    result<u32, str> x = success(2);
    REQUIRE(x.unwrap() ==  2u);
  }
  try {
    result<u32, str> x = failure("emergency failure"s);
    x.unwrap(); // panics with `emergency failure`
  }
  catch (runtime_panic const &p)
  {
    using namespace boost::xpressive;
    sregex re =
        as_xpr(
            "runtime panicked at 'called `basic_result::unwrap()` on a value: `failure(\"emergency failure\")`', ") >>
        *_ >> as_xpr(":") >> +range('0', '9') >> 	_n >> _n >> as_xpr("stacktrace:") >> +(_n | ~_n);
    smatch what;
    REQUIRE(regex_match(std::string{p.what()}, what, re));
  }
}

TEST_CASE("unwrap_err() test", "[result][unwrap_err]"){
  try {
    result<u32, str> x = success(2);
    x.unwrap_err(); // panics with `2`
  }
  catch (runtime_panic const &p)
  {
    using namespace boost::xpressive;
    sregex re =
        as_xpr(
            R"(runtime panicked at 'called `basic_result::unwrap_err()` on a value: `success(2)`', )") >>
        *_ >> as_xpr(":") >> +range('0', '9') >> 	_n >> _n >> as_xpr("stacktrace:") >> +(_n | ~_n);
    smatch what;
    REQUIRE(regex_match(std::string{p.what()}, what, re));
  }

  {
    result<u32, str> x = failure("emergency failure"s);
    REQUIRE(x.unwrap_err() ==  "emergency failure"s);
  }
}

TEST_CASE("unwrap_or_default() test", "[result][unwrap_or_default]"){
  auto good_year_from_input = "1909"s;
  auto bad_year_from_input = "190blarg"s;
  auto good_year = parse<int>(good_year_from_input).unwrap_or_default();
  auto bad_year = parse<int>(bad_year_from_input).unwrap_or_default();

  REQUIRE(1909 ==  good_year);
  REQUIRE(0 ==  bad_year);
}

TEST_CASE("transpose() test", "[result][transpose]"){
  result<maybe<i32>, std::monostate> x = success(just(5));
  maybe<result<i32, std::monostate>> y = just(success(5));

  REQUIRE(x.transpose() == y);
}

TEST_CASE("and_finally() test", "[result][and_finally]"){
  int hook = 0;
  result<int, std::string> x = failure("error"s);
  x.and_finally([&hook](int v){
    hook = v;
  });
  REQUIRE(hook == 0);

  result<int, std::string> y = success(1);
  y.and_finally([&hook](int v){
    hook = v;
  });
  REQUIRE(hook == 1);
}

TEST_CASE("or_finally() test", "[result][or_finally]"){
  std::string hook = "default";
  result<int, std::string> x = success(42);
  x.or_finally([&hook](std::string v){
    hook = v;
  });
  REQUIRE(hook == "default"s);

  result<int, std::string> y = failure("error"s);
  y.or_finally([&hook](std::string v){
    hook = v;
  });
  REQUIRE(hook == "error"s);
}

TEST_CASE("and_peek() test", "[result][and_peek]"){
  mut_result<int, std::string> x = success(42);
  REQUIRE(x.and_peek([](int& v){ v = 57; }) == success(57));
}

TEST_CASE("or_peek() test", "[result][and_peek]"){
  mut_result<int, std::string> x = failure("foo"s);
  REQUIRE(x.or_peek([](std::string& v){ v = "bar"; }) == failure("bar"s));
}

TEST_CASE("basics test", "[result][basics]"){
  auto even = [](u32 u) -> result<u32, str> {
    if (u % 2 == 0)
      return success(u);
    else
      return failure("odd"s);
  };
  auto func = [](auto u) -> result<u32, str> { if(u%3==0) return success(1u); else return failure("error"s); };
  REQUIRE(even(2).and_then(func) ==  failure("error"s));
  REQUIRE(even(2) ==  success(2u));
}

TEST_CASE("constructors and assignments test", "[result][constructors][assignments]"){
  {
    auto res = result<int,int>{success(2)};
    res = result<int, int>{failure(2)};
    (void)result<std::string, double>{in_place_ok, "hoge"};
    (void)result<double, std::string>{in_place_err, "hoge"};
    (void)result<std::vector<int>, double>{in_place_ok, {1, 2, 3, 4}};
    (void)result<double, std::vector<double>>{in_place_err, {1., 2., 3., 4.}};
  }
  {
    auto res = mut_result<int,int>{success(2)};
    res = success(1);
    res = failure(2);
  }
}

TEST_CASE("format test", "[result][format]"){
  SECTION("success"){
    using namespace std::literals;
    std::stringstream ss;
    ss << success(1);
    REQUIRE(ss.str() ==  "success(1)"s);
  }
  SECTION("failure"){
    using namespace std::literals;
    std::stringstream ss;
    ss << failure(1);
    REQUIRE(ss.str() ==  "failure(1)"s);
  }
  SECTION("result ok"){
    using namespace std::literals;
    std::stringstream ss;
    ss << result<int, std::string>{success(1)};
    REQUIRE(ss.str() ==  "success(1)"s);
  }
  SECTION("result err"){
    using namespace std::literals;
    std::stringstream ss;
    ss << result<int, std::string>{failure("hoge"s)};
    REQUIRE(ss.str() ==  "failure(\"hoge\")"s);
  }
  SECTION("result of range ok"){
    using namespace std::literals;
    std::stringstream ss;
    ss << success(std::vector<std::string>{"foo"s, "bar"s});
    REQUIRE(ss.str() == "success([\"foo\",\"bar\"])"s);
  }
  SECTION("result of range err"){
    using namespace std::literals;
    std::stringstream ss;
    ss << failure(std::vector<std::string>{"foo"s, "bar"s});
    REQUIRE(ss.str() == "failure([\"foo\",\"bar\"])"s);
  }
  SECTION("success of tuple"){
    using namespace std::literals;
    std::stringstream ss;
    ss << success(std::tuple{"foo"s, 1});
    REQUIRE(ss.str() == "success((\"foo\",1))"s);
  }
  SECTION("failure of tuple"){
    using namespace std::literals;
    std::stringstream ss;
    ss << failure(std::tuple{"foo"s, 1});
    REQUIRE(ss.str() == "failure((\"foo\",1))"s);
  }
  SECTION("result of tuple"){
    using namespace std::literals;
    {
      std::stringstream ss;
      ss << result<std::tuple<str, int>, int>(success(std::tuple{"foo"s, 1}));
      REQUIRE(ss.str() == "success((\"foo\",1))"s);
    }
    {
      std::stringstream ss;
      ss << result<int, std::tuple<str, int>>(failure(std::tuple{"foo"s, 1}));
      REQUIRE(ss.str() == "failure((\"foo\",1))"s);
    }
  }
  SECTION("result of dictionary"){
    using namespace std::literals;
    {
      std::stringstream ss;
      ss << result<std::map<str, int>, int>(success(std::map<str, int>{{"foo"s, 1}}));
      REQUIRE(ss.str() == "success({\"foo\": 1})"s);
    }
  }
  SECTION("result of tuple of tuple"){
    using namespace std::literals;
    {
      std::stringstream ss;
      ss << result<std::tuple<std::tuple<int, int>, int>, int>(success(std::tuple{std::tuple{1, 1}, 1}));
      REQUIRE(ss.str() == "success(((1,1),1))"s);
    }
  }
  SECTION("result of range of tuple"){
    using namespace std::literals;
    {
      std::stringstream ss;
      ss << result<std::vector<std::tuple<int, int>>, int>(success(std::vector{std::tuple{1, 1}, std::tuple{1, 1}}));
      REQUIRE(ss.str() == "success([(1,1),(1,1)])"s);
    }
  }
  SECTION("failure"){
    using namespace std::literals;
    std::stringstream ss;
    ss << failure("foo"s);
    REQUIRE(ss.str() ==  "failure(\"foo\")"s);
  }
  SECTION("replace"){
    using namespace std::literals;
    auto res = mut_result<int, std::vector<int>>{in_place_err, {1,2,3}};
    REQUIRE((boost::format("%1%") % res).str() == "failure([1,2,3])"s);
    res = success(1);
    REQUIRE((boost::format("%1%") % res).str() ==  "success(1)"s);
  }
}

TEST_CASE("monostate success test", "[result][monostate]"){
  auto func = []() -> result<std::monostate, std::string> {
    if (false) return failure<std::string>("hoge"s);
    return success<>();
  };

  REQUIRE(func().is_ok());
}

TEST_CASE("monostate failure test", "[result][monostate]"){
  auto func = []() -> result</*defaulted monostate*/> {
    if (false) return success<>();
    return failure<>();
  };
  REQUIRE(func().is_err());
}

TEST_CASE("contextually convertible to bool", "[result]"){
  auto err_func = []() -> result</*defaulted monostate*/> {
    if (false) return failure<>();
    return failure<>();
  };
  auto ok_func = []() -> result<std::monostate, std::string> {
    if (false) return failure<std::string>("hoge"s);
    return success<>();
  };
  REQUIRE(!err_func());
  REQUIRE(ok_func());
}

SCENARIO("test for reference type", "[result][ref]"){
  using namespace std::literals;
  GIVEN( "A result that refer to some string" ) {
    str hoge = "foo";
    mut_result<str&, str&> res(success<str&>(hoge));

    REQUIRE( hoge == "foo"s );
    REQUIRE( res.unwrap() == "foo"s );

    WHEN( "the result is overwritten" ) {
      res.unwrap() = "bar";

      THEN( "the referred string change" ) {
        REQUIRE( hoge == "bar"s );
        REQUIRE( res.unwrap() == "bar"s );
      }
    }
  }
}

SCENARIO("test for as_ref", "[result][as_ref]"){
  using namespace std::literals;
  GIVEN( "A new result, containing a reference into the original" ) {
    mut_result<str, str> res(success<str>("foo"s));
    auto ref /* mut_result<str&, str&> */ = res.as_ref();

    REQUIRE( res == ref );
    REQUIRE( res == success("foo"s) );
    REQUIRE( ref == success("foo"s) );
  }
}

SCENARIO("test for as_mut", "[result][as_mut]"){
  using namespace std::literals;
  GIVEN( "A new result, containing a reference into the original" ) {
    auto ptr = std::make_shared<str>("foo"s);
    mut_result<Rc<str>, Rc<str>> res(success<Rc<str>>(ptr));
    auto ref /* result<str const&, str const&> */ = res.as_mut();

    REQUIRE( res == success(ptr) );
    REQUIRE( ref == success(ptr) );

    WHEN( "The new result is overwritten" ) {
      *ref.unwrap() = "bar"s;

      THEN( "the original result change" ) {
        REQUIRE( res == ref );
        REQUIRE( *res.unwrap() == "bar"s );
        REQUIRE( *ref.unwrap() == "bar"s );
      }
    }
  }
}

SCENARIO("test for indirect", "[result][indirect]"){
  using namespace std::literals;
  using vec_iter = typename std::vector<int>::iterator;

  GIVEN( "A new result, containing a indirect reference into the original" ) {
    std::vector<int> vec{1, 2, 3};
    mut_result<vec_iter, vec_iter> res(success<vec_iter>(vec.begin()));
    auto indirect = res.indirect();

    REQUIRE( *res.unwrap() == indirect.unwrap() );
    REQUIRE( indirect == success(1) );

    WHEN( "The new result is overwritten" ) {
      auto& ref = indirect.unwrap();
      ref = 42;

      THEN( "the original result change" ) {
        REQUIRE( vec[0] == 42 );
        REQUIRE( indirect == success(42) );
      }
    }
  }
}

SCENARIO("test for dangling indirect", "[result][indirect][dangling]"){
  using namespace std::literals;
  using vec_iter = typename std::vector<int>::iterator;
  GIVEN( "A new result which is containing a dangling reference into the discarded unique_ptr" ) {
    auto indirect
      = mut_result<std::unique_ptr<int>, std::unique_ptr<int>>(success(std::make_unique<int>(1)))
        .as_ref()
        .indirect();
    REQUIRE( std::is_same_v<decltype(indirect.unwrap()), dangling<std::reference_wrapper<int>>&> );
    // indirect.unwrap().transmute()
    // ^~~~~~~~~~~~~~~~~~~~~~~~~~ Undefined Behavior!
  }
  GIVEN( "A new result which is containing a reference into the living vector" ) {
    std::vector<int> vec{1,3};
    auto indirect = result<vec_iter, vec_iter>(success(vec.begin())).indirect();

    REQUIRE( indirect.unwrap().transmute() == 1 );
    //       ^~~~~~~~~~~~~~~~~~~~~~~~~~ OK!
  }
}

struct incomplete_type;
incomplete_type& get_incomplete_type();
template <class T, class=void> struct is_complete_type: std::false_type {};
template <class T> struct is_complete_type<T, std::void_t<decltype(sizeof(T))>>: std::true_type {};

TEST_CASE("incomplete type reference", "[result]") {
  static_assert(!is_complete_type<incomplete_type>::value);
  [[maybe_unused]]
  result<incomplete_type&> res = success<incomplete_type&>(get_incomplete_type()); // use incomplete_type& for result
}

struct incomplete_type {};

incomplete_type& get_incomplete_type() {
  static incomplete_type obj = incomplete_type{};
  return obj;
}

TEST_CASE("less compare", "[result][less]"){
  result<int, int> ok1 = success(1);
  result<int, int> ok2 = success(2);
  result<int, int> err1 = failure(1);
  result<int, int> err2 = failure(2);

  REQUIRE(ok1 < ok2);
  REQUIRE_FALSE(ok2 < ok1);
  REQUIRE_FALSE(ok1 < ok1);
  REQUIRE_FALSE(ok2 < ok2);

  REQUIRE(success(1) < ok2);
  REQUIRE_FALSE(success(2) < ok1);
  REQUIRE_FALSE(success(1) < ok1);
  REQUIRE_FALSE(success(2) < ok2);

  REQUIRE(ok1 < success(2));
  REQUIRE_FALSE(ok2 < success(1));
  REQUIRE_FALSE(ok1 < success(1));
  REQUIRE_FALSE(ok2 < success(2));

  REQUIRE(success(1) < success(2));
  REQUIRE_FALSE(success(2) < success(1));
  REQUIRE_FALSE(success(1) < success(1));
  REQUIRE_FALSE(success(2) < success(2));

  REQUIRE(ok1 < 2);
  REQUIRE_FALSE(ok2 < 1);
  REQUIRE_FALSE(ok1 < 1);
  REQUIRE_FALSE(ok2 < 2);

  REQUIRE(1 < ok2);
  REQUIRE_FALSE(2 < ok1);
  REQUIRE_FALSE(1 < ok1);
  REQUIRE_FALSE(2 < ok2);

  REQUIRE(err1 < err2);
  REQUIRE_FALSE(err2 < err1);
  REQUIRE_FALSE(err1 < err1);
  REQUIRE_FALSE(err2 < err2);

  REQUIRE(failure(1) < err2);
  REQUIRE_FALSE(failure(2) < err1);
  REQUIRE_FALSE(failure(1) < err1);
  REQUIRE_FALSE(failure(2) < err2);

  REQUIRE(err1 < failure(2));
  REQUIRE_FALSE(err2 < failure(1));
  REQUIRE_FALSE(err1 < failure(1));
  REQUIRE_FALSE(err2 < failure(2));

  REQUIRE(failure(1) < failure(2));
  REQUIRE_FALSE(failure(2) < failure(1));
  REQUIRE_FALSE(failure(1) < failure(1));
  REQUIRE_FALSE(failure(2) < failure(2));

  REQUIRE(err1 < ok1);
  REQUIRE(err1 < ok2);
  REQUIRE(err2 < ok1);
  REQUIRE(err2 < ok2);

  REQUIRE(failure(1) < ok1);
  REQUIRE(failure(1) < ok2);
  REQUIRE(failure(2) < ok1);
  REQUIRE(failure(2) < ok2);

  REQUIRE(err1 < success(1));
  REQUIRE(err1 < success(2));
  REQUIRE(err2 < success(1));
  REQUIRE(err2 < success(2));

  REQUIRE(failure(1) < success(1));
  REQUIRE(failure(1) < success(2));
  REQUIRE(failure(2) < success(1));
  REQUIRE(failure(2) < success(2));

  REQUIRE_FALSE(ok1 < err1);
  REQUIRE_FALSE(ok1 < err2);
  REQUIRE_FALSE(ok2 < err1);
  REQUIRE_FALSE(ok2 < err2);

  REQUIRE_FALSE(success(1) < err1);
  REQUIRE_FALSE(success(1) < err2);
  REQUIRE_FALSE(success(2) < err1);
  REQUIRE_FALSE(success(2) < err2);

  REQUIRE_FALSE(ok1 < failure(1));
  REQUIRE_FALSE(ok1 < failure(2));
  REQUIRE_FALSE(ok2 < failure(1));
  REQUIRE_FALSE(ok2 < failure(2));

  REQUIRE_FALSE(success(1) < failure(1));
  REQUIRE_FALSE(success(1) < failure(2));
  REQUIRE_FALSE(success(2) < failure(1));
  REQUIRE_FALSE(success(2) < failure(2));

  REQUIRE_FALSE(1 < err1);
  REQUIRE_FALSE(1 < err2);
  REQUIRE_FALSE(2 < err1);
  REQUIRE_FALSE(2 < err2);

}

TEST_CASE("less_or_equal compare", "[result][less_or_equal]"){
  result<int, int> ok1 = success(1);
  result<int, int> ok2 = success(2);
  result<int, int> err1 = failure(1);
  result<int, int> err2 = failure(2);

  REQUIRE(ok1 <= ok2);
  REQUIRE_FALSE(ok2 <= ok1);
  REQUIRE(ok1 <= ok1);
  REQUIRE(ok2 <= ok2);

  REQUIRE(success(1) <= ok2);
  REQUIRE_FALSE(success(2) <= ok1);
  REQUIRE(success(1) <= ok1);
  REQUIRE(success(2) <= ok2);

  REQUIRE(ok1 <= success(2));
  REQUIRE_FALSE(ok2 <= success(1));
  REQUIRE(ok1 <= success(1));
  REQUIRE(ok2 <= success(2));

  REQUIRE(success(1) <= success(2));
  REQUIRE_FALSE(success(2) <= success(1));
  REQUIRE(success(1) <= success(1));
  REQUIRE(success(2) <= success(2));

  REQUIRE(1 <= ok2);
  REQUIRE_FALSE(2 <= ok1);
  REQUIRE(1 <= ok1);
  REQUIRE(2 <= ok2);

  REQUIRE(ok1 <= 2);
  REQUIRE_FALSE(ok2 <= 1);
  REQUIRE(ok1 <= 1);
  REQUIRE(ok2 <= 2);

  REQUIRE(err1 <= err2);
  REQUIRE_FALSE(err2 <= err1);
  REQUIRE(err1 <= err1);
  REQUIRE(err2 <= err2);

  REQUIRE(failure(1) <= err2);
  REQUIRE_FALSE(failure(2) <= err1);
  REQUIRE(failure(1) <= err1);
  REQUIRE(failure(2) <= err2);

  REQUIRE(err1 <= failure(2));
  REQUIRE_FALSE(err2 <= failure(1));
  REQUIRE(err1 <= failure(1));
  REQUIRE(err2 <= failure(2));

  REQUIRE(failure(1) <= failure(2));
  REQUIRE_FALSE(failure(2) <= failure(1));
  REQUIRE(failure(1) <= failure(1));
  REQUIRE(failure(2) <= failure(2));

  REQUIRE(err1 <= ok1);
  REQUIRE(err1 <= ok2);
  REQUIRE(err2 <= ok1);
  REQUIRE(err2 <= ok2);

  REQUIRE(failure(1) <= ok1);
  REQUIRE(failure(1) <= ok2);
  REQUIRE(failure(2) <= ok1);
  REQUIRE(failure(2) <= ok2);

  REQUIRE(err1 <= success(1));
  REQUIRE(err1 <= success(2));
  REQUIRE(err2 <= success(1));
  REQUIRE(err2 <= success(2));

  REQUIRE(failure(1) <= success(1));
  REQUIRE(failure(1) <= success(2));
  REQUIRE(failure(2) <= success(1));
  REQUIRE(failure(2) <= success(2));

  REQUIRE(err1 <= 1);
  REQUIRE(err1 <= 2);
  REQUIRE(err2 <= 1);
  REQUIRE(err2 <= 2);

  REQUIRE_FALSE(ok1 <= err1);
  REQUIRE_FALSE(ok1 <= err2);
  REQUIRE_FALSE(ok2 <= err1);
  REQUIRE_FALSE(ok2 <= err2);

  REQUIRE_FALSE(success(1) <= err1);
  REQUIRE_FALSE(success(1) <= err2);
  REQUIRE_FALSE(success(2) <= err1);
  REQUIRE_FALSE(success(2) <= err2);

  REQUIRE_FALSE(ok1 <= failure(1));
  REQUIRE_FALSE(ok1 <= failure(2));
  REQUIRE_FALSE(ok2 <= failure(1));
  REQUIRE_FALSE(ok2 <= failure(2));

  REQUIRE_FALSE(success(1) <= failure(1));
  REQUIRE_FALSE(success(1) <= failure(2));
  REQUIRE_FALSE(success(2) <= failure(1));
  REQUIRE_FALSE(success(2) <= failure(2));

  REQUIRE_FALSE(1 <= err1);
  REQUIRE_FALSE(1 <= err2);
  REQUIRE_FALSE(2 <= err1);
  REQUIRE_FALSE(2 <= err2);

}

TEST_CASE("greater compare", "[result][greater]"){
  result<int, int> ok1 = success(1);
  result<int, int> ok2 = success(2);
  result<int, int> err1 = failure(1);
  result<int, int> err2 = failure(2);

  REQUIRE_FALSE(ok1 > ok2);
  REQUIRE(ok2 > ok1);
  REQUIRE_FALSE(ok1 > ok1);
  REQUIRE_FALSE(ok2 > ok2);

  REQUIRE_FALSE(success(1) > ok2);
  REQUIRE(success(2) > ok1);
  REQUIRE_FALSE(success(1) > ok1);
  REQUIRE_FALSE(success(2) > ok2);

  REQUIRE_FALSE(ok1 > success(2));
  REQUIRE(ok2 > success(1));
  REQUIRE_FALSE(ok1 > success(1));
  REQUIRE_FALSE(ok2 > success(2));

  REQUIRE_FALSE(success(1) > success(2));
  REQUIRE(success(2) > success(1));
  REQUIRE_FALSE(success(1) > success(1));
  REQUIRE_FALSE(success(2) > success(2));

  REQUIRE_FALSE(1 > ok2);
  REQUIRE(2 > ok1);
  REQUIRE_FALSE(1 > ok1);
  REQUIRE_FALSE(2 > ok2);

  REQUIRE_FALSE(ok1 > 2);
  REQUIRE(ok2 > 1);
  REQUIRE_FALSE(ok1 > 1);
  REQUIRE_FALSE(ok2 > 2);

  REQUIRE_FALSE(err1 > err2);
  REQUIRE(err2 > err1);
  REQUIRE_FALSE(err1 > err1);
  REQUIRE_FALSE(err2 > err2);

  REQUIRE_FALSE(failure(1) > err2);
  REQUIRE(failure(2) > err1);
  REQUIRE_FALSE(failure(1) > err1);
  REQUIRE_FALSE(failure(2) > err2);

  REQUIRE_FALSE(err1 > failure(2));
  REQUIRE(err2 > failure(1));
  REQUIRE_FALSE(err1 > failure(1));
  REQUIRE_FALSE(err2 > failure(2));

  REQUIRE_FALSE(failure(1) > failure(2));
  REQUIRE(failure(2) > failure(1));
  REQUIRE_FALSE(failure(1) > failure(1));
  REQUIRE_FALSE(failure(2) > failure(2));

  REQUIRE_FALSE(err1 > ok1);
  REQUIRE_FALSE(err1 > ok2);
  REQUIRE_FALSE(err2 > ok1);
  REQUIRE_FALSE(err2 > ok2);

  REQUIRE_FALSE(failure(1) > ok1);
  REQUIRE_FALSE(failure(1) > ok2);
  REQUIRE_FALSE(failure(2) > ok1);
  REQUIRE_FALSE(failure(2) > ok2);

  REQUIRE_FALSE(err1 > success(1));
  REQUIRE_FALSE(err1 > success(2));
  REQUIRE_FALSE(err2 > success(1));
  REQUIRE_FALSE(err2 > success(2));

  REQUIRE_FALSE(failure(1) > success(1));
  REQUIRE_FALSE(failure(1) > success(2));
  REQUIRE_FALSE(failure(2) > success(1));
  REQUIRE_FALSE(failure(2) > success(2));

  REQUIRE_FALSE(err1 > 1);
  REQUIRE_FALSE(err1 > 2);
  REQUIRE_FALSE(err2 > 1);
  REQUIRE_FALSE(err2 > 2);

  REQUIRE(ok1 > err1);
  REQUIRE(ok1 > err2);
  REQUIRE(ok2 > err1);
  REQUIRE(ok2 > err2);

  REQUIRE(success(1) > err1);
  REQUIRE(success(1) > err2);
  REQUIRE(success(2) > err1);
  REQUIRE(success(2) > err2);

  REQUIRE(ok1 > failure(1));
  REQUIRE(ok1 > failure(2));
  REQUIRE(ok2 > failure(1));
  REQUIRE(ok2 > failure(2));

  REQUIRE(success(1) > failure(1));
  REQUIRE(success(1) > failure(2));
  REQUIRE(success(2) > failure(1));
  REQUIRE(success(2) > failure(2));

  REQUIRE(1 > err1);
  REQUIRE(1 > err2);
  REQUIRE(2 > err1);
  REQUIRE(2 > err2);

}

TEST_CASE("greater_or_equal compare", "[result][greater_or_equal]"){
  result<int, int> ok1 = success(1);
  result<int, int> ok2 = success(2);
  result<int, int> err1 = failure(1);
  result<int, int> err2 = failure(2);

  REQUIRE_FALSE(ok1 >= ok2);
  REQUIRE(ok2 >= ok1);
  REQUIRE(ok1 >= ok1);
  REQUIRE(ok2 >= ok2);

  REQUIRE_FALSE(success(1) >= ok2);
  REQUIRE(success(2) >= ok1);
  REQUIRE(success(1) >= ok1);
  REQUIRE(success(2) >= ok2);

  REQUIRE_FALSE(ok1 >= success(2));
  REQUIRE(ok2 >= success(1));
  REQUIRE(ok1 >= success(1));
  REQUIRE(ok2 >= success(2));

  REQUIRE_FALSE(success(1) >= success(2));
  REQUIRE(success(2) >= success(1));
  REQUIRE(success(1) >= success(1));
  REQUIRE(success(2) >= success(2));

  REQUIRE_FALSE(1 >= ok2);
  REQUIRE(2 >= ok1);
  REQUIRE(1 >= ok1);
  REQUIRE(2 >= ok2);

  REQUIRE_FALSE(ok1 >= 2);
  REQUIRE(ok2 >= 1);
  REQUIRE(ok1 >= 1);
  REQUIRE(ok2 >= 2);

  REQUIRE_FALSE(err1 >= err2);
  REQUIRE(err2 >= err1);
  REQUIRE(err1 >= err1);
  REQUIRE(err2 >= err2);

  REQUIRE_FALSE(failure(1) >= err2);
  REQUIRE(failure(2) >= err1);
  REQUIRE(failure(1) >= err1);
  REQUIRE(failure(2) >= err2);

  REQUIRE_FALSE(err1 >= failure(2));
  REQUIRE(err2 >= failure(1));
  REQUIRE(err1 >= failure(1));
  REQUIRE(err2 >= failure(2));

  REQUIRE_FALSE(failure(1) >= failure(2));
  REQUIRE(failure(2) >= failure(1));
  REQUIRE(failure(1) >= failure(1));
  REQUIRE(failure(2) >= failure(2));

  REQUIRE_FALSE(err1 >= ok1);
  REQUIRE_FALSE(err1 >= ok2);
  REQUIRE_FALSE(err2 >= ok1);
  REQUIRE_FALSE(err2 >= ok2);

  REQUIRE_FALSE(failure(1) >= ok1);
  REQUIRE_FALSE(failure(1) >= ok2);
  REQUIRE_FALSE(failure(2) >= ok1);
  REQUIRE_FALSE(failure(2) >= ok2);

  REQUIRE_FALSE(err1 >= success(1));
  REQUIRE_FALSE(err1 >= success(2));
  REQUIRE_FALSE(err2 >= success(1));
  REQUIRE_FALSE(err2 >= success(2));

  REQUIRE_FALSE(failure(1) >= success(1));
  REQUIRE_FALSE(failure(1) >= success(2));
  REQUIRE_FALSE(failure(2) >= success(1));
  REQUIRE_FALSE(failure(2) >= success(2));

  REQUIRE_FALSE(err1 >= 1);
  REQUIRE_FALSE(err1 >= 2);
  REQUIRE_FALSE(err2 >= 1);
  REQUIRE_FALSE(err2 >= 2);

  REQUIRE(ok1 >= err1);
  REQUIRE(ok1 >= err2);
  REQUIRE(ok2 >= err1);
  REQUIRE(ok2 >= err2);

  REQUIRE(success(1) >= err1);
  REQUIRE(success(1) >= err2);
  REQUIRE(success(2) >= err1);
  REQUIRE(success(2) >= err2);

  REQUIRE(ok1 >= failure(1));
  REQUIRE(ok1 >= failure(2));
  REQUIRE(ok2 >= failure(1));
  REQUIRE(ok2 >= failure(2));

  REQUIRE(success(1) >= failure(1));
  REQUIRE(success(1) >= failure(2));
  REQUIRE(success(2) >= failure(1));
  REQUIRE(success(2) >= failure(2));

  REQUIRE(1 >= err1);
  REQUIRE(1 >= err2);
  REQUIRE(2 >= err1);
  REQUIRE(2 >= err2);

}

#include <mitama/boolinators.hpp>

TEST_CASE("as_ok test", "[result][as_ok][boolinators]"){
  basic_result x = as_ok(true, 1);
  REQUIRE(x == success(1));
  basic_result y = as_ok(false, 1);
  REQUIRE(y == failure());
}

TEST_CASE("as_result test", "[result][as_result][boolinators]"){
  basic_result x = as_result(true, 1, "err"s);
  REQUIRE(x == success(1));
  basic_result y = as_result(false, 1, "err"s);
  REQUIRE(y == failure("err"));
}

TEST_CASE("as_result_from test", "[result][as_result_from][boolinators]"){
  basic_result x = as_result_from(true, []{ return 1; }, []{ return "err"s; });
  REQUIRE(x == success(1));
  basic_result y = as_result_from(false, []{ return 1; }, []{ return "err"s; });
  REQUIRE(y == failure("err"));
}

TEST_CASE("ok_or test", "[result][ok_or][boolinators]"){
  basic_result x = ok_or(true, "err"s);
  REQUIRE(x == success(std::monostate{}));
  basic_result y = ok_or(false, "err"s);
  REQUIRE(y == failure("err"s));
}

TEST_CASE("ok_or_else test", "[result][ok_or_else][boolinators]"){
  basic_result x = ok_or_else(true, []{ return "err"s; });
  REQUIRE(x == success(std::monostate{}));
  basic_result y = ok_or_else(false, []{ return "err"s; });
  REQUIRE(y == failure("err"s));
}


//  (C) Copyright Gert-Jan de Vos 2012.
//  Distributed under the Boost Software License, Version 1.0.
//  (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

//  See http://boosttestui.wordpress.com/ for the boosttestui home page.

#define BOOST_TEST_MODULE Boost Unit Test Sample
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(TestSuite)

BOOST_AUTO_TEST_CASE(TestCase)
{
	BOOST_REQUIRE_EQUAL(1 + 1, 2);
}

BOOST_AUTO_TEST_SUITE_END()


// Boost.Geometry Index
// Unit Test

// Copyright (c) 2016 Adam Wulkiewicz, Lodz, Poland.

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

#include <rtree/test_rtree.hpp>

#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/io/wkt/read.hpp>

template <typename Point>
inline void fill(Point & pt, double x, double y)
{
    bg::set<0>(pt, x);
    bg::set<1>(pt, y);
}

template <typename Point>
inline void fill(bg::model::box<Point> & box, double x, double y)
{
    bg::set<0, 0>(box, x);
    bg::set<0, 1>(box, y);
    bg::set<1, 0>(box, x + 20);
    bg::set<1, 1>(box, y + 20);
}

template <typename Rtree>
void test_rtree()
{
    typedef typename Rtree::value_type value_t;

    Rtree rtree;
    value_t v;

    // This is not fully valid because both point's longitude and box's min
    // longitude should be in range [-180, 180]. So if this stopped passing
    // in the future it wouldn't be that bad. Still it works as it is now.

    size_t n = 0;
    for (double x = -170; x < 400; x += 30, ++n)
    {
        //double lon = x <= 180 ? x : x - 360;
        double lon = x;
        double lat = x <= 180 ? 0 : 30;

        fill(v, lon, lat);
        rtree.insert(v);
        BOOST_CHECK_EQUAL(rtree.size(), n + 1);
        size_t vcount = 1; // x < 180 ? 1 : 2;
        BOOST_CHECK_EQUAL(rtree.count(v), vcount);
        std::vector<value_t> res;
        rtree.query(bgi::intersects(v), std::back_inserter(res));
        BOOST_CHECK_EQUAL(res.size(), vcount);
    }

    for (double x = -170; x < 400; x += 30, --n)
    {
        //double lon = x <= 180 ? x : x - 360;
        double lon = x;
        double lat = x <= 180 ? 0 : 30;

        fill(v, lon, lat);
        rtree.remove(v);
        BOOST_CHECK_EQUAL(rtree.size(), n - 1);
        size_t vcount = 0; // x < 180 ? 1 : 0;
        BOOST_CHECK_EQUAL(rtree.count(v), vcount);
        std::vector<value_t> res;
        rtree.query(bgi::intersects(v), std::back_inserter(res));
        BOOST_CHECK_EQUAL(res.size(), vcount);
    }
}

template <typename Value>
void test_value()
{
    test_rtree<bgi::rtree<Value, bgi::linear<4> > >();
    test_rtree<bgi::rtree<Value, bgi::quadratic<4> > >();
    test_rtree<bgi::rtree<Value, bgi::rstar<4> > >();
}

template <typename Rtree>
void test_ticket_12413()
{
    typedef typename Rtree::value_type pair_t;
    typedef typename pair_t::first_type point_t;

    Rtree rtree;
    rtree.insert(std::make_pair(point_t(-1.558444, 52.38664), 792));
    rtree.insert(std::make_pair(point_t(-1.558444, 52.38664), 793));
    rtree.insert(std::make_pair(point_t(-2.088824, 51.907406), 800));
    rtree.insert(std::make_pair(point_t(-1.576363, 53.784089), 799));
    rtree.insert(std::make_pair(point_t(-77.038816, 38.897282), 801));
    rtree.insert(std::make_pair(point_t(-1.558444, 52.38664), 794));
    rtree.insert(std::make_pair(point_t(-0.141588, 51.501009), 797));
    rtree.insert(std::make_pair(point_t(-118.410468, 34.103003), 798));
    rtree.insert(std::make_pair(point_t(-0.127592, 51.503407), 796));

    size_t num_removed = rtree.remove(std::make_pair(point_t(-0.127592, 51.503407), 796));

    BOOST_CHECK_EQUAL(num_removed, 1);
}

template <typename Point>
void test_cs()
{
    test_value<Point>();
    test_value<bg::model::box<Point> >();

    {
        typedef std::pair<Point, unsigned> value_t;
        test_ticket_12413<bgi::rtree<value_t, bgi::linear<4> > >();
        test_ticket_12413<bgi::rtree<value_t, bgi::quadratic<4> > >();
        test_ticket_12413<bgi::rtree<value_t, bgi::rstar<4> > >();
    }
}

int test_main(int, char* [])
{
    //test_cs<bg::model::point<double, 2, bg::cs::cartesian> >();
    test_cs<bg::model::point<double, 2, bg::cs::spherical_equatorial<bg::degree> > >();
    test_cs<bg::model::point<double, 2, bg::cs::geographic<bg::degree> > >();

    return 0;
}


// Functions for computing scores that assess the significance of TCR clusters
// Z_CO and S_size in the terminology of the original manuscript
//

#ifndef INCLUDED_pubtcrs_cluster_scores_HH
#define INCLUDED_pubtcrs_cluster_scores_HH

#include <boost/math/distributions/binomial.hpp>

#include "misc.hh"
#include "randutil.hh"

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// both lists should sum to the same thing: the sum of the subject counts for all the tcrs in the cluster
//
//
Real // return the best score
score_l1_versus_l2(
	Reals const & l1,  // should be sorted
	Reals const & l2,  // -- ditto --
	Size & min_switchpoint // for i <= min_switchpoint, score += l1[i] - l2[i];
) //                            i >  min_switchpoint, score += l2[i] - l2[i];
{
	Real const epsilon( 1e-4 );

	runtime_assert( l1.front() >= l1.back()-epsilon ); // silly check for sorted
	runtime_assert( l2.front() >= l2.back()-epsilon ); // ditto

	Size const nsubjects( l2.size() );

	runtime_assert( l1.size() == nsubjects );

	Reals total_l1_minus_l2;

	Real total(0);
	for ( Size i=0; i< nsubjects; ++i ) {
		total += l1[i] - l2[i];
		total_l1_minus_l2.push_back( total );
	}

	runtime_assert( fabs( total_l1_minus_l2[ nsubjects-1 ] - 0.0 )<epsilon );

	Real best_score( 0.0 ), score;
	min_switchpoint = nsubjects-1; // this should give a score of 0.0

	total=0.;

	for ( Size i=nsubjects-1; i > 0; --i ) {
		total += l2[i] - l1[i];
		// this is the score if min_switchpoint were i-1:
		score = total_l1_minus_l2[ i-1 ] + total;
		if ( score >= best_score - epsilon ) { // choose smaller switchpoints with equal scores
			best_score = score;
			min_switchpoint = i-1;
		}
	}

	return best_score;

}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Real // return diff_Zscore
analyze_cluster_occurrences(
	Sizes const & tcrs,
	vector< bools > const & all_occs, // could be just intra-HLA occs
	Sizes const & subject_indices, // for output, eg might be: ( hla_positive_subjects.find(hla)->second ) );
	Reals const & subject_sampling_bias,
	Size const nrep,
	string const tag, // empty for classic output
	Size & upper_tcr_count,
	Size & lower_tcr_count,
	ostream & out
)
{
	Size const total_subjects( all_occs.front().size() );
	runtime_assert( subject_sampling_bias.size() == total_subjects );
	runtime_assert( subject_indices.size() == total_subjects );

	Reals avg_counts( total_subjects, 0. );
	Real diff_score_mean, diff_score_sdev;

	bools chosen( total_subjects ); // re-use this array
	Sizes counts( total_subjects );
	Reals realcounts( total_subjects );

	Reals rand_diff_scores;

	// go through this loop 2*nrep times, building up the average the first time, then comparing the counts
	// 	to the average the second time, so we can get a Z-score for the observed counts
	for ( Size rep=1; rep<= 2*nrep; ++rep ) {
		std::fill( counts.begin(), counts.end(), Size(0) );

		// randomly choose tcr occurrences
		foreach_( Size t, tcrs ) {
			Size num_subjects_this_tcr(0);
			foreach_( bool occ, all_occs[t] ) { if ( occ ) ++num_subjects_this_tcr; }
			if ( num_subjects_this_tcr ) {
				choose_random_subjects_with_bias( num_subjects_this_tcr, subject_sampling_bias, chosen );
				for ( Size i=0; i< total_subjects; ++i ) {
					if ( chosen[i] ) ++counts[i];
				}
			}
		}

		sort( counts.begin(), counts.end() ); // increasing order
		std::reverse( counts.begin(), counts.end() ); // now decreasing order

		if ( rep <= nrep ) {
			for ( Size i=0; i< total_subjects; ++i ) avg_counts[i] += counts[i];

			if ( rep == nrep ) { // normalize avg scores, write them out
				for ( Size i=0; i< total_subjects; ++i ) avg_counts[i] /= nrep;
				out << "avg_subject_tcr_counts" << tag << ": total_subjects= " << total_subjects;
				for ( Size i=0; i< total_subjects; ++i ) out << ' ' << F(9,3,avg_counts[i]);
				out << '\n';
			}
		} else {
			/// score our counts against avg_counts
			for ( Size i=0; i< total_subjects; ++i ) realcounts[i] = Real(counts[i]);

			Size tmp_switchpoint;
			Real pos_score( score_l1_versus_l2( realcounts, avg_counts, tmp_switchpoint ) ),
				neg_score( score_l1_versus_l2( avg_counts, realcounts, tmp_switchpoint ) );

			if ( pos_score > neg_score ) {
				rand_diff_scores.push_back( pos_score );
			} else {
				rand_diff_scores.push_back( -1 * neg_score );
			}
			runtime_assert( rand_diff_scores.size() == rep-nrep );
		}
	} // rep=1, 2*nrep
	runtime_assert( rand_diff_scores.size() == nrep );
	get_mean_sdev( rand_diff_scores, diff_score_mean, diff_score_sdev );

	Sizes subject_tcr_counts( total_subjects, 0 );
	for ( Size i=0; i< tcrs.size(); ++i ) {
		bools const & i_occs( all_occs[ tcrs[i] ] );
		for ( Size k=0; k< total_subjects; ++k ) {
			if ( i_occs[k] ) ++subject_tcr_counts[k];
		}
	}
	SizePairs countslist;
	Size num_nonzero(0);
	for ( Size k=0; k< total_subjects; ++k ) {
		if ( subject_tcr_counts[k]>0 ) ++num_nonzero;
		countslist.push_back( make_pair( subject_tcr_counts[k], k ) );
	}
	sort( countslist.begin(), countslist.end () );
	reverse( countslist.begin(), countslist.end () );

	//Reals realcounts( total_subjects, 0.0 );
	for ( Size i=0; i< total_subjects; ++i ) {
		realcounts[i] = countslist[i].first;
	}

	Size pos_switchpoint, neg_switchpoint;
	Real pos_score( score_l1_versus_l2( realcounts, avg_counts, pos_switchpoint ) ),
		neg_score( score_l1_versus_l2( avg_counts, realcounts, neg_switchpoint ) ),
		diff_score( pos_score>neg_score ? pos_score : -1*neg_score ),
		diff_Zscore( ( diff_score - diff_score_mean )/ diff_score_sdev );

	upper_tcr_count = countslist[ pos_switchpoint ].first;
	lower_tcr_count = ( pos_switchpoint < total_subjects ? countslist[ pos_switchpoint+1 ].first : 0 );

	out << "subject_tcr_counts" << tag << ": total_subjects= " << total_subjects <<
		" num_nonzero_tcr_counts: " << num_nonzero <<
		" D_CO: " << F(9,3,diff_score) <<
		" Z_CO: " << F(9,3,diff_Zscore) <<
		" D_CO_rand_mean: " << F(9,3,diff_score_mean) <<
		" D_CO_rand_sdev: " << F(9,3,diff_score_sdev) <<
		" pos_switchpoint: " << pos_switchpoint <<
		" upper_tcr_count: " << upper_tcr_count <<
		" lower_tcr_count: " << lower_tcr_count;
	// Sizes const & hla_subs( hla_positive_subjects.find(hla)->second );
	// runtime_assert( hla_subs.size() == total_subjects );
	for ( Size k=0; k< total_subjects; ++k ) {
		if ( countslist[k].first == 0 ) break; // don't show the zeros
		Size const subject( subject_indices[ countslist[k].second ] );
		out << ' ' << subject << ':' << countslist[k].first;
			//<< ':' << subject_sampling_bias[ countslist[k].second ]; // now show the bias...
	}
	out << '\n';

	return diff_Zscore;
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// returns 1 if we don't have info for this (similarity_mode,nbr_pval_threshold)
//
//
// probability of seeing an equal or greater number of neighbors for a random TCR, given num_clustered_tcrs
//
Real
compute_cluster_size_score(
	Size const num_center_nbrs, // max nbr number for cluster member
	Size const num_clustered_tcrs,
	Size const similarity_mode,
	Real const nbr_pval_threshold
)
{
	using namespace boost::math;
	static bool init( false );
	static map< pair<Size,Real>, Reals > precomputed_nbr_rates;

	if ( !init ) {
		init = true;
		string const filename( misc::dbdir + "nbr_rates_for_P_size_calc.txt" );
		strings lines;
		read_lines_from_file( filename, lines );
		foreach_( string line, lines ) {
			istringstream l(line);
			string tmp;
			Size l_sim_mode;
			Real l_nbr_pval_threshold;
			l >> tmp >> l_sim_mode >> tmp >> l_nbr_pval_threshold >> tmp;
			Reals nbr_rates;
			while ( !l.fail() ) {
				Real rate;
				l >> rate;
				if ( !l.fail() ) nbr_rates.push_back( rate );
			}
			cout << "Read " << nbr_rates.size() << " precomputed_nbr_rates for similarity_mode: " << l_sim_mode <<
				" and nbr_pval_threshold: " << l_nbr_pval_threshold << endl;
			precomputed_nbr_rates[ make_pair( l_sim_mode, l_nbr_pval_threshold ) ] = nbr_rates;
		}
	}

	Reals nbr_rates;

	if ( similarity_mode == 1 ) {
		nbr_rates.push_back( nbr_pval_threshold );
	} else { // look for info in the file
		for ( map< pair<Size,Real>, Reals >::const_iterator it= precomputed_nbr_rates.begin();
					it!= precomputed_nbr_rates.end(); ++it ) {
			if ( it->first.first == similarity_mode && fabs( log( it->first.second ) - log( nbr_pval_threshold ) )<.1 ) {
				// match
				nbr_rates = it->second;
			}
		}
	}

	if ( nbr_rates.empty() ) {
		cerr << "Failed to find precomputed information for computing cluster size score:: " <<
			" similarity_mode: " << similarity_mode << " nbr_pval_threshold: " << nbr_pval_threshold << endl;
		return 1.0;
	}

	//
	Real P_size(0); // odds of seeing an equal or greater neighbor number, given the total number of tcrs clustered

	Real const wt( 1.0/ nbr_rates.size() );
	foreach_( Real rate, nbr_rates ) {
		P_size += wt * cdf( complement( binomial( num_clustered_tcrs-1, rate ), num_center_nbrs-1 ) );
	}

	return P_size;
}

#endif


//  ================================================================
//  Created by Gregory Kramida on 11/2/18.
//  Copyright (c) 2018 Gregory Kramida
//  Licensed under the Apache License, Version 2.0 (the "License");
//  you may not use this file except in compliance with the License.
//  You may obtain a copy of the License at

//  http://www.apache.org/licenses/LICENSE-2.0

//  Unless required by applicable law or agreed to in writing, software
//  distributed under the License is distributed on an "AS IS" BASIS,
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  See the License for the specific language governing permissions and
//  limitations under the License.
//  ================================================================
#pragma once

//libboost
#include <boost/property_tree/ptree.hpp>

//local
#include "../../math/typedefs.hpp"

// Short alias for this namespace
namespace pt = boost::property_tree;

namespace nonrigid_optimization {
namespace slavcheva{
class Optimizer2d {
public:
	class SharedParameters {
	public:
		static SharedParameters& get_instance() {
			static SharedParameters instance;
			return instance;
		}

		SharedParameters(const SharedParameters&) = delete;
		void operator=(SharedParameters const&) = delete;

		// when adding new parameters, please modify the following set functions
		void set_from_json(pt::ptree root);
		void set_from_values(float gradient_descent_rate = 0.1f,

		// termination condition parameters
		float maximum_warp_length_lower_threshold = 0.1f,
		float maximum_warp_length_upper_threshold = 10000,
		int maximum_iteration_count = 100,
		int minimum_iteration_count = 1,

		// logging
		bool enable_focus_spot_analytics = false,
		bool enable_energy_and_min_vector_logging = false,
		bool enable_live_sdf_progression_logging = false,
		bool enable_gradient_logging = false,
		bool enable_gradient_component_logging = false,
		int focus_spot_x = 0, int focus_spot_y = 0);

		float gradient_descent_rate = 0.1f;

		// termination condition parameters
		float maximum_warp_length_lower_threshold = 0.1f;
		float maximum_warp_length_upper_threshold = 10000;
		int maximum_iteration_count = 100;
		int minimum_iteration_count = 1;

		// logging
		// optimization-wide
		bool enable_convergence_reporting = false;
		//interiteration
		bool enable_focus_spot_analytics = false;
		bool enable_gradient_component_logging = false;
		//iteration-wise
		bool enable_live_sdf_progression_logging = false;
		bool enable_gradient_logging = false;
		bool enable_warp_statistics_logging = false;

		math::Vector2i focus_spot = math::Vector2i(0, 0);

	private:
		SharedParameters() = default;
	};

	static SharedParameters& shared_parameters();

	Optimizer2d() = default;
	virtual ~Optimizer2d() = default;

	/**
	 * \brief Perform non-rigid alignment of the live 2D TSDF field to the canonical 2D TSDF field
	 * \param live_field a scalar TSDF field (presumably obtained from raw depth image row pixel)
	 * \param canonical_field fused data in original position in a TSDF field representation
	 * \return warped live field: live field after alignment
	 */
	virtual eig::MatrixXf optimize(const eig::MatrixXf& live_field, const eig::MatrixXf& canonical_field) = 0;

protected:
	static bool are_termination_conditions_reached(int completed_iteration_count, float largest_warp_vector);

};
} //namespace slavcheva
}		//namespace nonrigid_optimization


#include "crypto/mnemonic/mnemonic.h"
#include "crypto/sha256.h"
#include <algorithm>
#include <bitset>
#include <sstream>
#include <string>
#include <boost/format.hpp>

namespace mnemonic
{
    namespace
    {
        std::string BinaryString(const uint8_t* data, const int& len)
        {
            std::stringstream ss;
            for(int i = 0; i < len; i++)
            {
                ss << std::bitset<8>((int)data[i]).to_string();
            }
            return ss.str();
        }
    }

    WordList MnemonicStringToWords(const std::string& mnemonic)
    {
        std::stringstream s;
        s << mnemonic;

        WordList words;
        while(s.good()) {
            std::string word;
            s >> word;

            if(!word.empty()) {
                words.push_back(word);
            }
        }
        return words;
    }

    bool IsAValidMnemonic(const WordList& words) {
        return words.size() == MNEMONIC_WORD_COUNT;
    }

    std::array<uint8_t, SEED_LENGTH> MnemonicToSeed(const WordList& mnemonic, const std::string& passphrase)
    {
        return MnemonicToSeed(Unwords(mnemonic), passphrase);
    }

    std::array<uint8_t, SEED_LENGTH> MnemonicToSeed(const std::string& mnemonic, const std::string& passphrase)
    {
        std::string salt = "mnemonic" + passphrase;

        std::array<uint8_t, SEED_LENGTH> seed;

        pkcs5_pbkdf2(mnemonic, salt, seed.begin(), SEED_LENGTH, 2048);

        return seed;
    }

    std::string Unwords(const WordList& phrase)
    {
        if(phrase.size() > 0 )
            return std::accumulate(std::next(phrase.begin()), phrase.end(), phrase[0],
                [](std::string left, std::string right)
                {
                    return left + " " + right;
                });
        return "";
    }

    WordList Entropy2Mnemonic(const std::vector<uint8_t>& entropy, const language::Dictionary& dict)
    {
        std::vector<uint8_t> hash(CSHA256::OUTPUT_SIZE);

        CSHA256 sha256;
        sha256.Write(entropy.data(), entropy.size()).Finalize(hash.data());

        auto checksumBits = entropy.size() * 8 / 32;
        std::string checksumString = BinaryString(hash.data(), (checksumBits/8)+1).substr(0, checksumBits);
        std::string entString = BinaryString(entropy.data(), entropy.size()) + checksumString;

        assert(entString.size() % 11 == 0); // getting 11 bits to pick index in 0-2047
        assert(entString.size() / 11 == MNEMONIC_WORD_COUNT); // maybe this doesn't have to be constant?

        std::vector<int> inds(MNEMONIC_WORD_COUNT);
        for(size_t i = 0; i < entString.size() / 11; i++) {
            inds[i] = std::stoi(entString.substr(11*i, 11), nullptr, 2);
        }

        WordList mnemonic(MNEMONIC_WORD_COUNT);
        std::transform(inds.begin(), inds.end(), mnemonic.begin(), [&dict](const int& i) { return dict[i]; });

        assert(IsAValidMnemonic(mnemonic));
        return mnemonic;
    }
}