#include <pybind11/eigen.h>
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
namespace py = pybind11;

#include <Eigen/Dense>
#include <MeshFEM/ElasticityTensor.hh>
#include <MeshFEM/Fields.hh>
#include <MeshFEM/SymmetricMatrix.hh>
#include <MeshFEM/Materials.hh>
#include <MeshFEM/VonMises.hh>
#include <MeshFEM/Utilities/NameMangling.hh>

template<typename _Real, size_t N>
void bindTensors(py::module& module, py::module& detail_module) {
    using ETensor = ElasticityTensor    <_Real, N>;
    using SMValue = SymmetricMatrixValue<_Real, N>;
    using SMF     = SymmetricMatrixField<_Real, N>;

    auto py_et = py::class_<ETensor>(module, NameMangler<ETensor>::name().c_str())
        .def(py::init<>())
        .def(py::init([](const std::string& material_file) { return Materials::Constant<N>(material_file).getTensor(); }), py::arg("material_file"))
        .def(py::init<_Real, _Real>(), py::arg("E"), py::arg("nu"))
        .def("setIsotropic", &ETensor::setIsotropic, py::arg("E"), py::arg("nu"))
        .def("setIdentity",  &ETensor::setIdentity)

        .def("getOrthotropicParameters", py::overload_cast<>(&ETensor::getOrthotropicParameters, py::const_))
        .def("anisotropy", &ETensor::anisotropy)

        .def("__call__", [](const ETensor &E, size_t i, size_t j, size_t k, size_t l) {
                    if ((i >= N) || (j >= N) || (k >= N) || (l >= N))
                        throw std::runtime_error("Index out of bounds");
                    return E(i, j, k, l);
                })
        .def_property_readonly("D", [](const ETensor &E) {
                    typename ETensor::DType D;
                    for (int i = 0; i < D.rows(); ++i) {
                        for (int j = 0; j < D.cols(); ++j) {
                            D(i, j) = E.D(i, j);
                        }
                    }
                    return D;
                })
        .def("doubleContract", [](const ETensor &E, const SMValue &smat) { return E.doubleContract(smat); }, py::arg("smat"))
        .def("doubleContract", [](const ETensor &E, const SMF     & smf) {
                    SMF result(smf.domainSize());
                    for (size_t i = 0; i < smf.domainSize(); ++i)
                        result(i) = E.doubleContract(smf(i));
                    return result;
                }, py::arg("smat"))
        // .def("doubleContract",    [](const ETensor &E, const ETensor &Eother) { return E.doubleContract(Eother);    }, py::arg("E")) // this produces a non-major-symmetric result, which we haven't bound yet
        .def("quadrupleContract", [](const ETensor &E, const ETensor &Eother) { return E.quadrupleContract(Eother); }, py::arg("E"))

        .def("computeEigenstrains", &ETensor::computeEigenstrains)

        .def("inverse",         &ETensor::inverse)
        .def("pseudoinverse",   &ETensor::inverse)
        .def("frobeniusNormSq", &ETensor::frobeniusNormSq)
        .def("transform",       &ETensor::transform, py::arg("R"), "Apply a *orthogonal* change of coordinates to this tensor")
        .def("__sub__", [](const ETensor &E, const ETensor &Eother) { return E - Eother; })
        .def("__repr__", [](const ETensor &E) {
                std::stringstream ss;
                ss << N << "D elasticity tensor with orthotropic moduli: ";
                E.printOrthotropic(ss);
                return ss.str(); })
        ;

    if (N == 3) {
        py_et.def("setOrthotropic",
            &ETensor::setOrthotropic3D,
            py::arg("Ex"),   py::arg("Ey"),   py::arg("Ez"),
            py::arg("nuYX"), py::arg("nuZX"), py::arg("nuZY"),
            py::arg("muYZ"), py::arg("myZX"), py::arg("muXY"));
    }

    if (N == 2) {
        py_et.def("setOrthotropic",
            &ETensor::setOrthotropic2D,
            py::arg("Ex"),   py::arg("Ey"),
            py::arg("nuYX"), py::arg("muXY"));
    }

    py::class_<SMValue>(detail_module, NameMangler<SMValue>::name().c_str())
        .def("__call__", [](const SMValue &sm, size_t i, size_t j) { if ((i >= N) || (j >= N)) throw std::runtime_error("Index out of bounds"); return sm(i, j); }, py::arg("i"), py::arg("j"))
        .def("toMatrix", [](const SMValue &sm) { return sm.toMatrix(); })
        .def("eigenvalues",        &SMValue::eigenvalues)
        .def("eigenDecomposition", &SMValue::eigenDecomposition)
        ;

    py::class_<SMF>(detail_module, ("SymmetricMatrixField" + std::to_string(N) + "D" + floatingPointTypeSuffix<_Real>()).c_str())
        .def("vonMises", [](const SMF &smf) {
                SMF smf_vm = vonMises(smf);
                Eigen::VectorXd result(smf_vm.domainSize());
                for (size_t i = 0; i < smf_vm.domainSize(); ++i)
                    result[i] = std::sqrt(smf_vm(i).frobeniusNormSq());
                return result;
            })
        .def("eigendecomposition", [](const SMF &smf) {
                std::vector<SMEigenDecompositionType<_Real, N>> result;
                result.reserve(smf.domainSize());
                for (size_t i = 0; i < smf.domainSize(); ++i)
                    result.push_back(smf(i).eigenDecomposition());
                return result;
            })
        .def("__call__", [](const SMF &smf, size_t i) { if (i >= smf.domainSize()) throw std::runtime_error("Index out of bounds."); return SMValue(smf(i)); })
        ;

    module.def("SymmetricMatrix", [](const Eigen::Matrix<_Real, flatLen(N), 1> &flatValues) { return SMValue(flatValues); }, py::arg("flatValues"));
    module.def("SymmetricMatrix", [](const Eigen::Matrix<_Real, N, N>          &mat)        { return SMValue(       mat); }, py::arg("mat"));
}

template<typename _Real>
void addBindings(py::module &m) {
    py::module detail_module = m.def_submodule("detail");
    py::class_<ETensorEigenDecomposition>(detail_module, "ETensorEigenDecomposition")
        .def_readonly("eigenstrains", &ETensorEigenDecomposition::strains) // flattened symmetric matrix field
        .def_readonly("eigenvalues",  &ETensorEigenDecomposition::lambdas)
        ;
    bindTensors<_Real, 2>(m, detail_module);
    bindTensors<_Real, 3>(m, detail_module);
}

PYBIND11_MODULE(tensors, m) {
    m.doc() = "Tensors and tensor fields used for elasticity simulations";

    addBindings<double>(m);
}


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

  @copyright 2015 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_SCALAR_FUNCTION_REM_PIO2_MEDIUM_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_REM_PIO2_MEDIUM_HPP_INCLUDED

#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>
#include <boost/simd/function/nearbyint.hpp>
#include <boost/simd/function/toint.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/three.hpp>
#include <boost/simd/detail/constant/pio2_1.hpp>
#include <boost/simd/detail/constant/pio2_1t.hpp>
#include <boost/simd/detail/constant/pio2_2.hpp>
#include <boost/simd/detail/constant/pio2_2t.hpp>
#include <boost/simd/constant/pio2_3.hpp>
#include <boost/simd/detail/constant/pio2_3t.hpp>
#include <boost/simd/constant/twoopi.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/bitwise_and.hpp>
#include <boost/simd/function/quadrant.hpp>
#include <utility>

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

  BOOST_DISPATCH_OVERLOAD (rem_pio2_medium_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_ < bd::floating_<A0> >
                          )
  {
    using result_t = std::pair<A0, A0>;
    BOOST_FORCEINLINE result_t operator() ( A0 t) const
    {
      const A0 fn = nearbyint(t*Twoopi<A0>());
      A0 r  = t-fn*Pio2_1<A0>();
      A0 w  = fn*Pio2_1t<A0>();
      A0 t2 = r;
      w  = fn*Pio2_2<A0>();
      r  = t2-w;
      w  = fn*Pio2_2t<A0>()-((t2-r)-w);
      t2 = r;
      w  = fn*Pio2_3<A0>();
      r  = t2-w;
      w  = fn*Pio2_3t<A0>()-((t2-r)-w);
      return  {quadrant(fn), r-w};
    }
  };

} } }

#endif


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

// Written by Cornelius Steinhardt

#ifndef ITL_TFQMR_INCLUDE
#define ITL_TFQMR_INCLUDE

#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/linear_algebra/inverse.hpp>
#include <boost/numeric/mtl/utility/irange.hpp>
#include <boost/numeric/mtl/operation/resource.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

#include <boost/numeric/itl/krylov/base_solver.hpp>

