#ifndef LUCCA_CAMERA_HPP
#define LUCCA_CAMERA_HPP

#include "lucca/config.hpp"

#include "lucca/viewport.hpp"
#include "lucca/serialization/camera_serializer.hpp"

#include <boost/signals2.hpp>

namespace ayla {
	class Ray;
}

namespace lucca {

/**
 * A viewpoint from which a scene will be rendered.
 *
 * @see lucca::RenderTarget
 *
 * @author O Programador
 */
class LUCCA_API Camera final {
public:
	Camera();
	Camera(const Viewport& viewport);
	Camera(const Camera& other);

	~Camera();

public:
	glm::mat4 getViewMatrix() const;
	glm::mat4 getProjectionMatrix() const;

	void setViewMatrix(const glm::mat4& viewMatrix);
	void setProjectionMatrix(Float fovyRadians, Float nearDistance, Float farDistance);

	glm::vec3 getPosition() const;
	glm::vec3 getDirection() const;

	Float getFovy() const;
	Float getNearDistance() const;
	Float getFarDistance() const;

	void setRenderTarget(RenderTarget* renderTarget);

	SizeType getDefaultFramebufferObject() const;
	void postRedisplay();

public:
	SizeType getViewportAbsoluteX() const;
	SizeType getViewportAbsoluteY() const;
	SizeType getViewportAbsoluteWidth() const;
	SizeType getViewportAbsoluteHeight() const;

public:
	ayla::Ray getCameraToViewportRay(const glm::vec2& pointOnViewport) const;
	glm::vec2 getNormalizedPointOnViewport(const glm::vec2& pointOnViewport) const;

public:
	/**
	 * Emitted when this camera get any change.
	 */
	using ChangedSignal = boost::signals2::signal<void(const Camera&)>;

	boost::signals2::connection connectToChangedSignal(ChangedSignal::slot_function_type slot);

private:
	Viewport _viewport;
	glm::mat4 _viewMatrix = glm::mat4(1.0f);
	Float _fovyRadians = Float(glm::radians(45.0f));
	Float _nearDistance = Float(0.1f);
	Float _farDistance = Float(1000.0f);
	ChangedSignal _signalChanged;

	template<class Archive> friend void boost::serialization::serialize(Archive&, lucca::Camera&, const unsigned int);
};

}

#endif // LUCCA_CAMERA_HPP


#include <pybind11/pybind11.h>
#include <iostream>
#include <sstream>
#include <pybind11/numpy.h>

#include <boost/format.hpp>
#include "xtensor-python/pytensor.hpp"

#include "nifty/python/graph/undirected_list_graph.hxx"
#include "nifty/python/graph/edge_contraction_graph.hxx"
#include "nifty/python/graph/opt/multicut/multicut_objective.hxx"

namespace py = pybind11;

namespace nifty{
namespace graph{
namespace opt{
namespace multicut{

    template<class GRAPH>
    void exportMulticutObjectiveT(py::module & multicutModule) {

        typedef GRAPH GraphType;
        typedef MulticutObjective<GraphType, double> ObjectiveType;

        const auto graphClsName = GraphName<GraphType>::name();
        const auto clsName = MulticutObjectiveName<ObjectiveType>::name();

        auto multicutObjectiveCls = py::class_<ObjectiveType>(multicutModule, clsName.c_str(),
            (
                boost::format(
                        "Multicut objective for a graph of type nifty.graph.%s\n\n"
                        "The multicut objective function is given by:\n\n"
                        ".. math::\n"
                        "      E(y) = \\sum_{e \\in E} w_e \\cdot y_e \n\n"
                        "      st. y \\in MulticutPolytop_{G}        \n\n"
                        "This energy function can be used to find the optimal multicut:\n\n"
                        ".. math::\n"
                        "      y^* = argmin_{y} \\sum_{e \\in E} w_e \\cdot y_e \n\n"
                        "      st. y \\in MulticutPolytop_{G}                     \n"
                    )%graphClsName
            ).str().c_str()

        );
        multicutObjectiveCls

            .def(py::init([](const GraphType & graph,
                            const xt::pytensor<double, 1> & array){

                    NIFTY_CHECK_OP(array.dimension(),==,1,"wrong dimensions");
                    NIFTY_CHECK_OP(array.shape()[0], ==, graph.edgeIdUpperBound()+1, "wrong shape");

                    auto objective = new ObjectiveType(graph);

                    auto & weights = objective->weights();
                    graph.forEachEdge([&](int64_t edge){
                        weights[edge] += array(edge);
                    });
                    return objective;
                }),
                py::keep_alive<1, 2>(),
                py::arg("graph"),
                py::arg("weights")
                ,
                (boost::format("Factory function to create a multicut objective\n\n"
                "Args:\n"
                "   graph: (%s) : The graph\n"
                "   weights: (numpy.ndarray) : weights map\n\n"
                "Returns:\n"
                "  %s :  multicut objective"
                )%graphClsName%clsName).str().c_str()
            )

            .def_property_readonly("graph", &ObjectiveType::graph)

            .def("evalNodeLabels",[](const ObjectiveType & objective,
                                     const xt::pytensor<uint64_t, 1> & array){
                return objective.evalNodeLabels(array);
            })
        ;

        multicutModule.def("multicutObjective",
            [](const GraphType & graph,
                xt::pytensor<double, 1> & array){

                NIFTY_CHECK_OP(array.dimension(),==,1,"wrong dimensions");
                NIFTY_CHECK_OP(array.shape()[0], ==, graph.edgeIdUpperBound()+1, "wrong shape");
                auto obj = new ObjectiveType(graph);
                auto & weights = obj->weights();
                graph.forEachEdge([&](int64_t edge){
                    weights[edge] += array(edge);
                });
                return obj;
            },
            py::return_value_policy::take_ownership,
            py::keep_alive<0, 1>(),
            py::arg("graph"),py::arg("weights")
        );
    }

    void exportMulticutObjective(py::module & multicutModule) {

        {
            typedef PyUndirectedGraph GraphType;
            exportMulticutObjectiveT<GraphType>(multicutModule);
        }
        {
            typedef PyContractionGraph<PyUndirectedGraph> GraphType;
            exportMulticutObjectiveT<GraphType>(multicutModule);
        }

    }
} // namespace nifty::graph::opt::multicut
} // namespace nifty::graph::opt
}
}


#ifndef NETWORKS_HPP
#define NETWORKS_HPP

// For External Library
#include <torch/torch.h>
#include <boost/program_options.hpp>

// Define Namespace
namespace nn = torch::nn;
namespace po = boost::program_options;

// Function Prototype
void weights_init(nn::Module &m);
void Convolution(nn::Sequential &sq, const size_t in_nc, const size_t out_nc, const size_t ksize, const size_t stride, const size_t pad, const bool BN, const bool LReLU, const bool bias=false);

// -------------------------------------------------
// struct{MC_DiscriminatorImpl}(nn::Module)
// -------------------------------------------------
struct MC_DiscriminatorImpl : nn::Module{
private:
    nn::Sequential features, avgpool, classifier;
public:
    MC_DiscriminatorImpl(){}
    MC_DiscriminatorImpl(po::variables_map &vm);
    void init();
    torch::Tensor forward(torch::Tensor x);
};

TORCH_MODULE(MC_Discriminator);

#endif


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

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

 * See LICENSE for the license information

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

/**
 * @file    testBarometricFactor.cpp
 * @brief   Unit test for BarometricFactor
 * @author  Peter Milani
 * @date   16 Dec, 2021
 */

#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/navigation/BarometricFactor.h>

#include <boost/bind/bind.hpp>

using namespace std::placeholders;
using namespace std;
using namespace gtsam;

// *************************************************************************
namespace example {}

double metersToBaro(const double& meters) {
    double temp = 15.04 - 0.00649 * meters;
    return 101.29 * std::pow(((temp + 273.1) / 288.08), 5.256);
}

// *************************************************************************
TEST(BarometricFactor, Constructor) {
    using namespace example;

    // meters to barometric.

    double baroMeasurement = metersToBaro(10.);

    // Factor
    Key key(1);
    Key key2(2);
    SharedNoiseModel model = noiseModel::Isotropic::Sigma(1, 0.25);
    BarometricFactor factor(key, key2, baroMeasurement, model);

    // Create a linearization point at zero error
    Pose3 T(Rot3::RzRyRx(0., 0., 0.), Point3(0., 0., 10.));
    double baroBias = 0.;
    Vector1 zero;
    zero << 0.;
    EXPECT(assert_equal(zero, factor.evaluateError(T, baroBias), 1e-5));

    // Calculate numerical derivatives
    Matrix expectedH = numericalDerivative21<Vector, Pose3, double>(
        std::bind(&BarometricFactor::evaluateError, &factor,
                  std::placeholders::_1, std::placeholders::_2, boost::none,
                  boost::none),
        T, baroBias);

    Matrix expectedH2 = numericalDerivative22<Vector, Pose3, double>(
        std::bind(&BarometricFactor::evaluateError, &factor,
                  std::placeholders::_1, std::placeholders::_2, boost::none,
                  boost::none),
        T, baroBias);

    // Use the factor to calculate the derivative
    Matrix actualH, actualH2;
    factor.evaluateError(T, baroBias, actualH, actualH2);

    // Verify we get the expected error
    EXPECT(assert_equal(expectedH, actualH, 1e-8));
    EXPECT(assert_equal(expectedH2, actualH2, 1e-8));
}

