#ifndef _ROBOT_PLUGIN_HH_
#define _ROBOT_PLUGIN_HH_

#include <ros/ros.h>
#include <ros/callback_queue.h>
#include <ros/subscribe_options.h>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo_braitenberg_robot/Sensor.h>
#include <thread>
#include <math.h>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

typedef Matrix<float, 2, 4> Matrix2_4f; // custom Matrix otherwise initialization doesn't seem to be working

namespace gazebo{

  /// \brief A plugin to control a MyRobot sensor.
  class RobotPlugin : public ModelPlugin{

  private:
    /// \brief Pointer to the model.
    physics::ModelPtr model;

    /// \brief A node use for ROS transport
    unique_ptr<ros::NodeHandle> rosNode;

    /// \brief A ROS subscriber
    ros::Subscriber rosSub;

    /// \brief A ROS callbackqueue that helps process messages
    ros::CallbackQueue rosQueue;

    /// \brief A thread the keeps running the rosQueue
    thread rosQueueThread;

    /// \brief Matrix used for each iteration
    Matrix2f cst;
    Matrix2_4f coeff;

    int MAX_SPEED; // speed in radian/s of wheels

    int BEHAVIOR; // behavior of robot (following/avoiding light)

  public:
    /// \brief tied to behavior
    const static int FOLLOW = 0;
    const static int AVOID = 1;

    /// \brief Constructor
    RobotPlugin() {}

    /// \brief The load function is called by Gazebo when the plugin is
    /// inserted into simulation
    /// \param[in] _model A pointer to the model that this plugin is
    /// attached to.
    /// \param[in] _sdf A pointer to the plugin's SDF element.
    virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf){
      // Safety check
      if(_model->GetJointCount() == 0){
	cerr << "Invalid joint count, MyRobot plugin not loaded\n";
	return;
      }

      // Store the model pointer for convenience.
      this->model = _model;

      // Check that the sdf elements exist, then read the values
      if (_sdf->HasElement("velocity"))
	MAX_SPEED = _sdf->Get<int>("velocity");
      if (_sdf->HasElement("behavior"))
	BEHAVIOR = _sdf->Get<int>("behavior");

      // Set up matrix
      cst << 1, 1,
	1, -1;
      switch(BEHAVIOR){
      case FOLLOW :
	coeff << 4, 6, 6, 4,
	  -4, -4, 4, 4;
	break;
      case AVOID :
	coeff << 4, 6, 6, 4,
	  4, 4, -4, -4;
	break;
      default:
	// FOLLOW
	coeff << 4, 6, 6, 4,
	  -4, -4, 4, 4;
	break;
      }

      // Initialize ros, if it has not already bee initialized.
      if(!ros::isInitialized()){
	int argc = 0;
	char **argv = NULL;
	ros::init(argc, argv, "gazebo",
		  ros::init_options::NoSigintHandler);
      }

      // Create our ROS node. This acts in a similar manner to
      // the Gazebo node
      this->rosNode.reset(new ros::NodeHandle("gazebo_client"));

      // Create a named topic, and subscribe to it.
      ros::SubscribeOptions so =
	ros::SubscribeOptions::create<gazebo_braitenberg_robot::Sensor>(
									"/lightSensor",
									100,
									boost::bind(&RobotPlugin::onRosMsg, this, _1),
									ros::VoidPtr(), &this->rosQueue);
      this->rosSub = this->rosNode->subscribe(so);

      // Spin up the queue helper thread.
      this->rosQueueThread =
	thread(bind(&RobotPlugin::QueueThread, this));
    }

    /// \brief Handle an incoming message from ROS
    /// \param[in] data Sensors data that is used to set the velocity
    /// of the MyRobot.
    void onRosMsg(const gazebo_braitenberg_robot::SensorConstPtr &msg){
      VectorXf sensors(msg->data.size());
      for(int i = 0; i < msg->data.size(); i++)
	sensors(i) = msg->data[i] / 60;

      Vector2f vel = coeff * sensors;
      Vector2f wheel_speed = cst * vel;

      float k = max(wheel_speed(0), wheel_speed(1)); // scale wheel speed on MAX_SPEED
      if(k == 0)
	k = 1;
      setVelocity(wheel_speed(0) * MAX_SPEED / k, wheel_speed(1) * MAX_SPEED / k);
    }

    /// \brief Set the velocity of the MyRobot
    /// \param[in] l New left target velocity
    /// \param[in] r New right target velocity
    void setVelocity(const double &l, const double &r){
      this->model->GetJoint("my_robot::left_wheel_hinge")->SetVelocity(0, l);
      this->model->GetJoint("my_robot::right_wheel_hinge")->SetVelocity(0, r);
    }

  private:
    /// \brief ROS helper function that processes messages
    void QueueThread(){
      static const double timeout = 0.01;
      while (this->rosNode->ok())
	{
	  this->rosQueue.callAvailable(ros::WallDuration(timeout));
	}
    }
  };

  // Tell Gazebo about this plugin, so that Gazebo can call Load on this plugin.
  GZ_REGISTER_MODEL_PLUGIN(RobotPlugin)
}
#endif


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

  @copyright 2016 NumScale SAS

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

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

 /*!

    @ingroup group-trigonometric
    Function object implementing atan2d capabilities

    atan2d function : atan2 in degrees.

    @par Semantic:

    For every parameters of same floating type

    @code
    auto r = atan2d(y, x);
    @endcode

    is similar  to:

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

    For any real arguments @c x and @c y not both equal to zero, <tt>atan2d(y, x)</tt>
    is the angle in degrees between the positive x-axis of a plane and the point
    given by the coordinates  <tt>(x, yx)</tt>.

    It is also the angle in \f$[-180,180[\f$ for which
    \f$x/\sqrt{x^2+y^2}\f$ and \f$y/\sqrt{x^2+y^2}\f$
    are respectively the sine and the cosine.

    @see atand, atan2, atan

  **/
  Value atan2d(Value const& x, Value const& y );
} }
#endif

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

#endif


/** @file SymmetricBetaDist.cpp
 * @author Mark J. Olah (mjo\@cs.unm DOT edu)
 * @date 2017-2019
 * @brief SymmetricBetaDist class definition
 *
 */
#include "PriorHessian/SymmetricBetaDist.h"
#include "PriorHessian/PriorHessianError.h"

#include <sstream>
#include <cmath>
#include <limits>

#include <boost/math/special_functions/beta.hpp>

namespace prior_hessian {

const StringVecT SymmetricBetaDist::_param_names = { "beta" };
const SymmetricBetaDist::NparamsVecT SymmetricBetaDist::_param_lbound = { 0 }; //Lower bound on valid parameter values
const SymmetricBetaDist::NparamsVecT SymmetricBetaDist::_param_ubound = { INFINITY }; //Upper bound on valid parameter values

/* Constructors */
SymmetricBetaDist::SymmetricBetaDist(double beta)
    : UnivariateDist(),
      _beta(checked_beta(beta)),
      llh_const_initialized(false)
{ }

/* Non-static member functions */
void  SymmetricBetaDist::set_beta(double val)
{
    _beta = checked_beta(val);
    llh_const_initialized = false;
}

double SymmetricBetaDist::cdf(double x) const
{
    if(x==0) return 0;
    if(x==1) return 1;
    return boost::math::ibeta(_beta, _beta, x);
}

double SymmetricBetaDist::icdf(double u) const
{
    if(u==0) return 0;
    if(u==1) return 1;
    return boost::math::ibeta_inv(_beta, _beta, u);
}

double SymmetricBetaDist::pdf(double x) const
{
   return boost::math::ibeta_derivative(_beta, _beta, x);
}

double SymmetricBetaDist::llh(double x) const
{
    if(!llh_const_initialized) initialize_llh_const();
    return rllh(x) + llh_const;
}

void SymmetricBetaDist::initialize_llh_const() const
{
    llh_const = compute_llh_const(beta());
    llh_const_initialized = true;
}

double SymmetricBetaDist::compute_llh_const(double beta)
{
    return -2*lgamma(beta) - lgamma(2*beta);//log(1/Beta(beta,beta))
}

double SymmetricBetaDist::checked_beta(double val)
{
    if(val<=0 || !std::isfinite(val)) {
        std::ostringstream msg;
        msg<<"SymmetricBetaDist: got bad beta value:"<<val;
        throw ParameterValueError(msg.str());
    }
    return val;
}

} /* namespace prior_hessian */


#ifndef IGLP_HPP
#define IGLP_HPP

#include <glpk.h>
#include <Eigen/Eigen>

#include <cstdio>
#include <cstdlib>
#include <string>

namespace iglp
{

inline double linprog(const Eigen::VectorXd &c,
                      const Eigen::MatrixXd &A,
                      const Eigen::VectorXd &b,
                      Eigen::VectorXd &x,
                      bool ipm = false,
                      bool verbose = false)
// linprog:
//         min cTx s.t. Ax<=b
// input:
//         c: d*1 objective coeffs
//         A: m*d constraint matrix
//         b: m*1 constraint bound
//         ipm: use interior point method
//              or simplex method
//         verbose: show details
// output:
//         x: d*1 decision variables
// return:
//         inf: No feasible solution or fail
//        -inf: Unbounded problem
//         real: minimum objective function
{
    int d = c.size();
    int m = b.size();
    int dm = d * m;
    x = Eigen::VectorXd::Zero(d);

    glp_prob *lp;
    int *ia = new int[dm + 1];
    int *ja = new int[dm + 1];
    double *ar = new double[dm + 1];
    int s;
    double z;

    lp = glp_create_prob();
    glp_set_prob_name(lp, "lp");
    glp_set_obj_dir(lp, GLP_MIN);

    glp_add_rows(lp, m);
    for (int i = 1; i <= m; i++)
    {
        glp_set_row_name(lp, i, (std::to_string(i) + "y").c_str());
        glp_set_row_bnds(lp, i, GLP_UP, 0.0, b(i - 1));
    }

    glp_add_cols(lp, d);
    for (int i = 1; i <= d; i++)
    {
        glp_set_col_name(lp, i, (std::to_string(i) + "x").c_str());
        glp_set_col_bnds(lp, i, GLP_FR, 0.0, 0.0);
        glp_set_obj_coef(lp, i, c(i - 1));
    }

    int k = 1;
    for (int i = 1; i <= m; i++)
    {
        for (int j = 1; j <= d; j++)
        {
            ia[k] = i;
            ja[k] = j;
            ar[k] = A(i - 1, j - 1);
            k++;
        }
    }
    glp_load_matrix(lp, dm, ia, ja, ar);

    if (!ipm)
    {
        glp_smcp param;
        glp_init_smcp(&param);
        param.msg_lev = verbose ? GLP_MSG_ALL : GLP_MSG_OFF;
        glp_simplex(lp, &param);
        s = glp_get_status(lp);
        z = INFINITY;
        if (s == GLP_OPT || s == GLP_UNBND)
        {
            z = (s == GLP_UNBND) ? -INFINITY : glp_get_obj_val(lp);
            for (int i = 1; i <= d; i++)
            {
                x(i - 1) = glp_get_col_prim(lp, i);
            }
        }
    }
    else
    {
        glp_iptcp param;
        glp_init_iptcp(&param);
        param.msg_lev = verbose ? GLP_MSG_ALL : GLP_MSG_OFF;
        glp_interior(lp, &param);
        s = glp_ipt_status(lp);
        z = INFINITY;
        if (s == GLP_OPT)
        {
            z = glp_ipt_obj_val(lp);
            for (int i = 1; i <= d; i++)
            {
                x(i - 1) = glp_ipt_col_prim(lp, i);
            }
        }
    }

    glp_delete_prob(lp);
    delete[] ia;
    delete[] ja;
    delete[] ar;

    return z;
}

} // namespace iglp

#endif


//  (C) Copyright Christopher Kormanyos 1999 - 2021.
//  (C) Copyright Matt Borland 2021.
//  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)

#ifndef BOOST_MATH_CCMATH_FREXP_HPP
#define BOOST_MATH_CCMATH_FREXP_HPP

#include <cmath>
#include <limits>
#include <type_traits>
#include <boost/math/ccmath/isinf.hpp>
#include <boost/math/ccmath/isnan.hpp>
#include <boost/math/ccmath/isfinite.hpp>

