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

/*
 Copyright (C) 2006 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.
*/

#ifndef quantlib_test_linear_least_squares_regression_hpp
#define quantlib_test_linear_least_squares_regression_hpp

#include <boost/test/unit_test.hpp>

/* remember to document new and/or updated tests in the Doxygen
   comment block of the corresponding class */

class LinearLeastSquaresRegressionTest {
  public:
    static void testRegression();
    static void testMultiDimRegression();
    static void test1dLinearRegression();
    static boost::unit_test_framework::test_suite* suite();
};

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

/*
 Copyright (C) 2006 Klaus Spanderen
 Copyright (C) 2010 Slava Mazur

 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.
*/

#include "utilities.hpp"
#include <ql/math/functional.hpp>
#include <ql/math/randomnumbers/rngtraits.hpp>
#include <ql/math/linearleastsquaresregression.hpp>
#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#endif
#include <boost/bind.hpp>
#include <boost/circular_buffer.hpp>
#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic pop
#endif

using namespace QuantLib;
using namespace boost::unit_test_framework;

void LinearLeastSquaresRegressionTest::testRegression() {

    BOOST_TEST_MESSAGE("Testing linear least-squares regression...");

    SavedSettings backup;

    const Real tolerance = 0.05;

    const Size nr=100000;
    PseudoRandom::rng_type rng(PseudoRandom::urng_type(1234u));

    std::vector<boost::function1<Real, Real> > v;
    v.push_back(constant<Real, Real>(1.0));
    v.push_back(QuantLib::identity<Real>());
    v.push_back(square<Real>());
    v.push_back(std::ptr_fun<Real, Real>(std::sin));

    std::vector<boost::function1<Real, Real> > w(v);
    w.push_back(square<Real>());

    for (Size k=0; k<3; ++k) {
        Size i;
        const Real a[] = {rng.next().value,
            rng.next().value,
            rng.next().value,
            rng.next().value};

        std::vector<Real> x(nr), y(nr);
        for (i=0; i<nr; ++i) {
            x[i] = rng.next().value;

            // regression in y = a_1 + a_2*x + a_3*x^2 + a_4*sin(x) + eps
            y[i] =  a[0]*v[0](x[i]) + a[1]*v[1](x[i]) + a[2]*v[2](x[i])
                + a[3]*v[3](x[i]) + rng.next().value;
        }

        LinearRegression m(x, y, v);

        for (i=0; i<v.size(); ++i) {
            if (m.standardErrors()[i] > tolerance) {
                BOOST_ERROR("Failed to reproduce linear regression coef."
                    << "\n    error:     " << m.standardErrors()[i]
                << "\n    tolerance: " << tolerance);
            }
            if (std::fabs(m.coefficients()[i]-a[i]) > 3*m.standardErrors()[i]) {
                BOOST_ERROR("Failed to reproduce linear regression coef."
                    << "\n    calculated: " << m.coefficients()[i]
                << "\n    error:      " << m.standardErrors()[i]
                << "\n    expected:   " << a[i]);
            }
        }

        m = LinearRegression(x, y, w);

        const Real ma[] = {m.coefficients()[0], m.coefficients()[1],
            m.coefficients()[2]+m.coefficients()[4],
            m.coefficients()[3]};
        const Real err[] = {m.standardErrors()[0], m.standardErrors()[1],
            std::sqrt( m.standardErrors()[2]*m.standardErrors()[2]
        +m.standardErrors()[4]*m.standardErrors()[4]),
            m.standardErrors()[3]};
        for (i=0; i<v.size(); ++i) {
            if (std::fabs(ma[i] - a[i]) > 3*err[i]) {
                BOOST_ERROR("Failed to reproduce linear regression coef."
                    << "\n    calculated: " << ma[i]
                << "\n    error:      " << err[i]
                << "\n    expected:   " << a[i]);
            }
        }
    }
}

namespace {
    Real f(const Array& a, Size i) {
        return a[i];
    }
}

void LinearLeastSquaresRegressionTest::testMultiDimRegression() {

    BOOST_TEST_MESSAGE(
        "Testing multi-dimensional linear least-squares regression...");

    SavedSettings backup;

    const Size nr=100000;
    const Size dims = 4;
    const Real tolerance = 0.01;
    PseudoRandom::rng_type rng(PseudoRandom::urng_type(1234u));

    std::vector<boost::function1<Real, Array> > v;
    v.push_back(constant<Array, Real>(1.0));
    for (Size i=0; i < dims; ++i) {
        v.push_back(boost::bind(f, _1, i));
    }

    Array coeff(v.size());
    for (Size i=0; i < v.size(); ++i) {
        coeff[i] = rng.next().value;
    }

    std::vector<Real> y(nr, 0.0);
    std::vector<Array> x(nr, Array(dims));
    for (Size i=0; i < nr; ++i) {
        for (Size j=0; j < dims; ++j) {
            x[i][j] = rng.next().value;
        }

        for (Size j=0; j < v.size(); ++j) {
            y[i] += coeff[j]*v[j](x[i]);
        }
        y[i] += rng.next().value;
    }

    LinearRegression m(x, y, v);

    for (Size i=0; i < v.size(); ++i) {
        if (m.standardErrors()[i] > tolerance) {
            BOOST_ERROR("Failed to reproduce linear regression coef."
                << "\n    error:     " << m.standardErrors()[i]
            << "\n    tolerance: " << tolerance);
        }

        if (std::fabs(m.coefficients()[i]-coeff[i]) > 3*tolerance) {
            BOOST_ERROR("Failed to reproduce linear regression coef."
                << "\n    calculated: " << m.coefficients()[i]
            << "\n    error:      " << m.standardErrors()[i]
            << "\n    expected:   " << coeff[i]);
        }
    }

    // much simpler
    LinearRegression m1(x, y, Real(1.0));

    for (Size i=0; i < m1.dim(); ++i) {
        if (m1.standardErrors()[i] > tolerance) {
            BOOST_ERROR("Failed to reproduce linear regression coef."
                << "\n    error:     " << m1.standardErrors()[i]
            << "\n    tolerance: " << tolerance);
        }

        if (std::fabs(m1.coefficients()[i]-coeff[i]) > 3*tolerance) {
            BOOST_ERROR("Failed to reproduce linear regression coef."
                << "\n    calculated: " << m1.coefficients()[i]
            << "\n    error:      " << m1.standardErrors()[i]
            << "\n    expected:   " << coeff[i]);
        }
    }
}

void LinearLeastSquaresRegressionTest::test1dLinearRegression() {

    BOOST_TEST_MESSAGE("Testing 1D simple linear least-squares regression...");

    /* Example taken from the QuantLib-User list, see posting
    * Multiple linear regression/weighted regression, Boris Skorodumov */

    SavedSettings backup;

    std::vector<Real> x(9),y(9);
    x[0]=2.4; x[1]=1.8; x[2]=2.5; x[3]=3.0;
    x[4]=2.1; x[5]=1.2; x[6]=2.0; x[7]=2.7; x[8]=3.6;

    y[0]=7.8; y[1]=5.5; y[2]=8.0; y[3]=9.0;
    y[4]=6.5; y[5]=4.0; y[6]=6.3; y[7]=8.4; y[8]=10.2;

    std::vector<boost::function1<Real, Real> > v;
    v.push_back(constant<Real, Real>(1.0));
    v.push_back(QuantLib::identity<Real>());

    LinearRegression m(x, y);

    const Real tol = 0.0002;
    const Real coeffExpected[]  = { 0.9448, 2.6853 };
    const Real errorsExpected[] = { 0.3654, 0.1487 };

    for (Size i=0; i < 2; ++i) {
        if (std::fabs(m.standardErrors()[i]-errorsExpected[i]) > tol) {
            BOOST_ERROR("Failed to reproduce linear regression standard errors"
                << "\n    calculated: " << m.standardErrors()[i]
            << "\n    expected:   " << errorsExpected[i]
            << "\n    tolerance:  " << tol);
        }

        if (std::fabs(m.coefficients()[i]-coeffExpected[i]) > tol) {
            BOOST_ERROR("Failed to reproduce linear regression coef."
                << "\n    calculated: " << m.coefficients()[i]
            << "\n    expected:   " << coeffExpected[i]
            << "\n    tolerance:  " << tol);
        }
    }

    // an alternative container type
    boost::circular_buffer<Real> cx(x.begin(), x.end()), cy(y.begin(), y.end());
    LinearRegression m1(cx, cy);

    for (Size i=0; i < 2; ++i) {
        if (std::fabs(m1.standardErrors()[i]-errorsExpected[i]) > tol) {
            BOOST_ERROR("Failed to reproduce linear regression standard errors"
                << "\n    calculated: " << m1.standardErrors()[i]
            << "\n    expected:   " << errorsExpected[i]
            << "\n    tolerance:  " << tol);
        }

        if (std::fabs(m1.coefficients()[i]-coeffExpected[i]) > tol) {
            BOOST_ERROR("Failed to reproduce linear regression coef."
                << "\n    calculated: " << m1.coefficients()[i]
            << "\n    expected:   " << coeffExpected[i]
            << "\n    tolerance:  " << tol);
        }
    }
}

test_suite* LinearLeastSquaresRegressionTest::suite() {
    test_suite* suite =
        BOOST_TEST_SUITE("linear least squares regression tests");
    suite->add(QUANTLIB_TEST_CASE(
        &LinearLeastSquaresRegressionTest::testRegression));
    suite->add(QUANTLIB_TEST_CASE(
        &LinearLeastSquaresRegressionTest::testMultiDimRegression));
    suite->add(QUANTLIB_TEST_CASE(
        &LinearLeastSquaresRegressionTest::test1dLinearRegression));
    return suite;
}