namespace itl {

/// Transposed-free Quasi-minimal residual
template < typename Matrix, typename Vector,
	   typename LeftPreconditioner, typename RightPreconditioner, typename Iteration >
int tfqmr(const Matrix &A, Vector &x, const Vector &b, const LeftPreconditioner &L,
	  const RightPreconditioner &R, Iteration& iter)
{
    mtl::vampir_trace<7009> tracer;
    using math::reciprocal; using mtl::size;
    typedef typename mtl::Collection<Vector>::value_type Scalar;

    if (size(b) == 0) throw mtl::logic_error("empty rhs vector");

    const Scalar                zero= math::zero(Scalar()), one= math::one(Scalar());
    Scalar                      theta(zero), eta(zero), tau, rho, rhon, sigma, alpha, beta, c;
    Vector                      rt(b - A*Vector(solve(R, x))) /* shift x= R*x */, r(solve(L, rt)), u1(resource(x)), u2(resource(x)),
                                y1(resource(x)), y2(resource(x)), w(resource(x)), d(resource(x), zero), v(resource(x));

    if (iter.finished(rt))
	return iter;
    y1= w= r;
    rt= A * Vector(solve(R, y1));
    u1= v= solve(L,rt);
    tau= two_norm(r);
    rho= tau * tau;

    // TFQMR iteration
    while (! iter.finished(tau)) {
	++iter;
	sigma= dot(r,v);
        if (sigma == zero)
	    return iter.fail(1, "tfgmr breakdown, sigma=0 #1");
        alpha= rho / sigma;

        // inner loop
        for(int j=1; j < 3; j++) {
            if (j == 1) {
                w-= alpha * u1;
                d= y1+ (theta * theta * eta / alpha) * d;
	    } else {
                y2= y1 - alpha * v;
                rt= A * Vector(solve(R, y2));
                u2= solve(L, rt);
                w-= alpha * u2;
                d= y2 + (theta * theta * eta / alpha) * d;
            }
            theta= two_norm(w) / tau;
            c= reciprocal(sqrt(one + theta*theta));
            tau*= theta * c;
            eta= c * c * alpha;
            x+= eta * d;
        } // end inner loop
        if (rho == zero)
            return iter.fail(1, "tfgmr breakdown, rho=0 #2");
        rhon= dot(r,w);
        beta= rhon/rho;
        rho= rhon;
        y1= w + beta*y2;
        rt= A * Vector(solve(R, y1));
        u1= solve(L, rt);
        v= u1 + beta*(u2 + beta*v);
        rt= A * x - b;
    }
    //shift back
    x= solve(R, x);
    return iter;
}

/// Solver class for Transposed-free quasi-minimal residual method; right preconditioner ignored (prints warning if not identity)
/** Methods inherited from \ref base_solver. **/
template < typename LinearOperator, typename Preconditioner= pc::identity<LinearOperator>,
	   typename RightPreconditioner= pc::identity<LinearOperator> >
class tfqmr_solver
  : public base_solver< tfqmr_solver<LinearOperator, Preconditioner, RightPreconditioner>, LinearOperator >
{
    typedef base_solver< tfqmr_solver<LinearOperator, Preconditioner, RightPreconditioner>, LinearOperator > base;
  public:
    /// Construct solver from a linear operator; generate (left) preconditioner from it
    explicit tfqmr_solver(const LinearOperator& A) : base(A), L(A), R(A) {}

    /// Construct solver from a linear operator and left preconditioner
    tfqmr_solver(const LinearOperator& A, const Preconditioner& L) : base(A), L(L), R(A) {}

    /// Construct solver from a linear operator and left preconditioner
    tfqmr_solver(const LinearOperator& A, const Preconditioner& L, const RightPreconditioner& R)
      : base(A), L(L), R(R) {}

    /// Solve linear system approximately as specified by \p iter
    template < typename HilbertSpaceX, typename HilbertSpaceB, typename Iteration >
    int solve(HilbertSpaceX& x, const HilbertSpaceB& b, Iteration& iter) const
    {
	return tfqmr(this->A, x, b, L, R, iter);
    }

  private:
    Preconditioner        L;
    RightPreconditioner   R;
};

} // namespace itl

#endif // ITL_TFQMR_INCLUDE


#include <vector>
#include <cfloat>

#include <boost/foreach.hpp>

#include "types.hpp"
#include "track.hpp"

using namespace std;

/**
 * iterates over all possible wire_pos_ptr combinations
 * if next combination is not available returns false
 */
bool	next_combination( vector<wire_pos_ptr_t> &wire_pos_ptr, const vector<int> &wire_count )
{
	const int chambers_count = wire_count.size();
	int	i = 0;

	while(1 + wire_pos_ptr[i] == wire_count[i])
	{
		i++;

		if (i == chambers_count)
		{
			/* reached last combination */
			return false;
		}
	}

	for(int j = 0; j < i; j++)
	{
		wire_pos_ptr[j] = 0;
	}

	wire_pos_ptr[i]++;

	return true;
}

/**
 * Removes items from data that haven't got any wires left in them.
 * @param data - data[i] is a pointer to list of wires for i-th chamber
 * @return false if there is no enough non-empty chambers left
 */
bool	delete_empty_chambers( vector< vector<wire_pos_t>* > &data, vector<double> &normal_pos, vector<uint> &used_chambers )
{
	uint	empty_chambers_count = 0;
	auto	data_it = data.begin();
	auto	normal_pos_it = normal_pos.begin();
	auto	used_chambers_it = used_chambers.begin();

	// check if there is enough non-empty chambers
	while(data_it != data.end())
	{
		if ((*data_it)->empty())
		{
			empty_chambers_count++;

			if (data.size() <= MIN_TRACK_CHAMBERS)
			{
				return false;
			}

			data_it = data.erase(data_it);
			normal_pos_it = normal_pos.erase(normal_pos_it);
			used_chambers_it = used_chambers.erase(used_chambers_it);
		}
		else
		{
			data_it++;
			normal_pos_it++;
			used_chambers_it++;
		}
	}

	return true;
}

track_info_t	reconstruct_track( const vector< vector<wire_pos_t>* > &data, const vector<double> &normal_pos )
{
	const chamber_id_t	chambers_count = data.size();
	vector<wire_pos_ptr_t>	best_wire_pos_ptr(chambers_count);
	vector<wire_pos_ptr_t>	wire_pos_ptr(chambers_count);
	vector<int>		wire_count(chambers_count);

	// initialize variables
	{
		int chamber_id = 0;

		BOOST_FOREACH(auto chamber_data, data)
		{
			wire_pos_ptr[chamber_id] = 0;
			wire_count[chamber_id] = chamber_data->size();

			chamber_id++;
		}
	}

	double	best_c0, best_c1;
	double	best_sumsq, prev_best_sumsq = FLT_MAX;
	vector<double>	best_wires_pos;
	bool	first = true;

	do
	{
		vector<double>	wires;
		double	c0, c1, sumsq;
		int i = 0;

		// fill array with values to fit
		BOOST_FOREACH(auto chamber_data, data)
		{
			wire_pos_t	wire_id = (*chamber_data)[wire_pos_ptr[i]];

			wires.push_back(wire_id);

			i++;
		}

		// perform linear fit
		{
			double	m_x = 0;
			double	m_y = 0;
			double	m_dxdy = 0;
			double	m_dxdx = 0;
			int		N = normal_pos.size();

			BOOST_FOREACH(double x, normal_pos)
			{
				m_x += x;
			}
			m_x /= N;

			BOOST_FOREACH(double y, wires)
			{
				m_y += y;
			}
			m_y /= N;

			auto	xit = normal_pos.begin();
			auto	yit = wires.begin();
			while(xit != normal_pos.end())
			{
				m_dxdy += (*xit - m_x) * (*yit - m_y);
				m_dxdx += (*xit - m_x) * (*xit - m_x);
				xit++; yit++;
			}
			m_dxdy /= N;
			m_dxdx /= N;

			c1 = m_dxdy / m_dxdx;
			c0 = m_y - c1 * m_x;

			xit = normal_pos.begin();
			yit = wires.begin();
			sumsq = 0;
			while(xit != normal_pos.end())
			{
				double	v = *yit - (c0 + *xit * c1);
				sumsq += v * v;
				xit++; yit++;
			}
		}

		if (first || (best_sumsq > sumsq))
		{
			prev_best_sumsq = best_sumsq;
			best_sumsq = sumsq;
			best_c0 = c0;
			best_c1 = c1;
			best_wires_pos = wires;
			first = false;

			// save best wires array positions
			for(chamber_id_t chamber_id = 0; chamber_id < chambers_count; chamber_id++)
			{
				best_wire_pos_ptr[chamber_id] = wire_pos_ptr[chamber_id];
			}
		}
	}
	while(next_combination(wire_pos_ptr, wire_count));

	return track_info_t({
			best_c0, best_c1, best_sumsq, prev_best_sumsq, best_wires_pos, best_wire_pos_ptr
				});
}

/**
 * @warning This function deletes wires that lie on the reconstructd track.
 */
template<track_type_t track_type>
vector<track_info_t>	reconstruct_all_tracks( vector< vector<wire_pos_t>* > data, vector<double> normal_pos, double max_chisq )
{
	vector<uint>	used_chambers;
	vector<track_info_t>	result;

	for(uint i = 0; i < data.size(); i++)
	{
		used_chambers.push_back(i);
	}

	// check if there is enough chambers
	if (data.size() < MIN_TRACK_CHAMBERS)
	{
		throw "Not enough chambers in original data!";
	}

	while(delete_empty_chambers(data, normal_pos, used_chambers))
	{
		track_info_t	track = reconstruct_track(data, normal_pos);

		track.used_chambers = used_chambers;

		if ((max_chisq > 0) && (track.chisq > max_chisq))
		{
			break;
		}

		result.push_back(track);

		chamber_id_t	chamber_id = 0;

		BOOST_FOREACH(auto chamber_data, data)
		{
			wire_pos_ptr_t	deletion_pos = track.wire_pos_ptr[chamber_id];

			// now delete wires that compose the reconstructd track
			if (track_type == track_type_t::prop)
			{
				chamber_data->erase(chamber_data->begin() + deletion_pos);
			}
			else if (track_type == track_type_t::drift)
			{
				// for drift chamber point consists of the left and right side
				// so we need to remove both
				deletion_pos &= ~1;
				chamber_data->erase(chamber_data->begin() + deletion_pos,
				                    chamber_data->begin() + deletion_pos + 2);
			}

			chamber_id++;
		}
	}

	return result;
}

template vector<track_info_t> reconstruct_all_tracks<track_type_t::prop>( vector< vector<wire_pos_t>* > data, vector<double> normal_pos, double max_chisq );
template vector<track_info_t> reconstruct_all_tracks<track_type_t::drift>( vector< vector<wire_pos_t>* > data, vector<double> normal_pos, double max_chisq );


#include <iostream>
#include <fstream>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include "headers/util.h"
#include "headers/rmat.h"
#include "headers/matutil.h"
#include "headers/matrix_wrapper.h"
#include "headers/timer.h"

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

