/*-----------------------------------------------------------------------------+
Interval Container Library
Author: Joachim Faulhaber
Copyright (c) 2007-2009: Joachim Faulhaber
Copyright (c) 1999-2006: Cortex Software GmbH, Kantstrasse 57, Berlin
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
/** Example month_and_week_grid.cpp \file month_and_week_grid.cpp
    \brief Creating and combining time grids.

    A split_interval_set preserves all interval borders on insertion
    and intersection operations. So given a split_interval_set ...
    \code
    x =  {[1,     3)}
    x.add(     [2,     4)) then
    x == {[1,2)[2,3)[3,4)}
    \endcode
    ... using this property we can intersect splitting interval containers
    in order to iterate over intervals accounting for all changes of
    interval borders.

    In this example we provide an intersection of two split_interval_sets
    representing a month and week time grid.

    \include month_and_week_grid_/month_and_week_grid.cpp
*/
//[example_month_and_week_grid
// The next line includes <boost/gregorian/date.hpp>
// and a few lines of adapter code.
#include <boost/icl/gregorian.hpp>
#include <iostream>
#include <boost/icl/split_interval_set.hpp>

using namespace std;
using namespace boost::gregorian;
using namespace boost::icl;

typedef split_interval_set<boost::gregorian::date> date_grid;

// This function splits a gregorian::date interval 'scope' into a month grid:
// For every month contained in 'scope' that month is contained as interval
// in the resulting split_interval_set.
date_grid month_grid(const discrete_interval<date>& scope)
{
    split_interval_set<date> month_grid;

    date frame_months_1st = first(scope).end_of_month() + days(1) - months(1);
    month_iterator month_iter(frame_months_1st);

    for(; month_iter <= last(scope); ++month_iter)
        month_grid += discrete_interval<date>::right_open(*month_iter, *month_iter + months(1));

    month_grid &= scope; // cut off the surplus

    return month_grid;
}

// This function splits a gregorian::date interval 'scope' into a week grid:
// For every week contained in 'scope' that month is contained as interval
// in the resulting split_interval_set.
date_grid week_grid(const discrete_interval<date>& scope)
{
    split_interval_set<date> week_grid;

    date frame_weeks_1st = first(scope) + days(days_until_weekday(first(scope), greg_weekday(Monday))) - weeks(1);
    week_iterator week_iter(frame_weeks_1st);

    for(; week_iter <= last(scope); ++week_iter)
        week_grid.insert(discrete_interval<date>::right_open(*week_iter, *week_iter + weeks(1)));

    week_grid &= scope; // cut off the surplus

    return week_grid;
}

// For a period of two months, starting from today, the function
// computes a partitioning for months and weeks using intersection
// operator &= on split_interval_sets.
void month_and_time_grid()
{
    date someday = day_clock::local_day();
    date thenday = someday + months(2);

    discrete_interval<date> itv = discrete_interval<date>::right_open(someday, thenday);

    // Compute a month grid
    date_grid month_and_week_grid = month_grid(itv);
    // Intersection of the month and week grids:
    month_and_week_grid &= week_grid(itv);

    cout << "interval : " << first(itv) << " - " << last(itv)
         << " month and week partitions:" << endl;
    cout << "---------------------------------------------------------------\n";

    for(date_grid::iterator it = month_and_week_grid.begin();
        it != month_and_week_grid.end(); it++)
    {
        if(first(*it).day() == 1)
            cout << "new month: ";
        else if(first(*it).day_of_week()==greg_weekday(Monday))
            cout << "new week : " ;
        else if(it == month_and_week_grid.begin())
            cout << "first day: " ;
        cout << first(*it) << " - " << last(*it) << endl;
    }
}

int main()
{
    cout << ">>Interval Container Library: Sample month_and_time_grid.cpp <<\n";
    cout << "---------------------------------------------------------------\n";
    month_and_time_grid();
    return 0;
}

// Program output:
/*
>>Interval Container Library: Sample month_and_time_grid.cpp <<
---------------------------------------------------------------
interval : 2008-Jun-22 - 2008-Aug-21 month and week partitions:
---------------------------------------------------------------
first day: 2008-Jun-22 - 2008-Jun-22
new week : 2008-Jun-23 - 2008-Jun-29
new week : 2008-Jun-30 - 2008-Jun-30
new month: 2008-Jul-01 - 2008-Jul-06
new week : 2008-Jul-07 - 2008-Jul-13
new week : 2008-Jul-14 - 2008-Jul-20
new week : 2008-Jul-21 - 2008-Jul-27
new week : 2008-Jul-28 - 2008-Jul-31
new month: 2008-Aug-01 - 2008-Aug-03
new week : 2008-Aug-04 - 2008-Aug-10
new week : 2008-Aug-11 - 2008-Aug-17
new week : 2008-Aug-18 - 2008-Aug-21
*/
//]


/*
 * Copyright (c) 2015, The Regents of the University of California (Regents).
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 *    1. Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *
 *    2. Redistributions in binary form must reproduce the above
 *       copyright notice, this list of conditions and the following
 *       disclaimer in the documentation and/or other materials provided
 *       with the distribution.
 *
 *    3. Neither the name of the copyright holder nor the names of its
 *       contributors may be used to endorse or promote products derived
 *       from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS AS IS
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * Please contact the author(s) of this library if you have any questions.
 * Authors: Erik Nelson            ( eanelson@eecs.berkeley.edu )
 *          David Fridovich-Keil   ( dfk@eecs.berkeley.edu )
 */

#include "eight_point_algorithm_solver.h"

#include <Eigen/SVD>
#include <glog/logging.h>

#include "normalization.h"

namespace bsfm {

bool EightPointAlgorithmSolver::ComputeFundamentalMatrix(
    const FeatureMatchList& matched_features,
    Matrix3d& fundamental_matrix) const {
  // Following: https://www8.cs.umu.se/kurser/TDBD19/VT05/reconstruct-4.pdf

  // First make sure we even have enough matches to run the eight-point
  // algorithm.
  if (matched_features.size() < 8) {
    VLOG(1) << "Cannot use the eight-point algorithm with less than 8 feature "
               "matches.";
    return false;
  }

  // Build the A matrix from matched features.
  MatrixXd A;
  A.resize(matched_features.size(), 9);

  // If requested, normalize feature positions prior to computing the A matrix.
  Matrix3d T1(MatrixXd::Identity(3, 3));
  Matrix3d T2(MatrixXd::Identity(3, 3));
  if (options_.normalize_features) {
    T1 = ComputeNormalization(matched_features, true /*feature set 1*/);
    T2 = ComputeNormalization(matched_features, false /*feature set 2*/);
  }

  // Incrementally add rows to A.
  for (size_t ii = 0; ii < matched_features.size(); ++ii) {
    double u1 = matched_features[ii].feature1_.u_;
    double v1 = matched_features[ii].feature1_.v_;
    double u2 = matched_features[ii].feature2_.u_;
    double v2 = matched_features[ii].feature2_.v_;

    if (options_.normalize_features) {
      u1 = T1(0, 0) * u1 + T1(0, 2);
      v1 = T1(1, 1) * v1 + T1(1, 2);
      u2 = T2(0, 0) * u2 + T2(0, 2);
      v2 = T2(1, 1) * v2 + T2(1, 2);
    }

    A(ii, 0) = u1*u2;
    A(ii, 1) = v1*u2;
    A(ii, 2) =    u2;
    A(ii, 3) = u1*v2;
    A(ii, 4) = v1*v2;
    A(ii, 5) =    v2;
    A(ii, 6) =    u1;
    A(ii, 7) =    v1;
    A(ii, 8) =     1;
  }

  // Get svd(A). Save some time and compute a thin U. We still need a full V.
  Eigen::JacobiSVD<Eigen::MatrixXd> svd;
  svd.compute(A, Eigen::ComputeThinU | Eigen::ComputeFullV);
  if (!svd.computeV()) {
    VLOG(1) << "Failed to compute a singular value decomposition of A matrix.";
    return false;
  }

  // Get the fundamental matrix elements from the SVD decomposition.
  const VectorXd f_vec = svd.matrixV().col(8);

  // Turn the elements of the fundamental matrix into an actual matrix.
  fundamental_matrix.row(0) = f_vec.topRows(3).transpose();
  fundamental_matrix.row(1) = f_vec.middleRows(3, 3).transpose();
  fundamental_matrix.row(2) = f_vec.bottomRows(3).transpose();

  // If requested, make sure that the computed fundamental matrix has rank 2.
  // This is step 2 of the eight-point algorithm from the slides.
  if (options_.enforce_fundamental_matrix_rank_deficiency) {
    // Get svd(F). We need full U and V to reconstruct the rank deficient F.
    svd.compute(fundamental_matrix, Eigen::ComputeFullU | Eigen::ComputeFullV);
    svd.compute(fundamental_matrix);
    if (!svd.computeU() || !svd.computeV()) {
      VLOG(1) << "Failed to compute a singular value decomposition of "
                 "fundamental matrix.";
      return false;
    }

    // Build a matrix of the first 8 singular values down the diagonal. Make the
    // last diagonal entry 0.
    MatrixXd S_deficient(MatrixXd::Zero(3, 3));
    S_deficient(0, 0) = svd.singularValues()(0);
    S_deficient(1, 1) = svd.singularValues()(1);
    fundamental_matrix = svd.matrixU() * S_deficient * svd.matrixV().transpose();
  }

  // If normalization was requested, we need to 'un-normalize' the fundamental
  // matrix.
  if (options_.normalize_features) {
    fundamental_matrix = T2.transpose() * fundamental_matrix * T1;
  }

  return true;
}

}  //\namespace bsfm


#include <Eigen/Core>
#include "test/catch.hpp"
#include "rose499/spline.hpp"

using namespace Eigen;

SCENARIO( "waypoints can be splined with a 5th order C^2 polynomial", "[spline]" )
{
    GIVEN( "trivial linear sequence of waypoints" )
    {
        constexpr auto SegmentCount = 1;
        Matrix<Spline::ValueType, 2, 2> waypoints;
        waypoints(0, 0) = 0; waypoints(1, 0) = 0;
        waypoints(0, 1) = 1; waypoints(1, 1) = 1;

        WHEN( "splined via contructor" )
        {
            Spline spline(waypoints);
            THEN( "waypoints should be on the spline" )
            {
                for(auto i = 0; i < waypoints.cols(); ++i)
                {
                    auto splinePoint = spline((i * 1.0) / SegmentCount);
                    auto waypoint = waypoints.col(i);
                    REQUIRE((splinePoint - waypoint).isZero() );
                }
            }
        }

        WHEN( "splined via contructor with a direction constraint" )
        {
            Spline spline(waypoints, std::atan2(0, 1));
            THEN( "waypoints should be on the spline" )
            {
                for(auto i = 0; i < waypoints.cols(); ++i)
                {
                    auto splinePoint = spline((i * 1.0) / SegmentCount);
                    auto waypoint = waypoints.col(i);
                    REQUIRE( (splinePoint - waypoint).norm() == Approx(0).margin(1e-12) );
                }
            }

            THEN( "derivative of spline at initial point is what was requested" )
            {
                auto slope = spline(0, 1);
                CAPTURE( slope );
                REQUIRE( std::atan2(slope[1], slope[0]) == Approx(std::atan2(0, 1)) );
            }
        }
    }

    GIVEN( "non-trivial sequence of waypoints" )
    {
        constexpr auto SegmentCount = 5 - 1;
        Matrix<Spline::ValueType, 2, 5> waypoints;
        waypoints(0, 0) = 0; waypoints(1, 0) = 0;
        waypoints(0, 1) = 2; waypoints(1, 1) = 1;
        waypoints(0, 2) = 4; waypoints(1, 2) = 0.5;
        waypoints(0, 3) = 6; waypoints(1, 3) = -0.5;
        waypoints(0, 4) = 3; waypoints(1, 4) = -1;

        WHEN( "splined via contructor" )
        {
            Spline spline(waypoints, std::atan2(1, 1));
            THEN( "waypoints should be on the spline" )
            {
                for(auto i = 0; i < waypoints.cols(); ++i)
                {
                    auto splinePoint = spline((i * 1.0) / SegmentCount);
                    auto waypoint = waypoints.col(i);
                    CAPTURE( spline.poly() );
                    REQUIRE( (splinePoint - waypoint).norm() == Approx(0.0).margin(1e-12) );
                }
            }

            THEN( "derivative of spline at initial point is what was requested" )
            {
                auto slope = spline(0, 1);
                CAPTURE( slope );
                REQUIRE( std::atan2(slope[1], slope[0]) == Approx(std::atan2(1, 1)) );
            }

            THEN( "spline should observe C1 continuity on all intermediate points" )
            {
                for(auto i = 1; i < waypoints.cols() - 1; ++i)
                {
                    auto derivativeFromLeft = spline((i * 1.0) / SegmentCount - 1e-12, 1);
                    auto derivativeFromRight = spline((i * 1.0) / SegmentCount + 1e-12, 1);
                    CAPTURE( spline.dpoly() );
                    CAPTURE( i );
                    REQUIRE( (derivativeFromLeft - derivativeFromRight).norm() == Approx(0.0).margin(1e-12) );
                }
            }

            THEN( "spline should observe C2 continuity on all intermediate points" )
            {
                for(auto i = 1; i < waypoints.cols() - 1; ++i)
                {
                    auto derivativeFromLeft = spline((i * 1.0) / SegmentCount - 1e-12, 2);
                    auto derivativeFromRight = spline((i * 1.0) / SegmentCount + 1e-12, 2);
                    CAPTURE( spline.ddpoly() );
                    CAPTURE( i );
                    REQUIRE( (derivativeFromLeft - derivativeFromRight).norm() == Approx(0.0).margin(1e-12) );
                }
            }
        }

        GIVEN( "a working spline" )
        {
            Spline spline(waypoints);

            WHEN( "finding the nearest point on the curve to the end point" )
            {
                Spline::ValueType lambda = spline.nearestPoint(waypoints.col(waypoints.cols()-1), 0.9);

                THEN( "it ought to be 1")
                {
                    REQUIRE( lambda == Approx(1.0).margin(1e-12) );
                }
            }

            WHEN( "finding the nearest point on the curve to some non-trivial point" )
            {
                auto point = Matrix<Spline::ValueType, 2, 1>::Constant(1);
                Spline::ValueType lambda = spline.nearestPoint(point, 0.1);

                THEN( "the tangent of the closest point on the curve should be orthogonal to the error" )
                {
                    CAPTURE( lambda );
                    REQUIRE( spline(lambda, 1).dot(spline(lambda) - point) == Approx(0.0).margin(1e-12) );
                }
            }
        }
    }
}