namespace boost::math::ccmath {

namespace detail
{

template <typename Real>
inline constexpr Real frexp_zero_impl(Real arg, int* exp)
{
    *exp = 0;
    return arg;
}

template <typename Real>
inline constexpr Real frexp_impl(Real arg, int* exp)
{
    const bool negative_arg = (arg < Real(0));

    Real f = negative_arg ? -arg : arg;
    int e2 = 0;
    constexpr Real two_pow_32 = Real(4294967296);

    while (f >= two_pow_32)
    {
        f = f / two_pow_32;
        e2 += 32;
    }

    while(f >= Real(1))
    {
        f = f / Real(2);
        ++e2;
    }

    if(exp != nullptr)
    {
        *exp = e2;
    }

    return !negative_arg ? f : -f;
}

} // namespace detail

template <typename Real, std::enable_if_t<!std::is_integral_v<Real>, bool> = true>
inline constexpr Real frexp(Real arg, int* exp)
{
    if(BOOST_MATH_IS_CONSTANT_EVALUATED(arg))
    {
        return arg == Real(0)  ? detail::frexp_zero_impl(arg, exp) :
               arg == Real(-0) ? detail::frexp_zero_impl(arg, exp) :
               boost::math::ccmath::isinf(arg) ? detail::frexp_zero_impl(arg, exp) :
               boost::math::ccmath::isnan(arg) ? detail::frexp_zero_impl(arg, exp) :
               boost::math::ccmath::detail::frexp_impl(arg, exp);
    }
    else
    {
        using std::frexp;
        return frexp(arg, exp);
    }
}

template <typename Z, std::enable_if_t<std::is_integral_v<Z>, bool> = true>
inline constexpr double frexp(Z arg, int* exp)
{
    return boost::math::ccmath::frexp(static_cast<double>(arg), exp);
}

inline constexpr float frexpf(float arg, int* exp)
{
    return boost::math::ccmath::frexp(arg, exp);
}

#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
inline constexpr long double frexpl(long double arg, int* exp)
{
    return boost::math::ccmath::frexp(arg, exp);
}
#endif

}

#endif // BOOST_MATH_CCMATH_FREXP_HPP


#include <iostream>
#include <fstream>
#include <sstream>
#include <iomanip>
#include <vector>
#include <cmath>
#include <algorithm>
#include <cstddef>
#include <cassert>
#include <limits>
#include <Eigen/Sparse>

using namespace std;

class Array2D
{
private:
	vector<double> m_data;
	size_t m_Nx, m_Ny;

public:
	Array2D(size_t nx, size_t ny, double val = 0.0) : m_Nx(nx), m_Ny(ny), m_data(nx*ny, val) {}

	// 0-based indexing
	double &at(int i, int j)
	{
		int idx = i + m_Nx * j;
		return m_data[idx];
	}

	double at(int i, int j) const
	{
		int idx = i + m_Nx * j;
		return m_data[idx];
	}

	// 1-based indexing
	double &operator()(int i, int j)
	{
		return at(i - 1, j - 1);
	}

	double operator()(int i, int j) const
	{
		return at(i - 1, j - 1);
	}
};

const size_t WIDTH = 16;
const size_t DIGITS = 7;

const double L = 0.5; // m
const double D = 0.01; // m
const double Ue = 1.0; // m/s
const double Pe = 0.0;
const double rho = 1.225; // Kg/m3
const double mu = 3.737e-5; // Kg/m/s

const int Nx = 21, Ny = 11;
const double dx = L / (Nx - 1), dy = D / (Ny - 1);
const double dx2 = 2 * dx, dy2 = 2 * dy;
const double dxdx = dx * dx, dydy = dy * dy;
vector<double> x(Nx, 0.0), y(Ny, 0.0);

const double dt = 0.001;
double t = 0.0;
int iter_cnt = 0;
const int MAX_ITER_NUM = 2000;

const double a = 2 * (dt / dxdx + dt / dydy);
const double b = -dt / dxdx;
const double c = -dt / dydy;
double d_min = numeric_limits<double>::max(), d_max = numeric_limits<double>::min(), d_15_5 = 0.0;

Array2D p(Nx, Ny, Pe), p_star(Nx, Ny, Pe), p_prime(Nx, Ny, 0.0);
Array2D u(Nx + 1, Ny, 0.0), u_wedge(Nx + 1, Ny, 0.0), u_star(Nx + 1, Ny, 0.0), u_prime(Nx + 1, Ny, 0.0);
Array2D v(Nx + 2, Ny + 1, 0.0), v_wedge(Nx + 2, Ny + 1, 0.0), v_star(Nx + 2, Ny + 1, 0.0), v_prime(Nx + 2, Ny + 1, 0.0);

// Full flowfield in TECPLOT ASCII Format.
void output1(void)
{
	Array2D u_interp(Nx, Ny, 0.0);
	for (int i = 1; i <= Nx; ++i)
		u_interp(i, 1) = 0.0; // Bottom
	for (int j = 2; j <= Ny - 1; ++j)
		for (int i = 1; i <= Nx; ++i)
			u_interp(i, j) = (u(i, j) + u(i + 1, j)) / 2; // Inner
	for (int i = 1; i <= Nx; ++i)
		u_interp(i, Ny) = Ue; // Top

	Array2D v_interp(Nx, Ny, 0.0);
	for (int i = 1; i <= Nx; ++i)
		v_interp(i, 1) = 0.0; // Bottom
	for (int j = 2; j <= Ny - 1; ++j)
	{
		v_interp(1, j) = 0.0; // Left
		for (int i = 3; i <= Nx + 1; ++i)
			v_interp(i - 1, j) = (v(i, j) + v(i, j + 1)) / 2; // Inner and Right
	}
	for (int i = 1; i <= Nx; ++i)
		v_interp(i, Ny) = 0.0; // Top

	// Create Tecplot data file.
	ofstream result("flow" + to_string(iter_cnt) + ".dat");
	if (!result)
		throw("Failed to create data file!");

	// Header
	result << "TITLE = \"t=" << t << "\"" << endl;
	result << "VARIABLES = \"X\", \"Y\", \"P\", \"U\", \"V\"" << endl;
	result << "ZONE I=" << Nx << ", J=" << Ny << ", F=POINT" << endl;

	// Flowfield data
	for (int j = 1; j <= Ny; ++j)
		for (int i = 1; i <= Nx; ++i)
		{
			result << setw(WIDTH) << setprecision(DIGITS) << x[i - 1];
			result << setw(WIDTH) << setprecision(DIGITS) << y[j - 1];
			result << setw(WIDTH) << setprecision(DIGITS) << p(i, j);
			result << setw(WIDTH) << setprecision(DIGITS) << u_interp(i, j);
			result << setw(WIDTH) << setprecision(DIGITS) << v_interp(i, j);
			result << endl;
		}

	// Finalize
	result.close();
}

// Statistics at (15, 5) and i=15
void output2(int iter)
{
	static const string fn("history_at_15_5.txt");

	ofstream fout;
	if (iter == 0)
	{
		fout.open(fn, ios::out);
		if (!fout)
			throw("Failed to open history file.");

		for (int j = 0; j < Ny; ++j)
			fout << setw(WIDTH) << setprecision(DIGITS) << y[j];
		fout << endl;
	}
	else
	{
		fout.open(fn, ios::app);
		if (!fout)
			throw("Failed to open history file.");
	}

	for (int j = 1; j <= Ny; ++j)
		fout << setw(WIDTH) << setprecision(DIGITS) << u(15, j);
	fout << endl;
	for (int j = 1; j <= Ny; ++j)
		fout << setw(WIDTH) << setprecision(DIGITS) << v(15, j);
	fout << endl;
	fout << d_15_5 << endl;

	fout.close();
}

void init(void)
{
	cout << "mu=" << mu << endl;
	cout << "dt=" << dt << endl;

	// Init
	for (int i = 1; i < Nx; ++i)
		x[i] = L * i / (Nx - 1); // X-Coordinates
	for (int j = 1; j < Ny; ++j)
		y[j] = D * j / (Ny - 1); // Y-Coordinates

	for (int i = 1; i <= Nx + 1; ++i)
		u(i, Ny) = u_wedge(i, Ny) = u_star(i, Ny) = Ue; // U at top
	v(15, 5) = v_wedge(15, 5) = v_star(15, 5) = 0.5; // Initial peak to ensure 2D flow structure
}

// Solve the pressure equation.
void ImplicitMethod1()
{
	typedef Eigen::SparseMatrix<double> SpMat;
	typedef Eigen::Triplet<double> T;

	const int m = Nx * Ny;
	vector<T> coef;
	Eigen::VectorXd rhs(m);
	SpMat A(m, m);

	// Calculating coefficients
	for (int i = 0; i < Nx; ++i)
		for (int j = 0; j < Ny; ++j)
		{
			const int id = j * Nx + i;
			const int id_w = id - 1;
			const int id_e = id + 1;
			const int id_n = id + Nx;
			const int id_s = id - Nx;

			if (i == 0 || i == Nx - 1) // Inlet and Outlet
			{
				coef.push_back(T(id, id, 1.0));
				rhs(id) = Pe;
			}
			else if (j == 0) // Bottom
			{
				coef.push_back(T(id, id_n, 1.0));
				coef.push_back(T(id, id, -1.0));
				const double ddvddx = 0.0;
				//const double ddvddy = 4.0 / 3 * (v_wedge.at(i + 1, j + 2) - 3 * v_wedge.at(i + 1, j + 1)) / dydy;
				const double ddvddy = 0.0;
				rhs(id) = mu * (ddvddx + ddvddy) * dy;
			}
			else if (j == Ny - 1) // Top
			{
				coef.push_back(T(id, id, 1.0));
				coef.push_back(T(id, id_s, -1.0));
				const double ddvddx = 0.0;
				//const double ddvddy = 4.0 / 3 * (v_wedge.at(i + 1, j - 1) - 3 * v_wedge.at(i + 1, j)) / dydy;
				const double ddvddy = 0.0;
				rhs(id) = mu * (ddvddx + ddvddy) * dy;
			}
			else // Inner
			{
				// Use 0-based interface
				const double d = (rho*u_wedge.at(i + 1, j) - rho * u_wedge.at(i, j)) / dx + (rho*v_wedge.at(i + 1, j + 1) - rho * v_wedge.at(i + 1, j)) / dy;

				coef.push_back(T(id, id, a));
				coef.push_back(T(id, id_w, b));
				coef.push_back(T(id, id_e, b));
				coef.push_back(T(id, id_n, c));
				coef.push_back(T(id, id_s, c));
				rhs(id) = -d;
			}
		}

	// Construct sparse matrix
	A.setFromTriplets(coef.begin(), coef.end());

	// Solve the linear system: Ax = rhs
	Eigen::SimplicialCholesky<SpMat> chol(A);
	Eigen::VectorXd x = chol.solve(rhs);

	// Update p
	for (int i = 0; i < Nx; ++i)
		for (int j = 0; j < Ny; ++j)
		{
			const int id = j * Nx + i;
			p.at(i, j) = x(id);
		}
}

// Solve the pressure-correction equation
void ImplicitMethod2()
{
	typedef Eigen::SparseMatrix<double> SpMat;
	typedef Eigen::Triplet<double> T;

	const int m = Nx * Ny;
	vector<T> coef;
	Eigen::VectorXd rhs(m);
	SpMat A(m, m);

	// Calculating coefficients
	for (int i = 0; i < Nx; ++i)
		for (int j = 0; j < Ny; ++j)
		{
			const int id = j * Nx + i;
			const int id_w = id - 1;
			const int id_e = id + 1;
			const int id_n = id + Nx;
			const int id_s = id - Nx;

			if (i == 0 || i == Nx - 1) // Inlet and Outlet
			{
				coef.push_back(T(id, id, 1.0));
				rhs(id) = 0.0;
			}
			else if (j == 0) // Bottom
			{
				coef.push_back(T(id, id, 1.0));
				coef.push_back(T(id, id_n, -1.0));
				rhs(id) = 0.0;
			}
			else if (j == Ny - 1) // Top
			{
				coef.push_back(T(id, id, 1.0));
				coef.push_back(T(id, id_s, -1.0));
				rhs(id) = 0.0;
			}
			else // Inner
			{
				// Use 0-based interface
				const double d = (rho*u_star.at(i + 1, j) - rho * u_star.at(i, j)) / dx + (rho * v_star.at(i + 1, j + 1) - rho * v_star.at(i + 1, j)) / dy;
				if (d > d_max)
					d_max = d;
				if (d < d_min)
					d_min = d;
				if (i == 15 && j == 5)
					d_15_5 = d;

				coef.push_back(T(id, id, a));
				coef.push_back(T(id, id_w, b));
				coef.push_back(T(id, id_e, b));
				coef.push_back(T(id, id_n, c));
				coef.push_back(T(id, id_s, c));
				rhs(id) = -d;
			}
		}

	// Construct sparse matrix
	A.setFromTriplets(coef.begin(), coef.end());

	// Solve the linear system: Ax = rhs
	Eigen::SimplicialCholesky<SpMat> chol(A);
	Eigen::VectorXd x = chol.solve(rhs);

	// Update p_prime
	for (int i = 0; i < Nx; ++i)
		for (int j = 0; j < Ny; ++j)
		{
			const int id = j * Nx + i;
			p_prime.at(i, j) = x(id);
		}
}

