#include <nori/integrator.h>
#include <nori/scene.h>
#include <nori/ray.h>
#include <filesystem/resolver.h>
#include <sh/spherical_harmonics.h>
#include <sh/default_image.h>
#include <Eigen/Core>
#include <fstream>
#include <random>
#include <stb_image.h>

NORI_NAMESPACE_BEGIN

namespace ProjEnv
{
    std::vector<std::unique_ptr<float[]>>
    LoadCubemapImages(const std::string &cubemapDir, int &width, int &height,
                      int &channel)
    {
        std::vector<std::string> cubemapNames{"negx.jpg", "posx.jpg", "posy.jpg",
                                              "negy.jpg", "posz.jpg", "negz.jpg"};
        std::vector<std::unique_ptr<float[]>> images(6);
        for (int i = 0; i < 6; i++)
        {
            std::string filename = cubemapDir + "/" + cubemapNames[i];
            int w, h, c;
            float *image = stbi_loadf(filename.c_str(), &w, &h, &c, 3);
            if (!image)
            {
                std::cout << "Failed to load image: " << filename << std::endl;
                exit(-1);
            }
            if (i == 0)
            {
                width = w;
                height = h;
                channel = c;
            }
            else if (w != width || h != height || c != channel)
            {
                std::cout << "Dismatch resolution for 6 images in cubemap" << std::endl;
                exit(-1);
            }
            images[i] = std::unique_ptr<float[]>(image);
            int index = (0 * 128 + 0) * channel;
            // std::cout << images[i][index + 0] << "\t" << images[i][index + 1] << "\t"
            //           << images[i][index + 2] << std::endl;
        }
        return images;
    }

    const Eigen::Vector3f cubemapFaceDirections[6][3] = {
        {{0, 0, 1}, {0, -1, 0}, {-1, 0, 0}},  // negx
        {{0, 0, 1}, {0, -1, 0}, {1, 0, 0}},   // posx
        {{1, 0, 0}, {0, 0, -1}, {0, -1, 0}},  // negy
        {{1, 0, 0}, {0, 0, 1}, {0, 1, 0}},    // posy
        {{-1, 0, 0}, {0, -1, 0}, {0, 0, -1}}, // negz
        {{1, 0, 0}, {0, -1, 0}, {0, 0, 1}},   // posz
    };

    float CalcPreArea(const float &x, const float &y)
    {
        return std::atan2(x * y, std::sqrt(x * x + y * y + 1.0));
    }

    float CalcArea(const float &u_, const float &v_, const int &width,
                   const int &height)
    {
        // transform from [0..res - 1] to [- (1 - 1 / res) .. (1 - 1 / res)]
        // ( 0.5 is for texel center addressing)
        float u = (2.0 * (u_ + 0.5) / width) - 1.0;
        float v = (2.0 * (v_ + 0.5) / height) - 1.0;

        // shift from a demi texel, mean 1.0 / size  with u and v in [-1..1]
        float invResolutionW = 1.0 / width;
        float invResolutionH = 1.0 / height;

        // u and v are the -1..1 texture coordinate on the current face.
        // get projected area for this texel
        float x0 = u - invResolutionW;
        float y0 = v - invResolutionH;
        float x1 = u + invResolutionW;
        float y1 = v + invResolutionH;
        float angle = CalcPreArea(x0, y0) - CalcPreArea(x0, y1) -
                      CalcPreArea(x1, y0) + CalcPreArea(x1, y1);

        return angle;
    }

    // template <typename T> T ProjectSH() {}

    template <size_t SHOrder>
    std::vector<Eigen::Array3f> PrecomputeCubemapSH(const std::vector<std::unique_ptr<float[]>> &images,
                                                    const int &width, const int &height,
                                                    const int &channel)
    {
        std::vector<Eigen::Vector3f> cubemapDirs;
        cubemapDirs.reserve(6 * width * height);
        for (int i = 0; i < 6; i++)
        {
            Eigen::Vector3f faceDirX = cubemapFaceDirections[i][0];
            Eigen::Vector3f faceDirY = cubemapFaceDirections[i][1];
            Eigen::Vector3f faceDirZ = cubemapFaceDirections[i][2];
            for (int y = 0; y < height; y++)
            {
                for (int x = 0; x < width; x++)
                {
                    float u = 2 * ((x + 0.5) / width) - 1;
                    float v = 2 * ((y + 0.5) / height) - 1;
                    Eigen::Vector3f dir = (faceDirX * u + faceDirY * v + faceDirZ).normalized();
                    cubemapDirs.push_back(dir);
                }
            }
        }
        constexpr int SHNum = (SHOrder + 1) * (SHOrder + 1);
        std::vector<Eigen::Array3f> SHCoeffiecents(SHNum);
        for (int i = 0; i < SHNum; i++)
            SHCoeffiecents[i] = Eigen::Array3f(0);
        float sumWeight = 0;
        for (int i = 0; i < 6; i++)
        {
            for (int y = 0; y < height; y++)
            {
                for (int x = 0; x < width; x++)
                {
                    // TODO: here you need to compute light sh of each face of cubemap of each pixel
                    // TODO: 此处你需要计算每个像素下cubemap某个面的球谐系数
                    Eigen::Vector3f dir = cubemapDirs[i * width * height + y * width + x];
                    int index = (y * width + x) * channel;
                    Eigen::Array3f Le(images[i][index + 0], images[i][index + 1],
                                      images[i][index + 2]);
                    auto wOmega = CalcArea(x, y, width, height);

                    for (auto l = 0; l <= SHOrder; ++l)
                    {
                        for (auto m = -l; m <= l; ++m)
                        {
                            auto k = sh::GetIndex(l, m);
                            auto basicFunc = sh::EvalSH(l, m, dir.cast<double>().normalized());

                            SHCoeffiecents[k] += wOmega * Le * basicFunc;
                        }
                    }
                }
            }
        }
        return SHCoeffiecents;
    }
}

class PRTIntegrator : public Integrator
{
public:
    static constexpr int SHOrder = 2;
    static constexpr int SHCoeffLength = (SHOrder + 1) * (SHOrder + 1);

    enum class Type
    {
        Unshadowed = 0,
        Shadowed = 1,
        Interreflection = 2
    };

    PRTIntegrator(const PropertyList &props)
    {
        /* No parameters this time */
        m_SampleCount = props.getInteger("PRTSampleCount", 100);
        m_CubemapPath = props.getString("cubemap");
        auto type = props.getString("type", "unshadowed");
        if (type == "unshadowed")
        {
            m_Type = Type::Unshadowed;
        }
        else if (type == "shadowed")
        {
            m_Type = Type::Shadowed;
        }
        else if (type == "interreflection")
        {
            m_Type = Type::Interreflection;
            m_Bounce = props.getInteger("bounce", 1);
        }
        else
        {
            throw NoriException("Unsupported type: %s.", type);
        }
    }

    virtual void preprocess(const Scene *scene) override
    {

        // Here only compute one mesh
        const auto mesh = scene->getMeshes()[0];
        // Projection environment
        auto cubePath = getFileResolver()->resolve(m_CubemapPath);
        auto lightPath = cubePath / "light.txt";
        auto transPath = cubePath / "transport.txt";
        std::ofstream lightFout(lightPath.str());
        std::ofstream fout(transPath.str());
        int width, height, channel;
        std::vector<std::unique_ptr<float[]>> images =
            ProjEnv::LoadCubemapImages(cubePath.str(), width, height, channel);
        auto envCoeffs = ProjEnv::PrecomputeCubemapSH<SHOrder>(images, width, height, channel);
        m_LightCoeffs.resize(3, SHCoeffLength);
        for (int i = 0; i < envCoeffs.size(); i++)
        {
            lightFout << (envCoeffs)[i].x() << " " << (envCoeffs)[i].y() << " " << (envCoeffs)[i].z() << std::endl;
            m_LightCoeffs.col(i) = (envCoeffs)[i];
        }
        std::cout << "Computed light sh coeffs from: " << cubePath.str() << " to: " << lightPath.str() << std::endl;
        // Projection transport
        m_TransportSHCoeffs.resize(SHCoeffLength, mesh->getVertexCount());
        fout << mesh->getVertexCount() << std::endl;
        for (int i = 0; i < mesh->getVertexCount(); i++)
        {
            const Point3f &v = mesh->getVertexPositions().col(i);
            const Normal3f &n = mesh->getVertexNormals().col(i);
            auto shFunc = [&](double phi, double theta) -> double
            {
                Eigen::Array3d d = sh::ToVector(phi, theta);
                const auto wi = Vector3f(d.x(), d.y(), d.z());
                const auto H = wi.dot(n);
                if (m_Type == Type::Unshadowed)
                {
                    // TODO: here you need to calculate unshadowed transport term of a given direction
                    // TODO: 此处你需要计算给定方向下的unshadowed传输项球谐函数值
                    return std::max(0.f, H);
                }
                else
                {
                    // TODO: here you need to calculate shadowed transport term of a given direction
                    // TODO: 此处你需要计算给定方向下的shadowed传输项球谐函数值
                    Ray3f ray(v, wi.normalized());
                    if (H > 0 && !scene->rayIntersect(ray))
                        return H;
                    return 0;
                }
            };
            auto shCoeff = sh::ProjectFunction(SHOrder, shFunc, m_SampleCount);
            for (int j = 0; j < shCoeff->size(); j++)
            {
                m_TransportSHCoeffs.col(i).coeffRef(j) = (*shCoeff)[j];
            }
        }
        if (m_Type == Type::Interreflection)
        {
            // TODO: leave for bonus
        }

        // Save in face format
        for (int f = 0; f < mesh->getTriangleCount(); f++)
        {
            const MatrixXu &F = mesh->getIndices();
            uint32_t idx0 = F(0, f), idx1 = F(1, f), idx2 = F(2, f);
            for (int j = 0; j < SHCoeffLength; j++)
            {
                fout << m_TransportSHCoeffs.col(idx0).coeff(j) << " ";
            }
            fout << std::endl;
            for (int j = 0; j < SHCoeffLength; j++)
            {
                fout << m_TransportSHCoeffs.col(idx1).coeff(j) << " ";
            }
            fout << std::endl;
            for (int j = 0; j < SHCoeffLength; j++)
            {
                fout << m_TransportSHCoeffs.col(idx2).coeff(j) << " ";
            }
            fout << std::endl;
        }
        std::cout << "Computed SH coeffs"
                  << " to: " << transPath.str() << std::endl;
    }

    Color3f Li(const Scene *scene, Sampler *sampler, const Ray3f &ray) const
    {
        Intersection its;
        if (!scene->rayIntersect(ray, its))
            return Color3f(0.0f);

        const Eigen::Matrix<Vector3f::Scalar, SHCoeffLength, 1> sh0 = m_TransportSHCoeffs.col(its.tri_index.x()),
                                                                sh1 = m_TransportSHCoeffs.col(its.tri_index.y()),
                                                                sh2 = m_TransportSHCoeffs.col(its.tri_index.z());
        const Eigen::Matrix<Vector3f::Scalar, SHCoeffLength, 1> rL = m_LightCoeffs.row(0), gL = m_LightCoeffs.row(1), bL = m_LightCoeffs.row(2);

        Color3f c0 = Color3f(rL.dot(sh0), gL.dot(sh0), bL.dot(sh0)),
                c1 = Color3f(rL.dot(sh1), gL.dot(sh1), bL.dot(sh1)),
                c2 = Color3f(rL.dot(sh2), gL.dot(sh2), bL.dot(sh2));

        const Vector3f &bary = its.bary;
        Color3f c = bary.x() * c0 + bary.y() * c1 + bary.z() * c2;
        // TODO: you need to delete the following four line codes after finishing your calculation to SH,
        //       we use it to visualize the normals of model for debug.
        // TODO: 在完成了球谐系数计算后，你需要删除下列四行，这四行代码的作用是用来可视化模型法线
        // if (c.isZero())
        // {
        //     auto n_ = its.shFrame.n.cwiseAbs();
        //     return Color3f(n_.x(), n_.y(), n_.z());
        // }
        return c;
    }

    std::string toString() const
    {
        return "PRTIntegrator[]";
    }

private:
    Type m_Type;
    int m_Bounce = 1;
    int m_SampleCount = 100;
    std::string m_CubemapPath;
    Eigen::MatrixXf m_TransportSHCoeffs;
    Eigen::MatrixXf m_LightCoeffs;
};

NORI_REGISTER_CLASS(PRTIntegrator, "prt");
NORI_NAMESPACE_END


#pragma once
#include <vector>

#include <Eigen/Dense>

namespace Caesar {

class Pose3d {
public:
  Pose3d() {
    position_ << 0.0, 0.0, 0.0;
    orientation_ = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0);
  };

  Pose3d(double x, double y, double z, double qw, double qx, double qy,
         double qz) {
    position_ << x, y, z;
    orientation_ = Eigen::Quaterniond(qw, qx, qy, qz);
  };

  Pose3d(const Eigen::Vector3d &position,
         const Eigen::Quaterniond &orientation) {
    position_ = position;
    orientation_ = orientation;
  };

  ~Pose3d(){};