int main(int argc, char** argv) {
  uint64_t n = atol(argv[1]);
  uint64_t m = atol(argv[2]);

  RmatConfig cfg(0.57, 0.19, 0.19);
  CustomMatrix *mat = new CustomMatrix(n);
  timer t;
  t.start();
  rmat<CustomMatrix>(mat, m, cfg);
  t.stop();
  cout << "Took: " << t.get_total() << endl;

  // timer t;
  // t.start();
  // list<Edge> l = listRmat(n, m, cfg);
  // t.stop();
  // cout << "Took: " << t.get_total() << endl;

  // fs.open("data/gen-32bit_uniform.csv", ios::trunc);
  // MatrixWrapper<CustomMatrix> *w = new MatrixWrapper<CustomMatrix>(1024, 1024, 0, true);
  // ofstream fs;
  // timer t;

  // RmatConfig cfg;
  // cout << "Starting now" << endl;

  // rmat<MatrixWrapper<CompAdjMat>>(w, 50000, cfg);
  // rmatSeq<AdjMatrix>(w, 25571, cfg);
  // rmat<CustomMatrix>(x, 5000, cfg);
  // rmatSeq<CustomMatrix>(x, 5000, cfg);
  // cout << "Finished generating graph, writing to file..." << endl;
  // will::matutil::writeAdjMatrix(*w, fs);
  // will::matutil::writeAdjMatrix(*x, fs);
  // cout << "Done!" << endl;

  // {
  //   timer t;
  //   MatrixWrapper<AdjMatrix> *w = new MatrixWrapper<AdjMatrix>(50000, 50000);
  //   RmatConfig cfg(0.57, 0.19, 0.19);
  //   t.start();
  //   rmatSeq<AdjMatrix>(w, 505571000, cfg);
  //   t.stop();
  //   cout << w->getMat() << endl;
  //   cout << "Took: " << t.get_total() << endl;
  // }

  // {
  //   timer t;
  //   RmatConfig cfg(0.57, 0.19, 0.19);
  //   t.start();
  //   std::list<Edge> res = listRmat(30000, 50557100, cfg);
  //   t.stop();
  //   cout << res.size() << endl;
  //   cout << "Took: " << t.get_total() << endl;
  // }

  // will::matutil::writeAdjMatrix(*w, std::cout);
  // std::cout << m << std::endl;
  // MatrixWrapper<CompAdjMat> w(10, 10);
  // w.insert(3, 3, 9);
  // w.set(1, 1, 9);
  // cout << static_cast<int>(w.get(3, 3)) << endl;
  // m = w.getMat();
  // will::matutil::writeAdjMatrix(*m, std::cout);
}


#include <iostream>
#include <cassert>
#include <vector>
#include <iomanip>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/successive_shortest_path_nonnegative_weights.hpp>
#include <boost/graph/find_flow_cost.hpp>

typedef boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> GraphTraits;
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, GraphTraits::edge_descriptor,
                                                                              boost::property<boost::edge_weight_t, long>>>>>
    Graph;

const int debug_level = 0;

#define DEBUG(min_level, x)      \
  if (debug_level >= min_level)  \
  {                              \
    std::cerr << x << std::endl; \
  }

struct Booking
{
  int s, t, d, a, p;
};

const int max_possible_profit = 100;

class TimeIndexer
{
public:
  void add_time(int t)
  {
    time_set.insert(t);
  }

  void rebuild()
  {
    time_vector.resize(time_set.size());
    std::copy(time_set.begin(), time_set.end(), time_vector.begin());
  }

  int time_to_index(int t) const
  {
    const auto it = std::lower_bound(time_vector.begin(), time_vector.end(), t);
    assert(it != time_vector.end() && *it == t);
    return it - time_vector.begin();
  }

  int index_to_time(int i) const
  {
    return time_vector.at(i);
  }

  int size() const
  {
    return time_set.size();
  }

private:
  std::set<int> time_set;
  std::vector<int> time_vector;
};

void testcase()
{
  int n, s;
  std::cin >> n >> s;
  assert(n >= 1 && n <= 10000 && s >= 2 && s <= 10);

  int total_cars = 0;
  std::vector<int> initial_cars_by_station(s);
  for (int &l : initial_cars_by_station)
  {
    std::cin >> l;
    assert(l >= 0 && l <= 100);
    total_cars += l;
  }

  std::vector<Booking> bookings(n);
  for (Booking &b : bookings)
  {
    std::cin >> b.s >> b.t >> b.d >> b.a >> b.p;
    assert(b.s >= 1 && b.s <= s && b.t >= 1 && b.t <= s);
    b.s--;
    b.t--;
    assert(b.d >= 0 && b.d < b.a && b.a <= 100000);
    assert(b.p >= 1 && b.p <= max_possible_profit);
  }

  TimeIndexer global_time_indexer;
  std::vector<TimeIndexer> local_time_indexers(s);
  for (Booking &b : bookings)
  {
    global_time_indexer.add_time(b.d);
    global_time_indexer.add_time(b.a);
    local_time_indexers.at(b.s).add_time(b.d);
    local_time_indexers.at(b.t).add_time(b.a);
  }

  global_time_indexer.rebuild();
  for (TimeIndexer &local_time_indexer : local_time_indexers)
  {
    // IMPORTANT The first and last global time must exist at every station, otherwise
    // the max_possible_profit * global_time_slot_delta compensation won't work correctly
    local_time_indexer.add_time(global_time_indexer.index_to_time(0));
    local_time_indexer.add_time(global_time_indexer.index_to_time(global_time_indexer.size() - 1));

    local_time_indexer.rebuild();
  }

  std::vector<int> local_time_slot_cumulative_sum{0};
  for (int i = 0; i < s; i++)
  {
    local_time_slot_cumulative_sum.push_back(local_time_slot_cumulative_sum.back() + local_time_indexers.at(i).size());
  }

  int next_free_node = 0;
  const int node_source = next_free_node++;
  const int node_target = next_free_node++;
  const auto get_station_time_node = [next_free_node, s, &local_time_indexers, &local_time_slot_cumulative_sum](const int i_time, const int i_station) {
    assert(i_station >= 0 && i_station < s && i_time >= 0 && i_time < local_time_indexers.at(i_station).size());
    return next_free_node + local_time_slot_cumulative_sum.at(i_station) + i_time;
  };
  next_free_node += local_time_slot_cumulative_sum.back();
  const auto get_trip_node = [next_free_node, n](const int i_trip) {
    assert(i_trip >= 0 && i_trip < n);
    return next_free_node + i_trip;
  };
  next_free_node += n;
  const int num_nodes = next_free_node;
  Graph G(num_nodes);

  const auto add_edge = [&G](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);
    const Graph::edge_descriptor e = boost::add_edge(from, to, G).first;
    const Graph::edge_descriptor rev_e = boost::add_edge(to, from, G).first;
    c_map[e] = capacity;
    c_map[rev_e] = 0;
    r_map[e] = rev_e;
    r_map[rev_e] = e;
    w_map[e] = cost;
    w_map[rev_e] = -cost;
  };

  for (int i_station = 0; i_station < s; i_station++)
  {
    const TimeIndexer &local_time_indexer = local_time_indexers.at(i_station);

    add_edge(node_source, get_station_time_node(0, i_station), initial_cars_by_station.at(i_station), 0);
    add_edge(get_station_time_node(local_time_indexer.size() - 1, i_station), node_target, total_cars, 0);

    for (int i_time = 1; i_time < local_time_indexer.size(); i_time++)
    {
      const int global_time_slot_delta = global_time_indexer.time_to_index(local_time_indexer.index_to_time(i_time)) - global_time_indexer.time_to_index(local_time_indexer.index_to_time(i_time - 1));
      assert(global_time_slot_delta > 0);
      add_edge(get_station_time_node(i_time - 1, i_station), get_station_time_node(i_time, i_station), total_cars, global_time_slot_delta * max_possible_profit);
    }
  }

  for (int i_trip = 0; i_trip < n; i_trip++)
  {
    Booking &b = bookings.at(i_trip);
    const int i_d = local_time_indexers.at(b.s).time_to_index(b.d), i_a = local_time_indexers.at(b.t).time_to_index(b.a);
    const int global_time_slot_delta = global_time_indexer.time_to_index(b.a) - global_time_indexer.time_to_index(b.d);
    assert(global_time_slot_delta > 0);
    add_edge(get_station_time_node(i_d, b.s), get_trip_node(i_trip), 1, max_possible_profit * global_time_slot_delta - b.p);
    add_edge(get_trip_node(i_trip), get_station_time_node(i_a, b.t), 1, 0);
  }

  boost::successive_shortest_path_nonnegative_weights(G, node_source, node_target);
  const int flow_cost = boost::find_flow_cost(G);
  const int total_profit = total_cars * (global_time_indexer.size() - 1) * max_possible_profit - flow_cost;
  std::cout << total_profit << "\n";
}

int main()
{
  std::ios_base::sync_with_stdio(false);
  std::cout << std::fixed << std::setprecision(0);

  int t;
  std::cin >> t;
  for (int i = 0; i < t; i++)
  {
    testcase();
    DEBUG(1, "");
  }

  return 0;
}


#include <deal.II/base/function.h>
#include <deal.II/base/function_parser.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/parameter_handler.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/timer.h>
#include <deal.II/base/table_handler.h>

#include <deal.II/lac/constraint_matrix.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/petsc_parallel_sparse_matrix.h>
#include <deal.II/lac/petsc_parallel_vector.h>
#include <deal.II/lac/petsc_solver.h>
#include <deal.II/lac/petsc_precondition.h>
#include <deal.II/lac/sparsity_tools.h>
#include <deal.II/lac/vector.h>

#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_out.h>
#include <deal.II/grid/grid_refinement.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>

#include <deal.II/distributed/grid_refinement.h>
#include <deal.II/distributed/tria.h>

#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/dofs/dof_tools.h>

#include <deal.II/fe/fe_values.h>
#include <deal.II/fe/fe_system.h>
#include <deal.II/fe/fe_q.h>

#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/error_estimator.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/vector_tools.h>

#include <mandy/elastic_tensor.h>
#include <mandy/lattice_tensor.h>
#include <mandy/matrix_creator.h>
#include <mandy/vector_creator.h>

#include <mandy/crystal_symmetry_group.h>

#include <fstream>
#include <iostream>

#include <algorithm>    // std::transform
#include <functional>   // std::plus

namespace mandy
{

  /**
   * Solve the system Mx=b, where M is the mass matrix and b is a
   * linear function.
   */
  template <int dim>
  class MaterialID
  {
  public:

    /**
     * Class constructor.
     */
    MaterialID (const std::string &prm);

    /**
     * Class destructor.
     */
    ~MaterialID ();

    /**
     * Wrapper function, that controls the order of excecution.
     */
    void run ();

  private:

    /**
     * Make intial coarse grid.
     */
    void make_coarse_grid ();

    /**
     * Setup system matrices and vectors.
     */
    void setup_system ();

    /**
     * Assemble system matrices and vectors.
     */
    void assemble_system();

