#include <Eigen/Jacobi>
#include <Eigen/SVD>

#include <sphericalsfm/so3.h>
#include <sphericalsfm/spherical_utils.h>

namespace sphericalsfm {
    void make_spherical_essential_matrix( const Eigen::Matrix3d &R, bool inward, Eigen::Matrix3d &E )
    {
        Eigen::Vector3d t( R(0,2), R(1,2), R(2,2)-1 );
        if ( inward ) t = -t;
        E = skew3(t)*R;
    }

    void decompose_spherical_essential_matrix( const Eigen::Matrix3d &E, bool inward, Eigen::Vector3d &r, Eigen::Vector3d &t )
    {
        Eigen::JacobiSVD<Eigen::Matrix3d> svdE(E,Eigen::ComputeFullU|Eigen::ComputeFullV);

        Eigen::Matrix3d U = svdE.matrixU();
        Eigen::Matrix3d V = svdE.matrixV();

        // from theia sfm
        if (U.determinant() < 0) {
            U.col(2) *= -1.0;
        }

        if (V.determinant() < 0) {
            V.col(2) *= -1.0;
        }

        Eigen::Matrix3d D;
        D <<
        0,1,0,
        -1,0,0,
        0,0,1;

        Eigen::Matrix3d DT;
        DT <<
        0,-1,0,
        1,0,0,
        0,0,1;

        Eigen::Matrix3d VT = V.transpose().eval();

        Eigen::Vector3d tu = U.col(2);

        Eigen::Matrix3d R1 = U*D*VT;
        Eigen::Matrix3d R2 = U*DT*VT;

        Eigen::Vector3d t1( R1(0,2), R1(1,2), R1(2,2)-1 );
        Eigen::Vector3d t2( R2(0,2), R2(1,2), R2(2,2)-1 );

        if ( inward ) { t1 = -t1; t2 = -t2; }

        Eigen::Vector3d myt1 = t1/t1.norm();
        Eigen::Vector3d myt2 = t2/t2.norm();

        Eigen::Vector3d r1 = so3ln(R1);
        Eigen::Vector3d r2 = so3ln(R2);

        double score1 = fabs(myt1.dot(tu));
        double score2 = fabs(myt2.dot(tu));

        if ( score1 > score2 ) { r = r1; t = t1; }
        else { r = r2; t = t2; }
    }

    static Eigen::Vector3d triangulateMidpoint( const Eigen::Matrix4d &rel_pose, const Eigen::Vector3d &u, const Eigen::Vector3d &v )
    {
        Eigen::Vector3d cu( 0, 0, 0 );
        Eigen::Vector3d cv( -rel_pose.block<3,3>(0,0).transpose() * rel_pose.block<3,1>(0,3) );

        Eigen::Matrix3d A;
        A <<
        u(0), -v(0), cu(0) - cv(0),
        u(1), -v(1), cu(1) - cv(1),
        u(2), -v(2), cu(2) - cv(2);

        const Eigen::Vector3d soln = A.jacobiSvd( Eigen::ComputeFullV ).matrixV().col(2);
        const double du = soln(0)/soln(2);
        const double dv = soln(1)/soln(2);

        const Eigen::Vector3d Xu = cu + u*du;
        const Eigen::Vector3d Xv = cv + v*dv;

        return (Xu+Xv)*0.5;
    }

    void decompose_spherical_essential_matrix( const Eigen::Matrix3d &E, bool inward,
                     RayPairList::iterator begin, RayPairList::iterator end, const std::vector<bool> &inliers,
                     Eigen::Vector3d &r, Eigen::Vector3d &t )
    {
        Eigen::JacobiSVD<Eigen::Matrix3d> svdE(E,Eigen::ComputeFullU|Eigen::ComputeFullV);

        Eigen::Matrix3d U = svdE.matrixU();
        Eigen::Matrix3d V = svdE.matrixV();

        // from theia sfm
        if (U.determinant() < 0) {
            U.col(2) *= -1.0;
        }

        if (V.determinant() < 0) {
            V.col(2) *= -1.0;
        }

        Eigen::Matrix3d D;
        D <<
        0,1,0,
        -1,0,0,
        0,0,1;

        Eigen::Matrix3d DT;
        DT <<
        0,-1,0,
        1,0,0,
        0,0,1;

        Eigen::Matrix3d VT = V.transpose().eval();

        Eigen::Matrix3d R1 = U*D*VT;
        Eigen::Matrix3d R2 = U*DT*VT;

        Eigen::Vector3d t1( R1(0,2), R1(1,2), (inward) ? R1(2,2)+1 : R1(2,2)-1 );
        Eigen::Vector3d t2( R2(0,2), R2(1,2), (inward) ? R2(2,2)+1 : R2(2,2)-1 );

        Eigen::Vector3d r1 = so3ln(R1);
        Eigen::Vector3d r2 = so3ln(R2);

        double r1test = r1.norm();
        double r2test = r2.norm();

        if ( r2test > M_PI/2 && r1test < M_PI/2 ) { r = r1; t = t1; return; }
        if ( r1test > M_PI/2 && r2test < M_PI/2 ) { r = r2; t = t2; return; }

        Eigen::Matrix4d P1( Eigen::Matrix4d::Identity() );
        Eigen::Matrix4d P2( Eigen::Matrix4d::Identity() );

        P1.block(0,0,3,3) = R1;
        P1.block(0,3,3,1) = t1;

        P2.block(0,0,3,3) = R2;
        P2.block(0,3,3,1) = t2;

        int ninfront1 = 0;
        int ninfront2 = 0;

        int i = 0;
        for ( RayPairList::iterator it = begin; it != end; it++,i++ )
        {
            if ( !inliers[i] ) continue;

            Eigen::Vector3d u = it->first.head(3);
            Eigen::Vector3d v = it->second.head(3);

            Eigen::Vector3d X1 = triangulateMidpoint(P1, u, v);
            Eigen::Vector3d X2 = triangulateMidpoint(P2, u, v);

            if ( X1(2) > 0 ) ninfront1++;
            if ( X2(2) > 0 ) ninfront2++;
        }

        if ( ninfront1 > ninfront2 )
        {
            r = so3ln(R1);
            t = t1;
        }
        else
        {
            r = so3ln(R2);
            t = t2;
        }
    }
}


#include <ros/ros.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <tf/transform_datatypes.h>
#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>
#include <cmath>
#include <cylbot_motion_model/multivariate_normal.h>
#include <iostream>

#define NUM_POSES 1000

geometry_msgs::PoseArray pose_array;

Eigen::internal::scalar_normal_dist_op<double> randN;
//Eigen::internal::scalar_normal_dist_op<double>::rng.seed(1);

double alpha[6];

// random number generator
boost::mt19937 rng;

geometry_msgs::TwistStamped::ConstPtr curr_vel_cmd;

void sampleVelocityMotionModel(const geometry_msgs::Twist& u, \
							   geometry_msgs::PoseArray::_poses_type::iterator x,
							   double dt,
							   const double (&alpha)[6],
							   boost::mt19937& rng)
{
	double v_hat_variance = alpha[0]*u.linear.y*u.linear.y + alpha[1]*u.angular.z*u.angular.z;
	double w_hat_variance = alpha[2]*u.linear.y*u.linear.y + alpha[3]*u.angular.z*u.angular.z;
	double gamma_variance = alpha[4]*u.linear.y*u.linear.y + alpha[5]*u.angular.z*u.angular.z;

	boost::normal_distribution<> v_hat_dist(0.0, sqrt(v_hat_variance));
	boost::normal_distribution<> w_hat_dist(0.0, sqrt(w_hat_variance));
	boost::normal_distribution<> gamma_dist(0.0, sqrt(gamma_variance));

	double v_hat = u.linear.y + v_hat_dist(rng);
	double w_hat = u.angular.z + w_hat_dist(rng);
	double gamma = gamma_dist(rng);

	double v_w_ratio = v_hat/w_hat;

	double yaw = tf::getYaw(x->orientation);

	double x_prime = x->position.x - v_w_ratio*sin(yaw) + v_w_ratio*sin(yaw + w_hat*dt);
	double y_prime = x->position.y + v_w_ratio*cos(yaw) - v_w_ratio*cos(yaw + w_hat*dt);
	double yaw_prime = yaw + w_hat*dt + gamma*dt;

	x->position.x = x_prime;
	x->position.y = y_prime;
	x->orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, yaw_prime);
}

void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& pose_with_cov)
{
	ROS_INFO("Received a new pose");

	Eigen::VectorXd mean(3);
	Eigen::MatrixXd covar(3, 3);

	mean << pose_with_cov->pose.pose.position.x,
		pose_with_cov->pose.pose.position.y,
		tf::getYaw(pose_with_cov->pose.pose.orientation);
	covar << pose_with_cov->pose.covariance[0], 0,                                  0,
		     0,                                 pose_with_cov->pose.covariance[7],  0,
		     0,                            0,                                       pose_with_cov->pose.covariance[35];

	Eigen::MatrixXd normTransform(3, 3);
	Eigen::LLT<Eigen::MatrixXd> cholSolver(covar);

	if(cholSolver.info() == Eigen::Success)
	{
		normTransform = cholSolver.matrixL();
	}
	else
	{
		Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigenSolver(covar);
		normTransform = eigenSolver.eigenvectors() * eigenSolver.eigenvalues().cwiseSqrt().asDiagonal();
	}

	Eigen::MatrixXd samples = (normTransform * Eigen::MatrixXd::NullaryExpr(3, NUM_POSES, randN)).colwise() + mean;

	ROS_INFO_STREAM("\nMean:\n" << mean << "\n");
	ROS_INFO_STREAM("\nCov:\n" << covar << "\n");

	// remove existing pose estimates
	pose_array.poses.clear();
	for(int i=0; i<NUM_POSES; i++)
	{
		geometry_msgs::Pose pose;
		pose.position.x = samples(0, i);
		pose.position.y = samples(1, i);
		pose.position.z = 0;

		// generate random yaw then convert it to a quaternion
		pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, samples(2, i));

		pose_array.poses.push_back(pose);
	}
}

void velocityCallback(const geometry_msgs::TwistStamped::ConstPtr& twist_msg)
{
	curr_vel_cmd = twist_msg;
}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "pose_array_test");
	ros::NodeHandle nh;

	// initialize alpha
	for(int i=0; i<6; i++)
		alpha[i] = .1;

	alpha[0] = .5;
	alpha[3] = .5;

	ros::Publisher pose_array_pub = nh.advertise<geometry_msgs::PoseArray>("pose_array_test", 1);
	//ros::Subscriber initial_pose_sub = nh.subscribe<geometry_msgs::PoseWithCovarianceStamped>("/initialpose", 10, &initialPoseCallback, ros::TransportHints().tcpNoDelay(false));
	ros::Subscriber velocity_sub = nh.subscribe<geometry_msgs::TwistStamped>("/cylbot/velocity", 1, &velocityCallback);

	pose_array.header.frame_id = "/map";

	geometry_msgs::Pose starting_pose;
	starting_pose.orientation.w = 1.0;
	for(int i=0; i<1000; i++)
	{
		pose_array.poses.push_back(starting_pose);
	}

	pose_array_pub.publish(pose_array);

	ros::Time last_time = ros::Time::now();
	ros::Rate loop_rate(100);
	while(ros::ok())
	{
		if(curr_vel_cmd != NULL)
		{
			// get the time delta
			ros::Time curr_time = ros::Time::now();
			ros::Duration dt = curr_time - last_time;
			last_time = curr_time;

			// skip when commands are at 0
			if(fabs(curr_vel_cmd->twist.linear.y) > 0.1 ||
			   fabs(curr_vel_cmd->twist.angular.z) > 0.1)
			{

				ROS_INFO_STREAM("Updating pose estimates with command\n" << curr_vel_cmd->twist);
				ROS_INFO_STREAM("\tdt: " << dt.toSec());

				// update the pose estimates with the probabilistic motion model
				for(geometry_msgs::PoseArray::_poses_type::iterator pose = pose_array.poses.begin();
					pose != pose_array.poses.end();
					pose++)
				{
					sampleVelocityMotionModel(curr_vel_cmd->twist, pose, dt.toSec(), alpha, rng);
				}
			}
		}

		pose_array.header.stamp = ros::Time::now();
		pose_array_pub.publish(pose_array);

		ros::spinOnce();
		loop_rate.sleep();
	}

}


/*
 * polynomial_splines.hpp
 *
 *  Created on: Mar 7, 2017
 *      Author: Dario Bellicoso
 */

#pragma once

// stl
#include <vector>
#include <iostream>

// eigen
#include <Eigen/Core>
#include <Eigen/QR>

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

namespace curves {

struct SplineOptions {

  SplineOptions()
      : tf_(0.0),
        pos0_(0.0), posT_(0.0),
        vel0_(0.0), velT_(0.0),
        acc0_(0.0), accT_(0.0)
  {

  }

  constexpr SplineOptions(double tf, double pos0, double posT, double vel0,
                          double velT, double acc0, double accT)
      : tf_(tf),
        pos0_(pos0), posT_(posT),
        vel0_(vel0), velT_(velT),
        acc0_(acc0), accT_(accT)
  {

  }

  //! The total duration of the spline in seconds.
  double tf_;

  //! The scalar position at time 0.
  double pos0_;

  //! The scalar position at time tf.
  double posT_;

  //! The scalar velocity at time 0.
  double vel0_;

  //! The scalar velocity at time tf.
  double velT_;

  //! The scalar acceleration at time 0.
  double acc0_;

  //! The scalar acceleration at time tf.
  double accT_;
};

namespace spline_traits {

// Main struct template.
template<typename Core_, int SplineOrder_>
struct spline_rep {

  static constexpr unsigned int splineOrder = SplineOrder_;
  static constexpr unsigned int numCoefficients = SplineOrder_+1;

  using TimeVectorType = std::array<Core_, numCoefficients>;
  using SplineCoefficients = std::array<Core_, numCoefficients>;

  static inline TimeVectorType tau(Core_ tk) noexcept;
  static inline TimeVectorType dtau(Core_ tk) noexcept;
  static inline TimeVectorType ddtau(Core_ tk) noexcept;

  static bool compute(const SplineOptions& opts, SplineCoefficients& coefficients);
};

// Specialization for third order splines.
template<>
struct spline_rep<double, 3> {

  static constexpr unsigned int splineOrder = 3;
  static constexpr unsigned int numCoefficients = splineOrder+1;

  using TimeVectorType = std::array<double, numCoefficients>;
  using SplineCoefficients = std::array<double, numCoefficients>;

  static inline TimeVectorType tau(double tk) noexcept {
    return { boost::math::pow<3>(tk), boost::math::pow<2>(tk), tk, 1.0 };
  }