// *************************************************************************

//***************************************************************************
TEST(BarometricFactor, nonZero) {
    using namespace example;

    // meters to barometric.

    double baroMeasurement = metersToBaro(10.);

    // Factor
    Key key(1);
    Key key2(2);
    SharedNoiseModel model = noiseModel::Isotropic::Sigma(1, 0.25);
    BarometricFactor factor(key, key2, baroMeasurement, model);

    Pose3 T(Rot3::RzRyRx(0.5, 1., 1.), Point3(20., 30., 1.));
    double baroBias = 5.;

    // Calculate numerical derivatives
    Matrix expectedH = numericalDerivative21<Vector, Pose3, double>(
        std::bind(&BarometricFactor::evaluateError, &factor,
                  std::placeholders::_1, std::placeholders::_2, boost::none,
                  boost::none),
        T, baroBias);

    Matrix expectedH2 = numericalDerivative22<Vector, Pose3, double>(
        std::bind(&BarometricFactor::evaluateError, &factor,
                  std::placeholders::_1, std::placeholders::_2, boost::none,
                  boost::none),
        T, baroBias);

    // Use the factor to calculate the derivative and the error
    Matrix actualH, actualH2;
    Vector error = factor.evaluateError(T, baroBias, actualH, actualH2);
    Vector actual = (Vector(1) << -4.0).finished();

    // Verify we get the expected error
    EXPECT(assert_equal(expectedH, actualH, 1e-8));
    EXPECT(assert_equal(expectedH2, actualH2, 1e-8));
    EXPECT(assert_equal(error, actual, 1e-8));
}

// *************************************************************************
int main() {
    TestResult tr;
    return TestRegistry::runAllTests(tr);
}
// *************************************************************************


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/config.hpp>
#include <boost/simd/detail/brigand.hpp>
#include <boost/simd/detail/dispatch.hpp>
#include <boost/simd/detail/constant_traits.hpp>
#include <boost/simd/detail/dispatch/function/make_callable.hpp>
#include <boost/simd/detail/dispatch/hierarchy/functions.hpp>
#include <boost/simd/detail/dispatch/as.hpp>

/*

    @ingroup group-constant

    This constant is such that, for pairs of types (Tup, T)
    (namely (float,  double) and (double, long double)) the sum:

    abs(Tup(Pio_4lo<T>())+Tup(Pio_4<T>())-Pio_4\<Tup\>())  is  less than
    a few Eps<Tup>().

    This is used to improve accurracy when computing sums of the kind
    Pio_4 + x with x small,  by replacing them by Pio_4 + (Pio_4lo+x)

    @par Semantic:

    For type T:

    @code
    T r = Pio_4lo<T>();
    @endcode

    is similar to:

    @code
    if T is double
      r = 3.061616997868383e-17
    else if T is float
      r = -2.1855694e-08
    @endcode

    @return a value of type T

*/

namespace boost { namespace simd
{
  namespace tag
  {
    struct pio_4lo_ : boost::dispatch::constant_value_<pio_4lo_>
    {
      BOOST_DISPATCH_MAKE_CALLABLE(ext,pio_4lo_,boost::dispatch::constant_value_<pio_4lo_>);
      BOOST_SIMD_REGISTER_CONSTANT(0, 0XB2BBBD2EUL, 0X3C81A62633145C07ULL);
    };
  }

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag,pio_4lo_);
  }

  namespace detail
  {
    BOOST_DISPATCH_CALLABLE_DEFINITION(tag::pio_4lo_,pio_4lo);
  }

  template<typename T> BOOST_FORCEINLINE auto Pio_4lo()
  BOOST_NOEXCEPT_DECLTYPE(detail::pio_4lo( boost::dispatch::as_<T>{}))
  {
    return detail::pio_4lo( boost::dispatch::as_<T>{} );
  }
} }

#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


// Copyright 2017 Nicolas Mellado
//
// 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.
//
// -------------------------------------------------------------------------- //
//
// Authors: Dror Aiger, Yoni Weill, Nicolas Mellado
//
// This file is part of the implementation of the 4-points Congruent Sets (4PCS)
// algorithm presented in:
//
// 4-points Congruent Sets for Robust Surface Registration
// Dror Aiger, Niloy J. Mitra, Daniel Cohen-Or
// ACM SIGGRAPH 2008 and ACM Transaction of Graphics.
//
// Given two sets of points in 3-space, P and Q, the algorithm applies RANSAC
// in roughly O(n^2) time instead of O(n^3) for standard RANSAC, using an
// efficient method based on invariants, to find the set of all 4-points in Q
// that can be matched by rigid transformation to a given set of 4-points in P
// called a base. This avoids the need to examine all sets of 3-points in Q
// against any base of 3-points in P as in standard RANSAC.
// The algorithm can use colors and normals to speed-up the matching
// and to improve the quality. It can be easily extended to affine/similarity
// transformation but then the speed-up is smaller because of the large number
// of congruent sets. The algorithm can also limit the range of transformations
// when the application knows something on the initial pose but this is not
// necessary in general (though can speed the runtime significantly).

// Home page of the 4PCS project (containing the paper, presentations and a
// demo): http://graphics.stanford.edu/~niloy/research/fpcs/fpcs_sig_08.html
// Use google search on "4-points congruent sets" to see many related papers
// and applications.

#ifndef _SUPER4PCS_ALGO_MATCH_4PCS_BASE_IMPL_
#define _SUPER4PCS_ALGO_MATCH_4PCS_BASE_IMPL_

#ifndef _SUPER4PCS_ALGO_MATCH_4PCS_BASE_
#include "super4pcs/algorithms/match4pcsBase.h"
#endif

#include <chrono>
#include <atomic>

#include <Eigen/Geometry>                 // MatrixBase.homogeneous()
#include <Eigen/SVD>                      // Transform.computeRotationScaling()