    /**
     * Solve the linear algebra system.
     */
    unsigned int solve ();

    /**
     * Output results, ie., finite element functions and derived
     * quantitites for this cycle.
     */
    void output_results (const unsigned int cycle);

    /**
     * Refine grid based on Kelly's error estimator working on the
     * material id (solution vector).
     */
    void refine_grid ();

    /**
     * MPI communicator.
     */
    MPI_Comm mpi_communicator;

    /**
     * A distributed grid on which all computations are done.
     */
    dealii::parallel::distributed::Triangulation<dim> triangulation;

    /**
     * Scalar DoF handler primarily used for interpolating material
     * identification.
     */
    dealii::DoFHandler<dim> dof_handler;

    /**
     * Scalar valued finite element primarily used for interpolating
     * material iudentification.
     */
    dealii::FESystem<dim> fe;

    /**
     * Index set of locally owned DoFs.
     */
    dealii::IndexSet locally_owned_dofs;

    /**
     * Index set of locally relevant DoFs.
     */
    dealii::IndexSet locally_relevant_dofs;

    /**
     * A list of (hanging node) constraints.
     */
    dealii::ConstraintMatrix constraints;

    /**
     * System matrix - a mass matrix.
     */
    dealii::PETScWrappers::MPI::SparseMatrix system_matrix;

    /**
     * Locally relevant solution vector.
     */
    dealii::PETScWrappers::MPI::Vector locally_relevant_solution;

    /**
     * System right hand side function - interpolated function.
     */
    dealii::PETScWrappers::MPI::Vector system_rhs;

    /**
     * Parallel iostream.
     */
    dealii::ConditionalOStream pcout;

    /**
     * Stop clock.
     */
    dealii::TimerOutput timer;

    /**
     * Input parameter file.
     */
    dealii::ParameterHandler parameters;

  }; // MaterialID

  /**
   * Class constructor.
   */
  template <int dim>
  MaterialID<dim>::MaterialID (const std::string &prm)
    :
    mpi_communicator (MPI_COMM_WORLD),
    triangulation (mpi_communicator,
                   typename dealii::Triangulation<dim>::MeshSmoothing
                   (dealii::Triangulation<dim>::smoothing_on_refinement |
                    dealii::Triangulation<dim>::smoothing_on_coarsening)),
    dof_handler (triangulation),
    fe (dealii::FE_Q<dim> (2), 1),
    // ---
    pcout (std::cout, (dealii::Utilities::MPI::this_mpi_process (mpi_communicator) == 0)),
    timer (mpi_communicator, pcout,
	   dealii::TimerOutput::summary,
	   dealii::TimerOutput::wall_times)
  {
    parameters.declare_entry ("Global mesh refinement steps", "5",
                              dealii::Patterns::Integer (0, 20),
                              "The number of times the 1-cell coarse mesh should "
                              "be refined globally for our computations.");

    parameters.declare_entry ("MaterialID", "0",
                              dealii::Patterns::Anything (),
                              "A functional description of the material ID.");

    parameters.parse_input (prm);
  }

  /**
   * Class destructor.
   */
  template <int dim>
  MaterialID<dim>::~MaterialID ()
  {
    // Wipe DoF handlers.
    dof_handler.clear ();
  }

  /**
   * Make initial coarse grid.
   */
  template <int dim>
  void
  MaterialID<dim>::make_coarse_grid ()
  {
    dealii::TimerOutput::Scope time (timer, "make coarse grid");

    // Create a coarse grid according to the parameters given in the
    // input file.
    dealii::GridGenerator::hyper_cube (triangulation, -10, 10);

    triangulation.refine_global (parameters.get_integer ("Global mesh refinement steps"));
  }

  /**
   * Setup system matrices and vectors.
   */
  template <int dim>
  void MaterialID<dim>::setup_system ()
  {
    dealii::TimerOutput::Scope time (timer, "setup system");

    // Determine locally relevant DoFs.
    dof_handler.distribute_dofs (fe);
    locally_owned_dofs = dof_handler.locally_owned_dofs ();
    dealii::DoFTools::extract_locally_relevant_dofs (dof_handler, locally_relevant_dofs);

    // Initialise distributed vectors.
    locally_relevant_solution.reinit (locally_owned_dofs, locally_relevant_dofs,
				      mpi_communicator);
    system_rhs.reinit (locally_owned_dofs,
		       mpi_communicator);

    // Setup hanging node constraints.
    constraints.clear ();
    constraints.reinit (locally_relevant_dofs);
    dealii::DoFTools::make_hanging_node_constraints (dof_handler, constraints);
    constraints.close ();

    // Finally, create a distributed sparsity pattern and initialise
    // the system matrix from that.
    dealii::DynamicSparsityPattern dsp (locally_relevant_dofs);
    dealii::DoFTools::make_sparsity_pattern (dof_handler, dsp, constraints, false);
    dealii::SparsityTools::distribute_sparsity_pattern (dsp,
							dof_handler.n_locally_owned_dofs_per_processor (),
							mpi_communicator,
							locally_relevant_dofs);

    system_matrix.reinit (locally_owned_dofs, locally_owned_dofs,
                          dsp, mpi_communicator);

  }

  /**
   * Assemble system matrices and vectors.
   *
   * TODO Ideally, we would use a function like this:
   *
   * dealii::MatrixCreator::create_mass_matrix (dof_handler, quadrature_rule, system_matrix, 1, constraints);
   *
   * however no such thing currently exists in the deal.II library for
   * parallel matrices and vectors. Instead, the mass matrix and right
   * hand side vector are assembled by hand in functions defined in
   * the namepsaces, mandy::MatrixCreator and mandy::VectorCreator,
   * respectively.
   */
  template <int dim>
  void
  MaterialID<dim>::assemble_system ()
  {
    dealii::TimerOutput::Scope time (timer, "assemble system");

    // Define quadrature rule to be used.
    const dealii::QGauss<dim> quadrature_formula (3);

    // Initialise the function parser.
    dealii::FunctionParser<dim> material_identification;
    material_identification.initialize (dealii::FunctionParser<dim>::default_variable_names (),
					parameters.get ("MaterialID"),
					typename dealii::FunctionParser<dim>::ConstMap ());

    mandy::MatrixCreator::create_mass_matrix<dim> (fe, dof_handler, quadrature_formula,
						   system_matrix, constraints,
						   mpi_communicator);

    mandy::VectorCreator::create_right_hand_side_vector<dim> (fe, dof_handler, quadrature_formula,
     							      system_rhs, constraints,
							      material_identification,
							      mpi_communicator);
  }

  /**
   * Solve the linear algebra system.
   */
  template <int dim>
  unsigned int
  MaterialID<dim>::solve ()
  {
    dealii::TimerOutput::Scope time (timer, "solve");

    dealii::PETScWrappers::MPI::Vector completely_distributed_solution (locally_owned_dofs, mpi_communicator);

    // Solve using conjugate gradient method with no preconditioner
    // (ie., system_matrix is ignored).
    dealii::SolverControl solver_control (dof_handler.n_dofs (), 1e-06);
    dealii::PETScWrappers::SolverCG solver (solver_control, mpi_communicator);
    dealii::PETScWrappers::PreconditionNone preconditioner (system_matrix);

    solver.solve (system_matrix, completely_distributed_solution, system_rhs,
		  preconditioner);

    // Ensure that all ghost elements are also copied as necessary.
    constraints.distribute (completely_distributed_solution);
    locally_relevant_solution = completely_distributed_solution;

    // Return the number of iterations (last step) of the solve.
    return solver_control.last_step ();
  }

  /**
   * Output results, ie., finite element functions and derived
   * quantitites for this cycle..
   */
  template <int dim>
  void
  MaterialID<dim>::output_results (const unsigned int cycle)
  {
    dealii::TimerOutput::Scope time (timer, "output_results");

    dealii::DataOut<dim> data_out;
    data_out.attach_dof_handler (dof_handler);
    data_out.add_data_vector (locally_relevant_solution, "material_id");

    dealii::Vector<float> subdomain (triangulation.n_active_cells ());
    for (unsigned int i=0; i<subdomain.size(); ++i)
      subdomain (i) = triangulation.locally_owned_subdomain ();
    data_out.add_data_vector (subdomain, "subdomain");

    data_out.build_patches ();

    const std::string filename = ("material_id-" +
                                  dealii::Utilities::int_to_string (cycle, 2) +
                                  "." +
                                  dealii::Utilities::int_to_string
                                  (triangulation.locally_owned_subdomain (), 4));

    std::ofstream output ((filename + ".vtu").c_str ());
    data_out.write_vtu (output);

    if (dealii::Utilities::MPI::this_mpi_process(mpi_communicator) == 1)
      {
	std::vector<std::string> filenames;

	for (unsigned int i=0;
	     i<dealii::Utilities::MPI::n_mpi_processes (mpi_communicator);
	     ++i)
	  filenames.push_back ("material_id-" +
			       dealii::Utilities::int_to_string (cycle, 2) +
			       "." +
			       dealii::Utilities::int_to_string (i, 4) +
			       ".vtu");
	std::ofstream master_output (("material_id-" +
				      dealii::Utilities::int_to_string (cycle, 2) +
				      ".pvtu").c_str ());

	data_out.write_pvtu_record (master_output, filenames);
      }
  }

  /**
   * Refine grid based on Kelly's error estimator working on the
   * material id (solution vector).
   */
  template <int dim>
  void MaterialID<dim>::refine_grid ()
  {
    dealii::TimerOutput::Scope time (timer, "refine grid");

    dealii::Vector<float> estimated_error_per_cell (triangulation.n_active_cells());

    dealii::KellyErrorEstimator<dim>::estimate (dof_handler, dealii::QGauss<dim-1>(4),
						typename dealii::FunctionMap<dim>::type (),
						locally_relevant_solution,
						estimated_error_per_cell);

    dealii::parallel::distributed::GridRefinement::
      refine_and_coarsen_fixed_number (triangulation,
				       estimated_error_per_cell,
				       0.250, 0.025);

    triangulation.execute_coarsening_and_refinement ();
  }