//==============================================================================
//         Copyright 2015 INSTITUT PASCAL UMR 6602 CNRS/Univ. Clermont II
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================

#include <iostream>
#include <fstream>

#include <boost/lexical_cast.hpp>
#include <boost/math/special_functions/sinc.hpp>

//#define USE_TOON
#include <libv/lma/lma.hpp>

namespace v
{
  Eigen::Matrix3d rotation_exp(const Eigen::Matrix3d &a)
  {
    double theta2 = a(0,1)*a(0,1) + a(0,2)*a(0,2) +  a(1,2)*a(1,2) + std::numeric_limits<double>::epsilon();
    double theta = std::sqrt(theta2);
    return Eigen::Matrix3d::Identity() + boost::math::sinc_pi(theta)*a+(1.0-std::cos(theta))/theta2*a*a;
  }

  void apply_rotation(Eigen::Matrix3d &m, const Eigen::Vector3d &d)
  {
    Eigen::Matrix3d skrew;
    skrew << 0, -d.z(), d.y(), d.z(), 0, -d.x(), -d.y(), d.x(), 0;
    m *= rotation_exp(skrew);
  }

  void apply_small_rotation_(Eigen::Matrix3d& rotation, double h, int i, int j)
  {
    const Eigen::Vector3d col_l = rotation.col(j) - rotation.col(i) * h;
    rotation.col(i) += rotation.col(j) * h;
    rotation.col(j) = col_l;
  }

  void apply_small_rotation_x(Eigen::Matrix3d& rotation, double h) { apply_small_rotation_(rotation,h,1,2); }
  void apply_small_rotation_y(Eigen::Matrix3d& rotation, double h) { apply_small_rotation_(rotation,-h,0,2); }
  void apply_small_rotation_z(Eigen::Matrix3d& rotation, double h) { apply_small_rotation_(rotation,h,0,1); }
}

// Pose camera = Rotation + Translation + intrinsics
struct Camera
{
  Eigen::Matrix3d rotation = Eigen::Matrix3d::Identity();
  Eigen::Vector3d translation = {0,0,0};
  double a=0,b=0,c=0;
};

// Point 3D = Eigen::Vector3d

namespace lma
{
  // Update policy of a Camera according to 9 degres of freedom
  // The Adl parameter enable the usage of a function defined after its use
  void apply_increment(Camera& camera, const double delta[9], const Adl&)
  {
    // update rotation using exponential map : camera.rotation *= exp(skew(delta{0,1,2}))
    v::apply_rotation(camera.rotation,{delta[0],delta[1],delta[2]});

    // update translation : camera.translation += delta{3,4,5}
    camera.translation += Eigen::Map<const Eigen::Vector3d>(delta + 3);

    // update instrinsics : (camera{a,b,c} += delta{6,7,8})
    Eigen::Map<Eigen::Array3d>(&camera.a) += Eigen::Map<const Eigen::Array3d>(delta+6);
  }

  // Only for numerical derivative:
  // Update policy of a Camera according to the Ie parameter (h ~ 1e-8).
  // The Adl parameter enable the usage of a function defined after its use
  template<int I> void apply_small_increment(Camera& camera, double h, v::numeric_tag<I>, const Adl&)
  {
    if      (I == 0) v::apply_small_rotation_x(camera.rotation,h);
    else if (I == 1) v::apply_small_rotation_y(camera.rotation,h);
    else if (I == 2) v::apply_small_rotation_z(camera.rotation,h);
    else if (I == 3) camera.translation.x() += h;
    else if (I == 4) camera.translation.y() += h;
    else if (I == 5) camera.translation.z() += h;
    else if (I == 6) camera.a += h;
    else if (I == 7) camera.b += h;
    else if (I == 8) camera.c += h;
  }

  //degree of freedom of a Camera
  template<> struct Size<Camera> { enum {value = 9}; };

  // Nothing to specify for the 3D points (Eigen::Vector3d) :
  //   - degree of freedom of an Eigen::Vector is the size.
  //   - The update policy of an Eigen::Vector is operator+.
}

struct BALProblem
{
  bool load(std::string filename)
  {
    std::ifstream file(filename);
    if (!file.is_open()) return false;

    size_t num_cameras,num_points,num_observations;

    file >> num_cameras >> num_points >> num_observations;

    std::cout << color.yellow() <<
      boost::format("Cameras[%1%], Points 3D[%2%], Observations[%3%]")%num_cameras%num_points%num_observations
      << color.reset() << std::endl;

    point_index.resize(num_observations);
    camera_index.resize(num_observations);
    observations.resize(num_observations);

    cameras.resize(num_cameras);
    points3d.resize(num_points);

    for(size_t i = 0; i < observations.size(); ++i)
      file >> camera_index[i]
           >> point_index[i]
           >> observations[i].x()
           >> observations[i].y();

    for(size_t i = 0; i < num_cameras; ++i)
    {
      std::array<double,9> params;
      for(size_t k = 0 ; k < 9 ; ++k)
        file >> params[k];
      lma::apply_increment(cameras[i],params.data(),lma::Adl{});
    }

    for(size_t i = 0; i < num_points; ++i)
      file >> points3d[i].x()
           >> points3d[i].y()
           >> points3d[i].z();

    return true;
  }

  std::vector<int> point_index;
  std::vector<int> camera_index;

  template<class T> using AlignedVector = std::vector<T,Eigen::aligned_allocator<T>>;
  AlignedVector<Eigen::Vector3d> points3d;
  AlignedVector<Eigen::Vector2d> observations;
  AlignedVector<Camera> cameras;
};

struct Reprojection
{
  const Eigen::Vector2d& obs;
  Reprojection(const Eigen::Vector2d& p2d):obs(p2d){}

  bool operator()(const Camera& camera, const Eigen::Vector3d& point, double (&error)[2]) const
  {
    const Eigen::Vector3d p = camera.rotation * point + camera.translation;
    const double
      xp = - p[0] / p[2],
      yp = - p[1] / p[2],
      r2 = xp*xp + yp*yp,
      distortion = 1.0 + r2  * (camera.b + camera.c  * r2);

    error[0] = camera.a * distortion * xp - obs[0];
    error[1] = camera.a * distortion * yp - obs[1];
    return true;
  }
};

namespace ttt
{
  template<> struct Name< Reprojection > { static std::string name(){ return "Reprojection"; } };
}

void call_lma(std::string file, double lambda, int iteration_max)
{

  BALProblem bal_problem;
  if (!bal_problem.load(file)) {
    std::cerr << "ERROR: unable to open file " << file << "\n";
    return ;
  }

  lma::Solver<Reprojection> solver(lambda,iteration_max);

  for (size_t i = 0; i < bal_problem.observations.size(); ++i)
    solver.add(
                Reprojection(bal_problem.observations[i]),
                &bal_problem.cameras[bal_problem.camera_index[i]],
                &bal_problem.points3d[bal_problem.point_index[i]]
              );

  solver.solve(lma::DENSE_SCHUR,lma::enable_verbose_output());
}

int main(int argc, char** argv)
{
  if (argc == 4)
    call_lma(argv[1],boost::lexical_cast<double>(argv[2]),boost::lexical_cast<int>(argv[3]));
  else
    std::cerr << "usage: test-lma3d <bal_problem> initial_lambda nb_iteration_max\n";

  return 0;
}


#include "CubicSpline.hpp"

#include "BezierCurve.hpp"

__pragma(warning(push, 0))
#include <Eigen/Eigen>
    __pragma(warning(pop))

#include "CubicSpline.hpp"
#include <tbb/tbb.h>

//#define CUBIC_USE_BEZIER

        namespace Ilum::geometry
{
	// Bezier Spline
	struct BezierCubicSpline : public Curve
	{
		// New generated control points
		std::vector<glm::vec3> control_points;

		virtual std::vector<glm::vec3> generateVertices(const std::vector<glm::vec3> &control_points, uint32_t sample) override;

		virtual glm::vec3 value(const std::vector<glm::vec3> &control_points, float t) override;

	  private:
		void generateControlPoints(const std::vector<glm::vec3> &control_points);
	};

	// B-Spline
	struct BCubicSpline : public Curve
	{
		virtual std::vector<glm::vec3> generateVertices(const std::vector<glm::vec3> &control_points, uint32_t sample) override;

		virtual glm::vec3 value(const std::vector<glm::vec3> &control_points, float t) override;

	  private:
		void genDeBoorPoints(const std::vector<glm::vec3> &control_points);

		std::vector<glm::vec3> m_de_boor;
		std::vector<float>     m_knots;
	};

	std::vector<glm::vec3> BezierCubicSpline::generateVertices(const std::vector<glm::vec3> &control_points, uint32_t sample)
	{
		BezierCurve bezier_curve;

		if (control_points.size() <= 2)
		{
			return bezier_curve.generateVertices(control_points, sample);
		}

		if (this->control_points.empty())
		{
			generateControlPoints(control_points);
		}

		uint32_t patch = sample / static_cast<uint32_t>(control_points.size() - 1);

		std::vector<glm::vec3> result;

		for (uint32_t i = 0; i < control_points.size() - 1; i++)
		{
			auto frag = std::move(bezier_curve.generateVertices({this->control_points[3 * i],
			                                                     this->control_points[3 * i + 1],
			                                                     this->control_points[3 * i + 2],
			                                                     this->control_points[3 * i + 3]},
			                                                    patch));
			result.insert(result.end(), std::make_move_iterator(frag.begin()), std::make_move_iterator(frag.end()));
		}

		return result;
	}

	glm::vec3 BezierCubicSpline::value(const std::vector<glm::vec3> &control_points, float t)
	{
		BezierCurve bezier_curve;

		if (control_points.size() <= 2)
		{
			return bezier_curve.value(control_points, t);
		}

		if (this->control_points.empty())
		{
			generateControlPoints(control_points);
		}

		return bezier_curve.value(this->control_points, t);
	}

	void BezierCubicSpline::generateControlPoints(const std::vector<glm::vec3> &control_points)
	{
		if (control_points.size() < 2)
		{
			return;
		}

		size_t n = control_points.size() - 1;

		Eigen::MatrixXf A(3 * n + 1, 3 * n + 1);
		Eigen::MatrixXf b(3 * n + 1, 3);
		A.setZero();
		b.setZero();

		tbb::parallel_for(tbb::blocked_range<size_t>(0, n + 1u), [this, n, control_points, &A, &b](const tbb::blocked_range<size_t> &count) {
			for (size_t i = count.begin(); i != count.end(); i++)
			{
				A(i, 3 * i) = 1;

				if (i >= 1 && i <= n - 1)
				{
					A(n + i, 3 * i - 1) = -1;
					A(n + i, 3 * i)     = 2;
					A(n + i, 3 * i + 1) = -1;

					A(2 * n + i - 1, 3 * i - 2) = 1;
					A(2 * n + i - 1, 3 * i - 1) = -2;
					A(2 * n + i - 1, 3 * i)     = 0;
					A(2 * n + i - 1, 3 * i + 1) = 2;
					A(2 * n + i - 1, 3 * i + 2) = -1;
				}

				b(i, 0) = control_points[i].x;
				b(i, 1) = control_points[i].y;
				b(i, 2) = control_points[i].z;
			}
		});

		// End condition
		A(3 * n - 1, 0) = 1;
		A(3 * n - 1, 1) = -2;
		A(3 * n - 1, 2) = 1;

		A(3 * n, 3 * n - 2) = 1;
		A(3 * n, 3 * n - 1) = -2;
		A(3 * n, 3 * n)     = 1;

		Eigen::MatrixXf x = A.colPivHouseholderQr().solve(b);

		this->control_points.resize(3 * n + 1);

		for (size_t i = 0; i < 3 * n + 1; i++)
		{
			this->control_points[i].x = x(i, 0);
			this->control_points[i].y = x(i, 1);
			this->control_points[i].z = x(i, 2);
		}
	}

	inline float gen_basis(const std::vector<float> &T, float t, size_t i, size_t k)
	{
		if (k == 1)
		{
			if ((t >= T[i] && t < T[i + 1]) || (t >= T[i] && t <= T[i + 1] && T[i + 1] == T.back()))
			{
				return 1.0;
			}
			else
			{
				return 0.0;
			}
		}
		float left = 0.f;
		left       = T[i + k - 1ull] - T[i] == 0.f ? 0.f : (t - T[i]) / (T[i + k - 1] - T[i]) * gen_basis(T, t, i, k - 1ull);

		float right = 0.f;
		right       = T[i + k] - T[i + 1ull] == 0.f ? 0.f : (T[i + k] - t) / (T[i + k] - T[i + 1ull]) * gen_basis(T, t, i + 1ull, k - 1ull);

		return left + right;
	}