namespace GlobalRegistration{

// The main 4PCS function. Computes the best rigid transformation and transfoms
// Q toward P by this transformation.
template <typename Sampler, typename Visitor>
Match4PCSBase::Scalar
Match4PCSBase::ComputeTransformation(const std::vector<Point3D>& P,
                                     std::vector<Point3D>* Q,
                                     Eigen::Ref<MatrixType> transformation,
                                     const Sampler& sampler,
                                     const Visitor& v) {

  if (Q == nullptr) return kLargeNumber;
  if (P.empty() || Q->empty()) return kLargeNumber;

  init(P, *Q, sampler);

  if (best_LCP_ != Scalar(1.))
    Perform_N_steps(number_of_trials_, transformation, Q, v);

#ifdef TEST_GLOBAL_TIMINGS
  Log<LogLevel::Verbose>( "----------- Timings (msec) -------------" );
  Log<LogLevel::Verbose>( " Total computation time  : ", totalTime   );
  Log<LogLevel::Verbose>( " Total verify time       : ", verifyTime  );
  Log<LogLevel::Verbose>( "    Kdtree query         : ", kdTreeTime  );
  Log<LogLevel::Verbose>( "----------------------------------------" );
#endif

  return best_LCP_;
}

template <typename Sampler>
void Match4PCSBase::init(const std::vector<Point3D>& P,
                         const std::vector<Point3D>& Q,
                         const Sampler& sampler){

#ifdef TEST_GLOBAL_TIMINGS
    kdTreeTime = 0;
    totalTime  = 0;
    verifyTime = 0;
#endif

    const Scalar kSmallError = 0.00001;
    const int kMinNumberOfTrials = 4;
    const Scalar kDiameterFraction = 0.3;

    centroid_P_ = VectorType::Zero();
    centroid_Q_ = VectorType::Zero();

    sampled_P_3D_.clear();
    sampled_Q_3D_.clear();

    // prepare P
    if (P.size() > options_.sample_size){
        sampler(P, options_, sampled_P_3D_);
    }
    else
    {
        Log<LogLevel::ErrorReport>( "(P) More samples requested than available: use whole cloud" );
        sampled_P_3D_ = P;
    }

    // prepare Q
    if (Q.size() > options_.sample_size){
        std::vector<Point3D> uniform_Q;
        sampler(Q, options_, uniform_Q);

        std::shuffle(uniform_Q.begin(), uniform_Q.end(), randomGenerator_);
        size_t nbSamples = std::min(uniform_Q.size(), options_.sample_size);
        auto endit = uniform_Q.begin(); std::advance(endit, nbSamples );
        std::copy(uniform_Q.begin(), endit, std::back_inserter(sampled_Q_3D_));
    }
    else
    {
        Log<LogLevel::ErrorReport>( "(Q) More samples requested than available: use whole cloud" );
        sampled_Q_3D_ = Q;
    }

    // center points around centroids
    auto centerPoints = [](std::vector<Point3D>&container,
            VectorType& centroid){
        for(const auto& p : container) centroid += p.pos();
        centroid /= Scalar(container.size());
        for(auto& p : container) p.pos() -= centroid;
    };
    centerPoints(sampled_P_3D_, centroid_P_);
    centerPoints(sampled_Q_3D_, centroid_Q_);

    initKdTree();
    // Compute the diameter of P approximately (randomly). This is far from being
    // Guaranteed close to the diameter but gives good results for most common
    // objects if they are densely sampled.
    P_diameter_ = 0.0;
    for (int i = 0; i < kNumberOfDiameterTrials; ++i) {
        int at = randomGenerator_() % sampled_Q_3D_.size();
        int bt = randomGenerator_() % sampled_Q_3D_.size();

        Scalar l = (sampled_Q_3D_[bt].pos() - sampled_Q_3D_[at].pos()).norm();
        if (l > P_diameter_) {
            P_diameter_ = l;
        }
    }

    // Mean distance and a bit more... We increase the estimation to allow for
    // noise, wrong estimation and non-uniform sampling.
    P_mean_distance_ = MeanDistance();

    // Normalize the delta (See the paper) and the maximum base distance.
    // delta = P_mean_distance_ * delta;
    max_base_diameter_ = P_diameter_;  // * estimated_overlap_;

    // RANSAC probability and number of needed trials.
    Scalar first_estimation =
            std::log(kSmallError) / std::log(1.0 - pow(options_.getOverlapEstimation(),
                                             static_cast<Scalar>(kMinNumberOfTrials)));
    // We use a simple heuristic to elevate the probability to a reasonable value
    // given that we don't simply sample from P, but instead, we bound the
    // distance between the points in the base as a fraction of the diameter.
    number_of_trials_ =
            static_cast<int>(first_estimation * (P_diameter_ / kDiameterFraction) /
                             max_base_diameter_);
    if (number_of_trials_ < kMinNumberOfTrials)
        number_of_trials_ = kMinNumberOfTrials;

    Log<LogLevel::Verbose>( "norm_max_dist: ", options_.delta );
    current_trial_ = 0;
    best_LCP_ = 0.0;

    Q_copy_ = Q;
    for (int i = 0; i < 4; ++i) {
        base_[i] = 0;
        current_congruent_[i] = 0;
    }
    transform_ = Eigen::Matrix<Scalar, 4, 4>::Identity();

    // call Virtual handler
    Initialize(P, Q);

    best_LCP_ = Verify(transform_);
    Log<LogLevel::Verbose>( "Initial LCP: ", best_LCP_ );
}

// Performs N RANSAC iterations and compute the best transformation. Also,
// transforms the set Q by this optimal transformation.
template <typename Visitor>
bool
Match4PCSBase::Perform_N_steps(int n,
                               Eigen::Ref<MatrixType> transformation,
                               std::vector<Point3D>* Q,
                               const Visitor &v) {
  using std::chrono::system_clock;
  if (Q == nullptr) return false;

#ifdef TEST_GLOBAL_TIMINGS
    Timer t (true);
#endif

  // The transformation has been computed between the two point clouds centered
  // at the origin, we need to recompute the translation to apply it to the original clouds
  auto getGlobalTransform = [this](Eigen::Ref<MatrixType> transformation){
    Eigen::Matrix<Scalar, 3, 3> rot, scale;
    Eigen::Transform<Scalar, 3, Eigen::Affine> (transform_).computeRotationScaling(&rot, &scale);
    transformation = transform_;
    transformation.col(3) = (qcentroid1_ + centroid_P_ - ( rot * scale * (qcentroid2_ + centroid_Q_))).homogeneous();
  };

  Scalar last_best_LCP = best_LCP_;
  v(0, best_LCP_, transformation);

  bool ok = false;
  std::chrono::time_point<system_clock> t0 = system_clock::now(), end;
  for (int i = current_trial_; i < current_trial_ + n; ++i) {
    ok = TryOneBase(v);

    Scalar fraction_try  = Scalar(i) / Scalar(number_of_trials_);
    Scalar fraction_time =
        std::chrono::duration_cast<std::chrono::seconds>
        (system_clock::now() - t0).count() /
                          options_.max_time_seconds;
    Scalar fraction = std::max(fraction_time, fraction_try);

    if (v.needsGlobalTransformation()) {
      getGlobalTransform(transformation);
    } else {
      transformation = transform_;
    }

    v(fraction, best_LCP_, transformation);

    // ok means that we already have the desired LCP.
    if (ok || i > number_of_trials_ || fraction >= 0.99 || best_LCP_ == 1.0) break;
  }

  current_trial_ += n;
  if (best_LCP_ > last_best_LCP) {
    *Q = Q_copy_;

    getGlobalTransform(transformation);

    // Transforms Q by the new transformation.
    for (size_t i = 0; i < Q->size(); ++i) {
      (*Q)[i].pos() = (transformation * (*Q)[i].pos().homogeneous()).head<3>();
    }
  }
#ifdef TEST_GLOBAL_TIMINGS
    totalTime += Scalar(t.elapsed().count()) / Scalar(CLOCKS_PER_SEC);
#endif

  return ok || current_trial_ >= number_of_trials_;
}

// Pick one base, finds congruent 4-points in Q, verifies for all
// transformations, and retains the best transformation and LCP. This is
// a complete RANSAC iteration.
template<typename Visitor>
bool Match4PCSBase::TryOneBase(const Visitor &v) {
  Scalar invariant1, invariant2;
  int base_id1, base_id2, base_id3, base_id4;

//#define STATIC_BASE

#ifdef STATIC_BASE
  static bool first_time = true;

  if (first_time){
      base_id1 = 0;
      base_id2 = 3;
      base_id3 = 1;
      base_id4 = 4;

      base_3D_[0] = sampled_P_3D_ [base_id1];
      base_3D_[1] = sampled_P_3D_ [base_id2];
      base_3D_[2] = sampled_P_3D_ [base_id3];
      base_3D_[3] = sampled_P_3D_ [base_id4];

      TryQuadrilateral(&invariant1, &invariant2, base_id1, base_id2, base_id3, base_id4);

      first_time = false;
  }
  else
      return false;

#else

  if (!SelectQuadrilateral(invariant1, invariant2, base_id1, base_id2,
                           base_id3, base_id4)) {
    return false;
  }
#endif

  // Computes distance between pairs.
  const Scalar distance1 = (base_3D_[0].pos()- base_3D_[1].pos()).norm();
  const Scalar distance2 = (base_3D_[2].pos()- base_3D_[3].pos()).norm();

  std::vector<std::pair<int, int>> pairs1, pairs2;
  std::vector<Quadrilateral> congruent_quads;

  // Compute normal angles.
  const Scalar normal_angle1 = (base_3D_[0].normal() - base_3D_[1].normal()).norm();
  const Scalar normal_angle2 = (base_3D_[2].normal() - base_3D_[3].normal()).norm();

  ExtractPairs(distance1, normal_angle1, distance_factor * options_.delta, 0,
                  1, &pairs1);
  ExtractPairs(distance2, normal_angle2, distance_factor * options_.delta, 2,
                  3, &pairs2);

//  Log<LogLevel::Verbose>( "Pair creation ouput: ", pairs1.size(), " - ", pairs2.size());

  if (pairs1.size() == 0 || pairs2.size() == 0) {
    return false;
  }

  if (!FindCongruentQuadrilaterals(invariant1, invariant2,
                                   distance_factor * options_.delta,
                                   distance_factor * options_.delta,
                                   pairs1,
                                   pairs2,
                                   &congruent_quads)) {
    return false;
  }

  size_t nb = 0;

  bool match = TryCongruentSet(base_id1, base_id2, base_id3, base_id4,
                               congruent_quads,
                               v,
                               nb);

  //if (nb != 0)
  //  Log<LogLevel::Verbose>( "Congruent quads: (", nb, ")    " );

  return match;
}

template <typename Visitor>
bool Match4PCSBase::TryCongruentSet(
        int base_id1,
        int base_id2,
        int base_id3,
        int base_id4,
        const std::vector<Quadrilateral>& congruent_quads,
        const Visitor& v,
        size_t &nbCongruent){
    static const double pi = std::acos(-1);

    // get references to the basis coordinates
    const Point3D& b1 = sampled_P_3D_[base_id1];
    const Point3D& b2 = sampled_P_3D_[base_id2];
    const Point3D& b3 = sampled_P_3D_[base_id3];
    const Point3D& b4 = sampled_P_3D_[base_id4];

    // set the basis coordinates in the congruent quad array
    const std::array<Point3D, 4> congruent_base {{b1, b2, b3, b4}};

    // Centroid of the basis, computed once and using only the three first points
    Eigen::Matrix<Scalar, 3, 1> centroid1 = (b1.pos() + b2.pos() + b3.pos()) / Scalar(3);

    std::atomic<size_t> nbCongruentAto(0);

#ifdef SUPER4PCS_USE_OPENMP
#pragma omp parallel for num_threads(omp_nthread_congruent_)
#endif
    for (int i = 0; i < int(congruent_quads.size()); ++i) {
      std::array<Point3D, 4> congruent_candidate;

      Eigen::Matrix<Scalar, 4, 4> transform;

      // Centroid of the sets, computed in the loop using only the three first points
      Eigen::Matrix<Scalar, 3, 1> centroid2;

      const int a = congruent_quads[i].vertices[0];
      const int b = congruent_quads[i].vertices[1];
      const int c = congruent_quads[i].vertices[2];
      const int d = congruent_quads[i].vertices[3];
      congruent_candidate[0] = sampled_Q_3D_[a];
      congruent_candidate[1] = sampled_Q_3D_[b];
      congruent_candidate[2] = sampled_Q_3D_[c];
      congruent_candidate[3] = sampled_Q_3D_[d];

  #ifdef STATIC_BASE
      Log<LogLevel::Verbose>( "Ids: ", base_id1, "\t", base_id2, "\t", base_id3, "\t", base_id4);
      Log<LogLevel::Verbose>( "     ", a, "\t", b, "\t", c, "\t", d);
  #endif

      centroid2 = (congruent_candidate[0].pos() +
                   congruent_candidate[1].pos() +
                   congruent_candidate[2].pos()) / Scalar(3.);

      Scalar rms = -1;

      const bool ok =
      ComputeRigidTransformation(congruent_base,     // input congruent quad
                                 congruent_candidate,// tested congruent quad
                                 centroid1,          // input: basis centroid
                                 centroid2,          // input: candidate quad centroid
                                 options_.max_angle * pi / 180.0, // maximum per-dimension angle, check return value to detect invalid cases
                                 transform,          // output: transformation
                                 rms,                // output: rms error of the transformation between the basis and the congruent quad
                             #ifdef MULTISCALE
                                 true
                             #else
                                 false
                             #endif
                                 );             // state: compute scale ratio ?

      if (ok && rms >= Scalar(0.)) {

        // We give more tolerant in computing the best rigid transformation.
        if (rms < distance_factor * options_.delta) {

          nbCongruentAto++;
          // The transformation is computed from the point-clouds centered inn [0,0,0]

          // Verify the rest of the points in Q against P.
          Scalar lcp = Verify(transform);

          // transformation has been computed between the two point clouds centered
          // at the origin, we need to recompute the translation to apply it to the original clouds
          auto getGlobalTransform =
              [this, transform, centroid1, centroid2]
              (Eigen::Ref<MatrixType> transformation){
            Eigen::Matrix<Scalar, 3, 3> rot, scale;
            Eigen::Transform<Scalar, 3, Eigen::Affine> (transform).computeRotationScaling(&rot, &scale);
            transformation = transform;
            transformation.col(3) = (centroid1 + centroid_P_ - ( rot * scale * (centroid2 + centroid_Q_))).homogeneous();
          };

          if (v.needsGlobalTransformation())
          {
            Eigen::Matrix<Scalar, 4, 4> transformation = transform;
            getGlobalTransform(transformation);
            v(-1, lcp, transformation);
          }
          else
            v(-1, lcp, transform);

#pragma omp critical
          if (lcp > best_LCP_) {
            // Retain the best LCP and transformation.
            base_[0] = base_id1;
            base_[1] = base_id2;
            base_[2] = base_id3;
            base_[3] = base_id4;

            current_congruent_[0] = a;
            current_congruent_[1] = b;
            current_congruent_[2] = c;
            current_congruent_[3] = d;

            best_LCP_    = lcp;
            transform_   = transform;
            qcentroid1_  = centroid1;
            qcentroid2_  = centroid2;
          }
          // Terminate if we have the desired LCP already.
          if (best_LCP_ > options_.getTerminateThreshold()){
            continue;
          }
        }
      }
    }

    nbCongruent = nbCongruentAto;

    // If we reached here we do not have yet the desired LCP.
    return best_LCP_ > options_.getTerminateThreshold() /*false*/;
}

} // namespace Super4PCS