  /**
   * Run the application in the order specified.
   */
  template <int dim>
  void
  MaterialID<dim>::run ()
  {
    const unsigned int n_cycles = 5;

    for (unsigned int cycle=0; cycle<n_cycles; ++cycle)
      {
        pcout << "MaterialID:: Cycle " << cycle << ':'
	      << std::endl;

	if (cycle==0)
	  make_coarse_grid ();

	else
	  refine_grid ();

	pcout << "   Number of active cells:       "
	      << triangulation.n_global_active_cells ()
	      << std::endl;

	setup_system ();

	pcout << "   Number of degrees of freedom: "
	      << dof_handler.n_dofs ()
	      << std::endl;

	assemble_system ();

	const unsigned int n_iterations = solve ();

	pcout << "   Solved in " << n_iterations
	      << " iterations."
	      << std::endl;

	pcout << "   Linfty-norm:                  "
	      << locally_relevant_solution.linfty_norm ()
	      << std::endl;

	// Output results if the number of processes is less than or
	// equal to 32.
	if (dealii::Utilities::MPI::n_mpi_processes (mpi_communicator) <= 32)
	  output_results (cycle);

	// timer.print_summary ();
        pcout << std::endl;

      } // for cycle<n_cycles
  }

} // namespace mandy

/**
 * Main function: Initialise problem and run it.
 */
int main (int argc, char *argv[])
{
  // Initialise MPI
  dealii::Utilities::MPI::MPI_InitFinalize mpi_initialization (argc, argv, 1);

  try
    {
      mandy::MaterialID<3> material_id ("step-0-material.prm");
      material_id.run ();
    }

  catch (std::exception &exc)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

      return 1;
    }

  catch (...)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }

  return 0;
}


#include "mpi_fsi.h"
#include "mpi_scnsim.h"
#include "mpi_shared_hypo_elasticity.h"
#include "parameters.h"
#include "utilities.h"
#include <deal.II/grid/grid_in.h>
#include <deal.II/grid/grid_out.h>

extern template class Fluid::MPI::SCnsIM<2>;
extern template class Solid::MPI::SharedHypoElasticity<2>;
extern template class MPI::FSI<2>;

using namespace dealii;

template <int dim>
class SigmaPMLField : public Function<dim>
{
public:
  SigmaPMLField(double sig, double l)
    : Function<dim>(), SigmaPMLMax(sig), PMLLength(l)
  {
  }
  virtual double value(const Point<dim> &p,
                       const unsigned int component = 0) const;
  virtual void value_list(const std::vector<Point<dim>> &points,
                          std::vector<double> &values,
                          const unsigned int component = 0) const;

private:
  double SigmaPMLMax;
  double PMLLength;
};

template <int dim>
class ArtificialBF : public TensorFunction<1, dim>
{
public:
  ArtificialBF() : TensorFunction<1, dim>() {}
  virtual Tensor<1, dim> value(const Point<dim> &p) const;
  virtual void value_list(const std::vector<Point<dim>> &points,
                          std::vector<Tensor<1, dim>> &values) const;
};

template <int dim>
double SigmaPMLField<dim>::value(const Point<dim> &p,
                                 const unsigned int component) const
{
  (void)component;
  (void)p;
  double SigmaPML = 0.0;
  double boundary = 0.0;
  // For tube acoustics
  if (p[0] < PMLLength + boundary)
    // A quadratic increasing function from boundary-PMLlength to the boundary
    SigmaPML = SigmaPMLMax * pow((PMLLength + boundary - p[0]) / PMLLength, 4);
  return SigmaPML;
}

template <int dim>
void SigmaPMLField<dim>::value_list(const std::vector<Point<dim>> &points,
                                    std::vector<double> &values,
                                    const unsigned int component) const
{
  (void)component;
  for (unsigned int i = 0; i < points.size(); ++i)
    values[i] = this->value(points[i]);
}

template <int dim>
Tensor<1, dim> ArtificialBF<dim>::value(const Point<dim> &p) const
{
  Tensor<1, dim> value;
  double rho = 1.3e-3;
  double bf = 1.0e4 / rho;
  if (p[0] > 1.0 - 5e-4 && p[0] < 2.0 + 5e-4)
    value[0] = bf;
  return value;
}

template <int dim>
void ArtificialBF<dim>::value_list(const std::vector<Point<dim>> &points,
                                   std::vector<Tensor<1, dim>> &values) const
{
  for (unsigned int i = 0; i < points.size(); ++i)
    values[i] = this->value(points[i]);
}

int main(int argc, char *argv[])
{
  using namespace dealii;

  double PMLlength = 1.0, SigmaMax = 340000;

  try
    {
      Utilities::MPI::MPI_InitFinalize mpi_initialization(argc, argv, 1);

      std::string infile("parameters.prm");
      if (argc > 1)
        {
          infile = argv[1];
        }
      Parameters::AllParameters params(infile);

      if (params.dimension == 2)
        {
          // Read solid mesh
          Triangulation<2> tria_solid;
          dealii::GridGenerator::subdivided_hyper_rectangle(
            tria_solid,
            {static_cast<unsigned int>(10), static_cast<unsigned int>(40)},
            Point<2>(0, 0),
            Point<2>(0.5, 2),
            true);

          // Read fluid mesh
          parallel::distributed::Triangulation<2> tria_fluid(MPI_COMM_WORLD);
          dealii::GridGenerator::subdivided_hyper_rectangle(
            tria_fluid,
            {static_cast<unsigned int>(114), static_cast<unsigned int>(29)},
            Point<2>(0, 0),
            Point<2>(5, 2),
            true);

          // Translate solid mesh
          Tensor<1, 2> offset({2, 0});
          GridTools::shift(offset, tria_solid);

          // placeholder for hard code BC
          std::shared_ptr<Functions::ZeroFunction<2>> ptr =
            std::make_shared<Functions::ZeroFunction<2>>(
              Functions::ZeroFunction<2>(3));
          // initialize the pml field
          auto pml = std::make_shared<SigmaPMLField<2>>(
            SigmaPMLField<2>(SigmaMax, PMLlength));
          // artificial body force
          auto bf_ptr = std::make_shared<ArtificialBF<2>>(ArtificialBF<2>());

          Fluid::MPI::SCnsIM<2> fluid(tria_fluid,
                                      params); //, ptr, pml, bf_ptr);
          Solid::MPI::SharedHypoElasticity<2> solid(
            tria_solid, params, 0.05, 1.3);
          MPI::FSI<2> fsi(fluid, solid, params);
          fsi.run();
        }
      else
        {
          AssertThrow(false, ExcNotImplemented());
        }
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }
  return 0;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 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
//==============================================================================
#ifndef NT2_TOOLBOX_HYPERBOLIC_FUNCTIONS_SIMD_COMMON_SINHCOSH_HPP_INCLUDED
#define NT2_TOOLBOX_HYPERBOLIC_FUNCTIONS_SIMD_COMMON_SINHCOSH_HPP_INCLUDED

#include <nt2/toolbox/hyperbolic/functions/sinhcosh.hpp>
#include <nt2/include/functions/simd/tofloat.hpp>
#include <nt2/include/functions/simd/abs.hpp>
#include <nt2/include/functions/simd/expm1.hpp>
#include <nt2/include/functions/simd/if_else.hpp>
#include <nt2/include/functions/simd/negif.hpp>
#include <nt2/include/functions/simd/is_equal.hpp>
#include <nt2/include/functions/simd/oneplus.hpp>
#include <nt2/include/functions/simd/is_negative.hpp>
#include <nt2/include/functions/simd/divides.hpp>
#include <nt2/include/functions/simd/multiplies.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/sdk/meta/as_logical.hpp>
#include <nt2/sdk/meta/cardinal_of.hpp>
#include <boost/fusion/tuple.hpp>

namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION_IF(  nt2::tag::sinhcosh_, tag::cpu_,(A0)(A1)(X)
                                  , (boost::mpl::equal_to < nt2::meta::cardinal_of<A0>
                                                        , nt2::meta::cardinal_of<A1>
                                                        >
                                )
                               , ((simd_<arithmetic_<A0>,X>))
                                 ((simd_<floating_<A1>,X>))
                                 ((simd_<floating_<A1>,X>))
                             )
  {
    typedef int result_type;
    inline result_type operator()(A0 const& a0,A1 & a1,A1 & a2) const
    {
      typedef typename meta::as_logical<A1>::type ltype;
      A1 a00 =  nt2::abs(a0);
      ltype test =  eq(a00, Inf<A1>());
      const A1 u = nt2::expm1(a00);
      const A1 up1 = oneplus(u);
      const A1 tmp =u/up1;
      a1 = negif(is_negative(a0), if_else(test, a00, Half<A1>()*tmp*(oneplus(up1))));
      a2 = if_else(test, a00, oneplus(Half<A1>()*tmp*u));
      return 0;
    }
  };

  NT2_FUNCTOR_IMPLEMENTATION_IF(nt2::tag::sinhcosh_, tag::cpu_,(A0)(A1)(X),
                                (boost::mpl::equal_to<nt2::meta::cardinal_of<A0>,
                                                 nt2::meta::cardinal_of<A1>
                                        >
                                ),
                                ((simd_ < arithmetic_<A0>,X > ))
                                ((simd_ < floating_<A1>,X > ))
                             )
  {
    typedef A1 result_type;
    inline result_type operator()(A0 const& a0,A1 & a2) const
    {
      A1 a1;
      sinhcosh(tofloat(a0),a1, a2);
      return a1;
    }
  };

  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::sinhcosh_, tag::cpu_,
                         (A0)(X),
                         ((simd_<arithmetic_<A0>,X>))
                        )
  {
      typedef typename meta::as_floating<A0>::type  rtype;
      typedef boost::fusion::tuple<rtype, rtype> result_type;

    NT2_FUNCTOR_CALL(1)
    {
      result_type res;
      boost::fusion::at_c<0>(res) = sinhcosh(tofloat(a0),
                                              boost::fusion::at_c<1>(res));
      return res;
    }
  };
} }
#endif


/* ----------------------------------------------------------------------------

 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/*
 * @file Unit3.h
 * @date Feb 02, 2011
 * @author Can Erdogan
 * @author Frank Dellaert
 * @author Alex Trevor
 * @brief The Unit3 class - basically a point on a unit sphere
 */

#include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/Point2.h>
#include <boost/random/mersenne_twister.hpp>