#endif


#include <iostream>

#include <elemental.hpp>
#include <boost/mpi.hpp>
#include <boost/format.hpp>
#include <skylark.hpp>

/*******************************************/
namespace bmpi =  boost::mpi;
namespace skybase = skylark::base;
namespace skysk =  skylark::sketch;
namespace skynla = skylark::nla;
namespace skyalg = skylark::algorithms;
namespace skyutil = skylark::utility;
/*******************************************/

const int m = 50000;
const int n = 500;

typedef elem::DistMatrix<double, elem::VC, elem::STAR> matrix_type;
typedef elem::DistMatrix<double, elem::VC, elem::STAR> rhs_type;
typedef elem::DistMatrix<double, elem::STAR, elem::STAR> sol_type;

template<typename MatrixType, typename RhsType, typename SolType>
void check_solution(const MatrixType &A, const RhsType &b, const SolType &x,
    const RhsType &r0,
    double &res, double &resAtr, double &resFac) {
    RhsType r(b);
    skybase::Gemv(elem::NORMAL, -1.0, A, x, 1.0, r);
    res = skybase::Nrm2(r);

    SolType Atr(x.Height(), x.Width(), x.Grid());
    skybase::Gemv(elem::TRANSPOSE, 1.0, A, r, 0.0, Atr);
    resAtr = skybase::Nrm2(Atr);

    skybase::Axpy(-1.0, r0, r);
    RhsType dr(b);
    skybase::Axpy(-1.0, r0, dr);
    resFac = skybase::Nrm2(r) / skybase::Nrm2(dr);
}

int main(int argc, char** argv) {
    double res, resAtr, resFac;

    elem::Initialize(argc, argv);

    bmpi::communicator world;
    int rank = world.rank();

    skybase::context_t context(23234);

    // Setup problem and righthand side
    // Using Skylark's uniform generator (as opposed to Elemental's)
    // will insure the same A and b are generated regardless of the number
    // of processors.
    matrix_type A =
        skyutil::uniform_matrix_t<matrix_type>::generate(m,
            n, elem::DefaultGrid(), context);
    matrix_type b =
        skyutil::uniform_matrix_t<matrix_type>::generate(m,
            1, elem::DefaultGrid(), context);

    sol_type x(n,1);
    rhs_type r(b);

    boost::mpi::timer timer;
    double telp;

    // Solve using Elemental. Note: Elemental only supports [MC,MR]...
    elem::DistMatrix<double> A1 = A, b1 = b, x1;
    timer.restart();
    elem::LeastSquares(elem::NORMAL, A1, b1, x1);
    telp = timer.elapsed();
    x = x1;
    check_solution(A, b, x, r, res, resAtr, resFac);
    if (rank == 0)
        std::cout << "Elemental:\t\t\t||r||_2 =  "
                  << boost::format("%.2f") % res
                  << "\t\t\t\t\t\t\t||A' * r||_2 = " << boost::format("%.2e") % resAtr
                  << "\t\tTime: " << boost::format("%.2e") % telp << " sec"
                  << std::endl;
    double res_opt = res;

    skybase::Gemv(elem::NORMAL, -1.0, A, x, 1.0, r);

    // Solve using Sylark
    timer.restart();
    skynla::FastLeastSquares(elem::NORMAL, A, b, x, context);
    telp = timer.elapsed();
    check_solution(A, b, x, r, res, resAtr, resFac);
    if (rank == 0)
        std::cout << "Skylark:\t\t\t||r||_2 =  "
                  << boost::format("%.2f") % res
                  << " (x " << boost::format("%.5f") % (res / res_opt) << ")"
                  << "\t||r - r*||_2 / ||b - r*||_2 = " << boost::format("%.2e") % resFac
                  << "\t||A' * r||_2 = " << boost::format("%.2e") % resAtr
                  << "\t\tTime: " << boost::format("%.2e") % telp << " sec"
                  << std::endl;

    // Approximately solve using Sylark
    timer.restart();
    skynla::ApproximateLeastSquares(elem::NORMAL, A, b, x, context);
    telp = timer.elapsed();
    check_solution(A, b, x, r, res, resAtr, resFac);
    if (rank == 0)
        std::cout << "Skylark (approximate):\t\t||r||_2 =  "
                  << boost::format("%.2f") % res
                  << " (x " << boost::format("%.5f") % (res / res_opt) << ")"
                  << "\t||r - r*||_2 / ||b - r*||_2 = " << boost::format("%.2e") % resFac
                  << "\t||A' * r||_2 = " << boost::format("%.2e") % resAtr
                  << "\t\tTime: " << boost::format("%.2e") % telp << " sec"
                  << std::endl;

    return 0;
}


#ifndef NCA_HPP
#define NCA_HPP

#include <iostream>
#include <limits>
#include <vector>

#include <Eigen/Core>

void nearest_neighbors(const std::vector<Eigen::VectorXd>& input, const std::vector<std::string>& label) {
    unsigned int correct = 0;

    for(unsigned int i = 0; i < input.size(); ++i) {
        double min_norm = std::numeric_limits<double>::infinity();
        std::string min_norm_label;
        for(unsigned int j = 0; j < input.size(); ++j) {
            if(i == j) continue;
            double norm = (input[i] - input[j]).norm();
            if(norm < min_norm) {
                min_norm = norm;
                min_norm_label = label[j];
            }
        }

        if(label[i] == min_norm_label) {
            ++correct;
        }
    }

    std::cout << "Got " << correct << " correct out of " << input.size() << std::endl;
}

Eigen::MatrixXd scaling_matrix(const std::vector<Eigen::VectorXd>& input) {
    Eigen::MatrixXd A;
    if(input.size() == 0) return A;

    int size = input[0].size();

    std::vector< std::pair<double, double> > minmax(size, std::make_pair(std::numeric_limits<double>::infinity(), -std::numeric_limits<double>::infinity()));
    for(std::vector<Eigen::VectorXd>::const_iterator i = input.begin(); i != input.end(); ++i) {
        for(int j = 0; j < i->size(); ++j) {
            double val = (*i)[j];
            if(val < minmax[j].first) {
                minmax[j] = std::make_pair(val, minmax[j].second);
            }
            if(val > minmax[j].second) {
                minmax[j] = std::make_pair(minmax[j].first, val);
            }
        }
    }

    A = Eigen::MatrixXd::Identity(size, size);
    for(unsigned int i = 0; i < minmax.size(); ++i) {
        A(i, i) = 1.0/(minmax[i].second - minmax[i].first);
    }

    return A;
}

std::vector<Eigen::VectorXd> scale(const Eigen::MatrixXd& ScaleA, const std::vector<Eigen::VectorXd>& input) {
    std::vector<Eigen::VectorXd> scaled_input;
    for(std::vector<Eigen::VectorXd>::const_iterator i = input.begin(); i != input.end(); ++i) {
        scaled_input.push_back(ScaleA * (*i));
    }

    return scaled_input;
}

Eigen::MatrixXd neighborhood_components_analysis(const std::vector<Eigen::VectorXd>& input, const std::vector<std::string>& label, const Eigen::MatrixXd& init, unsigned int iterations, double learning_rate) {
    Eigen::MatrixXd A = init;
    for(unsigned int it = 0; it < iterations; ++it) {
        unsigned int i = it % input.size();

        double softmax_normalization = 0.0;
        for(unsigned int k = 0; k < input.size(); ++k) {
            if(k == i) continue;
            softmax_normalization += std::exp(-(A*input[i] - A*input[k]).squaredNorm());
        }

        std::vector<double> softmax;
        for(unsigned int k = 0; k < input.size(); ++k) {
            if(k == i) softmax.push_back(0.0);
            else {
                softmax.push_back(std::exp(-(A*input[i] - A*input[k]).squaredNorm()) / softmax_normalization);
            }
        }

        double p = 0.0;
        for(unsigned int k = 0; k < softmax.size(); ++k) {
            if(label[k] == label[i]) p += softmax[k];
        }

        Eigen::MatrixXd first_term = Eigen::MatrixXd::Zero(input[0].size(), input[0].size());
        Eigen::MatrixXd second_term = Eigen::MatrixXd::Zero(input[0].size(), input[0].size());
        for(unsigned int k = 0; k < input.size(); ++k) {
            if(k == i) continue;
            Eigen::VectorXd xik = input[i] - input[k];
            Eigen::MatrixXd term = softmax[k] * (xik * xik.transpose());

            first_term += term;
            if(label[k] == label[i]) second_term += term;
        }
        first_term *= p;

        A += learning_rate*A*(first_term - second_term);
    }

    return A;
}

#endif // NCA_HPP


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2022, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "vision-precomp.h"	 // Precompiled headers
//
#include <Eigen/Dense>
#include <Eigen/SVD>
#include <iostream>

#include "posit.h"

mrpt::vision::pnp::posit::posit(
	Eigen::MatrixXd obj_pts_, Eigen::MatrixXd img_pts_,
	Eigen::MatrixXd camera_intrinsic_, int n0)
{
	obj_pts = obj_pts_;
	img_pts = img_pts_.block(0, 0, n0, 2);
	cam_intrinsic = camera_intrinsic_;
	R = Eigen::MatrixXd::Identity(3, 3);
	t = Eigen::VectorXd::Zero(3);
	f = (cam_intrinsic(0, 0) + cam_intrinsic(1, 1)) / 2;

	obj_matrix =
		(obj_pts.transpose() * obj_pts).inverse() * obj_pts.transpose();

	n = n0;

	obj_vecs = Eigen::MatrixXd::Zero(n0, 3);

	for (int i = 0; i < n; i++)
		obj_vecs.row(i) = obj_pts.row(i) - obj_pts.row(0);

	img_vecs = Eigen::MatrixXd::Zero(n0, 2);
	img_vecs_old = img_vecs;

	epsilons = Eigen::VectorXd::Zero(n);
}