#endif


// Copyright Abel Sinkovics (abel@sinkovics.hu) 2011.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <mpllibs/metamonad/state.hpp>
#include <mpllibs/metamonad/tmp_value.hpp>
#include <mpllibs/metamonad/lazy.hpp>
#include <mpllibs/metamonad/lambda_c.hpp>
#include <mpllibs/metamonad/name.hpp>
#include <mpllibs/metamonad/pair.hpp>
#include <mpllibs/metamonad/apply.hpp>
#include <mpllibs/metamonad/monad.hpp>

#include <boost/test/unit_test.hpp>

#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/quote.hpp>
#include <boost/mpl/assert.hpp>

#include "common.hpp"

using boost::mpl::plus;

using mpllibs::metamonad::tmp_value;
using mpllibs::metamonad::lazy;
using mpllibs::metamonad::lambda_c;
using mpllibs::metamonad::pair;

using namespace mpllibs::metamonad::name;

namespace
{
  MPLLIBS_METAFUNCTION(plusn, (N))
  ((lambda_c<a, b, lazy<pair<plus<a, N>, plus<b, N> > > >));
}

BOOST_AUTO_TEST_CASE(test_state)
{
  using mpllibs::metamonad::state_tag;
  using mpllibs::metamonad::monad;
  using mpllibs::metamonad::apply;

  using boost::mpl::equal_to;
  using boost::mpl::int_;
  using boost::mpl::quote1;

  typedef int_<24> int24;
  typedef int_<14> int14;
  typedef apply<monad<state_tag>::return_, int11>::type return11;

  // test_return_value
  BOOST_MPL_ASSERT((equal_to<int11, apply<return11, int13>::type::first>));

  // test_return_state
  BOOST_MPL_ASSERT((equal_to<int13, apply<return11, int13>::type::second>));

  // test_bind_value
  BOOST_MPL_ASSERT((
    equal_to<
      int24,
      apply<
        apply<monad<state_tag>::bind, return11, plusn<int13> >,
        int1
      >::type::first
    >
  ));

  // test_bind_state
  BOOST_MPL_ASSERT((
    equal_to<
      int14,
      apply<
        apply<monad<state_tag>::bind, return11, plusn<int13> >,
        int1
      >::type::second
    >
  ));
}


#include <Core/Animation/PoseOperation.hpp>
#include <Eigen/Geometry>

namespace Ra {
namespace Core {
namespace Animation {

bool compatible( const Pose& p0, const Pose& p1 ) {
    return ( p0.size() == p1.size() );
}

Pose relativePose( const Pose& modelPose, const RestPose& restPose ) {
    CORE_ASSERT( compatible( modelPose, restPose ), " Poses with different size " );
    Pose T( restPose.size() );
#pragma omp parallel for
    for ( int i = 0; i < int( T.size() ); ++i )
    {
        T[i] = modelPose[i] * restPose[i].inverse( Eigen::Affine );
    }
    return T;
}

Pose applyTransformation( const Pose& pose, const Container::AlignedStdVector<Math::Transform>& transform ) {
    Pose T( std::min( pose.size(), transform.size() ) );
#pragma omp parallel for
    for ( int i = 0; i < int( T.size() ); ++i )
    {
        T[i] = transform[i] * pose[i];
    }
    return T;
}

Pose applyTransformation( const Pose& pose, const Math::Transform& transform ) {
    Pose T( pose.size() );
#pragma omp parallel for
    for ( int i = 0; i < int( T.size() ); ++i )
    {
        T[i] = transform * pose[i];
    }
    return T;
}

bool areEqual( const Pose& p0, const Pose& p1 ) {
    CORE_ASSERT( compatible( p0, p1 ), " Poses with different size " );
    const uint n = p0.size();
    for ( uint i = 0; i < n; ++i )
    {
        if ( !p0[i].isApprox( p1[i] ) )
        {
            return false;
        }
    }
    return true;
}

Pose interpolatePoses( const Pose& a, const Pose& b, const Scalar t ) {
    CORE_ASSERT( ( a.size() == b.size() ), "Poses are wrong" );
    CORE_ASSERT( ( ( t >= 0.0 ) && ( t <= 1.0 ) ), "T is wrong" );

    const uint size = a.size();
    Pose interpolatedPose( size );

#pragma omp parallel for
    for ( int i = 0; i < int( size ); ++i )
    {
        // interpolate between the transforms
        Math::Transform aTransform = a[i];
        Math::Transform bTransform = b[i];

        Math::Quaternion aRot = Math::Quaternion( aTransform.rotation() );
        Math::Quaternion bRot = Math::Quaternion( bTransform.rotation() );
        Math::Quaternion interpRot = aRot.slerp( t, bRot );

        Math::Vector3 interpTranslation =
            ( 1.0 - t ) * aTransform.translation() + t * bTransform.translation();

        Math::Transform interpolatedTransform;
        interpolatedTransform.linear() = interpRot.toRotationMatrix();
        interpolatedTransform.translation() = interpTranslation;

        interpolatedPose[i] = interpolatedTransform;
    }

    return interpolatedPose;
}

void interpolateTransforms( const Math::Transform& a, const Math::Transform& b,
                            const Scalar t, Math::Transform& interpolated ) {
    Math::Quaternion aRot = Math::Quaternion( a.rotation() );
    Math::Quaternion bRot = Math::Quaternion( b.rotation() );
    Math::Quaternion interpRot = aRot.slerp( t, bRot );

    Math::Vector3 interpTranslation = ( 1.0 - t ) * a.translation() + t * b.translation();

    interpolated.linear() = interpRot.toRotationMatrix();
    interpolated.translation() = interpTranslation;
}

} // namespace Animation
} // namespace Core
} // namespace Ra