#ifdef __clang__
#  pragma clang diagnostic push
#  pragma clang diagnostic ignored "-Wunused-variable"
#endif
#include <boost/random/uniform_on_sphere.hpp>
#ifdef __clang__
#  pragma clang diagnostic pop
#endif

#include <boost/random/variate_generator.hpp>
#include <iostream>

using namespace std;

namespace gtsam {

/* ************************************************************************* */
Unit3 Unit3::FromPoint3(const Point3& point, boost::optional<Matrix&> H) {
  Unit3 direction(point);
  if (H) {
    // 3*3 Derivative of representation with respect to point is 3*3:
    Matrix D_p_point;
    point.normalize(D_p_point); // TODO, this calculates norm a second time :-(
    // Calculate the 2*3 Jacobian
    H->resize(2, 3);
    *H << direction.basis().transpose() * D_p_point;
  }
  return direction;
}

/* ************************************************************************* */
Unit3 Unit3::Random(boost::mt19937 & rng) {
  // TODO allow any engine without including all of boost :-(
  boost::uniform_on_sphere<double> randomDirection(3);
  // This variate_generator object is required for versions of boost somewhere
  // around 1.46, instead of drawing directly using boost::uniform_on_sphere(rng).
  boost::variate_generator<boost::mt19937&, boost::uniform_on_sphere<double> >
      generator(rng, randomDirection);
  vector<double> d = generator();
  Unit3 result;
  result.p_ = Point3(d[0], d[1], d[2]);
  return result;
}

/* ************************************************************************* */
const Unit3::Matrix32& Unit3::basis() const {

  // Return cached version if exists
  if (B_)
    return *B_;

  // Get the axis of rotation with the minimum projected length of the point
  Point3 axis;
  double mx = fabs(p_.x()), my = fabs(p_.y()), mz = fabs(p_.z());
  if ((mx <= my) && (mx <= mz))
    axis = Point3(1.0, 0.0, 0.0);
  else if ((my <= mx) && (my <= mz))
    axis = Point3(0.0, 1.0, 0.0);
  else if ((mz <= mx) && (mz <= my))
    axis = Point3(0.0, 0.0, 1.0);
  else
    assert(false);

  // Create the two basis vectors
  Point3 b1 = p_.cross(axis);
  b1 = b1 / b1.norm();
  Point3 b2 = p_.cross(b1);
  b2 = b2 / b2.norm();

  // Create the basis matrix
  B_.reset(Unit3::Matrix32());
  (*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z();
  return *B_;
}

/* ************************************************************************* */
/// The print fuction
void Unit3::print(const std::string& s) const {
  cout << s << ":" << p_ << endl;
}

void Unit3::print(std::ostream& os, const std::string& s) const {
  os << s << ":" << p_ << endl;
}

/* ************************************************************************* */
Matrix Unit3::skew() const {
  return skewSymmetric(p_.x(), p_.y(), p_.z());
}

/* ************************************************************************* */
Vector Unit3::error(const Unit3& q, boost::optional<Matrix&> H) const {
  // 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1
  Matrix Bt = basis().transpose();
  Vector xi = Bt * q.p_.vector();
  if (H)
    *H = Bt * q.basis();
  return xi;
}

/* ************************************************************************* */
double Unit3::distance(const Unit3& q, boost::optional<Matrix&> H) const {
  Vector xi = error(q, H);
  double theta = xi.norm();
  if (H)
    *H = (xi.transpose() / theta) * (*H);
  return theta;
}

/* ************************************************************************* */
Unit3 Unit3::retract(const Vector& v) const {

  // Get the vector form of the point and the basis matrix
  Vector p = Point3::Logmap(p_);
  Matrix B = basis();

  // Compute the 3D xi_hat vector
  Vector xi_hat = v(0) * B.col(0) + v(1) * B.col(1);

  double xi_hat_norm = xi_hat.norm();

  // Avoid nan
  if (xi_hat_norm == 0.0) {
    if (v.norm() == 0.0)
      return Unit3(point3());
    else
      return Unit3(-point3());
  }

  Vector exp_p_xi_hat = cos(xi_hat_norm) * p
      + sin(xi_hat_norm) * (xi_hat / xi_hat_norm);
  return Unit3(exp_p_xi_hat);

}

/* ************************************************************************* */
Vector Unit3::localCoordinates(const Unit3& y) const {

  Vector p = Point3::Logmap(p_);
  Vector q = Point3::Logmap(y.p_);
  double dot = p.dot(q);

  // Check for special cases
  if (std::abs(dot - 1.0) < 1e-16)
    return (Vector(2) << 0, 0);
  else if (std::abs(dot + 1.0) < 1e-16)
    return (Vector(2) << M_PI, 0);
  else {
    // no special case
    double theta = acos(dot);
    Vector result_hat = (theta / sin(theta)) * (q - p * dot);
    return basis().transpose() * result_hat;
  }
}
/* ************************************************************************* */

}


/*
 * Copyright 2018 James Dyer
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
 * use this file except in compliance with the License. You may obtain a copy of
 * the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations under
 * the License.
 *
 * This is being developed for the TANGO Project: http://tango-project.eu
 */

#include <iostream>
#include <sstream>
#include <NTL/RR.h>
#include <NTL/ZZ_p.h>
#include <json/json.h>
#include "HE1Encrypter.h"

#include "Random.h"

HE1Encrypter::HE1Encrypter(int lambda, int eta)
{
	generateParameters(lambda,eta);
}

HE1Encrypter::HE1Encrypter(int n, int d, int rho)
{
	NTL::RR two(2);
	NTL::RR exp1(rho*d);
	NTL::RR nplusone(n+1);
	NTL::RR exp2(d);
	NTL::RR ploBound;
	ploBound = pow(two,exp1)*pow(nplusone,exp2);
	NTL::ZZ pLowerBound;
	conv(pLowerBound,ploBound);
	long lambda = NumBits(pLowerBound) + 1;
	long eta = ((lambda * lambda / rho) - lambda);
	generateParameters(lambda,eta);
};

NTL::ZZ_p HE1Encrypter::encrypt(NTL::ZZ& plaintext)
{
	NTL::ZZ_p r = to_ZZ_p(rng->nextBigInteger(ONE, q));
	NTL::ZZ_p ptext = to_ZZ_p(plaintext);
	return ptext+r*(*pmod);
};

NTL::ZZ& HE1Encrypter::getKey()
{
	return p;
};

std::string HE1Encrypter::writeSecretsToJSON()
{
	Json::Value root;
	std::stringstream pStr;
	pStr << p;
	root["p"] = pStr.str();
	Json::FastWriter writer;
	return writer.write(root);
};


/*=============================================================================
    Copyright (c) 2001-2010 Joel de Guzman

    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)
=============================================================================*/
///////////////////////////////////////////////////////////////////////////////
//
//  Yet another calculator example! This time, we will compile to a simple
//  virtual machine. This is actually one of the very first Spirit example
//  circa 2000. Now, it's ported to Spirit2.
//
//  [ JDG Sometime 2000 ]       pre-boost
//  [ JDG September 18, 2002 ]  spirit1
//  [ JDG April 8, 2007 ]       spirit2
//
///////////////////////////////////////////////////////////////////////////////

#include <boost/config/warning_disable.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_container.hpp>
#include <boost/spirit/include/phoenix_statement.hpp>
#include <boost/spirit/include/phoenix_object.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>

#include <iostream>
#include <string>
#include <vector>

namespace client
{
    namespace qi = boost::spirit::qi;
    namespace phoenix = boost::phoenix;
    namespace ascii = boost::spirit::ascii;

    ///////////////////////////////////////////////////////////////////////////
    //  The Virtual Machine
    ///////////////////////////////////////////////////////////////////////////
    enum byte_code
    {
        op_neg,     //  negate the top stack entry
        op_add,     //  add top two stack entries
        op_sub,     //  subtract top two stack entries
        op_mul,     //  multiply top two stack entries
        op_div,     //  divide top two stack entries
        op_int,     //  push constant integer into the stack
    };

    class vmachine
    {
    public:

        vmachine(unsigned stackSize = 4096)
          : stack(stackSize)
          , stack_ptr(stack.begin())
        {
        }

        int top() const { return stack_ptr[-1]; };
        void execute(std::vector<int> const& code);

    private:

        std::vector<int> stack;
        std::vector<int>::iterator stack_ptr;
    };

    void vmachine::execute(std::vector<int> const& code)
    {
        std::vector<int>::const_iterator pc = code.begin();
        stack_ptr = stack.begin();

        while (pc != code.end())
        {
            switch (*pc++)
            {
                case op_neg:
                    stack_ptr[-1] = -stack_ptr[-1];
                    break;

                case op_add:
                    --stack_ptr;
                    stack_ptr[-1] += stack_ptr[0];
                    break;

                case op_sub:
                    --stack_ptr;
                    stack_ptr[-1] -= stack_ptr[0];
                    break;

                case op_mul:
                    --stack_ptr;
                    stack_ptr[-1] *= stack_ptr[0];
                    break;

                case op_div:
                    --stack_ptr;
                    stack_ptr[-1] /= stack_ptr[0];
                    break;

                case op_int:
                    *stack_ptr++ = *pc++;
                    break;
            }
        }
    }

    ///////////////////////////////////////////////////////////////////////////
    //  Our calculator grammar and compiler
    ///////////////////////////////////////////////////////////////////////////
    template <typename Iterator>
    struct calculator : qi::grammar<Iterator, ascii::space_type>
    {
        calculator(std::vector<int>& code)
          : calculator::base_type(expression)
          , code(code)
        {
            using namespace qi::labels;
            using qi::uint_;
            using qi::on_error;
            using qi::fail;

            using phoenix::val;
            using phoenix::ref;
            using phoenix::push_back;
            using phoenix::construct;

            expression =
                term
                >> *(   ('+' > term             [push_back(ref(code), op_add)])
                    |   ('-' > term             [push_back(ref(code), op_sub)])
                    )
                ;

            term =
                factor
                >> *(   ('*' > factor           [push_back(ref(code), op_mul)])
                    |   ('/' > factor           [push_back(ref(code), op_div)])
                    )
                ;

            factor =
                uint_                           [
                                                    push_back(ref(code), op_int),
                                                    push_back(ref(code), _1)
                                                ]
                |   '(' > expression > ')'
                |   ('-' > factor               [push_back(ref(code), op_neg)])
                |   ('+' > factor)
                ;

            expression.name("expression");
            term.name("term");
            factor.name("factor");

            on_error<fail>
            (
                expression
              , std::cout
                    << val("Error! Expecting ")
                    << _4                               // what failed?
                    << val(" here: \"")
                    << construct<std::string>(_3, _2)   // iterators to error-pos, end
                    << val("\"")
                    << std::endl
            );
        }