void mrpt::vision::pnp::posit::POS()
{
	Eigen::Vector3d I0, J0, r1, r2, r3;
	double I0_norm, J0_norm;

	int i;
	double scale;

	for (i = 0; i < 3; i++)
	{
		I0(i) = obj_matrix.row(i).dot(img_vecs.col(0));
		J0(i) = obj_matrix.row(i).dot(img_vecs.col(1));
	}

	I0_norm = I0.norm();
	J0_norm = J0.norm();

	scale = (I0_norm + J0_norm) / 2;

	/*Computing TRANSLATION */
	t(0) = img_pts(0, 0) / scale;
	t(1) = img_pts(0, 1) / scale;
	t(2) = f / scale;

	/* Computing ROTATION */
	r1 = I0 / I0_norm;
	r2 = J0 / J0_norm;
	r3 = r1.cross(r2);

	R.row(0) = r1;
	R.row(1) = r2;
	R.row(2) = r3;
}

/**
Iterate over results obtained by the POS function;
see paper "Model-Based Object Pose in 25 Lines of Code", IJCV 15, pp. 123-141,
1995.
*/
bool mrpt::vision::pnp::posit::compute_pose(
	Eigen::Ref<Eigen::Matrix3d> R_, Eigen::Ref<Eigen::Vector3d> t_)
{
	Eigen::FullPivLU<Eigen::MatrixXd> lu(obj_pts);
	if (lu.rank() < 3) return false;

	int i, iCount;
	long imageDiff = 1000;

	for (iCount = 0; iCount < LOOP_MAX_COUNT; iCount++)
	{
		if (iCount == 0)
		{
			for (i = 0; i < img_vecs.rows(); i++)
				img_vecs.row(i) = img_pts.row(i) - img_pts.row(0);
		}

		else
		{
			// Compute new image vectors
			epsilons.setZero();
			for (i = 0; i < n; i++)
			{
				epsilons(i) += obj_vecs.row(i).dot(R.row(2));
			}
			epsilons /= t(2);

			// Corrected image vectors
			for (i = 0; i < n; i++)
			{
				img_vecs.row(i) =
					img_pts.row(i) * (1 + epsilons(i)) - img_pts.row(0);
			}

			imageDiff = this->get_img_diff();
		}

		img_vecs_old = img_vecs;

		this->POS();

		if (iCount > 0 && imageDiff == 0) break;

		if (iCount == LOOP_MAX_COUNT)
		{
			std::cout << "Solution Not converged" << std::endl << std::endl;
			break;
		}
	}
	R_ = R;
	t_ = t;

	return true;
}

long mrpt::vision::pnp::posit::get_img_diff()
{
	double sumOfDiffs = 0;

	for (int i = 0; i < n; i++)
	{
		for (int j = 0; j < 2; j++)
		{
			sumOfDiffs += std::abs(
				floor(0.5 + img_vecs(i, j)) - floor(0.5 + img_vecs_old(i, j)));
		}
	}
	return static_cast<long>(sumOfDiffs);
}


#include <geometry.h>
#include <tiny.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

#include <vector>

BOOST_AUTO_TEST_SUITE(geometry);

BOOST_AUTO_TEST_CASE(intersection)
{
  typedef tiny::MathTypes<float> MT;
  typedef MT::vector3_type       V;

  {
    geometry::Plane<V> const A = geometry::make_plane( V::make(0,0,1), -1.0);
    geometry::Plane<V> const B = geometry::make_plane( V::make(0,1,0), -1.0);
    geometry::Line<V>  const L = geometry::make_intersection(A,B);

    BOOST_CHECK_CLOSE( geometry::get_distance(L.point(), A), 0.0, 0.01f );
    BOOST_CHECK_CLOSE( geometry::get_distance(L.point(), B), 0.0, 0.01f );
    BOOST_CHECK_CLOSE( inner_prod(L.direction(), A.normal()), 0.0, 0.01f );
    BOOST_CHECK_CLOSE( inner_prod(L.direction(), B.normal()), 0.0, 0.01f );
    BOOST_CHECK_CLOSE( inner_prod(L.direction(), L.direction()), 1.0, 0.01f );
  }

  {
    geometry::Plane<V> const A = geometry::make_plane( V::make(0,0,1), -1.0);
    geometry::Plane<V> const B = geometry::make_plane( V::make(0,1,0), -1.0);
    geometry::Plane<V> const C = geometry::make_plane( V::make(1,0,0), -1.0);

    V const p = geometry::make_intersection(A,B,C);

    BOOST_CHECK_CLOSE( geometry::get_distance(p, A), 0.0, 0.01f );
    BOOST_CHECK_CLOSE( geometry::get_distance(p, B), 0.0, 0.01f );
    BOOST_CHECK_CLOSE( geometry::get_distance(p, C), 0.0, 0.01f );
  }

}

BOOST_AUTO_TEST_SUITE_END();


#define GLM_ENABLE_EXPERIMENTAL 1
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Halide.h>
#include <fstream>
#include <glm/ext.hpp>
#include <glm/glm.hpp>
#include <halide_image_io.h>
#include <stdio.h>
#include <iostream>
#include <ImfRgbaFile.h>
#include <ImfStringAttribute.h>
#include <ImfMatrixAttribute.h>
#include <ImfArray.h>
#include <algorithm>
#include <ImfNamespace.h>
#include <tuple>
#include <math.h>

namespace IMF = OPENEXR_IMF_NAMESPACE;

using namespace IMF;
using namespace IMATH_NAMESPACE;

using namespace Halide;
using namespace Halide::Tools;
using namespace Eigen;
using namespace std;

using Vector4h = Matrix<Halide::Expr, 4, 1>;
using Matrix4h = Matrix<Halide::Expr, 4, 4>;

Var x, y, c, i, ii, xo, yo, xi, yi, img;

ostream &operator<<(ostream &os, const Matrix4h &m)
{
    os << m(0, 0) << " " << m(0, 1) << " " << m(0, 2) << " " << m(0, 3) << "\n"
       << m(1, 0) << " " << m(1, 1) << " " << m(1, 2) << " " << m(1, 3) << "\n"
       << m(2, 0) << " " << m(2, 1) << " " << m(2, 2) << " " << m(2, 3) << "\n"
       << m(3, 0) << " " << m(3, 1) << " " << m(3, 2) << " " << m(3, 3) << endl;
    return os;
}

ostream &operator<<(ostream &os, const Vector4h &v)
{
    os << v(0) << "\n"
       << v(1) << "\n"
       << v(2) << "\n"
       << v(3) << endl;
    return os;
}

Matrix4h getCameraMatrix(double focalLen, double pxDim, int width, int height)
{

    focalLen = focalLen * 10e-3;
    double u0 = width / 2.0;
    double v0 = height / 2.0;
    Matrix4h camMat;
    double diagEntry = focalLen / pxDim;
    camMat << diagEntry, 0.0d, u0, 0.0d,
        0.0d, diagEntry, v0, 0.0d,
        0.0d, 0.0d, 1.0d, 0.0d,
        0.0d, 0.0d, 0.0d, 1.0d;

    return camMat;
}

Matrix4h getInvCameraMat(double focalLen, double pxDim, int width, int height)
{
    focalLen = focalLen * 10e-3;
    double u0 = width / 2.0;
    double v0 = height / 2.0;
    Matrix4h camMat;
    camMat << pxDim / focalLen, 0.0d, -pxDim * u0 / focalLen, 0.0d,
        0.0d, pxDim / focalLen, -pxDim * v0 / focalLen, 0.0d,
        0.0d, 0.0d, 1.0d, 0.0d,
        0.0d, 0.0d, 0.0d, 1.0d;

    return camMat;
}

Matrix4h getTransMatProjToCam()
{
    Matrix4h transMat;
    Expr f64 = cast<double>(0.1d);
    transMat << cast<double>(0.9945219), 0.0d, -0.10452846d, -0.2d,
        0.0d, 1.0d, 0.0d, 0.0d,
        0.10452846d, 0.0d, 0.9945219d, 0.0d,
        0.0d, 0.0d, 0.0d, 1.0d;
    return transMat;
}

tuple<int, int> readOpenEXR(const char filename[], Array2D<Rgba> &pixels)
{
    RgbaInputFile file(filename);
    Box2i dw = file.dataWindow();

    int width = dw.max.x - dw.min.x + 1;
    int height = dw.max.y - dw.min.y + 1;
    cout << "Width: " << width << "    Height: " << height << endl;
    tuple<int, int> dim(width, height);
    pixels.resizeErase(height, width);

    file.setFrameBuffer(&pixels[0][0] - dw.min.x - dw.min.y * width, 1, width);
    file.readPixels(dw.min.y, dw.max.y);
    return dim;
}

void exrArrayToHalideBuffer(Array2D<Rgba> &pixels, Buffer<double> &halideBuffer, int width, int height)
{
    for (int row{0}; row < height; row++)
    {
        for (int col{0}; col < width; col++)
        {
            isinf(pixels[row][col].r) ? pixels[row][col].r = 0.0f : pixels[row][col].r = pixels[row][col].r;
            halideBuffer(col, row) = pixels[row][col].r;
        }
    }
}

void saveImage(Expr result, size_t width, size_t height, const string &basename)
{
    Target target = get_host_target();
    Func byteResult;
    byteResult(x, y) = cast<uint8_t>(clamp(result, 0.0f, 1.0f) * 255.0f);
    byteResult.compile_jit(target);
    Buffer<uint8_t> output(width, height);
    byteResult.realize(output);
    stringstream filename;
    filename << basename << ".png";
    save_image(output, filename.str());
}