  static inline TimeVectorType dtau(double tk) noexcept {
    return { 3.0*boost::math::pow<2>(tk), 2.0*tk, 1.0, 0.0 };
  }

  static inline TimeVectorType ddtau(double tk) noexcept {
    return { 6.0*tk, 2.0, 0.0, 0.0 };
  }

  static constexpr TimeVectorType   tauZero{{ 0.0, 0.0, 0.0, 1.0 }};
  static constexpr TimeVectorType  dtauZero{{ 0.0, 0.0, 1.0, 0.0 }};
  static constexpr TimeVectorType ddtauZero{{ 0.0, 2.0, 0.0, 0.0 }};

  static bool compute(const SplineOptions& opts, SplineCoefficients& coefficients) {

    Eigen::Matrix<double, numCoefficients, 1> b;
    b << opts.pos0_, opts.vel0_, opts.posT_, opts.velT_;

    Eigen::Matrix<double, numCoefficients, numCoefficients> A;
    A << Eigen::Map<const Eigen::Matrix<double, 1, numCoefficients>>(tauZero.data()),
         Eigen::Map<const Eigen::Matrix<double, 1, numCoefficients>>(dtauZero.data()),
         Eigen::Map<Eigen::Matrix<double, 1, numCoefficients>>((tau(opts.tf_)).data()),
         Eigen::Map<Eigen::Matrix<double, 1, numCoefficients>>((dtau(opts.tf_)).data());

    Eigen::Map<Eigen::VectorXd>(coefficients.data(), numCoefficients, 1) = A.colPivHouseholderQr().solve(b);

    return true;
  }
};

// Specialization for fifth order splines.
template<>
struct spline_rep<double, 5> {

  static constexpr unsigned int splineOrder = 5;
  static constexpr unsigned int numCoefficients = splineOrder+1;

  using TimeVectorType = std::array<double, numCoefficients>;
  using SplineCoefficients = std::array<double, numCoefficients>;

  static inline TimeVectorType tau(double tk) noexcept {
    return { boost::math::pow<5>(tk), boost::math::pow<4>(tk), boost::math::pow<3>(tk),
             boost::math::pow<2>(tk), tk, 1.0};
  }

  static inline TimeVectorType dtau(double tk) noexcept {
    return { 5.0*boost::math::pow<4>(tk), 4.0*boost::math::pow<3>(tk), 3.0*boost::math::pow<2>(tk),
             2.0*tk, 1.0, 0.0};
  }

  static inline TimeVectorType ddtau(double tk) noexcept {
    return { 20.0*boost::math::pow<3>(tk), 12.0*boost::math::pow<2>(tk), 6.0*tk,
              2.0, 0.0, 0.0};
  }

  static constexpr TimeVectorType   tauZero{{ 0.0, 0.0, 0.0, 0.0, 0.0, 1.0 }};
  static constexpr TimeVectorType  dtauZero{{ 0.0, 0.0, 0.0, 0.0, 1.0, 0.0 }};
  static constexpr TimeVectorType ddtauZero{{ 0.0, 0.0, 0.0, 2.0, 0.0, 0.0 }};

  static bool compute(const SplineOptions& opts, SplineCoefficients& coefficients) {
    Eigen::Matrix<double, numCoefficients, 1> b;
    b << opts.pos0_, opts.vel0_, opts.acc0_, opts.posT_, opts.velT_, opts.accT_;

    Eigen::Matrix<double, numCoefficients, numCoefficients> A;
    A << Eigen::Map<const Eigen::Matrix<double, 1, numCoefficients>>(tauZero.data()),
         Eigen::Map<const Eigen::Matrix<double, 1, numCoefficients>>(dtauZero.data()),
         Eigen::Map<const Eigen::Matrix<double, 1, numCoefficients>>(ddtauZero.data()),
         Eigen::Map<Eigen::Matrix<double, 1, numCoefficients>>((tau(opts.tf_)).data()),
         Eigen::Map<Eigen::Matrix<double, 1, numCoefficients>>((dtau(opts.tf_)).data()),
         Eigen::Map<Eigen::Matrix<double, 1, numCoefficients>>((ddtau(opts.tf_)).data());

    Eigen::Map<Eigen::VectorXd>(coefficients.data(), numCoefficients, 1) = A.colPivHouseholderQr().solve(b);

    return true;
  }

};

}

}


#pragma once
#include <Eigen/Dense>
#include <autoppl/math/autocorrelation.hpp>

namespace ppl {
namespace math {
namespace details {

template <class T>
using vec_cref_t = std::vector<
    std::reference_wrapper<const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>> >;

} // namespace details

/**
 * Computes the effective sample size (ESS) for a given vector of samples (matrices).
 * Every element of the vector is a matrix of samples for each chain.
 * Every matrix contains the samples as rows, i.e.
 * every row is a sample of an p-dimensional vector, where p
 * is the number of columns of the matrix (number of parameters).
 *
 * The algorithm assumes that every sample matrix has the same dimensions.
 *
 * @tparam  T           underlying Eigen expression type
 * @param   samples     vector of samples
 *
 * @return  a vector of ESS for each component
 *          If number of samples is 1 or less, or there are 0 components,
 *          or number of chains is 0, return an empty vector.
 *          In either case, the dimension of the return vector is same
 *          as the number of components.
 */
template <class T>
inline auto ess(const details::vec_cref_t<T>& samples)
{
    using value_t = T;
    using vec_t = Eigen::Matrix<value_t, Eigen::Dynamic, 1>;
    using mat_t = Eigen::Matrix<value_t, Eigen::Dynamic, Eigen::Dynamic>;

    vec_t tau_hat;

    size_t M = samples.size();      // number of chains

    if (M == 0) return tau_hat;

    size_t dim = samples[0].get().cols();    // sample dimension
    size_t N = samples[0].get().rows();      // number of samples

    if (N <= 1 || dim == 0) return tau_hat;

    tau_hat = vec_t::Zero(dim);

    auto var = [N](const auto& mat) {
        return (mat.rowwise() -
                mat.colwise().mean()).colwise()
                                     .squaredNorm() / (N-1);
    };

    // use N-1 scaling to compute variance
    // each col is the sample variance per chain
    mat_t sample_vars(dim, M);
    for (size_t i = 0; i < M; ++i) {
        sample_vars.col(i) = var(samples[i].get());
    }

    // column vector of average of sample variances
    vec_t W = sample_vars.rowwise().mean();

    // compute variance estimator
    vec_t var_est = static_cast<T>(N-1) / N * W;

    // if there is more than 1 chain, then update by N * B
    // where B is the between-chain variance
    if (M > 1) {
        mat_t sample_mean(dim, M);
        for (size_t i = 0; i < M; ++i) {
            sample_mean.col(i) = samples[i].get().colwise().mean();
        }
        var_est += var(sample_mean.transpose());
    }

    // compute autocorrelation vector for each component
    // every column vector (every component) is average AC over chains
    mat_t acov_mean(N, dim);
    acov_mean.setZero();

    for (size_t m = 1; m <= M; ++m) {
        mat_t next_acov = autocorrelation(samples[m-1].get());
        for (int j = 0; j < next_acov.cols(); ++j) {
            next_acov.col(j) *= sample_vars(j,m-1);
        }
        value_t m_inv = 1./m;
        acov_mean = m_inv * next_acov + (m-1) * m_inv * acov_mean;
    }

    // compute rho-hat at lag t for dimension d
    auto rho_hat = [&](size_t t, size_t d) {
        return 1. - (W(d) - acov_mean(t,d))/var_est(d);
    };

    // compute tau-hat directly to save memory
    for (size_t d = 0; d < dim; ++d) {

        // first two should not be corrected for positive and monotoneness
        value_t curr_rho_hat_even = rho_hat(0,d);
        value_t curr_p_hat = curr_rho_hat_even + rho_hat(1,d);  // current P_hat(t)
        value_t curr_min = curr_p_hat;                          // current min of P_hat(t)
        tau_hat(d) = curr_min;                                  // update with P_hat(0)

        // only estimate up to 3 samples before the end
        // and Geyer's positive condition holds
        size_t t = 2;
        for (; t < (N-3) && curr_p_hat > 0; t += 2) {
            curr_rho_hat_even = rho_hat(t,d);
            curr_p_hat = curr_rho_hat_even + rho_hat(t+1,d);

            // if positive condition holds, take the min
            // of current P_hat(t) with the min of previous P_hat's
            // to create a monotone sequence and accumulate to tau_hat
            if (curr_p_hat >= 0) {
                curr_min = std::min(curr_min, curr_p_hat);
                tau_hat(d) += curr_min;
            }
        }

        // correct to improve estimate (see STAN's implementation)
        value_t correction = (curr_rho_hat_even > 0) ?
                curr_rho_hat_even : rho_hat(t,d);

        tau_hat(d) *= 2.; // 2 * sum of adjusted P_hat(t)
        tau_hat(d) -= 1.; // -1 + 2 * sum of adjusted P_hat(t)
        tau_hat(d) += correction;
    }

    vec_t n_eff = N*M*(1./tau_hat.array()).min(std::log10(N)).matrix();

    return n_eff;
}

/**
 * Computes the effective sample size (ESS) for a given sample matrix.
 * This is an overload for when there is only chain and can supply
 * a single matrix instead.
 * See above overload for more details.
 *
 * @tparam  T           underlying Eigen expression type
 * @param   samples     sample matrix (1 chain)
 *
 * @return  a vector of ESS for each component
 */
template <class T>
inline auto ess(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& samples)
{
    details::vec_cref_t<T> v;
    v.emplace_back(samples);
    return ess(v);
}

} // namespace math
} // namespace ppl


#include <deal.II/base/point.h>

#include <gtest/gtest.h>

using namespace dealii;

TEST(Pythagoras, Norm)
{
  Point<2> x(3, 4);
  ASSERT_EQ(x.norm(), 5);
}

TEST(Pythagoras, Distance)
{
  Point<2> x(4, 5);
  Point<2> y(1, 1);
  ASSERT_EQ(x.distance(y), 5);
}

TEST(Pythagoras, ScalarProduct)
{
  Point<2> x(3, 4);
  ASSERT_EQ(x * x, 25);
}

TEST(Pythagoras3, Norm)
{
  Point<3> x(20, 4, 5);
  ASSERT_EQ(x.norm(), 21);
}

TEST(Pythagoras3, Distance)
{
  Point<3> x(21, 5, 6);
  Point<3> y(1, 1, 1);
  ASSERT_EQ(x.distance(y), 21);
}

TEST(Pythagoras3, ScalarProduct)
{
  Point<3> x(20, 4, 5);
  ASSERT_EQ(x * x, 441);
}


//
// Copyright (c) 2012 Juan Palacios juan.palacios.puyana@gmail.com
// This file is part of minimathlibs.
// Subject to the BSD 2-Clause License
// - see < http://opensource.org/licenses/BSD-2-Clause>
//
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE TestTransform3D
#include <boost/test/unit_test.hpp>
#include <iostream>
#include <iomanip>
#include <vector>
#include "minimath/rotation3d.hpp"
#include "minimath/transform3d.hpp"
#include "minimath/point3d.hpp"
#include "minimath/geom3d_ops.hpp"
#include "TestRotation3DUtils.h"

using namespace minimath;

namespace
{
struct setup
{
    setup() { std::cout << std::setprecision(18); }
};

}

BOOST_FIXTURE_TEST_SUITE(TestTransform3D, setup)

BOOST_AUTO_TEST_CASE(testInstantiation)
{
  rotation3d<double> rot1, rot2;
}

BOOST_AUTO_TEST_CASE(testDefaultEquality)
{
  BOOST_CHECK(rotation3d<double>()==rotation3d<double>());
}

BOOST_AUTO_TEST_CASE(testDefaultIsIdentity)
{
  transform3d<double> t;
  BOOST_CHECK(p100 == t*p100);
  BOOST_CHECK(p010 == t*p010);
  BOOST_CHECK(p001 == t*p001);
  BOOST_CHECK(p111 == t*p111);
}

BOOST_AUTO_TEST_CASE(testCopyConstruction)
{
  rotation3d<double> rot1;
  rotation3d<double> rot2(rot1);
  BOOST_CHECK(rot1 == rot2);
}

BOOST_AUTO_TEST_CASE(testAssignment)
{
  rotation3d<double> rot1, rot2;
  rot2 = rot1;
  BOOST_CHECK(rot1 == rot2);

}

BOOST_AUTO_TEST_CASE(testTranslation)
{
  transform3d<double> transf(translation3d<double>(111., 222., 333.));
  pointxyzd p;
  p = transf*p;
  BOOST_CHECK(p==pointxyzd(111., 222., 333.));

}

BOOST_AUTO_TEST_CASE(testRotation3D)
{
  rotation3d<double> rot;
  transform3d<double> transf(rot);
  pointxyzd p;
  p = transf*p;
  BOOST_CHECK(p==pointxyzd());
}

BOOST_AUTO_TEST_CASE(testRotationX45)
{
  rotation3d<double> rot(rotation3dx<double>(PI/4.));
  transform3d<double> transf(rot);
  pointxyzd pTest = transf*p100;
  BOOST_CHECK(pTest == pointxyzd(1., 0., 0.));

  pTest = transf*p010;
  BOOST_CHECK(pTest == pointxyzd(0., cos45, cos45));

  pTest = transf*p001;
  BOOST_CHECK(pTest == pointxyzd(0., -cos45, cos45));
}

BOOST_AUTO_TEST_CASE(testRotationX90)
{
  rotation3d<double> rot(rotation3dx<double>(PI/2.));
  transform3d<double> transf(rot);
  pointxyzd pTest = transf*p100;
  BOOST_CHECK(pTest == pointxyzd(1., 0., 0.));

  pTest = transf*p010;
  BOOST_CHECK(pTest == pointxyzd(0., 0., 1.));

  pTest = transf*p001;
  BOOST_CHECK(pTest == pointxyzd(0., -1., 0.));
}

BOOST_AUTO_TEST_CASE(testRotationX180)
{
  rotation3d<double> rot(rotation3dx<double>(1.*PI));
  transform3d<double> transf(rot);
  pointxyzd pTest = transf*p100;
  BOOST_CHECK(pTest == pointxyzd(1., 0., 0.));

  pTest = transf*p010;
  BOOST_CHECK(pTest == pointxyzd(0., -1, 0));

  pTest = transf*p001;
  BOOST_CHECK(pTest == pointxyzd(0., 0, -1));
}

