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

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

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

#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/plus.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( fma_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::unspecified_<A0> >
                          , bd::scalar_< bd::unspecified_<A0> >
                          , bd::scalar_< bd::unspecified_<A0> >
                          )
  {
    BOOST_FORCEINLINE
    A0 operator() ( A0 a0, A0 a1, A0 a2) const BOOST_NOEXCEPT
    {
      return plus(multiplies(a0, a1), a2);
    }
  };
} } }

#include <boost/simd/arch/common/scalar/function/correct_fma.hpp>
#include <boost/simd/arch/common/simd/function/correct_fma.hpp>

#endif


// Andrew Naplavkov

#ifndef BARK_DB_SLIPPY_TILE_HPP
#define BARK_DB_SLIPPY_TILE_HPP

#include <algorithm>
#include <array>
#include <bark/geometry/geometry_ops.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/range/algorithm_ext/erase.hpp>
#include <cmath>

namespace bark::db::slippy {

constexpr int Pixels = 256;

/// @see http://wiki.openstreetmap.org/wiki/Slippy_map_tilenames
struct tile {
    int x = 0;
    int y = 0;
    int z = 0;
};

using tiles = std::vector<tile>;

inline auto split(const tile& tl)
{
    std::array<tile, 4> res;
    for (int i = 0; i < 2; ++i)
        for (int j = 0; j < 2; ++j)
            res[i * 2 + j] = {2 * tl.x + i, 2 * tl.y + j, tl.z + 1};
    return res;
}

inline double left(const tile& tl)
{
    return tl.x / std::pow(2, tl.z) * 360 - 180;
}

inline double top(const tile& tl)
{
    constexpr auto Pi = boost::math::constants::pi<double>();
    auto n = Pi - 2 * Pi * tl.y / std::pow(2, tl.z);
    return 180 / Pi * atan((exp(n) - exp(-n)) / 2);
}

inline geometry::box extent(const tile& tl)
{
    return {{left(tl), top({tl.x, tl.y + 1, tl.z})},
            {left({tl.x + 1, tl.y, tl.z}), top(tl)}};
}

inline geometry::box pixel(const tile& tl)
{
    auto ext = extent(tl);
    auto center = boost::geometry::return_centroid<geometry::point>(ext);
    return {center,
            {center.x() + geometry::width(ext) / Pixels,
             center.y() + geometry::height(ext) / Pixels}};
}

template <class Predicate>
void depth_first_search(tiles& tls, const tile& tl, const Predicate& p)
{
    if (!p(tl))
        return;
    tls.push_back(tl);
    for (auto& sub_tl : split(tl))
        depth_first_search(tls, sub_tl, p);
}

template <class Predicate>
tiles depth_first_search(const Predicate& p)
{
    tiles tls;
    depth_first_search(tls, {}, p);
    return tls;
}

inline tile match(const geometry::box& px, int zmax)
{
    using namespace boost::geometry;

    auto px_area = area(px);
    auto sub_px_area = px_area / 4.;

    auto tiny = [&](const tile& tl) {
        return tl.z > zmax || area(pixel(tl)) < sub_px_area;
    };
    auto filter = [&](const tile& tl) {
        return intersects(px, extent(tl)) && !tiny(tl);
    };
    auto diff = [&](const tile& tl) {
        return std::fabs(area(pixel(tl)) - px_area);
    };
    auto cmp = [&](const tile& lhs, const tile& rhs) {
        return diff(lhs) < diff(rhs);
    };

    auto tls = depth_first_search(filter);
    return tls.empty() ? tile{}
                       : *std::min_element(tls.begin(), tls.end(), cmp);
}

inline tiles tile_coverage(const geometry::box& ext, int z)
{
    auto filter = [&](const tile& tl) {
        if (tl.z > z || !boost::geometry::intersects(ext, extent(tl)))
            return false;
        if (tl.z < z)
            return true;
        geometry::box visible{};
        boost::geometry::intersection(ext, extent(tl), visible);
        auto sub_pixel = pixel(split(tl).front());
        return geometry::width(visible) > geometry::width(sub_pixel) &&
               geometry::height(visible) > geometry::height(sub_pixel);
    };

    auto tls = depth_first_search(filter);
    boost::remove_erase_if(tls, [&](const tile& tl) { return tl.z != z; });
    return tls;
}

}  // namespace bark::db::slippy

#endif  // BARK_DB_SLIPPY_TILE_HPP


/// \file
/// Maintainer: Luzian Hug
///

#include "background_subtraction.h"

#include "Eigen/Core"

#include <opencv2/core/eigen.hpp>
#include <opencv2/core/mat.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <boost/log/trivial.hpp>

namespace MouseTrack {
BackgroundSubtraction::BackgroundSubtraction() {
  // empty
}

FrameWindow BackgroundSubtraction::operator()(const FrameWindow &window) const {
  // Initializing return object
  FrameWindow output = window;
  // Cycle through all streams
  for (size_t i = 0; i < _cage_frame.frames().size(); i++) {

    // Copy the stuff we need from the FrameWindow objects
    const PictureD &cage_image = _cage_frame.frames()[i].referencePicture;
    const PictureD &mouse_image = window.frames()[i].referencePicture;

    // Dimensions must match
    if (!(cage_image.rows() == mouse_image.rows() &&
          cage_image.cols() == mouse_image.cols())) {
      // If it fails, return the input (do nothing)
      BOOST_LOG_TRIVIAL(info)
          << "Frame dimensions (" << mouse_image.rows() << "x"
          << mouse_image.cols()
          << ") do not match empty cage frame dimensions (" << cage_image.rows()
          << "x" << cage_image.cols()
          << "). Background subtraction cannot be performed.";
      return window;
    }

    // Perform the subtraction
    PictureD sub = (mouse_image - cage_image).array().abs();

    // Convert to opencv format and from [0,1] to [0,255] format
    cv::Mat subcv;
    sub = Eigen::floor(sub.array() * 255);
    cv::eigen2cv(sub, subcv);
    subcv.convertTo(subcv, CV_8UC1);

    // Apply Otsu's Method for thresholding
    cv::Mat maskcv;
    double thresh_otsu = cv::threshold(subcv, maskcv, 0, 255,
                                       cv::THRESH_BINARY + cv::THRESH_OTSU);
    maskcv = subcv > (thresh_otsu * _otsu_factor);

    // Convert back to Eigen
    Eigen::MatrixXd mask;
    maskcv = maskcv / 255;
    cv::cv2eigen(maskcv, mask);
    // A very low threshold means there's no significant bright
    // spots, i.e. no mouse => set mask to zeros
    if (thresh_otsu < 0.01 * 255) {
      mask.setZero();
    }

    // Build Frame object...
    output.frames()[i].normalizedDisparityMap =
        mask.array() * output.frames()[i].normalizedDisparityMap.array();
  }

  return output;
}

const FrameWindow &BackgroundSubtraction::cage_frame() const {
  return _cage_frame;
}

void BackgroundSubtraction::cage_frame(FrameWindow &cage_frame) {

  _cage_frame = cage_frame;
}

double BackgroundSubtraction::otsu_factor() const { return _otsu_factor; }
void BackgroundSubtraction::otsu_factor(double otsu_factor) {
  _otsu_factor = otsu_factor;
}

} // namespace MouseTrack


#pragma once

#include <Eigen/Core>

#include <pcl/point_cloud.h>

class PointCloudDim2D
{
public:
    template<typename PointT>
    static PointCloudDim2D getMinMaxData2D(const pcl::PointCloud<PointT>& kCloud)
    {
        PointT minPt, maxPt;
        pcl::getMinMax3D(kCloud, minPt, maxPt);

        return PointCloudDim2D(
            Eigen::Vector2f(minPt.x, minPt.y),
            Eigen::Vector2f(maxPt.x, maxPt.y)
        );
    }

    float getCloudLenX() const
    {
        return maxPt.x() - minPt.x();
    }

    float getCloudLenY() const
    {
        return maxPt.y() - minPt.y();
    }

    const Eigen::Vector2f& getCloudMaxPt() const
    {
        return maxPt;
    }

    const Eigen::Vector2f& getCloudMinPt() const
    {
        return minPt;
    }

private:
    PointCloudDim2D(const Eigen::Vector2f& _minPt, const Eigen::Vector2f& _maxPt)
      : minPt(_minPt), maxPt(_maxPt)
    { }

    Eigen::Vector2f minPt, maxPt;
};


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

#ifndef BOOST_COMPUTE_ALGORITHM_ACCUMULATE_HPP
#define BOOST_COMPUTE_ALGORITHM_ACCUMULATE_HPP

#include <boost/preprocessor/seq/for_each.hpp>

#include <boost/compute/system.hpp>
#include <boost/compute/functional.hpp>
#include <boost/compute/command_queue.hpp>
#include <boost/compute/algorithm/reduce.hpp>
#include <boost/compute/algorithm/detail/serial_accumulate.hpp>
#include <boost/compute/container/array.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/detail/iterator_range_size.hpp>