// This is an open source non-commercial project. Dear PVS-Studio, please check it.
// PVS-Studio Static Code Analyzer for C, C++ and C#: http://www.viva64.com
#include "model.h"

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

using neuon::model_t;

model_t::model_t(std::shared_ptr<tensorflow::api_t> api, const std::vector<uint8_t>& model_payload, const normalization_t& normalization )
: api(api)
, graph(api->TF_NewGraph())
, normalization(normalization){

    tensorflow::buffer_ptr buf(api->TF_NewBufferFromString(model_payload.data(), model_payload.size()));

    tensorflow::import_graph_def_options_ptr opts( api->TF_NewImportGraphDefOptions());

    tensorflow::status_ptr status( api->TF_NewStatus());

    api->TF_GraphImportGraphDef(graph, buf, opts, status);

    tensorflow::session_options_ptr session_opts(api->TF_NewSessionOptions());

    session = api->TF_NewSession(graph, session_opts, status);
}

model_t::~model_t() = default;

std::vector<float> model_t::predict(const dlib::matrix<uint8_t> &x_video, const std::vector<int64_t>& video_dims, const dlib::matrix<double> &x_audio, const std::vector<int64_t>& audio_dims ) {
    static const std::string input_layer_video_name = "input_1";
    static const std::string input_layer_audio_name = "input_2";
    static const std::string output_layer_name = "lambda_1/Sqrt";

    std::vector<TF_Output> inputs{{api->TF_GraphOperationByName(graph, input_layer_video_name.c_str()), 0}, {api->TF_GraphOperationByName(graph, input_layer_audio_name.c_str()), 0}};
    TF_Output output = {api->TF_GraphOperationByName(graph, output_layer_name.c_str()), 0};

    auto video_tensor = api->TF_AllocateTensor(TF_FLOAT, video_dims.data(), static_cast<int>(video_dims.size()), x_video.size() *  sizeof(float));
    boost::numeric::ublas::tensor<float> r_video(boost::numeric::ublas::shape({static_cast<size_t>(x_video.nr()), static_cast<size_t>(x_video.nc())}));
    std::copy(x_video.begin(), x_video.end(), r_video.begin());
    r_video.reshape({static_cast<size_t>(video_dims[3]),  static_cast<size_t>(video_dims[2]), static_cast<size_t>(video_dims[1]) });
    r_video -= normalization.video.mean;
    r_video /= normalization.video.std;

    std::copy(r_video.begin(), r_video.end(), reinterpret_cast<float*>(api->TF_TensorData(video_tensor)));

    auto audio_tensor = api->TF_AllocateTensor(TF_FLOAT, audio_dims.data(), static_cast<int>(audio_dims.size()), x_audio.size() *  sizeof(float));
    boost::numeric::ublas::tensor<float> r_audio(boost::numeric::ublas::shape({static_cast<size_t>(x_audio.nr()), static_cast<size_t>(x_audio.nc())}));
    std::copy(x_audio.begin(), x_audio.end(), r_audio.begin());
    r_audio.reshape({static_cast<size_t>(audio_dims[3]), static_cast<size_t>(audio_dims[2]), static_cast<size_t>(audio_dims[1])});
    r_audio -= normalization.audio.mean;
    r_audio /= normalization.audio.std;

    std::copy(r_audio.begin(), r_audio.end(), reinterpret_cast<float*>(api->TF_TensorData(audio_tensor)));

    std::vector<tensorflow::tensor_ptr> input_tensors;
    input_tensors.emplace_back(std::move(video_tensor));
    input_tensors.emplace_back(std::move(audio_tensor));

    std::vector<int64_t> dims(1, 1);
    tensorflow:: tensor_ptr output_tensor = api->TF_AllocateTensor(TF_FLOAT, dims.data(), dims.size(), sizeof(float));
    *reinterpret_cast<float*>(api->TF_TensorData(output_tensor)) = 0.0f;

    tensorflow::status_ptr status( api->TF_NewStatus() );
    tensorflow::buffer_ptr run_metadata;
    api->TF_SessionRun(session, nullptr,
                       inputs.data(), input_tensors.data(), inputs.size(),
                       &output, &output_tensor, 1,
                       nullptr, 0,
                       run_metadata,
                       status);

    std::vector<float> prediction(reinterpret_cast<float*>(api->TF_TensorData(output_tensor)), reinterpret_cast<float*>(api->TF_TensorData(output_tensor)) + api->TF_TensorElementCount(output_tensor));
    return prediction;
}


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

  @copyright 2016 NumScale SAS

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

**/
//==================================================================================================
#ifndef BOOST_SIMD_REDUCTION_HPP_INCLUDED
#define BOOST_SIMD_REDUCTION_HPP_INCLUDED

namespace boost {
namespace simd {
/*!
  @ingroup group-functions
  @defgroup group-reduction Reduction Functions

  These functions provide algorithms for in-register reduction and prefix-scan operations.

  They are usually only defined for **SIMD** types but may, in some
  cases such as @ref sum or  @ref prod, have a valid scalar semantic.

    <center>
|                      |                         |                    |                            |
|:--------------------:|:-----------------------:|:------------------:|:--------------------------:|
| @ref all             | @ref any                | @ref compare_equal | @ref compare_greater_equal |
| @ref compare_greater | @ref compare_less_equal | @ref compare_less  | @ref compare_not_equal     |
| @ref cummax          | @ref cummin             | @ref cumprod       | @ref cumsum                |
| @ref dot             | @ref hmsb               | @ref isincluded    | @ref isincluded_c          |
| @ref maximum         | @ref minimum            | @ref nbtrue        | @ref none                  |
| @ref prod            | @ref sum                |                    |                            |
     </center>
**/
}
}

#include <boost/simd/function/all.hpp>
#include <boost/simd/function/any.hpp>
#include <boost/simd/function/compare_equal.hpp>
#include <boost/simd/function/compare_greater.hpp>
#include <boost/simd/function/compare_greater_equal.hpp>
#include <boost/simd/function/compare_less.hpp>
#include <boost/simd/function/compare_less_equal.hpp>
#include <boost/simd/function/compare_not_equal.hpp>
#include <boost/simd/function/cummax.hpp>
#include <boost/simd/function/cummin.hpp>
#include <boost/simd/function/cumprod.hpp>
#include <boost/simd/function/cumsum.hpp>
#include <boost/simd/function/dot.hpp>
#include <boost/simd/function/hmsb.hpp>
#include <boost/simd/function/isincluded.hpp>
#include <boost/simd/function/isincluded_c.hpp>
#include <boost/simd/function/maximum.hpp>
#include <boost/simd/function/minimum.hpp>
#include <boost/simd/function/nbtrue.hpp>
#include <boost/simd/function/none.hpp>
#include <boost/simd/function/prod.hpp>
#include <boost/simd/function/sum.hpp>

#endif


#ifndef QuadProgMm_hpp
#define QuadProgMm_hpp

// Standard library
#include <vector>
#include <map>
#include <set>

// Boost
#include <boost/operators.hpp>

namespace QuadProgMm {
  /*!
    The representation of a variable usable in QP problems.

    It is immutable: a Variable instance represents the same variable from its construction to its destruction.
  */
  struct Variable {
    /*!
      Construct a new variable.

      It represents a different variable from all \ref Variable "Variables" created before.
    */
    Variable();