void SIMPLER(void)
{
	// u_wedge at inner points
	for (int j = 2; j <= Ny - 1; ++j)
		for (int i = 2; i <= Nx; ++i)
		{
			double v_bar1 = 0.5*(v(i, j + 1) + v(i + 1, j + 1));
			double v_bar2 = 0.5*(v(i, j) + v(i + 1, j));

			double t11 = rho * pow(u(i + 1, j), 2) - rho * pow(u(i - 1, j), 2);
			double t12 = rho * u(i, j + 1)*v_bar1 - rho * u(i, j - 1)*v_bar2;
			double t21 = u(i + 1, j) - 2 * u(i, j) + u(i - 1, j);
			double t22 = u(i, j + 1) - 2 * u(i, j) + u(i, j - 1);
			double A = -(t11 / dx2 + t12 / dy2) + mu * (t21 / dxdx + t22 / dydy);

			u_wedge(i, j) = (rho * u(i, j) + A * dt) / rho;
		}

	// v_wedge at inner points
	for (int i = 3; i <= Nx; ++i)
		for (int j = 2; j <= Ny; ++j)
		{
			double u_bar1 = 0.5 *(u(i, j - 1) + u(i, j));
			double u_bar2 = 0.5 *(u(i - 1, j - 1) + u(i - 1, j));

			double t11 = rho * v(i + 1, j) * u_bar1 - rho * v(i - 1, j) * u_bar2;
			double t12 = rho * pow(v(i, j + 1), 2) - rho * pow(v(i, j - 1), 2);
			double t21 = v(i + 1, j) - 2 * v(i, j) + v(i - 1, j);
			double t22 = v(i, j + 1) - 2 * v(i, j) + v(i, j - 1);
			double B = -(t11 / dx2 + t12 / dy2) + mu * (t21 / dxdx + t22 / dydy);

			v_wedge(i, j) = (rho * v(i, j) + B * dt) / rho;
		}

	// Solve p
	ImplicitMethod1();

	// Set p_star to p
	for (int j = 1; j <= Ny; ++j)
		for (int i = 1; i <= Nx; ++i)
			p_star(i, j) = p(i, j);

	// u_star at inner points
	for (int j = 2; j <= Ny - 1; ++j)
		for (int i = 2; i <= Nx; ++i)
		{
			double v_bar1 = 0.5*(v(i, j + 1) + v(i + 1, j + 1));
			double v_bar2 = 0.5*(v(i, j) + v(i + 1, j));

			double t11 = rho * pow(u(i + 1, j), 2) - rho * pow(u(i - 1, j), 2);
			double t12 = rho * u(i, j + 1)*v_bar1 - rho * u(i, j - 1)*v_bar2;
			double t21 = u(i + 1, j) - 2 * u(i, j) + u(i - 1, j);
			double t22 = u(i, j + 1) - 2 * u(i, j) + u(i, j - 1);
			double A = -(t11 / dx2 + t12 / dy2) + mu * (t21 / dxdx + t22 / dydy);

			u_star(i, j) = (rho * u(i, j) + A * dt - dt / dx * (p_star(i, j) - p_star(i - 1, j))) / rho;
		}

	// v_star at inner points
	for (int i = 3; i <= Nx + 1; ++i)
		for (int j = 2; j <= Ny; ++j)
		{
			double u_bar1 = 0.5 *(u(i, j - 1) + u(i, j));
			double u_bar2 = 0.5 *(u(i - 1, j - 1) + u(i - 1, j));

			double t11 = rho * v(i + 1, j) * u_bar1 - rho * v(i - 1, j) * u_bar2;
			double t12 = rho * pow(v(i, j + 1), 2) - rho * pow(v(i, j - 1), 2);
			double t21 = v(i + 1, j) - 2 * v(i, j) + v(i - 1, j);
			double t22 = v(i, j + 1) - 2 * v(i, j) + v(i, j - 1);
			double B = -(t11 / dx2 + t12 / dy2) + mu * (t21 / dxdx + t22 / dydy);

			v_star(i, j) = (rho * v(i, j) + B * dt - dy / dy * (p_star(i - 1, j) - p_star(i - 1, j - 1))) / rho;
		}

	d_min = numeric_limits<double>::max();
	d_max = numeric_limits<double>::min();
	ImplicitMethod2();

	// Correct u at inner nodes
	for (int j = 2; j <= Ny - 1; ++j)
		for (int i = 2; i <= Nx; ++i)
		{
			u_prime(i, j) = -dt / dx * (p_prime(i, j) - p_prime(i - 1, j)) / rho; // u_prime
			u(i, j) = u_star(i, j) + u_prime(i, j);
		}

	// Linear extrapolation of u at virtual nodes
	for (int j = 2; j <= Ny - 1; ++j)
	{
		u(1, j) = 2 * u(2, j) - u(3, j);
		u(Nx + 1, j) = 2 * u(Nx, j) - u(Nx - 1, j);
	}

	// Correct v at inner nodes
	for (int i = 3; i <= Nx + 1; ++i)
		for (int j = 2; j <= Ny; ++j)
		{
			v_prime(i, j) = -dt / dy * (p_prime(i - 1, j) - p_prime(i - 1, j - 1)) / rho; // v_prime
			v(i, j) = v_star(i, j) + v_prime(i, j);
		}

	// Linear extrapolation of v at both top and bottom virtual nodes
	// No-Penetration at both top and bottom
	for (int i = 2; i <= Nx + 1; ++i)
	{
		v(i, 1) = -v(i, 2);
		v(i, Ny + 1) = -v(i, Ny);
	}

	// Linear extrapolation of v at right virtual nodes
	for (int j = 2; j <= Ny; ++j)
		v(Nx + 2, j) = 2 * v(Nx + 1, j) - v(Nx, j);
}

bool check_convergence(void)
{
	// Statistics of the mass flux residue
	cout << "Max(d)=" << d_max << " Min(d)=" << d_min << endl;

	// Statistics of u
	double u_max = numeric_limits<double>::min();
	double u_min = numeric_limits<double>::max();
	for (int i = 2; i <= Nx; ++i)
		for (int j = 1; j <= Ny; ++j)
		{
			u_max = max(u_max, u(i, j));
			u_min = min(u_min, u(i, j));
		}
	cout << "Max(u)=" << u_max << " Min(u)=" << u_min << endl;

	// Statistics of v
	double v_max = numeric_limits<double>::min();
	double v_min = numeric_limits<double>::max();
	for (int i = 2; i <= Nx + 1; ++i)
		for (int j = 2; j <= Ny; ++j)
		{
			v_max = max(v_max, v(i, j));
			v_min = min(v_min, v(i, j));
		}
	cout << "Max(v)=" << v_max << " Min(v)=" << v_min << endl;

	// Statistics of p
	double p_max = numeric_limits<double>::min();
	double p_min = numeric_limits<double>::max();
	for (int i = 1; i <= Nx; ++i)
		for (int j = 1; j <= Ny; ++j)
		{
			p_max = max(p_max, p(i, j));
			p_min = min(p_min, p(i, j));
		}
	cout << "Max(p)=" << p_max << " Min(p)=" << p_min << endl;

	return iter_cnt > MAX_ITER_NUM || max(abs(d_max), abs(d_min)) < 1e-4;
}

void loop(void)
{
	bool converged = false;
	while (!converged)
	{
		++iter_cnt;
		cout << "Iter" << iter_cnt << ":" << endl;

		SIMPLER();
		t += dt;

		output1();
		output2(iter_cnt);

		converged = check_convergence();
	}
}

int main(int argc, char *argv[])
{
	// Initialize
	init();

	// Output I.C.
	output1();
	output2(0);

	// Solve
	loop();

	return 0;
}


/*!=======================================================
  |                                                     |
  |                 test_tensor.cpp                     |
  |                                                     |
  -------------------------------------------------------
  | The unit test file for tensor.h/cpp.                |
  | This file tests the classes and functions defined   |
  | in tensor.h/cpp.                                    |
  |                                                     |
  | Generated files:                                    |
  |    Results.tex:  A LaTeX file which contains the    |
  |                  results as they will be included   |
  |                  in the generated report.           |
  =======================================================
  | Dependencies:                                       |
  | Eigen: An implementation of various matrix          |
  |        commands. The implementation of the data     |
  |        matrix uses such a matrix.                   |
  =======================================================*/

#include <functional>
#include <iostream>
#include <fstream>
#include <numeric>
#include <vector>
#include <Eigen/Dense>
#include <tensor.h>
#include <ctime>

int test_tensor_functionality(std::ofstream &results){
    /*!====================================
    |       test_tensor_functionality   |
    =====================================

    A test of some of the basic tensor functionality.
    This should show that tensors of different orders
    can be generated and manipulated.*/

    int  test_num        = 6;
    std::vector<bool> test_results(test_num,false);

    //Define a test vector
    std::vector< int > v_shape;                 //Vector shape vector
    v_shape.resize(1);                          //Resizing the shape vector to have only one index
    v_shape[0] = 6;                             //Setting the vector size
    tensor::Tensor V(v_shape);                  //Form the vector

    Eigen::MatrixXd V_compare(6,1); //Initialize the comparison matrix

    V_compare << 1,2,3,4,5,6;  //Set the initial values

    double inc = 1; //Set an initial increment vector

    //!Compare expected storage pattern vs. actual for a vector
    for(int i=0; i<v_shape[0]; i++){//Iterate through the vector setting the required values
        V(i) = inc;                 //Set the i'th value of V equal to inc
        inc++;                      //Increment inc
    }

    test_results[0] = V_compare.isApprox(V.data); //Check the results of the test

    //!Setting index test for a vector
    V_compare(3) = -1; //Set the 4th index value to -1
    V(3)         = -1; //Do the same for the tensor

    test_results[1] = V_compare.isApprox(V.data); //Check the results of the test

    //Define a test matrix
    std::vector < int > m_shape; //Matrix shape vector
    m_shape.resize(2);           //Resizing the shape vector to having two indices

    m_shape[0] = 3;              //Create a 3x3 matrix
    m_shape[1] = 3;

    tensor::Tensor M(m_shape);   //Initialize the matrix

    Eigen::MatrixXd M_compare(3,3); //Initialize the comparison matrix
    M_compare << 1,2,3,4,5,6,7,8,9;  //Set the initial values

    //!Compare expected storage pattern to actual for a 2nd order tensor
    inc = 1; //Set an initial increment vector

    for(int i=0; i<m_shape[0]; i++){
        for(int j=0; j<m_shape[1]; j++){
            M(i,j) = inc;
            inc++;
        }
    }

    test_results[2] =  M_compare.isApprox(M.data); //Check the results of the test

    //!Setting index test for a 2nd order tensor
    M_compare(2,1) = -8;
    M(2,1)         = -8;

    test_results[3] =  M_compare.isApprox(M.data); //Check the results of the test

    //Define a test 3rd order tensor
    std::vector < int > t_shape;                //Initialize the the tensor shape vector
    t_shape.resize(3);                          //Resize the shape vector of the tensor

    t_shape[0] = 4;                             //Initialize the tensor size
    t_shape[1] = 3;
    t_shape[2] = 7;

    tensor::Tensor T(t_shape);                  //Initialize the tensor

    Eigen::MatrixXd T_compare(4,21); //Initialize the comparison tensor
    T_compare <<  1,  2,  3,  4,  5,  6,  7,  8,  9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21,
                 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42,
                 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63,
                 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84;

    //!Compare expected storage pattern to actual for a 3rd order tensor
    inc = 1; //Set an initial increment vector

    for(int i=0; i<t_shape[0]; i++){
        for(int j=0; j<t_shape[1]; j++){
            for(int k=0; k<t_shape[2]; k++){
                T(i,j,k) = inc;
                inc++;
            }
        }
    }

    test_results[4] =  T_compare.isApprox(T.data); //Check the results of the test

    //!Setting index test for a 3nd order tensor
    T_compare(2,13) = -8;
    T(2,1,6)        = -8;

    test_results[5] =  T_compare.isApprox(T.data); //Check the results of the test

    //std::cout << "\nT_compare:\n" << T_compare << "\n" << "T:\n" << T.data << "\n";

    //Compare all test results
    bool tot_result = true;
    for(int i = 0; i<test_num; i++){
        //std::cout << "\nSub-test " << i+1 << " result: " << test_results[i] << "\n";
        if(!test_results[i]){
            tot_result = false;
        }
    }

    if(tot_result){
        results << "test_tensor_functionality & True\\\\\n\\hline\n";
    }
    else{
        results << "test_tensor_functionality & False\\\\\n\\hline\n";
    }

    return 1;

}