BOOST_AUTO_TEST_CASE(testRotationY45)
{
  rotation3d<double> rot(rotation3dy<double>(PI/4.));
  transform3d<double> transf(rot);
  pointxyzd pTest = transf*p100;
  BOOST_CHECK(pTest == pointxyzd(cos45, 0., -cos45));

  pTest = transf*p010;
  BOOST_CHECK(pTest == pointxyzd(0., 1., 0.));

  pTest = transf*p001;
  BOOST_CHECK(pTest == pointxyzd(cos45, 0., cos45));
}

BOOST_AUTO_TEST_CASE(testRotationY90)
{
  rotation3d<double> rot(rotation3dy<double>(PI/2.));
  transform3d<double> transf(rot);
  pointxyzd pTest = transf*p100;
  BOOST_CHECK(pTest == pointxyzd(0., 0., -1.));

  pTest = transf*p010;
  BOOST_CHECK(pTest == pointxyzd(0., 1., 0.));

  pTest = transf*p001;
  BOOST_CHECK(pTest == pointxyzd(1., 0., 0.));
}

BOOST_AUTO_TEST_CASE(testRotationY180)
{
  rotation3d<double> rot(rotation3dy<double>(1.*PI));
  transform3d<double> transf(rot);
  pointxyzd pTest = transf*p100;
  BOOST_CHECK(pTest == pointxyzd(-1., 0., 0.));

  pTest = transf*p010;
  BOOST_CHECK(pTest == pointxyzd(0., 1., 0.));

  pTest = transf*p001;
  BOOST_CHECK(pTest == pointxyzd(0., 0., -1.));
}

BOOST_AUTO_TEST_CASE(testRotationZ45)
{
  rotation3d<double> rot(rotation3dz<double>(PI/4.));
  transform3d<double> transf(rot);
  pointxyzd pTest = transf*p100;
  BOOST_CHECK(pTest == pointxyzd(cos45, cos45, 0.));

  pTest = transf*p010;
  BOOST_CHECK(pTest == pointxyzd(-cos45, cos45, 0.));

  pTest = transf*p001;
  BOOST_CHECK(pTest == pointxyzd(0., 0., 1.));
}

BOOST_AUTO_TEST_CASE(testRotationZ90)
{
  rotation3d<double> rot(rotation3dz<double>(PI/2.));
  transform3d<double> transf(rot);
  pointxyzd pTest = transf*p100;
  BOOST_CHECK(pTest == pointxyzd(0., 1., 0.));

  pTest = transf*p010;
  BOOST_CHECK(pTest == pointxyzd(-1., 0., 0.));

  pTest = transf*p001;
  BOOST_CHECK(pTest == pointxyzd(0., 0., 1.));
}

BOOST_AUTO_TEST_CASE(testRotationZ180)
{
  rotation3d<double> rot(rotation3dz<double>(1.*PI));
  transform3d<double> transf(rot);
  pointxyzd pTest = transf*p100;
  BOOST_CHECK(pTest == pointxyzd(-1., 0., 0.));

  pTest = transf*p010;
  BOOST_CHECK(pTest == pointxyzd(0., -1., 0.));

  pTest = transf*p001;
  BOOST_CHECK(pTest == pointxyzd(0., 0., 1.));
}

BOOST_AUTO_TEST_CASE(testNullRotationAndTranslation)
{
  transform3d<double> transf(rotation3d<double>(), translation3d<double>(111., 222, 333.));
  pointxyzd p;
  p = transf*p;
  BOOST_CHECK(p==pointxyzd(111., 222., 333.));
}

BOOST_AUTO_TEST_CASE(testRotationXAndTranslation)
{
  for (unsigned int i = 1; i<9; ++i)
  {
    rotation3d<double> rot(rotation3dx<double>(PI/i));
    transform3d<double> transf(rot, translation3d<double>(111., 222, 333.));
    pointxyzd translation(111.,222.,333.);
    pointxyzd pTest = transf*p100;
    BOOST_CHECK(minimath::equal(pTest, rot*p100 + translation));

    pTest = transf*p010;
    bool test = minimath::equal(pTest, rot*p010 + translation);
    if (!test) {
      std::cout << "\nFail i = " << i << " pTest = " << pTest
          << " rot*p010 = " << (rot*p010) << "\n";
      std::cout << " rot*p010 + trans = " << (rot*p010 + translation) << "\n";
    }

    BOOST_CHECK(minimath::equal(pTest, rot*p010 + translation));

    pTest = transf*p001;
    BOOST_CHECK(minimath::equal(pTest,rot*p001 + translation));
  }
}

BOOST_AUTO_TEST_CASE(testRotationYAndTranslation)
{
  for (unsigned int i = 1; i<9; ++i)
  {
    rotation3d<double> rot(rotation3dy<double>(PI/i));
    transform3d<double> transf(rot, translation3d<double>(111., 222, 333.));
    pointxyzd translation(111.,222.,333.);
    pointxyzd pTest = transf*p100;
    BOOST_CHECK(minimath::equal(pTest, rot*p100 + translation));

    pTest = transf*p010;
    BOOST_CHECK(minimath::equal(pTest, rot*p010 + translation));

    pTest = transf*p001;
    BOOST_CHECK(minimath::equal(pTest, rot*p001 + translation));
  }
}

BOOST_AUTO_TEST_CASE(testRotationZAndTranslation)
{
  for (unsigned int i = 1; i<9; ++i)
  {
    rotation3d<double> rot(rotation3dz<double>(PI/i));
    transform3d<double> transf(rot, translation3d<double>(111., 222, 333.));
    pointxyzd translation(111.,222.,333.);
    pointxyzd pTest = transf*p100;
    BOOST_CHECK(minimath::equal(pTest, rot*p100 + translation));

    pTest = transf*p010;
    BOOST_CHECK(minimath::equal(pTest, rot*p010 + translation));

    pTest = transf*p001;
    BOOST_CHECK(minimath::equal(pTest, rot*p001 + translation));
  }
}

BOOST_AUTO_TEST_CASE(testTranslationAndNullRotation)
{
  transform3d<double> transf(translation3d<double>(111., 222, 333.), rotation3d<double>());
  pointxyzd p;
  p = transf*p;
  BOOST_CHECK(p==pointxyzd(111., 222., 333.));
}

BOOST_AUTO_TEST_CASE(testTranslationAndRotationX)
{
  for (unsigned int i = 1; i<9; ++i)
  {
    rotation3d<double> rot(rotation3dx<double>(PI/i));
    pointxyzd translation(999.,999.,999.);
    transform3d<double> transf(translation3d<double>(translation), rot);

    pointxyzd pTest = transf*p100;

    pointxyzd res100 = rot*translation + rot*p100;
    BOOST_CHECK(minimath::equal(pTest, res100));

    pTest = transf*p010;
    pointxyzd res010 = rot*translation + rot*p010;
    BOOST_CHECK(minimath::equal(pTest, res010));

    pTest = transf*p001;
    pointxyzd res001 = rot*translation + rot*p001;
    BOOST_CHECK(minimath::equal(pTest, res001));

  }

}

BOOST_AUTO_TEST_CASE(testTranslationAndRotationY)
{
  for (unsigned int i = 1; i<9; ++i)
  {
    rotation3d<double> rot(rotation3dy<double>(PI/i));
    pointxyzd translation(999.,999.,999.);
    transform3d<double> transf(translation3d<double>(translation), rot);

    pointxyzd pTest = transf*p100;
    pointxyzd res100 = rot*translation + rot*p100;
    BOOST_CHECK(minimath::equal(pTest, res100, 200));

    pTest = transf*p010;
    pointxyzd res010 = rot*translation + rot*p010;
    BOOST_CHECK(minimath::equal(pTest, res010));

    pTest = transf*p001;
    pointxyzd res001 = rot*translation + rot*p001;
    BOOST_CHECK(minimath::equal(pTest, res001));

  }

}

BOOST_AUTO_TEST_CASE(testTranslationAndRotationZ)
{
  for (unsigned int i = 1; i<9; ++i)
  {
    rotation3d<double> rot(rotation3dz<double>(PI/i));
    pointxyzd translation(999.,999.,999.);
    transform3d<double> transf(translation3d<double>(translation), rot);

    pointxyzd pTest = transf*p100;
    pointxyzd res100 = rot*translation + rot*p100;
    BOOST_CHECK(minimath::equal(pTest, res100, 2000));

    pTest = transf*p010;
    pointxyzd res010 = rot*translation + rot*p010;
    BOOST_CHECK(minimath::equal(pTest, res010));

    pTest = transf*p001;
    pointxyzd res001 = rot*translation + rot*p001;
    BOOST_CHECK(minimath::equal(pTest, res001));
  }
}

// test that a multiplication of Transform3Ds is the same as applying
// the individual transformations to a point one by one.
// Since we are chaining many operations, we use a "loose" tolerance of
// ~1000 epsilons.
BOOST_AUTO_TEST_CASE(testMultiplication)
{
  for (unsigned int i = 0; i < 10; ++i)
  {

    // get some random rotations and translations
    rotation3d<double> rot0 = TestUtils::randomRotation();
    rotation3d<double> rot1 = TestUtils::randomRotation();
    rotation3d<double> rot2 = TestUtils::randomRotation();
    translation3d<double> transl0 = TestUtils::randomTranslation(100.);
    translation3d<double> transl1 = TestUtils::randomTranslation(10.);
    translation3d<double> transl2 = TestUtils::randomTranslation(50.);

    // multiply the rotations and translations together
    transform3d<double> trans = transform3d<double>(rot0)*transform3d<double>(transl0)*transform3d<double>(rot1)*transform3d<double>(transl1)*transform3d<double>(rot2)*transform3d<double>(transl2);

    // get a sets of random points
    for (unsigned int j = 0; j < 10; ++j)
    {
      pointxyzd r = TestUtils::randomPoint(999);
      pointxyzd p0 = trans*r;
      pointxyzd p1 = transl2*r;
      p1 = rot2*p1;
      p1 = transl1*p1;
      p1 = rot1*p1;
      p1 = transl0*p1;
      p1 = rot0*p1;
      // check the resulting transformation is equivalent to the
      // product of transformations, by applying to the random points.

      BOOST_CHECK(minimath::equal(p0,p1, 2048));
    }
  }
}

BOOST_AUTO_TEST_CASE(testInverse)
{
  BOOST_CHECK(TestUtils::testInverseTransform3D<rotation3dx<double> >());
  BOOST_CHECK(TestUtils::testInverseTransform3D<rotation3dy<double> >());
  BOOST_CHECK(TestUtils::testInverseTransform3D<rotation3dz<double> >());
}

BOOST_AUTO_TEST_CASE(testInvert)
{
  BOOST_CHECK(TestUtils::testInvertTransform3D<rotation3dx<double> >());
  BOOST_CHECK(TestUtils::testInvertTransform3D<rotation3dy<double> >());
  BOOST_CHECK(TestUtils::testInvertTransform3D<rotation3dz<double> >());
}

BOOST_AUTO_TEST_SUITE_END()


/**
 * @file
 * @brief NPDE homework ElementMatrixComputation code
 * @author Janik Schüttler
 * @date 03.03.2019
 * @copyright Developed at ETH Zurich
 */

#include <memory>

#include <Eigen/Core>
#include <Eigen/SparseLU>

#include <gtest/gtest.h>

#include <lf/assemble/assemble.h>
#include <lf/base/base.h>
#include <lf/mesh/test_utils/test_meshes.h>
#include <lf/uscalfe/uscalfe.h>

#include "../../meshes/mesh.h"
#include "../mylinearfeelementmatrix.h"
#include "../mylinearloadvector.h"
#include "../solve.h"

namespace ElementMatrixComputation::test {

double numericalPrecision = 1e-8;

double f(Eigen::Vector2d x) { return 1 + x(0) * x(0) + x(1) * x(1); };

// useful functors
Eigen::Matrix<double, 2, 2> zeroMatrixFunctor(Eigen::Vector2d x) {
  return Eigen::MatrixXd::Zero(2, 2);
};

double zeroScalar(Eigen::Vector2d x) { return 0.0; };

Eigen::Matrix<double, 2, 2> identityMatrixFunctor(Eigen::Vector2d x) {
  return Eigen::MatrixXd::Identity(2, 2);
};

double identityScalarFunctor(Eigen::Vector2d x) { return 1.0; };

//////////////////////
// Test solve
//////////////////////
TEST(Solve, test) {
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(8, 1.0 / 3.0);
  auto fe_space =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);
  const lf::mesh::Mesh &mesh{*(fe_space->Mesh())};
  const lf::assemble::DofHandler &dofh{fe_space->LocGlobMap()};
  const lf::base::size_type N_dofs(dofh.NumDofs());

  lf::mesh::utils::MeshFunctionGlobal mf_alpha{identityMatrixFunctor};
  lf::mesh::utils::MeshFunctionGlobal mf_gamma{identityScalarFunctor};

  // Galerkin matrix
  lf::uscalfe::ReactionDiffusionElementMatrixProvider<
      double, decltype(mf_alpha), decltype(mf_gamma)>
      elmat_builder(fe_space, mf_alpha, mf_gamma);

  lf::assemble::COOMatrix<double> A(N_dofs, N_dofs);
  lf::assemble::AssembleMatrixLocally(0, dofh, dofh, elmat_builder, A);
  Eigen::SparseMatrix<double> A_crs = A.makeSparse();

  // Right hand side
  lf::mesh::utils::MeshFunctionGlobal mf_f{ElementMatrixComputation::f};

  lf::uscalfe::LinearFELocalLoadVector<double, decltype(mf_f)> elvec_builder(
      mf_f);
  Eigen::Matrix<double, Eigen::Dynamic, 1> phi(N_dofs);
  phi.setZero();
  AssembleVectorLocally(0, dofh, elvec_builder, phi);

  // solve
  Eigen::VectorXd sol_vec = Eigen::VectorXd::Zero(N_dofs);
  Eigen::SparseLU<Eigen::SparseMatrix<double>> solver;
  solver.compute(A_crs);
  sol_vec = solver.solve(phi);

  double error =
      (sol_vec - ElementMatrixComputation::solve(elmat_builder, elvec_builder))
          .norm();

  EXPECT_LT(error, numericalPrecision);
}

//////////////////////
// Test SolvePoissonBVP
//////////////////////
TEST(SolvePoissonBVP, test) {
  lf::mesh::utils::MeshFunctionGlobal mf_f{ElementMatrixComputation::f};

  lf::uscalfe::LinearFELaplaceElementMatrix elmat_builder;
  lf::uscalfe::LinearFELocalLoadVector<double, decltype(mf_f)> elvec_builder(
      mf_f);

  Eigen::VectorXd solution =
      ElementMatrixComputation::solve(elmat_builder, elvec_builder);

  Eigen::VectorXd student_solution =
      ElementMatrixComputation::solvePoissonBVP();

  if (student_solution.norm() < numericalPrecision) {
    EXPECT_TRUE(false);
  } else {
    double error = (solution - student_solution).norm();

    EXPECT_LT(error, numericalPrecision);
  }
}