void saveImageEXR(Expr result, int width, int height, const char fileName[])
{
    Target target = get_host_target();
    Func byteResult;
    byteResult(x, y) = result;
    Buffer<double> output(width, height);
    byteResult.compile_jit(target);
    byteResult.realize(output);

    Array2D<Rgba> pixels;
    pixels.resizeErase(height, width);
    for (int row{0}; row < height; row++)
    {
        for (int col{0}; col < width; col++)
        {
            pixels[row][col].r = output(col, row);
            pixels[row][col].g = output(col, row);
            pixels[row][col].b = output(col, row);
        }
    }
    RgbaOutputFile file(fileName, width, height, WRITE_RGBA);
    file.setFrameBuffer(&pixels[0][0], 1, width);
    file.writePixels(height);
}

void debugImageEXR(Expr channel1Expr, Expr channel2Expr, Expr channel3Expr, int width, int height, const char fileName[])
{
    Target target = get_host_target();
    Func byteResult;
    byteResult(x, y, c) = 0.0f;
    byteResult(x, y, 0) = channel1Expr;
    byteResult(x, y, 1) = channel2Expr;
    byteResult(x, y, 2) = channel3Expr;
    Buffer<double> output(width, height, 3);
    byteResult.compile_jit(target);
    byteResult.realize(output);

    Array2D<Rgba> pixels;
    pixels.resizeErase(height, width);
    for (int row{0}; row < height; row++)
    {
        for (int col{0}; col < width; col++)
        {
            pixels[row][col].r = output(col, row, 0);
            pixels[row][col].g = output(col, row, 1);
            pixels[row][col].b = output(col, row, 2);
        }
    }
    RgbaOutputFile file(fileName, width, height, WRITE_RGBA);
    file.setFrameBuffer(&pixels[0][0], 1, width);
    file.writePixels(height);
}

int main(int argc, char **argv)
{
    const string INPUTFILE = "depth-image-gt.exr";
    const double FOCAL_LEN = 36.1;
    const double PX_DIM = 10 * 10e-5;

    Array2D<Rgba> pixels;
    int width, height;
    tie(width, height) = readOpenEXR(INPUTFILE.c_str(), pixels);
    Buffer<double> input(width, height);
    exrArrayToHalideBuffer(pixels, input, width, height);
    cout << "Width: " << width << "  Height: " << height << endl;
    Matrix4h camMat = getCameraMatrix(FOCAL_LEN, PX_DIM, width, height);
    Matrix4h camMatInv = getInvCameraMat(FOCAL_LEN, PX_DIM, width, height);
    Matrix4h transfMatProjToCam = getTransMatProjToCam();
    cout << "Camera matrix" << endl;
    cout << camMat << endl;
    cout << "Inverse camera matrix" << endl;
    cout << camMatInv << endl;
    cout << camMatInv(0, 0) << endl;
    cout << "Transformation matrix projector to camera" << endl;
    cout << transfMatProjToCam << endl;

    Expr zDepthCam = input(x, y);
    Vector4h pxCam{x, y, 1.0f, 0.0f};
    Vector4h normCam = camMatInv * pxCam;
    debugImageEXR(normCam(0), normCam(1), normCam(2), width, height, "normCam.exr");
    Vector4h ptCam = normCam / normCam(2) * zDepthCam;
    ptCam(3) = 1.0f;
    debugImageEXR(ptCam(0), ptCam(1), ptCam(2), width, height, "ptCam.exr");
    Vector4h ptProj = transfMatProjToCam * ptCam;
    debugImageEXR(ptProj(0), ptProj(1), ptProj(2), width, height, "ptProj.exr");
    Vector4h normProj = ptProj / ptProj(2);
    ptProj(3) = 0.0f;
    Vector4h pxProj = camMat * normProj;
    //normProj = normProj / normProj(2);
    debugImageEXR(pxProj(0), pxProj(1), pxProj(2), width, height, "normProj.exr");
    Expr xPxProj = pxProj(0);
    //saveImage(xValProj, width, height, "x-val-proj-gt");
    saveImageEXR(xPxProj, width, height, "x-val-proj-gt.exr");

    return 0;
}