  std::vector<double> Vector(void) const {
    std::vector<double> v = {
        position_.x(),    position_.y(),    position_.z(),   orientation_.w(),
        orientation_.x(), orientation_.y(), orientation_.z()};
    return v;
  }

  Eigen::Vector3d position() { return position_; };
  double x() const { return position_.x(); };
  double y() const { return position_.y(); };
  double z() const { return position_.z(); };
  double qw() const { return orientation_.w(); };
  double qx() const { return orientation_.x(); };
  double qy() const { return orientation_.y(); };
  double qz() const { return orientation_.z(); };

  Eigen::Quaterniond q() const { return orientation_; };

private:
  Eigen::Vector3d position_;
  Eigen::Quaterniond orientation_;
};
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2011-2012 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_STRATEGIES_SPHERICAL_SSF_HPP
#define BOOST_GEOMETRY_STRATEGIES_SPHERICAL_SSF_HPP

#include <boost/mpl/if.hpp>
#include <boost/type_traits.hpp>

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/radian_access.hpp>

#include <boost/geometry/util/select_coordinate_type.hpp>
#include <boost/geometry/util/math.hpp>

#include <boost/geometry/strategies/side.hpp>
//#include <boost/geometry/strategies/concepts/side_concept.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace side
{

/*!
\brief Check at which side of a Great Circle segment a point lies
         left of segment (> 0), right of segment (< 0), on segment (0)
\ingroup strategies
\tparam CalculationType \tparam_calculation
 */
template <typename CalculationType = void>
class spherical_side_formula
{

public :
    template <typename P1, typename P2, typename P>
    static inline int apply(P1 const& p1, P2 const& p2, P const& p)
    {
        typedef typename boost::mpl::if_c
            <
                boost::is_void<CalculationType>::type::value,

                // Select at least a double...
                typename select_most_precise
                    <
                        typename select_most_precise
                            <
                                typename select_most_precise
                                    <
                                        typename coordinate_type<P1>::type,
                                        typename coordinate_type<P2>::type
                                    >::type,
                                typename coordinate_type<P>::type
                            >::type,
                        double
                    >::type,
                CalculationType
            >::type coordinate_type;

        // Convenient shortcuts
        typedef coordinate_type ct;
        ct const lambda1 = get_as_radian<0>(p1);
        ct const delta1 = get_as_radian<1>(p1);
        ct const lambda2 = get_as_radian<0>(p2);
        ct const delta2 = get_as_radian<1>(p2);
        ct const lambda = get_as_radian<0>(p);
        ct const delta = get_as_radian<1>(p);

        // Create temporary points (vectors) on unit a sphere
        ct const cos_delta1 = cos(delta1);
        ct const c1x = cos_delta1 * cos(lambda1);
        ct const c1y = cos_delta1 * sin(lambda1);
        ct const c1z = sin(delta1);

        ct const cos_delta2 = cos(delta2);
        ct const c2x = cos_delta2 * cos(lambda2);
        ct const c2y = cos_delta2 * sin(lambda2);
        ct const c2z = sin(delta2);

        // (Third point is converted directly)
        ct const cos_delta = cos(delta);

        // Apply the "Spherical Side Formula" as presented on my blog
        ct const dist
            = (c1y * c2z - c1z * c2y) * cos_delta * cos(lambda)
            + (c1z * c2x - c1x * c2z) * cos_delta * sin(lambda)
            + (c1x * c2y - c1y * c2x) * sin(delta);

        ct zero = ct();
        return dist > zero ? 1
            : dist < zero ? -1
            : 0;
    }
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

/*template <typename CalculationType>
struct default_strategy<spherical_polar_tag, CalculationType>
{
    typedef spherical_side_formula<CalculationType> type;
};*/

template <typename CalculationType>
struct default_strategy<spherical_equatorial_tag, CalculationType>
{
    typedef spherical_side_formula<CalculationType> type;
};

template <typename CalculationType>
struct default_strategy<geographic_tag, CalculationType>
{
    typedef spherical_side_formula<CalculationType> type;
};

}
#endif

}} // namespace strategy::side

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_SPHERICAL_SSF_HPP


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

#include "PointCloud.h"

#include <Eigen/Eigenvalues>
#include <Core/Utility/Console.h>
#include <Core/Geometry/KDTreeFlann.h>

namespace three{

namespace {

double sqr(double x) { return x * x; }

Eigen::Vector3d FastEigen3x3(const Eigen::Matrix3d &A)
{
	// Based on:
	// https://en.wikipedia.org/wiki/Eigenvalue_algorithm#3.C3.973_matrices
	double p1 = sqr(A(0, 1)) + sqr(A(0, 2)) + sqr(A(1, 2));
	Eigen::Vector3d eigenvalues;
	if (p1 == 0.0) {
		eigenvalues(2) = std::min(A(0, 0), std::min(A(1, 1), A(2, 2)));
		eigenvalues(0) = std::max(A(0, 0), std::max(A(1, 1), A(2, 2)));
		eigenvalues(1) = A.trace() - eigenvalues(0) - eigenvalues(2);
	} else {
		double q = A.trace() / 3.0;
		double p2 = sqr((A(0, 0) - q)) + sqr(A(1, 1) - q) + sqr(A(2, 2) - q) +
				2 * p1;
		double p = sqrt(p2 / 6.0);
		Eigen::Matrix3d B = (1.0 / p) * (A - q * Eigen::Matrix3d::Identity());
		double r = B.determinant() / 2.0;
		double phi;
		if (r <= -1) {
			phi = M_PI / 3.0;
		} else if (r >= 1) {
			phi = 0.0;
		} else {
			phi = std::acos(r) / 3.0;
		}
		eigenvalues(0) = q + 2.0 * p * std::cos(phi);
		eigenvalues(2) = q + 2.0 * p * std::cos(phi + 2.0 * M_PI / 3.0);
		eigenvalues(1) = q * 3.0 - eigenvalues(0) - eigenvalues(2);
	}

	Eigen::Vector3d eigenvector =
			(A - Eigen::Matrix3d::Identity() * eigenvalues(0)) *
			(A.col(0) - Eigen::Vector3d(eigenvalues(1), 0.0, 0.0));
	double len = eigenvector.norm();
	if (len == 0.0) {
		return Eigen::Vector3d::Zero();
	} else {
		return eigenvector.normalized();
	}
}

Eigen::Vector3d ComputeNormal(const PointCloud &cloud,
		const std::vector<int> &indices)
{
	if (indices.size() == 0) {
		return Eigen::Vector3d::Zero();
	}
	Eigen::Matrix3d covariance;
	Eigen::Matrix<double, 9, 1> cumulants;
	cumulants.setZero();
	for (size_t i = 0; i < indices.size(); i++) {
		const Eigen::Vector3d &point = cloud.points_[indices[i]];
		cumulants(0) += point(0);
		cumulants(1) += point(1);
		cumulants(2) += point(2);
		cumulants(3) += point(0) * point(0);
		cumulants(4) += point(0) * point(1);
		cumulants(5) += point(0) * point(2);
		cumulants(6) += point(1) * point(1);
		cumulants(7) += point(1) * point(2);
		cumulants(8) += point(2) * point(2);
	}
	cumulants /= (double)indices.size();
	covariance(0, 0) = cumulants(3) - cumulants(0) * cumulants(0);
	covariance(1, 1) = cumulants(6) - cumulants(1) * cumulants(1);
	covariance(2, 2) = cumulants(8) - cumulants(2) * cumulants(2);
	covariance(0, 1) = cumulants(4) - cumulants(0) * cumulants(1);
	covariance(1, 0) = covariance(0, 1);
	covariance(0, 2) = cumulants(5) - cumulants(0) * cumulants(2);
	covariance(2, 0) = covariance(0, 2);
	covariance(1, 2) = cumulants(7) - cumulants(1) * cumulants(2);
	covariance(2, 1) = covariance(1, 2);

	return FastEigen3x3(covariance);
	//Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver;
	//solver.compute(covariance, Eigen::ComputeEigenvectors);
	//return solver.eigenvectors().col(0);
}

}	// unnamed namespace

bool EstimateNormals(PointCloud &cloud,
		const KDTreeSearchParam &search_param/* = KDTreeSearchParamKNN()*/)
{
	bool has_normal = cloud.HasNormals();
	if (cloud.HasNormals() == false) {
		cloud.normals_.resize(cloud.points_.size());
	}
	KDTreeFlann kdtree;
	kdtree.SetGeometry(cloud);
#ifdef _OPENMP
#pragma omp parallel for schedule(static)
#endif
	for (int i = 0; i < (int)cloud.points_.size(); i++) {
		std::vector<int> indices;
		std::vector<double> distance2;
		Eigen::Vector3d normal;
		if (kdtree.Search(cloud.points_[i], search_param, indices,
				distance2) >= 3) {
			normal = ComputeNormal(cloud, indices);
			if (normal.norm() == 0.0) {
				if (has_normal) {
					normal = cloud.normals_[i];
				} else {
					normal = Eigen::Vector3d(0.0, 0.0, 1.0);
				}
			}
			if (has_normal && normal.dot(cloud.normals_[i]) < 0.0) {
				normal *= -1.0;
			}
			cloud.normals_[i] = normal;
		} else {
			cloud.normals_[i] = Eigen::Vector3d(0.0, 0.0, 1.0);
		}
	}

	return true;
}

bool OrientNormalsToAlignWithDirection(PointCloud &cloud,
		const Eigen::Vector3d &orientation_reference
		/* = Eigen::Vector3d(0.0, 0.0, 1.0)*/)
{
	if (cloud.HasNormals() == false) {
		PrintDebug("[OrientNormalsToAlignWithDirection] No normals in the PointCloud. Call EstimateNormals() first.\n");
	}
#ifdef _OPENMP
#pragma omp parallel for schedule(static)
#endif
	for (int i = 0; i < (int)cloud.points_.size(); i++) {
		auto &normal = cloud.normals_[i];
		if (normal.norm() == 0.0) {
			normal = orientation_reference;
		} else if (normal.dot(orientation_reference) < 0.0) {
			normal *= -1.0;
		}
	}
	return true;
}

bool OrientNormalsTowardsCameraLocation(PointCloud &cloud,
		const Eigen::Vector3d &camera_location/* = Eigen::Vector3d::Zero()*/)
{
	if (cloud.HasNormals() == false) {
		PrintDebug("[OrientNormalsTowardsCameraLocation] No normals in the PointCloud. Call EstimateNormals() first.\n");
	}
#ifdef _OPENMP
#pragma omp parallel for schedule(static)
#endif
	for (int i = 0; i < (int)cloud.points_.size(); i++) {
		Eigen::Vector3d orientation_reference = camera_location -
				cloud.points_[i];
		auto &normal = cloud.normals_[i];
		if (normal.norm() == 0.0) {
			normal = orientation_reference;
			if (normal.norm() == 0.0) {
				normal = Eigen::Vector3d(0.0, 0.0, 1.0);
			} else {
				normal.normalize();
			}
		} else if (normal.dot(orientation_reference) < 0.0) {
			normal *= -1.0;
		}
	}
	return true;
}

}	// namespace three


#include <iostream>
#include <memory>
#include <random>
#include <string>

#include <Eigen/Sparse>

#include "GeometricMultigridOperators.h"
#include "GeometricMultigridPoissonSolver.h"
#include "InitialMultigridTestDomains.h"
#include "Renderer.h"
#include "ScalarGrid.h"
#include "Transform.h"
#include "UniformGrid.h"
#include "Utilities.h"

using namespace FluidSim2D::RenderTools;
using namespace FluidSim2D::SimTools;

std::unique_ptr<Renderer> renderer;

static constexpr int gridSize = 512;
static constexpr bool useComplexDomain = true;
static constexpr bool useSolidSphere = true;