namespace boost {
namespace compute {
namespace detail {

// Space complexity O(1)
template<class InputIterator, class T, class BinaryFunction>
inline T generic_accumulate(InputIterator first,
                            InputIterator last,
                            T init,
                            BinaryFunction function,
                            command_queue &queue)
{
    const context &context = queue.get_context();

    size_t size = iterator_range_size(first, last);
    if(size == 0){
        return init;
    }

    // accumulate on device
    array<T, 1> device_result(context);
    detail::serial_accumulate(
        first, last, device_result.begin(), init, function, queue
    );

    // copy result to host
    T result;
    ::boost::compute::copy_n(device_result.begin(), 1, &result, queue);
    return result;
}

// returns true if we can use reduce() instead of accumulate() when
// accumulate() this is true when the function is commutative (such as
// addition of integers) and the initial value is the identity value
// for the operation (zero for addition, one for multiplication).
template<class T, class F>
inline bool can_accumulate_with_reduce(T init, F function)
{
    (void) init;
    (void) function;

    return false;
}

/// \internal_
#define BOOST_COMPUTE_DETAIL_DECLARE_CAN_ACCUMULATE_WITH_REDUCE(r, data, type) \
    inline bool can_accumulate_with_reduce(type init, plus<type>) \
    { \
        return init == type(0); \
    } \
    inline bool can_accumulate_with_reduce(type init, multiplies<type>) \
    { \
        return init == type(1); \
    }

BOOST_PP_SEQ_FOR_EACH(
    BOOST_COMPUTE_DETAIL_DECLARE_CAN_ACCUMULATE_WITH_REDUCE,
    _,
    (char_)(uchar_)(short_)(ushort_)(int_)(uint_)(long_)(ulong_)
)

template<class T>
inline bool can_accumulate_with_reduce(T init, min<T>)
{
    return init == (std::numeric_limits<T>::max)();
}

template<class T>
inline bool can_accumulate_with_reduce(T init, max<T>)
{
    return init == (std::numeric_limits<T>::min)();
}

#undef BOOST_COMPUTE_DETAIL_DECLARE_CAN_ACCUMULATE_WITH_REDUCE

template<class InputIterator, class T, class BinaryFunction>
inline T dispatch_accumulate(InputIterator first,
                             InputIterator last,
                             T init,
                             BinaryFunction function,
                             command_queue &queue)
{
    size_t size = iterator_range_size(first, last);
    if(size == 0){
        return init;
    }

    if(can_accumulate_with_reduce(init, function)){
        T result;
        reduce(first, last, &result, function, queue);
        return result;
    }
    else {
        return generic_accumulate(first, last, init, function, queue);
    }
}

} // end detail namespace

/// Returns the result of applying \p function to the elements in the
/// range [\p first, \p last) and \p init.
///
/// If no function is specified, \c plus will be used.
///
/// \param first first element in the input range
/// \param last last element in the input range
/// \param init initial value
/// \param function binary reduction function
/// \param queue command queue to perform the operation
///
/// \return the accumulated result value
///
/// In specific situations the call to \c accumulate() can be automatically
/// optimized to a call to the more efficient \c reduce() algorithm. This
/// occurs when the binary reduction function is recognized as associative
/// (such as the \c plus<int> function).
///
/// Note that because floating-point addition is not associative, calling
/// \c accumulate() with \c plus<float> results in a less efficient serial
/// reduction algorithm being executed. If a slight loss in precision is
/// acceptable, the more efficient parallel \c reduce() algorithm should be
/// used instead.
///
/// For example:
/// \code
/// // with vec = boost::compute::vector<int>
/// accumulate(vec.begin(), vec.end(), 0, plus<int>());   // fast
/// reduce(vec.begin(), vec.end(), &result, plus<int>()); // fast
///
/// // with vec = boost::compute::vector<float>
/// accumulate(vec.begin(), vec.end(), 0, plus<float>());   // slow
/// reduce(vec.begin(), vec.end(), &result, plus<float>()); // fast
/// \endcode
///
/// Space complexity: \Omega(1)<br>
/// Space complexity when optimized to \c reduce(): \Omega(n)
///
/// \see reduce()
template<class InputIterator, class T, class BinaryFunction>
inline T accumulate(InputIterator first,
                    InputIterator last,
                    T init,
                    BinaryFunction function,
                    command_queue &queue = system::default_queue())
{
    return detail::dispatch_accumulate(first, last, init, function, queue);
}

/// \overload
template<class InputIterator, class T>
inline T accumulate(InputIterator first,
                    InputIterator last,
                    T init,
                    command_queue &queue = system::default_queue())
{
    typedef typename std::iterator_traits<InputIterator>::value_type IT;

    return detail::dispatch_accumulate(first, last, init, plus<IT>(), queue);
}

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

#endif // BOOST_COMPUTE_ALGORITHM_ACCUMULATE_HPP


#include "functions/functions.h"
#include "functions/RealVector.h"
#include <math.h>
#include <time.h>
#include <iostream>
#include <sstream>
#include <iomanip>
#include <string.h>
#include <boost/lexical_cast.hpp>

#ifndef M_PI
#define M_PI 3.14159265
#endif

#define MAX_BUFFER_SIZE_FUNCTIONS__  655536

using namespace std;

namespace functions{

std::string tabulate(const string& st, int n_tabs)
{
  ostringstream os;

  for (int i = 0; i < n_tabs;i ++) {
    os << "\t";
  }

  os << st;

  return os.str();
}

double reduceAngle(double phi)
{
  while ( fabs(phi) > M_PI) {
    phi = phi + ( phi > 0.0 ? -1.0 : 1.0 ) * 2.0 * M_PI;
  }

  return phi;
}

void rotateVector(std::vector<double> &v, double angle) {
  if (v.size() >= 2) {
    double x, y;

    cout << "Angle: " << angle << endl;

    x = v.at(0)*cos(angle) - v.at(1)*sin(angle);
    y = v.at(0)*sin(angle) + v.at(1)*cos(angle);
    v[0] = x;
    v[1] = y;
  }
}

void rotateVector(std::vector<double> &v, const DegMinSec &angle) {
  rotateVector(v, angle.toRadians());
}

std::vector<double>  getVectorFromFile(const std::string &fileName) throw (){

	vector <double> ret;

	ifstream filestr;
	try{
		filestr.open( fileName.c_str() );
		filestr.exceptions(ifstream::failbit | ifstream::badbit);
		ret = getVectorFromStream(filestr);
	}
	catch (...){
// 		cout << "Error flags: " << filestr.rdstate() << endl;
// 		ret.clear();
	}

	return ret;
}

std::vector< double > getVectorFromStream(istream& is)
{
  vector<double> ret;
  try {
    while ( is.good() && !is.eof() ){
      double au;
      if (is >> au) {
	ret.push_back(au);
      }
    }
  }catch (exception &e) {

  }
  return ret;
}

std::vector< double > getVectorFromString(const string& s)
{
  vector<double> ret;
  string ss = removeSpaces(s);
  char *copy_ = new char[ss.length() + 1];
  strcpy(copy_, ss.c_str());
  char *tok = copy_;
  tok = strtok(tok, " \n\t\r");
  try {
    while ( tok != NULL && strlen(tok) > 0 ) {
      ret.push_back(boost::lexical_cast<double>(tok));
      tok = strtok(NULL, " \n\t\r");
    }
  } catch (exception &e) {
    ret.clear();
  }
  delete[] copy_;
  return ret;
}

bool getMatrixFromFile(const std::string &fileName, int width, std::vector<std::vector<double> > &v) throw (){
	bool ret = true;

	if (v.size() > 0)
	  v.clear();

	ifstream filestr;
	try{

		double aux;
		int i;
		filestr.open( fileName.c_str() );
		filestr.exceptions(ifstream::failbit | ifstream::badbit);
		char buff[MAX_BUFFER_SIZE_FUNCTIONS__];
		filestr.getline(buff, MAX_BUFFER_SIZE_FUNCTIONS__ - 1); // Discard the first line
		while ( filestr.good() ){
			vector<double> v_aux;

			bool valid = false;

			for (i = 0; i < width && filestr.good(); i++) {
			  filestr >> aux;
			  v_aux.push_back(aux);
				if (aux != 0.0) {
					valid = true;
				}
			}

			if (i == width && i > 0) {
			  if (valid) {
			    // Discard the zero vectors
			    v.push_back(v_aux);
			  }
			} else {
			  ret = false;
			}
		}
	}
	catch (...){
		cout << "Error flags: " << filestr.rdstate() << endl;
		if  (!filestr.eof()){
			throw;
		}
	}

	return ret;
}

bool getMatrixFromFile(const std::string &fileName, std::vector<std::vector<double> > &v, bool discard_first) throw (){
	bool ret = true;

	if (v.size() > 0)
	  v.clear();

	ifstream filestr;
	char *buff = new char[MAX_BUFFER_SIZE_FUNCTIONS__ + 1];
	try{

		filestr.open( fileName.c_str() );

		if (filestr.is_open()) {
// 		  filestr.exceptions(ifstream::failbit | ifstream::badbit);
		  if (discard_first) {
			  filestr.getline(buff, MAX_BUFFER_SIZE_FUNCTIONS__ - 1); // Discard the first line
		  }
		  while ( !filestr.eof() && filestr.good() ){
			  vector<double> v_aux;
			  filestr.getline(buff, MAX_BUFFER_SIZE_FUNCTIONS__ - 1); // Get the current line
			  if (strlen(buff) > 0) {
			    string s(buff);
			    v_aux = getVectorFromString(s);
			    if (v_aux.size() > 0) {
				  v.push_back(v_aux);
			    }
			  }
		  }
		  filestr.close();
		} else {
		  ret = false;
		}
	}
	catch (exception &e){
		cerr << "Error flags: " << filestr.rdstate() << endl;
		cerr << "Exception catched while loading file " << fileName << " from disk. Content: " << e.what() << endl;
		if  (!filestr.eof()){
		  delete buff;
		  buff = NULL;
			throw e;
		}
		filestr.close();
	}
	delete[] buff;

	return ret;
}

std::string showTime(struct timeval t1, struct timeval t2){
	long usecs = (t2.tv_sec -t1.tv_sec)*1000.0 + (t2.tv_usec -t1.tv_usec)/1000.0;
	std::ostringstream os;
	os <<  ": "<<  usecs << "ms" << endl;

	return os.str();
}

float calculateLapseTime(const struct timeval &t1, const struct timeval &t2) {
	return (t2.tv_sec - t1.tv_sec) + (t2.tv_usec - t1.tv_usec)/1e6;
}

string numberToString(int number, int anchor) {
	ostringstream oss;

	oss.str("");
  oss.fill('0');
  oss.width(anchor);
  oss << right << number;
  return oss.str();
}

string numberToString(double data, int precision) {
  ostringstream oss;
  oss << fixed << setprecision(precision) << data;
  return oss.str();
}

string printMatrix(const std::vector< std::vector< double > >& mat, const std::string &sep)
{
	ostringstream os;

	for (unsigned int row = 0; row < mat.size(); row++) {
		os << printVector(mat[row]) << sep << endl;
	}

	return os.str();
}

string printMatrix(const std::vector< RealVector >& mat, const std::string &sep)
{
	ostringstream os;

	for (unsigned int row = 0; row < mat.size(); row++) {
		os << printVector(mat[row]) << sep << endl;
	}

	return os.str();
}

string matrixToMatlabString(const std::string& name, const std::vector< std::vector< double > >& mat)
{
	ostringstream os;

	os << name << " = [" << printMatrix(mat, ";") << "];" << endl;

	return os.str();
}

string matrixToMatlabString(const std::string& name, const std::vector< functions::RealVector>& mat)
{
	ostringstream os;

	os << name << " = [" << printMatrix(mat, ";") << "];" << endl;

	return os.str();
}

string removeSpaces(const std::string &st)
{
  string ret;
  if (st.size() == 0) {
    return st;
  }
  unsigned int first, last;
  for (first = 0; first < st.length() && (st.at(first)==' ' || st.at(first)=='\r' || st.at(first)=='\t') ; first++) {
  }
  for (last = st.length() ; last >= 1 && (st.at(last - 1)==' ' || st.at(last - 1)=='\r' || st.at(last - 1)=='\t') ; last--) {
  }
  if (first >= 0 && first < st.length() && last >= 0 && last <= st.length() && first < last) {
    string ret_2(st, first, last);
    ret = ret_2;
  }

  return ret;
}

bool writeStringToFile(const string& filename, const std::string &text)
{
  bool ret_val = true;

  ofstream ofs;

  ofs.open(filename.c_str());
  if (ofs.is_open()) {
    ofs << text;
  } else {
    ret_val = false;
  }
  ofs.close();

  return ret_val;
}

double pathLength(const vector< vector< double > >& path)
{
  double length = 0.0;

  for (unsigned int i = 0; i < path.size() - 1 && path.size() > 1; i++) {
    length += distance_(path[i], path[i + 1]);
  }

  return length;
}

double distance_(const std::vector<double> &v1, const std::vector<double> &v2)
{
  unsigned int length = minimum(v1.size(), v2.size());
  double distance = 0.0;

  for (unsigned int i = 0; i < length; i++) {
    distance += pow(v1.at(i) - v2.at(i), 2.0);
  }

  return sqrt(distance);
}

string loadStringFile(const string& filename)
{
  string ret;

  ifstream filestr;
  char *buff = new char[MAX_BUFFER_SIZE_FUNCTIONS__ + 1];
  try {
    filestr.open( filename.c_str() );
    if (filestr.is_open()) {
      filestr.exceptions(ifstream::failbit | ifstream::badbit);
      while ( !filestr.eof() && filestr.good() ) {
	filestr.getline(buff, MAX_BUFFER_SIZE_FUNCTIONS__ - 1); // Get the current line
	string s(buff);
	s.append("\n");
	if (s.size() > 0) {
	  ret.append(s);
	}
      }
      filestr.close();
    }
  }
  catch (exception &e){
    cerr << "Error flags: " << filestr.rdstate() << endl;
    cerr << "Exception catched while loading file " << filename << " from disk. Content: " << e.what() << endl;
    if  (!filestr.eof()){
      delete[] buff;
      buff = NULL;
      throw e;
    }
    filestr.close();
  }
  delete[] buff;

  return ret;
}

} // End of namespace functions


//
// Implements iLQR (on a traditional chain) for nonlinear dynamics and cost.
//
// Arun Venkatraman (arunvenk@cs.cmu.edu)
// December 2016
//

#pragma once

#include <templated/taylor_expansion.hh>
#include <utils/debug_utils.hh>

#include <Eigen/Dense>

#include <functional>
#include <vector>

namespace ilqr
{

// Defined in templated/taylor_expansion.hh
//template<int _rows, int _cols>
//using Matrix = Eigen::Matrix<double, _rows, _cols>;
//
//template<int _rows>
//using Vector = Eigen::Matrix<double, _rows, 1>;

template<int xdim, int udim>
struct HindsightBranch
{
    using Dynamics = std::function<Vector<xdim>(const Vector<xdim> &x, const Vector<udim> &u)>;
    using Cost = std::function<double(const Vector<xdim> &x, const Vector<udim> &u, const int t)>;
    using FinalCost = std::function<double(const Vector<xdim> &x)>;

    HindsightBranch(const Dynamics &dyn, const FinalCost &cost_final, const Cost &cost_regular, const double prob)
        : dynamics(dyn), final_cost(cost_final), cost(cost_regular), probability(prob)
    {
    }

    Dynamics dynamics;
    FinalCost final_cost;
    Cost cost;

    double probability = 0;

    // Feedback control gains.
    std::vector<Matrix<udim, xdim>> Ks;
    std::vector<Vector<udim>> ks;

    // Linearization points.
    std::vector<Vector<xdim>> xhat;
    std::vector<Vector<udim>> uhat;
};

template<int xdim, int udim>
class iLQRHindsightSolver
{
    static_assert(xdim > 0, "State dimension should be greater than 0");
    static_assert(udim > 0, "Control dimension should be greater than 0");
public:
    using Dynamics = std::function<Vector<xdim>(const Vector<xdim> &x, const Vector<udim> &u)>;
    using Cost = std::function<double(const Vector<xdim> &x, const Vector<udim> &u, const int t)>;
    using FinalCost = std::function<double(const Vector<xdim> &x)>;