/*
@copyright Louis Dionne 2014
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/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/functional.hpp>
#include <boost/hana/integral.hpp>
#include <boost/hana/list/instance.hpp>
#include <boost/hana/pair.hpp>
using namespace boost::hana;

int main() {
    //! [main]
    BOOST_HANA_CONSTEXPR_LAMBDA auto xs = list(int_<1>, int_<2>, int_<3>, int_<4>);
    BOOST_HANA_CONSTANT_ASSERT(
        span(xs, _ < int_<3>)
        ==
        pair(list(int_<1>, int_<2>), list(int_<3>, int_<4>))
    );

    BOOST_HANA_CONSTANT_ASSERT(
        span(xs, _ < int_<0>)
        ==
        pair(list(), xs)
    );

    BOOST_HANA_CONSTANT_ASSERT(
        span(xs, _ < int_<5>)
        ==
        pair(xs, list())
    );
    //! [main]
}


#ifndef _COCONUT_PULP_MATH_MATRIX_HPP_
#define _COCONUT_PULP_MATH_MATRIX_HPP_

#include <array>
#include <cmath>
#include <type_traits>
#include <iosfwd>
#include <algorithm>
#include <functional>

#include <boost/operators.hpp>

#include "coconut-tools/utils/InfixOstreamIterator.hpp"

#include "Angle.hpp"
#include "Handedness.hpp"
#include "Vector.hpp"
#include "ScalarEqual.hpp"

namespace coconut {
namespace pulp {
namespace math {

template <class NextViewType, class GetElementFunc>
class MatrixView {
public:

	using MatrixType = typename GetElementFunc::MatrixType;

	using Scalar = typename MatrixType::Scalar;

	static const auto ROWS = GetElementFunc::ROWS;

	static const auto COLUMNS = GetElementFunc::COLUMNS;

	constexpr MatrixView(NextViewType nextView, GetElementFunc getElementFunc = GetElementFunc()) :
		next_(nextView), // TODO: can't use std::move here when NextViewType is a reference. Figure out this view type better.
		getElementFunc_(std::move(getElementFunc))
	{
	}

	constexpr auto get(size_t row, size_t column) const noexcept -> decltype(auto) {
		return getElementFunc_(next_, row, column);
	}

	template <size_t ROWS_ = ROWS>
	auto determinant() const noexcept {
		static_assert(ROWS_ == ROWS, "Rows count changed");
		static_assert(ROWS == COLUMNS, "Determinant only available for square matrices");

		auto result = MatrixType::Scalar(0);
		for (size_t columnIndex = 0; columnIndex < COLUMNS; ++columnIndex) {
			const auto absElement = get(0, columnIndex) * submatrix(*this, 0, columnIndex).determinant();
			if (columnIndex % 2 == 0) {
				result += absElement;
			} else {
				result -= absElement;
			}
		}
		return result;
	}

	template <>
	auto determinant<1>() const noexcept {
		static_assert(ROWS == COLUMNS, "Determinant only available for square matrices");
		return get(0, 0);
	}

	auto cofactor(size_t rowIndex, size_t columnIndex) const noexcept {
		const auto det = submatrix(*this, rowIndex, columnIndex).determinant();
		return ((rowIndex + columnIndex) % 2 == 0) ? det : -det;
	}

	template <size_t ROWS_ = ROWS>
	auto inverse() const noexcept {
		static_assert(ROWS_ == ROWS, "Rows count changed");
		static_assert(ROWS == COLUMNS, "Inverse only available for square matrices");

		const auto det = determinant();
		assert(det != Scalar(0));
		const auto detInverse = Scalar(1) / det;

		auto result = MatrixType();

		for (size_t rowIndex = 0; rowIndex < COLUMNS; ++rowIndex) {
			for (size_t columnIndex = 0; columnIndex < COLUMNS; ++columnIndex) {
				result[rowIndex][columnIndex] = detInverse * cofactor(columnIndex, rowIndex);
			}
		}

		return result;
	}

private:

	NextViewType next_;

	GetElementFunc getElementFunc_;

};

template <class MatrixT>
class MatrixViewFunc {
public:

	using MatrixType = MatrixT;

	static const auto ROWS = MatrixType::ROWS;

	static const auto COLUMNS = MatrixType::COLUMNS;

	constexpr auto operator()(MatrixType& matrix, size_t row, size_t column) const noexcept
		-> decltype(auto)
	{
		return matrix[row][column];
	}

};

template <class MatrixType>
constexpr auto viewMatrix(MatrixType& matrix) noexcept {
	return MatrixView<MatrixType&, MatrixViewFunc<MatrixType>>(matrix);
}

template <class NextViewType>
class TransposedViewFunc {
public:

	using MatrixType = typename NextViewType::MatrixType;

	static const auto ROWS = NextViewType::ROWS;

	static const auto COLUMNS = NextViewType::COLUMNS;

	constexpr auto operator()(NextViewType nextView, size_t row, size_t column) const noexcept
		-> decltype(auto)
	{
		return nextView.get(column, row);
	}

};

template <class NextViewType>
constexpr auto transpose(NextViewType nextView) noexcept {
	return MatrixView<NextViewType, TransposedViewFunc<NextViewType>>(std::move(nextView));
}

template <class MatrixType>
constexpr auto viewMatrixTransposed(MatrixType& matrix) noexcept {
	return transpose(viewMatrix(matrix));
}

template <class NextViewType>
class SubmatrixViewFunc {
public:

	using MatrixType = typename NextViewType::MatrixType;

	static const auto ROWS = NextViewType::ROWS - 1;

	static const auto COLUMNS = NextViewType::COLUMNS - 1;

	constexpr SubmatrixViewFunc(size_t noRow, size_t noColumn) :
		noRow_(noRow),
		noColumn_(noColumn)
	{
		assert(noRow < NextViewType::ROWS);
		assert(noColumn < NextViewType::COLUMNS);
	}

	constexpr auto operator()(NextViewType nextView, size_t row, size_t column) const noexcept
		-> decltype(auto)
	{
		return nextView.get((row < noRow_) ? row : row + 1, (column < noColumn_) ? column : column + 1);
	}

private:

	size_t noRow_;

	size_t noColumn_;

};

template <class NextViewType>
constexpr auto submatrix(NextViewType nextView, size_t noRow, size_t noColumn) noexcept {
	return MatrixView<NextViewType, SubmatrixViewFunc<NextViewType>>(
		std::move(nextView), SubmatrixViewFunc<NextViewType>(noRow, noColumn));
}

template <class MatrixType>
constexpr auto viewSubmatrix(MatrixType& matrix, size_t noRow, size_t noColumn) noexcept {
	return submatrix(viewMatrix(matrix), noRow, noColumn);
}

template <
	class ScalarType,
	size_t ROWS_PARAM,
	size_t COLUMNS_PARAM,
	class ScalarEqualityFunc = ScalarEqual<ScalarType>
	>
class Matrix :
	boost::equality_comparable<Matrix<ScalarType, ROWS_PARAM, COLUMNS_PARAM, ScalarEqualityFunc>,
	boost::additive<Matrix<ScalarType, ROWS_PARAM, COLUMNS_PARAM, ScalarEqualityFunc>,
	boost::multiplicative<Matrix<ScalarType, ROWS_PARAM, COLUMNS_PARAM, ScalarEqualityFunc>, ScalarType
	>>>
{
public:

	using Scalar = ScalarType;

	static const auto ROWS = ROWS_PARAM;

	static const auto COLUMNS = COLUMNS_PARAM;

	static const Matrix IDENTITY;

	using Row = Vector<ScalarType, COLUMNS, ScalarEqualityFunc>;

	using Column = Vector<ScalarType, ROWS, ScalarEqualityFunc>;

	static constexpr auto IS_ROW_MAJOR = true;

	static constexpr auto IS_COLUMN_MAJOR = !IS_ROW_MAJOR;

	static constexpr auto VECTOR_IS_SINGLE_ROW_MATRIX = false;

	static constexpr auto VECTOR_IS_SINGLE_COLUMN_MATRIX = !VECTOR_IS_SINGLE_ROW_MATRIX;

	// --- CONSTRUCTORS AND OPERATORS

	template <class... CompatibleVectorType>
	explicit constexpr Matrix(CompatibleVectorType&&... rows) noexcept :
		elements_{ std::forward<CompatibleVectorType>(rows)... }
	{
		static_assert(sizeof...(rows) == ROWS, "Bad number of arguments");
	}

	template <class... CompatibleScalarType>
	explicit constexpr Matrix(std::initializer_list<CompatibleScalarType>... rows) noexcept :
		elements_{ rows... }
	{
		static_assert(sizeof...(rows) == ROWS || sizeof...(rows) == 0, "Bad number of arguments");
	}

	template <class NVT, class GEF>
	Matrix(const MatrixView<NVT, GEF>& view) {
		for (size_t rowIndex = 0; rowIndex < ROWS; ++rowIndex) {
			for (size_t columnIndex = 0; columnIndex < COLUMNS; ++columnIndex) {
				(*this)[rowIndex][columnIndex] = view.get(rowIndex, columnIndex);
			}
		}
	}

	friend std::ostream& operator<<(std::ostream& os, const Matrix& matrix) {
		os << '<';
		std::copy(matrix.elements_.begin(), matrix.elements_.end(),
			coconut_tools::InfixOstreamIterator<Row>(os, ", "));
		os << '>';
		return os;
	}

	friend bool operator==(const Matrix& lhs, const Matrix& rhs) noexcept {
		return std::equal(lhs.elements_.begin(), lhs.elements_.end(), rhs.elements_.begin());
	}

	Matrix& operator+=(const Matrix& other) noexcept {
		std::transform(elements_.begin(), elements_.end(), other.elements_.begin(), elements_.begin(), std::plus<>());
		return *this;
	}

	Matrix& operator-=(const Matrix& other) noexcept {
		std::transform(elements_.begin(), elements_.end(), other.elements_.begin(), elements_.begin(), std::minus<>());
		return *this;
	}

	Matrix operator-() noexcept {
		auto result = Matrix();
		std::transform(elements_.begin(), elements_.end(), result.elements_.begin(), std::negate<>());
		return result;
	}

	template <class CompatibleMatrixType>
	friend std::enable_if_t<
		COLUMNS == CompatibleMatrixType::ROWS,
		Matrix<ScalarType, ROWS, CompatibleMatrixType::COLUMNS, ScalarEqualityFunc>
		> operator*(const Matrix& lhs, const CompatibleMatrixType& rhs) noexcept
	{
		auto result = Matrix<ScalarType, ROWS, CompatibleMatrixType::COLUMNS, ScalarEqualityFunc>();
		for (auto rowIndex = 0u; rowIndex < ROWS; ++rowIndex) {
			for (auto columnIndex = 0u; columnIndex < CompatibleMatrixType::COLUMNS; ++columnIndex) {
				result[rowIndex][columnIndex] = dot(lhs[rowIndex], rhs.column(columnIndex));
			}
		}
		return result;
	}

	template <class CompatibleMatrixType>
	std::enable_if_t<
		COLUMNS == ROWS &&
		CompatibleMatrixType::COLUMNS == CompatibleMatrixType::ROWS &&
		ROWS == CompatibleMatrixType::ROWS,
		Matrix&
		> operator*=(const CompatibleMatrixType& rhs) noexcept
	{
		const auto result = *this * rhs;
		*this = result;
		return *this;
	}

	Matrix& operator*=(Scalar scalar) noexcept {
		std::transform(elements_.begin(), elements_.end(), elements_.begin(), [scalar](auto element) {
			return element * scalar;
		});
		return *this;
	}

	friend Vector<Scalar, ROWS, ScalarEqualityFunc> operator*(
		const Matrix& matrix,
		const Vector<Scalar, ROWS, ScalarEqualityFunc>& vector
		) noexcept
	{
		auto result = Vector<Scalar, ROWS, ScalarEqualityFunc>();
		for (auto columnIndex = 0u; columnIndex < COLUMNS; ++columnIndex) {
			result[columnIndex] = dot(matrix.column(columnIndex), vector);
		}
		return result;
	}

	Matrix& operator/=(Scalar scalar) noexcept {
		std::transform(elements_.begin(), elements_.end(), elements_.begin(), [scalar](auto element) {
			return element / scalar;
		});
		return *this;
	}

	// --- MATRIX-SPECIFIC OPERATIONS

	constexpr Matrix<Scalar, COLUMNS, ROWS, ScalarEqualityFunc> transpose() const noexcept {
		return viewMatrixTransposed(*this);
	}

	Scalar determinant() const noexcept {
		return viewMatrix(*this).determinant();
	}

	Scalar cofactor(size_t rowIndex, size_t columnIndex) const noexcept {
		return viewMatrix(*this).cofactor(rowIndex, columnIndex);
	}

	Matrix inverse() const noexcept {
		return viewMatrix(*this).inverse();
	}

	// --- ACCESSORS

	constexpr const Row& operator[](size_t rowIndex) const noexcept {
		assert(rowIndex < ROWS);
		return elements_[rowIndex];
	}

	Row& operator[](size_t rowIndex) noexcept {
		assert(rowIndex < ROWS);
		return elements_[rowIndex];
	}

	template <size_t ROW, size_t COLUMN>
	constexpr std::enable_if_t<(ROW < ROWS && COLUMN < COLUMNS), const Scalar&> get() const noexcept {
		return elements_[ROW].get<COLUMN>();
	}

	template <size_t ROW, size_t COLUMN>
	std::enable_if_t<(ROW < ROWS && COLUMN < COLUMNS), Scalar&> get() noexcept {
		return elements_[ROW].get<COLUMN>();
	}

	constexpr const Row& row(size_t rowIndex) const noexcept {
		assert(rowIndex < ROWS);
		return elements_[rowIndex];
	}

	Row& row(size_t rowIndex) noexcept {
		assert(rowIndex < ROWS);
		return elements_[rowIndex];
	}

	Column column(size_t columnIndex) const noexcept {
		assert(columnIndex < COLUMNS);
		auto column = Column();
		getColumn_<>(column, columnIndex);
		return column;
	}

	auto view() const noexcept {
		return viewMatrix(*this);
	}

	auto view() noexcept {
		return viewMatrix(*this);
	}

private:

	std::array<Row, ROWS> elements_;

	template <size_t ROW = 0>
	void getColumn_(Column& column, size_t columnIndex) const {
		column.get<ROW>() = elements_[ROW][columnIndex];
		getColumn_<ROW + 1>(column, columnIndex);
	}

	template <>
	void getColumn_<ROWS>(Column&, size_t) const {
	}

};

using Matrix4x4 = Matrix<float, 4, 4>;
static_assert(sizeof(Matrix4x4) == sizeof(float) * 16, "Empty base optimisation didn't work");
static_assert(std::is_trivially_copyable<Matrix4x4>::value, "Matrix is not trivially copyable");

namespace detail {

template <class ST, size_t R, size_t C, class SEF>
Matrix<ST, R, C, SEF> makeIdentity() {
	auto identity = Matrix<ST, R, C, SEF>();
	for (auto row = 0u; row < R; ++row) {
		identity[row][row] = ST(1);
	}
	return identity;
}

} // namespace detail

template <class ST, size_t R, size_t C, class SEF>
const Matrix<ST, R, C, SEF>	Matrix<ST, R, C, SEF>::IDENTITY =
	detail::makeIdentity<ST, R, C, SEF>();

} // namespace math

using math::Matrix;
using math::Matrix4x4;

} // namespace pulp
} // namespace coconut

#endif /* _COCONUT_PULP_MATH_MATRIX_HPP_ */