    /*!
      Construct a copy of an existing Variable.

      It represents the same variable as the original and is usable interchangably with the original everywhere.
    */
    Variable(const Variable&) = default;
    Variable& operator = (const Variable&) = delete;
    Variable& operator = (const Variable&&) = delete;

    /*!
      Strict comparison operator, to be able to use \ref Variable "Variables" in std::maps for example.
    */
    bool operator < (const Variable& other) const {return id < other.id;}

    private:
      int id;
      static int nextId;
  };

  /*!
    A polynomial of \ref Variable "Variables" of degree 0 or 1.
  */
  struct LinearExpression :
    boost::addable<LinearExpression>,
    boost::subtractable<LinearExpression>,
    boost::multipliable<LinearExpression, double>,
    boost::dividable<LinearExpression, double>
  {
    /*!
      Construct a polynomial of degree 0, equal to the parameter.
    */
    LinearExpression(double = 0);

    /*!
      Construct a single-variable, unitary, polynomial of degree 1.
    */
    LinearExpression(const Variable&);

    /*!
      Add another LinearExpression to the current one.
    */
    LinearExpression& operator += (const LinearExpression&);

    /*!
      Substract another LinearExpression to the current one.
    */
    LinearExpression& operator -= (const LinearExpression&);

    /*!
      Multiply the current LinearExpression by a constant.
    */
    LinearExpression& operator *= (double);

    /*!
      Divide the current LinearExpression by a constant.
    */
    LinearExpression& operator /= (double);

    // Not part of the public interface of the library
    std::set<Variable> getVariables() const;
    double getLinearCoefficient(const Variable&) const;
    double getConstantCoefficient() const;

    private:
      LinearExpression(const std::map<Variable, double>&, double);
      std::map<Variable, double> linearCoefficients;
      double constantCoefficient;
      friend class QuadraticExpression;
  };

  /*!
    A polynomial of \ref Variable "Variables" of degree 0, 1, or 2.
  */
  struct QuadraticExpression :
    boost::addable<QuadraticExpression>,
    boost::subtractable<QuadraticExpression>,
    boost::multipliable<QuadraticExpression, double>,
    boost::dividable<QuadraticExpression, double>
  {
    /*!
      Construct a polynomial of degree 0, equal to the parameter.
    */
    QuadraticExpression(double = 0);

    /*!
      Construct a single-variable, unitary, polynomial of degree 1.
    */
    QuadraticExpression(const Variable&);

    /*!
      Construct a polynomial of degree 0 or 1, equal to the parameter.
    */
    QuadraticExpression(const LinearExpression&);

    /*!
      Add another QuadraticExpression to the current one.
    */
    QuadraticExpression& operator +=(const QuadraticExpression&);

    /*!
      Substract another QuadraticExpression to the current one.
    */
    QuadraticExpression& operator -=(const QuadraticExpression&);

    /*!
      Multiply the current QuadraticExpression by a constant.
    */
    QuadraticExpression& operator *=(double);

    /*!
      Divide the current QuadraticExpression by a constant.
    */
    QuadraticExpression& operator /=(double);

    // Not part of the public interface of the library
    std::set<Variable> getVariables() const;
    double getQuadraticCoefficient(const Variable&, const Variable&) const;
    double getLinearCoefficient(const Variable&) const;
    double getConstantCoefficient() const;

    static QuadraticExpression multiply(const LinearExpression&, const LinearExpression&);

    private:
      QuadraticExpression(const std::map<std::pair<Variable, Variable>, double>&, const std::map<Variable, double>&, double);
      std::map<std::pair<Variable, Variable>, double> quadraticCoefficients;
      std::map<Variable, double> linearCoefficients;
      double constantCoefficient;
  };

  /*!
    A constraint.
  */
  // Contents not part of the public interface of the library
  struct Constraint {
    enum Type {ZERO, POSITIVE};

    Constraint(const LinearExpression&, Type);

    const LinearExpression& getLinearExpression() const;
    Type getType() const;

    private:
      LinearExpression linearExpression;
      Type type;
  };
}

namespace QuadProgMm {
  /**
   * \defgroup arithmetic_operators Arithmetic operators of the DLS
   * @{
   */
  LinearExpression operator + (const Variable&);
  LinearExpression operator + (const LinearExpression&);
  QuadraticExpression operator + (const QuadraticExpression&);

  LinearExpression operator - (const Variable&);
  LinearExpression operator - (const LinearExpression&);
  QuadraticExpression operator - (const QuadraticExpression&);

  LinearExpression operator + (double, const Variable&);
  LinearExpression operator + (const Variable&, double);
  LinearExpression operator + (const Variable&, const Variable&);

  LinearExpression operator - (double, const Variable&);
  LinearExpression operator - (const Variable&, double);
  LinearExpression operator - (const Variable&, const Variable&);

  LinearExpression operator * (double, const Variable&);
  LinearExpression operator * (const Variable&, double);

  QuadraticExpression operator * (const LinearExpression&, const LinearExpression&);

  LinearExpression operator / (const Variable&, double);
  /**@}*/

  /**
   * \defgroup comparison_operators Comparison operators of the DLS
   * @{
   */
  Constraint operator == (const LinearExpression&, const LinearExpression&);
  Constraint operator >= (const LinearExpression&, const LinearExpression&);
  Constraint operator <= (const LinearExpression&, const LinearExpression&);
  /**@}*/
}

namespace QuadProgMm {
  /*!
    The result of an optimization.

    Returned by \ref minimize and \ref maximize.
  */
  struct Solution {
    /*!
      Get the optimal value of the QuadraticExpression.

      This is the minimal value of the QuadraticExpression passed to \ref minimize and its maximum value when using \ref maximize.
    */
    double getCost() const {return cost;}

    /*!
      Get the value a variable must take to reach the optimum.

      Behavior is undefined when called with a variable that was not part of the QuadraticExpression optimized.
    */
    double get(const Variable& v) const {return values.find(v)->second;}

    public:
      // Not part of the public interface of the library
      Solution(double cost_, const std::map<Variable, double>& values_) : cost(cost_), values(values_) {}

    private:
      const double cost;
      std::map<Variable, double> values;
  };

  //! Minimize a QuadraticExpression subject to \ref Constraint "Constraints"
  Solution minimize(const QuadraticExpression&, const std::vector<Constraint>& = std::vector<Constraint>());

  //! Maximize a QuadraticExpression subject to \ref Constraint "Constraints"
  Solution maximize(const QuadraticExpression&, const std::vector<Constraint>& = std::vector<Constraint>());
}

#endif // Include guard


/*

Copyright 2019 Emanuele Vespa, Imperial College London

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

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

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

3. Neither the name of the copyright holder nor the names of its contributors
may be used to endorse or promote products derived from this software without
specific prior written permission.

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

*/
#ifndef BALANCING_HPP
#define BALANCING_HPP
#include "se/octree.hpp"
#include "se/node_iterator.hpp"
#include <Eigen/Dense>
#include <vector>
#include <unordered_set>

namespace se {
/*! \brief Enforces 2:1 balance on the tree.
 *  \param octree unbalanced octree to be balanced.
 *
 * */
template <typename T>
void balance(se::Octree<T>& octree) {
  std::unordered_set<key_t> octants;
  std::vector<key_t> alloc_buffer;
  Eigen::Matrix<int, 4, 6> N;
  se::node_iterator<T> it(octree);
  int depth = se::math::log2_const(octree.size());
  while(se::Node<T>* n = it.next()) {
    int depth = se::keyops::depth(n->code());
    if(depth == 0) continue; // skip root
    se::one_neighbourhood(N, se::parent(n->code(), depth),
        depth);
    for(int i = 0; i < 6; ++i) {
      Eigen::Ref<Eigen::Matrix<int, 4, 1>> coords(N.col(i));
      key_t key = octree.hash(coords.x(), coords.y(), coords.z(), depth - 1);
      if(!octree.fetch(coords.x(), coords.y(), coords.z()) &&
          octants.insert(key).second) {
        alloc_buffer.push_back(key);
      }
    }
  }

  int last = 0;
  int end = alloc_buffer.size();
  while(end - last > 0) {
    for(; last < end; ++last) {
      se::one_neighbourhood(N, se::parent(alloc_buffer[last], depth),
          depth);
      int depth = se::keyops::depth(alloc_buffer[last]);
      if(depth == 0) continue; // skip root
      for(int i = 0; i < 6; ++i) {
        Eigen::Ref<Eigen::Matrix<int, 4, 1>> coords(N.col(i));
        key_t key = octree.hash(coords.x(), coords.y(), coords.z(), depth - 1);
        if(!octree.fetch(coords.x(), coords.y(), coords.z()) &&
            octants.insert(key).second) {
          alloc_buffer.push_back(key);
        }
      }
    }
    end = alloc_buffer.size();
  }
  octree.allocate(alloc_buffer.data(), alloc_buffer.size());
}
}
#endif