//////////////////////
// Test MyLinearLoadVector
//////////////////////
TEST(MyLinearLoadVector, testTriangles) {
  std::shared_ptr<lf::mesh::Mesh> mesh_p =
      lf::mesh::test_utils::GenerateHybrid2DTestMesh(0, 1.0 / 3.0);

  lf::mesh::utils::MeshFunctionGlobal mf_f{ElementMatrixComputation::f};

  ElementMatrixComputation::MyLinearLoadVector elvec_builder(
      ElementMatrixComputation::f);
  lf::uscalfe::LinearFELocalLoadVector<double, decltype(mf_f)>
      elvec_builder_exact(mf_f);

  for (const lf::mesh::Entity *cell : mesh_p->Entities(0)) {
    if (cell->RefEl() == lf::base::RefEl::kTria()) {
      auto elem_vec = elvec_builder.Eval(*cell);
      auto elem_vec_exact = elvec_builder_exact.Eval(*cell);
      double error = (elem_vec - elem_vec_exact).norm();
      EXPECT_LT(error, numericalPrecision);
    }
  }
}

TEST(MyLinearLoadVector, testQuads) {
  auto mesh_p = Generate2DTestMesh();

  lf::mesh::utils::MeshFunctionGlobal mf_f{ElementMatrixComputation::f};

  ElementMatrixComputation::MyLinearLoadVector elvec_builder(
      ElementMatrixComputation::f);
  lf::uscalfe::LinearFELocalLoadVector<double, decltype(mf_f)>
      elvec_builder_exact(mf_f);

  for (const lf::mesh::Entity *cell : mesh_p->Entities(0)) {
    if (cell->RefEl() == lf::base::RefEl::kQuad()) {
      auto elem_vec = elvec_builder.Eval(*cell);
      auto elem_vec_exact = elvec_builder_exact.Eval(*cell);
      double error = (elem_vec - elem_vec_exact).norm();
      EXPECT_LT(error, numericalPrecision);
    }
  }
}

//////////////////////
// Test MyLinearFEElementMatrix
//////////////////////
TEST(MyLinearFEElementMatrix, testTriangles) {
  std::shared_ptr<lf::mesh::Mesh> mesh_p =
      lf::mesh::test_utils::GenerateHybrid2DTestMesh(0, 1.0 / 3.0);
  auto fe_space =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);
  const lf::mesh::Mesh &mesh{*(fe_space->Mesh())};

  lf::mesh::utils::MeshFunctionGlobal mf_alpha{identityMatrixFunctor};
  lf::mesh::utils::MeshFunctionGlobal mf_gamma{identityScalarFunctor};

  ElementMatrixComputation::MyLinearFEElementMatrix elmat_builder;
  lf::uscalfe::ReactionDiffusionElementMatrixProvider<
      double, decltype(mf_alpha), decltype(mf_gamma)>
      elmat_builder_exact(fe_space, mf_alpha, mf_gamma);

  for (const lf::mesh::Entity *cell : mesh.Entities(0)) {
    if (cell->RefEl() == lf::base::RefEl::kTria()) {
      auto elem_mat = elmat_builder.Eval(*cell);
      auto elem_mat_exact = elmat_builder_exact.Eval(*cell);
      double error = (elem_mat.block(0, 0, 3, 3) - elem_mat_exact).norm();
      EXPECT_LT(error, numericalPrecision);
    }
  }
}

TEST(MyLinearFEElementMatrix, testQuads) {
  // std::shared_ptr<lf::mesh::Mesh> mesh_p =
  //     lf::mesh::test_utils::GenerateHybrid2DTestMesh(0, 1.0 / 3.0);
  auto mesh_p = Generate2DTestMesh();
  auto fe_space =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);
  const lf::mesh::Mesh &mesh{*(fe_space->Mesh())};

  lf::mesh::utils::MeshFunctionGlobal mf_alpha{identityMatrixFunctor};
  lf::mesh::utils::MeshFunctionGlobal mf_gamma{identityScalarFunctor};

  ElementMatrixComputation::MyLinearFEElementMatrix elmat_builder;
  lf::uscalfe::ReactionDiffusionElementMatrixProvider<
      double, decltype(mf_alpha), decltype(mf_gamma)>
      elmat_builder_exact(fe_space, mf_alpha, mf_gamma);

  for (const lf::mesh::Entity *cell : mesh.Entities(0)) {
    if (cell->RefEl() == lf::base::RefEl::kQuad()) {
      auto elem_mat = elmat_builder.Eval(*cell);
      auto elem_mat_exact = elmat_builder_exact.Eval(*cell);
      double error = (elem_mat - elem_mat_exact).norm();
      EXPECT_LT(error, numericalPrecision);
    }
  }
}

//////////////////////
// Test solveNeumannEq
//////////////////////
TEST(solveNeumannEq, test) {
  ElementMatrixComputation::MyLinearFEElementMatrix elmat_builder;
  ElementMatrixComputation::MyLinearLoadVector elvec_builder(
      ElementMatrixComputation::f);

  Eigen::VectorXd solution =
      ElementMatrixComputation::solve(elmat_builder, elvec_builder);
  Eigen::VectorXd student_solution = ElementMatrixComputation::solveNeumannEq();

  if (student_solution.norm() < numericalPrecision) {
    EXPECT_TRUE(false);
  } else {
    double error = (solution - student_solution).norm();

    EXPECT_LT(error, numericalPrecision);
  }
}

}  // namespace ElementMatrixComputation::test


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

#ifndef META_MATH_IS_PRIME_INCLUDE
#define META_MATH_IS_PRIME_INCLUDE

#include <boost/mpl/bool.hpp>
#include <boost/numeric/meta_math/sqrt.hpp>
// #include <boost/config/concept_macros.hpp>
#ifdef __GXX_CONCEPTS__
#   include <concepts>
#endif

namespace meta_math {

namespace mpl = boost::mpl;

namespace impl {

    // Checks if 'x' is divisible by some odd number >= max_odd, checking decreasingly
    template <long int x, long int max_odd>
    struct is_prime_to_max_odd
    {
	static bool const value = x % max_odd != 0
                                  && is_prime_to_max_odd<x, max_odd-2>::value;
    };

    // Once we reach 1, it's prime
    template <long int x> struct is_prime_to_max_odd<x, 1> : mpl::true_ {};

    // Returns the largest number that x is tried to divided by.
    // This is a odd number slightly larger than the approximated square root.
    template <long int x>
    struct max_prime_compare
    {
	static long int const tmp = sqrt<x>::value,
                              value = tmp % 2 == 0 ? tmp + 1 : tmp + 2;
    };

    // Checks if there is an odd number between 3 and sqrt(x)+1 that divides x
    // if there is no divisor found then x is prime (otherwise not)
    // must be disabled when x is even
    template <long int x, bool Disable>
    struct check_odd
    {
	static bool const value = is_prime_to_max_odd<x, max_prime_compare<x>::value>::value;
    };

    // Intended for even numbers (> 2) which are always prime
    template <long int x>
    struct check_odd<x, true>
    {
	static bool const value = false;
    };

}

template <long int x>
struct is_prime
{
  static bool const value = impl::check_odd<x, x % 2 == 0>::value;
};

template <> struct is_prime<0> : mpl::false_ {};
template <> struct is_prime<1> : mpl::false_ {};
template <> struct is_prime<2> : mpl::true_ {};
template <> struct is_prime<3> : mpl::true_ {};
template <> struct is_prime<5> : mpl::true_ {};

#ifdef __GXX_CONCEPTS__
    concept Prime<long int N> {}

    template <long int N>
        where std::True<is_prime<N>::value>
    concept_map Prime<N> {}
#endif

} // namespace meta_math

#endif // META_MATH_IS_PRIME_INCLUDE


#pragma once
#include "PolynomialRootSolver.h"
#include <opencv2/core/core.hpp>
#include <Eigenvalues>
#include <opencv2/core/eigen.hpp>

PolynomialRootSolver::PolynomialRootSolver()
{

}

PolynomialRootSolver::~PolynomialRootSolver()
{

}

Eigen::MatrixXd computeCompanionMatrix(const cv::Mat rowOfCoefficientsInAscendingDegreeOrder)
{
    int numberOfColumns  = rowOfCoefficientsInAscendingDegreeOrder.size().width  -1 ;
    Eigen::MatrixXd eigenCoefficients(1,numberOfColumns+1);
    Eigen::MatrixXd companionMatrix(numberOfColumns,numberOfColumns);
    companionMatrix.setZero();
    Eigen::MatrixXd identitySubmatrix = Eigen::MatrixXd::Identity(numberOfColumns-1,numberOfColumns-1);
    cv::cv2eigen(rowOfCoefficientsInAscendingDegreeOrder, eigenCoefficients);
    eigenCoefficients /= eigenCoefficients(numberOfColumns);

    companionMatrix.block(0,1,numberOfColumns-1,numberOfColumns-1) = identitySubmatrix;
    companionMatrix.block(numberOfColumns-1,0,1,numberOfColumns) -= eigenCoefficients.block(0,0,1,numberOfColumns);
    return companionMatrix;
}

cv::Mat PolynomialRootSolver::computeRoots(const cv::Mat& rowOfCoefficientsInAscendingDegreeOrder)
{
    Eigen::MatrixXd eigenCompanionMatrix = computeCompanionMatrix(rowOfCoefficientsInAscendingDegreeOrder);
    Eigen::EigenSolver<Eigen::MatrixXd> eigenSolver(eigenCompanionMatrix);
    Eigen::MatrixXd realEigenvalues = eigenSolver.eigenvalues().real();
    realEigenvalues.transposeInPlace();
    cv::Mat roots;
    cv::eigen2cv(realEigenvalues, roots);
    return roots;
}


// (C) Copyright Andrew Sutton 2007
//
// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0 (See accompanying file
// LICENSE_1_0.txt or http://www.boost.org/LICENSE_1_0.txt)

//[mean_geodesic_example
#include <iostream>
#include <iomanip>

#include <boost/graph/undirected_graph.hpp>
#include <boost/graph/exterior_property.hpp>
#include <boost/graph/floyd_warshall_shortest.hpp>
#include <boost/graph/geodesic_distance.hpp>
#include "helper.hpp"

using namespace std;
using namespace boost;

// The Actor type stores the name of each vertex in the graph.
struct Actor
{
    string name;
};

// Declare the graph type and its vertex and edge types.
typedef undirected_graph<Actor> Graph;
typedef graph_traits<Graph>::vertex_descriptor Vertex;
typedef graph_traits<Graph>::edge_descriptor Edge;

// The name map provides an abstract accessor for the names of
// each vertex. This is used during graph creation.
typedef property_map<Graph, string Actor::*>::type NameMap;

// Declare a matrix type and its corresponding property map that
// will contain the distances between each pair of vertices.
typedef exterior_vertex_property<Graph, int> DistanceProperty;
typedef DistanceProperty::matrix_type DistanceMatrix;
typedef DistanceProperty::matrix_map_type DistanceMatrixMap;

// Declare the weight map so that each edge returns the same value.
typedef constant_property_map<Edge, int> WeightMap;

// Declare a container and its corresponding property map that
// will contain the resulting mean geodesic distances of each
// vertex in the graph.
typedef exterior_vertex_property<Graph, float> GeodesicProperty;
typedef GeodesicProperty::container_type GeodesicContainer;
typedef GeodesicProperty::map_type GeodesicMap;

int
main(int argc, char *argv[])
{
    // Create the graph and a property map that provides access
    // to the actor names.
    Graph g;
    NameMap nm(get(&Actor::name, g));

    // Read the graph from standad input.
    read_graph(g, nm, cin);

    // Compute the distances between all pairs of vertices using
    // the Floyd-Warshall algorithm. Note that the weight map is
    // created so that every edge has a weight of 1.
    DistanceMatrix distances(num_vertices(g));
    DistanceMatrixMap dm(distances, g);
    WeightMap wm(1);
    floyd_warshall_all_pairs_shortest_paths(g, dm, weight_map(wm));

    // Compute the mean geodesic distances for each vertex in
    // the graph and get the average mean geodesic distace (the
    // so-called small-world distance) as a result.
    GeodesicContainer geodesics(num_vertices(g));
    GeodesicMap gm(geodesics, g);
    float sw = all_mean_geodesics(g, dm, gm);

    // Print the mean geodesic distance of each vertex and finally,
    // the graph itself.
    graph_traits<Graph>::vertex_iterator i, end;
    for(tie(i, end) = vertices(g); i != end; ++i) {
        cout << setw(12) << setiosflags(ios::left)
             << g[*i].name << get(gm, *i) << endl;
    }
    cout << "small world distance: " << sw << endl;

    return 0;
}
//]


#define BOOST_TEST_MODULE "Test Cosine class"

#include <boost/test/unit_test.hpp>

#include "distance/Cosine.hpp"

using namespace genex;

#define TOLERANCE 1e-9

struct MockData
{
  data_t dat_1[10] = {5, 0, 3, 0, 2, 0, 0, 2, 0, 0};
  data_t dat_2[10] = {3, 0, 2, 0, 1, 1, 0, 1, 0, 1};
};

BOOST_AUTO_TEST_CASE( cosine_test, *boost::unit_test::tolerance(TOLERANCE)  )
{
  MockData data;
  TimeSeries ts_1(data.dat_1, 0, 0, 5);
  TimeSeries ts_2(data.dat_2, 0, 0, 5);
  Cosine dist;

  data_t* total = dist.init();

  for (int i = 0; i < ts_1.getLength(); i++) {
    total = dist.reduce(total, total, ts_1[i], ts_2[i]);
  }

  BOOST_TEST( dist.norm(total, ts_1, ts_2), 0.94 );

  delete total;
}


// This file is part of PoseEstimation.
// Copyright (c) 2021, Eijiro Shibusawa <phd_kimberlite@yahoo.co.jp>
// All rights reserved.

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:

// 1. Redistributions of source code must retain the above copyright notice, this
//    list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright notice,
//    this list of conditions and the following disclaimer in the documentation
//    and/or other materials provided with the distribution.

// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
// ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#ifndef THREE_POINT_HPP_
#define THREE_POINT_HPP_

#include <Eigen/Dense>

class ThreePointTest;