	std::vector<glm::vec3> BCubicSpline::generateVertices(const std::vector<glm::vec3> &control_points, uint32_t sample)
	{
		if (control_points.empty())
		{
			return {};
		}

		if (m_de_boor.empty())
		{
			genDeBoorPoints(control_points);
		}

		std::vector<glm::vec3> vertices(sample + 1u);

		tbb::parallel_for(tbb::blocked_range<size_t>(0, sample + 1u), [this, control_points, sample, &vertices](const tbb::blocked_range<size_t> &count) {
			for (size_t i = count.begin(); i != count.end(); i++)
			{
				float t     = m_knots.back() / static_cast<float>(sample) * static_cast<float>(i);
				vertices[i] = value(control_points, t);
			}
		});
		return vertices;
	}

	glm::vec3 BCubicSpline::value(const std::vector<glm::vec3> &control_points, float t)
	{
		if (m_de_boor.empty())
		{
			genDeBoorPoints(control_points);
		}

		glm::vec3 result = glm::vec3(0.f);

		for (size_t i = 0; i < m_de_boor.size(); i++)
		{
			result += gen_basis(m_knots, t, i, 4) * m_de_boor[i];
		}

		return result;
	}

	void BCubicSpline::genDeBoorPoints(const std::vector<glm::vec3> &control_points)
	{
		std::vector<float> s;

		// Knot sequence
		m_knots.clear();
		m_knots.push_back(0);
		m_knots.push_back(0);
		m_knots.push_back(0);

		for (size_t i = 0; i < control_points.size(); i++)
		{
			m_knots.push_back(static_cast<float>(i));
			s.push_back(static_cast<float>(i));
		}

		m_knots.push_back(static_cast<float>(control_points.size() - 1));
		m_knots.push_back(static_cast<float>(control_points.size() - 1));
		m_knots.push_back(static_cast<float>(control_points.size() - 1));

		size_t n = m_knots.size() - 4;

		m_de_boor.resize(n);

		Eigen::MatrixXf A(n, n);
		Eigen::MatrixXf b(n, 3);

		A.setZero();
		b.setZero();

		// Begin
		{
			A(0, 0) = 1;

			A(1, 0) = 2;
			A(1, 1) = -3;
			A(1, 2) = 1;

			b(0, 0) = control_points[0].x;
			b(0, 1) = control_points[0].y;
			b(0, 2) = control_points[0].z;
		}

		// Inner
		{
			for (size_t i = 1; i < control_points.size() - 1; i++)
			{
				A(i + 1, i)     = gen_basis(m_knots, s[i], i, 4);
				A(i + 1, i + 1) = gen_basis(m_knots, s[i], i + 1, 4);
				A(i + 1, i + 2) = gen_basis(m_knots, s[i], i + 2, 4);
				b(i + 1, 0)     = control_points[i].x;
				b(i + 1, 1)     = control_points[i].y;
				b(i + 1, 2)     = control_points[i].z;
			}
		}

		// End
		{
			A(n - 2, n - 3) = 1;
			A(n - 2, n - 2) = -3;
			A(n - 2, n - 1) = 2;

			A(n - 1, n - 1) = 1;

			b(n - 1, 0) = control_points.back().x;
			b(n - 1, 1) = control_points.back().y;
			b(n - 1, 2) = control_points.back().z;
		}

		Eigen::MatrixXf res = A.colPivHouseholderQr().solve(b);

		for (size_t i = 0; i < m_de_boor.size(); i++)
		{
			m_de_boor[i] = glm::vec3(res(i, 0), res(i, 1), res(i, 2));
		}
	}

	std::vector<glm::vec3> CubicSpline::generateVertices(const std::vector<glm::vec3> &control_points, uint32_t sample)
	{
#ifdef CUBIC_USE_BEZIER
		BezierCubicSpline curve;
		return curve.generateVertices(control_points, sample);
#else
		BCubicSpline curve;
		return curve.generateVertices(control_points, sample);
#endif        // CUBIC_USE_BEZIER
	}

	glm::vec3 CubicSpline::value(const std::vector<glm::vec3> &control_points, float t)
	{
#ifdef CUBIC_USE_BEZIER
		BezierCubicSpline curve;
		return curve.value(control_points, t);
#else
		BCubicSpline curve;
		return curve.value(control_points, t);
#endif        // CUBIC_USE_BEZIER
	}
}


//==================================================================================================
/*!
  @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_COTH_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_COTH_HPP_INCLUDED

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

 /*!

    @ingroup group-hyperbolic
    Function object implementing coth capabilities

    Returns the hyperbolic cotangent: \f$(e^{x}+e^{-x})/(e^{x}-e^{-x})\f$

    @par Semantic:

    For every parameter of floating type @c T

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

    @see sinh, cosh, sinhcosh

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

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

#endif


#include <boost/format.hpp>
#include <boost/program_options.hpp>
#include <iostream>
#include <cmath>
#include <complex>

class NCO {
public:
  NCO(double _fsamp) {
    fsamp = _fsamp;
  }

  virtual void setFreq(double f) = 0;

  virtual std::complex<double> step() = 0;

  double fsamp;
};

class NCOSinCos : public NCO {
public:
  NCOSinCos(double _fsamp) : NCO(_fsamp) {
    idx = 0;
    ang = 0.0;
  }

  void setFreq(double f) {
    ainc = 2.0 * M_PI * f / fsamp;
  }

  std::complex<double> step() {
    double s, c;
    ang += ainc;
    ang = (ang > M_PI) ? (ang - (2.0 * M_PI)) : ang;
    sincos(ang, &s, &c);
    return std::complex<double>(c, -s);
  }

private:
  double ang;
  double ainc;
  int idx;
};

class NCORecursive : public NCO {
public:
  NCORecursive(double _fsamp) : NCO(_fsamp) {
    idx = 0;
    ang = 0.0;
    ejw = std::complex<double>(1.0, 0.0);
    last = std::complex<double>(1.0, 0.0);
  }

  void setFreq(double f) {
    ainc = 2.0 * M_PI * f / fsamp;
    ejw = exp(std::complex<double>(0.0, -ainc));
  }

  std::complex<double> step() {
    std::complex<double> nval;
    idx++;
    ang += ainc;
    ang = (ang > M_PI) ? (ang - (2.0 * M_PI)) : ang;
    if(idx == 2048) {
      idx = 0;
      double s, c;
      sincos(ang, &s, &c);
      nval = std::complex<double>(c, -s);
    }
    else {
      nval = last * ejw;
    }
    last = nval;
    return nval;
  }

private:
  double ang;
  double ainc;
  std::complex<double> ejw, last;
  int idx;
};

class NCORecursive2 : public NCO {
public:
  NCORecursive2(double _fsamp) : NCO(_fsamp) {
    idx = 0;
    ejw = std::complex<double>(1.0, 0.0);
    last = std::complex<double>(1.0, 0.0);
  }

  void setFreq(double f) {
    double ainc = 2.0 * M_PI * f / fsamp;
    ejw = exp(std::complex<double>(0.0, -ainc));
  }

  std::complex<double> step() {
    std::complex<double> nval;
    idx++;
    nval = last * ejw;
    if(idx == 512) {
      idx = 0;
      nval = nval / abs(nval);
    }
    last = nval;
    return nval;
  }

private:
  std::complex<double> ejw, last;
  int idx;
};

int main(int argc, char * argv[])
{
  unsigned long testcount = 100000000;
  //  unsigned long testcount = 10000;

  if(argc == 1) {
    // compare the two schemes
    double maxerr[2] = { 0.0, 0.0 };
    double toterr[2] = { 0.0, 0.0 };
    NCOSinCos sco(100e3);
    NCORecursive ro(100e3);
    NCORecursive2 ro2(100e3);

    sco.setFreq(2.5325e3);
    ro.setFreq(2.5325e3);
    ro2.setFreq(2.5325e3);
    std::complex<double> scv, rv, rv2;
    for(unsigned long k = 0; k < 10; k++) {
      for(unsigned long i =  0; i < testcount; i++) {
	scv = sco.step();
	rv = ro.step();
	rv2 = ro2.step();
	// std::cout << boost::format("%d %g %g %g %g\n")
	//  	% i % scv.real() % scv.imag() % rv.real() % rv.imag();
	std::complex<double> diff[2];
	diff[0] = rv - scv;
	diff[1] = rv2 - scv;
	for(int j = 0; j < 2; j++) {
	  double err = abs(diff[j]);
	  if(err > maxerr[j]) maxerr[j] = err;
	  toterr[j] += err;
	}

      }

      for(int j = 0; j < 2; j++) {
	std::cout << boost::format("%d : RO[%d] Max error = %g  average error = %g\n")
	  % k % j % maxerr[j] % (toterr[j] / ((double) testcount));
      }
    }
  }
  else if(argv[1][0] == 't') {
    // do the trig version
    double sum = 0.0;
    NCOSinCos o(100e3);
    o.setFreq(2.5325e3);
    std::complex<double> v;
    for(unsigned long i =  0; i < testcount; i++) {
      v = o.step();
      sum += v.real();
    }
    std::cout << "NCOSinCos sum = " <<  sum << std::endl;
  }
  else if(argv[1][0] == '2') {
    // do the recursive gain compensation version
    double sum = 0.0;
    NCORecursive2 o(100e3);
    o.setFreq(2.5325e3);
    std::complex<double> v;
    for(unsigned long i =  0; i < testcount; i++) {
      v = o.step();
      sum += v.real();
    }
    std::cout << "NCORecursive2 sum = " << sum << std::endl;
  }
  else {
    // do the recursive version
    double sum = 0.0;
    NCORecursive o(100e3);
    o.setFreq(2.5325e3);
    std::complex<double> v;
    for(unsigned long i =  0; i < testcount; i++) {
      v = o.step();
      sum += v.real();
    }
    std::cout << "NCORecursive sum = " << sum << std::endl;
  }
}


#pragma once

#include <Eigen/Dense>

/**
 * @brief get Fourier modes of array indices
 *        in centered zero frequency convention
 *
 * @param i  array index
 * @param n  array size
 *
 * @return
 */
inline int
to_freq(int i, int n)
{
  int kmax = n / 2;
  int k = i - kmax;
  return k;
}

inline auto
ftgrid(int n)
{
  int o = 1 ? n % 2 == 0 : 0;
  return Eigen::ArrayXd::LinSpaced(n, -n / 2, n / 2 - o);
}


/* -*-C++-*- */
/*
   (c) Copyright 2002-2005, Hewlett-Packard Development Company, LP

   See the file named COPYING for license details
*/

/** @file
    \brief header file for StatsQuantile
*/

#ifndef LINTEL_STATSQUANTILE_HPP
#define LINTEL_STATSQUANTILE_HPP

#include <stdio.h>

#include <Lintel/Stats.hpp>
#include <Lintel/PriorityQueue.hpp>

#include <boost/utility.hpp>

/// \brief Non-sampled quantile statistics
///
/// Based on the paper:
/// "Approximate medians and other quantiles in one pass with limited memory"
/// by G. Manku and S. Rajagopalan and B. Lindsay
/// Proceedings of the ACM SIGMOD, 1998.
///
/// An improved algorithm (don't need nbound, reduced memory usage is
/// "Space-Efficient Online Computation of Quantile Summaries",
/// Greenwald and Khanna, http://eprints.kfupm.edu.sa/66004/1/66004.pdf
class StatsQuantile : public Stats, boost::noncopyable {
public:
    /// The default tuning parameters use up about 59 KiB/StatsQuantile
    /// and support up to a billion inputs with quantile 0.9 as
    /// guarenteed between 0.89 and 0.91, and potentially much better.
    /// In practice, the actual error appears to be 2-3x better on the
    /// absolute worst number, and about 10x better on average
    ///
    /// quantile_error means that if with quantile(phi) = the element
    /// at position ceil(phi * N) in the sorted list indexed from
    /// 1..n, then the position of the returned element is between
    /// ceil((phi - quantile_error) * N) and ceil((phi +
    /// quantile_error) * N) in the sorted list.  if phi = 0.5, then
    /// the value is a median.  If phi == 0, then the quantile the
    /// element at position 1.  Note that this indexing is different
    /// than the standard C++ indexing of arrays from 0. Nbound has to
    /// be larger than N (# elements actually added) for the quantile
    /// error to be guarenteed.
    ///
    /// Here are some statistics on the memory usage of various
    /// quantile errors and Nbounds.  Approximately increasing the
    /// nbound by 10x increases the memory usage by 1.15x, but
    /// decreasing the quantile error by 10x increases the memory usage
    /// by 7x
    ///
    /// \verbatim
    /// quantile_error  Nbound  Approx Memory Usage (MiB)
    ///        0.0001    1e8      2.096 MiB
    ///        0.0001    1e9      2.825 MiB
    ///        0.0001    1e10     4.542 MiB
    ///        0.0001    1e11     5.835 MiB
    ///        0.0001    1e12     7.022 MiB
    ///        0.0001    1e13     8.919 MiB
    ///
    ///        0.001     1e8      0.282 MiB
    ///        0.001     1e9      0.454 MiB
    ///        0.001     1e10     0.584 MiB
    ///        0.001     1e11     0.702 MiB
    ///        0.001     1e12     0.892 MiB
    ///        0.001     1e13     1.112 MiB
    ///
    ///        0.01      1e8      0.045 MiB
    ///        0.01      1e9      0.058 MiB
    ///        0.01      1e10     0.070 MiB
    ///        0.01      1e11     0.089 MiB
    ///        0.01      1e12     0.111 MiB
    ///        0.01      1e13     0.130 MiB
    /// \endverbatim