int main(int argc, char** argv)
{
	using namespace GeometricMultigridOperators;

	using StoreReal = double;
	using SolveReal = double;

	UniformGrid<CellLabels> domainCellLabels;
	VectorGrid<StoreReal> boundaryWeights;

	int mgLevels;
	{
		UniformGrid<CellLabels> baseDomainCellLabels;
		VectorGrid<StoreReal> baseBoundaryWeights;

		// Complex domain set up
		if (useComplexDomain)
			buildComplexDomain(baseDomainCellLabels,
								baseBoundaryWeights,
								gridSize,
								useSolidSphere);
		// Simple domain set up
		else
			buildSimpleDomain(baseDomainCellLabels,
								baseBoundaryWeights,
								gridSize,
								1 /*dirichlet band*/);

		// Build expanded domain
		std::pair<Vec2i, int> mgSettings = buildExpandedDomain(domainCellLabels, boundaryWeights, baseDomainCellLabels, baseBoundaryWeights);

		mgLevels = mgSettings.second;
	}

	SolveReal dx = boundaryWeights.dx();

	UniformGrid<StoreReal> rhsGrid(domainCellLabels.size(), 0);

	UniformGrid<StoreReal> solutionGrid(domainCellLabels.size(), 0);

	tbb::parallel_for(tbb::blocked_range<int>(0, domainCellLabels.voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
	{
		for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
		{
			Vec2i cell = domainCellLabels.unflatten(cellIndex);

			if (domainCellLabels(cell) == CellLabels::INTERIOR_CELL ||
				domainCellLabels(cell) == CellLabels::BOUNDARY_CELL)
			{
				Vec2f point(dx * Vec2f(cell));
				solutionGrid(cell) = 4. * (std::sin(2 * PI * point[0]) * std::sin(2 * PI * point[1]) +
											std::sin(4 * PI * point[0]) * std::sin(4 * PI * point[1]));
			}
		}
	});

	auto l1Norm = [&](const UniformGrid<StoreReal> &grid)
	{
		assert(grid.size() == domainCellLabels.size());

		tbb::enumerable_thread_specific<SolveReal> parallelAccumulatedValue(SolveReal(0));
		tbb::parallel_for(tbb::blocked_range<int>(0, domainCellLabels.voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
		{
			auto& localAccumulatedValue = parallelAccumulatedValue.local();

			for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
			{
				Vec2i cell = domainCellLabels.unflatten(cellIndex);

				if (domainCellLabels(cell) == CellLabels::INTERIOR_CELL ||
					domainCellLabels(cell) == CellLabels::BOUNDARY_CELL)
				{
					localAccumulatedValue += fabs(grid(cell));
				}
			}
		});

		SolveReal accumulatedValue = 0;
		parallelAccumulatedValue.combine_each([&](const SolveReal localAccumulatedValue)
		{
			accumulatedValue += localAccumulatedValue;
		});

		return accumulatedValue;
	};

	// Print initial guess
	solutionGrid.printAsOBJ("initialGuess");

	std::cout << "L-1 initial: " << l1Norm(solutionGrid) << std::endl;

	// Pre-build multigrid preconditioner
	GeometricMultigridPoissonSolver mgSolver(domainCellLabels, boundaryWeights, mgLevels, dx);

	for (int iteration = 0; iteration < 50; ++iteration)
	{
		mgSolver.applyMGVCycle(solutionGrid, rhsGrid, true /* use initial guess */);

		std::cout << "L-1 v-cycle " << iteration << ": " << l1Norm(solutionGrid) << std::endl;

		// Print corrected solution after one v-cycle
		//solutionGrid.printAsOBJ("solutionGrid" + std::to_string(iteration));
	}

	// Print domain labels to make sure they are set up correctly
	int pixelHeight = 1080;
	int pixelWidth = pixelHeight;
	renderer = std::make_unique<Renderer>("One level correction test", Vec2i(pixelWidth, pixelHeight), Vec2f(0), 1, &argc, argv);

	ScalarGrid<float> tempGrid(Transform(dx, Vec2f(0)), domainCellLabels.size());

	tbb::parallel_for(tbb::blocked_range<int>(0, tempGrid.voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
	{
		for (int flatIndex = range.begin(); flatIndex != range.end(); ++flatIndex)
		{
			Vec2i cell = tempGrid.unflatten(flatIndex);

			tempGrid(cell) = float(domainCellLabels(cell));
		}
	});

	tempGrid.drawVolumetric(*renderer, Vec3f(0), Vec3f(1), float(CellLabels::INTERIOR_CELL), float(CellLabels::BOUNDARY_CELL));

	renderer->run();
}


#ifndef SCALARTYPE_HPP
#define SCALARTYPE_HPP

#include <boost/rational.hpp>
#include <boost/operators.hpp>
#include <sstream>
#include <iomanip>
#include <iostream>

struct scalarreal : boost::operators<scalarreal> {
	template<typename... T>
	using variant = boost::variant<T...>;
	using rtype = boost::rational<int>;

	struct eulerconst {};
	struct piconst {};

	variant<rtype,double,eulerconst,piconst> v;

	scalarreal(double val=0.0) : v(val) {}
	explicit scalarreal(eulerconst ec) : v(ec) {}
	explicit scalarreal(piconst pc) : v(pc) {}
	explicit scalarreal(rtype rt) : v(std::move(rt)) {}

	scalarreal(int i) : v(rtype(i)) {}

	template<typename I1, typename I2>
	explicit scalarreal(I1 &&num, I2 &&den) : v(rtype(std::forward<I1>(num),std::forward<I2>(den))) {}

	/*
	scalarreal(scalarreal &r) : v(r.v) {}
	scalarreal(const scalarreal &r) : v(r.v) {}
	scalarreal(scalarreal &&r) : v(std::move(r.v)) {}
	*/

	explicit operator double() const {
		switch(v.which()) {
			case 0: return (double)(boost::get<rtype>(v).numerator())/(double)(boost::get<rtype>(v).denominator());
			case 1: return boost::get<double>(v);
			case 2: return std::exp(1.0);
			case 3: return std::acos(-1.0);
			default: return 0.0;
		}
	}

	explicit operator int() const {
		return asint();
	}

	scalarreal operator-() const {
		if (v.which()==0) return scalarreal{-boost::get<rtype>(v)};
		else return scalarreal{-(double)(*this)};
		return *this;
	}

	scalarreal &operator+=(const scalarreal &s) {
		if (v.which()==0 && s.v.which()==0) boost::get<rtype>(v) += boost::get<rtype>(s.v);
		else v = (double)(*this) + (double)(s);
		return *this;
	}
	scalarreal &operator-=(const scalarreal &s) {
		if (v.which()==0 && s.v.which()==0) boost::get<rtype>(v) -= boost::get<rtype>(s.v);
		else v = (double)(*this) - (double)(s);
		return *this;
	}
	scalarreal &operator*=(const scalarreal &s) {
		if (v.which()==0 && s.v.which()==0) boost::get<rtype>(v) *= boost::get<rtype>(s.v);
		else v = (double)(*this) * (double)(s);
		return *this;
	}
	scalarreal &operator/=(const scalarreal &s) {
		if (v.which()==0 && s.v.which()==0 && s) boost::get<rtype>(v) /= boost::get<rtype>(s.v);
		else v = (double)(*this) / (double)(s);
		return *this;
	}
	bool operator<(const scalarreal &s) const {
		if (v.which()==0 && s.v.which()==0) return boost::get<rtype>(v) < boost::get<rtype>(s.v);
		else return (double)(*this) < (double)(s);
	}
	bool operator==(const scalarreal &s) const {
		if (v.which()==0 && s.v.which()==0) return boost::get<rtype>(v) == boost::get<rtype>(s.v);
		else return (double)(*this) == (double)(s);
	}
	bool operator==(const double &d) const {
		return (double)(*this) == d;
	}
	bool operator<(const double &d) const {
		return (double)(*this) < d;
	}
	bool operator==(const int &i) const {
		switch (v.which()) {
			case 0: return boost::get<rtype>(v)==i;
			case 1: return (double)(*this) == i;
			default: return false;
		}
	}
	bool operator<(const int &i) const {
		if (v.which()==0) return boost::get<rtype>(v)<i;
		else return (double)(*this) < i;
	}
	const scalarreal &operator++() {
		if (v.which()==0) boost::get<rtype>(v)++;
		else v = (double)(*this)+1.0;
		return *this;
	}
	const scalarreal &operator--() {
		if (v.which()==0) boost::get<rtype>(v)--;
		else v = (double)(*this)-1.0;
		return *this;
	}

	explicit operator bool() const {
		switch(v.which()) {
			case 0: return (bool)(boost::get<rtype>(v));
			case 1: return boost::get<double>(v)!=0.0;
			case 2: return true;
			case 3: return true;
			default: return true;
		}
	}
	bool operator!() const {
		switch(v.which()) {
			case 0: return !(boost::get<rtype>(v));
			case 1: return boost::get<double>(v)==0.0;
			case 2: return false;
			case 3: return false;
			default: return false;
		}
	}

	bool isint() const {
		switch(v.which()) {
			case 0: return boost::get<rtype>(v).numerator() % boost::get<rtype>(v).denominator() == 0;
			case 1: return std::fmod(boost::get<double>(v),1.0) == 0.0;
			default: return false;
		}
	}

	bool iseven() const {
		return isint() && asint()%2==0;
	}
	bool isodd() const {
		return isint() && asint()%2==1;
	}

	scalarreal round() const {
		switch(v.which()) {
			case 0: {
				   auto &r = boost::get<rtype>(v);
				   auto div = std::div(r.numerator(),r.denominator());
				   // away from zero... as in std::round
				   if (div.rem*2<=-abs(r.denominator()))
					   return div.quot - 1;
				   else if (div.rem*2>=abs(r.denominator()))
					   return div.quot + 1;
				   else return div.quot;
			   }
			default: return std::round((double)(*this));
		}
	}

	scalarreal floor() const {
		switch(v.which()) {
			case 0: {
				   auto &r = boost::get<rtype>(v);
				   auto div = std::div(r.numerator(),r.denominator());
				   if (div.rem<0)
					   return div.quot - 1;
				   else return div.quot;
			   }
			default: return std::floor((double)(*this));
		}
	}

	scalarreal ceil() const {
		switch(v.which()) {
			case 0: {
				   auto &r = boost::get<rtype>(v);
				   auto div = std::div(r.numerator(),r.denominator());
				   if (div.rem>0)
					   return div.quot + 1;
				   else return div.quot;
			   }
			default: return std::ceil((double)(*this));
		}
	}

	int asint() const {
		switch(v.which()) {
			case 0: return boost::get<rtype>(v).numerator() / boost::get<rtype>(v).denominator();
			default: return std::round((double)(*this));
		}
	}

	bool iszero() const {
		switch(v.which()) {
			case 0: return boost::get<rtype>(v).numerator() ==0;
			case 1: return boost::get<double>(v) == 0.0;
			default: return false;
		}
	}
};

namespace std {
	template<> struct numeric_limits<scalarreal> {
		static scalarreal infinity() {
			return scalarreal{std::numeric_limits<double>::infinity()};
		}
		static constexpr bool is_specialized = true;
		static constexpr bool is_signed = true;
		static constexpr bool is_integer = false;
		static constexpr bool is_exact = false;
		static constexpr int digits = numeric_limits<double>::digits;
		static constexpr int digits10 = numeric_limits<double>::digits10;
		static constexpr int radix = numeric_limits<double>::radix;
	};

	bool isfinite(const scalarreal &s) {
		switch(s.v.which()) {
			case 1: return std::isfinite(boost::get<double>(s.v));
			default: return true;
		}
	}
}

auto round(const scalarreal &s) {
	return s.round();
}
auto ceil(const scalarreal &s) {
	return s.ceil();
}
auto floor(const scalarreal &s) {
	return s.floor();
}

std::string tostring(const scalarreal &s) {
	if (s.isint()) return std::to_string(s.asint());
	switch (s.v.which()) {
		case 0: {
				   auto &r = boost::get<scalarreal::rtype>(s.v);
				   return std::string("(")+std::to_string(r.numerator())+"/"+std::to_string(r.denominator())+")";
			   }
		case 1: {
				   std::ostringstream ss;
				   ss << std::setprecision(17) << boost::get<double>(s.v);
				   return {ss.str()};
			   }
		case 2: return {"e"};
		case 3: return {"pi"};
		default: return {"?"};
	}
}

/*
std::ostream &operator<<(std::ostream &os, const scalarreal &s) {
	return os << tostring(s);
}

std::istream &operator>>(std::istream &is, scalarreal &s) {
	// not implemented
	return is;
}
*/

scalarreal abs(const scalarreal &s) {
	if (s.v.which()==0)
		return scalarreal{abs(boost::get<scalarreal::rtype>(s.v))};
	return scalarreal{(double)std::abs((double)(s))};
}

scalarreal log(const scalarreal &e) {
	if (e==1.0) return scalarreal{0};
	switch(e.v.which()) {
		case 2: return scalarreal{1};
		default: return log((double)e);
	}
}

template<typename T>
T intpow1(T b, int e) {
	if (e==1) return b;
	T ret = intpow1(b,e/2);
	ret *= ret;
	if (e&1) ret *= b;
	return ret;
}

template<typename T>
T intpow(T b, int e) {
	if (e==0) return T{1};
	if (e<0) return T{1}/intpow1(b,-e);
	return intpow1(b,e);
}

scalarreal pow(const scalarreal &b, const int &e) {
	if (e==1) return b;
	if (e==0) return scalarreal{1};
	if (e==-1) return 1/b;
	if (b.v.which()==2)
		return scalarreal{std::exp((double)e)};
	if (b.v.which()==1)
		return scalarreal{ std::pow((double)b,(double)e) };
	if (b==1) return scalarreal{1};
	if (b.v.which()==0)
		return intpow(b,e);
	else
		return scalarreal{ std::pow((double)b,(double)e) };
}

scalarreal pow(const scalarreal &b, const scalarreal &e) {
	if (e==1) return b;
	if (e==0) return scalarreal{1};
	if (e==-1) return 1/b;
	if (b.v.which()==2)
		return scalarreal{std::exp((double)e)};
	if (b.v.which()==1 || e.v.which()==1)
		return scalarreal{ std::pow((double)b,(double)e) };
	if (b==1) return scalarreal{1};
	if (b.v.which()==0 && e.isint())
		return intpow(b,e.asint());
	else
		return scalarreal{ std::pow((double)b,(double)e) };
}

#endif


#include <vector>
#include <algorithm>
#include <iostream>
#include <fstream>
#include <string>
#include <iterator>
#include <Eigen/Dense>

class Point {
  public:
    Point();
    Point(std::istream& in);
    std::istream& read(std::istream&);
    void print( std::ostream& );
    double getX() const { return x; };
    double getY() const { return y; };

  private:
    double x;
    double y;
    std::string name;
};

Point::Point() : x(0), y(0) {}

Point::Point(std::istream& in){
  read( in );
}

std::istream& Point::read( std::istream& in){
  in >> name >> x >> y;
  return in;
}

void Point::print( std::ostream& out ) {
  out << name << " " << x << " " << y << std::endl;
}

std::istream& operator>>(std::istream& in, Point& p){
  p.read(in);
  return in;
}

void parse_args(int argc , char* argv[], std::string &fname)
{
  if ( 1 == argc ) {
    fname = "data.txt";
  } else {
    fname = argv[1];
  }
}

int main(int argc, char* argv[])
{
  std::string fname;

  parse_args(argc, argv, fname);

  std::ifstream in(fname);
  if (!in) {
    std::cout << "File not found " << fname << std::endl;
    return 1;
  }

  // Use STL to read numbers from file into a vector
  std::vector<Point> data;

  std::copy( std::istream_iterator<Point>(in),
    std::istream_iterator<Point>(), std::back_inserter(data));

  // Transform data
  std::vector<double> x;
  std::transform( data.begin(), data.end(), std::back_inserter(x),
    [](Point p){ return p.getX(); });

  std::vector<double> y;
  std::transform( data.begin(), data.end(), std::back_inserter(y),
    [](Point p){ return p.getY(); });

  Eigen::Map<Eigen::VectorXd> X(x.data(),x.size());
  Eigen::Map<Eigen::VectorXd> Y(y.data(),y.size());

  // Store x and y coordinates as 2 x N matrix [x1, x2, ...; y1, y2, ...]
  Eigen::MatrixXd m( 2, x.size());
  m.row(0) = X;
  m.row(1) = Y;
  std::cout << m << std::endl;

  // Shear transformation
  Eigen::Matrix2d A;
  A << 1, 2, 0, 1;
  m = A*m;
  std::cout << m << std::endl;
  X = m.row(0);

  for( auto d : data){
    d.print( std::cout );
  }
  std::cout << std::endl;

  for( auto xv : x){
    std::cout << xv << std::endl;
  }

  return 0;
}


// Author(s): Jan Friso Groote
// Copyright: see the accompanying file COPYING or copy at
// https://svn.win.tue.nl/trac/MCRL2/browser/trunk/COPYING
//
// 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)
//
/// \file find_test.cpp
/// \brief Test the linear_inequality functionality

#include <boost/test/minimal.hpp>
#include "mcrl2/data/parse.h"
#include "mcrl2/data/linear_inequalities.h"

using namespace mcrl2;
using namespace mcrl2::core;
using namespace mcrl2::data;

void split_conjunction_of_inequalities_set(const data_expression& e, std::vector < linear_inequality >& v, const rewriter& r)
{
  if (sort_bool::is_and_application(e))
  {
    split_conjunction_of_inequalities_set(application(e)[0],v,r);
    split_conjunction_of_inequalities_set(application(e)[1],v,r);
  }
  else
  {
    v.push_back(linear_inequality(e,r));
  }
}

bool test_consistency_of_inequalities(const std::string& vars,
                                      const std::string& inequalities,
                                      const bool expect_consistent)
{
  // Take care that reals are part of the data type.
  data_specification data_spec;
  variable_list variables=parse_variables(vars);
  data_spec.add_context_sort(sort_real::real_());
  const data_expression e=parse_data_expression(inequalities,variables,data_spec);

  rewriter r(data_spec);
  std::vector < linear_inequality > v_inequalities;
  split_conjunction_of_inequalities_set(e,v_inequalities,r);

  if (is_inconsistent(v_inequalities,r))
  {
    if (expect_consistent)
    {
      std::cout << "Expected consistent, found inconsistent\n";
      std::cout << variables << ": " << inequalities << "\n";
      std::cout << "Internal inequalities: " << pp_vector(v_inequalities) << "\n";
      return false;
    }
  }
  else
  {
    if (!expect_consistent)
    {
      std::cout << "Expected inconsistent, found consistent\n";
      std::cout << variables << ": " << inequalities << "\n";
      std::cout << "Internal inequalities: " << pp_vector(v_inequalities) << "\n";
      return false;
    }
  }
  return true;
}

bool test_application_of_Fourier_Motzkin(const std::string& vars,
                                         const std::string& variables_to_be_eliminated,
                                         const std::string& inequalities,
                                         const std::string& inconsistent_with)
{
  // Take care that reals are part of the data type.
  data_specification data_spec;
  data_spec.add_context_sort(sort_real::real_());
  const variable_list variables=parse_variables(vars);
  const data_expression e_in=parse_data_expression(inequalities,variables,data_spec);
  const variable_list v_elim=data::detail::parse_variables_new(variables_to_be_eliminated);

  rewriter r(data_spec);
  std::vector < linear_inequality > v_inequalities;
  split_conjunction_of_inequalities_set(e_in,v_inequalities,r);

  std::vector < linear_inequality> resulting_inequalities;
  fourier_motzkin(v_inequalities, v_elim.begin(), v_elim.end(), resulting_inequalities, r);

  std::vector < linear_inequality> inconsistent_inequalities=resulting_inequalities;
  inconsistent_inequalities.push_back(linear_inequality(parse_data_expression(inconsistent_with,variables,data_spec),r));
  if (!is_inconsistent(inconsistent_inequalities,r, false))
  {
    std::cout << "Expected set of inequations to be inconsisten with given inequality after applying Fourier-Motzkin elimination\n";
    std::cout << "Input: " << variables << ": " << inequalities << "\n";
    std::cout << "Parsed input : " << pp_vector(v_inequalities) << "\n";
    std::cout << "Variables to be eliminated: " << v_elim << "\n";
    std::cout << "Input after applying Fourier Motzkin: " << pp_vector(resulting_inequalities) << "\n";
    std::cout << "Should be inconsistent with " << inconsistent_with << "\n";
    std::cout << "Inconsistent inequality after parsing " << pp(linear_inequality(parse_data_expression(inconsistent_with,variables,data_spec),r)) << "\n";
    return false;
  }
  return true;
}

int test_main(int /* argc */, char** /* argv[]*/)
{
  // BOOST_CHECK(test_consistency_of_inequalities("x:Real;", "x<3  && x>=4", false));
  // BOOST_CHECK(test_consistency_of_inequalities("x:Real;", "x<3  && x>=2", true));
  // BOOST_CHECK(test_consistency_of_inequalities("x:Real;", "x<3  && x>=3", false));
  // BOOST_CHECK(test_consistency_of_inequalities("x:Real;", "x<=3  && x>=3", true));
  // BOOST_CHECK(test_consistency_of_inequalities("u:Real;","0 <= u && -u <= -4 && -u < 0",true));
  BOOST_CHECK(test_consistency_of_inequalities("u,t:Real;","u + -t <= 1 && -u <= -4 && t < u && -u < 0 && -t <= 0 ",true));
  // BOOST_CHECK(test_consistency_of_inequalities("u,t,l:Real;","u + -t <= 1 && -u <= -4 && -u + l < 0 && -u < 0 && -t <= 0 && -l + t <= 0",true));

  // BOOST_CHECK(test_application_of_Fourier_Motzkin("x,y:Real;", "y:Real;", "-y + x < 0 &&  y < 2", "x>=2"));
  return 0;
}


#include <gtest/gtest.h>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <vector>

#include "../geometry/Polygon2.hh"
#include "helpers.hh"

using namespace std;
using namespace bold;
using namespace Eigen;

TEST (Polygon2Tests, basics)
{
  Polygon2<int>::PointVector points;
  points.emplace_back(0,0);
  points.emplace_back(0,1);
  points.emplace_back(1,0);

  Polygon2<int> poly(points);

  ASSERT_EQ ( 3, poly.vertexCount() );
  EXPECT_TRUE ( VectorsEqual(Vector2i(0,0), poly[0] ));
  EXPECT_TRUE ( VectorsEqual(Vector2i(0,1), poly[1] ));
  EXPECT_TRUE ( VectorsEqual(Vector2i(1,0), poly[2] ));
}

TEST (Polygon2Tests, int_squareContains)
{
  Polygon2<int>::PointVector points;
  points.emplace_back(0,0);
  points.emplace_back(0,2);
  points.emplace_back(2,2);
  points.emplace_back(2,0);

  Polygon2<int> poly(points);

  EXPECT_TRUE( poly.contains(Vector2i(0,0)) );
  EXPECT_TRUE( poly.contains(Vector2i(0,1)) );
  EXPECT_TRUE( poly.contains(Vector2i(1,1)) );

  EXPECT_FALSE( poly.contains(Vector2i(2,1)) );
  EXPECT_FALSE( poly.contains(Vector2i(2,2)) );
  EXPECT_FALSE( poly.contains(Vector2i(3,2)) );
  EXPECT_FALSE( poly.contains(Vector2i(3,3)) );
  EXPECT_FALSE( poly.contains(Vector2i(2,3)) );
  EXPECT_FALSE( poly.contains(Vector2i(0,3)) );
  EXPECT_FALSE( poly.contains(Vector2i(3,0)) );
  EXPECT_FALSE( poly.contains(Vector2i(-1,-1)) );
  EXPECT_FALSE( poly.contains(Vector2i(-1,1)) );
  EXPECT_FALSE( poly.contains(Vector2i(1,-1)) );
}

TEST (Polygon2Tests, float_squareContains)
{
  Polygon2<float>::PointVector points;
  points.emplace_back(0,0);
  points.emplace_back(0,2);
  points.emplace_back(2,2);
  points.emplace_back(2,0);

  Polygon2<float> poly(points);

  EXPECT_TRUE( poly.contains(Vector2f(0,0)) );
  EXPECT_TRUE( poly.contains(Vector2f(0,1)) );
  EXPECT_TRUE( poly.contains(Vector2f(1,1)) );
  EXPECT_TRUE( poly.contains(Vector2f(1.99999,1)) );
  EXPECT_TRUE( poly.contains(Vector2f(1.99999,1.99999)) );

  EXPECT_FALSE( poly.contains(Vector2f(2,1)) );
  EXPECT_FALSE( poly.contains(Vector2f(2,2)) );
  EXPECT_FALSE( poly.contains(Vector2f(3,2)) );
  EXPECT_FALSE( poly.contains(Vector2f(3,3)) );
  EXPECT_FALSE( poly.contains(Vector2f(2,3)) );
  EXPECT_FALSE( poly.contains(Vector2f(0,3)) );
  EXPECT_FALSE( poly.contains(Vector2f(3,0)) );
  EXPECT_FALSE( poly.contains(Vector2f(-1,-1)) );
  EXPECT_FALSE( poly.contains(Vector2f(-1,1)) );
  EXPECT_FALSE( poly.contains(Vector2f(1,-1)) );
}

TEST (Polygon2Tests, double_diamondContains)
{
  Polygon2<double>::PointVector points;
  points.emplace_back(0,0);
  points.emplace_back(-1,1);
  points.emplace_back(0,2);
  points.emplace_back(1,1);

  Polygon2<double> poly(points);

  // vertices
  EXPECT_TRUE( poly.contains(Vector2d(-1,1)) );
  EXPECT_FALSE( poly.contains(Vector2d(0,0)) );
  EXPECT_FALSE( poly.contains(Vector2d(1,1)) );
  EXPECT_FALSE( poly.contains(Vector2d(0,2)) );

  // clearly inside
  EXPECT_TRUE( poly.contains(Vector2d(0,1)) );
  EXPECT_TRUE( poly.contains(Vector2d(0,0.001)) );

  // clearly outside
  EXPECT_FALSE( poly.contains(Vector2d(1,0)) );
  EXPECT_FALSE( poly.contains(Vector2d(1,1)) );
}

TEST (Polygon2Tests, double_triangleContains)
{
  Polygon2<double>::PointVector points;
  points.emplace_back(0,0);
  points.emplace_back(1,0);
  points.emplace_back(0,1);

  Polygon2<double> poly(points);

  EXPECT_TRUE( poly.contains(Vector2d(0,0)) );
  EXPECT_TRUE( poly.contains(Vector2d(0.0001,0.0001)) );
  EXPECT_TRUE( poly.contains(Vector2d(0.0005,0.9990)) );
  EXPECT_TRUE( poly.contains(Vector2d(0.9990,0.0005)) );
  EXPECT_TRUE( poly.contains(Vector2d(0.4999,0.4999)) );

  EXPECT_FALSE( poly.contains(Vector2d(0.0005,1)) );
  EXPECT_FALSE( poly.contains(Vector2d(1,0.0005)) );
  EXPECT_FALSE( poly.contains(Vector2d(0.5001,0.5001)) );
  EXPECT_FALSE( poly.contains(Vector2d(1,1)) );
  EXPECT_FALSE( poly.contains(Vector2d(-0.0001,-0.0001)) );
  EXPECT_FALSE( poly.contains(Vector2d(-0.0001,0.0001)) );
  EXPECT_FALSE( poly.contains(Vector2d(0.0001,-0.0001)) );
}

TEST (Polygon2Tests, clip)
{
  // unit square
  Polygon2<double>::PointVector points;
  points.emplace_back(0,0);
  points.emplace_back(1,0);
  points.emplace_back(1,1);
  points.emplace_back(0,1);

  Polygon2<double> poly(points);

  // Exactly corner to corner, diagonally (touching all four line segments)
  auto clipped = poly.clipLine(LineSegment2d(Vector2d(0,0),Vector2d(1,1)));
  ASSERT_TRUE(clipped.hasValue());
  ASSERT_TRUE(LinesEqual(
    LineSegment2d(Vector2d(0,0),Vector2d(1,1)),
    clipped.value()
  ));

  // Inside, completely contained
  clipped = poly.clipLine(LineSegment2d(Vector2d(0.25,0.25),Vector2d(0.75,0.75)));
  ASSERT_TRUE(clipped.hasValue());
  ASSERT_TRUE(LinesEqual(
    LineSegment2d(Vector2d(0.25,0.25),Vector2d(0.75,0.75)),
    clipped.value()
  ));

  // From inside (corner) to outside, running through diagonally opposite corner (touching all four line segments)
  clipped = poly.clipLine(LineSegment2d(Vector2d(0,0),Vector2d(5,5)));
  ASSERT_TRUE(clipped.hasValue());
  ASSERT_TRUE(LinesEqual(
    LineSegment2d(Vector2d(0,0),Vector2d(1,1)),
    clipped.value()
  ));

  // Completely inside to outside, touching one line segment
  clipped = poly.clipLine(LineSegment2d(Vector2d(0.5,0.5),Vector2d(1.5,0.5)));
  ASSERT_TRUE(clipped.hasValue());
  ASSERT_TRUE(LinesEqual(
    LineSegment2d(Vector2d(0.5,0.5),Vector2d(1,0.5)),
    clipped.value()
  ));

  // Run horizontally through cube, intersecting both left and right edges
  clipped = poly.clipLine(LineSegment2d(Vector2d(-1,0.5),Vector2d(2,0.5)));
  ASSERT_TRUE(clipped.hasValue());
  ASSERT_TRUE(LinesEqual(
    LineSegment2d(Vector2d(0,0.5),Vector2d(1,0.5)),
    clipped.value()
  ));

  // Run diagonally through cube, bisecting the top left quadrant
  clipped = poly.clipLine(LineSegment2d(Vector2d(-0.5,0),Vector2d(1,1.5)));
  ASSERT_TRUE(clipped.hasValue());
  ASSERT_TRUE(LinesEqual(
    LineSegment2d(Vector2d(0,0.5),Vector2d(0.5,1)),
    clipped.value()
  ));
}


//==================================================================================================
/*!
  @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_FUNCTION_SCALAR_SQRT1PM1_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_FUNCTION_SCALAR_SQRT1PM1_HPP_INCLUDED

#include <boost/simd/constant/half.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/divides.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/is_less.hpp>
#include <boost/simd/function/minusone.hpp>
#include <boost/simd/function/oneplus.hpp>
#include <boost/simd/function/sqrt.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( sqrt1pm1_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0) const BOOST_NOEXCEPT
    {
      A0 tmp =  bs::sqrt(oneplus(a0));
      return  bs::if_else((bs::abs(a0) < bs::Half<A0>()),
                          a0/bs::oneplus(tmp),
                          bs::minusone(tmp));
    }
  };
} } }

#endif


/*
* Copyright 2019 © Centre Interdisciplinaire de développement en Cartographie des Océans (CIDCO), Tous droits réservés
*/

#ifndef GEOREFERENCING_HPP
#define GEOREFERENCING_HPP

#include <Eigen/Dense>
#include "../math/CoordinateTransform.hpp"
#include "Raytracing.hpp"
#include "../Ping.hpp"

/*!
* \brief Georeferencing class
* \author Guillaume Labbe-Morissette, Jordan McManus, Emile Gagne
* \date October 2, 2018, 9:39 AM
*/
class Georeferencing{
public:
  /**
  * Georeferences a ping
  *
  * @param georeferencedPing georeferenced ping in vector form
  * @param attitude the attitude of the ship in the IMU frame
  * @param position the position of the ship in the TRF
  * @param ping the ping of the georeference in the sonar frame
  * @param svp the SoundVelocityProfile
  * @param leverArm vector from the position reference point (PRP) to the acoustic center
  *
  */
  virtual void georeference(Eigen::Vector3d & georeferencedPing,Attitude & attitude,Position & position,Ping & ping,SoundVelocityProfile & svp,Eigen::Vector3d & leverArm,Eigen::Matrix3d & boresight){};
};

/*!
* \brief TRF Georeferencing class
*
* Extends Georeferencing class
*/
class GeoreferencingTRF : public Georeferencing{
public:

  /**
  * Georeferences a ping in the TRF
  *
  * @param georeferencedPing vector of a ping georeferenced
  * @param attitude the attitude of the ship in the IM frame
  * @param position the position of the ship in the TRF
  * @param ping the ping of the georeference in the sonar frame
  * @param svp the sound velocity profile
  * @param leverArm vector from the position reference point (PRP) to the acoustic center
  *
  */
  void georeference(Eigen::Vector3d & georeferencedPing,Attitude & attitude,Position & position,Ping & ping,SoundVelocityProfile & svp,Eigen::Vector3d & leverArm,Eigen::Matrix3d & boresight) {
    //Compute transform matrixes
    Eigen::Matrix3d ned2ecef;
    CoordinateTransform::ned2ecef(ned2ecef,position);

#ifdef DEBUG
        std::cerr << "NED 2 ECEF: " << std::endl << ned2ecef << std::endl << std::endl;
#endif

    Eigen::Matrix3d imu2ned;
    CoordinateTransform::getDCM(imu2ned,attitude);

#ifdef DEBUG
        std::cerr << "IMU 2 NED: " << std::endl << imu2ned << std::endl << std::endl;
#endif

    //Convert position to ECEF
    Eigen::Vector3d positionECEF;
    CoordinateTransform::getPositionECEF(positionECEF,position);

#ifdef DEBUG
        std::cerr << "Position ECEF: " << std::endl << positionECEF << std::endl << std::endl;
#endif

    //Convert ping to ECEF
    Eigen::Vector3d pingVectorNED;
    Raytracing::rayTrace(pingVectorNED,ping,svp,boresight,imu2ned);

#ifdef DEBUG
        std::cerr << "Raytraced ping: " << std::endl << pingVectorNED << std::endl << std::endl;
#endif

    Eigen::Vector3d pingECEF = ned2ecef * pingVectorNED;

#ifdef DEBUG
        std::cerr << "Ping ECEF: " << std::endl << pingECEF << std::endl << std::endl;
#endif

    //Convert lever arm to ECEF
    Eigen::Vector3d leverArmECEF =  ned2ecef * (imu2ned * leverArm);

    //Compute total ECEF vector

    georeferencedPing = positionECEF + pingECEF + leverArmECEF;
  }
};

/*!
* \brief LGF Georeferencing class
*/
class GeoreferencingLGF : public Georeferencing{
public:

    /**
     * Georeferences a ping in the LGF (NED)
     *
     * @param georeferencedPing vector of a ping georeferenced
     * @param attitude the attitude of the ship in the IM frame
     * @param position the position of the ship in the TRF
     * @param ping the ping of the georeference in the sonar frame
     * @param svp the sound velocity profile
     * @param leverArm vector from the position reference point (PRP) to the acoustic center
     *
     */

    virtual void georeference(Eigen::Vector3d & georeferencedPing,Attitude & attitude,Position & position,Ping & ping,SoundVelocityProfile & svp,Eigen::Vector3d & leverArm,Eigen::Matrix3d & boresight) {
        Eigen::Matrix3d imu2ned;
        CoordinateTransform::getDCM(imu2ned,attitude);

	//Convert position's geographic coordinates to ECEF, and then from ECEF to NED
        Eigen::Vector3d positionECEF;
        CoordinateTransform::getPositionECEF(positionECEF,position);

#ifdef DEBUG
        std::cerr << "Position ECEF: " << std::endl << positionECEF << std::endl << std::endl;
#endif

        Eigen::Vector3d centered = positionECEF-centroidECEF;

	Eigen::Vector3d positionNED = ecef2ned * centered;

        //Convert ping to NED
        Eigen::Vector3d pingNED;
        Raytracing::rayTrace(pingNED,ping,svp,boresight,imu2ned);

        //Convert lever arm to NED
        Eigen::Vector3d leverArmNED =  imu2ned * leverArm;

        //Compute total NED vector
        georeferencedPing = positionNED + pingNED + leverArmNED;
    }

    /**
     * Sets centroid and inits ECEF 2 NED matrix
     */
    void setCentroid(Position & c){
	if(this->centroid) delete centroid;

	this->centroid=new Position(c.getTimestamp(), c.getLatitude(), c.getLongitude(), c.getEllipsoidalHeight());
        CoordinateTransform::getPositionECEF(centroidECEF,*this->centroid);
	CoordinateTransform::ned2ecef(ecef2ned,*this->centroid);
        ecef2ned.transposeInPlace();
    }

    /**
    *  Get a pointer to the centroid
    */

    Position * getCentroid(){ return centroid;};

protected:

        Eigen::Vector3d centroidECEF;
	Eigen::Matrix3d ecef2ned;

private:
	Position * centroid = NULL; //in geographic coordinates
};

#endif


// Copyright Louis Dionne 2013-2016
// 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/config.hpp>
#include <boost/hana/div.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/eval_if.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/lazy.hpp>
#include <boost/hana/monadic_fold_right.hpp>
#include <boost/hana/optional.hpp>
#include <boost/hana/tuple.hpp>
namespace hana = boost::hana;

int main() {
    BOOST_HANA_CONSTEXPR_LAMBDA auto safe_div = [](auto x, auto y) {
        return hana::eval_if(y == hana::int_c<0>,
            hana::make_lazy(hana::nothing),
            [=](auto _) {
                return hana::just(_(x) / y);
            }
        );
    };

    // with an initial state
    BOOST_HANA_CONSTANT_CHECK(
        hana::monadic_fold_right<hana::optional_tag>(
            hana::tuple_c<int, 1000, 8, 4>, hana::int_c<2>, safe_div
        )
            ==
        hana::just(hana::int_c<1000> / (hana::int_c<8> / (hana::int_c<4> / hana::int_c<2>)))
    );

    BOOST_HANA_CONSTANT_CHECK(
        hana::monadic_fold_right<hana::optional_tag>(
            hana::tuple_c<int, 1000, 8, 4>, hana::int_c<0>, safe_div
        )
            ==
        hana::nothing
    );

    // without an initial state
    BOOST_HANA_CONSTANT_CHECK(
        hana::monadic_fold_right<hana::optional_tag>(
            hana::tuple_c<int, 1000, 8, 4, 2>, safe_div
        )
            ==
        hana::just(hana::int_c<1000> / (hana::int_c<8> / (hana::int_c<4> / hana::int_c<2>)))
    );

    BOOST_HANA_CONSTANT_CHECK(
        hana::monadic_fold_right<hana::optional_tag>(
            hana::tuple_c<int, 1000, 8, 4, 0>, safe_div
        )
            ==
        hana::nothing
    );
}


//=======================================================================
// Copyright (c)
//
// 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)
//=======================================================================
/**
 * @file n_queens_test.cpp
 * @brief
 * @author Piotr Wygocki
 * @version 1.0
 * @date 2014-01-04
 */

#include "test_utils/logger.hpp"

#include "paal/local_search/n_queens/n_queens_local_search.hpp"
#include "paal/local_search/local_search.hpp"
#include "paal/data_structures/components/components_replace.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/range/algorithm_ext/iota.hpp>
#include <boost/range/algorithm/random_shuffle.hpp>

BOOST_AUTO_TEST_CASE(n_queens_test) {
    namespace ls = paal::local_search;
    typedef ls::n_queens_solution_adapter<std::vector<int>> Adapter;
    for (int i : { 4, 5, 6, 7, 8, 9, 10, 100, 500 }) {
        std::vector<int> queens(i);
        boost::iota(queens, 0);
        boost::random_shuffle(queens);

        LOGLN("n = " << i
                     << " start obj fun val = " << Adapter(queens).obj_fun());

        ls::n_queens_local_search_components<> comps;
        ls::n_queens_solution_first_improving(queens, comps);
        LOGLN("end obj fun val = " << Adapter(queens).obj_fun());
    }
}


//==================================================================================================
/*!
  @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_REMROUND_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_REMROUND_HPP_INCLUDED

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

 /*!
    @ingroup group-arithmetic
    Function object implementing remround capabilities

    Computes the remainder of division.
    The return value is x-n*y, where n is the value x/y,
    rounded toward infinity.

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

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

    For floating point values the code is equivalent to:

    @code
    T r = x-divround(x, y)*y;
    @endcode

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

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

#endif


/**
 *
 */
#include <stdio.h>
#include <iostream>
#include <vector>
#include <string>
#include <Eigen/Dense>

const char* TRAIN_FILE_NAME = "../DataSets/zip.data/zip.train";
const char* TEST_FILE_NAME = "../DataSets/zip.data/zip.test";

typedef struct {
  std::vector<int> labels;
  Eigen::MatrixXd data;
} LabeledData;

Eigen::MatrixXd StlVectorsToEigenMatrix(const std::vector<std::vector<double> >& data) {
/**
 *
 */
  Eigen::MatrixXd matrix(data.size(), data[0].size());

  unsigned i;
  unsigned j;
  for (i = 0; i < data.size(); ++i) {
    for (j = 0; j < data[0].size(); ++j) {
      matrix(i, j) = data[i][j];
    }
  }

  return matrix;
}

Eigen::MatrixXd GetDataSubset(const LabeledData* labeledData, std::vector<int> labels) {
/**
 *
 */

}

LabeledData ReadDataFromFile(const char* file_name, const int dimensionality) {
/**
 *
 */
  FILE* file = fopen(file_name, "r");
  if (file == NULL) {
    printf("Failed to read from file: %s\n", file_name);
    exit(0);
  }

  std::vector<int> labels;
  std::vector<std::vector<double> > data;

  double dummy;
  int read_result = fscanf(file, "%lf", &dummy);
  while (read_result >= 1) {
    labels.push_back((int) dummy);

    std::vector<double> temp;
    unsigned i;
    for (i = 0; i < dimensionality; ++i) {
      fscanf(file, " %lf", &dummy);
      temp.push_back(dummy);
    }
    data.push_back(temp);

    read_result = fscanf(file, " %lf", &dummy);
  }

  LabeledData labeledData = {labels, StlVectorsToEigenMatrix(data)};

  return labeledData;
}

int main(int argc, char** argv) {
  // printf("%s\n%s\n", TRAIN_FILE_NAME, TEST_FILE_NAME);

  LabeledData train = ReadDataFromFile(TRAIN_FILE_NAME, 256);
  std::cout << train.data.row(0) << std::endl;

  std::cout << train.labels[0] << ' ' << train.labels[1] << ' ' << train.labels[2];

  return 0;
}


#pragma once

#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"

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

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);// Check if Solution is feasible

double f(
	const vector<bool> &yi,
	const vector<double> &fi,
	const double &transportation_cost); // Objective Function

bool canUpdateXij(
	const vector<double> &bi,
	const vector<bool> &yi,
	const double &sum_dj); // Checks if Capacity can fullfill the Demand


/// \file type.hpp

#ifndef COLORUTIL_TYPE_HPP
#define COLORUTIL_TYPE_HPP

#include <Eigen/Core>

namespace colorutil
{
    /// \brief RGB
    ///
    /// \details Each value is defined in [0, 1].
    using RGB = Eigen::Vector3d;

    /// \brief HSL
    ///
    /// \details Each value is defined in [0, 1].
    using HSL = Eigen::Vector3d;

    /// \brief CIEXYZ
    using XYZ = Eigen::Vector3d;

    /// \brief CIELAB
    using Lab = Eigen::Vector3d;
} // namespace colorutil

#endif // COLORUTIL_TYPE_HPP


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2016 Klaus Spanderen

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file squarerootclvmodel.cpp
    \brief CLV model with a square root kernel process
*/

#include <ql/processes/blackscholesprocess.hpp>
#include <ql/processes/squarerootprocess.hpp>
#include <ql/math/integrals/gaussianquadratures.hpp>

#include <ql/experimental/models/squarerootclvmodel.hpp>
#include <ql/methods/finitedifferences/utilities/gbsmrndcalculator.hpp>

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

#include <utility>

namespace QuantLib {
    SquareRootCLVModel::SquareRootCLVModel(
        const ext::shared_ptr<GeneralizedBlackScholesProcess>& bsProcess,
        ext::shared_ptr<SquareRootProcess> sqrtProcess,
        std::vector<Date> maturityDates,
        Size lagrangeOrder,
        Real pMax,
        Real pMin)
    : pMax_(pMax), pMin_(pMin), bsProcess_(bsProcess), sqrtProcess_(std::move(sqrtProcess)),
      maturityDates_(std::move(maturityDates)), lagrangeOrder_(lagrangeOrder),
      rndCalculator_(ext::make_shared<GBSMRNDCalculator>(bsProcess)) {}

    Real SquareRootCLVModel::cdf(const Date& d, Real k) const {
        return rndCalculator_->cdf(k, bsProcess_->time(d));
    }

    Real SquareRootCLVModel::invCDF(const Date& d, Real q) const {
        return rndCalculator_->invcdf(q, bsProcess_->time(d));
    }

    std::pair<Real, Real> SquareRootCLVModel::nonCentralChiSquaredParams(
        const Date& d) const {

        const Time t = bsProcess_->time(d);

        const Real kappa = sqrtProcess_->a();
        const Real theta = sqrtProcess_->b();
        const Real sigma = sqrtProcess_->sigma();

        const Real df  = 4*theta*kappa/(sigma*sigma);
        const Real ncp = 4*kappa*std::exp(-kappa*t)
            / (sigma*sigma*(1-std::exp(-kappa*t)))*sqrtProcess_->x0();

        return std::make_pair(df, ncp);
    }

    Array SquareRootCLVModel::collocationPointsX(const Date& d) const {

        const std::pair<Real, Real> p = nonCentralChiSquaredParams(d);

        Array x = GaussianQuadrature(lagrangeOrder_,
            GaussNonCentralChiSquaredPolynomial(p.first, p.second))
             .x();

        std::sort(x.begin(), x.end());

        const boost::math::non_central_chi_squared_distribution<Real>
            dist(p.first, p.second);

        const Real xMin = std::max(x.front(),
            (pMin_ == Null<Real>())
                ? 0.0 : boost::math::quantile(dist, pMin_));

        const Real xMax = std::min(x.back(),
            (pMax_ == Null<Real>())
            ? QL_MAX_REAL : boost::math::quantile(dist, pMax_));

        const Real b = xMin - x.front();
        const Real a = (xMax - xMin)/(x.back() - x.front());

        for (double& i : x) {
            i = a * i + b;
        }

        return x;
    }

    Array SquareRootCLVModel::collocationPointsY(const Date& d) const {

        const Array x = collocationPointsX(d);
        const std::pair<Real, Real> params = nonCentralChiSquaredParams(d);
        const boost::math::non_central_chi_squared_distribution<Real>
            dist(params.first, params.second);

        Array s(x.size());
        for (Size i=0, n=s.size(); i < n; ++i) {
            const Real q = boost::math::cdf(dist, x[i]);

            s[i] = invCDF(d, q);
        }

        return s;
    }

    ext::function<Real(Time, Real)> SquareRootCLVModel::g() const {
        calculate();
        return g_;
    }

    void SquareRootCLVModel::performCalculations() const {
        g_ = ext::function<Real(Time, Real)>(MappingFunction(*this));
    }

    SquareRootCLVModel::MappingFunction::MappingFunction(
        const SquareRootCLVModel& model)
    : s_(ext::make_shared<Matrix>(
         model.maturityDates_.size(), model.lagrangeOrder_)),
      x_(ext::make_shared<Matrix>(
         model.maturityDates_.size(), model.lagrangeOrder_)) {

        std::vector<Date> maturityDates = model.maturityDates_;
        std::sort(maturityDates.begin(), maturityDates.end());

        const ext::shared_ptr<GeneralizedBlackScholesProcess>&
            bsProcess = model.bsProcess_;

        for (Size i=0, n = maturityDates.size(); i < n; ++i) {
            const Date maturityDate = maturityDates[i];

            const Array x = model.collocationPointsX(maturityDate);
            const Array y = model.collocationPointsY(maturityDate);

            std::copy(x.begin(), x.end(), x_->row_begin(i));
            std::copy(y.begin(), y.end(), s_->row_begin(i));

            const Time maturity = bsProcess->time(maturityDate);

            interpl.insert(
                std::make_pair(maturity,
                    ext::make_shared<LagrangeInterpolation>(
                        x_->row_begin(i), x_->row_end(i),
                        s_->row_begin(i))));
        }
    }

    Real SquareRootCLVModel::MappingFunction::operator()(Time t,Real x) const {
        const interpl_type::const_iterator ge = interpl.lower_bound(t);

        if (close_enough(ge->first, t)) {
            return (*ge->second)(x, true);
        }

        QL_REQUIRE(ge != interpl.end() && ge != interpl.begin(),
             "extrapolation to large or small t is not allowed");

        const Time t1 = ge->first;
        const Real y1 = (*ge->second)(x, true);

        interpl_type::const_iterator lt = ge;
        std::advance(lt, -1);

        const Time t0 = lt->first;
        const Real y0 = (*lt->second)(x, true);

        return y0 + (y1 - y0)/(t1 - t0)*(t - t0);
    }
}


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>
#include <boost/simd/function/fnms.hpp>
#include <boost/simd/function/nearbyint.hpp>
#include <boost/simd/function/quadrant.hpp>
#include <boost/simd/function/bitwise_and.hpp>
#include <boost/simd/constant/three.hpp>
#include <boost/simd/constant/twoopi.hpp>
#include <boost/simd/detail/constant/pio2_1.hpp>
#include <boost/simd/detail/constant/pio2_2.hpp>
#include <boost/simd/detail/constant/pio2_3.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <utility>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD ( rem_pio2_cephes_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_ < bd::floating_<A0> >
                          )
  {
//    using i_t = bd::as_integer_t<A0>;
    using result_t = std::pair<A0, A0>              ;

    BOOST_FORCEINLINE result_t operator() ( A0 const& x) const
    {
      A0 xi =  bs::nearbyint(x*bs::Twoopi<A0>());
      A0 xr  = fnms(xi, bs::Pio2_1<A0>(), x);
      xr -= xi*bs::Pio2_2<A0>();
      xr -= xi*bs::Pio2_3<A0>();
      return {quadrant(xi), xr};
    }
  };

} } }