//
// Copyright 2010 Scott McMurray.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef BOOST_HASH_BLOCK_CYPHERS_SHACAL_HPP
#define BOOST_HASH_BLOCK_CYPHERS_SHACAL_HPP

#include <boost/hash/block_cyphers/basic_shacal.hpp>

//
// Implemented directly from the SHA standard as found at
// http://csrc.nist.gov/publications/fips/fips180-2/fips180-2.pdf
//
// In SHA terminology:
// - plaintext = H^(i-1)
// - cyphertext = H^(i)
// - key = M^(i)
// - schedule = W
//

namespace boost {
namespace hashes {
namespace block_cyphers {

// The original FIPS-180 seems to be gone, but FIPS 180-1
// (http://www.itl.nist.gov/fipspubs/fip180-1.htm) says the only change
// in SHA-1 from SHA(-0) is the rotation in the key scheduling.
class shacal : public basic_shacal {
  public:
    shacal(key_type const &k)
     : basic_shacal(build_schedule(k)) {}
    shacal(schedule_type s)
     : basic_shacal((prepare_schedule(s), s)) {}
  private:
    static schedule_type
    build_schedule(key_type const &key) {
        // Copy key into beginning of schedule
        schedule_type schedule;
        for (unsigned t = 0; t < key_words; ++t) {
            schedule[t] = key[t];
        }
        prepare_schedule(schedule);
        return schedule;
    }
    static void
    prepare_schedule(schedule_type &schedule) {
#ifdef BOOST_HASH_SHOW_PROGRESS
        for (unsigned t = 0; t < key_words; ++t) {
            std::printf(word_bits == 32 ?
                        "W[%2d] = %.8x\n" :
                        "W[%2d] = %.16lx\n",
                        t, schedule[t]);
        }
#endif

        for (unsigned t = key_words; t < rounds; ++t) {
            schedule[t] = schedule[t-3]
                        ^ schedule[t-8]
                        ^ schedule[t-14]
                        ^ schedule[t-16];
        }
    }
};
typedef shacal shacal0;

} // namespace block_cyphers
} // namespace hashes
} // namespace boost

#endif // BOOST_HASH_BLOCK_CYPHERS_SHACAL_HPP


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <boost/make_shared.hpp>
#include <boost/test/unit_test.hpp>
#include <ored/marketdata/marketimpl.hpp>
#include <ored/portfolio/builders/swap.hpp>
#include <ored/portfolio/enginedata.hpp>
#include <ored/portfolio/portfolio.hpp>
#include <ored/portfolio/swap.hpp>
#include <ored/utilities/indexparser.hpp>
#include <ored/utilities/log.hpp>
#include <oret/toplevelfixture.hpp>
#include <ql/cashflows/cpicoupon.hpp>
#include <ql/cashflows/cpicouponpricer.hpp>
#include <ql/cashflows/iborcoupon.hpp>
#include <ql/indexes/inflation/ukrpi.hpp>
#include <ql/termstructures/inflation/inflationhelpers.hpp>
#include <ql/termstructures/inflation/piecewisezeroinflationcurve.hpp>
#include <ql/termstructures/yield/discountcurve.hpp>
#include <ql/time/calendars/unitedkingdom.hpp>
#include <ql/time/daycounters/actual365fixed.hpp>
#include <ql/time/daycounters/actualactual.hpp>

using namespace QuantLib;
using namespace boost::unit_test_framework;
using namespace ore::data;
using std::vector;

// Inflation CPI Swap paying fixed * CPI(t)/baseCPI * N
// with last XNL N * (CPI(T)/baseCPI - 1)

namespace {

class TestMarket : public MarketImpl {
public:
    TestMarket() {
        // valuation date
        asof_ = Date(18, July, 2016);

        // build vectors with dates and discount factors for GBP
        vector<Date> datesGBP = {asof_,
                                 asof_ + 6 * Months,
                                 asof_ + 7 * Months,
                                 asof_ + 8 * Months,
                                 asof_ + 9 * Months,
                                 asof_ + 10 * Months,
                                 asof_ + 11 * Months,
                                 asof_ + 12 * Months,
                                 asof_ + 13 * Months,
                                 asof_ + 14 * Months,
                                 asof_ + 15 * Months,
                                 asof_ + 16 * Months,
                                 asof_ + 17 * Months,
                                 asof_ + 18 * Months,
                                 asof_ + 19 * Months,
                                 asof_ + 20 * Months,
                                 asof_ + 21 * Months,
                                 asof_ + 22 * Months,
                                 asof_ + 23 * Months,
                                 asof_ + 2 * Years,
                                 asof_ + 3 * Years,
                                 asof_ + 4 * Years,
                                 asof_ + 5 * Years,
                                 asof_ + 6 * Years,
                                 asof_ + 7 * Years,
                                 asof_ + 8 * Years,
                                 asof_ + 9 * Years,
                                 asof_ + 10 * Years,
                                 asof_ + 15 * Years,
                                 asof_ + 20 * Years};

        vector<DiscountFactor> dfsGBP = {1,      0.9955, 0.9953, 0.9947, 0.9941, 0.9933, 0.9924, 0.9914,
                                         0.9908, 0.9901, 0.9895, 0.9888, 0.9881, 0.9874, 0.9868, 0.9862,
                                         0.9855, 0.9849, 0.9842, 0.9836, 0.9743, 0.9634, 0.9510, 0.9361,
                                         0.9192, 0.9011, 0.8822, 0.8637, 0.7792, 0.7079};

        // build vectors with dates and inflation zc swap rates for UKRPI
        vector<Date> datesZCII = {asof_,
                                  asof_ + 1 * Years,
                                  asof_ + 2 * Years,
                                  asof_ + 3 * Years,
                                  asof_ + 4 * Years,
                                  asof_ + 5 * Years,
                                  asof_ + 6 * Years,
                                  asof_ + 7 * Years,
                                  asof_ + 8 * Years,
                                  asof_ + 9 * Years,
                                  asof_ + 10 * Years,
                                  asof_ + 12 * Years,
                                  asof_ + 15 * Years,
                                  asof_ + 20 * Years};

        vector<Rate> ratesZCII = {2.825, 2.9425, 2.975,  2.983, 3.0,  3.01,  3.008,
                                  3.009, 3.013,  3.0445, 3.044, 3.09, 3.109, 3.108};

        // build UKRPI fixing history
        Schedule fixingDatesUKRPI =
            MakeSchedule().from(Date(1, May, 2015)).to(Date(1, July, 2016)).withTenor(1 * Months);
        Real fixingRatesUKRPI[] = {258.5, 258.9, 258.6, 259.8, 259.6, 259.5,  259.8, 260.6,
                                   258.8, 260.0, 261.1, 261.4, 262.1, -999.0, -999.0};

        // build GBP discount curve
        yieldCurves_[make_tuple(Market::defaultConfiguration, YieldCurveType::Discount, "GBP")] =
            intDiscCurve(datesGBP, dfsGBP, ActualActual(), UnitedKingdom());

        // build GBP Libor index
        hGBP = Handle<IborIndex>(
            parseIborIndex("GBP-LIBOR-6M", intDiscCurve(datesGBP, dfsGBP, ActualActual(), UnitedKingdom())));
        iborIndices_[make_pair(Market::defaultConfiguration, "GBP-LIBOR-6M")] = hGBP;

        // add Libor 6M fixing (lag for GBP is 0d)
        hGBP->addFixing(Date(18, July, 2016), 0.0061731);

        // build UKRPI index
        boost::shared_ptr<UKRPI> ii;
        boost::shared_ptr<ZeroInflationTermStructure> cpiTS;
        RelinkableHandle<ZeroInflationTermStructure> hcpi;
        bool interp = false;
        ii = boost::shared_ptr<UKRPI>(new UKRPI(interp, hcpi));
        for (Size i = 0; i < fixingDatesUKRPI.size(); i++) {
            // std::cout << i << ", " << fixingDatesUKRPI[i] << ", " << fixingRatesUKRPI[i] << std::endl;
            ii->addFixing(fixingDatesUKRPI[i], fixingRatesUKRPI[i], true);
        };
        // now build the helpers ...
        vector<boost::shared_ptr<BootstrapHelper<ZeroInflationTermStructure>>> instruments;
        for (Size i = 0; i < datesZCII.size(); i++) {
            Handle<Quote> quote(boost::shared_ptr<Quote>(new SimpleQuote(ratesZCII[i] / 100.0)));
            boost::shared_ptr<BootstrapHelper<ZeroInflationTermStructure>> anInstrument(
                new ZeroCouponInflationSwapHelper(
                    quote, Period(2, Months), datesZCII[i], UnitedKingdom(), ModifiedFollowing, ActualActual(), ii,
                    yieldCurves_.at(make_tuple(Market::defaultConfiguration, YieldCurveType::Discount, "GBP"))));
            ;
            instruments.push_back(anInstrument);
        };
        // we can use historical or first ZCIIS for this
        // we know historical is WAY off market-implied, so use market implied flat.
        Rate baseZeroRate = ratesZCII[0] / 100.0;
        boost::shared_ptr<PiecewiseZeroInflationCurve<Linear>> pCPIts(new PiecewiseZeroInflationCurve<Linear>(
            asof_, UnitedKingdom(), ActualActual(), Period(2, Months), ii->frequency(), ii->interpolated(),
            baseZeroRate, intDiscCurve(datesGBP, dfsGBP, ActualActual(), UnitedKingdom()), instruments));
        pCPIts->recalculate();
        cpiTS = boost::dynamic_pointer_cast<ZeroInflationTermStructure>(pCPIts);
        hUKRPI = Handle<ZeroInflationIndex>(
            parseZeroInflationIndex("UKRPI", interp, Handle<ZeroInflationTermStructure>(cpiTS)));
        zeroInflationIndices_[make_pair(Market::defaultConfiguration, "UKRPI")] = hUKRPI;
    }