    StatsQuantile(double quantile_error = 0.01,
		  int64_t Nbound = 1000 * 1000 * 1000,
		  int print_nrange = 10,
		  bool lazy = true);

    virtual ~StatsQuantile();
    virtual void reset();

    virtual void add(const double value);
    // add(Stats &) is linear in the number of values added to stat,
    // and may double the error bounds.
    virtual void add(const Stats &stat);

    double getQuantile(double quantile, bool allow_invalid_nbound = false) const;

    int getBufferSize() { return buffer_size; }
    int getNBuffers() { return nbuffers; }

    void dumpState(); // for debugging :(
#if STATSQUANTILE_TIMING
    Clock::T accum_gq_all, accum_gq_init, accum_gq_search, accum_gq_inner;
    int accum_gq_nelem;
#endif
    void setNrange(int _print_nrange) { print_nrange = _print_nrange; }

    // nranges is the number of ranges the printed quantiles will divide
    // [min .. max] into, so to get 10%, 20%, 30%..., nranges = 10
    void printFile(FILE *out, int nranges=-1);
    // prints the 90% quantile, 95%, 99%, 99.5%, 99.9%, ...
    void printTail(FILE *out);
    virtual void printRome(int depth, std::ostream &out) const;

    /// calls printTextRanges, printTextTail with default arguments
    virtual void printText(std::ostream &out) const;

    /// nranges specifies how many quantile values to print out.  The
    /// multiplier allows you to convert the units of the data during
    /// printing, for example to be able to print data in both MB/s
    /// and Mbps for network data.
    void printTextRanges(std::ostream &out, int nranges=-1, double multiplier = 1.0) const;

    /// multiplier is the same as for printTextRanges.
    void printTextTail(std::ostream &out, double multiplier = 1.0) const;

    /// How much memory will this StatsQuantile use?
    size_t memoryUsage() const;

    /// this function is only here for some of the regression testing,
    /// the previous one should probably always be used.  The
    /// type_disambiguate string is only there because otherwise a call
    /// to the constructor that specifies (double, int) is ambiguous
    StatsQuantile(const std::string &type_disambiguate,
		  int nbuffers, int buffer_size, int print_nrange = 10);

private:
    friend class StatsQuantileTest;
    // add for only the quantile portion
    void addQuantile(const double value);

    typedef double *one_buffer;
    int collapseFindFirstBuffer();
    // returns total weight across these buffers, also initializes collapse_pos
    int64_t collapseSortBuffers(int first_buffer);
    int64_t collapseNextQuantileOffset(int64_t total_weight);

    void collapse();

    double getQuantileByIndex(uint64_t target_index) const;
    double getQuantileByBinSearchIndex(uint64_t target_index) const;

    const double quantile_error;
    const int64_t Nbound;

    int buffer_size, nbuffers;

    /// Do initial buffer allocation and field setting.
    void init(int _nbuffers, int _buffer_size);

    /// Zero-clear all the buffer contents for quantile computation.
    void init_buffers();

    // TODO: replace these with vectors, make structure with all buffer
    // related thing, e.g. weight, level, sorted(probably)

    one_buffer *all_buffers;
    // int64 is necessary for very big counts, with small buffers
    // (~100 elements at 0.1 epsilon, ~1000 at 0.01 epsilon), we could
    // overflow an int32 at 200 billion - 2 trillion added values.
    int64_t *buffer_weight;
    int *buffer_level; // int is sufficient, goes up by one each time we collapse
    int cur_buffer, cur_buffer_pos; // cur_buffer_pos points to the current empty position in cur_buffer
    bool *buffer_sorted; // makes output faster by eliminating the need for
    // re-sorting

    // stuff for implementing collapse easier, ought to be able to get away
    // with putting tmp_buffer on the stack or some-such, but who cares
    one_buffer tmp_buffer;
    int *collapse_pos;
    bool collapse_even_low;
    int print_nrange;

    bool lazy;

    double collapseVal(int buffer) const {
	return all_buffers[buffer][collapse_pos[buffer]];
    }
    struct pairCmp {
	inline bool operator()(const std::pair<double, int> &a,
			       const std::pair<double, int> &b) const {
	    return a.first >= b.first;
	}
    };
};

#endif


/* main-lake_DPS.cpp

  Riddhi Singh, May, 2014
  The Pennsylvania State University
  rus197@psu.edu

  Adapted by Tori Ward, July 2014
  Cornell University
  vlw27@cornell.edu

  Adapted by Jonathan Herman and David Hadka, Sept-Dec 2014
  Cornell University and The Pennsylvania State University

  Adapted by Julianne Quinn, July 2015
  Cornell University
  jdq8@cornell.edu

  A multi-objective represention of the lake model from Carpenter et al., 1999
  This simulation is designed for optimization with Multi-Master Borg.

  Stochasticity is introduced by natural phosphorous inflows.
  These follow a lognormal distribution with specified mu and sigma.

  Decision variable
    vars : vector of 100 years of P emissions

  Objectives
  1: max avg P concentration
  2: mean economic benefits
  3: mean inertia
  4: mean reliability

  Constraints
  Reliability must be > 85%

*/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <unistd.h>
#include <sstream>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/math/tools/roots.hpp>
#include "./../borg/moeaframework.h"
#include "./../boostutil.h"
#include "./../borg/borgms.h"

#define nYears 100
#define nSamples 100
#define inertia_threshold -0.02
#define reliability_threshold 0.85

double b, q, alpha, delta, pCrit;

int nvars = nYears;
int nobjs = 4;
int nconstrs = 1;
double nat_flowmat [10000][nYears]; // create a matrix of [ 10000 x nYears ]

namespace ublas = boost::numeric::ublas;
namespace tools = boost::math::tools;
using namespace std;

ublas::vector<double> average_annual_P(nYears);
ublas::vector<double> discounted_benefit(nSamples);
ublas::vector<double> yrs_inertia_met(nSamples);
ublas::vector<double> yrs_pCrit_met(nSamples);

ublas::vector<double> lake_state(nYears+1);

void lake_problem(double* vars, double* objs, double* constrs)
{
  // initialize variables
  zero(average_annual_P);
  zero(discounted_benefit);
  zero(yrs_inertia_met);
  zero(yrs_pCrit_met);
  zero(lake_state);

  int linesToUse [nSamples];
  srand (time(NULL)); //gives PRNG a seed based on time
  for (int s=0; s < nSamples; s++) {
    //pick a random number based on time
    //choose 100 of 10,000 available inflow value lines
    linesToUse[s] = rand() % 10000;
  }

  // run lake model simulation
  for (int s = 0; s < nSamples; s++){
    // randomly generated natural phosphorous inflows
    double *nat_flow = new double [nYears];
    int index = linesToUse[s];
    // get the random natural flow from the States of the world file
    //each line of SOW file covers 100 days of inflow
    for (int i=0; i < nYears; i++){
      nat_flow[i] = nat_flowmat[index][i];
    }

    // initialize lake_state
    lake_state(0) = 0;
    // find initial policy-derived release

    //implement the lake model from Carpenter et al. 1999
    for (int i = 0; i < nYears; i++)
    {
      // new state: previous state - decay + recycling + pollution
      lake_state(i+1) = lake_state(i)*(1-b) + pow(lake_state(i),q)/(1+pow(lake_state(i),q)) + vars[i] + nat_flow[i];
      average_annual_P(i) += lake_state(i+1)/nSamples;
      discounted_benefit(s) += alpha*vars[i]*pow(delta,i);

      if (i>=1 && (vars[i] - vars[i-1]) > inertia_threshold)
        yrs_inertia_met(s) += 1;

      if(lake_state(i+1) < pCrit)
        yrs_pCrit_met(s) += 1;
    }
  }

  objs[0] = -1*vsum(discounted_benefit)/nSamples; // average economic benefit
  objs[1] = vmax(average_annual_P);; // max average annual P concentration
  objs[2] = -1*vsum(yrs_inertia_met)/((nYears-1)*nSamples); // average inertia
  objs[3] = -1*vsum(yrs_pCrit_met)/(nYears*nSamples); // average reliability

  constrs[0] = max(0.0, reliability_threshold - (-1*objs[3]));

  average_annual_P.clear();
  discounted_benefit.clear();
  yrs_inertia_met.clear();
  yrs_pCrit_met.clear();
  lake_state.clear();

}

double root_function(double x) {
  return pow(x,q)/(1+pow(x,q)) - b*x;
}

bool root_termination(double min, double max) {
  return abs(max - min) <= 0.000001;
}

int main(int argc, char* argv[])
{
  // initialize defaults
  b = 0.42;
  q = 2;
  alpha = 0.4;
  delta = 0.98;

  std::pair<double, double> root = tools::bisect(root_function, 0.01, 1.0, root_termination);
  pCrit = (root.first + root.second) / 2;

  for (int i=0;i<10000;i++){   //this is 10,000 to match nat_flowmat's size
    for (int j=0;j<nYears;j++){
      nat_flowmat[i][j] = 0.0;
    }
  }

  FILE * myfile;
  myfile = fopen("./../SOWs_Type6.txt","r");

  int linenum = 0;
  int maxSize = 5000;

  if (myfile==NULL){
    perror("Error opening file");
  } else {
    char buffer [maxSize];
    while (fgets(buffer, maxSize, myfile)!=NULL){
      linenum++;
      if (buffer[0]!='#'){
        char *pEnd;
        char *testbuffer = new char [maxSize];
        for (int i=0; i <maxSize; i++){
          testbuffer[i] = buffer[i];
        }

        for (int cols=0; cols < nYears; cols++){
          nat_flowmat[linenum-1][cols] = strtod(testbuffer, &pEnd);
          testbuffer  = pEnd;
        }
      }
    }
  }

  fclose(myfile);

  // setting random seed
  unsigned int seed = atoi(argv[1]);
  srand(seed);
  int NFE = atoi(argv[2]);

  // interface with Borg-MS
  BORG_Algorithm_ms_startup(&argc, &argv);
  BORG_Algorithm_ms_max_evaluations(NFE);
  BORG_Algorithm_output_frequency(NFE/200);

  // Define the problem with decisions, objectives, constraints and the evaluation function
  BORG_Problem problem = BORG_Problem_create(nvars, nobjs, nconstrs, lake_problem);

  // Set all the parameter bounds and epsilons
  for (int j=0; j < nvars; j++){
    BORG_Problem_set_bounds(problem, j, 0.01, 0.1);
  }

  BORG_Problem_set_epsilon(problem, 0, 0.01); // average economic benefit
  BORG_Problem_set_epsilon(problem, 1, 0.01); // max average annual P concentration
  BORG_Problem_set_epsilon(problem, 2, 0.0001); // average inertia
  BORG_Problem_set_epsilon(problem, 3, 0.0001); // average reliability

  //This is set up to run only one seed at a time
  char outputFilename[256];
  char runtime[256];
  FILE* outputFile = NULL;
  sprintf(outputFilename, "./sets/LakeIT_S%d.set", seed);
  sprintf(runtime, "./runtime/LakeIT_S%d.runtime", seed);

  BORG_Algorithm_output_runtime(runtime);

  BORG_Random_seed(seed);
  BORG_Archive result = BORG_Algorithm_ms_run(problem); // this actually runs the optimization

  //If this is the master node, print out the final archive
  if (result != NULL){
    outputFile = fopen(outputFilename, "w");
    if(!outputFile){
      BORG_Debug("Unable to open final output file\n");
    }
    BORG_Archive_print(result, outputFile);
    BORG_Archive_destroy(result);
    fclose(outputFile);
  }

  BORG_Algorithm_ms_shutdown();
  BORG_Problem_destroy(problem);

  return EXIT_SUCCESS;

}


#include <catch.hpp>
#include <complex>
#include <cmath>
#include <functional>
#include <vector>
#include <armadillo>
#include "../util/cubatureintegration.h"
#include "helpers.h"

namespace CubatureIntegrationTest {
  double tangent(std::vector<double> x) {
    return std::tan(x[0]);
  }

  struct TestFunctor {
    double operator()(std::vector<double> x) {
      return std::log(x[0]);
    }
  };

  template <class IntegrationProvider>
  class IntegrationTestClass {
    const double pi = arma::datum::pi;