#endif


//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   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 <nt2/include/functions/ceil.hpp>

#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <complex>
#include <nt2/sdk/complex/complex.hpp>
#include <nt2/sdk/complex/dry.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>

#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>

NT2_TEST_CASE_TPL ( ceil_real,  BOOST_SIMD_REAL_TYPES)
{
  using nt2::ceil;
  using nt2::tag::ceil_;
  typedef typename std::complex<T> cT;
  typedef typename nt2::dry<T> dT;

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_EQUAL(ceil(nt2::Inf<cT>()), nt2::Inf<cT>());
  NT2_TEST_EQUAL(ceil(nt2::Minf<cT>()), nt2::Minf<cT>());
  NT2_TEST_EQUAL(ceil(nt2::Nan<cT>()), nt2::Nan<cT>());
  NT2_TEST_EQUAL(ceil(dT(nt2::Inf<T>())), dT(nt2::Inf<T>()));
  NT2_TEST_EQUAL(ceil(dT(nt2::Minf<T>())), dT(nt2::Minf<T>()));
  NT2_TEST_EQUAL(ceil(dT(nt2::Nan<T>())), dT(nt2::Nan<T>()));
#endif
  NT2_TEST_EQUAL(ceil(cT(T(-1.1))), T(-1));
  NT2_TEST_EQUAL(ceil(cT(T(1.1))), T(2));
  NT2_TEST_EQUAL(ceil(nt2::Mone<cT>()), nt2::Mone<cT>());
  NT2_TEST_EQUAL(ceil(nt2::One<cT>()), nt2::One<cT>());
  NT2_TEST_EQUAL(ceil(nt2::Zero<cT>()), nt2::Zero<cT>());
  NT2_TEST_EQUAL(ceil(dT(T(-1.1))), T(-1));
  NT2_TEST_EQUAL(ceil(dT(T(1.1))), T(2));
  NT2_TEST_EQUAL(ceil(dT(nt2::Mone<T>())), dT(nt2::Mone<T>()));
  NT2_TEST_EQUAL(ceil(dT(nt2::One<T>())), dT(nt2::One<T>()));
  NT2_TEST_EQUAL(ceil(dT(nt2::Zero<T>())), dT(nt2::Zero<T>()));
}