    iLQRHindsightSolver(const std::vector<HindsightBranch<xdim,udim>> &branches);

    // Computes the control at timestep 0 using K0_, k0_ that is
    // shared across all branches.
    inline Vector<udim> compute_first_control(const Vector<xdim> &x0) const;

    // Computes the control at timestep t at xt.
    // :param alpha - Backtracking line search parameter.
    //      Setting to 1 gives regular forward pass.
    inline Vector<udim> compute_control_stepsize(const int branch_num, const Vector<xdim> &xt, const int t, const double alpha) const;

    // :param alpha - Backtracking line search parameter.
    //      Setting to 1 gives regular forward pass.
    inline double forward_pass(const int branch_num,
            const Vector<xdim> x_init,
            std::vector<Vector<xdim>> &states, std::vector<Vector<udim>> &controls,
            const double alpha ) const;

    // :param x_init - Initial state from which to start the system from.
    // :param u_nominal - Initial control used for the whole sequence during
    //      the first forward pass.
    // :param mu - Levenberg-Marquardt parameter for damping the least-squares.
    //      Setting it to 0 gets the default behavior. The damping makes the
    //      state-space steps smaller over iterations.
    inline void solve(const int T, const Vector<xdim> &x_init,
            const Vector<udim> u_nominal, const double mu,
            const int max_iters = 1000, bool verbose = false,
            const double cost_convg_ratio = 1e-4, const double start_alpha = 1.0,
            const bool warm_start = false, const int t_offset = 0);

    // Returns how many timesteps we have computed control policies for.
    inline int timesteps() const;

    // Set the probability of a branch. Allows it to be done in place so
    // warm start solving can be used.
    inline void set_branch_probability(const int branch_num, const double probability);

private:
    std::vector<HindsightBranch<xdim,udim>> branches_;

    // Feedback control gains.
    Matrix<udim, xdim> K0_;
    Vector<udim> k0_;

    // Linearization points for the first timestep.
    Vector<xdim> xhat0_ = Vector<xdim>::Zero();
    Vector<udim> uhat0_ = Vector<udim>::Zero();

    // Performs one timestep of the bellman backup.
    // :param t - passed to the cost runction
    // :param mu - Levenberg-Marquardt parameter
    inline void bellman_backup(const int branch_num, const int t,
        const double mu,
        const Matrix<xdim,xdim> &Vt1, const Matrix<1,xdim> &Gt1,
        Matrix<xdim,xdim> &Vt, Matrix<1,xdim> &Gt);

    inline double total_branch_probability();

};

} // namespace lqr

#include <templated/iLQR_hindsight_impl.hh>


// Copyright 2015-2018 Hans Dembinski
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

//[ guide_histogram_streaming

#include <boost/histogram.hpp>
#include <boost/histogram/ostream.hpp>
#include <cassert>
#include <iostream>
#include <sstream>
#include <string>

int main() {
  using namespace boost::histogram;

  std::ostringstream os;

  auto h1 = make_histogram(axis::regular<>(5, -1.0, 1.0, "axis 1"));
  h1.at(0) = 2;
  h1.at(1) = 4;
  h1.at(2) = 3;
  h1.at(4) = 1;

  // 1D histograms are rendered as an ASCII drawing
  os << h1;

  auto h2 = make_histogram(axis::regular<>(2, -1.0, 1.0, "axis 1"),
                           axis::category<std::string>({"red", "blue"}, "axis 2"));

  // higher dimensional histograms just have their cell counts listed
  os << h2;

  std::cout << os.str() << std::endl;

  assert(
      os.str() ==
      "histogram(regular(5, -1, 1, metadata=\"axis 1\", options=underflow | overflow))\n"
      "               +-------------------------------------------------------------+\n"
      "[-inf,   -1) 0 |                                                             |\n"
      "[  -1, -0.6) 2 |==============================                               |\n"
      "[-0.6, -0.2) 4 |============================================================ |\n"
      "[-0.2,  0.2) 3 |=============================================                |\n"
      "[ 0.2,  0.6) 0 |                                                             |\n"
      "[ 0.6,    1) 1 |===============                                              |\n"
      "[   1,  inf) 0 |                                                             |\n"
      "               +-------------------------------------------------------------+\n"
      "histogram(\n"
      "  regular(2, -1, 1, metadata=\"axis 1\", options=underflow | overflow)\n"
      "  category(\"red\", \"blue\", metadata=\"axis 2\", options=overflow)\n"
      "  (-1 0): 0 ( 0 0): 0 ( 1 0): 0 ( 2 0): 0 (-1 1): 0 ( 0 1): 0\n"
      "  ( 1 1): 0 ( 2 1): 0 (-1 2): 0 ( 0 2): 0 ( 1 2): 0 ( 2 2): 0\n"
      ")");
}

//]


#include <iostream>
#include <cstdlib>
#include <cstring>
#include <helib/FHE.h>
#include <NTL/ZZ.h>
#include <NTL/ZZX.h>
#include <map>
#include <string>
#include <ctime>
#include <chrono>
//#include <NTL/lzz_pXFactoring.h>

#include <helib/EncryptedArray.h>
#include <fstream>
#include <sstream>
#include <sys/time.h>

#include "mc_driver.hpp"
NTL_CLIENT

using namespace MC;

int main( const int argc, const char **argv )
{
    /** check for the right # of arguments **/
    if( argc == 3 )
    {
        MC::MC_Driver driver;

        /** example for piping input from terminal, i.e., using cat /
          if( std::strncmp( argv[ 1 ], "-o", 2 ) == 0 )
          {
          driver.parse( std::cin );
          }
         * simple help menu *
         else if( std::strncmp( argv[ 1 ], "-h", 2 ) == 0 )
         {
         std::cout << "use -o for pipe to std::cin\n";
         std::cout << "just give a filename to count from a file\n";
         std::cout << "use -h to get this menu\n";
         return( EXIT_SUCCESS );
         }
         * example reading input from a file *
         else
         {

         * assume file, prod code, use stat to check **/
        driver.parse( argv[1] );
        vector<string> inputlist = driver.inputlist;
        vector<string> outputlist = driver.outputlist;
        vector<tuple<string, MC::Bexp*>> eqnlist = driver.eqnlist;

        // init HElib argument
        long m = 0, p = 2, r = 1;
        long depth = atoi(argv[2]);
        long L = depth * 30; //Level
        long c = 2;
        long w = 64;
        long d = 1;
        long security = 128;
        long s = 0;//slot
        ZZX G;

        m = FindM(security, L, c, p, d, s, 0);
	cout << "selected m : " << m << endl;
	cerr << m << ", " ;
        FHEcontext context(m, p, r);
        buildModChain(context, L, c);
        FHESecKey sk(context);
        const FHEPubKey& pk = sk;

        G = context.alMod.getFactorsOverZZ()[0];

        sk.GenSecKey(w);

        addSome1DMatrices(sk);
        cout << "generated Key : " << endl;

        //encrypt inputlist, make memory
        map<string, Ctxt> memory;

        for(vector<string>::size_type i = 0; i < inputlist.size(); i++){
            Ctxt tmp(pk);
            int plaintext = 0;
            //plaintext modification
            if(i == 3 || i ==11)
                plaintext=1;
            cout << "input : " << inputlist[i] << " : " << plaintext << endl;
            pk.Encrypt(tmp, to_ZZX(plaintext));
            memory.insert( make_pair(inputlist[i], tmp) );
        }
        Ctxt true_ctxt(pk);
        pk.Encrypt(true_ctxt, to_ZZX(1));
        memory.insert( make_pair("true", true_ctxt) );

        Ctxt false_ctxt(pk);
        pk.Encrypt(false_ctxt, to_ZZX(0));
        memory.insert( make_pair("false", false_ctxt) );

        /* for(auto i = memory.begin(); i!= memory.end(); i++){
            ZZX memory_res;
            sk.Decrypt(memory_res, i->second);
            cout << i->first << " : " << memory_res[0] << endl;
        }*/
	//int start_time = time(0);
        std::chrono::system_clock::time_point StartTime = std::chrono::system_clock::now();

        for(auto i = 0 ; i < eqnlist.size(); i++){
            string lv = get<0>(eqnlist[i]);
            Bexp* bexp = get<1>(eqnlist[i]);
            auto top_op = bexp->head;
            int constant = bexp->constant;
            string var = bexp->var;
            Bexp* l_child = bexp->left;
            Bexp* r_child = bexp->right;
            if(top_op == MC::Bexp::Head::CONST){
                if(constant == 1){
                    memory.insert(make_pair(lv, true_ctxt ));
                }
                else if(constant == 0){
                    memory.insert(make_pair(lv, false_ctxt));
                }
            }
            else if(top_op == MC::Bexp::Head::VAR){
                memory.insert(make_pair(lv, memory.find(var)->second));
            }
            else if(top_op == MC::Bexp::Head::AND){
                auto lchild_op = l_child->head;
                string lchild_var = l_child->var;
                int lchild_const = l_child->constant;
                Ctxt lchild_ctxt(pk);
                if(lchild_op == MC::Bexp::Head::CONST){
                    if(lchild_const == 1){
                        lchild_ctxt = true_ctxt;
                    }
                    else{
                        lchild_ctxt = false_ctxt;
                    }
                }
                else if(lchild_op == MC::Bexp::Head::VAR){
                    lchild_ctxt = memory.find(lchild_var)->second;
                }

                auto rchild_op = r_child->head;
                string rchild_var = r_child->var;
                int rchild_const = r_child->constant;
                Ctxt rchild_ctxt(pk);
                if(rchild_op == MC::Bexp::Head::CONST){
                    if(rchild_const == 1){
                        rchild_ctxt = true_ctxt;
                    }
                    else{
                        rchild_ctxt = false_ctxt;
                    }
                }
                else if(rchild_op == MC::Bexp::Head::VAR){
                    rchild_ctxt = memory.find(rchild_var)->second;
                }

                /*
                ZZX lchild_res;
                ZZX rchild_res;
                sk.Decrypt(lchild_res, lchild_ctxt);
                sk.Decrypt(rchild_res, rchild_ctxt);
                */

                lchild_ctxt *= rchild_ctxt;
                lchild_ctxt.reLinearize();

                //ZZX and_res;
                //sk.Decrypt(and_res, lchild_ctxt);
                //cout << lchild_var << " * " << rchild_var << " = " << lv << endl;
                //cout << lchild_res[0] << " * " << rchild_res[0] << " = " << and_res[0] << endl;

                memory.insert(make_pair(lv, lchild_ctxt));

            }
            else if(top_op == MC::Bexp::Head::XOR){
                auto lchild_op = l_child->head;
                string lchild_var = l_child->var;
                int lchild_const = l_child->constant;
                Ctxt lchild_ctxt(pk);
                if(lchild_op == MC::Bexp::Head::CONST){
                    if(lchild_const == 1){
                        lchild_ctxt = true_ctxt;
                    }
                    else{
                        lchild_ctxt = false_ctxt;
                    }
                }
                else if(lchild_op == MC::Bexp::Head::VAR){
                    lchild_ctxt = memory.find(lchild_var)->second;
                }

                auto rchild_op = r_child->head;
                string rchild_var = r_child->var;
                int rchild_const = r_child->constant;
                Ctxt rchild_ctxt(pk);
                if(rchild_op == MC::Bexp::Head::CONST){
                    if(rchild_const == 1){
                        rchild_ctxt = true_ctxt;
                    }
                    else{
                        rchild_ctxt = false_ctxt;
                    }
                }
                else if(rchild_op == MC::Bexp::Head::VAR){
                    rchild_ctxt = memory.find(rchild_var)->second;
                }

                /*
                ZZX lchild_res;
                ZZX rchild_res;
                sk.Decrypt(lchild_res, lchild_ctxt);
                sk.Decrypt(rchild_res, rchild_ctxt);
                */

                lchild_ctxt += rchild_ctxt;

                /*
                ZZX and_res;
                sk.Decrypt(and_res, lchild_ctxt);
                cout << lchild_var << " + " << rchild_var << " = " << lv << endl;
                cout << lchild_res[0] << " + " << rchild_res[0] << " = " << and_res[0] << endl;
                */

                memory.insert(make_pair(lv, lchild_ctxt));

            }
            else if(top_op == MC::Bexp::Head::OR){
                auto lchild_op = l_child->head;
                string lchild_var = l_child->var;
                int lchild_const = l_child->constant;
                Ctxt lchild_ctxt(pk);
                if(lchild_op == MC::Bexp::Head::CONST){
                    if(lchild_const == 1){
                        lchild_ctxt = true_ctxt;
                    }
                    else{
                        lchild_ctxt = false_ctxt;
                    }
                }
                else if(lchild_op == MC::Bexp::Head::VAR){
                    lchild_ctxt = memory.find(lchild_var)->second;
                }

                auto rchild_op = r_child->head;
                string rchild_var = r_child->var;
                int rchild_const = r_child->constant;
                Ctxt rchild_ctxt(pk);
                if(rchild_op == MC::Bexp::Head::CONST){
                    if(rchild_const == 1){
                        rchild_ctxt = true_ctxt;
                    }
                    else{
                        rchild_ctxt = false_ctxt;
                    }
                }
                else if(rchild_op == MC::Bexp::Head::VAR){
                    rchild_ctxt = memory.find(rchild_var)->second;
                }

                /*
                ZZX lchild_res;
                ZZX rchild_res;
                sk.Decrypt(lchild_res, lchild_ctxt);
                sk.Decrypt(rchild_res, rchild_ctxt);
                */

                Ctxt tmp_ctxt1 = lchild_ctxt;
                tmp_ctxt1 += rchild_ctxt;
                lchild_ctxt *= rchild_ctxt;
                lchild_ctxt +=tmp_ctxt1;
                lchild_ctxt.reLinearize();

                /*
                ZZX and_res;
                sk.Decrypt(and_res, lchild_ctxt);
                cout << lchild_var << " or " << rchild_var << " = " << lv << endl;
                cout << lchild_res[0] << " or " << rchild_res[0] << " = " << and_res[0] << endl;
                */

                memory.insert(make_pair(lv, lchild_ctxt));

            }
            else if(top_op == MC::Bexp::Head::NOT){
                auto rchild_op = r_child->head;
                string rchild_var = r_child->var;
                int rchild_const = r_child->constant;
                Ctxt rchild_ctxt(pk);
                if(rchild_op == MC::Bexp::Head::CONST){
                    if(rchild_const == 1){
                        rchild_ctxt = true_ctxt;
                    }
                    else{
                        rchild_ctxt = false_ctxt;
                    }
                }
                else if(rchild_op == MC::Bexp::Head::VAR){
                    rchild_ctxt = memory.find(rchild_var)->second;
                }

                //ZZX rchild_res;
                //sk.Decrypt(rchild_res, rchild_ctxt);

                rchild_ctxt += true_ctxt;

                /*
                ZZX and_res;
                sk.Decrypt(and_res, rchild_ctxt);
                cout << "not " << rchild_var << " = " << lv << endl;
                cout << "not " << rchild_res[0] << " = " << and_res[0] << endl;
                */

                memory.insert(make_pair(lv, rchild_ctxt));
            }

        }
        cout << "circuit evaluation finished" << endl;
        for(auto i = 0; i < outputlist.size(); i++){
            ZZX tmp_res;
            sk.Decrypt(tmp_res, memory.find(outputlist[i])->second) ;
            cout << "output : " << outputlist[i] << " : " << tmp_res[0] << endl;
        }

        std::chrono::system_clock::time_point EndTime = std::chrono::system_clock::now();
        std::chrono::milliseconds mill  = std::chrono::duration_cast<std::chrono::milliseconds>(EndTime - StartTime);

	//int eval_time = time(0) - start_time;
	string circuit_filename = argv[1];
	string depth_string = argv[2];
	circuit_filename += depth_string;

        cerr << depth_string << ", " <<  mill.count()  << endl;
        /*}*/
        //driver.print( std::cout ) << "\n";
    }
    else
    {
        /** exit with failure condition **/
        return ( EXIT_FAILURE );
    }
    return( EXIT_SUCCESS );
}


/*

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

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

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

*/

#ifndef PIC_COMPUTER_VISION_NELDER_MEAD_OPT_HOMOGRAPHY_HPP
#define PIC_COMPUTER_VISION_NELDER_MEAD_OPT_HOMOGRAPHY_HPP

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

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

namespace pic {

#ifndef PIC_DISABLE_EIGEN

class NelderMeadOptHomography: public NelderMeadOptBase<float>
{
public:
    std::vector< Eigen::Vector2f > m0, m1;