        qi::rule<Iterator, ascii::space_type> expression, term, factor;
        std::vector<int>& code;
    };

    template <typename Grammar>
    bool compile(Grammar const& calc, std::string const& expr)
    {
        std::string::const_iterator iter = expr.begin();
        std::string::const_iterator end = expr.end();
        bool r = phrase_parse(iter, end, calc, ascii::space);

        if (r && iter == end)
        {
            std::cout << "-------------------------\n";
            std::cout << "Parsing succeeded\n";
            std::cout << "-------------------------\n";
            return true;
        }
        else
        {
            std::cout << "-------------------------\n";
            std::cout << "Parsing failed\n";
            std::cout << "-------------------------\n";
            return false;
        }
    }
}

///////////////////////////////////////////////////////////////////////////////
//  Main program
///////////////////////////////////////////////////////////////////////////////
int
main()
{
    std::cout << "/////////////////////////////////////////////////////////\n\n";
    std::cout << "Expression parser...\n\n";
    std::cout << "/////////////////////////////////////////////////////////\n\n";
    std::cout << "Type an expression...or [q or Q] to quit\n\n";

    typedef std::string::const_iterator iterator_type;
    typedef client::calculator<iterator_type> calculator;

    client::vmachine mach;          //  Our virtual machine
    std::vector<int> code;          //  Our VM code
    calculator calc(code);          //  Our grammar

    std::string str;
    while (std::getline(std::cin, str))
    {
        if (str.empty() || str[0] == 'q' || str[0] == 'Q')
            break;

        code.clear();
        if (client::compile(calc, str))
        {
            mach.execute(code);
            std::cout << "\n\nresult = " << mach.top() << std::endl;
            std::cout << "-------------------------\n\n";
        }
    }

    std::cout << "Bye... :-) \n\n";
    return 0;
}


/*! EXAMPLE
	shows calculation of ECP integrals and first derivatives for HI
	in a cc-pVDZ(-PP) basis, using ECP28MDF

	USAGE
	./example [LIBECPINT_SHARE_DIR_PATH]
 */

#include "libecpint.hpp"

// if you want to try out the Eigen matrix build
#ifdef _WITH_EIGEN
	#include <Eigen/Dense>
	#include <Eigen/Core>
#endif

#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <array>
#include <algorithm>
#include <iterator>

using DVec = std::vector<double>;
using IVec = std::vector<int>;
using Coord = std::array<double, 3>;

void read_basis_file(std::string, DVec&, DVec&, DVec&, IVec&, IVec&, Coord&);

int main(int argc, char* argv[]) {
	using namespace libecpint;

	// this code expects the path to the libecpint/share directory
	// to be passed as an argument to the executable
	std::string share_dir = argv[1];

	// Roughly equilibrium position HI molecule
	// NOTE: coordinate values must be in BOHR
	Coord H_pos = {0.0, 0.0, 0.0};
	Coord I_pos = {0.0, 0.0, 3.0};

	// Let's read in the GTO basis from file
	// store in standard vectors
	DVec g_exps, g_coeffs; // length = total number of primitives in basis
	IVec g_ams, g_lens; // length = total number of shells in basis
	DVec g_coords; // length = 3*total number of shells

	// hydrogen cc-pVDZ
	read_basis_file("hydrogen.bas", g_exps, g_coeffs, g_coords, g_ams, g_lens, H_pos);
	// iodine cc-pVDZ-PP
	read_basis_file("iodine.bas", g_exps, g_coeffs,  g_coords, g_ams, g_lens, I_pos);
	// check sizes, should read 'Basis read: 44, 12, 36'
	std::cout << "Basis read: " << g_exps.size() << ", " << g_ams.size() << ", " << g_coords.size() << std::endl;

	// Now to perform a calculation using the high-level API we do the following:
	ECPIntegrator factory; // object used to build ECP integral matrices

	// Set the orbital basis sets
	factory.set_gaussian_basis(g_ams.size(), g_coords.data(), g_exps.data(), g_coeffs.data(), g_ams.data(), g_lens.data());
	// put an ECP on the iodine (charge = 53, name="ecp28mdf")
	std::vector<std::string> names = {"ecp28mdf"};
	int charges[1] = {53};
	factory.set_ecp_basis_from_library(1, I_pos.data(), charges, names, share_dir);

	// initialise - we want integrals and derivatives, so deriv_order=1
	factory.init(1);

	// and compute both the integrals and derivatives
	std::cout << "Computing integrals..." << std::endl;
	factory.compute_integrals();
	std::cout << "Computing first derivs..." << std::endl;
	factory.compute_first_derivs();
	std::cout << "Done." << std::endl;

	// we can now access the integrals
	// as an example, let's build them into an
	// eigen matrix
#ifdef _WITH_EIGEN
	 // grab the integrals
	 std::shared_ptr<DVec> ints = factory.get_integrals();

	 // map into a square matrix
	 // note that we use ROW-MAJOR ORDERING
	 // 12 basis functions
	 Eigen::MatrixXd ecpints = Eigen::Map<Eigen::Matrix<double, 12, 12, Eigen::RowMajor>>(ints->data());
	 std::cout << "ECP Integrals:" << std::endl;
	 std::cout << ecpints << std::endl;

	 // we could do the same with derivatives
	 // this returns pointers in order [Hx, Hy, Hz, Ix, Iy, Iz]
	 std::vector<std::shared_ptr<DVec>> derivs = factory.get_first_derivs();
	 // e.g. the Iodine y-derivative
	 Eigen::MatrixXd iodine_y_derivs = Eigen::Map<Eigen::Matrix<double, 12, 12, Eigen::RowMajor>>(derivs[4]->data());
	 std::cout << "y-derivs on iodine:" << std::endl;
	 std::cout << iodine_y_derivs << std::endl;
#endif

	return 0;
}

/*! Reads in the Gaussian orbital basis from a file.

	filename - the path/name of the basis file
	exps - vector to place exponents in
	coeffs - vector to place coefficients in
	coords - the vector of xyz coords for each shell
	ams - vector to place angular momenta in
	lens - vector to place shell lengths in
	atom - the position of the atom being read in
 */
void read_basis_file(std::string filename, DVec& exps, DVec& coeffs, DVec& coords, IVec& ams, IVec& lens, Coord& atom) {
	std::ifstream input_file(filename);
	if (input_file.is_open()) {
		// We expect the file to have the format
		// L; x, c; x, c; x, c; x, c;
		// with a line for each shell
		std::string line, token;
		while (!input_file.eof()) {
			std::getline(input_file, line);

			// split the line by semicolon
			size_t sc_pos = line.find(';');
			int len = -1;
			int am;
			while (sc_pos != std::string::npos) {
				token = line.substr(0, sc_pos);
				line.erase(0, sc_pos+1);

				// First bit is L
				if (len == -1) {
					am = std::stoi(token);
					len++;
				} else {
					// subsequent bits are x,c
					size_t c_pos = token.find(',');
					if (c_pos != std::string::npos) {
						exps.push_back(std::stod(token.substr(0, c_pos)));
						coeffs.push_back(std::stod(token.substr(c_pos+1, token.length())));
						len++;
					}
				}
				sc_pos = line.find(';');
			}
			if (len > 0) {
				// non-empty shell found
				ams.push_back(am);
				lens.push_back(len);
				std::copy(atom.begin(), atom.end(), std::back_inserter(coords));
			}
		}
	}
}


#include <Eigen/Dense>
#include <vector>
#include <iostream>
#include "gpu_func.h"
#include <sys/time.h>
#include "proxgpu_types.h"

// Compute the gradient at B
numeric get_value_only(cox_cache &dev_cache,
                  cox_data &dev_data,
                  cox_param &dev_param,
                  numeric *B,
                  int *ncase,
                  int *ncase_cumu,
                  int K,
                  int p,
                  cublasHandle_t handle,
                  cudaStream_t *streams,
                  numeric *cox_val_host)
{
    numeric result = 0.0;
    for (int k = 0; k < K; ++k)
    {
        int n = ncase[k];
        int offset = ncase_cumu[k];
        compute_product(dev_data.X+offset*p, B+p*k, dev_cache.eta+offset, n, p, streams[k], handle,CUBLAS_OP_N);
        apply_exp(dev_cache.eta+offset, dev_cache.exp_eta+offset, n, streams[k]);
        // Save rev_cumsum result to dev_cache.outer_accumu to avoid more cache variables
        rev_cumsum(dev_cache.exp_eta+offset, dev_cache.outer_accumu+offset ,n, streams[k]);
        adjust_ties(dev_cache.outer_accumu+offset, dev_data.rankmin+offset, dev_cache.exp_accumu+offset, n, streams[k]);

        get_coxvalue(dev_cache.exp_accumu+offset, dev_cache.eta+offset, dev_data.censor+offset, dev_cache.cox_val+k, n, streams[k]);
        cudaMemcpyAsync(cox_val_host+k, dev_cache.cox_val+k, sizeof(numeric)*1, cudaMemcpyDeviceToHost, streams[k]);
    }

    cudaDeviceSynchronize();
    for(int k = 0; k < K; ++k)
    {
        result += cox_val_host[k];
    }
    return result;
}

// Compute the gradient at B and save the result to dev_grad
numeric get_gradient(cox_cache &dev_cache,
                  cox_data &dev_data,
                  cox_param &dev_param,
                  numeric *dev_grad,
                  numeric *B,
                  int *ncase,
                  int *ncase_cumu,
                  int K,
                  int p,
                  cublasHandle_t handle,
                  cudaStream_t *streams,
                  bool get_val = false,
                  numeric *cox_val_host=0)
{
    for(int k = 0; k <K; ++k)
    {
        int n = ncase[k];
        int offset = ncase_cumu[k];
        compute_product(dev_data.X+offset*p, B+p*k, dev_cache.eta+offset, n, p, streams[k], handle,CUBLAS_OP_N);
        apply_exp(dev_cache.eta+offset, dev_cache.exp_eta+offset, n, streams[k]);
        // Save rev_cumsum result to dev_cache.outer_accumu to avoid more cache variables
        rev_cumsum(dev_cache.exp_eta+offset, dev_cache.outer_accumu+offset ,n, streams[k]);
        adjust_ties(dev_cache.outer_accumu+offset, dev_data.rankmin+offset, dev_cache.exp_accumu+offset, n, streams[k]);
        // Above is  _update_exp()
        // Below is _update_outer()
        // Save the result of division to residual to avoid more cache variables
        cwise_div(dev_data.censor+offset, dev_cache.exp_accumu+offset, dev_cache.residual+offset,  n, streams[k]);
        cumsum(dev_cache.residual+offset, n, streams[k]);
        adjust_ties(dev_cache.residual+offset, dev_data.rankmax+offset, dev_cache.outer_accumu+offset, n, streams[k]);
        mult_add(dev_cache.residual+offset,
                 dev_cache.exp_eta+offset,
                 dev_cache.outer_accumu+offset,
                 dev_data.censor+offset,
                 n, streams[k]);
        // residual ready
        compute_product(dev_data.X+offset*p,
                        dev_cache.residual+offset,
                        dev_grad + k*p, n, p, streams[k], handle, CUBLAS_OP_T);
        // Gradient ready
        // get_val will modify eta, but it's fine
        if(get_val)
        {
            get_coxvalue(dev_cache.exp_accumu+offset, dev_cache.eta+offset, dev_data.censor+offset, dev_cache.cox_val+k, n, streams[k]);
            cudaMemcpyAsync(cox_val_host+k, dev_cache.cox_val+k, sizeof(numeric)*1, cudaMemcpyDeviceToHost, streams[k]);
        }
    }
    numeric result = 0.0;
    if (get_val)
    {
        cudaDeviceSynchronize();
        for(int k = 0; k < K; ++k)
        {
            result += cox_val_host[k];
        }

    }
    return result;
}

// [[Rcpp::export]]
Rcpp::List solve_path(const Rcpp::List & X_list,
                        const Rcpp::List & censoring_list,
                        MatrixXd B,
                        const Rcpp::List & rankmin_list,
                        const Rcpp::List & rankmax_list,
                        double step_size,
                        VectorXd lambda_1_all,
                        VectorXd lambda_2_all,
                        Eigen::RowVectorXd penalty_factor, // Penalty factor for each group of variables
                        int niter, // Maximum number of iterations
                        double linesearch_beta,
                        double eps, // convergence criteria
                        double tol = 1e-10// line search tolerance
                        )
{
    // B is a long and skinny matrix now! rows are features and cols are responses
    const int p = B.rows();
    const int K = B.cols();
    // Create CUDA streams and handle
    cudaStream_t *streams = (cudaStream_t *) malloc(K * sizeof(cudaStream_t));
    for (int k = 0; k<K; ++k)
    {
        cudaStreamCreate(&streams[k]);
    }
    cublasHandle_t handle;
    cublasCreate(&handle);
    cudaStream_t copy_stream;
    cudaStream_t nest_stream;
    cudaStreamCreate(&copy_stream);
    cudaStreamCreate(&nest_stream);

    cox_data dev_data;
    cox_cache dev_cache;
    cox_param dev_param;
    numeric *cox_val_host=(numeric *)malloc(sizeof(numeric)*K);
    MatrixXd host_B(p,K);

    int *ncase_cumu = (int *)malloc(sizeof(int)*(K+1));
    ncase_cumu[0] = 0;
    int *ncase = (int*)malloc(sizeof(int)*K);
    std::vector<MapMatd> X_all;
    std::vector<MapVecd> censor; // Modify this in R
    std::vector<MapVeci> rankmin;
    std::vector<MapVeci> rankmax;
    for (int k = 0; k<K; ++k)
    {
        X_all.emplace_back(Rcpp::as<MapMatd>(X_list[k]));
        censor.emplace_back(Rcpp::as<MapVecd>(censoring_list[k]));
        rankmin.emplace_back(Rcpp::as<MapVeci>(rankmin_list[k]));
        rankmax.emplace_back(Rcpp::as<MapVeci>(rankmax_list[k]));
        ncase[k] = X_all[k].rows();
        ncase_cumu[k+1] = ncase_cumu[k] + ncase[k];
    }

    allocate_device_memory(dev_data, dev_cache, dev_param, ncase_cumu[K], K, p);

    // initialize parameters on the device
    cudaMemcpy(dev_param.B, &B(0,0), sizeof(numeric)*K*p, cudaMemcpyHostToDevice);
    cudaMemcpy(dev_param.v, &B(0,0), sizeof(numeric)*K*p, cudaMemcpyHostToDevice);
    cudaMemcpy(dev_param.penalty_factor, &penalty_factor(0), sizeof(numeric)*p, cudaMemcpyHostToDevice);

    // Copy the data
    for(int k = 0; k <K; ++k)
    {
        int n = ncase[k];
        cudaMemcpyAsync(dev_data.X+ncase_cumu[k]*p, &(X_all[k](0,0)), sizeof(numeric) * p *n, cudaMemcpyHostToDevice, streams[k]);
        cudaMemcpyAsync(dev_data.censor+ncase_cumu[k], &(censor[k][0]), sizeof(numeric) *n, cudaMemcpyHostToDevice, streams[k]);
        cudaMemcpyAsync(dev_data.rankmin+ncase_cumu[k], &(rankmin[k][0]), sizeof(int)*n, cudaMemcpyHostToDevice, streams[k]);
        cudaMemcpyAsync(dev_data.rankmax+ncase_cumu[k], &(rankmax[k][0]), sizeof(int)*n, cudaMemcpyHostToDevice, streams[k]);
    }

    numeric cox_val;
    numeric cox_val_next;
    numeric rhs_ls; // right-hand side of line search condition
    numeric diff; // Max norm of the difference of two consecutive iterates
    numeric lambda_1;
    numeric lambda_2;
    const int num_lambda = lambda_1_all.size();
    numeric step_size_intial = step_size;
    Rcpp::List result(num_lambda);
    bool stop; // Stop line searching
    numeric weight_old, weight_new;
    // Initialization done, starting solving the path
    struct timeval start, end;
    for (int lam_ind = 0; lam_ind < num_lambda; ++lam_ind){
        gettimeofday(&start, NULL);

        lambda_1 = lambda_1_all[lam_ind];
        lambda_2 = lambda_2_all[lam_ind];
        weight_old = 1.0;
        step_size = step_size_intial;
        // Inner iteration
        for (int i = 0; i < niter; ++i)
        {

            // Set prev_B = B
            cublas_copy(dev_param, K*p, copy_stream, handle);
            // Wait for Nesterov weight update
            cudaStreamSynchronize(nest_stream);
            // Update the gradient at v, compute cox_val at v
            cox_val = get_gradient(dev_cache,
                                    dev_data,
                                    dev_param,
                                    dev_param.grad,
                                    dev_param.v,
                                    ncase,
                                    ncase_cumu,
                                    K,
                                    p,
                                    handle,
                                    streams,
                                    true,
                                    cox_val_host);

            // Enter line search
            while(true)
            {
                // Update  B
                update_parameters(dev_param,
                                    K,
                                    p,
                                    step_size,
                                    lambda_1,
                                    lambda_2);

                // Get cox_val at updated B
                cox_val_next = get_value_only(dev_cache,
                                                dev_data,
                                                dev_param,
                                                dev_param.B,
                                                ncase,
                                                ncase_cumu,
                                                K,
                                                p,
                                                handle,
                                                streams,
                                                cox_val_host);

                stop = false;
                // This block are the line search conditions
                if(abs((cox_val_next - cox_val)/fmax(1.0, abs(cox_val_next))) > tol){
                    rhs_ls = cox_val + ls_stop_v1(dev_param, step_size,K,p);
                    stop = (cox_val_next <= rhs_ls);
                } else
                {
                    get_gradient(dev_cache,
                                dev_data,
                                dev_param,
                                dev_param.grad_ls,
                                dev_param.B,
                                ncase,
                                ncase_cumu,
                                K,
                                p,
                                handle,
                                streams);
                    rhs_ls = ls_stop_v2(dev_param, step_size,K,p);
                    stop = (rhs_ls >= 0);

                }

                if (stop)
                {
                    break;
                }
                step_size /= linesearch_beta;
            }

            diff = max_diff(dev_param, K, p);
            if (diff < eps)
            {
                std::cout << "convergence based on parameter change reached in " << i <<" iterations\n";
                std::cout << "current step size is " << step_size << std::endl;
                gettimeofday(&end, NULL);
                double delta  = ((end.tv_sec  - start.tv_sec) * 1000000u + end.tv_usec - start.tv_usec) / 1.e6;
                std::cout <<  "elapsed time is " << delta << " seconds" << std::endl;
                Rcpp::checkUserInterrupt();
                break;
            }

             // Nesterov weight
            weight_new = 0.5*(1+sqrt(1+4*weight_old*weight_old));
            nesterov_update(dev_param,K,p, weight_old, weight_new, nest_stream, handle);
            weight_old = weight_new;

            if (i != 0 && i % 100 == 0)
            {
                std::cout << "reached " << i << " iterations\n";
                gettimeofday(&end, NULL);
                double delta  = ((end.tv_sec  - start.tv_sec) * 1000000u + end.tv_usec - start.tv_usec) / 1.e6;
                std::cout <<  "elapsed time is " << delta  << " seconds" << std::endl;
                Rcpp::checkUserInterrupt();
            }

        }
        cudaMemcpy(&host_B(0,0), dev_param.B, sizeof(numeric)*K*p, cudaMemcpyDeviceToHost);
        result[lam_ind] = host_B;
        std::cout << "Solution for the " <<  lam_ind+1 << "th lambda pair is obtained\n";
    }

    free_device_memory(dev_data, dev_cache, dev_param);
    cublasDestroy(handle);
    for (int si = 0; si< K;++si){
        cudaStreamDestroy(streams[si]);
    }
    cudaStreamDestroy(copy_stream);
    cudaStreamDestroy(nest_stream);
    free(streams);
    free(ncase_cumu);
    free(ncase);
    return result;
}