//==================================================================================================
/*!

  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)
*/
//==================================================================================================
#include <boost/simd/function/scalar/cot.hpp>
#include <boost/simd/function/restricted.hpp>
#include <scalar_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/constant/pio_2.hpp>
#include <boost/simd/constant/pio_4.hpp>

STF_CASE_TPL (" cot",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::cot;

  using r_t = decltype(cot(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(cot(-bs::Zero<T>()), -bs::Inf<r_t>(), 0.5);
  STF_ULP_EQUAL(cot(bs::Inf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(cot(bs::Minf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(cot(bs::Nan<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(cot(bs::Zero<T>()), bs::Inf<r_t>(), 0.5);
#endif
  STF_ULP_EQUAL(cot(-bs::Pio_2<T>()), bs::Zero<r_t>(), 0.5);
  STF_ULP_EQUAL(cot(-bs::Pio_4<T>()), bs::Mone<r_t>(), 0.5);
  STF_ULP_EQUAL(cot(bs::Pio_2<T>()), bs::Zero<r_t>(), 0.5);
  STF_ULP_EQUAL(cot(bs::Pio_4<T>()), bs::One<r_t>(), 0.5);
}

STF_CASE_TPL (" cot restricted_",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::cot;

  using r_t = decltype(cot(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(bs::restricted_(cot)(-bs::Zero<T>()), -bs::Inf<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(cot)(bs::Inf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(cot)(bs::Minf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(cot)(bs::Nan<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(cot)(bs::Zero<T>()), bs::Inf<r_t>(), 0.5);
#endif
  STF_ULP_EQUAL(bs::restricted_(cot)(-bs::Pio_2<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(cot)(-bs::Pio_4<T>()), bs::Mone<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(cot)(bs::Pio_2<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(bs::restricted_(cot)(bs::Pio_4<T>()), bs::One<r_t>(), 0.5);
}


#ifndef RSVD_GRAMSCHMIDT_HPP_
#define RSVD_GRAMSCHMIDT_HPP_

#include <Eigen/Dense>
#include <algorithm>
#include <limits>

namespace Rsvd {

namespace Internal {

/// \brief Orthonormalize matrix columns inplace. Deflate if needed.
///
/// \warning This is a self-implemented algorithm, use it with caution. Use
/// #Rsvd::SubspaceIterationConditioner::Lu or #Rsvd::SubspaceIterationConditioner::Qr if numerical
/// stability and implementation quality is important.
///
/// \long This function implements the modified Gram--Schmidt process with deflation.
///
/// The deflation is implemented as follows: Let \f$i\f$ be the index of the current matrix column.
/// Let \f$n_i\f$ be its norm after projection onto a subspace orthogonal to the subspace spanned
/// by the previous columns \f$1, \ldots, n - 1\f$. Obviously, if \f$n_i\f$ is equal to zero, then
/// the \f$i\f$-th column is a linear combination of previous columns. Due to the limited precision
/// of the floating-point computations, \f$n_i\f$ will be small but greater than zero. Hence, we
/// use the following heuristic rule: If \f[ n_i < \max\{n_1, \ldots, n_{i - 1}\} \cdot
/// \varepsilon_{\mathrm{mach}} \cdot 100, \f] then the \f$i\f$-th column is deemed to be linearly
/// dependent and is filled with zeros.
///
/// Here, \f$ \varepsilon_{\mathrm{mach}} \f$ is the machine epsilon.
///
/// \note Although this function deflates if needed, it asserts that the matrix has fewer columns
/// than rows. This might help during debugging. If compiled without assertions, this function will
/// silently deflate on column rank loss.
///
/// \tparam MatrixType Eigen matrix type.
///
/// \param a Matrix whose columns should be orthonormalized inplace. The matrix can be over a real
/// or complex field.
template <typename MatrixType> void modifiedGramSchmidt(MatrixType &a) {
  using RealType = typename Eigen::NumTraits<typename MatrixType::Scalar>::Real;

  RealType largestNormSeen{0};
  // 100 is just an educated guess...
  const RealType tol{100 * std::numeric_limits<RealType>::epsilon()};

  // If a matrix has fewer rows than columns then the columns are linearly dependent
  assert(a.cols() <= a.rows());

  Eigen::Index currCol;
  for (currCol = 0; currCol < a.cols(); ++currCol) {
    for (Eigen::Index prevCol{0}; prevCol < currCol; ++prevCol) {
      /// \note Implementation detail: The order in the dot product is important for vectors over
      /// complex fields!
      a.col(currCol) -= a.col(prevCol).dot(a.col(currCol)) * a.col(prevCol);
    }

    // If the current column has near zero norm, it is a linear combination of previous columns
    const auto currColNorm{a.col(currCol).norm()};
    if (currColNorm < tol * largestNormSeen) {
      // Deflate
      a.col(currCol).setZero();
    } else {
      // Normalize
      a.col(currCol) /= currColNorm;
      largestNormSeen = std::max(largestNormSeen, currColNorm);
    }
  }
}

} // namespace Internal

} // namespace Rsvd

#endif


// Copyright 2012 Davide Anastasia
// 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 lab_test.cpp
/// \brief Unit test for LAB Colorspace
/// \author Davide Anastasia <davideanastasia@users.sourceforge.net>

#include <boost/test/unit_test.hpp>

#define TEST_CHECK_CLOSE(a, b) \
    BOOST_CHECK_CLOSE(a, b, 0.0005f)

#include <iostream>
#include <boost/gil/gil_all.hpp>
#include <boost/gil/extension/toolbox/color_spaces/lab.hpp>

using namespace boost;

BOOST_AUTO_TEST_SUITE(Lab_Test)

BOOST_AUTO_TEST_CASE(Lab_to_XYZ_Test1)
{
    gil::lab32f_pixel_t lab_pixel(40.366198f, 53.354489f, 26.117702f);
    gil::xyz32f_pixel_t xyz_pixel;

    gil::color_convert(lab_pixel, xyz_pixel);

    TEST_CHECK_CLOSE(static_cast<float>(xyz_pixel[0]), 0.197823f);
    TEST_CHECK_CLOSE(static_cast<float>(xyz_pixel[1]), 0.114731f);
    TEST_CHECK_CLOSE(static_cast<float>(xyz_pixel[2]), 0.048848f);
}

BOOST_AUTO_TEST_CASE(Lab_to_XYZ_Test2)
{
    gil::lab32f_pixel_t lab_pixel(50, 0, 0);
    gil::xyz32f_pixel_t xyz_pixel;

    gil::color_convert(lab_pixel, xyz_pixel);

    TEST_CHECK_CLOSE(static_cast<float>(xyz_pixel[0]), 0.175064f);
    TEST_CHECK_CLOSE(static_cast<float>(xyz_pixel[1]), 0.184187f);
    TEST_CHECK_CLOSE(static_cast<float>(xyz_pixel[2]), 0.200548f);
}

BOOST_AUTO_TEST_CASE(XYZ_to_Lab_Test1)
{
    gil::lab32f_pixel_t lab_pixel;
    gil::xyz32f_pixel_t xyz_pixel(0.085703f, 0.064716f, 0.147082f);

    gil::color_convert(xyz_pixel, lab_pixel);

    TEST_CHECK_CLOSE(static_cast<float>(lab_pixel[0]), 30.572438f);
    TEST_CHECK_CLOSE(static_cast<float>(lab_pixel[1]), 23.4674f);
    TEST_CHECK_CLOSE(static_cast<float>(lab_pixel[2]), -22.322275f);
}

BOOST_AUTO_TEST_CASE(RGB_to_Lab_Test1)
{
    gil::rgb32f_pixel_t rgb_pixel(0.75f, 0.5f, 0.25f);
    gil::lab32f_pixel_t lab_pixel;

    gil::color_convert(rgb_pixel, lab_pixel);

    TEST_CHECK_CLOSE(static_cast<float>(lab_pixel[0]), 58.7767f);
    TEST_CHECK_CLOSE(static_cast<float>(lab_pixel[1]), 18.5851f);
    TEST_CHECK_CLOSE(static_cast<float>(lab_pixel[2]), 43.7975f);
}

BOOST_AUTO_TEST_CASE(RGB_to_Lab_Test2)
{
    gil::rgb32f_pixel_t rgb_pixel(1.f, 0.f, 0.f);
    gil::lab32f_pixel_t lab_pixel;

    gil::color_convert(rgb_pixel, lab_pixel);

    TEST_CHECK_CLOSE(static_cast<float>(lab_pixel[0]), 53.2408f);
    TEST_CHECK_CLOSE(static_cast<float>(lab_pixel[1]), 80.0925f);
    TEST_CHECK_CLOSE(static_cast<float>(lab_pixel[2]), 67.2032f);
}

BOOST_AUTO_TEST_CASE(RGB_to_Lab_Test3)
{
    gil::rgb32f_pixel_t rgb_pixel(0.f, 1.f, 0.f);
    gil::lab32f_pixel_t lab_pixel;

    gil::color_convert(rgb_pixel, lab_pixel);

    TEST_CHECK_CLOSE(static_cast<float>(lab_pixel[0]), 87.7347f);
    TEST_CHECK_CLOSE(static_cast<float>(lab_pixel[1]), -86.1827f);
    TEST_CHECK_CLOSE(static_cast<float>(lab_pixel[2]), 83.1793f);
}

BOOST_AUTO_TEST_CASE(RGB_to_Lab_Test4)
{
    gil::rgb32f_pixel_t rgb_pixel(0.f, 0.f, 1.f);
    gil::lab32f_pixel_t lab_pixel;

    gil::color_convert(rgb_pixel, lab_pixel);

    TEST_CHECK_CLOSE(static_cast<float>(lab_pixel[0]), 32.2970f);
    TEST_CHECK_CLOSE(static_cast<float>(lab_pixel[1]), 79.1875f);
    TEST_CHECK_CLOSE(static_cast<float>(lab_pixel[2]), -107.8602f);
}

BOOST_AUTO_TEST_CASE(RGB_to_Lab_Test5)
{
    gil::rgb32f_pixel_t rgb_pixel(1.f, 1.f, 1.f);
    gil::lab32f_pixel_t lab_pixel;

    gil::color_convert(rgb_pixel, lab_pixel);

    TEST_CHECK_CLOSE(static_cast<float>(lab_pixel[0]), 100.f);
    TEST_CHECK_CLOSE(static_cast<float>(lab_pixel[1]), 0.f);
    TEST_CHECK_CLOSE(static_cast<float>(lab_pixel[2]), 0.f);
}

BOOST_AUTO_TEST_CASE(Lab_to_RGB_Test1)
{
    gil::lab32f_pixel_t lab_pixel(75.f, 20.f, 40.f);
    gil::rgb32f_pixel_t rgb_pixel;

    gil::color_convert(lab_pixel, rgb_pixel);

    TEST_CHECK_CLOSE(static_cast<float>(rgb_pixel[0]), 0.943240f);
    TEST_CHECK_CLOSE(static_cast<float>(rgb_pixel[1]), 0.663990f);
    TEST_CHECK_CLOSE(static_cast<float>(rgb_pixel[2]), 0.437893f);
}

BOOST_AUTO_TEST_CASE(Lab_to_RGB_Test2)
{
    gil::lab32f_pixel_t lab_pixel(100.f, 0.f, 0.f);
    gil::rgb32f_pixel_t rgb_pixel;

    gil::color_convert(lab_pixel, rgb_pixel);

    TEST_CHECK_CLOSE(static_cast<float>(rgb_pixel[0]), 1.f);
    TEST_CHECK_CLOSE(static_cast<float>(rgb_pixel[1]), 1.f);
    TEST_CHECK_CLOSE(static_cast<float>(rgb_pixel[2]), 1.f);
}

BOOST_AUTO_TEST_CASE(Lab_to_RGB_Test3)
{
    gil::lab32f_pixel_t lab_pixel(56.8140f, -42.3665f, 10.6728f);
    gil::rgb32f_pixel_t rgb_pixel;

    gil::color_convert(lab_pixel, rgb_pixel);

    TEST_CHECK_CLOSE(static_cast<float>(rgb_pixel[0]), 0.099999f);
    TEST_CHECK_CLOSE(static_cast<float>(rgb_pixel[1]), 0.605568f);
    TEST_CHECK_CLOSE(static_cast<float>(rgb_pixel[2]), 0.456662f);
}

BOOST_AUTO_TEST_CASE(Lab_to_RGB_Test4)
{
    gil::lab32f_pixel_t lab_pixel(50.5874f, 4.0347f, 50.5456f);
    gil::rgb32f_pixel_t rgb_pixel;

    gil::color_convert(lab_pixel, rgb_pixel);

    TEST_CHECK_CLOSE(static_cast<float>(rgb_pixel[0]), 0.582705f);
    TEST_CHECK_CLOSE(static_cast<float>(rgb_pixel[1]), 0.454891f);
    TEST_CHECK_CLOSE(static_cast<float>(rgb_pixel[2]), 0.1f);
}

BOOST_AUTO_TEST_SUITE_END()


#include "global.hpp"
#include "util.hpp"
#include "test_helpers.hpp"

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

BOOST_AUTO_TEST_SUITE(global_test)

BOOST_AUTO_TEST_CASE(pi_test)
{
    BOOST_CHECK_SMALL( std::sin(pi), 1e-15);
    BOOST_CHECK_EQUAL( std::sin(pi / 2), 1.0 );
    BOOST_CHECK_EQUAL( std::cos(pi), -1.0 );
}

BOOST_AUTO_TEST_CASE(pow_int_test)
{
    BOOST_CHECK_EQUAL( pow_int(10, 5), 100000 );
    BOOST_CHECK_EQUAL( pow_int(2u, 30), 1u<<30 );

    /// test overflow protection
    BOOST_CHECK_THROW( pow_int(-1u, 3), std::overflow_error);
}

BOOST_AUTO_TEST_CASE( scale_vector_test )
{
    std::vector<double> test= {1, 4, 3, 6, 3, 6};
    scaleVectorBy(test, 2.0);

    BOOST_CHECK_EQUAL(test[0], 2);
    BOOST_CHECK_EQUAL(test[1], 4*2);
    BOOST_CHECK_EQUAL(test[5], 6*2);
}

BOOST_AUTO_TEST_SUITE_END()


static bool eigen_did_assert = false;
#define eigen_assert(X) if(!eigen_did_assert && !(X)){ std::cout << "### Assertion raised in " << __FILE__ << ":" << __LINE__ << ":\n" #X << "\n### The following would happen without assertions:\n"; eigen_did_assert = true;}

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

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  Matrix2d a, b, c; a << 1,2,3,4; b << 5,6,7,8;
c.noalias() = a * b; // this computes the product directly to c
cout << c << endl;

  return 0;
}


// Copyright John Maddock 2006.
// Copyright Paul A. Bristow 2007, 2009
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifdef _MSC_VER
#  pragma warning(disable : 4127) // conditional expression is constant
#  pragma warning(disable : 4512) // assignment operator could not be generated
#  pragma warning(disable : 4756) // overflow in constant arithmetic
// Constants are too big for float case, but this doesn't matter for test.
#endif

#include <boost/math/concepts/real_concept.hpp>
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/array.hpp>
#include "functor.hpp"

#include "handle_test_result.hpp"
#include "table_type.hpp"

#ifndef SC_
#define SC_(x) static_cast<typename table_type<T>::type>(BOOST_JOIN(x, L))
#endif

template <class Real, class T>
void do_test_hermite(const T& data, const char* type_name, const char* test_name)
{
#if !(defined(ERROR_REPORTING_MODE) && !defined(HERMITE_FUNCTION_TO_TEST))
   typedef Real                   value_type;

   typedef value_type (*pg)(unsigned, value_type);
#ifdef HERMITE_FUNCTION_TO_TEST
   pg funcp = HERMITE_FUNCTION_TO_TEST;
#elif defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
   pg funcp = boost::math::hermite<value_type>;
#else
   pg funcp = boost::math::hermite;
#endif

   boost::math::tools::test_result<value_type> result;

   std::cout << "Testing " << test_name << " with type " << type_name
      << "\n~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n";

   //
   // test hermite against data:
   //
   result = boost::math::tools::test_hetero<Real>(
      data,
      bind_func_int1<Real>(funcp, 0, 1),
      extract_result<Real>(2));
   handle_test_result(result, data[result.worst()], result.worst(), type_name, "hermite", test_name);

   std::cout << std::endl;
#endif
}

template <class T>
void test_hermite(T, const char* name)
{
   //
   // The actual test data is rather verbose, so it's in a separate file
   //
   // The contents are as follows, each row of data contains
   // three items, input value a, input value b and erf(a, b):
   //
#  include "hermite.ipp"

   do_test_hermite<T>(hermite, name, "Hermite Polynomials");
}

template <class T>
void test_spots(T, const char* t)
{
   std::cout << "Testing basic sanity checks for type " << t << std::endl;
   //
   // basic sanity checks, tolerance is 100 epsilon:
   // These spots were generated by MathCAD, precision is
   // 14-16 digits.
   //
   T tolerance = (std::max)(boost::math::tools::epsilon<T>() * 100, static_cast<T>(1e-14));
   BOOST_CHECK_CLOSE_FRACTION(::boost::math::hermite(0, static_cast<T>(1)), static_cast<T>(1.L), tolerance);
   BOOST_CHECK_CLOSE_FRACTION(::boost::math::hermite(1, static_cast<T>(1)), static_cast<T>(2.L), tolerance);
   BOOST_CHECK_CLOSE_FRACTION(::boost::math::hermite(1, static_cast<T>(2)), static_cast<T>(4.L), tolerance);
   BOOST_CHECK_CLOSE_FRACTION(::boost::math::hermite(1, static_cast<T>(10)), static_cast<T>(20), tolerance);
   BOOST_CHECK_CLOSE_FRACTION(::boost::math::hermite(1, static_cast<T>(100)), static_cast<T>(200), tolerance);
   BOOST_CHECK_CLOSE_FRACTION(::boost::math::hermite(1, static_cast<T>(1e6)), static_cast<T>(2e6), tolerance);
   if(std::numeric_limits<T>::max_exponent >= std::numeric_limits<double>::max_exponent)
   {
      BOOST_CHECK_CLOSE_FRACTION(::boost::math::hermite(1, static_cast<T>(1e307)), static_cast<T>(2e307), tolerance);
      BOOST_CHECK_CLOSE_FRACTION(::boost::math::hermite(99, static_cast<T>(100)), static_cast<T>(4.967223743011310E+227L), tolerance);
   }
   BOOST_CHECK_CLOSE_FRACTION(::boost::math::hermite(10, static_cast<T>(30)), static_cast<T>(5.896624628001300E+17L), tolerance);
   BOOST_CHECK_CLOSE_FRACTION(::boost::math::hermite(10, static_cast<T>(1000)), static_cast<T>(1.023976960161280E+33L), tolerance);
   BOOST_CHECK_CLOSE_FRACTION(::boost::math::hermite(10, static_cast<T>(10)), static_cast<T>(8.093278209760000E+12L), tolerance);
   BOOST_CHECK_CLOSE_FRACTION(::boost::math::hermite(10, static_cast<T>(-10)), static_cast<T>(8.093278209760000E+12L), tolerance);
   BOOST_CHECK_CLOSE_FRACTION(::boost::math::hermite(3, static_cast<T>(-10)), static_cast<T>(-7.880000000000000E+3L), tolerance);
   BOOST_CHECK_CLOSE_FRACTION(::boost::math::hermite(3, static_cast<T>(-1000)), static_cast<T>(-7.999988000000000E+9L), tolerance);
   BOOST_CHECK_CLOSE_FRACTION(::boost::math::hermite(3, static_cast<T>(-1000000)), static_cast<T>(-7.999999999988000E+18L), tolerance);
}


#include <limits>
#include <iostream>
#include <iomanip>
#include <string>
#include <cmath>
#include <algorithm>
#include <vector>

// BGL includes
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/cycle_canceling.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>
#include <boost/graph/successive_shortest_path_nonnegative_weights.hpp>
#include <boost/graph/find_flow_cost.hpp>

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

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

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

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

struct Request {
    int si;
    int ti;
    int di;
    int ai;
    int pi;
};

void testcase() {
    long n, s;
    std::cin >> n >> s;
    std::vector<int> l(s);
    long total_cars = 0;
    for(int i = 0; i < s; i++) {
        std::cin >> l[i];
        total_cars += l[i];
    }

    std::vector<std::set<int>> timestamps_per_s(s);

    std::vector<Request> requests(n);
    long max_arrival = 100'000;
    long max_p = 100;
    for(int i = 0; i < n; i++) {
        int si, ti, di, ai, pi;
        std::cin >> si >> ti >> di >> ai >> pi;
        requests[i] = {si - 1, ti - 1, di, ai, pi};
        if(di != 0) timestamps_per_s[si - 1].insert(di);
        if(ai != max_arrival) timestamps_per_s[ti - 1].insert(ai);
        // std::cerr << s * di / 30 + si - 1 << " " << s * ai / 30 + ti - 1 <<  " " << pi << std::endl;
    }

    int sum_dist_nodes = 0;
    std::vector<std::unordered_map<int, int>> map_to_g(s); // map (si, ti) -> j in graph
    for(int i = 0; i < s; i++) {
        map_to_g[i].insert({0, sum_dist_nodes++});
        map_to_g[i].insert({max_arrival, sum_dist_nodes++});
        for(auto t : timestamps_per_s[i]) {
            map_to_g[i].insert({t, sum_dist_nodes++});
            // std::cerr << "i " << i << " " << t  << "\n";
        }
    }
    graph G(sum_dist_nodes);
    const int v_source = boost::add_vertex(G);
    const int v_sink = boost::add_vertex(G);
    edge_adder adder(G);

    for(auto r : requests) {
        int si, ti, di, ai, pi;
        si = r.si; ti = r.ti; di = r.di;
        ai = r.ai; pi = r.pi;
        // std::cerr << -pi + max_p * (ai - di) << std::endl;
        adder.add_edge(map_to_g[si].at(di) , map_to_g[ti].at(ai), 1, -pi + max_p * (ai - di));
    }

    for(int i = 0; i < s; i++) {
        // source to time 0
        adder.add_edge(v_source, map_to_g[i].at(0), l[i], 0);
        adder.add_edge(map_to_g[i].at(max_arrival), v_sink, total_cars, 0);
        // time 0 to first time
        if(timestamps_per_s[i].size() > 0) {
            auto t1 = timestamps_per_s[i].begin();
            // zero to first time
            adder.add_edge(map_to_g[i].at(0), map_to_g[i].at(*t1), total_cars, *t1 * max_p);
            auto t2 = timestamps_per_s[i].begin(); t2++;
            for(; t2 != timestamps_per_s[i].end(); t1++, t2++) {
                adder.add_edge(map_to_g[i].at(*t1), map_to_g[i].at(*t2), total_cars, (*t2 - *t1) * max_p);
            }
            // second to last to max time
            adder.add_edge(map_to_g[i].at(*t1), map_to_g[i].at(max_arrival), total_cars, (max_arrival - *t1) * max_p);
        } else {
            adder.add_edge(map_to_g[i].at(0), map_to_g[i].at(max_arrival), total_cars, (max_arrival) * max_p);
        }
    }

    boost::successive_shortest_path_nonnegative_weights(G, v_source, v_sink);
    long cost = boost::find_flow_cost(G);
    // std::cout << -(cost) << "\n";
    std::cout << -(cost - (total_cars * max_p * max_arrival)) << "\n";

//   // // Retrieve the capacity map and reverse capacity map
//   const auto c_map = boost::get(boost::edge_capacity, G);
//   const auto rc_map = boost::get(boost::edge_residual_capacity, G);

//   // Iterate over all the edges to print the flow along them
//   auto edge_iters = boost::edges(G);
//   std::cerr << v_source << " " << v_sink;
//   for (auto edge_it = edge_iters.first; edge_it != edge_iters.second; ++edge_it) {
//     const edge_desc edge = *edge_it;
//     const long flow_through_edge = c_map[edge] - rc_map[edge];
//     std::cerr << "edge from " << boost::source(edge, G) << " to " << boost::target(edge, G)
//               << " with capacity " << c_map[edge]
//               << " runs " << flow_through_edge
//               << " units of flow (negative for reverse direction). \n";
//   }
    return;
}

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

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


#ifndef CANNON_ML_RECURSIVE_PIECEWISE_LSTD_H
#define CANNON_ML_RECURSIVE_PIECEWISE_LSTD_H

/*!
 * \file cannon/ml/piecewise_recursive_lstd.hpp
 * \brief File containing PiecewiseRecursiveLSTDFilter class definition.
 */

#include <Eigen/Core>
#include <Eigen/Sparse>

using namespace Eigen;

namespace cannon {
  namespace ml {

    /*!
     * \brief Class representing a Least-Squares Temporal Difference (LSTD)
     * approximator using a feature set that produces a piecewise-affine value
     * function approximation. This is for use in reinforcement learning
     * algorithms. This version of the algorithm is similar to a recursive
     * least squares filter.
     */
    class PiecewiseRecursiveLSTDFilter {
      public:
        PiecewiseRecursiveLSTDFilter() = delete;

        /*!
         * \brief Constructor taking state space dimension, number of affine
         * regions, and discount factor.
         */
        PiecewiseRecursiveLSTDFilter(unsigned int in_dim, unsigned int num_refs, double
            discount_factor, double alpha=1.0) : in_dim_(in_dim + 1),
        param_dim_(in_dim_ * num_refs), num_refs_(num_refs),
        discount_factor_(discount_factor), alpha_(alpha) {
          a_inv_ = MatrixXd::Identity(param_dim_, param_dim_) * alpha_;
          theta_ = VectorXd::Zero(param_dim_);
        }

        /*!
         * \brief Update this approximation in light of a single data point,
         * which is a transition from one state to another with associated
         * reward.
         *
         * \param in_vec Feature vector for first state.
         * \param next_in_vec Feature vector for next state.
         * \param idx Region index for first state.
         * \param next_idx Region index for next state.
         * \param reward Reward associated with this state transition.
         */
        void process_datum(const VectorXd& in_vec, const VectorXd& next_in_vec,
            unsigned int idx, unsigned int next_idx, double reward);

        /*!
         * \brief Get the matrix representing the linear portion of the local,
         * affine approximation in the region with for the input index.
         *
         * \param idx Region index
         *
         * \returns Estimated local linear approximation parameter matrix.
         */
        VectorXd get_mat(unsigned int idx) const;

        /*!
         * \brief Predict the value of the input state using the estimated
         * piecewise-affine value function.
         *
         * \param in_vec Feature vector for the state.
         * \param idx Region index of the state.
         *
         * \returns Value function prediction.
         */
        double predict(const VectorXd& in_vec, unsigned int idx) const;

        /*!
         * \brief Reset this value function approximation.
         */
        void reset();

      private:
        /*!
         * \brief Make internal feature vector for the input state in the
         * region with the input index.
         *
         * \param in_vec Input features for state.
         * \param idx Region index for the state.
         *
         * \returns Internal feature representation leading to piecewise-affine
         * function.
         */
        SparseMatrix<double>
        make_feature_vec_(VectorXd in_vec, unsigned int idx) const;

        /*!
         * \brief Update approximation of the linear portion of the LSTD filter
         * given a particular state transition.
         *
         * \param a_inv_feat_t A^-1 * feat.transpose()
         * \param diff feat - (discount_factor_ * next_feat)
         * \param inv_denom 1.0 / (diff * A^-1 * feat^T)
         */
        void update_a_inv_(const Ref<const VectorXd> &a_inv_feat_t,
                           const Ref<const SparseMatrix<double>> &diff,
                           double inv_denom);

        // Parameters
        unsigned int in_dim_; //!< Dimension of input
        unsigned int param_dim_; //!< Dimension of internal feature space
        unsigned int num_refs_; //!< Number of linear regions
        double discount_factor_; //!< Discount factor for value function
        double alpha_; //!< L2 regularization parameter

        // Matrices
        MatrixXd a_inv_; //!< Linear portion of LSTD filter
        VectorXd theta_; //!< Parameters of value function approximation
    };

  } // namespace ml
} // namespace cannon
#endif /* ifndef CANNON_ML_RECURSIVE_PIECEWISE_LSTD_H */