    /**
     * @brief NelderMeadOptHomography
     * @param m0
     * @param m1
     * @param inliers
     */
    NelderMeadOptHomography(std::vector< Eigen::Vector2f > &m0,
                            std::vector< Eigen::Vector2f > &m1,
                            std::vector< unsigned int > inliers) : NelderMeadOptBase()
    {
        filterInliers(m0, inliers, this->m0);
        filterInliers(m1, inliers, this->m1);
    }

    /**
     * @brief Homography
     * @param H
     * @param p
     * @return
     */
    Eigen::Vector2f Homography(Eigen::Matrix3f &H, Eigen::Vector2f &p)
    {
        Eigen::Vector3f ret = H * Eigen::Vector3f(p[0], p[1], 1.0f);

        return Eigen::Vector2f(ret[0] / ret[2], ret[1] / ret[2]);
    }

    /**
     * @brief function
     * @param x
     * @param n
     * @return
     */
    float function(float *x, unsigned int n)
    {
        float err = 0.0f;

        Eigen::Matrix3f H = getMatrixfFromLinearArray(x, 3, 3);
        H(2, 2) = 1.0f;

        Eigen::Matrix3f H_inv = H.inverse();

        for(unsigned int i = 0; i < m0.size(); i++) {
            float dU, dV;

            // | H p0 - p1 | error
            Eigen::Vector2f p0_H = Homography(H, m0[i]);

            dU = m1[i][0] - p0_H[0];
            dV = m1[i][1] - p0_H[1];

            err += dU * dU + dV * dV;

            // | H p1 - p0 | error
            Eigen::Vector2f p1_H = Homography(H_inv, m1[i]);

            dU = m0[i][0] - p1_H[0];
            dV = m0[i][1] - p1_H[1];

            err += dU * dU + dV * dV;

        }

        return err;
    }

    /**
     * @brief run
     * @param x_start
     * @param n
     * @param epsilon
     * @param x
     * @return
     */
    float *run(float *x_start, unsigned int n, float epsilon = 1e-4f, int max_iterations = 1000, float *x = NULL)
    {
        if(n != 8) {
            return x_start;
        }

        if(x == NULL) {
            x = new float[n + 1];
        }

        x = run_aux(x_start, n, epsilon, max_iterations, x);
        x[8] = 1.0f;

        return x;
    }
};

#endif

}

#endif // PIC_COMPUTER_VISION_NELDER_MEAD_OPT_HOMOGRAPHY_HPP


#include "ScannerCorridor.h"

#include <AdventOfCodeCommon/DisableLibraryWarningsMacros.h>

__BEGIN_LIBRARIES_DISABLE_WARNINGS
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
__END_LIBRARIES_DISABLE_WARNINGS

namespace AdventOfCode
{
namespace Year2017
{
namespace Day13
{

ScannerCorridor::ScannerCorridor(RangeToDepthMap rangeToDepthMap)
    : m_rangeToDepthMap{rangeToDepthMap}
{

}

unsigned ScannerCorridor::severityOfWholeTrip() const
{
    unsigned severity = 0;
    for (const auto& depthRangePair : m_rangeToDepthMap)
    {
        unsigned depth;
        unsigned range;
        std::tie(depth, range) = depthRangePair;

        if (isCaught(range, depth))
        {
            severity += depth * range;
        }
    }

    return severity;
}

unsigned ScannerCorridor::smallestDelayNotToGetCaught() const
{
    unsigned smallestDelay = 0;
    while (true)
    {
        bool isEverCaught = false;
        for (const auto& depthRangePair : m_rangeToDepthMap)
        {
            unsigned depth;
            unsigned range;
            std::tie(depth, range) = depthRangePair;

            if (isCaught(range, depth, smallestDelay))
            {
                isEverCaught = true;
                break;
            }
        }

        if (!isEverCaught)
        {
            break;
        }

        ++smallestDelay;
    }

    return smallestDelay;
}

ScannerCorridor ScannerCorridor::fromScannerRangeLines(const std::vector<std::string> scannerRangeLines)
{
    RangeToDepthMap rangeToDepthMap;
    for (const auto& scannerRangeLine : scannerRangeLines)
    {
        std::vector<std::string> tokens;
        boost::split(tokens, scannerRangeLine, boost::is_any_of(": "), boost::token_compress_on);

        if (tokens.size() != 2)
        {
            throw std::runtime_error("Each line needs to have exactly 2 tokens.");
        }

        unsigned depth = boost::lexical_cast<unsigned>(tokens[0]);
        unsigned range = boost::lexical_cast<unsigned>(tokens[1]);

        if (range < 2)
        {
            throw std::runtime_error("Range less than 2 is invalid.");
        }

        bool insertionTookPlace;
        std::tie(std::ignore, insertionTookPlace) = rangeToDepthMap.insert(std::make_pair(depth, range));
        if (!insertionTookPlace)
        {
            throw std::runtime_error("Same depth appears twice in the input lines.");
        }
    }

    return ScannerCorridor{std::move(rangeToDepthMap)};
}

constexpr bool ScannerCorridor::isCaught(unsigned range, unsigned depth, unsigned delay)
{
    unsigned totalStepsInPath = (2 * (range - 1));

    // Smallest delay value that has the equivalent effect
    unsigned minEquivalentDelay = delay % totalStepsInPath;

    // Number of steps the scanner has taken since last being at the top
    unsigned scannerNumStepsFromTop = (depth + minEquivalentDelay) % totalStepsInPath;

    unsigned scannerPos{};

    // Scanner in the first half of its path
    if (scannerNumStepsFromTop <= range - 1)
    {
        scannerPos = scannerNumStepsFromTop;
    }

    // Scanner in the second half of its path
    else
    {
        // Number of steps the scanner has taken since last being at the bottom
        unsigned scannerNumStepsFromBottom = scannerNumStepsFromTop - (range - 1);

        scannerPos = (range - 1) - scannerNumStepsFromBottom;
    }

    return scannerPos == 0;
}

}
}
}


#pragma once

#include <boost/optional.hpp>
#include <crab/domains/term/term_expr.hpp>
#include <crab/domains/term/term_operators.hpp>

/*
   Simplifiers for table terms after giving meaning to functors.
*/

namespace crab {
namespace domains {
namespace term {

// Common API to simplifiers
template <class Num, class Ftor> class Simplifier {
protected:
  using term_table_t = term_table<Num, Ftor>;
  using term_id_t = typename term_table_t::term_id_t;

  term_table_t &_ttbl;

public:
  Simplifier(term_table_t &term_table) : _ttbl(term_table) {}

  virtual ~Simplifier() {}

  virtual void simplify() = 0;

  // This should not modify the term table
  // FIXME: cannot make const the method without changing
  // constness of other methods.
  virtual boost::optional<term_id_t> simplify_term(term_id_t t) = 0;
};

// Trivial simplifier by giving standard mathematical meaning
// to arithmetic operators assuming that conmutativity,
// associativity etc properties hold as expected.
template <class Num> class NumSimplifier : Simplifier<Num, term_operator_t> {
  using simplifier_t = Simplifier<Num, term_operator_t>;

  using term_table_t = term_table<Num, term_operator_t>;
  using term_id_t = typename term_table_t::term_id_t;
  using term_t = typename term_table_t::term_t;

  // Simplify term f(left,right)
  boost::optional<term_id_t> simplify_term(term_operator_t f, term_id_t left,
                                           term_id_t right) {
    // Only consider these two rules:
    //   '/'('*'(x,y),x) = y
    //   '/'('*'(x,y),y) = x
    switch (f) {
    case TERM_OP_SDIV:
    case TERM_OP_UDIV: {
      term_t *tleft = this->_ttbl.get_term_ptr(left);
      term_t *tright = this->_ttbl.get_term_ptr(right);

      if ((tleft->kind() == TERM_APP) && term_ftor(tleft) == TERM_OP_MUL) {
        const std::vector<term_id_t> &args(term_args(tleft));
        assert(args.size() == 2);
        term_t *tl = this->_ttbl.get_term_ptr(args[0]);
        term_t *tr = this->_ttbl.get_term_ptr(args[1]);

        if (tl == tright)
          return args[1];
        if (tr == tright)
          return args[0];
      }
    }
    default:
      return boost::optional<term_id_t>();
    }
  }

public:
  NumSimplifier(term_table_t &term_table) : simplifier_t(term_table) {}

  void simplify() {}

  boost::optional<term_id_t> simplify_term(term_id_t t) {
    if (term_t *tt = this->_ttbl.get_term_ptr(t)) {
      if (tt->kind() == TERM_APP) {
        const std::vector<term_id_t> &args(term_args(tt));
        assert(args.size() == 2);
        return simplify_term(term_ftor(tt), args[0], args[1]);
      }
    }
    return boost::optional<term_id_t>();
  }
};

} // end namespace term
} // end namespace domains
} // end namespace crab


//
// Created by Vlad Argunov on 19/10/2021.
//

#include "FileManagement.h"
#include <string>
#include <fstream>
#include <sstream>
#include <Eigen/Dense>
#include <stdlib.h>

std::vector<int> size_of_csv(const std::string& path){
// The solution is from https://stackoverflow.com/questions/415515/how-can-i-read-and-manipulate-csv-file-data-in-c?noredirect=1&lq=1
// It is enhanced to compute the rows and columns of csv
    int rows = 0;
    int cols = 0;

    std::ifstream  data(path);
    std::string line;
    bool compute_cols = 0;
    while(std::getline(data,line))
    {
        if (compute_cols == 0){
            cols = count(line.begin(), line.end(), ',');
            compute_cols = 1;
        }

        rows++;
    }
    std::vector<int> v {rows, cols + 1};
    return v;
}

void read_csv_into_matrix(const std::string& path, Eigen::MatrixXd & result ){
// The solution is from https://stackoverflow.com/questions/415515/how-can-i-read-and-manipulate-csv-file-data-in-c?noredirect=1&lq=1
// It is enhanced to handle Eigen matrices

    std::ifstream data(path);
    std::string line;

    std::vector<int> matrix_size = size_of_csv(path);
    result.resize(matrix_size[0], matrix_size[1]);

    int row = 0;
    while(std::getline(data,line))
    {

        std::stringstream  lineStream(line);
        std::string        cell;

        int col = 0;
        while(std::getline(lineStream,cell,','))
        {

            if (row == 0 && col == 0){
                cell = cell.substr(3);

            }

            double numeric_cell = atof(cell.c_str());
            result(row, col) = numeric_cell;
            col++;
        }
        row++;
    }

}

void saveData(std::string fileName, Eigen::MatrixXd  matrix)
{
    // Code from https://aleksandarhaber.com/eigen-matrix-library-c-tutorial-saving-and-loading-data-in-from-a-csv-file/
    //https://eigen.tuxfamily.org/dox/structEigen_1_1IOFormat.html
    const static Eigen::IOFormat CSVFormat(Eigen::FullPrecision, Eigen::DontAlignCols, ", ", "\n");

    std::ofstream file(fileName);
    if (file.is_open())
    {
        file << matrix.format(CSVFormat);
        file.close();
    }
}


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

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

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

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