int test_eye(std::ofstream &results){
    /*!========================
    |       test_eye       |
    ========================

    A test of the generation of the second
    order identity tensor. Note that with
    different storage schemes this tensor
    may not always be the same in terms of
    the way it is stored.*/

    //Initialize the results
    int  test_num        = 2;
    std::vector<bool> test_results(test_num,false);

    //Compute the identity tensor
    tensor::Tensor23 I = tensor::eye();

    //Define a test matrix
    std::vector < int > m_shape; //Matrix shape vector
    m_shape.resize(2);           //Resizing the shape vector to having two indices

    m_shape[0] = 3;              //Create a 3x3 tensor
    m_shape[1] = 3;

    tensor::Tensor M = tensor::Tensor(m_shape); //Initialize the matrix
    M.data << 1,2,3,4,5,6,7,8,9;  //Set the initial values

    //!Run multiplication tests
    tensor::Tensor R1 = tensor::Tensor(m_shape); //Initialize the first results matrix
    tensor::Tensor R2 = tensor::Tensor(m_shape); //Initialize the second results matrix
    for(int i=0; i<3; i++){
        for(int j=0; j<3; j++){
            for(int k=0; k<3; k++){
                R1(i,j) += I(i,k)*M(k,j);
                R2(i,j) += M(i,k)*I(k,j);
            }
        }
    }

    //Check the results of the tests
    test_results[0] = R1.data.isApprox(M.data);
    test_results[1] = R2.data.isApprox(M.data);

    //Compare all test results
    bool tot_result = true;
    for(int i = 0; i<test_num; i++){
        //std::cout << "\nSub-test " << i+1 << " result: " << test_results[i] << "\n";
        if(!test_results[i]){
            tot_result = false;
        }
    }

    if(tot_result){
        results << "test_eye & True\\\\\n\\hline\n";
    }
    else{
        results << "test_eye & False\\\\\n\\hline\n";
    }

}

int test_FOT_eye(std::ofstream &results){
    /*!========================
    |     test_FOT_eye     |
    ========================

    A test of the generation of the fourth
    order identity tensor. Note that with
    different storage schemes this tensor
    may not always be the same in terms of
    the way it is stored.*/

    //Initialize the results
    int  test_num        = 2;
    std::vector<bool> test_results(test_num,false);

    //Compute the identity tensor
    tensor::Tensor43 FOTI = tensor::FOT_eye();

    //Define a test matrix
    std::vector < int > m_shape; //Matrix shape vector
    m_shape.resize(4);           //Resizing the shape vector to having two indices

    m_shape[0] = 3;              //Create a 3x3x3x3 tensor
    m_shape[1] = 3;
    m_shape[2] = 3;
    m_shape[3] = 3;

    tensor::Tensor FOT = tensor::Tensor(m_shape); //Initialize the matrix
    FOT.data << 2,4,3,5,1,3,4,6,1,
                4,5,2,7,2,5,3,5,7,
                6,2,8,3,6,2,6,4,5,
                7,2,9,1,5,3,7,3,5,
                9,8,9,4,1,2,4,3,5,
                4,2,5,6,2,7,4,5,3,
                6,6,5,2,4,1,5,4,8,
                9,1,3,2,2,4,5,6,7,
                5,2,1,2,1,1,4,5,6;

    //!Run multiplication tests
    tensor::Tensor R1 = tensor::Tensor(m_shape); //Initialize the first results matrix
    tensor::Tensor R2 = tensor::Tensor(m_shape); //Initialize the second results matrix
    for(int i=0; i<3; i++){
        for(int j=0; j<3; j++){
            for(int k=0; k<3; k++){
                for(int l=0; l<3; l++){
                    for(int m=0; m<3; m++){
                        for(int n=0; n<3; n++){
                            R1(i,j,k,l) += FOTI(i,j,m,n)*FOT(m,n,k,l);
                            R2(i,j,k,l) += FOT(i,j,m,n)*FOTI(m,n,k,l);
                        }
                    }
                }
            }
        }
    }

    //Check the results of the tests
    test_results[0] = R1.data.isApprox(FOT.data);
    test_results[1] = R2.data.isApprox(FOT.data);

    //Compare all test results
    bool tot_result = true;
    for(int i = 0; i<test_num; i++){
        //std::cout << "\nSub-test " << i+1 << " result: " << test_results[i] << "\n";
        if(!test_results[i]){
            tot_result = false;
        }
    }

    if(tot_result){
        results << "test_FOT_eye & True\\\\\n\\hline\n";
    }
    else{
        results << "test_FOT_eye & False\\\\\n\\hline\n";
    }

}

int test_inverse(std::ofstream &results){
    /*!========================
    |     test_inverse     |
    ========================

    A test of the inverse of the tensor computation.
    Several different tensor formulations are compared
    and examined to make sure that the product of the
    inverse and the original tensor is the identity
    tensor.*/

    int  test_num               = 2;
    std::vector<bool> test_results(test_num,false);

    /*!Test the inverse of a second order tensor*/
    std::vector< int > m_shape; //Initialize the shape vector
    m_shape.resize(2);          //Resize the shape vector to a second order tensor

    m_shape[0] = 3;             //Set the tensor dimensions
    m_shape[1] = 3;

    tensor::Tensor T = tensor::Tensor(m_shape); //Initialize and populate the tensor
    T.data << 2,4,3,5,1,3,4,6,1;

    tensor::Tensor Tinv = T.inverse(); //Invert the tensor

    tensor::Tensor product = tensor::Tensor(m_shape); //Compute the product of the tensor and its inverse

    for(int i=0; i<3; i++){
        for(int j=0; j<3; j++){
            for(int k=0; k<3; k++){
                product(i,j) += T(i,k)*Tinv(k,j);
            }
        }
    }

    tensor::Tensor23 I = tensor::eye(); //Get the second order identity tensor

    test_results[0] = I.data.isApprox(product.data);

    /*!Test the inverse of a fourth order tensor*/
    std::vector< int > fot_shape; //Initialize the shape vector
    fot_shape.resize(4);          //Resize the shape vector to a fourth order tensor

    fot_shape[0] = 3;             //Set the tensor dimensions
    fot_shape[1] = 3;
    fot_shape[2] = 3;
    fot_shape[3] = 3;

    tensor::Tensor FOT = tensor::Tensor(fot_shape); //Initialize and populate the tensor

    FOT.data << 2,4,3,5,1,3,4,6,1,
                4,5,2,7,2,5,3,5,7,
                6,2,8,3,6,2,6,4,5,
                7,2,9,1,5,3,7,3,5,
                9,8,9,4,1,2,4,3,5,
                4,2,5,6,2,7,4,5,3,
                6,6,5,2,4,1,5,4,8,
                9,1,3,2,2,4,5,6,7,
                5,2,1,2,1,1,4,5,6;

    tensor::Tensor FOTinv = FOT.inverse(); //Invert the tensor

    tensor::Tensor productFOT = tensor::Tensor(fot_shape); //Compute the product of the tensor and its inverse

    for(int i=0; i<3; i++){
        for(int j=0; j<3; j++){
            for(int k=0; k<3; k++){
                for(int l=0; l<3; l++){
                    for(int m=0; m<3; m++){
                        for(int n=0; n<3; n++){
                            productFOT(i,j,k,l) += FOT(i,j,m,n)*FOTinv(m,n,k,l);
                        }
                    }
                }
            }
        }
    }
    tensor::Tensor43 FOTI = tensor::FOT_eye();
    test_results[1] = FOTI.data.isApprox(productFOT.data);

    //Compare all test results
    bool tot_result = true;
    for(int i = 0; i<test_num; i++){
        //std::cout << "\nSub-test " << i+1 << " result: " << test_results[i] << "\n";
        if(!test_results[i]){
            tot_result = false;
        }
    }

    if(tot_result){
        results << "test_inverse & True\\\\\n\\hline\n";
    }
    else{
        results << "test_inverse & False\\\\\n\\hline\n";
    }
    return 1;
}

int test_det(std::ofstream &results){
    /*!========================
    |       test_det       |
    ========================

    A test of the determinant of the tensor computation.
    Tensors with known determinants are compared to
    those computed.*/

    int  test_num               = 2;
    std::vector<bool> test_results(test_num,false);

    /*!Test the determinant of a second order tensor*/
    std::vector< int > m_shape; //Initialize the shape vector
    m_shape.resize(2);          //Resize the shape vector to a second order tensor

    m_shape[0] = 3;             //Set the tensor dimensions
    m_shape[1] = 3;

    tensor::Tensor T = tensor::Tensor(m_shape); //Initialize and populate the tensor
    T.data << 2,4,3,5,1,3,4,6,1;

    double Tdet = T.det(); //Compute the determinant

    if(fabs(Tdet-72.)<1e-6){test_results[0] = true;}

    /*!Test the determinant of a fourth order tensor*/
    std::vector< int > fot_shape; //Initialize the shape vector
    fot_shape.resize(4);          //Resize the shape vector to a fourth order tensor

    fot_shape[0] = 3;             //Set the tensor dimensions
    fot_shape[1] = 3;
    fot_shape[2] = 3;
    fot_shape[3] = 3;

    tensor::Tensor FOT = tensor::Tensor(fot_shape); //Initialize and populate the tensor

    FOT.data << 2,4,3,5,1,3,4,6,1,
                4,5,2,7,2,5,3,5,7,
                6,2,8,3,6,2,6,4,5,
                7,2,9,1,5,3,7,3,5,
                9,8,9,4,1,2,4,3,5,
                4,2,5,6,2,7,4,5,3,
                6,6,5,2,4,1,5,4,8,
                9,1,3,2,2,4,5,6,7,
                5,2,1,2,1,1,4,5,6;

    double FOTdet = FOT.det(); //Compute the determinant

    tensor::Tensor productFOT = tensor::Tensor(fot_shape); //Compute the product of the tensor and its inverse

    if(fabs(FOTdet-226209.)<1e-6){test_results[1] = true;}

    //Compare all test results
    bool tot_result = true;
    for(int i = 0; i<test_num; i++){
        //std::cout << "\nSub-test " << i+1 << " result: " << test_results[i] << "\n";
        if(!test_results[i]){
            tot_result = false;
        }
    }

    if(tot_result){
        results << "test_inverse & True\\\\\n\\hline\n";
    }
    else{
        results << "test_inverse & False\\\\\n\\hline\n";
    }
    return 1;
}