    public:
      IntegrationTestClass() {}
      void testMethod() {
        SECTION("Check if it implements IntegrationInterface") {
          REQUIRE(std::is_base_of<POWannier::IntegrationInterface, IntegrationProvider>::value == true);
        }

        SECTION("Calculate simple 1D integral using lambda expression") {
          std::vector<double> xmin = {0};
          std::vector<double> xmax = {1};
          double result = IntegrationProvider::integrate(
              [=] (std::vector<double> x) {
                  auto value = std::sin(2*pi*x[0]) * std::sin(2*pi*x[0]);
                  return value;
              }, xmin, xmax);

          REQUIRE(result == Approx(0.5));
        }

        SECTION("Calculate simple 2d integral using lambda expression") {
          std::vector<double> xmin = {0, 0};
          std::vector<double> xmax = {1, 1};
          double result = IntegrationProvider::integrate(
              [=] (std::vector<double> x) {
                  auto value = std::pow(std::sin(2*pi*x[0]), 2) *
                      std::pow(std::sin(2*pi*x[1]), 2);
                  return value;
               }, xmin, xmax);

          REQUIRE(result == Approx(0.25));
        }

        SECTION("Calculate integral using a function in a global namespace") {
          std::vector<double> xmin = {0};
          std::vector<double> xmax = {pi/4};
          double result = IntegrationProvider::integrate(
              &CubatureIntegrationTest::tangent, xmin, xmax);

          REQUIRE(result == Approx(0.5 * std::log(2)));
        }

        SECTION("Calculate integral using a functor") {
          std::vector<double> xmin = {1};
          std::vector<double> xmax = {std::exp(1.0)};
          CubatureIntegrationTest::TestFunctor testfunctor;
          double result = IntegrationProvider::integrate(
              std::ref(testfunctor), xmin, xmax);

          REQUIRE(result == Approx(1));
        }

        SECTION("Calculate two-dimensional integral") {
          std::vector<double> xmin = {0, 0};
          std::vector<double> xmax = {1, 1};
          double result = IntegrationProvider::integrate(
              [=] (std::vector<double> x) {
                  auto value = std::sin(pi * (x[0] + x[1])) *
                    std::sin(pi * (x[0] - x[1])) *
                    std::cos(2 * pi * x[0]);
                  return value;
              }, xmin, xmax);
          REQUIRE(result == Approx(-0.25));
        }

     SECTION("Calculate complex integral") {
          std::vector<double> xmin = {0};
          std::vector<double> xmax = {1};
          std::complex<double> result = IntegrationProvider::integrate(
              [=] (std::vector<double> x) {
                  auto value = std::sin(2 * pi * x[0]) *
                  std::exp(std::complex<double>(0, 2.0 * pi * x[0]));
                  return value;
              }, xmin, xmax);
          REQUIRE(std::real(result) == Approx(0).margin(1e-15));
          REQUIRE(std::imag(result) == Approx(0.5));
        }

     SECTION("Calculate complex two-dimensional integral") {
          arma::rowvec xmin(2, arma::fill::zeros);
          arma::rowvec xmax(2, arma::fill::ones);
          std::complex<double> result = IntegrationProvider::integrate(
              [=] (arma::rowvec r) {
                  auto x = 2 * r[0];
                  auto y = -2 * r[0] + std::sqrt(2) * r[1];
                  auto an = -2 * r[0];
                  auto value = (std::pow(std::sin(pi * (x + y) / std::sqrt(2)), 2) +
                        std::pow(std::sin(0.5 * pi * x), 2) +
                        std::pow(std::sin(pi * (x + 0.25)), 2) )*
                        std::exp(std::complex<double>(0, 2 * pi * an ));
                  return value;
              }, xmin, xmax);
          REQUIRE(std::real(result) == Approx(0).margin(1e-15));
          REQUIRE(std::imag(result) == Approx(-0.25));
        }
      }

  };

}

METHOD_AS_TEST_CASE(CubatureIntegrationTest::IntegrationTestClass<POWannier::CubatureIntegration>::testMethod,
    "Calculating integrals using cubature", "[integral, cubature]");


#include <iostream>
#include <Eigen/Dense>
#include <manifold/SO3.h>
#include <manifold/gradientDescentSO3.h>

class GDSO3vMF : public GDSO3<double> {
 public:
  GDSO3vMF(const SO3d& Rmu) : Rmu_(Rmu)
  {};

  virtual void ComputeJacobian(const SO3d& theta, Eigen::Matrix<double,3,1>* J, double* f) {
    SO3d R = theta;
    if (J) {
      Eigen::Matrix3d JMat = -0.5*((R.Inverse() + Rmu_).matrix()
          - (Rmu_.Inverse() + R).matrix());
      *J = SO3d::vee(JMat);
//      std::cout << R << std::endl;
//      std::cout << Rmu_ << std::endl;
//      std::cout << JMat << std::endl;
//      std::cout << J->transpose() << std::endl;
    }
    if (f) {
      *f = -(Rmu_.Inverse() + R).matrix().trace();
    }
  };
 protected:
  SO3d Rmu_;
};

int main (int argc, char** argv) {

  SO3d R;
  std::cout << R << std::endl;

  double theta = 15.*M_PI/180.;
  Eigen::Matrix3d Rmu_;
  Rmu_ << 1, 0, 0,
         0, cos(theta), sin(theta),
         0, -sin(theta), cos(theta);
  SO3d Rmu(Rmu_);

  std::cout << Rmu << std::endl;
  std::cout << R+Rmu << std::endl;
  std::cout << R << std::endl;
  std::cout << Rmu+R << std::endl;

  std::cout << R-Rmu << std::endl;

  Eigen::Vector3d w = R-Rmu;
  std::cout << Rmu.Exp(w) << std::endl;

  std::cout << Rmu-R << std::endl;

  GDSO3vMF gd(Rmu);
  gd.Compute(R, 1e-8, 100);
  R = gd.GetMinimum();

//  double delta = 0.1;
//  double f_prev = 1e99;
//  double f = (Rmu.Inverse() + R).matrix().trace();
//  std::cout << "f=" << f << std::endl;
//  for (uint32_t it=0; it<100; ++it) {
//    Eigen::Matrix3d J = -0.5*((R.Inverse() + Rmu).matrix() - (Rmu.Inverse() + R).matrix());
//    Eigen::Vector3d Jw = SO3d::vee(J);
////    R = R.Exp(-delta*Jw);
//    R += -delta*Jw;
////    std::cout << Jw << std::endl;
//    f_prev = f;
//    f = (Rmu.Inverse() + R).matrix().trace();
////    if ((f_prev - f)/f < 1e-3)
////      break;
//    std::cout << "f=" << f << " df/f=" << (f_prev - f)/f
//      << std::endl;
////      << std::endl << R << std::endl;
//  }
  std::cout << std::endl << Rmu << std::endl;
  std::cout << std::endl << R << std::endl;
}


#include <Eigen/Dense>

#include <gpd/conv_layer.h>

int main(int argc, char* argv[])
{
  // Create example input, weights, bias.
  Eigen::MatrixXf X(5,5);
  Eigen::MatrixXf W(3,3);
  Eigen::VectorXf b = Eigen::VectorXf::Zero(1);
  X <<  1, 1, 1, 0, 0,
            0, 1, 1, 1, 0,
            0, 0, 1, 1, 1,
            0, 0, 1, 1, 0,
            0, 1, 1, 0, 0;
  W <<      1, 0, 1,
            0, 1, 0,
            1, 0, 1;

  std::vector<float> w_vec;
  Eigen::Matrix<float,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> W_rowmajor(W);
  Eigen::Map<Eigen::VectorXf> w(W_rowmajor.data(), W_rowmajor.size());
  w_vec.assign(w.data(), w.data() + w.size());

  std::vector<float> b_vec;
  b_vec.assign(b.data(), b.data() + b.size());

  // Create a convolutional layer and execute a forward pass.
  ConvLayer conv1(5, 5, 1, 1, 3, 1, 0);
  conv1.setWeightsAndBiases(w_vec, b_vec);
  Eigen::Matrix<float,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> X_row_major(X);
  Eigen::Map<Eigen::VectorXf> v1(X_row_major.data(), X_row_major.size());
  std::vector<float> vec1;
  vec1.assign(v1.data(), v1.data() + v1.size());
  Eigen::MatrixXf Y = conv1.forward(vec1);

  std::cout << "Y: " << Y.rows() << " x " << Y.cols() << std::endl;
  std::cout << Y << std::endl;
  std::cout << std::endl;
}


#define BOOST_TEST_MODULE "test_clementi_dihedral_potential"

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

#include <mjolnir/forcefield/local/ClementiDihedralPotential.hpp>
#include <mjolnir/math/constants.hpp>

BOOST_AUTO_TEST_CASE(ClementiDihedral_double)
{
    using real_type = double;
    constexpr std::size_t N  = 1000;
    constexpr real_type   h  = 1e-6;
    constexpr real_type   pi = mjolnir::math::constants<real_type>::pi();

    const real_type k1 = 1.0;
    const real_type k3 = 2.0;
    const real_type r0 = pi * 2. / 3.;

    mjolnir::ClementiDihedralPotential<real_type> c(k1, k3, r0);

    const real_type x_min = -pi;
    const real_type x_max =  pi;
    const real_type dx = (x_max - x_min) / N;

    for(std::size_t i=0; i<N; ++i)
    {
        const real_type x    = x_min + dx * i;
        const real_type x_up = (x+h < 2. * pi) ? x+h : x+h - 2 * pi;
        const real_type x_bt = (x-h > 0)       ? x-h : x-h + 2 * pi;
        const real_type pot1 = c.potential(x_up);
        const real_type pot2 = c.potential(x_bt);
        const real_type dpot = (pot1 - pot2) / (2 * h);
        const real_type deri = c.derivative(x);

        BOOST_TEST(dpot == deri, boost::test_tools::tolerance(h));
        // periodicity
        BOOST_TEST(c.potential(x)  == c.potential (x + 2 * pi),
                   boost::test_tools::tolerance(1e-8));
        BOOST_TEST(c.derivative(x) == c.derivative(x + 2 * pi),
                   boost::test_tools::tolerance(1e-8));
    }
}

BOOST_AUTO_TEST_CASE(ClementiDihedral_float)
{
    using real_type = float;
    constexpr static std::size_t N  = 100;
    constexpr static real_type   h  = 1e-2f;
    constexpr static real_type   pi = mjolnir::math::constants<real_type>::pi();

    const real_type k1 = 1.0f;
    const real_type k3 = 2.0f;
    const real_type r0 = pi * 2. / 3.;

    mjolnir::ClementiDihedralPotential<real_type> c(k1, k3, r0);

    const real_type x_min = 0.f;
    const real_type x_max = 2.f * pi;
    const real_type dx = (x_max - x_min) / N;

    for(std::size_t i=0; i<N; ++i)
    {
        const real_type x    = x_min + dx * i;
        const real_type x_up = (x+h < 2.0f * pi) ? x+h : x+h - 2 * pi;
        const real_type x_bt = (x-h > 0.0f)      ? x-h : x-h + 2 * pi;
        const real_type pot1 = c.potential(x_up);
        const real_type pot2 = c.potential(x_bt);
        const real_type dpot = (pot1 - pot2) / (2.0f * h);
        const real_type deri = c.derivative(x);

        BOOST_TEST(dpot == deri, boost::test_tools::tolerance(h));
        // periodicity
        BOOST_TEST(c.potential(x)  == c.potential (x + 2 * pi),
                   boost::test_tools::tolerance(h));
        BOOST_TEST(c.derivative(x) == c.derivative(x + 2 * pi),
                   boost::test_tools::tolerance(h));
   }
}


#include <boost/dynamic_bitset.hpp>
#include <boost/numeric/ublas/matrix.hpp>

#include "encode.hpp"

using namespace std;
using namespace boost::numeric;
using boost::dynamic_bitset;

const auto generator_mat = [] {
    ublas::matrix<bool> generator_m(7, 4);
    vector<bool> generator_init = {
        1, 1, 0, 1,
        1, 0, 1, 1,
        1, 0, 0, 0,
        0, 1, 1, 1,
        0, 1, 0, 0,
        0, 0, 1, 0,
        0, 0, 0, 1
    };

    copy(generator_init.begin(),
         generator_init.end(),
         generator_m.data().begin()
    );

    return generator_m;
}();

vector<uint8_t> encode(const vector<uint8_t> &bytes) {
    vector<bool> plain;
    plain.reserve(bytes.size() * 8);

    for (auto && c : bytes) {
        for (auto i = 0u; i < 8u; ++i) {
            plain.push_back(c & (1 << i));
        }
    }

    dynamic_bitset<uint8_t> result;
    for (auto it = plain.begin(); it != plain.end(); advance(it, 4)) {
        ublas::matrix<bool> value_mat(4, 1);
        copy(it, it+4, value_mat.data().begin());

        auto prod = ublas::prod(generator_mat, value_mat);

        for_each(prod.begin1(), prod.end1(), [&result] (auto v) {
            result.push_back(v % 2);
        });
    }

    result.push_back(1);
    while (result.size() % 8 != 0) {
        result.push_back(0);
    }

    vector<uint8_t> blocks(result.num_blocks());
    to_block_range(result, blocks.begin());

    return blocks;
}


//
// Created by keszocze on 15.10.18.
//

#pragma once

#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <cudd/cplusplus/cuddObj.hh>
#include <vector>

#include "number_representation.hpp"

