/*
 * Copyright Nick Thompson, 2017
 * 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)
 */

#include <boost/math/quadrature/naive_monte_carlo.hpp>
#include <iostream>
#include <iomanip>
#include <limits>
#include <cmath>
#include <thread>
#include <future>
#include <string>
#include <chrono>
#include <boost/math/special_functions/pow.hpp>
#include <boost/math/constants/constants.hpp>

using std::vector;
using std::pair;
using boost::math::quadrature::naive_monte_carlo;

void display_progress(double progress,
                      double error_estimate,
                      double current_estimate,
                      std::chrono::duration<double> estimated_time_to_completion)
{
    int barWidth = 70;

    std::cout << "[";
    int pos = barWidth * progress;
    for (int i = 0; i < barWidth; ++i) {
        if (i < pos) std::cout << "=";
        else if (i == pos) std::cout << ">";
        else std::cout << " ";
    }
    std::cout << "] "
              << int(progress * 100.0)
              << "%, E = "
              << std::setprecision(3)
              << error_estimate
              << ", time to completion: "
              << estimated_time_to_completion.count()
              << " seconds, estimate: "
              << std::setprecision(5)
              << current_estimate
              << "     \r";

    std::cout.flush();
}

int main()
{
    double exact = 1.3932039296856768591842462603255;
    double A = 1.0 / boost::math::pow<3>(boost::math::constants::pi<double>());
    auto g = [&](std::vector<double> const & x)
    {
      return A / (1.0 - cos(x[0])*cos(x[1])*cos(x[2]));
    };
    vector<pair<double, double>> bounds{{0, boost::math::constants::pi<double>() }, {0, boost::math::constants::pi<double>() }, {0, boost::math::constants::pi<double>() }};
    naive_monte_carlo<double, decltype(g)> mc(g, bounds, 0.001);

    auto task = mc.integrate();

    int s = 0;
    std::cout << "Hit ctrl-c to cancel.\n";
    while (task.wait_for(std::chrono::seconds(1)) != std::future_status::ready)
    {
        display_progress(mc.progress(),
                         mc.current_error_estimate(),
                         mc.current_estimate(),
                         mc.estimated_time_to_completion());
        // TODO: The following shows that cancellation works,
        // but it would be nice to show how it works with a ctrl-c signal handler.
        if (s++ > 25){
          mc.cancel();
          std::cout << "\nCancelling because this is too slow!\n";
        }
    }
    double y = task.get();
    display_progress(mc.progress(),
                     mc.current_error_estimate(),
                     mc.current_estimate(),
                     mc.estimated_time_to_completion());
    std::cout << std::setprecision(std::numeric_limits<double>::digits10) << std::fixed;
    std::cout << "\nFinal value: " << y << std::endl;
    std::cout << "Exact      : " << exact << std::endl;
    std::cout << "Final error estimate: " << mc.current_error_estimate() << std::endl;
    std::cout << "Actual error        : " << abs(y - exact) << std::endl;
    std::cout << "Function calls: " << mc.calls() << std::endl;
    std::cout << "Is this good enough? [y/N] ";
    bool goodenough = true;
    std::string line;
    std::getline(std::cin, line);
    if (line[0] != 'y')
    {
         goodenough = false;
    }
    double new_error = -1;
    if (!goodenough)
    {
        std::cout << "What is the new target error? ";
        std::getline(std::cin, line);
        new_error = atof(line.c_str());
        if (new_error >= mc.current_error_estimate())
        {
           std::cout << "That error bound is already satisfied.\n";
           return 0;
        }
    }
    if (new_error > 0)
    {
        mc.update_target_error(new_error);
        auto task = mc.integrate();
        std::cout << "Hit ctrl-c to cancel.\n";
        while (task.wait_for(std::chrono::seconds(1)) != std::future_status::ready)
        {
            display_progress(mc.progress(),
                             mc.current_error_estimate(),
                             mc.current_estimate(),
                             mc.estimated_time_to_completion());
        }
        double y = task.get();
        display_progress(mc.progress(),
                         mc.current_error_estimate(),
                         mc.current_estimate(),
                         mc.estimated_time_to_completion());
        std::cout << std::setprecision(std::numeric_limits<double>::digits10) << std::fixed;
        std::cout << "\nFinal value: " << y << std::endl;
        std::cout << "Exact      : " << exact << std::endl;
        std::cout << "Final error estimate: " << mc.current_error_estimate() << std::endl;
        std::cout << "Actual error        : " << abs(y - exact) << std::endl;
        std::cout << "Function calls: " << mc.calls() << std::endl;
    }
}


// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>

// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl

// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.

// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// Lesser General Public License for more details.

// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

// Modified to account for "mimic" joints, i.e. joints whose motion has a
// linear relationship to that of another joint.
// Copyright  (C)  2013  Sachin Chitta, Willow Garage

#ifndef KDL_CHAIN_IKSOLVERVEL_PINV_Mimic_HPP
#define KDL_CHAIN_IKSOLVERVEL_PINV_Mimic_HPP

#include <kdl/config.h>
#include <kdl/chainiksolver.hpp>
#include <kdl/chainjnttojacsolver.hpp>

#include <moveit/kdl_kinematics_plugin/joint_mimic.hpp>
#include <Eigen/SVD>

namespace KDL
{
/**
 * Implementation of a inverse velocity kinematics algorithm based
 * on the generalize pseudo inverse to calculate the velocity
 * transformation from Cartesian to joint space of a general
 * KDL::Chain. It uses a svd-calculation based on householders
 * rotations.
 *
 * @ingroup KinematicFamily
 */
class ChainIkSolverVelMimicSVD : public ChainIkSolverVel
{
public:
  /**
   * Constructor of the solver
   *
   * @param chain the chain to calculate the inverse velocity kinematics for
   * @param mimic_joints A vector of indices that map each (and every) joint onto the corresponding joint in a
   * reduced set of joints that do not include the mimic joints. This vector must be of size chain.getNrOfJoints().
   * E.g. if an arm has 7 joints: j0 to j6. Say j2 mimics (follows) j0. Then, mimic_joints should be: [0 1 0 3 4 5 6]
   * @param num_mimic_joints The number of joints that are setup to follow other joints
   * @param position_ik false if you want to solve for the full 6 dof end-effector pose,
   *        true if you want to solve only for the 3 dof end-effector position.
   * @param threshold if a singular value is below this value, its inverse is set to zero, default: 0.001
   */
  explicit ChainIkSolverVelMimicSVD(const Chain& chain_,
                                    const std::vector<kdl_kinematics_plugin::JointMimic>& mimic_joints,
                                    bool position_ik = false, double threshold = 0.001);

// TODO: simplify after kinetic support is dropped
#define KDL_VERSION_LESS(a, b, c) ((KDL_VERSION) < ((a << 16) | (b << 8) | c))
#if KDL_VERSION_LESS(1, 4, 0)
  void updateInternalDataStructures();
#else
  void updateInternalDataStructures() override;
#endif
#undef KDL_VERSION_LESS

  ~ChainIkSolverVelMimicSVD() override;

  int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out) override
  {
    return CartToJnt(q_in, v_in, qdot_out, Eigen::VectorXd::Constant(svd_.cols(), 1.0),
                     Eigen::Matrix<double, 6, 1>::Constant(1.0));
  }

  /** Compute qdot_out = W_q * (W_x * J * W_q)^# * W_x * v_in
   *
   * where W_q and W_x are joint- and Cartesian weights respectively.
   * A smaller joint weight (< 1.0) will reduce the contribution of this joint to the solution. */
  int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out, const Eigen::VectorXd& joint_weights,
                const Eigen::Matrix<double, 6, 1>& cartesian_weights);

  /// not implemented.
  int CartToJnt(const JntArray& q_init, const FrameVel& v_in, JntArrayVel& q_out) override
  {
    return -1;
  }

  /// Return true iff we ignore orientation but only consider position for inverse kinematics
  bool isPositionOnly() const
  {
    return svd_.rows() == 3;
  }

private:
  bool jacToJacReduced(const Jacobian& jac, Jacobian& jac_reduced);

  // Mimic joint specific
  const std::vector<kdl_kinematics_plugin::JointMimic>& mimic_joints_;
  int num_mimic_joints_;

  const Chain& chain_;
  ChainJntToJacSolver jnt2jac_;

  Eigen::JacobiSVD<Eigen::MatrixXd> svd_;
  Eigen::VectorXd qdot_out_reduced_;

  Jacobian jac_;          // full Jacobian
  Jacobian jac_reduced_;  // reduced Jacobian with contributions of mimic joints mapped onto active DoFs
};
}
#endif


//////////////////////////////////////////////////////////////////////////////////////////////
/// \file StaticNoiseModel-inl.hpp
///
/// \author Sean Anderson, ASRL
//////////////////////////////////////////////////////////////////////////////////////////////

#include <steam/problem/NoiseModel.hpp>

#include <iostream>
#include <stdexcept>

#include <Eigen/Cholesky>