int test_operators(std::ofstream &results){
    /*!==============================
    |       test_operators       |
    ==============================

    A test of the operators on the tensor
    object. These include, addition, subtraction,
    +=, and -=*/

    //Define a test matrix
    std::vector < int > m_shape1; //Matrix shape vector
    m_shape1.resize(2);           //Resizing the shape vector to having two indices

    m_shape1[0] = 3;              //Create a 3x3 tensor
    m_shape1[1] = 3;

    tensor::Tensor M1 = tensor::Tensor(m_shape1); //Initialize the matrix
    M1.data << 1,2,3,4,5,6,7,8,9;  //Set the initial values

    //Define a second test matrix
    std::vector < int > m2_shape; //Matrix shape vector
    m2_shape.resize(2);           //Resizing the shape vector to having two indices

    m2_shape[0] = 3;              //Create a 3x3 tensor
    m2_shape[1] = 3;

    tensor::Tensor M2 = tensor::Tensor(m2_shape); //Initialize the matrix
    M2.data << 6,2,2,7,2,1,3,9,0;  //Set the initial values

    //Compute the values
    tensor::Tensor Tsum =  M1+M2;
    tensor::Tensor Tsub =  M1-M2;
    tensor::Tensor Tneg = -M1;
    M1 += M2;
    M2 -= M2;

    //Set the answers
    Eigen::MatrixXd MA1 = Eigen::MatrixXd(3,3);
    MA1 << 1+6,2+2,3+2,4+7,5+2,6+1,7+3,8+9,9+0;
    Eigen::MatrixXd MA2 = Eigen::MatrixXd(3,3);
    MA2 << 1-6,2-2,3-2,4-7,5-2,6-1,7-3,8-9,9-0;
    Eigen::MatrixXd MA3 = Eigen::MatrixXd(3,3);
    MA3 << -1,-2,-3,-4,-5,-6,-7,-8,-9;
    Eigen::MatrixXd MA4 = Eigen::MatrixXd(3,3);
    MA4 << 1+6,2+2,3+2,4+7,5+2,6+1,7+3,8+9,9+0;
    Eigen::MatrixXd MA5 = Eigen::MatrixXd::Zero(3,3);

    //Compare the results to the solutions
    int test_num = 5;
    std::vector<bool> test_results = {false,false,false,false,false};

    test_results[0] = MA1.isApprox(Tsum.data);
    test_results[1] = MA2.isApprox(Tsub.data);
    test_results[2] = MA3.isApprox(Tneg.data);
    test_results[3] = MA4.isApprox(M1.data);
    test_results[4] = MA5.isApprox(M2.data);

    //Compare all test results
    bool tot_result = true;
    for(int i = 0; i<test_num; i++){
        //std::cout << "\nSub-test " << i+1 << " result: " << test_results[i] << "\n";
        if(!test_results[i]){
            tot_result = false;
        }
    }

    if(tot_result){
        results << "test_inverse & True\\\\\n\\hline\n";
    }
    else{
        results << "test_inverse & False\\\\\n\\hline\n";
    }
    return 1;
}

int main(){
    /*!==========================
    |         main            |
    ===========================

    The main loop which runs the tests defined in the
    accompanying functions. Each function should output
    the function name followed by & followed by True or
    False if the test passes or fails respectively.*/

    std::ofstream results;
    //Open the results file
    results.open ("results.tex");

    //!Run the test functions
    test_tensor_functionality(results);
    test_inverse(results);
    test_eye(results);
    test_FOT_eye(results);
    test_det(results);
    test_operators(results);

    //Close the results file
    results.close();
}


#include "sine_series.h"
#include <Eigen/Dense>
#include <benchmark/benchmark.h>
#include <cmath>

using Scalar = double;
constexpr Scalar angle = 1.2345;

static void BM_Sine_Series_Naive(benchmark::State& state) {
    // Perform setup here
    for (auto _ : state) {
        // This code gets timed
        Eigen::Matrix<Scalar, 5, 1> sines;
        generate_odd_sine_series_reference(5, angle, sines.data());
        benchmark::DoNotOptimize(sines.sum());
    }
}
BENCHMARK(BM_Sine_Series_Naive);

static void BM_Sine_Series_Custom(benchmark::State& state) {
    // Perform setup here
    for (auto _ : state) {
        // This code gets timed
        Eigen::Matrix<Scalar, 5, 1> sines;
        generate_odd_sine_series(5, angle, sines.data());
        benchmark::DoNotOptimize(sines.sum());
    }
}
BENCHMARK(BM_Sine_Series_Custom);

// Run the benchmark
BENCHMARK_MAIN();


#pragma once
#include <Eigen/Dense>
#include "mtao/iterator/enumerate.hpp"

namespace mtao::eigen {
    //Packs a ColVector type matrix V and writes it to a new matrix according to an indexer indices
    template <typename Derived>
        auto index_packer(const Eigen::EigenBase<Derived>& V, const std::vector<int>& indices) {
            Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime, Eigen::Dynamic> R(V.rows(), indices.size());
            for(auto&& [i,v]: indices) {
                R.col(i) = V.col(v);
            }
            return R;
        }
    //Packs a ColVector type matrix V and writes it to a new matrix according to an indexer indices
    template <typename Derived, int D>
        auto index_packer(const Eigen::EigenBase<Derived>& V, const std::array<int,D>& indices) {
            Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime, D> R(V.rows(), indices.size());
            for(auto&& [i,v]: indices) {
                R.col(i) = V.col(v);
            }
            return R;
        }
}


/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author(s):       Vincent Rouvreau
 *
 *    Copyright (C) 2020 Inria
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "weighted_alpha_complex"
#include <boost/test/unit_test.hpp>
#include <boost/mpl/list.hpp>

#include <CGAL/Epeck_d.h>

#include <vector>
#include <random>
#include <array>
#include <cmath> // for std::fabs

#include <gudhi/Alpha_complex.h>
#include <gudhi/Alpha_complex_3d.h>
#include <gudhi/Simplex_tree.h>

BOOST_AUTO_TEST_CASE(Weighted_alpha_complex_3d_comparison) {
  // check that for random weighted 3d points in safe mode the 3D and dD codes give the same result with some tolerance

  // Random points construction
  using Kernel_dD = CGAL::Epeck_d< CGAL::Dimension_tag<3> >;
  using Bare_point_d = typename Kernel_dD::Point_d;
  using Weighted_point_d = typename Kernel_dD::Weighted_point_d;
  std::vector<Weighted_point_d> w_points_d;

  using Exact_weighted_alpha_complex_3d =
    Gudhi::alpha_complex::Alpha_complex_3d<Gudhi::alpha_complex::complexity::EXACT, true, false>;
  using Bare_point_3 = typename Exact_weighted_alpha_complex_3d::Bare_point_3;
  using Weighted_point_3 = typename Exact_weighted_alpha_complex_3d::Weighted_point_3;
  std::vector<Weighted_point_3> w_points_3;

  std::uniform_real_distribution<double> rd_pts(-10., 10.);
  std::uniform_real_distribution<double> rd_wghts(-0.5, 0.5);
  std::random_device rand_dev;
  std::mt19937 rand_engine(rand_dev());
  for (int idx = 0; idx < 20; idx++) {
    std::vector<double> point {rd_pts(rand_engine), rd_pts(rand_engine), rd_pts(rand_engine)};
    double weight = rd_wghts(rand_engine);
    w_points_d.emplace_back(Bare_point_d(point.begin(), point.end()), weight);
    w_points_3.emplace_back(Bare_point_3(point[0], point[1], point[2]), weight);
  }

  // Structures necessary for comparison
  using Points = std::vector<std::array<double,3>>;
  using Points_and_filtrations = std::map<Points, double>;
  Points_and_filtrations pts_fltr_dD;
  Points_and_filtrations pts_fltr_3d;

  // Weighted alpha complex for dD version
  Gudhi::alpha_complex::Alpha_complex<Kernel_dD, true> alpha_complex_dD_from_weighted_points(w_points_d);
  Gudhi::Simplex_tree<> w_simplex_d;
  BOOST_CHECK(alpha_complex_dD_from_weighted_points.create_complex(w_simplex_d));

  std::clog << "Iterator on weighted alpha complex dD simplices in the filtration order, with [filtration value]:"
            << std::endl;
  for (auto f_simplex : w_simplex_d.filtration_simplex_range()) {
    Points points;
    for (auto vertex : w_simplex_d.simplex_vertex_range(f_simplex)) {
      CGAL::NT_converter<Kernel_dD::RT, double> cgal_converter;
      Bare_point_d pt = alpha_complex_dD_from_weighted_points.get_point(vertex).point();
      points.push_back({cgal_converter(pt[0]), cgal_converter(pt[1]), cgal_converter(pt[2])});
    }
    std::clog << "   ( ";
    std::sort (points.begin(), points.end());
    for (auto point : points) {
      std::clog << point[0] << " " << point[1] << " " << point[2] << " | ";
    }
    std::clog << ") -> " << "[" << w_simplex_d.filtration(f_simplex) << "] ";
    std::clog << std::endl;
    pts_fltr_dD[points] = w_simplex_d.filtration(f_simplex);
  }

  // Weighted alpha complex for 3D version
  Exact_weighted_alpha_complex_3d alpha_complex_3D_from_weighted_points(w_points_3);
  Gudhi::Simplex_tree<> w_simplex_3;
  BOOST_CHECK(alpha_complex_3D_from_weighted_points.create_complex(w_simplex_3));

  std::clog << "Iterator on weighted alpha complex 3D simplices in the filtration order, with [filtration value]:"
            << std::endl;
  for (auto f_simplex : w_simplex_3.filtration_simplex_range()) {
    Points points;
    for (auto vertex : w_simplex_3.simplex_vertex_range(f_simplex)) {
      Bare_point_3 pt = alpha_complex_3D_from_weighted_points.get_point(vertex).point();
      CGAL::NT_converter<Exact_weighted_alpha_complex_3d::Kernel::RT, double> cgal_converter;
      points.push_back({cgal_converter(pt[0]), cgal_converter(pt[1]), cgal_converter(pt[2])});
    }
    std::clog << "   ( ";
    std::sort (points.begin(), points.end());
    for (auto point : points) {
      std::clog << point[0] << " " << point[1] << " " << point[2] << " | ";
    }
    std::clog << ") -> " << "[" << w_simplex_3.filtration(f_simplex) << "] " << std::endl;
    pts_fltr_3d[points] = w_simplex_d.filtration(f_simplex);
  }

  // Compares structures
  auto d3_itr = pts_fltr_3d.begin();
  auto dD_itr = pts_fltr_dD.begin();
  for (; d3_itr != pts_fltr_3d.end() && dD_itr != pts_fltr_dD.end(); ++d3_itr) {
    if (d3_itr->first != dD_itr->first) {
      for(auto point : d3_itr->first)
        std::clog << point[0] << " " << point[1] << " " << point[2] << " | ";
      std::clog << " versus ";
      for(auto point : dD_itr->first)
        std::clog << point[0] << " " << point[1] << " " << point[2] << " | ";
      std::clog << std::endl;
      BOOST_CHECK(false);
    }
    // In safe mode, relative error is less than 1e-5 (can be changed with set_relative_precision_of_to_double)
    if (std::fabs(d3_itr->second - dD_itr->second) > 1e-5 * (std::fabs(d3_itr->second) + std::fabs(dD_itr->second))) {
      std::clog << d3_itr->second << " versus " << dD_itr->second << " diff "
                << std::fabs(d3_itr->second - dD_itr->second) << std::endl;
      BOOST_CHECK(false);
    }
    ++dD_itr;
  }
}


#include <bits/stdc++.h>
#include <boost/rational.hpp>
#include <boost/multiprecision/cpp_int.hpp>

using namespace std;
using namespace boost;

namespace GaussianElimination
{
    ///------------------------------------------------------------
    constexpr int MAX_EQ = 500;
    constexpr int MAX_VAR = 500;

    using T = boost::multiprecision::cpp_int;

    rational<T> A[MAX_EQ + 1][MAX_VAR + 1];
    rational<T> B[MAX_EQ + 1][MAX_VAR + 1];

    rational<T> solutions[MAX_VAR + 1];

    int N, M;
    ///------------------------------------------------------------

    ///------------------------------------------------------------
    void clear()
    {
        for (int i = 0; i <= MAX_EQ; ++i)
            for (int j = 0; j <= MAX_VAR; ++j)
                A[i][j] = B[i][j] = 0;

        for (int j = 0; j <= MAX_VAR; ++j)
            solutions[j] = 0;
    }

    void makeIdentity()
    {
        for (int i = 1; i <= N; ++i)
            B[i][i] = 1;
    }