 /*!

    @ingroup group-euler
    Function object implementing stirling capabilities

    Computes stirling formula for the gamma function

    @par Semantic:

    For every parameter of floating type T

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

    Computes \f$\Gamma(x) \approx \sqrt{2 \pi} x^{x-\frac12} e^{-x} ( 1 + \frac1{x} P(\frac1{x}))\f$,
    where \f$P\f$ is a polynomial.

    The formula implementation is usable for x between 33 and 172, according cephes

    @see gamma

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

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

#endif


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

#include <boost/hana/assert.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/if.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/minus.hpp>
#include <boost/hana/optional.hpp>
#include <boost/hana/pair.hpp>
#include <boost/hana/tuple.hpp>
#include <boost/hana/unfold_right.hpp>
namespace hana = boost::hana;

BOOST_HANA_CONSTANT_CHECK(
    hana::unfold_right<hana::tuple_tag>(hana::int_c<10>, [](auto x) {
        return hana::if_(x == hana::int_c<0>,
            hana::nothing,
            hana::just(hana::make_pair(x, x - hana::int_c<1>))
        );
    })
    ==
    hana::tuple_c<int, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1>
);

int main() { }


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

  @copyright 2012-2015 NumScale SAS
  @copyright 2015 J.T.Lapreste

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!

    @ingroup group-arithmetic
    Function object function implementing fma capabilities

    Computes the (fused) multiply add of the three parameters.

    @par semantic:
    For any given value @c x,  @c y,  @c z of type @c T:

    @code
    T r = fma(x, y, z);
    @endcode

    The code is similar to:

    @code
    T r = x*y+z;
    @endcode

    @par Note
    Correct fused multiply/add implies

    - only one rounding

    - no "intermediate" overflow

    fma provides this each time it is reasonable
    in terms of performance (mainly if the system has the hard
    wired capability).

    If you need "real" fma capabilities in all circumstances in your own
    code you can use correct_fma (although it can be expansive).

    Also :

    - fma(x, y, z, nooverflow_) provides a "only one rounding" mode but does not care for possible
    intermediate overflow.

    @par Decorators

    std_ for floating entries

    @par Alias

    @c madd

    @see  correct_fma, fms, fnma, fnms
  **/
    const boost::dispatch::functor<tag::fma_> fma = {};
  }
} }
#endif

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

#endif


//////////////////////////////////
// This implementation has been //
// placed in the public domain  //
// by			        //
//			      	//
// Petter Solnoer - 1/11/2021 	//
//////////////////////////////////

#include "joye_libert.h"
#include "misc.h"

#include <iostream>
//#include <NTL/ZZ_pXFactoring.h>
//#include <NTL/ZZ_pEX.h>
//#include <NTL/GF2X.h>
//#include <NTL/GF2E.h>
//#include <NTL/GF2XFactoring.h>

void jl_key_gen(mpz_t N, mpz_t y, mpz_t k, mpz_t p)
{
	//** Generate primes p, q congruent to 1 mod 2^{k}. **
	// Start by initializing a prng.
	gmp_randstate_t state;
	gmp_randinit_mt(state);

	// Variable to hold q
	mpz_t q;
	mpz_init(q);
	// Variables to hold test primes
	mpz_t p_test, q_test;
	mpz_init(p_test);
	mpz_init(q_test);

	// Variable to hold pseudo-safeness test divisor
	// and pseudo_test assume 32-bit messages.
	mpz_t pseudo_safeness_divisor, pseudo_test_p, pseudo_test_q;
	mpz_init(pseudo_safeness_divisor);
	mpz_init(pseudo_test_p);
	mpz_init(pseudo_test_q);

	// Set divisor to 2^{32}
	mpz_ui_pow_ui(pseudo_safeness_divisor, 2, 32);

	// Set p to random value with 1024 bits
	mpz_urandomb(p_test, state, 1024);

	// Find the modulus
	mpz_t mod;
	mpz_init(mod);
	mpz_mod(mod, p_test, pseudo_safeness_divisor);

	mpz_sub(p_test, p_test, mod);
	mpz_add_ui(p_test, p_test, 1);

	// We now have a number that is congruent to 1, as desired.
	// Now we must check if it is prime!

	//mpz_nextprime(p_test, p_test);
	// Start by finding appropriate p
	mpz_t one;
	mpz_init_set_ui(one, 1);

	std::cout << "Find appropriate p\n";
	mpz_t t;
	mpz_init(t);
	while(true)
	{
		// check if prime
		//mpz_mod(t, p_test, pseudo_safeness_divisor);
		//gmp_printf("%Zd is the congruence.\n", t);
		//if (mpz_congruent_2exp_p(p_test, one, 32))
		if (mpz_probab_prime_p(p_test, 30))
		{
			// check for pseudo-safeness
			// condition: p_test - 1 / 2^{k} is also prime
			mpz_t tmp;
			mpz_init(tmp);
			mpz_sub_ui(tmp, p_test, 1);

			mpz_divexact(pseudo_test_p, tmp, pseudo_safeness_divisor);
			if (mpz_probab_prime_p(pseudo_test_p, 30))
			{
				// We have probably found a good value
				// for p
				gmp_printf("Good value for p: %Zd\n", p_test);
				mpz_set(p, p_test);
				break;
			}
		}
		// Check next prime, probabilistic, chance of composite passing
		// is extremely small
		mpz_add(p_test, p_test, pseudo_safeness_divisor);
	}

	// Proceed to find appropriate q
	std::cout << "Find appropriate q\n";
	mpz_add(q_test, p_test, pseudo_safeness_divisor);
	//mpz_nextprime(q_test, p_test);
	while(true)
	{
		// check if prime
		if (mpz_probab_prime_p(q_test, 30))
		{
			// Check for pseudo-safeness
			// condition: q_test - 1 / 2^{k} is also prime
			mpz_t tmp;
			mpz_init(tmp);
			mpz_sub_ui(tmp, q_test, 1);

			mpz_cdiv_q(pseudo_test_q, tmp, pseudo_safeness_divisor);
			if (mpz_probab_prime_p(pseudo_test_q, 30))
			{
				// We have probably found a good value
				// for q
				gmp_printf("Good value for q: %Zd\n", q_test);
				mpz_set(q, q_test);
				break;
			}
		}
		// Check next prime, probabilistic, chance of composite passing
		// is extremely small
		mpz_add(q_test, q_test, pseudo_safeness_divisor);
	}

	// Set N = p*q
	mpz_mul(N, p, q);

	// Find y, using algorithm 2 from Joye-Libert paper
	// Assume that the authors mean an arbitrary primitive 2^{th}
	// root of unity, i.e., a generator for the cyclic group under multiplication.

	// First select elements from the multiplicative groups Zp* and Zq*
	//
	// Select a generator for the multiplicative group 2^{k}
	// Iterate through primes and divide (?)
	//std::set<mpz_t> prime_factors;
	//std::set<mpz_t>::iterator it;
	// Array to hold prime factors
	// and number of prime factors
	/* ###################################### CONFERENCE PAPER SHIT #############
	mpz_t prime_factors[1000];
	int num_prime_factors = 0;

	mpz_t i, j;
	mpz_init(i);
	mpz_init(j);

	mpz_set_ui(i, 2);
	mpz_sub_ui(j, pseudo_safeness_divisor, 1);
	std::cout << "Find prime factors\n";
	while(true)
	{
		if(mpz_divisible_p(j, i))
		{
			gmp_printf("%Zd is a prime factor\n", i);
			mpz_set(prime_factors[num_prime_factors], i);
			num_prime_factors++;
			mpz_divexact(j, j, i);
			gmp_printf("j is now: %Zd\n", j);
			if(!mpz_cmp(j,one))
			{
				std::cout << "Breaking out\n";
				break;
			}
		} else {
			mpz_nextprime(i, i);
		}
	}

	std::cout << "Find a generator\n";

	// We need to 2^{k} is an extension field with characteristic 2.
	// So we need to use polynomials to represent the elements of
	// the multiplicative group. We use the NTL library by
	// Victor Shoup to do this.

	// We begin by defining the underlying field GF(2):
	//NTL::ZZ_p::init(NTL::ZZ(2));
	// Declare a polynomial and initialize with degree k=32

	NTL::GF2X irred_polynomial;

	NTL::BuildIrred(irred_polynomial, 32);
	NTL::GF2XModulus pol_mod(irred_polynomial);
	NTL::GF2E::init(irred_polynomial);
	// Declare and initialize a polynomial used
	// to search for generator.
	NTL::GF2X g_pol;
	NTL::GF2X f_pol;
	NTL::BuildIrred(f_pol, 30);

	// Variable to check if generator is found
	int generator_found = 0;

	mpz_t exponent, flag, g;
	mpz_init(exponent);
	mpz_init(flag);
	mpz_init(g);

	NTL::ZZ ntl_exponent;
	NTL::GF2X ntl_flag, g_pol_min;
	*/ // #### CONFERENCE PAPER SHIT

	/*mpz_t flag, exponent, g;
	mpz_init(flag);
	mpz_init(exponent);
	mpz_init_set_ui(g, 1);*/
	// j should contain 2^{k} - 1
	//
	/* #### CONFERENCE PAPER SHIT
	mpz_sub_ui(j, pseudo_safeness_divisor, 1);
	while(true)
	{
		generator_found = 1;
		// Pick a value in the set
		//mpz_add_ui(g, g, 1);
		g_pol = NTL::GF2X::zero();
		while (NTL::IsZero(g_pol))
		{
			// A primitive polynomial must be
			// irreducible
			NTL::BuildRandomIrred(g_pol, f_pol);
		}
		std::cout << "pol_mod: " << irred_polynomial << std::endl;
		std::cout << "g_pol: " << g_pol << std::endl;

		// Test if value is a generator
		for (int i = 0; i < num_prime_factors; ++i)
		{
			mpz_divexact(exponent, j, prime_factors[i]);
			gmp_printf("Exponent being tested: %Zd\n", exponent);

			// Convert exponent to MPZ::ZZ
			MPZToZZ(&ntl_exponent, exponent);

			//mpz_powm(flag, g, exponent, pseudo_safeness_divisor);
			NTL::PowerMod(ntl_flag, g_pol, ntl_exponent, pol_mod);

			std::cout << "Flag: " << ntl_flag << std::endl;

			unsigned char buffer[4];
			unsigned int integer = 0;
			MyBytesFromGF2X(buffer, ntl_flag, 4);
			std::memcpy(&integer, buffer, 4);
			//ZZToMPZ(flag, &ntl_flag);

			std::cout << "Integer: " << integer << std::endl;

			//gmp_printf("Flag: %Zd\n", flag);
			//if (!mpz_cmp(flag, one))
			if(integer == 1)
			{
				// Not a generator
				//gmp_printf("%Zd is not a generator!\n", g);
				generator_found = 0;
				//exit(1);
				break;
			}
		}
		if(generator_found)
		{
			std::cout << "g_pol before: " << g_pol << std::endl;
			unsigned char buffer[4];
			unsigned int g_int = 0;
			MyBytesFromGF2X(buffer, g_pol, 4);
			std::memcpy(&g_int, buffer, 4);
			mpz_set_ui(g, g_int);
			break;
		}
	}

	gmp_printf("The generator found: %Zd\n", g);

	// We have found a generator! Now we complete algorithm 2 from Joye-Libert paper
	// Pick yp and yq at random from the multiplicative cyclic groups of Zp and Zq
	mpz_t yp, yq;
	mpz_init(yp);
	mpz_init(yq);

	while(true)
	{
		mpz_urandomm(yp, state, p);
		mpz_urandomm(yq, state, q);
		if ( mpz_cmpabs_ui(yp, 0) && mpz_cmpabs_ui(yq, 0) )
		{
			break;
		}
	}
	// Check legendre symbol, update using generator
	int legendre_p, legendre_q;

	gmp_printf("yp: %Zd\n", yp);
	gmp_printf("yq: %Zd\n", yq);

	int test_leg;

	legendre_p = mpz_legendre(yp, p);
	legendre_q = mpz_legendre(yq, q);
	std::cout << "Legendre yp/p pre-check: " << legendre_p << std::endl;
	std::cout << "Legendre yq/q pre-check: " << legendre_q << std::endl;

	std::cout << "Check legendre symbols\n";

	if (legendre_p == 1)
	{
		// Update yp using generator
		mpz_mul(yp, yp, g);
		mpz_mod(yp, yp, p);
		legendre_p = mpz_legendre(yp, p);
		std::cout << "Legendre yp/p post-check: " << legendre_p << std::endl;
	}
	if (legendre_q == 1)
	{
		// Update yq
		mpz_mul(yq, yq, g);
		mpz_mod(yq, yq, q);
		legendre_q = mpz_legendre(yq, q);
		std::cout << "Legendre yq/q post-check: " << legendre_q << std::endl;
	}

	std::cout << "Legendre symbols are good\n";
	// Set y
	mpz_t p_inv;
	mpz_init(p_inv);
	//mpz_invert(p_inv, p, q);
	mpz_invert(p_inv, p, N);

	mpz_sub(y, yq, yp);
	mpz_mod(y, y, q);
	mpz_mul(y, y, p_inv);
	//mpz_mod(y, y, q);
	mpz_mul(y, y, p);
	mpz_add(y, y, yp);

	gmp_printf("N: %Zd\n", N);
	gmp_printf("The y found: %Zd\n", y);

	// Check if desirable properties for y has been attained:
	// Check jacibo first:
	int jacobi_n;
	*/ // ################### CONFERENCE PAPER SHIT
	// Generate a random y in Z_{N}
	mpz_set_ui(y, 0);
	while ((mpz_legendre(y, p) != -1) & (mpz_legendre(y,q) != -1))
	{
		// Generate random y in Z_N
		mpz_urandomm(y, state, N);
	}
	int jacobi_n = mpz_jacobi(y, N);
	int legendre_p = mpz_legendre(y, p);
	int legendre_q = mpz_legendre(y, q);

	std::cout << "Jacobi: " << jacobi_n << std::endl;
	std::cout << "Legendre y/p final: " << legendre_p << std::endl;
	std::cout << "Legendre y/q final: " << legendre_q << std::endl;
}