namespace ThreePoint
{
template <typename FloatType>
class ThreePoint
{
	friend ::ThreePointTest;

private:
	typedef Eigen::Matrix<FloatType, Eigen::Dynamic, 1> Matrix_Dx1;
	typedef Eigen::Matrix<FloatType, 3, 1> Matrix_3x1;
	typedef Eigen::Matrix<FloatType, 3, 3> Matrix_3x3;
	typedef Eigen::Matrix<FloatType, 3, 3, Eigen::RowMajor> Matrix_3x3R;
	typedef Eigen::Matrix<FloatType, 3, Eigen::Dynamic> Matrix_3xD;
	typedef Eigen::Matrix<FloatType, 4, 4> Matrix_4x4;
	typedef Eigen::Matrix<FloatType, 4, 1> Matrix_4x1;

public:
	// q = [x, y, z, w] = w + x*i + y*j + z*k
	static inline void getRotation(const FloatType *q, FloatType *R)
	{
		const FloatType q0_2 = 2 * q[0], q1_2 = 2 * q[1], q2_2 = 2 * q[2];
		const FloatType q03_2 = q0_2 * q[3], q13_2 = q1_2 * q[3], q23_2 = q2_2 * q[3];
		const FloatType q00_2 = q0_2 * q[0], q01_2 = q0_2 * q[1], q02_2 = q0_2 * q[2];
		const FloatType q11_2 = q1_2 * q[1], q12_2 = q1_2 * q[2], q22_2 = q2_2 * q[2];
		R[0] = 1 - (q11_2 + q22_2);
		R[1] = q01_2 - q23_2;
		R[2] = q02_2 + q13_2;
		R[3] = q01_2 + q23_2;
		R[4] = 1 - (q00_2 + q22_2);
		R[5] = q12_2 - q03_2;
		R[6] = q02_2 - q13_2;
		R[7] = q12_2 + q03_2;
		R[8] = 1 - (q00_2 + q11_2);
	}

	static inline void transformPoints(int n, FloatType s, const FloatType *R, const FloatType *t, const FloatType *X1, FloatType *X2)
	{
		Eigen::Map<const Matrix_3x3R> mR(R);
		Eigen::Map<const Matrix_3x1> mt(t);
		Eigen::Map<const Matrix_3xD> mX1(X1, 3, n);
		Eigen::Map<Matrix_3xD> mX2(X2, 3, n);
		mX2 = (s * mR * mX1).colwise() + mt;
	}

	// X1 = [X11 Y11 Z11 X12 Y12 ... X1n Y1n Z1n]
	// X2 = [X21 Y21 Z21 X22 Y22 ... X2n Y2n Z2n]
	// X2 = s*R*X1 + t
	static void getRT(int n, const FloatType *X1, const FloatType *X2, FloatType *R, FloatType *t, FloatType &s)
	{
		Eigen::Map<const Matrix_3xD> mX1(X1, 3, n);
		Eigen::Map<const Matrix_3xD> mX2(X2, 3, n);
		// centroid
		Matrix_3x1 mc1 = mX1.rowwise().sum() / mX1.cols();
		Matrix_3x1 mc2 = mX2.rowwise().sum() / mX2.cols();
		// translate origin to centroid
		Matrix_3xD mX1c = mX1.colwise() - mc1;
		Matrix_3xD mX2c = mX2.colwise() - mc2;

		// estimate scale
		Matrix_Dx1 mX1n = mX1c.colwise().norm();
		Matrix_Dx1 mX2n = mX2c.colwise().norm();
		s = (mX1n.array() * mX2n.array()).sum() / (mX1n.array() * mX1n.array()).sum();
		mX2c *= s;

		// corrrelation matrix
		Matrix_3x3 mC = mX1c * mX2c.transpose();
		// system matrix
		Matrix_4x4 A;
		A(3, 3) = mC(0, 0) + mC(1, 1) + mC(2, 2);
		A(3, 0) = A(0, 3) = mC(1, 2) - mC(2, 1);
		A(3, 1) = A(1, 3) = mC(2, 0) - mC(0, 2);
		A(3, 2) = A(2, 3) = mC(0, 1) - mC(1, 0);
		A(0, 0) = mC(0, 0) - mC(1, 1) - mC(2, 2);
		A(0, 1) = A(1, 0) = mC(0, 1) + mC(1, 0);
		A(0, 2) = A(2, 0) = mC(2, 0) + mC(0, 2);
		A(1, 1) = -mC(0, 0) + mC(1, 1) - mC(2, 2);
		A(1, 2) = A(2, 1) = mC(1, 2) + mC(2, 1);
		A(2, 2) = -mC(0, 0) - mC(1, 1) + mC(2, 2);

		// estimate quaternion from eiven vector
		Eigen::SelfAdjointEigenSolver<Matrix_4x4> eig(A, Eigen::ComputeEigenvectors);
		getRotation(eig.eigenvectors().col(3).data(), R);
		Eigen::Map<Matrix_3x3R> mR(R);
		Eigen::Map<Matrix_3x1> mt(t);
		mt = -mR * mc1 * s + mc2;
	}
};

}

#endif // THREE_POINT_HPP_


// Copyright (c) 2018 by University Paris-Est Marne-la-Vallee
// MetaData.hpp
// This file is part of the Garamon Generator.
// Authors: Stephane Breuils and Vincent Nozick
// Contact: vincent.nozick@u-pem.fr
//
// Licence MIT
// A a copy of the MIT License is given along with this program

/// \file MetaData.hpp
/// \author Stephane Breuils, Vincent Nozick
/// \brief Contains all the required information to define and create an algebra (dimension, metric, name, code options, ...)

#ifndef GARAGEN_METADATA_HPP__
#define GARAGEN_METADATA_HPP__

#include <iostream>
#include <vector>
#include <string>
#include <Eigen/Core>
#include "MetricTools.hpp"

// data required to build a Geometric Algebra Library
class MetaData
{
public:

    // namespace of the generated lib (ex: cga::)
    std::string namespaceName;

    // dimension of the algebra vector space (grade 1)
    unsigned int dimension;

    // metric = inner product of vectors, should be a symetric matrix
    Eigen::MatrixXd metric;

    // name of each basis vector, without the first letter (1,2,3 => e1, e2, e3, e12, ...).
    // for hig dimensions, add a '_' at the end of the name to avoid: 1, 2, 3, ..., 16 => e114 : 11,4 ou 1,14 ?? => e11_4_
    std::vector<std::string> basisVectorName;

    // true if the proposed metric is diagonal
    bool inputMetricDiagonal;

    // true if the final (or initial) diagonal metric is actually identity
    bool identityMetric;

    // true if the metric has rank = dimension of the vector space supporting the algebra
    bool fullRankMetric;

    // true if the initial metric is a permutation of a diagonal matrix
    bool inputMetricPermutationOfDiagonal;

    // numerical refinement of the eigen vectors / values
    bool useEigenRefinement;

    // replace near zeros by zeros, near integers by integers, ...
    bool useNumericalCleanUp;

    // Each multivector component of grade k can have a dedicated precomputed product if its cardinality (max number of element of grade k) is lower than this threshold.
    // Else, the product is performed recursively (this is only for high dimensional Geometric Algebra).
    unsigned int maxDimPrecomputedProducts;

    // Each multivector component of grade k can have a dedicated basis accessor constant (E123 in "mv[E123]=42;") if its cardinality (max number of element of grade k) is lower than this threshold.
    // Else, this accessor is not created (this is only for high dimensional Geometric Algebra).
    unsigned int maxDimBasisAccessor;

    // numerical threshold for the numerical clean up
    double epsilon;

    // diagonal form of the metric after eigen vector/value decomposition
    Eigen::VectorXd diagonalMetric;

    // eigen vectors of the decomposition of the metric
    Eigen::MatrixXd transformationMatrix;

    // eigen vectors of the decomposition of the metric
    Eigen::MatrixXd inverseTransformationMatrix;

public:

    MetaData();

    MetaData(const std::string &filename);

    ~MetaData();

    void display() const;

    bool checkConsistency() const;

    bool metricDiagonalization();
};

#endif // GARAGEN_METADATA_HPP__


#include <tagloc/tracking.h>
#include <stdio.h>
#include <iostream>
#include <vector>
#include <boost/program_options/cmdline.hpp>
#include <boost/program_options/options_description.hpp>
#include <boost/program_options/parsers.hpp>
#include <boost/program_options/variables_map.hpp>

using namespace std;
using namespace RSN;
namespace po = boost::program_options;
#define DEF_R .1

int main(int argc, char** argv){

	double R;
	po::options_description desc("estimator options");
	desc.add_options()
		("help,h","produce help message")
		("iwls,w","(Default) Use Iterated Weighted Least Squares estimator. The line-intersection of all bearings is used as an initial estimate")
		("ekf,e","Use Extended Kalman Filter. The first two measurements are used to form the initial estimate")
		("iekf,i","Use Iterated Extended Kalman Filter. The first two measurements are used to form the initial estimate, and the update equations are re-linearized and iterated until convergence")
		("grad,g","Use gradient ascent. The line intersection is the initial estimate")
		("line,l","Use simple intersection of lines. R is ignored")
		("ambig,a","use ambiguous measurements")
		("degrees,d","measurements given in degrees")
		("noise,R", po::value<double>()->default_value(DEF_R), "set sensor noise in radians")
		("sequence,s",po::value<vector<double> > ()->multitoken(), "A full sequence of the form x y z x2 y2 z2 ... xn yn zn")
		;

	try{
		po::variables_map vm;
		po::positional_options_description p;
		p.add("sequence",-1);
		//po::store(po::parse_command_line(argc, argv,desc),vm);
		po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(),vm);
		po::notify(vm);

		if (vm.count("help") || argc == 1) {
			throw -1;
		}

		R=vm["noise"].as<double>();
		vector<double> sequence = vm["sequence"].as<vector<double> >();
		if (((int)sequence.size()) % 3 !=0 ){
			cerr<<"Incorrect number of args: "<<sequence.size()<<endl;
			throw -2;
		}
		bool degrees = (bool) vm.count("degrees");
		vector<double> px;
		vector<double> py;
		vector<double> r;
		vector<double> z;
		double t[2];
		double c[4];
		for (unsigned int i=0;i<sequence.size();){
			px.push_back(sequence[i++]);
			py.push_back(sequence[i++]);
			if (degrees){
				z.push_back((sequence[i++]/180.0)*M_PI);
			} else {
				z.push_back(sequence[i++]);
			}
			r.push_back(R);
		}
		int n = sequence.size()/3;

		bool ambig = (bool) vm.count("ambig");
		bool done=false;
		if (vm.count("grad") ) {
			RSN::line_intersection(n,px,py,z,t[0],t[1],c);
			RSN::BOT::Ml_Grad_Asc_2D(n,px,py,z,r,t,c,ambig);
			cout<<"grad: (";
			cout<<t[0]<<","<<t[1]<<")";
			cout<<" (";
			cout<<c[0]<<",";
			cout<<c[1]<<",";
			cout<<c[2]<<",";
			cout<<c[3]<<")";
			cout<<endl;
			done = true;
		}
		if (vm.count("line") ) {
			RSN::line_intersection(n,px,py,z,t[0],t[1],c);
			cout<<"line: (";
			cout<<t[0]<<","<<t[1]<<")";
			cout<<" (";
			cout<<c[0]<<",";
			cout<<c[1]<<",";
			cout<<c[2]<<",";
			cout<<c[3]<<")";
			cout<<endl;
			done = true;
		}
		if (vm.count("ekf") ) {
			vector<double> pxp(2);
			vector<double> pyp(2);
			for (int i=0;i<2;i++){
				pxp[i] = px[i];
				pyp[i] = py[i];
			}
			RSN::line_intersection(n,pxp,pyp,z,t[0],t[1],c);
			for (int i=2;i<n;i++){
				RSN::BOT::EKF_UP_2D(px[i],py[i],z[i],r[i],t,c,ambig);
			}
			cout<<"ekf: (";
			cout<<t[0]<<","<<t[1]<<")";
			cout<<" (";
			cout<<c[0]<<",";
			cout<<c[1]<<",";
			cout<<c[2]<<",";
			cout<<c[3]<<")";
			cout<<endl;
			done = true;
		}
		if (vm.count("iekf") ) {
			vector<double> pxp(2);
			vector<double> pyp(2);
			for (int i=0;i<2;i++){
				pxp[i] = px[i];
				pyp[i] = py[i];
			}
			RSN::line_intersection(n,pxp,pyp,z,t[0],t[1],c);
			for (int i=2;i<n;i++){
				RSN::BOT::IEKF_2D(px[i],py[i],z[i],r[i],t,c,ambig);
			}
			cout<<"iekf: (";
			cout<<t[0]<<","<<t[1]<<")";
			cout<<" (";
			cout<<c[0]<<",";
			cout<<c[1]<<",";
			cout<<c[2]<<",";
			cout<<c[3]<<")";
			cout<<endl;
			done = true;
		}
		if (vm.count("iwls") || !done) {
			RSN::line_intersection(n,px,py,z,t[0],t[1],c);
			RSN::BOT::IWLS_2D(n,px,py,z,r,t,c,ambig);
			cout<<"iwls: (";
			cout<<t[0]<<","<<t[1]<<")";
			cout<<" (";
			cout<<c[0]<<",";
			cout<<c[1]<<",";
			cout<<c[2]<<",";
			cout<<c[3]<<")";
			cout<<endl;
		}
	} catch (int err){
		switch(err){
			case -2:{cerr<<"Bad arg count"<<endl;}
		}
		cerr<<desc<<endl;
		return EXIT_FAILURE;
	} catch (...){
		cerr<<"Error from boost?"<<endl;
		cerr<<desc<<endl;
		return EXIT_FAILURE;
	}
	return EXIT_SUCCESS;
}


/* test_lognormal.cpp
 *
 * Copyright Steven Watanabe 2011
 * 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)
 *
 * $Id: test_lognormal.cpp 71018 2011-04-05 21:27:52Z steven_watanabe $
 *
 */

#include <boost/random/lognormal_distribution.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/math/distributions/lognormal.hpp>

#define BOOST_RANDOM_DISTRIBUTION boost::random::lognormal_distribution<>
#define BOOST_RANDOM_DISTRIBUTION_NAME lognormal
#define BOOST_MATH_DISTRIBUTION boost::math::lognormal
#define BOOST_RANDOM_ARG1_TYPE double
#define BOOST_RANDOM_ARG1_NAME m
#define BOOST_RANDOM_ARG1_DEFAULT 10.0
#define BOOST_RANDOM_ARG1_DISTRIBUTION(n) boost::uniform_real<>(-n, n)
#define BOOST_RANDOM_ARG2_TYPE double
#define BOOST_RANDOM_ARG2_NAME s
#define BOOST_RANDOM_ARG2_DEFAULT 10.0
#define BOOST_RANDOM_ARG2_DISTRIBUTION(n) boost::uniform_real<>(0.0001, n)

#include "test_real_distribution.ipp"