    bool checkIfInvertible()
    {
        assert(N == M);

        for (int i = 1; i <= N; ++i)
            if (A[i][i] != 1)
                return false;

        return true;
    }
    ///------------------------------------------------------------

    ///------------------------------------------------------------
    void swapLines(int x, int y) /// swap(X, Y)
    {
        swap(A[x], A[y]);
        swap(B[x], B[y]);

        cerr << "SWAP LINES : " << x << " " << y << endl;
    }

    void divideLine(int line, rational<T> rat) /// X = X / rat
    {
        assert(rat != 0);

        for (int j = 1; j <= M; ++j)
            A[line][j] /= rat;

        for (int j = 1; j <= M; ++j)
            B[line][j] /= rat;

        cerr << "DIVIDE LINE_" << line << " BY " << rat << endl;
    }

    void changeLines(int x, int y, rational<T> rat) /// X = X - rat * Y
    {
        for (int i = 1; i <= M; ++i)
            A[x][i] -= rat * A[y][i];

        for (int i = 1; i <= M; ++i)
            B[x][i] -= rat * B[y][i];

        cerr << "ADD TO LINE_" << x << " => LINE_" << y << " x " << -rat << endl;
    }
    ///------------------------------------------------------------

    void rowEchelonForm()
    {
        cerr << "START ROW ECHELON" << endl;

        int i = 1, j = 1;

        while (i <= N && j <= M)
        {
            int k = i;

            while (k <= N && A[k][j] == 0)
                k++;

            if (k == N + 1)
            {
                j++;
                continue;
            }

            if (k != i)
                swapLines(k, i);

            assert(A[i][j] != 0);
            divideLine(i, A[i][j]);

            for (int l = i + 1; l <= N; ++l)
                changeLines(l, i, A[l][j]);

            i++;
            j++;
        }

        cerr << "FINISH ROW ECHELON" << endl << endl;
    }

    void reducedRowEchelonForm()
    {
        cerr << "START REDUCED ROW ECHELON" << endl;

        for (int i = N; i >= 1; i--)
        {
            int j = 1;

            while (j <= M && A[i][j] == 0)
                j++;

            if (j <= M)
            {
                for (int k = i - 1; k >= 1; k--)
                    changeLines(k, i, A[k][j]);
            }
        }

        cerr << "FINISH REDUCED ROW ECHELON" << endl << endl;
    }

    vector<vector<double>> getInverse(const vector<vector<int>> &coef, ostream &out = cout)
    {
        assert(coef.size() >= 1);
        assert(coef[0].size() >= 1);

        GaussianElimination::N = coef.size();
        GaussianElimination::M = coef[0].size();

        if (N != M)
        {
            out << "Matrix not invertible : N != M (non-square)\n";
            return {};
        }

        clear();

        for (int i = 0; i < N; ++i)
            for (int j = 0; j < M; ++j)
                A[i + 1][j + 1] = coef[i][j];

        makeIdentity();
        rowEchelonForm();

        if (checkIfInvertible() == false)
        {
            out << "Matrix not invertible\n";
            return {};
        }

        reducedRowEchelonForm();

        vector<vector<double>> inverse(N + 2);

        for (int i = 1; i <= N; ++i)
        {
            inverse[i - 1].resize(M + 1);

            for (int j = 1; j <= M; ++j)
            {
                inverse[i - 1][j - 1] = boost::rational_cast<double>(B[i][j]);
                out << B[i][j] << " ";
            }

            out << endl;
        }

        return inverse;
    }

    vector<double> solveSystemEquations(const vector<vector<int>> &coef, const vector<int> &bs, ostream &out = cout)
    {
        assert(coef.size() >= 1);
        assert(coef[0].size() >= 1);

        GaussianElimination::N = coef.size();
        GaussianElimination::M = coef[0].size();

        assert(static_cast<int>(bs.size()) == GaussianElimination::N);

        clear();

        for (int i = 0; i < N; ++i)
            for (int j = 0; j < M; ++j)
                A[i + 1][j + 1] = coef[i][j];

        for (int i = 0; i < N; ++i)
            B[i + 1][1] = bs[i];

        rowEchelonForm();
        reducedRowEchelonForm();

        for (int i = N; i >= 1; i--)
        {
            int j = 1;

            while (j <= M && A[i][j] == 0)
                j++;

            if (j == M + 1)
            {
                if (B[i][1] != 0)
                {
                    out << "Impossible!" << endl;
                    return {};
                }
            }
            else
            {
                solutions[i] = B[i][1];

                for (int p = j + 1; p <= M; ++p)
                    solutions[i] -= A[i][p] * solutions[p];
            }
        }

        vector<double> solution(M);

        for (int j = 0; j < M; ++j)
            solution[j] = boost::rational_cast<double>(solutions[j + 1]);

        for (int j = 1; j <= M; ++j)
            out << solutions[j] << " ";

        out << endl;

        return solution;
    }
}

int main()
{
    ifstream in("data.in");

    assert(in.is_open());

    vector<vector<int>> A;
    int n, m;

    in >> n >> m;
    A.resize(n);
    vector<int> bs(n);

    for (int i = 0; i < n; ++i)
    {
        A[i].resize(m);

        for (int j = 0; j < m; ++j)
            in >> A[i][j];

        in >> bs[i];
    }

    vector<double> xs = GaussianElimination::solveSystemEquations(A, bs);

    return 0;
}


#include "MatrixMultiply.hpp"

#include <iostream>
#include <exception>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <numeric>
#include <stdlib.h>

# define ROWMATRIXPOS(rowSize , row, col) (rowSize * row) + col
# define COLMATRIXPOS(colSize , row, col) (colSize * col) + row

scottgs::MatrixMultiply::MatrixMultiply()
{
	;
}

scottgs::MatrixMultiply::~MatrixMultiply()
{
	;
}

scottgs::FloatMatrix scottgs::MatrixMultiply::operator()(const scottgs::FloatMatrix& lhs, const scottgs::FloatMatrix& rhs) const
{
	// Verify acceptable dimensions
	if (lhs.size2() != rhs.size1())
		throw std::logic_error("matrix incompatible lhs.size2() != rhs.size1()");

	scottgs::FloatMatrix result(lhs.size1(),rhs.size2());

	int lhsSize1 = lhs.size1();
	int lhsSize2 = lhs.size2();
	int rhsSize1 = rhs.size1();
	int rhsSize2 = rhs.size2();
	float sum = 0;

	const float* lef = &lhs(0,0);

	float* right = new float[rhsSize1*rhsSize2];
	const float * rightHolder = &rhs(0,0);

	for(int i = 0; i < rhsSize1; i++){
		for(int j = 0; j < rhsSize2; j++){
			right[COLMATRIXPOS(rhsSize1,i,j)] = rightHolder[ROWMATRIXPOS(rhsSize2,i,j)];
		}
	}

	for(int i = 0; i < lhsSize1; i++){
		for(int j = 0; j < rhsSize2; j++){
			sum = 0;
			for(int k = 0; k < lhsSize2; k++){

				sum += lef[ROWMATRIXPOS(lhsSize2,i,k)] * right[COLMATRIXPOS(rhsSize1,k,j)];
			}

			result(i,j) = sum;

		}
	}

	delete[] right;

	return result;
}

scottgs::FloatMatrix scottgs::MatrixMultiply::multiply(const scottgs::FloatMatrix& lhs, const scottgs::FloatMatrix& rhs) const
{
	// Verify acceptable dimensions
	if (lhs.size2() != rhs.size1())
		throw std::logic_error("matrix incompatible lhs.size2() != rhs.size1()");

	return boost::numeric::ublas::prod(lhs,rhs);
}


#include "Gaussian.h"
#include <boost/math/distributions/normal.hpp>

BaseDistributionGaussian::BaseDistributionGaussian(const Options& iOptions, const Data& iData) : BaseDistribution(iOptions, iData) {

}
float BaseDistributionGaussian::getCdf(float iX, const std::vector<float>& iMoments) const {
   assert(iMoments.size() == 2);
   float mean     = iMoments[0];
   float variance = iMoments[1];
   if(variance == 0) {
      return Global::MV;
   }
   boost::math::normal dist(mean, sqrt(variance));
   return boost::math::cdf(dist, iX);
}
float BaseDistributionGaussian::getPdf(float iX, const std::vector<float>& iMoments) const {
   assert(iMoments.size() == 2);
   float mean     = iMoments[0];
   float variance = iMoments[1];
   if(variance == 0) {
      return Global::MV;
   }
   boost::math::normal dist(mean, sqrt(variance));
   return boost::math::pdf(dist, iX);
}
float BaseDistributionGaussian::getInv(float iCdf, const std::vector<float>& iMoments) const {
   assert(iMoments.size() == 2);
   float mean     = iMoments[0];
   float variance = iMoments[1];
   if(variance == 0) {
      return Global::MV;
   }
   boost::math::normal dist(mean, sqrt(variance));
   return boost::math::quantile(dist, iCdf);

}
int BaseDistributionGaussian::getNumMoments() const {
   return 2;
}


/**
 * ravg.hpp
 *
 * 2021 Gabriel A. Moreira
 *
 * gmoreira at isr.tecnico.ulisboa.pt
 * https://github.com/gabmoreira/maks
 *
 * This software and the related documents  are provided as  is,  with no express
 * or implied  warranties,  other  than those  that are  expressly stated  in the
 * License.
 */

#ifndef RAVG_HPP
#define RAVG_HPP

#include <Eigen/Dense>
#include <Eigen/Sparse>

/**
 * Cycle Solver in SO(3).
 *
 * Closed-form cycle graph rotation averaging solver in SO(3).
 *
 * @param pairwise (input) Eigen::MatrixXd - 3 x 3n Eigen matrix with SO(3) blocks.
 * @param num_nodes (input) int - number of variables.
 * @param R (output) Eigen::MatrixXd - 3n x 3 Eigen matrix containing optimal solution.
 */
void cycleSolverSO3(const Eigen::Ref<Eigen::MatrixXd> pairwise,
                    int num_nodes,
                    Eigen::Ref<Eigen::MatrixXd> R);

/**
 * Rotation averaging Primal-Dual method in SO(3).
 *
 * Primal-dual update method for averaging rotations in SO(3).
 *
 * @param Rtilde (input) Eigen::SparseMatrix<double> - 3n x 3n sparse rotation adjacency matrix.
 * @param R (output) Eigen::MatrixXd - 3n x 3 solution.
 * @param A (input/output) Eigen::SparseMatrix<double> - 3n x 3n symmetric and sparse graph adjacency matrix.
 * @param num_nodes (input) Int - number of variables.
 * @param maxiter (input) int - maxiter.
 * @param dual (output) Double - dual problem.
 * @param eta (input) Double - minimum eigenvalue stopping criterion.
 * @param sigma (input) Double - spectral shift.
 */
void primalDualSO3(const Eigen::SparseMatrix<double>& Rtilde,
                   const Eigen::SparseMatrix<double>& A,
                   Eigen::Ref<Eigen::MatrixXd> R,
                   int num_nodes,
                   int maxiter,
                   double& dual,
                   double eta,
                   double sigma);

#endif /* RAVG_HPP */


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

#include "linear_algebra.h"

#include <cassert>

#include <Eigen/Core>
#include <unsupported/Eigen/CXX11/Tensor>

#include "tensor/tensor_op.h"