// solving A * X = B
// using driver function gesv()

#include <cstddef>
#include <iostream>
#include <vector>
#include <boost/numeric/bindings/lapack/driver/gesv.hpp>
#include <boost/numeric/bindings/std/vector.hpp>

#if !defined(TEST_MATLIB_UBLAS) && !defined(TEST_MATLIB_GLAS) && !defined(TEST_MATLIB_MTL) && !defined(TEST_MATLIB_EIGEN)
#define TEST_MATLIB_UBLAS
#endif

#if defined(TEST_MATLIB_UBLAS)

#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
namespace ublas = boost::numeric::ublas;
typedef ublas::matrix<double, ublas::column_major> m_t;
typedef std::size_t size_type;

#elif defined(TEST_MATLIB_GLAS)

#include <boost/numeric/bindings/glas/dense_matrix.hpp>
#include <glas/toolbox/la/algorithm/operators.hpp>
using namespace glas::la;
typedef glas::dense_matrix<double, glas::column_orientation> m_t;
typedef std::ptrdiff_t size_type;

#elif defined(TEST_MATLIB_MTL)

#include <boost/numeric/bindings/mtl/dense2D.hpp>
#include <boost/numeric/mtl/operation/operators.hpp>
typedef mtl::dense2D<double, mtl::matrix::parameters<mtl::tag::col_major> > m_t;
typedef std::ptrdiff_t size_type;