void jl_encrypt(mpz_t *c, mpz_t *m, mpz_t *y)
{

}

void jl_decrypt(mpz_t *m, mpz_t *c, mpz_t *p)
{
}


/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011-2014, Willow Garage, Inc.
 *  Copyright (c) 2014-2015, Open Source Robotics Foundation
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Open Source Robotics Foundation 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 OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

/** \author Jia Pan */

#include <hpp/fcl/math/transform.h>
#include <boost/math/constants/constants.hpp>

namespace fcl
{

void Quaternion3f::fromRotation(const Matrix3f& R)
{
  const int next[3] = {1, 2, 0};

  FCL_REAL trace = R(0, 0) + R(1, 1) + R(2, 2);
  FCL_REAL root;

  if(trace > 0.0)
  {
    // |w| > 1/2, may as well choose w > 1/2
    root = sqrt(trace + 1.0);  // 2w
    data[0] = 0.5 * root;
    root = 0.5 / root;  // 1/(4w)
    data[1] = (R(2, 1) - R(1, 2))*root;
    data[2] = (R(0, 2) - R(2, 0))*root;
    data[3] = (R(1, 0) - R(0, 1))*root;
  }
  else
  {
    // |w| <= 1/2
    int i = 0;
    if(R(1, 1) > R(0, 0))
    {
      i = 1;
    }
    if(R(2, 2) > R(i, i))
    {
      i = 2;
    }
    int j = next[i];
    int k = next[j];

    root = sqrt(R(i, i) - R(j, j) - R(k, k) + 1.0);
    FCL_REAL* quat[3] = { &data[1], &data[2], &data[3] };
    *quat[i] = 0.5 * root;
    root = 0.5 / root;
    data[0] = (R(k, j) - R(j, k)) * root;
    *quat[j] = (R(j, i) + R(i, j)) * root;
    *quat[k] = (R(k, i) + R(i, k)) * root;
  }
}

void Quaternion3f::toRotation(Matrix3f& R) const
{
  assert (.99 < data [0]*data [0] + data [1]*data [1] +
	  data [2]*data [2] + data [3]*data [3]);
  assert (data [0]*data [0] + data [1]*data [1] +
	  data [2]*data [2] + data [3]*data [3] < 1.01);
  FCL_REAL twoX  = 2.0*data[1];
  FCL_REAL twoY  = 2.0*data[2];
  FCL_REAL twoZ  = 2.0*data[3];
  FCL_REAL twoWX = twoX*data[0];
  FCL_REAL twoWY = twoY*data[0];
  FCL_REAL twoWZ = twoZ*data[0];
  FCL_REAL twoXX = twoX*data[1];
  FCL_REAL twoXY = twoY*data[1];
  FCL_REAL twoXZ = twoZ*data[1];
  FCL_REAL twoYY = twoY*data[2];
  FCL_REAL twoYZ = twoZ*data[2];
  FCL_REAL twoZZ = twoZ*data[3];

  R.setValue(1.0 - (twoYY + twoZZ), twoXY - twoWZ, twoXZ + twoWY,
             twoXY + twoWZ, 1.0 - (twoXX + twoZZ), twoYZ - twoWX,
             twoXZ - twoWY, twoYZ + twoWX, 1.0 - (twoXX + twoYY));
}

void Quaternion3f::fromAxes(const Vec3f axis[3])
{
  // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
  // article "Quaternion Calculus and Fast Animation".

  const int next[3] = {1, 2, 0};

  FCL_REAL trace = axis[0][0] + axis[1][1] + axis[2][2];
  FCL_REAL root;

  if(trace > 0.0)
  {
    // |w| > 1/2, may as well choose w > 1/2
    root = sqrt(trace + 1.0);  // 2w
    data[0] = 0.5 * root;
    root = 0.5 / root;  // 1/(4w)
    data[1] = (axis[1][2] - axis[2][1])*root;
    data[2] = (axis[2][0] - axis[0][2])*root;
    data[3] = (axis[0][1] - axis[1][0])*root;
  }
  else
  {
    // |w| <= 1/2
    int i = 0;
    if(axis[1][1] > axis[0][0])
    {
      i = 1;
    }
    if(axis[2][2] > axis[i][i])
    {
      i = 2;
    }
    int j = next[i];
    int k = next[j];

    root = sqrt(axis[i][i] - axis[j][j] - axis[k][k] + 1.0);
    FCL_REAL* quat[3] = { &data[1], &data[2], &data[3] };
    *quat[i] = 0.5 * root;
    root = 0.5 / root;
    data[0] = (axis[j][k] - axis[k][j]) * root;
    *quat[j] = (axis[i][j] + axis[j][i]) * root;
    *quat[k] = (axis[i][k] + axis[k][i]) * root;
  }
}

void Quaternion3f::toAxes(Vec3f axis[3]) const
{
  FCL_REAL twoX  = 2.0*data[1];
  FCL_REAL twoY  = 2.0*data[2];
  FCL_REAL twoZ  = 2.0*data[3];
  FCL_REAL twoWX = twoX*data[0];
  FCL_REAL twoWY = twoY*data[0];
  FCL_REAL twoWZ = twoZ*data[0];
  FCL_REAL twoXX = twoX*data[1];
  FCL_REAL twoXY = twoY*data[1];
  FCL_REAL twoXZ = twoZ*data[1];
  FCL_REAL twoYY = twoY*data[2];
  FCL_REAL twoYZ = twoZ*data[2];
  FCL_REAL twoZZ = twoZ*data[3];

  axis[0].setValue(1.0 - (twoYY + twoZZ), twoXY + twoWZ, twoXZ - twoWY);
  axis[1].setValue(twoXY - twoWZ, 1.0 - (twoXX + twoZZ), twoYZ + twoWX);
  axis[2].setValue(twoXZ + twoWY, twoYZ - twoWX, 1.0 - (twoXX + twoYY));
}

void Quaternion3f::fromAxisAngle(const Vec3f& axis, FCL_REAL angle)
{
  FCL_REAL half_angle = 0.5 * angle;
  FCL_REAL sn = sin((double)half_angle);
  data[0] = cos((double)half_angle);
  data[1] = sn * axis[0];
  data[2] = sn * axis[1];
  data[3] = sn * axis[2];
}

void Quaternion3f::toAxisAngle(Vec3f& axis, FCL_REAL& angle) const
{
  double sqr_length = data[1] * data[1] + data[2] * data[2] + data[3] * data[3];
  if(sqr_length > 0)
  {
    angle = 2.0 * acos((double)data[0]);
    double inv_length = 1.0 / sqrt(sqr_length);
    axis[0] = inv_length * data[1];
    axis[1] = inv_length * data[2];
    axis[2] = inv_length * data[3];
  }
  else
  {
    angle = 0;
    axis[0] = 1;
    axis[1] = 0;
    axis[2] = 0;
  }
}

FCL_REAL Quaternion3f::dot(const Quaternion3f& other) const
{
  return data[0] * other.data[0] + data[1] * other.data[1] + data[2] * other.data[2] + data[3] * other.data[3];
}

Quaternion3f Quaternion3f::operator + (const Quaternion3f& other) const
{
  return Quaternion3f(data[0] + other.data[0], data[1] + other.data[1],
                      data[2] + other.data[2], data[3] + other.data[3]);
}

const Quaternion3f& Quaternion3f::operator += (const Quaternion3f& other)
{
  data[0] += other.data[0];
  data[1] += other.data[1];
  data[2] += other.data[2];
  data[3] += other.data[3];

  return *this;
}

Quaternion3f Quaternion3f::operator - (const Quaternion3f& other) const
{
  return Quaternion3f(data[0] - other.data[0], data[1] - other.data[1],
                      data[2] - other.data[2], data[3] - other.data[3]);
}

const Quaternion3f& Quaternion3f::operator -= (const Quaternion3f& other)
{
  data[0] -= other.data[0];
  data[1] -= other.data[1];
  data[2] -= other.data[2];
  data[3] -= other.data[3];

  return *this;
}

Quaternion3f Quaternion3f::operator * (const Quaternion3f& other) const
{
  return Quaternion3f(data[0] * other.data[0] - data[1] * other.data[1] - data[2] * other.data[2] - data[3] * other.data[3],
                      data[0] * other.data[1] + data[1] * other.data[0] + data[2] * other.data[3] - data[3] * other.data[2],
                      data[0] * other.data[2] - data[1] * other.data[3] + data[2] * other.data[0] + data[3] * other.data[1],
                      data[0] * other.data[3] + data[1] * other.data[2] - data[2] * other.data[1] + data[3] * other.data[0]);
}

const Quaternion3f& Quaternion3f::operator *= (const Quaternion3f& other)
{
  FCL_REAL a = data[0] * other.data[0] - data[1] * other.data[1] - data[2] * other.data[2] - data[3] * other.data[3];
  FCL_REAL b = data[0] * other.data[1] + data[1] * other.data[0] + data[2] * other.data[3] - data[3] * other.data[2];
  FCL_REAL c = data[0] * other.data[2] - data[1] * other.data[3] + data[2] * other.data[0] + data[3] * other.data[1];
  FCL_REAL d = data[0] * other.data[3] + data[1] * other.data[2] - data[2] * other.data[1] + data[3] * other.data[0];

  data[0] = a;
  data[1] = b;
  data[2] = c;
  data[3] = d;
  return *this;
}

Quaternion3f Quaternion3f::operator - () const
{
  return Quaternion3f(-data[0], -data[1], -data[2], -data[3]);
}

Quaternion3f Quaternion3f::operator * (FCL_REAL t) const
{
  return Quaternion3f(data[0] * t, data[1] * t, data[2] * t, data[3] * t);
}

const Quaternion3f& Quaternion3f::operator *= (FCL_REAL t)
{
  data[0] *= t;
  data[1] *= t;
  data[2] *= t;
  data[3] *= t;

  return *this;
}

Quaternion3f& Quaternion3f::conj()
{
  data[1] = -data[1];
  data[2] = -data[2];
  data[3] = -data[3];
  return *this;
}

Quaternion3f& Quaternion3f::inverse()
{
  FCL_REAL sqr_length = data[0] * data[0] + data[1] * data[1] + data[2] * data[2] + data[3] * data[3];
  if(sqr_length > 0)
  {
    FCL_REAL inv_length = 1 / std::sqrt(sqr_length);
    data[0] *= inv_length;
    data[1] *= (-inv_length);
    data[2] *= (-inv_length);
    data[3] *= (-inv_length);
  }
  else
  {
    data[1] = -data[1];
    data[2] = -data[2];
    data[3] = -data[3];
  }

  return *this;
}

Vec3f Quaternion3f::transform(const Vec3f& v) const
{
  Vec3f u(getX(), getY(), getZ());
  double s = getW();
  Vec3f vprime = 2*u.dot(v)*u + (s*s - u.dot(u))*v + 2*s*u.cross(v);
  return vprime;
}

Quaternion3f conj(const Quaternion3f& q)
{
  Quaternion3f r(q);
  return r.conj();
}

Quaternion3f inverse(const Quaternion3f& q)
{
  Quaternion3f res(q);
  return res.inverse();
}

void Quaternion3f::fromEuler(FCL_REAL a, FCL_REAL b, FCL_REAL c)
{
  Matrix3f R;
  R.setEulerYPR(a, b, c);

  fromRotation(R);
}

void Quaternion3f::toEuler(FCL_REAL& a, FCL_REAL& b, FCL_REAL& c) const
{
  Matrix3f R;
  toRotation(R);
  a = atan2(R(1, 0), R(0, 0));
  b = asin(-R(2, 0));
  c = atan2(R(2, 1), R(2, 2));

  if(b == boost::math::constants::pi<double>() * 0.5)
  {
    if(a > 0)
      a -= boost::math::constants::pi<double>();
    else
      a += boost::math::constants::pi<double>();

    if(c > 0)
      c -= boost::math::constants::pi<double>();
    else
      c += boost::math::constants::pi<double>();
  }
}

Vec3f Quaternion3f::getColumn(std::size_t i) const
{
  switch(i)
  {
  case 0:
    return Vec3f(data[0] * data[0] + data[1] * data[1] - data[2] * data[2] - data[3] * data[3],
                 2 * (- data[0] * data[3] + data[1] * data[2]),
                 2 * (data[1] * data[3] + data[0] * data[2]));
  case 1:
    return Vec3f(2 * (data[1] * data[2] + data[0] * data[3]),
                 data[0] * data[0] - data[1] * data[1] + data[2] * data[2] - data[3] * data[3],
                 2 * (data[2] * data[3] - data[0] * data[1]));
  case 2:
    return Vec3f(2 * (data[1] * data[3] - data[0] * data[2]),
                 2 * (data[2] * data[3] + data[0] * data[1]),
                 data[0] * data[0] - data[1] * data[1] - data[2] * data[2] + data[3] * data[3]);
  default:
    return Vec3f();
  }
}

Vec3f Quaternion3f::getRow(std::size_t i) const
{
  switch(i)
  {
  case 0:
    return Vec3f(data[0] * data[0] + data[1] * data[1] - data[2] * data[2] - data[3] * data[3],
                 2 * (data[0] * data[3] + data[1] * data[2]),
                 2 * (data[1] * data[3] - data[0] * data[2]));
  case 1:
    return Vec3f(2 * (data[1] * data[2] - data[0] * data[3]),
                 data[0] * data[0] - data[1] * data[1] + data[2] * data[2] - data[3] * data[3],
                 2 * (data[2] * data[3] + data[0] * data[1]));
  case 2:
    return Vec3f(2 * (data[1] * data[3] + data[0] * data[2]),
                 2 * (data[2] * data[3] - data[0] * data[1]),
                 data[0] * data[0] - data[1] * data[1] - data[2] * data[2] + data[3] * data[3]);
  default:
    return Vec3f();
  }
}

const Matrix3f& Transform3f::getRotationInternal() const
{
  boost::mutex::scoped_lock slock(const_cast<boost::mutex&>(lock_));
  if(!matrix_set)
  {
    q.toRotation(R);
    matrix_set = true;
  }

  return R;
}

Transform3f inverse(const Transform3f& tf)
{
  Transform3f res(tf);
  return res.inverse();
}

void relativeTransform(const Transform3f& tf1, const Transform3f& tf2,
                       Transform3f& tf)
{
  const Quaternion3f& q1_inv = fcl::conj(tf1.getQuatRotation());
  tf = Transform3f(q1_inv * tf2.getQuatRotation(), q1_inv.transform(tf2.getTranslation() - tf1.getTranslation()));
}

void relativeTransform2(const Transform3f& tf1, const Transform3f& tf2,
                       Transform3f& tf)
{
  const Quaternion3f& q1inv = fcl::conj(tf1.getQuatRotation());
  const Quaternion3f& q2_q1inv = tf2.getQuatRotation() * q1inv;
  tf = Transform3f(q2_q1inv, tf2.getTranslation() - q2_q1inv.transform(tf1.getTranslation()));
}

}