namespace MOTION {

template <typename T>
void matrix_multiply(std::size_t dim_l, std::size_t dim_m, std::size_t dim_n, const T* A,
                     const T* B, T* output) {
  using MatrixType = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
  Eigen::Map<MatrixType> matrix_output(output, dim_l, dim_n);
  Eigen::Map<const MatrixType> matrix_A(A, dim_l, dim_m);
  Eigen::Map<const MatrixType> matrix_B(B, dim_m, dim_n);
  matrix_output = matrix_A * matrix_B;
}

template <typename T>
std::vector<T> matrix_multiply(std::size_t dim_l, std::size_t dim_m, std::size_t dim_n,
                               const std::vector<T>& A, const std::vector<T>& B) {
  assert(A.size() == dim_l * dim_m);
  assert(B.size() == dim_m * dim_n);
  std::vector<T> output(dim_l * dim_n);
  matrix_multiply(dim_l, dim_m, dim_n, A.data(), B.data(), output.data());
  return output;
}

template <typename T>
void matrix_multiply(const tensor::GemmOp& gemm_op, const T* A, const T* B, T* output) {
  using MatrixType = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
  assert(gemm_op.verify());
  Eigen::Map<MatrixType> matrix_output(output, gemm_op.output_shape_[0], gemm_op.output_shape_[1]);
  Eigen::Map<const MatrixType> matrix_A(A, gemm_op.input_A_shape_[0], gemm_op.input_A_shape_[1]);
  Eigen::Map<const MatrixType> matrix_B(B, gemm_op.input_B_shape_[0], gemm_op.input_B_shape_[1]);

  if (gemm_op.transA_ && gemm_op.transB_) {
    matrix_output = matrix_A.transpose() * matrix_B.transpose();
  } else if (gemm_op.transA_) {
    matrix_output = matrix_A.transpose() * matrix_B;
  } else if (gemm_op.transB_) {
    matrix_output = matrix_A * matrix_B.transpose();
  } else {
    matrix_output = matrix_A * matrix_B;
  }
}

template void matrix_multiply(std::size_t, std::size_t, std::size_t, const std::uint8_t*,
                              const std::uint8_t*, std::uint8_t*);
template void matrix_multiply(std::size_t, std::size_t, std::size_t, const std::uint16_t*,
                              const std::uint16_t*, std::uint16_t*);
template void matrix_multiply(std::size_t, std::size_t, std::size_t, const std::uint32_t*,
                              const std::uint32_t*, std::uint32_t*);
template void matrix_multiply(std::size_t, std::size_t, std::size_t, const std::uint64_t*,
                              const std::uint64_t*, std::uint64_t*);
template std::vector<std::uint8_t> matrix_multiply(std::size_t, std::size_t, std::size_t,
                                                   const std::vector<std::uint8_t>&,
                                                   const std::vector<std::uint8_t>&);
template std::vector<std::uint16_t> matrix_multiply(std::size_t, std::size_t, std::size_t,
                                                    const std::vector<std::uint16_t>&,
                                                    const std::vector<std::uint16_t>&);
template std::vector<std::uint32_t> matrix_multiply(std::size_t, std::size_t, std::size_t,
                                                    const std::vector<std::uint32_t>&,
                                                    const std::vector<std::uint32_t>&);
template std::vector<std::uint64_t> matrix_multiply(std::size_t, std::size_t, std::size_t,
                                                    const std::vector<std::uint64_t>&,
                                                    const std::vector<std::uint64_t>&);
template std::vector<__uint128_t> matrix_multiply(std::size_t, std::size_t, std::size_t,
                                                  const std::vector<__uint128_t>&,
                                                  const std::vector<__uint128_t>&);
template void matrix_multiply(const tensor::GemmOp&, const std::uint8_t*, const std::uint8_t*,
                              std::uint8_t*);
template void matrix_multiply(const tensor::GemmOp&, const std::uint16_t*, const std::uint16_t*,
                              std::uint16_t*);
template void matrix_multiply(const tensor::GemmOp&, const std::uint32_t*, const std::uint32_t*,
                              std::uint32_t*);
template void matrix_multiply(const tensor::GemmOp&, const std::uint64_t*, const std::uint64_t*,
                              std::uint64_t*);
template void matrix_multiply(const tensor::GemmOp&, const __uint128_t*, const __uint128_t*,
                              __uint128_t*);

template <typename T>
void convolution(const tensor::Conv2DOp& conv_op, const T* input_buffer, const T* kernel_buffer,
                 T* output_buffer) {
  using TensorType3 = Eigen::Tensor<T, 3, Eigen::RowMajor>;
  using CTensorType3 = Eigen::Tensor<const T, 3, Eigen::RowMajor>;
  using CTensorType4 = Eigen::Tensor<const T, 4, Eigen::RowMajor>;
  assert(conv_op.verify());
  const auto& output_shape = conv_op.output_shape_;
  const auto& input_shape = conv_op.input_shape_;
  const auto& kernel_shape = conv_op.kernel_shape_;

  Eigen::TensorMap<CTensorType3> input(input_buffer, input_shape[0], input_shape[1],
                                       input_shape[2]);
  Eigen::TensorMap<CTensorType4> kernel(kernel_buffer, kernel_shape[0], kernel_shape[1],
                                        kernel_shape[2], kernel_shape[3]);
  Eigen::TensorMap<TensorType3> output(output_buffer, output_shape[0], output_shape[1],
                                       output_shape[2]);
  const std::array<Eigen::Index, 2> kernel_matrix_dimensions = {
      static_cast<Eigen::Index>(kernel_shape[1] * kernel_shape[2] * kernel_shape[3]),
      static_cast<Eigen::Index>(kernel_shape[0])};
  const std::array<Eigen::Index, 2> input_matrix_dimensions = {
      static_cast<Eigen::Index>(output_shape[1] * output_shape[2]),
      static_cast<Eigen::Index>(kernel_shape[1] * kernel_shape[2] * kernel_shape[3])};

  auto kernel_matrix =
      kernel.shuffle(std::array<int, 4>{3, 2, 1, 0}).reshape(kernel_matrix_dimensions);

  auto input_matrix =
      input.shuffle(Eigen::array<Eigen::Index, 3>{2, 1, 0})
          .extract_image_patches(kernel_shape[2], kernel_shape[3], conv_op.strides_[0],
                                 conv_op.strides_[1], conv_op.dilations_[0], conv_op.dilations_[1],
                                 1, 1, conv_op.pads_[0], conv_op.pads_[2], conv_op.pads_[1],
                                 conv_op.pads_[3], 0)
          .reshape(input_matrix_dimensions);

  const std::array<Eigen::IndexPair<Eigen::Index>, 1> contraction_dimensions = {
      Eigen::IndexPair<Eigen::Index>(1, 0)};
  auto output_matrix =
      kernel_matrix.shuffle(std::array<Eigen::Index, 2>{1, 0})
          .contract(input_matrix.shuffle(std::array<Eigen::Index, 2>{1, 0}), contraction_dimensions)
          .shuffle(std::array<Eigen::Index, 2>{1, 0});

  const std::array<Eigen::Index, 3> rev_output_dimensions = {
      output.dimension(2), output.dimension(1), output.dimension(0)};
  output =
      output_matrix.reshape(rev_output_dimensions).shuffle(Eigen::array<Eigen::Index, 3>{2, 1, 0});
}

template <typename T>
std::vector<T> convolution(const tensor::Conv2DOp& conv_op, const std::vector<T>& input_buffer,
                           const std::vector<T>& kernel_buffer) {
  assert(conv_op.verify());
  assert(input_buffer.size() == conv_op.compute_input_size());
  assert(kernel_buffer.size() == conv_op.compute_kernel_size());
  std::vector<T> output_buffer(conv_op.compute_output_size());
  convolution(conv_op, input_buffer.data(), kernel_buffer.data(), output_buffer.data());
  return output_buffer;
}

void convolution(const tensor::Conv2DOp&, const std::uint8_t*, const std::uint8_t*, std::uint8_t*);
void convolution(const tensor::Conv2DOp&, const std::uint16_t*, const std::uint16_t*,
                 std::uint16_t*);
void convolution(const tensor::Conv2DOp&, const std::uint32_t*, const std::uint32_t*,
                 std::uint32_t*);
void convolution(const tensor::Conv2DOp&, const std::uint64_t*, const std::uint64_t*,
                 std::uint64_t*);
template std::vector<std::uint8_t> convolution(const tensor::Conv2DOp&,
                                               const std::vector<std::uint8_t>&,
                                               const std::vector<std::uint8_t>&);
template std::vector<std::uint16_t> convolution(const tensor::Conv2DOp&,
                                                const std::vector<std::uint16_t>&,
                                                const std::vector<std::uint16_t>&);
template std::vector<std::uint32_t> convolution(const tensor::Conv2DOp&,
                                                const std::vector<std::uint32_t>&,
                                                const std::vector<std::uint32_t>&);
template std::vector<std::uint64_t> convolution(const tensor::Conv2DOp&,
                                                const std::vector<std::uint64_t>&,
                                                const std::vector<std::uint64_t>&);
template std::vector<__uint128_t> convolution(const tensor::Conv2DOp&,
                                              const std::vector<__uint128_t>&,
                                              const std::vector<__uint128_t>&);

template <typename T>
void sum_pool(const tensor::AveragePoolOp& avgpool_op, const T* input, T* output) {
  assert(avgpool_op.verify());
  using TensorType3C = Eigen::Tensor<const T, 3, Eigen::RowMajor>;
  using TensorType3 = Eigen::Tensor<T, 3, Eigen::RowMajor>;
  const auto in_channels = static_cast<Eigen::Index>(avgpool_op.input_shape_[0]);
  const auto in_rows = static_cast<Eigen::Index>(avgpool_op.input_shape_[1]);
  const auto in_columns = static_cast<Eigen::Index>(avgpool_op.input_shape_[2]);
  const auto out_channels = static_cast<Eigen::Index>(avgpool_op.output_shape_[0]);
  const auto out_rows = static_cast<Eigen::Index>(avgpool_op.output_shape_[1]);
  const auto out_columns = static_cast<Eigen::Index>(avgpool_op.output_shape_[2]);
  const auto kernel_rows = static_cast<Eigen::Index>(avgpool_op.kernel_shape_[0]);
  const auto kernel_columns = static_cast<Eigen::Index>(avgpool_op.kernel_shape_[1]);
  const auto stride_rows = static_cast<Eigen::Index>(avgpool_op.strides_[0]);
  const auto stride_columns = static_cast<Eigen::Index>(avgpool_op.strides_[1]);

  Eigen::TensorMap<TensorType3C> tensor_src(input, in_channels, in_rows, in_columns);
  Eigen::TensorMap<TensorType3> tensor_dst(output, out_channels, out_rows, out_columns);

  tensor_dst = tensor_src.shuffle(Eigen::array<Eigen::Index, 3>{2, 1, 0})
                   .extract_image_patches(kernel_rows, kernel_columns, stride_rows, stride_columns,
                                          1, 1, 1, 1, 0, 0, 0, 0, T(0))
                   .sum(Eigen::array<Eigen::Index, 2>{1, 2})
                   .reshape(Eigen::array<Eigen::Index, 3>{out_columns, out_rows, out_channels})
                   .shuffle(Eigen::array<Eigen::Index, 3>{2, 1, 0});
}

template void sum_pool(const tensor::AveragePoolOp&, const std::uint8_t*, std::uint8_t*);
template void sum_pool(const tensor::AveragePoolOp&, const std::uint16_t*, std::uint16_t*);
template void sum_pool(const tensor::AveragePoolOp&, const std::uint32_t*, std::uint32_t*);
template void sum_pool(const tensor::AveragePoolOp&, const std::uint64_t*, std::uint64_t*);
template void sum_pool(const tensor::AveragePoolOp&, const __uint128_t*, __uint128_t*);

}  // namespace MOTION


#include <Eigen/Dense>
#include <unsupported/Eigen/NonLinearOptimization>
#include <unsupported/Eigen/AutoDiff>
#include <iostream>

// Computes the energy terms into costs. The first part contains termes related to edge length variation,
// and the second to angle variations.
template<typename Scalar>
void ikLikeCosts(const Eigen::Matrix<Scalar,Eigen::Dynamic,1>& curve, const Eigen::VectorXd& targetAngles, const Eigen::VectorXd& targetLengths, double beta,
                 Eigen::Matrix<Scalar,Eigen::Dynamic,1>& costs)
{
    using namespace Eigen;
    using std::atan2;

    typedef Matrix<Scalar,2,1> Vec2;
    int nb = curve.size()/2;

    costs.setZero();
    for(int k=1;k<nb-1;++k)
    {
        Vec2 pk0 = curve.template segment<2>(2*(k-1));
        Vec2 pk  = curve.template segment<2>(2*k);
        Vec2 pk1 = curve.template segment<2>(2*(k+1));

        if(k+1<nb-1) {
            costs((nb-2)+(k-1)) = (pk1-pk).norm() - targetLengths(k-1);
        }

        Vec2 v0 = (pk-pk0).normalized();
        Vec2 v1 = (pk1-pk).normalized();

        costs(k-1) = beta * (atan2(-v0.y() * v1.x() + v0.x() * v1.y(), v0.x() * v1.x() + v0.y() * v1.y()) - targetAngles(k-1));
    }
}