#elif defined(TEST_MATLIB_EIGEN)

#include <boost/numeric/bindings/eigen/matrix.hpp>
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> m_t;
typedef int size_type;

#endif

#include "utils.h"
namespace lapack = boost::numeric::bindings::lapack;
namespace bindings = boost::numeric::bindings;
using std::cout;
using std::endl;

int main() {

  cout << endl;

  size_type n = 5;
  m_t a (n, n);   // system matrix

  size_type nrhs = 2;
  m_t x (n, nrhs), b (n, nrhs);  // b -- right-hand side matrix

  init_symm (a);
  //     [n   n-1 n-2  ... 1]
  //     [n-1 n   n-1  ... 2]
  // a = [n-2 n-1 n    ... 3]
  //     [        ...       ]
  //     [1   2   ...  n-1 n]

  m_t aa (a); // copy of a, because a is `lost' after gesv()

#if defined(TEST_MATLIB_UBLAS)
  ublas::matrix_column<m_t> xc0 (x, 0), xc1 (x, 1);
  for (int i = 0; i < xc0.size(); ++i) {
    xc0 (i) = 1.;
    xc1 (i) = 2.;
  }
  b = prod (a, x);
#elif defined(TEST_MATLIB_GLAS) || defined(TEST_MATLIB_MTL) || defined(TEST_MATLIB_EIGEN)
  for (int i = 0; i < bindings::size_row (x); ++i) {
    x (i,0) = 1.;
    x (i,1) = 2.;
  }
  b = a * x;
#endif

  print_m (a, "A");
  cout << endl;
  print_m (b, "B");
  cout << endl;

//  lapack::gesv (a, b);  // solving the system, b contains x
//  no ipiv less version is currently provided, so fall back to using ipiv
  std::vector<fortran_int_t> ipiv(n);
  lapack::gesv (a, ipiv, b);  // solving the system, b contains x

  print_m (b, "X");
  cout << endl;

#if defined(TEST_MATLIB_UBLAS)
  x = prod (aa, b);
#elif defined(TEST_MATLIB_GLAS) || defined(TEST_MATLIB_MTL) || defined(TEST_MATLIB_EIGEN)
  x = aa * b;
#endif
  print_m (x, "B = A X");

  cout << endl;

}