#include <shift/math/intersection.hpp>
#include <shift/math/interval.hpp>
#include <shift/core/algorithm.hpp>
#include <shift/platform/fpexceptions.hpp>
#include <shift/core/boost_disable_warnings.hpp>
#include <boost/test/unit_test.hpp>
#include <shift/core/boost_restore_warnings.hpp>

using namespace shift;
using namespace shift::math;

BOOST_AUTO_TEST_CASE(intersect_rectangle_rectangle)
{
  platform::floating_point_exceptions fpexceptions;

  auto p = [](auto x, auto y) { return make_vector_from(x, y); };
  auto r = [](auto p1, auto p2) { return make_interval_from(p1, p2); };

  // Same rectangles.
  BOOST_CHECK(
    intersection::rect_rect<true>(r(p(1, 1), p(4, 4)), r(p(1, 1), p(4, 4))));
  // First fully inside second.
  BOOST_CHECK(
    intersection::rect_rect<true>(r(p(2, 2), p(3, 3)), r(p(1, 1), p(4, 4))));
  // Second fully inside first.
  BOOST_CHECK(
    intersection::rect_rect<true>(r(p(1, 1), p(4, 4)), r(p(2, 2), p(3, 3))));
  // Second fully outside of first.
  BOOST_CHECK(
    !intersection::rect_rect<true>(r(p(1, 1), p(4, 4)), r(p(5, 1), p(8, 4))));

  // Only edge overlaps.
  BOOST_CHECK(
    intersection::rect_rect<true>(r(p(1, 1), p(3, 4)), r(p(3, 2), p(6, 5))));
  BOOST_CHECK(
    !intersection::rect_rect<false>(r(p(1, 1), p(3, 4)), r(p(3, 2), p(6, 5))));

  // Only top right corder overlaps.
  BOOST_CHECK(
    intersection::rect_rect<true>(r(p(1, 1), p(4, 4)), r(p(4, 4), p(8, 8))));
  BOOST_CHECK(
    !intersection::rect_rect<false>(r(p(1, 1), p(4, 4)), r(p(4, 4), p(8, 8))));

  // Only bottom left corder overlaps.
  BOOST_CHECK(
    intersection::rect_rect<true>(r(p(4, 4), p(8, 8)), r(p(1, 1), p(4, 4))));
  BOOST_CHECK(
    !intersection::rect_rect<false>(r(p(4, 4), p(8, 8)), r(p(1, 1), p(4, 4))));
}

BOOST_AUTO_TEST_CASE(intersect_point_triangle)
{
  auto v0 = make_vector_from(403.0f, 162.0f);
  auto v1 = make_vector_from(197.0f, 19.0f);
  auto v2 = make_vector_from(77.0f, 272.0f);

  // Check if the triangle corner points are within the triangle.
  BOOST_CHECK(intersection::point_triangle<true>(v0, v0, v1, v2));
  BOOST_CHECK(intersection::point_triangle<true>(v1, v0, v1, v2));
  BOOST_CHECK(intersection::point_triangle<true>(v2, v0, v1, v2));

  // Check three points that are known to be inside.
  auto i1 = make_vector_from(295.0f, 91.0f);
  auto i2 = make_vector_from(112.0f, 213.0f);
  auto i3 = make_vector_from(315.0f, 187.0f);
  BOOST_CHECK(intersection::point_triangle<true>(i1, v0, v1, v2));
  BOOST_CHECK(intersection::point_triangle<true>(i2, v0, v1, v2));
  BOOST_CHECK(intersection::point_triangle<true>(i3, v0, v1, v2));

  // Check three points that are known to be outside.
  auto o1 = make_vector_from(300.0f, 83.0f);
  auto o2 = make_vector_from(131.0f, 129.0f);
  auto o3 = make_vector_from(150.0f, 257.0f);
  BOOST_CHECK(!intersection::point_triangle<true>(o1, v0, v1, v2));
  BOOST_CHECK(!intersection::point_triangle<true>(o2, v0, v1, v2));
  BOOST_CHECK(!intersection::point_triangle<true>(o3, v0, v1, v2));
}


#include <gsl/gsl_math.h>
#include <gsl/gsl_sf.h>
#include <math.h>
#include <complex>
#include <boost/math/special_functions/jacobi_elliptic.hpp>
#include <boost/math/special_functions/ellint_1.hpp>

double am(double x, double m) {
	if (m == 0) {
		return x;
	} else if (x == 0) {
		return 0;
	} else {
		// function to calculate jacobi amplitude for real input
		// "borrowed" from sagemath
		double sn, cn, dn;
		if (fabs(m) == 1) {
			double tanhx = tanh(x);
			return asin(tanhx);
		} else if (fabs(m) > 1){
			gsl_sf_elljac_e(x, m, &sn, &cn, &dn);
			return atan2(sn, cn);
		}
		else{
			double K = gsl_sf_ellint_Kcomp(sqrt(m), GSL_PREC_DOUBLE);
			if (fabs(x) <= K) {
				gsl_sf_elljac_e(x, m, &sn, &cn, &dn);
				return atan2(sn, cn);
			} else {
				// Do argument reduction on x to end up with z = x - 2nK, with
				// abs(z) <= K
				double tK = 2 * K;
				int n = floor(x / tK);
				double tnK = n * tK;
				double npi = n * M_PI;
				double z = x - tnK;

				gsl_sf_elljac_e(z, m, &sn, &cn, &dn);
				return atan2(sn, cn) + npi;
			}
		}
	}
}

double calc_radius(double psi, double slr, double ecc) {
	double r = slr / (1 + ecc * cos(psi));
	// printf("slr = %.17g \n", slr);
	// printf("ecc = %.17g \n", ecc);
	// printf("psi = %.17g \n", psi);
	return r;
}

double calc_lambda_r(double r, double r1, double r2, double r3,
	double r4, double En) {

	// compute function to change from t[lambda] -> t[psi] with psi an angle
	// associated with radial motion.

	double yr, F_asin, kr;
	kr = ((r1 - r2)*(r3 - r4))/((r1 - r3)*(r2 - r4));
	// there is some problem here because of the high precision part of the
	// code. r can be less than r2 (peribothron).
	// TODO: fix the code, but for now a temporary fix:
	if (r < r2) {
		r = r2;
		yr = sqrt(((r - r2)*(r1 - r3))/((r1 - r2)*(r - r3)));
	}
	else {
		yr = sqrt(((r - r2)*(r1 - r3))/((r1 - r2)*(r - r3)));
	}
	// printf("r = %.17g \n", r);
	// printf("r2 = %.17g \n", r2);
	F_asin = gsl_sf_ellint_F(asin(yr), sqrt(kr), GSL_PREC_DOUBLE);
	return (2*F_asin)/(sqrt(1 - En * En)*sqrt((r1 - r3)*(r2 - r4)));
}

double calc_lambda_psi(double psi, double ups_r, double r1,
	double r2, double r3, double r4, double En, double slr, double ecc) {

	double lam_r, lam_r1, r, turns, res, kr;
	kr = ((r1 - r2)*(r3 - r4))/((r1 - r3)*(r2 - r4));
	r = calc_radius(psi, slr, ecc);
	lam_r = 2 * M_PI / ups_r;  // radial period
	lam_r1 = calc_lambda_r(r2, r1, r2, r3, r4, En);
	turns = floor(psi / (2 * M_PI));
	if (fmod(psi, 2 * M_PI) <= M_PI) {
		// printf("psi = %.17g\n", psi);
		res = calc_lambda_r(r, r1, r2, r3, r4, En) - lam_r1;
		// printf("res = %.17g\n", res);
	} else {
		res = lam_r1 - calc_lambda_r(r, r1, r2, r3, r4, En);
	}
	// printf("two\n");

	return lam_r * turns + res;
}

double calc_lambda_chi(double chi, double zp, double zm, double En, double aa) {
	// compute function to change from t[lambda] -> t[chi] with chi an angle
	// associated with polar motion.
	double beta, k, k2, prefactor, ellipticK_k, ellipticF;
	beta = aa * aa * (1 - En * En);
	k = sqrt(zm / zp);
	k2 = k * k;
	prefactor = 1 / sqrt(beta * zp);
	ellipticK_k = gsl_sf_ellint_Kcomp(sqrt(k2), GSL_PREC_DOUBLE);
	ellipticF = gsl_sf_ellint_F(M_PI / 2 - chi, sqrt(k2), GSL_PREC_DOUBLE);

	return prefactor * (ellipticK_k - ellipticF);
}

// routines used to find t, r, theta, phi

double calc_theta_chi(double chi, double zm) {
	return acos(zm * cos(chi));
}

double calc_rq(double qr, double r1, double r2,
	double r3, double r4) {

	double arg, sn2, sn, cn, dn, kr;

	kr = ((r1 - r2)*(r3 - r4))/((r1 - r3)*(r2 - r4));

	arg = (qr*gsl_sf_ellint_Kcomp(sqrt(kr), GSL_PREC_DOUBLE))/M_PI;
	gsl_sf_elljac_e(arg, kr, &sn, &cn, &dn);
	sn2 = sn * sn;

	return ((-(r2*(r1 - r3)) + (r1 - r2)*r3*sn2)/ (-r1 + r3 + (r1 - r2)*sn2));
}

double calc_zq(double qz, double zp, double zm, double En, double aa) {
	double ktheta, sn, dn, cn, ellipticK_k, arg1, arg2;
	ktheta = (aa*aa *(1 - En*En)*zm*zm)/zp*zp;
	ellipticK_k = gsl_sf_ellint_Kcomp(sqrt(ktheta), GSL_PREC_DOUBLE);
	arg1 = (2*(M_PI/2. + qz)*ellipticK_k)/M_PI;
	arg2 = ktheta;
	gsl_sf_elljac_e(arg1, arg2, &sn, &cn, &dn);

	return zm * sn;
}

double calc_psi_r(double qr, double r1, double r2, double r3, double r4) {
	double kr, amplitude;
	kr = ((r1 - r2)*(r3 - r4))/((r1 - r3)*(r2 - r4));
	amplitude = am((qr * gsl_sf_ellint_Kcomp(sqrt(kr), GSL_PREC_DOUBLE))/M_PI,
				   kr);
	return amplitude;
}

double calc_t_r(double qr, double r1, double r2, double r3, double r4,
	double En, double Lz, double aa, double M=1) {
	double psi_r, kr, rp, rm, hr, hp, hm, aa2, En2, sin_psi_r, ellipe_k, ince_k;
	double ellippi_hm, ellippi_hr, ellippi_hp, inc_pi_hm, inc_pi_hr, inc_pi_hp;
	aa2 = aa * aa;
	En2 = En * En;
	psi_r = calc_psi_r(qr, r1, r2, r3, r4);

	kr = ((r1 - r2)*(r3 - r4))/((r1 - r3)*(r2 - r4));

	rp = M + sqrt(-aa*aa + M*M);
	rm = M - sqrt(-aa*aa + M*M);

	hr = (r1 - r2)/(r1 - r3);
	hp = ((r1 - r2)*(r3 - rp))/((r1 - r3)*(r2 - rp));
	hm = ((r1 - r2)*(r3 - rm))/((r1 - r3)*(r2 - rm));

	sin_psi_r = pow(sin(psi_r), 2);
	ince_k = gsl_sf_ellint_E(psi_r, sqrt(kr), GSL_PREC_DOUBLE);
	ellipe_k = gsl_sf_ellint_Ecomp(sqrt(kr), GSL_PREC_DOUBLE);
	ellippi_hm = gsl_sf_ellint_Pcomp(sqrt(kr), -hm, GSL_PREC_DOUBLE);
	ellippi_hp = gsl_sf_ellint_Pcomp(sqrt(kr), -hp, GSL_PREC_DOUBLE);
	ellippi_hr = gsl_sf_ellint_Pcomp(sqrt(kr), -hr, GSL_PREC_DOUBLE);

	// printf("ellippi_hm = %.17g \n", ellippi_hm);

	inc_pi_hm = gsl_sf_ellint_P(psi_r, sqrt(kr), -hm, GSL_PREC_DOUBLE);
	inc_pi_hp = gsl_sf_ellint_P(psi_r, sqrt(kr), -hp, GSL_PREC_DOUBLE);
	inc_pi_hr = gsl_sf_ellint_P(psi_r, sqrt(kr), -hr, GSL_PREC_DOUBLE);

	return  (-((En * ((-4*(r2 - r3)*(-(((-2*aa2 + (4 - (aa*Lz)/En)*rm)*
			((qr*ellippi_hm)/M_PI - inc_pi_hm))/
			((r2 - rm)*(r3 - rm))) +
			((-2*aa2 + (4 - (aa*Lz)/En)*rp)*
			((qr*ellippi_hp)/M_PI - inc_pi_hp))/
			((r2 - rp)*(r3 - rp))))/(-rm + rp) +
			4*(r2 - r3)*((qr*ellippi_hr)/M_PI - inc_pi_hr) +
			(r2 - r3)*(r1 + r2 + r3 + r4)*
			((qr*ellippi_hr)/M_PI - inc_pi_hr) +
			(r1 - r3)*(r2 - r4)*((qr*ellipe_k)/M_PI - ince_k +
			(hr*cos(psi_r)*sin(psi_r)*sqrt(1 - kr*sin_psi_r))/
			(1 - hr*sin_psi_r))))/sqrt((1 - En2)*(r1 - r3)*(r2 - r4))));

}

double calc_phi_r(double qr, double r1, double r2, double r3, double r4,
	double En, double Lz, double aa, double M=1){

	double psi_r, kr, rp, rm, hp, hm, aa2, En2;
	double ellpi_hp, ellpi_hm, incpi_hp, incpi_hm;
	aa2 = aa * aa;
	En2 = En * En;
	psi_r = calc_psi_r(qr, r1, r2, r3, r4);
	kr = ((r1 - r2)*(r3 - r4))/((r1 - r3)*(r2 - r4));
	rp = M + sqrt(-aa2 + M*M);
	rm = M - sqrt(-aa2 + M*M);

	hp = ((r1 - r2)*(r3 - rp))/((r1 - r3)*(r2 - rp));
	hm = ((r1 - r2)*(r3 - rm))/((r1 - r3)*(r2 - rm));

	ellpi_hp = gsl_sf_ellint_Pcomp(sqrt(kr), -hp, GSL_PREC_DOUBLE);
	ellpi_hm = gsl_sf_ellint_Pcomp(sqrt(kr), -hm, GSL_PREC_DOUBLE);
	incpi_hp = gsl_sf_ellint_P(psi_r, sqrt(kr), -hp, GSL_PREC_DOUBLE);
	incpi_hm = gsl_sf_ellint_P(psi_r, sqrt(kr), -hm, GSL_PREC_DOUBLE);

	return ((2*aa*En*(-(((r2 - r3)*(-((aa*Lz)/En) + 2*rm)*
			((qr*ellpi_hm)/M_PI - incpi_hm))/
			((r2 - rm)*(r3 - rm))) +
			((r2 - r3)*(-((aa*Lz)/En) + 2*rp)*
			((qr*ellpi_hp)/M_PI - incpi_hp))/
			((r2 - rp)*(r3 - rp))))/
			(sqrt((1 - En2)*(r1 - r3)*(r2 - r4))*(-rm + rp)));

}