//  (C) Copyright 2006 Eric Niebler, Olivier Gygi
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/random.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/weighted_median.hpp>

using namespace boost;
using namespace unit_test;
using namespace accumulators;

///////////////////////////////////////////////////////////////////////////////
// test_stat
//
void test_stat()
{
    // Median estimation of normal distribution N(1,1) using samples from a narrow normal distribution N(1,0.01)
    // The weights equal to the likelihood ratio of the corresponding samples

    // two random number generators
    double mu = 1.;
    double sigma_narrow = 0.01;
    double sigma = 1.;
    boost::lagged_fibonacci607 rng;
    boost::normal_distribution<> mean_sigma_narrow(mu,sigma_narrow);
    boost::variate_generator<boost::lagged_fibonacci607&, boost::normal_distribution<> > normal_narrow(rng, mean_sigma_narrow);

    accumulator_set<double, stats<tag::weighted_median(with_p_square_quantile) >, double > acc;
    accumulator_set<double, stats<tag::weighted_median(with_density) >, double >
        acc_dens( density_cache_size = 10000, density_num_bins = 1000 );
    accumulator_set<double, stats<tag::weighted_median(with_p_square_cumulative_distribution) >, double >
        acc_cdist( p_square_cumulative_distribution_num_cells = 100 );

    for (std::size_t i=0; i<1000000; ++i)
    {
        double sample = normal_narrow();
        double w = std::exp(
            0.5 * (sample - mu) * (sample - mu) * (
                1./sigma_narrow/sigma_narrow - 1./sigma/sigma
            )
        );
        acc(sample, weight = w);
        acc_dens(sample, weight = w);
        acc_cdist(sample, weight = w);
    }

    BOOST_CHECK_CLOSE(1., weighted_median(acc), 2);
    BOOST_CHECK_CLOSE(1., weighted_median(acc_dens), 3);
    BOOST_CHECK_CLOSE(1., weighted_median(acc_cdist), 3);
}

///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("weighted_median test");

    test->add(BOOST_TEST_CASE(&test_stat));

    return test;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          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 <boost/simd/arithmetic/include/functions/divround2even.hpp>
#include <boost/simd/sdk/config.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>

#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/inf.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/minf.hpp>
#include <boost/simd/include/constants/mone.hpp>
#include <boost/simd/include/constants/nan.hpp>
#include <boost/simd/include/constants/valmin.hpp>
#include <boost/simd/include/constants/valmax.hpp>

NT2_TEST_CASE_TPL ( divround2even_real,  BOOST_SIMD_REAL_TYPES)
{
  using boost::simd::divround2even;
  using boost::simd::tag::divround2even_;
  typedef typename boost::dispatch::meta::call<divround2even_(T,T)>::type r_t;
  typedef T wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS( r_t, wished_r_t );

  // specific values tests
  NT2_TEST_EQUAL(divround2even(boost::simd::Inf<T>(), boost::simd::Inf<T>()), boost::simd::Nan<r_t>());
  NT2_TEST_EQUAL(divround2even(boost::simd::Minf<T>(), boost::simd::Minf<T>()), boost::simd::Nan<r_t>());
  NT2_TEST_EQUAL(divround2even(boost::simd::Mone<T>(), boost::simd::Mone<T>()), boost::simd::One<r_t>());
  NT2_TEST_EQUAL(divround2even(boost::simd::Nan<T>(), boost::simd::Nan<T>()), boost::simd::Nan<r_t>());
  NT2_TEST_EQUAL(divround2even(boost::simd::One<T>(), boost::simd::One<T>()), boost::simd::One<r_t>());
} // end of test for floating_

NT2_TEST_CASE_TPL ( divround2even_unsigned_int,  BOOST_SIMD_UNSIGNED_TYPES)
{

  using boost::simd::divround2even;
  using boost::simd::tag::divround2even_;
  typedef typename boost::dispatch::meta::call<divround2even_(T,T)>::type r_t;
  typedef T wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS( r_t, wished_r_t );

  // specific values tests
  NT2_TEST_EQUAL(divround2even(boost::simd::One<T>(), boost::simd::One<T>()), boost::simd::One<r_t>());
  NT2_TEST_EQUAL(divround2even(T(5), T(2)), T(2));
  NT2_TEST_EQUAL(divround2even(T(7), T(2)), T(4));
  NT2_TEST_EQUAL(divround2even(T(9), T(3)), T(3));
  NT2_TEST_EQUAL(divround2even(T(10), T(3)), T(3));
  NT2_TEST_EQUAL(divround2even(T(11), T(3)), T(4));
  NT2_TEST_EQUAL(divround2even(T(12), T(3)), T(4));
  NT2_TEST_EQUAL(divround2even(T(18), T(6)), T(3));
  NT2_TEST_EQUAL(divround2even(T(20), T(6)), T(3));
  NT2_TEST_EQUAL(divround2even(T(22), T(6)), T(4));
  NT2_TEST_EQUAL(divround2even(T(24), T(6)), T(4));
  NT2_TEST_EQUAL(divround2even(boost::simd::Valmax<T>(),boost::simd::Two<T>()), boost::simd::Valmax<T>()/boost::simd::Two<T>()+boost::simd::One<T>());

} // end of test for unsigned_int_