    Handle<IborIndex> hGBP;
    Handle<ZeroInflationIndex> hUKRPI;

private:
    Handle<YieldTermStructure> intDiscCurve(vector<Date> dates, vector<DiscountFactor> dfs, DayCounter dc,
                                            Calendar cal) {
        boost::shared_ptr<YieldTermStructure> idc(
            new QuantLib::InterpolatedDiscountCurve<LogLinear>(dates, dfs, dc, cal));
        return Handle<YieldTermStructure>(idc);
    }
};
} // namespace

BOOST_FIXTURE_TEST_SUITE(OREDataTestSuite, ore::test::TopLevelFixture)

BOOST_AUTO_TEST_SUITE(CPISwapTests)

BOOST_AUTO_TEST_CASE(testCPISwapPrice) {

    BOOST_TEST_MESSAGE("Testing CPI Swap Price...");

    // build market
    Date today(18, July, 2016);
    Settings::instance().evaluationDate() = today;
    boost::shared_ptr<TestMarket> market = boost::make_shared<TestMarket>();
    Date marketDate = market->asofDate();
    BOOST_CHECK_EQUAL(today, marketDate);
    Settings::instance().evaluationDate() = marketDate;

    // Test if GBP discount curve is empty
    Handle<YieldTermStructure> dts = market->discountCurve("GBP");
    QL_REQUIRE(!dts.empty(), "GBP discount curve not found");
    BOOST_CHECK_CLOSE(market->discountCurve("GBP")->discount(today + 1 * Years), 0.9914, 0.0001);

    // Test if GBP Libor curve is empty
    Handle<IborIndex> iis = market->iborIndex("GBP-LIBOR-6M");
    QL_REQUIRE(!iis.empty(), "GBP LIBOR 6M ibor Index not found");
    BOOST_TEST_MESSAGE(
        "CPISwap: Projected Libor fixing: " << market->iborIndex("GBP-LIBOR-6M")->forecastFixing(today + 1 * Years));

    // Test if GBP discount curve is empty
    Handle<ZeroInflationIndex> infidx = market->zeroInflationIndex("UKRPI");
    QL_REQUIRE(!infidx.empty(), "UKRPI inflation index not found");
    BOOST_TEST_MESSAGE("CPISwap: Projected UKRPI rate: " << infidx->fixing(today + 1 * Years));

    // envelope
    Envelope env("CP");

    // Start/End date
    Date startDate = today;
    Date endDate = today + 5 * Years;

    // date 2 string
    std::ostringstream oss;
    oss << io::iso_date(startDate);
    string start(oss.str());
    oss.str("");
    oss.clear();
    oss << io::iso_date(endDate);
    string end(oss.str());

    // Schedules
    string conv = "MF";
    string rule = "Forward";
    ScheduleData scheduleLibor(ScheduleRules(start, end, "6M", "UK", conv, conv, rule));
    ScheduleData scheduleCPI(ScheduleRules(start, end, "1Y", "UK", conv, conv, rule));

    // Leg variables
    bool isInArrears = false;
    string dc = "ACT/ACT";
    vector<Real> notional(1, 10000000);
    string paymentConvention = "F";

    // GBP Libor Leg
    bool isPayerLibor = true;
    string indexLibor = "GBP-LIBOR-6M";
    vector<Real> spread(1, 0);
    LegData legLibor(boost::make_shared<FloatingLegData>(indexLibor, 0, isInArrears, spread), isPayerLibor, "GBP",
                     scheduleLibor, "A365F", notional, vector<string>(), paymentConvention);

    // GBP CPI Leg
    bool isPayerCPI = false;
    string indexCPI = "UKRPI";
    Real baseCPI = 210.0;
    string CPIlag = "2M";
    std::vector<double> fixedRate(1, 0.02);
    bool interpolated = false;
    LegData legCPI(
        boost::make_shared<CPILegData>(indexCPI, start, baseCPI, CPIlag, (interpolated ? "Linear" : "Flat"), fixedRate),
        isPayerCPI, "GBP", scheduleCPI, dc, notional, vector<string>(), paymentConvention, false, true);

    // Build swap trades
    boost::shared_ptr<Trade> CPIswap(new ore::data::Swap(env, legLibor, legCPI));

    // engine data and factory
    boost::shared_ptr<EngineData> engineData = boost::make_shared<EngineData>();
    engineData->model("Swap") = "DiscountedCashflows";
    engineData->engine("Swap") = "DiscountingSwapEngine";
    boost::shared_ptr<EngineFactory> engineFactory = boost::make_shared<EngineFactory>(engineData, market);
    engineFactory->registerBuilder(boost::make_shared<SwapEngineBuilder>());

    // build swaps and portfolio
    boost::shared_ptr<Portfolio> portfolio(new Portfolio());
    CPIswap->id() = "CPI_Swap";

    portfolio->add(CPIswap);
    portfolio->build(engineFactory);

    // check CPI swap NPV against pure QL pricing
    Schedule floatSchedule(startDate, endDate, 6 * Months, UnitedKingdom(), ModifiedFollowing, ModifiedFollowing,
                           DateGeneration::Forward, false);
    Schedule cpiSchedule(startDate, endDate, 1 * Years, UnitedKingdom(), ModifiedFollowing, ModifiedFollowing,
                         DateGeneration::Forward, false);
    Leg floatLeg = IborLeg(floatSchedule, *market->hGBP).withNotionals(10000000.0);
    Leg cpiLeg = CPILeg(cpiSchedule, *market->hUKRPI, baseCPI, 2 * Months)
                     .withFixedRates(0.02)
                     .withNotionals(10000000)
                     .withObservationInterpolation(CPI::Flat)
                     .withPaymentDayCounter(ActualActual())
                     .withPaymentAdjustment(Following);

    QuantLib::Swap qlSwap(floatLeg, cpiLeg);
    auto dscEngine = boost::make_shared<DiscountingSwapEngine>(market->hGBP->forwardingTermStructure());
    qlSwap.setPricingEngine(dscEngine);
    BOOST_TEST_MESSAGE("Leg 1 NPV: ORE = "
                       << boost::static_pointer_cast<QuantLib::Swap>(CPIswap->instrument()->qlInstrument())->legNPV(0)
                       << " QL = " << qlSwap.legNPV(0));
    BOOST_TEST_MESSAGE("Leg 2 NPV: ORE = "
                       << boost::static_pointer_cast<QuantLib::Swap>(CPIswap->instrument()->qlInstrument())->legNPV(1)
                       << " QL = " << qlSwap.legNPV(1));
    BOOST_CHECK_CLOSE(CPIswap->instrument()->NPV(), qlSwap.NPV(), 1E-8); // this is 1E-10 rel diff
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


//==============================================================================
//         Copyright 2009 - 2013 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2013 MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_SWAR_FUNCTIONS_COMPLEX_SIMD_COMMON_COMBINE_HPP_INCLUDED
#define NT2_SWAR_FUNCTIONS_COMPLEX_SIMD_COMMON_COMBINE_HPP_INCLUDED

#include <nt2/swar/functions/combine.hpp>
#include <nt2/include/functions/real.hpp>
#include <nt2/include/functions/imag.hpp>
#include <boost/simd/sdk/simd/meta/vector_of.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( combine_
                            , tag::cpu_
                            , (A0)(X)
                            , ((simd_< complex_< unspecified_<A0> >, X >))
                              ((simd_< complex_< unspecified_<A0> >, X >))
                            )
  {
    typedef typename  boost::simd::meta::vector_of< typename A0::value_type
                                                  , A0::static_size * 2
                                                  >::type           result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A0 const& a1) const
    {
      return result_type( nt2::combine(nt2::real(a0),nt2::real(a1))
                        , nt2::combine(nt2::imag(a0),nt2::imag(a1))
                        );
    }
  };
} } }

#endif