namespace steam {

template<int MEAS_DIM>
DynamicNoiseModel<MEAS_DIM>::DynamicNoiseModel(std::shared_ptr<NoiseEvaluator<MEAS_DIM>> eval) :
eval_(eval) {
  this->setByCovariance(eval_->evaluateCovariance());
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Get a reference to the square root information matrix
//////////////////////////////////////////////////////////////////////////////////////////////
template<int MEAS_DIM>
const Eigen::Matrix<double,MEAS_DIM,MEAS_DIM>& DynamicNoiseModel<MEAS_DIM>::getSqrtInformation() const {
  this->setByCovariance(eval_->evaluateCovariance());
  return this->sqrtInformation_;
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Get the norm of the whitened error vector, sqrt(rawError^T * info * rawError)
//////////////////////////////////////////////////////////////////////////////////////////////
template<int MEAS_DIM>
double DynamicNoiseModel<MEAS_DIM>::getWhitenedErrorNorm(
    const Eigen::Matrix<double,MEAS_DIM,1>& rawError) const {
  this->setByCovariance(eval_->evaluateCovariance());
  return (this->sqrtInformation_*rawError).norm();
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Get the whitened error vector, sqrtInformation*rawError
//////////////////////////////////////////////////////////////////////////////////////////////
template<int MEAS_DIM>
Eigen::Matrix<double,MEAS_DIM,1> DynamicNoiseModel<MEAS_DIM>::whitenError(
    const Eigen::Matrix<double,MEAS_DIM,1>& rawError) const {
  this->setByCovariance(eval_->evaluateCovariance());
  return this->sqrtInformation_*rawError;
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Default constructor
//////////////////////////////////////////////////////////////////////////////////////////////
template<int MEAS_DIM>
StaticNoiseModel<MEAS_DIM>::StaticNoiseModel() {
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief General constructor
//////////////////////////////////////////////////////////////////////////////////////////////
template<int MEAS_DIM>
BaseNoiseModel<MEAS_DIM>::BaseNoiseModel(const Eigen::Matrix<double,MEAS_DIM,MEAS_DIM>& matrix,
                                 MatrixType type) {

  // Depending on the type of 'matrix', we set the internal storage
  switch(type) {
    case COVARIANCE :
      setByCovariance(matrix);
      break;
    case INFORMATION :
      setByInformation(matrix);
      break;
    case SQRT_INFORMATION :
      setBySqrtInformation(matrix);
      break;
  }
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Set by covariance matrix
//////////////////////////////////////////////////////////////////////////////////////////////
template<int MEAS_DIM>
void BaseNoiseModel<MEAS_DIM>::setByCovariance(
    const Eigen::Matrix<double,MEAS_DIM,MEAS_DIM>& matrix) const {

  // Information is the inverse of covariance
  this->setByInformation(matrix.inverse());
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Set by information matrix
//////////////////////////////////////////////////////////////////////////////////////////////
template<int MEAS_DIM>
void BaseNoiseModel<MEAS_DIM>::setByInformation(
    const Eigen::Matrix<double,MEAS_DIM,MEAS_DIM>& matrix) const {

  // Check that the matrix is positive definite
  this->assertPositiveDefiniteMatrix(matrix);

  // Perform an LLT decomposition
  Eigen::LLT<Eigen::Matrix<double,MEAS_DIM,MEAS_DIM> > lltOfInformation(matrix);

  // Store upper triangular matrix (the square root information matrix)
  this->setBySqrtInformation(lltOfInformation.matrixL().transpose());
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Set by square root of information matrix
//////////////////////////////////////////////////////////////////////////////////////////////
template<int MEAS_DIM>
void BaseNoiseModel<MEAS_DIM>::setBySqrtInformation(
    const Eigen::Matrix<double,MEAS_DIM,MEAS_DIM>& matrix)  const {

  // Set internal storage matrix
  sqrtInformation_ = matrix; // todo: check this is upper triangular
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Assert that the matrix is positive definite
//////////////////////////////////////////////////////////////////////////////////////////////
template<int MEAS_DIM>
void BaseNoiseModel<MEAS_DIM>::assertPositiveDefiniteMatrix(
    const Eigen::Matrix<double,MEAS_DIM,MEAS_DIM>& matrix)  const{

  // Initialize an eigen value solver
  Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double,MEAS_DIM,MEAS_DIM> >
      eigsolver(matrix, Eigen::EigenvaluesOnly);

  // Check the minimum eigen value
  if (eigsolver.eigenvalues().minCoeff() <= 0) {
    std::stringstream ss; ss << "Covariance \n" << matrix << "\n must be positive definite. "
                             << "Min. eigenvalue : " << eigsolver.eigenvalues().minCoeff();
    throw std::invalid_argument(ss.str());
  }
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief General constructor
//////////////////////////////////////////////////////////////////////////////////////////////
template<int MEAS_DIM>
StaticNoiseModel<MEAS_DIM>::StaticNoiseModel(const Eigen::Matrix<double,MEAS_DIM,MEAS_DIM>& matrix,
                                             MatrixType type)
: BaseNoiseModel<MEAS_DIM>::BaseNoiseModel(matrix,type) {

}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Get a reference to the square root information matrix
//////////////////////////////////////////////////////////////////////////////////////////////
template<int MEAS_DIM>
const Eigen::Matrix<double,MEAS_DIM,MEAS_DIM>& StaticNoiseModel<MEAS_DIM>::getSqrtInformation() const {
  return this->sqrtInformation_;
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Get the norm of the whitened error vector, sqrt(rawError^T * info * rawError)
//////////////////////////////////////////////////////////////////////////////////////////////
template<int MEAS_DIM>
double StaticNoiseModel<MEAS_DIM>::getWhitenedErrorNorm(
    const Eigen::Matrix<double,MEAS_DIM,1>& rawError) const {
  return (this->sqrtInformation_*rawError).norm();
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Get the whitened error vector, sqrtInformation*rawError
//////////////////////////////////////////////////////////////////////////////////////////////
template<int MEAS_DIM>
Eigen::Matrix<double,MEAS_DIM,1> StaticNoiseModel<MEAS_DIM>::whitenError(
    const Eigen::Matrix<double,MEAS_DIM,1>& rawError) const {
  return this->sqrtInformation_*rawError;
}

} // steam


/*
 * Copyright (C) 2018, Matt Zucker
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 *
 * Original source: https://github.com/mzucker/ccdwrapper
 */

#ifndef _CCD_EIGEN_H_
#define _CCD_EIGEN_H_

#include <ccd/ccd.h>
#include <Eigen/Geometry>
#include <iostream>
#include <type_traits>

static_assert(std::is_same<double, ccd_real_t>::value,
    "This plugin requires libccd to be built with double precision support.");

namespace ccdw {

typedef Eigen::Matrix<ccd_real_t, 3, 1> vec3;
typedef Eigen::Matrix<ccd_real_t, 4, 1> vec4;

typedef Eigen::Matrix<ccd_real_t, 3, 3> mat3;
typedef Eigen::Matrix<ccd_real_t, 4, 4> mat4;
typedef Eigen::Quaternion<ccd_real_t> quat;

class Transform3 {
public:

    Transform3():
        _rotation(quat::Identity()),
        _translation(0,0,0) { _update(); }

    explicit Transform3(const vec3& t):
        _rotation(quat::Identity()),
        _translation(t) { _update(); }

    explicit Transform3(const quat& r):
        _rotation(r),
        _translation(0, 0, 0) { _update(); }

    Transform3(const quat& q, const vec3& t):
        _rotation(q), _translation(t) { _update(); }

    Transform3 inverse() const {
        return Transform3(rotation().inverse(),
                          -(rotation().inverse()*translation()));

    }

    void setTranslation(const vec3& t) {
        _translation = t;
        _update();
    }

    void setRotation(const quat& q) {
        _rotation = q;
        _update();
    }

    const vec3& translation() const {
        return _translation;
    }

    const quat& rotation() const {
        return _rotation;
    }

    const Eigen::Affine3d& tobj() const {
        return _xform;
    }

    mat4 matrix() const {
        return _xform.matrix();
    }

    vec3 transformInv(const vec3& p) const {
        return _xform_inv * p;
    }

    vec3 transformFwd(const vec3& p) const {
        return _xform * p;
    }

    Transform3 compose(const Transform3& other) const {
        return Transform3(this->rotation() * other.rotation(),
                          this->translation() + this->rotation()*other.translation());
    }

private:

    quat _rotation;
    vec3 _translation;

    Eigen::Affine3d _xform;
    Eigen::Affine3d _xform_inv;

    void _update() {
        _xform = Eigen::Translation<double, 3>(_translation) * _rotation;
        _xform_inv = _rotation.inverse() * Eigen::Translation<double, 3>(-_translation);
    }

};

inline vec3 operator*(const Transform3& x, const vec3& v) {
    return x.transformFwd(v);
}

inline Transform3 operator*(const Transform3& x1, const Transform3& x2) {
    return x1.compose(x2);
}

class Box3 {
public:
    vec3 p0;
    vec3 p1;

    Box3(): p0(1,1,1), p1(0,0,0) {}
    Box3(const vec3& a, const vec3& b): p0(a), p1(b) {}

};

inline quat quatFromTwoVectors(const vec3& yy,
                               const vec3& z) {

    vec3 x = yy.cross(z);
    vec3 y = z.cross(x);

    mat3 m;
    m.col(0) = x / x.norm();
    m.col(1) = y / y.norm();
    m.col(2) = z / z.norm();

    return quat(m);

}

inline quat quatFromOneVector(const vec3& z) {

    int maxaxis = 0;
    if (fabs(z[1]) > fabs(z[maxaxis])) { maxaxis = 1; }
    if (fabs(z[2]) > fabs(z[maxaxis])) { maxaxis = 2; }

    vec3 y = vec3(0, 0, 0);
    y[(maxaxis + 1) % 3] = 1;

    return quatFromTwoVectors(y, z);

}

inline quat quatFromAxisAngle(const vec3& axis,
                              double angle) {

    return quat(Eigen::AngleAxisd(angle, axis));

}

inline quat quatFromOmega(const vec3& v) {
    double l = v.norm();
    vec3 vn = v / l;
    return quat(Eigen::AngleAxisd(l, vn));
}

} // namespace ccdw

#endif


#include "common.h"

#include "hexutil/basics/hexgrid.h"

#define BOOST_TEST_MODULE HexgridTest
#include <boost/test/included/unit_test.hpp>

namespace hex {

struct Fixture {
};

BOOST_FIXTURE_TEST_SUITE(hexgrid_test, Fixture)

void check_neighbours(Point point, Point *expected_neighbours) {
    PointNeighbours all_neighbours = get_neighbours(point);
    for (int i = 0; i < 6; i++) {
        Point neighbour = get_neighbour(point, i);
        BOOST_CHECK_EQUAL(neighbour, expected_neighbours[i]);
        BOOST_CHECK_EQUAL(all_neighbours[i], expected_neighbours[i]);
        int dir = get_direction(point, neighbour);
        BOOST_CHECK_EQUAL(dir, i);
        int dist = distance_between(point, neighbour);
        BOOST_CHECK_EQUAL(dist, 1);
    }
}

BOOST_AUTO_TEST_CASE(test_neighbours_even) {
    Point even_point(6,6);
    Point expected_even_neighbours[] = { Point(6,5), Point(7,5), Point(7,6), Point(6,7), Point(5,6), Point(5,5) };
    check_neighbours(even_point, expected_even_neighbours);
}

BOOST_AUTO_TEST_CASE(test_neighbours_odd) {
    Point odd_point(7,7);
    Point expected_odd_neighbours[] = { Point(7,6), Point(8,7), Point(8,8), Point(7,8), Point(6,8), Point(6,7) };
    check_neighbours(odd_point, expected_odd_neighbours);
}

BOOST_AUTO_TEST_CASE(test_get_circle_even) {
    Point even_point(6,6);
    int radius = 3;
    int expected_num_scanlines = radius*2 + 1;
    int expected_scanlines[] = { 1, 3, 3, 3, 3, 2, 0 };

    std::vector<int> scanlines = get_circle_scanlines(even_point, radius);
    BOOST_CHECK_EQUAL(scanlines.size(), expected_num_scanlines);
    for (int i = 0; i < expected_num_scanlines; i++) {
        BOOST_CHECK_EQUAL(scanlines[i], expected_scanlines[i]);
    }
}

BOOST_AUTO_TEST_CASE(test_get_circle_odd) {
    Point odd_point(7,7);
    int radius = 3;
    int expected_num_scanlines = radius*2 + 1;
    int expected_scanlines[] = { 0, 2, 3, 3, 3, 3, 1 };

    std::vector<int> scanlines = get_circle_scanlines(odd_point, radius);
    BOOST_CHECK_EQUAL(scanlines.size(), expected_num_scanlines);
    for (int i = 0; i < expected_num_scanlines; i++) {
        BOOST_CHECK_EQUAL(scanlines[i], expected_scanlines[i]);
    }
}

BOOST_AUTO_TEST_CASE(test_pixel_to_point) {
    int x_spacing = 16;
    int y_spacing = 16;
    int slope_width = 4;
    int slope_height = 8;
    Point point = pixel_to_point(0, 0, x_spacing, y_spacing, slope_width, slope_height);
    BOOST_CHECK_EQUAL(point, Point(-1, -1));
    point = pixel_to_point(2, 0, x_spacing, y_spacing, slope_width, slope_height);
    BOOST_CHECK_EQUAL(point, Point(-1, -1));
    point = pixel_to_point(4, 0, x_spacing, y_spacing, slope_width, slope_height);
    BOOST_CHECK_EQUAL(point, Point(0, 0));
    point = pixel_to_point(11, 0, x_spacing, y_spacing, slope_width, slope_height);
    BOOST_CHECK_EQUAL(point, Point(0, 0));
}

BOOST_AUTO_TEST_CASE(test_circle_iterator_zero_radius) {
    hexgrid_circle circle(Point(7,21), 0);
    auto iter = circle.begin();
    BOOST_CHECK_EQUAL(*iter, Point(7, 21));
    BOOST_CHECK_EQUAL(iter == circle.end(), false);
    BOOST_CHECK_EQUAL(iter != circle.end(), true);
    iter++;
    BOOST_CHECK_EQUAL(iter == circle.end(), true);
    BOOST_CHECK_EQUAL(iter != circle.end(), false);
}

BOOST_AUTO_TEST_CASE(test_circle_iterator_big) {
    hexgrid_circle circle(Point(7,21), 2);
    std::vector<Point> points;
    int max = 20;
    for (auto iter = circle.begin(); iter != circle.end() && max > 0; iter++, max--) {
        points.push_back(*iter);
    }
    std::vector<Point> expected_points = get_circle_points(Point(7,21), 2, 100, 100);
    BOOST_CHECK_EQUAL(points, expected_points);
}

BOOST_AUTO_TEST_SUITE_END()

};


#include <boost/hana.hpp>

using namespace boost::hana;

int main()
{
	constexpr auto t1 = make_tuple(int_c<5>, int_c<2>, int_c<3>, int_c<1>, int_c<4>);
	constexpr auto r1 = index_if(t1, equal.to(int_c<3>));
	static_assert(r1.value().value == 2);

	constexpr auto t2 = tuple_c<int, 5, 2, 3, 1, 4>;
	constexpr auto r2 = index_if(t2, equal.to(int_c<6>));
	static_assert(r2.value_or(-1) == -1);
}


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

  @copyright 2016 NumScale SAS

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

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

 /*!

    @ingroup group-trigonometric
    Function object implementing acosd capabilities

    inverse cosine in degree.

    @par Semantic:

    For every parameter of floating type

    @code
    auto r = acosd(x);
    @endcode

    Returns the arc @c r in the interval
    \f$[0, 180[\f$ such that <tt>cosd(r) == x</tt>.
    If @c x is outside \f$[-1, 1[\f$ the result is @ref Nan.

    @see acos, acospi, cosd

  **/
  Value acosd(Value const & v0);
} }
#endif

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

#endif


// Copyright 2016 Arizona Board of Regents. See README.md and LICENSE for more.

#include <iostream>
#include <algorithm>
#include <iterator>
#include <ctime>

#include <boost/random.hpp>
#include <boost/generator_iterator.hpp>
#include <boost/date_time/gregorian/gregorian.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/algorithm/string.hpp>

#include "nanocube.h"
#include "nanocube_traversals.h"
#include "debug.h"
#include "test_utils.h"

int atoi(const std::string &s) { return atoi(s.c_str()); }
double atof(const std::string &s) { return atof(s.c_str()); }

// convert lat,lon to quad tree address
int64_t loc2addr(double lat, double lon, int qtreeLevel)
{
    double xd = (lon + M_PI) / (2.0 * M_PI);
    double yd = (log(tan(M_PI / 4.0 + lat / 2.0)) + M_PI) / (2.0 * M_PI);
    //cout << lat << " " << lon << " " << endl;
    int x = xd * (1 << qtreeLevel), y = yd * (1 << qtreeLevel);

    int64_t z = 0; // z gets the resulting Morton Number.

    for (int i = 0; i < sizeof(x) * 8; i++) // unroll for more speed...
    {
        z |= (x & 1U << i) << i | (y & 1U << i) << (i + 1);
    }

    return z;
}

int main(int argc, char **argv)
{
    using namespace boost::gregorian;
    using namespace boost::posix_time;

    ifstream is(argv[1]);
    std::cout << "Data file: " << argv[1] << std::endl;
    string s;

    int qtreeLevel = 16;

    vector<int> schema = {qtreeLevel*2, qtreeLevel*2};
    // use a quadtree
    Nanocube<int> nc(schema);

    vector<pair<int64_t,int64_t> > dataarray;

    int i = 0;

    clock_t begin = clock();

    while (std::getline(is, s)) {
        vector<string> output;
        boost::split(output,s,boost::is_any_of("\t"));
        if (output.size() != 4) {
            cerr << "Bad line:" << s << endl;
            continue;
        }

        double ori_lat = atof(output[0]) * M_PI / 180.0;
        double ori_lon = atof(output[1]) * M_PI / 180.0;
        double des_lat = atof(output[2]) * M_PI / 180.0;
        double des_lon = atof(output[3]) * M_PI / 180.0;

        if (ori_lat > 85.0511 || ori_lat < -85.0511 ||
            des_lat > 85.0511 || des_lat < -85.0511 ){
            cerr << "Invalid latitude: " << ori_lat << ", " << des_lat;
            cerr << " (should be in [-85.0511, 85.0511])" << endl;
            continue;
        }

        int64_t d1 = loc2addr(ori_lat, ori_lon, qtreeLevel);
        int64_t d2 = loc2addr(des_lat, des_lon, qtreeLevel);

        nc.insert(1, {d1, d2});
        dataarray.push_back({d1,d2});

        if (++i % 10000 == 0) {
            //nc.report_size();
            cout << i << endl;
        }
    }

    clock_t end = clock();
    double elapsed_secs = double(end - begin) / CLOCKS_PER_SEC;
    cout << "Running time: " << elapsed_secs << endl;

    //nc.dump_internals(true);
    //{
        ////nc.content_compact();
        //ofstream os("flights.nc");
        //nc.write_to_binary_stream(os);
    //}

    test(nc, dataarray, schema);
}


#include <boost/algorithm/hex.hpp>
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>

#include <nil/crypto3/zk/components/blueprint.hpp>
#include <nil/crypto3/zk/components/blueprint_variable.hpp>

#include <nil/crypto3/zk/snark/schemes/ppzksnark/r1cs_gg_ppzksnark.hpp>
#include <nil/crypto3/zk/snark/schemes/ppzksnark/r1cs_gg_ppzksnark/marshalling.hpp>

#include <nil/crypto3/zk/snark/algorithms/generate.hpp>
#include <nil/crypto3/zk/snark/algorithms/verify.hpp>
#include <nil/crypto3/zk/snark/algorithms/prove.hpp>

#include <nil/crypto3/algebra/curves/bls12.hpp>
#include <nil/crypto3/algebra/fields/bls12/base_field.hpp>
#include <nil/crypto3/algebra/fields/bls12/scalar_field.hpp>
#include <nil/crypto3/algebra/fields/arithmetic_params/bls12.hpp>
#include <nil/crypto3/algebra/curves/params/multiexp/bls12.hpp>
#include <nil/crypto3/algebra/curves/params/wnaf/bls12.hpp>
#include "detail/components.hpp"

typedef algebra::curves::bls12<381> curve_type;
typedef curve_type::scalar_field_type field_type;
typedef field_type::value_type value_type;
typedef zk::snark::r1cs_gg_ppzksnark<curve_type> scheme_type;

using namespace nil::crypto3::zk::components;
using namespace nil::crypto3::zk::snark;

boost::filesystem::path PRIMARY_KEY_PATH = "prov_key",
    VERIFICATION_KEY_PATH = "ver_key",
    PROOF_PATH = "proof";

std::vector<std::uint8_t> readfile(boost::filesystem::path path) {
    boost::filesystem::ifstream stream(path, std::ios::in | std::ios::binary);
    auto eos = std::istreambuf_iterator<char>();
    auto buffer = std::vector<uint8_t>(std::istreambuf_iterator<char>(stream), eos);
    return buffer;
}

bool generate_keys() {
    blueprint<field_type> bp;
    contest::bank_component<field_type> contest_component(bp);
    contest_component.generate_r1cs_constraints();

    const r1cs_constraint_system<field_type> constraint_system = bp.get_constraint_system();

    scheme_type::keypair_type keypair = generate<scheme_type>(constraint_system);

    std::vector<std::uint8_t> proving_key_byteblob =
        nil::marshalling::verifier_input_serializer_tvm<scheme_type>::process(keypair.first);
    std::vector<std::uint8_t> verification_key_byteblob =
        nil::marshalling::verifier_input_serializer_tvm<scheme_type>::process(keypair.second);

    boost::filesystem::ofstream pk_out(PRIMARY_KEY_PATH);
    for (const auto &v : proving_key_byteblob) {
        pk_out << v;
    }
    pk_out.close();
    std::cout << "prooving key saved to " << PRIMARY_KEY_PATH << std::endl;

    boost::filesystem::ofstream vk_out(VERIFICATION_KEY_PATH );
    for (const auto &v : verification_key_byteblob) {
        vk_out << v;
    }
    vk_out.close();
    std::cout << "verification key saved to " << VERIFICATION_KEY_PATH << std::endl;

    return true;
}

bool generate_proof(uint min_salary, uint min_age, uint salary, uint age) {
    std::vector<std::uint8_t> proving_key_byteblob = readfile(PRIMARY_KEY_PATH);
    nil::marshalling::status_type provingProcessingStatus = nil::marshalling::status_type::success;
    typename scheme_type::proving_key_type pk = nil::marshalling::verifier_input_deserializer_tvm<scheme_type>::proving_key_process(
        proving_key_byteblob.cbegin(),
        proving_key_byteblob.cend(),
        provingProcessingStatus);

    blueprint<field_type> bp;
    contest::bank_component<field_type> contest_component(bp);
    contest_component.generate_r1cs_constraints();
    contest_component.generate_r1cs_witness(min_salary, min_age, salary, age);

    std::cout << "Circuit satisfied: " << bp.is_satisfied() << std::endl;
    if (!bp.is_satisfied()) {
        return false;
    }

    const scheme_type::proof_type proof = prove<scheme_type>(pk, bp.primary_input(), bp.auxiliary_input());

    std::vector<std::uint8_t> proof_byteblob =
        nil::marshalling::verifier_input_serializer_tvm<scheme_type>::process(proof);

    std::cout << "proof is saved to " << PROOF_PATH << std::endl;
    boost::filesystem::ofstream proof_out(PROOF_PATH);
    for (const auto &v : proof_byteblob) {
        proof_out << v;
    }
    proof_out.close();

    return true;
}

bool verify_proof(uint min_salary, uint min_age) {
    std::vector<std::uint8_t> proof_byteblob = readfile(PROOF_PATH);
    nil::marshalling::status_type proofProcessingStatus = nil::marshalling::status_type::success;
    typename scheme_type::proof_type proof = nil::marshalling::verifier_input_deserializer_tvm<scheme_type>::proof_process(
        proof_byteblob.cbegin(),
        proof_byteblob.cend(),
        proofProcessingStatus);

    std::vector<std::uint8_t> verification_key_byteblob = readfile(VERIFICATION_KEY_PATH);
    nil::marshalling::status_type verificationProcessingStatus = nil::marshalling::status_type::success;
    typename scheme_type::verification_key_type vk = nil::marshalling::verifier_input_deserializer_tvm<scheme_type>::verification_key_process(
        verification_key_byteblob.cbegin(),
        verification_key_byteblob.cend(),
        verificationProcessingStatus );

    r1cs_primary_input<field_type> input = contest::get_public_input<field_type>(min_salary, min_age);
    using basic_proof_system = r1cs_gg_ppzksnark<curve_type>;
    const bool verified = verify<basic_proof_system>(vk, input, proof);
    std::cout << "proof verified " << verified << std::endl;

    return verified;
}

int main(int argc, char *argv[]) {
    uint salary, age;
    uint min_salary, min_age;

    boost::program_options::options_description options(
        "R1CS Generic Group PreProcessing Zero-Knowledge Succinct Non-interactive ARgument of Knowledge "
        "(https://eprint.iacr.org/2016/260.pdf) CLI Proof Generator");
    options.add_options()
    ("help,h", "Display help message")
    ("keygen", "Generate keys")
    ("proof", "Generate proof")
    ("verify", "Verify proof")
    ("min-age,x", boost::program_options::value<uint>(&min_age)->default_value(18))
    ("min-salary,y", boost::program_options::value<uint>(&min_salary)->default_value(1000))
    ("age,a", boost::program_options::value<uint>(&age)->default_value(0))
    ("salary,s", boost::program_options::value<uint>(&salary)->default_value(0));

    boost::program_options::variables_map vm;
    boost::program_options::store(boost::program_options::command_line_parser(argc, argv).options(options).run(), vm);
    boost::program_options::notify(vm);

    if (vm.count("help") || argc < 2) {
        std::cout << options << std::endl;
        return 0;
    } else if (vm.count("keygen")) {
        generate_keys();
    } else if (vm.count("proof")) {
        generate_proof(min_salary, min_age, salary, age);
    } else if (vm.count("verify")) {
        verify_proof(min_salary, min_age);
    }
    return 0;
}


//---------------------------------------------------------------------------//
//!
//! \file   DataGen_AdjointBremsstrahlungCrossSectionEvaluator.cpp
//! \author Luke Kersting
//! \brief  Adjoint bremsstrahlung cross section evaluator definition
//!
//---------------------------------------------------------------------------//

// Std Lib Includes
#include <limits>

// Boost Includes
#include <boost/function.hpp>
#include <boost/bind.hpp>

// FRENSIE Includes
#include "DataGen_AdjointBremsstrahlungCrossSectionEvaluator.hpp"
#include "Utility_GaussKronrodIntegrator.hpp"
#include "Utility_PhysicalConstants.hpp"
#include "Utility_ContractException.hpp"
#include "MonteCarlo_TwoDDistributionHelpers.hpp"

namespace DataGen{

// Constructor
AdjointBremsstrahlungCrossSectionEvaluator::AdjointBremsstrahlungCrossSectionEvaluator(
    Teuchos::RCP<MonteCarlo::ElectroatomicReaction>& bremsstrahlung_reaction,
    const BremsstrahlungDistribution& energy_loss_distribution )
  : d_bremsstrahlung_reaction( bremsstrahlung_reaction ),
    d_energy_loss_distribution( energy_loss_distribution )
{
  // Make sure the data is valid
  testPrecondition( !d_bremsstrahlung_reaction.is_null() );
  testPrecondition( d_energy_loss_distribution.size() > 0 );
}

// Evaluate the differential adjoint bremsstrahlung cross section (dc/dx)
double AdjointBremsstrahlungCrossSectionEvaluator::evaluateDifferentialCrossSection(
	  const double incoming_energy,
          const double outgoing_energy ) const
{
  // Make sure the energies are valid
  testPrecondition( incoming_energy > 0.0 );
  testPrecondition( outgoing_energy > 0.0 );

  // Evaluate the forward cross section at the incoming energy
  double forward_cs = d_bremsstrahlung_reaction->getCrossSection( incoming_energy );

  // Evaluate the energy loss distribution at a given incoming and outgoing energy
  double forward_pdf = MonteCarlo::evaluateTwoDDistributionCorrelatedPDF(
                                     incoming_energy,
                                     outgoing_energy,
                                     d_energy_loss_distribution );

  return forward_cs*forward_pdf;
}

// Return the cross section value at a given energy
double AdjointBremsstrahlungCrossSectionEvaluator::evaluateCrossSection(
                               const double energy,
			       const double precision ) const
{
  // Make sure the energies are valid
  testPrecondition( energy > 0.0 );

  double cross_section = 0.0;

  // Create boost rapper function for the adjoint Bremsstahlung differential cross section
  boost::function<double (double x)> diff_adjoint_brem_wrapper =
    boost::bind<double>( &AdjointBremsstrahlungCrossSectionEvaluator::evaluateDifferentialCrossSection,
                         boost::cref( *this ),
                         _1,
                         energy );

    double abs_error;

    Utility::GaussKronrodIntegrator integrator( precision );

    integrator.integrateAdaptively<15>(
					diff_adjoint_brem_wrapper,
					d_energy_loss_distribution.front().first,
					d_energy_loss_distribution.back().first,
					cross_section,
					abs_error );
}

} // end DataGen namespace

//---------------------------------------------------------------------------//
// end DataGen_AdjointBremsstrahlungCrossSectionEvaluator.cpp
//---------------------------------------------------------------------------//


//
// Created by Alex Beccaro on 18/01/18.
//

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include "../../src/problems/1-50/24/problem24.hpp"

BOOST_AUTO_TEST_SUITE( Problem24 )

    BOOST_AUTO_TEST_CASE( Example ) {
        std::vector<uint32_t> base = {0,1,2};
        std::vector<uint32_t> sol = base;
        for (uint32_t i = 1; i <= 6; i++) {
            std::vector<uint32_t> res = problems::problem24::solve(base, i);
            BOOST_CHECK_EQUAL_COLLECTIONS(res.begin(), res.end(),sol.begin(), sol.end());
            next_permutation(sol.begin(), sol.end());
        }
    }

    BOOST_AUTO_TEST_CASE( Solution ) {
        auto res = problems::problem24::solve();
        std::vector<uint32_t> sol = {2,7,8,3,9,1,5,4,6,0};
        BOOST_CHECK_EQUAL_COLLECTIONS(res.begin(), res.end(), sol.begin(), sol.end());
    }

BOOST_AUTO_TEST_SUITE_END()


#include "alpha_shapes.hpp"
#include <boost/lexical_cast.hpp>
#include <iostream>
#include <fstream>

typedef float f_t;

int main(int argc, char* argv[]){
	if(argc != 3){
		std::cout << "usage: " << argv[0] << " <alpha> <coord/radii file>" << std::endl;
		return 1;
	}
	f_t alpha = boost::lexical_cast<f_t>(argv[1]);
	const char* infilename = argv[2];

	std::vector<f_t> points, radii;
	f_t x, y, z, r;
	std::ifstream fs;
	fs.open(infilename, std::ifstream::in);
	while(fs >> x >> y >> z >> r){
		points.push_back(x);
		points.push_back(y);
		points.push_back(z);
		radii.push_back(r);
	}
	size_t numPoints = radii.size();
	std::cout << "number of points: " << numPoints << std::endl;

	unsigned int numEdges, numTriangles, numTetrahedra;
	unsigned int* edges, * triangles, * tetrahedra, * t;
	busv::alpha_shapes<f_t, unsigned int>(
		numPoints, alpha,
		&(points[0]), &(radii[0]),
		&numEdges, &edges,
		&numTriangles, &triangles,
		&numTetrahedra, &tetrahedra
	);

	std::cout << "tetrahedra" << std::endl;
	t = tetrahedra;
	for(unsigned int i = 0; i<numTetrahedra; ++i){
		std::cout << "\t" << t[0] << " " << t[1] << " " << t[2] << " " << t[3] << std::endl;
		t += 4;
	}
	std::cout << std::endl;

	std::cout << "triangles" << std::endl;
	t = triangles;
	for(unsigned int i = 0; i<numTriangles; ++i){
		std::cout << "\t" << t[0] << " " << t[1] << " " << t[2] << std::endl;
		t += 3;
	}
	std::cout << std::endl;

	std::cout << "edges" << std::endl;
	t = edges;
	for(unsigned int i = 0; i<numEdges; ++i){
		std::cout << "\t" << t[0] << " " << t[1] << std::endl;
		t += 2;
	}
	std::cout << std::endl;

	return 0;
}


/**
 * @file /sophus_ros_conversions/src/lib/geometry.cpp
 */
/*****************************************************************************
** Includes
*****************************************************************************/

#include <Eigen/Geometry>
#include "../../include/sophus_ros_conversions/eigen.hpp"
#include "../../include/sophus_ros_conversions/geometry.hpp"

/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace sophus_ros_conversions {

/*****************************************************************************
** Implementation
*****************************************************************************/

void poseMsgToSophus(const geometry_msgs::Pose &pose, Sophus::SE3f &se3)
{
  Eigen::Quaternion<Sophus::SE3f::Scalar> orientation;
  Sophus::SE3f::Point translation;
  pointMsgToEigen(pose.position, translation);
  quaternionMsgToEigen(pose.orientation, orientation);
  se3 = Sophus::SE3f(orientation, translation);  // TODO faster way to set this than reconstructing
}

geometry_msgs::Pose sophusToPoseMsg(const Sophus::SE3f& s) {
  geometry_msgs::Pose pose;
  Eigen::Vector3f translation = s.translation();
  pose.position = eigenToPointMsg(translation);
  Eigen::Quaternionf quaternion = s.unit_quaternion();
  pose.orientation = eigenToQuaternionMsg(quaternion);
  return pose;
}

// Sophus uses SE3f::Point as the translation type in the constructors of SE3f types.
void vector3MsgToSophus(const geometry_msgs::Vector3 &v, Sophus::SE3f::Point &translation)
{
  translation << v.x, v.y, v.z;
}

void transformMsgToSophus(const geometry_msgs::Transform &transform, Sophus::SE3f &se3)
{
  Sophus::SE3f::Point translation;
  Eigen::Quaternion<Sophus::SE3f::Scalar> orientation;
  vector3MsgToSophus(transform.translation, translation);
  quaternionMsgToEigen(transform.rotation, orientation);
  se3 = Sophus::SE3f(orientation, translation);  // TODO faster way to set this than reconstructing
}

Sophus::SE3f transformMsgToSophus(const geometry_msgs::Transform &transform)
{
  Sophus::SE3f T;
  transformMsgToSophus(transform, T);
  return T;
}

geometry_msgs::Transform sophusToTransformMsg(const Sophus::SE3f& se3) {
  geometry_msgs::Transform msg;
  msg.translation.x = se3.translation().x();
  msg.translation.y = se3.translation().y();
  msg.translation.z = se3.translation().z();
  msg.rotation.x = se3.unit_quaternion().x();
  msg.rotation.y = se3.unit_quaternion().y();
  msg.rotation.z = se3.unit_quaternion().z();
  msg.rotation.w = se3.unit_quaternion().w();
  return msg;
}

/*****************************************************************************
 ** Trailers
 *****************************************************************************/

} // namespace sophus_ros_conversions


#define EIGEN_DONT_PARALLELIZE
#ifndef _MSC_VER
#include "update_ops_cpp.hpp"
extern "C" {
#include "utility.h"
#include "update_ops.h"
}
#else
#include "update_ops_cpp.hpp"
#include "update_ops.h"
#include "utility.h"
#endif
#include <Eigen/Core>

void double_qubit_dense_matrix_gate(UINT target_qubit_index1, UINT target_qubit_index2, const CTYPE matrix[16], CTYPE *state, ITYPE dim) {
	double_qubit_dense_matrix_gate_c(target_qubit_index1, target_qubit_index2, matrix, state, dim);
}

void double_qubit_dense_matrix_gate(UINT target_qubit_index1, UINT target_qubit_index2, const Eigen::Matrix4cd& eigen_matrix, CTYPE *state, ITYPE dim) {
	double_qubit_dense_matrix_gate_eigen(target_qubit_index1, target_qubit_index2, eigen_matrix, state, dim);
}

void double_qubit_dense_matrix_gate_eigen(UINT target_qubit_index1, UINT target_qubit_index2, const Eigen::Matrix4cd& eigen_matrix, CTYPE *state, ITYPE dim) {
	// target mask

	const UINT min_qubit_index = get_min_ui(target_qubit_index1, target_qubit_index2);
	const UINT max_qubit_index = get_max_ui(target_qubit_index1, target_qubit_index2);
	const ITYPE min_qubit_mask = 1ULL << min_qubit_index;
	const ITYPE max_qubit_mask = 1ULL << (max_qubit_index - 1);
	const ITYPE low_mask = min_qubit_mask - 1;
	const ITYPE mid_mask = (max_qubit_mask - 1) ^ low_mask;
	const ITYPE high_mask = ~(max_qubit_mask - 1);

	const ITYPE target_mask1 = 1ULL << target_qubit_index1;
	const ITYPE target_mask2 = 1ULL << target_qubit_index2;
	std::complex<double>* eigen_state = reinterpret_cast<std::complex<double>*>(state);

	// loop variables
	const ITYPE loop_dim = dim / 4;
	ITYPE state_index;

	for (state_index = 0; state_index < loop_dim; ++state_index) {
		// create index
		ITYPE basis_0 = (state_index&low_mask)
			+ ((state_index&mid_mask) << 1)
			+ ((state_index&high_mask) << 2);

		// gather index
		ITYPE basis_1 = basis_0 + target_mask1;
		ITYPE basis_2 = basis_0 + target_mask2;
		ITYPE basis_3 = basis_1 + target_mask2;

		// fetch values
		Eigen::Vector4cd vec(state[basis_0], state[basis_1], state[basis_2], state[basis_3]);
		vec = eigen_matrix * vec;
		eigen_state[basis_0] = vec[0];
		eigen_state[basis_1] = vec[1];
		eigen_state[basis_2] = vec[2];
		eigen_state[basis_3] = vec[3];
	}
}


#include "sv/util/eigen.h"

#include <benchmark/benchmark.h>
#include <glog/logging.h>
#include <gtest/gtest.h>

#include <Eigen/Cholesky>
#include <Eigen/Geometry>

namespace sv {
namespace {

namespace bm = benchmark;
using Eigen::MatrixXd;
using Eigen::VectorXd;

/// @brief Invert PSD matrix, taken from ceres
template <int N>
void InvertPSDMatrix(const MatrixMNd<N, N>& m, MatrixXdRef m_inv) {
  const auto size = m.rows();

  // If the matrix can be assumed to be full rank, then if it is small
  // (< 5) and fixed size, use Eigen's optimized inverse()
  // implementation.
  //
  // https://eigen.tuxfamily.org/dox/group__TutorialLinearAlgebra.html#title3
  if constexpr (0 < N && N < 5) {
    m_inv = m.inverse();
  }
  m_inv = m.template selfadjointView<Eigen::Upper>().llt().solve(
      MatrixMNd<N, N>::Identity(size, size));
}

TEST(EigenTest, TestSafeCwiseInverse) {
  VectorXd x0 = VectorXd::Zero(4);
  x0(0) = 1;
  x0(2) = 1;
  VectorXd x1 = x0;

  SafeCwiseInverse(x1);
  EXPECT_EQ(x1, x0);
}

TEST(EigenTest, TestStableRotateBlockUpperLeftSize1) {
  Eigen::Matrix3d H;
  // clang-format off
  H << 1, 2, 3,
       4, 5, 6,
       7, 8, 9;
  // clang-format on
  Eigen::Vector3d b;
  b << 1, 2, 3;

  StableRotateBlockTopLeft(H, b, /*ind*/ 2, /*size*/ 1);

  Eigen::Matrix3d H1;
  // clang-format off
  H1 << 9, 7, 8,
        3, 1, 2,
        6, 4, 5;
  // clang-format on
  EXPECT_EQ(H, H1);
  EXPECT_EQ(b, Eigen::Vector3d(3, 1, 2));
}

TEST(EigenTest, TestStableRotateBlockUpperLeftSize2) {
  // 0, 1, 2,
  // 3, 4, 5,
  // 6, 7, 8

  Eigen::Matrix<double, 6, 6> H;
  Eigen::Matrix<double, 6, 1> b;
  int k = 0;
  for (int i = 0; i < 3; ++i) {
    b.segment<2>(i * 2).setConstant(i);
    for (int j = 0; j < 3; ++j) {
      H.block<2, 2>(i * 2, j * 2).setConstant(k);
      ++k;
    }
  }

  LOG(INFO) << "\n" << H;
  LOG(INFO) << "\n" << b.transpose();

  StableRotateBlockTopLeft(H, b, 2, 2);

  LOG(INFO) << "\n" << H;
  LOG(INFO) << "\n" << b.transpose();

  const auto H00 = H.topLeftCorner<2, 2>().eval();
  EXPECT_EQ(H00, Eigen::Matrix2d::Constant(8));

  const auto b0 = b.head<2>().eval();
  EXPECT_EQ(b0, Eigen::Vector2d::Constant(2));
}

TEST(EigenTest, TestStableRotateBlockUpperLeft) {
  using Matrix6d = MatrixMNd<6, 6>;
  using Vector6d = MatrixMNd<6, 1>;
  MatrixXd A = MatrixXd::Random(6, 100);
  Matrix6d H = A * A.transpose();

  Vector6d x;
  x << 1, 2, 3, 4, 5, 6;

  Vector6d b = H * x;
  Vector6d x0 = H.selfadjointView<Eigen::Lower>().llt().solve(b);
  EXPECT_EQ(x.isApprox(x0), true);

  // Now we rotate
  StableRotateBlockTopLeft(H, b, 1, 2);
  Vector6d x1 = H.selfadjointView<Eigen::Lower>().llt().solve(b);
  LOG(INFO) << x1.transpose();
}

TEST(EigenTest, TestFillLowerTriangular) {
  Eigen::Matrix3d M;
  // clang-format off
  M << 1, 2, 3,
       4, 5, 6,
       7, 8, 9;
  // clang-format on
  M = M.triangularView<Eigen::Upper>();

  Eigen::Matrix3d Mu;
  // clang-format off
  Mu << 1, 2, 3,
        0, 5, 6,
        0, 0, 9;
  // clang-format on
  EXPECT_EQ(M, Mu);

  Eigen::Matrix3d M0;
  // clang-format off
  M0 << 1, 2, 3,
        2, 5, 6,
        3, 6, 9;
  // clang-format on

  FillLowerTriangular(M);
  EXPECT_EQ(M, M0);
}

TEST(EigenTest, TestFillUpperTriangular) {
  Eigen::Matrix3d M;
  // clang-format off
  M << 1, 2, 3,
       4, 5, 6,
       7, 8, 9;
  // clang-format on
  M = M.triangularView<Eigen::Lower>();

  Eigen::Matrix3d Ml;
  // clang-format off
  Ml << 1, 0, 0,
        4, 5, 0,
        7, 8, 9;
  // clang-format on
  EXPECT_EQ(M, Ml);

  Eigen::Matrix3d M0;
  // clang-format off
  M0 << 1, 4, 7,
        4, 5, 8,
        7, 8, 9;
  // clang-format on

  FillUpperTriangular(M);
  EXPECT_EQ(M, M0);
}

TEST(EigenTest, TestMakeSymmetric) {
  MatrixXd A = MatrixXd::Random(5, 5);
  MakeSymmetric(A);
  CHECK_EQ(A, A.transpose()) << "\n" << A;
}

TEST(MargTest, TestMargTopLeftBlock) {
  MatrixXd Hsc = MatrixXd::Zero(4, 4);
  Hsc.topLeftCorner<2, 2>().setIdentity();
  Hsc.topRightCorner<2, 2>().setOnes();
  Hsc.bottomLeftCorner<2, 2>().setOnes();
  Hsc.bottomRightCorner<2, 2>().setIdentity();

  VectorXd bsc = VectorXd::Ones(4);
  MatrixXd Hpr = MatrixXd::Zero(2, 2);
  VectorXd bpr = VectorXd::Zero(2);

  MargTopLeftBlock(Hsc, bsc, Hpr, bpr, 2);

  MatrixXd Hpr0(2, 2);
  Hpr0 << -1, -2, -2, -1;
  VectorXd bpr0(2);
  bpr0 << -1, -1;
  EXPECT_EQ(Hpr, Hpr0);
  EXPECT_EQ(bpr, bpr0);
}

TEST(MargTest, TestMargTopLeftBlock2) {
  MatrixXd A = MatrixXd::Random(10, 40);
  MatrixXd Hsc = A * A.transpose();
  MakeSymmetric(Hsc);
  VectorXd bsc = VectorXd::Ones(10);

  MatrixXd Hpr(5, 5);
  VectorXd bpr(5);

  MargTopLeftBlock(Hsc, bsc, Hpr, bpr, 5);
  EXPECT_EQ(Hpr, Hpr.transpose()) << "\n" << Hpr;
  EXPECT_EQ(Hpr.isApprox(Hpr.transpose()), true) << "\n" << Hpr;
}

/// ============================================================================
void BM_CwiseInverse(bm::State& state) {
  Eigen::VectorXd v(state.range(0));
  v.setOnes();

  for (auto _ : state) {
    v = v.cwiseInverse();
    bm::DoNotOptimize(v);
  }
}
BENCHMARK(BM_CwiseInverse)->Arg(1024)->Arg(2048)->Arg(4096);

void BM_SafeCwiseInverse(bm::State& state) {
  Eigen::VectorXd v(state.range(0));
  v.setZero();
  for (int i = 0; i < v.size(); i += 2) {
    v[i] = 1;
  }

  for (auto _ : state) {
    SafeCwiseInverse(v);
    bm::DoNotOptimize(v);
  }
}
BENCHMARK(BM_SafeCwiseInverse)->Arg(1024)->Arg(2048)->Arg(4096);

/// ============================================================================
void BM_InvertPSDEigen(bm::State& state) {
  const auto n = state.range(0);
  Eigen::MatrixXd m(n, n);
  Eigen::MatrixXd m_inv(n, n);
  m.setIdentity();

  for (auto _ : state) {
    m_inv = m.inverse();
    bm::DoNotOptimize(m_inv);
  }
}
BENCHMARK(BM_InvertPSDEigen)->Arg(4)->Arg(8)->Arg(16)->Arg(32);

void BM_InvertPSDCeres(bm::State& state) {
  const auto n = state.range(0);
  Eigen::MatrixXd m(n, n);
  Eigen::MatrixXd m_inv(n, n);
  m.setIdentity();

  for (auto _ : state) {
    InvertPSDMatrix(m, m_inv);
    bm::DoNotOptimize(m_inv);
  }
}
BENCHMARK(BM_InvertPSDCeres)->Arg(4)->Arg(8)->Arg(16)->Arg(32);

}  // namespace
}  // namespace sv


// date_time_gregorian.cpp
//
#include <iostream>
#include <string>

#include <boost/date_time/gregorian/gregorian.hpp>

int main(int argc, char* argv[])
{
    namespace greg = boost::gregorian;

    greg::date d(2015, 4, 1);
    std::cout << d.year() << ' '
              << d.month() << ' '
              << d.day() << ' '
              << d.day_of_week() << '\n';
    greg::date d1 = d + greg::date_duration(30);
    greg::date_period dp(d, d1);
    std::cout << std::boolalpha << dp.contains(greg::from_simple_string("2015-04-25")) << '\n';

    greg::date today = greg::day_clock::local_day();
    greg::date two_days_later = today + greg::days(2);
    greg::day_iterator it(today);
    while (*it != two_days_later)
        std::cout << *(++it) << '\n';
}


//  Copyright John Maddock 2006.
//  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)

#define BOOST_MATH_ASSERT_UNDEFINED_POLICY false

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

template <class RealType>
void instantiate(RealType)
{
   using namespace boost;
   using namespace boost::math;
   using namespace boost::math::concepts;

   function_requires<DistributionConcept<normal_distribution<RealType> > >();
   function_requires<DistributionConcept<beta_distribution<RealType> > >();
   function_requires<DistributionConcept<binomial_distribution<RealType> > >();
   function_requires<DistributionConcept<cauchy_distribution<RealType> > >();
   function_requires<DistributionConcept<bernoulli_distribution<RealType> > >();
   function_requires<DistributionConcept<chi_squared_distribution<RealType> > >();
   function_requires<DistributionConcept<exponential_distribution<RealType> > >();
   function_requires<DistributionConcept<extreme_value_distribution<RealType> > >();
   function_requires<DistributionConcept<fisher_f_distribution<RealType> > >();
   function_requires<DistributionConcept<gamma_distribution<RealType> > >();
   function_requires<DistributionConcept<students_t_distribution<RealType> > >();
   function_requires<DistributionConcept<pareto_distribution<RealType> > >();
   function_requires<DistributionConcept<poisson_distribution<RealType> > >();
   function_requires<DistributionConcept<rayleigh_distribution<RealType> > >();
   function_requires<DistributionConcept<weibull_distribution<RealType> > >();
   function_requires<DistributionConcept<lognormal_distribution<RealType> > >();
   function_requires<DistributionConcept<triangular_distribution<RealType> > >();
   function_requires<DistributionConcept<uniform_distribution<RealType> > >();
   function_requires<DistributionConcept<negative_binomial_distribution<RealType> > >();
   function_requires<DistributionConcept<non_central_chi_squared_distribution<RealType> > >();
}

int main()
{
   instantiate(float(0));
   instantiate(double(0));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   instantiate((long double)(0));
#endif
}


#include <string>
#include <sstream>
#include <iostream>
#include <map>
#include <regex>
#include <fstream>
#include <boost/algorithm/string.hpp>

#include "../common.hpp"

using namespace std;

map<int, long> mem;

long dfs(const vector<int> & adapters, int root) {
    assert(root >= 0 && root < adapters.size());

    if (root == adapters.size() - 1) {
        return 1;
    }

    long counts = 0;
    for (int i = root + 1; i < adapters.size(); i++) {
        // Gone too far
        if (adapters[root] + 3 < adapters[i]) {
            break;
        }

        if (mem.find(adapters[i]) != mem.end()) {
            counts += mem[adapters[i]];
        } else {
            counts += dfs(adapters, i);
        }
    }
    return mem[adapters[root]] = counts;
}

int main() {
    ifstream file ("2020/10.txt");
    if (!file.is_open()) {
        cout << "Failed to open file: " << strerror(errno) << endl;
        return -1;
    }

    int answer1 = 0;
    long answer2 = 0;

    vector<int> adapters;
    string line;
    while (getline(file, line, '\n')) {
        boost::trim(line);
        if (line == "") {
            continue;
        }

        adapters.push_back(stoi(line));
    }

    adapters.push_back(0);
    sort(adapters.begin(), adapters.end());
    adapters.push_back(adapters[adapters.size() - 1] + 3);

    map<int, int> diffs;
    for (int i = 1; i < adapters.size(); i++) {
        int diff = adapters[i] - adapters[i-1];

        diffs[diff]++;
    }

    answer1 = diffs[1] * diffs[3];
    answer2 = dfs(adapters, 0);

    cout << "Answer 10.1: " << answer1 << endl;
    cout << "Answer 10.2: " << answer2 << endl;

    file.close();
}


//  (C) Copyright John Maddock 2005.
//  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 NTL_STD_CXX
#  define NTL_STD_CXX
#endif

#include <NTL/RR.h>
#include <iostream>
#include <iomanip>

int main()
{
   NTL::RR r, root_two;
   r.SetPrecision(256);
   root_two.SetPrecision(256);
   r = 1.0;
   root_two = 2.0;
   root_two = NTL::sqrt(root_two);
   r /= root_two;
   NTL::RR lim = NTL::pow(NTL::RR(NTL::INIT_VAL, 2.0), NTL::RR(NTL::INIT_VAL, -128));
   NTL::RR::SetOutputPrecision(40);
   while(r > lim)
   {
      std::cout << "   { " << r << "L, " << NTL::log1p(r) << "L, " << NTL::expm1(r) << "L, }, \n";
      std::cout << "   { " << -r << "L, " << NTL::log1p(-r) << "L, " << NTL::expm1(-r) << "L, }, \n";
      r /= root_two;
   }
   return 0;
}


#include <boost/test/unit_test.hpp>
#include "functions/streaming.hh"
#include "functions/std_functions.hh"
#include "functions/polynomial.hh"
#include "functions/variables.hh"
#include "functions/operators.hh"
#include "functions/all_simplifications.hh"
#include <iostream>

BOOST_AUTO_TEST_CASE(streaming_test) {
  using namespace manifolds;
  auto p1 = x * x;
  auto p = p1(IP<1>() + x * x) + x * x * x;
  Stream2(std::cout, p) << "\n\n";
}


// Copyright (c) by respective owners including Yahoo!, Microsoft, and
// individual contributors. All rights reserved. Released under a BSD (revised)
// license as described in the file LICENSE.

#include "vw/core/loss_functions.h"

#include "test_common.h"
#include "vw/core/named_labels.h"

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

BOOST_AUTO_TEST_CASE(squared_loss_test)
{
  auto& vw = *VW::initialize("--quiet");
  const std::string loss_type("squared");

  auto loss = get_loss_function(vw, loss_type);
  shared_data sd;
  sd.min_label = 0.0f;
  sd.max_label = 1.0f;
  constexpr float eta = 0.1f;     // learning rate
  constexpr float weight = 1.0f;  // example weight

  constexpr float label = 0.5f;
  constexpr float prediction = 0.4f;
  constexpr float update_scale = eta * weight;
  constexpr float pred_per_update = 1.0f;  // Use dummy value here, see gd.cc for details.

  BOOST_CHECK_EQUAL(loss_type, loss->get_type());
  BOOST_CHECK_CLOSE(0.0f, loss->get_parameter(), FLOAT_TOL);

  BOOST_CHECK_CLOSE(0.01f, loss->get_loss(&sd, prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(0.01812692f, loss->get_update(prediction, label, update_scale, pred_per_update), FLOAT_TOL);
  BOOST_CHECK_CLOSE(0.02f, loss->get_unsafe_update(prediction, label, update_scale), FLOAT_TOL);

  BOOST_CHECK_CLOSE(0.04f, loss->get_square_grad(prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(-0.2f, loss->first_derivative(&sd, prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(2.0f, loss->second_derivative(&sd, prediction, label), FLOAT_TOL);

  VW::finish(vw);
}

BOOST_AUTO_TEST_CASE(expectile_loss_label_is_greater_than_prediction_test1)
{
  auto& vw = *VW::initialize("--quiet");
  const std::string loss_type("expectile");
  constexpr float parameter(0.4f);

  auto loss = get_loss_function(vw, loss_type, parameter);
  shared_data sd;
  sd.min_label = 0.0f;
  sd.max_label = 1.0f;
  constexpr float eta = 0.1f;     // learning rate
  constexpr float weight = 1.0f;  // example weight

  constexpr float label = 0.5f;
  constexpr float prediction = 0.4f;
  constexpr float update_scale = eta * weight;
  constexpr float pred_per_update = 1.0f;  // Use dummy value here, see gd.cc for details.

  BOOST_CHECK_EQUAL(loss_type, loss->get_type());
  BOOST_CHECK_CLOSE(parameter, loss->get_parameter(), FLOAT_TOL);

  BOOST_CHECK_CLOSE(0.006f, loss->get_loss(&sd, prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(0.011307956f, loss->get_update(prediction, label, update_scale, pred_per_update), FLOAT_TOL);
  BOOST_CHECK_CLOSE(0.012f, loss->get_unsafe_update(prediction, label, update_scale), FLOAT_TOL);

  BOOST_CHECK_CLOSE(0.0144f, loss->get_square_grad(prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(-0.12f, loss->first_derivative(&sd, prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(1.2f, loss->second_derivative(&sd, prediction, label), FLOAT_TOL);

  VW::finish(vw);
}

BOOST_AUTO_TEST_CASE(expectile_loss_label_is_greater_than_prediction_test2)
{
  auto& vw = *VW::initialize("--quiet");
  const std::string loss_type("expectile");
  constexpr float parameter(0.25f);

  auto loss = get_loss_function(vw, loss_type, parameter);
  shared_data sd;
  sd.min_label = 0.0f;
  sd.max_label = 1.0f;
  constexpr float eta = 0.1f;     // learning rate
  constexpr float weight = 1.0f;  // example weight

  constexpr float label = 0.74f;
  constexpr float prediction = 0.18f;
  constexpr float update_scale = eta * weight;
  constexpr float pred_per_update = 1.0f;  // Use dummy value here, see gd.cc for details.

  BOOST_CHECK_EQUAL(loss_type, loss->get_type());
  BOOST_CHECK_CLOSE(parameter, loss->get_parameter(), FLOAT_TOL);

  BOOST_CHECK_CLOSE(0.2352f, loss->get_loss(&sd, prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(0.0780035332f, loss->get_update(prediction, label, update_scale, pred_per_update), FLOAT_TOL);
  BOOST_CHECK_CLOSE(0.084f, loss->get_unsafe_update(prediction, label, update_scale), FLOAT_TOL);

  BOOST_CHECK_CLOSE(0.7056f, loss->get_square_grad(prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(-0.84f, loss->first_derivative(&sd, prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(1.5f, loss->second_derivative(&sd, prediction, label), FLOAT_TOL);

  VW::finish(vw);
}

BOOST_AUTO_TEST_CASE(expectile_loss_label_is_greater_than_prediction_test3)
{
  auto& vw = *VW::initialize("--quiet");
  const std::string loss_type("expectile");
  constexpr float parameter(0.2f);

  auto loss = get_loss_function(vw, loss_type, parameter);
  shared_data sd;
  sd.min_label = 0.0f;
  sd.max_label = 1.0f;
  constexpr float eta = 0.1f;     // learning rate
  constexpr float weight = 1.0f;  // example weight

  constexpr float label = 0.84f;
  constexpr float prediction = 0.48f;
  constexpr float update_scale = eta * weight;
  constexpr float pred_per_update = 1.0f;  // Use dummy value here, see gd.cc for details.

  BOOST_CHECK_EQUAL(loss_type, loss->get_type());
  BOOST_CHECK_CLOSE(parameter, loss->get_parameter(), FLOAT_TOL);

  BOOST_CHECK_CLOSE(0.10368f, loss->get_loss(&sd, prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(0.05322823597f, loss->get_update(prediction, label, update_scale, pred_per_update), FLOAT_TOL);
  BOOST_CHECK_CLOSE(0.0576f, loss->get_unsafe_update(prediction, label, update_scale), FLOAT_TOL);

  BOOST_CHECK_CLOSE(0.331776f, loss->get_square_grad(prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(-0.576f, loss->first_derivative(&sd, prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(1.6f, loss->second_derivative(&sd, prediction, label), FLOAT_TOL);

  VW::finish(vw);
}

BOOST_AUTO_TEST_CASE(expectile_loss_label_is_greater_than_prediction_test4)
{
  auto& vw = *VW::initialize("--quiet");
  const std::string loss_type("expectile");
  constexpr float parameter(0.3f);

  auto loss = get_loss_function(vw, loss_type, parameter);
  shared_data sd;
  sd.min_label = 0.0f;
  sd.max_label = 1.0f;
  constexpr float eta = 0.1f;     // learning rate
  constexpr float weight = 1.0f;  // example weight

  constexpr float label = 0.29f;
  constexpr float prediction = 0.06f;
  constexpr float update_scale = eta * weight;
  constexpr float pred_per_update = 1.0f;  // Use dummy value here, see gd.cc for details.

  BOOST_CHECK_EQUAL(loss_type, loss->get_type());
  BOOST_CHECK_CLOSE(parameter, loss->get_parameter(), FLOAT_TOL);

  BOOST_CHECK_CLOSE(0.03703f, loss->get_loss(&sd, prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(0.03004760586f, loss->get_update(prediction, label, update_scale, pred_per_update), FLOAT_TOL);
  BOOST_CHECK_CLOSE(0.0322f, loss->get_unsafe_update(prediction, label, update_scale), FLOAT_TOL);

  BOOST_CHECK_CLOSE(0.103684f, loss->get_square_grad(prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(-0.322f, loss->first_derivative(&sd, prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(1.4f, loss->second_derivative(&sd, prediction, label), FLOAT_TOL);

  VW::finish(vw);
}

BOOST_AUTO_TEST_CASE(expectile_loss_label_is_greater_than_prediction_test5)
{
  auto& vw = *VW::initialize("--quiet");
  const std::string loss_type("expectile");
  constexpr float parameter(0.25f);

  auto loss = get_loss_function(vw, loss_type, parameter);
  shared_data sd;
  sd.min_label = 0.0f;
  sd.max_label = 1.0f;
  constexpr float eta = 0.1f;     // learning rate
  constexpr float weight = 1.0f;  // example weight

  constexpr float label = 0.50f;
  constexpr float prediction = 0.09f;
  constexpr float update_scale = eta * weight;
  constexpr float pred_per_update = 1.0f;  // Use dummy value here, see gd.cc for details.

  BOOST_CHECK_EQUAL(loss_type, loss->get_type());
  BOOST_CHECK_CLOSE(parameter, loss->get_parameter(), FLOAT_TOL);

  BOOST_CHECK_CLOSE(0.126075f, loss->get_loss(&sd, prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(0.05710972967f, loss->get_update(prediction, label, update_scale, pred_per_update), FLOAT_TOL);
  BOOST_CHECK_CLOSE(0.0615f, loss->get_unsafe_update(prediction, label, update_scale), FLOAT_TOL);

  BOOST_CHECK_CLOSE(0.378225f, loss->get_square_grad(prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(-0.615f, loss->first_derivative(&sd, prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(1.5f, loss->second_derivative(&sd, prediction, label), FLOAT_TOL);

  VW::finish(vw);
}

BOOST_AUTO_TEST_CASE(expectile_loss_prediction_is_greater_than_label_test1)
{
  auto& vw = *VW::initialize("--quiet");
  const std::string loss_type("expectile");
  constexpr float parameter(0.4f);

  auto loss = get_loss_function(vw, loss_type, parameter);
  shared_data sd;
  sd.min_label = 0.0f;
  sd.max_label = 1.0f;
  constexpr float eta = 0.1f;     // learning rate
  constexpr float weight = 1.0f;  // example weight

  constexpr float label = 0.4f;
  constexpr float prediction = 0.5f;
  constexpr float update_scale = eta * weight;
  constexpr float pred_per_update = 1.0f;  // Use dummy value here, see gd.cc for details.

  BOOST_CHECK_EQUAL(loss_type, loss->get_type());
  BOOST_CHECK_CLOSE(parameter, loss->get_parameter(), FLOAT_TOL);

  BOOST_CHECK_CLOSE(0.004f, loss->get_loss(&sd, prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(-0.007688365f, loss->get_update(prediction, label, update_scale, pred_per_update), FLOAT_TOL);
  BOOST_CHECK_CLOSE(-0.008f, loss->get_unsafe_update(prediction, label, update_scale), FLOAT_TOL);

  BOOST_CHECK_CLOSE(0.0064f, loss->get_square_grad(prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(0.08f, loss->first_derivative(&sd, prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(0.8f, loss->second_derivative(&sd, prediction, label), FLOAT_TOL);

  VW::finish(vw);
}

BOOST_AUTO_TEST_CASE(expectile_loss_prediction_is_greater_than_label_test2)
{
  auto& vw = *VW::initialize("--quiet");
  const std::string loss_type("expectile");
  constexpr float parameter(0.25f);

  auto loss = get_loss_function(vw, loss_type, parameter);
  shared_data sd;
  sd.min_label = 0.0f;
  sd.max_label = 1.0f;
  constexpr float eta = 0.1f;     // learning rate
  constexpr float weight = 1.0f;  // example weight

  constexpr float label = 0.57f;
  constexpr float prediction = 0.64f;
  constexpr float update_scale = eta * weight;
  constexpr float pred_per_update = 1.0f;  // Use dummy value here, see gd.cc for details.

  BOOST_CHECK_EQUAL(loss_type, loss->get_type());
  BOOST_CHECK_CLOSE(parameter, loss->get_parameter(), FLOAT_TOL);

  BOOST_CHECK_CLOSE(0.001225f, loss->get_loss(&sd, prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(-0.003413940285f, loss->get_update(prediction, label, update_scale, pred_per_update), FLOAT_TOL);
  BOOST_CHECK_CLOSE(-0.0035f, loss->get_unsafe_update(prediction, label, update_scale), FLOAT_TOL);

  BOOST_CHECK_CLOSE(0.001225f, loss->get_square_grad(prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(0.035f, loss->first_derivative(&sd, prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(0.5f, loss->second_derivative(&sd, prediction, label), FLOAT_TOL);

  VW::finish(vw);
}

BOOST_AUTO_TEST_CASE(expectile_loss_prediction_is_greater_than_label_test3)
{
  auto& vw = *VW::initialize("--quiet");
  const std::string loss_type("expectile");
  constexpr float parameter(0.2f);

  auto loss = get_loss_function(vw, loss_type, parameter);
  shared_data sd;
  sd.min_label = 0.0f;
  sd.max_label = 1.0f;
  constexpr float eta = 0.1f;     // learning rate
  constexpr float weight = 1.0f;  // example weight

  constexpr float label = 0.79f;
  constexpr float prediction = 0.91f;
  constexpr float update_scale = eta * weight;
  constexpr float pred_per_update = 1.0f;  // Use dummy value here, see gd.cc for details.

  BOOST_CHECK_EQUAL(loss_type, loss->get_type());
  BOOST_CHECK_CLOSE(parameter, loss->get_parameter(), FLOAT_TOL);

  BOOST_CHECK_CLOSE(0.00288f, loss->get_loss(&sd, prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(-0.004705267302f, loss->get_update(prediction, label, update_scale, pred_per_update), FLOAT_TOL);
  BOOST_CHECK_CLOSE(-0.0048f, loss->get_unsafe_update(prediction, label, update_scale), FLOAT_TOL);

  BOOST_CHECK_CLOSE(0.002304f, loss->get_square_grad(prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(0.048f, loss->first_derivative(&sd, prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(0.4f, loss->second_derivative(&sd, prediction, label), FLOAT_TOL);

  VW::finish(vw);
}

BOOST_AUTO_TEST_CASE(expectile_loss_prediction_is_greater_than_label_test4)
{
  auto& vw = *VW::initialize("--quiet");
  const std::string loss_type("expectile");
  constexpr float parameter(0.2f);

  auto loss = get_loss_function(vw, loss_type, parameter);
  shared_data sd;
  sd.min_label = 0.0f;
  sd.max_label = 1.0f;
  constexpr float eta = 0.1f;     // learning rate
  constexpr float weight = 1.0f;  // example weight

  constexpr float label = 0.02f;
  constexpr float prediction = 0.29f;
  constexpr float update_scale = eta * weight;
  constexpr float pred_per_update = 1.0f;  // Use dummy value here, see gd.cc for details.

  BOOST_CHECK_EQUAL(loss_type, loss->get_type());
  BOOST_CHECK_CLOSE(parameter, loss->get_parameter(), FLOAT_TOL);

  BOOST_CHECK_CLOSE(0.01458f, loss->get_loss(&sd, prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(-0.01058685143f, loss->get_update(prediction, label, update_scale, pred_per_update), FLOAT_TOL);
  BOOST_CHECK_CLOSE(-0.0108f, loss->get_unsafe_update(prediction, label, update_scale), FLOAT_TOL);

  BOOST_CHECK_CLOSE(0.011664f, loss->get_square_grad(prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(0.108f, loss->first_derivative(&sd, prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(0.4f, loss->second_derivative(&sd, prediction, label), FLOAT_TOL);

  VW::finish(vw);
}

BOOST_AUTO_TEST_CASE(expectile_loss_prediction_is_greater_than_label_test5)
{
  auto& vw = *VW::initialize("--quiet");
  const std::string loss_type("expectile");
  constexpr float parameter(0.4f);

  auto loss = get_loss_function(vw, loss_type, parameter);
  shared_data sd;
  sd.min_label = 0.0f;
  sd.max_label = 1.0f;
  constexpr float eta = 0.1f;     // learning rate
  constexpr float weight = 1.0f;  // example weight

  constexpr float label = 0.24f;
  constexpr float prediction = 0.44f;
  constexpr float update_scale = eta * weight;
  constexpr float pred_per_update = 1.0f;  // Use dummy value here, see gd.cc for details.

  BOOST_CHECK_EQUAL(loss_type, loss->get_type());
  BOOST_CHECK_CLOSE(parameter, loss->get_parameter(), FLOAT_TOL);

  BOOST_CHECK_CLOSE(0.016f, loss->get_loss(&sd, prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(-0.01537673072f, loss->get_update(prediction, label, update_scale, pred_per_update), FLOAT_TOL);
  BOOST_CHECK_CLOSE(-0.016f, loss->get_unsafe_update(prediction, label, update_scale), FLOAT_TOL);

  BOOST_CHECK_CLOSE(0.0256f, loss->get_square_grad(prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(0.16f, loss->first_derivative(&sd, prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(0.8f, loss->second_derivative(&sd, prediction, label), FLOAT_TOL);

  VW::finish(vw);
}
BOOST_AUTO_TEST_CASE(expectile_loss_parameter_equals_zero_test)
{
  auto& vw = *VW::initialize("--quiet");
  const std::string loss_type("expectile");
  constexpr float parameter(0.0f);

  auto loss = get_loss_function(vw, loss_type, parameter);
  shared_data sd;
  sd.min_label = 0.0f;
  sd.max_label = 1.0f;
  constexpr float eta = 0.1f;     // learning rate
  constexpr float weight = 1.0f;  // example weight

  constexpr float label = 0.5f;
  constexpr float prediction = 0.4f;
  constexpr float update_scale = eta * weight;
  constexpr float pred_per_update = 1.0f;  // Use dummy value here, see gd.cc for details.

  BOOST_CHECK_EQUAL(loss_type, loss->get_type());
  BOOST_CHECK_CLOSE(parameter, loss->get_parameter(), FLOAT_TOL);

  BOOST_CHECK_CLOSE(0.01f, loss->get_loss(&sd, prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(0.01812692f, loss->get_update(prediction, label, update_scale, pred_per_update), FLOAT_TOL);
  BOOST_CHECK_CLOSE(0.02f, loss->get_unsafe_update(prediction, label, update_scale), FLOAT_TOL);

  BOOST_CHECK_CLOSE(0.04f, loss->get_square_grad(prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(-0.2f, loss->first_derivative(&sd, prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(2.0f, loss->second_derivative(&sd, prediction, label), FLOAT_TOL);

  VW::finish(vw);
}

BOOST_AUTO_TEST_CASE(expectile_loss_parameter_equals_one_test)
{
  auto& vw = *VW::initialize("--quiet");
  const std::string loss_type("expectile");
  constexpr float parameter(1.0f);

  auto loss = get_loss_function(vw, loss_type, parameter);
  shared_data sd;
  sd.min_label = 0.0f;
  sd.max_label = 1.0f;
  constexpr float eta = 0.1f;     // learning rate
  constexpr float weight = 1.0f;  // example weight

  constexpr float label = 0.5f;
  constexpr float prediction = 0.4f;
  constexpr float update_scale = eta * weight;
  constexpr float pred_per_update = 1.0f;  // Use dummy value here, see gd.cc for details.

  BOOST_CHECK_EQUAL(loss_type, loss->get_type());
  BOOST_CHECK_CLOSE(parameter, loss->get_parameter(), FLOAT_TOL);

  BOOST_CHECK_CLOSE(0.0f, loss->get_loss(&sd, prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(0.0f, loss->get_update(prediction, label, update_scale, pred_per_update), FLOAT_TOL);
  BOOST_CHECK_CLOSE(0.0f, loss->get_unsafe_update(prediction, label, update_scale), FLOAT_TOL);

  BOOST_CHECK_CLOSE(0.0f, loss->get_square_grad(prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(0.0f, loss->first_derivative(&sd, prediction, label), FLOAT_TOL);
  BOOST_CHECK_CLOSE(0.0f, loss->second_derivative(&sd, prediction, label), FLOAT_TOL);

  VW::finish(vw);
}

BOOST_AUTO_TEST_CASE(compare_expectile_loss_with_squared_loss_test)
{
  auto& vw = *VW::initialize("--quiet");
  const std::string loss_type_expectile("expectile");
  const std::string loss_type_squared("squared");
  constexpr float parameter(0.3f);

  auto loss_expectile = get_loss_function(vw, loss_type_expectile, parameter);
  auto loss_squared = get_loss_function(vw, loss_type_squared);
  shared_data sd;
  sd.min_label = 0.0f;
  sd.max_label = 1.0f;
  constexpr float eta = 0.1f;     // learning rate
  constexpr float weight = 1.0f;  // example weight

  constexpr float label = 0.4f;
  constexpr float prediction = 0.5f;
  constexpr float update_scale = eta * weight;
  constexpr float pred_per_update = 1.0f;  // Use dummy value here, see gd.cc for details.

  BOOST_CHECK_CLOSE(loss_expectile->get_loss(&sd, prediction, label),
      loss_squared->get_loss(&sd, prediction, label) * parameter, FLOAT_TOL);
  BOOST_CHECK_CLOSE(loss_expectile->get_update(prediction, label, update_scale, pred_per_update),
      loss_squared->get_update(prediction, label, update_scale * parameter, pred_per_update), FLOAT_TOL);
  BOOST_CHECK_CLOSE(loss_expectile->get_unsafe_update(prediction, label, update_scale),
      loss_squared->get_unsafe_update(prediction, label, update_scale * parameter), FLOAT_TOL);

  BOOST_CHECK_CLOSE(loss_expectile->get_square_grad(prediction, label),
      loss_squared->get_square_grad(prediction, label) * parameter * parameter, FLOAT_TOL);
  BOOST_CHECK_CLOSE(loss_expectile->first_derivative(&sd, prediction, label),
      loss_squared->first_derivative(&sd, prediction, label) * parameter, FLOAT_TOL);
  BOOST_CHECK_CLOSE(loss_expectile->second_derivative(&sd, prediction, label),
      loss_squared->second_derivative(&sd, prediction, label) * parameter, FLOAT_TOL);

  VW::finish(vw);
}


/*******************************************************************************
 * Copyright (c) 2014, 2015  IBM Corporation and others
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 *******************************************************************************/

#ifndef ArrayUtils_hpp
#define ArrayUtils_hpp

#include <stdio.h>
#include <iostream>
#include <algorithm>
#include <vector>
#include <cmath>

#include <Eigen/Core>

class ArrayUtils{

public:
    static void normalize(double outArray[], const double inArray[], int n);
    static std::vector<double> arrayToVector(double *array);

    static std::vector<double> computeWeightsFromLogLikelihood(std::vector<double> logLikelihoods);

    static Eigen::VectorXd vectorToEigenVector(std::vector<double>);
    static std::vector<double> eigenVectorToEigen(Eigen::VectorXd);
};

#endif /* ArrayUtils_hpp */


/* Copyright 2020, 2021 Evandro Chagas Ribeiro da Rosa <evandro.crr@posgrad.ufsc.br>
 * Copyright 2020, 2021 Rafael de Santiago <r.santiago@ufsc.br>
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "../include/ket_bitwise.hpp"
#include <boost/container/map.hpp>
#include <random>
#include <algorithm>

using namespace ket;
using namespace std::complex_literals;

Bitwise::Bitwise() {
    qbits[Index()] = 1;
}

void Bitwise::x(size_t idx, const ctrl_list& ctrl) {
    map qbits_tmp{};
    for (auto &i : qbits) {
        bool exec = true;
        for (auto j : ctrl) exec &= i.first.is_one(j);
        if (exec) {
            auto j = i.first;
            j.flip(idx);
            qbits_tmp[j] = i.second;
        } else {
            qbits_tmp[i.first] = i.second;
        }
    }
    qbits.swap(qbits_tmp);
}

void Bitwise::y(size_t idx, const ctrl_list& ctrl) {
    map qbits_tmp{};
    for (auto &i : qbits) {
        bool exec = true;
        for (auto j : ctrl) exec &= i.first.is_one(j);
        if (exec) {
            auto j = i.first;
            j.flip(idx);
            if (i.first.is_one(idx)) {
                qbits_tmp[j] = i.second*-1i;
            } else {
                qbits_tmp[j] = i.second*1i;
            }
        } else {
            qbits_tmp[i.first] = i.second;
        }
    }
    qbits.swap(qbits_tmp);
}

void Bitwise::z(size_t idx, const ctrl_list& ctrl) {
    for (auto &i : qbits) {
        bool exec = true;
        for (auto j : ctrl) exec &= i.first.is_one(j);
        if (exec and i.first.is_one(idx)) {
            qbits[i.first] *= -1;
        }
    }
}

void Bitwise::h(size_t idx, const ctrl_list& ctrl) {
    map qbits_tmp{};
    for (auto &i : qbits) {
        bool exec = true;
        for (auto j : ctrl) exec &= i.first.is_one(j);
        if (exec) {
            if (i.first.is_one(idx)) {
                qbits_tmp[i.first] -= i.second/std::sqrt(2);
            } else {
                qbits_tmp[i.first] += i.second/std::sqrt(2);
            }
            if (std::abs(qbits_tmp[i.first]) < 1e-10) {
                qbits_tmp.erase(i.first);
            }
            auto j = i.first;
            j.flip(idx);
            qbits_tmp[j] += i.second/std::sqrt(2);
            if (std::abs(qbits_tmp[j]) < 1e-10) {
                qbits_tmp.erase(j);
            }
        } else {
            qbits_tmp[i.first] = i.second;
        }

    }
    qbits.swap(qbits_tmp);
}

void Bitwise::s(size_t idx, const ctrl_list& ctrl) {
    for (auto &i : qbits) {
        bool exec = true;
        for (auto j : ctrl) exec &= i.first.is_one(j);
        if (exec and i.first.is_one(idx)) {
            qbits[i.first] *= 1i;
        }
    }
}

void Bitwise::sd(size_t idx, const ctrl_list& ctrl) {
    for (auto &i : qbits) {
        bool exec = true;
        for (auto j : ctrl) exec &= i.first.is_one(j);
        if (exec and i.first.is_one(idx)) {
            qbits[i.first] *= -1i;
        }
    }
}

void Bitwise::t(size_t idx, const ctrl_list& ctrl) {
    for (auto &i : qbits) {
        bool exec = true;
        for (auto j : ctrl) exec &= i.first.is_one(j);
        if (exec and i.first.is_one(idx)) {
            qbits[i.first] *= std::exp(1i*M_PI/4.0);
        }
    }
}

void Bitwise::td(size_t idx, const ctrl_list& ctrl) {
    for (auto &i : qbits) {
        bool exec = true;
        for (auto j : ctrl) exec &= i.first.is_one(j);
        if (exec and i.first.is_one(idx)) {
            qbits[i.first] *= std::exp(-1i*M_PI/4.0);
        }
    }
}

void Bitwise::cnot(size_t ctrl, size_t target,  const ctrl_list& ctrl2) {
    map qbits_tmp{};

    for (auto &i : qbits) {
        bool exec = true;
        for (auto j : ctrl2) exec &= i.first.is_one(j);
        if (exec and i.first.is_one(ctrl)) {
            auto j = i.first;
            j.flip(target);
            qbits_tmp[j] = i.second;
        } else {
            qbits_tmp[i.first] = i.second;
        }
    }

    qbits.swap(qbits_tmp);
}

void Bitwise::p(double lambda, size_t idx, const ctrl_list& ctrl) {
    for (auto &i : qbits) {
        bool exec = true;
        for (auto j : ctrl) exec &= i.first.is_one(j);
        if (exec and i.first.is_one(idx)) {
            qbits[i.first] *= std::exp(1i*lambda);
        }
    }
}

void Bitwise::u2(double phi, double lambda, size_t idx, const ctrl_list& ctrl) {
    map qbits_tmp{};
    for (auto &i : qbits) {
        bool exec = true;
        for (auto j : ctrl) exec &= i.first.is_one(j);
        if (exec) {
            auto j = i.first;
            j.flip(idx);
            if (i.first.is_one(idx)) {
                qbits_tmp[i.first] += i.second*std::exp(1i*(lambda+phi))/std::sqrt(2);
                if (std::abs(qbits_tmp[i.first]) < 1e-10)
                    qbits_tmp.erase(i.first);

                qbits_tmp[j] -= i.second*std::exp(1i*lambda)/std::sqrt(2);
                if (std::abs(qbits_tmp[j]) < 1e-10)
                    qbits_tmp.erase(j);
            } else {
                qbits_tmp[i.first] += i.second/sqrt(2);
                if (std::abs(qbits_tmp[i.first]) < 1e-10)
                    qbits_tmp.erase(i.first);

                qbits_tmp[j] += i.second*std::exp(1i*phi)/std::sqrt(2);
                if (std::abs(qbits_tmp[j]) < 1e-10)
                    qbits_tmp.erase(j);
            }

        } else {
            qbits_tmp[i.first] = i.second;
        }
    }

    qbits.swap(qbits_tmp);
}

void Bitwise::u3(double theta, double phi, double lambda, size_t idx, const ctrl_list& ctrl) {
    map qbits_tmp{};
    std::complex<double> amp;
    for (auto &i : qbits) {
        bool exec = true;
        for (auto j : ctrl) exec &= i.first.is_one(j);
        if (exec) {
            auto j = i.first;
            j.flip(idx);
            if (i.first.is_one(idx)) {
                amp = std::exp(1i*(lambda+phi))*std::cos(theta/2);
                qbits_tmp[i.first] += i.second*amp;
                if (std::abs(qbits_tmp[i.first]) < 1e-10)
                    qbits_tmp.erase(i.first);

                amp = std::exp(1i*lambda)*std::sin(theta/2);
                qbits_tmp[j] -= i.second*amp;
                if (std::abs(qbits_tmp[j]) < 1e-10)
                    qbits_tmp.erase(j);
            } else {
                amp = std::cos(theta/2);
                qbits_tmp[i.first] += i.second*amp;
                if (std::abs(qbits_tmp[i.first]) < 1e-10)
                    qbits_tmp.erase(i.first);

                amp = std::exp(1i*phi)*std::sin(theta/2);
                qbits_tmp[j] += i.second*amp;
                if (std::abs(qbits_tmp[j]) < 1e-10)
                    qbits_tmp.erase(j);
            }
        } else {
            qbits_tmp[i.first] = i.second;
        }
    }

    qbits.swap(qbits_tmp);
}

void Bitwise::rx(double theta, size_t idx, const ctrl_list& ctrl) {
    u3(theta, -M_PI_2, M_PI_2, idx, ctrl);
}

void Bitwise::ry(double theta, size_t idx, const ctrl_list& ctrl) {
    u3(theta, 0, 0, idx, ctrl);
}

void Bitwise::rz(double lambda, size_t idx, const ctrl_list& ctrl) {
    for (auto &i : qbits) {
        bool exec = true;
        for (auto j : ctrl) exec &= i.first.is_one(j);
        if (exec) {
            if (i.first.is_one(idx))
                qbits[i.first] *= std::exp(1i*lambda/2.0);
            else
                qbits[i.first] *= std::exp(-1i*lambda/2.0);
        }
    }
}

int Bitwise::measure(size_t idx) {
    double p = 0;

    for (auto &i : qbits) {
        if (i.first.is_zero(idx)) {
            p += std::pow(std::abs(i.second), 2);
        }
    }

    auto result = p != 0 and
                  (double(std::rand()) / double(RAND_MAX) <= p)?
                  0 : 1;

    p = result == 0? std::sqrt(p) : std::sqrt(1.0-p);

    map qbits_tmp{};

    for (auto &i : qbits)
        if (i.first.is_zero(idx) xor result)
            qbits_tmp[i.first] = i.second/p;

    qbits.swap(qbits_tmp);
    return result;
}

void Bitwise::measure_zero(size_t idx) {
    double p = 0;

    for (auto &i : qbits)
        if (i.first.is_zero(idx))
            p += std::pow(std::abs(i.second), 2);

    auto result = p != 0 and
                  (double(std::rand()) / double(RAND_MAX) <= p)?
                  0 : 1;

    p = result == 0? std::sqrt(p) : std::sqrt(1.0-p);

    map qbits_tmp{};

    for (auto &i : qbits) {
        if (i.first.is_zero(idx) xor result) {
            if (result == 0) {
                qbits_tmp[i.first] = i.second/p;
            } else {
                auto j = i.first;
                j.flip(idx);
                qbits_tmp[j] = i.second/p;
            }
        }
    }

    qbits.swap(qbits_tmp);
}

std::ostream& ket::operator<<(std::ostream &os, const Bitwise& q) {
    boost::container::map<Index, complex> sorted;
    sorted.insert(q.qbits.begin(), q.qbits.end());
    for (auto &i : sorted) {
        os << i.first << ' ' << i.second << std::endl;
    }
    return os;
}

Bitwise::Bitwise(const Bitwise& a, const Bitwise& b) {
    for (const auto &i: a.qbits) for (const auto &j: b.qbits)
        qbits[i.first|j.first] = i.second*j.second;
}

void Bitwise::swap(size_t a, size_t b) {
    map qbits_tmp{};
    for (auto &i : qbits) {
        if (i.first.is_one(a) != i.first.is_one(b)) {
            auto j = i.first;
            j.flip(a);
            j.flip(b);
            qbits_tmp[j] = i.second;
        } else {
            qbits_tmp[i.first] = i.second;
        }
    }
    qbits.swap(qbits_tmp);
}

map& Bitwise::get_map() {
    return qbits;
}

dump_t Bitwise::dump(size_t size) const {
    dump_t state;

    for (auto &i : qbits) {
        std::vector<uint64_t> tmp_state;
        for (auto j = 0ul; j < size/64; j++)
            tmp_state.push_back(i.first[j]);
        tmp_state.push_back(i.first[size/64] & ((1ul << size%64) -1));
        state[tmp_state].push_back(i.second);
    }
    for (auto &i : state) {
        std::sort(i.second.begin(), i.second.end(), [](std::complex<double> a, std::complex<double> b) {
            if (a.real() == b.real()) return a.imag() < b.imag();
            else return a.real() < b.real();
        });
    }

    return state;
}


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

  This source file is part of the Avogadro project.

  Copyright 2012-2014 Kitware, Inc.

  This source code is released under the New BSD License, (the "License").

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

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

#include "camera.h"

#include <Eigen/LU>

#include <cmath>

namespace Avogadro {
namespace Rendering {

Camera::Camera() : m_width(0), m_height(0),
  m_projectionType(Perspective), m_orthographicScale(1.0)
{
  m_projection.setIdentity();
  m_modelView.setIdentity();
}

Camera::~Camera()
{
}

void Camera::translate(const Vector3f &translate_)
{
  m_modelView.translate(translate_);
}

void Camera::preTranslate(const Vector3f &translate_)
{
  m_modelView.pretranslate(translate_);
}

void Camera::rotate(float angle, const Vector3f &axis)
{
  m_modelView.rotate(Eigen::AngleAxisf(angle, axis));
}

void Camera::preRotate(float angle, const Vector3f &axis)
{
  m_modelView.prerotate(Eigen::AngleAxisf(angle, axis));
}

void Camera::scale(float s)
{
  if (m_projectionType == Perspective)
    m_modelView.scale(s);
  else
    m_orthographicScale *= s;
}

void Camera::lookAt(const Vector3f &eye, const Vector3f &center,
                    const Vector3f &up)
{
  Vector3f f = (center - eye).normalized();
  Vector3f u = up.normalized();
  Vector3f s = f.cross(u).normalized();
  u = s.cross(f);

  m_modelView.setIdentity();
  m_modelView(0, 0) = s.x();
  m_modelView(0, 1) = s.y();
  m_modelView(0, 2) = s.z();
  m_modelView(1, 0) = u.x();
  m_modelView(1, 1) = u.y();
  m_modelView(1, 2) = u.z();
  m_modelView(2, 0) =-f.x();
  m_modelView(2, 1) =-f.y();
  m_modelView(2, 2) =-f.z();
  m_modelView(0, 3) =-s.dot(eye);
  m_modelView(1, 3) =-u.dot(eye);
  m_modelView(2, 3) = f.dot(eye);
}

float Camera::distance(const Vector3f &point) const
{
  return (m_modelView * point).norm();
}

Vector3f Camera::project(const Vector3f &point) const
{
  Eigen::Matrix4f mvp = m_projection.matrix() * m_modelView.matrix();
  Vector4f tPoint(point.x(), point.y(), point.z(), 1.0f);
  tPoint = mvp * tPoint;
  Vector3f result(static_cast<float>(m_width)
                  * (tPoint.x() / tPoint.w() + 1.0f) / 2.0f,
                  static_cast<float>(m_height)
                  * (tPoint.y() / tPoint.w() + 1.0f) / 2.0f,
                  (tPoint.z() / tPoint.w() + 1.0f) / 2.0f);
  return result;
}

Vector3f Camera::unProject(const Vector3f &point) const
{
  Eigen::Matrix4f mvp = m_projection.matrix() * m_modelView.matrix();
  Vector4f result(2.0f * point.x() / static_cast<float>(m_width) - 1.0f,
                  2.0f * (static_cast<float>(m_height) - point.y()) /
                  static_cast<float>(m_height) - 1.0f,
                  2.0f * point.z() - 1.0f,
                  1.0f);
  result = mvp.matrix().inverse() * result;
  return Vector3f(result.x() / result.w(), result.y() / result.w(),
                  result.z() / result.w());
}

Vector3f Camera::unProject(const Vector2f &point,
                           const Vector3f &reference) const
{
  return unProject(Vector3f(point.x(), point.y(), project(reference).z()));
}

void Camera::calculatePerspective(float fieldOfView, float aspectRatio,
                                  float zNear, float zFar)
{
  m_projection.setIdentity();
  float f = 1.0f / std::tan(fieldOfView * float(M_PI) / 360.0f);
  m_projection(0, 0) = f / aspectRatio;
  m_projection(1, 1) = f;
  m_projection(2, 2) = (zNear + zFar) / (zNear - zFar);
  m_projection(2, 3) = (2.0f * zFar * zNear) / (zNear - zFar);
  m_projection(3, 2) = -1;
  m_projection(3, 3) = 0;
}

void Camera::calculatePerspective(float fieldOfView, float zNear, float zFar)
{
  calculatePerspective(fieldOfView, static_cast<float>(m_width) /
                       static_cast<float>(m_height), zNear, zFar);
}

void Camera::calculateOrthographic(float left, float right,
                                   float bottom, float top,
                                   float zNear, float zFar)
{
  left *= m_orthographicScale;
  right *= m_orthographicScale;
  bottom *= m_orthographicScale;
  top *= m_orthographicScale;
  m_projection.setIdentity();
  m_projection(0, 0) = 2.0f / (right - left);
  m_projection(0, 3) = -(right + left) / (right - left);
  m_projection(1, 1) = 2.0f / (top - bottom);
  m_projection(1, 3) = -(top + bottom) / (top - bottom);
  m_projection(2, 2) = -2.0f / (zFar - zNear);
  m_projection(2, 3) = -(zFar + zNear) / (zFar - zNear);
  m_projection(3, 3) = 1;
}

void Camera::setViewport(int w, int h)
{
  m_width = w;
  m_height = h;
}

void Camera::setProjection(const Eigen::Affine3f &transform)
{
  m_projection = transform;
}

void Camera::setModelView(const Eigen::Affine3f &transform)
{
  m_modelView = transform;
}

} // End Rendering namespace
} // End Avogadro namespace