namespace abo::error_metrics {

/**
 * @brief Computes the maximal value of a function represented by a vector of BDDs
 *
 * The function is assumed to return an natural number
 *
 * @param mgr
 * @param fun The function given by a vector of BDDs
 */
boost::multiprecision::uint256_t get_max_value(const Cudd& mgr,
                                               const std::vector<BDD>& fun);

/**
 * @brief Computes the maximum absolute difference between the f and f_hat for any input
 * The computation is performed symbolically using BDDs
 * @param mgr The BDD object manager
 * @param f The original function
 * @param f_hat The approximated function. Must have the same number of bits as f
 * @param num_rep The number representation for f and f_hat
 * @return The maximum absolute difference
 */
boost::multiprecision::uint256_t
worst_case_error(const Cudd& mgr,
                 const std::vector<BDD>& f,
                 const std::vector<BDD>& f_hat,
                 const abo::util::NumberRepresentation num_rep
                    = abo::util::NumberRepresentation::BaseTwo);

/**
 * @brief Computes the maximum absolute difference between the f and f_hat for any input
 * divided by 2^n - 1 to normalize it to the range [0, 1] regardless of the function size (with n =
 * f.size()) This is mainly intended to be a helper function to make the use of the worst case error
 * easier
 * @param mgr The BDD object manager
 * @param f The original function
 * @param f_hat The approximated function. Must have the same number of bits as f
 * @param num_rep The number representation for f and f_hat
 * @return The maximum absolute difference normalized to [0, 1]
 */
double worst_case_error_percent(const Cudd& mgr, const std::vector<BDD>& f,
                                const std::vector<BDD>& f_hat,
                                const abo::util::NumberRepresentation num_rep
                                    = abo::util::NumberRepresentation::BaseTwo);

/**
 * @brief Computes the maximum absolute difference between the f and f_hat for any input
 * The computation is performed using BDDs and will be typically slow compared to the BDD based
 * variant
 * @param mgr The BDD object manager
 * @param f The original function
 * @param f_hat The approximated function. Must have the same number of bits as f
 * @param num_rep The number representation for f and f_hat
 * @return The maximum absolute difference
 */
boost::multiprecision::uint256_t
worst_case_error_add(const Cudd& mgr,
                     const std::vector<BDD>& f,
                     const std::vector<BDD>& f_hat,
                     const abo::util::NumberRepresentation num_rep
                        = abo::util::NumberRepresentation::BaseTwo);

/**
 * @brief approximate_worst_case_error
 *  Calculates the worst case error approximately, to a given relative error.
 *  The time and memory this function uses scales exponentially with the desired precision (n) (in
 * the worst case). With m being the input size (f, f_hat), in the worst case, it lies in O(m ^ n).
 * @param mgr
 * @param f
 * @param f_hat
 * @param n The precision to calculate in number of bits, for more details see the return value. It
 * must be greater than zero
 * @param num_rep The number representation for f and f_hat
 * @return The approximated worst case error. It is an upper bound and therefore guaranteed to be
 * larger than the actual error. Let wc be the correct worst case error and x be the result of this
 * function. Then it holds that wc <= x <= wc * (1 + 1 / (2 ^ (n + 1) - 1)))
 */
boost::multiprecision::uint256_t
approximate_worst_case_error(const Cudd& mgr, const std::vector<BDD>& f,
                             const std::vector<BDD>& f_hat, int n,
                             const abo::util::NumberRepresentation num_rep
                                = abo::util::NumberRepresentation::BaseTwo);

} // namespace abo::error_metrics


/**
 * test ode calculation
 * @author Tobias Weber <tweber@ill.fr>
 * @date 28-oct-20
 * @license GPLv3, see 'LICENSE' file
 * @desc forked from https://github.com/t-weber/misc/blob/master/boost/ode3.cpp
 *
 * g++ -std=c++20 -DUSE_LAPACK -I.. -I/usr/include/lapacke -Iext/lapacke/include -Lext/lapacke/lib -o ode ode.cpp -llapacke
 *
 * ----------------------------------------------------------------------------
 * tlibs
 * Copyright (C) 2017-2021  Tobias WEBER (Institut Laue-Langevin (ILL),
 *                          Grenoble, France).
 * Copyright (C) 2015-2017  Tobias WEBER (Technische Universitaet Muenchen
 *                          (TUM), Garching, Germany).
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, version 3 of the License.
 *
 * This program 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 General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 * ----------------------------------------------------------------------------
 */

#define BOOST_TEST_MODULE Ode Test
#include <boost/test/included/unit_test.hpp>
namespace test = boost::unit_test;
namespace testtools = boost::test_tools;

#include <iostream>
#include <vector>

#include "libs/maths.h"
using namespace tl2_ops;

#define DEBUG_OUTPUT

// ----------------------------------------------------------------------------
// numeric integration
#include <boost/numeric/odeint.hpp>
namespace odeint = boost::numeric::odeint;

// mark custom vector as resizeable
template<> struct odeint::is_resizeable<tl2::vec<float, std::vector>>
{
	using type = typename boost::true_type;
	static const bool value = type::value;
};
template<> struct odeint::is_resizeable<tl2::vec<double, std::vector>>
{
	using type = typename boost::true_type;
	static const bool value = type::value;
};

template<class t_mat, class t_vec, class t_val=typename t_vec::value_type>
t_vec odesys(const t_mat& C, const t_vec& y0, t_val x_start, t_val x_end, t_val x_step = 0.01)
{
	t_vec y = y0;
	odeint::integrate_adaptive(odeint::runge_kutta4<t_vec>{},
		[&C](const t_vec& y, t_vec& y_diff, t_val x) -> void
		{
			y_diff = C*y;
		}, y, x_start, x_end, x_step);

	return y;
}
// ----------------------------------------------------------------------------

using t_types = std::tuple<double, float>;

BOOST_AUTO_TEST_CASE_TEMPLATE(test_ode, t_real, t_types)
{
	const t_real eps = 1e-3;

	using t_cplx = std::complex<t_real>;
	using t_vec_cplx = tl2::vec<t_cplx, std::vector>;
	using t_mat_cplx = tl2::mat<t_cplx, std::vector>;
	using t_vec = tl2::vec<t_real, std::vector>;
	using t_mat = tl2::mat<t_real, std::vector>;

	t_mat_cplx coeff = tl2::create<t_mat_cplx>({
		0., 1.,
		1., 1. });

	t_vec_cplx f0 = tl2::create<t_vec_cplx>({1., 1.});
	t_cplx x0 = 0.;
	t_cplx n0 = 0.;

	const auto [coeff_re, coeff_im] = tl2::split_cplx<t_mat_cplx, t_mat>(coeff);
	const auto [f0_re, f0_im] = tl2::split_cplx<t_vec_cplx, t_vec>(f0);

#ifdef DEBUG_OUTPUT
	std::cout << "coeff = " << coeff << std::endl;
	std::cout << "x0 = " << x0 << ", f0 = " << f0 << std::endl;
	std::cout << std::endl;
#endif

	for(t_cplx x=0; x.real()<10; x+=1)
	{
		auto [ok, f] = tl2_la::odesys_const<t_mat_cplx, t_vec_cplx, t_cplx>(coeff, x, x0, f0);
		BOOST_TEST(ok);

		// compare with numerical result
		t_vec num_val = odesys<t_mat, t_vec>(coeff_re, f0_re, t_real{0.}, x.real(), t_real{0.01});
		BOOST_TEST(f[0].real() == num_val[0], testtools::tolerance(eps));
		BOOST_TEST(f[1].real() == num_val[1], testtools::tolerance(eps));
		BOOST_TEST(f[0].imag() == t_real{0}, testtools::tolerance(eps));
		BOOST_TEST(f[1].imag() == t_real{0}, testtools::tolerance(eps));

#ifdef DEBUG_OUTPUT
		std::cout << "x = " << x << std::endl;
		std::cout << "ok = " << std::boolalpha << ok << std::endl;

		for(std::size_t i=0; i<f.size(); ++i)
			std::cout << "f_" << i << " = " << f[i] << std::endl;
		std::cout << std::endl;
#endif
	}

	std::size_t idx=0;
	const t_cplx fibo[] = {1, 1, 2, 3, 5, 8, 13, 21, 34, 55, 89};
	for(t_cplx n=0; n.real()<10; n+=1)
	{
		auto [ok, f] = tl2_la::diffsys_const<t_mat_cplx, t_vec_cplx, t_cplx>(coeff, n, n0, f0);
		BOOST_TEST(ok);
		BOOST_TEST(tl2::equals(f[0], fibo[idx], eps));
		BOOST_TEST(tl2::equals(f[1], fibo[idx+1], eps));

#ifdef DEBUG_OUTPUT
		std::cout << "n = " << n << std::endl;
		std::cout << "ok = " << std::boolalpha << ok << std::endl;

		for(std::size_t i=0; i<f.size(); ++i)
			std::cout << "f_" << i << " = " << f[i] << std::endl;
		std::cout << std::endl;
#endif
		++idx;
	}
}


//
// Created by krab1k on 31/10/18.
//

#include <vector>
#include <cmath>
#include <Eigen/LU>

#include "denr.h"
#include "../structures/molecule.h"
#include "../parameters.h"

CHARGEFW2_METHOD(DENR)

std::vector<double> DENR::calculate_charges(const Molecule &molecule) const {

    size_t n = molecule.atoms().size();

    Eigen::MatrixXd eta = Eigen::MatrixXd::Zero(n, n);
    Eigen::MatrixXd L = Eigen::MatrixXd::Zero(n, n);
    Eigen::VectorXd chi = Eigen::VectorXd::Zero(n);
    Eigen::VectorXd q = Eigen::VectorXd::Zero(n);

    for (size_t i = 0; i < n; i++) {
        auto &atom_i = molecule.atoms()[i];
        chi(i) = parameters_->atom()->parameter(atom::electronegativity)(atom_i);
        eta(i, i) = parameters_->atom()->parameter(atom::hardness)(atom_i);
    }

    for (const auto &bond: molecule.bonds()) {
        auto i1 = bond.first().index();
        auto i2 = bond.second().index();
        L(i1, i1) += 1;
        L(i2, i2) += 1;
        L(i1, i2) -= 1;
        L(i2, i1) -= 1;
    }

    double step = parameters_->common()->parameter(common::step);

    Eigen::PartialPivLU<Eigen::MatrixXd> x = (Eigen::MatrixXd::Identity(n, n) + step * L * eta).partialPivLu();
    Eigen::VectorXd tmp = step * L * chi;
    for (int i = 0; i < parameters_->common()->parameter(common::iterations); i++) {
        q = x.solve(q - tmp);
    }

    return std::vector<double>(q.data(), q.data() + q.size());
}


#pragma once
#include <boost/serialization/access.hpp>

#include "Node.hxx"

class Scalar : public Node {
 private:
  std::map<size_t, mpq_class> map;

  friend class boost::serialization::access;

 public:
  Scalar () = default;

  Scalar (size_t variable, mpq_class const & fraction);

  Scalar (Scalar const & other);

  char order () const override;
  std::map<size_t, mpq_class> const & get() const;
  std::string print () const override;
  std::string printMaple () const override;

  int applyTensorSymmetries (int parity) override;
  void exchangeTensorIndices (std::map<char, char> const & exchange_map);

  void multiply (mpq_class const & factor) override;

  std::unique_ptr<Node> clone () const override;

  std::set<size_t> getVariableSet () const override;
  std::map<size_t, mpq_class> const * getCoefficientMap () const override;

  void shiftVariables (int i);

  void addOther(Scalar const * other);

  void removeZeros ();

  bool lessThan (Node const * other) const override;
  bool equals (Node const * other) const override;
  bool isZero () const;

  void substituteVariables (std::map<size_t, size_t> const & subs_map) override;
  void removeVariables (std::set<size_t> const & variables) override;

  template<class Archive>
  void serialize (Archive & ar, unsigned int const version);

  ~Scalar() = default;
};


#include <limits>
#include <iostream>
#include <iomanip>
#include <string>
#include <cmath>
#include <algorithm>
#include <vector>
// BGL includes
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/cycle_canceling.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>
#include <boost/graph/successive_shortest_path_nonnegative_weights.hpp>
#include <boost/graph/find_flow_cost.hpp>

// Graph Type with nested interior edge properties for Cost Flow Algorithms
typedef boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> traits;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
    boost::property<boost::edge_capacity_t, long,
        boost::property<boost::edge_residual_capacity_t, long,
            boost::property<boost::edge_reverse_t, traits::edge_descriptor,
                boost::property <boost::edge_weight_t, long> > > > > graph; // new! weightmap corresponds to costs

typedef boost::graph_traits<graph>::edge_descriptor             edge_desc;
typedef boost::graph_traits<graph>::out_edge_iterator           out_edge_it; // Iterator

// Custom edge adder class
class edge_adder {
 graph &G;

 public:
  explicit edge_adder(graph &G) : G(G) {}
  void add_edge(int from, int to, long capacity, long cost) {
    auto c_map = boost::get(boost::edge_capacity, G);
    auto r_map = boost::get(boost::edge_reverse, G);
    auto w_map = boost::get(boost::edge_weight, G); // new!
    const edge_desc e = boost::add_edge(from, to, G).first;
    const edge_desc rev_e = boost::add_edge(to, from, G).first;
    c_map[e] = capacity;
    c_map[rev_e] = 0; // reverse edge has no capacity!
    r_map[e] = rev_e;
    r_map[rev_e] = e;
    w_map[e] = cost;   // new assign cost
    w_map[rev_e] = -cost;   // new negative cost
  }
};

struct edge {
    int u;
    int v;
    int r;
};