#include <Eigen/Core>
#include <iostream>

using namespace Eigen;

// [functor]
template<class ArgType, class RowIndexType, class ColIndexType>
class indexing_functor {
  const ArgType &m_arg;
  const RowIndexType &m_rowIndices;
  const ColIndexType &m_colIndices;
 public:
  typedef Matrix<typename ArgType::Scalar,
                 RowIndexType::SizeAtCompileTime,
                 ColIndexType::SizeAtCompileTime,
                 ArgType::Flags & RowMajorBit ? RowMajor : ColMajor,
                 RowIndexType::MaxSizeAtCompileTime,
                 ColIndexType::MaxSizeAtCompileTime> MatrixType;

  indexing_functor(const ArgType &arg, const RowIndexType &row_indices, const ColIndexType &col_indices)
      : m_arg(arg), m_rowIndices(row_indices), m_colIndices(col_indices) {}

  const typename ArgType::Scalar &operator()(Index row, Index col) const {
    return m_arg(m_rowIndices[row], m_colIndices[col]);
  }
};
// [functor]

// [function]
template<class ArgType, class RowIndexType, class ColIndexType>
CwiseNullaryOp<indexing_functor<ArgType, RowIndexType, ColIndexType>, typename indexing_functor<ArgType,
                                                                                RowIndexType,
                                                                                ColIndexType>::MatrixType>
indexing(const Eigen::MatrixBase<ArgType> &arg, const RowIndexType &row_indices, const ColIndexType &col_indices) {
  typedef indexing_functor<ArgType, RowIndexType, ColIndexType> Func;
  typedef typename Func::MatrixType MatrixType;
  return MatrixType::NullaryExpr(row_indices.size(), col_indices.size(), Func(arg.derived(), row_indices, col_indices));
}
// [function]

int main() {
  std::cout << "[main1]\n";
  Eigen::MatrixXi A = Eigen::MatrixXi::Random(4, 4);
  Array3i ri(1, 2, 1);
  ArrayXi ci(6);
  ci << 3, 2, 1, 0, 0, 2;
  Eigen::MatrixXi B = indexing(A, ri, ci);
  std::cout << "A =" << std::endl;
  std::cout << A << std::endl << std::endl;
  std::cout << "A([" << ri.transpose() << "], [" << ci.transpose() << "]) =" << std::endl;
  std::cout << B << std::endl;
  std::cout << "[main1]\n";

  std::cout << "[main2]\n";
  B = indexing(A, ri + 1, ci);
  std::cout << "A(ri+1,ci) =" << std::endl;
  std::cout << B << std::endl << std::endl;
#if __cplusplus >= 201103L
  B = indexing(A, ArrayXi::LinSpaced(13, 0, 12).unaryExpr([](int x) { return x % 4; }), ArrayXi::LinSpaced(4, 0, 3));
  std::cout << "A(ArrayXi::LinSpaced(13,0,12).unaryExpr([](int x){return x%4;}), ArrayXi::LinSpaced(4,0,3)) ="
            << std::endl;
  std::cout << B << std::endl << std::endl;
#endif
  std::cout << "[main2]\n";
}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "CNNEdges"
#include <boost/test/unit_test.hpp>

#include <vector>

#include "cnn/tests/test_utils.h"
#include "cnn/tensor.h"
#include "cnn/edges.h"
#include "cnn/c2w.h"

using namespace std;
using namespace cnn;

BOOST_GLOBAL_FIXTURE(TestTensorSetup)

Dim size(const Tensor& t) {
  if (t.cols() > 1)
    return Dim(t.rows(), t.cols());
  return Dim(t.rows());
}

BOOST_AUTO_TEST_CASE(ESqrL2)
{
  auto U = Ccm({2}, {4,5});
  auto V = Ccm({2}, {1,1});
  cerr << str(U) << endl;
  SquaredEuclideanDistance e;
  vector<const Tensor*> xs = {&U, &V};
  Tensor W = e.forward(xs);
  cerr << "Norm^2:" << str(W) << endl;
  double eps = 1e-5;
  BOOST_CHECK_CLOSE(t(W,0),25., eps);
  Tensor dEdf = Ccm({1}, {1});
  Tensor d1 = e.backward(xs, W, dEdf, 0);
  Tensor d2 = e.backward(xs, W, dEdf, 1);
  cerr << d1 << endl;
  cerr << d2 << endl;
  BOOST_CHECK_CLOSE(t(d1,0), 6., eps);
  BOOST_CHECK_CLOSE(t(d1,1), 8., eps);
  BOOST_CHECK_CLOSE(t(d2,0), -6., eps);
  BOOST_CHECK_CLOSE(t(d2,1), -8., eps);
}

BOOST_AUTO_TEST_CASE(EMatrixMultiply) {
  Tensor U = Ccm({2,3}, {1,2,3,4,5,6});
  Tensor V = Ccm({3,2}, {7,8,9,10,11,12});
  MatrixMultiply mm;
  vector<const Tensor*> xs = {&U, &V};
  Tensor W = mm.forward(xs);
  BOOST_REQUIRE_EQUAL(Dim({2,2}),size(W));
  double eps = 1e-5;
  BOOST_CHECK_CLOSE(t(W,0,0), 76., eps);
  BOOST_CHECK_CLOSE(t(W,1,0), 100., eps);
  BOOST_CHECK_CLOSE(t(W,0,1), 103., eps);
  BOOST_CHECK_CLOSE(t(W,1,1), 136., eps);
  Tensor dEdf = Ccm({2,2}, {-1,0.5,1,2});
  Tensor dEdx0 = mm.backward(xs, W, dEdf, 0);
  cerr << str(dEdx0) << endl;
  BOOST_CHECK_CLOSE(t(dEdx0,0,0),3.,eps);
  BOOST_CHECK_CLOSE(t(dEdx0,0,1),3.,eps);
  BOOST_CHECK_CLOSE(t(dEdx0,0,2),3.,eps);
  BOOST_CHECK_CLOSE(t(dEdx0,1,0),23.5,eps);
  BOOST_CHECK_CLOSE(t(dEdx0,1,1),26.,eps);
  BOOST_CHECK_CLOSE(t(dEdx0,1,2),28.5,eps);
  Tensor dEdx1 = mm.backward(xs, W, dEdf, 1);
  cerr << str(dEdx1) << endl;
  BOOST_CHECK_CLOSE(t(dEdx1,0,0),0.,eps);
  BOOST_CHECK_CLOSE(t(dEdx1,1,0),-1.,eps);
  BOOST_CHECK_CLOSE(t(dEdx1,2,0),-2.,eps);
  BOOST_CHECK_CLOSE(t(dEdx1,0,1),5.,eps);
  BOOST_CHECK_CLOSE(t(dEdx1,1,1),11.,eps);
  BOOST_CHECK_CLOSE(t(dEdx1,2,1),17.,eps);
}

BOOST_AUTO_TEST_CASE(EColumnConcat)
{
  Tensor u1 = Ccm({2}, {1, 2});
  Tensor u2 = Ccm({2}, {3, 4});
  Tensor u3 = Ccm({2}, {5, 6});
  cerr << u1 << endl;
  cerr << u2 << endl;
  cerr << u3 << endl;
  vector<const Tensor*> xs = {&u1, &u2, &u3};
  ConcatenateColumns cc;
  Tensor U = cc.forward(xs);
  cerr << U << endl;
  Tensor V = Ccm({3,2}, {7,8,9,10,11,12});
  MatrixMultiply mm;
  vector<const Tensor*> xs2 = {&U, &V};
  Tensor W = mm.forward(xs2);
  cerr << W << endl;
  cerr << str(W) << endl;
  BOOST_REQUIRE_EQUAL(Dim({2,2}),size(W));
  double eps = 1e-5;
  BOOST_CHECK_CLOSE(t(W,0,0), 76., eps);
  BOOST_CHECK_CLOSE(t(W,1,0), 100., eps);
  BOOST_CHECK_CLOSE(t(W,0,1), 103., eps);
  BOOST_CHECK_CLOSE(t(W,1,1), 136., eps);
  Tensor b1 = cc.backward(xs, U, U, 0);
  Tensor b2 = cc.backward(xs, U, U, 1);
  Tensor b3 = cc.backward(xs, U, U, 2);
  cerr << str(b1) << endl;
  cerr << str(b2) << endl;
  cerr << str(b3) << endl;
  BOOST_CHECK_EQUAL(t(u1,0), t(b1,0));
  BOOST_CHECK_EQUAL(t(u1,1), t(b1,1));
  BOOST_CHECK_EQUAL(t(u2,0), t(b2,0));
  BOOST_CHECK_EQUAL(t(u2,1), t(b2,1));
  BOOST_CHECK_EQUAL(t(u3,0), t(b3,0));
  BOOST_CHECK_EQUAL(t(u3,1), t(b3,1));
}

BOOST_AUTO_TEST_CASE(ERowConcat)
{
  Tensor u1 = Ccm({2}, {1, 4});
  Tensor u2 = Ccm({2}, {2, 5});
  Tensor u3 = Ccm({3}, {3, 6, 7});
  cerr << str(u1) << endl;
  cerr << str(u2) << endl;
  cerr << str(u3) << endl;
  vector<const Tensor*> xs = {&u1, &u2, &u3};
  Concatenate cr;
  Tensor U = cr.forward(xs);
  cerr << str(U) << endl;
  //BOOST_REQUIRE_EQUAL(Dim({7}),size(U));
  double eps = 1e-5;
  BOOST_CHECK_CLOSE(t(U,0), 1., eps);
  BOOST_CHECK_CLOSE(t(U,1), 4., eps);
  BOOST_CHECK_CLOSE(t(U,2), 2., eps);
  BOOST_CHECK_CLOSE(t(U,3), 5., eps);
  BOOST_CHECK_CLOSE(t(U,4), 3., eps);
  BOOST_CHECK_CLOSE(t(U,5), 6., eps);

  Tensor b1 = cr.backward(xs, U, U, 0);
  Tensor b2 = cr.backward(xs, U, U, 1);
  Tensor b3 = cr.backward(xs, U, U, 2);
  cerr << str(b1) << endl;
  cerr << str(b2) << endl;
  cerr << str(b3) << endl;
  BOOST_CHECK_EQUAL(t(u1,0), t(b1,0));
  BOOST_CHECK_EQUAL(t(u1,1), t(b1,1));
  BOOST_CHECK_EQUAL(t(u2,0), t(b2,0));
  BOOST_CHECK_EQUAL(t(u2,1), t(b2,1));
  BOOST_CHECK_EQUAL(t(u3,0), t(b3,0));
  BOOST_CHECK_EQUAL(t(u3,1), t(b3,1));
  BOOST_CHECK_EQUAL(t(u3,2), t(b3,2));
}