double calc_psi_z(double qz, double zp, double zm, double En, double aa){
	double ktheta, ellK_k;
	ktheta = (aa*aa *(1 - En*En)*zm*zm)/zp*zp;
	ellK_k = gsl_sf_ellint_Kcomp(sqrt(ktheta), GSL_PREC_DOUBLE);
	return am((2*(M_PI/2. + qz) * ellK_k)/M_PI, ktheta);
}

double calc_t_z(double qz, double zp, double zm, double En, double aa){
	double psi_z, ktheta, elle_k, ince_k;
	psi_z = calc_psi_z(qz, zp, zm, En, aa);
	ktheta = (aa*aa *(1 - En*En)*zm*zm)/zp*zp;
	elle_k = gsl_sf_ellint_Ecomp(sqrt(ktheta), GSL_PREC_DOUBLE);
	ince_k = gsl_sf_ellint_E(psi_z, sqrt(ktheta), GSL_PREC_DOUBLE);
	return ((En*zp*((2*(M_PI/2. + qz)*elle_k)/M_PI - ince_k))/
			(1 - En*En));
}

double calc_phi_z(double qz, double zp, double zm, double En, double Lz,
	double aa){

	double psi_z, ktheta, ellpi_k, incpi_k;
	psi_z = calc_psi_z(qz, zp, zm, En, aa);
	ktheta = (aa*aa * (1 - En*En)*zm*zm)/zp*zp;
	ellpi_k = gsl_sf_ellint_Pcomp(sqrt(ktheta), -zm*zm, GSL_PREC_DOUBLE);
	incpi_k = gsl_sf_ellint_P(psi_z, sqrt(ktheta), -zm*zm, GSL_PREC_DOUBLE);
	return (-((Lz*((2*(M_PI/2. + qz)*ellpi_k)/M_PI -
			incpi_k))/zp));
}

double calc_Ct(double qr0, double qz0, double r1, double r2, double r3,
	double r4, double zp, double zm, double En, double Lz, double aa) {

	double t_r, t_z;
	t_r = calc_t_r(qr0, r1, r2, r3, r4, En, Lz, aa);
	t_z = calc_t_z(qz0, zp, zm, En, aa);
	return t_r + t_z;
}

double calc_Cz(double qr0, double qz0, double r1, double r2, double r3, double r4,
	double zp, double zm, double En, double Lz, double aa) {

	double phi_r, phi_z;
	phi_r = calc_phi_r(qr0, r1, r2, r3, r4, En, Lz, aa);
	phi_z = calc_phi_z(qz0, zp, zm, En, Lz, aa);
	return phi_r + phi_z;
}

double calc_t(double mino_t, double ups_r, double ups_theta, double gamma,
	double qt0, double qr0, double qz0, double r1, double r2, double r3,
	double r4, double zp, double zm, double En, double Lz, double aa){

	double eta_t, eta_r, eta_z, t_r, t_z, Ct;
	eta_t = qt0 + gamma * mino_t;
	eta_r = qr0 + ups_r * mino_t;
	eta_z = qz0 + ups_theta * mino_t;
	if (r1 == r2) {
		t_r = 0;
	} else {
		t_r = calc_t_r(eta_r, r1, r2, r3, r4, En, Lz, aa);
		// printf("t_r = %.17g \n", t_r);
	}
	if (zm == 0){
		t_z = 0;
	} else {
		t_z = calc_t_z(eta_z, zp, zm, En, aa);
	}
	if (qr0 == 0 && qz0 == 0) {
		Ct = 0;
	} else {
		Ct = calc_Ct(qr0, qz0, r1, r2, r3, r4, zp, zm, En, Lz, aa);
	}
	return eta_t + t_r + t_z - Ct;
}

double calc_r(double mino_t, double ups_r, double qr0, double r1, double r2,
	double r3, double r4){

	double eta;
	eta = ups_r * mino_t + qr0;
	return calc_rq(eta, r1, r2, r3, r4);
}

double calc_theta(double mino_t, double ups_theta, double qz0, double zp,
	double zm, double En, double aa){

	double eta;
	eta = ups_theta * mino_t + qz0;
	return acos(calc_zq(eta, zp, zm, En, aa));
}

double calc_phi(double mino_t, double ups_r, double ups_theta, double ups_phi,
	double qphi0, double qr0, double qz0, double r1, double r2, double r3,
	double r4, double zp, double zm, double En, double Lz, double aa){

	double eta_phi, eta_r, eta_theta, phi_r, phi_z, Cz;
	eta_phi = ups_phi * mino_t + qphi0;
	eta_r = ups_r * mino_t + qr0;
	eta_theta = ups_theta * mino_t + qz0;
	if (r1 == r2) {
		phi_r = 0;
	} else {
		phi_r = calc_phi_r(eta_r, r1, r2, r3, r4, En, Lz, aa);
	}

	if (zm == 0) {
		phi_z = 0;
	}
	else {
		phi_z = calc_phi_z(eta_theta, zp, zm, En, Lz, aa);
	}

	if (qr0 == 0 && qz0 == 0){
		Cz = 0;
	} else {
		Cz = calc_Cz(qr0, qz0, r1, r2, r3, r4, zp, zm, En, Lz, aa);
	}
	return eta_phi + phi_r + phi_z - Cz;
}

double calc_J(double chi, double En, double Lz, double Q, double aa, double slr, double ecc) {
    // """
    // Schmidt's J function.

    // Parameters:
    //     chi (float): radial angle
    //     En (float): energy
    //     Lz (float): angular momentum
    //     Q (float): Carter constant
    //     aa (float): spin
    //     slr (float): semi-latus rectum
    //     ecc (float): eccentricity

    // Returns:
    //     J (float)
    // """
    double En2 = En * En;
    double ecc2 = ecc * ecc;
    double aa2 = aa * aa;
    double Lz2 = Lz * Lz;
    double slr2 = slr * slr;

    double eta = 1 + ecc * cos(chi);
    double eta2 = eta * eta;

    double J = (
        (1 - ecc2) * (1 - En2)
        + 2 * (1 - En2 - (1 - ecc2) / slr) * eta
        + (
            ((3 + ecc2) * (1 - En2)) / (1 - ecc2)
            + ((1 - ecc2) * (aa2 * (1 - En2) + Lz2 + Q)) / slr2
            - 4 / slr
        )
        * eta2
    );

    return J;
}

// double calc_wr(double psi, double ups_r, double En, double Lz,
// 			   double Q, double aa, double slr, double ecc, double x) {
// 	// note that this has only been checked for psi in [0, pi]
// 	double aa2 = aa * aa;
// 	double slr2 = slr * slr;
// 	double ecc2 = ecc * ecc;
// 	double En2 = En * En;
// 	double Lz2 = Lz * Lz;
// 	double a1 = (1 - ecc2) * (1 - En2);
//     double b1 = 2 * (1 - En2 - (1 - ecc2) / slr);
//     double c1 = (((3 + ecc2)*(1 - En2))/(1 - ecc2) - 4/slr +
//           		 ((1 - ecc2) * (aa2*(1 - En2) + Lz2 + Q)) / slr2);

// 	double b12 = b1 * b1;
// 	if (psi == M_PI) {
// 		return M_PI;
// 	} else {
// 		complex<double> phi {0, asinh(sqrt((a1 - (-1 + ecc)*(b1 + c1 - c1*ecc))/
//        						 (a1 + b1 + c1 - c1*ecc2 + sqrt(b12 - 4*a1*c1)*ecc2)))* tan(psi/2.))};

// 	 	double m = ((a1 + b1 + c1 - c1*ecc2 + sqrt((b12 - 4*a1*c1)*ecc2))/
//   				    (a1 + b1 + c1 - c1*ecc2 - sqrt((b12 - 4*a1*c1)*ecc2)));
// 		double k = sqrt(m);
// 		complex<double> ellint_f = ellint_1(k, phi);
// 		complex<double> res {0, ((-2*(1 - ecc2) * ellint_f * ups_r * power(cos(psi/2.),2)*
// 				sqrt(2 + (2*(a1 - (-1 + ecc)*(b1 + c1 - c1*ecc))*power(tan(psi/2.),2))/
// 					(a1 + b1 + c1 - c1*ecc2 - sqrt(b12 - 4*a1*c1)*ecc2)))*
// 				sqrt(1 + ((a1 - (-1 + ecc)*(b1 + c1 - c1*ecc))*power(tan(psi/2.),2))/
// 					(a1 + b1 + c1 - c1*ecc2 + sqrt((b12 - 4*a1*c1)*ecc2))))/
// 			(sqrt((a1 - (-1 + ecc)*(b1 + c1 - c1*ecc))/
// 				(a1 + b1 + c1 - c1*ecc2 + sqrt((b12 - 4*a1*c1)*ecc2)))*
// 				slr*sqrt(2*a1 + 2*b1 + 2*c1 + c1*ecc2 + 2*(b1 + 2*c1)*ecc*cos(psi) +
// 				c1*ecc2*cos(2*psi))))};
// 		double re_res = real(res);

// 		return re_res;
// 	}
// }

double calc_dwr_dpsi(double psi, double ups_r, double En, double Lz,
					 double Q, double aa, double slr, double ecc) {
    double J = calc_J(psi, En, Lz, Q, aa, slr, ecc);
	double ecc2 = ecc * ecc;
    return (1 - ecc2) / slr * ups_r / sqrt(J);
}

// double calc_wtheta(double chi, double ups_theta, double zp, double zm,
// 				   double En, double Lz, double aa, double slr, double x) {
//     // """
//     // w_theta = ups_theta * lambda as a function of polar angle chi

//     // Parameters:
//     //     chi (float): polar angle
//     //     ups_theta (float): theta mino frequency
//     //     En (float): energy
//     //     Lz (float): angular momentum
//     //     aa (float): spin
//     //     slr (float): semi-latus rectum
//     //     x (float): inclination

//     // Returns:
//     //     w_theta (float)
//     // """
//     double pi = M_PI;
//     if (chi >= 0 && chi <= pi / 2) {
//         return ups_theta * calc_lambda_chi(chi, zp, zm, En, Lz, aa, slr, x);
// 	} else if (chi > pi / 2 && chi <= pi) {
//         return pi - ups_theta * calc_lambda_chi(pi - chi, zp, zm, En, Lz, aa, slr, x);
// 	} else if (chi > pi && chi <= 3 * pi / 2) {
//         return pi + ups_theta * calc_lambda_chi(chi - pi, zp, zm, En, Lz, aa, slr, x);
// 	} else if (chi > 3 * pi / 2 && chi <= 2 * pi) {
//         return 2 * pi - ups_theta * calc_lambda_chi(2 * pi - chi, zp, zm, En, Lz, aa, slr, x);
// 	} else {
//         // print("Something went wrong in calc_wtheta!")
//         return 0.0;  // this case should not occur, but is required by C++
// 	}
// }

double calc_dwtheta_dchi(double chi, double zp, double zm) {
    // """
    // derivative of w_theta

    // Parameters:
    //     chi (float): polar angle
    //     zp (float): polar root
    //     zm (float): polar root

    // Returns:
    //     dw_dtheta (float)
    // """
    double k = sqrt(zm / zp);
    double ellipticK_k = gsl_sf_ellint_Kcomp(k, GSL_PREC_DOUBLE);
    return M_PI / (2 * ellipticK_k) * (1 / (1 - k * k * pow(cos(chi),2)));
}

void calc_circular_eq_coords(double &t, double &r, double &theta, double &phi,
							 double psi, double En, double Lz, double aa, double slr, double M) {
    // lam_psi is used to cross check with circular orbits in BHPTK
    // lam_psi = (psi / (sqrt(1 - En**2 + 3*(1 - En**2) + 2*(1 - En**2 - 1/slr) +
    //            (aa**2*(1 - En**2) + Lz**2)/slr**2 - 4/slr)*slr))
    // print(lam_psi)

	double x, Vr, Vphi, Vt, J, denom;
	double aa2 = aa * aa;
	double slr2 = slr * slr;
	x = Lz - aa * En;
	double x2 = x * x;

    Vr = aa2 + x2 + 2 * aa * x * En - 6 * M * x2 / slr;
    Vphi = x + aa * En - 2 * M * x / slr;
    Vt = aa2 * En - 2 * aa * M * x / slr + En * slr2;
    J = 1 - 2 * M / slr + aa2 / slr2;
    denom = J * sqrt(Vr);
	t = Vt / denom * psi;
    phi = Vphi / denom * psi;
    r = slr;
	theta = M_PI / 2;
}

void calc_equatorial_coords(double &t, double &r, double &theta, double &phi,
	double psi, double ups_r, double ups_theta,
	double ups_phi, double gamma, double r1, double r2, double r3,
	double r4, double zp, double zm, double En, double Lz, double aa,
	double slr, double ecc, double qt0, double qr0, double qz0,
	double qphi0) {

	double lam_psi;
	if (zm != 0){
		printf("The orbit specified is not equatorial.\n");
	}
	r = calc_radius(psi, slr, ecc);
	lam_psi = calc_lambda_psi(psi, ups_r, r1, r2, r3, r4, En, slr, ecc);
	t = calc_t(lam_psi, ups_r, ups_theta, gamma, qt0, qr0, qz0, r1, r2, r3, r4,
			   zp, zm, En, Lz, aa);
	theta = M_PI / 2;  // TODO(aaron): check generic theta
	phi = calc_phi(lam_psi, ups_r, ups_theta, ups_phi, qphi0, qr0, qz0, r1, r2,
				   r3, r4, zp, zm, En, Lz, aa);

}

void calc_gen_coords_mino(double &t, double &r, double &theta, double &phi,
	double mino_t, double ups_r, double ups_theta, double ups_phi, double gamma,
    double r1, double r2, double r3, double r4, double zp, double zm, double En,
    double Lz, double aa, double qphi0, double qr0, double qz0, double qt0) {

	t = calc_t(mino_t, ups_r, ups_theta, gamma, qt0, qr0, qz0, r1, r2, r3, r4, zp, zm, En, Lz, aa);
	r = calc_r(mino_t, ups_r, qr0, r1, r2, r3, r4);
	theta = calc_theta(mino_t, ups_theta, qz0, zp, zm, En, aa);
	phi = calc_phi(mino_t, ups_r, ups_theta, ups_phi, qphi0, qr0, qz0, r1, r2, r3, r4, zp, zm, En, Lz, aa);
}