// Generic functor
template<typename _Scalar>
struct Functor
{
    typedef _Scalar Scalar;
    enum {
        InputsAtCompileTime = Eigen::Dynamic,
        ValuesAtCompileTime = Eigen::Dynamic
                          };
    typedef Eigen::Matrix<Scalar,InputsAtCompileTime,1> InputType;
    typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,1> ValueType;
    typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType;

    const int m_inputs, m_values;

    Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}
    Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {}

    int inputs() const { return m_inputs; } // number of degree of freedom (= 2*nb_vertices)
    int values() const { return m_values; } // number of energy terms (= nb_vertices + nb_edges)

    // you should define that in the subclass :
    //    void operator() (const InputType& x, ValueType* v, JacobianType* _j=0) const;
};

// Specialized functor warping the ikLikeCosts function
struct iklike_functor : Functor<double>
{
    typedef Eigen::AutoDiffScalar<Eigen::Matrix<Scalar,Eigen::Dynamic,1> > ADS;
    typedef Eigen::Matrix<ADS, Eigen::Dynamic, 1> VectorXad;

    // pfirst and plast are the two extremities of the curve
    iklike_functor(const Eigen::VectorXd& targetAngles, const Eigen::VectorXd& targetLengths, double beta, const Eigen::Vector2d pfirst, const Eigen::Vector2d plast)
        :   Functor<double>(targetAngles.size()*2-4,targetAngles.size()*2-1),
            m_targetAngles(targetAngles), m_targetLengths(targetLengths), m_beta(beta),
            m_pfirst(pfirst), m_plast(plast)
    {}

    // input = x = {  ..., x_i, y_i, ....}
    // output = fvec = the value of each term
    int operator()(const Eigen::VectorXd &x, Eigen::VectorXd &fvec)
    {
        using namespace Eigen;
        VectorXd curves(this->inputs()+8);

        curves.segment(4,this->inputs()) = x;

        Vector2d d(1,0);
        curves.segment<2>(0)                   = m_pfirst - d;
        curves.segment<2>(2)                   = m_pfirst;
        curves.segment<2>(this->inputs()+4)    = m_plast;
        curves.segment<2>(this->inputs()+6)    = m_plast + d;

        ikLikeCosts(curves, m_targetAngles, m_targetLengths, m_beta, fvec);
        return 0;
    }

    // Compute the jacobian into fjac for the current solution x
    int df(const Eigen::VectorXd &x, Eigen::MatrixXd &fjac)
    {
        using namespace Eigen;
        VectorXad curves(this->inputs()+8);

        // Compute the derivatives of each degree of freedom
        // -> grad( x_i ) = (0, ..., 0, 1, 0, ..., 0) ; 1 is in position i
        for(int i=0; i<this->inputs();++i)
            curves(4+i) = ADS(x(i), this->inputs(), i);

        Vector2d d(1,0);
        curves.segment<2>(0)                   = (m_pfirst - d).cast<ADS>();
        curves.segment<2>(2)                   = (m_pfirst).cast<ADS>();
        curves.segment<2>(this->inputs()+4)    = (m_plast).cast<ADS>();
        curves.segment<2>(this->inputs()+6)    = (m_plast + d).cast<ADS>();

        VectorXad v(this->values());

        ikLikeCosts(curves, m_targetAngles, m_targetLengths, m_beta, v);

        // copy the gradient of each energy term into the Jacobian
        for(int i=0; i<this->values();++i)
            fjac.row(i) = v(i).derivatives();

        return 0;
    }

    const Eigen::VectorXd& m_targetAngles;
    const Eigen::VectorXd& m_targetLengths;
    double m_beta;
    Eigen::Vector2d m_pfirst, m_plast;
};

void draw_vecX(const Eigen::VectorXd& res)
{
  for( int i = 0; i < (res.size()/2) ; i++ )
  {
    std::cout << res.segment<2>(2*i).transpose() << "\n";
  }
}

using namespace Eigen;

int main()
{
    Eigen::Vector2d pfirst(-5., 0.);
    Eigen::Vector2d plast ( 5., 0.);

    // rest pose is a straight line starting between first and last point
    const int nb_points = 30;
    Eigen::VectorXd targetAngles (nb_points);
    targetAngles.fill(0);

    Eigen::VectorXd targetLengths(nb_points-1);
    double val = (pfirst-plast).norm() / (double)(nb_points-1);
    targetLengths.fill(val);

    // get initial solution
    Eigen::VectorXd x((nb_points-2)*2);
    for(int i = 1; i < (nb_points - 1); i++)
    {
        double s = (double)i / (double)(nb_points-1);
        x.segment<2>((i-1)*2) = plast * s + pfirst * (1. - s);
    }

    // move last point
    plast = Eigen::Vector2d(4., 1.);

    // Create the functor object
    iklike_functor func(targetAngles, targetLengths, 0.1, pfirst, plast);

    // construct the solver
    Eigen::LevenbergMarquardt<iklike_functor> lm(func);

    // adjust tolerance
    lm.parameters.ftol *= 1e-2;
    lm.parameters.xtol *= 1e-2;
    lm.parameters.maxfev = 2000;

    int a = lm.minimize(x);
    std::cerr << "info = " << a  << " " << lm.nfev << " " << lm.njev << " "  <<  "\n";

    std::cout << pfirst.transpose() << "\n";
    draw_vecX( x);
    std::cout << plast.transpose() << "\n";
}


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

// Copyright (c) 2020 Tinko Bartels, Berlin, Germany.

// Contributed and/or modified by Tinko Bartels,
//   as part of Google Summer of Code 2020 program.

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

#ifndef BOOST_GEOMETRY_EXTENSIONS_GENERIC_ROBUST_PREDICATES_STRATEGIES_CARTESIAN_DETAIL_EXPRESSIONS_HPP
#define BOOST_GEOMETRY_EXTENSIONS_GENERIC_ROBUST_PREDICATES_STRATEGIES_CARTESIAN_DETAIL_EXPRESSIONS_HPP

#include <boost/geometry/extensions/generic_robust_predicates/strategies/cartesian/detail/expression_tree.hpp>

namespace boost { namespace geometry
{

namespace detail { namespace generic_robust_predicates
{

template
<
    typename A11, typename A12,
    typename A21, typename A22
>
using det2x2 = difference
    <
        product<A11, A22>,
        product<A12, A21>
    >;

using orient2d = det2x2
        <
            difference <_1, _5>, difference<_2, _6>,
            difference <_3, _5>, difference<_4, _6>
        >;

template
<
    typename A11, typename A12, typename A13,
    typename A21, typename A22, typename A23,
    typename A31, typename A32, typename A33
>
struct det3x3_helper
{
private:
    using minor1 = product<A11, det2x2<A22, A23, A32, A33>>;
    using minor2 = product<A21, det2x2<A12, A13, A32, A33>>;
    using minor3 = product<A31, det2x2<A12, A13, A22, A23>>;
public:
    using type = sum<minor1, sum<minor2, minor3>>;
};

template
<
    typename A11, typename A12, typename A13,
    typename A21, typename A22, typename A23,
    typename A31, typename A32, typename A33
>
using det3x3 = typename det3x3_helper
    <
        A11, A12, A13,
        A21, A22, A23,
        A31, A32, A33
    >::type;

using orient3d = det3x3
    <
        difference<_1, _10>, difference<_2, _11>, difference<_3, _12>,
        difference<_4, _10>, difference<_5, _11>, difference<_6, _12>,
        difference<_7, _10>, difference<_8, _11>, difference<_9, _12>
    >;

struct incircle_helper
{
private:
    using adx = difference<_1, _7>;
    using ady = difference<_2, _8>;
    using bdx = difference<_3, _7>;
    using bdy = difference<_4, _8>;
    using cdx = difference<_5, _7>;
    using cdy = difference<_6, _8>;
    using alift = sum<product<adx, adx>, product<ady, ady>>;
    using blift = sum<product<bdx, bdx>, product<bdy, bdy>>;
    using clift = sum<product<cdx, cdx>, product<cdy, cdy>>;
public:
    using type = det3x3
        <
            alift, adx, ady,
            blift, bdx, bdy,
            clift, cdx, cdy
        >;
};

using incircle = incircle_helper::type;

}} // namespace detail::generic_robust_predicates

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_GENERIC_ROBUST_PREDICATES_STRATEGIES_CARTESIAN_DETAIL_EXPRESSIONS_HPP


#ifndef TRIUMF_NMR_HEBEL_SLICHTER_HPP
#define TRIUMF_NMR_HEBEL_SLICHTER_HPP

#include <cmath>
#include <complex>

#include <boost/math/quadrature/exp_sinh.hpp>
#include <boost/math/quadrature/gauss_kronrod.hpp>

#include <triumf/constants/codata_2018.hpp>
#include <triumf/statistical_mechanics/fermi_dirac.hpp>
#include <triumf/superconductivity/bcs.hpp>
#include <triumf/superconductivity/dynes.hpp>

// TRIUMF: Canada's particle accelerator centre
namespace triumf {

// Nuclear Magnetic Resonance (NMR)
namespace nmr {

// Hebel-Slichter
namespace hebel_slichter {

template <typename T = double>
T integrand(T energy, T temperature, T critical_temperature, T gap_meV, T alpha,
            T Gamma) {
  // alias some values
  T Delta = triumf::superconductivity::bcs::gap<T>(
      temperature, critical_temperature, gap_meV);
  constexpr T E_0 = 0.0;
  constexpr T E_F = 0.0;
  //
  T E = energy;
  T E_p = E + alpha * gap_meV;

  // calculate the Fermi factors
  // Note: 1e-3 used to convert energyies from meV to eV
  T f_E = triumf::statistical_mechanics::fermi_dirac::distribution<T>(
      temperature, E * 1e-3, E_0, E_F);
  T f_E_p = triumf::statistical_mechanics::fermi_dirac::distribution<T>(
      temperature, E_p * 1e-3, E_0, E_F);

  //
  return (triumf::superconductivity::dynes::N(E, Gamma * gap_meV, Delta) *
              triumf::superconductivity::dynes::N(E_p, Gamma * gap_meV, Delta) +
          triumf::superconductivity::dynes::M(E, Gamma * gap_meV, Delta) *
              triumf::superconductivity::dynes::M(E_p, Gamma * gap_meV,
                                                  Delta)) *
         f_E * (1.0 - f_E_p);
}

// ratio of SLR rates in the superconducting and normal states
template <typename T = double>
T slr_ratio(T temperature, T critical_temperature, T gap_meV, T alpha,
            T Gamma) {
  // define some convenience values
  T reduced_temperature = temperature / critical_temperature;
  constexpr T k_B =
      1e3 *
      triumf::constants::codata_2018::Boltzmann_constant_in_eV_K<T>::value();
  T beta = 1.0 / (k_B * temperature);
  // return limiting values...
  // if (temperature >= critical_temperature) {
  //   return 1.0;
  // } else if (temperature <= 0.0) {
  //  return 0.0;
  if (temperature <= 0.0) {
    return 0.0;
    // ...before attempting to evaluate the intergral!
  } else {
    // define the integrand
    auto hs_integrand = [&](T E) -> T {
      return integrand<T>(E, temperature, critical_temperature, gap_meV, alpha,
                          Gamma);
    };
    // setup values for numeric integration
    // const T tolerance = std::numeric_limits<T>::epsilon();
    // const T tolerance = std::pow(std::numeric_limits<T>::epsilon(), 2.0
    // / 3.0);
    const T tolerance = std::sqrt(std::numeric_limits<T>::epsilon());

    const std::size_t max_refinements = 15;
    static boost::math::quadrature::exp_sinh<T> hs_integrator(max_refinements);
    return 2.0 * beta *
           hs_integrator.integrate(hs_integrand, tolerance, nullptr, nullptr,
                                   nullptr);
    /*
    const unsigned max_depth = 15;
    return 2.0 * beta *
           boost::math::quadrature::gauss_kronrod<T, 61>::integrate(
               hs_integrand, 0.0, std::numeric_limits<T>::infinity(), max_depth,
               tolerance, nullptr, nullptr);
    */
  }
}

} // namespace hebel_slichter

} // namespace nmr

} // namespace triumf

#endif // TRIUMF_NMR_HEBEL_SLICHTER_HPP