BOOST_AUTO_TEST_CASE(EMultilinear) {
  Tensor b = Ccm({3},{1,2,3});
  Tensor W = Ccm({3,2},{2,4,6,3,5,7});
  Tensor x = Ccm({2},{-1,1});
  Multilinear ml;
  vector<const Tensor*> mlxs = {&b, &W, &x};
  Tensor r1 = ml.forward(mlxs);
  Sum se;
  MatrixMultiply mm;
  Tensor p = mm.forward(vector<const Tensor*>({&W, &x}));
  Tensor r2 = se.forward(vector<const Tensor*>({&p, &b}));
  BOOST_REQUIRE(size(r1) == size(r2));
  double eps = 1e-5;
  cerr << r1 << endl;
  cerr << r2 << endl;
  BOOST_CHECK_CLOSE(t(r1,0), 2., eps);
  BOOST_CHECK_CLOSE(t(r1,1), 3., eps);
  BOOST_CHECK_CLOSE(t(r1,2), 4., eps);
  BOOST_CHECK_CLOSE(t(r2,0), 2., eps);
  BOOST_CHECK_CLOSE(t(r2,1), 3., eps);
  BOOST_CHECK_CLOSE(t(r2,2), 4., eps);
  cerr << "Multilinear forward complete\n";
  Tensor dEdf = Ccm({3}, {1., 0.5, 0.25});
  Tensor dEdx = ml.backward(mlxs, r1, dEdf, 0);
  cerr << "BACK 0:\n";
  BOOST_CHECK(size(dEdx) == size(b));
  cerr << str(dEdx) << endl;
  BOOST_CHECK_CLOSE(t(dEdx,0), 1., eps);
  BOOST_CHECK_CLOSE(t(dEdx,1), 0.5, eps);
  BOOST_CHECK_CLOSE(t(dEdx,2), 0.25, eps);
  dEdx = ml.backward(mlxs, r1, dEdf, 1);
  cerr << "BACK 1:\n";
  BOOST_CHECK(size(dEdx) == size(W));
  cerr << str(dEdx) << endl;
  BOOST_CHECK_CLOSE(t(dEdx,0,0), -1., eps);
  BOOST_CHECK_CLOSE(t(dEdx,1,0), -0.5, eps);
  BOOST_CHECK_CLOSE(t(dEdx,2,0), -0.25, eps);
  BOOST_CHECK_CLOSE(t(dEdx,0,1), 1., eps);
  BOOST_CHECK_CLOSE(t(dEdx,1,1), 0.5, eps);
  BOOST_CHECK_CLOSE(t(dEdx,2,1), 0.25, eps);
  dEdx = ml.backward(mlxs, r1, dEdf, 2);
  cerr << "BACK 2:\n";
  BOOST_CHECK(size(dEdx) == size(x));
  cerr << str(dEdx) << endl;
  BOOST_CHECK_CLOSE(t(dEdx,0), 5.5, eps);
  BOOST_CHECK_CLOSE(t(dEdx,1), 7.25, eps);
}

BOOST_AUTO_TEST_CASE(ELogisticSigmoid) {
  Tensor x = Ccm({5,1},{-6.f,-logf(3),0.f,logf(3),6.f});
  LogisticSigmoid ls;
  vector<const Tensor*> xs = {&x};
  Tensor r = ls.forward(xs);
  BOOST_REQUIRE_EQUAL(size(r), size(x));
  double eps = 1e-2;
  BOOST_CHECK_CLOSE(t(r,0,0), 1. /(1. + exp(6.)), eps);
  BOOST_CHECK_CLOSE(t(r,1,0), 0.25, eps);
  BOOST_CHECK_CLOSE(t(r,2,0), 0.5, eps);
  BOOST_CHECK_CLOSE(t(r,3,0), 0.75, eps);
  BOOST_CHECK_CLOSE(t(r,4,0), 1. - t(r,0,0), eps);
  cerr << "HERE\n";
  cerr << str(r) << endl;
  Tensor dEdf = Ccm({5,1},{1.,1.,1.,1.,1.});
  Tensor dEdx = ls.backward(xs, r, dEdf, 0);
  BOOST_CHECK_CLOSE(t(dEdx,1,0), 0.1875, eps);
  BOOST_CHECK_CLOSE(t(dEdx,2,0), 0.25, eps);
  BOOST_CHECK_CLOSE(t(dEdx,3,0), t(dEdx,1,0), eps);
  BOOST_CHECK_CLOSE(t(dEdx,4,0), t(dEdx,0,0), eps);
}

BOOST_AUTO_TEST_CASE(ETanh) {
  Tensor x = Ccm({5,1},{-6.f,-logf(3),0.f,logf(3),6.f});
  Tanh th;
  vector<const Tensor*> xs = {&x};
  Tensor r = th.forward(xs);
  BOOST_REQUIRE(size(r) == size(x));
  double eps = 1e-2;
  BOOST_CHECK_CLOSE(t(r,1,0), -0.8, eps);
  BOOST_CHECK_CLOSE(t(r,2,0), 0, eps);
  BOOST_CHECK_CLOSE(t(r,3,0), 0.8, eps);
  BOOST_CHECK_CLOSE(t(r,4,0), -t(r,0,0), eps);
  Tensor dEdf = Ccm({5,1},{1.,1.,1.,1.,1.});
  cerr << "Tanh complete forward\n";
  Tensor dEdx = th.backward(xs, r, dEdf, 0);
  BOOST_CHECK_CLOSE(t(dEdx,1,0), 0.36, eps);
  BOOST_CHECK_CLOSE(t(dEdx,2,0), 1.0, eps);
  BOOST_CHECK_CLOSE(t(dEdx,3,0), t(dEdx,1,0), eps);
  BOOST_CHECK_CLOSE(t(dEdx,4,0), t(dEdx,0,0), eps);
}

BOOST_AUTO_TEST_CASE(MatrixVector) {
  cerr << "Matrix-Vector\n";
  Tensor W = Ccm({3,2},{2,4,6,3,5,7});
  Tensor x = Ccm({2},{-1,1});
  MatrixMultiply mm;
  vector<const Tensor*> xs = {&W, &x};
  Tensor fx = mm.forward(xs);
  cerr << str(fx) << endl;
  Tensor dEdf = Ccm({3},{-.5,0.25,5});
  Tensor M = mm.backward(xs, fx, dEdf, 0);
  cerr << "Diff with respect to W:\n";
  cerr << str(M) << endl;
  double eps = 1e-5;
  BOOST_CHECK_CLOSE(t(M,0,0), 0.5, eps);
  BOOST_CHECK_CLOSE(t(M,1,0), -0.25, eps);
  BOOST_CHECK_CLOSE(t(M,2,0), -5, eps);
  BOOST_CHECK_CLOSE(t(M,0,1), -0.5, eps);
  BOOST_CHECK_CLOSE(t(M,1,1), 0.25, eps);
  BOOST_CHECK_CLOSE(t(M,2,1), 5, eps);
  Tensor vv = mm.backward(xs, fx, dEdf, 1);
  cerr << "Diff with respect to x:\n";
  cerr << str(vv) << endl;
  BOOST_CHECK_CLOSE(t(vv,0), 30., eps);
  BOOST_CHECK_CLOSE(t(vv,1), 34.75, eps);
}

BOOST_AUTO_TEST_CASE(EConstantMinus) {
  Tensor W = Ccm({2,2},{1,2,3,-4});
  ConstantMinusX om(1);
  vector<const Tensor*> xs(1, &W);
  Tensor O = om.forward(xs);
  cerr << str(W) << endl;
  cerr << str(O) << endl;
  double eps = 1e-6;
  BOOST_CHECK_CLOSE(10 + 1 - t(W,0,0), 10 + t(O,0,0), eps);
  BOOST_CHECK_CLOSE(10 + 1 - t(W,0,1), 10 + t(O,0,1), eps);
  BOOST_CHECK_CLOSE(10 + 1 - t(W,1,0), 10 + t(O,1,0), eps);
  BOOST_CHECK_CLOSE(10 + 1 - t(W,1,1), 10 + t(O,1,1), eps);
  Tensor V = -W;
  cerr << str(W) << endl;
  cerr << str(V) << endl;
}

BOOST_AUTO_TEST_CASE(ESoftmaxUnif) {
  for (float v = -12.; v < 12.; v += 1.) {
    Tensor u = Ccm({4}, {v, v, v, v});
    Softmax sm;
    vector<const Tensor*> xs = {&u};
    Tensor m = sm.forward(xs);
    BOOST_REQUIRE_EQUAL(Dim({4}),size(m));
    double eps = 1e-5;
    for (unsigned i = 0; i < 4; ++i)
      BOOST_CHECK_CLOSE(t(m, i), 0.25, eps);
    Tensor dEdf = Ccm({4}, {1., 0., 0., 0.});
    Tensor d = sm.backward(xs, m, dEdf, 0);
    BOOST_CHECK_CLOSE(t(d,0), 0.1875, eps);
    BOOST_CHECK_CLOSE(t(d,1), -0.0625, eps);
    BOOST_CHECK_CLOSE(t(d,2), -0.0625, eps);
    BOOST_CHECK_CLOSE(t(d,3), -0.0625, eps);
//    cerr << d << endl;

    LogSoftmax lsm;
    Tensor lm = lsm.forward(xs);
    BOOST_REQUIRE_EQUAL(Dim({4}),size(lm));
    for (unsigned i = 0; i < 4; ++i)
      BOOST_CHECK_CLOSE(log(t(m, i)), t(lm, i), eps);
    Tensor b = lsm.backward(xs, lm, dEdf, 0);
    BOOST_CHECK_CLOSE(t(b, 0), 0.75, eps);
    BOOST_CHECK_CLOSE(t(b, 1), -0.25, eps);
    BOOST_CHECK_CLOSE(t(b, 2), -0.25, eps);
    BOOST_CHECK_CLOSE(t(b, 3), -0.25, eps);
  }
}

#ifdef WITH_THPP_BACKEND
BOOST_AUTO_TEST_CASE(TensorInner3D_1D) {
  Tensor A = Ccm({24}, {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23});
  A.reshape(A, {2,3,4});
  Tensor v = Ccm({4}, {-0.5, 1, 1.5, 2});
  Tensor B = Ccm({2,3}, {1, 2, 3, 4, 5, 6});
  vector<const Tensor*> xs = {&A, &v, &B};
  InnerProduct3D_1D e;
  Tensor Y = e.forward(xs);
  cerr << str(Y) << endl;
  double eps = 1e-5;
  BOOST_CHECK_CLOSE(t(Y, 0, 0), 11, eps);
  BOOST_CHECK_CLOSE(t(Y, 1, 0), 60, eps);
  BOOST_CHECK_CLOSE(t(Y, 0, 1), 29, eps);
  BOOST_CHECK_CLOSE(t(Y, 1, 1), 78, eps);
  BOOST_CHECK_CLOSE(t(Y, 0, 2), 47, eps);
  BOOST_CHECK_CLOSE(t(Y, 1, 2), 96, eps);
  Tensor dEdY = Ccm({2,3}, {1, 0.1, -1, 1.2, 2, -0.25});
  Tensor dEdx3 = e.backward(xs, Y, dEdY, 2);
  cerr << str(dEdY) << endl;
  cerr << str(dEdx3) << endl;
  Tensor dEdx1 = e.backward(xs, Y, dEdY, 0);
  cerr << dEdx1 << endl;
  //cerr << str(dEdx1) << endl;
  Tensor dEdx2 = e.backward(xs, Y, dEdY, 1);
  cerr << str(dEdx2) << endl;
}
#endif


//=======================================================================
// Copyright 2013 University of Warsaw.
// Authors: Piotr Wygocki
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#ifndef BOOST_GRAPH_FIND_FLOW_COST_HPP
#define BOOST_GRAPH_FIND_FLOW_COST_HPP

#include <boost/graph/iteration_macros.hpp>

namespace boost {

template<class Graph, class Capacity, class ResidualCapacity, class Weight>
typename property_traits<Weight>::value_type
find_flow_cost(const Graph & g, Capacity capacity, ResidualCapacity residual_capacity, Weight weight) {
    typedef typename property_traits<Weight>::value_type Cost;

    Cost cost = 0;
    BGL_FORALL_EDGES_T(e, g, Graph) {
        if(get(capacity, e) > Cost(0)) {
            cost +=  (get(capacity, e) - get(residual_capacity, e)) * get(weight, e);
        }
    }
    return cost;
}

template <class Graph, class P, class T, class R>
typename detail::edge_weight_value<Graph, P, T, R>::type
find_flow_cost(const Graph & g,
               const bgl_named_params<P, T, R>& params) {
    return find_flow_cost(g,
           choose_const_pmap(get_param(params, edge_capacity), g, edge_capacity),
           choose_const_pmap(get_param(params, edge_residual_capacity),
                       g, edge_residual_capacity),
           choose_const_pmap(get_param(params, edge_weight), g, edge_weight));
}

template <class Graph>
typename property_traits<typename property_map < Graph, edge_capacity_t >::type>::value_type
find_flow_cost(const Graph &g) {
    bgl_named_params<int, buffer_param_t> params(0);
    return find_flow_cost(g, params);
}

} //boost

#endif /* BOOST_GRAPH_FIND_FLOW_COST_HPP */