// void calc_gen_coords_psi(double &t, double &r, double &theta, double &phi,
// 	double psi, double ups_r, double ups_theta, double ups_phi, double gamma,
//     double r1, double r2, double r3, double r4, double zp, double zm, double En,
// 	double Lz, double Q, double aa, double slr, double ecc, double x,
// 	double qphi0, double qr0, double qz0, double qt0) {

// 	double wr = calc_wr(psi, ups_r, En, Lz, Q, aa, slr, ecc, x);
// 	double mino_t = wr / ups_r;
// 	t = calc_t(mino_t, ups_r, ups_theta, gamma, qt0, qr0, qz0, r1, r2, r3, r4, zp, zm, En, Lz, aa);
// 	r = calc_r(mino_t, ups_r, qr0, r1, r2, r3, r4);
// 	theta = calc_theta(mino_t, ups_theta, qz0, zp, zm, En, aa);
// 	phi = calc_phi(mino_t, ups_r, ups_theta, ups_phi, qphi0, qr0, qz0, r1, r2, r3, r4, zp, zm, En, Lz, aa);
// }


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2006 - 2016 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level directory of deal.II.
 *
 * ---------------------------------------------------------------------
 *
 * Author: Ivan Christov, Wolfgang Bangerth, Texas A&M University, 2006
 */
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/utilities.h>
#include <deal.II/lac/vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/affine_constraints.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.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_q.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/data_out.h>
#include <fstream>
#include <iostream>
namespace Step25
{
  using namespace dealii;
  template <int dim>
  class SineGordonProblem
  {
  public:
    SineGordonProblem();
    void run();
  private:
    void         make_grid_and_dofs();
    void         assemble_system();
    void         compute_nl_term(const Vector<double> &old_data,
                                 const Vector<double> &new_data,
                                 Vector<double> &      nl_term) const;
    void         compute_nl_matrix(const Vector<double> &old_data,
                                   const Vector<double> &new_data,
                                   SparseMatrix<double> &nl_matrix) const;
    unsigned int solve();
    void         output_results(const unsigned int timestep_number) const;
    Triangulation<dim> triangulation;
    FE_Q<dim>          fe;
    DoFHandler<dim>    dof_handler;
    SparsityPattern      sparsity_pattern;
    SparseMatrix<double> system_matrix;
    SparseMatrix<double> mass_matrix;
    SparseMatrix<double> laplace_matrix;
    const unsigned int n_global_refinements;
    double       time;
    const double final_time, time_step;
    const double theta;
    Vector<double> solution, solution_update, old_solution;
    Vector<double> M_x_velocity;
    Vector<double> system_rhs;
    const unsigned int output_timestep_skip;
  };
  template <int dim>
  class ExactSolution : public Function<dim>
  {
  public:
    ExactSolution(const unsigned int n_components = 1, const double time = 0.)
      : Function<dim>(n_components, time)
    {}
    virtual double value(const Point<dim> & p,
                         const unsigned int component = 0) const override;
  };
  template <int dim>
  double ExactSolution<dim>::value(const Point<dim> &p,
                                   const unsigned int /*component*/) const
  {
    double t = this->get_time();
    switch (dim)
      {
        case 1:
          {
            const double m  = 0.5;
            const double c1 = 0.;
            const double c2 = 0.;
            return -4. * std::atan(m / std::sqrt(1. - m * m) *
                                   std::sin(std::sqrt(1. - m * m) * t + c2) /
                                   std::cosh(m * p[0] + c1));
          }
        case 2:
          {
            const double theta  = numbers::PI / 4.;
            const double lambda = 1.;
            const double a0     = 1.;
            const double s      = 1.;
            const double arg    = p[0] * std::cos(theta) +
                               std::sin(theta) * (p[1] * std::cosh(lambda) +
                                                  t * std::sinh(lambda));
            return 4. * std::atan(a0 * std::exp(s * arg));
          }
        case 3:
          {
            const double theta = numbers::PI / 4;
            const double phi   = numbers::PI / 4;
            const double tau   = 1.;
            const double c0    = 1.;
            const double s     = 1.;
            const double arg   = p[0] * std::cos(theta) +
                               p[1] * std::sin(theta) * std::cos(phi) +
                               std::sin(theta) * std::sin(phi) *
                                 (p[2] * std::cosh(tau) + t * std::sinh(tau));
            return 4. * std::atan(c0 * std::exp(s * arg));
          }
        default:
          Assert(false, ExcNotImplemented());
          return -1e8;
      }
  }
  template <int dim>
  class InitialValues : public Function<dim>
  {
  public:
    InitialValues(const unsigned int n_components = 1, const double time = 0.)
      : Function<dim>(n_components, time)
    {}
    virtual double value(const Point<dim> & p,
                         const unsigned int component = 0) const override;
  };
  template <int dim>
  double InitialValues<dim>::value(const Point<dim> & p,
                                   const unsigned int component) const
  {
    return ExactSolution<dim>(1, this->get_time()).value(p, component);
  }
  template <int dim>
  SineGordonProblem<dim>::SineGordonProblem()
    : fe(1)
    , dof_handler(triangulation)
    , n_global_refinements(6)
    , time(-5.4414)
    , final_time(2.7207)
    , time_step(10 * 1. / std::pow(2., 1. * n_global_refinements))
    , theta(0.5)
    , output_timestep_skip(1)
  {}
  template <int dim>
  void SineGordonProblem<dim>::make_grid_and_dofs()
  {
    GridGenerator::hyper_cube(triangulation, -10, 10);
    triangulation.refine_global(n_global_refinements);
    std::cout << "   Number of active cells: " << triangulation.n_active_cells()
              << std::endl
              << "   Total number of cells: " << triangulation.n_cells()
              << std::endl;
    dof_handler.distribute_dofs(fe);
    std::cout << "   Number of degrees of freedom: " << dof_handler.n_dofs()
              << std::endl;
    DynamicSparsityPattern dsp(dof_handler.n_dofs(), dof_handler.n_dofs());
    DoFTools::make_sparsity_pattern(dof_handler, dsp);
    sparsity_pattern.copy_from(dsp);
    system_matrix.reinit(sparsity_pattern);
    mass_matrix.reinit(sparsity_pattern);
    laplace_matrix.reinit(sparsity_pattern);
    MatrixCreator::create_mass_matrix(dof_handler,
                                      QGauss<dim>(fe.degree + 1),
                                      mass_matrix);
    MatrixCreator::create_laplace_matrix(dof_handler,
                                         QGauss<dim>(fe.degree + 1),
                                         laplace_matrix);
    solution.reinit(dof_handler.n_dofs());
    solution_update.reinit(dof_handler.n_dofs());
    old_solution.reinit(dof_handler.n_dofs());
    M_x_velocity.reinit(dof_handler.n_dofs());
    system_rhs.reinit(dof_handler.n_dofs());
  }
  template <int dim>
  void SineGordonProblem<dim>::assemble_system()
  {
    system_matrix.copy_from(mass_matrix);
    system_matrix.add(std::pow(time_step * theta, 2), laplace_matrix);
    SparseMatrix<double> tmp_matrix(sparsity_pattern);
    compute_nl_matrix(old_solution, solution, tmp_matrix);
    system_matrix.add(std::pow(time_step * theta, 2), tmp_matrix);
    system_rhs = 0.;
    Vector<double> tmp_vector(solution.size());
    mass_matrix.vmult(system_rhs, solution);
    laplace_matrix.vmult(tmp_vector, solution);
    system_rhs.add(std::pow(time_step * theta, 2), tmp_vector);
    mass_matrix.vmult(tmp_vector, old_solution);
    system_rhs.add(-1.0, tmp_vector);
    laplace_matrix.vmult(tmp_vector, old_solution);
    system_rhs.add(std::pow(time_step, 2) * theta * (1 - theta), tmp_vector);
    system_rhs.add(-time_step, M_x_velocity);
    compute_nl_term(old_solution, solution, tmp_vector);
    system_rhs.add(std::pow(time_step, 2) * theta, tmp_vector);
    system_rhs *= -1.;
  }
  template <int dim>
  void SineGordonProblem<dim>::compute_nl_term(const Vector<double> &old_data,
                                               const Vector<double> &new_data,
                                               Vector<double> &nl_term) const
  {
    nl_term = 0;
    const QGauss<dim> quadrature_formula(fe.degree + 1);
    FEValues<dim>     fe_values(fe,
                            quadrature_formula,
                            update_values | update_JxW_values |
                              update_quadrature_points);
    const unsigned int dofs_per_cell = fe.dofs_per_cell;
    const unsigned int n_q_points    = quadrature_formula.size();
    Vector<double>                       local_nl_term(dofs_per_cell);
    std::vector<types::global_dof_index> local_dof_indices(dofs_per_cell);
    std::vector<double>                  old_data_values(n_q_points);
    std::vector<double>                  new_data_values(n_q_points);
    typename DoFHandler<dim>::active_cell_iterator cell =
                                                     dof_handler.begin_active(),
                                                   endc = dof_handler.end();
    for (; cell != endc; ++cell)
      {
        local_nl_term = 0;
        fe_values.reinit(cell);
        fe_values.get_function_values(old_data, old_data_values);
        fe_values.get_function_values(new_data, new_data_values);
        for (unsigned int q_point = 0; q_point < n_q_points; ++q_point)
          for (unsigned int i = 0; i < dofs_per_cell; ++i)
            local_nl_term(i) +=
              (std::sin(theta * new_data_values[q_point] +
                        (1 - theta) * old_data_values[q_point]) *
               fe_values.shape_value(i, q_point) * fe_values.JxW(q_point));
        cell->get_dof_indices(local_dof_indices);
        for (unsigned int i = 0; i < dofs_per_cell; ++i)
          nl_term(local_dof_indices[i]) += local_nl_term(i);
      }
  }
  template <int dim>
  void SineGordonProblem<dim>::compute_nl_matrix(
    const Vector<double> &old_data,
    const Vector<double> &new_data,
    SparseMatrix<double> &nl_matrix) const
  {
    QGauss<dim>   quadrature_formula(fe.degree + 1);
    FEValues<dim> fe_values(fe,
                            quadrature_formula,
                            update_values | update_JxW_values |
                              update_quadrature_points);
    const unsigned int dofs_per_cell = fe.dofs_per_cell;
    const unsigned int n_q_points    = quadrature_formula.size();
    FullMatrix<double> local_nl_matrix(dofs_per_cell, dofs_per_cell);
    std::vector<types::global_dof_index> local_dof_indices(dofs_per_cell);
    std::vector<double>                  old_data_values(n_q_points);
    std::vector<double>                  new_data_values(n_q_points);
    typename DoFHandler<dim>::active_cell_iterator cell =
                                                     dof_handler.begin_active(),
                                                   endc = dof_handler.end();
    for (; cell != endc; ++cell)
      {
        local_nl_matrix = 0;
        fe_values.reinit(cell);
        fe_values.get_function_values(old_data, old_data_values);
        fe_values.get_function_values(new_data, new_data_values);
        for (unsigned int q_point = 0; q_point < n_q_points; ++q_point)
          for (unsigned int i = 0; i < dofs_per_cell; ++i)
            for (unsigned int j = 0; j < dofs_per_cell; ++j)
              local_nl_matrix(i, j) +=
                (std::cos(theta * new_data_values[q_point] +
                          (1 - theta) * old_data_values[q_point]) *
                 fe_values.shape_value(i, q_point) *
                 fe_values.shape_value(j, q_point) * fe_values.JxW(q_point));
        cell->get_dof_indices(local_dof_indices);
        for (unsigned int i = 0; i < dofs_per_cell; ++i)
          for (unsigned int j = 0; j < dofs_per_cell; ++j)
            nl_matrix.add(local_dof_indices[i],
                          local_dof_indices[j],
                          local_nl_matrix(i, j));
      }
  }
  template <int dim>
  unsigned int SineGordonProblem<dim>::solve()
  {
    SolverControl solver_control(1000, 1e-12 * system_rhs.l2_norm());
    SolverCG<>    cg(solver_control);
    PreconditionSSOR<> preconditioner;
    preconditioner.initialize(system_matrix, 1.2);
    cg.solve(system_matrix, solution_update, system_rhs, preconditioner);
    return solver_control.last_step();
  }
  template <int dim>
  void SineGordonProblem<dim>::output_results(
    const unsigned int timestep_number) const
  {
    DataOut<dim> data_out;
    data_out.attach_dof_handler(dof_handler);
    data_out.add_data_vector(solution, "u");
    data_out.build_patches();
    const std::string filename =
      "solution-" + Utilities::int_to_string(timestep_number, 3) + ".vtk";
    std::ofstream output(filename);
    data_out.write_vtk(output);
  }
  template <int dim>
  void SineGordonProblem<dim>::run()
  {
    make_grid_and_dofs();
    {
      AffineConstraints<double> constraints;
      constraints.close();
      VectorTools::project(dof_handler,
                           constraints,
                           QGauss<dim>(fe.degree + 1),
                           InitialValues<dim>(1, time),
                           solution);
    }
    output_results(0);
    unsigned int timestep_number = 1;
    for (time += time_step; time <= final_time;
         time += time_step, ++timestep_number)
      {
        old_solution = solution;
        std::cout << std::endl
                  << "Time step #" << timestep_number << "; "
                  << "advancing to t = " << time << "." << std::endl;
        double initial_rhs_norm = 0.;
        bool   first_iteration  = true;
        do
          {
            assemble_system();
            if (first_iteration == true)
              initial_rhs_norm = system_rhs.l2_norm();
            const unsigned int n_iterations = solve();
            solution += solution_update;
            if (first_iteration == true)
              std::cout << "    " << n_iterations;
            else
              std::cout << '+' << n_iterations;
            first_iteration = false;
          }
        while (system_rhs.l2_norm() > 1e-6 * initial_rhs_norm);
        std::cout << " CG iterations per nonlinear step." << std::endl;
        Vector<double> tmp_vector(solution.size());
        laplace_matrix.vmult(tmp_vector, solution);
        M_x_velocity.add(-time_step * theta, tmp_vector);
        laplace_matrix.vmult(tmp_vector, old_solution);
        M_x_velocity.add(-time_step * (1 - theta), tmp_vector);
        compute_nl_term(old_solution, solution, tmp_vector);
        M_x_velocity.add(-time_step, tmp_vector);
        if (timestep_number % output_timestep_skip == 0)
          output_results(timestep_number);
      }
  }
} // namespace Step25
int main()
{
  try
    {
      using namespace dealii;
      using namespace Step25;
      SineGordonProblem<1> sg_problem;
      sg_problem.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;
}