void testcase() {
    int e, w, m, d, p, l;
    std::cin >> e >> w >> m >> d >> p >> l;
    std::vector<edge> nondiff(m);
    for(int i = 0; i < m; i++) {
        int u, v, r;
        std::cin >> u >> v >> r;
        nondiff[i] = {u, v, r};
    }
    std::vector<edge> diff(d);
    for(int i = 0; i < d; i++) {
        int u, v, r;
        std::cin >> u >> v >> r;
        diff[i] = {u, v, r};
    }
    int min_needed = std::max(e * l, w * l);
    if(min_needed > p) {
        std::cout << "No schedule!" << std::endl;
        return;
    }
    graph G(2 * (e + w));
    edge_adder adder(G);
    // Retrieve the capacity map and reverse capacity map
    auto c_map = boost::get(boost::edge_capacity, G);
    auto rc_map = boost::get(boost::edge_residual_capacity, G);
    auto r_map = boost::get(boost::edge_reverse, G);
    // edge from sink to two paths: one for the l matches for each, one for the rest
    // same goes for sink
    // --> by forcing the "rest" path to have at max p-e*l matches, we have at least l matches
    // for each
    int source = boost::add_vertex(G);
    int source_out_l = boost::add_vertex(G);
    int source_out_rest = boost::add_vertex(G);
    int sink = boost::add_vertex(G);
    int sink_out_l = boost::add_vertex(G);
    int sink_out_rest = boost::add_vertex(G);

    for(int i = 0; i < e; i++) {
        adder.add_edge(source_out_l, i, l, 0);
        adder.add_edge(source_out_rest, i, std::numeric_limits<long>::max(), 0);
        adder.add_edge(source_out_rest, e + w + i, std::numeric_limits<long>::max(), 0);
    }

    for(int i = 0; i < w; i++) {
        adder.add_edge(2 * e + w + i, sink_out_rest, std::numeric_limits<long>::max(), 0);
        adder.add_edge(e + i, sink_out_rest, std::numeric_limits<long>::max(), 0);
        adder.add_edge(e + i, sink_out_l, l, 0);
    }

    for(auto ed : nondiff) {
        adder.add_edge(ed.u, e + ed.v, 1, ed.r);
    }
    // diff matches are in a copy of e + w, where the e and w edges are connected to
    // the "rest" source and sink nodes with capacity
    for(auto ed : diff) {
        adder.add_edge(e + w + ed.u, 2 * e + w + ed.v, 1, ed.r);
    }
    adder.add_edge(source, source_out_l, e * l, 0);
    adder.add_edge(source, source_out_rest, p - e * l, 0);
    adder.add_edge(sink_out_l, sink, w * l, 0);
    adder.add_edge(sink_out_rest, sink, p - w * l, 0);

    boost::successive_shortest_path_nonnegative_weights(G, source, sink);
    int risk = boost::find_flow_cost(G);

    int flow = c_map[boost::edge(source, source_out_l, G).first] - rc_map[boost::edge(source, source_out_l, G).first];
    flow += c_map[boost::edge(source, source_out_rest, G).first] - rc_map[boost::edge(source, source_out_rest, G).first];
    if(flow != p) {
        std::cout << "No schedule!" << std::endl;
        return;
    } else {
        std::cout << risk << std::endl;
    }

    return;
}

int main() {
    std::ios_base::sync_with_stdio(false);

    int t;
    std::cin >> t;
    for (int i = 0; i < t; ++i)
        testcase();
}


#include <gtest/gtest.h>
#include "../util/util.h"
#include <Eigen/Core>
#include <string>
#include <algorithm>

#ifndef _MSC_VER
extern "C" {
#include <csim/memory_ops.h>
#include <csim/stat_ops.h>
#include <csim/update_ops.h>
#include <csim/init_ops.h>
}
#else
#include <csim/memory_ops.h>
#include <csim/stat_ops.h>
#include <csim/update_ops.h>
#include <csim/init_ops.h>
#endif
#include <csim/update_ops_cpp.hpp>

void test_single_qubit_named_gate(UINT n, std::string name, std::function<void(UINT, CTYPE*, ITYPE)> func, Eigen::MatrixXcd mat) {
	const ITYPE dim = 1ULL << n;
	const UINT max_repeat = 2;

	auto state = allocate_quantum_state(dim);
	initialize_Haar_random_state_with_seed(state, dim, 0);

	Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
	for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];
	std::vector<UINT> indices;
	for (UINT i = 0; i < n; ++i) indices.push_back(i);

	for (UINT rep = 0; rep < max_repeat; ++rep) {
		for (UINT i = 0; i < n; ++i) {
			UINT target = indices[i];
			func(target, state, dim);
			test_state = get_expanded_eigen_matrix_with_identity(target, mat, n) * test_state;
			state_equal(state, test_state, dim, name);
		}
		std::random_shuffle(indices.begin(), indices.end());
	}
	release_quantum_state(state);
}

TEST(UpdateTest, XGate) {
	Eigen::MatrixXcd mat(2, 2);
	mat << 0, 1, 1, 0;
	test_single_qubit_named_gate(6, "XGate", X_gate, mat);
	test_single_qubit_named_gate(6, "XGate", X_gate_single_unroll, mat);
#ifdef _OPENMP
	test_single_qubit_named_gate(6, "XGate", X_gate_parallel_unroll, mat);
#endif
#ifdef _USE_SIMD
	test_single_qubit_named_gate(6, "XGate", X_gate_single_simd, mat);
#ifdef _OPENMP
	test_single_qubit_named_gate(6, "XGate", X_gate_parallel_simd, mat);
#endif
#endif
}
TEST(UpdateTest, YGate) {
	Eigen::MatrixXcd mat(2, 2);
	mat << 0, -1.i, 1.i, 0;
	test_single_qubit_named_gate(6, "YGate", Y_gate, mat);
	test_single_qubit_named_gate(6, "YGate", Y_gate_single_unroll, mat);
#ifdef _OPENMP
	test_single_qubit_named_gate(6, "YGate", Y_gate_parallel_unroll, mat);
#endif
#ifdef _USE_SIMD
	test_single_qubit_named_gate(6, "YGate", Y_gate_single_simd, mat);
#ifdef _OPENMP
	test_single_qubit_named_gate(6, "YGate", Y_gate_parallel_simd, mat);
#endif
#endif
}
TEST(UpdateTest, ZGate) {
	const UINT n = 3;
	Eigen::MatrixXcd mat(2, 2);
	mat << 1, 0, 0, -1;
	test_single_qubit_named_gate(6, "ZGate", Z_gate, mat);
	test_single_qubit_named_gate(6, "ZGate", Z_gate_single_unroll, mat);
#ifdef _OPENMP
	test_single_qubit_named_gate(6, "ZGate", Z_gate_parallel_unroll, mat);
#endif
#ifdef _USE_SIMD
	test_single_qubit_named_gate(6, "ZGate", Z_gate_single_simd, mat);
#ifdef _OPENMP
	test_single_qubit_named_gate(6, "ZGate", Z_gate_parallel_simd, mat);
#endif
#endif
}
TEST(UpdateTest, HGate) {
	const UINT n = 3;
	Eigen::MatrixXcd mat(2, 2);
	mat << 1, 1, 1, -1; mat /= sqrt(2.);
	test_single_qubit_named_gate(n, "HGate", H_gate, mat);
	test_single_qubit_named_gate(6, "HGate", H_gate_single_unroll, mat);
#ifdef _OPENMP
	test_single_qubit_named_gate(6, "HGate", H_gate_parallel_unroll, mat);
#endif
#ifdef _USE_SIMD
	test_single_qubit_named_gate(6, "HGate", H_gate_single_simd, mat);
#ifdef _OPENMP
	test_single_qubit_named_gate(6, "HGate", H_gate_parallel_simd, mat);
#endif
#endif
}

TEST(UpdateTest, SGate) {
	const UINT n = 3;
	Eigen::MatrixXcd mat(2, 2);
	mat << 1, 0, 0, 1.i;
	test_single_qubit_named_gate(n, "SGate", S_gate, mat);
	test_single_qubit_named_gate(n, "SGate", Sdag_gate, mat.adjoint());
}

TEST(UpdateTest, TGate) {
	const UINT n = 3;
	Eigen::MatrixXcd mat(2, 2);
	mat << 1, 0, 0, (1. + 1.i) / sqrt(2.);
	test_single_qubit_named_gate(n, "TGate", T_gate, mat);
	test_single_qubit_named_gate(n, "TGate", Tdag_gate, mat.adjoint());
}

TEST(UpdateTest, sqrtXGate) {
	const UINT n = 3;
	Eigen::MatrixXcd mat(2, 2);
	mat << 0.5 + 0.5i, 0.5 - 0.5i, 0.5 - 0.5i, 0.5 + 0.5i;
	test_single_qubit_named_gate(n, "SqrtXGate", sqrtX_gate, mat);
	test_single_qubit_named_gate(n, "SqrtXdagGate", sqrtXdag_gate, mat.adjoint());
}

TEST(UpdateTest, sqrtYGate) {
	const UINT n = 3;
	Eigen::MatrixXcd mat(2, 2);
	mat << 0.5 + 0.5i, -0.5 - 0.5i, 0.5 + 0.5i, 0.5 + 0.5i;
	test_single_qubit_named_gate(n, "SqrtYGate", sqrtY_gate, mat);
	test_single_qubit_named_gate(n, "SqrtYdagGate", sqrtYdag_gate, mat.adjoint());
}

void test_projection_gate(std::function<void(UINT, CTYPE*, ITYPE)> func, std::function<double(UINT, CTYPE*, ITYPE)> prob_func, Eigen::MatrixXcd mat) {
	const UINT n = 6;
	const ITYPE dim = 1ULL << n;
	const UINT max_repeat = 10;
	const double eps = 1e-14;
	UINT target;
	double prob;

	auto state = allocate_quantum_state(dim);
	std::vector<UINT> indices;
	for (UINT i = 0; i < n; ++i) indices.push_back(i);

	for (UINT rep = 0; rep < max_repeat; ++rep) {
		for (int i = 0; i < n; ++i) {
			target = indices[i];
			initialize_Haar_random_state(state, dim);
			Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
			for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];

			// Z-projection operators
			prob = prob_func(target, state, dim);
			EXPECT_GT(prob, 1e-10);
			func(target, state, dim);
			ASSERT_NEAR(state_norm_squared(state, dim), prob, eps);
			normalize(prob, state, dim);

			test_state = get_expanded_eigen_matrix_with_identity(target, mat, n)*test_state;
			ASSERT_NEAR(test_state.squaredNorm(), prob, eps);
			test_state.normalize();
			state_equal(state, test_state, dim, "Projection gate");
		}
		std::random_shuffle(indices.begin(), indices.end());
	}
	release_quantum_state(state);
}

TEST(UpdateTest, ProjectionAndNormalizeTest) {
	Eigen::MatrixXcd P0(2, 2), P1(2, 2);
	P0 << 1, 0, 0, 0;
	P1 << 0, 0, 0, 1;
	test_projection_gate(P0_gate, M0_prob, P0);
	test_projection_gate(P1_gate, M1_prob, P1);
	test_projection_gate(P0_gate_single, M0_prob, P0);
	test_projection_gate(P1_gate_single, M1_prob, P1);
#ifdef _OPENMP
	test_projection_gate(P0_gate_parallel, M0_prob, P0);
	test_projection_gate(P1_gate_parallel, M1_prob, P1);
#endif
}

TEST(UpdateTest, SingleQubitRotationGateTest) {
	const UINT n = 6;
	const ITYPE dim = 1ULL << n;
	const UINT max_repeat = 10;

	Eigen::MatrixXcd Identity(2, 2), X(2, 2), Y(2, 2), Z(2, 2);
	Identity << 1, 0, 0, 1;
	X << 0, 1, 1, 0;
	Y << 0, -1.i, 1.i, 0;
	Z << 1, 0, 0, -1;

	UINT target;
	double angle;

	auto state = allocate_quantum_state(dim);
	initialize_Haar_random_state(state, dim);
	Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
	for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];
	typedef std::tuple<std::function<void(UINT, double, CTYPE*, ITYPE)>, Eigen::MatrixXcd, std::string> testset;
	std::vector<testset> test_list;
	test_list.push_back(std::make_tuple(RX_gate, X, "Xrot"));
	test_list.push_back(std::make_tuple(RY_gate, Y, "Yrot"));
	test_list.push_back(std::make_tuple(RZ_gate, Z, "Zrot"));

	for (UINT rep = 0; rep < max_repeat; ++rep) {
		for (auto tup : test_list) {
			target = rand_int(n);
			angle = rand_real();
			auto func = std::get<0>(tup);
			auto mat = std::get<1>(tup);
			auto name = std::get<2>(tup);
			func(target, angle, state, dim);
			test_state = get_expanded_eigen_matrix_with_identity(target, cos(angle / 2)*Identity + 1.i*sin(angle / 2)*mat, n) * test_state;
			state_equal(state, test_state, dim, name);
		}
	}
	release_quantum_state(state);
}

void test_two_qubit_named_gate(UINT n, std::string name, std::function<void(UINT, UINT, CTYPE*, ITYPE)> func,
	std::function<Eigen::MatrixXcd(UINT, UINT, UINT)> matfunc) {
	const ITYPE dim = 1ULL << n;
	const UINT max_repeat = 2;

	auto state = allocate_quantum_state(dim);
	initialize_Haar_random_state_with_seed(state, dim, 0);

	Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
	for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];
	std::vector<UINT> indices;
	for (UINT i = 0; i < n; ++i) indices.push_back(i);

	for (UINT rep = 0; rep < max_repeat; ++rep) {
		for (UINT i = 0; i + 1 < n; i += 2) {
			UINT target = indices[i];
			UINT control = indices[i + 1];
			func(control, target, state, dim);
			Eigen::MatrixXcd mat = matfunc(control, target, n);
			test_state = mat * test_state;
			state_equal(state, test_state, dim, name);
		}
		std::random_shuffle(indices.begin(), indices.end());
	}
	release_quantum_state(state);
}

TEST(UpdateTest, CNOTGate) {
	const UINT n = 4;
	test_two_qubit_named_gate(n, "CNOT", CNOT_gate, get_eigen_matrix_full_qubit_CNOT);
	test_two_qubit_named_gate(6, "CNOTGate", CNOT_gate_single_unroll, get_eigen_matrix_full_qubit_CNOT);
#ifdef _OPENMP
	test_two_qubit_named_gate(6, "CNOTGate", CNOT_gate_parallel_unroll, get_eigen_matrix_full_qubit_CNOT);
#endif
#ifdef _USE_SIMD
	test_two_qubit_named_gate(6, "CNOTGate", CNOT_gate_single_simd, get_eigen_matrix_full_qubit_CNOT);
#ifdef _OPENMP
	test_two_qubit_named_gate(6, "CNOTGate", CNOT_gate_parallel_simd, get_eigen_matrix_full_qubit_CNOT);
#endif
#endif
}