NT2_TEST_CASE_TPL ( divround2even_signed_int, BOOST_SIMD_INTEGRAL_SIGNED_TYPES)
{

  using boost::simd::divround2even;
  using boost::simd::tag::divround2even_;
  typedef typename boost::dispatch::meta::call<divround2even_(T,T)>::type r_t;
  typedef T wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS( r_t, wished_r_t );

  // specific values tests
  NT2_TEST_EQUAL(divround2even(boost::simd::Mone<T>(), boost::simd::Mone<T>()), boost::simd::One<r_t>());
  NT2_TEST_EQUAL(divround2even(boost::simd::One<T>(), boost::simd::One<T>()), boost::simd::One<r_t>());
  NT2_TEST_EQUAL(divround2even(T(5), T(2)), T(2));
  NT2_TEST_EQUAL(divround2even(T(7), T(2)), T(4));
  NT2_TEST_EQUAL(divround2even(T(-5), T(2)), T(-2));
  NT2_TEST_EQUAL(divround2even(T(-7), T(2)), T(-4));
  NT2_TEST_EQUAL(divround2even(T(-4),T(0)), boost::simd::Valmin<r_t>());
  NT2_TEST_EQUAL(divround2even(T(4),T(0)), boost::simd::Valmax<r_t>());
  NT2_TEST_EQUAL(divround2even(T(4),T(3)), T(1));
  NT2_TEST_EQUAL(divround2even(T(-4),T(-3)), T(1));
  NT2_TEST_EQUAL(divround2even(T(4),T(-3)), T(-1));
  NT2_TEST_EQUAL(divround2even(T(-4),T(3)), T(-1));
  NT2_TEST_EQUAL(divround2even(T(5),T(3)), T(2));
  NT2_TEST_EQUAL(divround2even(T(-5),T(-3)), T(2));
  NT2_TEST_EQUAL(divround2even(T(5),T(-3)), T(-2));
  NT2_TEST_EQUAL(divround2even(T(-5),T(3)), T(-2));

  NT2_TEST_EQUAL(divround2even(T(5),T(4)), T(1));
  NT2_TEST_EQUAL(divround2even(T(-5),T(-4)), T(1));
  NT2_TEST_EQUAL(divround2even(T(5),T(-4)), T(-1));
  NT2_TEST_EQUAL(divround2even(T(-5),T(4)), T(-1));
  NT2_TEST_EQUAL(divround2even(T(6),T(4)), T(2));
  NT2_TEST_EQUAL(divround2even(T(-6),T(-4)), T(2));
  NT2_TEST_EQUAL(divround2even(T(6),T(-4)), T(-2));
  NT2_TEST_EQUAL(divround2even(T(-6),T(4)), T(-2));
  NT2_TEST_EQUAL(divround2even(T(8),T(4)), T(2));
  NT2_TEST_EQUAL(divround2even(T(-8),T(-4)), T(2));
  NT2_TEST_EQUAL(divround2even(T(8),T(-4)), T(-2));
  NT2_TEST_EQUAL(divround2even(T(-8),T(4)), T(-2));
  NT2_TEST_EQUAL(divround2even(T(9),T(4)), T(2));
  NT2_TEST_EQUAL(divround2even(T(-9),T(-4)), T(2));
  NT2_TEST_EQUAL(divround2even(T(9),T(-4)), T(-2));
  NT2_TEST_EQUAL(divround2even(T(-9),T(4)), T(-2));
  NT2_TEST_EQUAL(divround2even(T(10),T(4)), T(2));
  NT2_TEST_EQUAL(divround2even(T(-10),T(-4)), T(2));
  NT2_TEST_EQUAL(divround2even(T(10),T(-4)), T(-2));
  NT2_TEST_EQUAL(divround2even(T(-10),T(4)), T(-2));

} // end of test for signed_int_


#include <iostream>
#include <fstream>
#include <algorithm>
#include <vector>
#include <numeric>
#include <tuple>

#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/storage.hpp>
#include <boost/numeric/ublas/io.hpp>

#include "Solution.hpp"
#include "Helper.hpp"

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

// Check if Solution is feasible
bool checkSolution(
	const vector<bool> &yi,
	const vector<tuple<int,int,double>> &flow,
	const unsigned int &flowNumber,
	const ublas::matrix<double> &cij,
	const vector<double> &bi,
	vector<double> dj,
	const double &sum_dj)
{
	double sum = 0.0;
	// Conditions are: forall j in J: sum of xij = 1
	//				   if xij !0 0 then yi = 1
	//				   sum_{j in J} xij * dj <= bi*yi, forall i in I
	for (size_t f = 0; f < flowNumber; f++)
	{
		if (yi[get<0>(flow[f])] == 0)
			return false;

		sum += get<2>(flow[f]);
		dj[get<1>(flow[f])] -= get<2>(flow[f]);
	}

	if (sum != sum_dj)
		return false;

	for (size_t j = 0; j != cij.size2(); ++j)
	{
		if (dj[j] != 0.0)
			return false;
	}

	return true;
}

// Obejtive Function of FLP
double f(
	const vector<bool> &yi,
	const vector<double> &fi,
	const double &transportation_cost)
{
	double z = 0.0;

	for (size_t i = 0; i != yi.size(); ++i)
		z += yi[i] * fi[i];

	z += transportation_cost;

	return z;
}

// Checks if Capacity can fullfill the Demand
bool canUpdateXij(
	const vector<double> &bi,
	const vector<bool> &yi,
	const double &sum_dj)
{
	if (inner_product(yi.begin(), yi.end(), bi.begin(), 0.0) < sum_dj)
		return false;
	else
		return true;
}


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

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

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

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

/*! \file qle/termstructures/optionletstripper1.hpp
    \brief Optionlet (caplet/floorlet) volatility strippers
    \ingroup termstructures
*/

#ifndef quantext_optionletstripper1_hpp
#define quantext_optionletstripper1_hpp

#include <ql/instruments/capfloor.hpp>
#include <ql/quotes/simplequote.hpp>
#include <qle/termstructures/optionletstripper.hpp>

#include <boost/optional.hpp>

namespace QuantExt {
using namespace QuantLib;
using boost::optional;

typedef std::vector<std::vector<boost::shared_ptr<CapFloor> > > CapFloorMatrix;

/*! Helper class to strip optionlet (i.e. caplet/floorlet) volatilities
    (a.k.a. forward-forward volatilities) from the (cap/floor) term
    volatilities of a CapFloorTermVolSurface.
    \ingroup termstructures
*/
class OptionletStripper1 : public QuantExt::OptionletStripper {
public:
    // If dontThrow is set to true than any vols that would throw are set to dontThrowMinVol (default is 0.0)
    OptionletStripper1(const boost::shared_ptr<QuantExt::CapFloorTermVolSurface>&,
                       const boost::shared_ptr<IborIndex>& index, Rate switchStrikes = Null<Rate>(),
                       Real accuracy = 1.0e-6, Natural maxIter = 100,
                       const Handle<YieldTermStructure>& discount = Handle<YieldTermStructure>(),
                       const VolatilityType type = ShiftedLognormal, const Real displacement = 0.0,
                       bool dontThrow = false, const optional<VolatilityType> targetVolatilityType = boost::none,
                       const optional<Real> targetDisplacement = boost::none, Real dontThrowMinVol = 0.0);

    const Matrix& capFloorPrices() const;
    const Matrix& capletVols() const;
    const Matrix& capFloorVolatilities() const;
    const Matrix& optionletPrices() const;
    Rate switchStrike() const;
    const Handle<YieldTermStructure>& discountCurve() const { return discount_; }

    //! \name LazyObject interface
    //@{
    void performCalculations() const;
    //@}
private:
    mutable Matrix capFloorPrices_, optionletPrices_;
    mutable Matrix capFloorVols_;
    mutable Matrix optionletStDevs_, capletVols_;

    mutable CapFloorMatrix capFloors_;
    mutable std::vector<std::vector<boost::shared_ptr<SimpleQuote> > > volQuotes_;
    mutable std::vector<std::vector<boost::shared_ptr<PricingEngine> > > capFloorEngines_;
    bool floatingSwitchStrike_;
    mutable bool capFlooMatrixNotInitialized_;
    mutable Rate switchStrike_;
    Real accuracy_;
    Natural maxIter_;
    bool dontThrow_;
    Real dontThrowMinVol_;
    const VolatilityType inputVolatilityType_;
    const Real inputDisplacement_;
};
} // namespace QuantExt

#endif


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

// Currently needs -DMTL_DEEP_COPY_CONSTRUCTOR !!!

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

int main(int, char**)
{
    using namespace std;

    // Define a 6x6 sparse matrix in a 3x3 block-sparse
    // Should be
#if 0
    typedef mat::parameters<row_major, mtl::index::c_index, fixed::dimensions<2, 2> > parameters1;
    typedef dense2D<double, parameters1>    m_t;
#endif

    // Define a 6x6 sparse matrix in a 3x3 block-sparse
	typedef mtl::dense2D<double>    m_t;
	typedef mtl::compressed2D<m_t>  matrix_t;
    matrix_t                        A(3, 3);
    {
		mtl::mat::inserter<matrix_t> ins(A);

	// First block
	m_t  B(2, 2);
	B= 0.0;
	B[0][0]= 1.0; B[1][1]= 5.0;
	ins(0, 2) << B;

	// Second block
	B=       0.0;
	B[0][1]= 2.0; B[1][0]= 3.0;
	ins(1, 0) << B;

	B= 0.0;
	B[1][0]= 6.0; B[1][1]= 4.0;
	ins(2, 1) << B;
    }
    // cout << "A is " << A << endl; // doesn't works and before it was completely unreadable anyway

    /* Should be something like this:

    [             [ 1 0]] // b1
    [             [ 0 5]]
    [[ 0 2]             ] // b2
    [[ 3 0]             ]
    [       [ 0 0]      ] // b3
    [       [ 6 4]      ]

    */

    // Access blocks (they are read-only) for sparse matrices
    cout << "The block A(1, 0) is \n" << A(1, 0) << endl;
    cout << "The block A[1][0] is \n" << A[1][0] << endl;

    // Access elements in blocks
    cout << "In block A(1, 0), the element (0, 1) is " << A(1, 0)(0, 1) << endl;
    cout << "In block A(1, 0), the element [0][1] is " << A(1, 0)[0][1] << endl;
    cout << "In block A[1][0], the element [0][1] is " << A[1][0][0][1] << endl << endl;

	typedef mtl::dense_vector<double> v_t;
	typedef mtl::dense_vector<v_t>    vector_t;
    vector_t                     x(3);

    // x= [[1, 2], [3, 4], [4, 6]]^T
    x[0]= v_t(2, 1.0); x[0][1]= 2.0; // first block of x = [1, 2]^T
    x[1]= v_t(2, 3.0); x[1][1]= 4.0;
    x[2]= v_t(2, 5.0); x[2][1]= 6.0;
    cout << "x is " << x << endl;

    // For y we would only need the vector sizes [[?, ?], [?, ?], [?, ?]]^T
    vector_t                     y(3, v_t(2));

    // Block-sparse matrix * blocked vector !!!
    y= A*x;

    cout << "y after multiplication is " << y << endl
	 << "Should be [[5, 30], [4, 3], [0, 34]]^T." << endl;

    MTL_THROW_IF(y[1][1] != 3.0, mtl::runtime_error("y[1][1] should be 3.0!\n"));

    return 0;
}


#ifndef MULTIARRAY_HH
#define MULTIARRAY_HH

#include <Eigen/Core>

/*!
 * \brief Class to store multiple matrices
 *
 * Class MultiArray stores multiple two-dimenional arrays in a single, bigger
 * array, making sure that each subarray is aligned on an appropriate memory
 * boundary.
 */
class MultiArray
{
	//! The number of elements in a single (SSE) packet
	static const int pkt_size = Eigen::internal::packet_traits<double>::size;

public:
	//! Return type of a sub-array
	typedef Eigen::ArrayXXd::AlignedMapType Block;
	//! Read-only type of a sub-array
	typedef Eigen::ArrayXXd::ConstAlignedMapType ConstBlock;

	/*!
	 * \brief Constructor
	 *
	 * Create a new MultiArray object that can hold \a m matrices of size
	 * \a n1 x \a n2.
	 * \param n1 The number of rows in each sub-array
	 * \param n2 The number of columns in each sub-array
	 * \param m  The number of sub-arrays in this object
	 */
	MultiArray(int n1, int n2, int m):
		_n1(n1), _n2(n2),
		_nn(pkt_size * ((_n1*_n2+pkt_size-1) / pkt_size)),
		_A(_nn, m) {}

	//! Return the number of sub-arrays in this MultiArray
	int blocks() const { return _A.cols(); }

	//! Return the \a m'th sub-array, writable
	Block operator[](int m)
	{
		return Eigen::ArrayXXd::MapAligned(_A.data()+m*_nn, _n1, _n2);
	}
	//! Return the \a m'th sub-array, read-only
	ConstBlock operator[](int m) const
	{
		return Eigen::ArrayXXd::MapAligned(_A.data()+m*_nn, _n1, _n2);
	}

	//! Copy \a count sub-arrays from \a arr into this array
	void copy(int m, const MultiArray& arr, int am, int count)
	{
		_A.block(0, m, _nn, count) = arr._A.block(0, am, _nn, count);
	}

private:
	MultiArray(int n1, int n2, int nn, Eigen::ArrayXXd A):
		_n1(n1), _n2(n2), _nn(nn), _A(A) {}

	//! The number of rows in each sub-array
	int _n1;
	//! The number of columns in each sub-array
	int _n2;
	//! The number of elements in each sub-array, rounded up to an entire packet
	int _nn;
	//! The actual data matrix
	Eigen::ArrayXXd _A;
};

#endif // MULTIARRAY_HH