TEST(UpdateTest, CZGate) {
	const UINT n = 4;
	test_two_qubit_named_gate(n, "CZ", CZ_gate, get_eigen_matrix_full_qubit_CZ);
	test_two_qubit_named_gate(6, "CZGate", CZ_gate_single_unroll, get_eigen_matrix_full_qubit_CZ);
#ifdef _OPENMP
	test_two_qubit_named_gate(6, "CZGate", CZ_gate_parallel_unroll, get_eigen_matrix_full_qubit_CZ);
#endif
#ifdef _USE_SIMD
	test_two_qubit_named_gate(6, "CZGate", CZ_gate_single_simd, get_eigen_matrix_full_qubit_CZ);
#ifdef _OPENMP
	test_two_qubit_named_gate(6, "CZGate", CZ_gate_parallel_simd, get_eigen_matrix_full_qubit_CZ);
#endif
#endif
}

TEST(UpdateTest, SWAPGate) {
	const UINT n = 4;
	test_two_qubit_named_gate(n, "SWAP", SWAP_gate, get_eigen_matrix_full_qubit_SWAP);
	test_two_qubit_named_gate(6, "SWAPGate", SWAP_gate_single_unroll, get_eigen_matrix_full_qubit_SWAP);
#ifdef _OPENMP
	test_two_qubit_named_gate(6, "SWAPGate", SWAP_gate_parallel_unroll, get_eigen_matrix_full_qubit_SWAP);
#endif
#ifdef _USE_SIMD
	test_two_qubit_named_gate(6, "SWAPGate", SWAP_gate_single_simd, get_eigen_matrix_full_qubit_SWAP);
#ifdef _OPENMP
	test_two_qubit_named_gate(6, "SWAPGate", SWAP_gate_parallel_simd, get_eigen_matrix_full_qubit_SWAP);
#endif
#endif
}


/******************************************************************************
 *
 * AMDiS - Adaptive multidimensional simulations
 *
 * Copyright (C) 2013 Dresden University of Technology. All Rights Reserved.
 * Web: https://fusionforge.zih.tu-dresden.de/projects/amdis
 *
 * Authors:
 * Simon Vey, Thomas Witkowski, Andreas Naumann, Simon Praetorius, et al.
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 *
 * This file is part of AMDiS
 *
 * See also license.opensource.txt in the distribution.
 *
 ******************************************************************************/

#ifndef AMDIS_FUNCTORS_H
#define AMDIS_FUNCTORS_H

#include "AbstractFunction.h"
#include <boost/math/special_functions/cbrt.hpp>
#include <boost/math/special_functions/pow.hpp>

// NOTE: should be removed or replaced by functors in operation/functors.hpp

namespace AMDiS
{

  template<typename T>
  struct Id : public AbstractFunction<T,T>
  {
    Id(int degree = 1) : AbstractFunction<T,T>(degree) {}
    T operator()(const T& v) const
    {
      return v;
    }
  };

  template<typename T1,typename T2>
  struct Const : public AbstractFunction<T1,T2>
  {
    Const(T1 val_) : AbstractFunction<T1,T2>(0), val(val_) {}
    T1 operator()(const T2& v) const
    {
      return val;
    }
  private:
    T1 val;
  };

  template<typename T=double>
  struct Factor : public AbstractFunction<T,T>
  {
    Factor(double fac_, int degree = 1) : AbstractFunction<T,T>(degree), fac(fac_) {}
    T operator()(const T& x) const
    {
      return fac*x;
    }
  private:
    double fac;
  };

  template<typename T=double>
  struct Add : public BinaryAbstractFunction<T,T,T>
  {
    Add(int degree = 1) : BinaryAbstractFunction<T,T,T>(degree) {}
    T operator()(const T& v1, const T& v2) const
    {
      return v1+v2;
    }
  };

  template<typename T=double>
  struct AddFactor : public BinaryAbstractFunction<T,T,T>
  {
    AddFactor(double factor_ = 1.0, int degree = 1) : BinaryAbstractFunction<T,T,T>(degree), factor(factor_) {}
    T operator()(const T& v1, const T& v2) const
    {
      return v1 + factor*v2;
    }
  private:
    double factor;
  };

  template<typename T=double>
  struct Subtract : public BinaryAbstractFunction<T,T,T>
  {
    Subtract(int degree = 1) : BinaryAbstractFunction<T,T,T>(degree) {}
    T operator()(const T& v1, const T& v2) const
    {
      return v1-v2;
    }
  };

  template<typename T=double>
  struct AddScal : public AbstractFunction<T,T>
  {
    AddScal(T scal_, int degree = 1) : AbstractFunction<T,T>(degree), scal(scal_) {}
    T operator()(const T& v) const
    {
      return v+scal;
    }
  private:
    T scal;
  };

  template<typename T>
  struct Mult : public BinaryAbstractFunction<T,T,T>
  {
    Mult(int degree = 2) : BinaryAbstractFunction<T,T,T>(degree) {}
    T operator()(const T& v1, const T& v2) const
    {
      return v1*v2;
    }
  };

  template<typename T1, typename T2, typename T3>
  struct Mult2 : public BinaryAbstractFunction<T1,T2,T3>
  {
    Mult2(int degree = 2) : BinaryAbstractFunction<T1,T2,T3>(degree) {}
    T1 operator()(const T2& v1, const T3& v2) const
    {
      return v1*v2;
    }
  };

  template<typename T>
  struct MultScal : public BinaryAbstractFunction<T,T,T>
  {
    MultScal(T scal_, int degree = 2) : BinaryAbstractFunction<T,T,T>(degree), scal(scal_) {}
    T operator()(const T& v1, const T& v2) const
    {
      return v1*v2*scal;
    }
  private:
    T scal;
  };

  template<typename T>
  struct Max : public BinaryAbstractFunction<T,T,T>
  {
    Max(int degree = 1) : BinaryAbstractFunction<T,T,T>(degree) {}
    T operator()(const T& v1, const T& v2) const
    {
      return std::max(v1,v2);
    }
  };

  template<typename T>
  struct Min : public BinaryAbstractFunction<T,T,T>
  {
    Min(int degree = 1) : BinaryAbstractFunction<T,T,T>(degree) {}
    T operator()(const T& v1, const T& v2) const
    {
      return std::min(v1,v2);
    }
  };

  template<typename T>
  struct Diff : public BinaryAbstractFunction<T,T,T>
  {
    Diff(int degree = 1) : BinaryAbstractFunction<T,T,T>(degree) {}
    T operator()(const T& v1, const T& v2) const
    {
      return std::abs(v1-v2);
    }
  };

  template<typename T=double>
  struct Abs : public AbstractFunction<T,T>
  {
    Abs(int degree = 1) : AbstractFunction<T,T>(degree) {}
    T operator()(const T& v) const
    {
      return std::abs(v);
    }
  };

  template<typename T=double>
  struct Signum : public AbstractFunction<T,T>
  {
    Signum() : AbstractFunction<T,T>(0) {}
    T operator()(const T& v) const
    {
      return (v>0.0?1.0:(v<0.0?-1.0:0.0));
    }
  };

  template<typename T=double>
  struct Sqr : public AbstractFunction<T,T>
  {
    Sqr(int degree = 2) : AbstractFunction<T, T>(degree) {}
    T operator()(const T& v) const
    {
      return sqr(v);
    }
  };

  template<typename T=double>
  struct Sqrt : public AbstractFunction<T,T>
  {
    Sqrt(int degree = 4) : AbstractFunction<T,T>(degree) {}
    T operator()(const T& v) const
    {
      return std::sqrt(v);
    }
  };

  namespace detail
  {
    template<int p, typename T, typename Enabled = void>
    struct Pow
    {
      typedef typename traits::mult_type<T, typename Pow<p-1,T>::result_type>::type result_type;
      static result_type eval(const T& v)
      {
        return v*Pow<p-1,T>::eval(v);
      }
    };

    template<int p, typename T>
    struct Pow<p, T, typename boost::enable_if_c<
      boost::is_same<T, typename traits::mult_type<T, T>::type>::value&&
    (p > 1)
    >::type >
    {
      typedef T result_type;
      static T eval(const T& v)
    {
      return boost::math::pow<p>(v);
    }
    };

    template<typename T>
    struct Pow<1,T>
    {
      typedef T result_type;
      static result_type eval(const T& v)
      {
        return v;
      }
    };

    template<typename T>
    struct Pow<0, T>
    {
      typedef double result_type;
      static result_type eval(const T& v)
      {
        return 1.0;
      }
    };
  }

  template<int p, typename T=double>
  struct Pow : public AbstractFunction<typename detail::Pow<p,T>::result_type, T>
  {
    typedef typename detail::Pow<p,T>::result_type result_type;
    Pow(double factor_=1.0, int degree = p) : AbstractFunction<result_type,T>(degree), factor(factor_) {}
    result_type operator()(const T& v) const
    {
      return factor * detail::Pow<p,T>::eval(v);
    }
  private:
    double factor;
  };

  template<typename TIn, typename TOut = typename traits::mult_type<TIn, TIn>::type>
  struct Norm2 : public AbstractFunction<TOut, TIn>
  {
    Norm2(int degree = 4) : AbstractFunction<TOut, TIn>(degree) {}
    TOut operator()(const TIn& v) const
    {
      return std::sqrt(v*v);
    }
  };

  template<typename TIn, typename TOut = typename traits::mult_type<TIn, TIn>::type>
  struct Norm2Sqr : public AbstractFunction<TOut, TIn>
  {
    Norm2Sqr(int degree = 2) : AbstractFunction<TOut, TIn>(degree) {}
    TOut operator()(const TIn& v) const
    {
      return v*v;
    }
  };

  template<typename T>
  struct Norm2_comp2 : public BinaryAbstractFunction<T,T,T>
  {
    Norm2_comp2(int degree = 4) : BinaryAbstractFunction<T,T,T>(degree) {}
    T operator()(const T& v1, const T& v2) const
    {
      return std::sqrt(sqr(v1)+sqr(v2));
    }
  };

  template<typename T>
  struct Norm2Sqr_comp2 : public BinaryAbstractFunction<T,T,T>
  {
    Norm2Sqr_comp2(int degree = 2) : BinaryAbstractFunction<T,T,T>(degree) {}
    T operator()(const T& v1, const T& v2) const
    {
      return sqr(v1)+sqr(v2);
    }
  };

  template<typename T>
  struct Norm2_comp3 : public TertiaryAbstractFunction<T,T,T,T>
  {
    Norm2_comp3(int degree = 4) : TertiaryAbstractFunction<T,T,T,T>(degree) {}
    T operator()(const T& v1, const T& v2, const T& v3) const
    {
      return std::sqrt(sqr(v1)+sqr(v2)+sqr(v3));
    }
  };

  template<typename T>
  struct Norm2Sqr_comp3 : public TertiaryAbstractFunction<T,T,T,T>
  {
    Norm2Sqr_comp3(int degree = 2) : TertiaryAbstractFunction<T,T,T,T>(degree) {}
    T operator()(const T& v1, const T& v2, const T& v3) const
    {
      return sqr(v1)+sqr(v2)+sqr(v3);
    }
  };

  template<typename T>
  struct L1Diff : public BinaryAbstractFunction<T,T,T>
  {
    T operator()(const T& v1, const T& v2) const
    {
      return std::abs(v1-v2);
    }
  };

  template<typename TOut, typename T=TOut>
  struct L2Diff : public BinaryAbstractFunction<TOut,T,T>
  {
    TOut operator()(const T& v1, const T& v2) const
    {
      return Norm2<TOut, T>()(v1-v2);
    }
  };

  template<typename T>
  struct Vec1WorldVec : public AbstractFunction<WorldVector<T>,T>
  {
    WorldVector<T> operator()(const T& v0) const
    {
      WorldVector<T> result;
      result[0]=v0;
      return result;
    }
  };
  template<typename T>
  struct Vec2WorldVec : public BinaryAbstractFunction<WorldVector<T>,T,T>
  {
    WorldVector<T> operator()(const T& v0, const T& v1) const
    {
      WorldVector<T> result;
      result[0]=v0;
      result[1]=v1;
      return result;
    }
  };
  template<typename T>
  struct Vec3WorldVec : public TertiaryAbstractFunction<WorldVector<T>,T,T,T>
  {
    WorldVector<T> operator()(const T& v0, const T& v1, const T& v2) const
    {
      WorldVector<T> result;
      result[0]=v0;
      result[1]=v1;
      result[2]=v2;
      return result;
    }
  };
  template<int c, typename T=double>
  struct Component : public AbstractFunction<T, WorldVector<T>>
  {
    Component(int degree = 1) : AbstractFunction<T, WorldVector<T>>(degree) {}
    T operator()(const WorldVector<T>& x) const
    {
      return x[c];
    }
  };
  template<typename T=double>
  struct Component2 : public AbstractFunction<T, WorldVector<T>>
  {
    Component2(int comp, int degree = 1) : AbstractFunction<T, WorldVector<T>>(degree), c_(comp) {}
    T operator()(const WorldVector<T>& x) const
    {
      return x[c_];
    }
  private:
    int c_;
  };

  struct FadeOut : public TertiaryAbstractFunction<double, double, double ,double>
  {
    double operator()(const double& v, const double& dist, const double& mean) const
    {
      return dist*mean+(1.0-dist)*v;
    }
  };

  struct Random : public AbstractFunction<double, WorldVector<double>>
  {
    Random(double mean_, double amplitude_) : mean(mean_), amplitude(amplitude_)
    {
      std::srand(time(0));
    }

    double operator()(const WorldVector<double>& x) const
    {
      return mean + 2.0*amplitude * ((std::rand() / static_cast<double>(RAND_MAX)) - 0.5);
    }

  private:
    double mean;
    double amplitude;
  };

}

#endif // AMDIS_FUNCTORS_H